Optimization of Robot Trajectory with Motion Constraints

Optimization of Robot Trajectory with Motion Constr

aints

Viktor Michna*, Petr Wagner** Zdenek Machacek*** Department of Measurement and Control, Faculty of E lectrical Engineering and Computer Science, VSB – Technical University of Ostrava, 17. listopad u 15, Ostrava-Poruba 708 33, Czech Republic (*[email protected], **[email protected], ***zd [email protected]) Abstract: This paper deals with time-optimization of traject ories of wheeled robots within the speed and other constraints. The cubic Hermite spline curve w ith method of speed profile computation is used to determine the trajectory. This method is extended t o allow the optimization with the constraints described. It ensures fulfillment of required initi al parameters of motion. There is analyzed both sit uation without obstacle and with single stationary obstacl e. Neural network is then introduced to accelerate the calculation of numerically computed optimal results . Keywords: Mobile robots, Trajectory planning, Splines, Param Neural networks.

eter optimization, Obstacle avoidance,

with solving the trajectory for single robot, the s olution of optimization task for cooperating robots can be fou nd in Requirements of optimal control have constantly eme rged(Glocker, 2006). in lots of current applications. The typical area i s the control 2. CALCULATION OF TRAJECTORY of robotic movement. There are many optimality crit eria depending on the type of application, e.g. trajecto ry passage2.1 Trajectory defined by Hermite spline in minimum time with respect to physical constraint s or with minimum energy consumption. Another kind of require mentsThe Hermite spline curves showed useful to plan rob ot can be real-time control and fulfilment of desired time limits. trajectory. These curves consist of the third order polynomial One of the attractive and utilizable tasks is the r obot soccer and are defined using four control points. The firs t two are (Srovnal, 2004). the origin and the target points and the other two are tangents ve in origin This paper continues with recent research of planni ng and(or control vectors) defining derivation of the cur and target point. More complex trajectories shall c onsist of control of the motion of wheeled robots. Current co ntrol is based on use of feedback and feedforward controller alongmore segments of the Hermite spline. 1. INTRODUCTION

given trajectory. The controller requires exact inf ormationGeneral definition of cubic curve in parametric for m with about the position of robot. The position and speed is filtered parameter u ˛ 0;1 is given by: and predicted using discrete form of extended Kalma n filter (Wagner, 2010a). The prediction is important for x(u ) = a3u 3 + a2u 2 + a1u + a0 (1) compensation of transport delay caused mainly by th e chainy(u ) = b u 3 + b u 2 + b u + b 3 2 1 0 of image processing and wireless transceiver. There is also a of Hermite spline are determined using t his Parameters possibility to move some of the signal processing i nto the formula: robot to reduce the reaction time (Machacek, 2005). The Hermite splines are used for trajectory plannin g with a 3 b3   2 - 2 1 1   x 0 y 0        (2) real-time recomputation of the trajectory. The most important a 2 b2  = - 3 3 - 2 - 1  x1 y1  task when using splines is the calculation of speed profile. a1 b1   0 0 1 0  cx 0 cy 0  This profile ensures the meeting of physical constr aints likea 0 b0   1 0 0 0   cx1 cy1  maximum speed and acceleration for the chosen splin e where p0 = x0+i y0 and p1 = x1+i y1 are the coordinates of the parameters (Wagner, 2010b). origin and the target points and c0 = cx0+i cy0 and c1 = cx1+i cy1 are the tangents determining direction of the First performance goal is minimal time of the robot motion curve through the origin and the target point respe ctively. between 2 points. Second goal is avoiding of the so lid and moving obstacles i.e. playground area and another r obots.The tangents are in relation with origin v0 and target v1 speed This paper deals with exploration of parameter opti mizationof the robot: of existing solution to achieve the minimum motion time and , . (3) it outlines possible use of existing solution to so lve thearg(c 0 ) = arg( v 0 ) arg(c1 ) = arg( v1 ) problem of obstacles as well. The problem of avoidi ng obstacles is solved frequently (Haddad, 2005). We w ill deal

2) Calculation of the maximum speed vmaxArad(u) assuming that the tangential component is zero.

v0 v1

a

