Planning with Uncertainty in Position: an Optimal and Efficient Planner

Transcription

1 Planning with Unertainty in Position: an Optimal and Effiient Planner Juan Pablo Gonzalez and Anthony (Tony) Stentz The Robotis Institute Carnegie Mellon University Pittsburgh, USA {jgonzale, Abstrat We introdue a resolution-optimal path planner that onsiders unertainty while optimizing any monotoni objetive funtion suh as mobility ost, ris, or energy expended. The resulting path minimizes the expeted ost 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. Although the problem domain is stohasti in nature, our algorithm taes advantage of deterministi path-planning tehniues to ahieve signifiant performane improvements. Index Terms - path planning, unertainty, optimal planner, error propagation, mobile robot. I. INTRODUCTION Current approahes to path planning freuently ignore the inherent unertainty in the position of mobile robots. If the position unertainty is small ompared to the size of the robot these approahes provide adeuate solutions. However, when the unertainty is large, it an have a signifiant impat on the robot s ability to follow a planned path and should not be ignored. The widespread use of GPS (Global Positioning System) alleviates the position estimation problem for outdoor mobile robots. However, there are many senarios where GPS overage is limited or unavailable: under tree anopies, in anyons, indoors, underground, and in many urban environments. In this paper we propose a resolution-optimal path planner that models position unertainty arising from drift in onboard dead-reoning, while optimizing any monotoni objetive funtion suh as mobility ost, ris, or energy expended. The environment is assumed to be nown, and 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 nearest edge. Nontraversable areas are assigned infinite ost and onsidered obstales. 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 its safety or the reahability of the goal. We assume a robot with a reasonably good deadreoning system (unertainty rate less than 0% of distane traveled), whih may or may not have GPS on board. We assume that the robot is not be able to detet the obstales in the environment that are represented in the map. Instead, the robot relies on the planner to eep it safe from these obstales. The planner uses a single-parameter Gaussian distribution to model position unertainty in order to minimize the number of dimensions reuired. We show that this model is a onservative estimate of the true error propagation model and, depending on the type of error that is dominant in the system, an provide an aurate approximation of the true model. By using this simplified unertainty model we are able to plan in a 3-D approximation of the belief spae of the robot using effiient deterministi path planning tehniues []. Thans to these tehniues, and beause of the harateristis of the resulting searh spae, our planner has a omputational omplexity lose to that of a deterministi 2-D planner. The planning time is less than one seond for worlds of up to 50x50 ells, and less than ten seonds for worlds of up to 250x250 ells, for unertainty rates between % and 0% of distane traveled. The planner also utilizes GPS-based loalization when it is available; in regions with GPS overage, the unertainty stops propagating and is redued to a fixed value. This paper is organized as follows: Setion II ontains a review of the relevant literature in the area of path planning with unertainty. Setion III explains the motion model and the unertainty propagation problem. Setion IV explains our approah to the problem. Setion V shows some results of the planner applied to syntheti and real geographi data and analyzes the performane of the algorithm. Setion VI ontains our onlusions and explores future diretions for this researh. II. RELATED WORK There is an abundane of wor addressing path planning with unertainty in the researh literature. In the field of lassial path planning, Latombe [2][3] has an extensive review on the state of the art as of 99. Sine then, important ontributions by Lazanas and Latombe [4], Bouilly [5][6], Haït et al. [7], Fraihard [8], and others have not only expanded the theoretial approahes to planning with unertainty, but also addressed some of its pratial Proessing times for a Pentium M,.4GHz.

2 limitations. There is, however, little wor aimed at reating paths that are optimal with respet to more general objetive funtions. Although the planner proposed by Bouilly [5] alulates an optimal path with respet to unertainty or path length, the approah is not appliable to finding optimal paths with respet to other important riteria suh as mobility ost, ris, or energy expended. In the field of partially observable Marov deision proesses (POMDPs), the problem of planning with unertainty has been freuently addressed. However, most algorithms beome omputationally intratable when dealing with worlds with a large number of states. To the best of our nowledge, only Roy and Thrun [9] have solved the problem of finding optimal paths for large, ontinuous-ost worlds in the presene of unertainty. The planner they propose inludes some of the elements of the planner proposed here but is based on an approximate solution to a POMDP. This approah reuires pre-proessing of all the states in the searh spae, whih later allows for very fast planning. However, the total planning time (inluding the pre-proessing stage) an tae from several minutes to a few hours [0]. The alternative we are presenting is a semi-deterministi approah based on A*, whih inorporates some of the advantages of both approahes (lassi path planning and POMDPs), and whih has muh lower average time omplexity than the wor of Roy and Thrun. III. MOTION MODEL AND UNCERTAINTY PROPAGATION The first-order motion model for a point-sized robot moving in two dimensions is: x& () t = v()os t θ () t yt &() = vt ()sin θ() t () & θ() t = ω() t 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). Euation () an also be expressed as: & () t = f( (), t u()) t (2) where ( t) = ( x( t), y( t), θ ( t)) and u () t = ( v(), t ω()) t. A typial sensor onfiguration for a mobile robot is to have an odometry sensor and an onboard gyro. We an model the errors in the odometry and the gyro as errors in the inputs where wv () t is the error in vt ()(error due to the longitudinal speed ontrol), and wω () t is the error in ω () t (error due to the gyro random wal). Inorporating these error terms into () yields: x& () t = ( v() t + wv ())os t θ () t yt &() = ( vt () + wv ())sin t θ () t (3) & θ() t = ω() t + w () t ω or, in disrete-time: x( + ) = x( ) + ( v( ) + wv ( ))os θ ( ) t y ( + ) = y ( ) + ( v ( ) + wv ( ))sin θ ( ) t θ( + ) = θ( )+ ( ω( ) + w ( )) t 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: where ( ) ( ) ( ) ( ) T T P + = F P F + L( ) Q( ) L ( ) (5) 2 P T σ v 0 ( ) = E( ˆ( ) ˆ( ) ) Q ( ) = 2 t 0 σ (6) ω f( i( ), uj( )) Fij = i 0 v ( t)sin( θ ( )) t F = 0 v ( t)os( θ ( )) t 0 0 f( i( ), uj( )) Lij = u j os( θ ( )) t 0 L = sin( θ ( )) t 0 0 t Although there are no general losed-form solutions for these euations, Kelly [2] alulated losed-form solutions for some trajetories, and showed that a straight line trajetory maximizes eah one of the error terms. We an use the results for this trajetory as an upper bound on the error for any trajetory. For a straight trajetory along the x-axis eeping all inputs onstant, the error terms behave as follows. The error due to the longitudinal speed ontrol w v is refleted in the x 2 2 diretion, and is given by σx = σv vt, or σx = σv vt. The error due to the gyro random wal w ω is refleted in the y ω y diretion, and is given by σ = σ ω vt, or σ y = σ ω v t. 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, propagate linearly with t. For initial angle errors of less than 5 degrees, the small angle approximation ( sinθ θ ) an be used to obtain the expression σy = σ θ. vt. o The dominant terms in the error propagation model depend on the navigation system and on the planning horizon for the robot. A typial senario for a mobile robot with good inertial sensors is to have a planning horizon of up to 3m, at a speed of 5 m/s, with longitudinal ontrol error of 0% of the 3/2 (4) (7) (8)

