Robot Motion Planning and Control-Springer

242 A. De Luca, G. Oriolo and C. Samson Fig, 37. Point stabilization with nonsmooth time-varying feedback (I): ~ (tad) vs. time (sec) Z o 2o 4o m m lOO izo 14o Fig. 38. Point stabilization with nonsmooth time-varying feedback (I): ¢ (red) vs. time (see) Ii ) iiiiIillii o .1 .2 ~o 4o eo m loo Izo i,lo Fig. 39. Point stabilization with nonsmooth time-varying feedback (I): vl (m/sec) vs, time (sec)

Feedback Control of a Nonholonomic Car-Like Robot 243 .! -2 Fig. 40. Point stabilization with nonsmooth time-varying feedback (I): v2 (rad/sec) vs. time (sec) ...........................i..........i............................. ........... i....... i ....... i ................. !....... Fig. 41. Point stabilization with nonsmooth time-varying feedback (II): cartesian motion 2O 40 e3 m ~ o t ~ Fig. 42. Point stabilization with nonsmooth time-varying feedback (II): x (m) vs. time (sec)

244 A De Luca, G. Oriolo and C. Samson ...... i. . . . . . . . . i .........i....................i................i ...............i...............i............... .t . ............. i .......... !. .......... i ............. i .............. ~= ............. ................ i ................ i ............... i ................ i ............... !4 Fig. 43. Point stabilization with nonsmooth time-varying feedback (II): y (m) vs. time (sec) i ............. i...............i................i............... i............... i............... .o.ii | , , ,,| , Fig. 44. Point stabilization with nonsmooth time-varying feedback (II): 0 (rad) vs. time (see) t !!!!! oJ ............. ~................ ! ............... 4 ............... i ................ ! ............... ..........i......*, ............................i ................i...............~ ............... / Fig. 45. Point stabilizationwith nonsmooth time-varying feedback (II):¢ (rad) vs. time (sec)

Feedback Control of a Nonholonomic Car-Like Robot 245 2.'.- , !............... ? ............... i................ ? ............... 2 .......................... 1: ......... ................i..... i i o~ .......... : ........... i ............... 4 ............. i ................ i .............. o ........... i. . . . . . . i i .... .~ . . . . . . . . . . . . . . . ; ............. . . . . . . . . . . . . . . . ~. . . . . . . . . . . . . . :~. . . . . . . . . . . . -1 s ~2o Fig. 46. Point stabilization with nonsmooth time-varying feedback (II): vl (m/sec) vs. time (see) 2 Q .....,2122i1212'i112112112121112121112121212211211211111121121Zil;i211111212i21121121211111 ~~ ...... ; ,o ® ,,o , & ' ,~o Fig. 4?. Point stabilization with nonsmooth time-varying feedback (II): v2 (rad/sec) vs. time (sec)

