Lecture Notes in Control and Information Sciences doc

The motion planning problem asks for determining such a path through the free space in an efficient way.. In this context, motion planning consists in building the free configuration spa

in Control and Information Sciences 229

Editor: M Thoma

A Bensoussan • M.J Grimble • P Kokotovic H Kwakernaak

J.L Massey • Y.Z Tsypkin

British Library Cataloguing in Publication Data

Robot motion planning and control - (Lecture notes in

control and information sciences ; 229)

1.Robots - Motion 2.Robots - Control systems

I.Laumond, J.-P

629.8'92

ISBN 35400762191

Library o f Congress Cataloging-in-Publication Data

Robot motion planning and control / J -P Laumond (ed.)

p crr~ - - (Lecture notes in control and information sciences ; 229)

Includes bibliographical references

ISBN3-540-76219-1 (pbk : alk, paper)

1 Robots- -Motion 2 Robots- -Control systems L Laumond, J -P (Jean-Paul) IL Series

Apart from any fair dealing for the purposes of research or private study, or criticism or review, as permitted under the Copyright, Designs and Patents Act 19S8, this publication may only be reproduced, stored or transmitted, in any form or by any means, with the prior permission in writing o f the publishers, or in the case of reprographic reproduction in accordance with the terms of licences issued

by the Copyright Licensing Agency Enquiries concerning reproduction outside those terms should be sent to the publishers

Springer-Verlag London Limited 1998

Printed in Great Britain

The use o f registered names, trademarks, etc in this publication does not imply, even in the absence o f a specific statement, that such names are exempt from the relevant laws and regulations and therefore free for general use

The publisher makes no representation, express or implied, with regard to the accuracy of the information contained in this book and cannot accept any legal responsibility or liability for any errors

or omissions that may be made

Typesetting: Camera ready by editor

Printed and bound at the Athenaeum Press Ltd, Gateshead

6913830-543210 Printed on acid-free paper

in robot simulation software to help work cell designers to determine collision free paths for robots performing specific tasks

R o b o t M o t i o n Planning and C o n t r o l r e q u i r e s interdisciplinarity

