Advance in Linear Algebra & Matrix Theory, 2012, 2, 20-24 http://dx.doi.org/10.4236/alamt.2012.22003 Publihed Online June 2012 (http://www.scirp.org/journal/alamt) A Linear Interpolation-Baed Algorithm for Path Planning and Replanning on Gird * Changwen Zheng, Jiawei Cai, Huafei Yin National Key Laboratory of Integrated Information Sytem Technology Intitute of Software, Chinee Academy of Science, Beijing, China Email: changwen@ica.ac.cn Received April 5, 2012; revied May 5, 2012; accepted May 20, 2012 ABSTRACT Field D* algorithm i widely ued in mobile robot navigation ince it can plan and replan any-angle path through nonuniform cot grid. However, it till uffer from inefficiency and ub-optimality. In thi article, a new linear interpolation-baed planning and replanning algorithm, Update-Reducing Field D*, i propoed. It employ different approache during initial planning and replanning repectively in order to reduce the number of update of the rh-value of vertice. Experiment have hown that Update-Reducing Field D* run fater than Field D* and return moother and lower-cot path. Keyword: Field D* Algorithm; Path Planning and Replanning; Any-Angle Path; Linear Interpolation; Grid Cell 1. Introduction In mobile robot navigation, path planning lead a robot from it initial location to ome deired location. The two mot popular technique for path planning are deteritic algorithm and randomized algorithm [1]. Among deteritic algorithm, A* provide heuritic earch in tatic, known environment [2]. LPA* combine heuritic earch and incremental earch [3]. D* Lite could replan in unknown environment efficiently [4]. When provided with a grid-baed repreentation of environment, thee algorithm are limited by the dicrete et of poible heading between gird cell. For example, the eight-connected grid retrict the agent heading change by multiple of π 4. A a reult, the path are uboptimal and unrealitic looking. To alleviate thi problem, everal method for any-angle path planning have been invetigated. A* PS ue pot-moothing to generate any-angle path [5]. But it doe not alway work a i howed in Figure 1 (The path returned by A* PS (in black line) i not the optimal path (in dah line) from S to G). Baic Theta* algorithm and AP Theta* algorithm (Angle-Propagation Theta*) allow the parent of a node to be a node other than it local neighbor [6]. AA* (Accelerated A*) can plan a hortet any angle path fat [7]. However, thee algorithm are only uable for uniform cot environment. Baed on D* Lite and linear interpolation, Field D* * Thi work wa upported in part by the National Natural Science Foundation of China under Grant 60873183. could fat plan and replan any-angle path in grid environment, whatever the environment cot are known or partially-known, uniform or non-uniform [8]. Field D* i employed a the path replanner in a wide range of fielded robotic ytem. However, Field D* till uffer from two major drawback: 1) It plan and replan much lower than D* Lite. 2) The path returned by Field D* i not alway the optimal olution. Motivated by thee obervation, a linear interpolation-baed planning and replanning algorithm, Update-Reducing Field D* (URFD*), i propoed in thi paper. It reduce the number of update of the rh-value o a to peed up the earch. It alo employ a method of pot-moothing to generate a lower-cot and moother path. Beide, a heuritic with a variable factor according to the environment i ued. A a reult, the novel algorithm could efficiently produce a near-optimal path in non-uniform cot grid. 2. Linear Interpolation-Baed Path Planning 2.1. The Idea of Field D* Field D* tore the rh-value, a one-tep look ahead etimate of the ditance (by the ditance of a vertex we mean the cot of an optimal path from thi vertex to the ). For vertex it atifie: 0, if rh g otherwie nbr (1)
C. W. ZHENG ET AL. 21 Figure 1. Sub-Optimality of A* PS. where i the vertex. Nbr denote the et of all neighboring vertice of. g i an etimate of ditance of. c, i the cot of a path between and. In claical grid-baed method, it i aumed that and are two corner vertice and the path between and i a traight line. Field D* relaxe thi aumption and take any point along the boundary of a cell into conideration. To make it poible, Field D* make an approximation that the path cot of any point y reiding on the edge between two conecutive corner vertice 1 and 2 i a linear combination of g 1 and g : 2 y 2 1 1 g yg y g (2) where y i the ditance from 1 to y (aug unit cell). With the form of the optimal path in a unit cell, Field D* could compute rh and find the point to move by making d c, g dv 0, (3) where v i the variable on which the path cot depend. 2.2. Difference and Inefficiency A linear interpolation-baed replanner perform differently from a claical path replanner uch a D* Lite, which lead to it inefficiency. To explain thi, we define g a the cot of the optimal path from vertex to the with repect to the linear interpolation aumption (o it i lightly different from the cot of the actual optimal path). We call g (or rh ) i inaccurate when g (or rh ) i not equal to g. Alo we call g i more accurate than rh when g i more cloe to g than rh. g atifie: 0, if g (4) nbrg otherwie. A linear interpolation-baed replanner expand vertice in a different way from D* Lite. With a conitent heuritic, a locally overconitent vertex (whoe g-value i lager than rh-value) become locally conitent (the g-value equal rh-value) after elected for expanion and then remain locally conitent until edge cot change are detected in D* Lite. It implie that D* Lite expand any locally overconitent vertex at mot once. However, a linear interpolation-baed replanner tend to expand a locally overconitent vertex for many time. Beide, the key value (denoting the prioritie of vertice in the priority queue) of the vertice elected for expanion are monotonically nondecreaing over time in D* Lite, while it i not naturally the cae in a linear interpolation-baed replanner. Thi can be explained a follow: For ome vertex, the computation of it rh-value i baed on the g-value of one neighboring vertex in D* Lite, but the g-value of two neighboring vertice in a linear interpolation-baed replanner. During the planning and replanning proce, it i common that at leat one of the two neighboring vertice ha an inaccurate rh-value. Relied upon them, will get an inaccurate rh-value. And the vertice relied upon will alo be affected, and o on. It i the reaon that a linear interpolation-baed replanner update the rh-value of vertice repeatedly to their final reult. Such a phenomenon i eaily oberved particularly in environment coniting mainly of free pace ince the g-value of a vertex i very cloe to thoe of it neighbor in free pace. After the cot field created, path extraction for linear interpolation-baed replanner i alo different from that for D* Lite. When backpointer are not recorded, D* Lite can trace back a lowet-cot path from tart to by alway moving from the current vertex, tarting at tart, to any neighbor that imize c, g until i reached. So the optimality of the reult depend on the accuracy of g. But in a linear interpolationbaed replanner the g-value are not the ditance exactly, reulting in the ub-optimality of path. From the dicuion above, we can ee that there exit two major drawback of Field D*: 1) It plan and replan much lower than D* Lite, epecially in the environment coniting mainly of free pace. 2) The olution path i not the optimal olution. Figure 2 how the econd problem. The path returned by Field D* ha unneceary heading change even if no obtacle exit (ee Figure 2(a)). To extract a moother path, [9] give a gradient interpolation method. The reult i howed in Figure 2(b), from which we can ee that the unneceary heading change till exit. Obtacle can alo make the interpolation aumption break down o that affect the quality of extracted path. To alleviate it, [8] ue a onetep look ahead mechanim. But thi method check very limited tep o that cannot avoid generating a pathologic path between vertice a and b in Figure 2(c). 3. Update-Reducing Field D* 3.1. Baic Idea Since the runtime of a linear interpolation-baed replanner
22 C. W. ZHENG ET AL. (a) (b) (c) Figure 2. Path returned by interpolation method. depend heavily on the number of update of the rhvalue of vertice, the key to high efficiency of our algorithm i reducing the number of update. We ue the fringe vertice to refer to the vertice on the fringe of the expanded vertice during initial planning. The fringe vertice have not been expanded yet o that their g-value are not computed (namely infinite), leading the rh-value computed by the g-value of the fringe vertice to inaccuracy. Then the inaccurate rh-value along with the infinite g-value affect next vertex expanion. Thi i the main ource of the repeated update of rh-value. Note that mot of the fringe vertice are jut locally overconitent vertice during initial planning, a i howed in Figure 3. (The locally conitent vertice (in light grey), which have been expanded at leat once, are almot urrounded by the locally overconitent vertice (in dark grey). Vertice in black are obtacle.) The rh-value of locally overconitent vertice are better informed and thu more accurate than the g-value. So when it i a locally overconitent vertex, we can ue the rh-value intead of the g-value to make the computation more cloe to the g * -value. When it i a locally conitent vertex, we can alo ue the rh-value becaue it equal the g-value and thu could get a reult at leat no poorer than that computed by the g-value. During replanning, if we only encounter cell cot decreae the approach above i till ueful. However, when locally underconitent vertice (whoe g-value are maller than rh-value) appear, thi approach tend to make the algorithm le efficient and even incomplete. It could be explained a follow: When the rh-value of a vertex become larger due to edge cot increae, the old rh-value i out of date and thu to be abandoned. However, the algorithm doe not ditinguih between the old and the new o that it i poible for the old rh-value to be ued to compute the rh-value of another vertex, reulting in fale relation between thee two vertice. For example, there exit two vertice a and b. After initial planning, g(a), rh(a) and g(b) are all infinite, rh(b) i 50. Then rh(a) and rh(b) are updated due to cell cot increae. When rh(a) i recomputed, old rh(b) (namely Figure 3. A naphot during initial planning of Field D*. 50) i ued. Then rh(b) i updated to a new value of 64. Thu, the computation of rh(a) eem to rely on rh(b) but in fact thi relation poibly doe not exit. Furthermore the wrong rh(a) lead to a priority error of a (g(a) i infinite o that doe not affect the key value), which poibly make a expand while b can never be expanded again. Thu the fale relation ha no chance to be corrected, reulting in the incompletene of the algorithm. In order to reduce the update of the rh-value during replanning, we ue a technique imilar to which Delayed D* ued to peed up D* Lite [10], that i, delaying the proceing of locally underconitent vertice. During path extraction, A* earch, which depend on the accuracy of heuritic le heavily than greedy earch doe, could avoid error caued by obtacle (ee Figure 2(c)). However, baed on the linear approximation, A* earch till cannot enure an optimal path even if no obtacle exit (ee Figure 2(b)). And greedy earch need to be kept for checking olution path for any loop. Note that the limitation of pot-moothing howed in Figure 1 can be overcome if it i already an any-angle path before moothing. Combined with the method above, Update-Reducing Field D* (URFD*) i a modified verion of Field D*. It redefine the rh-value (denoted by rh -value to be ditinguihed from the original) a 0 if rh rh otherwie nbr (5) where notation follow from (1). URFD* calculate the rh-value of vertice according to (5) during initial planning. During replanning it calculate the rh-value according to (1), which i imilar to Field D*, and delay the propagation of cot increae. It check the conitency of a path in every path extraction and end with a pot-moothing tep. 3.2. Algorithm Decription Figure 4 how the peudocode of the URFD* algorithm.
C. W. ZHENG ET AL. 23 Key() 01. return [( g ( ), rh ( )) h (, );( g ( ), rh ( ))]; tart Initialize() 02. for all S rh( ) g( ) ; 03. rh( ) 0; U ; raie fale; 04. Inert( U,,Key( )); ComputeCot (, a, b, cot) 05. Ue cot( ) and cot( ) to compute rh( ); a UpdateState() 06. if ( g ( ) rh ( )) 07. if ( U) Remove( U, ); 08. Inert( U,,Key( )); 09. ele if( g ( ) rh ( ) AND U) Remove( U, ); UpdateStateLower() 10. if ( g ( ) rh ( )) 11. if ( U) Remove( U, ); 12. Inert( U,,Key( )); 13. ele if ( g ( ) rh ( ) AND U) Remove( U, ); b ComputeState() 14. if ( ) 15. if (it i inital planning) rh ( ) ( ', ") connbr( ) ComputeCot(, ', ", rh); 16. ele rh( ) ComputeCot(, ', ", g); ( ', ") connbr( ) ComputeShortetPath() 17. while( U (Key( )) Key( tart )) OR rh( tart) g( tart)) 18. U.Top(); 19. if ( g ( ) rh ( )) 20. g rh( ); 21. Remove( U, ); 22. for all ' nbr( ) 23. ComputeState( '); UpdateStateLower( '); 24. ele 25. g ( ) ; 26. for all ' nbr( ) { } 27. ComputeS tate( '); UpdateState( '); FindRaieStateOnPath() 28. raie fale; tart ; ctr 0; loop fale; 29. while( AND loop fale AND ctr maxtep) 30. x arg ' ucc( ) ( c(, ') g ( ')); 31. for all ' vicinity( c) 32. if ( ' i locally inconitent AND ' ha never been added into U with local underconitency during thi replann ing epiode) 33. ComputeState( '); UpdateState( '); raie true; 34. if ( i viited before ) loop true; 35 x; ctr; Main() 36. Initialize(); 37. ComputeShortetPath(); 38. Path extraction; 39. Pot-Smoothing; 40. forever 41 Wait for change in cell cot; 42. for all cell x with changed cot 43. for each tate on a corner of x 44. ComputeState( ); UpdateStateLower( ); 45. ComputeState( tart ); UpdateState( tart ); 46. ComputeShortetPath(); FindRaieStateOnPath();. 47. while( raie) 48. ComputeShortetPath(); FindRaieStateOnPath();. 49. Pot-Smoothing; Figure 4. The update-reducing Field D* algorithm. During initial planning, URFD* call ComputeShortet- Path() to expand vertice. ComputeCot() calculate the rh-value in a way imilar to the interpolation-baed path cot calculation in Field D*, but every vertex ue rh- value, intead of g-value, of it neighbor. ComputeState() then compute the rh-value according to (5) (line 15). During replanning, ComputeState() call Compute- Cot() to compute the rh-value according to (1) (line 16). The rh-value of the tart vertex and every vertex immediately affected by the changed edge cot are updated, but only the locally inconitent tart vertex and locally overconitent vertice are inerted into priority queue U for expanion (line 42-45). Then FindRaieStateOnPath() (FRSOP) i called. FROSP check whether locally underconitent vertice are in the vicinity of the node. All the unproceed locally underconitent vertice that are adjacent to thi node will be added into priority queue U (line 31-33). Here vicinity() refer to the et of all corner vertice in the vicinity of node ( i included). When the number of node exceed the given limit maxtep, or a loop i found, which indicate a potential failure of path extraction, FRSOP top the extraction to expand locally underconitent vertice in priority queue U. After path extraction, the olution path i pot-proceed by a moothing tep. (line 39, 49). Given two cell boundary node along the path, the potmoothing replace the olution path between thee two node with a traight line path if the latter i le cotly. It i done with cell boundary node along the olution path iteratively. However, ome mall technique are ued to avoid a large amount of computation: 1) It only perform a ingle iteration. 2) It only moothe the path between cell corner becaue neceary and harp heading change uually occur on them. 3) Before moothing a path between two cell corner, it check whether the cot of all grid cell that the original path i through are all ame. If they are, the original path i kept. 4. Experimental Reult We compared the performance of URFD*, Field D* and Delayed D*. 800 different 500 500 random grid environment were generated: 400 environment with uniform cot grid and 400 with non-uniform cot grid. For uniform cot grid environment, four different initial percentage of obtacle cell were elected: 10%, 20%, 30% and 40%. For non-uniform cot grid environment, we aigned each traverable cell an integer cot between 1 (free pace) and 15, and four different initial percentage of free pace cell were elected: 90%, 70%, 50% and 30%, while the ret of the cell each got a cot (infinity or an integer between 2 and 15) randomly with the ame probability (namely 1/15). For each environment, the initial tak wa to plan a path from the lower left corner to a randomly elected on the right edge. After that, we altered the cot of cell cloe to the agent with probability 0.1 (1.6% of the cell in the environment were changed) and had each approach repair it olution
24 C. W. ZHENG ET AL. Table 1. Performance comparion among URFD*, Field D* and Delayed D* in three kind of environment. A B C Path Cot Rh-value Update Vertex Expanion Runtime Initial Replan Initial Replan Initial Replan Initial Replan Field D* 0.9538 0.9544 6.1560 64.1851 5.2317 32.3386 5.7784 58.7625 URFD* 0.9510 0.9519 2.5438 28.5094 2.2528 14.7824 2.3987 21.0833 Field D* 0.9619 0.9618 2.4143 10.7327 1.8484 8.9259 2.1848 13.0324 URFD* 0.9568 0.9567 1.4702 4.9103 1.4780 4.2553 1.8199 5.1806 Field D* 0.9639 0.9643 1.3155 1.2895 1.0600 0.9707 1.2174 4.9793 URFD* 0.9592 0.9596 0.8337 1.4234 0.9218 1.0877 0.7672 2.3122 path. We ue the weighted heuritic, which i decribed in the previou ection, for URFD* and Field D*. We elected the reult in three kind of environment (A: uniform cot grid with 10% obtacle cell. B: uniform cot grid with 30% obtacle cell. C: non-uniform cot grid with 50% free pace cell) and howed them in Table 1. Four performance meaure were ued here: the path cot, the total number of rh-value update (that i, update of the rh-value), the total number of vertex expanion (update of the g-value) and the runtime. Each value i a ratio of a performance meaure of URFD* (or Field D*) to that of Delayed D* averaged over initial planning (or replanning) epiode. Note that in environment with more free pace the runtime of initial planning and replanning of Field D* dratically increaed while thoe of URFD* increaed much more tably. The performance in environment C how the poibility that the number of update of the rh-value during replanning could be lightly larger than that of Field D* in ome cenario. However, ince the number of vertice in the priority queue i limited by electively proceing locally underconitent vertice, making the priority queue operation le expenive, the runtime of URFD* i till horter than that of Field D* in thoe cenario. 5. Concluion We preent URFD*, a linear interpolation-baed algorithm that plan and replan any-angle path in dynamic environment with uniform and non-uniform cot grid. It make effort in the reduction of update of the rhvalue, which contribute to the gain in efficiency. The olution path returned by URFD* are mooth and nearoptimal. A oppoed to Field D*, it perform fater planning and replanning and return a path with lower cot and fewer heading change. However, URFD* i not optimal either due to the linear interpolation aumption. Heuritic-Baed Path Planning, Proceeding of the Workhop on Planning under Uncertainty for Autonomou Sytem at the International Conference on Automated Planning and Scheduling, Monterey, 5-10 June 2005, pp. 9-18. [2] P. Hart, N. Nilon and B. Raphael, A Formal Bai for the Heuritic Deteration of Minimum Cot Path, IEEE Tranaction on Sytem Science and Cybernetic, 1968, Vol. 4, No. 2, pp. 100-107. [3] S. Koenig, M. Likhachev and D. Furcy, Lifelong Planning A*, Artificial Intelligence Journal, Vol. 155, No. 1-2, 2004, pp. 93-146. [4] S. Koenig and M. Likhachev, Improved Fat Replanning for Robot Navigation in Unknown Terrain, Proceeding of the IEEE International Conference on Robotic and Automation (ICRA 2002), Wahington, 11-15 May 2002, pp. 968-975. [5] A. Botea, M. Müller and J. Schaeffer, Near Optimal Hierarchical Path-Finding, Journal of Game Development, 2004, Vol. 1, No. 1, pp. 1-22. [6] A. Nah, K. Daniel, S. Koenig and A. Felner, Theta*: Any-Angle Path Planning on Grid, Proceeding of the National Conference on Artificial Intelligence, 22-26 July 2007, Vancouver, pp. 1177-1183. [7] D. Šišlák, P. Volf and M. Pěchouček, Accelerated A* Path Planning, Springer-Verlag, Berlin, 2009. [8] D. Ferguon and A. Stentz, The Field D* Algorithm for Improved Path Planning and Replanning in Uniform and Non-Uniform Cot Environment, Technical Report, Carnegie Mellon Univerity, Pittburgh, 2005. [9] M. W. Otte and G. Grudic, Extracting Path from Field Built with Linear Interpolation, IEEE/RSJ International Conference on Intelligent Robot and Sytem, St. Loui, 10-15 October 2009, pp. 4406-4413. [10] D. Ferguon and A. Stentz, The Delayed D* Algorithm for Efficient Path Replanning, Proceeding of the IEEE International Conference on Robotic and Automation, Barcelona, 18-22 April 2005, pp. 2045-2050. REFERENCES [1] D. Ferguon, M. Likhachev and A. Stentz, A Guide to