Journal of Intelligent and Robotic Systems 18: 329–350, 1997. c 1997 Kluwer Academic Publishers. Printed in the Netherlands.

329

Robust Path Planning for Non-Holonomic Robots ALAIN PRUSKI LAEI, University of Metz, 57045 Metz Cedex 1, France. e-mail: [email protected]

SERGE ROHMER UTT, BP 2060, 10010 Troye, France (Received: 24 April 1995; in final form: 8 March 1996) Abstract. This paper deals with mobile robots path planning. We decompose the problem in three parts. In the first part, we describe a modeling method based on a configuration space discretization. Each model element is built following a particular structure which is easy to handle, as we will show. We describe the methodologies and the algorithms allowing to build the model. In the second part, we propose a path-planning application for a non-holonomic robot in configuration space. In the third part, we modify the path in order to be robust according to the control errors. Key words: path planning, mobile robotics, non-holonomic robots

1. Introduction Path planning is a central point in mobile-robot programming presently studied by most research teams. The methodologies developed are of all kinds and each provides a solution to the basic path-planning problems. The basic problem may be defined in the following manner: for a robot in a cluttered environment, the path-planning problem consists in finding a control sequence in the free space allowing for a robot to join a source point to a goal point. The first solutions are derived from the artificial intelligence field by finding a trajectory in a graph considering two environment elements: a set of accessible places and the connexities between them with an associated cost [4]. This method has the disadvantage of not considering the mobile’s size in the path planning, which leads to unrealistic solutions. Methodology evolution leads to building the environment graph considering the admissible configuration space [12]. The mobile’s size is implicitly taken into account. The set of admissible configurations is given according to the desired problem resolution. In [11] the author proposes to cut the configuration space in slices of orientations taken by the mobile and only consider points belonging to the visibility graph. In [5] the author considers rectangloid configuration slices. In [1] the authors reduce the accessible physical space to the obstacles edge allowing to perform the exact form of the contact between the mobile and the environment in an analytical manner. In [9] the authors reduce the space to the boundary of the Vorono¨ı diagram by considering the mobile’s

VTEX(P) PIPS No.: 109952 MATHKAP JINT1326.tex; 14/05/1997; 13:20; v.5; p.1

330

ALAIN PRUSKI AND SERGE ROHMER

size. In [3] the author reduces the space to the axis of the biggest possible generalised cones fitted on the free space. The mobile’s size is taken into account by choosing the axis whose normal distance to the boundary of the cone is greater than half the width of the robot. Another solution consists in representing the environment by artificial potential functions whose gradient defines a fictive force which pulls the robot towards the goal and pulls it away from the obstacles. The number of the robot-force application points is important. Generally, the force is applied on the point nearest the obstacle [6] or possibly on two extreme points [19]. The potential field modelling requires a discretization of the isopotential lines. As was described, neither method considers the mobile’s physical possibility. In general the wheeled robot kinematical model is defined by a set of three equations with three unknown variables (position and orientation) which are not independent. This leads to a dimension of the configuration space that is superior to the number of degrees of freedom. The path-planning methods described above do not take this aspect into account. J. P. Laumond was the first [7, 8] to study the path-planning problem under non-holonomic constraints. He proved that if a trajectory exists for a holonomic robot then it is always possible to find a trajectory for a non-holonomic robot when performing maneuvers. This principle was used in [17]. It leads to a non-unique solution, the path is defined as a saw teeth form that is difficult to apply to a real robot. In [2], the authors proved that only two states of the control variables are needed to achieve the goal. An environment discretization following a grid allows to generate a set of trials. The free cells reached are made up of nodes of a search tree. This interesting method allows to find optimal paths for problems of great complexity. Unfortunately the computing cost is a function of the grid resolution. The uncertainty problems and the precision of trajectory tracking or geometrical environment modeling were not mentioned in the above methods. Generally the environment model is known with enough precision but it is not the same for trajectory tracking. During motion the mobile is controlled by a localization process including the dead-reckoning whose precision is weak. A resetting is sometimes performed by more precise methods (triangulation, map matching...) but the resulting trajectory may not be smooth. This paper aims to describe a method of path planning which is feasible for a non-holonomic mobile robot, taking into account the localization errors determined by the dead-reckoning. We propose to deliver a robust trajectory. 2. Research Context The path planning developed is a module used on an assistance robot designed to help the disabled drive their electric wheelchair. The problem we address may be defined in the following terms. Let be a known environment, the problem consists in describing a set of sequences joining two points and avoiding static obstacles. Some constraints must be taken into consideration: those associated to the robot