The research in robot motion planning can be traced back to the late 60's, during the early stages of the development of computer-controlled robots Nev- ertheless, most of the effort is more recent and has been conducted during the 80's (Robot Motion Planning, J.C Latombe's book constitutes the reference in the domain)

The position (configuration) of a robot is normally described by a number

of variables For mobile robots these typically are the position and orientation

of the robot (i.e 3 variables in the plane) For articulated robots (robot arms) these variables are the positions of the different joints of the robot arm A motion for a robot can, hence, be considered as a path in the configuration space Such a path should remain in the subspace of configurations in which there is no collision between the robot and the obstacles, the so-called free space The motion planning problem asks for determining such a path through the free space in an efficient way

Motion planning can be split into two classes When all degrees of freedom can be changed independently (like in a fully actuated arm) we talk about

hotonomic motion planning In this case, the existence of a collision-free path

is characterized by the existence of a connected component in the free config- uration space In this context, motion planning consists in building the free configuration space, and in finding a path in its connected components Within the 80's, Roboticians addressed the problem by devising a variety

of heuristics and approximate methods Such methods decompose the config- uration space into simple cells lying inside, partially inside or outside the free space A collision-free path is then searched by exploring the adjacency graph

of free cells

In the early 80's, pioneering works showed how to describe the free config- uration space by algebraic equalities and inequalities with integer coefficients (i.e as being a semi-algebraic set) Due to the properties of the semi-algebraic sets induced by the Tarski-Seidenberg Theorem, the connectivity of the free configuration space can be described in a combinatorial way From there, the road towards methods based on Real Algebraic Geometry was open At the same time, Computational Geometry has been concerned with combinatorial bounds and complexity issues It provided various exact and efficient meth- ods for specific robot systems, taking into account practical constraints (like environment changes)

More recently, with the 90's, a new instance of the motion planning problem has been considered: planning motions in the presence of kinematic constraints (and always amidst obstacles) When the degrees of freedom of a robot sys- tem are not independent (like e.g a car that cannot rotate around its axis without also changing its position) we talk about nonholonomic motion plan- ning In this case, any path in the free configuration space does not necessarily correspond to a feasible one Nonholonomic motion planning turns out to be much more difficult than holonomic motion planning This is a fundamental issue for most types of mobile robots This issue attracted the interest of an increasing number of research groups The first results have pointed out the necessity of introducing a Differential Geometric Control Theory framework in nonholonomic motion planning

On the other hand, at the motion execution level, nonholonomy raises an- other difficulty: the existence of stabilizing smooth feedback is no more guaran- teed for nonholonomic systems Tracking of a given reference trajectory com- puted at the planning level and reaching a goal with accuracy require non- standard feedback techniques

Four main disciplines are then involved in motion planning and control However they have been developed along quite different directions with only little interaction The coherence and the originality that make motion plan- ning and control a so exciting research area come from its interdisciplinarity

It is necessary to take advantage from a common knowledge of the different theoretical issues in order to extend the state of the art in the domain

A b o u t t h e b o o k

The purpose of this book is not to present a current state of the art in motion planning and control We have chosen to emphasize on recent issues which have been developed within the 90's In this sense, it completes Latombe's book published in 1991 Moreover an objective of this book is to illustrate the necessary interdisciplinarity of the domain: the authors come from Robotics,

VII

Computational Geometry, Control Theory and Mathematics All of them share

a common understanding of the robotic problem

The chapters cover recent and fruitful results in motion planning and con- trol Four of them deal with nonholonomic systems; another one is dedicated

to probabilistic algorithms; the last one addresses collision detection, a critical operation in algorithmic motion planning

Nonholonomic Systems The research devoted to nonholonomic systems is mo-

tivated mainly by mobile robotics The first chapter of the book is dedicated

to nonholonomic path planning It shows how to combine geometric algorithms and control techniques to account for the nonholonomic constraints of most mobile robots The second chapter develops the mathematical machinery nec- essary to the understanding of the nonholonomic system geometry; it puts emphasis on the nonholonomic metrics and their interest in evaluating the combinatorial complexity of nonholonomic motion planning In the third chap- ter, optimal control techniques are applied to compute the optimal paths for car-like robots; it shows that a clever combination of the maximum principle and a geometric viewpoint has permitted to solve a very difficult problem The fourth chapter highlights the interactions between feedback control and motion planning primitives; it presents innovative types of feedback controllers facing nonholonomy

Probabilistic Approaches While complete and deterministic algorithms for mo-

tion planning are very time-consuming as the dimension of the configuration space increases, it is now possible to address complicated problems in high di- mension thanks to alternative methods that relax the completeness constraint for the benefit of practical efficiency and probabilistic completeness The fifth chapter of the book is devoted to probabilistic algorithms

Collision Detection Collision checkers constitute the main bottleneck to con-

ceive efficient motion planners Static interference detection and collision detec- tion can be viewed as instances of the same problem, where objects are tested for interference at a particular position, and along a trajectory Chapter six presents recent algorithms benefiting from this unified viewpoint

The chapters are self-contained Nevertheless, many results just mentioned

in some given chapter may be developed in another one This choice leads to repetitions but facilitates the reading according to the interest or the back- ground of the reader

O n t h e origin o f t h e b o o k

All the authors of the book have been involved in PROMotion PROMotion was a European Project dedicated to robot motion planning and control It has progressed from September 1992 to August 1995 in the framework of the Basic Research Action of ESPRIT 3, a program of research and development in In- formation Technologies supported by the European Commission (DG III) The work undertaken under the project has been aimed at solving concrete prob- lems Theoretical studies have been mainly motivated by a practical efficiency Research in PROMotion has then provided methods and their prototype im- plementations which have the potential of becoming key components of recent programs in advanced robotics

In few numbers, PROMotion is a project whose cost has been 1.9 MEcus 1 (1.1 MEcus supported by European Community), for a total effort of more than 70 men-year, 179 research reports (most them have been published in international conferences and journals), 10 experiments on real robot platforms,

an International Spring school and 3 International Workshops This project has been managed by LAAS-CNRS in Toulouse; it has involved the "Universitat Politecnica de Catalunya" in Barcelona, the "Ecole Normale Sup@rieure" in Paris, the University "La Sapienza" in Roma, the Institute INRIA in Sophia- Antipolis and the University of Utrecht

J.D Boissonnat (INRIA, Sophia-Antipolis), A De Luca (University "La Sapienza" of Roma), M Overmars (Utrecht University), J.J Risler (Ecole Nor- male Sup6rieure and Paris 6 University), C Torras (Universitat Politecnica de Catalunya, Barcelona) and the author make up the steering committee of PRO- Motion This book benefits from contributions of all these members and their co-authors and of the work of many people involved in the project

On behalf of the project committee, I thank J Wejchert (Project oflicier

of PROMotion for the European Community), A Blake (Oxford University),

H Chochon (Alcatel) and F Wahl (Brannschweig University) who acted as reviewers of the project during three years Finally I thank J Som for her efficient help in managing the project and M Herrb for his help in editing this book

Jean-Paul Laumond LAAS-CNRS, Toulouse

August 1997

1 US $ 1 ~ 1 Ecu

2004, Route des Lucioles BP 93

06902 Sophia Antipolis Cedex, France

boissonn©sophia, inria, fr

F Jean Institut de Math@matiques Universit@ Pierre et Marie Curie Tour 46, 5~me @tage, Boite 247

4 Place Jussieu

75252 Paris Cedex 5 France

j ean~math, j u s s i e u , fr

F Lamiraux LAAS-CNRS

7 Avenue du Colonel Roche

31077 Toulouse Cedex 4 France

lamiraux©laas, f r

G Oriolo Dipartimento di Informatica

e Sistemistica Universit£ di Roma "La Sapienza" Via Eudossiana 18

00184 Roma Italy

oriolo@giannutri, caspur, it

INRIA Centre de Sophia Antipolis

2004, Route des Lucioles BP 93

06902 Sophia Antipolis Cedex,

4 Place Jussieu

75252 Paris Cedex 5 France

risler@math, jussieu, fr

S Sekhavat LAAS-CNRS

7 Avenue du Colonel Roche

31077 Toulouse Cedex 4 France

sepanta©laas, fr

P Svestka Department of Computer Science, Utrecht University

P.O.Box 80.089,

3508 TB Utrecht, the Netherlands

petr@cs, ruu.nl

C Torras Institut de Robbtica

i Informatica Industrial Gran Capita, 2

08034-Barcelona Spain

torras@iri, upc es

Table of Contents

G u i d e l i n e s i n N o n h o l o n o m i c M o t i o n P l a n n i n g f o r M o b i l e R o b o t s 1

J.P Laumond, S Sekhavat, F Lamiraux

1 Introduction 1

2 Controllabilities of mobile robots 2

3 P a t h planning and small-time controllability 10

4 Steering methods 13

5 Nonholonomic p a t h planning for small-time controllable systems 23

6 Other approaches, other systems 42

7 Conclusions 44

G e o m e t r y o f N o n h o l o n o m i c S y s t e m s 55

A BeUa~'che, F Jean, J.-J Risler 1 Symmetric control systems: an introduction 55

2 The car with n trailers 73

3 Polynomial systems 82

O p t i m a l T r a j e c t o r i e s f o r N o n h o l o n o m i c M o b i l e R o b o t s 93

P Sou~res, J.-D Boissonnat 1 Introduction 93

2 Models and optimization problems 94

3 Some results from Optimal Control Theory 97

4 Shortest paths for the Reeds-Shepp car 107

5 Shortest paths for Dubins' Car 141

6 Dubins model with inertial control law 153

7 Time-optimal trajectories for Hilare-tike mobile robots 161

8 Conclusions 166

F e e d b a c k C o n t r o l o f a N o n h o l o n o m i c C a r - L i k e R o b o t 1 7 1 A De Luca, G Oriolo, C Samson 1 Introduction 171

2 Modeling and analysis of the car-like robot 179

3 Trajectory tracking 189

4 P a t h following and point stabilization 213

5 Conclusions 247

6 Further reading 249

P r o b a b i l i s t i c P a t h P l a n n i n g

P Svestka, M 1t Overmars 2 5 5 1 I n t r o d u c t i o n 255

2 T h e P r o b a b i l i s t i c P a t h P l a n n e r 258

3 A p p l i c a t i o n t o h o l o n o m i c r o b o t s 266

4 A p p l i c a t i o n t o n o n h o l o n o m i c r o b o t s 270

5 O n p r o b a b i l i s t i c c o m p l e t e n e s s of p r o b a b i l i s t i c p a t h p l a n n i n g 279

6 O n t h e e x p e c t e d c o m p l e x i t y of p r o b a b i l i s t i c p a t h p l a n n i n g 285

7 A m u l t i - r o b o t e x t e n s i o n 291

8 C o n c l u s i o n s 300

C o l l i s i o n D e t e c t i o n A l g o r i t h m s for M o t i o n P l a n n i n g 305

P Jimdnez, F Thomas, C Torras 1 I n t r o d u c t i o n 305

2 I n t e r f e r e n c e d e t e c t i o n 306

3 C o l l i s i o n d e t e c t i o n 317

4 C o l l i s i o n d e t e c t i o n in m o t i o n p l a n n i n g 335

5 C o n c l u s i o n s 338

Guidelines in Nonholonomic Motion Planning

for Mobile Robots

J.P Laumond, S Sekhavat and F Lamiraux

LAAS-CNRS, Toulouse

1 I n t r o d u c t i o n

Mobile robots did not wait to know that they were nonholonomic to plan and execute their motions autonomously It is interesting to notice that the first navigation systems have been published in the very first International Joint Conferences on Artificial Intelligence from the end of the 60's These systems were based on seminal ideas which have been very fruitful in the development

of robot motion planning: as examples, in 1969, the mobile robot Shakey used

a grid-based approach to model and explore its environment [61]; in 1977 Jason used a visibility graph built from the corners of the obstacles [88]; in 1979 Hilare decomposed its environment into collision-free convex cells [30]

At the end of the 70's the studies of robot manipulators popularized the notion of configuration space of a mechanical system [53]; in this space the

"piano" becomes a point The motion planning for a mechanical system is reduced to path finding for a point in the configuration space The way was open

to extend the seminal ideas and to develop new and well-grounded algorithms (see Latombe's book [42])

One more decade, and the notion of nonholonomy (also borrowed from Mechanics) appears in the literature [44] on robot motion planning through the problem of car parking which was not solved by the pioneering mobile robot navigation systems Nonholonomic Motion Planning then becomes an attractive research field [52]

This chapter gives an account of the recent developments of the research in this area by focusing on its application to mobile robots

Nonholonomic systems are characterized by constraint equations involving the time derivatives of the system configuration variables These equations are non integrable; they typically arise when the system has less controls than configuration variables For instance a car-like robot has two controls (linear and angular velocities) while it moves in a 3-dimensional configuration space

As a consequence, any path in the configuration space does not necessarily correspond to a feasible path for the system This is basically why the purely geometric techniques developed in motion planning for holonomic systems do not apply directly to nonholonomic ones

While the constraints due to the obstacles are expressed directly in the man- ifold of configurations, nonholonomic constraints deal with the tangent space

In the presence of a link between the robot parameters and their derivatives, the first question to be addressed is: does such a link reduce the accessible con- figuration space ? This question may be answered by studying the structure of the distribution spanned by the Lie algebra of the system controls

Now, even in the absence of obstacle, planning nonholonomic motions is not an easy task Today there is no general algorithm to plan motions for any nonholonomic system so that the system is guaranteed to exactly reach a given goal The only existing results are for approximate methods (which guarantee only that the system reaches a neighborhood of the goal) or exact methods for special classes of systems; fortunately, these classes cover almost all the existing mobile robots

Obstacle avoidance adds a second level of difficulty At this level we should take into account both the constraints due to the obstacles (i.e., dealing with the configuration parameters of the system) and the nonholonomic constraints linking the parameter derivatives It appears necessary to combine geometric techniques addressing the obstacle avoidance together with control theory tech- niques addressing the special structure of the nonholonomic motions Such a combination is possible through topological arguments

The chapter may be considered as self-contained; nevertheless, the basic necessary concepts in differential geometric control theory are more developed

in Bella'iche-Jean-Risler's chapter

Finally, notice that Nonholonomic Motion Planning may be consider as the problem of planning open loop controls; the problem of the feedback control is the purpose of DeLuca-Oriolo-Samson's chapter

The goal of this section is to state precisely what kind of controllability and what level of mobile robot modeling are concerned by motion planning

2.1 Controllabilities

Let us consider a n-dimensional manifold, U a class of functions of time t taking their values in some compact sub-domain/~ of R m The control systems considered in this chapter are differential systems such that

= f ( Z ) u + g(X)

u is the control of the system The i - t h column of the matrix f ( X ) is a vector field denoted by fi g(X) is called the drift An admissible trajectory is a

Guidelines in Nonholonomic Motion Planning for Mobile Robots 3

solution of the differential system with given initial and final conditions and u belonging to L/

The following definitions use Sussmann's terminology [83]

D e f i n i t i o n 1 ~ is locally controllable from X if the set of points reachable from X by an admissible trajectory contains a neighborhood of X It is small- time controllable from X if the set of points reachable from X before a given time T contains a neighborhood o] X for any T

A control system will be said to be small-time controllable if it is small-time controllable from everywhere

Small-time controllability clearly implies local controllability The converse

is false

Checking the controllability properties of a system requires the analysis of the control Lie algebra associated with the system Considering two vector fields ] and g, the Lie bracket [f, g] is defined as being the vector field Of.g - Og f 1

