Finding Collision-Free Smooth Trajectories for a Non-Holonomic Mobile Robot

Introduction

This paper is part of the research work currently done on the mobile robot Hilare [Giralt 84] [Chatila 86], with respect to automatic path planning in a constrained space, taking into account the vehicle's kinematic constraints.

If the study of navigation in a constrained space leads to cons i dering the problem in terms of path­ finding in the configuration space [Lozano-Perez 83] (the well-known "piano mover" problem), some possible kinematic constraints of a mobile robot do not guarantee the feasibility of the trajectories generated by classical methods. Indeed, the number of degrees of freedom is less than the dimension of the configurati on space. Taking into account these constraints increases the complexity of the path­ finding problem [Lozano-Perez 86].

In a recent paper [Laumond 86] we studied this original problem. It was demonstrated that the existence of a trajectory f or a mobile robot with two degrees of freedom is characterized by the robot of the same shape but with three degrees of freedom. It appears that the passing through highly const rained spaces can lead to a large number of backing up maneuvers.

The aim of this paper is to present a study of smooth trajectory planning for a mobile robot whose kinematics is that of a car. This study relies on two properties . The first one characterizes the existence of smooth trajectories by the existence of trajectories consisting of line segments, circle arcs of fixed radius , or contact arcs . The other one determines, from the shortest path, the minimal angular variations that any trajectory belonging to the same class of homotopic mapping, will execute. The associat i on of these two properties allows us to transform the path planning problem into one of polygonal line finding in a dual space of the conf iguration space, i.e. the space ol centers ol curvature.

This second property is established in an Euclidean plane. This restriction leads us to consider the app~ication of the results to the case of a circular robot. Note that the method presented is applicable to more general environments than polygonal ones.

2 Smooth trajectories existence. delinitions, property ol

We consider a mobile robot MR evolving in a planar world, whose kinematics is that of a car. A configuration is defined by a triplet (x,y,9) of R 2 *s 1 where (x,y) are the coordinates of the rear axle middle point and e, the vehicle's orientation expressed in S 1 , the oriented unit circle. Such a vehicle is subject to a non-holonomic joint [Whittaker 65] dy- dx.tge = 0 (1) and has two degrees of freedom. This kinematics is analogous to that of a robot with two separate driving wheels as Hilare, although in the case of the car , the turning radius is lower bounded .

In the following, we consider that the mobile robot, MR, is circular, of radius r, subject to the joint (1) and that its turning radius is lower bounded by a constant r 0 . Let M be the center of MR. In this context, the admissible configuration space is characterized by its projectiqn on R2, noted ACSP . ACSP is very simply obtained by an isotropic growth of the obstacles by the radius r (see figure 4a). In the sequel, we suppose that ACSP is bounded by generalized polygonal lines (i . e . consisting of line segments and arcs of circle [Laumond 87])

From the non-holonomic joint (1) it is deduced that e Arctg(dy/dx) . Thus there is a one to one correspondance between the set of trajectories of M in R 2 *s 1 and that of the trajectories of Min R2.

