Robot Motion Planning and Control - J.P. Laumond Part 4 pps

66 A. Bella~che, F. Jean and J J. Risler of Chow. We will denote by d the distance defined on M by means of vector fields X1, . . . , Xm. Let £1 = £1(X1, " ,Xm) be the set of linear combinations, with real coefficients, of the vector fields X1, , Xm. We define recursively £s = £s(X1, . . . , Xm) by setting Ls= £s-1 + It,eft iq-j=s for s = 2,3, , as well as L ° = 0. The union £ = £(XI, ,Xm ) of all £s is a Lie subalgebra of the Lie algebra of vector fields on M which is called the control Lie algebra associated to (E). Now, for p in M, let LS(p) be the subspace of TpM which consists of the values X(p) taken, at the point p, by the vector fields X belonging to £s. Chow's condition states that for each point p E M, there is a smallest integer r = r(p) such that L r(p) (p) = TpM. This integer is called the degree of nonholonomy at p. It is worth noticing that r(q) < r(p) for q near p. For each point p E M, there is in fact an increasing sequence of vector subspaces, or flag: {0} = L°(p) C LI(p) C , C LS(p) C,-" C Lr(P)(p) = TpM. We shall denote this flag by ~T(p). Points of the control system split into two categories: regular states, around which the behaviour of the system does not change in a qualitative way, and singular states, where some qualitative changes occur. Definition. We say that p is a regular point if the integers dimLS(q) (s = 1, 2, ) remain constant for q in some neighbourhood of p. Otherwise we say that p is a singular point. Let us give an example. Take M = R 2, and (0) X1 = ,X2 = x ~ (k is some integer). Then for c = (x,y) we have dimLX(c) = 1 if x = 0, dim L 1 (c) = 2 if x ~ 0, so all points on the line x = 0 are singular and the others are regular. For other examples, arising in the context of mobile robot with trailers, see Section 2. It is worth to notice that, when M and vector fields X1, ,Xm are an- alytic, regular points form an open dense set in M. Moreover, the sequence dim LS(p), s = 0, 1, 2, , is the same for all regular points in a same connected component of M and is streactly increasing for 0 < s < r(p). Thus the degree Geometry of Nonholonomic Systems 67 of nonholonomy at a regular point is bounded by n - m + 1 (if we suppose that no one of the Xi's is at each point a linear combination of the other vector fields). It may be easily computed when the definition of the Xi's allows sym- bolic computation, as for an analytic function, being non-zero at the formal level is equivalent to being non-zero at almost every point. Computing, or even bounding the degree of nonholonomy at singular points is much harder, and motivated, for some part, sections 2 and 3 (see also [9,11,19,24]). 1.7 Distance estimates and privileged coordinates Now, fix a point p in M, regular or singular. We set n8 = dim LS(p) (s = 0,1, ,r). Consider a system of coordinates centered at p, such that the differentials dyl, , dyn form a basis of T~,M adapted to Y(p) (we will see below how to build such coordinates). If r = 1 or 2, then it is easy to prove the following local estimate for the sub-Riemannian distance. For y closed enough to 0, we have d(0, (Yl, , Yn)) X lYl ]-'~-''" + lYnl I ''~ ]Ynl+l I 1/2 or''" q-[y.[ 1/2 (12) where nt = dim L l(p) (the notation f(y) × g(y) means that there exists con- stants c, C > 0 such that cg(y) < f(y) <_ Cg(y)). Coordinates yl, ,ym are said to be of weight 1, and coordinates Ynl+l, ,Yn are said to be of weight 2. In the general case, we define the weight wj as the smallest integer s such that dyj is non identically zero on LS(p). (So that wj = s if ns-1 < j < ns.) Then the proper generalization of (12) would be d(0,(yl, ,y~))xlYll 1/wl + +ly, l 1/~ (13) It turns out that this estimate is generically ]alse as soon as r > 3. A simple counter-example is given by the system (i) (°1) X1 = , X2= x 2 +y (14) on R 3. We have LI(0)=L2(0)=R 2×{0}, La(0)=R a, 68 A. Bellaiche, F. Jean and J J. Risler so that yl = x, y2 = Y, y3 = z are adapted coordinates and have weight 1, 1 and 3. In this case, the estimates (13) cannot be true. Indeed, this would imply Izl const.(d(0, (x,y, z)) 3, whence z (exp(tX2)p)] <const. t 3, but this is impossible since )h -~ z exp(tX2)(p) = (X~z)(p) = 1. t=O However a slight nonlinear change of coordinates allows for (13) to hold. It is sufficient to replace yl, y2, Y3 by zl = x, z2 = y, z3 = z - y2/2. In the above example, the point under consideration is singular, but one can give similar examples with regular p in dimension > 4. To formulate conditions on coordinate systems under which estimates like (13) may hold, we introduce some definitions. Call Xlf, ,Xmf the nonholonomic partial derivatives of order 1 of f relative to the considered system (compare to O~lf, ,Ox, f). Call further XiXjf, XiXjXkf, the nonholonomic derivatives of order 2, 3, of f. Proposition 1.4. For a smooth function f defined near p, the following con- ditions are equivalent: (i) One has f(q) = 0 (d(p, q)S) for q near p. (ii) The nonholonomic derivatives of order < s - 1 of f vanish at p This is proven by the same kind of computations as in the study of example (14). Definition. If Condition (i), or (ii), holds, we say that f is of order >__ s at p. Definition. We call local coordinates zl, , zn centered at p a system of privileged coordinates if the order of zj at p is equal to wj (j = 1, , n). If zl, ,zn are privileged coordinates, then dzl, ,dzn form a basis of TiM adapted to ~'(p). The converse is not true. Indeed, if dzl, ,dzn form an adapted basis, one can show that the order of zj is < wj, but it may be < wj: for the system (14), the order of coordinate ys = z at 0 is 2, while w3 = 3. To prove the existence, in an effective way, of privileged coordinates, we first choose vector fields Y1, , Y, whose values at p form a basis of TpM in the following way. Geometry of Nonholonomic Systems 69 First, choose among X1, , Xm a number nl of vector fields such that their values form a basis of Ll(p). Call them Y1, , Y,~I. Then for each s (s = 2, , r) choose vector fields of the form Y~.~. ,._ = [x~l, [x~, [x~._l, x~.] ]] (15) which form a basis of LS(p) mod LS-l(p), and call them Yn,_~+I, , ym. Choose now any system of coordinates yl, , yn centered at p such that the differentials dyl, ,dyn form a basis dual to YI(P), ,Yn~). (Starting from any system of coordinates xt, ,xn centered at p, one can obtain such a system Yt, , Y, by a linear change of coordinates.) Theorem 1.5. The functions zl, , zn recursively defined by Zq = yq - E 1 Zq_ 1 (~1[ aq-l! (Y~I "Y:Jl~Yq)(P) z?~ "q-~ (16) form a system of privileged coordinates near p. (We have set w(a) = wlal + • + wna ) The proof is based on the following lemma. Lemma 1.6. For a function f to be of order > s at p, it is necessary and sufficient that (Y~ . . . Yr'" f) (P) = 0 for all ~ = (at, ,an) such that wtat + + wnan <_ s. This is is an immediate consequence of the following, proved by J J. Risler [4]: any product Xi~Xi2 Xi., where it, , is are integers, can be rearranged as a sum of ordered monomials E c,~ ,. (xlY~ . . . Yg" with Wlal + + wnOln <~ 8, and where the ca~ a.'s are smooth functions. This result reminds of the Poincarfi-Birkhoff-Witt theorem. Observe that the coordinates zl, • , zn supplied by the construction of The- orem 1.5 are given from original coordinates by expressions of the form Zl = Yl z2 y~ + pol(y~) z, = Yn + pol(yl, ,Y,-I) 70 A. Bella~che, F. Jean and J J. Risler where pol denotes a polynomial, without constant or linear term, and that the reciprocal change of coordinates has exactly the same form. Other ways of getting privileged coordinates are to use the mappings (zl, ,zn) ~ exp(zlY1 + +znYn)p (see [14]), (zl, ,zn) ~-~ exp(znYn)'"exp(zlY1)p (see [18]). Following the usage in Lie group theory, such coordinates are called canonical coordinates of the first (resp. second) kind. 1.8 Ball-Box Theorem Using privileged coordinates, the control system (Z) may be rewritten near p as m (j= i=l where the functions fij are weighted homogeneous polynomials of degree wj - 1. By dropping the o(llzll ;), we get a control system (~) Zj-~- ~Ui[fij(Z1, ,Zj_l) ] (j = 1, ,n), i=1 or, in short, m by setting Xi = ~j~=l fij(2"l, , Zn)Ozj. This system is nilpotent and the vec- tor fields )(i are homogeneous of degree -1 under the non-isotropic dilations (zl, , zn) ~ (A~.lzl, , Aw~ zn). The system (~) is called the nitpotent ho- mogeneous approximation of the system (Z). For the sub-Riemaniann distance associated to the nilpotent approximation, the estimate (17) below can be shown by homogeneity arguments. The following theorem is then proved by comparing the distances d and d (for a detailed proof, see Bella'/che [2]). Theorem 1.7. The estimate d (O, (Zl, . . . , Zn) ) x 12"1t 1/w' -1-'" Jr Iznl x/wn (17) holds near p if and only if 2'1, , 2"n form a system of privileged coordinates at p. Geometry of Nonholonomic Systems 71 The estimate (17) of the sub-Riemannian distance allows to describe the shape of the accessible set in time ~. A(x, ~) can indeed be viewed as the sub- Riemannian ball of radius ~ and Theorem 1.7 implies A(x,e) × [_e~1,¢~1] × × [_Ew.,e~.]. Then A(x, ~) looks like a box, the sides of the box being of length proportionnal to cu'~, , ew'. By the fact, Theorem 1.7 is called the Ball-Box Theorem (see Gromov [16]). 1.9 Application to complexity of nonholonomic motion planning The Ball-Box Theorem can be used to address some issues in complexity of motion planning. The problem of nonholonomic motion planning with obstacle avoidance has been presented in Chapter [Laumond-Sekhavat]. It can be for- mulated as follows. Let us consider a nonholonomic system of control in the form (Z). We assume that Chow's Condition is satisfied. The constraints due to the obstacles can be seen as closed subsets F of the configuration space M. The open set M - F is called the free space. Let a, b E M - F. The motion planning problem is to find a trajectory of the system linking a and b contained in the free space. From Chow's Theorem (§1.4), deciding the existence of a trajectory linking a and b is the same thing as deciding if a and b are in the same connected component of M - F. Since M - F is an open seL the connexity is equivalent to the arc connexity. Then the problem is to decide the existence of a path in M - F linking a and b. In particular this implies that the decision part of the motion planning problem is the same for nonholonomic controllable systems as for holonomic ones. For the complete problem, some algorithms are presented in Chapter [Lanmond-Sekhavat]. In particular we see that there is a general method (called "Approximation of a collision-free holonomic path"). It consists in dividing the problem in two parts: - find a path in the free space linking the configurations a and b (this path is called also the collision-free holonomic path); - approximate this path by a trajectory of the system close enough to be contained in the free space. The existence of a trajectory approximating a given path can be shown as follows. Choose an open neighbourhood U of the holonomic path small enough to be contained in M - F. We can assume that U is connected and then, from Chow's Theorem, there is a trajectory lying in U and linking a and b. 72 A. Bella'iche, F. Jean and J J. Risler What is the complexity of this method? The complexity of the first part (i.e., the motion planning problem for holonomic systems) is very well modeled and understood. It depends on the geometric complexity of the environment, that is on the complexity of the geometric primitives modeling the obstacles and the robot in the real world (see [6,30]). The complexity of the second part requires more developments. It can be seen actually as the "complexity" of the output trajectory. We have then to define this complexity for a trajectory approximating a given path. Let 7 be a collision-free path (provided by solving the first part of the problem). For a given p, we denote by Tube(% p) the reunion of the balls of radius p centered at q, for any point q of 7. Let e be the biggest radius p such that Tube(y, p) is contained in the free space. We call e the size of the free space around the path 7. The output trajectories will be the trajectories following 7 to within e, that is the trajectories contained in Tube(% e). Let us assume that we have already defined a complexity a(c) of a trajectory c. We denote by a(7, e) the infimum of a(c) for c trajectory of the system linking a and b and contained in Tube(7, s). a(7, e) gives a complexity of an output trajectory. Thus we can choose it as a definition of the complexity of the second part of our method. It remains to define the complexity of a trajectory. We will present here some possibilities. Let us consider first bang-bang trajectories, that is trajectories obtained with controls in the form (ul, ,Um) = (0, ,:t:1, ,0). For such a tra- jectory the complexity a(c) can be defined as the number of switches in the controls associated to c. We will now extend this definition to any kind of trajectory. Following [3], a complexity can be derived from the topological complexity of a real- valued function (i.e., the number of changes in the sign of variation of the function). The complexity a(c) appears then as the total number of sign changes for all the controls associated to the trajectory c. Notice that, for a bang- bang trajectory, this definition coincides with the previous one. We will call topological complexity the complexity at(7, ~) obtained with this definition. Let us recall that the complexity of an algorithm is the number of elemen- tary steps needed to get the result. For the topological complexity, we have chosen as elementary step the construction of a piece of trajectory without change of sign in the controls (that is without manoeuvring, if we think to a car-like robot). Geometry of Nonholonomic Systems 73 Another way to define the complexity is to use the length introduced in §1.3 (see Formula (4)). For a trajectory c contained in Tube(7, ~), we set length(c) o (c) - g and we call metric complexity the complexity am(V, ~) obtained with aE(c). Let us justify this definition on an example. Consider a path 7 such that, for any q E 7 and any i E {1, ,m}, the angle between Tq7 and Xi(q) is greater than a given 0 ~ 0. Then, for a bang-bang trajectory without switches contained in Tube(7, ~), the length cannot exceed ~/sin 0. Thus, the number of switches in a bang-bang trajectory (C Tube(7, ~)) is not greater than the length of the trajectory divided by ~ (up to a constant). This links ae(c) and am(7, ~) to the topological complexity. Let us give an estimation of these complexities for the system of the car-like robot (see Chapter [Laumond-Sekhavat]). The configurations are parametrized by q = (x, y, ~)T E R 2 × 81 and the system is given by: ~=ulXl+u2X2, with XI= ~si00 ), X2= • It is well-known that, for all q E R 2 × S 1, the space Le(q) has rank 3 (see Section 2). Let us consider a non-feasible path 7 C R 2 x 31. When 7 is C 1 and its tangent vector is never in Ll(q), one can link the complexity am(V,E) to the number of e-balls needed to cover 7. By the Ball-Box Theorem (§1.8), this number is greater than Kc -2, where the constant depends on 7. More precise results have been proven by F. Jean (see also [22] for weaker estimates). Let T(q) (I]TH = 1) be the tangent vector to 7. Assume that T(q) belongs to L2(q) - Ll(q) almost everywhere and that 7 is parametrized by its arclength s. Then we have, for small ~ ~ 0: // at(V,e) and am(7, s) × e -2 det(X1,X2,T)(7(s)) ds (let us recall that the notation a(7, ~) × f(7, ~) means that there exist c, C > 0 independant on 7 and e such that c](7, e) < a(7, e) < C](7, e)). 2 The car with n trailers 2.1 Introduction This section is devoted to the study of an example of nonholonomic control system: the car with n trailers. This system is nonholonomic since it is subject 74 A. Bella~che, F. Jean and J J. Risler to non integrable constraints, the rolling without skiding of the wheels. The states of the system are given by two planar coordinates and n + 1 angles: the configuration space is then R 2 x (S 1)n+1, a (n + 3)-dimensional manifold. There are only two inputs, namely one tangential velocity and one angular velocity which represent the action on the steering wheel and on the accelerator of the car. Historically the problem of the car is important, since it is the first non- holonomic system studied in robotics. It has been intensively treated in many papers throughout the litterature, in particular from the point of view of find- ing stabilizing control laws: see e.g. Murray and Sastry ([25]), Fliess et al. ([8]), Laumond and Risler ([23])• We are interested here in the properties of the control system (see below §2.2). The first question is indeed the controllability. We will prove in §2.4 that the system is controllable at each point of the configuration space. The second point is the study of the degree of nonholonomy. We will give in §2.6 an upper bound which is exponential in terms of the number of trailers. This bound is the sharpest one since it is a maximum. We give also the value of the degree of nonholonomy at the regular points (§2.5). The last problem is the singular locus. We have to find the set of all the singular points (it is done in §2.5) and also to determinate its structure. We wilt see in §2.7 that one has a natural stratification of the singular locus related to the degree of nonholonomy. 2.2 Equations and notations Different representations have been used for the car with n trailers. The problem is to choose the variables in such a way that simple induction relation may appear. The kinematic model introduced by Fliess [8] and Scrdalen [33] satisfies this condition. A car in this context will be represented by two driving wheels connected by an axle. The kinematic model of a car with two degrees of freedom pulling n trailers can be given by: :~ = COS OOVO, = sin Oovo, /~o = sin(01 - Oo) ~, vl/ ri+l ' 0 1 = sin(O. - O 1)k, ~n. = 02, (18) Geometry of Nonholonomic Systems 75 where the two inputs of the system are the angular velocity w of the car and its tangential velocity v = vn. The state of the system is parametrized by q = (x,y, O0, ,On) T where: - (x, y) are the coordinates of the center of the axle between the two wheels of the last trailer, - On is the orientation angle of the pulling car with respect to the x-axis, - 8i, for 0 < i < n - 1, is the orientation angle of the trailer (n - i) with respect to the x-axis. Finally ri is the distance from the wheels of trailer n - i + 1 to the wheels of trailer n - i, for 1 < i < n - 1, and rn is the distance from the wheels of trailer 1 to the wheels of the car. The point of this representation is that the system is viewed from the last trailer to the car: the numbering of the angles is made in this sense and the position coordinates are those of the last trailer. The converse notations would be more natural but unfortunately it would lead to complicated computations. The tangential velocity vi of the trailer n - i is given by: or vi =fiv where n j=i+l = fl cos(0j - 0j-l). j=i+l The motion of the system is then characterized by the equation: (t = (q) + vX2(q) with the control system {X1, X2) given by: Xx = X2 = cos Oo fo ) sin 8o fo ¼ sin(O,, - 0 [...]... length not exceeding 2 k-2 This implies Xk(O) e L2k-2(Z)(O) (this is the same reasoning as in the proof of Theorem 2.1) We have then x~(O) E L2 ~-~ (z~)(O) and, Vk, ~k(O) E L2 .-~ +~2 .-3 (Z)(O) Since det(~l, , ~ ) ~ 0, the subspace L2 ~-2 +v2 .-3 (Z)(O) is of dimension n and then n-b3 r(O) < 2 ~-2 (1 + 2~n( ~-2 )-2 d 2n ~ k 4 k2n) 90 A Bellaiche, F Jean and J.-J Risler References 1 A A Agrachev and R V Gamkrelidze,... RS, r = degR, s = degS, q = r + s, then r(r+ d - 1 ) (r + ( n - 1 ) ( d - 1))+ s(s + d - 1 ) - ( s + ( n - 1 ) ( d - 1 ) ) + 2 ( n - 1) < q(q+ d - 1 ) ( q + ( n - 1 ) ( d - 1)) + n - 1 which is immediate by induction on n If A is a C-algebra, let us denote by dim A its dimension as a ring (it is its "Krull dimension"), and dime A its dimension as a C-vector space If A is an analytic algebra, i.e.,... representation of flows and the chronological calculus," Mat Sbornik (N.S.), 107 ( 149 ), 46 7-5 32, 639, 1978 English transl.: Math USSR Sbornik, 35, 72 7-7 85, 1979 2 A Bella'iche, "The tangent space in sub-Riemannian Geometry," in SubRiemannian Geometry, A Bellaiche and J.-J Risler Ed., Progress in Mathematics, Birkh~user, 1996 3 A Beltaiche, J.-P Laumond and J Jacobs, "Controllability of car-like robots and complexity... and complexity of the motion planning problem," in International Symposium on Intelligent Robotics, 32 2-3 37, Bangalore, India, 1991 4 A Bella~che, J.-P Laumond and J.-J Risler, "Nilpotent infinitesimal approximations to a control Lie algebra," in Proceedings of the IFAC Nonlinear Control Systems Design Symposium, 17 4- 1 81, Bordeaux, France, June 1992 5 R W Brockett, "Control theory and singular Riemannian... at = 7r/2 and aT = arctansinaT_l Vq E R 2 × (S1) n+l, for 2 < i < n + 3 , / ~ ( q ) is streactly increasing with respect to i and can be computed, for i e {3, n + 3}, by the following induction formulae: I if On - 0n-1 = ± ~ , then Z~(q) = Z~_ (qt) + ZT:~(q~) -1 2 i f 2 p E [1, n - 2 ] and e = ±1 such that Ok Ok-1 = eak-p for every k E { p + 1,n}, then ~.~(q) n-1 t n-2 2 = 2~_~ (q ) - ~i-2 ( q ) 3... Hilton and G J Young Ed., SpringerVertag, 1982 6 J F Canny, The complexity of robot motion planning, MIT Press, 1998 7 W L Chow, "Uber Systeme von linearen partiellen Differentialgleichungen erster Ordnung," Math Ann., 117, 9 8-1 15, 1 940 8 M Fliess, J Levine, P Martin and P Rouchon, "On differential flat nonlinear systems," in Proceedings of the IFA C Non linear Control Systems Design Symposium, 40 8 -4 12,... following results: - 4- ~ , then the first independant brackets are X1 (q), IX1, x2l(q), IX2, x l](q) and [X2, IX2, [X1, x2lll(q); if 05 - 01 = 4- ~ , then the first independant brackets are X1 (q), [Xx, X~](q), [X2, IX1, X2l](q) and [Xl[X2, [X~, IX1, X~]]]](q) if 02 - 01 # X2(q), X2(q), Thus the car with 2 trailers is also controllable since, in both cases, the subspaee L5 (X1, X2)(q) is 5-dimensional However... of ring homomorphisms: C{Xl, ,X,} c{x~ .x~} (Q,QI, Q~-I) - ~" 2 c{t} ~* ~ - Geometry of Nonholonomic Systems 85 where the vertical arrows represent the canonical maps Since V* is surjective, we have also that 9" is surjective This implies that c{t} c{xl, ,x,} u - n + 1 = dimc (t,_n+l) < dimc (Q, ,Qn-1) . problem in two parts: - find a path in the free space linking the configurations a and b (this path is called also the collision-free holonomic path); - approximate this path by a trajectory. generic sequence (j3 ~(q)) and the non generic points are: 82 A. Bellaiche, F. Jean and J J. Risler - either in the intersection with another hyperplane 0j - 0j- 1 = =t:~ which corresponds to the. the problem is to decide the existence of a path in M - F linking a and b. In particular this implies that the decision part of the motion planning problem is the same for nonholonomic controllable