Time-Optimal Trajectory Planning Along Predefined Path for Mobile Robots with Velocity and Acceleration Constraints

Transcription

1 Time-Optimal Trajectory Planning Along Predefined Path for Mobile Robot with Velocity and Acceleration Contraint Mišel Brezak and Ivan Petrović Abtract Thi paper i concerned with the problem of finding an optimal velocity profile along the predefined path in order to travere the path in hortet time. A dynamic model of the mobile robot that account for robot actuator contraint i derived. It i hown how to ue thi dynamic model to expre acceleration limit and velocity limit curve required by the optimal time-caling algorithm. The developed trajectory planning algorithm i demontrated on the Pioneer 3DX mobile robot. A method i very flexible and can be extended to account for other contraint, uch a limited grip between the robot wheel and the ground. I. INTRODUCTION The field of mobile robotic i an active reearch area with promiing new application domain in indutrial a well a in ervice ector. Mobile robot are epecially appropriate in application where flexible motion planning i required. There are many robot navigation trategie, and in cae where operating environment map i known, trajectory planning i commonly ued. The term trajectory denote the path that robot hould travere a a function of time. Trajectory planner generate the appropriate trajectory with goal of arriving at a particular location, patrolling trough pecified area etc, and at the ame time avoiding colliion with different kin of obtacle. To obtain feaible trajectory, that i, the trajectory that i robot actually able to track, the planner alo nee to conider variou robot phyical and dynamic limitation uch a it velocity and acceleration limit. A trajectory can be generated in realtime on the bai of current enor reading or generated in advance on the bai of operating environment map. A priori trajectory planning, unlike reactive approache, reult in determinitic and predictable motion, which i deired for automated guided vehicle (AGV), fork lift, intelligent tranportation ytem and many other robotic application. Trajectory planning problem can be olved uing two approache: () the direct approach, where the earch i performed directly in the ytem tate pace or (2) the decoupled approach, where firt a path in the configuration pace i found and then a velocity profile ubject to the actuator contraint i computed. In thi paper the decoupled approach i ued, mainly due to it computational efficiency. Decoupled approach can alo be referred a trajectory planning problem contrained to a precomputed path. Thi reearch wa upported by the Minitry of Science, Education and Sport of the Republic of Croatia (grant No ). Mišel Brezak and Ivan Petrović are with Univerity of Zagreb, Faculty of Electrical Engineering and Computing, Zagreb, Croatia miel.brezak, ivan.petrovic}@fer.hr An algorithm that olve problem of moving a manipulator in minimum time along a pecified geometric path ubject to input torque/force contraint i decribed in [], [2]. The algorithm i primarily ued for offline trajectory planning for tatic manipulator. However, in thi paper we how that on modern hardware it i alo poible to ue it for online motion planning of a mobile robot. In [3] a imple velocity planning for autonomou fork lift truck i ued, however it i not capable of accounting for more complex actuator contraint, uch a wheel lipping prevention. Another example i trajectory planning which i pecifically deigned to deal with motor current and battery voltage contraint [4]. A contribution of thi paper i development of the dynamic model of mobile robot that account for velocity and acceleration limit and enable expreion of acceleration limit and velocity limit curve. Uing thi model, the algorithm for time-optimal velocity planning along predefined path ubject to given contraint i uccefully implemented and ued in real time. Thi paper i organized a follow. In ection 2 a timeoptimal velocity planning algorithm i introduced. In ection 3 the time-optimal velocity planning algorithm i applied for the middle ize mobile robot and the correponding dynamic model i derived. In ection 4 experimental reult are preented. The paper en with concluion and idea for future work. II. TIME-OPTIMAL VELOCITY PLANNING A path q() can be defined a a twice-differentiable curve in the configuration pace q() :[, g ] C, that map ome path parameter to a curve in configuration pace C, where g i path parameter at goal configuration. Path may be found by e.g. roadmap or ampling baed metho (ee [5] for path planning metho overview). To pecify a trajectory, we firt introduce a time caling function (t) a (t) :[,t g ] [, g ], which aign a value to each time t [,t g ].The time caling (t) i aumed to be twice-differentiable and monotonic, i.e. (t) >, t (,t g ), where (t) denote time derivative of (t). The twice-differentiability of (t) enure that the acceleration i well defined and bounded. Uing both path and time caling, a trajectory can now be defined a q((t)):[,t g ] C, () which i hort-written a q(t). We aume that a mobile robot i operating in a planar workpace, o that it configuration q(t) i given by poition coordinate (x(t),y(t)) and orientation θ(t), i.e. q(t) =[x(t) y(t) θ(t)] T. In thi cae the requirement for

