The CyCa b: a car-like robot navigating...

Robotics and Autonomous Systems 50 (2005) 51–67

TheCyCab: a car-like robot navigating autonomouslyand safely among pedestrians

Cedric Pradalier, Jorge Hermosillo, Carla Koike, Christophe Braillon,Pierre Bessiere, Christian Laugier∗

GRAVIR-INRIA-INPG Grenoble, INRIA Rhˆone-Alpes, 655, avenue de l’Europe-Montbonnot, 38334 Saint Ismier Cedex, France

Received 1 November 2003; received in revised form 11 October 2004; accepted 12 October 2004Available online 25 December 2004

Abstract

The recent development of a new kind of public transportation system relies on a particular double-steering kinematic structureenhancing manoeuvrability in cluttered environments such as downtown areas. We callbi-steerable cara vehicle showingthis kind of kinematics. Endowed with autonomy capacities, the bi-steerable car ought to combine suitably and safely a setof abilities: simultaneous localisation and environment modelling, motion planning and motion execution amidst moderatelydynamic obstacles. In this paper we address the integration of these four essential autonomy abilities into a single application.Specifically, we aim at reactive execution of planned motion. We address the fusion of controls issued from the control law andthe obstacle avoidance module using probabilistic techniques.©

K

1

s

f

jccpc

e of

au-ernam-RIA(seeofsit

omof

0

2004 Elsevier B.V. All rights reserved.

eywords:Car-like robot; Navigation; Path planning; Obstacle avoidance; Autonomous navigation

. Introduction

The development of new intelligent transportationystems (ITS), more practical, safe and accounting for

∗ Corresponding author. Tel.: +33 4 76 61 52 22;ax: +33 4 76 61 52 10.

E-mail addresses:[email protected] (C. Pradalier),[email protected] (J. Hermosillo),[email protected] (C. Koike),[email protected] (C. Braillon),[email protected] (P. Bessiere),[email protected] (C. Laugier).

environmental concerns, is a technological issuhighly urbanised societies today[18]. One of the longrun objectives is to reduce the use of the privatetomobile in downtown areas, by offering new modand convenient public transportation systems. Exples of these are the CyCab robot—designed at INand currently traded by the Robosoft companyhttp://www.robosoft.fr/)—and the pi-Car prototypeIEF (Institut d’Electronique Fondamentale, UnivereParis-Sud).

The kinematic structure of these robots differs frthat of a car-like vehicle in that it allows the steering

921-8890/$ – see front matter © 2004 Elsevier B.V. All rights reserved.doi:10.1016/j.robot.2004.10.002

http://www.robosoft.fr/

52 C. Pradalier et al. / Robotics and Autonomous Systems 50 (2005) 51–67

both the front axle and the rear one. We call a vehicleshowing this feature a bi-steerable car (or BiS-car forshort).

Endowed with autonomy capacities, the bi-steerablecar ought to combine suitably and safely a set of abili-ties that eventually could come to the relief of the end-user in complex tasks (e.g. parking the vehicle). Partof these abilities have been tackled separately in previ-ous work: simultaneous localisation and environmentmodelling, motion planning execution amidst static ob-stacles and obstacle avoidance in a moderately dynamicenvironment without accounting for a planned motion.

In this paper we address the integration of these fouressential autonomy abilities into a single application.Specifically, we aim at reactive execution of plannedmotion. We address the fusion of controls issued fromthe control law and the obstacle avoidance moduleusing probabilistic techniques. We are convinced thatthese results represent a step further towards the mo-tion autonomy of this kind of transportation system.The structure of the paper is as follows.

In Section2, we sketch the environment reconstruc-tion and localisation methods we used and we recallhow the central issue regarding the motion planning andexecution problem for the general BiS-car was solved.Section3 explains how our obstacle avoidance sys-tem was designed and Section4 explains how it wasadapted to the trajectory tracking system. In Section5we present the experimental settings showing the fu-sion of these essential autonomy capacities in our bi-s aperw turew

2p

wea nvi-r e off

2

bleo nti t

10,000 m2). For localisation purposes, we did not wantto focus on the detection of natural features in the envi-ronment, since such detection is often subject to failureand not very accurate. So, in order to ensure reliability,we decided to install artificial landmarks in the envi-ronment. These landmarks had to be detected easilyand accurately, and they should be identified with areasonable computation effort.Figs. 1 and 2show ourrobot, its sensor and the landmarks: cylinder coveredwith reflector sheets, specially designed for our Sicklaser range finder.

Moreover, in order to keep flexibility, we wanted tobe able to equip the environment with non-permanentbeacons. For this reason, we could not rely on a defini-tive landmark map, and we had to build a system ableto learn the current state of the car-park area. This ledus to use SLAM1 methods. The method which was thebest suited to our needs was the geometric projectionfilter (see[21] for reference, and[24] for implementa-tion details). It consists in building a map of featuresuncorrelated with the robot state. Such features are,for instance, the distance between landmarks or anglesbetween three of them.

Owing to the accuracy of the laser range finder, tothe good choice of our landmarks, and to the strength ofthe SLAM methods we use, we evaluate the worst caseaccuracy of our localisation system to the followingvalue: about 10 cm in position and 2◦ in orientation.We refer the reader to[24] for more details about thew

2

ildsa vedo thisgo ma-t acei

botd eo en-v icha n. In

teerable platform the CyCab robot. We close the pith some concluding remarks and guidelines on fuork in Section6.

. Localisation, environment modelling, motionlanning and execution

In the design of an autonomous car-like robot,re convinced that localisation, modelling of the eonment, path planning and trajectory tracking arundamental importance.

.1. Map-building and localisation

The CyCab robot is the size of a golf-cab capaf attaining up to 30 km/h. Its “natural” environme

