optimal path finding for a mining shovel - tu/e · 2008. 2. 1. · the mining shovel has three...
TRANSCRIPT
Optimal path finding for amining shovel
S.R. Bögemann, F.B.J.W.M.Hendriks
DCT 2008.006
Traineeship report
Coach(es): J. Welsh
Supervisor: M.Steinbuch, G.Goodwin
Technische Universiteit EindhovenDepartment Mechanical EngineeringDynamics and Control Technology Group
Eindhoven, January, 2008
2
3
1. Introduction....................................................................................................................................5 2. The robot arm.................................................................................................................................7 3. Identification ..................................................................................................................................9 4. Trajectory planning.......................................................................................................................13
4.1 Visibility graph .......................................................................................................................13 4.2 Dijkstra algorithm...................................................................................................................14 4.3 System dynamics ....................................................................................................................15 4.4 The modified Dijkstra algorithm..............................................................................................16 4.5 Turning ellipses ......................................................................................................................17 4.6 The planning algorithm in practice: .........................................................................................18
5. MPC.............................................................................................................................................21 6. Simulation ....................................................................................................................................23 7. Results..........................................................................................................................................27
7.1 Reference generation...............................................................................................................27 7.2 Stepsize ..................................................................................................................................28 7.3 Constant horizon (seconds)......................................................................................................29 7.4 Weight function ......................................................................................................................29 7.5 Horizon length ........................................................................................................................30
8. Conclusions and Recommendations ..............................................................................................33 References........................................................................................................................................35 Appendix A: The robot arm used ......................................................................................................37 Appendix B: How to connect the robot-arm to a D-Space system ......................................................39 Appendix C: Simulation parameters..................................................................................................41 Appendix D: State space matrices .....................................................................................................42
4
5
1. Introduction In today’s mining there is strong interest in automating the digging process. There are many investigations on collision avoidance systems, which prevent the operator from hitting objects with his mining shovel. The next step will be a form of cruise control; the operator of the mining shovel only has to take care of the digging, after this the mining shovel itself will take over and empty the bucket in a nearby truck. The control circuit of the mining shovel will have to find the shortest path without colliding with objects in its environment. This project focuses on finding an optimal path through a field of objects using a receding horizon controller in combination with a trajectory planning algorithm. To experiment with the controller in practice, a robot arm setup was prepared. Time shortage prevented actual implementation of the setup. Therefore this report will only contain preparation and theoretical testing. First, the robot arm used for the experiments is presented. The identification of this robot arm is then discussed next. After this, the trajectory planning will be examined in two parts, namely a coarse and a fine trajectory planning.
6
7
2. The robot arm A typical mining shovel is depicted in figure 2.1. The mining shovel has three degrees of freedom; it can rotate around its vertical axis and the bucket can be moved in lateral and vertical direction. The robot arm used to simulate the mining shovel is an UMI RT100 (fig. 2.2). To simulate all the movements of a shovel, only three degrees of freedom need to be used. The shoulder, elbow and z-axis combined will provide all the motions needed. A schematic overview of the robot arm is depicted in Appendix A.
The arm is originally operated using a controller board connected to a pc using a RS232 connection. This resulted in a problem with the bandwidth of the data transaction between the RS232 port of the system and the computer. The maximum data-rate of this connection is 9600kbps and has a maximum update frequency 60 Hz. Every cycle only one action can be performed, either the encoder position can be requested or a control action can be sent. The connection has to be shared by the three motors. This comes down to a practical update frequency of 10Hz for each encoder on each motor. This situation only allows very slow control and was not considered a realistic option. Therefore it was decided to use a D-space system connected directly to the encoders and amplifiers for the motors of the robot. The system now provides an update frequency of 25 kHz for all three motors simultaneously. This resulted in a smooth operation. More details of the connection of the system can be found in Appendix B.
Fig. 2.2 – The UMI RT100 robot arm [4]
Fig. 2.1 – Open pit mining shovel [3]
8
9
3. Identification To identify the robotic arm system used in the implementation, a closed loop 3-point frequency response measurement is performed (fig. 3.1). The measurement is done in closed loop, because of the integrators in the plant. The system is as described in chapter 2. A band-limited white noise (w) is added to the control loop between the controller (C) and the system (H). A band-limited white noise is a persistently exciting signal which contains all frequencies. The transfer function between the noise and the input of the system (u) is the Sensitivity (S = 1/(1+CH)) of the control loop. The transfer function between the noise and the error (e) in the closed loop is the Process Sensitivity (PS = H/(1+HC)) of the control loop. By dividing the PS by the S, the system can be identified. To reduce the non linear effects in the measurement, a sine is added to the system as a reference (r). The sine is chosen in such a way that the system is ‘in motion’ as much as possible to reduce the influence of stick-slip. The sine should also be ‘fast’ enough in combination with the noise to reduce the influence of play in the measurement. The first measurement is performed with a bandwidth of ~1 Hz. To increase the coherence of the PS, the bandwidth of the controller is increased. The coherence of the S will drop a bit, but the drop is less than the increase in the coherence of the PS. These measurements showed that the frequencies below 20 Hz contained all the important information of the system. To further increase the coherence of the PS and the S, the noise was passed through a 2nd order low-pass filter at 20 Hz before it was added to the control loop. In this way, the energy of the noise is concentrated at the interesting frequencies. The coherence will increase at these frequencies, but it will drop at higher frequencies.
100
101
100
|H|
white noiselow bandwidth noisesine
100
101
-180-160-140-120-100
-80-60-40-20
0
Hz
angl
e(H
)
Figure 3.1: Block scheme of the control loop
Figure 3.2: Bode plot of measured data
10
The new frequency response with the higher coherence changed, for one of the axes, compared to the older frequency responses. It changed in such a way, that a frequency that was once an anti-resonance has become a resonance (fig. 3.2). To check which frequency response is better, sine waves were added to the system instead of white noise. The overall coherence will be very bad for these measurements at most frequencies, but very good at the frequencies of the added sine waves. Comparing the measurements with the white noise and the sine waves, it can be concluded that the last measurement, with the highest coherence, is the best to approximate the system. To get a useful model out of the data, a continuous time transfer function is fitted through the measurement data. This transfer function is converted to a state space model and then transformed into a discrete state space model. The discrete state space model is used for MPC. The bode plots of the models are given in figure 3.3. The matrices of the state space models can be found in appendix D.
10-1
100
101
10-1
100
101
102
|Hel
bow
|
measurementfit
10-1
100
101
-200
-150
-100
-50
0
angl
e(H
elb
ow)
Hz
Figure 3.3: The bode plot of the measured data of the elbow and its fitted model.
11
10-1
100
101
10-1
100
101
102
|Hsh
ould
er|
measurementfit
10-1
100
101
-200
-150
-100
-50
0
angl
e(H
shou
lder)
Hz
10-1
100
101
10-1
100
101
102
|HZ|
measurementfit
101
-200
-150
-100
-50
0
angl
e(H
Z)
Hz
Figure 3.5: The bode plot of the measured data of the Z-direction and its fitted model.
Figure 3.4: The bode plot of the measured data of the shoulder and its fitted model.
12
13
4. Trajectory planning The robot arm will find its way through a field of objects using a receding horizon (MPC) controller. This controller uses an optimization routine to calculate future inputs and outputs. The optimization problem of the controller grows enormously as the controller horizon increases. To plan the whole trajectory at once, using the controller would result in unacceptable calculation costs. Therefore the planning of the complete trajectory has been split into a coarse and a fine trajectory planning. The fine planning will be done by the controller which will be explained in chapter 5. The coarse planning will be performed by a separate trajectory planning routine. This routine will build a field with straight line connections. A Dijkstra algorithm [2] will find the shortest path, following these straight line connections. This coarse path will be used to create setpoints for the receding horizon controller. This means the horizon of the controller can be kept small, resulting in small calculation costs [1]. This chapter will examine the coarse planning routine, which is created in Matlab.
4.1 Visibility graph The first step in building a trajectory is creating a visibility graph. A visibility graph is a graphical representation of all the possible routes the system can take to form the shortest path in a space containing objects. The corners of the objects are the nodes for the possible routes. This is because the shortest path around rectangular objects will be a straight line connection between their corners. First, all the corner points of all the objects are inspected individually. Each point is connected with all the other points on the map. After this, the connections are checked to be collision free. This is done by checking on possible intersections of the connections with object boundaries. An example of the result of the program is presented in figure 4.1.a.
These connections will be used to generate the shortest path using a Dijkstra algorithm. Calculation time of this algorithm will be reduced if the number of connections is reduced. This is possible, because some of the lines will never be used to form the shortest path, and can be discarded.
Fig. 4.1.a – Visibility Graph Fig. 4.1.b – Obsolete connections discarded
14
For example, the connection between node A and B in figure 4.2 will never result in the shortest path. The shortest path will always be around an object. So unless one of the nodes is the begin- or endpoint, this will be an obsolete connection. To eliminate these obsolete connections, all the lines are extended a distance ε. If the line then ends in an obstacle, that connection can be pruned. This may lead to a large reduction of the available connections (fig 4.1.b).
4.2 Dijkstra algorithm A Dijkstra algorithm is used to find the shortest path in the tree of connections in the visibility graph. The algorithm creates a tree of connections which all have labels representing their distance to the begin point. The next paragraph presents a brief explanation of the Dijkstra algorithm with an example. All the nodes in the field will have a temporary distance label. They will get an initial label which is infinite, until it is updated (see table 4.1). The algorithm will start at the begin point (A) (see fig. 4.3), where it will update all the distance labels of the nodes it has a connection to (B and C). Now the algorithm will continue to perform the following steps: The node with the smallest distance label (B) will be added to the tree and the distance label will be fixed. This node will be the next node of interest, and the temporary distance labels of all its neighbors will be inspected. If the path from the begin point to the neighbor node through node B is smaller than the temporary distance label, the label is updated. Then the next overall node with the smallest distance label (C) will become the node of interest. And the algorithm will start over again. The distance label from D will be smaller when travelling through point C, therefore the label of D will be updated to 8. When the endpoint is reached (D) and the distance label is fixed, the algorithm will stop and the shortest path between the begin and end point is found.
initial (A) (B) (C) A ∞ 0 0 0 B ∞ 3 3 3 C ∞ 5 5 5 D ∞ ∞ 9 8
A
D
B
C
5
3
6
3
Fig. 4.3 – Explanation of the Dijkstra algorithm
Fig. 4.2 – Example of a connection which can be pruned
Table. 4.1 – Distance labels in different situations. A gray labels means the label is fixed.
15
4.3 System dynamics To make a coarse trajectory which is feasible, the trajectory planning has to account for the system dynamics in order to create a path the system is able to follow. The system will have several constraints, mainly on its acceleration. To account for these constraints, a minimum turning radius is used [1]. The resulting path created by the visibility graph and Dijkstra algorithm consists of a number of nodes connected with straight lines. This may lead to infeasible situations. In (fig 4.4.a) the straight line path approximation will go through the narrow passage. When the receding horizon controller is creating the fine trajectory (fig 4.4.b), it will enter the narrow passage, but will then get stuck in the first corner. The system will be unable to make the small turn because of the system dynamics.
To eliminate these infeasible situations, the Dijkstra algorithm has to account for the system dynamics hence the algorithm has to be modified. Since the robot arm can control the movements in different directions separately, the turning radius of the robot arm is infinitely small. Therefore, the robot-arm won’t need a modified algorithm. But since the mining shovel will have very slow dynamics, the modification of the Dijkstra algorithm is considered. Not only to account for infeasible solutions, but to create a smooth trajectory. This will often eliminate the need for deceleration and acceleration and save time.
4.3.1 Turning circle
In order to find a feasible, collision free route turning circles are used [1]. They can be used to simulate a path, suitable for the system to follow, without collision with objects. The path around a corner can be described by three circles: a leave, corner and comeback circle (fig 4.5).
Fig. 4.4.b – Receding horizon controller Fig. 4.4.a – Straight line connections
16
The system will leave the straight line connection on the leave point to follow the leave circle. After this it will follow the corner circle and comeback circle. It will return on the straight line connection to the following corner on the comeback point. To make sure the system is able to follow the path, the arcs should not cross objects. Even if the arcs are “collision free”, the system can’t follow the path before it completes the previous turn. The trajectory planning accounts for that by making sure the leave point and comeback point don’t overlap each other on a connection.
4.4 The modified Dijkstra algorithm The Dijkstra algorithm will check if the connection is feasible before updating the temporary distance label. To do this, the turning circles will be placed around each current node. If the connection does not meet the criteria, it is rejected. The method of placing the circles requires the Dijkstra Algorithm to work in reverse. Therefore in this case the algorithm will start at the end point and searches for a connection to the begin point.
1. Set the end point node i=1, set all distance labels not connected to 1 at infinity. Update all distance labels of the nodes connected to 1 at their separation distance from the end point.
2. Each node connected to the end point will have this node as the next node on the path towards the goal.
3. Of all the nodes were the distance labels are not yet fixed, find the node with the smallest distance label and set it as the new current node i.
4. Place the corner circle and comeback circle around node i. 5. Fix the distance label of node i and update the set of fixed nodes P. 6. If all the nodes are in P, terminate. 7. For all nodes j where the distance labels are unfixed and are visible form the current node i
a. Place a leave circle around i and put a leave point on the connection between i and j. b. Check the feasibility of the connection:
i. The leave point lies between i and j ii. The path from i to j along the corner circle and leave circle is collision free
If the connection is not feasible, reject the connection, pick next j and return to step 7.
c. Update the temporary distance label if the connection through i is smaller than the current distance label.
d. If the distance label of j is updated, set i as the next path towards the goal of node j. e. Pick next node j and go to step 7
8. go to step 3 for the next iteration
Fig. 4.5 – Turning circles
17
4.5 Turning ellipses The modified algorithm in [1] uses a minimum turning radius to account for the system dynamics. In the mining shovel and the robot arm the system dynamics depend highly on the direction of actuation. The inertia and power of actuation in the seperate directions of movement are very different, hence the concept of a minimum turning radius will not work on these systems. All directions will have their own stopping distance, which will result in an ellipse rather than a circle. The geometry of an ellipse is much more complex than that of a circle. Therefore it is hard to find an analytical way to place the ellipses. Due to lack of time the implementation of ellipses is beyond the scope of this project.
18
4.6 The planning algorithm in practice: The algorithm which is presented earlier has been implemented in Matlab. Different fields of obstacles are created to test the different situations the system will encounter. The main goal of these test fields is to show the algorithm functions correctly and the program can be implemented on the robotarm. A number of the test cases are presented in this chapter. Notice that all the routes consist of straight line connections. The program leaves the creation of the turns to the fine trajectory planning and only creates the shortest path which the system will be able to follow. The small circle and star represent the begin point and end point respectively. The first and most simple test case is simply putting a block between the begin- and endpoint. The program finds the shortest route, going below the block (fig 4.6).
Secondly, the program should not be able to create a path trough a block. Therefore the begin point is put into a block. As expected, the program doesn’t find a route (fig 4.7). Next, the implementation of the Dijkstra algorithm is to be tested. The small passage mentioned in paragraph 4.3 is the reason for putting the Dijkstra algorithm in the program. First, the system is allowed to make very tight corners and is able to find a way through the narrow passage (fig 4.8). Secondly, the turning radius of the system is increased and a different route is chosen (fig 4.9).
Increasing the turning radius of the system even further results in an error, the system is not able to create a route through the passages (fig 4.10).
Fig. 4.6 – One obstacle between the begin and end point Fig. 4.7 – The begin point in an object
Fig. 4.8 – Small turning radius Fig. 4.9 – Larger turning radius
19
Fig. 4.10 – Too large turning radius
20
21
5. MPC In the last 15 years, the interest in Model Predictive Control (MPC) has increased enormously. The two main reasons for this increase are the development of better identification techniques and the availability of faster computers. Nowadays, MPC is commonly used in the process industry, but it is also very well applicable to other kinds of systems. Although MPC has some big advantages with respect to constraint handling and MIMO systems when compared to the well-known PID controller they should be considered complementary control strategies. PID controllers are simple and very fast, therefore very suitable for low level control. MPC is much slower, but can handle a wider variety of systems. MPC can handle nonlinear multivariable interactive systems, systems with constraints, deadtimes, feedforward and minimum phase behavior for example. Therefore MPC is very suitable for high level control and/or very slow systems, which explains why it is used commonly in the process industry. MPC is a control strategy which works intuitively. It is not a ‘standard’ controller like a PID controller. MPC is a collection of strategies that has two features in common. Firstly, MPC uses a model of the system to predict, on-line, future outputs of the system. Secondly, it calculates the optimal future control inputs based on minimizing a cost function with possibly some constraints. To follow a reference, the optimizer in the MPC controller finds the optimal sequence of inputs by minimizing a cost function, possibly taking some constraints into account. A cost function depends on the output of the system and generally also on the input. The optimizer also needs the current states or the past inputs to calculate reliable outputs. After finding the optimal inputs, it is common to only apply the first input. The other inputs are not applied. To find the next input, the calculation starts over again. So every timestep a sequence of inputs is calculated, but only the first input is applied. The optimization of the cost function makes MPC slow, but it is also responsible for the ability to cope with a lot of ‘nasty’ features of some systems. There are a lot of optimization algorithms available to optimize the cost function (linear, quadratic, with constraints, without constraints, etc.). In this project an MPC controller is used to find the optimal path for a robot arm through a maze.
22
23
6. Simulation To test and analyze the behaviour of the Model Predictive Controller, simulations have been performed. The simulations have been performed with a NEC Versa P520 notebook, with a 1.4 GHz Intel Pentium M processor (Centrino) with 512 MB RAM internal memory. The program used for the simulations and programming is MATLAB Version 7.0.4.365 (R14) Service Pack 2. To optimize the cost function, lpsolve is used. This is a standalone linear programming routine written in C, which can be called from Matlab. To analyze the behaviour of the MPC controller, the step size, the cost function, the length of the horizons (time) and the length of the control and prediction horizon (steps) are varied. The behaviour in a constrained (con.) and unconstrained (unc.) environment (fig. 6.2 and fig. 6.3) is also investigated. The constraint environment has narrow passages and corners relatively close together. The unconstrained environment has wider passages and the corners are further apart. Furthermore the path through the unconstrained environment is longer than through the constraint environment and therefore a simulation will need more steps to complete the unconstrained path. The reference to follow is offered in two different ways to the controller. As described in [1], the size of the obstacles should be increased due to discretization (fig. 6.1). However, for ease of computation, the size of the ‘real’ obstacles is varied, so the expanded obstacles are the same size in all simulations. Although simulations with different step size are not in exactly the same ‘real’ environment, the results of the simulations are still useful to compare with each other. Two of the models mentioned in chapter 3 are used for the simulations. These models are used for the ‘real’ system in the simulations, in the remaining of this report referred to as actual system. Two simplified models of the actual system are used as models of the system in the simulations, referred to as model (fig. 6.4 and fig. 6.5). In this way, the effect of model errors can be investigated. All simulations have been performed with the parameters given in Appendix C, unless otherwise stated.
Figure 6.1: Corner cutting due to discretization
24
0 500 1000 1500 2000 25000
500
1000
1500
2000
2500
-500 0 500 1000 1500 2000
0
500
1000
1500
2000
Figure 6.2: ‘Unconstrained’ environment
Figure 6.3: ‘Constrained’ environment
25
10-1
100
101
102
10-5
100
105
|H|
10-1
100
101
102
-3.5
-3
-2.5
-2
-1.5
ang
le(H
)
frequency [Hz]
realmodel
0 0.1 0.2 0.3 0.4 0.5 0.6 0.70
10
20
30
40
50
60
70
80
time [s]
spee
d [
coun
ts/s
]
realmodel
10-1
100
101
102
10-5
100
105
|H|
10-1
100
101
102
-3.5
-3
-2.5
-2
-1.5
ang
le(H
)
frequency [Hz]
realmodel
0 0.5 1 1.50
10
20
30
40
50
60
70
80
time [s]
spe
ed [
coun
ts/s
]
realmodel
Figure 6.5: Bode plot and step response of the z-axis
Figure 6.4: Bode plot and step response of the shoulder
26
27
7. Results
7.1 Reference generation The optimization used in the MPC controller is of the form
)))(((*min UYRabsWu
− ,
with W the weighing vector, R the reference vector and Y the output vector, dependent on input vector U. The optimal values for U are dependant on R, but R can be constructed in various ways. In a lot of applications R is a constant (setpoint) or known in time. In case of trajectory planning, the speed of the system is constant. This is not true for the system considered, R is not constant, not known in time and the speed of the system is not constant. This means there is a freedom in choosing R, but this also imposes the question how to choose R. To investigate this, several simulations have been performed with different ways to calculate R. To calculate the reference, a ‘reasonable measure’ should be used to calculate the distance covered by the system in one time step. The measure is then the maximum velocity of the system. Although the system will not work continuously at this speed (and cannot reach this speed in one time step), this is the only measure known to provide an estimate of the covered distance in one time step. The measure should not be smaller than the maximum speed, because it would slow down the system when it is possible to operate at full speed. Other information used to calculate R is the coarse reference calculated with the Dijkstra-algorithm described in section 4.2. The coarse reference consists of a vector with all succeeding corners visited in the straight line shortest path. Three ways of calculating the reference are considered. (1) The first method calculates the exact future position of the system according to the maximum velocity, current position and the coarse reference. The calculated reference consists of points lying on a straight line between the current position and the next corner, and, if necessary, following straight lines between the last calculated reference before a corner and the next corner. The distance between the reference points is the distance the system can cover in one time step according to the maximum velocity. (2) Another method is to just use the next corner as reference. For example; the next corner is 1.8 steps away from the current position, the second corner 1.3 steps from the first corner and the third corner 4.1 steps from the second corner. Assuming a prediction horizon of 5, the first element of the reference will be corner 1 (floor(1.8) = 1), the next two elements of the reference will be corner 2 (floor(1.8+1.3) – floor(1.8) = 2) and the fourth and fifth element of the reference will be corner 3. (3) With the last method considered, all the elements of the reference vector are the same. The corner following the last corner that can be reached within the prediction horizon will be the reference. Using the example from the previous method, the reference would be 5 times corner 3. Only results of the last two methods are presented. The first method was dropped very early in the project because of the drastic increase in calculation time. It is unknown if the calculations involved were very difficult, or the implementation used was very crude, causing the increase in calculation time.
1 2 3 4 5 6 7 860
80
100
120
140
160
180
prediction horizon [#]
# of
ste
ps
con. 2con. 3unc. 2unc. 3
Figure 7.1: Number of steps needed for complete simulation in different environments
28
1 2 3 4 5 6 7 810
-2
10-1
100
101
102
103
prediction horizon [#]
calc
ulat
ion
time
[s]
W = [1 0 0 0 10-5]
con. 2con. 3unc. 2unc. 3
1 2 3 4 5 6 7 810
-2
10-1
100
101
102
103
prediction horizon [#]
calc
ulat
ion
time
[s]
W = [e-1 e-2 e-3 e-4 e-5]
con. 2con. 3unc. 2unc. 3
Figure 7.1 shows the number of steps that is needed to complete the trajectory for both methods in the constrained and the unconstrained case. As can be seen, the number of steps is equal for both methods, except for the case with a horizon of 1, but this is not much of interest in a practical case. Figures 7.2.a and 7.2.b show the maximum calculation time that occurred in one time step, for different weighting functions. It can easily be seen that the second reference method works as good as or better than the third method, especially for a horizon of more than 3, which is the interesting part of the figure for a practical case. It is worth mentioning that the weighting function in figure 7.2.b performs better and the differences between the two methods are more distinct than with the weighting function in figure 7.2.a. So it is still a question if the second reference method performs better in combination with other weighting functions than the third reference method. However based on these simulations, the second method outperforms the third.
7.2 Stepsize Here the influence of the step size on the time it takes to complete the trajectory is investigated. The influence on the total time to complete the unconstrained trajectory is only minor (fig. 7.3.a). It only decreases a slightly with increasing step size. This is probably due to ‘corner cutting’. However, the total time to complete the constrained trajectory decreases a lot more. This is probably because there are relatively more corners to cut compared to the unconstrained case. Figure 7.3.b shows the longest calculation time for one step that occurs during a simulation. For the unconstrained case, it can be seen that this maximum calculation time is fairly constant. For the constrained case, there is no clear relation between step size and maximum calculation time. Also, the simulation with and without the real system show totally different calculation times.
0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 0.050.88
0.9
0.92
0.94
0.96
0.98
1total time
stepsize [s]
tota
l ca
lc.
time
(sca
led)
con. model onlycon. model/realunc. model onlyunc. model/real
0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 0.051
2
3
4
5
6
7
8
9
10
11max. calc. time per step
stepsize [s]
calc
. tim
e [s
]
con. model onlycon. model/realunc. model onlyunc. model/real
Figure 7.2: Influence of environment and reference method on the maximum calculation time per step for different weight functions
Figure 7.3: Influence of step size on calculation time (a) (b)
29
7.3 Constant horizon (seconds) In these simulations the control and prediction horizon are held approximately constant. Simulations have been performed with step sizes of 0.02, 0.03, 0.04 and 0.05 seconds and horizons (prediction and control) of 8, 5, 4 and 3 steps respectively. As can be seen in figure 7.4.a, the number of steps approximately decreases with 1/stepsize. This implies that the stepsize has almost no influence on the time it takes to complete the trajectory with control and prediction horizons of equal length (in seconds). The calculation time, however, decreases a lot with increasing stepsize, because of a lower dimensional optimization problem (fig. 7.4.b).
0.02 0.025 0.03 0.035 0.04 0.045 0.05
0.4
0.5
0.6
0.7
0.8
0.9
1# of steps
stepsize [s]
step
s (s
cale
d)
con. model onlyunc. model onlycon. model/realunc. model/real1/stepsize
0.02 0.025 0.03 0.035 0.04 0.045 0.05
10-1
100
101
102
103
104
max. calc. time per step
stepsize [s]
calc
. tim
e [s
]
con. model onlyunc. model onlycon. model/realunc. model/real
7.4 Weight function To investigate the influence of the weight function on the performance of the controller, several simulations with different weight functions have been performed. All simulations are done with a control and prediction horizon of 5 steps. For example, the weight function for one direction looks like: [ w(yk+6) w(yk+5) w(yk+4) w(yk+3) w(yk+2) ], with w the weight of the prediction of the output y calculated at time step k. In total, 8 different weight functions have been used, namely: W1 = [1 0 0 0 10-5 ] W2 = [1/3 1/9 1/27 1/81 1/243 ] W3 = [1/e 1/e2 1/e3 1/e4 1/e5 ] W4 = [1/2 1/4 1/8 1/16 1/32 ] W5 = [1/1.5 1/1.52 1/1.53 1/1.54 1/1.55 ] W6 = [1 1 1 1 1 ] W7 = [1/1 1/2 1/3 1/4 1/5 ] W8 = [1 0 0 0 1 ] To make it the most important, w(yk+6) is chosen largest. In this way all previous predictions steps will align in such a way that yk+6 will be closest to the target, which is the fastest way of following the reference. To make the MPC controlled system stable, w(yk+2) is always non-zero, i.e., it will always reach the last reference point. If w(yk+2) = 0, it could 'hang' at a distance from the last reference point, 'reaching it in a few steps' every timestep, over and over again. In the constrained environment it took all simulations 73 steps to complete the trajectory. In the unconstrained environment it took weight functions 1, 2 and 3 169 steps, and the other weight functions 170 steps to complete the trajectory. According to the required steps to complete the trajectories, the first 3 weight functions seem to perform slightly better. Another very important criterion for an MPC controller is the calculation time for one step. The maximum calculation time for one step that occurred is shown in figure 7.5 for both trajectories. It can easily be seen that weight functions 1, 2, 3 and 4 outperform weight functions 5, 6, 7 and 8 by far with the constrained trajectory. With the unconstrained trajectory, however, weight functions 5 and 7 perform the best, although the difference in calculation time isn’t as big as with the constrained trajectory. According to these simulations it is not clear which weight function performs best overall. It seems the performance is very environment dependant and therefore will depend on an empirical choice.
Figure 7.4: Influence of length of horizon (sec) for different step sizes (a) (b)
30
1 2 3 4 5 6 7 81
2
3
4
5
6
7
8
9
10
11max. calc. time per step (constrained)
weight function
calc
. tim
e [s
]
1 2 3 4 5 6 7 82.8
3
3.2
3.4
3.6
3.8
4
4.2
4.4
4.6
4.8max. calc. time per step (unconstrained)
weight function
calc
. tim
e [s
]
7.5 Horizon length Choosing the length of the control and prediction horizon is a very important step in the design of a MPC controller. An increasing horizon decreases the chance of a collision and the optimized trajectory will be better, up to a certain level. However an increasing horizon probably implies an increase in calculation time due to an optimization with more variables. A balanced decision should be made considering the increase in safety and optimized trajectory and the increase in calculation time.
02
46
8
0
2
4
6
8160
170
180
190
200
control horizon
unconstrained model only
prediction horizon
# o
f st
eps
02
46
8
0
2
4
6
8160
170
180
190
200
control horizon
unconstrained model real
prediction horizon
# o
f st
eps
Figure 7.6 shows the results of simulations in the unconstrained environment with the actual system equal to the model and the actual system different from the model respectively. The number of steps to complete the trajectory is a bit higher when using the model and the actual system, but the overall behaviour is about the same. It is clear that a long prediction horizon in combination with a short control horizon does not work well.
02
46
8
0
2
4
6
810
-4
10-2
100
102
control horizon
unconstrained model only
prediction horizon
ma
x. c
alc.
tim
e pe
r st
ep [
s]
02
46
8
0
2
4
6
810
-4
10-2
100
102
104
control horizon
unconstrained model real
prediction horizon
ma
x. c
alc.
tim
e pe
r st
ep [
s]
Figure 7.7 shows the maximum calculation time that occurred during simulations for one time step, for the same simulations as figure 7.6. The behaviour in calculation time is almost the same for simulations with and without the actual system. The calculation time for the simulations with the model and the
Figure 7.5: Calculation time with different weight functions
Figure 7.6: Influence of the prediction and control horizon in an unconstrained environment on the number of steps
Figure 7.7: Influence of the prediction and control horizon in an unconstrained environment on the calculation time
(a) (b)
31
actual system is a bit higher, because the actual system behaves a bit different from the model, and therefore a few extra optimization iterations are needed. The calculation time increases approximately logarithmic with increasing prediction horizon.
02
46
8
0
2
4
6
860
80
100
120
140
160
180
control horizon
constrained model only
prediction horizon
# o
f st
eps
02
46
8
0
2
4
6
850
100
150
200
250
300
350
control horizon
constrained model real
prediction horizon
# o
f st
eps
Figure 7.8 shows the results of simulations in the constrained environment with the actual system equal to the model and the actual system different from the model respectively. Simulations that took a very long time are cut off at a certain time step, causing the flat surface in the corner with the short control horizon and long prediction horizon. In the constrained environment, the behaviour with the model only differs a lot from the behaviour with the model and the actual system. With the model only, the controller does not perform well with a small control horizon and a large prediction horizon only. In all other cases, the results are better, the performance of the system is almost the same then. However, when the actual system and the model are used, the controller does not perform well. It only performs reasonable when the control horizon is equal to the prediction horizon.
02
46
8
0
2
4
6
810
-4
10-2
100
102
104
control horizon
constrained model only
prediction horizon
max
. ca
lc.
time
per
ste
p [s
]
02
46
8
0
2
4
6
810
-4
10-2
100
102
104
control horizon
constrained model real
prediction horizon
max
. ca
lc.
time
per
ste
p [s
]
Figure 7.9 shows the maximum calculation time that occurred during simulations for one time step, for the same simulations as figure 7.8. Although a lot of simulations were not finished, some conclusions can be drawn. The trajectory had a few hard parts to optimize, not all of the simulations were past those parts. The conclusions drawn are based on the simulations that were past the hard parts. The behaviour in calculation time is almost the same for simulations with the model only and the model and actual system together. The calculation time is a bit higher for simulations with different models. Also for this trajectory, the calculation time increases approximately logarithmic with increasing prediction horizon.
Figure 7.8: Influence of the prediction and control horizon in a constrained environment on the number of steps
Figure 7.9: Influence of the prediction and control horizon in a constrained environment on the calculation time
32
33
8. Conclusions and Recommendations The simulations in this report show that it is possible to use MPC for trajectory planning. Programming the different algorithms for the project was time consuming, preventing the implementation of the controller on the actual robot arm. Another project could investigate the practical problems that arise when implementing MPC on a real system. The coarse trajectory planning is able to create a path through a 2D field of objects. For mining purposes the planning should also be able to plan a route through a 3D field. Programming of a 3D field is more complex than that of a 2D field. Programming a 2D field already consumed the available time. Another complex problem is the implementation of turning ellipses instead of turning circles. This will be more realistic in the operation of a mining shovel. The performance of the controller depends a lot on the weight function used in combination with the environment. It is not yet clear what kind of weight function works well in what kind of environment. Furthermore, a good model of the system is needed. With an inaccurate model, performance decreases tremendously, particularly in a constrained environment. Another point of interest when applying MPC is the prediction horizon. Calculation time increases approximately logarithmic with the length of the prediction horizon. To restrict the calculation time, the horizon should not be too long. The weight function (in combination with the environment), model quality and the length of the horizon for a MPC controller are all good subjects for future research.
34
35
References [1] Y.Kuwata. “Real-time Trajectory Design for Unmanned Aerial Vehicles using Receding Horizon Control”. PhD Thesis, (Massachusetts, USA), June 2003. [2] E. W. Dijkstra, “A note on two problems in connexion with graphs” In Numerische Mathematik, 1 (1959), p. 269–271 [3] www.reliability.co.au [4] floppy.org.uk/work/robotics/rt100
36
37
Appendix A: The robot arm used
Fig. A.1 – Schematical overview of the capabilities of the robot arm
38
39
Appendix B: How to connect the robot-arm to a D-Space system Overview of the system:
The elbow and shoulder motor are connected with two high frequency amplifiers, which are connected with the D-space system to generate the control signals. The BNC connectors are labeled on the 40 pins ribbon cable. The motor in z-direction is driven with its own amplifier in the RT100. The signal used to drive this amplifier is pulse width modulated. The BNC connectors for the z-direction are on the 10 pin ribbon cable and are labeled 1, 2 and 3. These are to be connected in the simulink model with speed, direction and IC-power respectively. The speed and direction will be a constant signal, either on or off (TTL). The enable signal consists of a pulse whose width is modulated to make operation at different speeds possible. Pulse width modulation: The pulse width modulation signal is created in Simulink with a saw tooth signal and a comparator. The saw tooth is sampled at 25 kHz. For the saw tooth to have sufficient points, the frequency of the signal has to be much lower than this. Otherwise the signal Simulink generates will be aliased and won’t look anything like a saw tooth. This will make the variation in speed with the PWM-block impossible. Information about the motors: Motor Voltage Output power Shoulder 24 VDC 3 Watt Elbow 24 VDC 3 Watt Z-direction 24 VDC 20 Watt Encoder specification: Axis Endstop to endstop encoder counts Total range. mm. or
degrees. Encoder counts
Z 0 to -3554 948 mm 3554 Shoulder +2630 to -2630 180 degrees 5260 Elbow +2206 to -2630 331 degrees 4836 Connection: The robot-arm has an external connection board for a 40 pins and a 10 pins ribbon cable. To operate the robot with the RS323 connection, the cables which come out of the robot are to be connected. With the external cables the robot-arm can be connected to a D-space system. The encoders of all the motors can be directly connected with the D-space encoder inputs using the 15 pins D-sub connectors. The BNC connectors of the elbow and shoulder connection connect to the amplifiers. The amplifiers are to be connected with the D-Space system. The BNC connectors of the Z motor can directly be connected
Fig. B.1 Schematic connection overview
40
to the D-Space system. Since the driver of the RT100 is used for the motion in z direction, the robot arm should be switched on and the green button should be pressed so the green light is burning. Tips/ pointers: • Keep in mind that D-space will scale its signals in simulink. The in- and output are scaled
between 0 and 1V in stead of 0 and 10V • The encoder counts do not correspond to the specification above. It’s not clear why, but the
scaling is constant. • If the green switch on the RT100 doesn’t light when pressed, the safety stop is most likely
activated • Since the gripper and wrist motor are not connected to the D-Space system, the only way to
initialize the gripper and wrist motor is using the program created by [4].
41
Appendix C: Simulation parameters Prediction horizon 5 [Steps] Control horizon 5 [Steps] Step size 0.03 [Seconds] Weight function [1 0 0 0 1*10-5] [-] Reference calculation Method 2 [-]
42
Appendix D: State space matrices This appendix gives the discrete time state space matrices of the elbow, the shoulder and the z axis of the robot for a step size of 0.03 seconds. Elbow A matrix
-3,25E-01 1,22E-01 1,99E-01 2,90E-01 2,50E-01 3,07E-01 3,26E-01 2,06E-01
-2,00E+00 -1,30E+00 -9,41E-01 -5,83E-01 -2,81E-01 -2,27E-01 -9,02E-02 -3,75E-02
-3,28E-01 -1,16E+00 -1,50E+00 -2,11E+00 -1,40E+00 -1,49E+00 -1,32E+00 -7,96E-01
1,52E+00 6,61E-01 3,24E-01 -4,47E-01 -1,01E+00 -1,11E+00 -1,04E+00 -6,34E-01
1,34E+00 1,03E+00 1,13E+00 1,25E+00 5,18E-01 -5,34E-01 -5,15E-01 -3,14E-01
3,44E-01 3,35E-01 4,63E-01 8,00E-01 8,72E-01 9,02E-01 -9,61E-02 -5,88E-02
6,57E-02 7,51E-02 1,23E-01 2,77E-01 4,48E-01 9,45E-01 9,85E-01 -8,96E-03
1,02E-02 1,32E-02 2,48E-02 6,85E-02 1,46E-01 4,59E-01 9,58E-01 9,99E-01
1,33E-03 1,92E-03 4,09E-03 1,33E-02 3,52E-02 1,47E-01 4,61E-01 9,60E-01
3,79E-05 6,01E-05 1,43E-04 5,37E-04 1,69E-03 8,84E-03 3,69E-02 1,15E-01
1,20E-07 2,07E-07 5,41E-07 2,31E-06 8,48E-06 5,31E-05 2,76E-04 1,15E-03
1,26E-01 1,08E-01 0,00E+00
2,04E-02 3,56E-02 0,00E+00
-4,17E-01 -3,30E-01 0,00E+00
-3,54E-01 -2,90E-01 0,00E+00
-1,79E-01 -1,49E-01 0,00E+00
-3,40E-02 -2,85E-02 0,00E+00
-5,24E-03 -4,41E-03 0,00E+00
-6,83E-04 -5,78E-04 0,00E+00
1,00E+00 -6,58E-05 0,00E+00
2,40E-01 1,00E+00 0,00E+00
3,60E-03 3,00E-02 1,00E+00 B matrix
-2,49E-01
-8,19E-02
7,60E-01
6,69E-01
3,44E-01
6,57E-02
1,02E-02
1,33E-03
1,52E-04
3,83E-06
1,09E-08 C matrix
0,00E+00 2,44E-01 2,14E-01 4,86E-01 4,50E-01 9,02E-01 1,08E+00 1,16E+00
9,36E-01 1,82E+00 2,94E+01
43
Shoulder A matrix
-6,16E+01 -2,76E+01 -1,98E+01 0,00E+00
6,40E+01 0,00E+00 0,00E+00 0,00E+00
0,00E+00 1,60E+01 0,00E+00 0,00E+00
0,00E+00 0,00E+00 2,50E-01 0,00E+00 B matrix
6,40E+01
0,00E+00
0,00E+00
0,00E+00 C matrix
0,00E+00 7,85E-01 4,75E-01 9,49E+01 Z axis A matrix
-3,85E+01 -7,45E+01 -5,44E+01 0,00E+00
6,40E+01 0,00E+00 0,00E+00 0,00E+00
0,00E+00 3,20E+01 0,00E+00 0,00E+00
0,00E+00 0,00E+00 1,25E-01 0,00E+00 B matrix
2,56E+02
0,00E+00
0,00E+00
0,00E+00 C matrix
0,00E+00 1,28E-01 5,16E-02 1,34E+02