246 A. De Luca, G. Oriolo and C. Samson 4.3 About exponential convergence The peculiar convergence behavior of both presented stabilizing methods de- serves some comments. We have already pointed out in Sect. 2.2 that the failure of the linear controllability test for the car-like robot indicates that smooth ex- ponential stability in the sense of Lyapunov cannot be obtained. Recall that (local) exponential stability means that the system trajectories X(t) satisfy the following inequality HX(t)H <_KlJX(to)lle -~(t-t°), VX(to) ~ B, Vt > to, (90) with K, A positive real numbers and B a neighborhood of the origin. The prac- tical significance of this relationship is twofold: (i) small initial errors cannot produce arbitrarily large transient deviations since IIX(t)ll < KIIX(to)l I, and (ii) all solutions converge to zero exponentially. While it is still unclear whether both properties can be simultaneously achieved for nonholonomic systems, one can still design a control law that guarantees at least one of the two. In the case of smooth time-varying feedback laws, such as the one presented in Sect. 4.1, it may be easily verified that tlX(t)l I _<KllX(to)ll, VX(to), Vt ___to, (91) holds for some positive constant K. However, when using the control law w2 of Prop. 4.2, convergence to zero of ]lZ[I (and hence, of IIXIf) cannot be exponen- tial. In fact, if this were the case, ul would itself converge to zero exponentially, and thus the integral f to lul('r)]dT would not diverge. This is in contradiction with the fact that divergence of this integral is necessary for the asymptotic convergence of tIZ2ll to zero. As a matter of fact, it is only possible to show that [[X(t)[] _<K[IX(to)[lP(t), with p(O) = 1, thin p(t) = 0, (92) where p(t) is a decreasing function whose convergence rate is strictly less than exponential. This theoretical expectation is confirmed by the simulations results of Sect. 4.1. In particular, it has been observed [41] that smooth time-varying feedback control applied to a unicycle yields a convergence rate slower than t-1/2 for most initial configurations, a fact that can be proven using center manifold theory. On the other hand, existing nonsmooth feedback laws for nonholonomic systems do not guarantee uniform boundedness of the transient error ratio IIX(t)ll/llx(to)ll. For example, the piecewise-continuous time-invariant feed- back law proposed in [8] for the stabilization of a unicycle yields Hx(t)l[ <_(K1 + K21lX(to)N)e -~'(t-t°), VX(to), Vt >__to, (93)

Feedback Control of a Nonholonomic Car-Like Robot 247 with K1, K2 positive real numbers. All solutions converge to zero exponentially, but a small initial error or perturbation may produce transient deviations whose size is larger than some constant. Similarly, we have seen that the nonsmooth time-varying feedback of Sect. 4.2 guarantees ]~-exponential stability for general chained-form systems. Even if all solutions converge to zero exponentially, this type of asymptotic stability is weaker than property (90), in the sense that small initial errors or perturbations can produce transient deviations of much larger amplitude. Nev- ertheless, it is stronger than (93), for such deviations are not bounded below by some positive constant. The above discussion may suggest that smooth time-varying feedback laws are somewhat less sensitive to initial errors than nonsmooth feedback laws. This degree of robustness is paid in terms of the asymptotic rate of conver- gence, which is not exponential. However, smooth time-varying feedback may be modified to achieve practical exponential stability, in the sense that the sys- tem state may be steered to any desired small neighborhood of the origin in arbitrary time. This fact is illustrated by the simulation results obtained with the heat function qa in Sect. 4.1. 5 Conclusions We have presented and compared several feedback solutions for point stabiliza- tion, path following and trajectory tracking control tasks executed by a mobile robot with car-like kinematics. The problem of accurate tracking of a persistent trajectory can be solved using either linear control synthesis, based on the approximate linearization of the system around the nominal trajectory, or nonlinear (static or dynamic) control synthesis, achieving exact linearization of the (input-output or full- state) closed-loop equations. Local exponential convergence to zero tracking error is obtained in the linear case, while global exponential convergence with prescribed error dynamics is guaranteed in the nonlinear case. In both ap- proaches, the closed-loop controller consists of a nominal feedforward term and of an error feedback action. For the stabilization to a fixed configuration, the use of new classes of time- varying nonlinear controllers has proven to be effective. From a theoretical point of view, time-varying feedback overcomes the obstruction on the existence of smooth time-invariant stabilizing control laws for nonholonomic systems. Two types of time-varying control laws were presented, respectively expressed by a smooth and a nonsmooth function of the robot state. In both cases, we have recognized that path following can be formulated as a subproblem of point stabilization. The asymptotic rate of convergence of the smooth controller is

248 A. De Luca, G. Oriolo and C. Samson lower than the exponential orie obtained in the nonsmooth case. However, it may be questioned whether the theoretical convergence rate alone is a good measure of the overall control performance. In practice, what really matters is a rapid initial decay of the error to a small neighborhood of zero under realistic experimental conditions. The reported numerical simulations have shown the benefit of feedback control in recovering from initial errors with respect to the desired fixed or moving target. In order to fully appreciate these results, we remark that errors (at the initial time or later) can be interpreted as the effect of an instantaneous disturbance acting on the system. Therefore, the robot motion under feedback control is robust with respect to such non-persistent disturbances. Most of the results have been presented using a canonical transformation of the system into chained form. Although the use of chained forms is not needed in principle, it allows to obtain systematic results that can be extended beyond the considered case study of a car-like mobile robot. For example, the control results hold true also for a car towing N trailers, each attached at the midpoint of the rear axle of the previous one (zero hooking). On the other hand, the control problem for the general case of N trailers with nonzero hooking is still open, because a chained-form transformation is not available for this system. Throughout this study, we have dealt with a first-order kinematic model of the mobile robot, in which velocities were assumed to be the control in- puts. Extension to second-order kinematics, with accelerations as inputs, and inclusion of vehicle dynamics, with generalized forces as inputs, are possible. In particular, we point out that the nominal dynamics of the vehicle can be completely canceled by means of a nonlinear state feedback so as to obtain a second-order, purely kinematic problem. Concerning the application of the proposed feedback controllers to real mo- bile robot systems, there are several non-ideal conditions that may affect the actual behavior of the controlled robot, notably: uncertain kinematic parame- ters of the vehicle (including, e.g., the wheels' radius); mechanical limitations such as backlash at the steering wheels and limited range of the steering an- gle; actuator saturation and dead-zone; noise and biases in the transformation from physical sensor data to the robot state; quantization errors in a digital implementation. Control robustness with respect to these kinds of uncertainties and/or disturbances is an open and challenging subject of research. For linear as well as nonlinear systems, Lyapunov exponential stability implies some degree of robustness with respect to perturbations. However, since this kind of stability has not been demonstrated for the point stabilization problem of nonholonomic systems, the connection between robustness properties and asymptotic (even exponential) rate of convergence is not yet well understood. It should also be noted that perturbations acting on nonholonomic mobile robots are not of equal importance, depending on which component of the state

Feedback Control of a Nonholonomic Car-Like Robot 249 is primarily affected. A deviation in a direction compatible with the vehicle mobility (e.g., sliding of the wheels on the ground) is clearly not as severe as a deviation which violates the kinematic constraints of the system (e.g., lateral skidding of the car-like robot). In any case, proprioceptive sensors may not reveal these perturbing actions and all the controllers presented in this chapter--which assume that the exact robot state is available--would fail in completing their task. A possible solution would be to close the feedback loop using exteroceptive sensor measurements, which provide absolute information about the robot location in its workspace. Currently, it is not clear whether the best solution would be to estimate the robot state from these measurements and then use the previous controllers, or to design new control laws aimed at zeroing the task error directly at the sensor-space level. 6 Further reading In addition to the references cited to support the results so far presented, many other related works have appeared in the literature. Hereafter, we mention some of the most significant ones. A detailed reference on the kinematics of wheeled mobile robots is [2]. The dynamics of general nonholonomic systems was thoroughly analyzed in [31]. A controllability study for kinematic models of car-like robots with trailers was presented in [24], while stabilizability results for both kinematic and dynamic models of nonholonomic systems were given in [5,7]. The problem of designing input commands that drive a nonholonomic mobile robot to a desired configuration has been first addressed through open-loop techniques. Purely differential-geometric approaches were followed in [23,50], while the most effective solutions have been obtained by resort- ing to chained-form transformations and sinusoidal steering [28], or by using piecewise-constant functions as control inputs [26]. In [36] it was shown how the existence of differentially flat outputs can be exploited in order to design efficiently open-loop controls. A number of works have dealt with the problem of controlling via feedback the motion of a unicycle. In fact, both discontinuous and time-varying feedback controllers were first proposed and analyzed for this specific kinematics. The trajectory tracking problem was solved in [39] by means of a local feedback action. Use of dynamic feedback linearization was proposed in [14]. A piecewise- continuous feedback with an exponential rate of convergence was presented in [8] for the point stabilization task, and later extended to the path following problem in [47]. Another piecewise-continuous controller, obtained through an appropriate switching sequence, was devised in [5]. The explicit inclusion of the exogenous time variable in a smooth feedback law was proposed in [38].

250 A. De Luca, G. Oriolo and C. Samson In [34], a hybrid stabilization strategy was introduced that makes use of a time-invariant feedback law far from the destination and of a time-varying law in its vicinity. The use of a discontinuous transformation in polar coordinates allowing to overcome the limitation of Brockett's theorem was independently proposed in [1] and [3] for the point stabilization problem; strictly speaking, these schemes are not proven to be stable in the sense of Lyapunov, for they only ensure exponential convergence of the error to zero. A survey of control techniques for the unicycle can be found in [9]. For car-like robots, the trajectory tracking problem was also addressed in [13] through the use of dynamic feedback linearization, and in [16] via flat outputs design and time-scaling. Path following via input scaling was proposed in [15,37]. As for the point stabilization problem, the successful application of time-varying feedback to the case of car-like robots [43] has subsequently moti- vated basic research work aimed at exploring the potentialities of this approach. In particular, results have been obtained for the whole class of controllable drift- less nonlinear systems in [11,12], while general synthesis procedures were given in [33] for chained-form systems and in [35] for power-form systems; in the lat- ter, the use of a nonsmooth but continuous time-varying feedback guarantees exponential convergence to the desired equilibrium point. Using an analysis based on homogeneous norms, similar results were obtained for driftless sys- tems in [30], and for chained-form systems in [27] by means of a backstepping technique. Other related works include [17] and [51]. In the first, the problem of approximating a holonomic path via a nonholonomic one is solved by using time-periodic feedback control. In the second, the open-loop sinusoidal steer- ing method is converted to a stabilization strategy, by adding to the nominal command a mixed discontinuous/time-varying feedback action. Very few papers have explicitly addressed robustness issues in the control of nonholonomic systems. The robustness of a particular class of nonsmooth controllers based on invariant manifolds was analyzed in [10]. Robust stabiliza- tion of car-like robots in chained form was obtained in [4] and [25] by applying iteratively a contracting open-loop controller; exponential convergence to the desired equilibrium is obtained for small model perturbations. Another possible approach to the design of effective control laws in the presence of nonidealities and uncertainties is represented by learning control, as shown in [32]. Finally, the design of sensor-levelcontrollers for nonholonomic mobile robots is at the beginning stage. The general concept of task-driven feedback control for holonomic manipulators is described in [40]. A first attempt to extend this idea to the point stabilization problem of a mobile robotic system can be found in [52].

Feedback Control of a Nonholonomic Car-Like Robot 251 References 1. M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, \"Closed loop steering of unicycle-like vehicles via Lyapunov techniques,\" IEEE Robotics 6J Automation Mag., vol. 2, no. 1, pp. 27-35, 1995. 2. J. C. Alexander and J. H. Maddocks, \"On the kinematics of wheeled mobile robots,\" Int. J. of Robotics Research, vol. 8, no. 5, pp. 15-27, 1989. 3. A. Astolfi, \"Exponential stabilization of a mobile robot,\" 3rd European Control Conf., Roma, I, pp. 3092-3097, 1995. 4. M. K. Bennani and P. Rouchon, \"Robust stabilization of flat and chained sys- tems,\" 3rd European Control Conf., Roma, I, pp. 2642-2646, 1995. 5. A. M. Bloch, M. Reyhanoglu, and N. H. McClamroch, \"Control and stabilization of nonholonomic dynamic systems,\" IEEE Trans. on Automatic Control, vol. 37, no. 11, pp. 1746-1757, 1992. 6. R.W. Brockett, \"Asymptotic stability and feedback stabilization,\" in Differential Geometric Control Theory, R. W. Brockett, R. S. Millman, H. J. Sussmann (Eds.), Birkh~iuser, Boston, MA, pp. 181-191, 1983. 7. G. Campion, B. d'Andrea-Novel, and G. Bastin, \"Modeling and state feedback control of nonholonomic mechanical systems,\" 30th IEEE Conf. on Decision and Control, Brighton, UK, pp. 1184-1189, 1991. 8. C. Canudas de Wit and O. J. Sordalen, \"Exponential stabilization of mobile robots with nonholonomic constraints,\" IEEE Trans. on Automatic Control, vol. 37, no. 11, pp. 1791-1797, 1992. 9. C. Canudas de Wit, H. Khennouf, C. Samson, and O. J. Sordalen, \"Nonlinear control design for mobile robots,\" in Recent Trends in Mobile Robots, Y. F. Zheng (Ed.), World Scientific Publisher, 1993. 10. C. Canudas de Wit and H. Khennouf, \"Quasi-continuous stabilizing controllers for nonholonomic systems: Design and robustness considerations,\" 3rd European Control Conf., Roma, I, pp. 2630-2635, 1995. 11. J.-M. Coron, \"Global asymptotic stabilization for controllable systems without drift,\" Mathematics of Control, Signals, and Systems, vol. 5, pp. 295-312, 1992. 12. J.-M. Coron, \"Links between local controllability and local continuous stabi- lization,\" 2nd IFAC Syrup. on Nonlinear Control System Design, Bordeaux, F, pp. 477-482, 1992. 13. B. d'Andrea-Novel, G. Bastin, and G. Campion, \"Dynamic feedback linearization of nonholonomic wheeled mobile robots,\" 1992 IEEE Int. Conf. on Robotics and Automation, Nice, F, pp. 2527-2532, 1992. 14. A. De Luca and M. D. Di Benedetto, \"Control of nonholonomic systems via dynamic compensation,\" Kybernetica, vol. 29, no. 6, pp. 593-608, 1993. 15. E. D. Dickmanns and A. Zapp, \"Autonomous high speed road vehicle guidance by computer vision,\" lOth IFAC World Congr., Miinchen, D, pp. 221-226, 1987. 16. M. Fliess, J. Ldvine, P. Martin, and P. Rouchon, \"Design of trajectory stabiliz- ing feedback for driftless flat systems,\" 3rd European Control Conf., Roma, I, pp. 1882-1887, 1995. 17. L. Gurvits and Z. Li, \"Smooth time-periodic feedback solutions for nonholonomic motion planning,\" in Nonholonomic Motion Planning, Z. Li, J. Canny (Eds.), Kluwer Academic Publishers, Norwell, MA, pp. 53-108, 1992.

252 A. De Luca, G. Oriolo and C. Samson 18. A. Isidori, Nonlinear Control Systems, 3rd Edition, Springer-Verlag, London, UK, 1995. 19. T. Kailath, Linear Systems, Prentice-Hall, Englewood Cliffs, NJ, 1980. 20. H. K. Khalil, Nonlinear Systems, Macmillan, New York, NY, 1992. 21. I. V. Kolmanovsky, M. Reyhanoglu, and N. H. McClamroch, \"Discontinuous feed- back stabilization of nonholonomic systems in extended power form,\" 33rd IEEE Conf. on Decision and Control, Lake Buena Vista, FL, pp. 3469-3474, 1994. 22. M. Krstid, I. Kanellakopoulos, and P. Kokotovid, Nonlinear and Adaptive Control Design, John Wiley • Sons, New York, NY, 1995. 23. G. Lafferriere and H. J. Sussmann, \"Motion planning for controllable systems without drift,\" 1991 IEEE Int. Conf. on Robotics and Automation, Sacramento, CA, pp. 1148-1153, 1991. 24. J.-P. Laumond, '~Controllability of a multibody mobile robot,\" IEEE Trans. on Robotics and Automation, vol. 9, no. 6, pp. 755-763, 1993. 25. P. Lucibello and G. Oriolo, \"Stabilization via iterative state steering with ap- plication to chained-form systems,\" 35th IEEE Conf. on Decision and Control, Kobe, J, pp. 2614-2619, 1996. 26. S. Monaco and D. Normand-Cyrot, \"An introduction to motion planning under multirate digital control,\" 31st IEEE Conf. on Decision and Control, Tucson, AZ, pp. 1780-1785, 1992. 27. P. Morin and C. Samson, \"Time-varying exponential stabilization of chained sys- tems based on a backstepping technique,\" 35th IEEE Conf. on Decision and Control, Kobe, J, pp. 1449-1454, 1996. 28. 1%.M. Murray and S. S. Sastry, \"Nonholonomic motion planning: Steering using sinusoids,\" IEEE Trans. on Automatic Control, vol. 38, no. 5, pp. 700-716, 1993. 29. 1%. M. Murray, \"Control of nonholonomic systems using chained forms,\" Fields Institute Communications, vol. 1, pp. 219-245, 1993. 30. R. M. Murray and R. T. M'Closkey, \"Converting smooth, time-varying, asymp- totic stabilizers for driftless systems to homogeneous, exponential stabilizers,\" 3rd European Control Conf., Roma, t, pp. 2620-2625, 1995. 31. J. I. Neimark and F. A. Fufaev, Dynamics of Nonholonomie Systems, American Mathematical Society, Providence, RI, 1972. 32. G. Oriolo, S. Panzieri, and G. Ulivi, \"An iterative learning controller for nonholo- nomic robots,\" I996 IEEE Int. Conf. on Robotics and Automation, Minneapolis, MN, pp. 2676-2681, 1996. 33. J.-B. Pomet, \"Explicit design of time-varying stabilizing control laws for a class of controllable systems without drift,\" Systems gJ Control Left., vol. 18, pp. 147-158, 1992. 34. J.-B. Pomet, B. Thuilot, G. Bastin, and G. Campion, \"A hybrid strategy for the feedback stabilization of nonholonomic mobile robots,\" 1992 IEEE Int. Conf. on Robotics and Automation, Nice, F, pp. 129-134, 1992. 35. J.-B. Pomet and C. Samson, \"Time-varying exponential stabilization of nonholo- nomic systems in power form,\" INR/A Rep. 2126, Dec. 1993. 36. P. Rouchon, M. Fliess, J. Ldvine, and P. Martin, \"Flatness and motion planning: The car with n trailers,\" Pnd European Control Conf., GrSningen, NL, pp. 1518- 1522, 1993.

Feedback Control of a Nonholonomic Car-Like Robot 253 37. M. Sampei, T. Tamura, T. Itoh, and M. Nakamichi, \"Path tracking control of trailer-like mobile robot,\" 1991 IEEE/RSJ Int. Work. on Intelligent Robots and Systems, Osaka, J, pp. 193-198, 1991. 38. C. Samson, \"Velocity and torque feedback control of a nonholonomic cart,\" in Advanced Robot Control, C. Canudas de Wit (Ed.), Birkh~iuser, Boston, MA, pp. 125-151, 1991. 39. C. Samson and K. Ait-Abderrahim, \"Feedback control of a nonholonomic wheeled cart in cartesian space,\" 1991 IEEE Int. Conf. on Robotics and Automation, Sacramento, CA, pp. 1136-1141, 1991. 40. C. Samson, M. Le Borgne, B. Espian, Robot Control: The Task Function Ap- proach,\" Oxford Science Publications, Oxford, UK, 1991. 41. C. Samson and K. Ait-Abderrahim, \"Feedback stabilization of a nonholonomic wheeled mobile robot, 1991 IEEE/RSJ Int. Work. on Intelligent Robots and Sys- tems,\" Osaka, J, pp. 1242-1247, 1991. 42. C. Samson, \"Path following and time-varying feedback stabilization of a wheeled mobile robot~\" 2nd Int. Conf. on Automation, Robotics and Computer Vision, Singapore, 1992. 43. C. Samson, \"Time-varying feedback stabilization of car-like wheeled mobile robots,\" Int. J. of Robotics Research, vol. 12, no. 1, pp. 55-64, 1993. 44. C. Samson, \"Control of chained systems. Application to path following and time- varying point-stabilization of mobile robots,\" IEEE Trans. on Automatic Control, vol. 40, no. 1, pp. 64-77, 1995. 45. E. D. Sontag, \"Feedback stabilization of nonlinear systems,\" in Robust Control of Linear Systems and Nonlinear Control, M. A. Kaashoek, J. H. van Schuppen, A. C. M. Ran (Eds.), Birkh/iuser, Cambridge, MA, pp. 61-81, 1990. 46. O. J. Sordalen, \"Conversion of the kinematics of a car with n trailers into a chained form,\" 1993 IEEE Int. Conf. on Robotics and Automation, Atlanta, GA, vol. 1, pp. 382-387, 1993. 47. O . J . Sordalen and C. Canudas de Wit, \"Exponential control law for a mobile robot: Extension to path following,\" IEEE Trans. on Robotics and Automation, vol. 9, no. 6, pp. 837-842, 1993. 48. O.J. Sordalen, \"Feedback control of nonholonomic mobile robots,\" Ph.D. Thesis, The Norwegian Institute of Technology, Trondheim, NOt Mar. 1993. 49. O. J. Strdalen and O. Egeland, \"Exponential stabilization of nonholonomic chained systems,\" IEEE Trans. on Automatic Control, vol. 40, no. 1, pp. 35- 49, 1995. 50. H. J. Sussmann and W. Liu, \"Limits of highly oscillatory controls and the approx- imation of general paths by admissible trajectories,\" Tech. Rep. SYCON-91-02, Rutgers University, Feb. 1991. 51. A. R. Teel, R. M. Murray, and G. Walsh, \"Nonholonomic control systemsi From steering to stabilization with sinusoids,\" 31st IEEE Conf. on Decision and Con- trol, ~hcson, AZ, pp. 1603-1609, 1992. 52. D. Tsakiris, C. Samson, and P. Rives, \"Vision-based time-varying stabilization of a mobile manipulator,\" 6th Int. Conf. on Control, Automation, Robotics and Vision, Singapore, 1996.

Probabilistic Path Planning P. Svestka and M. H. Overmars Utrecht University 1 Introduction The robot path planning problem, which asks for the computation of colli- sion free paths in environments containing obstacles, has received a great deal of attention in the last decades [25,15]. In the basic problem, there is one robot present in a static and known environment, and the task is to compute a collision-free path describing a motion that brings the robot from its current position to some desired goal position. Variations and extensions of this basic problem statement are numerous. To start with, for a large class of robots (i.e., nonholonomic robots) compu- tation of collision-free paths is not sufficient. Not only are the admissible robot placements constrained by obstacles and the robot geometry, but also are the directions of motion subject to constraints. For example, mobile robots moving on wheels have such nonholonomic constraints, due to the fact that their wheels are not allowed to slide. Another realistic scenario is that of multiple robots acting in the same environment. In this case, apart from the restrictions im- posed by the obstacles, robot geometry, and possible nonholonomic constraints, one also has to avoid collisions between the robots mutually. Moving obstacles, uncertainties in sensing, and inexact control add further levels of difficulty. In order to build robots that can autonomously act in real-life environ- ments, path planning problems as sketched above need to be solved. However, it has been proven that, in general, solving even the basic path planning prob- lem requires time exponential in the robots number of degrees of freedom. In spite of this discouraging problem complexity, various such complete planners have been proposed. Their high complexity however makes them impractical for most applications. And every extension of the basic path planning prob- lem adds in computational complexity. For example, if we have n robots of d degrees of freedom each, the complexity becomes exponential in nd. Or if we allow for moving obstacles, the problem becomes exponential in their number [9,35]. Assuming uncertainties in the robots sensing and control, leads to an exponential dependency on the complexity of the obstacles [9]. The above bounds deal with the exact problem, and therefore apply to complete planners. These are planners that solve any solvable problem, and return failure for each non-solvable one. So for most practical problems it seems

256 P. Svestka and M. H. Overmars impossible to use such complete planners. This has lead many researchers to consider simplifications of the problem statement. A quite recent direction of research, which we just want to mention briefly here, deals with the formulation of assumptions on the robot environment that reduce the path planning complexity. This is based on the belief that there exists a substantial gap between the theoretical worst-case bounds of path planning algorithms and their practical complexity. A number of researchers have attempted to formulate assumptions on the obstacles that prohibit the (artificial) constructions that cause the worst-case bounds. Examples of such assumptions are, amongst others, fatness [54,53], boundedlocalcomplexity[36], and dispersion [33]. However, this line of research has been mainly of theo- retical nature, and has not yet resulted in implementations of practical path planners. Also, it is currently not clear whether similar results can be obtained for extensions of the basic path planning problem. Instead of assuming things about the robot environment, many researchers have simply dropped the requirement of completeness for the planner. Heuris- tic planners have been developed that solve particular difficult problems in impressively low running times. However, the same planners also fail or con- sume prohibitive time on seemingly simpler ones. For autonomous robots in realistic environments this might be a problem, since one cannot predict the path planning problems such robots will face. So, on one hand, completeness is a preferred property of motion planners for autonomous robots, while, on the other hand, only heuristic algorithms are capable of solving many of the practical problems that people are interested in. This has lead to the design of path planners that satisfy weaker forms of com- pleteness, in particular resolution completenessand probabilistic completeness. In this chapter we deal with the latter. A planner is called probabilistically complete if, given a solvable problem, the probability of solving it converges to I as the running time goes to infinity. Such a planner is guaranteed to solve any solvable problem within finite time. And if one can estimate the probability of solving a problem with respect to the running time, one has an even stronger result. Two of the most successful such planners are the probabilisticpath planner (PPP) and the randomizedpath planner (RPP). RPP [5,17] is a potential field planner, that escapes local minima by per- forming Brownian motions. It has successfully been applied to articulated robots with many degrees of freedom (dof). Also, it has been used for check- ing whether parts can be removed from aircraft engines for inspection and maintenance, and for automatically synthesising a video clip with graphically simulated human and robot characters entailing a 78-dimensional configuration space. The planner works as follows : Given a goal configuration g, a potential field U is computed, being a function that assigns positive real values to con-

Probabilistic Path Planning 257 figurations. Roughly, as is the case for potential fields in general, U 4s defined by an attracting potential of the goal configuration, and repulsing potentials of the obstacles, and can be seen as a landscape with the obstacles as mountains and the goal configuration as lowest point. Connecting the start configuration s to the goal configuration g is attempted by \"descending\" along U. Such a down motion always ends in a local minimum. If this local minimum is g (the global minimum), then the problem is solved. If this is not the case, a Brownian mo- tion is performed, and the process is repeated. Using well-known properties of Brownian motions, RPP can be proven to be probabilistically complete [17,24]. Moreover, a calculation of the finite expected number of Brownian motions is given in [24]. This calculation uses the fact that the basins Bi of attraction of the local minima form a partition of the free configuration space. For each pair (Bi, Bj) one can define the transition probability pij that a Brownian motion, starting at the minimum of Bi, terminates somewhere in Bj. The ex- pected number of Brownian motions can then be expressed as a function of the transition probabilities pij's. This nice theoretical result has however the practical drawback that the pq's are, in non-trivial cases, unknown. Although RPP proves to be very powerful for many practical problems, the method also has some drawbacks. For example, since the planner is potential field based, it does not memories any knowledge about the configuration space after having solved a particular problem, and, for this reason, each new problem requires a whole new search. In other words, it is a single shot approach. Furthermore, it appears to be easy to create seemingly simple problems for which the planner consumes a more than reasonable amount of time, due to very low transition probabilities between certain basins. Also, the method does not apply directly to nonholonomic robots. Other probabilistically complete planners for static and dynamic environ- ments utilising genetic algorithms are described in [1,7]. Other work on related probabilistic path planning approaches includes [16]. We will not go into details here. This chapter gives a survey on the probabilistic path planner PPP, which is a very general planner, or planning scheme, building probabilistic roadmaps by randomly selecting configurations from the free configuration space and interconnecting certain pairs by simple feasible paths. The method is proba- bilistically complete and not restricted to any particular class of robots. A first single-shot version of the planner for free-flying planar robots was de- scribed in [31] and subsequently expanded into a general learning approach, for various robot types, in [32]. Independently, \"PPP-like\" preprocessing schemes for holonomic robots where introduced in [21] and [14]. These schemes also build probabilistic roadmaps in the free C-space, but focus on the case of many-dof robots. In [23] the ideas developed in [32] and [21] have been combined, result- ing in an even more powerful planner for high-dof robots. Simultaneously, PPP

258 P. Svestka and M. H. Overmars has been applied to nonholonomic robots. Planners for car-like robots that can move both forwards and backwards as well as such that can only move forwards are described in [45,47]. PPP applied to tractor-trailer robots is the topic of [49,39]. Probabilistic completeness of the planners for nonholonomic robots is proven in [47]. Recently some first results on the expected running times of PPP, under certain geometric assumptions on the free configuration space, have been obtained [22,20,3]. For a thorough survey of probabilistic path planning for holonomic robots we also refer to the thesis of Kavraki [19]. Fi- nally, extensions of PPP addressing multi-robot path planning problems have been presented in [46,48]. In this chapter an overview is given of the algorithmic aspects of PPP and applications of the planning scheme to various robot types are discussed. Also, some theory is presented regarding probabilistic completeness and expected running times. The chapter is organised as follows: In Section 2 the probabilis- tic paradigm is described in its general form. In the following two sections the paradigm is applied to specific robot types, i.e., to holonomic robots (free-flying and articulated) in Section 3, and to nonholonomic mobile robots (car-like and tractor-trailer) in Section 4. In both sections the robot specific components of the algorithm are defined, and obtained simulation results are presented. Sections 5 and 6 are of a more theoretical nature. In Section 5 aspects re- garding probabilistic completeness of the method are discussed, and proofs of probabilistic completeness are given for the planners described in this chapter. Section 6 deals with analyses of expected running time. Results by Kavraki et al. [22,20,3] are reviewed, and some new results are presented as well. In Section 7 an extension of PPP for solving multi-robot path planning problems is described, and simulation results are given for problems involving multiple car-like robots. 2 The Probabilistic Path Planner The Probabilistic Path Planner (PPP) can be described in general terms, with- out focusing on any specific robot type. The idea is that during the roadmap construction phase a data structure is incrementally constructed in a proba- bilistic way, and that this data structure is later, in the query phase, used for solving individual path planning problems. The data-structure constructed during the roadn'lap construction phase is an undirected graph G = (l/, E), where the nodes V are probabilistically gener- ated free configurations and the edges E correspond to (simple) feasible paths. These simple paths, which we refer to as local paths, are computed by a local planner. A local planner L is simply a function that takes two configurations as axguments, and returns a path connecting them~ that is feasible in absence

Probabilistic Path Planning 259 of obstacles (that is, the path respects the constraints of the robot). Proper choice of the local planner guarantees probabilistic completeness of the global planner, as we will see in Section 5. If, given two configurations a and b, the path L(a, b) is collision-free, then we will say that L connects from a to b. In the query phase, given a start configuration s and a goal configuration g, we try to connect s and g to suitable nodes ~ and ~ in V. Then we perform a graph search to find a sequence of edges in E connecting ~ to ~, and we transform this sequence into a feasible path. So the paths generated in the query phase (that is described in detail later) are basically just concatenations of local paths, and therefore the properties of these \"global paths\" are induced by the local planner. 2.1 The roadmap construction phase We assume that we are dealing with a robot .4, and that L is a local planner that constructs paths for ,4. We assume that L is symmetric, that is, for any pair of configurations (a, b) L(a, b) equals L(b, a) reversed. See Section 2.3 for remarks on non-symmetric local planners. As mentioned above, in the roadmap construction phase a probabilistic roadmap is constructed, and stored in an undirected graph G = (V, E). The construction of the roadmap is performed incrementally in a probabilistic way. Repeatedly a random free configuration c is generated and added to V. Heuristics however are used for generating more nodes in \"difficult\" areas of the free configuration space (or ]ree C-space). We try to connect each generated node c to previously added ones with L, and each such successful connection results in a corresponding edge being added to E. More precisely, this edge adding is done as follows : First, a set Nc of neighbours is chosen from V. This set consists of nodes lying within a certain distance from c, with respect to some distance measure D. Then, in order of increasing distance from c, we pick nodes from No. For each such picked node n, we test whether L connects from c to n, and, if so, (c, n) is added to E. However, if n is already graph-connected with c at the moment that it is picked, n is simply ignored. So no cycles can be created and the resulting graph is a forest, i.e., a collection of trees. The motivation for preventing cycles is that no query would ever succeed thanks to an edge that is part of a cycle. Hence, adding an edge that creates a cycle can impossibly improve the planners performance in the query phase. A price to be paid for disallowing cycles in the graph is that in the query phase often unnecessarily long paths will be obtained. Suppose that a and b are two configurations that can easily be connected by some short feasible path. Due to the probabilistic nature of the roadmap construction algorithm, it is very well possible that, at some point, a and b get connected by some very

260 P. Svestka and M. H. Overmars long path. Obtaining a shorter connection between a and b would require the introduction of a cycle in the graph, which we prevent. So, for any pair of nodes, the first graph path connecting them blocks other possibilities. There are a number of ways for dealing with this problem. One possibility is to apply an edge adding method that does allow cycles in the graph [32]. These methods however have the disadvantage that they slow down the roadmap construction algorithm, due to the fact that the adding of a node requires more calls of the local planner. Another possibility is to build a forest as described above, but, before using the graph for queries, \"smoothing\" the graph by adding certain edges that create cycles. Some experiments that we have done indicated that smoothing the graph for just a few seconds significantly reduces the path lengths in the query phase. Finally, it is possible to apply some smoothing techniques on the paths constructed in the query phase. We briefly describe a simple but efficient and general probabilistic path smoothing technique in Section 2.4. Let C denote the C-space of the robot, and C / t h e free portion of C (i.e., the free C-space). To describe the roadmap construction algorithm formally, we need a function D E g x C --+ R +. It defines the distance measure used, and should give a suitable notion of distance for arbitrary pairs of configurations, taking the properties of the robot ,4 into account. We assume that D is symmetric. The graph G = (V, E) E gf × g~ is constructed as follows: The roadmap construction algorithm (1) V= ,E=0 (2) loop (3) c = a \"randomly\" chosen free configuration (4) v = y u {c} (5) Nc = a set of neighbours of c chosen from V (6) forall n e No, in order of increasing D(c, n) do (7) if -~connected(c,n) A L(c,n) C C! t h e n E = E tJ {(c,n)) The construction algorithm, as described above, leaves a number of choices to be made: A local planner must be chosen, a distance measure must be de- fined, and it must be defined what the neighbours of a node are. ~rthermore, heuristics for generating more nodes in interesting C-space areas should be de- fined. Some choices must be left open as long as we do not focus on a particular robot type, but certain global remarks can be made here. Local planner One of the crucial ingredients in the roadmap construction phase is the local planner. As mentioned before, the local planner must construct paths that are ]casiblc for A, in absence of obstacles. This simply

Probabilistic Path Planning 261 means that the paths it constructs describe motions that are performable by the robot, that is, motions that respect the robots constraints. For example, assume the robot is a car-like vehicle moving on wheels, then a local planner that just connects the two argument configurations with a straight-line segment (in C-space) is not suitable, since it describes motions that force the wheels of the robot to slide. Furthermore, we want the roa~lmap construction algorithm to be fast. For this, it is important that (1) the local planner constructs its paths in a time efficient manner and (2) the probability that local paths intersect with obstacles is low. The first requirement can be met by keeping the path con- structs as simple as possible. For obtaining low intersection probabilities, the local planner should construct paths with relatively small sweep vol- umes. That is, the volumes (in workspace) swept by the robot when moving along local paths should preferably be small. Clearly, local planners min- imising these sweep volumes also minimise the probabilities of the local paths intersecting obstacles. Finally, the local planner should guarantee probabilistic completeness of PPP. In Section 5 we give sufficient properties. Neighbours and edge adding methods Another important choice to be made is that of the neighbours Nc of a (new) node c. As is the case for the choice of the local planner, the definition of Nc has large impact on the performance of the roadmap construction algorithm. Reasons for this are that the choice of the neighbours strongly influences the overall structure of the graph, and that, regardless of how the local planner is exactly defined, the calls of the local planner are by far the most time-consuming operations of the roadmap construction algorithm (due to the collision tests that must be performed). So it is clear that calls of the local planner that do not effectively extend the knowledge stored in the roadmap should be avoided as much as possible. Firstly, as mentioned before, attempts to connect to nodes that are already in c's connected component are useless. For this reason the roadmap con- struction algorithm builds a forest. Secondly, local planner calls that fail add no knowledge to the roadmap. To avoid too many local planner fail- ures we only submit pairs of configurations whose relative distance (with respect to D) is small, that is, less than some constant threshold maxdist. Thus: Nc C {5 E v l n ( c , 5) <_maxdist} (1) This criterion still leaves many possibilities regarding the actual choice for No. We have decided on taking all nodes within distance maxdist as neighbours. Experiments with various definitions for Nc on a wide range of problems lea~ to this choice.

262 P. ~vestka and M. H. Overmars Hence, according to the algorithm outline given above, we try to connect to all \"nearby\" nodes of c, in order of increasing distance D, but we skip those nodes that are already in c's connected component at the moment that the connection is to be attempted. By considering elements of Nc in this order we expect to maximise the chances of quickly connecting c to other configurations and, consequently, reduce the number of calls to the local planner (since every successful connection results in merging two connected components into one). We refer to the described edge adding method as the forest method. D i s t a n c e We have seen that a distance function D is used for choosing and sorting the neighbours Nc of a new node c. It should be defined in such a way that D(a, b) (for arbitrary a and b) somehow reflects the chance that the lo- cal planner will fail to connect a to b. For example, given two configurations a and b, a possibility is to define D(a, b) as the size of the sweep volume (in the workspace) of L(a, b), that is, as the volume of the area swept by the robot when moving along L(a, b). In this way each local planner L induces its own distance measure, that reflects the described \"failure-chance\" very well. In fact, if the obstacles were randomly distributed points, then this definition would reflect the local planner's failure chance exactly. However, in the general case, exact computations of the described sweep-volumes tend to be rather expensive, and in practice it turns out that certain rough but cheap to evaluate approximations of the sweep volumes are to be preferred. Node adding heuristics If the number of nodes generated during the road- map construction phase is large enough, the set V gives a fairly uniform covering of the free C-space. In easy cases, for example for holonomic robots with few degrees of freedom (say not more than 4), G is then well connected. But in more complicated cases where the free C-space is actually connected, G tends to remain disconnected for a long time in certain narrow (and hence difficult) areas of the free C-space. Due to the probabilistic completeness of the method, we are sure that even- tually G will grasp the connectivity of the free space, but to prevent exor- bitant running times, it is wise to guide the node generation by heuristics that create higher node densities in the difficult areas. To identify these, there are a number of possibilities. In some cases, one can use the geometry of the workspace obstacles. For example, for car-like robots adding (extra) configurations that correspond to placements of the robot \"parallel\" to obstacle edges and \"around\" con- vex obstacle corners boosts the performance of the roadmap construction algorithm significantly. A more general criterion is to use the (run-time) structure of the roadmap G. Given a node c E V, one can count the number of nodes of V lying within some predefined distance of c. If this number is low, the obstacle

Probabilistic Path Planning 263 region probably occupies a large subset of c's neighbourhood. This suggests that c lies in a difficult area. Another possibility is to look at the distance from c to the nearest connected component not containing c. If this distance is small, then c lies in a region where two components failed to connect, which indicates that this region might be a difficult one (it may also be actually obstructed). Alternatively, rather than using the structure of the obstacles or the road- map to identify difficult regions, one can look at the run-time behaviour of the local planner. For example, if the local planner often fails to connect c to other nodes, this is also an indication that c lies in a difficult region. Which particular heuristic function should be used depends to some extent on the input scene. 2.2 The query phase During the query phase, paths are to be found between arbitrary start and goal configurations, using the graph G computed in the roadmap construction phase. The idea is that, given a start configuration s and a goal configuration g, we try to find feasible paths Ps and Pg, such that P8 connects s to a graph node $, and Pg connects g to a graph node ~, with ~ graph-connected to (that is, they lie in the same connected component of G). If this succeeds, we perform a graph search to obtain a path Pa in G connecting ~ to ~. A feasible path (in C-space) from s to g is then constructed by concatenating Ps, the subpaths constructed by the local planner when applied to pairs of consecutive nodes in PG, and Pg reversed. Otherwise, the query fails. The queries should preferably terminate 'instantaneously', so no expensive algorithm is allowed for computing Ps and Pg. For finding the nodes ~ and ~ we use the function query_mapping E C x C -¢ V x V, defined as follows: query_mapping(a, b) =(~, b), such that h and b are connected, and n(a, 5) + n(b, b) = MIN(=,u)e W : D(a, x) + n(y, b) where W = {(x, y) E V x VIconnected(x,y)} So query_mapping(a, b) returns the pair of connected graph nodes (5, b) that minimise the total distance from a to 5 and from b to b. We will refer to 5 as a's graph retraction, and to b ~ b's graph retraction. The most straightforward way for performing a query with start configu- ration s and goal configuration g is to compute (~, ~) = query_mapping(s, g), and to try to connect with the local planner from s to $ and from ~ to g. However, since no obstacle avoidance is incorporated in the local planner, it

264 P. Svestka and M. H. Overmars may, in unlucky cases, fail find the connections even if the graph captures the connectivity of free C-space well. Experiments with different robot types indicated that simple probabilistic methods that repeatedly perform short random walks from s and g, and try to connect to the graph retractions of the end-points of those walks with the local planner, achieve significantlybetter results. These random walks should aim at maneuvering the robot out of narrow C-space areas (that is, areas where the robot is tightly surrounded by obstacles), and hereby improving the chances for the local planner to succeed. For holonomic robots very good performance is obtained by what we refer to as the random bounce walk (see also [32]). The idea is that repeatedly a random direction (in C-space) is chosen, and the robot is moved in this direction until a collision occurs (or time runs out). When a collision occurs, a new random direction is chosen. This method performs much better than for example pure Brownian motion in C-space. For nonholonomic robots walks of a similar nature can be performed, but care must of course be taken to respect the nonholonomic constraints. 2.3 Using a directed graph In the algorithm outline of PPP, as described in the previous section, the computed roadmaps are stored in undirected graphs. For many path planning problems this is sufficient, and it appears that the method is easier and more efficient to implement when based on undirected graphs. For example, path planning problems involving free-flyingrobots, articulated robots, and (normal) car-like robots can all be dealt With using undirected underlying graphs. There are however path planning problems for which undirected underlying graphs not sufficient, and directed ones are required instead. For example, problems in- volving car-like robots that can only move forwards require directed underlying graphs. The existence of an edge (a, b) in the underlying graph G corresponds to the statement that the local planner constructs a feasible path from a to b. If however G is undirected, then the edge contains no information about the direction in which the local planner can compute the path, and, hence, it must correspond to the statement that the local planner constructs a feasible path from a to b, as well as one from b to a. So an edge (a, b) can be added only if the local planner connects in both directions. Doing so, useful information might be thrown away. This will happen in those cases where the local planner connects in exactly one direction, and the fact that it has successfully constructed a feasible path will not be stored. If however the local planner is symmetric, which means that it connects from say a to b whenever it connects from b to a, then obviously this problem will never occur. So if the local planner is

Probabilistic Path Planning 265 symmetric, the underlying graph can be undirected, and if it is not symmetric, then it is better to use a directed graph. Whether it is possible to implement (good) local planners that are sym- metric, depends on the properties of the robot A, defined by the constraints imposed on it. Definition 4. A robot A is C-symmetric (configuration space symmetric) i] and only if any feasible path ]or A remains feasible when reversed. All holonomic robots are C-symmetric. For nonholonomic robots this is not the case. For example, a car-like robot that can drive forwards as well as backwards is C-symmetric while one that can only drive forwards is not. In terms of control theory, a (nonholonomic) robot is C-symmetric if its control system is symmetric. That is, it can attain a velocity v (in C-space) if and only if it can also attain the velocity -v. Clearly, if ,4 is C-symmetric, then any local planner L that constructs feasi- ble paths for A can be made symmetric in a trivial way, by reversing computed paths when necessary. So this implies that for any C-symmetric robot an undi- rected graph can be used for storing the local paths, and otherwise a directed graph is required. For directed graphs it is less straightforward to omit the adding of redundant edges than was the case for undirected graphs. We refer to [45] and [47] for discussions on this topic, and sensitive strategies for the adding of directed edges. 2.4 Smoothing the paths Paths computed in the query phase can be quite ugly and unnecessarily long. This is due to the probabilistic nature of the algorithm, and to the fact that cycle-creating edges are never added. To improve this, one can apply some path smoothing techniques on these 'ugly' paths. The smoothing routine that we use is very simple. It repeatedly picks a pair of random configurations (Cl,C2) on the \"to be smoothed\" path Pc, tries to connect these with a feasible path Qnew using the local planner. If this succeeds and Qnew is shorter than the path segment Qotd in P c from cl to c2, then it replaces Qolg by Q~ew (in Pc). So basically, randomly picked segments of the path are replaced, when possible, by shorter ones, constructed by the local planner. The longer this is done, the shorter (and nicer) the path gets. Typically, this method smoothes a path very well in less than a second for low dof robots, and in a few seconds for high dof robots. Still one can argue that this is too much for a query. In that case one must either accept the ugly paths, or use a more expensive edge adding method that builds graphs containing loops. This will result in a slowdown of the roadmap