The following theorem (see [82]) gives a powerful result for symmetric systems (i.e.,/C is symmetric with respect to the origin) without drift (i.e, g(X) = 0)

T h e o r e m 2.1 A symmetric system without drift is small-time controllable from X iff the rank of the vector space spanned by the family of vector fields fi together with all their brackets is n at X

Checking the Lie algebra rank condition (LARC) on a control system con- sists in trying to build a basis of the tangent space from a basis (e.g., a P Hall family) of the free Lie algebra spanned by the control vector fields An algo- rithm appears in [46,50]

2.2 M o b i l e robots: from d y n a m i c s to k i n e m a t i c s

Modeling mobile robots with wheels as control systems may be addressed with

a differential geometric point of view by considering only the classical hypoth- esis of "rolling without slipping" Such a modeling provides directly kinematic models of the robots Nevertheless, the complete chain from motion planning

to motion execution requires to consider the ultimate controls that should be applied to the true system With this point of view, the kinematic model should

be derived from the dynamic one Both view points converge to the same mod- eling (e.g., [47]) but the later enlightens on practical issues more clearly than the former

1 The k-th coordinate of If, g] is

ts, gt[kl = (g[il Slkl - Stil glkl)

Let us consider two systems: a two-driving wheel mobile robot and a car (in [17] other mechanical structure of mobile robots are considered)

T w o - d r i v i n g w h e e l m o b i l e r o b o t s A classical locomotion system for mobile robot is constituted by two parallel driving wheels, the acceleration of each being controlled by an independent motor The stability of the platform is insured by castors T h e reference point of the robot is the midpoint of the two wheels; its coordinates, with respect to a fixed frame are denoted by (x, y); the main direction of the vehicle is the direction 0 of the driving wheels With t designating the distance between the driving wheels the dynamic model is:

with lull _< Ut,max, lull _ u~,,~a~ and vl and v2 as the respective wheel speeds

Of course vl and v2 are also bounded; these bounds appear at this level as

"obstacles" to avoid in the 5-dimensional manifold This 5-dimensional system

is not small-time controllable from any point (this is due to the presence of the drift and to the bounds on ul and u2)

By setting v = ½(Vl + v2) and w = ~(Vl - v2) we get the kinematic model which is expressed as the following 3-dimensional system:

(i) ( o0) (i) = si 0 0 v + w (2)

T h e bounds on Vl and v2 induce bounds vmax and Wm,~ on the new controls v and w This system is symmetric without drift; applying the LARC condition shows t h a t it is small-time controllable from everywhere Notice t h a t v and w should be C 1

C a r - l i k e r o b o t s From the driver's point of view, a car has two controls: the accelerator and the steering wheel The reference point with coordinates (x, y)

is the midpoint of the rear wheels We assume that the distance between b o t h rear and front axles is 1 We denote w as the speed of the front wheels of the car and ~ as the angle between the front wheels and the main direction 0 of the car 2 Moreover a mechanical constraint imposes [~t -< ~max and consequently a

2 More precisely, the front wheels are not exactly parallel; we use the average of their angles as the turning angle

Guidelines in Nonholonomic Motion Planning for Mobile Robots 5

minimum turning radius Simple computation shows t h a t the dynamic model

of the car is:

(i) ,,,wcos cos0 , (i)Ill • i w c o s e s i n e / ° +

Let us assume t h a t we do not care about the direction of the front wheels

We m a y still simplify the model By setting v = w cos ~ and w = w sin ~ we get

a 3-dimensionated control system:

(i) ( o 0)0 (i) = sin 0 v + w (5)

By construction v and w are C 1 and their values axe bounded This system looks like the kinematic model of the two-driving wheel mobile robot The main difference lies on the admissible control domains Here the constraints

on v and w are no longer independent Indeed, by setting wmax = vf2 and

¢ , ~ = ~ we get: 0 <_ lwl < Ivl <_ 1 This means t h a t the admissible control domain is no longer convex It remains symmetric; we can still apply the LARC condition to prove t h a t this system is small-time controllable from everywhere The main difference with the two-driving wheel mobile robot is t h a t the feasible paths of the car should have a curvature lesser than 1

A last simplification consists in putting Ivl - 1 and even v _= 1; by ref- erence to the work in [65] and [22] on the shortest paths in the plane with

bounded curvature such systems will be called Reeds&Shepp's car and Dubins' car respectively (see Sou~res-Boissonnat's chapter for an overview of recent results on shortest paths for car-like robots) The admissible control domain of Reeds&Shepp's car is symmetric; LARC condition shows that it is small-time controllable from everywhere 3 Dubins' car is a system with drift; it is locally controllable but not small-time controllable from everywhere; for instance, to

go from (0, 0, 0) to (1 - cos e, sin e, 0) with Dubins car takes at least 27r - c unity

of time

The difference between the small-time local controllability of the car of Reeds & Shepp and the local controllability of Dubins' car may be illustrated geometrically Figure 1 shows the accessibility surfaces in R 2 x S 1 of both sys- tems for a fixed length of the shortest paths Such surfaces have been computed from the synthesis of the shortest paths for these systems (see [76,51,15] and Sou~res-Boissonnat's chapter) In the case of Reeds&Shepp's car, the surface encloses a neighborhood of the origin; in the case of Dubins' car the surface is not connected and it does not enclose any neighborhood of the origin

2.3 K i n e m a t i c m o d e l o f m o b i l e r o b o t s w i t h trailers

Let us now introduce the mobile robot with trailers which has been the canoni- cal example of the work in nonholonomic motion planning; it will be the leading thread of the rest of the presentation

Figure 2 (left) shows a two-driving wheel mobile robot pulling two trailers; each trailer is hooked up at the middle point of the rear wheels of the previous one The distance between the reference points of the trailers is assumed to be

1 The kinematic model is defined by the following control system (see [47]) :

After noticing that [f2, If1, f2]] = fl, one may check that the family com-

posed of {fl, f2, [fl, f2], Ill, [fl, f2]], Ill, Ill, Ill, f2]]]} spans the tangent space