3) Location of N local minimums of vmaxArad(u); the robot tangentially accelerates before and after them; cal culation of maximal speeds with tangential acceleration vk [n]max(u).

g

L Fig. 1. Diagram of the Hermite spline based traject the initial motion parameters.

4) Determination of resulting speed: vbase(u) = min(vmaxArad(u), vk [1]max(u), ..., vk [N]max(u), vmax) Now we can modify this algorithm by including addit ory and steps (Michna, 2010):

(5) ional

5) Calculation of the angle speed on the curve: 2.2 Calculation of the speed profile

w

max V

(u ) = vbase (u ) k

(6)

(u )

The relationship between the parameter u ˛ 0;1 and time

6) Localization of N local minimums uwk [n] of angle speed m. ed. wItmaxV(u); the robot angle accelerates before and after the

must be determined first before the curve can be us corresponds to calculation of the speed profile. Th e speed7) Calculation of profile is described by function v(u) and it defines maximal speed of the robot in given trajectory point. Great er speedw k [ n ] max = t (u ) - t (uwk [ n ] max ) e max could lead to the skid of the robot or the robot ma y notwhere be t(u) is time profile derived from the speed profile: able to reach the required speed at all. In this me aning, calculated speed profile is time optimal with regar d to D x (u ) 2 + D y (u ) 2 D t (u ) = selected control vectors. v (u ) Detailed description of calculation of the speed pr ofile is in8) Determination of resulting angle speed: (Wagner, 2010b), here we mention only the constrain t w (u) = min( w maxV(u), w k [1]max(u), ..., w k[N]max(u), w parameters of the robot motion. These are maximum r obot speed vmax, maximum tangential acceleration atan, maximum 9) Determination of new resulting speed radial (normal) acceleration arad, maximum angle speed w max, w (u ) v final (u ) = maximum angle acceleration e max. k (u )

(7)

(8)

max)

(9) (10)

Parameters are limited by drive of the robot, const ruction, 3. OPTIMIZATION mass, inertia and moment of inertia of the robot an d its as disposition to skid. The limits for currently used robots are The algorithm of calculation of the speed profile w presented briefly in previous section. From the tim e profile vmax = 1500 mm/s, (8), we get the total time of the robot motion alon g the given atan = 1500 mm/s2, 2 arad = 2000 mm/s , w max = 40 rad/s, e max = 60 rad/s2. 1.9 The last two mentioned parameters are not introduce d in 1.85 (Wagner, 2010b) and therefore they are not included in algorithm of speed profile calculation. It does not necessary 1.8 lead to problems if the tangents are chosen by simp lified 1.75 method because the curves do not have large local c urvature: L 2

(4)

For the purpose of time optimization of the curves, we need to upgrade the calculation of the speed profile by including constraints resulting from the angle speed w max and the angle acceleration e max. Otherwise, the most time optimal curve would be in the case c0=c1=0 that corresponds to immediate rotation of the robot in the origin and target poin ts. The basic calculation described in (Wagner, 2010b) performed in discrete steps D u when the curve is divided into sufficient count of segments (e.g. for 100 segments D u = 0.01): 1) Calculation of the curvature k (u).

is

Cost J (s)

1.7

c0 = c1 =

1.65 1.6 1.55 1.5 1.45 1.4 0

0 500

1000 1000

1500

Tangent length c 1 (mm)

2000 2000

2500

3000

Tangent length c 0 (mm)

Fig. 2. Plot of cost function depending on the tang ent lengths for L=1500 mm, v0=v1=750 mm/s, a =p /4, g =p /8.

trajectory. This describes the cost function as wel 1

J (c 0 , c1 ) = ∫ 0

x(u ) + i y (u ) v(u )

¢

du = t (u = 1)

l:

2200 2000

(11)

1800 Optimal tangent length (mm)