2 twice differentiable path tranlate to the requirement for continuou path curvature. Additionally, if the motion i to be planned for a nonholonomic mobile robot, the planned path mut alo atify nonholonomic contraint of the particular robot. We chooe that the path parameter denote the ditance traveled along the path in the cae that robot motion ha tranlation component. In thi cae longitudinal velocity v(t) i non-zero (i.e. v(t) ) and longitudinal and angular velocitie can be expreed a function of v(t) =(t), ω(t) =κ((t))(t), (2) where ω(t) i angular velocity and κ() i igned path curvature. By differentiating (2), we obtain acceleration v(t) = (t), ω(t) = dκ() (t) 2 + κ((t)) (t). (3) In cae of pure rotation, i.e. v(t) =and ω(t), will denote the travered angle. Then the velocitie are and acceleration v(t) =, ω(t) =(t), (4) v(t) =, ω(t) = (t). (5) The motion planning problem i olved uing the decoupled approach. Therefore, the trajectory planning module take a mooth path q() a an input, and trie to find the fatet feaible trajectory that follow thi path. The term fatet actually denote the time-optimal caling (t), wherea the term feaible refer to the actuator limit and poibly other external contraint. The appropriate optimal time-caling algorithm for our purpoe i developed by Dobrow et al. [2] and Shin and McKay [], ee alo [5]. To make the paper motly elfcontained we give a hort decription of the algorithm. Let a general dynamic model of the robot be given by u = M(q) q + q T Γ(q) q + g(q), (6) where u i the vector of generalized force acting on the generalized coordinate q, M(q) i an n C n C ymmetric, poitive definite ma or inertia matrix, Γ(q) R nc nc nc can be viewed a an n C -dimenional column vector, where each element i a matrix whoe element are Chritoffel ymbol of the inertia matrix M(q), and g(q) R nc i a vector of gravitational force. The developed force are ubject to the actuator limit u min i (q, q) u i u max i (q, q). (7) In the mot general form the actuator limit are expreed a function of the robot configuration and velocity. An example may be the torque available to accelerate a DC motor, which decreae a it angular velocity increae. By expreing a path q a a function of parameter, (6) can be written more compactly a the vector equation a() + b() 2 + c() =u, (8) ( lim, lim) U(, ) L(, ) (, ) Velocity limit curve V () A A A 2 ( tan, tan) max min 2 max 3 min F (, g) Fig.. An illutration of the optimal time-caling algorithm. The feaible acceleration at point on the velocity limit curve are hown a arrow. which define robot dynamic model contrained to the path q(). The vector function a(), b(), and c() are inertial, velocity product, and gravitational term in term of, repectively. Becaue the robot motion i contrained to the path q(), it tate at any time i determined by (, ). Now the actuator limit can be expreed a a function of (, ), yielding lower limit u min (, ) and upper limit u max (, ). From equation (8), we conclude that the ytem mut atify the contraint u min (, ) a() + b() 2 + c() u max (, ). (9) Thi equation enable u to expre the minimum and maximum acceleration a function of parameter (, ), which are required to obtain time-optimal caling function. We denote the minimum and maximum acceleration atifying the i-th component of equation (9) by L i (, ) and U i (, ), repectively. We additionally define overall acceleration limit a L(, ) = max L i (, ), U(, ) = min U i (, ), i...n C i...n C () the actuator limit (9) can now imply be expreed a L(, ) U(, ). () The problem of finding the time-optimal trajectory contrained to a path i defined a: given a path q() :[, g ] C, an initial tate (, ), and a final tate ( g, g ), find a monotonically increaing twice-differentiable time caling (t) :[,t g ] [, g ] that (i) atifie () =, () =, (t g )= g, (t g )= g, and (ii) minimize the total travel time t g along the path while repecting the actuator contraint () for all time t [,t g ]. The problem i bet viualized in the (, ) tate pace. The feaible acceleration contraint () can be illutrated a a cone of tangent vector defined at any tate (, ). Then the lower edge of the cone correpon to the minimum acceleration L(, ), while upper edge correpon to the maximum acceleration U(, ). The interior of the cone correpon to a range of feaible acceleration at the tate (, ) along the path o that L(, ) U(, ). One uch cone i illutrated in Fig..