at every point in R 2 x ($1) 3 verifying ~1 ~ ~ (regular points) The family {]1, f2, [fl, f2], [fl, [£, f2]], [£, If1, [•, [fl, f2]]]]) spans the tangent space else- where (i.e., at singular points) Thanks to the LARC, we conclude that the

a A geometric proof appears in [44]

Guidelines in Nonholonomic Motion Planning for Mobile Robots 7

Fig 1 Accessibility domains by shortest paths of fixed length

system is small-time controllable at any point Its degree of nonholonomy 4 is 4

at regular points and 5 at singular points A more general proof of small-time controllability for this system with n trailers appears in [47]

Another hooking system is illustrated in Figure 2 (right) Let us assume

t h a t the distance between the middle point of the wheels of a trailer and the hookup of the preceding one is 1 The control system is the same as (6), with

4 The minimal length of the Lie bracket required to span the tangent space at a point

is said to be the degree of nonholonomy of the system at this point The degree of nonholonomy of the system is the upper bound d of all the degrees of nonholonomy defined locally (see Bellai'che-Jean-Pdsler's Chapter for details)

Y [ '\\\\\ "

\,\ ,, e

Fig 2 Two types of mobile robots with trailers

f l ( X ) = (cosS, sin0, 0, - s i n a i , - sin~o2 cos~l+cos~o2 sin~l+sin~al) T and

f 2 ( X ) = (0, 0, 1, - 1 - cos~l, sin~al sin~o2 + cos~al cos~o2 + cos~al) T

The family { f l , f2, [/1, f2], [fl, [fl, f2]], [f2, [fl, f2]]} spans the tangent space

at every point in R 2 x ($1) 3 verifying ~al ~ ~r, ~a2 ~ r and ~ol ~ ~2 (regular points) The degree of nonholonomy is then 3 at regular points The family

{ f l , f2, If1, ]2], If1, If1, f2]], If1, [fl, If1, ]2]]]} spans the tangent space at points

verifying ~Ol -= ~2 The degree of nonholonomy at these points is then 4 When

~1 - 7r or ~2 = 7r the system is no more controllable; this is a special case of mechanical singularities

2.4 A d m i s s i b l e p a t h s a n d t r a j e c t o r i e s

Constrained paths and trajectories Let C$ be the configuration space of some mobile robot (i.e., the minimal number of parameters locating the whole system in its environment) In the sequel a trajectory is a continuous function

from some real interval [0, T] in C$ An admissible trajectory is a solution of the

differential system corresponding to the kinematic model of the mobile robot (including the control constraints), with some initial and final given conditions

A path is the image of a trajectory in CS An admissible path is the image of

Two-driving wheels

Guidelines in Nonholonomic Motion Planning for Mobile Robots

Car-like

fD

Fig 3 Kinematic mobile robot models: four types of control domains

Remark 1: Due to the constraint [w] < Iv[, the admissible paths for the car-like,

Reeds&Shepp's and Dubins' robots have their curvature upper bounded by 1 everywhere As a converse any curve with curvature upper bounded by 1 is an admissible path (i.e., it is possible to compute an admissible trajectory from it)

Remark 2: This geometric constraint can be taken into account by consid-

ering the four-dihmnsionated control system (4) with I~] < ~" _ ~, the inequality constraint on the controls for the 3-dimensionated system is then transformed into a geometric constraint on the state variable ~ Therefore the original con- trol constraint [w[ < Iv[ arising in system (5) can be addressed by applying

"obstacle" avoidance techniques to the system (4)

F r o m p a t h s t o t r a j e c t o r i e s The goal of nonholonomic motion planning

is to provide collision-free admissible paths in the configuration space of the

mobile robot system Obstacle avoidance imposes a geometric point of view that dominates the various approaches addressing the problem The motion planners compute paths which have to be transformed into trajectories

In almost all applications, a black-box module allows to control directly the linear and angular velocities of the mobile robot Velocities and accelerations are of course submitted bounds

The more the kinematic model of the robot is simplified, the more the trans- formation of the path into a trajectory should be elaborated Let us consider for instance an elementary path consisting of an arc of a circle followed by a tangent straight line segment Due to the discontinuity of the curvature of the path at the tangent point, a two driving-wheel mobile robot should stop at this point; the resulting motion is clearly not satisfactory This critical point may be overcome by "smoothing" the path before computing the trajectory For instance clothoids and involutes of a circle are curves that account for the dynamic model of a two driving-wheel mobile robot: they correspond to bang-

bang controls for the system (1) [35]; they may be used to smooth elementary

paths [25]

Transforming an admissible path into an admissible trajectory is a classical problem which has been investigated in robotics community mainly through the study of manipulators (e.g., [67] for a survey of various approaches) Formal solutions exist (e.g., [75] for an approach using optimal control); they apply

to our problem Nevertheless, their practical programming tread on delicate numerical computations [40]

On the other hand, some approaches address simultaneously the geometric constraints of obstacle avoidance, the kinematic and the dynamic ones; this is the so-called "kinodynamic planning problem" (e.g., [20,21,66]) These methods consist in exploring the phase space (i.e., the tangent bundle associated to the configuration space of the system) by means of graph search and discretiza- tion techniques In general, such algorithms provide approximated solutions (with the exception of one and two dimensional cases [62,19]) and are time- consuming Only few of them report results dealing with obstacle avoidance for nonholonomic mobile robots (e.g., [28])

The following developments deal with nonholonomic path planning

3 P a t h p l a n n i n g a n d s m a l l - t i m e c o n t r o l l a b i l i t y

Path planning raises two problems: the first one addresses the existence of a collision-free admissible path (this is the decision problem) while the second one addresses the computation of such a path (this is the complete problem) The results overviewed in this section show that the decision problem is solved for any small-time controllable system; even if approximated algorithms exist to solve the complete problem, the exact solutions deal only with some special classes of small-time controllable systems

We may illustrate these statements with the mobile robot examples intro- duced in the previous section:

- Dubins' robot: this is the simplest example of a system which is locally controllable and not small-time controllable For this system, the decision problem is solved when the robot is reduced to a point [27] An approx- imated solution of the complete problem exists [34]; exact solutions exist for a special class of environments consisting of moderated obstacles (mod- erated obstacles are generalized polygons whose boundaries are admissible paths for Dubins' robot) [2,13] Notice that the decision problem is still open when the robot is a polygon

Guidelines in Nonholonomic Motion Planning for Mobile Robots 11

- Reeds&Shepp's, car-like and two-driving wheel robots: these systems are small-time controllable We will see below that exact solutions exist for both problems

- Mobile robots with trailers: the two systems considered in the previous section are generic of the class of small-time controllable systems For both

of them the decision problem is solved For the system appearing in Figure 2 (left) we will see that the complete problem is solved; it remains open for the system in Figure 2 (right)

Small-time controllability (Definition 1) has been introduced with a control theory perspective To make this definition operational for path planning, we should translate it in purely geometric terms

Let us consider a small-time controllable system, with H a class of control functions taking their values in some compact domain ]C of R m We assume that the system is symmetric 5 As a consequence, for any admissible path between two configurations X1 and X2, there are two types of admissible trajectories: the first ones go from X1 to X2, the second ones go from X2 to XI

Let X be some given configuration For a fixed time T, let T~eachx (T) be the set of configurations reachable from X by an admissible trajectory before the time T K: being compact, Tteachx(T) tends to {X} when T tends to 0 Because the system is small-time controllable, Tieachx (T) contains a neigh- borhood of X We assume that the configuration space is equipped with a (Riemannian) metric: any neighborhood of a point contains a ball centered at this point with a strictly positive radius Then there exists a positive real num- ber r/such that the ball B(X, rl) centered at X with radius ~/is included in

7~eachx(T)

Now, let us consider a (not necessarily admissible) collision-free path 7 with finite length linking two configurations Xstart and Xgoal 7 being compact, it is possible to define the clearance e of the path as the minimum distance of 7 to the obstacles 6, e is strictly positive Then for any X on 7, there exists Tx > 0