3 ommanded speed ( σ v = 0. v = 0.5 m/ s) and gyro drift (random wal) of 20 / h/ Hz ( σ ω = / s 5 = rad / s). In this senario, the dominant term is the error in the longitudinal ontrol. However, if there is unertainty in the initial angle, this term beomes the dominant one for errors as small as 0.5 degrees (See Figure ). Figure 2 shows the results of ombining all the types of error (with the values mentioned above, and an initial heading angle error of 0.5 degrees) for a straight trajetory. Figure 3 shows the same analysis for a random trajetory. See [3] for a more detailed analysis of the different modes of error propagation. IV. PLANNING WITH UNCERTAINTY As shown in the previous setion, unertainty in the position of a robot an uily arue even when the robot is euipped with good onboard sensors. It is thus important to inorporate position unertainty into the planning proess in order to generate paths that are robust to this unertainty. Figure 3. Error propagation for a random trajetory with all error soures ombined and inset showing detail. The blue dots are the Monte Carlo simulation, the red ellipse is the EKF model (2σ), and the bla irle is the single parameter approximation (2σ). A. Simplified error propagation model In order to eep the planning problem tratable and effiient, we use a single-parameter error propagation model. We approximate the probability density funtion (pdf) of the error with a symmetri Gaussian distribution, entered at the most liely loation of the robot at step : = ( x, y) (9) : N (, σ ) where is the most liely loation of the robot at step, and σ = σx = σ y is the standard deviation of the distribution. Let us define: ε = 2 σ (0) Figure. Comparison between different values of initial angle error and longitudinal ontrol error We an then model the boundary of the unertainty region as a dis entered at with a radius ε. As shown in the previous setion, this model is a onservative estimate of the true error propagation model and, depending on the type of error that is dominant in the system, an provide an aurate approximation of the true model. Sine the dominant term in the error propagation for our planning horizon is linear with distane traveled, we use the following model to propagate the unertainty: ud ε( )= ε( ) + α (, ) () Figure 2. Error propagation for a straight trajetory with all error soures ombined and inset showing detail. The blue dots are the Monte Carlo simulation, the red ellipse is the EKF model (2σ), and the bla irle is the single parameter approximation (2σ) where α u is the unertainty arued per unit of distane traveled, is the previous position along the path, and d(, ) is the distane between the two adjaent path loations and. Euivalently, we an define the unertainty at loation as: 0 0 ud ε( )= ε( ) + α (, ) (2)

