Planning with Unertainty in Position: an Optimal Planner Juan Pablo Gonzalez Anthony (Tony) Stentz CMU-RI -TR-04-63 The Robotis Institute Carnegie Mellon University Pittsburgh, Pennsylvania 15213 Otober 2004 2002 Carnegie Mellon University The views and onlusions ontained in this doument are those of the authors and should not be interpreted as representing the offiial poliies or endorsements, either expressed or implied, of Carnegie Mellon University.
Abstrat We propose a resolution-optimal planner that onsiders unertainty while optimizing any monotoni objetive funtion suh as mobility ost, ris, energy expended, et. The resulting path is a one that minimizes the expeted ost value of the objetive funtion, while ensuring that the unertainty in the position of the robot does not ompromise the safety of the robot or the reahability of the goal. Keywords: mobile robot, path planning, unertainty, optimal planner, error propagation
Contents Abstrat... 4 Contents... 6 1 Introdution... 7 2 Related Wor... 7 3 Motion Model and Unertainty Propagation... 8 4 Implementation... 13 4.1 Simplified error propagation model... 13 4.2 Planning with unertainty... 14 4.3 Using loalization regions... 16 5 Results... 17 5.1 Route planning example... 17 5.2 Performane... 19 6 Conlusions and Future Wor... 21 7 Anowledgments... 22 Referenes... 23
1 Introdution Unertainty is always present in the position estimation of mobile robots. For path planning, however, its effets are frequently ignored. For most problems, this is an appropriate approah if the unertainty is smaller or omparable to the size of the robot. However, when the unertainty is larger, its effets should not be ignored. The widespread use of GPS (Global Positioning System) has failitated the position estimation problem for outdoor mobile robots. Nonetheless, there are many senarios where GPS overage is limited or unavailable: under tree anopies, in anyons, and in many urban environments. In indoor settings GPS is unavailable, although sometimes landmars an be used for absolute positioning. In this paper we propose a resolution-optimal path planner that onsiders unertainty, while optimizing any monotoni objetive funtion suh as mobility ost, ris, energy expended, et. The environment is represented as a grid, in whih the ost of eah ell orresponds to the ost of traveling from the enter of the ell to its edge. The resulting path minimizes the expeted value of the objetive funtion along the path, while ensuring that the unertainty in the position of the robot does not ompromise the safety of the robot or the reahability of the goal. The planner uses a single-parameter error propagation model in order to minimize the number of dimensions required. This model is a onservative estimate of the true error projetion model and an provide an aurate estimate of the higher-dimension model, depending on the type of error that is predominant in the system. Additionally, the planner is able to handle regions with GPS overage, where the unertainty stops propagating and is redued to a fixed value. Even though the planner utilized is a 3-D planner, the harateristis of the searh spae and the simplified error propagation model allow the planner to have a time omplexity lose to that of a 2- D planner. The planning time is under 1 seond for worlds of up to 150x150 ells, and under 10 seonds for worlds of up to 250x250 ells. This paper is organized as follows: setion 2 ontains a review the relevant literature in the area of planning with unertainty. Setion 3 explains the motion model and the unertainty propagation problem. Setion 4 explains our approah to the problem. Setion 5 shows some results of the planner applied to syntheti and geographi data and analyzes the performane of the algorithm. Setion 6 ontains the onlusions and explores future diretions for this researh. 2 Related Wor There is abundant wor in planning with unertainty in the researh literature. Latombe [1][2] has an extensive review on the state of the art as of 1991. Sine then, important ontributions by Lazanas and Latombe [3], Bouilly [4][5], Haït et al. [6], Fraihard [7], and others have not only expanded the theoretial approahes to planning with unertainty, but also addressed some of its pratial limitations. There is, however, little wor in reating optimal planners. Although the planner proposed by Bouilly [4] alulates an optimal path with respet to unertainty or path length (i.e.: a path of minimum unertainty or minimum path length), the approah is not appliable to finding optimal paths with respet to other important riteria. To the best of our nowledge, only Roy and Thrun [8] have solved the problem of finding optimal paths for ontinuous ost worlds in the presene of unertainty. The planner they propose inludes many of the elements of the planner proposed here, but is based on an approximate solution to a POMDP. This approah requires pre-proessing all the states in the searh spae, whih later allows for very fast planning. However, the total planning time (inluding the pre-proessing stage) is very long. We are presenting an alternate semi-deterministi approah based on A*, whih in average has muh lower time omplexity.
3 Motion Model and Unertainty Propagation The first-order motion model for a point-sized robot moving in two dimensions is: x ( t) = v( t)os θ( t) y ( t) = v( t)sin θ ( t) θ ( t) = ω( t) (1) where the state of the robot is represented by x(t), y(t) and θ(t) (x-position, y-position and heading respetively), and the inputs to the model are represented by v(t) and ω(t) (longitudinal speed and rate of hange for the heading respetively). Equation (1) an also be expressed as: where x ( t) = ( x( t), y( t), θ( t)) and u ( t) = ( v( t), ω( t)). If we allow for error terms in (1), the resulting equation is: x ( t) = f ( x( t), u( t)) (2) x ( t) = ( v( t) + w ( t))os θ( t) + w ( t) y ( t) = ( v( t) + w ( t))sin θ( t) + w ( t) θ ( t) = ω( t) + w ( t) v v ω x y (3) wherew and w ω are random errors in the inputs v(t) and ω(t) (longitudinal ontrol error and gyro v drift), and w x and wy are random model errors. Using the extended Kalman filter (EKF) analysis for this system, whih assumes that the random errors are zero-mean Gaussian distributions, we an model the error propagation as follows: T T P( + 1) = A P( ) A + L Q( ) L (4) where and 1 0 v( t)sin( θ ( )) t A = 0 1 v( t)os( θ ( )) t 0 0 1 (5) os( θ ( )) t 0 t 0 L = sin( θ ( )) t 0 0 t 0 t 0 0 (6) w = [ w ( ) w ( ) w ( ) w ( )] T (7) v ω x y Kelly [10] alulated losed-form solutions for some trajetories. For a straight trajetory along the x-axis eeping all inputs onstant, the errors behave as follows: - The error due to the longitudinal ontrol w v is refleted in the x diretion, and is given by σ 2 2 x σ v vt =, or σ x = σ v vt (See Figure 1).
Figure 1. Unertainty propagation for error in longitudinal ontrol - The error due to w ω is refleted in the y diretion, and is given by σ y σ ω 3/ 2 = v t (See Figure 2). 2 2 2 3 y v t σ = σ ω, or Figure 2. Unertainty propagation for error due to gyro drift - The error due to w x is refleted in the x diretion, and is given by Figure 3). σ 2 2 x = σx t, or σx = σx t (See
Figure 3. Unertainty propagation for errors due to w x - The error due to w y is refleted in the y diretion, and is given by Figure 4). σ 2 2 x = σx t, or σx = σx t (See Figure 4. Unertainty propagation for errors due to Additionally, there are errors in the initial position of the robot. Errors in x and y do not inrease unless there is unertainty in the model or in the ontrols. Errors in the heading angle, however, do propagate linearly with t. For initial angle errors of less than 15 degrees, the small angle approximation an be used to obtain the expression: σ y = σ θ. vt (See Figure 5 and Figure 6). o A straight line trajetory maximizes eah one of the error terms (see [10] for the proof) therefore we an use the results of this trajetory as an upper bound on the error for any trajetory. The dominant terms in the error propagation model depend on the navigation system and on the planning horizon for the robot. w y
Figure 5. Unertainty propagation for errors in initial position alone. Figure 6. Unertainty propagation for errors in initial angle alone. For a planning horizon of 3m at a speed of 5 m/s, with longitudinal ontrol error of 10% of the ommanded speed ( σ v = 0.1 v = 0.5 m / s), gyro drift of 10 deg/hr ( σ ω =10 deg/hr = 4.84. 10-5 rad/se) and model error of 0.1m/s in x and y ( σ x = σ y = 0.1 m/s), the dominant term is the longitudinal ontrol error (See Figure 7). However, when onsidering the error aused by the unertainty in the initial angle, this beomes the dominant error soure for errors as small as 0.25 degrees (See Figure 8).
Figure 7. Comparison between different types of error Figure 8. Comparison between different values of initial angle error and longitudinal ontrol error Figure 9 shows the results of ombining all the types of error (with the values mentioned above, and an initial heading angle of 0.25 degrees) for a straight trajetory. Figure 10 shows the same analysis for a random trajetory.
Figure 9. Error propagation for a straight trajetory with all error soures ombined Figure 10. Error propagation for a random trajetory with all error soures ombined 4 Implementation 4.1 Simplified error propagation model In order to eep the planning problem tratable and effiient, we will approximate the probability density funtion (pdf) of the error with a symmetri Gaussian distribution, entered at the most liely loation of the robot: q = ( x, y) q : N ( q, σ ) (8) where q is the most liely loation of the robot at step, and σ is the standard deviation of the distribution.
Let us define: ε = 2 σ (9) We an then model the boundary of the unertainty region as a dis entered at radius ε. To propagate the unertainty, we use the model: 1 1 ) (, ε ud ) 1 q with a ε( q ) = ( q + q q (10) where u is the unertainty arued per unit of distane traveled, q is the previous position along the 1 path, and d( q, q ) is the distane between the two adjaent path loations q and where Equivalently, we an define the unertainty at loation 0 0 0 ε ud q is the initial most liely loation of the robot, and q as: 1 q. ε( q ) = ( q ) + ( q, q ) (11) 0 D( q, q ) is the total distane traveled 0 along the path from q to q. The unertainty rate u is typially between 0.01 and 0.1 (1% to 10%) of distane traveled. By modeling the error propagation in this manner, we are assuming that the dominant term is the unertainty in the initial angle. Even though we are not expliitly modeling θ as a state variable, the effets of unertainty in this variable are aounted for in the unertainty propagation model for q=(x,y). 4.2 Planning with unertainty In a deterministi planner, the position of the robot on the plane is usually defined as q=(x,y), where x and y are deterministi variables. Beause the position of the robot is now a probability distribution, the new representation for the position of the robot is where q is a random variable representing the position of the robot, the robot, and p( q q, ε ) (12) q is the most liely loation of ε is the unertainty in the position. As mentioned in the previous setion, this pdf is modeled as a Gaussian distribution entered at q and with standard deviation σ = ε / 2. + 1 q is The expeted ost of going from a most liely loation q to an adjaent most liely loation ( (, ),( + 1, + 1 ) 1 1 1 1 ) (, + ) (, +,, +, + ε ε o i j i j ε ε ) E C q q = C p q q q q q q (13) i j where + 1 C o i j ( q, q ) is the deterministi ost of traveling from q i to Expressing the joint probability as a onditional probability: 1 1 E + + C (( q, ε ),( q, ε )) = i j C + 1 j q. + 1 + 1 + 1 + 1 ( q, q ) p( q q, q, ε, q, ε ) o i j j i p + 1 + 1 ( qi q, ε, q, ε ) (14)
i + 1 + 1 (( q, ε ),( q, ε )) E C = j C + 1 + 1 + 1 + 1 ( q, q ) p( q q, q, ε, q, ε ) o i j j i p ( qi q, ε ) (15) from If u q to << ε, then 1 ε ε + (for a single step inrement). We an then model the transition + 1 q as a deterministi transition for the distribution p( i, ε ) Under this assumption: p ( q j qi, q, ε, q, ε ) q q. 1 1 1 1 if j i ' = 0 otherwise + + + = where i' is the image of i under a transformation T suh that T ( q ) = q (See Figure 11) Substituting (16) in (15): + 1 + 1 (( q, ε ),( q, ε )) E C = i C + 1 ( q, q ' ) p( q q, ε ) o i i i + 1 (16) (17) ε 1 ε + q q + 1 q i q + 1 1 j = q + i ' + 1 + 1 Figure 11. p( i, ε,, ε ) q q q and state transitions when u << ε Sine i q and q i ' are neighbors, we an express ( ' ) + 1 C o i i ' ( q, q ) as: + 1 + 1 o i i = q i + q i ' C q, q ac ( q ) bc ( q ) (18) where a=b=1 for horizontal or vertial neighboring ells, and a=b= 2 for diagonal neighboring ells. Substituting (18) in (17): i + 1 + 1 (( q, ε ),( q, ε )) E C = + 1 ( q ) ( q q, ε ) + ( q ' ) ( q q, ε ) a C p b C p o i i o i i i (19)
( ) 1 1 1 E + + + C ( q, ε ),( q, ε ) = a CE ( q, ε ) + bce ( q, ε ) (20) where ( ) ( ε ) E o i i i C ( q, ε ) = C q p q q, (21) is the expeted ost of traversing ell q if the unertainty at this loation is Sine it is possible to reah a ell state vetor r suh that: ε. q with different unertainties, we will reate an augmented r = ( q, ε ) (22) hene defining a 3-D onfiguration spae where x and y are the first two dimensions, and ε is the third dimension. The planner used for planning with unertainty is a modified version of A* in 3-D. The planner wors as follows: we have a start loation q 0 with unertainty ε 0, an end loation q f with unertainty ε f 0 0 0, and a 2-D ost map C. We form the augmented state variable r = ( q, ε ) and plae it in the OPEN list. The state r with lowest expeted total ost to the goal is popped from the OPEN list. Next, r is expanded, and its suessors r +1 j are plaed in the OPEN list. Sine ε is funtion of q, the suessors of r are alulated only in the 2-D worspae defined by q, instead of the 3-D onfiguration spae defined by r (See Figure 12). As the states are plaed in the OPEN list, their unertainty is updated using (10), and the expeted total path ost of r +1 j is updated aording to (20). However, if any states within the unertainty region of r +1 j are labeled as OBST (obstales), then the expeted total path ost of r +1 j is set to infinity, therefore preventing any further expansion of that state. q q + 1 8 + 1 7 q + 1 1 q q + 1 2 q + 1 3 q + 1 6 q + 1 5 q + 1 4 Figure 12. Suessors of state q + 1 = Lie in A*, the proess desribed above is repeated until a ell r +1 suh that q q and 1 f ε + ε is popped out of the OPEN list. The path onneting the bapointers from r +1 to r 0 is the 0 f optimal path between q and q with a final unertainty lower than ε f. If the OPEN list beomes 0 f empty and no suh state has been found, then there is not path between q and q suh that the final f unertainty is lower than ε. 4.3 Using loalization regions The planner has the apability of using loalization regions, suh as areas with GPS overage. If a state propagation yields a distribution that is totally ontained in a GPS region (within 2σ), the unertainty of this state is set to zero (or other onstant value assoiated with the GPS region) instead of using (10) to propagate the unertainty. This allows the planner to hop from one GPS region to another, if the lowest ost path requires it. f
5 Results 5.1 Route planning example The following is an example of our planner applied to the problem of finding the best path between two loations on opposite sides of Indiantown Gap, PA. In Figure 13 we an see the results when unertainty is not being onsidered. We an use a regular 2-D planner for this tas, or we ould use our planner with an unertainty rate of zero. If the unertainty rate is zero, the third dimension of the planning s pae is ollapsed, and the searh spae beomes two-dimensional. Figure 14 shows the resulting path when we use our planner and a propagation model with an unertainty rate of 2%. Notie how the path is shortened in order to fit through the narrow opening in the lower left orner of the map. Figure 15 shows the resulting path when the unertainty rate is inreased to 4%. The path annot pass through the opening in the lower left orner, and now it has to go through the opening in the top right, produing a more ostly path. Figure 13. Path alulated with no unertainty. Total path ost : 9357 Figure 16 shows the resulting path for the same unertainty rate when there is a GPS region in the left side of the map (small retangle). In this ase the lowest ost path is obtained by going to the GPS region first (whih resets the unertainty to zero) and then going through the gap in the lower left orner (whih was not feasible without the GPS region)
Figure 14. Path alulated with unertainty rate u = 2.0%. Total expeted path ost: 20598 Figure 15. Path alulated with unertainty rate u = 4.0%. Total expeted path ost = 44107
Figure 16. Path alulated with unertainty rate u = 4.0%, and using GPS regions (small square area on the left). Total expeted path ost = 19458 5.2 Performane The spae omplexity of the urrent implementation of the planner is O( n n n ), where n x, ny and n u are the dimensions along the x, y and u diretions. The time omplexity is O( Q+ R), where: - 2 u ( x y ) u x y u Q = n n n is the number of operations required to alulate the expeted ost. - R = n n n log( n n n )) is the number of operations required by A* to alulate the path x y u x y u - u is the unertainty rate. For u > 0, the Q term dominates the time omplexity of the algorithm. If u = 0, or the alulation of the expeted value is performed beforehand, then the R term beomes the dominant one. However, beause of the unertainty is dependent on x and y, the states expanded by the algorithm are far fewer than n x n y n u, and the average time omplexity of the algorithm is onsequently muh lower as well. For binary-ost worlds, the searh spae for the algorithm is a 2-D manifold, with a one-lie shape around the start loation (Figure 17 shows a ross-setion of the searh spae ontaining the start loation). In this type of world, the number of propagations is O( n n ) For ontinuous-ost worlds, the searh spae is something between a 2-D manifold and a thin 3-D volume (Figure 18 shows a ross-setion of the searh spae ontaining the start loation). In this type of world the number of propagations is O( nx ny nu ' ), where n u ' is the average number of propagations per ell. Figure 19 shows n u ' vs nu for a bath of 1800 simulations with u varying between 1% and 10%, and nx = ny varying from 50 to 250 ells. We an see that even though n u ' does inrease with n u, it is always a small fration of n u. For n u = 100 the average value of n u ' is 3.4, and the maximum value is 7.9. x y
Figure 17. State expansion for binary ost world Figure 18. State expansion for ontinuous ost world Figure 19. Propagations per ell vs number of unertainty levels
6 Conlusions and Future Wor We have introdued an effiient path planner that alulates resolution-optimal paths while onsidering unertainty in position. Even though the planner uses three dimensions to represent the state variables, the omputational omplexity of the algorithm is very lose to that of a 2-D algorithm. The algorithm taes advantage of the small inrease in unertainty from one step to the next, to model the transitions in a semi-deterministi fashion. This enables the use of a deterministi planner suh as A* to solve the planning problem. Beause of the effiient use of the state spae, the algorithm does not require a lengthy preproessing stage as required by Roy and Thrun [8]. The planning time is under 1 seond for worlds up to 150x150, and under 10 seonds for worlds up to 250x250. Figure 20 shows the average planning time for 1800 simulations modeling 10 different worlds with sizes from 50x50 to 250x250 (in xy). The unertainty levels used varied from 1 to 100, and the unertainty rates from 1% to 10%. The proessor used was a Pentium M 1.4GHz. Our algorithm runs in signifiantly less time than algorithms that require pre-proessing of all states. Depending on the size of the world and the unertainty rate the speed up fator of our algorithm is between 8 and 80 times, depending on the size and harateristis of the world (See Figure 21). Future wor inludes produing a version of the algorithm that allows for more effiient replanning and exploring more omplex representations of the error propagation model. Figure 20. Planning time for different world sizes and unertainty rates
Figure 21. Speed up fator in total planning time ompared to preproessing all states. 7 Anowledgments This wor was sponsored by the U.S. Army Researh Laboratory under ontrat Robotis Collaborative Tehnology Alliane (ontrat number DAAD19-01-2-0012). The views and onlusions ontained in this doument do not represent the offiial poliies or endorsements of the U.S. Government.
Referenes [1] Latombe, J.C., Lazanas, A., and Shehar, S., "Robot Motion Planning with Unertainty in Control and Sensing," Artifiial Intelligene J., 52(1), 1991, pp. 1-47. [2] J. C. Latombe_Robot Motion Planning._Kluwer Aademi Publishers. [3] Lazanas, A., and Latombe, J.-C. 1992. Landmar-based robot navigation. In Pro. 10th National Conf. on Artifiial Intelligene (AAAI-92), 816--822. Cambridge, MA: AAAI Press/The MIT Press. http://iteseer.nj.ne.om/lazanas92landmarbased.html [4] B. Bouilly. Planifiation de Strategies de Deplaement Robuste pour Robot Mobile. PhD thesis, Insitut National Polytehnique, Tolouse, Frane, 1997 [5] B. Bouilly, T. Sim'eon, and R. Alami. A numerial tehnique for planning motion strategies of a mobile robot in presene of unertainty. In Pro. of the IEEE Int. Conf. on Robotis and Automation, volume 2, pages 1327--1332, Nagoya (JP), May 1995. http://iteseer.nj.ne.om/bouilly95numerial.html [6] Haït, A., Simeon, T. and Taïx, M. "Robust motion planning for rough terrain navigation".published in IEEE Int. Conf. on Intelligent Robots and Systems Kyongju, Korea,. http://iteseer.nj.ne.om/319787.html [7] Th. Fraihard and R. Mermond "Integrating Unertainty And Landmars In Path Planning For Car-Lie Robots" Pro. IFAC Symp. on Intelligent Autonomous Vehiles Marh 25-27, 1998. http://iteseer.nj.ne.om/fraihard98integrating.html [8] Roy, N. and Thrun, S. (1999). Coastal navigation with mobile robots. In Advanes in Neural Proessing Systems 12, volume 12, pages 1043--1049 [9] K. Goldberg, M. Mason, and A. Requiha. Geometri unertainty in motion planning. Summary report and bibliography. IRIS TR. USC Los Angeles [10] Kelly, A, "Linearized Error Propagation in Odometry", The International Journal of Robotis Researh. February 2004, vol. 23, no. 2, pp.179-218(40).