such that ~eachx(Tx) does not intersect any obstacle Let ~/Tx be the radius

of the ball centered at X whose points are all reachable from X by admissible trajectories that do not escape T~eachx (Tx) The set of all the balls B(X, rlTx),

X E 7, constitutes a covering of % 7 being compact, it is possible to get a finite sequence of configurations (Xi)l<i<k (with X1 = Xsta~t, Xk = X~oal), such that the balls B(XI, rlTx, ) cover 7-

Consider a point Yi,i+l lying on 7 and in B(Xi,rlTx, )fl B(Xi+l, ~Txi+l )

Between Xi and Y/,i+l (respectively Xi+a and Y/,i+I) there is an admissible

5 Notice that, with the exception of Dubins' robot, all the mobile robots introduced

in the previous section are symmetric

We consider that a configuration where the robot touches an obstacle is not collision-free

trajectory (and then an admissible path) that does not escape ~eachx~ (Tx~) (respectively 7~eachx~+l (Tx,+I)) Then there is an admissible path between Xi and Xi+l that does not escape Tteachx, (Tx~)U ~eachx~+~ (Tx,+~); this path is then collision-free The sequence (Xi)l_<i<k is finite and we can conclude that there exists a collision-free admissible path between Xstart and Xgoat

T h e o r e m 3.1 For symmetric small-time controllable systems the existence o]

an admissible collision-free path between two given configurations is equivalent

to the existence of any collision-free path between these configurations

Remark 3: We have tried to reduce the hypothesis required by the proof to a minimum They are realistic for practical applications For instance the com- pactness of E holds for all the mobile robots considered in this presentation Moreover we assume that we are looking for admissible paths without contact with the obstacles: this hypothesis is realistic in mobile robotics (it does not hold any more for manipulation problems) On the other hand we suggest that two configurations belonging to the same connected component of the collision- free path can be linked by a finite length path; this hypothesis does not hold for any space (e.g., think to space with a fractal structure); nevertheless it holds for realistic workspaces where the obstacles are compact, where their shape is simple (e.g., semi-algebraic) and where their number is finite

Consequence 1: Theorem 3.1 shows that the decision problem of motion plan- ning for a symmetric small-time controllable nonholonomic system is the same

as the decision problem for the holonomic associated one (i.e., when the kine- matics constraints are ignored): it is decidable Notice that deciding whether some general symmetric system is small-time controllable (from everywhere) can be done by a only semi-decidable procedure [50] The combinatorial com- plexity of the problem is addressed in [77] Explicit bounds of complexity have been recently provided for polynomial systems in the plane (see [68] and refer- ences therein)

Consequence 2: Theorem 3.1 suggests an approach to solve the complete prob- lem First, one may plan a collision-free path (by means of any standard meth- ods applying to the classical piano mover problem); then, one approximates this first path by a finite sequence of admissible and collision-free ones This idea is at the origin of a nonholonomic path planner which is presented below (Section 5.3) It requires effective procedures to steer a nonholonomic system from a configuration to another The problem has been first attacked by ignor- ing the presence of obstacles (Section 4); numerous methods have been mainly developed within the control theory community; most of them account only for local controllability Nevertheless, the planning scheme suggested by Theo- rem 3.1 requires steering methods that accounts for small-time controllability

Guidelines in Nonholonomic Motion Planning for Mobile Robots 13

(i.e., not only for local controllability) In Section 5.1 we introduce a topological property which is required by steering methods in order to apply the planning scheme We show that some among those presented in Section 4 verify this property, another one does not, and finally a third one may be extended to guaranty the property

4 Steering methods

What we call a steering method is an algorithm that solves the path planning problem without taking into account the geometric constraints on the state Even in the absence of obstacles, computing an admissible path between two configurations of a nonholonomic system is not an easy task Today there is

no algorithm that guarantees any nonholonomic system to reach an accessible goal exactly In this section we present the main approaches which have been applied to mobile robotics

4.1 F r o m v e c t o r fields to effective paths

The concepts from differential geometry that we want to introduce here are thoroughly studied in [79,90,80,81] and in Bella'iche-Jean-Risler's Chapter They give a combinatorial and geometric point of view of the path planning problem

Choose a point X on a manifold and a vector field f defined around this point There is exactly one path 7(~-) starting at this point and following f That is, it satisfies 7(0) = X and ~/(T) = f(7(Z)) One defines the exponential

of f at point X to be the point 7(1) denoted by e f X Therefore e f appears

as an operation on the manifold, meaning "slide from the given point along the vector field f for unit time" This is a diffeomorphism With a being a real number, applying eaf amounts to follow f for a time a In the same way, applying J + g is equivalent to follow f + g for unit time

It remains that, whenever [f, g] ~ 0, following directly a f + fig or following first a f , then fig, are no longer equivalent Intuitively, the bracket If, g] mea- sures the variation of g along the paths of f; in some sense, the vector field g

we follow in a f + fig is not the same as the vector field g we follow after having followed a f first (indeed g is not evaluated at the same points in both cases) Assume that f l , , fn are vector fields defined in a neighborhood Af of