4 0 where is the most liely initial loation of the robot, and 0 D(, ) is the total distane traveled along the path from 0 to. The unertainty rate α u is typially between 0.0 and 0. (% to 0%) 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 =(x,y). B. Implementation In a deterministi planner, the loation of the robot on the plane is usually defined as =(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 robot s position is p(, ε ) (3) where is a random variable representing the position of the robot, is the most liely loation of the robot, and ε is the unertainty in the position. As mentioned previously, this probability density funtion is modeled as a Gaussian distribution entered at and with standard deviation σ = ε /2. Having a probability distribution over possible robot positions rather than a fixed loation means that we are planning over a belief spae of the robot []. The belief spae for this simplified model an be represented by the augmented state variable r = (, ε ) (4) hene defining a 3-D onfiguration spae where x and y are the first two dimensions, and ε is the third dimension. In order to use a deterministi planner to plan over this spae we need to define the transition ost between adjaent ells. In our 3-D onfiguration spae, we are interested in alulating the ost of moving between the onfiguration r (at path step ) and an adjaent onfiguration r + (at path step +). This is euivalent to alulating the expeted ost of going from a most liely worspae loation, with unertainty ε to an adjaent most liely worspae loation + with unertainty ε + (See [3] for details): ( ) C( r, r + ) = E C (, ε ),( +, ε + ) (5) Euivalently, (6) E + + C( (, ε ),(, ε )) = i j C (, ) p(,, ε,, ε ) o i j i j i + where is eah of the i possible states at path step, j is eah of the j possible states at path step +, and + C o ( i, j ) is the deterministi ost of traveling from i + to j (see Figure 4) 2. Sine we are assuming a low unertainty rate ( α u < 0. ), we an mae additional simplifiations that transform (6) into: + ((, ),( +, ε ε )) E C = a C (, ) + (, ) + E ε bce ε (7) where a and b are onstants determined by the relative + position of and, and ( ) ( ε ) E o i i i C (, ε ) = C p, (8) is the expeted ost of traversing ell this loation is ε. Therefore, + + (, ) = E(, ε) + E(, ε ) if the unertainty at C r r a C bc. (9) The planner used for planning with unertainty is a modified version of A* in 3-D in whih the suessors of eah state are alulated only in a 2-D plane, and state dominane is used to prune unneessary states. The planner wors as follows: we have a start loation 0 with unertainty ε 0, an end loation f with unertainty ε f, and a 2-D ost map C. We form the augmented state variable r = (, ε ) and plae it in the OPEN list. States in the OPEN list are sorted by its expeted total ost to the goal: 0 0 ( ) C (, ε ) = C (, ε ),(, ε ) + h (, ) (20) ET E E f y ε Figure 4. p( i, ε) + and the state transitions from r = (, ε ) to = ε r (, ) x i ε + + j 2 As mentioned previously, we assume a disretized grid of states orresponding to a nown map.

5 0 0 where CE ((, ε ),(, ε )) is the expeted ost of the best path from the start loation r 0 = ( 0, ε 0 ) to the urrent state r = (, ε ), and he (, f ) is the heuristi of the expeted ost from the urrent state to the goal. The heuristi used is a funtion of the Eulidean distane between and f. The state r with lowest expeted total ost to the goal is popped from the OPEN list. Next, r is expanded. To determine if a suessor + ( +, + rj = j ε j ) should be plaed in + the OPEN list, we use state dominane as follows: a state r j is plaed in the OPEN list if + ) no states with (x,y) oordinates j have been expanded, or + 2) the expeted ost from r 0 to rj is lower than the ost + to any other state with the same (x,y) oordinates j, or 3) the unertainty ε + j is lower than the unertainty of + any other state with the same (x,y) oordinates j In other words, a state is only expanded if it an provide a path with lower unertainty or lower ost from the start loation. Additionally, sine ε is a funtion of, the suessors of r may be alulated in the 2-D worspae defined by, instead of the full 3-D onfiguration spae defined by r (See Figure 5). As the states are plaed in the OPEN list, their unertainty is updated using (), and the expeted ost of r + j is updated aording to (7) and (20). However, if any states within 2σ in the unertainty region of r + j are labeled as obstales then the + expeted total path ost of r j is set to infinity, thereby preventing any further expansion of that state. As in A*, the proess desribed above is repeated until a ell r + with the same (x,y) oordinates as the goal position + = f ( ), and unertainty less than the target unertainty f ( ε + ε ) is popped off the OPEN list. The path onneting the bapointers from r + to r 0 is the optimal path between 0 and f with a final unertainty lower than ε f. If the OPEN list beomes empty and no suh state has been found, 0 f then there is not a path between and suh that the final f unertainty is lower than ε. y x Figure 5. Suessors of state C. Using loalization regions The planner an also tae advantage of loalization regions, suh as areas with GPS overage. If a state propagation yields a distribution that is ontained in a GPS region (within 2σ), the unertainty of this state is set to zero (or some onstant value assoiated with the auray of GPS within the region) instead of using () to propagate the unertainty. This approximation preserves the deterministi nature of the belief spae. V. RESULTS A. 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. The reuired tas is to find the path with lowest ost from the start to the end loation, with a final unertainty smaller than 500 m, and whih avoids all nown obstales in the map (within 2σ). The ost map used is defined as the mobility ost of traveling through eah ell in the map, and is proportional to the slope of the terrain. Slopes greater than 30 o are onsidered obstales. This example shows the results of using our planner to alulate a path with unertainty rates of 0%, 2% and 4% of distane traveled, and uses the results of a Monte Carlo simulation to illustrate the importane of planning with unertainty. In Figure 6 we an see the results when unertainty is not being onsidered. Figure 7 shows the resulting path when we use our planner and a propagation model with an unertainty rate of 2%. Figure 8 shows the resulting path when the unertainty rate is inreased to 4%. In order to understand the importane of planning with unertainty, a set of Monte Carlo simulations were run for eah of the previous three senarios with and without onsidering unertainty: ) Planning without onsidering unertainty: we planned a path without unertainty, and simulated exeutions with unertainty rates of 0%, 2% and 4%. 2) Planning onsidering unertainty: we planned and simulated exeutions with unertainty rates of 0%, 2% and 4%. In both ases, we alulated the probability of ollision as the perentage of simulations that resulted in a trajetory through obstales. Table I shows the results of the simulations. When there is no unertainty (for example, if there is good GPS overage throughout the path), the planner that onsiders unertainty produes the same path as the planner without it (as expeted).