Time optimal parameters c0 and c1 are dependent on the parameters of motionDDD p=p1-p0, v0 and v1. These conditions ters): can be re-written into these scalars (motion parame (12)

L = ∆p

a = arg( v 0 ) - arg(∆p)

g = arg( v1 ) - arg(∆p)

v 0, v 1.

1600 1400 1200 1000 800 600 400

Our goal is to minimize the cost function J and therefore to get the minimum possible total time of motion. The example of such function is in the Fig. 2.

c0

200 0

c1 0

500

1000

1500 2000 Distance L (mm)

2500

The constraint can be described by the equation: err (c 0 , c1 ) =

v0 v1 + - 2=0 v(u = 0) v(u = 1)

(13)

Consequently, any c0, c1 pair must first satisfy this equation and after it may be used for the time optimization J(cof 0,c1). The function err(c0,c1) results practically from the same the process of the speed profile calculation. Note that err(c0,c1) is always greater or equal to zero, because the calculated speed (by the speed profile calculation process) is always lower or equal than the desired initial or t arget speed.

Optimal tangent length (mm)

However, we must include the constraints. The metho d of the speed profile calculation can not guarantee that th e initialFig. 3. Plot of time-optimal tangent lengths depend speed v0 and the target speed v1 are always reached for any the distance. given lengths of tangents c0 and c1. This happens when the 6000 given trajectory does not provide enough space for robot to c0 decelerate in the beginning or accelerate in the en d. The c1 problem is present when using the simplified method of the 5000 tangent selection (4).

3000

2000

1000

0

0

500

1000

1500

Speed v0 (mm/s)

3500

We restrict the problem to a special case a >=0, g >=0, v1=v0. We will be searching time-optimal tangent length c0 and c1 by minimizing the cost function:

3000

c  c =  0  c1 

(14)

For the optimization we are using the solver fmincon available in MATLAB. It attempts to find a constrai ned minimum of a scalar function of several variables s tarting at an initial estimate. The nonlinear equality constra int is specified in the form of err(c0,c1) function (13). We choose the bounds of c0 and c1: c0 , c1 ˛ (0;4 L

and the initial estimate according to (4).

(15)

Optimal tangent length (mm)

3.1 Simplification of the problem

c

ing on

4000

It would be very painful to derive an analytic rela tionship of c0 and c1 taking all motion parameters L, a , g , v0, v1 into Fig. 4. Plot of time-optimal tangent lengths depend account. We have decided to solve this optimization problemthe speed. numerically.

min J (c)

3000

ing on

2500

2000

1500

1000 c0

500

0

c1 0

0.5

1

1.5 Angle a (rad)

2

2.5

Fig. 5. Plot of time-optimal tangent lengths depend the origin angle.

3

ing on

Let the test task has these motion parameters: L = 1500 mm v0=750 mm/s a =p /4 g =p /8

2.2 2 1.8

Now we examine the dependence between time-optimize d parameters c0, c1 and motion parameters L, v0, a for chosen initial conditions. The behaviour of optimal tangen t lengths depending on the consequent change of distance L, speed v0 and angle a is shown in Fig. 3, Fig. 4 and Fig.5 respectively.

Cost J (s)

1.6 1.4 1.2 Simplification Time-optimized Neural network

1 0.8 0.6

3.2 Use of neural networks

0.4

We can use neural network to obtain optimal paramet ers 0.2 0 500 1000 1500 2000 faster than when using numerical searching methods. For this Distance L (mm) purpose, we will design the feed-forward neural net work with back-propagation and neural transfer functions purelin and Fig. 7. Plot of the total motion time (value of the tansig with 20 and 10 neurons in hidden layers respective ly function) depending on the distance. (Fig. 6).

2500

cost

2.8 Simplification Time-optimized Neural network

2.6 2.4 2.2 Cost J (s)

Fig. 6. Structure of neural network.

For this network, the training set was generated in space L˛ (150;2500〉 · v0˛ (0;1500〉 · a ˛ 〈0;p ) · g ˛ 〈0;p ) with size of 9000 elements. The process of train set generati on took almost 3 hours of computing in MATLAB on the PC wit h 2 GHz CPU.

2 1.8 1.6 1.4

The network was trained by the Levenberg-Marquardt algorithm (trainlm) afterwards. Because the speed profile is time reversible, we ca symmetrically calculate both tangent lengths c0(L,v0,a ,g ) and c1(L,v1,g ,a ) with only one network output (Michna, 2010).

0

500

1000

1500

Speed v0 (mm/s)

n Fig. 8. Plot of the total motion time (value of the function) depending on the initial and the target s

3.3 Comparison of used methods

2.6

Now we compare results of each used method – simpli fied method (4), time-optimized method and neural networ k with learned optimal results. For the inspection we shal l choose the same motion parameters as previous.

2.4

cost peed.

Cost J (s)

2.2

For each method, the total time of motion of the ro bot on the computed trajectory depending on the change of the parameters L, v0 and a is shown in Fig. 7, Fig. 8, Fig. 9 respectively.

2

1.8

1.6

There are several outcomes. The optimal solution ha s up to Simplification Time-optimized 0.2 s shorter total time of motion than the simplif ied method, 1.4 Neural network but it’s slower for some of the motion parameters v alues. This is caused by the mentioned speed constraint (1 3). The 0 0.5 1 1.5 2 2.5 Angle a (rad) characteristics obtained by use of the neural netwo rks are very close to the optimal solution and therefore us able for the Fig. 9. Plot of the total motion time (value of the speed-up of the trajectory calculation for this tas k. function) depending on the origin angle. The profile of computed trajectory is in Fig. 10, t he speed profile is in Fig. 11.

3

cost

Motion parameters

200 y (mm)

100 0 -100 -200

0

v0

A

Simplification Time-optimized Neural network 500

1000

LC

a L

Robot

1500

B

LA

C

-g

Obstacle

x (mm)

Fig. 10. Plot of the trajectory. 1600

v1

cB

Trajectory parameters

b B

c0 1400

A

C

Speed v0 (mm/s)

1200

c1

1000

800 Simplification Time-optimized Neural network

600

400

0

0.2

0.4

0.6

0.8 Time t (s)

1

1.2

1.4

Fig. 12. Diagram of the motion situation and its pa and trajectory parameters with single obstacle.

rameters

one bypassing point, the tangents have to be comput all at once or in sequence with partial approximati case, we have so far one bypassing point with the t

ed either on. In our angent cB .

1.6

The situation is described by the same motion param eters as earlier – distance L, initial speed v0, a and target speed v1, g Fig. 11. Plot of the speed profile. and they include new distances LA, LC. To calculate ters – trajectory, we need the values of trajectory parame Although the optimal trajectory is longer, the tota l time oflengths of the tangents c , c , c and the direction b in the 0 1 B motion is less than for the non-optimal trajectory. passing point B. These parameters are determined by the time The optimal solution reaches the required initial a nd targetoptimization within constraints mentioned in 2.2. S imilarly, speed 750 mm/s. we can calculate the speed profile from these param eters. The difference is the range of curve parameter u: u˛ 〈0;2〉. This is 4. EXTENSION OF OPTIMIZATION TASK because the curve now consists of two segments whic h are We can now use gathered results to generalize the t ask of joined in u=1. Thus the cost function is once again derived trajectory optimization. from the time profile as the total time of motion 4.1 Description of general task

J (d) = t (u = 2)

d = [c 0

c1

cB

b

]T

Extensions to the already mentioned task include us e of and the function is minimized independent motion parameters and obstacles. Obstac les can be both stationary and non-stationary (Michna, 2010 ). min J (d)

(16)

(17)

d

At first we focus on the situations with simpler ca se with with nonlinear equality constraint stationary obstacles in the path of robot. Diagram of this case v0 v1 is in the Fig. 12. The first diagram shows the situ ation and the err (d) = + - 2=0 v ( u = 0) v ( u = 2) motion parameters. We want the robot to move from t he point A to the point C but there is the obstacle in the path. Thus the robot has to bypass the obstacle. The solu tion to this 4.3 Example of solution task includes two steps – selection of bypassing po int B and calculation of optimal trajectory leading through t he point B. Let choose these motion parameters: Selection of position of the B point is performed w to the size of the robot and obstacle. It is behind this paper and requires a further research. 4.2 Calculation of optimal trajectory

L = 1500 mm ith respect the scope Lof A = 1000 mm LC = 600 mm v0 = 1000 mm/s v1 = 600 mm/s a = p /4 Hermite points sog = p /4

The trajectory is described by the piecewise cubic spline curves. Its segments are joined in bypassing that the adjacent segments share the same tangents. We can have generally several bypassing points. If there i s more than

(18)

C

1500

LC

1000

L X (mm)

X (mm)

A

0

B

1000

LA

500

0

LC

B L

200 Y (mm)

LA 500

400

0

A0

200 Y (mm)

Fig. 13. Plot of the a) optimal, b) simplified traj its parameters with single obstacle. 1400

