Towards an Efficient Optimal Trajectory Planner for Multiple Mobile Robots

Transcription

1 Toward an Efficient Optimal Trajectory Planner for Multiple Mobile Robot Jaon Thoma, Alan Blair, Nick Barne Department of Computer Science and Software Engineering The Univerity of Melbourne, Victoria, 3010, Autralia {jrth, blair, Abtract In thi paper, we preent a real-time algorithm that plan motly optimal trajectorie for multiple mobile robot in a dynamic environment. Thi approach combine the ue of a Delaunay triangulation to dicretie the environment, a novel efficient ue of the A* earch method, and a novel cubic pline repreentation for a robot trajectory that meet the kinematic and dynamic contraint of the robot. We how that for complex environment the hortet-ditance path i not alway the hortet-time path due to thee contraint. The algorithm ha been implemented on real robot, and we preent experimental reult in cluttered environment. I. INTRODUCTION Path planning i fundamental to mobile robot and ha been tudied extenively over many year. Path planning i the proce of calculating a path for a mobile robot to follow o that it can move from one poe (poition and orientation to another, while avoiding colliion with obtacle. Trajectory planning i the extenion of path planning to conider velocity. A wide variety of method have been developed for path planning and can be broadly claified a either local planner or global planner. Local planner derive a path baed on local enor on the robot and do not make ue of a map of their environment. A uch, local planner are reactive, baed on immediate enor data. Converely, global planner utilie a map of the world, either a a priori knowledge, ened by ome global enor (e.g. a camera over the environment, or built up through exploration. Beyond finding a path, ome global planner attempt to find the optimal or near-optimal trajectory for a mobile robot and in ome cae multiple mobile robot. Thi however, ha been limited to tatic environment only. We ee a need for an algorithm to find optimal trajectorie for multiple robot, in a dynamic environment, in realtime. We have developed an algorithm that will find optimal trajectorie for multiple robot in mot cae, and preent real robot experimental reult. A. Local Planner There are a number of algorithm for planning path from local enor data. We dicu three popular method: potential field; the vector field hitogram; and the dynamic window approach. Potential field[1][2], ometime referred to a vector force field (VFF or velocity field, are baed on the concept of electric or magnetic field. Variou object in the environment can impart an attractive or repulive force on the robot. Thi method i computationally fat, however it doe uffer from certain limitation. Often attractive and repulive force can negate each other reulting in the robot topping, or moving lowly. Potential field, a with all purely local planner, cannot handle the box-canyon problem ( a local minimum. The Vector Field Hitogram (VFH algorithm [3] aim at handling the vector negation problem in potential field by uing a hitogram. The VFH firt create a one-dimenional polar hitogram then elect the mot uitable ector for the teering direction. Thi allow the robot to maintain a higher average velocity. The Dynamic Window Approach [4] goe further to earch through the pace of poible intantaneou velocitie of the robot, taking into account the dynamic of the robot. Thi i achieved by reducing it earch pace to only circular trajectorie which can be uniquely determined by pair of tranlational and rotational velocitie, a two-dimenional earch pace. Only velocitie that are afe and will not collide with obtacle given the known dynamic contraint are allowed and only obtacle within a mall window of pace, reachable within a hort amount of time, are conidered. However, due to the lack of knowledge of the whole environment, all local planner are at bet able to produce a globally optimal path in trivial environment. B. Global Planner Knowledge of the entire environment allow global planner to earch for a complete path from the tarting poe to the goal poe. Thi knowledge mut be repreented in a form that can be earched. Thi generally tranlate to dicretiing the world in ome way. The typical approache for thi can be categoried into cell decompoition, or graph baed method. We examine the ue of occupancy grid which are a form of cell decompoition, and Voronoi and viibility graph, which are graphical method. Method uing Occupancy Grid [5] repreent the environment a a grid. The reolution of the grid can vary depending on the fidelity of the available information, either a priori knowledge or ened. Grid coordinate are claified a occupied by an obtacle, empty, or unknown. Thi grid can then be earched for available path generally either uing an A* earch [5] or the ditance tranform [6]. For a high reolution grid the earch pace can become large leading to high computational cot.

2 Graph baed method try to overcome thi caling problem by repreenting the knowledge a a graph. The graph i dependent on the complexity of the environment (number of obtacle rather than phyical ize. Graph baed path planning i often referred to a road-map [7] planning ince the arc on the graph can be conidered road the robot can travere with the vertice a interection. Thi graph mut then be earched to find a path. Generalied Voronoi Diagram (GVD [8] are often ued for thi purpoe. The GVD i defined a the locu of point which are equiditant from two or more obtacle boundarie including the workpace boundary [9]. A GVD can be generated in O(NlogN[8]. GVD reult in a path that i a far away from all obtacle a poible. Thi i a good repreentation when the pace between obtacle i lightly larger than the robot itelf, however it i conervative if the relative ize of the robot to the pace between obtacle i mall, ince thi reult in a path that i ub-optimal due to the equiditant contraint on the graph. Voronoi diagram have alo been ued for tructure extraction [10] to generate topological map from other map. A Voronoi diagram can be efficiently created from it dual, the Delaunay Triangulation, which i ometime eaier to extract directly than the Voronoi graph. Additionally, viibility graph are often ued in robot motion planing [7]. Each obtacle i reduced to a vertex lit. For imple object thi can often be one vertex repreenting a point ource, however wall and polygon can be repreented a a connected vertex lit. A graph i produced uch that each arc on the graph connect vertice that are viible to each other. When ued for path planning the tarting point and goal point are included in the graph, and the planner imply earche through the graph from the tart vertex to the goal vertex. Thi can eaily be implemented with an A* earch uing Euclidean ditance a the heuritic. Algorithm for contructing and earching thi graph exit with running time O(N 2. While a Voronoi graph will force a path far away from obtacle, a viibility graph ha the oppoite effect of taking the path cloe to obtacle. Thi increae the rik of colliion with obtacle and can reult in a ub-optimal path. C. Optimal Planner Optimal motion planner find a colliion free path, with a trajectory that i optimal according to ome cot function, for example, the hortet ditance or time to reach the final poe. Previouly thi ha been done uing cubic pline to control a trajectory through an environment for robot arm working together [11]. However, without firt reducing the environment to a mall earch pace, thi problem i computationally expenive and mut be performed offline. A a reult, thi method i intolerant to dynamic obtacle. Cubic pline have alo been ued with kinematic viibility graph to generate a path [12]. However, creating the viibility graph i time-expenive forcing the graph generation to be performed off-line. A computationally fater method for cubic pline ue randomly elected initial point and applie a Fig. 1. Way-point Tuning. The way-point can be moved along the line to get a curve with a fater velocity profile. genetic algorithm (GA [13]. Both, again, are only ued on tatic environment and are intolerant to dynamic obtacle. Another method for optimal planning [14] ha been developed for ue in a dynamic environment, uing a earch through a five dimenional pace. The dimenion are the poe (x, y, θ, and tranlational and rotational velocitie (v, ω. Thi method firt find the hortet ditance path acro an occupancy grid uing an A* earch, then etablihe how far it can look ahead for the five dimenional earch baed on computational power available. For the pace etablihed, each dimenion i dicretied (e.g. 10cm for poition, π/16 degree for orientation and the five dimenional pace i earched. Thi algorithm find the hortet ditance path globally, and the hortet time trajectory locally. Finding the hortet time trajectory globally would be preferable. To find the hortet time trajectory globally, we mut earch acro the dynamic pace for the full path. For cloed, controlled environment, uch a a factory floor, office, home, hopping mall, or robotic occer field, thi i made poible by mounting global enor around the environment. The difference in hortet ditance and hortet time trajectorie produced i illutrated in figure 2(c. II. ALGORITHM The algorithm we have developed i baed on a graphical repreentation of the environment in the form of a Delaunay triangulation (Fig 2(b. Thi graph contain ueful information for the purpoe of path planning which i not preent in a Voronoi or viibility graph. Each arc on the graph repreent a line joining two obtacle that are cloet to each other. It i exactly thi information that i important to a path planner plotting path through the gap between obtacle. In order to optimie the trajectory, we conider thee arc a contrained variable way-point through which the trajectory mut pa (Fig 1. If we are treating the robot and obtacle a point ource we mut reduce the line egment repreenting the contrained variable way-point along the arc to the robot configuration pace (the phyical pace the robot i able to occupy. The ability to tune way-point give an advantage over Voronoi and viibility graph. Before earching we can prune any arc that the robot cannot travere. Thee arc occur along wall and when obtacle are too cloe to each other for the robot to pa between them. Thi ha the effect of making obtacle in cloe proximity to each other appear topologically a one obtacle.

3 Thi pruning, and the fact that the graph i a triangulation, reult in a low branching factor. If we alo prevent the earch from entering the ame triangle twice we have a branching factor of 0, 1, or 2 at any point. Thi reduce the earch pace coniderably. We earch the graph uing an A* earch. Thi earche baed on a cot function f(n = g(n + h(n, where g(n i the cot of reaching a point in the graph, and h(n i an admiible heuritic (never overetimate etimate of the cot to the goal. A* expand node in order of mallet f(n firt. We can continue expanding node after we have our firt olution to get a orted lit of all path. Ideally the algorithm would ue the actual time the robot will take to travere the path (See Sec. II-A and II-B a g(n, however thi would be too computationally expenive to run at frame-rate. For efficiency we ue traight-line ditance (SLD connecting the way-point on one arc with the way-point on the next. We ue the ame cot function for h(n connecting the current way-point to the goal point. We can calculate a lower bound on the traveral time for a path by dividing the total SLD of the line egment, by the maximum peed at which the robot can travel. Thi time (time SLD i a lower bound becaue it i the hortet ditance path, and we are ignoring the kinematic and dynamic contraint of the robot which prevent it from travelling at maximum peed all the time. Once a path of traight-line egment reache the goal node we convert it to a cubic pline trajectory, a dicued in II- B. A hown in ection II-A, we can calculate an accurate etimate of the time (time cubic to travere thi path. Since thi trajectory i contrained by phyic it will alway take the ame or a longer time to travere than the traight-line etimate (time SLD above. We continue converting traight-line egment path, which we get in time SLD orted order from the A* earch, to cubic pline trajectorie until we get a path length that reult in a time SLD etimate that i longer than that of our bet cubic pline trajectory (time SLD > bet time cubic. No further earching i required at thi point a it i impoible for that path to reult in a trajectory that will be better than the bet one already converted. Thi algorithm i hown a peudo-code in table I. A. Parametric Cubic (PC Curve We ue Parametric Cubic (PC curve a they can repreent a pace-time curve, or trajectory, algebraically. A unique PC curve can be generated from the following information: initial poition (P x0, P y0 and velocity (V x0, V y0 ; final poition (P x, P y and velocity (V x, V y ; and maximum peed (V and acceleration (A. We can olve a et of equation to find the time ( it take to complete thi curve. Conider a curve of the form: Px (t ax P (t = = t P y (t a 3 bx + t y b 2 cx dx + t +, y c y d y (1 TABLE I PSEUDO-CODE OF THE ALGORITHM for each frame Update map from enor data (Fig 2(a Build Delaunay graph from new map (Fig 2(b for each robot, calculate a trajectory Perform A* Search on graph (uing Euclidean ditance Calculate traight line time ignoring dynamic: time SLD = ditance V max if goal point i reached then if time SLD > bet time cubic then top earching ele convert new path to cubic pline trajectory (Fig 2(c Calculate cubic pline time conidering dynamic (Sec II-A: time cubic if time cubic < bet time cubic then bet time cubic = time cubic keep new cubic pline trajectory a bet continue earching return bet cubic pline trajectory which we write in horthand form a: P(t = at 3 + bt 2 + ct + d. (2 We can calculate the equation for velocity and acceleration at any point through differentiation: P (t = 3at 2 + 2bt + c, (3 P (t = 6at + 2b. (4 We aume the following initial and final condition: Px0 Px P(0 =, P( =, P y0 P y P Vx0 (0 =, P Vx ( =. (5 V y0 V y Let: Solving for t = 0: Solving for t = : A = ( P(0 P( P (0 P (. (6 P(0 = d, P (0 = c. (7 P( = a 3 + b 2 + c + d, (8 P ( = 3a 2 + 2b + c. (9 Thee equation can be combined in matrix form a follow: A = ( a b c d (

4 (a An environment. (b A Delaunay Graph. (c Difference between ditance and time cot function. (d Path Permutation. Fig. 2. Illutration Equation (2 then become: P(t = a b c d t t ( t 3 = A t t ( t = A t t ( Thee equation uniquely determine the trajectory, once the time for travering the entire path egment ha been choen. For curve fitting application, it i common to arbitrarily chooe = 1 a it implifie the equation. However, for our trajectory planning tak, we would like to chooe the minimal atifying the following dynamic contraint (which, for our holonomic robot, we approximate with a unit circle in velocity and acceleration pace: P (t A + P (t V 2 1, for 0 t, (14 where A and V are the maximal acceleration and velocity of the robot. From Equation (3 and (4 thi become: ( 1 1 AV P(0 P( P (0 P ( t 3t [ V A 2t 2 1 ] ( Ideally, we would like to find the mallet (poitive value of uch that the vector on the RHS of Equation (15 doe not exceed unit length for any t between 0 and. Unfortunately, ince the (vector-valued function i quadratic in t and quartic in, thi problem i, in general, analytically intractable. However, provided that the length of the individual egment are ufficiently mall relative to the moothne of the curve, we t 3 can get a reaonable approximation by impoing the dynamic contraint only at the endpoint. (In particular, if we were to ignore friction by taking the limit a V, the vectorvalued expreion in Equation (4 would become a linear function of t, enuring that it mut achieve it maximum length at one of the endpoint. At the initial point, Equation (15 become: 1 P (0 A + P (0 V 2 2 Px0 P = x V x0 V x AV 2 P y0 P y V y0 V y 4 A Vx0 = A 2 V V y0 Thu: A 2 V V 2Vx0 + V x + 3V 2V y0 + V y A2 4 ( Vx0 3V 3V 2V + A 2 2 V 2 Px P 2 x0 P y P y0 ( Vx0 2 (16 2 V 4 2Vx0 + V AV.( x y0 V y0 2V y0 + V 3 y Vx0 Px P +3AV.( x0 V y0 P y P 2 y0 2 +V 2 2Vx0 + V x 2V y0 + V 2 y 6V 2 ( 2Vx0 + V x 2V y0 + V y.( Px P x0 P y P y0 +9V 2 ( Px P x0 P y P y0 2 (17 The kinematic contraint at the final point lead to an inequality of the ame form, but with the role of (x 0, y 0 and (x, y revered. The appropriate value of will therefore occur at a root of one of thee two quartic equation. It i eay to ee that each of thee equation mut have at leat one poitive root. Thu the deired value for can be obtained by checking the poitive root in increaing order, until one i found to atify both contraint. Thi value for will be the minimum time the robot can travere the curve given the contraint in Equation (14.

5 TABLE II SMOOTHING THE SPLINE Initialie all variable way-point (k to the poition ued in the A* earch and a velocity of the maximum peed (V of the robot, in the direction from point k 1 to point k + 1. do for each way-point k (not including the end point Generate a Parametric Cubic (PC curve from point k 1 and k + 1. (Sec. II-A Find the interection of thi curve with the variable way-point line at k Set the way-point at k to the poition and velocity value generated at the interection. if either poition or velocity violate a contraint then bring it back within the contraint while change are larger than error threhold or until time-out When i ubtituted back into Equation (13 we have a unique repreentation for thi trajectory. Additionally, by uing Equation (2, (3, and (4, we can calculate the poition, velocity, and acceleration at any time t along the curve, where 0 t. B. Cubic Spline Trajectory A PC pline i a chain of PC curve where the final poition and tangent vector of one curve (ay k, i the initial poition and tangent vector for the next (k + 1. A et of imultaneou equation (often repreented with a tridiagonal matrix [15] can be ued to find tangent vector for all k internal control point if the poition are known. Thi produce the moothet path along the PC pline. With variable way-point the et of equation i under-contrained and cannot be olved analytically. A mooth path i required to optimize the trajectory. Smoothing the trajectory allow the robot to maintain higher velocitie for longer period of time. For thi reaon we developed a new algorithm for moothing the pline, hown below. It i eentially a gradient decent method acro the four dimenional pace of poition and velocity at each way-point. (See table II. Although the trajectorie produced are not guaranteed to be optimal, a they could fall into a local minimum, they are almot alway fater, and a more accurate repreentation, than a path modelled uing traight line egment for a dynamically contrained path. An example of when thi i ub-optimal i when the maximal velocity and acceleration i ufficiently high that intantaneou change in direction (kink in the path are poible. In thi cae a globally mooth curve i not optimal for time. III. IMPLEMENTATION DETAILS The following aumption and implification were made for implementation: 1 All dynamic object are treated a circle. The centre of the circle are the vertice of the graph. The radii affect the configuration pace of the robot. 2 Wall are repreented a point ource at regular interval along the wall. The eparation between point i approximately three time the ize of the robot. Delaunay arc connecting point on the ame wall are pruned from the graph prior to earching. 3 The implementation of the graph allow four degree polygon to exit if the converion to two triangle would reult in a very thin triangle. Thi i required ince very thin triangle cannot be travered in all direction. 4 The curve were further contrained to remove loop. Thi wa achieved by over-riding the magnitude of the final velocity o that it doe not need a run up. IV. EXPERIMENTAL RESULTS The algorithm preented wa written in C and run under Linux (A creen hot i hown in Fig. 3(b. It wa developed for ue in the robotic occer competition: RoboCup Small- Sized Robot league. The environment ued i approximately three by four meter. It i urrounded by wall and contain ten robot of 0.18 meter diameter. The fatet team move at peed up to 2.5m 1 and acceleration of approximately 2.0m 2. A ingle camera i mounted 3m above the field with an unobtructed view of the environment [16]. Experiment were performed in imulation uing a Pentium III 866MHz computer, and on real robot uing a dual proceor 1800 Athlon MP computer. The fater computer wa required for the real experiment due to the increae in computation required for full frame-rate image proceing. The robot are cutom built mall, omni-directional drive robot equipped with radio tranceiver to communicate with the central computer [17]. Their maximum velocity wa 1.5m 1 and maximum acceleration wa 2.0m 2. We compare the algorithm with our path planner previouly ued in RoboCup competition, an A* earch uing traightline ditance cot. The cae we examined (Illutrated in Fig. 3(a were finding a path/trajectory in: 1 trivial environment where a traight line path i poible; 2 an environment with the zig-zag layout; and 3 cluttered environment where an indirect path i required. Extenive teting wa performed in imulation. The reult hown in the graph (Fig. 3(c are from real-robot experiment and how the average time taken to complete a path. Once the robot detination point wa reached, the original tarting point wa et a a new detination point and the proce wa repeated. The robot continued until 20 lap were completed and the time taken wa averaged. A new path/trajectory wa calculated at each frame. For the trivial environment the SLD algorithm reult in the ame path and trajectory a the cubic pline algorithm. Thi i to be expected. Although the planned path in the zig-zag cae differred, the travered path were identical due to overhoot in motion control. In thi cae the trajectory planned by our algorithm more accurately repreent the one travered than that planned by the SLD algorithm. For the randomly cluttered environment the SLD choice varied greatly, ometime taking almot twice a long a the

6 Time (econd SLD algorithm Trivial Zig Zag Random Clutter (a Experimental Layout. (b Screen Capture. (c Experimental Reult. Fig. 3. Experiment cubic pline trajectory method. In ome intance it took le time, however thi wa due to error in motion control. In all experiment the average computation time for calculating trajectorie wa low. Building the Delaunay graph took approx. 2.9m on the 866MHz PC and 2.4m on the dual 1800MHz PC. Plotting a trajectory baed on the graph took approx. 1.4m on the 866 MHz PC and 0.4m on the dual 1800MHz PC. We conider the graph building to be independent of the trajectory planning a it i done once, and ued by all robot. Thee reult how that thi algorithm i efficient enough to be run at frame-rate. Thi allow u to plan trajectorie in a dynamic environment. V. CONCLUSION In thi paper we preented an algorithm to find the timeoptimal dynamic trajectory for mobile robot. Our ytem repreent the environment a a Delaunay triangulation. We ue an A* earch to find all poible hortet ditance path. We then convert each complete path to a more accurate repreentation of an actual path uing a PC pline coniting of PC curve egment. From thi we can calculate an accurate etimate of the time required for each path. We continue to convert hortet ditance path to hortet time path until we reach a path length that cannot poibly be better than our hortet time path given the known velocity and acceleration limit of the robot. Due to the efficiency of thi algorithm, it can be run at the rate enor data i collected and wa teted at a frame rate of 25Hz. Thi allow it to handle dynamic obtacle. Experimental reult how that in mot cae thi algorithm can plot time-optimal trajectorie for multiple robot in real time. There are limited cae where the path produced i uboptimal, uch a when the velocity and acceleration contraint permitted kinked path. ACKNOWLEDGMENT The author would like to thank the AECM and the ARC Linkage grant LP for providing funding aociated with thi project. REFERENCES [1] Ronald Arkin. Motor chema baed mobile robot navigation. In Proc. of the IEEE Int. Conf. on Robotic and Automation (ICRA, page , [2] O. Khatib. Real-time obtacle avoidance for manipulator and mobile robot. The International Journal of Robotic Reearch, 5(1:90 99, [3] Johann Borentein and Yoran Koren. The vector field hitogram - fat obtacle avoidance for mobile robot. In Journal of the IEEE Tranaction on Robotic and Automation, [4] Dieter Fox et al. The dynamic window approach to colliion avoidance. IEEE Robotic and Automation Magazine, [5] Alberto Elfe. Occupancy grid: A tochatic patial repreentation for active robot perception. In Proceeding of the Sixth Conference on Uncertainty in AI, July, [6] A. Zelinky. A mobile robot exploration algorithm. In Journal of the IEEE Tranaction on Robotic and Automation, 8(6: , December [7] J.-C Latombe. Robot Motion Planning. Kluwer Academic Publiher, [8] Oamu Takahahi and R. J. Schilling. Motion planning in a plane uing generalized voronoi diagram. In Journal of the IEEE Tranaction on Robotic and Automation, 5(2, April [9] D. T. Lee and R. L. Drydale III. Generalized voronoi diagram in the plain. SIAM J. Comput., 10(1:73 87, Feb [10] Vachirauk Setalaphruk; Takahi Uneno; Yauyuki Kono; Maatugu Kidode. Topological map generation from implified map for mobile robot navigation. In In Proc. of The 16th Annual Conference of Japanee Society for Artificial Intelligence, [11] B. Cao; G.I. Dodd; G.W. Irwin. Contrained time-efficient and mooth cubic pline trajectory generation for indutrial robot. IEE Proceeding Control Theory and Application, 144(5: , Sept [12] V. F. Munoz and A. Ollero. Smooth trajectory planning method for mobile robot. Special iue on Intelligent Autonomou Vehicle of the Journal of Integrated Computed-Aided Engineering, 6(4, [13] Kai-Ming Te and Chi-Hu Wang. Evolutionary optimization of cubic polynomial joint trajectorie for indutrial robot. IEEE International Conference on Sytem, Man, and Cybernetic, 4: , Oct [14] Cyrill Stachni and Wolfram Burgard. An integrated approach to goal-directed obtacle avoidance under dynamic contraint for dynamic environment. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robot and Sytem (IROS, [15] Bruce R. Dewey. Computer Graphic for Engineer. Harper and Row, Publiher, [16] wyeth/f180 Rule/index.htm. [17] Jaon Thoma and Andrew Peel. Roobot team decription. In RoboCup- 2002: Robot Soccer World Cup VI, page In Pre. Springer, 2003.