266 P. Svestka and M. H. Overmars construction phase, but the gain is that the paths (directly) retrieved in the query phase will be shorter. 3 Application to holonomic robots In this section an application of PPP to two types of holonomic robots is described: free-flying robots and articulated robots. We consider here only planar holonomic robots. A free-flying robot is rep- resented as a polygon that can rotate and translate freely in the plane among a set of polygonal obstacles. Its C-space is represented by R 2 x [0, 2r[. A planar articulated robot .4 consists of n links L 1 , . . . , Ln, which are some solid planar bodies (we use polygons), connected to each other by n - 1 joints J2,..., jn. Furthermore, the first link L1 is connected to some base point in the workspace by a joint J1. Each joint is either a prismatic joint, or a revolute joint. If Ji is a prismatic joint, then link Li can translate along some vector that is fixed to link Li-1 (or to the workspace, if i = 1), and if Ji is a revolute joint, then link Li can rotate around some point that is fixed to link Li-1 (or to the workspace, if i = 1). The range of the possible translations or rotations of each link Li is constrained by Ji's joint bounds, consisting of a lower bound lowi and an up- per bound upi. The C-space of a n-linked planar articulated robot can, hence, be represented by [low1, up1] x [low2, up2] x ... x [lown, upn]. In the scenes we show, the revolute joints are indicated by small black discs, and the prismatic joints by small black discs with double arrows. Since holonomic robots are g-symmetric, it is feasible to use undirected graphs for storing the roadmaps. Some of the (robot specific) details, left open in the discussion of the general method, must be specified. 3.1 Filling in the details The local planner: A very general local planner exists, that is directly appli- cable to all holonomic robots. Given two configurations, it connects them by a straight line segment in C-space and checks this line segment for collision and joint limits (if any). We refer to this planner as the general holonomic local planner. Collision checking can be done as follows: First, discretise the line segment into a number of configurations Cl, . . . , cm, such that for each pair of consecutive configurations (ci, ci+l) no point on the robot, when positioned at configuration ci, lies further than some e away from its po- sition when the robot is at configuration Ci+l (e is a positive constant). Then, for each configuration c~, test whether the robot, when positioned at ci and \"grown\" by e, is collision-free. If none of the m configurations yield collision, conclude that the path is collision-free.

Probabilistic Path Planning 267 The distance measure: The distance between two configurations a and b is defined as the length (in C-space) of the local path connecting a and b, but scaled in the various C-space dimensions appropriately, in order to reflect the local planners failure chance reasonably. For example, in the case of of a long and thin free flying robot, small variations in orientation (that is, variations in the third dimension) correspond to motions sweeping relatively large volumes in the workspace, and should hence be reflected by large distances, while, on the other hand, for disc-like robots they should be reflected by small distances. The r a n d o m walks in the query phase: Section 2.2 described a general scheme for solving a query using a graph constructed in the roadmap con- struction phase. Multiple random walks were performed from the query configurations s and g, aimed at connecting the end-points of these walks to their graph retractions with the local planner. Remains to define the specific random walks. For holonomic robots, a random bounce walk con- sists of repeatedly picking at random a direction of motion in C-space and moving in this direction until an obstacle is hit. When a collision occurs, a new random direction is chosen. And so on. The (maximal) number of these walks (per query) and their (maximal) lengths are parameters of the planner, which we denote by, respectively, Nw and Lw. N o d e adding heuristics: For both the free-flying robots as the articulated robots, we utilise the (run-time) structure of G to identify \"difficult\" areas in which more \"random\" nodes are to be added than in others. We increase the chances for node generation in areas (of C-space) where the graph shows disconnectivities (that is, where there are a number of separate connected components present). For high dof robots it also proves helpful to identify nodes lying in difficult areas by considering the success/failure ratio of the local planner. If this ration is low for a particular node (that is, the local planner fails to connect to the node relatively often), this is an indication that the node lies in some difficult area. In this case, more nodes are added in the (near) neighbour- hood of the node, in order to locally improve the graph connectivity. We say that the node is expanded[21],[23]. 3.2 Simulation results We have implemented the method for planar free-flying and articulated robots in the way described above, and we present some simulation results obtained with the resulting planners. The implementations are in C++ and the experi- ments were performed on a Silicon Graphics Indigo2 workstation with an R4400

268 P. ~vestka and M. H. Overmars processor running at 150 MHZ. This machine is rated with 96.5 SPECfp92 and 90.4 SPECint92. In the test scenes used, the coordinates of all workspace obstacles lie in the unit square. Furthermore, in all scenes we have added an obstacle boundary around the unit square, hence no part of the robot can move outside this square. The experiments are aimed at measuring the \"knowledge\" acquired by the method after having constructed roadmaps for certain periods of time. This is done by testing how well the method solves certain (interesting) queries. For each scene S we define a query test set TO. = {(sl, sl), (s2,g2),..., (sra,gm)}, consisting of a number of configuration pairs (that is, queries). Then, we re- peatedly construct a graph for some specified time t, and we count how many of these graphs solve the different queries in TQ. This experiment is repeated for a number of different construction times t. The results are presented in the tables under the figures. The numbers in the boxes indicate the percentage of the runs that solve the corresponding query within the given time bound. The values for the random walk parameters Nw and Lw are, respectively, 10 and 0.05. This guarantees that the time spent per query is bounded by approximately 0.3 seconds (on our machine). Clearly, if we allow more time per query, the method will be more successful in the query phase, and vice versa. Hence there is a trade-off between the construction time and the time allowed for a query. In Figure 1 we have a free flying L-shaped robot, placed at the configurations a, b, and c. Simulation results are shown for the three corresponding queries, and two paths are shown, both smoothed in 1 second. We see that around 1 second of roadmap construction is required for obtaining roadmaps that solve the queries. These roadmaps consist of approximately 125 nodes. In Figures 2 to 4 results are given for articulated robots. In the first two scenes, just one query is tested, and well the query (a, b). In both figures, several robot configurations along a path solving the query are displayed using various grey levels. The results of the experiments are given in the two tables. We see that the query in Figure 2 is solved in all cases after 10 seconds of construction time. Roadmap construction for 5 seconds however suffices to successfully answer the query in more than 90% of the cases. In Figure 3 we observe something similar. For both scenes the roadmaps constructed in 10 seconds contain around 500 nodes. Figure 4 is a very difficult one. We have a seven dof robot in a very con- strained environment. The configurations a, b, c, and d define 6 different queries, for which the results are shown. These where obtained by a customised imple- mentation by Kavraki et al. [23]. In this implementation, optimised collision

Probabilistic Path Planning 269 Fig. 1. An L-shaped free-flying robot and its test configurations are shown. At the top right, we see two paths computed by the planner and smoothed in 1 second. Fig. 2. A four dof articulated robot, and a path.

270 P. Svestka and M. H. Overmars Fig. 3. A five dof articulated robot, and a path. checking routines are used, as well as a robot-specific local planner. ~ r t h e r - more, \"difficult\" nodes are heuristically identified during the roadmap construc- tion phase, and \"expanded\" subsequently. We see that roughly 1 minute was sufficient to obtain roadmaps solving the 6 queries. These roadmaps consist of approximately 4000 nodes. 4 Application to nonholonomic robots In this section we deal with nonholonomic mobile robots. More specifically, we apply PPP to car-like robots and tractor-trailer robots. We consider two types of car-like robots, i.e., such that can drive both forwards and backwards, and such that can only drive forwards. We refer to the former as general car-like robots, and to the latter as forward car-like robots. First however we give a brief overview on previous work on nonholonomic motion planning. 4.1 Some previous work on nonholonomic motion planning Nonholonomic constraints add an extra level of difficulty to the path planning problem. The paths must (1) be collision free and (2) describe motions that are executable for the robot. We refer to such paths as feasible paths.

Probabilistic Path Planning 271 Fig. 4. A seven dof articulated robot in a very constrained environment and the query test set. For locally controllable robots [6], the existence of a feasible path between two configurations is equivalent to the existence of a collision free path, due to the fact that for any collision free path there exists a feasible path lying arbi- trarily close to it. This fundamental property has led to a family of algorithms, decomposing the search in two phases. They first try to solve the geometric problem (i.e., the problem for the holonomic robot that is geometrically equiv- alent to the nonholonomic one). Then they use the obtained collision-free path to build a feasible one. So in the first phase the decision problem is solved, and only in the second phase are the nonholonomic constraints taken into ac- count. One such approach was developed for car-like robots [26], using Reeds and Shepp works on optimal control to approximate the geometric path. In [34] Reeds and Shepp presented a finite family of paths composed of line segments and circle arcs containing a length-optimal path linking any two configurations (in absence of obstacles). The planner introduced in [26] replaces the collision- free geometric path by a sequence of Reeds and Shepp paths. This complete and fast planner was extended to the case of tractor-trailer robots, using near optimal paths numerically computed [27,12] (so far the exact optimal paths for the tractor-trailer system in absence of obstacle are unknown). The result- ing planners are however neither complete nor time-efficient. The same scheme was used for systems that can be put into the chained form. For these sys- tems, Tilbury et al. [50] proposed different controls to steer the system from

272 P. ~vestka and M. H. Overmars one configuration to another, in absence of obstacles. Sekhavat and Laumond prove in [38] that the sinusoidal inputs proposed by Tilbury et al. can be used in a complete algorithm transforming any collision-free path to a feasible one. This algorithm was implemented for a car-like robot towing one or two trail- ers, which can be put into the chained form, and finds paths in reasonable times [38]. A multi-level extension of this approach has been presented in [40] which further improves the running times of this scheme by separating the nonholonomic constraints mutually, and introducing separately. The scheme is however, as pointed out, only applicable to locally controllable robots. For example, forward car-like robots do not fall in this class. Barraquand and Latombe [6] have proposed a heuristic brute-force approach to motion planning for nonholonomic robots. It consists of heuristically build- ing and searching a graph whose nodes are small axis-parallel cells in C-space. Two such cells are connected in the graph if there exists a basic path between two particular configurations in the respective cells. The completeness of this algorithm is guaranteed up to appropriate choice of certain parameters, and it does not require local controllability of the robot. The main drawback of this planner is that when the heuristics fail it requires an exhaustive search in the discretised C-space. Furthermore, only the cell containing the goal configura- tion is reached, not the goal configuration itself. Hence the planner is inexact. Nevertheless, in many cases the method produces nice paths (with minimum number of reversals) for car-like robots and tractors pulling one trailer. For sys- tems of higher dimension however it becomes too time consuming. Ferbach [11] builds on the approach of Barraqua.nd and Latombe method in his progressive constraints algorithm in order to solve the problem in higher dimensions. First a geometric path is computed. Then the nonholonomic constraints are intro- duced progressively in an iterative algorithm. Each iteration consists of explor- ing a neighbourhood of the path computed in the previous iteration, searching for a path that satisfies more accurate constraints. Smooth collision-free paths in non-trivial environments were obtained with this method for car-like robots towing two and three trailers. The algorithm however does not satisfy any form of completeness. The probabilistic path planner PPP has been applied to various types of nonholonomic robots. An advantage over the above single shot methods is the fact that a roadmap is constructed just ones, from which paths can subse- quently be retrieved quasi-instantaneously. Also, local robot controllability is not required. A critical point of PPP when applied to nonholonomic robots is however the speed of the nonholonomic local planner. For car-like robots very fast local planners have been developed. Thanks to this, PPP applied to the car-like robots resulted in fast and probabilistically complete planners for car-like robots that move both forwards and backwards, as well as for such that can only move forwards [45,47]. Local planners for tractor-trailer robots

Probabilistic Path Planning 273 however tend to be much more time-consuming, which makes direct use of PPP less attractive. In [49] a local planner is presented and integrated into PPP, that uses exact closed form solutions for the kinematic parameters of a tractor- trailer robot. In [39] the local planner using sinusoidal inputs for chained form systems is used. For robots pulling more than one trailer, this local planner ap- peared to be too expensive for capturing the connectivity of the free C-space. For this reason, and inspired by the earlier mentioned works [26,27,12,38], in [39] a two-level scheme is proposed, where at the first level the portion of CSf~ee is reduced to a neighbourhood of a geometric path, and at the second level a (real) solution is searched for within this neighbourhood (by PPP). The multi- level algorithm proposed in [40] can in fact been seen as a generalisation of this two level scheme. 4.2 Description of the car-like and tractor-trailer robots We model a car-like robot as a polygon moving in R 2, and its C-space is rep- resented by R 2 x [0, 27r). The motions it can perform are subject to nonholo- nomic constraints. It can move forwards and backwards, and perform curves of a lower bounded turning radius rmi,~,as an ordinary car. A tractor-trailer robot is modelled as a car-like one, but with an extra polygon attached to it by a revolute joint. Its C-space is (hence) 4-dimensional, and can be represented by R 2 x [0, 21r) x [ - a m ~ , ama~], where ama~ is the (symmetric) joint bound. The car-like part (the tractor) is a car-like robot. The extra part (the trailer) is sub- ject to further nonholonomic constraints. Its motions are (physically) dictated by the motions of the tractor (For details, see for example [25,40]). For car-like robots, the paths constructed will be sequences of translational paths (describing straight motions) and rotational paths (describing motions of constant non-zero curvature) only. It is a well-known fact [25] that if for a (general or forward) car-like robot a feasible path in the open free C-space exists between two configurations, then there also exists one that is a (finite) sequence of rotational paths. We include translational paths to enable straight motions of the robot, hence reducing the path lengths. For tractor-trailer robots we will use paths that are computed by transformation of the configuration coordinates to the chained form, and using sinusoidal inputs. 4.3 Application to general car-like robots We now apply PPP, using an undirected graph, to general car-like robots. This again asks for filling in some of the (robot specific) details that have been left open in the discussion of the general method.

274 P. ~vestka and M. H. Overmars Filling in the details T h e local planner: A RTR path is defined as the concatenation of a rota- tional path, a translational path, and another rotational path. Or, in other words, it is the concatenation of two circular arcs and a straight line seg- ment, with the latter in the middle. The RTR localplanner constructs the shortest RTR path connecting its argument configurations. Figure 5 shows two RTR paths. It can easily be proven that any pair of configurations is connected by a number of RTR paths (See [45] for more details). Fur- thermore, the RTR local planner satisfies a local topological property that guarantees probabilistic completeness (See Section 5). eS • ! iI el Fig. 5. Two RTR paths for a triangular car-like robot, connecting configurations a and b. An alternative to the RTR local planner is to use a local planner that con- structs the shortest (car-like) path connecting its argument configurations [34], [42]. Constructing shortest car-like paths is however a relatively ex- pensive operation, and the construct requires more expensive intersection checking routines than does the RTR construct. On the other hand, RTR paths will, in general, be somewhat longer than the shortest paths, and, hence, they have a higher chance of intersection with the obstacles. Collision checking for a RTR path can be done very efficiently, perform- ing three intersection tests for translational and rotational sweep volumes. These sweep volumes are bounded by linear and circular segments (such objects are also called generalisedpolygons) and hence the intersection tests can be done exactly and efficiently. Moreover, the intersection tests for the

Probabilistic Path Planning 275 rotational path segments can be eliminated by storing some extra informa- tion in the graph nodes, hence reducing the collision check of a RTR path to one single intersection test for a polygon. The distance measure: We use a distance measure that is induced by the RTR local planner, and can be regarded as an approximation of the (too expensive) induced \"sweep volume metric\", as described in Section 2.1. The distance between two configurations is defined as the length (in workspace) of the shortest RTR path connecting them. We refer to this distance mea- sure as the RTR distance measure, and we denote it by DRTR. The random walks in the query phase: Random walks, respecting the car-like constraints, are required. The (maximal) number of these walks (per query) and their (maximal) lengths are parameters of the method, which we again denote by, respectively, Nw and Lw. Let cs be the start configuration of a random walk. As actual length Iw of the walk we take a random value from [0, Lw]. The random walk is now performed in the following way: First, the robot is placed at configuration cs, and a random steering angle ¢ and random velocity v are chosen. Then, the motion defined by (¢, v) is performed until either a collision of the robot with an obstacle occurs, or the total length of the random walk has reached Iw. In the former case, a new random control is picked, and the process is repeated. In the latter case, the random walk ends. Good values for Nw and Lw must be experimentally derived (the values we use are given in the next section). Node adding heuristics: We use the geometry of the workspace obstacles to identify areas in which is advantageous to add some extra, geometri- cally derived, non-random nodes. Particular obstacle edges and (convex) obstacle corners define such geometric nodes (See [47] for more details). Furthermore, as for free-flying robots, we use the (run-time) structure of the graph G in order to guide the node generation. Simulation results We have implemented the planner as described above, and some simulation results are presented in this section. The planner was run on a machine as described in Section 3. Again the presented scenes correspond to the unit square with an obstacle boundary, and the chosen values for Nw and Lw are, respectively, 10 and 0.05. The simulation results are presented in the same form as for the holonomic robots in Section 3. That is, for different roadmap construction times we count how often graphs are obtained that solve particular, predefined, queries. Figure 6 shows a relatively easy scene, together with the robot ,4 positioned at a set of configurations {a, b, c, d, e}. The topology is simple and there are only a few narrow passages. We use ((a, b), (a, d), (b, e), (c, e), (d, e)} as query test set TQ. (At the top-right of Figure 6 paths solving the queries (a, d) and (b, e),

276 P. Svestka and M. H. Overmars smoothed in 1 second, are shown.) The minimal turning radius rmin used in the experiments is 0.1, and the neighbourhood size maxdist is 0.5. We see that after only 0.3 seconds of roadmap construction, the networks solve each of the queries in most cases (but not all). Half a second of construction is sufficient for solving each of the queries, in all 20 trials. The corresponding roadmaps contain about 150 nodes. Fig. 6. A simple scene. At the top right, two paths computed by the planner and smoothed in 1 second are shown. Figure 7 (again together with a robot .4 placed at different configurations {a, b, c, d}), shows a completely different type of scene. It contains many (small) obstacles and is not at all \"corridor-like\". Although many individual path plan- ning problems in this scene are quite simple, the topology of the free C-space is quite complicated, and can only be captured well with relatively compli- cated graphs. As query test set TQ we use {(a, b), (a, c), (a, d), (c, d)}. Further- more, as in the previous scene, rmin --- 0.1 and maxdist -- 0.5. Again, we show two (smoothed) paths computed by our planner (solving the queries (a, b) and (c, d)). We see that about 2 seconds of construction are required to obtain roadmaps that are (almost) guaranteed to solve each of the queries. Their number of nodes is about 350.

Probabilistic Path Planning 277 Fig. 7. A more complicated scene, and its test configurations. At the top right, two paths computed by the planner and smoothed in 1 second are shown. 4.4 Application to forward car-like robots Forward car-like robots, as pointed out before, are not C-symmetric. Hence, as explained in Section 2.3, directed instead of undirected graphs are used for storing the roadmaps. For details regarding the exact definition of the roadmap construction algorithm we refer to [32]. The robot specific components, such as the local planner, the metric, and the random walks are quite similar to those used for general car-like robots, as described in Section 4.3. The local planner constructs the shortest forwardRTR path connecting its argument configurations. A forward RTR path is defined exactly as a normal RTR path, except that the rotational and translational paths are required to describe forward robot motions. The distance between two configurations is defined as the (workspace) length of the shortest forward RTR path connecting them. A random walk is performed as for general car- like robots, with the difference that the randomly picked velocity must be positive, and that, when collision occurs, the random walk is resumed from a random configuration on the previously followed trajectory (instead of from the configuration where collision occurred).

278 P. Svestka and M. H. Overmars Simulation results In Figure 8 we give some results for the same scene as Figure 7. We see that the queries are most likely to be solved after 5 seconds of roadmap construction, and (almost) surely after 7.5 seconds, by roadmaps consisting of around 700 nodes. This means that about four times more time is required than for general car-like robots. Fig. 8. Motion planning for a forward car-like robot. 4.5 Application to tractor-trailer robots As last example of nonholonomic robots, we now (briefly) consider tractor- trailer robots, and well such that can drive both forwards and backwards. These robots have symmetrical control systems and, hence, undirected under- lying graphs are sufficient. We will not go into many details. We refer to [39,40] for a more thorough discussion of the topic. We use a local planner, by Sekha- vat and Laumond [38], that transforms its configuration coordinates into the chained form, and uses sinusoidal inputs. We refer to it as the sinusoidal local planner. This local planner verifies a local topological property that guarantees probabilistic completeness of the global planner. As distance measure we use

Probabilistic Path Planning 279 (cheap) approximations of the workspace lengths of the local paths. The ran- dom walks in the query phase are basically as those for general car-like robots, except that the trailers orientation must be kept track of during each motion of the tractor. This can be done using exact closed form solutions for the kine- matic parameters of tractor-trailer robots under constant curvature motions of the tractor [49]. If, during such a motion, the tractors orientation gets out of bounds (relative to the orientation of the tractor), this is treated as a collision. S i m u l a t i o n results See Figure 9 for two feasible paths computed by the Probabilistic Path Planner. The computation time of the roadmap from which the paths where retrieved took about 10 seconds (on the average). d Fig. 9. Two feasible paths for a tractor-trailer robot, obtained in 10 seconds. 5 On probabilistic completeness of probabilistic path planning In this section we discuss some aspects regarding probabiIistic completenessof PPP, and we prove this completeness for the specific planners described in this chapter. We will assume a slightly simplified version of the planning scheme. Instead of trying to connect the start configuration s and goal configuration g to the graph with some graph retractions (as described in Section 2.2), we simply add s and g to the graph as (initial) nodes. A query consists of just a

280 P. Svestka and M. H. Overmars graph search. This simplification of the query phase is for ease of presentation. All results presented in this section directly hold for the case where queries are performed as described earlier (in Section 2.2), using graph retractions and random walks. A path planner is called probabilisticallycompleteif, given any problem that is solvable in the open free C-space, the probability that the planner solves the problem goes to 1 as the running time goes to infinity. Hence, a probabilisti- cally complete path planner is guaranteed to solve such a problem, provided that it is executed for a sufficient amount of time. For ease of presentation we introduce some shorthand notations. We denote the version of PPP using undi- rected underlying graphs (respectively directed graphs) by PPP~,(respectively PPPd). The notation PPPu(L) (respectively PPPd(L)) is used for referring to PPPu (respectively PPPd) with a specific local planner L. We say L is sym- metric iff, for arbitrary configurations a and b, L(a, b) equals L(b,a) reversed. Furthermore, we again denote the C-space, respectively the free C-space, by C, respectively g/. We point out that with PPP one obtains a probabilistically complete plan- ner for any robot that is locally controllable (see below), provided that one defines the local planner properly. If, in addition to the local controllability, the robot also has a symmetric control system then PPP~(L) is suitable, oth- erwise PPPd(L) must be used. In Section 5.1 we define a general property for local planners that is sufficient for probabilistic completeness of PPP, and we point out that, given the local controllability of the robot, a local planner satisfying this property always exists (but it must be found). We also men- tion a relaxation of the property, that guarantees probabilistic completeness of PPPu(L) as well, for locally controllable robots with symmetric control sys- tems. All holonomic robots, as well as for example general car-like robots and tractor-trailer robots, fall into this class. Forward car-like robots however are not locally controllable (and neither symmetric). In Section 5.2 we show that all the planners described in this chapter are probabilistically, complete. First we define the concept local controllability (in the literature also re- ferred to as small-time local controllability or local-local controllability), adopt- ing the terminology introduced by Sussman [43]. Given a robot A, let Z.4 be its control system. That is, ZA describes the velocities that ,4 can attain in C-space. For a configuration c of a robot A, the set of configurations that .4 can reach within time T is denoted by AE, (_< T, c). A is defined to be locally controllable iff for any configuration c E C A~.~(<_T,c) contains a neighbour- hood of c (or, equivalently, a ball centred at c) for all T > 0. It is a well-known fact that, given a configuration c, the area a locally controllable robot ¢4 can reach without leaving the e-ball around c (for any e > 0) is the entire open e-ball around c.

Probabilistic Path Planning 281 5.1 The general local topology property W e assume now that robot A is locallycontrollable.For probabilisticcomplete- ness of P P P a local planner L is required that exploits the local controllability of A. This will be the case if L has what we call the general local topologi- cal property, or GLT-property, as defined in Definition fi using the notion of e-teachability introduced in Definition 5. We denote the ball (in C-space) of radius e centred at configuration c by Be(c). Definition 5. Let L be a local planner for .A. Furthermore let ~ > 0 and c 6 C be given. The e-reachable area of c by L, denoted by RL,e(c), is defined by RL,~(c) = {5 6 B~(c)lL(c, 5) is entirely contained in Be(c)} Definition 6. Let L be a local planner for A. We say L has the GLT-property V~ > 0 : 35 > 0 :Vc 6 g : B~(c) C RL,~(c) We refer to B~ (c) as the e-reachable g-ball of c. A local planner verifying the GLT-property, at least in theory, always ex- ists, due to the robots local controllability. Theorem 5.1 now states that this property is sufficient to guarantee probabilistic completeness of PPP. That is, of PPPu(L) if L is symmetric, and of PPPd(L) otherwise. T h e o r e m 5.1. If L is a local planner verifying the GLT-propcrty, then PPP(L) is probabilistically complete. Proof. The theorem can be proven quite straightforwardly (for both PPPu(L) and PPPd(L)). Assume L verifies the GLT-property. Given two configurations s and g, lying in the same connected component of the open free C-space, take a path P that connects s and g and lies in the open free C-space as well. Let e be the C-space clearance of P (that is, the minimal distance between P and a C-space obstacle), and take 5 > 0 such that Vc 6 C : B~(c) C RL,¼e(c). Then, consider a covering of P by balls B 1 , . . . , Bk of radius ¼6, such that balls Bi and Bi+l, for i 6 {1,..., k - 1}, partially overlap. Assume each such ball Bi contains a node vi of G. Then, ]vi - Vi+l] < 5, and each node v~ has a C-space clearance of at least e - ¼5 > 43-e(since 5 <_ e). Hence, due to the definition of 6, we have L(vi, vi+l) C B¼~(vi) C gl It follows that if all the balls B1,..., Bk contain a node of G, s and g will be graph-connected. Since, due to the random node adding, this is guaranteed to be the case within a finite amount of time, this concludes the proof. See also Figure 10. •