a point X such that at each point of A/', { f l , ,fn} constitutes a basis of the tangent space Then there is a smaller neighborhood of X on which the maps ( a l , , an) ~-~ e alfl'k'''-t'°~"'f" • X and ( a l , , a n ) ~-> e c~'J'' " e c~Ifl • X

are two coordinate systems, called the first and the second normal coordinate system associated to ( f l , , f , }

The Campbell-Baker-Hausdorff-Dynkin formula states precisely the differ- ence between the two systems: for a sufficiently small T, one has:

eVf erg _~ evf÷rg - lr2[f,g]'~r2e(T)

where e(r) + 0 when 7 -+ 0

Actually, the whole formula as proved in [90] gives an explicit form for the

e function More precisely, e yields a formal series whose coefficients ch of ~.k are combinations of brackets of degree k, 7 i.e

o o

k-~.3

Roughly speaking, the Campbell-Baker-Hansdorff-Dynkin formula tells us

how a small-time nonholonomic system can reach any point in a neighborhood

of a starting point This formula is the hard core of the local controllability

concept It yields methods for explicitly computing admissible paths in a neigh-

borhood of a point

4.2 Nilpotent systems and nilpotentization

One method among the very first ones has been defined by Lafferiere and Sussmann [39] in the context of nilpotent system A control system is nilpotent

as soon as the Lie brackets of the control vector fields vanish from some given length

For small-time controllable nilpotent systems it is possible to compute a

basis Y of the Control Lie Algebra L A ( A ) from a Philipp Hall family (see

for instance [46]) The method assumes that a holonomic path 7 is given If

we express locally this path on B, i.e., if we write the tangent vector ~/(t)

as a linear combination of vectors in B(7(t)), the resulting coefficients define

a control t h a t steers the holonomic system along 7 Because the system is

nilpotent, each exponential of Lie bracket can be developed exactly as a finite

combination of the control vector fields: such an operation can be done by using the Campbell-Baker-Hausdorff-Dynkin formula above An introduction

to this machinery through the example of a car-like robot appears in [48] It

is then possible to compute an admissible and piecewise constant control u for the nonholonomic system that steers the system exactly to the goal

For a general system, Lafferiere and Sussmann reason as if the system were nilpotent of order k In this case, the synthesized path deviates from the goal Nevertheless, thanks to a topological property, the basic method may be used

T As an example the degree of [[f, g], If, [g, If, g]l]] is 6

Guidelines in Nonhotonomic Motion Planning for Mobile Robots 15

in an iterated algorithm that produces a path ending as close to the goal as wanted

In [33], Jacob gives an account of Lafferiere and Sussmann's strategy by using another coordinate system This system is built from a Lyndon basis of the free Lie algebra [93] instead of a P Hall basis This choice reduces the number of pieces of the solution

In [11], Bella'/che et al apply the nilpotentization techniques developed in

[10] (see also [31]) They show how to transform any controllable system into a canonical form corresponding to a nilpotent system approximating the original one Its special triangular form allows to apply sinusoidal inputs (see below) to steer the system locally Moreover, it is possible to derive from the proposed canonical form an estimation of the metrics induced by the shortest feasible paths This estimation holds at regular points (as in [92]) as welt as at singular points These results are critical to evaluate the combinatorial complexity of the approximation of holonomic paths by a sequence of admissible ones (see Section 5.7)

The mobile robots considered in this presentation are not nilpotent s A uilpotentization of this system appear in [39] We conclude this section by the nilpotentization of a mobile robot pulling a trailer [11]

Example: Let us consider the control system 6:

cos ~ /

We check easily that {fl, f2, f3, f4} is a basis of the tangent space at every point

of the manifold R 2 x ($1) 2 Let Xo = (xo,yo,O0,9o) and X = (x,y,O,~o) be

s Consider the system (2); let us denote fl and f~ the two vector fields corresponding

to a straight line motion and a rotation respectively By setting adI(g ) -. [f, g], we

check that a d ~ ( f l ) = (-1)'~fl # 0

two points of the manifold By writing Ax = x - Xo, Ay = y - Yo, A0 = 0 - 8o and Aqv = ~v - ~ o , the coordinates (YI,Y2,Y3,Y4) of X in the chart a t t a c h e d to

Xo with the basis { f l , f2, f3, f4}(Xo) are:

Yl = cos 0oAx + sin OoAy

Y2 - AO

Y3 = sin~o Ax - cos~oAy

Y4 = sin(~vo - 00)Ax + cos(~oo - ~o)Ay - A0 + A~v

T h e goal of the following c o m p u t a t i o n s is to provide a new coordinate sys-

t e m (zl,z2,z3,z4) at Xo such that:

- there exists i and j such t h a t ((fi.fj)z3)(Xo) ~ O,

- for any i and j, ((£.fj)z4)(Xo) = O, and

- there exists i, j and k such t h a t ((fh.fi.fj)z4)(Xo) ~ 0

with h , i , j e {1,2} a n d k e {1,2,3,4}; 5~ = 1 iff i = k; ( f ) designates the differential o p e r a t o r associated to the vector field f ; (f.g) is the p r o d u c t of the corresponding differential operators Such coordinates axe called privileged coordinates

One m a y check t h a t ((]~)yk)(Xo) = ~ik for i e {1,2} and k e { 1 , 2 , 3 , 4 } Moreover ((fl)2y3)(Xo) : ((f2)2y3)(Xo) 0 a n d ((f2.£)y3)(Xo) = 1 Now, it

a p p e a r s t h a t ((fl)2y4)(Xo) = sin ~Vo cos ~o0; t h e n (Yl, Y:, Y3, Y4) is not a privi- leged coordinate system if sin ~o cos ~o ~ 0

One gets privileged coordinates by keeping

z l = y l , z 2 = y ~ , z3=ya

a n d taking

1 z4 = Y4 - ~ sin ~o cos ~oy 2

In such coordinates, we have

where

( C°o 2 )

f l = | - sinz2 f2 =

\ F(zl, z3, z4)

F(zl, z2, z3, z4) = - z l (cos z2 sin 2~Vo)/2 + sin(~vo + z2)

sin(~o - zl sin~o + z12(sin 2~vo)/4 + z2 + z3 cos ~oo + z4)

Guidelines in Nonholonomic Motion Planning for Mobile Robots 17

The nilpotent approximation is obtained by taking in the Taylor expansions

of (7) the terms of homogeneous degree wi - 1 for the i-th coordinate where wi

is the degree of the vector field fi (i.e., W 1 = W 2 = 1,w3 = 2, w4 = 3) We get

where

L=

Z 2

l~(Zl, Z2, Z3)

F ( z 1 , z 2 , z 3 ) ~ -z~(sin ~0 cos 2~0)/2 - z l z 2 sin 2 ~o0 - z3 cos 2 ~o0

It is easy to check that this new system is nilpotent of order 3

At the same time as Lafferiere and Sussmann work, Murray and Sastry explored

in [58,59] the use of sinusoidal inputs to steer certain nonholonomic systems: the class of systems which can be converted into a chained form A chained system has the following form:

Because of this special form, there exists simple sinusoidal control that may

be used for generating motions affecting the ith set of coordinates while leaving the previous sets of coordinates unchanged The algorithm then is:

1 Steer Xl to the desired value using any input and ignoring the evolutions

an exact steering method for chained form systems [57]

Even if a system is not triangular, it may be possible to transform it into

a triangular one by feedback transformations (see [59,60]) Moreover, notice that the nilpotentization techniques introduced in the previous section leads to approximated systems which are in chained form

E x a m p l e : Let us consider our canonical example of a mobile robot with two trailers (Figure 2, left) The clever idea which enables the transformation of the system into a chained form was to consider a frame attached to the last trailer rather than attached to the robot [86] Denoting by 01 and 02 the angle

of the trailers, and by x2 and Y2 the coordinates of the middle point of the last trailer, the system (6) may be re-written as:

Guidelines in Nonholonomic Motion Planning for Mobile Robots 19

Let Z start be a starting configuration Equations (8) are integrable Then each z i ( T ) can be computed from the five coordinates of Z start and the six parameters (ao,al,bo,b:,b2,b3) For a given al ¢ 0 and a given configu- ration Z ~t~rt, Tilbury et al show that the function computing Z ( T ) from (ao, bo, b:, b2, b3) is a C 1 diffeomorphism at the origin; then the system is invert- ible and the parameters (ao, b0, bl, b2, b3) can be computed from the coordinates

of two configurations Z st~r~ and Z g°al The system inversion can be done with the help of any symbolic computation software The corresponding sinusoidal inputs steer the system from Z ~ta~t to Z 9°at

The shape of the path only depends on the parameter a: Figure 4 from [71] illustrates this dependence Moreover the shape of the paths is not invariant

by rotation (i.e., it depends on the variables 0 star~ and 0 g°at and not only on the difference (0 start - Og°al) )

Fig 4 Three paths solving the same problem with three values of a:: -30, 70, 110

Polynomial inputs: Another steering method is also proposed in [86] The poly- nomial inputs:

Vl(t) = I

V2(t) CO -b Cl t -{- C2 t2 -t- C3 t3 q- C4 t4

goal

steer the system from any configuration Z 8tart to any Z g°al verifying z 1

z l tart In this case T should be equal to ]z~ °al - zltart t As for the case of the sinusoidal inputs, the system can be inverted by symbolic computation

To reach configurations such that ~:'g°at = z~t~rt it is sufficient to choose an intermediate configuration respecting the inequality and to apply the steering method twice

Extensions: The previous steering methods deal with two-input chained form systems In [16] Bushnell, Tilbury and Sastry extend these results to three-input nonholonomic systems with the fire-truck system as a canonical example 9 They give sufficient conditions to convert such a system to two-chain, single generator chained forms Then they show that multirate digital controls, sinusoidal inputs and polynomial inputs may be used as steering methods

of their derivatives The variables yi's are called the linearizing outputs of the system

Example: In [63] Rouchon et al show t h a t mobile robots with trailers are flat

as soon as the trailers are hitched to the middle point of the wheels of the previous ones The proof is based on the same idea allowing to transform the system into a chained form: it consists in modeling the system by starting from the last trailer

Let us consider the system (6) (Figure 2, left) Let us denote the coordinates

of the robot and the two trailers by (x, y, O), (xl, Yl, 81) and (x2, Y2, 82) respec- tively Remind that the distance between the reference points of the bodies is

1 The holonomic equations allow to compute x, y, Xl and Yl from x2, Y2, 81

and 82:

Xl ~ x2 ~ c0882 x = x 2 -~- cos82 -[- cosSl

Yl = Y2 -k sin 82 Y : Y2 q- sin 82 + sin 81 The rolling without slipping conditions lead to three nonholonomic equations xisin0i -~)icosSi = 0 allowing to compute 62 (resp 81 and 8) from (i2,~/2) (resp (~h,yh) and (J/2,~/2)) Finally the controls v and w are given by v = cos0

Guidelines in Nonholonomic Motion Planning for Mobile Robots 21

A steering method: Let us consider a path ~/2 followed by the reference point

P2 of the second trailer (Figure 5) ~/2 is parametrized by arc length s2 Let

d

us assume that 72 is sufficiently smooth, i.e., d-~P2 is defined everywhere and the curvature t~2 can be differentiated at least once The point P1 belongs to the tangent to ~/2 at P2 and P1 = P2 + ~'2, with v2 the unitary tangent vector

d

to 72 Differentiating this relation w.r.t, s2 leads to d ~2Pl = r2 + a2v2 with

~'2 the unitary vector orthogonal to 1"2 The angle of the first trailer is then

O~ = 82 + atan(a2) We then deduce the path 71 followed by the first trailer Parametrizing ~ with s~ defined by ds~ = (1 + g2)½ds2 leads to

as2

Applying the same geometric construction from P1 we can compute the path

7 followed by the robot when the second trailer follows ~'2- The only required

d 2 condition is the existence of ~ a 2 , moreover the relative angles ~Pl and ~2 should belong to ] - ~, ~[ (see [26] for details)

Two configurations X s~art and X g°al being given, one computes geomet-

goat

rically the values of ~start ~s~a~tl , ~ t a ~ t ~aoa~, '~l~g°at and ~2 ; each of them being a function of a2 and its derivative, it is straightforward to compute 72 satisfying such initial and final conditions (e.g., by using polynomial curves)

'.: ,~ :~ :~(

Fig 5 Geometric construction of P1 (resp P) path from P2 (resp P1) path

Remark: Because the curvature of "?2 should be defined everywhere, the method

can not provide any cusp point; nevertheless such points are required in some

situations like the parking task; in that case, Rouchon et al enter the cusp point

by hand [63] We will see below how to overcome this difficulty

Flatness conditions: In cite [64], Rouchon gives conditions to check whether

a system is flat Among them there is a necessary and sufficient condition for

two-input driftless systems: it regards the rank of the various vector space

A k iteratively defined by glo = span{f1, f2}, A1 = span{f1, f2, [fl, f2]} and

A/+I = Ao + [Ai, Ai] with [Ai, Ai] = span{[f,g] , f E A~, g E Ai} A system

with two inputs is flat iff rank(Ai) = 2 + i

Let us apply this condition to the mobile robot system with two trailers According to the computations presented in Section 2.3:

- for the case shown in Figure 2 (left), we get:

rank(Ao) = 2, rank(A1) = 3, rank(A2) = 4 and rank(A3) = 5

the system is fiat

- for the case shown in Figure 2 (right), we get:

rank(Ao) = 2, rank(Z~l) = 3 and rank(A2) = 5

the system is not flat

- for the same case shown in Figure 2 (right) but with only one trailer, one can check that:

rank(Ao) = 2, rank(A1) = 3 and rank(A2) = 4

the system is flat

We have seen that the linearizing outputs in the first case are the coordinates

of the reference point of the second trailer In the last case, the linearizing outputs are more difficult to translate into geometric terms (see [63]) Notice that there is no general method to compute the tinearizing outputs when the system is flat

4.5 S t e e r i n g w i t h o p t i m a l c o n t r o l

Optimal length paths have been at the origin of the very first nonholonomic motion planners for car-like mobile robots (see for instance [48,43] and below) Nevertheless, today the only existing results allowing to compute optimal paths for nonholonomic systems have been obtained for the car-like systems (see Sou~res-Boissonnat's Chapter) For general systems, the only possibility is to call on numerical methods

We sketch here the method developed by Fernandez, Li and Gurvitz in [24] Let us consider a dynamical system: J( = B ( X ) u together with a cost

function J = f J < u(r), u(~-) > dT Both starting and goal configurations

being given, the optimization problem is to find the control law (if any) that steers the system from the starting configuration to the goal in time T by

Guidelines in Nonholonomic Motion Planning for Mobile Robots 23

minimizing the cost function J The p a t h corresponding to an optimal control law is said to be an optimal path

Let us consider a continuous and piecewise C 1 control law u defined over [0, T] We denote by fi the periodic extension of u over R We may write fi in terms of a Fourier basis:

e k e {e r , p e Z} T h e choice of the reals ak being given, the point X(T)

reached after a time T with the control law fi appears as a function f ( a ) from

Because we do not know f and 6f/Sa explicitly we use numerical methods (numerical integration of the differential equations and numerical optimization like Newton's algorithm) to compute a solution of the problem Such a solution

is said to be a near-optimal solution of the original problem

Figure 6 from [71] shows three examples of near-optimal paths computed from this m e t h o d for a mobile robot with two trailers [49]

5 Nonholonomic path planning for small-time

controllable s y s t e m s

Consider the following steering m e t h o d for a two-driving wheel mobile robot

To go from the origin (0,0,0) to some configuration ( x , y , 8) the robot first

10 This approximation restricts the family of the admissible control laws

Fig 6 Three examples of near-optimal paths

executes a pure rotation to the configuration (0, 0, atan~), then it moves along

a straight line segment to (x,y, atan~), and a final rotation steers it to the goal This simple method accounts for local controllability: any point in any neighborhood of the origin can be reached by this sequence of three elementary paths (when x = 0, replace atan~ by ~ ) Nevertheless such a method does not account for small-time controllability If the space is very constraint it does not hold Think to the parking task (Figure 17): the allowed mobile robot orientations 0 vary in some interval ] - ~/, 7[ To go from (0, 0, 0) to (0, e, 0) the steering method violates necessarily this constraint

Therefore, obstacle avoidance requires steering methods accounting for small-time controllability Such a requirement can be translated into geometric terms

5.1 Toward steering m e t h o d s accounting for small-time

Let ~' be the set of feasible paths defined over an interval of the type [0, T]

A steering function is a mapping from g S x g S into :P:

(X 1 , X 2) -+ Steer(X 1 , X 2 ) where Steer(X 1, X 2) is defined over the interval [0, T], such that

Steer(X 1 , X 2 ) (0) = X 1 , Steer(X 1 , X 2 ) (T) = X 2

Guidelines in Nonholonomic Motion Planning for Mobile Robots 25

D e f i n i t i o n 2 Steer verifies the weak topological property iff:

Ve > 0,VX 1 E CS, 3/] > 0,VX 2 E CS, (10)

d c s ( X 1 , X 2) < ~7 ~ V t e [0,T], dcs(Steer(X1,X2)(t),X 1) < e

By using a steering method that verifies the weak topological property,

it is possible to approximate any collision-free path 7Iree Nevertheless, this property is not sufficient from a computational point of view Indeed, it is local:

the real number ~/depends on X 1 Situations as shown in Figure 7 may appear: let us consider a sequence of configurations X i converging to the critical point

X c, and such that limx~-~x¢ ~I(X i) = 0; to be collision-free any admissible path should necessary goes through the configuration X c The computation of

X c may set numerical problems To overcome this difficulty, we introduce a stronger property for the steering methods

X~

B(X, cE)

Fig 7 Weak topological property

D e f i n i t i o n 3 Steer verifies the topological property iff:

Ve > 0,3~/> 0 , V ( X I , X 2) E (C8) 2, (11)

d c s ( X 1 , X 2) < T/ ~ Vt E [0,T], dcs(Steer(X1,X2)(t),X 1) < e

In this definition ~/does not depend on any configuration (Figure 8) This

is a global property that not only accounts for small-time controllability but also holds uniformly everywhere

X goal

X start

% /

t

X \

Fig 8 Topological property

erty is not an easy task The following sufficient condition appears in [71,74] Let us equip 7 ~ with a metric d~, between paths:/"1 and F2 being two paths on [0, 1], we define 47, (/"1, F2) = max~e[0j] dcs (1"1 (t), F2 (t))

Let us consider a steering method Steer continuous w.r.t, to the topology induced by dT, Steer is uniformly continuous on any compact set ]C included

in CS 2, i.e.,

Vc> 0, 3r/> 0, V { ( X 1,X2),(Y1,Y2)} eK:

Guidelines in Nonholonomic Motion Planning for Mobile Robots 27

Remark 2: The first general result taking into account the necessary uniform convergence of steering methods is due to Sussmann and Liu [84]: the authors propose an algorithm providing a sequence of feasible paths that uni]ormty con- verge to any given path This guarantees that one can choose a feasible path

arbitrarily close to a given collision-free path The method uses high frequency sinusoidal inputs Though this approach is general, it is quite hard to imple- ment in practice In [87], Tilbury et al exploit the idea for a mobile robot with two trailers Nevertheless, experimental results show that the approach cannot

be applied in practice, mainly because the paths are highly oscillatory There- fore this method has never been connected to a geometric planner in order to get a global planner which would take into account both environmental and kinematic constraints

5.2 Steering methods and topological property

In this section we review the steering methods of Section 4 with respect to the topological property

Optimal p a t h s Let us denote by Steeropt the steering method using optimal control Steeropt naturally verifies the weak topological property Indeed the cost of the optimal paths induce a special metric in the configuration space; such a metric is said to be a nonholonomic [92], or singular [14], or Carnot- Caratheory [56], or sub-Riemannian [80] metric By definition any optimal path with cost r does not escape the nonholonomic ball centered at the starting point with radius r A general result states that the nonholonomic metrics induce the same topology as the "natural" metrics des This means that for any nonholonomic ball Bah(X, r) with radius r, there are two real numbers e and ~ strictly positive such that B(X,y) C Bnh(X,r) C B ( X , @

The nonholonomic distance being continuous, to get the topological prop- erty, it suffices to restrict the application of Steeropt to a compact domain of the configuration space 11

There is no general result that gives the exact shape of the nonholonomic balls; nevertheless the approximated shape of these balls is well understood (e.g., see Bella'iche-Jean-PSsler's chapter)

The metric induced by the length of the shortest paths for Reeds&Shepp's car is close to a nonholonomic metric; car-like robots are the only known cases where it is possible to compute the exact shape of the balls (see Figure 1)

11 Notice that the steering method Steeropt is not necessarily continuous w.r.t, the topology induced by d~,

Sinusoidal inputs and chained form systems Let us consider the two-

input chained form system (8) together with the sinusoidal inputs (9) presented

in Section 4.3 We have seen that the shape of the paths depends on al (Fig- ure 4) The only constraint on the choice of al is that it should be different from zero

The steering method using such inputs is denoted by Steersi n For a fixed a~

a l value of al, Steersi n does not verify the topological property Indeed, for any

a l configuration Z, the path Steersin(Z, Z) is not reduced to a point 12

Therefore, the only way to build a steering method based on sinusoidal inputs and verifying the topological property is not to keep al constant One has to prove the existence of a continuous expression of al (Z 1, Z 2) such that:

lim at(Z I , Z 2 ) = 0 lim ao(Z 1, Z 2 , a l ( Z 1, Z2)) = 0

Z 2 ~ Z 1

lim bi(Zl,Z2,al(Z1,Z2)) = 0 Z2~Z 1

The proof appears in [73] It first states that, for a fixed value of al,

Steers~ n is continuous w.r.t, to the topology induced by dp This implies that when the final configuration Z 2 tends to the initial configuration Z 1, the path Steera~n(Z 1, Z 2) tends to the path Steera~n(Z I, Z1) Moreover, for any

a l

Z, Steersin(Z, Z) tends to {Z} when al tends to zero Combining these two

of Steers~ n to a compact domain/C statements and restricting the application ~1

of CS 2, one may conclude that:

Ve > 0 , 3A1 > 0 Val < A1, 3y(al), V ( Z I , Z 2) E Y~,

d c s ( Z l , Z 2) < ~?(al), ~ V t e [0,1], d c s ( Z l , S t e e r ~ n ( Z l , Z 2 ) ( t ) ) < e

Then, by tuning al, it is a priori possible to design a steering method Steersi,~ based on sinusoidal inputs and verifying the topological property It remains to define a constructive way to tune the parameter al The problem is not easy Indeed the general expression of parameters ao and bi are unknown Then we do not dispose of a unique expression of Steersin Nevertheless, it is possible to "simulate" a steering method verifying the topological property, by

Steersi n according to the distance between the start switching between different al

and goal configurations The principle of the construction presented in Annex consists in introducing the possibility to iteratively compute subgoals and then

a sequence of subpaths that reaches the final goal without escaping a bounded domain

12 The first coordinate of points lying on the path is zl(t) = zx + ~-(1 -coswt)

Guidelines in Nonholonomic Motion Planning for Mobile Robots 29

A flatness b a s e d steering m e t h o d for m o b i l e r o b o t s w i t h trailers We

have seen in Section 4.4 t h a t a mobile robot with two trailers (with centered hooking up system) is flat with the coordinates (x2, Y2) of the second trailer reference point P2 as linearizing outputs Planning an admissible path for the system then consists in finding a sufficiently smooth planar curve 72(s) for P2- All the coordinates (x, y, 0, ~1, ~2) of a configuration can be geometrically

d ~ deduced from (x2,y2,02, ~2, ~ 2) Nevertheless this steering method cannot verify the topological properties Indeed, due to the conditions on ~/2 (absence of cusp points), going from a configuration (x2, Y2,82, , ) to some configuration (x2,y2 + e,02, , • ) should necessarily contain a configuration ( , ,82 4-

7r

~ , , )

[74] takes advantage of the flatness property of a mobile robot with one trailer to design a steering method verifying the topological property [41] gen- eralizes the method to a system with n trailers Let us sketch the method for

a mobile robot with two trailers

Let us consider a configuration X = (x, y, O, ~Ol, ~2) of the system If F is an admissible p a t h in the configuration space, we will denote by ~/2 the curve in R 2 followed by P2 Among all the admissible paths containing a configuration X ,

d there exists exactly one path F such that ~7~2a2 remains constant everywhere: the corresponding curve 72 is a clothoid 13

Let X s~ar~ and X g°at be the initial and goal configurations respectively Let 72,s~r~ and 72,~oal be the associated clothoids defined on [0,1] and such

t h a t Fstart(O ) -~- X start and Fgoal(1) = X g°al Then any combination 7(t) =

a(t)72,s~r~(t) ÷ (1 - a(t))72,goal(t ) is a C 3 path; it then corresponds to an admissible path for the whole system To make this path starting at X start

and ending at X 9°~1, a should verify: a(0) = &(0) = &(0) = ~ (0) = &(1) = 5(1) = K (1) = 0 and a(1) = 1 (indeed, the three first derivatives of 7 should

be the same as those of 72,~t~rt at 0 and the same as those of 72,go~l at 1)

At this level we get a steering method (denoted by Steer~la~ ) t h a t allows the mobile robot with its two trailers to reach any configuration from any other one Nevertheless, this method does not verify the required topological property: indeed it cannot generate cusps and the steering method we want to provide should be able to do it when it is necessary

Let X s~ar~ be an initial configuration and 72,sta~, the corresponding elothoid in R 2 In [41] we prove that, for any e > 0, if we choose a configuration

X within a "cone" Cs~ar~,~ centered around Fs~ar~ with vertex X st~r~ (see Figure 9), then the path Steer~la~(X st~r~, q) does not escape the ball B ( X star~, e)

Moreover, if X s~ar~ moves within a small open set, the clothoid ^/2,s~a~ is submitted to a continuous deformation: for instance a change on the coordinates

13 If we consider only one trailer, we impose m to remain constant; in this case the trailer follows a circle with radius Lcota.nTh