motion parameters) and fits all the requirements. F or more than one bypassing points and therefore more trajec tory parameters, it seems useful to use genetic algorith ms to numerically compute the optimal results (Tian, 2004 ).

C

1500

5. CONCLUSIONS In this paper we have explored some of the possibil ities of optimization of the robot motion. We focused on tra jectory planning by Hermite splines. First situation was op timization of the motion without obstacles. For this purpose, neural network was used to speed up the time of calculatio n of trajectory parameters. It was verified that neural network has similar results as optimal solution and thus it can be used in this application. Second situation was optimization of the motion with one stationary obstacle. We have manage d to extend the basic task to the task with single obsta cle and to minimize the total time of motion while meeting the speed 400 constraints and requirements. In the future, we wil l focus on the design of the quick way to compute the trajecto ry ectory and parameters and also to examine performance in realtime mode. Possible solution is the use of neural networ ks, but as the number of motion parameters is increasing, we m ust figure out how to reduce the size of the resulting training set. ACKNOWLEDGEMENT

1300 1200

Speed (mm/s)

1100 1000 900 800

Simplification Time-optimized

This work is being solved on the VSB-Technical Univ ersity of Ostrava, Czech Republic. This project is support ed by the GAČR project 102/08/1429 and also by the internal gran t SP/2010165 „Development and using new methods for design and realization of embedded systems”. REFERENCES