JINT1326.tex; 14/05/1997; 13:20; v.5; p.2

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

331

control and those associated to the handicap. The first pertains to the mobile’s kinematical characteristics. Our mobile has a classical mechanical structure: two powered rear wheels and two casters. Such a mechanism is well known for its non-holonomic characteristics. Navigation is controlled by a position feedback by dead-reckoning. It is well known that this method is subject to errors that automatically deviate the mobile from its original trajectory. A specific localization system would be more efficient. We mount onboard ultrasonic sensors and we use range information to obstacles to relocate the mobile from time to time, generally at the beginning and the end of motion. During the motion only deadreckoning is used to compute cost consideration. The second constraints are of a physiological nature. The person using such a mobile has a reduced possibility of movement. For that reason it is not acceptable to allow backward motions. The person must see the place towards which he is moving permanently. The general form of the trajectory must be relatively smooth in order to guaratee maximum comfort. Trajectory tracking results in activating the obstacle avoidance module during navigation near the obstacles, creating modifications of the original path, limiting the comfort and increasing driving stress. The path-planing method we propose allows to generate a forward trajectory from a source configuration to a goal configuration considering the mobile’s non-holonomic characteristic. We take into account trajectory tracking errors at path-planning level. 3. Method Principle The method principle consists in modeling the space according to admissible configurations by a set of rectangloids, and then finding a trajectory between two configurations. This approach was employed in [17]. Our contribution lies in the type of rectangloids used and in their connexity evaluation. Path planning is performed in a classical manner by an A∗ algorithm, the evaluation function takes into account the uncertainties that should appear during navigation. In fact during the path-planning process a collision verification is made by enlarging the trajectory using the track of the uncertainty ellipse of the robot’s position. During the whole process, the environment model used has a great importance. It allows an easy manipulation of the different elements of the environment. 4. Environment Modeling 4.1. THE PRINCIPLE The method used to model the environment is described in [14] and [15]. We only make a simple review insisting on the main points used by the planning method. The proposed model is a configuration space discretization by a set of rectangloids the structure of which represents intervals in each dimension. The interval size is both equal to a power of two and a function of its minimal value.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.3

332

ALAIN PRUSKI AND SERGE ROHMER

We write for one dimension: T = [Vmin , Vmax ] with Vmin = k2u and Vmax = (k + 1)2u − 1 with u ∈ [0, n − 1]; k ∈ [0, 2n−u − 1] and 2n = Tmax + 1

The set of the values of T may be defined through mathematics. 4.2. A KARNAUGH’S BOARD ANALOGY We will tackle it through physics in order to better understand its implication in the proposed application. It is possible to make an analogy with Karnaugh’s board. For example a two-dimension Karnaugh board of 2n lines and 2n columns each represents a logical AND function between the set of states of n binary variables for the lines and in the same way for the columns. The different board cells represent the set of all possible logical states of a combinatorial process. Line and column coding is performed by a Gray code such that it is possible to combine two neighbour cells. The combination makes a variable useless since the process is activated whatever the state of the variable. Thus a combination of more cells erases some variables from the logical equations. Our approach is based on this principle. In the sequel we consider a grid of 2n × 2n cells laid on the environment. Each line and column is coded according to a natural binary code of n binary variables (Fig. 1a). This code was preferred to the Gray code in order to allow a swift association between the position in the cell and the numerical value represented by the code. The method originality resides in the coding of association of cells. In the Karnaugh method the variable whose state changes in the combination of two cells is erased. In our approach the variable is kept and takes a third state that we note X . This state represents the states 0 and 1 simultaneously. Thus if an association of some cells according to the lines or the columns gives T = 1010XX and if we find the equivalent numerical value we obtain: (T )10 = 1.25 + 0.24 + 1.23 + 0.22 + X.21 + X.20 (T )10 = {40, 41, 42, 43}

It yields four numerical values because two X are present in T . If we number the lines and columns from 0 to 2n − 1 then the numerical values of T represent the cells to be modeled. The lines and colums are coded independently. We may verify that the X are always fitted on the right of T by performing a continuous sequence of numerical values. We see that the interval structure proposed above is verified. For the above example we have Vmin = k2u

Vmax = (k + 1)2u − 1 with u = 2 and k = 10

We call such numbers Multivalue Numbers (MVN) or Multivalue Codes.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.4

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

