IEEE Int. Conf. on Robotics and Automation May 16{21, Leuven (BE). Vol. 1, pp. 27{32 Path Planning with Uncertainty for Car-Like Robots Th. Frai

Transcription

1 IEEE Int. Conf. on Robotics and Automation May 16{21, Leuven (BE). Vol. 1, pp. 27{32 Path Planning with Uncertainty for Car-Like Robots Th. Fraichard and R. Mermond Inria a Rh^one-Alpes & Gravir b Zirst. 655 av. de l'europe, Montbonnot Saint Martin, France thierry.fraichard@inria.fr June 16, 1998 Abstract this paper presents the rst path planner taking into account both non-holonomic and uncertainty constraints. The case of a car-like robot subject to cumulative and unbounded conguration uncertainty related to bounded control and sensing errors is considered. Assuming the existence of landmarks allowing the robot to relocalize itself in particular places, we present an algorithm that computes paths that are both feasible and robust. In other words, they respect the non-holonomic constraints of a car-like robot and the robot is assured to reach its goal by following them as long as its control and sensing errors remain within bounded sets. Keywords mobile-robot, non-holonomic-system, path-planning, uncertainty, landmark. Acknowledgements this work was partially supported by the Inria- Inrets c Praxitele programme on individual urban public transports [ ], and the Inco-Copernicus ERB-IC15-CT project \Multi-agent robot systems for industrial applications in the transport domain" [ ]. a Institut National de Recherche en Informatiue et en Automatiue. b Lab. d'informatiue GRAphiue, VIsion et Robotiue. c Institut National de REcherche sur les Transports et leur Securite.

2

3 Path Planning with Uncertainty for Car-Like Robots Th. Fraichard and R. Mermond Inria Rh^one-Alpes & Gravir y Inria. Zirst. 655 av. de l'europe Montbonnot Saint Martin, France Abstract this paper presents the rst path planner taking into account both non-holonomic and uncertainty constraints. The case of a car-like robot subject to cumulative and unbounded conguration uncertainty related to bounded control and sensing errors is considered. Assuming the existence of landmarks allowing the robot to relocalize itself in particular places, we present an algorithm that computes paths that are both feasible and robust. In other words, they respect the non-holonomic constraints of a car-like robot and the robot is assured to reach its goal by following them as long as its control and sensing errors remain within bounded sets. 1 Introduction Path planning for robots is a fundamental issue in Robotics (cf. [12]). The purpose of a path planner is to compute a path, i.e. a continuous seuence of congurations, 1 that leads the robot to its goal. The primary concern of path planning is to compute collision-free paths. Several extensions to the basic path planning problem have been considered. One of them regards the non-holonomy problem. This problem arises when a robot is subject to non-holonomic kinematic constraints that restrict the set of its admissible directions of motion (a wheel is a non-holonomic system; it can only move in a direction perpendicular to its axle). In this case, the planned paths must be feasible also, i.e. respect these non-holonomic constraints. Another key issue in path planning is the uncertainty problem. It is related to the various sources of uncertainty aecting the actual robot (control and sensing errors, inaccurate models of the environment, etc.). These uncertainties may lead to failures during the execution of a path preventing the robot from ever reaching its goal. A solution to this problem is to design a path planner that explicitly takes into account uncertainty so as to compute robust paths, i.e. paths that guarantee that the goal will be reached in spite of these uncertainties. As shown in the literature review of x2, non-holonomy and uncertainty have been addressed separately to a large extent. Non-holonomy is a key issue in path planning for Inst. Nat. de Recherche en Informatiue et en Automatiue. y Lab. d'informatiue GRAphiue, VIsion et Robotiue. 1 The conguration of a robot is a set of independent parametres that uniuely describes the position and orientation of every part of the robot. wheeled mobile robots, whereas uncertainty has been of primary concern in assembly tasks (the `peg-in-hole' problem). Uncertainty is also a challenging problem in the context of path planning for mobile robots. Yet, despite the fact that most wheeled mobile robots are subject to nonholonomic constraints, previous research works on path planning with uncertainty for mobile robots never considered non-holonomic constraints. It is therefore our purpose to address robust non-holonomic path planning, i.e. path planning with both uncertainty and non-holonomic constraints. The case of a car-like robot moving in a stationary workspace is considered. A car-like robot is the typical non-holonomic robot: at each time instant, it can only move forward and backward in a direction tangent to its main axis; besides its turning radius is lower bounded. The robot is subject to cumulative and unbounded conguration uncertainty related to bounded control and sensing errors. To overcome the cumulative nature of the conguration uncertainty, the existence of landmarks has to be assumed (otherwise, it is impossible to reach distant goals). Broadly speaking, a landmark is any device that allows the robot to reduce its conguration uncertainty, e.g. GPS, triangulation systems, beacons, etc. In this framework, this paper presents a robust nonholonomic path planner. To the best of our knowledge, it is the rst time that the combined complexity of both non-holonomic and uncertainty constraints are considered simultaneously. The contribution of this paper is the following: a model of the evolution of the conguration uncertainty for a car-like robot is given. This model is then used to develop a `local' robust non-holonomic path planner; this planner is local in the sense that it does not aim at being complete (for the sake of eciency, the search for a solution path is restricted to a nite and discrete subset of the whole set of solution paths). The local planner does not take landmarks into account. However it is shown how to embed it in a global path planning scheme such as the Ariadne's Clew Algorithm [3], or the Probabilistic Path Planner [10] in order to obtain a general and complete robust non-holonomic path planner. 2 Background Uncertainty appeared in path planning as early as 1976 [20]. Ever since, it has motivated a lot of research 1

4 works, especially for assembly tasks. Non-holonomy was introduced later for mobile robots [13], and it also formed the basis of a large body of research. The reader is referred to [12] for more details on this two topics. As for non-holonomic path planning, following a result established in [6], most of the existing path planners compute paths made up of straight segments connected with tangent circular arcs of minimum radius, e.g. [2, 9, 14]. As for uncertain path planning, the seminal preimage concept introduced in [16] has been applied to mobile robots, e.g. [1, 15]. Other approaches have been developed also, e.g. [4, 5, 19]. However, the computational issues involved have restricted these works to simple cases (point robots performing straight motions) and, to the best of our knowledge, the combined complexity of both uncertain and non-holonomic constraints has not been fully addressed before. 3 The Problem 3.1 The PSfrag Robot replacements and Its Workspace Let A be a car-like robot. It moves on a planar workspace W IR 2 cluttered up with xed obstacles B i; i 2 f1; : : : ; bg. A conguration of A is dened as = (x; y; ) 2 IR 2 S 1 where (x; y) are the coordinates of the rear axle midpoint R, and y w R x Fig. 1: a car-like robot. the orientation of A (Fig. 1). The steering angle of A, denoted by, is the average of the orientations of the two front wheels; it is mechanically limited. A is subject to the two following non-holonomic kinematic constraints [2]: _x sin? _y cos = 0 (1) min (2) E. (1) expresses the fact that R has to move in a direction normal to the rear wheels axle. E. (2) stems from the limits on. The turning radius of A, i.e. = w tan?1 () where w is the wheelbase of A, is lower bounded by a given value min. Let C be the conguration space of A, a path for A is a continuous seuence of congurations, i.e. a continuous mapping : [0; 1]?! C. A path is feasible if it respects (1) and (2). Let denote the set of the feasible paths. 3.2 Conguration Uncertainty The knowledge that A has of its current conguration is always uncertain, i.e. with a limited accuracy. Let u denote the conguration uncertainty of A. It is a set of parametres that describes the uncertainty associated with the conguration of A. The space of these parametres is the uncertainty space of A; it is denoted by U. The couple (, u) represents an uncertain conguration. (, u) x y d Fig. 2: (, u) with u = (d; ), represented in C (left) and in W (right). A non-deterministic (or bounded-set) representation was chosen to model the uncertainty [7]: the conguration uncertainty of A is characterized by two parametres that respectively represent the uncertainty on the position of A and on its orientation, namely u = (d; ) 2 IR S 1. Accordingly (, u), i.e. the set of possible congurations for A when its nominal conguration is and its uncertainty u, is a cylindrical region of C dened as (Fig. 2): (; u) = f p j dist(; p) < d and j? pj < g where dist(; p) denotes the Euclidean distance between the position components of and p. An uncertain conguration (, u) is collision-free i: 8i 2 f1; : : : ; bg; 8 p 2 (; u); A( p) \ B i = ; where A() is the region of W occupied by A when in the conguration. 3.3 Localization and Landmarks Assuming that A relies upon a relative localization procedure, e.g. dead-reckoning, to determine its current conguration yields a cumulative and unbounded conguration uncertainty for A. A will therefore never be assured to reach a distant conguration. For practical reasons, relocalization is necessary from time to time hence the assumption of the existence of landmarks. A landmark is any device that allows A to relocalize itself, i.e. to reduce its conguration uncertainty. Broadly speaking, a landmark is characterized by a couple (F, u ): F is the eld of inuence of ; it is the set of congurations where A is able to relocalize itself with the uncertainty u. Whenever A enters the eld of inuence of a landmark, i.e. when (; u) F, its conguration uncertainty drops down to u. To model the evolution of the conguration uncertainty of A when it moves along a given path, an uncertainty evolution function " is introduced. It is a mapping from [0; 1] to U that integrates both the increase of uncertainty due to the relative localization procedure and the uncertainty reduction due to landmarks. 3.4 Problem Statement Path planning with uncertainty can now be stated as follows: let ( 0, u 0) and ( 1, u 1) be two uncertain congurations. 0 is the start conguration and u 0 the corresponding uncertainty, whereas 1 is the goal conguration and u 1 the maximum nal uncertainty allowed. A path that is feasible, i.e. that respects (1) and (2), is a solution to the problem at hand i: x y d 2

5 The start and goal constraints are satised: (0) = 0; "(; 0) = u 0 (1) = 1; ((1); "(; 1)) ( 1; u 1) is collision-free: 8s 2 [0; 1]; ((s); "(; s)) is collision-free A solution path between two uncertain congurations is called a robust path. 4 The Solution 4.1 Outline of the Approach Recall that the conguration uncertainty of A increases continuously when A moves around (except when it enters the eld of inuence of a landmark). If the goal conguration is too far away, it is then impossible for A to reach it without entering the eld of inuence of at least one landmark. In the general case, a robust path is then a seuence of `jumps' from landmark to landmark until the goal is reached. This remark provides the basis for a two-phase path planner able to solve the problem at hand: 1. Learning phase: given the set of landmarks, build a graph G whose nodes are uncertain congurations belonging to the eld of inuence of the landmarks and whose edges are robust paths. 2. Query phase: given ( 0, u 0) and ( 1, u 1), a start and and a goal uncertain congurations, nd a direct robust path between 0 and 1. otherwise nd a robust path between 0 (resp. 1) and a node of G. Then perform a graph search in order to build the complete solution path. Note that this algorithm implies the existence of a connecting function able to compute a robust path in the absence of landmarks between two uncertain congurations. This function is used to connect: two nodes of G, 0 to 1, and 0 (resp. 1) to a node of G. As such, the solution algorithm is similar to two recent path planning schemes, namely [10]'s Probabilistic Path Planner (PPP), and [3]'s Ariadne's Clew Algorithm (ACA). They share the same two-phase algorithmic structure and both feature a robot independent global planner relying on a robot specic local path planner. The global planner incrementally builds G by selecting new nodes in C. PPP and ACA dier mainly in the way a new node is selected: it is probabilistically chosen in PPP whereas, in ACA, it is the output of an optimization procedure that tends to maximize the distance between the new node and the existing nodes of G. The local path planner is the aforementioned connecting function. The notable feature of algorithms such as PPP or ACA is the fact that the local path planner does not need to be complete: eciency is a more desirable property. Completeness is achieved at the global level. Considering the complexity of designing a path planner dealing with both non-holonomic and uncertainty constraints, this property is most interesting. The core of the solution algorithm is therefore the local path planner, it is detailed in x The Local Path Planner The purpose of the local path PSfrag replacements planner (LPP) is to compute a robust path in the absence of landmarks between two uncertain congurations. As mentioned in x4.1, LPP is embedded in a global planning scheme and does not have to be complete; eciency is a more rs 0 2 d 1 Fig. 3: Dubins' ( d ) and Reeds and Shepp's ( rs) paths. desirable property. Accordingly the search for an uncertain path is restricted to paths made up of straight segments connected with tangent circular arcs of minimum turning radius. When A is allowed to move forward only, such a path is a Dubins' path [6], otherwise it is a Reeds and Shepp's path [17] (Fig. 3). These paths are optimal in length, they are also geometrically simple and straightforward to compute. Once a feasible path has been computed, LPP must check that the nal uncertainty is ok, and that the path is collision-free (these two points are respectively presented in x4.3 and x4.4). LPP operates as follows: LPP( 0; u 0; 1; u 1) j = Dubins' or Reeds and Shepp's path from 0 to 1 j if ((1); "(; 1)) ( 1; u 1) then j j if is collision-free then else nil; j else nil; end; 4.3 Uncertainty Model Where Does Uncertainty Come From? Previous approaches to path planning with uncertainty considered omnidirectional robots performing straight motions, e.g. [4, 5, 11, 15]. Conguration uncertainty was a conseuence of uncertainty on the motion direction (cone of possible velocity directions). Such a model does not permit to evaluate the conguration uncertainty of a non omnidirectional robot such as A moving along a given path, hence an appropriate uncertainty evolution model was developed: it was decided to attach uncertainty to two parametres related to the control of a car-like robot, i.e. the traveling distance l and the steering angle. The traveling uncertainty is characterized by 2 IR, i.e. the percentage of error on the traveling distance. In other words, the actual traveling distance is bounded by l l. The steering uncertainty is represented by 2 IR, i.e. the upper bound on the modulus of the error between the nominal and the actual steering angle. The actual steering angle is bounded by. Under these assumptions, it is possible to de- ne the uncertainty evolution function " that models the 3

6 d E evolution of the uncertainty along paths. This point is addressed afterwards after a study on how the conguration of A changes along a path Conguration Evolution A path is made up of straight segments connected with tangent circular arcs of minimum turning radius. An elementary path is dened as being either a straight segment or a circular arc. It is fully characterized by (l; ) where l is the traveling distance and the steering angle. Let be the current conguration of A, its new conguration 0 at the end of an elementary path is: 0 1 = T(; l; ) where T is a 4 4 matrix dened as: ( if = 0 T(; l; ) = then T L(; l) else T C(; l; ) with T L and T C two homogeneous transforms respectively corresponding to a linear and a circular motion (Table 1). Henceforth (3) will simply be written 0 = T(; l; ) Uncertainty Evolution A solution path is a seuence of elementary paths. Accordingly, in order to evaluate ", it suces to know how to compute the uncertainty evolution along an elementary path. Let E(; u; l ;;; ) denote the uncertainty at the end of the elementary path characterized by (l; ), starting from the uncertain conguration (; u), and subject to the traveling and distance uncertainties (; ). Henceforth E(; u; l ;;; ) will simply be denoted E. To compute E, it is necessary to characterize the set of congurations possibly reached by A at the end of. Let R(; u; l ;;; ) be this set of congurations. Henceforth R(; u; l ;;; ) will simply be denoted R, it is dened as: R = 1 (T(p; lp; p)p; p 2(; u) l p 2[l? l; l + l] p 2[? ; + ] Recall that E = (d E; E) where d E and E respectively represent the position and the orientation uncertainty of A at the end of ; they are respectively dened as: de = max p2r dist(0 ; p) (5) E = max p2r j0? pj where 0 is the conguration reached at the end of. Computing d E. d E is the radius of the circle centered in 0 that circumscribes the projection of R in W. E. (4) shows that R is characterized by four variable factors: the position of the start conguration, (x p; yp) 2 D(x; y; d), its orientation, p 2 [? ; + ], the traveling uncertainty, l p 2 [l? l; l + l], and the steering uncertainty, p 2 [? ; + ]. The analysis of the respective inuence of each of these factors on the resulting (x p; yp) yields the following results (Fig. 4, left): ) (3) (4) v 1 v 2 v 3 v 4 v 5 v 6 A o C s 0 A t D(x; y; d) D(x 0 ; y 0 ; d) v 6 v 5 0 D(x; y; d) + D(x 0 ; y 0 ; d) A o A t C s v 1 0? v4 d E v 2 v 3 Fig. 4: (left) uncertainty inuence on the nal conguration, (right) the projection of R in W 1. Position uncertainty: (x p; yp) 2 D(x 0 ; y 0 ; d). 2. Orientation uncertainty: (x p; yp) belongs to a circular arc A o centered in (x; y). 3. Traveling uncertainty: (x p; yp) belongs to a circular arc A t whose curvature is. 4. Steering uncertainty: (x p; yp) belongs to a certain curve C s [8]. R is dened by the simultaneous inuence of these four factors. It can be shown how to derive the analytical expression of the boundary of the projection of R in W so as to easily evaluate d E (cf. [8] for more details). The righthand side of Fig. 4 depicts an example of the projection of R in W; it is bounded by four circular (dv 1v 2; dv 2v 3; dv 4v 5 and dv 5v 6) and two pieces of curve (dv 3v 4 and dv 6v 1). Computing E. E. (3) yields 0 = + w?1 l tan ; the denition of E in (5) can then be rewritten: E = max jw?1 (l tan? l p tan p) + pj (6) with l p 2 [l? l; l + l], p 2 [? ; + ] and p 2 [?; ]. To compute E, it suces then to evaluate (6) for the extremum values of l p; p and p. Clearly, both d E and E are monotonously and strictly increasing functions. However their non algebraic nature makes it dicult to characterize exactly how the uncertainty changes along a path. 4.4 Collision Checking For the sake of eciency, collision checking is not performed in the three dimensional con- guration space C, but in the two dimensional workspace PSfrag replacements W by computing the intersection between the obstacles B i; i 2 f1; : : : ; bg, and the region swept by A when it moves pied by A for. Fig. 5: region occu- along a given path. 4

7 T L (; l) = l cos l sin ; T C(; l; ) = ? sin + sin( + ) cos? cos( + ) Table 1: The homogeneous transforms T L(; l) and T C(; l; ) respectively corresponding to a linear and a circular motion. = w tan?1 () is the turning radius of A. = w?1 l tan is the angular displacement. Let A(; u) be the region of W occupied by A when its conguration is = (x; y; ), and its conguration uncertainty is u = (d; ). A(; u) is dened as: " [ [ xi # A(; u) = y i ) i A( (x i;yi)2d(x;y;d) i2[?;+] where D(x; y; d) is the disc of centre (x; y) and radius d. It can be shown that A(; u) is the region occupied by A for all its possible orientations (Fig. 5) isotropically grown of d [8]. Let be an elementary path characterized by (l; ) and let A( 0 ; u 0 ) S(; u; l; ) be the region of W swept by A when it moves along from (; u) to ( 0 ; u 0 ) S (; u; l; ) 0 with 0 = T(; l; ) and u 0 = E(; u; l; ). The way the uncertainty changes along A(; u) Fig. 6: S (; u; l; ). (cf. x4.3.3) makes it PSfrag dicult replacements to characterize the exact shape of S(; u; l; ). It was decided to use a conservative approximation instead. This approximation, denoted by S (; u; l; ), is dened as the region swept by A with a constant uncertainty set to the nal uncertainty u 0 (Fig. 6). Since the uncertainty is monotonously increasing, this approximation is conservative. The reader is referred to [8] for more details. S is used within a recursive and resolution-dependent collision checking function Free that operates the following way: Free(; u; l; ) j if 8i2f1; : : : ; bg; S (; u; l; ) \ B i = ; then TRUE; j else if l < r then FALSE; j else Free(; u; l=2; ) and j Free(T(; l=2; ); E(; u; l=2; ); l=2; ); end; 5 Results The path planning algorithm has been implemented in C ++ on a Sparc station. To begin with, the uncertainty evolution function E was implemented and tested. Then the path planner itself was tested on various workspaces including dierent sets of obstacles and landmarks. Fig. 7: a test workspace (left), an example of G (middle) and a path planning result (right). A test workspace is depicted in Fig. 7. It contains 5 polygonal obstacles and 41 circular landmarks. The robot considered has the characteristics of a small commercial car. The landmarks are reector systems that the car detectd thanks to a camera and uses to relocalize itself with a (1 meter, 2 degrees) accuracy. Also depicted in Fig. 7 is an example of the graph G created in the learning phase of the algorithm. It has 205 nodes and 448 vertices, it was generated in 387 seconds. A path planning result is also shown in Fig. 7. Fig. 8: miscellaneous path planning results. Other path planning results in dierent workspaces are presented in Figs. 8 and 9. The two rightmost frames of Fig. 9 illustrates how uncertainty can aect the output of the path planner. In the two examples presented, the start uncertainty was null in one case and set to (1 meter, 15 degrees) in the other. The resulting paths are uite dierent. 6 Conclusion This paper has presented the rst (to the best of our knowledge) attempt to address simultaneously two impor- 5

8 Fig. 9: miscellaneous path planning results. tant issues in path planning: the uncertainty and the nonholonomy problems, i.e. the problems related to the uncertainty aecting the dierent parametres characterizing the robot and the non-holonomic kinematic constraints that restrict its motion. The case of a car-like robot subject to cumulative and unbounded conguration uncertainty related to bounded control and sensing errors has been considered. Assuming the existence of landmarks allowing the robot to relocalize itself in particular places, we have presented an algorithm that computes paths that are both feasible and robust. In other words, they respect the non-holonomic constraints of a car-like robot and the robot is assured to reach its goal by following them as long as its control and sensing errors remain within bounded sets. The solution presented is generic; it features a robot independent global planner relying on a robot specic local path planner. It can therefore be adapted to a wide variety of robots and uncertainty models. Future developments will include experiments with a real car-like robot in order to validate the uncertainty model proposed and the planning algorithm developed. Also it would be interesting to consider other uncertainty models (probabilistic) and other types of non-holonomic path (with continuous curvature prole [18]). References [1] R. Alami and Simeon T. Planning robust motion strategies for a mobile robot. In Proc. of the IEEE Int. Conf. on Robotics and Automation, volume 2, pages 1312{1318, San Diego CA (US), May [2] J. Barrauand and J.-C. Latombe. On non-holonomic mobile robots and optimal maneuvering. Revue d'intelligence Articielle, 3(2):77{103, [3] P. Bessiere, E. Mazer, J.-M. Ahuactzin, and G. Talbi. Planning in a continuous space with forbidden regions: the Ariadne's clew algorithm. In Proc. of the Workshop on the Algorithmic Fondations of Robotics. A.K. Peters, Boston (MA), [4] B. Bouilly, T. Simeon, and R. Alami. A numerical techniue for planning motion strategies of a mobile robot in presence of uncertainty. In Proc. of the IEEE Int. Conf. on Robotics and Automation, volume 2, pages 1327{1332, Nagoya (JP), May [5] F. De la Rosa, C. Laugier, and J. Najera. Robust path planning in the plane. IEEE Trans. Robotics and Automation, 12(2):347{352, April [6] L. E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 79:497{516, [7] M. Erdmann. Randomization in robot tasks. Int. Journal of Robotics Research, 11(5):399{436, [8] Th. Fraichard and R. Mermond. Path planning with uncertainty for car-like vehicles. Research Report, Inst. Nat. de Recherche en Informatiue et en Automatiue, Montbonnot (FR), [9] P. Jacobs and J. Canny. Planning smooth paths for mobile robots. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 2{7, Scottsdale AZ (US), May [10] L. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars. Probalistic roadmaps for path planning in highdimensional conguration spaces. IEEE Trans. Robotics and Automation, 12(4):566{580, [11] M. Khatib, B. Bouilly, T. Simeon, and R. Chatila. Indoor navigation with uncertainty using sensor-based motions. In Proc. of the IEEE Int. Conf. on Robotics and Automation, Albuuerue NM (US), April [12] J.-C. Latombe. Robot motion planning. Kluwer Academic Press, [13] J.-P. Laumond. Feasible trajectories for mobile robots with kinematic and environment constraints. In Proc. of the Int. Conf. on Intelligent Autonomous Systems, pages 346{354, Amsterdam (NL), December [14] J.-P. Laumond, P. E. Jacobs, M. Tax, and R. M. Murray. A motion planner for non-holonomic mobile robots. IEEE Trans. Robotics and Automation, 10(5):577{593, October [15] A. Lazanas and J.-C. Latombe. Motion planning with uncertainty: a landmark approach. In Articial Intelligence, volume 76, pages 287{317, [16] T. Lozano-Perez, M. T. Mason, and R. H. Taylor. Automatic synthesis of ne motion strategies for robots. Int. Journal of Robotics Research, 3(1):3{24, [17] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacic Journal of Mathematics, 145(2):367{393, [18] A. Scheuer and Th. Fraichard. Continuous-curvature path planning for car-like vehicles. In Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robots and Systems, volume 2, pages 997{1003, Grenoble (FR), September [19] H. Takeda, C. Facchinetti, and J.-C. Latombe. Planning the motions of a mobile robot in a sensory uncertainty eld. IEEE Trans. on Pattern Analysis and Machine Intelligence, 16(10):1002{1017, October [20] R. Taylor. Synthesis of manipulator control programs from task-level specications. PhD thesis, Dept. of Computer Science, Stanford University, CA (US), Acknowledgements. This work was partially supported by the Inria-Inrets 2 Praxitele programme on individual urban public transports [ ], and the Inco-Copernicus ERB- IC15-CT project \Multi-agent robot systems for industrial applications in the transport domain" [ ]. 2 Institut National de REcherche sur les Transports et leur Securite. 6