On line reactive planning for a non holonomic mobile in a dynamic world

Proc. of the 1991 IEEE Int. Conf. on Roboticss and Automation April 7-12, 1991. Sacramento (CA). pp.432{437.

On line reactive planning for a non holonomic mobile in a dynamic world Th. Fraichard and C. Laugier

INRIA Rh^one-Alpes ZIRST. 655 av. de l'Europe, 38330 Montbonnot Saint Martin, France [email protected]

Abstract | This paper deals with the problem of planning and controlling the motion of a car like mobile moving in a dynamic and roadway like environment. The contribution presented here is a motion controller which executes in a reactive way a given nominal motion plan. Such a plan is made up of a smooth trajectory C and of time constraints of the type \reach location l at time tl ". Data concerning the actual environment of the vehicle considered are assumed to be obtained through perception. In order to get the required reactivity, we have developed a motion controller with two main components: the pilot which analyses the current situation and adapts the nominal plan accordingly, and the executor which generates the required motion commands. The pilot operates at a symbolic level using a set of behavioural rules. The executor makes use of a potential eld approach to generate the motion commands. Keywords | mobile-robot, non-holonomic-system, motion-execution, potential-

eld.

Acknowledgements | This work was partly supported by the European EUREKA EU-153 PROMETHEUS project. Note | Figure 4 is missing.

On line reactive planning for a non holonomic mobile in a dynamic world Th. Fraichard, C. Laugier



IMAG{LIFIA 46, av. Felix Viallet, 38031 Grenoble Cedex, France

Abstract

nal plan periodically provided by the motion planner to the vehicle. This nominal plan is made up of a geometric trajectory and of an associated set of time constraints of the type \reach location l at time tl ". Data concerning the actual environment of the vehicle considered are assumed to be obtained through perception|by perception, we mean either direct perception through sensors or communication with the other vehicles. Since there may be a discrepancy between the hypotheses made at planning time and the actual environment, the nominal plan has to be adapted at execution time.

This paper deals with the problem of planning and controlling the motion of a car like mobile moving in a dynamic and roadway like environment. The contribution presented here is a motion controller which executes in a reactive way a given nominal motion plan. Such a plan is made up of a smooth trajectory C and of time constraints of the type \reach location l at time t ". Data concerning the actual environment of the vehicle considered are assumed to be obtained through perception. In order to get the required reactivity, we have developed a motion controller with two main components: the pilot which analyses the current situation and adapts the nominal plan accordingly, and the executor which generates the required motion commands. The pilot operates at a symbolic level using a set of behavioural rules. The executor makes use of a potential eld approach to generate the motion commands. l

1.2 Related works

Solving the previous problem requires rst to deal with two major motion planning problems: the non holonomy problem|i.e. the problem of the existence of non integrable kinematic constraints preventing the mobile from moving in any direction|and the world dynamicity problem. Dealing with non holonomy in motion planning is relatively recent; nevertheless important theoretical and practical results have already been obtained (see [9] for a recent overview). The problem of motion planning for several mobiles has been studied by several authors (see [4] and [11] for example). But most of the developed methods assume a centralized planner and the fact that the motions of the moving obstacles are known beforehand. There are very little results concerning the on line motion planning in a dynamic context. The earliest work concerning the obstacle avoidance problem in a dynamic context is [8] who introduced the concept of potential eld. In order to take advantage of an a priori knowledge about the structure of the world, [3] describes a method combining a global path planner with a motion controller allowing the mobile to follow a connected set of channels using its proximity sensors and a potential eld. As we will see further, our approach is based upon this philosophy. It also makes use of an extension of the method described in [1] to deal with non holonomic mobiles in a static environment.

1 Introduction

1.1 The problem

This paper deals with the problem of planning and controlling the motions of non holonomic mobiles in a dynamic world|i.e. in a time varying environment including other mobiles and various static and moving obstacles whose locations and behaviours are partly known beforehand. The framework of this research program is the European Prometheus Eureka project whose purpose is to design new cars and new road infrastructures to solve some critical points of today's trac|mainly safety and congestion problems. A system including a motion planner (made up of a smooth trajectory planner and a temporal planner) and a motion controller has thus been developed. The whole system architecture and the temporal planner are presented in [6] while [5] describes the smooth trajectory planner. This paper focuses on the motion controller which deals with the reactive execution of the nomi

Research Director at INRIA

1

1.3 Our approach

Motion planner

Let M be the vehicle considered, let W be the actual environment and let P be the nominal motion plan periodically provided by the motion planner. P is made up of (1) a geometric trajectory|i.e. a curve C of W | and (2) time constraints of the type \reach location l at time tl "|i.e. t-uples (l; tl ). The purpose of the motion controller of M is to generate the appropriate motion commands which are required to execute P while constantly adapting the behaviour of M to the characteristics of W (unpredicted events in particular). The on line adjustment of P must obviously respect the set of behaviour rules (highway code) prevailing in W . Since W is a dynamic environment, the commands to apply must simultaneously control the position, orientation and velocity changes. In order to satisfy the previous requirements, we have developed a hierarchical system with two main modules (see gure 1): the pilot which analyses the current situation and adapts the nominal plan accordingly, and the executor which generates the required motion commands. The pilot enables M to react appropriately to unexpected events. It is implemented as a symbolic layer enforcing a set of behavioral rules. This approach consisting in combining a \rule layer" and a \process layer" to get an appropriate reactive behaviour has already been proposed in [2] and [10]. The executor represents the numerical layer which is needed to generate the required motion commands at every time interval t. It has been implemented using a \dynamic potential eld" which combines classical information about the task (distance to the current subgoal and to the static obstacles), but also information about the nominal plan (trajectory and time constraints) and data about the dynamic obstacles (position and velocity). The paper is organized as follows: x2 and x3 respectively describe the pilot and the executor. Experimental results are nally discussed in x4.

World model Nominal plan Motion controller Pilot

Perception

Completed plan

Executor

Motion commands

Actual world

Vehicle

Figure 1: the system architecture velocity of M becomes very dierent of the nominal velocity. Once activated, the pilot analyses the current situation, checks for the validity of P and possibly adapt P using a set of behaviour rules. Since our system deals with road trac, these behaviour rules stem from the highway code and take into account such things as the priority rules, the trac lights, the speed limit and so on. The inputs of the pilot are respectively P , perception data about the actual environment of M and an a priori model of W . W is a subset of the road network. As such, it is a highly structured environment; this structure is explicitly represented through a connected set of regions: lanes, con ict and post con ict regions. Basically the roadway is an organized set of lanes. A lane is characterized by a main spine along which the vehicles are supposed to move in a given direction. A con ict region represents the sub part of the roadway corresponding to the intersection of two or more lanes. It is clear that only one vehicle at a time can occupy such a region. This means that a vehicle heading for a particular con ict region r has to check for the availability of r before entering it. Besides, in order not to block the trac ow, the vehicle must also check whether it is possible to cross r without stopping. In other words, there must be no obstacle right after r. Checking for such a condition led us to de ne appropriate regions called post con ict regions. In the symbolic model of W , the connectivity of the roadway along with the associated motion constraints are represented using a labelled oriented graph structure. The information labelling the graph is either known a priori or obtained at execution time through

2 The pilot

2.1 General presentation

The pilot of the vehicle M can be seen as the symbolic layer of a reactive motion planner. Its main task is to adapt the nominal motion plan P to the characteristics of the actual environment of M . For that purpose, the pilot is activated through an asynchronous mechanism based upon the perception data and the feedback from the executor. For instance, the pilot is activated when a new obstacle is detected ahead of M , or when M is about to cross an intersection. or when the current 2

perception (for instance, when detecting a beacon indicating the presence of a trac light or informing the vehicle that it will soon cross a higher priority lane). In the current version of the system, a priori information about W is provided to the system. Information about unpredicted events and the surrounding vehicles (position and velocity parameters) is obtained using a simulation process. The main output of the pilot is a \completed" motion plan P + made up of: (1) the geometric trajectory C , (2) time constraints (l; tl ) and (3) symbolic behaviour instructions of the type \stop at l", \overtake". .. As we will see further, the symbolic instructions are used by the executor to tune some parameters of its potential eld.

The crossing intersection behaviour is activated by con ictual situations|i.e. when a con ict region aimed by M is already occupied. Solving such con icts requires in general to introduce waiting delays in P + : M must stop before a con ict region or behind a preceding vehicle and wait until the involved con ict and post con ict regions are free. A consequence of such a behaviour is usually to generate a time delay
t incompatible with the next time constraints of P . An obvious solution to this problem is to ask the motion planner for a new set of time constraints. This is not implemented yet in the current version of the system; we simply add
t to the next time constraints.

As explained earlier, the task of the pilot is to analyze the current situation and to adapt P accordingly when necessary. In a rst step, the system must determine whether the event which activated the pilot will generate a failure or a degenerated execution of P |i.e. a collision, a non satis ed time constraint or an excessive delay. The second step of the reasoning is applied when a local adjustment of P must take place. It leads the pilot to temporarily modify the current symbolic instruction of P + and/or the next time constraint (l; tl ). The reasoning is based upon a process which simulates the execution of the planned maneuver. This simulation process assumes that the behaviour of the other vehicles will remain constant in the near future. Five types of behaviour have been considered in the current implementation of the system: normal and cautious|i.e. when approaching an intersection|road driving, overtaking, stopping at a given location and crossing an intersection. Each behaviour is characterized by a set of preconditions, a set of symbolic instructions and a set of postconditions indicating that the symbolic instructions have been carried out. For instance, the preconditions of an overtaking maneuver are: (M follows X )^(jv ? vd j < k1)^(delay > k2 ) where v and vd are respectively the current and the desired velocity of M (see x3.3.3), delay is the delay associated with the nominal time constraints and X is an other vehicle moving in the same lane as M . The symbolic instruction \Overtake" added to P + will have several eects at the executor level: the attractive potential eld associated with the nominal trajectory C is weakened and the desired velocity vd will be increased in order to overtake as fast as possible. The postcondition associated with the overtaking maneuver is characterized by: d(M; C ) <  where d is the euclidean distance.

The executor of the vehicle M can be seen as a motion commands generator. At every time interval t, the executor has to generate the position, orientation and velocity changes which are required rst to follow as closely as possible the nominal trajectory, second to satisfy the associated time constraints and nally to avoid any collision with the close obstacles. The inputs of the executor are respectively the completed motion plan P + and a description of the local environment of M . The description of the local environment of M includes an appropriate representation of the geometry and the instantaneous position and velocity parameters of each obstacle surrounding M (the static obstacles are obviously represented using only geometric and position characteristics). It is assumed that all relevant information about the surrounding obstacles is provided by the perceptive system of M . A static obstacle is represented using a simple bitmap description (as in [1]). The model of a dynamic obstacle is completed by a vector representing its instantaneous velocity. In the sequel, we will call OS (resp. OD ) the set of the static (resp. dynamic) obstacles belonging to this local world model. The main output of the executor is a motion command. Let us see what is a motion command. The natural control parameters of a car like vehicle are the velocity (throttle and brake) and the steering angle (driving wheel). If we assume M to be front wheel driven, these control parameters can be characterized by the pair (v; ) where v and  are respectively the module and the orientation of the instantaneous velocity ~v applied at the front axle midpoint F (see gure 2). This representation has already been used in [1] to nd a path for a car like vehicle in a static environment. The basic idea consisted in considering at each step all the pairs (v; )

3 The executor

3.1 General presentation

2.2 The approach

3

belonging to f?vmax ; +vmax gf ? max ; +max g in order to determine the next elementary movement. But this approach cannot be directly applied to solve our problem because it does not deal with the dynamic aspect of the vehicle motions (e.g. the velocity cannot instantaneously change from ?vmax to +vmax ). Therefore we chose to use the time derivatives of v and  to represent the motion commands: (_v; _ ) 2 [?v_ max ; +_vmax ][?_ max ; +_ max ] where v_ max and _ max are respectively the maximum acceleration and the maximumangular velocity associated with the steering angle of M . R

F

on three parameters: the time t and the instantaneous con guration q of M and the instantaneous velocity ~v of M . Thus U is a combination of three terms U1 , U2 and U3 whose purpose are respectively to guide M along the nominal trajectory, to avoid the dynamic obstacles and to satisfy the time constraints. Being given U , our algorithm to select the next motion command to be applied at time t is: 1. Compute all the pairs (q ;~v ) obtained after application of the allowed motion commands (_v; _ ). Any motion command yielding a velocity ~v ) which does not verify the kinematic constraints of M is discarded (as explained in [7]M is subject to constraints on its acceleration, its steering angle and its centrifugal acceleration). 2. Choose the pair (q ;~v )|and the associated motion command (_v; _ )|which minimizes the value of U without generating any collision in W . 0

0

theta phi

v

0

Figure 2: a car like vehicle Another output of the executor is a feedback to the pilot about the current status of M |non satis ed time constraints, or excessive delay...

In the current implementation, U combines three terms: U (q;~v; t) = F (U1(q); U2 (q;~v); U3 (~v; t)). The basic idea underlying U is to track the valley generated by U1 while using U3 to adjust the velocity of M . The purpose of U2 is to locally modify the previous valley by introducing some new repulsive elds representing the dynamic obstacles. This operation may generate local minima in which M may remain stuck. Hopefully such situations are rare (because of the structure of the roadway and of the choice of U2 ); besides the associated local minima are time varying. So far the three terms U1 ; U2 and U3 are linearly combined. The symbolic command provided by the pilot may modify the coecients of this linear combination (to perform an overtaking maneuver for instance).

Let K = [?v_ max ; +_vmax ][?_ max ; +_ max ] be the control space of M . The problem to be solved by the executor is to select at time t the best motion command (_v; _ ) to be applied to M during the time interval [t; t ] where t = t + t]. Selecting such a command requires in theory to analyze the eects of all the motion commands allowed at time t; in practice, this is achieved by reasoning about a discretized representation of K. In the current implementation of the system, we use the following discretization: f?v_ max ; 0; +_vmaxgf? _ max ; 0; +_ maxg. Let (q;~v) be the instantaneous state of M at time t; q represents the current con guration of M |i.e. the t-uple (x; y; ) where (x; y) are the coordinates of the rear axle midpoint R and  is the orientation of the main axis of M |and ~v is the instantaneous velocity of M . Let (_v; _ ) be a motion command chosen in the previous representation of K. Assuming that (_v; _ ) is instantaneously executed by M (this assumption is consistant since t is small) then the state of M during the time interval [t; t ] is characterized by the pair v = v + t:v_ and  =  + t:_ . Computing the con guration q of M at time t is carried out using the kinematic equations of M (see [7]. In order to guide the choice of the next motion command to be applied, the executor makes use of a potential eld U . Since W is time varying, U depends 0

0

3.3.1 De ning U1

U1 is de ned as a usual static potential eld combining

an attractive term associated with the nominal trajectory described in the motion plan and a repulsive one associated with the static obstacles of OS . This means that the trajectory represents a \potential valley" that the reference point of M has to follow. Computing U1 for M |i.e. a continuous set of points|is complex and time consuming; thus the expression of U1 is approximated using a prede ned set fp1 ; p2; :::; pkg of \control points" appropriately chosen in M . U1 (q) is computed for the con guration q of M by combining the potential elds Upi associated with these control points in W . There are many ways to de ne and to combine

0

0

0

3.3 The potential eld U

3.2 The approach

0

0

0

0

4

3.3.3 De ning U3

these potential elds (see [1]). In our implementation,

U1 is de ned as U1 (q) = maxi=1:::k Upi with

The purpose of U3 is to guide the choice of the velocity parameters in order to try to satisfy the current time constraint (l; tl ) of P + . This function clearly depends on the current time t, the current velocity ~v of M and the distance between the current position q of M and the goal l. Let us notice that the traditional euclidean distance is not relevant for our application because of the trajectory constraint imposed by P + . Thus we decided to use of a particular distance d2 (q; l; C) representing the length of the sub part of C included between q (or the point of C which is the closest) and the current goal point l. Since it is also necessary to provide M with a realistic kinematic behaviour (depending on the type of maneuver to execute), we decided to use a parameterized function to compute the required velocity pro les of the type \acceleration{constant speed{ deceleration" and which yields desired velocities over certain time intervals. Such a velocity pro le is computed each time a new time constraint (l; tl ) is provided by the pilot. It allows to generate the last potential eld U3 (~v; t) = jv ? vd j where vd is the current desired velocity. The symbolic command provided by the pilot will obviously in uence the way vd is determined.

Upi = d1(pi; C i ) + d1 (pi ; Ci )= min d1(pi ; o) o s 2O

where ; 2 R and C i is the nominal trajectory associated with pi; d1 is a distance de ned as follows 8a 2 W ; 8B  W ; d1(a; B ) = minb B (jxb ? xaj + jyb ? ya j). It should be noticed that U1 does not necessarily t with the no collision property, especially when using a small number of control points. Indeed it may happen that there exists o 2 OS such as minp M d1(p; o)  mini=1:::k d1 (pi; o). This means that it is necessary to check the no collision using a appropriate interference algorithm. 2

2

3.3.2 De ning U2

U2 is intrinsically more complex than U1 since it must

deal with the dynamic obstacles. The basic principle of U2 is to associate a new repulsive potential eld with each dynamic obstacle. Let UD be the repulsive potential eld associated with D2OD and let qD (resp. ~vD ) be the current con guration (resp. velocity vector) of D. The idea underlying the de nition of UD is to consider that UD is only de ned in a region RD characterized as follows: RD = fx2W j yx ~ ^(~v ? ~vD ) = ~0 and yx: ~ (~v ? ~vD )0; 8y2M g. RD can be seen as the shadow of M in the direction (~v ? ~vD ) (see gure 3). It is clear that if ~v and ~vD do not change then M will collide with D in the near future. This collision condition can be stated as D \ RM 6= ; and then:  if D \ RD = ; UD (q;~v) = k0 ~v ? ~v k = otherwise D

4 Experiments

A prototype of the motion controller is currently being developed and tested. It is implemented in C on a SPARC Sun workstation and it includes a simulator for a car like vehicle. The executor is almost totally implemented. The pilot includes the ve behaviours presented earlier but only the overtaking and intersection crossing behaviours have been tested. As mentioned earlier, W is represented by a bitmap. The potential elds Upi contributing to the determination of U1 are also represented by bitmaps and are computed as in [1]. These potential elds are computed each time a new geometric trajectory C is provided to the motion controller. U2 and U3 are dynamic and must obviously be computed at every time step t. The determination of U2 is complex since it involves vectorial calculus and distance computation between continuous set of points. In the current implementation and for eciency reason, U2 is computed using a set of points regularly distributed on the boundaries of each vehicle. Several examples have been processed in simulation with M behaving as expected. Figure 4 presents some snapshots of the simulation of the motion of M within an intersection and among other vehicles. W is represented by a 500  500 bitmap. It takes about 30s to compute the bitmap associated with U1 and the motion

where = miny M;x D M d(y; x) and d is the euclidean distance. An interesting property of UD for our application is that the potential eld is null in three cases: (1) when D follows M with vD  v (v and vD are respectively the modules of ~v and ~vD ), (2) when M follows D with vD  v and (3) when M and D move on two parallel lanes. UD is also computed using a set of control points belonging P to M and to D. Finally U2 is de ned as: U2 (q;~v) = D OD UD (q;~v). 2

2

\R

2

V

D

V

D

V - VD

M RD

Figure 3: UD 5

Acknowledgements

commands are generated every 0:5s. M must reach the point g. In snapshot #1, M starts to overtake the preceding vehicle. The attraction associated with the nominal trajectory has been weakened and the velocity will increase (this is visible in snapshot #2). At the end of the overtaking maneuver, U1 is set back to its normal value and M then crosses the intersection following its nominal plan and adjusting its velocity so as to avoid collision with the other vehicles (snapshot #3 and #4).

This work was supported by the French Ministry of Research and Technology and the European Prometheus Eureka project.

References

[1] J. Barraquand and J-C Latombe. On non-holonomic mobile robots and optimal maneuvering. Revue d'intelligence Arti cielle, 3(2):77{103, 1989. [2] P. Caloud. Distributed motion planning and motion coordination for multiple robots. Working paper, Robotics Lab., Computer Science Dept, Stanford Univ., CA (USA), 1990. [3] W. Choi, D. Zhu, and J.C. Latombe. Contingency tolerant robot motion planning and control. In Proc. of the IEEE/RSJ Int. Workshop on Intelligent Robots and Systems, pages 78{86, Tsukuba (Japan), 1989. [4] M. Erdmann and T. Lozano-Perez. On multiple moving objects. A.I. Memo 883, MIT AI Lab., Boston, MA (USA), May 1986. [5] Th. Fraichard. Smooth trajectory planning for a car in a structured world. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 318{323, Sacramento, CA (USA), Apr. 1991. [6] Th. Fraichard and C. Laugier. Planning movements for several coordinated vehicles. In Proc. of the IEEE/RSJ Int. Workshop on Intelligent Robots and Systems, pages 466{472, Tsukuba (Japan), Sep. 1989. [7] Th. Fraichard, C. Laugier, and G. Lievin. Robot motion planning: the case of non-holonomic mobiles in a dynamic world. In Proc. of the IEEE/RSJ Int. Workshop on Intelligent Robots and Systems, pages 757{ 764, Tsuchiura (Japan), July 1990. [8] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. Int. Journal of Robotics Research, 5(1), Spring 1986. [9] J-P. Laumond, M. Taix, and P. Jacobs. A motion planner for car-like robots based on a mixed global/local approach. In Proc. of the IEEE/RSJ Int. Workshop on Intelligent Robots and Systems, pages 765{773, Tsuchiura (Japan), July 1990. [10] E. Sandewall et al. Approach and architecture for a pro-art car control system. Research Report LAICIDA-1989-14, Linkoping Univ. (Sw), 1989. [11] C.W. Warren. Multiple robot path coordination using arti cial potential elds. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 500{ 505, Cincinnatti, OH (USA), May 1990.

Figure 4: The experiments have shown that the system works fairly well when M is moving in a rather unconstrained environment and with few other vehicles (about 10). However the choice of the correct coecients for the function combining U1 , U2 and U3 is so far empirical. It means that the introduction of a new behaviour requires to experiment new coecients in order to get motion commands consistant with this behaviour. This point has to be improved in the future. An other problem stems from the choice of the discretization parameters for W , K and time|in the current implementation, x = l=500 where l is the length of W , t = 0:5s and there are 9 possible motion commands (_v; _ ). A ner discretization would provide M with a smoother behaviour but at the expense of the computation load. In order to improve this point, we are studying the possibility to \wire" the algorithms used to compute U . 6