From Reeds and Shepp's to Continuous-Curvature Paths

Download From Reeds and Shepp's to Continuous-Curvature Paths

Post on 08-Dec-2016

214 views

Category:

Documents

3 download

TRANSCRIPT

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1025

    [3] Z. Shiller and S. Dubowsky, Robust computation of path constrainedtime optimal motions, in Proc. IEEE Conf. Robotics and Automation,Cincinnati, OH, 1990, pp. 144149.

    [4] Z. Shiller and H. H. Lu, Computation of path constrained time optimalmotions with dynamic singularities, ASME J. Dynam. Syst., Meas.,Contr., vol. 114, no. 2, pp. 3440, 1992.

    [5] Y. Chen and A. A. Desrochers, Structure of minimum-time control lawfor robotic manipulators with constrained paths, in Proc. IEEE Conf.Robotics and Automation, Scottsdale, AZ, 1989, pp. 971976.

    [6] J. M. McCarthy and J. E. Bobrow, The number of saturated actuatorsand constraint forces during time-optimal movement of a general roboticsystem, IEEE Trans. Robot. Automat., vol. 8, pp. 407409, June 1992.

    [7] Z. Shiller, On singular time-optimal control along specied paths,IEEE Trans. Robot. Automat., vol. 10, pp. 561566, Aug. 1994.

    [8] P. Fiorini and Z. Shiller, Time optimal trajectory planning in dynamicenvironments, in Proc. IEEE Conf. Robotics and Automation, 1996, pp.15531558.

    [9] M. Galicki, The structure of time optimal controls for kinematically re-dundant manipulators with end-effector path constraints, in Proc. IEEEConf. Robotics and Automation, 1998, pp. 101106.

    [10] , Time-optimal controls of kinematically redundant manipulatorswith geometric constraints, IEEE Trans. Robot. Automat., vol. 16, pp.8993, Feb. 2000.

    [11] , The planning of robotic optimal motions in the presence of ob-stacles, Int. J. Robot. Res., vol. 17, no. 3, pp. 248259, 1998.

    [12] M. Galicki and I. Pajak, Optimal motion of redundant manipulatorswith state equality constraints, in Proc. IEEE Int. Symp. Assembly andTask Planning, 1999, pp. 181185.

    [13] I. Pajak and M. Galicki, The planning of suboptimal collision-freerobotic motions, in Proc. IEEE IES 1st Workshop Robot Motion andControl, 1999, pp. 229234.

    [14] D. Nenchev, Tracking manipulator trajectories with ordinary singulari-ties. A null space-based approach, Int. J. Robot. Res., vol. 14, no. 4, pp.399404, 1995.

    [15] K. Tchon, A normal form appraisal of the null space-based singularpath tracking, in Proc. IEEE IES 1st Workshop Robot Motion and Con-trol, 1999, pp. 263271.

    [16] M. Galicki, Real-time trajectory generation for redundant manipulatorswith path constraints, Int. J. Robot. Res., vol. 20, no. 7, pp. 673690,2001.

    [17] A. Balestrino, G. DeMaria, and L. Sciavicco, Robust control of roboticmanipulators, in Preprints 9th IFAC World Congr., 1984, pp. 8085.

    [18] W.A.Wolovich andH. A. Elliot, A computational technique for inversekinematics, in Proc. 23rd Conf. Decision and Control, Las Vegas, NV,1984, pp. 13591363.

    [19] L. Sciavicco and B. Siciliano, A solution algorithm to the inverse kine-matic problem for redundant manipulators, IEEE J. Robot. Automat.,vol. 4, pp. 403410, Aug. 1988.

    [20] B. Siciliano, A closed-loop inverse kinematic scheme for on-line joint-based robot control, Robotica, vol. 8, pp. 231243, 1990.

    [21] P. Chiacchio and S. Chiaverini, Coping with joint velocity limits inrst-order inverse kinematics algorithms: Analysis and real-time imple-mentation, Robotica, vol. 13, pp. 515519, 1995.

    [22] S. Chiaverini and G. Fusco, A new inverse kinematics algorithmwith path tracking capability under velocity and acceleration con-straints, in Proc. IEEE Conf. Decision and Control, 1999, pp.20642069.

    [23] G. Antonelli, S. Chiaverini, and G. Fusco, Kinematic control of re-dundant manipulators with on-line end-effector path tracking capabilityunder velocity and acceleration constraints, in Preprints 6th IFACSymp. Robot Control (SYROCO00), 2000, pp. 609614.

    [24] , An algorithm for on-line inverse kinematics with path trackingcapability under velocity and acceleration constraints, in Proc. IEEEConf. Decision and Control, 2000, pp. 50795084.

    [25] , Real-time end-effector path following for robot manipulatorssubject to velocity, acceleration, and jerk joint limits, in Proc. 2001IEEE/ASME Int. Conf. Advanced Intelligent Mechatronics, 2001, pp.452457.

    [26] H. Seraji and R. Colbaugh, Improved conguration control for redun-dant robots, J. Robot. Syst., vol. 6, no. 6, pp. 897928, 1990.

    [27] K. Glass, R. Colbaugh, D. Lim, and H. Seraji, Real-time collisionavoidance for redundant manipulators, IEEE Trans. Robot. Automat.,vol. 11, pp. 448457, June 1995.

    [28] A. V. Fiacco and G. P.McCormick,Nonlinear Programming: SequentialUnconstrained Minimization Techniques. New York: Wiley, 1968.

    [29] O. Khatib, Real-time obstacle avoidance for manipulators and mobilemanipulators, Int. J. Robot. Res., vol. 5, no. 1, pp. 9098, 1986.

    [30] E. G. Gilbert and D. W. Johnson, Distance functions and their appli-cation to robot path planning, IEEE J. Robot. Automat., vol. RA-1, pp.2130, Feb. 1985.

    [31] J.-H. Chuang, Potential-based modeling of three-dimensionalworkspace for obstacle avoidance, IEEE Trans. Robot. Automat.,vol. 14, pp. 778785, Oct. 1998.

    [32] J.-O. Kim and P. K. Khosla, Real-time obstacle avoidance using har-monic potential functions, IEEE Trans. Robot. Automat., vol. 8, pp.338349, June 1992.

    [33] L. Singh, J. Wen, and H. Stephanou, Motion planning and dynamiccontrol of a linked manipulator using modied magnetic elds, in Proc.IEEE Conf. Robotics and Automation, 1997, pp. 11421147.

    [34] E. Rimon and D. E. Koditschek, Exact robot navigation using articialpotential functions, IEEE Trans. Robot. Automat., vol. 8, pp. 501518,Oct. 1992.

    [35] R. A. Conn and M. Kam, Robot motion planning on N -dimensionalstar worlds amongmoving obstacles, IEEE Trans. Robot. Automat., vol.14, pp. 320325, Apr. 1998.

    [36] M. Spong and M. Vidyasagar, Robot Dynamics and Control. NewYork: Wiley, 1989.

    [37] M. Krstic, I. Kanellakopoulos, and P. Kokotovic, Nonlinear and Adap-tive Control Design. New York: Wiley, 1995.

    [38] X.Wang and E. K. Blum, Discrete-time versus continuous-timemodelsof neural networks, J. Comput. Syst. Sci., vol. 45, pp. 119, 1992.

    From Reeds and Shepps to Continuous-Curvature Paths

    Thierry Fraichard and Alexis Scheuer

    AbstractThis paper presents Continuous Curvature (CC) Steer, asteering method for car-like vehicles, i.e., an algorithm planning paths inthe absence of obstacles. CC Steer is the rst to compute paths with: 1) con-tinuous curvature; 2) upper-bounded curvature; and 3) upper-boundedcurvature derivative. CC Steer also veries a topological property thatensures that when it is used within a general motion-planning scheme, ityields a complete collision-free path planner. The coupling of CC Steerwith a general planning scheme yields a path planner that computescollision-free paths verifying the properties mentioned above. Accordingly,a car-like vehicle can follow such paths without ever having to stop in orderto reorient its front wheels. Besides, such paths can be followed with anominal speed which is proportional to the curvature derivative limit. Thepaths computed by CC Steer are made up of line segments, circular arcs,and clothoid arcs. They are not optimal in length. However, it is shownthat they converge toward the optimal Reeds and Shepp paths when thecurvature derivative upper bound tends to innity. The capabilities of CCSteer to serve as an efcient steering method within two general planningschemes are also demonstrated.

    Index TermsNonholonomic vehicles, smooth path planning.

    I. INTRODUCTION

    Ever since Laumonds pioneering paper in 1986 [1], much researchhas addressed collision-free path planning for nonholonomic systemsin general, and car-like vehicles in particular. Nonholonomic systems

    Manuscript received June 3, 2003; revised December 20, 2003. This paperwas recommended for publication by Associate Editor G. Oriolo and EditorH. Arai upon evaluation of the reviewers comments. This paper was presentedin part at the IEEE International Conference on Advanced Robotics, Tokyo,Japan, October 1999.T. Fraichard is with Inria, Montbonnot 38333 Saint Ismier Cedex, France

    (e-mail: thierry.fraichard@inria.fr).A. Scheuer is with the University Henry Poincar, Loria, BP 239, 54506 Van-

    doeuvre-ls-Nancy Cedex, France (e-mail: alexis.scheuer@loria.fr).Digital Object Identier 10.1109/TRO.2004.833789

    1552-3098/04$20.00 2004 IEEE

  • 1026 IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004

    are subject to kinematic constraints that restrict their admissible di-rections of motion. Nonholonomy makes path planning more difcult,since the paths planned must take into account the constraints imposedboth by the obstacles and the nonholonomic constraints (the reader isreferred to [2] for a recent and extensive review on this topic). Car-likevehicles are archetypal nonholonomic systems. They can only moveforward or backward in a direction perpendicular to the orientation oftheir rear wheels axle; besides, their turning radius is lower boundedbecause of the mechanical limits on the steering angle.The review of the research works that plan collision-free paths for

    car-like vehicles shows that most of them use a simplied model forthe car-like vehicle (one with only three conguration parameters inwhich the control is the angular velocity), and compute planar pathsmade up of line segments connected with tangential circular arcs ofminimum radius, e.g., [3][6], etc. The reason for this must be that theshortest path between two congurations for the simplied car is sucha path (result established rst by Dubins [7] for the car moving forwardonly, and later by Reeds and Shepp [8] for the car moving both forwardand backward).However, the curvature of this type of path is discontinuous. Dis-

    continuities occur at the transitions between segments and arcs and be-tween arcs with an opposite direction of rotation. The curvature beingrelated to the front wheels orientation, if a real car were to track pre-cisely such a path, it would have to stop at each curvature discontinuityso as to reorient its front wheels. Curvature continuity is, therefore,a desirable property. Besides, since the derivative of the curvature isrelated to the steering velocity of the car, it is also desirable that thederivative of the curvature be upper bounded so as to ensure that suchpaths can be followed with a given speed (proportional to the curvaturederivative limit). Reference [9] was the rst to recognize this issue andto address the problem of computing continuous-curvature paths withan upper-bounded curvature derivative; it introduced a model for thecar-like vehicle moving forward only, with curvature as an extra con-guration parameter in which the control is the angular accelerationrather than the angular velocity. This paper follows in [9]s footsteps.It also addresses the problem of planning paths with: 1) continuous cur-vature and 2) an upper-bounded curvature derivative for car-like vehi-cles. In addition to that, however, it considers: 3) upper-bounded cur-vature; 4) forward and backward motions; and 5) collision avoidance.The approach we have chosen to solve the problem at hand relies

    upon the design of a steering method, i.e., an algorithm that computesa path between two congurations in the absence of obstacles. Givensuch a steering method, it is possible to use it within a general mo-tion-planning scheme such as the Probabilistic Path Planner (PPP)[10], the Ariadnes Clew Algorithm (ACA) [11], or the HolonomicPath Approximation Algorithm [12], in order to deal with the obstaclesand solve the full problem (in these schemes, the steering methodis used along with a collision checker to connect pairs of selectedcongurations).The steering method is a key component in these planning schemes,

    and the main contribution of this paper is the rst steering method thatcomputes paths with: a) continuous curvature; b) upper-bounded cur-vature; and c) upper-bounded curvature derivative for car-like vehi-cles. Our steering method is topologically admissible, i.e., it veries atopological property [13] that ensures that the coupling with one of theaforementioned planning schemes yields a complete (or probabilisti-cally complete) collision-free path planner. Unlike [9] or [14], the focusof our work is not on optimal path planning, and our steering methoddoes not compute minimal-length paths. However, it computes paths

    whose length is close to the length of the optimal paths for the simpli-ed car (as a matter of fact, when the curvature derivative limit tendsto innity, the paths computed become Reeds and Shepp paths).There are a number of research works that are relevant to the design

    of a steering method such as ours; they are reviewed in Section II. Thenthe model of the car is detailed in Section III, while the steering methodproposed is described in Section IV. Finally, Section V presents exper-imental results for the steering method on its own, and embedded intwo general planning schemes.

    II. RELATED WORKS

    The rst class of works related to the problem of computing contin-uous-curvature paths falls into the path-smoothing class. The focusis on turning a sequence of congurations (or a nominal route, suchas a polygonal line) into a smooth curve that is then passed to thecontrol system of the vehicle. The curves used fall into two categories:1) curves whose coordinates have a closed-form expression, e.g.,B-splines [15], quintic polynomials [16], or polar splines [17]; and2) parametric curves whose curvature is a function of their arc length,e.g., clothoids [18], cubic spirals [19], quintic G2-splines [20], orintrinsic splines [21]. These approaches are interesting. Unfortunately,issues such as completeness or topological admissibility are com-pletely ignored. They usually make simplifying assumptions (on therespective positions of the congurations that are to be connected, onthe type of path generated, etc.) that render them unsuitable for ourpurpose.More relevant to our problem are the works stemming from the re-

    cent application of control-theory tools and ideas to study the con-trollability of nonholonomic systems in order to derive novel steeringmethods.First, optimal control theory could be used to solve the problem at

    hand [22]. Indeed, optimal-length paths would be the paths of choicefor a steering method. Unfortunately, optimal paths are extremely dif-cult to characterize, in general. This characterization is available for thesimplied car-like system [8], but not for the system considered herein.Besides, there are indications that the optimal paths for the system con-sidered herein are, in general, irregular, and cannot be used in practice,since they involve innite chattering (cf. Section III-B). Nevertheless,it would be possible, in theory, to compute an approximation of the op-timal paths using a numerical optimizationmethod, such as the one pre-sented in [23]. In practice, however, such a method poses a number ofproblems (cf. the experimental results reported in [24]). Among them,the fact that the convergence to the optimum is not guaranteed, meaningthat the topological admissibility is not guaranteed, either.Second, given that the car-like vehicle is similar (from a control point

    of view) to the system made up of a differential-drive vehicle pullingone trailer, it could be possible to use one of the steering methods thatwere proposed for such a system. These steering methods exploit dif-ferent properties of such a system, namely, nilpotence [25], chainedform [26], and differential atness [27]. We briey review them now.The reader is referred to [28] for underlying theoretical details (aboutthese properties, in particular), and to [24] for an experimental com-parison between these steering methods, when applied to the case of adifferential-drive vehicle pulling several trailers.1) Nilpotence: Nilpotent systems were rst studied in [25]. For

    such systems, it is possible to compute piecewise-constant controls,steering the system exactly to the goal. The car-like vehicle is notnilpotent, but it is possible to compute a nilpotent approximation ofsuch a system [29] and use it to steer the vehicle. Unfortunately, thegoal conguration would never be reached exactly.2) Chained Form: As for systems that can be converted into

    chained form [26], it is possible to steer them exactly to the goal

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1027

    using either sinusoidal [26], polynomial [30], or piecewise-constant[31] controls. For these steering methods, path planning takes placein a space dened by parameters whose physical meaning is unclear.The relationship between the shape of the path in the parameter spaceand the shape of the corresponding path in the actual workspace is avery complex one, to the extent that it is extremely difcult to obtaintopological admissibility (cf. [24]).3) Differential Flatness: It is a property enjoyed by certain me-

    chanical systems which was rst studied in [27]. In this case, too, pathplanning takes place in a space dened by parameters different, in gen-eral, from the conguration parameters. Exploiting this property, [32]designed an efcient steering method for differential-drive vehiclespulling trailers, a method that was later adapted to the case of a car-likevehicle [33]. This steering method, developed in parallel with the onepresented in this paper, is very close to being a good candidate to solveour problem. It is topologically admissible, and it computes contin-uous-curvature paths for car-like vehicles. However, it does not takeinto account the upper-bounded curvature-derivative constraint nor theupper-bounded curvature constraint (what it does is to compute a pathrst, and then check afterwards that the upper-bounded curvature con-straint is not violated).

    III. STATEMENT OF THE PROBLEM

    A. Model of the Car-Like VehicleAs mentioned earlier, in order to address the curvature discontinuity

    problem, [9] introduced the following model for a car-like vehicle Amoving forward only (Fig. 1):_x

    _y

    _

    _

    =

    cos

    sin

    0

    v +

    0

    0

    0

    1

    : (1)

    This model extends the well-known Dubins model [7] by consideringthe car as a four-dimensional system, with the curvature as an extraconguration parameter (the three other conguration parameters are(x; y), the coordinates of the reference pointR, and , the main orien-tation ofA). This system has two controls: 1) v, the driving velocity ofthe rear wheels; and 2) , the angular acceleration which is related to_

    , the steering velocity of the front wheels. Let b denote the wheelbaseof A, the following relationships hold:

    =

    tan

    b

    ; = _ =

    _

    b cos

    2

    : (2)

    The focus in [9] is on shortest-path planning. A constant unit-drivingvelocity is assumed, along with an upper bound on the angular accel-eration (stemming from the fact that the steering velocity of an actualcar is limited), in other wordsv = 1 and jj max

    : (3)Under these assumptions, planning a trajectory between two con-

    gurations is equivalent to computing a continuous-curvature planarcurve, with a bound on the curvature derivative, between two points inthe plane with prescribed tangents and curvatures.This model bounds the angular acceleration , rather than the

    steering velocity _ (although it is the steering velocity of A which isphysically limited). This choice is, in fact, conservative; it ensures thatthe steering velocity limit is never violated, no matter what the valueof is (indeed, 8 : _ _= cos2 ).For our purpose, we keep [9]s model and the constant unit-driving

    velocity assumption. However, for further realism, we enrich themodel as follows. First, to allow backward motions, v can nowtake two values: v = 1 (forward motion); and v = 1 (backwardmotion). In other words, cusp points are now allowed. At a cusp, A

    Fig. 1. Car-like vehicleA : R = (x; y) is the reference point and the mainorientation. is the steering angle and b the wheelbase.

    instantaneously changes its motion direction, both its orientation andcurvature remaining continuous. Second, given that is related to thesteering angle and that is mechanically limited, jj max

    , a

    bounded curvature constraint is introduced

    jj

    max

    = tan

    max

    =b: (4)Accordingly, our nal model for A is given by (1) withjj

    max

    ; jvj = 1; and jj max

    : (5)Henceforth, the term CC Car (for continuous-curvature car) is used

    to denote a vehicle with such a model. Planning a trajectory for the CCCar between two congurations is equivalent to computing a contin-uous-curvature planar curve joining two points in the plane with pre-scribed tangents and curvatures. Such a curve must verify the upperbounds on its curvature and curvature derivative.By design, such a curve can be followed at unit speed by A. To

    enableA to follow it with a different constant speed vd

    , the curvature-derivative limit should be set to max

    v

    1

    d

    .

    B. Properties of the CC CarFollowing the study of model (1) carried out by [9], [14], and [34],

    the CC Car model (1)+(5) was studied by Scheuer in [35] and [36]. It isestablished that the CC Car is small-time controllable [35, Th. 1]. Theset of congurations reachable from any conguration q before a timet contains a neighborhood of q for any t. The condition of existence ofthe optimal, i.e., shortest, paths is also established. In the absence ofobstacles, if a path exists between two congurations, then an optimalpath exists [35, Th. 2].The nature of the optimal paths is more difcult to establish. How-

    ever, [36] demonstrates that, for the CC Car moving forward only (v =1), the optimal paths are made up of line segments, circular arcs of ra-dius 1max

    , and clothoid arcs1 of sharpnessmax

    . It also demonstratesthat, whenever the shortest path includes a line segment (which is thecase as soon as the distance between the start and end congurationsis large enough), it involves innite chattering. In other words, it con-tains an innite number of clothoid arcs that accumulate toward theendpoints of the segment (these results are the extension of similar re-sults obtained in the case of the model (1) by [9] and [14]).Characterising the true nature of the optimal paths for the CC Car is

    beyond the scope of this paper. Based on the results mentioned above,it is conjectured that they will (at least) be made up of line segments,circular arcs, and clothoid arcs, and that they will be irregular. Thisprevented us from designing a steering method computing the optimalpath between two congurations. Instead, it prompted us to settle fora steering method computing paths essentially made up of locallyoptimal paths, i.e., line segments, circular arcs of radius 1max

    , andclothoid arcs of sharpness max

    .

    1A clothoid is a curve whose curvature varies linearly with its arc length:(s) = s + (0); is the sharpness of the clothoid.

  • 1028 IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004

    Fig. 2. Discontinuous curvature prole of a RS Path (left) versus piecewise-continuous curvature prole of a CC Path (right). The part from a to b is aclothoid arc.

    IV. STEERING THE CC CAR

    A. PrincipleThe steering method we have designed is called CC Steer. It com-

    putes paths called CC Paths that are derived from Reeds and Sheppspaths (RS Paths) [8].The RS Path between two congurations is the shortest path made up

    of line segments and circular arcs of minimum radius 1max

    . Its curva-ture prole looks like the one depicted in Fig. 2. CC Paths are similar toRS Paths, but in order to ensure curvature continuity, the circular arcsare replaced by special transitions calledCC Turns, whose purpose is tochange the CC Cars orientation. Their curvature varies continuouslyand piecewise linearly from 0 up and then down back to 0. They aremade up of circular arcs of minimum radius 1max

    and clothoid arcs ofsharpness jj max

    (Fig. 2).CC Turns and line segments are combined in order to form the CC

    Paths (cf. Sections IV-B and IV-C). However, the CC Paths obtainedby combining CC Turns and line segments only cannot account for thesmall-time controllability of theCCCar. To avoid this problem, an extratype of CC Paths made up only of line segments and clothoid arcs ofsharpness jj max

    is introduced (cf. Section IV-D).

    B. CC Turns1) General Case: In general, a CC Turn is made up of three parts:

    a) a clothoid arc of sharpness = max

    whose curvature varies from0 tomax

    ; b) a circular arc of radius1max

    ; and c) a clothoid arc ofsharpness , whose curvature varies from max

    to 0.What happens when the CC Car follows a CC Turn is illustrated in

    Fig. 3. Let qs

    = (x

    s

    ; y

    s

    ;

    s

    ; 0) be the start conguration. Without lossof generality, it is assumed that qs

    = (0; 0; 0; 0) and that the CC Carmoves forward while turning to the left. First, it follows a clothoid arcof length max

    =

    max

    and sharpness max

    until it reaches qi

    q

    i

    =

    x

    i

    = =

    max

    C

    f

    2

    max

    =

    max

    y

    i

    = =

    max

    S

    f

    2

    max

    =

    max

    i

    =

    2

    max

    =2

    max

    i

    =

    max

    (6)

    with Cf

    and Sf

    , the Fresnel integrals. Then it follows a circular arcof radius 1max

    until it reaches qj

    = (x

    j

    ; y

    j

    ;

    j

    ;

    max

    ). The center ofthis circular arc, , is located at distance 1max

    from qi

    in the directionnormal to i

    =

    x

    = x

    i

    1

    max

    sin

    i

    y

    = y

    i

    +

    1

    max

    cos

    i

    :

    (7)

    Finally, it follows a clothoid arc of sharpnessmax

    until it reachesthe goal conguration qg

    = (x

    g

    ; y

    g

    ;

    g

    ; 0). Let = (g

    s

    )mod2

    denote the change of orientation between qs

    and q. is the deectionof the CC Turn, and is used to characterize CC Turns. The deection ofthe CC Turn whose circular arc has zero length is min

    =

    2

    max

    1

    max

    .

    It is the angular value of the circular arc of a CC Turn, i.e., min

    ,

    that actually determines where the goal conguration is. The locus

    Fig. 3. CC Turns. General case.

    Fig. 4. CC Turns. = 0 and 0 < < cases.

    Fig. 5. Curvature prole and track of an elementary path of sharpness andlength l [37].

    of the goal congurations is a circle C+l

    (q

    s

    ), henceforth called a CCCircle, whose center is , and whose radius r is

    r = x

    2

    + y

    2

    : (8)In addition, the angle between the orientation of qg

    and the tangentto C+l

    (q

    s

    ) at qg

    is constant; it is the opposite of the angle between theorientation of qs

    and the tangent to C+l

    (q

    s

    ) at qs

    = atan (x

    =y

    ): (9)2) CC Turns of Small and Large Deections: With the denition

    above, a CC Turn makes a loop and intersects itself when the angleof its circular arc becomes too important (Fig. 4). It can be shown thatself-intersection happens for deections 0 < < min

    . In this case, wepropose to use instead a loopless and shorter path called an elementarypathmade up of a clothoid arc of sharpness max

    and a symmetricclothoid arc of sharpness (Fig. 5). Such a path is feasible, and we

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1029

    Fig. 6. CC Turns. + < 2 case.

    have shown in [38] that there is a unique max

    such that qg

    belongsto C+l

    (q

    s

    )

    =

    (cos(=2)C

    f

    ( =) + sin(=2)S

    f

    ( =))

    2

    r

    2

    sin

    2

    (=2 + )

    : (10)

    As for the = 0 case, the CC Turn reduces to the line segment oflength 2r sin, so as to ensure that the goal conguration also belongsto C+l

    (q

    s

    ) (Fig. 4).Finally, since the CC Car can make backward and forward motions,

    it is possible to further rene CC Turns so as to reduce their length.Once qi

    is reached, it is shorter for CC Turns of large deection toback up to qj

    instead of moving forward (Fig. 6). Given that the angleof the circular arc of a CC Turn of deection is min

    when theCC Car moves forward from qi

    to qj

    , and min

    2 when it movesbackward, the backward motion is shorter when min

    + .

    3) Properties of the CC Turns: In summary, a CC Turn allows theCC Car to reach any goal conguration qg

    with a null curvature whichis located on the circle C+l

    (q

    s

    ), and such that the angle between theorientation of qg

    and the tangent to C+l

    (q

    s

    ) at qg

    is constant. It is thedeection associated with qg

    that determines the nature of the CC Turn. = 0: The CC Turn is a line segment. 0 < < min

    : The CC Turn is made up of a clothoid arc ofsharpness max

    and a symmetric clothoid arc of sharpness.

    min

    <

    min

    + : The CC Turn is made up of a clothoidarc of sharpness max

    whose curvature varies from 0 to max

    , a

    forward circular arc of radius 1max

    , and a clothoid arc of sharp-ness max

    whose curvature varies from max

    to 0. min

    + < 2: The CC Turn is made up of a clothoidarc of sharpness max

    whose curvature varies from 0 to max

    ,

    a backward circular arc of radius 1max

    , and a clothoid arc ofsharpness max

    whose curvature varies from max

    to 0.The above analysis was carried out for the case of the CCCarmoving

    forward while turning to the left. The case where the CC Car is turningto the right is dealt with in the same manner, it yields a symmetric CCCircle C+r

    (q

    s

    ) (Fig. 7). Two similar CC Circles, Cl

    (q

    s

    ) and Cr

    (q

    s

    ),

    are obtained when the CC Car moves backward (Fig. 7).The arc length of a CC Turn depends upon its nature. Let l() denote

    the arc length of a CC Turn of deection . It is dened as follows(Fig. 8).

    = 0: The CC Turn is a line segment of length l(0) = 2r sin. 0 < < min

    : Let be the sharpness characterising the CCTurn in this case: l() = 2p

    1

    . l() increases monotonouslyfrom 2r sin to lmin

    = 2

    max

    1

    max

    . lmin

    is the arc length of thegeneral CC Turn whose circular arc has zero length.

    min

    <

    min

    +: In this case, the arc length of the CC Turnis lmin

    plus the arc length of its circular arc: l() = lmin

    + (

    min

    )

    1

    max

    . l() increases linearly from lmin

    to lmin

    +

    1

    max

    .

    Fig. 7. The four CCCirclesC (q ); C (q ); C (q ); andC (q ) attachedto q .

    Fig. 8. Arc length of a CC Turn ( = 1; = 1) as a function of .

    Fig. 9. How RS Paths are computed. Two circles of radius are attachedto the start and goal congurations q and q . Then a tangent intermediate linesegment (or circular arc of radius ) q q is used to connect two of thesecircles.

    min

    + < 2: This is the same case as above: l() =l

    min

    +(2+

    min

    )

    1

    max

    . l() decreases linearly from lmin

    +

    1

    max

    to lmin

    +

    min

    1

    max

    = 3

    max

    1

    max

    .

    C. From CC Turns to CC Paths

    CC Steer computes CC Paths by combining CC Turns and line seg-ments the same way circular arcs and line segments are combined toform RS Paths (a basic illustration of how RS Paths are computedis depicted in Fig. 9). Accordingly, CC Steer has to analyze the tan-gency relationships that may exist between the two sets of four CCCircles attached to the start and goal congurations, and line segmentsor other similar CC Circles. However, due to the fact that the orien-tation of the congurations located on the CC Circles make a con-stant angle with the tangent to these circles, the tangency relation-ships considered here (henceforth denoted by -tangency) is slightlydifferent from the classical one. Accordingly, before detailing how CC

  • 1030 IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004

    Fig. 10. -tangent line segments between two CC Circles. Top: external.Bottom: internal.

    Steer operates (Section IV-C.3), we present the particulars of the -tan-gency. Two cases are considered: -tangency between line segmentsand CC Circles (Section IV-C.1), and -tangency between CC Circles(Section IV-C.2).1) Line Segments-CC Circles -Tangency: In Reeds and Shepps

    case, two circles associated with two given congurations can be con-nected by one of the tangent line segments existing between them. Inour case, however, the -tangent line segments are different. Theymustcross the CC Circles so as to make an angle with the tangent at theintersection points.As in the regular tangent case, -tangent line segments can be in-

    ternal (i.e., separating) or external, depending on the type of CCCirclesthat are to be connected. Fig. 10 illustrates how-tangent line segmentsare obtained in two cases (the other cases are symmetric and derivedsimilarly).

    ConnectingC+r

    andCl

    : The -tangent line segment is externaland parallel to the line of centers12 [Fig. 10 (top)]. A straight-forward geometric analysis shows that the external -tangent linesegment q1

    q

    2

    exists iff

    l(

    1

    2

    ) 2r sin (11)and its length is

    l(q

    1

    q

    2

    ) = l(

    1

    2

    ) 2r sin: (12) Connecting C+r

    andCr

    : The -tangent line segment is internaland crosses the line of centers 12 [Fig. 10 (bottom)]. Onceagain, a straightforward geometric analysis shows that the in-ternal -tangent line segment exists iff

    l(

    1

    2

    ) 2r (13)and its length is

    l(q

    1

    q

    2

    ) = l(

    1

    2

    )

    2

    4r

    2

    cos

    2

    2r sin: (14)

    2) CC Circles -Tangency: In Reeds and Shepps case, two circlesassociated with two given congurations can also be connected by acircle tangent to both of them [Fig. 9 (left)]. Similarly, two CC Circlescan be connected, thanks to a third CC Circle. In our case, however,the tangency relationship between two CC Circles is different from theclassical tangency relationship existing between two circles. Two casesarise, depending on whether there is a change of direction of motionwhen passing from the rst CC Circle to the second one.

    Fig. 11. -tangency between two CC Circles. Top: No change of direction ofmotion. Bottom: change of direction of motion.

    No change of direction of motion: In this case, the -tangencycondition between two CCCirclesC1 andC2 is the classical one.C

    1 and C2 are -tangent if they are in the disposition depictedin the top part of Fig. 11. Let q1

    denote the conguration locatedat the contact point betweenC1 andC2, and that makes an angle with both C1 and C2. Then the path made up of the CC Turnfrom qs

    to q1

    and the CC Turn from q1

    to qs

    is a valid CC Path.q

    1

    belongs to the line of centers 12, and it exists iff

    l(

    1

    2

    ) = 2r: (15) Change of direction of motion: In this case, the -tangency con-dition between twoCCCirclesC1 andC2 is different.C1 andC2are -tangent if they are in the disposition depicted in the bottompart of Fig. 11. Let q1

    denote the conguration located at one ofthe intersection points between C1 and C2, and that makes anangle with both C1 and C2. Then the path made up of the CCTurn from qs

    to q1

    (moving forward), and the CC Turn from q1

    to qs

    (moving backward) is a valid CC Path. In this case, q1

    doesnot belong to the line of centers 12, and it can be shown thatit exists iff

    l(

    1

    2

    ) = 2r cos: (16)

    3) Building CC Paths Using CC Turns: There is an innite numberof ways to connect two given congurations with paths combining CCTurns and line segments. It is the same for RS Paths, but the key re-sult established by Reeds and Shepp [8] is that the shortest RS Pathsbelongs to a set of nine families. The conjectured irregularity of theoptimal paths for the CC Car would rule out the determination of suchfamilies. We settled for heuristically selected families instead. Our rstchoice was to use the families determined by Reeds and Shepp (welater experimented with an extended set of families but obtained mixedresults only [39]). These families are:

    (i)(ii)(iii) CCC or CjCC or CCjC(iv) CCjCC(v) CjCCjC(vi) CjCSCjC

    (vii) (viii) CjCSC or CSCjC(ix) CSC (17)

    where C (resp. S) denotes a CC Turn (resp. line segment), and j achange of direction of motion (a cusp point). Note that, by design, the

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1031

    CC Paths above can connect congurations with null curvature only,i.e., for which the front wheels orientation of the car is null (this re-striction is addressed later in Section V-B). Given two congurationswith null curvature, CC Steer operates by computing the shortest CCPaths among the families in (17).As far as computing the candidate CC Paths between two given con-

    gurations is concerned, it should be noted rst that all the families(17) are made up of one or more parts of type CSC;CC or CjC , andthat such parts are precisely the type of paths that were built in thetwo previous sections while studying the various -tangency properties(Sections IV-C.1 and IV-C.2). It should be noted also that the conditionof existence and the characterization of the CSC;CC and CjC pathsstem directly from these -tangency properties. Accordingly, the con-dition of existence of a CC Path of any given type is readily obtainedby combining these conditions of existence. Once the existence of agiven type of CC Path is ascertained, it can be computed and its arclength can be determined using (12), (14), and the results establishedin Section IV-B.3 on the arc length of a CC Turn.

    D. Small-Time Controllability Issues

    1) Topological Admissibility: As mentioned in Section I, the pur-pose of our steering method CC Steer is to be used within a generalmotion-planning scheme, such as the PPP [10], the ACA [11], or theHolonomic Path Approximation Algorithm [12]. In order to ensure thatthe coupling between the planning scheme and the steering methodyields a complete (or probabilistically complete) collision-free pathplanner, the steering method is required to account for the small-timecontrollability of the system under consideration. In other words, itmust be topologically admissible, i.e., verify the following topologicalproperty [13]:

    8" > 0;9 > 0; 8(q

    1

    ; q

    2

    ) 2 C

    2

    q

    2

    2 B(q

    1

    ; ) =) Steer (q1

    ; q

    2

    ) B(q

    1

    ") (18)

    where B(q; ") denotes the conguration space ball of size " centeredaround q, and Steer (q1

    ; q

    2

    ) denotes the path from q1

    to q2

    computedby the steering method. In other words, the steering method must beable to connect two -neighbor congurations with a path that remainsin an " neighborhood.Because of the nature of the CC paths, CC Steer is not topologically

    admissible. Indeed, no matter how close the start and goal congu-rations are, the CC Path connecting them includes at least one CCTurn (except if the start and goal congurations are perfectly aligned),and since the length of a CC Turn is lower bounded by 2r sin(cf. Section IV-B.3), (18) is violated. The approach we have chosen tomake CC Steer topologically admissible is to extend the set of families(17). A new family of CC paths is introduced, henceforth calledtopological paths. They are made up of line segments and clothoidarcs, they do not include CC Turns, and are designed so as to verify(18).2) Topological Paths: The topological path between two congu-

    rations qs

    and qg

    is the concatenation of two parts: reorientation path between qs

    and an intermediate congurationq

    j

    which has the same orientation as qg

    , and is located on the linepassing through qg

    with a direction perpendicular to the orienta-tion of qg

    (Fig. 12); lateral path between qj

    and qg

    (Fig. 13).Reorientation and lateral paths are described in the next two sections,

    respectively, whereas Appendix C establishes that the topological pathsverify the topological property (18).3) Reorientation Paths: The reorientation path dened for two con-

    gurations qs

    and qg

    in the disposition depicted in Fig. 12 has two parts:

    Fig. 12. Reorientation path dened for q and q , connecting q and q .

    Fig. 13. Lateral path between q and q .

    a backward turn starting from qs

    made up of two symmetricclothoid arcs to an intermediate conguration qi

    which has thesame orientation as qg

    ; a forward motion along a line segment until qj

    , the congurationlocated on the line passing through qg

    with a direction perpendic-ular to the orientation of qg

    , is reached.Appendix B details how reorientation paths are computed.4) Lateral Paths: The lateral path between between qj

    and qg

    hasthree parts (Fig. 13):

    a forward turn made up of two symmetric clothoid arcs con-necting qj

    and an intermediate conguration qk

    ; a backward motion along a line segment to an intermediate con-guration ql

    ; a forward turn made up of two symmetric clothoid arcs con-necting ql

    to qg

    .

    There is an innite number of such paths between qj

    and qg

    . Two extraconstraints are introduced in order to reduce this number to one. First,the lateral path should be symmetric with respect to the midpoint ofthe line segment connecting qj

    and qg

    . This constraint forces the linesegment part of the lateral path to pass through this midpoint. Second,the shortest lateral path verifying the previous constraint is selected tobe the lateral path between qj

    and qg

    . Appendix A details how lateralpaths are computed.

    E. CC SteerIn summary, given two congurations with null curvature, CC Steer

    computes all the existing CC Paths of the different families (17), plusthe topological path connecting them. Then CC Steer selects and re-turns the shortest candidate.In order to be complete, CC Steer must be able to compute a con-

    necting path between any pair of arbitrary congurations with null cur-vature. Given two such congurations, the existence of the CC Pathof one of the different families (17) connecting them depends on theirdisposition (cf. Section IV-C.3). However, by construction, the topo-logical path connecting them always exists (cf. Appendixes A and B).Accordingly, CC Steer is complete; it can connect any given pair ofcongurations with null curvature.In general, the topological path between two congurations is longer

    than the shortest CC Paths of the different families (17). However, whenthe start and goal congurations converge toward one another, the topo-logical path eventually becomes the shortest one, and it is selected byCC Steer. CC Steer therefore accounts for the small-time controlla-bility of the CC Car, and the coupling between CC Steer and one of theaforementioned general planning schemes yields a complete (or prob-abilistically complete) collision-free path planner.

  • 1032 IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004

    Fig. 14. Convergence of C (q ) when tends to innity.

    Fig. 15. Examples of CC Paths. (left) CjCC . (right) CSC .

    F. CC Paths and RS PathsLooking back at Fig. 2, it can be noted that the curvature prole of

    a CC Path would converge toward the curvature prole of a RS Pathshould max

    tend to innity. As a matter of fact, according to (6)(9),it can be veried that, when max

    tends to innity

    q

    i

    = (0; 0; 0;

    max

    )

    =

    1

    max

    sin

    s

    ;

    1

    max

    cos

    s

    r =

    1

    max

    = 0:

    In other words, C+l

    (q

    s

    ) becomes a circle of radius 1max

    tangent toq

    s

    (Fig. 14). This circle is precisely one of the circles that are used tocompute RS Paths. Similar convergence affects the other CC CirclesC

    +

    r

    (q

    s

    ); C

    l

    (q

    s

    ) and Cr

    (q

    s

    ). Accordingly, when max

    tends to in-nity, the CC Paths obtained converge toward the corresponding RSPaths.

    V. EXPERIMENTAL RESULTS

    A. CC Paths versus RS PathsFig. 15 depicts examples of paths computed by CC Steer. By de-

    sign, the transition congurations between CC Turns and line segments(marked as a cross in Fig. 15) have a null curvature (the orientation ofthe front wheels of A is null). A can therefore pass from one part of aCC Path to the next without having to stop in order to reorient its frontwheels (this is also true for cusp points).CC Steer has been implemented along with a function computing RS

    Paths. Comparisons were made regarding the length of the paths andthe time required for their computation. The ratio of CC over RS Pathslengths were computed for 1000 pairs of (start, goal) congurations.The results obtained for jmax

    j = 1 are summarized in Table I. Inmost cases (82%), CC paths are only about 10% longer than RS Paths.Similar experiments were carried out for the computation time. Therunning time of CC Steer is of the same order of magnitude (Table II).

    B. Embedding CC Steer in General Planning SchemesTo demonstrate the capabilities of CC Steer for collision-free path

    planning, we embedded it into two general planning schemes.First, we used the PPP [10] to plan collision-free paths for the CCCar

    moving forward only. In this case, the families of CC Paths considered

    TABLE IRS VERSUS CC PATH LENGTH (1000 PATHS)

    TABLE IIRS VERSUS CC PATH COMPUTATION TIME

    Fig. 16. Examples of collision-free paths (made up of CC Paths) for the CCCar moving forward only [38].

    Fig. 17. Collision-free path planning for the CC Car in a parallel-parking-likesituation [39].

    by CCSteer were restricted to paths without cusp points. Fig. 16 depictssome results obtained for a polygonal environment [38].Second, we used the ACA [11] to plan collision-free paths for the

    regular CC Car, i.e., with forward and backward motions. Unlike PPP,which is roadmap-based, ACA is a direct method; it develops a treerooted at the start conguration until the goal is reached. Fig. 17 de-picts the result obtained in a parallel-parking-like situation (the explo-ration tree is depicted on the left part of the gure). Fig. 18 shows apath-planning example in the Inria Rhne-Alpes parking lot [39]. Theexploration tree is displayed in grey. Note that since CC Steer permitsplacing tree nodes very far from each other, few tree nodes sufce tocover the whole free space, and the resulting path looks quite natural.In all cases, the collision-free paths resulting from the coupling of

    CC Steer with a general planning scheme are concatenations of CCPaths. Because CC Paths are designed to connect congurations withnull curvature, the transition congurations between two consecutiveCC Paths must have null curvature, and so must the start and goal con-gurations. The curvature prole of the overall resulting path is, there-fore, continuous, and a car-like vehicle can move from one CC path tothe next without ever having to stop in order to reorient its front wheels(of course, it has to stop at cusp points in order to change its motiondirection).In our opinion, the restriction on the curvature of the start and goal

    congurations is not so important, since an actual car-like vehiclecan always reorient its front wheels on the spot (and such a motion

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1033

    Fig. 18. Collision-free path planning for the CC Car in a parking lot [39].

    is collision-free). In other words, to solve a path-planning problembetween two congurations with nonnull curvature, the correspondingpath-planning problem between the same two congurations withnull curvature is solved using a general planning scheme coupledwith CC Steer. Then, the collision-free path obtained is completed bytwo on-the-spot front-wheel reorientations, both at the start and goalcongurations.

    VI. CONCLUSION

    In this paper, we have presented CC Steer, the rst steering methodfor a car-like vehicle that computes paths with continuous curvature,upper-bounded curvature, and upper-bounded curvature derivative.CC Steer computes paths made up of line segments, circular arcs,and clothoid arcs. It is designed to connect congurations with nullcurvature, i.e., for which the front wheels orientation of the caris null. CC Steer is complete, i.e., it can connect any such pair ofcongurations, and it veries a topological property that ensures thatthe coupling between CC Steer and a general planning scheme yieldsa complete collision-free path planner. CC Steer does not computeminimal-length paths. However, we have shown that it computes, inabout the same time, paths whose length is close to the length of theoptimal paths for the RS car (as a matter of fact, when the curvaturederivative limit tends to innity, the paths computed by CC Steer tendto the optimal RS paths).The coupling of CC Steer with a general motion-planning scheme

    yields a path planner that computes collision-free paths with a contin-uous curvature prole. A car-like vehicle can, therefore, follow such apath without ever having to stop in order to reorient its front wheels.Besides, such paths can be followed with a nominal speed which is pro-portional to the curvature derivative limit. Given that paths with con-tinuous curvature, upper-bounded curvature, and upper-bounded cur-vature derivative can be tracked at high speed with a much greater ac-curacy by real vehicles (cf. the experimental results obtained in [35]),the results reported herein fully demonstrate the interest of CC Steer.

    APPENDIX ACOMPUTING LATERAL PATHS

    With reference to Fig. 13, let qj

    denote the conguration of nullorientation, and let L denote the half-line of orientation anchored atq

    j

    (Fig. 19). Let qk

    (r) denote a conguration of orientation 2 locatedon L at a given distance r. The elementary path (cf. Section IV-B.2)connecting qj

    and qk

    (r) exists, and does not violate the upper-bounded

    Fig. 19. Computing a lateral path (see Fig. 13).

    curvature constraint (4) iff r is lower bounded (cf. [36, Property 4] or[37, Th. 1])

    r r

    1

    min

    () = 2

    1

    max

    2jjD(jj) (19)

    where D is the real function dened over [0; ] as

    D(u) = cosuC

    f

    2u

    + sinuS

    f

    2u

    with Cf

    and Sf

    the Fresnel integrals [40].The elementary path connecting qj

    and qk

    (r) is uniquely dened.The sharpness of its clothoid arcs is ([36, Property 4] or [37, eq. (3)])

    (; r) = 4sgn()

    D(jj)

    2

    r

    2

    (20)

    and its length is

    l(; r) = 2

    2

    (; r)

    : (21)

    Now, the upper-bounded curvature derivative constraint (3) yields anupper bound on the sharpness in (20), j(; r)j max

    , which yieldsan additional lower bound on r

    r r

    2

    min

    () = 2

    1

    max

    D(jj): (22)

    Let us dene the following function:

    r() = max r

    1

    min

    (); r

    2

    min

    () : (23)

    Given an orientation ; r() is the distance of the closest congu-ration qk

    of orientation 2 located on L that can be connected by anelementary path verifying both the upper-bounded curvature constraint(4) and the upper-bounded curvature-derivative constraint (3).The line of orientation 2 passing through qk

    intersects the line per-pendicular to qj

    at a point located at a distance d() from qj

    d() =

    r() sin(2)

    2 cos() cos(2)

    : (24)

    Now, to determine the lateral path between two congurations qj

    andq

    g

    separated by a distance ", it sufces to use (24) in order to determine such that d() = "=2.

    APPENDIX BCOMPUTING REORIENTATION PATHS

    Let qg

    denote a goal conguration of null orientation, and let de-note the orientation gap between qg

    and the start conguration qs

    . LetL denote the half-line of orientation +=2 anchored at qs

    (Fig. 20).As per Appendix A, a backward elementary path starting from qs

    can

  • 1034 IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004

    Fig. 20. Computing a reorientation path.

    connect any conguration of null orientation located on L at a distancegreater than r(=2).Let qi

    denote the conguration of null orientation located onL at theminimum distance r(=2). The elementary path connecting qs

    andq

    i

    is uniquely dened (cf. Section A), and so is the conguration qj

    ,

    which is the intersection between the line of null orientation passingthrough qi

    and the line passing through qs

    , with a direction perpendic-ular to the orientation of qs

    . The reorientation path dened for qs

    andq

    g

    is, therefore, completely determined.

    APPENDIX CTOPOLOGICAL ADMISSIBILITY OF THE TOPOLOGICAL PATHS

    Let us assume that qs

    and qg

    are separated by a distance (in bothposition and orientation).Consider Fig. 20. qi

    is located at a distance r(=2) from qs

    , and thelength of the elementary path connecting qs

    and qi

    is l(=2; r(=2)).A straightforward geometric analysis shows that the distance betweenq

    i

    and qj

    is jr(=2) cos(=2) j. It can be concluded then that thereorientation path from qs

    to qj

    is entirely contained in a ball centeredat qs

    of radius

    r

    r

    = l(=2; r(=2))+ jr(=2) cos(=2) j:

    Consider now Figs. 13 and 19. It can be shown that the distancebetween qj

    and qg

    is jr(=2) sin(=2)j. Let be such that d() =jr(=2) sin(=2)j=2, then qk

    (resp. ql

    ) is located at a distance r()from qj

    (resp. qg

    ). As for the elementary paths from qj

    to qk

    , and ql

    toq

    g

    , their lengths are l(; r()). It can be concluded then that the lateralpath from qj

    to qg

    is entirely contained in a ball centered at qj

    of radius

    r

    l

    = jr(=2) sin(=2) j+ l(; r()):

    Accordingly, the topological path between qs

    and qg

    is entirely con-tained in a ball centered at qs

    of radius

    r

    t

    = r

    r

    + r

    l

    :

    When qs

    converges toward qg

    , i.e., when converges toward 0,the orientation , such that d() = jr(=2) sin(=2) j=2, alsoconverges toward 0 (indeed, = 0 is a straightforward solution tod() = 0). Now, given that both

    lim

    !0

    r() = 0 and lim;r!0

    l(; r) = 0

    we have rt

    that converges toward 0. Accordingly, thanks to the topo-logical paths, CC Steer can access a neighborhood of a congurationq

    s

    without escaping a ball centered at qs

    (no matter how small the ball).It accounts for the small-time controllability of A.

    REFERENCES

    [1] J.-P. Laumond, Feasible trajectories for mobile robots with kinematicand environment constraints, in Proc. Int. Conf. Intelligent AutonomousSystems, Amsterdam, The Netherlands, Dec. 1986, pp. 346354.

    [2] J.-P. Laumond, Ed., Robot Motion Planning and Control. New York:Springer, 1998, vol. 229, Lecture Notes in Control and Information Sci-ence.

    [3] J. Barraquand and J.-C. Latombe, On nonholonomic mobile robots andoptimal maneuvering, Revue dIntell. Artif., vol. 3, no. 2, pp. 77103,1989.

    [4] P. Jacobs and J. Canny, Planning smooth paths for mobile robots, inProc. IEEE Int. Conf. Robotics and Automation, Scottsdale, AZ, May1989, pp. 27.

    [5] T. Fraichard, Smooth trajectory planning for a car in a structuredworld, in Proc. IEEE Int. Conf. Robotics and Automation, vol. 1,Sacramento, CA, Apr. 1991, pp. 318323.

    [6] P. Svestka and M. H. Overmars, Probabilistic path planning, Dept.Comput. Sci., Utrecht Univ., Utrecht, The Netherlands, Tech. Rep.UU-CS-1995-22, May 1995.

    [7] L. E. Dubins, On curves of minimal length with a constraint on av-erage curvature, and with prescribed initial and terminal positions andtangents, Amer. J. Math., vol. 79, pp. 497517, 1957.

    [8] J. A. Reeds and L. A. Shepp, Optimal paths for a car that goes bothforward and backward, Pacic J. Math., vol. 145, no. 2, pp. 367393,1990.

    [9] J.-D. Boissonnat, A. Crzo, and J. Leblond, A note on shortest pathsin the plane subject to a constraint on the derivative of the curvature,INRIA, Rocquencourt, France, Res. Rep. 2160, Jan. 1994.

    [10] L. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, Proba-bilistic roadmaps for path planning in high dimensional congurationspaces, IEEE Trans. Robot. Automat., vol. 12, pp. 566580, Aug.1996.

    [11] E. Mazer, J.-M. Ahuactzin, and P. Bessire, The Ariadnes Clew algo-rithm, J. Artif. Intell. Res., vol. 9, pp. 295316, JulyDec. 1998.

    [12] J.-P. Laumond, P. E. Jacobs, M. Tax, and R. M. Murray, A motionplanner for nonholonomicmobile robots, IEEE Trans. Robot. Automat.,vol. 10, pp. 577593, Oct. 1994.

    [13] S. Sekhavat and J.-P. Laumond, Topological property for collision-freenonholonomic motion planning: The case of sinusoidal inputs forchained form systems, IEEE Trans. Robot. Automat., vol. 14, pp.671680, Oct. 1998.

    [14] H. J. Sussmann, The MarkovDubins problem with angular acceler-ation control, in Proc. IEEE Conf. Decision and Control, vol. 3, SanDiego, CA, Dec. 1997, pp. 26392643.

    [15] K. Komoriya and K. Tanie, Trajectory design and control of a wheel-typemobile robot using B-spline curve, inProc. IEEE/RSJ Int. Conf. In-telligent Robots and Systems, Tsukuba, Japan, Sept. 1989, pp. 398405.

    [16] A. Takahashi, T. Hongo, and Y. Ninomiya, Local path planning andcontrol for AGV in positioning, in Proc. IEEE/RSJ Int. Conf. IntelligentRobots and Systems, Tsukuba, Japan, Sept. 1989, pp. 392397.

    [17] W. L. Nelson, Continuous curvature paths for autonomous vehicles, inProc. IEEE Int. Conf. Robotics and Automation, vol. 3, Scottsdale, AZ,May 1989, pp. 12601264.

    [18] R. Liscano and D. Green, Design and implementation of a trajectorygenerator for an indoor mobile robot, in Proc. IEEE/RSJ Int. Conf.Intelligent Robots and Systems, Tsukuba, Japan, Sept. 1989, pp.380385.

    [19] Y. Kanayama and B. I. Hartman, Smooth local path planning for au-tonomous vehicles, in Proc. IEEE Int. Conf. Robotics and Automation,vol. 3, Scottsdale, AZ, May 1989, pp. 12651270.

    [20] A. Piazzi and C. Guarino Lo Bianco, QuinticG -splines for trajectoryplanning of autonomous vehicles, in Proc. IEEE Intelligent VehiclesSymp., Dearborn, MI, Oct. 2000, pp. 198203.

    [21] H. Delingette, M. Hbert, and K. Ikeuchi, Trajectory generation withcurvature constraint based on energy minimization, in Proc. IEEE/RSJInt. Conf. Intelligent Robots and Systems, vol. 1, Osaka, Japan, Nov.1991, pp. 206211.

    [22] A. E. Bryson and Y. C. Ho, Applied Optimal Control. Bristol, PA:Hemisphere, 1975.

    [23] C. Fernandez, L. Gurvits, and Z. L. Li, A variational approach tooptimal nonholonomic motion planning, in Proc. IEEE Int. Conf.Robotics and Automation, vol. 1, Nagoya, Japan, May 1995, pp.680685.

    [24] S. Sekhavat, Planication de mouvements sans collisions pour des sys-tmes non holonomes, Ph.D. dissertation, Inst. Nat. Polytech. Toulouse,Toulouse, France, 1995.

  • IEEE TRANSACTIONS ON ROBOTICS, VOL. 20, NO. 6, DECEMBER 2004 1035

    [25] G. Lafferire and H. Sussmann, A differential geometric approachto motion planning, in Nonholonomic Motion Planning, Z. Li and J.Canny, Eds. Norwell, MA: Kluwer, 1993, pp. 235270.

    [26] R. M. Murray and S. S. Sastry, Nonholonomic motion planning:Steering using sinusoids, IEEE Trans. Automat. Contr., vol. 38, pp.700716, Sept. 1993.

    [27] P. Rouchon,M. Fliess, M. Lvine, and P.Martin, Flatness, motion plan-ning and trailer system, in Proc. IEEE Conf. Decision and Control, SanAntonio, TX, Dec. 1993, pp. 27002705.

    [28] J.-P. Laumond, S. Sekhavat, and F. Lamiraux, Guidelines in nonholo-nomic motion planning for mobile robots, in Robot Motion Planningand Control, J.-P. Laumond, Ed. New York: Springer, 1998, vol. 229,Lecture Notes in Control and Information Science, pp. 153.

    [29] A. Bellache, J.-P. Laumond, and M. Chyba, Canonical nilpotent ap-proximation of control systems: Application to nonholonomic motionplanning, in Proc. IEEE Int. Conf. Decision and Control, San Antonio,TX, 1993, pp. 26942699.

    [30] D. Tilbury, R. Murray, and S. Sastry, Trajectory generation for then-trailer problem using Goursat normal form, IEEE Trans. Automat.Contr., vol. 40, pp. 802819, May 1995.

    [31] S.Monaco and D. Normand-Cyrot, An introduction to motion planningunder multi-rate digital control, in Proc. IEEE Int. Conf. Decision andControl, Tucson, AZ, 1992, pp. 17801785.

    [32] S. Sekhavat, F. Lamiraux, J.-P. Laumond, G. Bauzil, and A. Ferrand,Motion planning and control for Hilare pulling a trailer: Experimentalissues, inProc. IEEE Int. Conf. Robotics and Automation, Albuquerque,NM, Apr. 1997, pp. 33063311.

    [33] F. Lamiraux and J.-P. Laumond, Smooth motion planning for car-likevehicles, IEEE Trans. Robot. Automat., vol. 17, pp. 498502, Aug.2001.

    [34] E. Degtiariova-Kostova and V. Kostov, Irregularity of optimal trajecto-ries in a control problem for a car-like robot, INRIA, Sophia Antipolis,France, Res. Rep. 3411, Apr. 1998.

    [35] A. Scheuer and C. Laugier, Planning sub-optimal and continuous-cur-vature paths for car-like robots, in Proc. IEEE/RSJ Int. Conf. IntelligentRobots and Systems, vol. 1, Victoria, BC, Canada, Oct. 1998, pp. 2531.

    [36] A. Scheuer, Planication de chemins courbure continue pourrobot mobile non-holonome, Ph.D. dissertation, Inst. Nat. Polytech.Grenoble, Grenoble, France, Jan. 1998.

    [37] A. Scheuer and T. Fraichard, Collision-free and continuous-curvaturepath planning for car-like robots, in Proc. IEEE Int. Conf. Robotics andAutomation, vol. 1, Albuquerque, NM, Apr. 1997, pp. 867873.

    [38] , Continuous-curvature path planning for car-like vehicles, inProc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, vol. 2,Grenoble, France, Sept. 1997, pp. 9971003.

    [39] T. Fraichard and J.-M. Ahuactzin, Smooth path planning for cars, inProc. IEEE Int. Conf. Robotics and Automation, Seoul, Korea, May2001, pp. 37223727.

    [40] M.Abramovitz and I. A. Stegun, Eds.,Handbook ofMathematical Func-tions. New York: Dover, 1965.