333

Figure 1. The coding principle.

Figure 2. MVN example. This example needs 7107 nodes and 1417 codes for a 274 ms computing time on a PC-486 DX.

4.3. THE MVN REPRESENTATION The MVN representation in a computer imposes the use of a minimum of two bits for each variable to represent the three states 0, 1 and X . A MVN of dimension one having a maximal value equal to 2n − 1 needs 2n bits decomposed in 2 words of n bits each. The first one is noted CODE and the second one VALIDATION. Each bit ai of the first word defines the state of the binary variable 0 or 1. Each bit bi of the second word defines the state of the same bit of CODE if bi = 0 then

JINT1326.tex; 14/05/1997; 13:20; v.5; p.5

334

ALAIN PRUSKI AND SERGE ROHMER

ai = X and if bi = 1 then ai = {0, 1} (Fig. 1b). This representation allows to use simple logical functions in order to perform treatments such as intersection, complementation, maximal or minimal value computation, fusion ... The MVN may be represented in a tree form (Fig. 1c) which is more efficient than the numerical representation, especially for code search operations or intersections. Each cell ai of T from left to right is represented by a node connected to a father having two sons. An element X is leaf node. The greater the number of values represented by the MVN is, the fewer nodes the tree has. For a multidimensional MVN each interval following each dimension is represented by a sub-tree. The tree of dimension i turns into a sub-tree connected to the leaf node of the sub-tree of dimension i − 1. We can see that cells defined by an n tree are represented in the same manner. The fundamantal difference with the MVN is the independence in dimension that allows for the MVN to associate some cells of the n trees decreasing the total amount of cells in the same way. Figure 2 represents an example of a 2D environment. The coding algorithm is in O(n2 ) described in [14].

5. Model Manipulation This chapter aims to describe some specific treatment procedures used in the modeling method which we describe later. Some low-level treatments such as basic modeling starting from a binary grid or code translations or rotations are not described here. For more detail the reader may refer to [14] and [15]. 5.1. CODING A RECTANGULAR ZONE CREATE CODE (Xmin , Xmax , Ymax , Ymin ) This algorithm determines a set of codes describing a rectangular zone defined by its boundaries. It may be applied in n dimensions. We consider the problem in dimension two in order to illustrate our explanations. First of all we treat the problem in dimension one which consists in finding the MVN representing the interval [Xmin , Xmax ]. The principle consists in applying the following procedure: 1. 2. 3. 4. 5. 6.

i = 1; Q = Xmin ; Find for Xmin the parameters k and u such that Q = k2u ; v = 0; MNV(i) = (k, v); Do X = k2v ; If X > Xmax . Then {Q = k + 2v−1 ; jump to 2}; If v = u THEN {MVN(i) = (k, v); END} ELSE {v = v + 1; jump to 4}.

Step 2 is obtained by analysing the structure of the integer Q under its binary form. Thus u equals to the position of the fitrst ’1’ from the right. The first bit is at position 0. K is simple to determine if u is known. The algorithm has a complexity of O(log(Xmax − Xmin )). For more dimension each coding is performed according to each dimension independently. The set of obtained

JINT1326.tex; 14/05/1997; 13:20; v.5; p.6

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

335

codes is the combination of each code of each dimension with all other codes of all other dimensions. Thus the number of the obtained codes is equal to the multiplication of each code in each dimension. When the codes are defined they may be put on the tree. 5.2. INTERSECTION The intersection treatment is very frequently used for the collision verification in the case of mobile robot control. This may be performed by following two procedures according to the form under which the MVN are represented. First we take two MVN in the form (CODE, VALIDATION). Let M1 = (Code1, Validation1) and M2 = (Code2, Validation2), an intersection exists if (Code1 XOR Code2) AND Validation1 AND Validation2 = 0

If it exists its value will be Code Intersection = (Code1 OR NOT(Validation1))AND (Code2 OR NOT(Validation2)) Validation Intersection = Validation1 OR Validation2. The procedure is different to find the common intersection between a MVN and a tree. This is performed by following on the tree a path defined by the MVN in each dimension. We take two examples. First we consider that the cardinality of the starting MVN is less than the MVN on the tree which has a common intersection. We progress on the tree starting at the node source and we stop when a leaf node is reached. Then we progress towards the next dimension if it exists. If on the contrary the cardinality of the origine MVN is greater than

Figure 3. The tree for the codes T1 = (10XXX, 01XXX), T2 = (101XX, 110XX), T3 = (10XXX, 001XX).