Let T be a feasible trajectory of M, and f (respectively g) be a characteristic representation of T in the plane (resp. R2.s1 ) . f is a continuous mapping of [0,1] on R2 , piecewise of class c2, i.e . there exists a finite sequence (t 1 ) 1 such that the restriction of f to ]t1,t 1· 1[ is differentiable twice and its second derivative is continuous; moreover, at each point of f(]t 1 ,t 1 · 1 [) the curvature is less than 1/r0 . If, in addition, f is of class c 1 , Tis said to be smooth (or "without backing up maneuver"; in [Laumond 86] we said "without maneuver"). T is said to perlorm an angular gap [e,e·] included in s 1, if there exists an interval [t 1,tJ] included in [0,1] such that [e,e · ] is included 1n the projection of g([t 1 , tj]) on s 1 . A turn refers to a trajectory arc whose direction of curvature is constant.

Property 1 : A feasible smooth trajectory exists between two configurations of MR if and only if there exists a smooth trajectory consisting of line segments, circle arcs of radius r 0 or contact arcs whose curvature at any point is less than 1/r0 .

Demonstration Let T be a smooth trajectory feasible by MR and f be a representation of T in R2; f being piecewise of class c 2, there exists a partition of [0,1] into intervals [t 1,t 1· 1] such that the curvature off between f(t 1) and f(t 1· 1) has a constant sign. Refine [t 1,t1· 1] into intervals [t 1J,t!J+l] according to whether or not f (t 1J,t!J·l) 1s a part of the contact trajectory . The contact arcs have a curvature less than 1/r0 . Consider the intervals [t 1J,t!J+l] corresponding to parts of the contact-free trajectory ([t 10 ,t 11 ], [t 12 ,t 13 ] in figure 1 ). Construct the shortest path L, between f(t 1J) and f(t!J+l) in ACSP, so that the tangents in the extremities are preserved. L is a curve of class c1 (a generalized polygonal line) whose curvature has the same sign as that of T (see [Hershberger and Guibas 86] and [Laumond 87] for algorithmical aspects). All the arcs of L, except possibly those at the ends, are contact arcs . If all these arcs have a radius of curvature greater than ·r 0 , L has the property searched for (for example as is the case between f(t 12 ) and f(t 13 )). In the opposite case, let t 1k = (t 1J + t!J+l )/2 and iterate the process on the intervals [t 1J,t 1k] and [t 1k,t!J+l], so that the radius of curvature at t 1k is r 0 . This process converges since f is contact­ free. So, a trajectory satisfying the property searched for is progressively constructed. The converse follows obviously. 1

Thus, to prove the existence of a smooth trajectory feasible by MR, it suffices to prove the existence of a smooth trajectory whose arcs which are not contact arcs are circle arcs of fixed radius. This type of trajectory is characterized only by the different centers of curvature. Indeed, consider a generalized polygonal line L consisting of circle arcs of the same radius r and tangent segments (such a line is noted g.p.l.r). This line is associated with the polygonal line L* linking the arc centers of same curvature. To each change in the curvature of L corresponds a new connected component of L*. Some of these components can be isolated points. L and L* are said to be duals (see figure 2).

Thus the algorithm consisting of searching for the centers of curvature becomes clearer. The following property will enable us to state more accurately in which domains this search has to be carried out.

3 "Canonicity" of the shortest path.

Consider the shortest path between two points in ACSP. All parts of this path which are not in contact are line segments. It appears that this path which is optimal in distance is equally optimal in terms of angular variations in its homotopy class . We only consider the trajectories of class c 1. The shortest path will be referred to as the canonical trajectory of its class.

Property 2 : Let T be a canonical trajectory and L a homotopic trajectory in ACSP. Let A be a contact arc between two consecutive line segments s 1 and S2 of T, and let e 1 and e 2 be the respective directions of s 1 and s2. L performs the angular gap [e 1,e2] in the concave domain D bounded by A and the half-lines supporting s 1 and s 2 (see figure 3) . More precisely, if P 8 is the half-plane bounded by the tangent in A whose direction is e and which do not contain the obstacle, then

D = ( U 8 E[ 8 1 , 8 2 l P 8 ) n ACSP .

Demonstration (sketch) : With the above hypotheses and notations, T being the shortest path, any ho­ motopic trajectory L have to pass through P 8 · Thus, if L is smooth, it has a point in P8 whose tangent direction is e . So L performs [e 1 ,e2] in D. I

In other words, the shortest trajectory performs the minimum angular gap. The advantage of this property lies in that it enables us to deter.nine in which domains the arcs have to be searched for, and then in which domains their centers have to be searched for, since the arcs have a fixed and known radius (property 1 ).

4 Space of centers of curvature.

This space is defined for each homotopy class . For each turn [e 1 ,e2] performed by the canonical trajectory T, we define the domain o· containing the duals of all the g .p.l.r0 ·s ·homotopic with T and performing the angular gap [e1 ,e2] in the domain D defined in property 2. With the notations of the property 2, let p* 8 = t(P 8 ), where tis the translation of vector (r 0cose, -r0sine) . D* is included in U 81 [ 81 , 821 P* 8 · To computeD* requires that the obstacles located in D are taken into account . This is easily achieved by means of an isotropic growth of radius r 0 , of the boundaries of ACSP contained in D (see figure 3). The space of curvature centers is the set of all the domains associated with the canonical trajectory turns.

Let A be the arc of T executing the turn and L the

(part of) g.p.l.r 0 performing [e1 ,e2] and holonomic with A. The direction of curvature of L is constant; L* is convex and passes through all the p* 8; then we must have L*ncneE[el ,e 21P* 8) ~.¢. We call kernel of D* the set Ker(D ) o*ncneE[e 1,e 21P*e>· Thus, a necessary and sufficient condition of existence of one g.p.l.r 0 homotopic to the arc A and contained in D, is that K(D*)/¢. Any polygonal line dual of a solution g.p.l.r 0, has a non-empty intersection with K(D*) (condition C1) . Figure 3 illustrates the notions of domain and kernel.

Note that a domain of curvature center space is not necessarily connected and that its connected components are not necessarily simply connected (they may have holes). This remark leads us to the use of tree data structures which will allow to represent the domains of the curvature center space.

5 Linking of turns.

Let A1 and A2 be two consecutive turns on a canonical trajectory T, D1* and D2* being the associated domains of the curvature centers. With the formalism introduced, it is very simple to express the conditions of existence of one g.p.l . r 0 L executing the two t~rns and homotopic to T. Two different cases are considered , namely :

. if A1 and A2 are of the same curvature, L exists if and only if L* is connected and is the union of L1* and L2*, such that L1*c:D1*, L 1 *nK(D 1 *)~¢, L2"cD2* and L2*nK(D2*H¢ (condition C2) .

. if A1 and A2 are of opposite curvature, L exists if and only if there exist two polygonal lines L 1 · and L2* (with a non-empty intersection with K(D1 ) and K(D2" ) respectively) in D1* and D2* such that two of their end segments are parallel and at a distance of 2r 0 (condition C') (see figure 2 : the parallel segments shown in dashed lines). Note that the condi tions impose that there exist two points in K(D,") and K(D2*) respectively, located at a distance greater than 2r 0, hence that : Max (dist(x,y) I xEK(D1"), yEK(D2")} ) 2r0 ( 0 ).

This last condition supports the introduction of the following propagation function F : let X and Y be two kernels of the domains associated with two consecutive turns . If these two turns have the same direction, let F(X)=Y (no constraints on the distance from X toY). If these two turns are of opposite directions, let :

F(X) = { pEY 1 Max{dist(p,p')/p'EX})2r0 }. Condition ( 0 ) is written F(X)#¢. Let F(¢)·¢.

6 Sketch of algorithm.

The algorithm concerning the search for smooth trajectories consists of four steps. We assume the environment to be made up of generalized polygons.

Step 1 : In the first step, we find the shortest ·canonical trajectory T. To do this, we use a path planning procedure in the generalized visibility graph of ACSP (i.e. the set of positions reachable by M) [Hershberger and Guibas 86) [Laumond 87). If r)r0, ACSP is bounded by generalized polygons whose radius of curvature is, at any point, greater than or equal to r, hence to r 0. T is smooth and feasible; the algorithm stops here. Otherwise

Step 2 : Let A1, ... ,An be the sequence of turns (contact arcs) executed by T. For each A1 compute the domain D1* of the possible centers of curvature as well as kernels K(D1*) . If one of these kernels is empty, the corresponding turn cannot be negociated without backing up maneuver. If none of these domains is empty, the linking of different turns must then be envisaged.

Step ' : Let t 0 and tn+l be the initial and final centers of curvature of T. Generally they correspond to contact-free arcs of radius r 0 (see figure 4b). With function F we successively propagate {t 0} onto K(D1*) , ... ,K(Dn"l and {tn+1}. If Fn+1({to})·¢ there is no smooth trajectory . Let j be the greatest integer such that FJ({to})/¢ , it is necessary to per form maneuvers to link turns aJ and aJ+l' If not :

Step 4 : In each domain D1*, search for a polygonal line satisfying the three conditions C1, C2 and C,, This is by far the most difficult step to implement. Indeed, the search for the polygonal lines must be continued as long as a solution has not been found , in each connected component of each D1* domain. This m81 prove a very costly search since the non-existence of a solution in the current step leads to a backtracking on the previously and partially explored domains.

Figures 4 give an example of solution.

7 Complexit7.

At this stage of the development of the algorithm, the overall complexity cannot be precisely evaluated yet. On the whole, it is governed by step 4. Indeed, the complexity of step 1 is O(n2) , n standing for the number of primitives (segments or arcs) of the environment. The computation of the different domains and of their kernels, which may attain a quadratic complexity in the worst case, may be linear when the environment complexity is locally bounded (i.e., if there is an environment paving such that the, number of primitives in each of the pavements is bounded by a constant). At worst, the computation of the propagation function reaches a oubio complexity (i.e., when the number of domains is in O(n) and when each domain kernel is in O(n)), but this complexity is less in practice (see the example presented). The complexity of step 4 depends on the number of the connected components of the domains D". Paradoxically the algorithm will be all the more efficient as the space is highly constrained. Indeed in a highly constrained space, the space of solutions is much reduced and this is most of the time reflected by a bonnectivity of the envisaged curvature domains.

8 Directions for use and extensions.

In a highly constrained space, it is not realistic to envisage an off-line planning of collision-free trajectories [Laumond 86]. The algorithm presented must be used to define strategies in order to negociate a particular turn. These strategies are instantiated on-line with the informations provided by the sensors.

we have presented here only a sketch algorithm. Step 4 requires more technical developments which are in progress. The extension of our approach to the case o.C a non-circular robot (except for an approximation approach which may prove highly efficient in practice) poses theoritical problems : how can a metric, defined in the confi~uration space R2.s1 (and not in its projection on R ) - and supporting the notion of shortest path - retain the Euclidean metric when projected onto real space ? The extension to the nD case poses non-trivial problems indeed, the canonicity property for shortest paths do not hold in the nD case when n>2.

Acknowledgments The original problem (a mobile robot is not a piano ! ) was set by G. Giralt and discussed with him. I am grateful to the anonymous referees for their helpful suggestions .

References.

[Chatila 86] "Mobile robot navigation space modeling and decisional processes." Robotics Research 3, Eds Faugeras ! Giralt, MIT Press .

[Giralt 84] "Mobile robots." in Robotics and Artificial Intelligence, NATO ASI, Serie F (11 ).

[Hershberger and Guibas 86] "An O(n2) shortest path algorithm for a non-rotating convex body." Technical note 14, Dig.i tal Systems Research Center, Palo Alto.

[Laumond 86] "Feasible trajectories for mobile robot with kinematic and environment constraints." Proceedings of Intelligent Autonomous Systems, Amsterdam, December 1986.

[Laumond 87] "Obstacle growing world." Information Processing in a non-polygonal Letters, 25 ( 1).

[Lozano-P6rez 83] "Spatial configuration space approach." Comp. , 32 ( 2) . planning a IEEE Trans. on

[Lozano-P6rez 86] Personal communication

[Whitakker 65] "Analytical and rigid bodies." Cambridge dynamics of particles University Press .

·r. ..

Figure 4c The corresponding solution in ACSP and in the environment .