6 TABLE I COMPARISON BETWEEN PLANNING WITH AND WITHOUT UNCERTAINTY Unert. rate Planner without unertainty Expeted Average Prob. of ost ost ollision Planner with unertainty Expeted Average ost ost Prob. of ollision 0% % % 2% % % 4% % % Position in y, m Figure 8. Path alulated with unertainty rate α u = 4.0%. Position in y, m Figure 6. Path alulated assuming no position unertainty. The start loation is the small suare in the top left, and the goal is the small irle in the bottom Position in y, m Figure 7. Path alulated with unertainty rate α u = 2.0%. If there is a 2% unertainty rate in the motion model, and we fail to aount for it, the average ost of the resulting path is 27705, and the probability of ollision is % (The expeted ost of this path as reported by the planner with no unertainty is 9337). If we use our planner with unertainty, the average ost is 9052 (30% lower). The expeted ost alulated by the planner (8679), is also muh loser to the average ost of the simulations, and the probability of ollision is signifiantly lower (5%). If there is a 4% unertainty rate in the motion model but we do not aount for it, the average ost of the resulting path is 37726, while the expeted ost as alulated by the planner is still The probability of ollision is now 36%. When we use the planner that onsiders unertainty to solve this ase, the average ost is 49727, the expeted ost 42677, and the probability of ollision 5%. The ollision heing riterion for the planner with unertainty is 2σ, whih implies that paths with probabilities of ollision of more than 5% would be rejeted. Under this riterion, the path returned by the planner without unertainty onsiderations is not a feasible path (36% probability of ollision). B. Route Planning with GPS regions As disussed earlier, our planner an use GPS regions to improve the loalization of the robot while planning. Figure 9 shows the resulting path for a 4% 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) C. Complexity The worst-ase spae omplexity of the urrent implementation of the planner is On ( x ny nu), where n x, ny and n u are the dimensions along the x, y and u diretions. The average time omplexity is OQ ( + R), where 2 Q = αu ( nx ny) nu is the number of operations reuired to alulate the expeted ost, R = nx ny nu log( nx ny nu)) is the number of operations reuired by A* to alulate the path, and α 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.