JINT1326.tex; 14/05/1997; 13:20; v.5; p.7

336

ALAIN PRUSKI AND SERGE ROHMER

the intersection MVN in the tree then all leaf nodes of a greater depth than the source code have a common intersection with the source MVN. The tree in Figure 3 represents the codes on 5 bits (10XXX, 01XXX), (101XX, 110XX), (10XXX, 001XX). The intersection between this tree with the code (10010, 0110X) gives (10XXX, 01XXX) and with the code (10XXX, 0XXXX) gives [(01XXX, 01XXX), (01XXX, 001XX)]. These codes correspond to those having an common intersection. The intersection values are defined by logical equations as mentioned above. 5.3. COMPLEMENTATION The complement operation of a code C1 in a Code C2 consists in finding the set of codes contained in C2 and having no common intersection with C1. It is possible to use a method based on the theorem of De Morgan [14] but we propose a different procedure which is just as swift. When two codes have a common intersection then it may be computed as seen above. We reason for a one dimension problem. If C1 = (Xmin , Xmax ) and the intersection code C1 ∩ C2 = Intersection = [Imin , Imax ] then the complement of C1 in C2 equals to Create Code(Xmin , Imin ) ∪ Create Code(Imax , Xmax ) 5.4. EDGE DETECTION This procedure aims to determine the codes constituting the boundaries of a compact set of codes. Such codes have a cardinality unity according to one dimension. The procedure consists in performing the following process: Consider S Code a set of codes, the set of edge codes is determined in the following manner: we consider for each code of S Code the boundary codes tangent to the code on the exterior. We take for example a 2D code (01XXX, 1110X). There are four boundary codes defined by [(00111, 1110X), (10000, 1110X), (01XXX, 11011), (01XXX, 11110)]

Figure 4. Edge detection.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.8

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

337

We note the set codes Boundaries. The edge codes Edges are defined by a set of codes such as Edges = COMPLEMENT(INTERSECTION(S Code, Boundaries)) 6. Configuration Space Modeling Our path-planning method is based on a configuration space model proposed in [12]. We apply the Minkowski difference to the 2D free-space model and we deduce 3D model of the admissible configurations. 6.1. THE ROBOT MODEL We use a rectangular robot but the methodology may by applied to all convex robots without loss of generality. The rectangular robot is of known dimensions and we take a reference point P at the center of the rear wheel axle. The robot configuration model is defined by a set of orientation intervals discretized around the reference point. The physical space swept by the robot from angle Θ1 to Θ2 is a convex polygon. The model is built as follows: – put the reference point P at the center of the 2D space considered. If the 2D discretized space is 28 × 28 then the point P in placed in 27 × 27. This aspect is very important because it allows to decompose the parts NORTH, SOUTH, EAST and WEST of the robot model very simply, as we will see later. – code for a given angular interval the space of the convex hull swept by the robot. – perform a rotation around P and begin the coding again, for the new angular sector. – start the process again for N angular sectors. – when N sectors are coded all codes are fusioned according to the dimension Θ.

Figure 5. The physical space needed for a given robot orientation.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.9

338

ALAIN PRUSKI AND SERGE ROHMER

Figure 6. The robot model.

We propose to apply an alternative to that method. It consists in creating a bit map in 3D of the amount of cells occupied by the robot for the different orientations. Starting from this model the coding is made directly according to the procedure defined in [14]. Both methods are equivalent in time consumption. For example a rectangular robot of size 32 × 16 for 64 angular intervals requires 0.8 seconds for the coding on a PC 486 and 2268 codes. The sizes are given in number of elementary cells.

6.2. ENVIRONMENT MODEL In the environment modeling step we apply the Minkowski difference between the 2D free-space model and the robot model. The general principle of the modeling process consists in determining the COMPLEMENT of the track of the simmetrized robot on the 3D free space when its reference point is placed on the contour edge of the obstacles (Fig. 7). The modeling process is performed according to the following sequences: 1. First the space and robot model must be homogeneous. The first one must be transformed in 3D. The third dimension which is the orientation is validated for all angular values. The orientation dimension is added to each code such that the interval [0, 2n − 1] is validated. (2n is the number of possible cells for that dimension). We obtain the C SPACE codes. 2. We perform the edge detection of the new model defined in (1). We obtain the C CONTOUR. We split the S CONTOUR set in four parts depending on whether the contour edge forms the boundaries of an object on the EAST, NORTH, SOUTH or WEST. We call them C EAST, C NORTH, C SOUTH and C WEST side respectively. This process is done by verifying if the neighbour