282 P. Svestka and M. H. Overmaxs P Fig. 10. Path P has clearance e > 0. If 5 > 0 is chosen such that Vc E C : B~(c) C RL,~e(C), then we see that nodes in overlapping ¼5-balls,centred at configurations of P, can always be connected by the local planner. Clearly, given a locally controllable robot, the GLT-property is a proper criterion for choosing the local planner (sufficient conditions for local control- lability of a robot are given in, e.g., [44]). Path planning among obstacles for car-like robots using local planners with the GLT-property has also been stud- ied by Laumond [28,18]. For locally controllable robots with symmetric control systems, a weaker property exists that guarantees probabilistic completeness as well. We refer to this property as the LTP-property. The basic relaxation is that we no longer require the e-reachable J-ball of a configuration a to be centred around c. We do however make a certain requirement regarding the relationship between configurations and the corresponding e-reachable (f-balls. Namely, it must be described by a Lipschitz continuous function. For a formal definition of the LTP-property and a proof of probabilistic completeness with local planners verifying it, we refer to [47]. 5.2 Probabillstlc completeness with the used local planners The local planners used for holonomic robots, general car-like robots, forward car-like robots, and tractor-trailer robots, as described earlier in this chapter, guarantee probabilistic completeness.

Probabilistic Path Planning 283 Locally controllable robots The general holonomic local planner b for holo- nomic robots constructs the straight line path (in C-space) connecting its ar- gument configurations. It immediately follows that RE,L(C) = BE(c), for any configuration c and any ~ > 0. Hence, L verifies the GLT-property. T h e o r e m 5.2. PPPu(L), with L being the general holonomie local planner, is probabilistically complete for all holonomic robots. The planner for general car-like robots uses the RTR local planner. One can prove that this planner verifies the LTP-property [47]. Again, as stated in the following theorem, this guarantees probabilistic completeness. T h e o r e m 5.3. PPP~(L), with L being the R T R local planner, is probabilisti- cally complete for general car-like robots. Regarding tractor-trailer robots, Sekhavat and Lanmond prove in [38] that the sinusoidal local planner, used for the tractor-trailer robots, verifies the GLT-property. Hence, for tractor-trailer robots we also have probabilistic com- pleteness. T h e o r e m 5.4. PPP~(L), with L being the sinusoidal local planner, is proba- bilistically complete for tractor-trailer robots (with arbitrary number of trailers). Forward car-like robots As pointed out before, the theory of the previous sections applies only to robots that are locally controllable. If a robot does not have this property, a local planner verifying the GLT-property will not exist. A local planner verifying the weaker LTP-property may exist, but this planner will not be symmetric (this would imply the existence of a local planner verifying GTP). Forward car-like robots are not locally controllable. One can nevertheless prove probabilistic completeness of PPPd(L), with L being the RTR forward local planner. That is, one can prove that, given two configurations s and g such that there exists a feasible path in the open free C-space connecting them, PPPd(L) will surely solve the problem within finite time. The proof, which does not directly generalise to other cases, uses a property of RTR forward paths stated in Lemma 5.5. L e m m a 5.5. Let L be the RTR forward local planner, and let Q be a RTR forward path connecting configurations a and b with a straight line path of non- zero length and both arc paths of total curvature less than 17r. Then: Ve > 0 : 3 5 > 0 : V(h,b) e Bz(a) × B6(b) : L(h,b) lies within distance c o f Q 1 1 We say a path P lies within distance e of a path Q, iff Vp E P : 3q E Q : Ip- ql < e (in C-space)

284 P. Svestka and M. H. Overmars T h e o r e m 5.6. PPPd(L), with L being the RTR forward local planner, is prob- abilistieally complete for forward car-like robots. Fig. 11. This figure illustrates the proof of Theorem 5.6. P2 is a path~ feasible for a forward car-like robot, of clearance e > 0. Centred at the configurations mi are balls Bi of a radius 5 > 0, such that any pair of configurations (a, b) E Bi × B~+I is connected by the RTR forward local planner L with a path lying within distance e of P2, and hence lying in Of. We give only a sketch of the proof here (See also Figure 11). Let L be the RTR forward local planner. Assume PI is a path in the open free C-space connecting a (start) configuration s to a (goal) configuration g, that is feasible for our forward car-like robot A. Then, one can prove, there exists also a feasible path P2 in the open free C-space, connecting s to g, that consists of (a finite number of) straight line segments and circular arcs, such that no two distinct arcs are adjacent and each arc has a total curvature of less than ½7r 2 Assume k is the number of arcs in P2. Let ml = s, m k = g, and {m2,..., ink-l) be points on P2 such that mi is the midpoint of the i-th arc of P2 (that is, the unique point on the arc with equal distance to both end-points). Clearly, mi is connected mi+l by a forward RTR path with a straight line segment of non-zero length and both arc paths of total curvature less than ½r (for all j e {1,...,k- 1}). 2 This does not necessarily hold if P1 consists of just one or two circular arcs of maximal curvature. In this case however P1 can be found directly with the local planner.

Probabilistic Path Planning 285 Let e > 0 be the clearance of P2, and take 5 > 0 such that, for all j E ( 1 , . . . , k - 1}: V(a,b) E B~(mj) x B~(mj+l) :L(a,b) lies within distance e of Q It follows from Lemma 5.5 that such a 5 > 0 always exists. When a node of G is present in every ball B~(mj) for 2 _~j < k, G will contain a path connecting s to g. We know, due to the probabilistic nature of the node adding, that the probability of obtaining such a graph grows to I when the roadmap construction time goes to infinity. 6 On the expected complexity of probabilistic path planning In the previous section we have formulated properties of local planners that guarantee probabilistic completeness of PPP for locally controllable robots. If these properties are satisfied, we know that as the running time of PPP goes to infinity, the probability of solving any solvable problem goes to 1. However, nothing formal has yet been said about (expected) convergence times of the algorithm. In practice, one will not be satisfied with the guarantee that \"eventually a path will be found\". For real life applications, some estimate of the running time beforehand is desirable. Simulation results obtained by the application of PPP on certain \"typical\" problems can increase our trust in the planners performance and robustness, but they do not describe a formal relation between the probabilities of fail- ure and running times in general, and neither do they provide a theoretical explanation for the empirically observed success of the probabilistic planner. Recently some first theoretical results on expected running times of probabitis- tic planners have been obtained. Kavraki et al. [22,20,3] show that, under certain geometric assumptions about the free C-space Cf, it is possible to establish a relation between the probability that probabilistic planners like pppa find paths solving particular problems, and their running times. They suggest two such assumptions, i.e., the visibility volume assumption and the path clearance assumption. We will discuss these assumptions and present the main results obtained by Kavraki et al.. Also, we will discuss to what extent these results hold for nonholonomic robots, and, where possible, we will adapt them appropriately. Furthermore, we introduce a new assumption on configuration space, the e-complexity assumption, under which it is possible to relate the success probabilities and running times of PPP to the complexity of the problems that are to be solved. a Kavraki et al. refer to PPP by the name PRM (Probabilistic RoadMap planner).

286 P. Svestka and M. H. Overmars Throughout this section we use the notations introduced in the previous section, and, unless stated otherwise, we will assume that PPP with undirected underlying graphs (i.e., PPPu) is used. 6.1 T h e visibility volume assumption The visibility volume assumption uses a notion of \"visibility\" defined by the used local planner. A free configuration a is said to \"see\" a free configuration b if the local path L(a, b) lies entirely in gf. The visibility volume assumption now states that every free configuration \"sees\" a subset of CI whose volume is at least an e fraction of the total volume of gl. If this holds, CI is called e-good. The analyses assumes a somewhat more complex planner than PPP as de- fined in Section 2. It differs from PPP in that after a probabilistic roadmap G = (V, E) has been constructed (by the roadmap construction algorithm in Section 2.1), an extra post-processing step is performed, referred to by the au- thors as Permeation. Permeation assumes the existence of a complete planner, that is~ a planner solving any solvable problem, and returning failure for any non-solvable one. What permeation does, is invoking the complete planner for certain pairs (a, b) E V × V that are not graph connected. Planners based on this scheme have been implemented by Kavraki et al. (E.g., [21]). However, instead of a complete planner (which, in general, is not available) the poten- tial field planner RPP has been utilised. Since RPP is only probabilistically complete, the mentioned planners are merely approximations of the algorithm sketched above. Due to the assumed completeness of the invoked planner, provided that the complete planner is invoked for enough pairs of nodes in V, permeation leads to a roadmap where every connected component of gl contains at most one connected component of the roadmap G. For convenience, we will say that such roadmaps have perfect connectivity. Let us now assume that Gp has such perfect connectivity. Then, if both s and g \"see\" a node of Gp, the planner will return a path solving this problem if it is solvable, and return failure otherwise. In other words, on the portion of CI that \"sees\" some node of the roadmap~ the planner is complete. Note that during the permeation step, no nodes are added to G. Hence, the question whether a solvable query will be answered coITectly depends solely on the set of nodes V in G. Theorem 6.1 addresses this question. V is called adequate if the portion of g/, not visible from any c E V, has a volume of at most 1 Volume(gl). The theorem gives a lower bound for the probability of V being adequate. Theorem 6.1. (Kavraki et aL [22], Barraquand et al, [3]) Assume G = (V, E) is a roadmap generated by P P P in a ]ree C-space that is e-good. Let

Probabilistic Path Planning 287 E (0, 1] be a real constant, and let C be a positive constant large enough such thatVx • (0,1] : ( l - x ) (-c-'°g~) <- x { Now, illVI -> ~log 71, then V is adequate with probability at least 1 - 8. Regarding the complexity of the roadmap construction, one must estimate the number of calls to the complete planner during the permeation step, re- quired for obtaining a roadmap Gp of perfect connectivity. Theorem 6.2 pro- vides such an estimate. T h e o r e m 6.2. (Kavraki et al. [22], B a r r a q u a n d et al. [3]) Let wl >_ w2 >_ ... >_Wk be the sizes of the connected components of the roadmap G = (V, E). There exists a (randomised) algorithm that extends G to a roadmap Gp of perfect connectivity, whose expected number of calls of the complete planner is at most: k 2~ i . w, - I Y l - k i=1 Furthermore, with high probability, the number of calls is at most: 0 y~i. wi. log(lVl) i=I Symmetry of the local planner L is required and assumed in the above. However, apart from this, the e-goodness assumption is very general, and the given analysis is not only valid for holonomic robots (on which the authors focus), but also for nonholonomic ones. However, the question arises in how far the theoretical results are \"practical\" for nonholonomie robots, since the \"visibility\" induced by a nonholonomic local planner will be a quite hard to grasp concept that can hardly be regarded as a geometric property of CI. There does not seem to be much hope that we will ever be able to measure the e-goodness of CI for nonholonomic robots, in non-trivial cases (insofar as there is such hope for holonomic ones). 6.2 The path clearance assumption In the path clearance assumption, there exists a collision-free path 79 between the start configuration s and goal configuration g, that has some clearance e > 0 with the C-space obstacles. Throughout this section we denote the volume of an object A by ];(A). In [20], Kavraki et al. study the dependence of the failure probability of PPP (the normal version) to connect s and g on (1) the length of :P, (2) the clearance e, and (3) the number of nodes in the probabilistic roadmap G. Their main result is described by the following theorem:

288 P. Svestka and M. H. Overmars T h e o r e m 6.3. ( K a v r a k i et aL [20], B a r r a q u a n d et al. [3]) Let .4 be a holonomic robot, L be the general holonomie local planner (for .4), and G = (V, E) be a graph constructed by P P P ( L ) . Assume configurations s and g are connectable by a path \"P of length A, that has a clearance e > 0 with the (C- space) obstacles. Let a E (0,1] be a real constant, and let a be the constant ~Ac)?(B1)/~;(g]), where 131 denotes the unit ball in the C-space ]~n Now i~ IV I is such that 2~(1 - a ~ ) lvt < a g then, with probability at least 1 - a, s and g will be graph-connected in G (See also Figure 12). mmw~mu~m~m~mmm c ~s I | | | | p' | | ii ! | | | Fig. 12. We see that configuration s is connectable to configuration g by a P of clearance e. Let x0 = s, x l , . . . , xk = g be points on P, such that [xj - xj-bl [ _< ~e,1for all j. If each ball B½~(xj) contains a node of G, then s and g will be graph-connected. The proof of this theorem is quite straightforward. Given a path P of clear- ance e > 0, one can consider a covering of P by balls of radius ~1e as shown in Figure 12, and bound the probability that one of these balls contains no node of G. Since, if each of these balls does contain a node, G is guaranteed to contain a path connecting the start and goal configuration, this gives an upper bound for the failure probability of PPP. A number of important facts are implied by Theorem 6.3. E.g., the number of nodes required to be generated, in order for the planner to succeed with probability at least 1 - a, is logarithmic in ~ and )~, and polynomial in 71\" Furthermore, the failure probability c~ is linear in the p a t h length A.

Probabilistic Path Planning 289 The analyses assumes the use of the general holonomic local planner (as described in Section 3.1). Hence it is assumed that the robot is holonomic. An underlying assumption is namely that the e-reachable area of any configuration e consists of the entire e-ball B~(c), surrounding e. From theoretical point of view, as pointed out in the previous section, for any locally controllable robot a local planner exists for which the e-reachable area of any configuration c con- sists of the entire open e-ball centred at c. Such a local planner would allow for the result assuming the path clearance to be directly applied to such robots. However, it is not realistic to assume the \"e-ball reachability\" for a nonholo- nomic local planner, since for most robots we are not able to construct such local planners, and, if we could, they would probably be vastly outperformed (in terms of computation time) by simple local planners verifying only weaker (but sufficient) topological properties, such as those presented in the previ- ous section. However, the analyses presented in [20] can be extended to the case where the local planner verifies only the GLT-property. Through this, we can give running time estimates for locally controllable nonholonomic robots that are realistic in the sense that we can actually build the planners that we analyse. Corollary 6.4 extends the result of Theorem 6.3 to locally controllable nonholonomic robots with local planners verifying the GLT-property. C o r o l l a r y 6.4. Let A be a fully controllable robot, L be a local planner for A verifying the GLT-property, and G = (V, E) be a graph constructed by PPP(L). Assume configurations s and g are connectable by a path 7) of length ~, that has a clearance e > 0 with the (configuration space) obstacles. Take 5 > 0 such that Vc e C: B~ (c) c RL,~ (c) Let a E (0, 1] be a real constant, and 1)1 be the volume of the unit ball in the C-space ~'~. Now if tVI is such that 2A ( l;1 -n\\ IVl then, with probability at least 1 - ~, s and g will be graph-connected in G. Since, by definition of the GLT-property, 5 is a constant with respect to e, the dependencies implied by Theorem 6.3 hold for nonholonomic robots as well. 6.3 The e-complexity assumption A drawback of Theorem 6.3 and the above corollary is that no relation is established between the failure probability and the complexity of a particular

290 P. Svestka and M. H. Overmars problem. In our opinion, to a considerable extent, the observed success of PPP lies in the fact that not the complexity of the C-space, but the complexity of the resulting path defines the (expected) running time of PPP. For example, assume a particular problem is solvable by a path P of clearance e > 0, consisting of say 4 straight line segments. Consider three balls of radius e, centred at the 3 inner nodes of P. Then, as is illustrated in Figure 13, it suffices that all the 3 balls contain a node of G to guarantee that the problem is solved. We see in this example that the failure probability in no way relates to the length of the path, and neither to the complexity of gI. The only relevant factors are the clearance and the complexity of the path. Definition 7 introduces the notion of e-complexity, which captures this measure of problem complexity. We refer here to a path composed of k straight line segments as a piecewise linear path of complexity k. Definition 7. Given a holonomic robot and a particular path planning problem (s, g), let P be the lowest complexity piecewise-linear path connecting s and g, that has a C-space clearance of e > O. We define the e-complexity of problem (s, g) as the complexity of P. Fig. 13. We see that configuration s is connectable to configuration g by a piecewise linear path P (dashed) of complexity 4 and clearance e. If each of 3 dark grey balls (of radius e, placed at the vertices of P) contains a node of G, then the G contains a path, lying in the grey area, that connects s and g. Theorem 6.5 gives a result relating the failure probability of PPP to the e-complexity of the problem to be solved. It applies only to holonomic robots and assumes the use of the general hotonomic local planner.

Probabilistic Path Planning 291 T h e o r e m 6.5. Let A be a holonomic robot, L be the general holonomic local planner (for A), and G = (V, E) be a graph constructed by PPP(L). Assume (s, 9) is a problem of e-complexity ~. Let a E (0, 1] be a real constant, and ~1 be the volume of the unit ball in the C-space Rn . Now if IV[ is such that (¢- 1) (1 \"~1 ) IV' V(Cs ) e~ <_ then, with probability at least 1 - a, s and g will be graph-connected in G. This theorem can be proven quite easily. Given a problem of e-complexity (, there exists a piecewise linear path P of complexity ( and clearance e > 0 solving it. We can place balls of radius e at the vertices of P, and bound the probability the one of these balls contains no node of G. Since, if each of these balls does contain a node, G is guaranteed to contain a path connecting the start and goal configuration, this gives us an upper bound for the failure probability of PPP. So we now also have a linear dependence of the failure probability, and a logarithmic dependence of lvI, on the complezity ~ of the path P, that is, on the e-complexity of the problem. We note that the existence of a path of a certain clearance e > 0 and implies the existence of a piecewise linear path of a similar clearance. 7 A multi-robot extension We conclude this chapter with an extension of PPP for solving multi-robot path planning problems. That is, problems involving a number of robots, present in the same workspace, that are to change their positions while avoiding (mu- tual) collisions. Important contributions on multi-robot path planning include [37,13,10,51,41,8,30,29,4,5,17,2,52]. For overviews we refer to [25] and [15]. Most previous successful planners fall into the class of decoupled planners, that is, planners that first plan separate paths for the individual robots more or less independently, and only in a later stage, in case of collisions, try to adapt the paths locally to prevent the collisions. This however inherently leads to planners that are not complete, that is, that can lead to deadlocks. To obtain some form of completeness, one must consider the separate robots as one composite system, and perform the planning for this entire system. However, this tends to be very expensive, since the composite C-space is typically of high dimension, and the constraints of all separate add up. For example, multi-robot problems could be tackled by direct application of PPP. The robot considered would be composed of the separate \"simple\" robots, and the local planner would construct paths for this composite robot.

292 P. Svestka and M. H. Overmars This is a very simple way of obtaining (probabilistically complete) multi-robot planners. However, as mentioned above, a drawback is the high dimension of the configuration space, which, in non-trivial scenes, will force PPP to construct very large roadmaps for capturing the structure of Cf. Moreover, each local path in such a roadmap will consists of a number of local paths for the simple robots, causing the collision checking to be rather expensive. In this section we describe a scheme where a roadmap for the composite robot is constructed only after a discretisation step that allows for disregarding the actual C-space of the composite robot. See Figure 14 for an example of a multi-robot path planning problem, and a solution to it, computed by a planner based on the scheme. We will refer to the separate robots A I , . . . , An as the simple robots. One can also consider the simple robots together to be one robot (with many degrees of freedom), the so-called composite robot. A feasible path for the composite robot will be referred to as a coordinatedpath. We assume in this paper that the simple robots are identical, although, with minor adaptions, the presented concepts are applicable to problems involving non-identical robots as well. A roadmap for the composite robot is constructed in two steps. First, a simple roadmap is constructed for just one robot with PPP. Then n of such roadmaps are combined into a roadmap for the composite robot (consisting of n simple robots). We will refer to such a composite roadmap as a super-graph. After such a super-graph has been constructed, which needs to be done just once for a given static environment, it can be used for retrieval of coordinated paths. We will present two super-graph structures: fiat super-graphsand multi- level super-graphs. The latter are a generalisation of flat super-graphs, that consume much less memory for problems involving more than 3 robots. The scheme is a flexible one, in the sense that it is easily applicable to various robot types, provided that one is able to construct simple roadmaps for one such robot. Furthermore, proper construction of the simple roadmaps guarantees probabilistic completeness of the resulting multi-robot planners [46]. In this paper we apply the super-graph approach to car-like robots. We give simulation results for problems involving up to 5 robots moving in the same constrained environment. 7.1 Discretisation of the multi-robot planning problem The first step of our multi-robot planning scheme consists of computing a simple roadmap, that is, a roadmap for the simple robot A. We assume that this roadmap is stored as a graph G = (V, E), with the nodes V corresponding to collision-free configurations, and the edges E to feasible paths, also referred to as localpaths. We say a node blocksa local path, if the volume occupied by ,4