7 Position in y, m Figure 9. Path alulated with unertainty rate α u = 4.0%, and using GPS regions (small suare area on the left). However, beause the unertainty is dependent on x and y, the states expanded by the algorithm when using state dominane are far fewer than nx ny nu, and the average time and spae omplexity of the algorithm is onseuently muh lower as well. The omplete searh spae is a 3-D volume of dimensions nx ny nu. State dominane redues it to thin 3- D volume in most ases. Figure 0 shows an example where state dominane greatly redues the size of the searh spae. The figure on the enter shows a ross-setion of the searh spae at y=20, and the figure on the right shows the same ross-setion when state dominane is used. We an see that state domi nane produes a searh spae with signifiantly fewer states expanded (the searh spae on the right is only a few states thi, while the one on the enter is a full 3-D volume). The average number of states expanded by the algorithm is nx ny nu, where n u is the average number of propagations along the unertainty axis (the thiness of the searh spae). n u is typially muh smaller than the size of the unertainty dimension ( n u ), maing n x n y n u << n x n y n u. Figure shows n u vs. n u for a bath of 800 simulations with α u varying between % and 0%, 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 = 00 the average value of n u is 3.4, and the maximum value is 7.9. D. Performane Our algorithm taes signifiant advantage of two tools available for deterministi searh: heuristis and state dominane. For this reason, the algorithm does not reuire a lengthy pre-proessing stage as reuired by Roy and Thrun [9]. In pratie, we have found the planning time to be under seond for worlds up to 50x50, and under 0 seonds for worlds up to 250x250. Figure 2 shows the average planning time for 800 simulations modeling 0 different worlds with sizes from 50x50 to 250x250. The unertainty levels used varied from to 00, and the unertainty rates from % to 0%. The proessor used was a Pentium M.4GHz. Our algorithm reuires signifiantly fewer state expansions than proessing all the states in the searh spae. Depending on the size of the world and the unertainty rate the speed-up fator of our algorithm is between 8 and 80, depending on the size and harateristis of the world (see Figure 3). VI. CONCLUSIONS AND FUTURE WORK We have introdued an effiient path planner that alulates resolution-optimal paths while onsidering unertainty in position. The resulting paths are safer and have lower average osts than paths alulated without onsidering unertainty. Position in y, Unertainty Unertainty Position in x, Figure 0. Effet of state dominane on reduing the searh spae. Left: sample world used and resulting path (darer areas are higher ost, and green areas are non-traversable). Center: ross-setion of state expansion in (x,y,ε) for y=20 without state dominane (blue dots are states expanded, solid green are obstales). Right: same ross-setion with state dominane. Notie how the number of states expanded is signifiantly redued.