3 The problem i now to find a curve from (, ) to ( g, g ) uch that and the curve tangent i inide the cone at each tate. Further, to minimize the travel time, the velocity along the path hould be maximized. Thi can be obtained by maximizing the area beneath the curve. A conequence of thi i that the curve alway follow the upper or lower bound of the cone at each tate, i.e. the ytem alway operate at minimum or maximum acceleration. The problem i now to find an alternating equence, i.e. the witching point between maximum and minimum acceleration. Actuation contraint () impoe that there could be ome tate at which there i no feaible acceleration that i required for the ytem to continue to follow the path. Thi region i depicted in gray in Fig. and i called inadmiible region. If the robot tate i in thi region, it will leave the path immediately. But even if robot tate i in admiible region, robot may till be doomed to leave the path, no matter of the elected command acceleration. Thi occur if the range of admiible acceleration i directed to the inadmiible region. Here it i aumed that, for any, the robot i trong enough to maintain it configuration tatically, o all = tate are admiible and the path can be executed arbitrarily low. It i alo aumed that a increae from zero for a given, there will be at mot one witch from admiible to inadmiible, which occur at the velocity limit curve V (), coniting of tate (, ) atifying L(, ) =U(, ). (2) Becaue of min( ) and max( ) function ued in calculu of L(, ) and U(, ), thee function, and velocity limit curve are generally not mooth. However, in ome pecial cae the equation (2) may have multiple olution for a ingle value of. Thi may be due to friction in the ytem or weak actuator. In thi cae, there may be inadmiible ilan in the phae plane. Thi complicate the problem of finding an optimal time caling, and the olution i given in []. The algorithm that give optimal time caling, i.e. the optimal equence of value where the witching between maximum and minimum acceleration hould occur, i given by the following algorithm [5]: ) Initialize an empty lit of witche S = } and a witch counter i =. Set ( i, i )=(, ). 2) Integrate the equation = L(, ) backward in time from ( g, g ) until the velocity limit curve i penetrated (reached tranverally, not tangentially), =, or = at < g. There i no olution if =i reached. Otherwie, call thi phae plane curve F and proceed to the next tep. 3) Integrate the equation = U(, ) forward in time from ( i, i ). Call thi curve A i. Continue integrating until either A i croe F or A i penetrate the velocity limit curve. (If A i croe = g or =before either of thee two cae occur, there i no olution.) If A i croe F, then increment i, let i be the value at which the croing occur, and append i to the lit of witche S. The problem i olved and S i the olution. If intead the velocity limit curve i penetrated, let ( lim, lim ) be the point of penetration and proceed to the next tep. 4) Search the velocity limit curve V () forward in from ( lim, lim ) until finding the firt point where the feaible acceleration (L = U on the velocity limit curve) i tangent to the velocity limit curve. If the velocity limit i V (), then a point (,v( )) atifie the tangency condition if dv = = U(,v( ))/v( ). Call the firt tangent point reached ( tan, tan ).From ( tan, tan ), integrate the curve = L(, ) backward in time until it interect A i. Increment i and call thi new curve A i.let i be the value of the interection point. Thi i a witch point from maximum to minimum acceleration. Append i to the lit S. 5) Increment i and et ( i, i )=( tan, tan ).Thiia witch point from minimum to maximum acceleration. Append i to the lit S. Gototep3. An illutration of the time-caling algorithm i hown in Fig.. Although the decribed optimal time-caling algorithm i general, to ue it we have to model actuator limit (), which are pecific for each particular robot type. One drawback of planning at maximum acceleration i that it doe not leave any maneuver pace to recover from eventual trajectory tracking error. Such an error can be caued by many factor, e.g. a non-flat floor, which can be characterized a an unpredictable external diturbance. A poible olution i to to leave ome acceleration reerve and work with lower acceleration than the actual maximum. III. TRAJECTORY PLANNING FOR MOBILE ROBOT WITH VELOCITY AND ACCELERATION CONSTRAINTS Here we derive a time-optimal caling algorithm pecifically for robot that have fixed limit on maximum longitudinal and angular velocitie and acceleration. It i thu aumed that a robot ha wheel-driving motor that are trong enough to attain both maximum longitudinal and angular acceleration imultaneouly. A typical example of uch robot i the Pioneer 3DX mobile robot hown in Fig. 2, whoe maximum allowed acceleration can be cutomized in robot firmware. The Pioneer 3DX robot will therefore be ued thorough thi paper for algorithm derivation and experiment. Thi i a nonholonomic mobile robot with the differential drive that ha two driving wheel and a cater (paive) wheel. An aumption of independent maximum longitudinal and angular acceleration may not be fulfilled if the uer chooe too high acceleration and velocity limit, however, here we do not tudy uch cae. The command input to the Pioneer 3DX robot are referent longitudinal and angular velocitie. However, dynamic model given by equation (6) require ue of generalized force a the command input. Therefore longitudinal acceleration a and angular acceleration α (which are proportional to the correponding force) will be ued a component of generalized force vector u, i.e. u =[aα] T. Final output of