JINT1326.tex; 14/05/1997; 13:20; v.5; p.10

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

339

Figure 7. The model building process.

cell on the east, north, south and west has a common intersection with the free space or not. 3. An identical decomposition is done on the robot model. We obtain R EAST, R NORTH, R SOUTH and R WEST. The robot decomposition is far more simple because of a MVN property. The decomposition is natural if the reference point is placed in the center of the 2D space. The East codes are constituted by the set of codes having a value in X greater than 2n − 1. The North codes are constituted by the set of codes having a value in Y less than 2n − 1. The South codes are constituted by the set of codes having a value in Y greater than 2n − 1. The West codes are constituted by the set of codes having a value in X less than 2n − 1. 2n is the number of cells in X and Y . 4. This step consists in determining the track of the robot if the reference point is moved on the contour of the free space. This is true for the couples of codes R SOUTH R NORTH R WEST R EAST

with C NORTH with C SOUTH with C EAST with C WEST.

We take the first case to explain the principle. For each code of C NORTH we determine Xmax and Xmin knowing that the set of intervals according to Y has a

JINT1326.tex; 14/05/1997; 13:20; v.5; p.11

340

ALAIN PRUSKI AND SERGE ROHMER

cardinality one and that the interval on Θ is equal to [0, 2n − 1]. The Xmax and Xmin computing [14] is Xmin = CODE AND VALIDATION and Xmax = CODE OR NOT(VALIDATION)

Card(Code) = NOT(VALIDATION) + 1; For a code C I of C NORTH the track of the robot is given by: DO FOR all code C I of C NORTH XCImin = CODEX C I AND VALIDATION C I; Trans X = 2n − 1 XCImin ; Card(C I) = NOT(VALIDATION C I) + 1; YCImin = CODEY C I AND VALIDATIONY C I; Trans Y = 2n − 1 − YCImin ; FOR all Code R I of R SOUTH {Xmin = CODE R I AND VALIDATION R I; Xmax = CODE R I OR NOT(VALIDATION R I); Create Code(Xmin + Trans X, Xmax + Trans X + card(C I), Ymin + Trans Y, Ymax + Trans Y ); } The same operation is performed on the codes C SOUTH, C EAST and C WEST. The obtained track forms a set of codes in 3D called TRACK. 5. The next step consists in finding the complement codes of TRACK in the free space codes C SPACE. The admissible configuration space equals CONFIG = COMPLEMENT(TRACK, C SPACE). 6. The last modeling step consists in optimizing the code number of CONFIG. A fusion process is run to do that. 7. Path Planning This Section deals with two aspects of path planning in configuration space according to the mobile’s kinematics. First we consider that the robot is holonomic and then we take into account the non-holonomic constraints. 7.1. PATH PLANNING FOR HOLONOMIC ROBOT We described in a previous paper [16] the methodology used to compute the path for a point robot in 2D free space. We proceed in the same manner for a

JINT1326.tex; 14/05/1997; 13:20; v.5; p.12

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

341

finite-sized robot in configuration space. The multivalue tree (MCT) is used as a research tree. The A∗ algorithm allows to determine the sequence of MVN from a MVN including the source configuration to the MVN including the goal configuration. 7.2. PATH PLANNING FOR A NON-HOLONOMIC ROBOT In [17], the author employed the configuration space-decomposition method by rectangloids. He have shown that a path may be found for a non-holonomic robot. In a first step the principle consists in not considering the angular configuration Θ. Then the author applies J. P. Laumond’s principle which proved that in a given space where a robot has a range of free orientations two points may be joined by an admissible trajectory. This method has the disadvantage of connecting the number of maneuvers to the size of the cell: the larger the cell, the larger in small the number of maneuvers. As we have seen above our aim consists in reducing the maneuvers. We propose a configuration space-decomposition method in the time of execution of the A∗ algorithm such that the transition from a cell to another neighbour cell depends on the angular value allowed inside the cell. 7.3. METHOD PRINCIPLE Roughly, the method consists in starting a MVN cell input window, in projecting it towards the cell boundaries according to the allowed angular interval. These boundaries are used to create a set of sub-sets which are added to the MVN tree. Let two neighbour cells in the configuration space C1 and C2, we call ‘Input

Figure 8. The image concept.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.13

342