8 approah runs signifiantly faster sine it expands far fewer states. Future wor inludes produing a version of the algorithm that allows for more omplex representations of the error propagation model, as well as the ability to use obstales and features in the environment for loalization during the planning proess. Figure. Propagations per ell vs number of unertainty levels Figure 2. Planning time for different world sizes and unertainty rates Figure 3. Speed up fator in total planning time ompared to preproessing all states. The algorithm provides an alternative approah to the wor of Roy and Thrun [9]. Our approah models a stohasti problem as a deterministi searh over a simplified belief spae, enabling the use of a deterministi planner suh as A* to solve the planning problem. Beause of the harateristis of the searh spae and the motion model, there are signifiant performane gains from the use of state dominane. Our ACKNOWLEDGMENTS The authors would lie to than Drew Bagnell and David Ferguson for their input at different stages in the development of this researh. This wor was sponsored by the U.S. Army Researh Laboratory under ontrat Robotis Collaborative Tehnology Alliane (ontrat number DAAD ). The views and onlusions ontained in this doument do not represent the offiial poliies or endorsements of the U.S. Government. REFERENCES [] B. Bonet and H. Geffner, "Planning with inomplete information as heuristi searh in belief spae," in Proeedings of the 6th International Conferene on Artifiial Intelligene in Planning Systems (AIPS), pp. 52-6, AAAI Press, [2] J.C. Latombe, A. Lazanas, and S. Shehar, "Robot Motion Planning with Unertainty in Control and Sensing," Artifiial Intelligene J., 52(), 99, pp [3] J.C. Latombe, Robot Motion Planning. Kluwer Aademi Publishers. [4] A. Lazanas, and J.C. Latombe, Landmar-based robot navigation. In Pro. 0th National Conf. on Artifiial Intelligene (AAAI-92), Cambridge, MA: AAAI Press/The MIT Press. [5] B. Bouilly. Planifiation de Strategies de Deplaement Robuste pour Robot Mobile. PhD thesis, Insitut National Polytehniue, Tolouse, Frane, 997 [6] B. Bouilly, T. Siméon, and R. Alami. A numerial tehniue 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 , Nagoya (JP), May [7] A. Haït, T. Simeon, and M. Taïx, "Robust motion planning for rough terrain navigation".published in IEEE Int. Conf. on Intelligent Robots and Systems Kyongju, Korea. [8] 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, [9] N. Roy, and S. Thrun, Coastal navigation with mobile robots. In Advanes in Neural Proessing Systems 2, volume 2, pages [0] N. Roy, Private Conversation. September, 2004 [] K. Goldberg, M. Mason, and A. Reuiha. Geometri unertainty in motion planning. Summary report and bibliography. IRIS TR. USC Los Angeles [2] A. Kelly, "Linearized Error Propagation in Odometry", The International Journal of Robotis Researh. February 2004, vol. 23, no. 2, pp.79-28(40). [3] J.P. Gonzalez and A. Stentz, Planning with Unertainty in Position: an Optimal Planner, teh. report CMU-RI-TR-04-63, Robotis Institute, Carnegie Mellon University, November, 2004.