4 Fig. 2. Pioneer 3DX mobile robot. the trajectory planning algorithm will be the velocity profile a the function of time. Normally, robot configuration i decribed by the vector [x y θ] T, where (x, y) denote robot poition, and θ it orientation. However, with thi choice it i not poible to directly ue decribed time-caling algorithm ince we have to control three variable while there are only two component of the command input vector thi i a conequence of the nonholonomic contraint. Neverthele, the velocity pair [v ω] T can be ued to decribe robot tate, o that dynamic model of the robot can be written a [ ] v = u. (3) ω Although the tate vector [v ω] T doe not anwer u about the robot configuration directly, thi information can be obtained implicitly through the time hitory of the velocity vector. In thi way the configuration can be computed if an initial configuration [x y θ ] T, time hitory of the velocity vector and kinematic model of the robot are known. Kinematic model of the differential drive robot i given by ẋ co θ [ ] ẏ = in θ v. (4) ω θ Additionally, the nonholonomic contraint that come from the fact that the driving wheel can only roll, but not lip, mut be fulfilled: ẋ in θ +ẏ co θ =. (5) For v we can ubtitute (2) and (3) into dynamic model (3), obtaining [ ] [ ] [ ] + 2 a =. (6) κ() α dκ() From thi model, and uing analogy with (8) we have the following limit due to limited longitudinal acceleration L (, ) =a min (, ), U (, ) =a max (, ), (7) where the minimum longitudinal acceleration i nonlinear function given by āmin, ( v a min (, ) = max, v max ), (8), otherwie where ā min i a negative contant that denote the minimum longitudinal acceleration, while v max i the maximum abolute longitudinal velocity of the robot. The maximum longitudinal acceleration i given imilarly by āmax, ( v a max (, ) = max, v max ), (9), otherwie where ā max i a poitive contant denoting the maximum longitudinal acceleration. It i uually ā max = ā min, although ome electronic motor driver achieve ā min > ā max, i.e. they enable fater braking than accelerating. From robot dynamic model (6), the acceleration contraint due to angular acceleration limit are α min (, ) dκ() 2, κ() > κ() L 2 (, ) = undefined, κ() = α max (, ) dκ() 2, κ() < κ() (2) α max (, ) dκ() 2, κ() > κ() U 2 (, ) = undefined, κ() =, α min (, ) dκ() 2, κ() < κ() where the minimum angular acceleration i nonlinear function given by [ ] ω ᾱ α min (, ) = min, κ() = max κ(), ωmax κ(),, otherwie (2) where ᾱ min i a negative contant denoting minimum angular acceleration, while ω max i the maximum abolute angular velocity of the robot. The maximum angular acceleration i given imilarly by [ ] ω ᾱ α max (, ) = max, κ() = max κ(), ωmax κ(),, otherwie (22) where ᾱ max i the a poitive contant denoting maximum angular acceleration. For κ() =we ee that eq. (2) i undefined, becaue pure tranlational motion i not contrained by angular acceleration limit. In cae when v =, we have a pure rotation. Then the dynamic model reduce to a ingle equation = α, from which we derive acceleration limit a L,rot (, ) =α min,rot, U,rot (, ) =α max,rot, (23) where ᾱmin, [ ω α min,rot (, ) = max,ω max ], otherwie ᾱmax, [ ω α max,rot (, ) = max,ω max ], otherwie. (24) Computation of the velocity limit curve i complicated by the fact that min( ) and max( ) function are ued in equation (). Thu we do not know in advance from