Glocker, M. et al. (2006). Optimal task allocation and dynamic trajectory planning for multi-vehicle syste ms using nonlinear hybrid optimal control. In Proceedings 500 of the 1st IFAC-Symposium on Multivehicle Systems, pp. 38-43. 400 0 0.5 1 1.5 2 2.5 Haddad, M. et al. (2005). A New Approach for Minimu m Time (s) Time Motion Planning Problem of Wheeled Mobile Robots. Proceedings of the 16th IFAC World Congress, Fig. 14. Plot of the speed profile with single obst acle. Vol. 16, No. 1, pp. 307-312. At first we use the simplified method for tangent l ength Machacek, Z., Srovnal, V. (2005). Automated system for data results. selection for subsequent comparison with optimized measuring and analyses from embedded systems. In 7th The cB tangent length corresponds to the Catmull-Rome International Conference on Automatic Control, spline. Modeling and Simulation, pp. 43-48. WSEAS, Atheny. Michna, V. et al. (2010). Constrained Optimization of Robot c0 = c1 = cB = 0.5 L = 750 mm Trajectory and Obstacle Avoidance. 15th IEEE b =0 International Conference on ETFA 2010. Mobile Now, we can use the mentioned optimization procedur e inSrovnal, V. et al. (2004). Strategy Extraction for Embedded Control Systems Apply the Multi-Agent MATLAB with fmincon solver. The result trajectory Technology. In Lecture Notes in Computer Science, parameters are: Vol. 3038, pp. 631-637. Springer-Verlag, Berlin. c0 = 0.8035 L = 1205.3mm Tian, L., Collins, C. (2004). An effective robot tr ajectory c1 = 0.4889 L = 733.4 m planning method using a genetic algorithm, c2 = 0.6488 L = 973.2 m Mechatronics, Vol. 14, No. 5, pp. 455-470. b = -0.4049 rad Wagner, P. et al. (2010a). Control of Group of Robo ts. 9th In RoEduNet International Conference. We can see the comparison between trajectories for both Wagner, P. et al. (2010b). Path Planning and Tracki ng for cases in Fig. 13. The same comparison for the speed profiles Robots Based on Cubic Hermite Splines in Real-Time. is in Fig. 14. The optimized trajectory leads to le sser total time of motion (this does not have to be true for g eneral 15th IEEE International Conference on ETFA 2010. 700 600