ALAIN PRUSKI AND SERGE ROHMER

Window’ Wi an area situated on a C1 face. It represents a C1 input transition area. We call ‘Output Window’ Wo the contact area between the two cells C1 and C2. We call ‘Image Window’ Im(Wi) the Wi projection on the C1 faces according to the allowed angular interval in C1. If Im(Wi) ∩ Wo 6= ∅ a motion without manoeuvre from C1 to C2 is then possible. The image Im(Wi) is stored in the MVN tree by set of codes contained in C1 as represented in Figure 8. Each code representing an image is labeled by NORTH, SOUTH, EAST and WEST indicating the face on which the window is. The MVN orientation label allows to compute the trajectory crossing point. We have for the MVN = [CodeX/ ValidX, CodeY/ValidY]EAST the crossing point x = CodeX + (ValidX + 1) y = CodeY + (ValidY + 1)/2

The MVN tree structure allows to store a code inside another code. That possibility is not available on the octrees. 7.4. PATH-PLANNING ALGORITHM The path-planning algorithm for a non-holonomic robot in the configuration space is defined hereby considering S as an image code including the source configuration and G a MVN containing the goal. PLANNING(S, G) { O = {S}; /∗ Initialize an Open list O with the code S ∗ / Compute f ∗ (S); /∗ f ∗ (S) is the evaluation function of A∗ .∗ / C = {∅}; /∗ Initialize a Closed list C including no element∗ / WHILE O 6= ∅ DO { Choose a MVN n from O such the F∗ (n) is minimum; IF G ∩ n THEN Return(Success) ELSE { Put n in C; FOR Each neighbour n0 of n DO FOR Each neighbour m of n0 DO { Compute the Image window Im(n0 ); Compute the codes MVN[Im(n0 )]; FOR each Code i of MVN[Im(n0 )] DO { f (i) = g∗ (n) + cost(n, i) + h∗ (i); IF i (C ∪ O) THEN

JINT1326.tex; 14/05/1997; 13:20; v.5; p.14

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

343

Figure 9. Some path-planning examples.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.15

344

ALAIN PRUSKI AND SERGE ROHMER

{ Add i to O with f ∗ (i) = f ; Attach a pointer from i to n; }

ELSE

}

}

IF f (i) < f ∗ (i) DO {f ∗ (i) = f (i); Modify the pointer toward n; IF i ∈ C THEN put it in O; Erase i from C; }

} Return(NO SOLUTION); }

The evalution function is performed according to the crossing point. Figure 9 represents three planning cases for a non-holonomic robot considering that it can turn on the spot. 8. Robust Path Planning The above path-planning method is based on purely computational geometry without taking into account the uncertainties appearing during motion. The trajectory tracking supposes that errors on the control level and on the perception level are weak. The aim of this study is to determine a trajectory that activates as little as possible in order to spare the execution controller the computations which are time-consuming. A second reason concerns the trajectory shape. A frequently modified trajectory cannot be smooth and is uncomfortable for the user in our case of application. Another argument can be pointed out, the requirement to achieve a constraint area with the guarantee to cross it safely. Generally the sensors sense the local environment too late, the maneuvers are then either complex to generate in order to come out of some given situations or create loss of references or are in a blocking situation. We suppose that the environment is well known. The proposed method consists in modifying the above planning algorithm by projecting the mobile position uncertainty ellipse in the configuration space. 8.1. MOBILE POSITION UNCERTAINTY MODEL We have adopted the uncertainty model defined in [13, 18] supposing that the errors have a Gaussian distribution. For a wheeled mobile robot the configuration

JINT1326.tex; 14/05/1997; 13:20; v.5; p.16

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

345

variables are defined by 



∆θn xn = xn−1 + ∆Dn cos θn−1 + 2   ∆θn yn = yn−1 + ∆Dn sin θn−1 + 2 θn = θn−1 + ∆θn

xn , yn , θn are the coordinates of the robot position and orientation at sample n, Xn is the coordinate vector of the robot at sample n, ∆Dn is the trajectory covered from n − 1 by the centre of the robot, ∆θ is the orientation variation from n − 1 applied to the center of the robot.

The covariance matrix associated to the configuration vector is given by [13]. t t C[Xn+1 ] = JXn C(Xn )JX + J∆D,∆θ C(∆D, ∆θ)J∆D,∆θ n t with J∆D,∆θ C(∆D, ∆θ)J∆D,∆θ =α



D0 0θ



where JXn is the Jacobian of Xn evaluated at n − 1, JX∆D,∆θ is the Jacobian of Xn evaluated at n − 1, D and θ are the displacement and orientation values from the origin. The mobile’s localization is represented by an uncertainty ellipse centered on the mean value whose equation is given by (X − X)t C −1 (X − X) = k2

with k2 a coefficient depending on the probability that the robot is located inside the ellipse. 8.2. ROBOT TRAJECTORY MODEL When the robot moves from a point Xi−1 to a point Xi it is always situated inside the track of the equiprobability ellipsoid. The track is represented in Figure 10. We characterize the ellipsoid track by a quadrilateral formed by the boundaries T 1 and T 2 constituting the tangents between the ellipse in i − 1 and the ellipse in i. The two other sides are formed of the segments Q1 and Q2 joining the boundaries T 1 and T 2. More than two segments are of this definition, we choose the one with the biggest quadrilateral. 8.3. ALGORITHM MODIFICATION The modified algorithm runs in two steps. First a path planning is executed as we have described above. Then in a second step a collision verification is made

JINT1326.tex; 14/05/1997; 13:20; v.5; p.17

346

ALAIN PRUSKI AND SERGE ROHMER

Figure 10. The path track.

between the track of the equiprobability ellipse and the configuration space. If all the track is in the admissible configuration space then a safe motion is possible with the given probability. The collision verification is made starting from the contour codes. Of these codes are available because they were computed during the configuration space coding. Three cases occur: Case 1: there is an intersection between an obstacle and a boundary T 1 or T 2. Case 2: there is an intersection with the two boundaries T 1 and T 2. Case 3: a whole obstacle is included in the track. Case 1. The verification of the collision between T 1 or T 2 with the environment is performed by a ray-tracing method generated by a Bresenham algorithm. Each point belonging to the segment constitutes a MVN of cardinality one in the configuration space. The collision verification between the point and an obstacle is rapidly performed on the MVN tree. If a collision occurs then we virtually slide on the obstacle towards the track axis till the obstacle disappears. The motion from the collision point is reproduced on the track axis on P which constitutes an intermediary point between Xi−1 and Xi (Fig. 11). The procedure is run again in order to verify if the new trajectory with the new orientation is included in the admissible configuration space. Case 2. This is a doorway crossing case. The ellipse track collides both with T 1 and T 2. Here an intermediate point creation won’t prevent the collision. The procedure consists in choosing the closest collision point on T 1 or T 2 the from they origin and to apply the treatment proposed in Case 1. An intermediary point is created. If the collision occurs again on one side then we compute a Risk coefficient that is equal to R = M ∗ /M . M is the length of the major ellipse axis centered on P and M ∗ the length of the major ellipse axis of same orientation as M , centered in P and tangent at the point T belonging to the obstacle in collision at the closest point from the track axis. Figure 12 reproduces the treatment sequences. The MVN N containing the point Xi is affected by the risk coefficient R such as g(i) = g(i) • R with g(i) the cost computed by the A∗ algorithm for

JINT1326.tex; 14/05/1997; 13:20; v.5; p.18

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

347

Figure 11. Creation of an intermediary point.

Figure 12. Computation of the risk function.

joining the node source to the node i constituted by N . We run again the A∗ algorithm with the MVN belonging to the trajectory from the source code to the code including Xi in the open list. The algorithm may be executed in one step considering that the collision verification is made at each node development. Such a procedure would be time-consuming. Case 3. The third case concerns the obstacle’s whole inclusion in the ellipse track. This case is verified if all contour codes of an obstacle are included between the segments T 1, T 2, Q1 and Q2. In this case we run the procedure defined for Case 1 while the obstacle is not out of the track or till we are in Case 2. We have implemented the algorithm with the three cases on a PC-486 in C language. The time for path planning depends on the number of obstacles colliding the ellipse track. We may consider, following the experiments, that it is bound to 1 to 2 s. Figure 13 represents a path-planning case in three steps

JINT1326.tex; 14/05/1997; 13:20; v.5; p.19

348

ALAIN PRUSKI AND SERGE ROHMER

Figure 13. Planning a robust path.

with two times Case 2 and two times Case 1. The robot is here considered as circular. Figure 14 represents a real application of a path planning for a rectangular ROBUTER robot. The initial position is defined in a circle with a 2 2 are 1 cm radius. The non-collision probability is 90%. The values σ∆D and σ∆θ respectively 2 cm and 2 degrees. The linear speed is set 200 mm/s and the angular speed to 0.4 rad/s. On Figure 14a the robot follows a trajectory theoretically free of collision defined by the first algorithm. The control uncertainties induce a collision. In Figure 14b the new robust algorithm is used and we see that an intermediary point was created avoiding the collision.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.20

ROBUST PATH PLANNING FOR NON-HOLONOMIC ROBOTS

349

Figure 14. Robust path planning for a rectangular ROBUTER robot.

9. Conclusion In this article we propose a robust path-planning method for a non-holonomic robot. The proposed trajectory decreases potential collisions and avoids frequent resorting to the avoidance modules. The robot we used in our simulation and for our experiments performs the motions by a set of translations and pure rotations. Future works consist in taking into account a minimum giration radius. References 1. Avnaim, F., Boissonnat, J. D., and Faverjon, B.: A practical exact motion planning algorithm for polygonal objects amidst polygonal obstacles, in IEEE Conf. Robotics and Automation, 1988, pp. 1656–1661. 2. Barraquand, J. and Latombe, J. C.: On non-holonomic moble robots and optimal maneuvring, Rev. d’Intelligence Artificielle 1(2) (1989), 77–103. 3. Brooks, R. A.: Solving the findpath problem by good representation if free space, IEEE Trans. SMC-13(3), 190–197. 4. Chatilla, R.: Syst`eme de navigation pour un robot mobile autonome: Mod´elisation et processus de d´ecision, Th`ese de DDI, Toulouse, 1981. 5. Gouzenes, L.: Strategy for solving collision free trajectories, Problems for mobile and manipulator robots, Int. J. Robotics Res. 3(4) (Winter 1984). 6. Khatib, O.: Real time obstacle avoidance for manipulator and mobile robots, Int. J. Robotics Res. 5(1) (Spring 1986). 7. Laumond, J. P.: Faisible trajectories for mobile robot with kinematic and environment constraints, in Proc. Intelligent and Autonomous Systems, Amsterdam, 1986, pp. 346–354. 8. Laumond, J. P., Jacobs, P. E., Taix, M., and Murray, R. M.: A motion planner for nonholonomic mobile robots, IEEE Trans. Robotics and Automation 10(5) (Oct. 1994). 9. Leven, D. and Sharir, M.: Planning a purely translational motion for a convex object in twodimensional space using generalised Vorono¨ı diagrams, Discrete Computer Geometry (1987), 9–31. 10. Lozano-Perez, T.: Automatic planning of manipulator transfer movements, IEEE Trans. Systems, Man and Cybernetics SMC-11(10) (1981). 11. Lozano-Perez, T.: Spatial planning: A configuration space approach, IEEE Trans. Computer C32(2) (1983), 108–120. 12. Lozano-Perez, T. and Wesley, M. A.: An algorithm for planning collision-free path among polyhedral obstacles, Comm. ACM 22(10) (1979).

JINT1326.tex; 14/05/1997; 13:20; v.5; p.21

350

ALAIN PRUSKI AND SERGE ROHMER

13. Moutarlier, P. and Chatilla, R.: An experimental system for incremental environment modelling by an autonomous mobile robot, Lecture Notes in Control and Information Sciences, Experimental Robotics, I. 1st Int. Symp., Montr´eal, June 1989, pp. 327–346. 14. Pruski, A.: Multivalue coding for image and solid processing, Rev. Int. CFAO et’Infographie 6(2) (1991), 135–152. 15. Pruski, A.: Multivalue coding: Application to autonomous robots, Robotica 10 (1992), 125– 133. 16. Pruski, A. and Rohmer, S.: Multivalue coding application to autonomous robot path planning with rotations, in IEEE Conf. Robotics and Automation, Sacramento, 1991, pp. 694–699. 17. Simeon, T.: Motion planning for a non-holonomic mobile robot on 3D terrain, IEEE Workshop on Intelligent Robots and Systems, Osaka, 1991. 18. Smith, R., Self, M., and Cheesman, P.: Estimating uncertainty spatial relationships in robotics, Proc. 2nd Workshop on Uncertainty in Artificial Intelligence, Philadelphia, May 1986. 19. Zhu, D. and Latombe, J. C.: Constrainsts formulation in a hierarchical path planner, in IEEE Conf. Robotics and Automation, Cincinnati, OH, 1990, pp. 1918–1923.

JINT1326.tex; 14/05/1997; 13:20; v.5; p.22