5 y [m] Fig x [m] (a) κ() [m ] [m] (b) (a) The experimental path. (b) Curvature profile of the path (a).8 which contraint to compute the curve for particular. In other wor, we mut check all poible combination of i and j in equation L i (, ) = U j (, ), which could be computationally expenive for large number of contraint. However, if the value of robot parameter are known in advance, we can ubtitute them into correponding equation to predict which i and j combination are relevant. If we do that for the Pioneer 3DX robot, it become evident (a will be demontrated by the experimental reult) that for traight path egment (i.e. κ() =), only the condition L (, ) =U (, ) i relevant for computing the velocity limit curve. Thu, from L (, ) =U (, ) and uing (7) the velocity limit curve i V () = v max. For κ() three condition can be relevant, namely L 2 (, ) =U 2 (, ), L (, ) =U 2 (, ), and L 2 (, ) = U (, ). All of thee equation have cloed form olution that i eay to find. It mut be emphaized that thee reult are not general for different robot or parameter value the involved condition might change. IV. EXPERIMENTAL RESULTS The value of model parameter for the Pioneer 3DX robot that are ued for trajectory planning experiment are hown in Table I. The parameter that are marked by a tar can be adjuted by uer in robot firmware. TABLE I PARAMETERS OF THE PIONEER 3DX ROBOT. Parameter Symbol Value Unit Ma m 28.5 kg Inertia moment J.75 kg m 2 Min. long. acceleration ā min -.3 m/ 2 Max. long. acceleration ā max.3 m/ 2 Min. ang. acceleration ᾱ min rad/ 2 Max. ang. acceleration ᾱ max.745 rad/ 2 Max. long. velocity v max.75 m/ Max. ang. velocity ω max.745 rad/ An experimental path and it curvature profile are hown in Fig. 3. It i generated uing algorithm decribed in [6] and conit of four clothoi and a traight line egment. It ha part with high curvature in order to better demontrate (b) (c) (d) Fig. 4. Acceleration limit and velocity limit curve computed for the Pioneer 3DX robot with path in Fig. 3. (a) Velocity limit curve V (). (b) Velocity limit curve V 22 (). (c) Overall velocity limit curve V (). Acceleration limit L i (, ), U j (, ), and velocity limit curve V ij () originating from different contraint are plotted with the following color: i =,j =(blue), i =2,j =2(red), i =,j =2(green), i =2,j = (magenta). (d) Time-optimal velocity along the experimental path (blackolid) and overall velocity limit curve V () (red-dahed). Note: The lope of acceleration cone in figure (a)-(c) are obtained a d = d dt dt =, therefore e.g. in figure (a) we ee that lope of the cone decreae a increae.