s the car-park area of the INRIA Rhone-Alpes (abou

ay we evaluate these values.

.2. The obstacle map

The previous method localises the robot and bulandmark map. But, we still miss a map of obserbstacles in order to plan safe paths. To achieveoal, we build a kind of simplified occupancy grid[8]n the environment. This structure gives us infor

ions correlated with the probability that a given pls the boundary of an obstacle.

Both maps are built online, in real-time, by the rouring the construction phase.Fig. 1 shows how thbstacle map evolves while we are exploring theironment. This map is made of small patches whre added according to the need of the applicatio

1 Simultaneous Localisation And Mapping.

C. Pradalier et al. / Robotics and Autonomous Systems 50 (2005) 51–67 53

Fig. 1. Obstacle map evolution: experimental images during the obstacle map-building phase. The vehicle is driven within the car-park area aslong as needed. Simultaneously, the laser range sensor is used to detect the landmarks to build-up the localisation map.

this way, the map can be extended in any direction, aslong as memory is available. Once the map-buildingphase has finished, the obstacle map is converted intoa pixmap and passed to the motion planning stage.

Fig. 2. CyCab robot and its landmarks for localisation.

2.3. Motion planning amidst static obstacles

The motion planner adopted for the CyCab was pre-sented in[26]. Essentially, it is a two-step approach,dealing separately with the physical constraints (theobstacles) and with the kinematic constraints (the non-holonomy). The planner first builds a collision-freepath without taking into account the non-holonomicconstraints of the system. Then, this path is approx-imated by a sequence of collision-free feasible sub-paths computed by asuitable2 steering method. Finally,the resulting path is smoothed.

A key issue in non-holonomic motion planning is tofind a steering method accounting for the kinematicsof the robot. One way of designing steering methodsfor a non-holonomic system is to use itsflatnessprop-

2 That is, verifying the topological property as explained in[26].


Fig. 3. CyCab robot, its landmarks and its kinematics model show-ing the coordinates of the flat output (pointH) with respect to thereference frame of the robot placed at pointF. In our case we havethat (xF, yF, θ, ϕ) is the state of the robot.

erty[10] allowing also for feedback linearisation of thenonlinear system (this is discussed in Section2.6). Thisis what we did for the general BiS-car for which a flatoutput—or linearising output—was given in[26].

2.4. Steering a BiS-car

The kinematics model of a general bi-steerable ve-hicle and its flat output are shown inFig. 3.

The striking advantage of planning a path in theflat space is that we only need to parameterise atwo-dimensional curve whose points and derivativesdefine everywhere the currentn-dimensional state3 ofthe robot (in the case of the BiS-carn= 4). The maincharacteristic of such a curve is its curvatureκ fromwhich the steering angle can be computed.

Fig. 4shows the outcome of the motion planner us-ing an obstacle map generated as described in the pre-vious section.

2.5. User–planner interface

The user–planner interface in the CyCab is achievedthrough atouch-screensuperposed to a 640× 480 pix-els LCD display. Additionally, we use the keyboard toallow for the entrance of data.

3 The configuration space in robotics is called thestate spaceincontrol theory, so we will use indistinctly both terms.

Fig. 4. Path computed by the motion planner using a real obstaclemap. The obstacles are grown as well as the robot before computingthe path.

The interface is used to display the current positionof the robot within its environment and capture the goalposition entered by the user. These positions togetherwith the obstacle map are passed to the motion planner.The output path is then displayed allowing the user tovalidate the path or start a new search.

Finally, the reference trajectory is generated using aregular parameterisation of the path[16] and the useris requested to accept to start the execution of the tra-jectory.

2.6. Trajectory tracking using flatness

It is well known that a non-holonomic system can-not be stabilised using only smooth state static feed-backs[6]. Ever since then, time-varying feedbacks[25]and dynamic feedbacks have been successfully used inparticular for the canonical tractor-trailer and car-likerobots[9].

Flat systems are feedback linearisable by means of arestricted class of dynamic feedback calledendogenous[10]. The interest is that we are able to use state-of-the-art linear control techniques to stabilise the system.We present here results coming from recent work onfeedback linearisation of the general BiS-car.

For a reference frame of the robot placed at pointF in Fig. 3, the flat outputy= (y1, y2)T of a BiS-carare the coordinates of a pointH= (xH, yH)T = (y1, y2)T


Computed as a function of the state as follows:

H = F + P(ϕ)�uθ +Q(ϕ)�uθ⊥,

whereP(ϕ) andQ(ϕ) are the coordinate functions rela-tive to the robot’s reference frame (see[26] for details)and where�uθ (resp.�uθ⊥) are the unitary vector in thedirectionθ (resp. the directionθ +π/2).

Looking for a tractable relation between the controlsof the robot and the linearising output, we found an ex-pression giving the flat output dynamics with respect toa more convenient reference frame placed at the mid-dle of the front axle of the robot (pointF) and havingorientationγ = [θ +β(ϕ)] ±π where the functionβ(ϕ)is the characteristic angle of the velocity vector of theflat output.

The convenience of this new reference frame relieson the fact that the velocity of the flat output has a sin-gle component in it. More precisely—assuming thatγ = θ +β(ϕ) +π—one can show that, in this referenceframe, the flat output dynamics is given by the follow-ing expression[14]:

∂H

∂t= υH �uγ,

υH = υF [cos(ϕ−β−π) −QF]+ωϕ[∂P

∂ϕ− ∂β

∂ϕQ

],

F

w heha ther(

ther nti iS-c factt dif-f -d neralbe loopc

following form:

y(3)i = (y∗

i )(3) −

2∑j=2

ki,j

(y

(j)i − (y∗

i )(j)

), i = 1,2,

(2)

where (·)(p) stands for the total derivative of orderp.See[7] for details.

3. Obstacle avoidance using probabilisticreasoning

The previous approach considers trajectories in astatic environment. In order to make the executionof these trajectories more robust, an obstacle avoid-ance system should be prepared to react to unpredictedchanges in the environment. This section presents theprinciples of our obstacle avoidance module.

3.1. State of the art on reactive trajectory tracking

Most of the approaches for obstacle avoidance arelocal [11,5], that is they do not try to model the wholeenvironment. Their goal is rather to use sensor mea-sures to deduce secure commands. Being simpler andless computationally intensive, they seem more appro-priate to fast reactions in a non-static environment. Onthe other hand, we cannot expect optimal solutionsfrom a local method. It is possible that some peculiaro hicht only.

3pro-

p c-t needf s tod easen e re-d thec umo ectt s cana

aveb citet s

(ϕ) = sin(ϕ − f (ϕ))

L cos(f (ϕ)), (1)

here (υF , ωϕ) are the controls of the robot (i.e. teading and the front-steering speeds), (ϕ−β−π) thengle subtended between the velocity vector ofobot �VF and the velocity vector of the flat output�VHseeFig. 3).

From expression (1) the open-loop controls ofobot can be found as soon as the trajectory of poiHs known. As we are interested in stabilising the Bar around a reference trajectory, we explored thehat, owing to the flatness property, the system iseomorphic to a linear controllable one[10]. The enogenous dynamic feedback that linearises the gei-steerable system is presented in[14]. Then, from lin-ar control theory, it can be shown that the closed-ontrol stabilising the reference trajectoryy* has the

bstacle configuration create a dead-end from whe robot cannot escape with obstacle avoidance

.1.1. Potential fieldsThe general idea of potential fields methods,

osed initially by O. Khatib in 1986, is to build a funion representing both the navigation goals and theor obstacle avoidance. This function is built so aecrease when going closer to the goal and increar obstacles. Then, the navigation problems aruced to an optimisation problem, that is, to findommands that brings the robot to the global minimf the function. This later can be defined with resp

o the goal and the obstacles but other constraintlso be added therein.

Numerous extensions to the potential fields heen proposed since 1986. Among others, we can

he virtual force fields[3], the vector field histogram


[4] and their extensions VFH+[28] and VFH* [29].Basically, these methods try to find the best path to thegoal among the secure ones.

3.1.2. Steering angle field (SAF)The SAF method, proposed by Feiten et al. in 1994,

use obstacles to constrain steering angle in a continu-ous domain. Simultaneously, speed control is an itera-tive negotiation process between the high-level drivingmodule and the local obstacle-avoidance module.

One of the first extension to this method waspublished in[27]. It expresses the collision avoidanceproblem as an optimisation problem in the robotcontrols space (linear and rotational speeds).

3.1.3. Dynamic windowThe dynamic window approach[11] proposes to

avoid obstacles by exploring command space in orderto maximise an objective function. This later accountsfor the progression toward the goal, the position ofcloser obstacles and current robot controls. Beingdirectly derived from the robot dynamic, this methodis particularly well adapted to high speed movements.

The computational cost of the optimisation processis reduced using the dynamic characteristics of therobot (bounded linear and angular acceleration) so asto reduce the searched space. This kind of constraintsare calledhard constraintssince the must be respected.Conversely, when the objective function includes pref-e ltingc

3o

cialmo inp ctedm So,e s int nt thec .

etersa cedf thenc ntly,i of

the obstacles future trajectory. Consequently, theseobstacle avoidance methods are not applicable in realsituations yet.

3.1.5. Obstacle avoidance and trajectoryfollowing

When we want to perform obstacle avoidance ma-noeuvres while following a trajectory, a specific prob-lem appears. On our non-holonomous robot, the pathplanning stage took into account the kinematic of therobot and planned a feasible path. When the reactiveobstacle avoidance generates commands, the vehicleleaves its planned trajectory. Then, we cannot be sureanymore that the initial objective of the trajectory isstill reachable.

A solution to this problem was proposed in[20].This method tries to deform the global trajectory inorder to avoid the obstacle, respect the kinematicconstraints and ensure that the final goal is stillreachable. Even if theoretically very interesting, thisobstacle avoidance scheme is still difficult to apply inreal situations due to its computational complexities,especially on an autonomous car. In our experiments[20], the vehicle had to stop for several minutes inorder to perform the trajectory deformation.

3.2. Objectives

After all these results on obstacle avoidance, its newsp riatet re-a inka rob-l ntly,t iona

3

edas meo nsoro st ob-s

rences on the robot movement, we call the resuonstraintssoft constraints.

.1.4. Dynamic environments and velocitybstacles

In the specific case of moving obstacles, speethods have been proposed[17,2] using thevelocitybstacle notion. Basically, this notion consistsrojecting perceived obstacles and their expeovement in the space of secure commands.ach mobile object generates a set of obstacle

he command space. These obstacles represeommands that will bring to a collision in the future

In the general case, obstacle movement paramre not known a priori, so they have to be dedu

rom sensor data. Obstacle avoidance controls areomputed in reaction to theses previsions. Curret is still quite difficult to get reliable previsions

eems obvious that our goal is not to propose aolution to this problem. It has been shown[19,1] thatrobabilities and Bayesian inference are approp

ools to deal with real world uncertainty and modelctive behaviours. With this in mind, we wanted to thbout the expression of the obstacle avoidance p

em as a Bayesian inference problem. Consequehe originality of our approach is mainly its expressnd the semantic we can express with it.

.3. Specification

The CyCab can be commanded through a speVnd a steering angleΦ. It is equipped withπ radiansweeping laser range finder. In order to limit the voluf the data we manipulate, we summarised the seutput as eight values: the distances to the nearetacle in aπ/8 angular sector (seeFig. 5). We will call


Fig. 5. Obstacle avoidance: situation.

Dk, k= 1, . . ., 8 the probabilistic variables correspond-ing to these measures.

Besides, we will assume that this robot is com-manded by some high-level system (trajectory follow-ing for instance) which provides it with a pair of desiredcommands (Vd,Φd).

Our goal is to find commands to apply to the robot,guarantying the vehicle security while following thedesired command as much as possible.

3.4. Sub-models definition

Given the distanceDi measured in an angular sec-tor, we want to express a command to apply that issafe while tracking desired command. Nevertheless,since this sector only has limited information aboutrobot surrounding, we choose to express the followingconservative semantic: tracking the desired commandshould be a soft constraint whereas an obstacle avoid-ance command should be a hard constraint, the closeris the obstacle, the harder is the constraint.

We express this semantic using a probability distri-bution over the commands to apply (V,Φ) knowing thedesired commands and the distanceDi measured in thissector:

Pi(VΦ|VdΦdDi) = Pi(V |VdDi)Pi(Φ|ΦdDi), (3)

wherePi(V|VdDi) andPi(Φ|ΦdDi) are the Gaussiandistributions, respectively, centred onµV(Vd, Di) andµΦ(Φd, Di) with standard deviationσV(Vd, Di) andσΦ(Φd,Di). FunctionsµV,µΦ, σV,σΦ are defined withsigmoid shape as illustrated inFig. 6. The examples ofresulting distributions are shown inFig. 7.

There are two specific aspects to notice inFigs. 6 and 7. First, concerning the meansµV andµΦ,we can see that, the farther is the obstacle, the closerto the desired commandµ will be, and conversely, the

Fig. 6. Evolution of mean and standard deviation ofPi (V|VdD
i ) andPi (Φ|ΦdDi ) according to distance measured.


Fig. 7. Shape ofPi (VΦ|VdΦdDi ) for far and close obstacles.

nearer is the obstacle, the more secure isµ: minimalspeed, strong steering angle.

Second, the standard deviation can be seen as a con-straint level. For instance, when an obstacle is veryclose to the robot (smallDi), its speedmustbe stronglyconstrained to zero, this is expressed by a small stan-dard deviation. Conversely, when obstacle is far, robotspeedcan follow the desired command, but there isno damage risk in not applying exactly this command.This low level constraint is the result of a big standarddeviation.

3.5. Command fusion

Knowing desired controls and distance to the near-est obstacle in its sector, each sub-model, defined byPi(VΦ|VdΦdDi), provides us with a probability distri-bution over the robot controls. As we have eight sectors,we will have to fuse the controls from eight sub-models.Then we will find the best control in terms of securityand desired control following.

To this end, we define the following joint distribu-tion:

P(VΦVdΦdD1, . . . , D8S)

= P(D1, . . . , D8) P(VdΦd)P(S)

P(VΦ|VdΦdD1, . . . , D8S), (4)

where variableS∈ [1, . . ., 8] express which sector iscd ific

pu-t

sub-model, we defineP(S) as a uniform distribution.The semantic ofSwill be emphasised by the definitionof P(VΦ|VdΦdD1, . . ., D8S):

P(VΦ|VdΦdD1, . . . , D8[S = i]) = Pi(VΦ|VdΦdDi).

In this equation, we can see that the variableSacts asmodel selector: given its valuei, the distribution overthe commands will be computed by the sub-modeli,taking into account only distanceDi .

Using Eq.(4), we can now express the distributionwe are really interested in, that is the distribution overthe commands accounting for all the distances but notvariableS:

P(VΦ|VdΦdD1, . . . , D8)

=∑S

(P(S)P(VΦ|VdΦdD1, . . . , D8S)). (5)

This equation is actually the place where the differentconstraint level expressed by functionsσV andσΦ willbe useful. The more security constraints there will be,the more peaked will be the sub-model control distri-bution. So sub-models who see no obstacles in theirsector will contribute to the sum with quasi-flat distri-bution, and those who see perilous obstacles will adda peaky distribution, hence having more influence (seeFig. 8). Finally the command really executed by therobot is the one which maximiseP(VΦ|VdΦdD1, . . .,D8) (Eq.(5)).

3

nces latedC are

onsidered.P(D1, . . ., D8) andP(VdΦd) are unknownistribution.4 As there is no need to favour a spec

4 Actually, as we know we will not need them in future comation, we do not have to specify them.

.6. Results

Fig. 9illustrates the result of the obstacle avoidaystem applied on a simulated example. The simuyCab is driven manually with a joystick in a squ


Fig. 8. Probability distribution over speed and steering, resulting from the obstacle avoidance system.

environment. In this specific situation, the driver is con-tinuously asking for maximum speed, straightforward(null steering angle). We can observe on the dotted tra-jectory that, first obstacle avoidance module bends thetrajectory in order to avoid the walls, and second, whenthere is no danger of collisions, desired commands areapplied exactly as requested.

From the density of dots, we can figure out the robotspeed: it breaks when it comes close to the walls andwhile its turning and try to follow desired speed whenobstacles are not so threatening.

3.7. Relation to fuzzy logic approaches

The design of our obstacle avoidance modulesmay remind some readers of a fuzzy logic controller[15,22,12]. It is rather difficult to say that one approachis better than the other. Both fuzzy logic and Bayesianinference view themselves as extension of classicallogic. Furthermore, both methods will deal with thesame kind of problems, providing the same kind of so-lutions. Some will prefer the great freedom of fuzzylogic modelling and others will prefer to rely on the

Fig. 9. Robot trajectory while driven manually with constant desired steering angle.


strong mathematical background behind Bayesian in-ference.

As far as we can see, the choice between fuzzy logicand Bayesian inference is rather a personal choice, sim-ilar to the choice of a programming language: it hasmore consequences on the way we express our solu-tion than on the solution itself. To extend the analogy,one might relate fuzzy logic to the C language whereasBayesian inference would be closer to Ada.

4. Trajectory tracking with obstacle avoidance

The method presented in the previous section pro-vides us with an efficient way to fuse a security sys-tem and orders from a high level system. Neverthelessthe perturbations introduced in the trajectory follow-ing system by obstacle avoidance are such that they canmake it become unstable. In this section will show howwe integrate trajectory tracking and obstacle avoidance.

While following the trajectory, obstacle avoidancewill modify certain commands in order to follow asmuch as possible desired orders while granting secu-rity. These modifications may introduce delay or di-versions in the control loop. If no appropriate action istaken to manage these delays the control law may gen-erate extremely strong accelerations or even becomeunstable when obstacles are gone. This is typically thecase when our system evolves among moving pedes-trians. Thus we designed a specific behaviour to adaptsmoothly our control system to the perturbations in-d

4

4sed

o t 1 m

and 15◦ around nominal trajectory. Furthermore, as thiscontrol law controls the third derivative of the flat out-put (Eq.(2)), it is a massively integrating system. Forthis reason, a constant perturbation such as immobili-sation due to a pedestrian standing in front of the ve-hicle will result in a quadratic increase of the controllaw output. This phenomenon is mainly due to the factthat when obstacle avoidance slows the robot down,it strongly breaks the dynamic rules around which theflat control law was built. So, there is no surprise in itsfailure.

4.1.2. Probabilistic control lawIn order to deal with the situations that flat control

law cannot manage, we designed a trajectory trackingbehaviour (TTB) based again on probabilistic reason-ing (Section4.2). As this behaviour has many simi-larities with a weighted sum of proportional controllaws, we do not expect it to be sufficient to stabilise therobot on its trajectory. Nevertheless, it is sufficient tobring it back in the convergence domain of the flat con-trol law when obstacle avoidance perturbations haveoccurred. Basically, the resulting behaviour is as fol-lows: while the robot is close to its nominal position,it is commanded by flat control law. When, due to ob-stacle avoidance, it is too far from its nominal posi-tion, TTB takes control, and try to bring it back toflat control law’s convergence domain. When it en-ters this domain, flat control law is reinitialised ands d inF

4

a ent n, it

ector m

uced by obstacle avoidance.

.1. Multiplexed trajectory tracking

.1.1. Validity domain of flat control lawExperimentally, we found that the control law ba

n flatness can manage errors in a range of abou

Fig. 10. Basic diagram of the control law sel

tarts accurate trajectory tracking (this is illustrateig. 10).

.1.3. Time controlPath resulting from path planning (Section2.3) is

list of robot configuration indexed by time. So whhe robot is slowed down by a traversing pedestria

echanism and validity domains of the control laws.


compensates its delay by accelerating. Nevertheless,when the robot is stopped during a longer time, letus say 15 s, it should not consider to be delayed of15 s, otherwise it will try to reach a position fifteensecond ahead, without tracking the intermediarytrajectory. To tackle this difficulty, we introduceda third mode to the trajectory tracking: when therobot comes too far from its nominal position, wefreeze the nominal position, and we use the TTB toreenter the domain where nominal position can beunfrozen.

The global system is illustrated byFig. 10: we imple-mented some kind of multiplexer/demultiplexer whichmanage transitions between control laws. In order toavoid oscillating between control laws when at the in-terface between two domains of validity, we had to in-troduce some hysteresis mechanism in the switching.This is illustrated inFig. 10.

4.2. Trajectory tracking behaviour

Our trajectory tracking behaviour was built as aprobabilistic reasoning, in a way similar to the obstacleavoidance presented above (Section3). Functionally, itis very similar to a fuzzy control scheme as presentedin [15] and illustrated in[12].

To specify our module, we use a mechanism offusion with diagnosis[23]. If A andB are two vari-ables, we will define a diagnosis Boolean variableI

T so

thed ls( rrori tt sv

illaVw ingt ndse nceo

eent tri-

Fig. 11. Variables involved in trajectory tracking behaviour.

bution:

P(VdΦdVrΦr�X, �Y, �θI�X

VdIVrVdI�θΦdIΦrΦd

)

= P(VdΦd)P(VrΦr)P(�X, �Y, �θ)P(I�XΦd

|Vd�X)

P(IVrVd

|VdVr)P(I�YΦd

|Φd�Y )P(I�θΦd

|Φd, �θ, Vd)

P(IΦrΦd

|ΦdΦr). (6)

Using this joint distribution and Bayes rule, we will beable to infer

P(VdΦd|(VrΦr)(�X, �Y, �θ)[I�XVd

= 1]

[IVrVd

= 1][I�YΦd

= 1][I�θΦd

= 1][IΦrΦd

= 1]). (7)

Basically, this equation expresses the fact that weare looking for the most likely commands in order tocorrect tracking error while accounting for referencecommands. Having all the diagnosis variables set toone enforces this semantic. In the preceding jointdistribution (Eq.(6)), all the diagnosed variables areassumed to be independent, and to have uniformdistributions. All the information concerning therelation between them will be encoded in the distri-bution over diagnosis variables. In order to define thisdistributions, we first define the functiondσ(x, y) as aMahalanobis distance betweenx andy:

dσ(x, y) = e−(1/2)((x−y)/σ)2.

T

P

BA which express a consistency betweenA and B.henA andB will be called thediagnosed variablef IBA .

Our goal is to express the distribution overesired controls (Vd, Φd) knowing reference controVr, Φr) planned by the path planning stage, and en position (�X, �Y) and orientation�θ with respeco the nominal position.Fig. 11 illustrates theseariables.

In addition to the preceding variables, we wdd five diagnosis variablesI�X

Vd, IVrVd, I�YΦd, I�θΦd

andIΦrΦd

.ariables linked to an error variable (�X, �Y, �θ)ill diagnose if a given command helps correct

his error. Variables linked to reference commavaluate if a command is similar to the referene.

All these variables describe the relation betwheir diagnosed variables in the following joint dis

hen, for two variablesA andB, we define

([IBA = 1]|AB) = dS(A,B)(A, f (B)).


Let us see how preceding functionsSandf are definedin specific cases.

4.2.1. Proportional compensation of errorsIn the case ofI�X

Vd, we setf(�X) =α·�X and

S(Vd, �X) = max((1− β, �X)σmax, σmin).

Expression of f implies that the maximum ofP(I�X

Vd|Vd�X) will be for a value ofVd proportional

to the error�X. Expression ofSdefines the constraintlevel associated to this speed: the bigger is the error,the more confident we are that a proportional correc-tion will work, so the smaller isσ.

The basic behaviour resulting from this definitionis that when the robot is behind it nominal position, it

will move forward to reduce its error: the bigger is itserror, the faster and with more confidence that this isthe good control to apply.

For I�YΦd

, we use a similar proportional scheme. Itsbasic meaning is that when the robot has a lateral error,it has to steer, left or right, depending on the sign of thiserror. Again, the bigger the error, the more confidentwe are that we have to steer.

Finally, the same apply forI�θΦd

, except that the steer-ing direction depends not only of the orientation error,but also of the movement directionVd.

4.2.2. Using planned controlsIn the path planning stage, the trajectory was defined

as a set of nominal position, associated with planned

Fig. 12. Trajectory tracking: resulting command fusion.

Fig. 13. Collaboration of trajectory tracking an
d obstacle avoidance on a simulated example.


Fig. 14. An experimental setting showing from left to right: the arbitrary placing of the landmarks; the manual driving phase for landmark andobstacle map-building; the obstacle map generated together with the current position of the robot as seen on the LCD display; the capture of thegoal position given by the user by means of the touch-screen; the execution of the found trajectory among aggressive pedestrians.

speed and steering angle. They have to be accountedfor, especially when error is small.

Let us consider firstIVrVd

. We setf andSas follows:f(Vr) =Vr and S(Vd, Vr) =σVr ∈ [σmin, σmax], ratherclose toσmax. By this way, planned speed is used asan indication to the trajectory following system. Thedistribution overIΦr

Φdis defined using the same reason-

ing.

4.3. Results

Fig. 12 illustrates the basic behaviour of ourtrajectory tracking behaviour. In both graphs, de-sired command will maximise eitherP(V |�XVc)orP(Φ|�Y�θΦc). Since curveP(V|�XVc) is closer toP(V|�X) than toP(V|Vc), we can observe that longitudi-nal error (�X) has much more influence than reference

Fig. 15. Executed trajectory among static obstacles and moving pedestrians. Rear middle point (R inFig. 3) trajectory is drawn.


Fig. 16. Executed trajectory with respect to planned trajectory, and multiplexer mode.

Fig. 17. Applied speeds with respect to planned speed, and multiplexer mode.

command on the vehicle speed. In the same manner,steering angle is a trade-off between what should bedone to correct lateral error (�Y) and orientation error(�θ), lightly influenced by reference steering angle.

Fig. 13shows the collaboration of obstacle avoid-ance and trajectory following on a simulated example.Planned trajectory passes through an obstacle which

was not present at map building time. Obstacle avoid-ance modifies controls in order to grant security. Whenerrors with respect to nominal trajectory is too big, ourcontrol law selector switch to the trajectory trackingbehaviour. Here it is a big longitudinal error, due toobstacle avoidance slowing down the vehicle, whichtrigger the switching.


Fig. 18. Applied steering with respect to planned steering, and multiplexer mode.

4.4. Discussion

Using the multiplexed control laws we managed tointegrate, in the same control loop, our flat control, ac-curate but sensible to perturbation, with our TTB, lessaccurate but robust to perturbations. By this way we ob-tained a system capable of tracking trajectory generatedby our path planner while accounting for unexpectedobject in the environment.

Finally, when the robot has gone too far from ref-erence trajectory, or when reactive obstacle avoidancecannot find suitable controls anymore, it may be nec-essary to replan a new trajectory to the goal. This hasnot been implemented on the robot yet, but this shouldnot be considered neither a technical nor a scientificissue.

5. Experimental setup

We tested the integration of these essential au-tonomy capacities in our experimental platform theCyCab robot. The aim was to validate the theo-retical considerations made for the BiS-car and to

get insight into the limitations of the whole motionscheme.

The computation power on-board the CyCab is aPentium IITM 233 MHz running a Linux system. Allprograms were written in C/C++ language.

During the experiments the speed of the robot waslimited to 1.5 m s−1. The control rate of the robot wasfixed at 50 ms. The throughput rate of the laser range-finder was limited to 140 ms;5 therefore the control sys-tem has to rely momentarily in odometry[13] readings.

Fig. 14shows a set of pictures showing a completeapplication integrating the stages described throughoutthe paper.

Figs. 15–18illustrate how a planned trajectory isexecuted while avoiding moving pedestrians. In thisenvironment, the control law using flatness could onlybe used at the beginning and at the end of the trajec-tory. On the remaining of the trajectory, speed andsteering angle are adjusted in order to maintain se-curity while keeping pace with the plan as much aspossible.

5 This rate is fair enough for our needs, even though we could usea real-time driver.


6. Discussion and conclusions

In this paper, we presented our new steps towardthe autonomy of a bi-steerable car. The integrationof localisation, map building, trajectory planningand execution in a moderately dynamic environmentwas discussed. Control law using the CyCab flatnessproperty was found to be insufficient for trajectorytracking among moving pedestrians.

Even if this integration was successful and providessatisfactory results, we are convinced that a reactivebehaviour cannot be sufficient for the autonomy of ve-hicle in a real urban environment. For this reason, weare working on the perception and identification of roadusers (pedestrians, cars, bikes or trucks). By this way,we will be able to predict future movement of “obsta-cles” and react accordingly, in asmarterway than thesimple scheme proposed in this paper.

References

[1] P. Bessiere, BIBA-INRIA Research Group, Survey: probabilis-tic methodology and techniques for artefact conception anddevelopment, Technical Report RR-4730, INRIA, Grenoble,France, February 2003. http://www.inria.fr/rrrt/rr-4730.html.

[2] S. Blondin, Planification de mouvements pour vehicule au-tomatise en environnement partiellement connu., Memoire deDiplome d’Etudes Approfondies, Inst. Nat. Polytechnique deGrenoble, Grenoble, France, June 2002.

[3] J. Borenstein, Y. Koren, Real-time obstacle avoidance for fastCy-

sta-otics

balter-MI,

ion,Dif-

A,

y of

tionous

oryings, pp.

[10] M. Fliess, J. Levine, P. Martin, P. Rouchon, Flatness and de-fects of nonlinear systems: introductory theory and examples,International Journal of Control 61 (6) (1995) 1327–1361.

[11] D. Fox, W. Burgard, S. Thrun, The dynamic window approach tocollision avoidance, IEEE Robotics and Automation Magazine4 (1) (1997) 23–33.

[12] Th. Fraichard, Ph. Garnier, Fuzzy control to drive car-like ve-hicles, Robotics and Autonomous Systems 34 (1) (2000) 1–22.

[13] J. Hermosillo, C. Pradalier, S. Sekhavat, Modelling odometryand uncertainty propagation for a bi-steerable car, in: Proceed-ings of the IEEE Intelligent Vehicle Symposium, Poster Session,Versailles, France, June, 2002.

[14] J. Hermosillo, S. Sekhavat, Feedback control of a bi-steerablecar using flatness; application to trajectory tracking, in: Proceed-ings of the American Control Conference, Denver, CO, USA,June, 2003.

[15] L.A. Klein, Sensor Data Fusion Concepts and Applications,SPIE, 1993.

[16] F. Lamiraux, S. Sekhavat, J.-P. Laumond, Motion planningand control for hilare pulling a trailer, IEEE Transactions onRobotics and Automation 15 (4) (1999) 640–652.

[17] F. Large, S. Sekhavat, Z. Shiller, C. Laugier, Towards real-timeglobal motion planning in a dynamic environment using theNLVO concept, in: Proceedings of the IEEE-RSJ InternationalConference on Intelligent Robots and Systems, Lausanne (CH),September–October 2002, 2003.

[18] Ch. Laugier, S. Sekhavat, L. Large, J. Hermosillo, Z. Shiller,Some steps towards autonomous cars, in: Proceedings of theIFAC Symposium on Intelligent Autonomous Vehicles, Sap-poro (JP), September, 2001, pp. 10–18.

[19] O. Lebeltel, P. Bessiere, J. Diard, E. Mazer, Bayesian robotsprogramming, Autonomous Robots, 2003.

[20] O. Lefebvre, F. Lamiraux, C. Pradalier, Th. Fraichard, Obstaclesavoidance for car-like robots, integration and experimentationon two robots, in: Proceedings of the IEEE International Con-

SA,

[ s lo-Cen-

[ :tice-

[ usionro-

gent

[ hings ofand

[ of atheand

[ forional

mobile robots, IEEE Transactions on Systems, Man, andbernetics 19 (5) (1989) 1179–1187.

[4] J. Borenstein, Y. Koren, The vector field histogram—fast obcle avoidance for mobile robots, IEEE Transactions on Roband Automation 7 (3) (1991) 278–288.

[5] O. Brock, O. Khatib, High-speed navigation using the glodynamic window approach, in: Proceedings of the IEEE Innational Conference on Robotics and Automation, Detroit,USA, 1999.

[6] R.W. Brockett, Asymptotic stability and feedback stabilizatin: R.W. Brockett, R.S. Millman, H.J. Sussmann (Eds.),ferential Geometric Control Theory, Birkhauser, Boston, M1983, pp. 181–191.

[7] C. Canudas de Wit, B. Siciliano, G. Bastin (Eds.), TheorRobot Control, Springer-Verlag, 1996.

[8] A. Elfes, Using occupancy grids for mobile robot percepand navigation, IEEE Computer, Special Issue on AutonomIntelligent Machines, June 1989.

[9] M. Fliess, J. Levine, P. Martin, P. Rouchon, Design of trajectstabilizing feedback for driftless flat systems, in: Proceedof the European Control Conference, Rome, Italy, 19951882–1887.

ference on Robotics and Automation, New Orleans, LA, UApril, 2004.

21] P. Newman, On the structures and solution of simultaneoucalization and mapping problem, Ph.D. Thesis, Australianter for Field Robotics, Sydney, 1999.

22] G. Oriolo, G. Ulivi, M. Vendittelli, Applications of Fuzzy LogicTowards High Machine Intelligent Quotient Systems., PrenHall, 1997.

23] C. Pradalier, F. Colas, P. Bessiere, Expressing Bayesian fas a product of distributions: applications in robotics, in: Pceedings of the IEEE International Conference on IntelliRobots and Systems, 2003.

24] C. Pradalier, S. Sekhavat, Concurrent localization, matcand map building using invariant features, in: Proceedingthe IEEE International Conference on Intelligent RobotsSystems, 2002.

25] C. Samson, K. Ait-Abderrahim, Feedback stabilizationnonholonomic wheeled mobile robot, in: Proceedings ofIEEE-RSJ International Conference on Intelligent RobotsSystems, Osaka (JP), November, 1991, pp. 1242–1246.

26] S. Sekhavat, J. Hermosillo, P. Rouchon, Motion planninga bi-steerable car, in: Proceedings of the IEEE Internat


Conference on Robotics and Automation, Seoul (KR), May,2001, pp. 3294–3299.

[27] R. Simmons, The curvature-velocity method for local obstacleavoidance, in: Proceedings of the IEEE International Confer-ence on Robotics and Automation, Minneapolis, MN, USA,April, 1996, pp. 3375–3382.

[28] I. Ulrich, J. Borenstein, VFH+: reliable obstacle avoidance forfast mobile robots, in: Proceedings of the IEEE InternationalConference on Robotics Automation, Leuven, Belgium, May,1998, pp. 1572–1577.

[29] I. Ulrich, J. Borenstein, VFH*: local obstacle avoidance withlook-ahead verification, in: Proceedings of the IEEE Interna-tional Conference on Robotics Automation, San Francisco, CA,April, 2000, pp. 2505–2511.

Cedric Pradalier received the Diplome d’Ingnieur from the EcoleNationale Superieure d’Informatique et de Mathematiques Ap-pliquees de Grenoble (ENSIMAG), France, in 2001. He took hisPh.D. in Computer Science from the Institut National Polytechniquede Grenoble (INPG), France, in 2004. He is currently working withprofessors A. Zelinsky and P. Corke on the SmartCar project in Can-berra (Australia). His current research interests include all kind ofperception interpretation and treatments for robotics and artificialintelligence.

Jorge Hermosillo received the B.Eng. degree in Electronics andCommunications Engineering from ITESM, Monterrey, Mexico, in1990, the M.Sc. degree in Instrument Design and Application fromUMIST, Manchester, UK, in 1994, and the Ph.D. in Imaging, Visionand Robotics from l’Institut National Polytechnique de Grenoble(INPG), France, in June 2003. From 1991 to 1998 he was Researcherat the Instituto de Investigaciones Electricas (IIE) and Associate Pro-fessor at the ITESM from 1995 to 1998. His current research interestsinclude non-holonomic motion planning, sensors and sensor fusiontechniques and control architectures for autonomous navigation.

C nalP .Eng.d antaC eringf azil.S until2 ologyf boticsa

Christian Laugier received the M.S., Ph.D., and “State Doctor”degrees in Computer Science from Grenoble University (France)in 1973, 1976, and 1987, respectively. He has been involved in re-search in the fields of computer graphics and robotics for more than25 years. In 1995 he joined INRIA Rhone-Alpes and the GRAVIRLaboratory, and his current research interests lie in the areas of mo-tion planning, telerobotics, intelligent vehicles, and virtual reality.He has authored papers in the areas of computer graphics, geometricmodeling, robot programming, motion planning, dexterous manipu-lation, intelligent vehicles, dynamic simulation, medical simulators,and haptic interfaces. He also supervised more than 20 Ph.D. the-ses in these areas. Dr. C. Laugier is a Member of several scientificnational (Robea, Predit, Robotics network, etc.) and international(Adcom of IROS, Euron European network, etc.) committees, andhe is regularly involved in the program or in the organizing commit-tees (e.g. as general chair, program chair, publicity chair, etc.) of themajor international conferences in robotics (IEEE ICRA, IEEE/RSJIROS). In addition to his research and teaching activities, Dr. C.Laugier participated in the start-up of four industrial companiesin the fields of robotics, computer vision, and computer graphics.In 1997, he was awarded the “Nakamura Prize” for his contribu-tion to the advancement of the technology on intelligent robots andsystems.

Pierre Bessiere is a Senior Researcher at CNRS (Centre National dela Recherche Scientifique) since 1992. He took his Ph.D. in Artifi-cial Intelligence in 1983 from the Institut National Polytechnique ofGrenoble, France. He did a Post-doctorate at the Stanford ResearchInstitute and then worked for several years in the computer scienceindustry. He has been working for the last 15 years on evolutionaryalgorithms and Bayesian inference. He leads, with Emmanuel Mazer,The “LAPLACE Research Group: Stochastic Models for Perception,Inference and Action” (http://www.laplace.imag.fr/).

Christophe Braillon received the Diplome d’Ingenieur from theE s Ap-p lomed e deG ofes-s h.D.d ts in-c ensorf viga-t otict

arla Koike is currently a Ph.D. student in the Institut Natioolytechnique de Grenoble (INPG), France. She received an Megree in Electrical Engineering from Federal University of Satarina in 1994 and a B.Eng. degree also in Electrical Engine

rom Federal University of Cearo in 1991, both universities in Brhe worked as Associated Researcher at Bras’lia University001, and she was a research student at Tokyo Institute of Techn

rom 1996 to 1998. Her main research interests are situated rond its applications.

cole Nationale Superieure d’Informatique et de Mathematiqueliquees de Grenoble (ENSIMAG), France, in 2004, and the Dip’etudes Approfondies from the Institut National Polytechniqurenoble (INPG), France, also in 2004, being advised by pr

ors C. Laugier and J. Crowley. He is currently pursuing the Pegree in Robotics from the INPG. His current research intereslude computer vision, optical flow computation, sensors and susion techniques and control architectures for autonomous naion. Since 2002, he is a leading Member of the ENSIMAG robeam for the French robotic cup.

http://www.laplace.imag.fr/

The CyCa b: a car-like robot navigating...

Documents

Transcript of The CyCa b: a car-like robot navigating...