6 a [m/ 2 ] α [rad/ 2 ] t [] (a) t [] (b) Fig. 5. Acceleration computed by time-optimal caling algorithm (bluedotted) and reference acceleration generated by nonlinear controller (reolid). (a) Longitudinal acceleration. (b) Angular acceleration. effect of angular velocity and acceleration limit. Acceleration limit L i and U i and velocity limit curve V ii obtained from contraint L i (, ) =U i (, ) are hown in Fig. 4 (a) and (b) for i =and i =2, repectively. Velocity limit curve V 2 and V 2 are not diplayed in a eparate figure becaue of lack of pace, however, in Fig. 4 (c) all velocity limit curve V ij are hown, and the overall velocity limit curve V () i their minimum. It can be noticed that in zerocurvature egment only the velocity limit curve V () i relevant. The optimal time-caling algorithm i implemented in C++ programming language. One ha to be extremely careful in algorithm implementation, a many involved function have dicontinuitie that make numerical integration prone to error. To accelerate algorithm execution, the value of acceleration limit and velocity limit curve are tored into a lookup table o that mot of the computational burden tem from numerical integration. With the firt order integration and tep ize of 2 m a reaonable preciion i achieved with planning time of le than m on 3 GHz PC, which i fat enough for real-time application. The trajectory tracking experiment i performed in imulator baed on ODE library that imulate rigid body dynamic conidering variable uch a ma, inertia, velocity, friction, etc. [7]. The robot comman are generated uing feedback control with nonlinear trajectory tracking controller [8], which enable compenation of error generated by parameter and meaurement uncertaintie and other diturbance. For feedback control to be effective, in experiment we ued acceleration limit omewhat higher than thoe ued for trajectory planning. The final velocity profile obtained by the time-optimal caling algorithm i hown in Fig. 4 (d). It can be een that it conit of four acceleration egment, four deceleration egment and two contant velocity egment. Baed on the computed velocity profile and uing numerical integration, one can compute velocity and acceleration profile in dependency of time. For the velocity profile from Fig. 4 (d) the correpondent longitudinal and angular acceleration profile are diplayed in Fig. 5. It can be een that both acceleration never exceed the limit. The total travel time of the obtained trajectory i approximately 4, while the average longitudinal velocity i.36 m/. V. CONCLUSION In thi work the optimal time-caling algorithm i applied for trajectory planning along the predefined path while repecting the actuator contraint. The dynamic model of the mobile robot that account for velocity and acceleration limit i developed and utilized to expre acceleration limit and velocity limit curve for the optimal time-caling algorithm, which are further ued for algorithm implementation in cae of Pioneer 3DX differential-drive robot. Thi paper cover only imple velocity and acceleration contraint, but it etablihe a bai for deign of more complex robot model that account for e.g. cae with dependent maximum longitudinal and angular acceleration, limited grip between the wheel and the ground (thi i very important for reliable operation of lightweight robot at high velocitie), or tip over prevention of tall mobile robot. Such more complex model will be developed in future work. It i alo planned to tet the planner on the real robot and to further optimize numerical integration by uing more advanced integration technique. REFERENCES [] K. G. Shin and N. D. McKay, Minimum-time control of robot manipulator with geometric path contraint, IEEE Tranaction on Automatic Control, vol. 3, no. 6, pp , 985. [2] J. E. Bobrow, S. Dubowky, and J. S. Gibon, Time-optimal control of robotic manipulator along pecified path, International Journal of Robotic Reearch, vol. 4, no. 4, pp. 3 7, 985. [3] M. Hentchel, D. Lecking, and B. Wagner, Determinitic path planning and navigation for an autonomou fork lift truck, in 6th IFAC Sympoium on Intelligent Autonomou Vehicle (IAV), 27. [4] J. Choi and B. Kim, Near-time-optimal trajectory planning for wheeled mobile robot with tranlational and rotational ection, IEEE Tranaction on Robotic and Automation, vol. 7, pp. 85 9, 2. [5] H. Choet, K. M. Lynch, S. Hutchinon, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principle of Robot Motion. MIT Pre, 25. [6] M. Brezak and I. Petrović, Path moothing uing clothoi for differential drive mobile robot, 2, accepted for publication for World Congre of the International Federation of Automatic Control (IFAC). [7] G. Klančar, M. Brezak, I. Petrović, and D. Matko, Two approache to mobile robot imulator deign, in Proceeding of the 6th EUROSIM congre on Modelling and Simulation, 27. [8] M. Brezak, I. Petrović, and N. Perić, Experimental comparion of trajectory tracking algorithm for nonholonomic mobile robot, in Proc. of 35th Annual Conference of the IEEE Indutrial Electronic Society, Porto, Portugal, 29.