Document not found! Please try again

Motion Planning for Kinematic systems - arXiv

4 downloads 0 Views 1MB Size Report
Mar 26, 2012 - Nicolas Boizot and Jean-Paul Gauthier. Abstract—In ...... [11] J.P. Gauthier, V. Zakalyukin, Robot Motion Planning, a wild case,. Proceedings of ...
1

Motion Planning for Kinematic systems

arXiv:1203.5623v1 [math.OC] 26 Mar 2012

Nicolas Boizot and Jean-Paul Gauthier

Abstract—In this paper, we present a general theory of motion planning for kinematic systems. This theory has been developed for long by one of the authors in a previous series of papers. It is mostly based upon concepts from subriemannian geometry. Here, we summarize the results of the theory, and we improve on, by developping in details an intricated case: the ball with a trailer, which corresponds to a distribution with flag of type 2,3,5,6. This paper is dedicated to Bernard Bonnard for his 60th birthday.

VI

Index Terms—Optimal control, Subriemannian geometry, robotics, motion planning

References

I

Introduction

1

II

Basic concepts II-A Entropy versus metric complexity . . . II-B Normal coordinates . . . . . . . . . . . II-C Normal forms, Nilpotent approximation along Γ . . . . . . . . . . . . . . . . . . II-C1 Frames . . . . . . . . . . . . II-C2 The general normal form . . II-C3 Two-step bracket-generating case . . . . . . . . . . . . . . II-C4 The 2-control case . . . . . .

3 3 3

III

IV

Results III-A III-B III-C III-D

4 4 4 4 4

Normal forms . . . . . . . . Invariants in the 6-2 case,

5

and the ball with a trailer . .

5

General results . . . . . . . . . . . . . Generic distribution in R3 . . . . . . The one-step bracket-generating case . The 2-control case, in R4 and R5 . . .

The ball with a trailer

VI-A VI-B

VI-C

11

Appendix 1: Normal form in the 6-2 case 11 Appendix 2: Plane curves whose curvature is a function of the distance to the origin . . . . . . . . . . . . . . . . . . . 11 Appendix 3: periodicity of the optimal curves in the 6-2 case . . . . . . . . . 11 12 I. I NTRODUCTION

C ONTENTS

II-C5 II-C6

Appendix

. . . .

6 6 6 7 8 9

Here we present the main lines of a theory of motion planning for kinematic systems, which is developped for about ten years in the papers [8], [9], [10], [11], [12], [13], [14]. One of the purposes of the paper is to survey the whole theory disseminated in these papers. But also we improve on the theory, by treating one more case, in which ”the fourth order brackets are involved”. We aslo improve on several previous results (periodicity of our optimal trajectories for instance). Potential application of this theory is motion planning for kinematic robots. We will show several basic examples here. The theory starts from the seminal work of F. Jean, in the papers [16], [17], [18]. At the root of this point of view in robotics, there are also more applied authors like J.P. Laumond [20]. See also [25]. We consider kinematic systems that are given under the guise of a vector-distribution ∆ over a n-dimensional manifold M . The rank of the distribution is p, and the corank k = n−p. Motion planning problems will aways be local problems in an open neighborhood of a given finite path Γ in M. Then we may always consider that M = Rn . From a control point of view, a kinematic system can be specified by a control system, linear in the controls, typically denoted by Σ: (Σ) x˙ =

p X Fi (x)ui ,

(1)

i=1

where the Fi ’s are smooth (C ∞ ) vector fields that span V Expectations and conclusions 10 the distribution ∆. The standard controllability assumption is always assumed, i.e. the Lie algebra generated by the Fi ’s is V-A Universality of some pictures in normal coordinates . . . . . . . . . . . . . . . . 10 transitive on M. Consequently, the distribution ∆ is completely V-B Robustness . . . . . . . . . . . . . . . . 10 nonintegrable, and any smooth path Γ : [0, T ] → M can be unifomly approximated by an admissible path γ : [0, θ] → M , V-C The practical importance of normal coordinates . . . . . . . . . . . . . . . . . 10 i.e. a Lipschitz path, which is almost everywhere tangent to V-D Final conclusion . . . . . . . . . . . . . 11 ∆, i.e., a trajectory of (1). This is precisely the abstract answer to the kinematic moThe authors are with LSIS, laboratoire des sciences de l’information et des tion planning probem: it is possible to approximate uniformly syst`emes, UMR CNRS 6168, Domaine universitaire de Saint J´erˆome, Avenue Escadrille Normandie Niemen, 13397 MARSEILLE Cedex 2, France. J.P. nonadmissible paths by admissible ones. The purpose of this Gauthier is also with INRIA team GECO. paper is to present a general constructive theory that solves c 2012 0000–0000/00$00.00this

problem IEEE in a certain optimal way.

2

More precisely, in this class of problems, it is natural to try to minimize a cost of the following form: v Zθ u p uX J(u) = t (ui )2 dt, 0

i=1

for several reasons: 1. the optimal curves do not depend on their parametrization, 2. the minimization of such a cost produces a metric space (the associated distance is called the subriemannian distance, or the Carnot-Caratheodory distance), 3. Minimizing such a cost is equivalent to minimize the following (called the energy of the path) quadratic cost JE (u), in fixed time θ: Zθ X p (ui )2 dt. JE (u) = 0 i=1

The distance is defined as the minimum length of admissible curves connecting two points, and the length of the admissible curve corresponding to the control u : [0, θ] → M is just J(u). In this presentation, another way to interpret the problem is as follows: the dynamics is specified by the distribution ∆ (i.e. not by the vector fields Fi , but their span only). The cost is then determined by an Euclidean metric g over ∆, specified here by the fact that the Fi ’s form an orthonormal frame field for the metric. At this point we would like to make a more or less philosophical comment: there is, in the world of nonlinear control theory, a permanent twofold critic against the optimal control approach: 1. the choice of the cost to be minimized is in general rather arbitrary, and 2. optimal control solutions may be non robust. Some remarkable conclusions of our theory show the following: in reasonable dimensions and codimensions, the optimal trajectories are extremely robust, and in particular, do not depend at all (modulo certain natural transformations) on the choice of the metric, but on the distribution ∆ only. Even stronger: they depend only on the nilpotent approximation along Γ (a concept that will be defined later on, which is a good local approximation of the problem). For a lot of low values of the rank p and corank k, these nilpotent approximations have no parameter (hence they are in a sense universal). The asymptotic optimal sysntheses (i.e. the phase portraits of the admissible trajectories that approximate up to a small ε) are also universal. Given a motion planning problem, specified by a (nonadmissible) curve Γ, and a Subriemannian structure (1), we will consider two distinct concepts, namely: 1. The metric complexity M C(ε) that measures asymptotically the length of the best ε-approximating admissible trajectories, and 2. The interpolation entropy E(ε), that measures the length of the best admissible curves that interpolate Γ with pieces of length ε. The first concept was introduced by F. Jean in his basic paper [16]. The second concept is closely related with the entropy of F. Jean in [17], which is more or less the same as the Kolmogorov’s entropy of the path Γ, for the metric

structure induced by the Carnot-Caratheodory metric of the ambient space. Also, along the paper, we will deal with generic problems only (but generic in the global sense, i.e. stable singularities are considered). That is, the set of motion planning problems on Rn is the set of couples (Γ, Σ), embedded with the C ∞ topology of uniform convergence over compact sets, and generic problems (or problems in general position) form an open-dense set in this topology. For instance, it means that the curve Γ is always tranversal to ∆ (except maybe at isolated points, in the cases k = 1 only). Another example is the case of a surface of degeneracy of the Lie bracket distribution [∆, ∆] in the n = 3, k = 1 case. Generically, this surface (the Martinet surface) is smooth, and Γ intersects it transversally at a finite number of points only. Also, along the paper, we will illustrate our results with one of the following well known academic examples: Example 1: the unicycle: x˙ = cos(θ)u1 , y˙ = sin(θ)u1 , θ˙ = u2

(2)

Example 2: the car with a trailer: x˙ = cos(θ)u1 , y˙ = sin(θ)u1 , θ˙ = u2 , ϕ˙ = u1 − sin(ϕ)u2 (3) Example 3: the ball rolling on a plane:  0 0 0 x˙ = u1 , y˙ = u2, R˙ =  0 −u1 −u2

 u1 u2  R, 0

(4)

where (x, y) are the coordinates of the contact point between the ball and the plane, R ∈ SO(3, R) is the right orthogonal matrix representing an othonormal frame attached to the ball. Example 4: the ball with a trailer   0 0 u1 0 u2  R, (5) x˙ = u1 , y˙ = u2, R˙ =  0 −u1 −u2 0 1 θ˙ = − (cos(θ)u1 + sin(θ)u2 ). L Typical motion planning problems are: 1. for example (2), the parking problem: the non admissible curve Γ is s → (x(s), y(s), θ(s), ϕ(s)) = (s, 0, π2 , 0), 2. for example (3), the full rolling with slipping problem, Γ : s → (x(s), y(s), R(s)) = (s, 0, Id), where Id is the identity matrix. On figures 1, 2 we show our approximating trajectories for both problems, that are in a sense universal. In figure 1, of course, the x-scale is much larger than the y-scale.

Up to now, our theory covers the following cases: (C1) The distribution ∆ is one-step bracket generating (i.e. dim([∆, ∆] = n) except maybe at generic singularities, (C2) The number of controls (the dimension of ∆) is p = 2, and n ≤ 6. The paper is organized as follows: In the next section II, we introduce the basic concepts, namely the metric complexity,

3

Fig. 1.

Parking of the car with a trailer

endpoints Γ(0), Γ(T ) of Γ, and remaining in the tube Tε . Definition 6: The interpolation entropy E(ε) of P is 1ε times the minimum length of an admissible curve γε connecting the endpoints Γ(0), Γ(T ) of Γ, and ε-interpolating Γ, that is, in any segment of γε of length ≥ ε, there is a point of Γ. These quantities M C(ε), E(ε) are functions of ε which tends to +∞ as ε tends to zero. They are considered up to equivalence.The reason to divide by ε is that the second quantity counts the number of ε-balls to cover Γ, or the number of pieces of length ε to interpolate the full path. This is also the reason for the name ”entropy”. Definition 7: An asymptotic optimal synthesis is a oneparameter family γε of admissible curves, that realizes the metric complexity or the entropy. Our main purpose in the paper is twofold: 1. We want to estimate the metric complexity and the entropy, in terms of certain invariants of the problem. Actually, in all the cases treated in this paper, we will give eplicit formulas. 2. We shall exhibit explicit asymptotic optimal syntheses realizing the metric complexity or/and the entropy.

B. Normal coordinates Take a parametrized p-dimensional surface S, transversal to ∆ (maybe defined in a neighborhood of Γ only), S = {q(s1 , ..., sp−1 , t) ∈ Rn }, with q(0, ..., 0, t) = Γ(t). Fig. 2.

Approximating rolling with slipping

the interpolation entropy, the nilpotent approximation along Γ, and the normal coordinates, that will be our basic tools. Section III summarizes the main results of our theory, disseminated in our previous papers, with some complements and details. Section IV is the detailed study of the case n = 6, k = 4, which corresponds in particular to example 4, the ball with a trailer. In Section V, we state a certain number of remarks, expectations and conclusions. II. BASIC CONCEPTS In this section, we fix a generic motion planning problem P =(Γ, Σ). Also, along the paper there is a small parameter ε (we want to approximate up to ε), and certain quantities f (ε), g(ε) go to +∞ when ε tends to zero. We say that such (ε) = 1. Also, quantities are equivalent (f ' g) if limε→0 fg(ε) d denotes the subriemannian distance, and we consider the ε-subriemammian tube T ε and cylinder Cε around Γ : Tε = {x ∈ M | d(x, Γ) ≤ ε}, Cε = {x ∈ M | d(x, Γ) = ε}. A. Entropy versus metric complexity Definition 5: The metric complexity M C(ε) of P is 1ε times the minimum length of an admissible curve γε connecting the

Such a germ exists if Γ is not tangent to ∆. The exclusion of a neighborhood of an isolated point where Γ is tangent to ∆, (that is Γ becomes ”almost admissible”), will not affect our estimates presented later on (it will provide a term of higher order in ε). . In the following, CεS will denote the cylinder {ξ; d(S, ξ) = ε}. Lemma 8: (Normal coordinates with respect to S). There are mappings x : Rn → Rp , y : Rn → Rk−1 , w : Rn → R, such that ξ = (x, y, w) is a coordinate system on some neighborhood of S in Rn , such that: 0. S(y, w) = (0, y, w), Γ = {(0, 0, w)} 1. The restriction ∆|S = ker dw ∩i=1,..k−1 ker dyi , the Pp metric g|S = i=1 (dxi )2 , Pp 2. CεS = {ξ| i=1 xi 2 = ε2 }, 3. geodesics of the Pontryagin’s maximum principle ([21]) meeting the transversality conditions w.r.t. S are the straight lines through S, contained in the planes Py0 ,w0 = {ξ|(y, w) = (y0 , w0 )}. Hence, they are orthogonal to S. These normal coordinates are unique up to changes of coordinates of the form x ˜ = T (y, w)x, (˜ y , w) ˜ = (y, w), where T (y, w) ∈ O(p), the p-orthogonal group.

(6)

4

C. Normal forms, Nilpotent approximation along Γ 1) Frames: Let us denote by F = (F1 , ..., Fp ) the orthonormal frame of vector fields generating ∆. Hence, we will also write P = (Γ, F ). If a global coordinate system (x, y, w), not necessarily normal, is given on a neighborhood of Γ in Rn , with x ∈ Rp , y ∈ Rk−1 , w ∈ R, then we write: Fj =

p X

k−1

Qi,j (x, y, w)

i=1

+ Mj (x, y, w)

X ∂ ∂ + Li,j (x, y, w) ∂xi i=1 ∂yi

(7)

∂ , ∂w

j = 1, ..., p. Hence, the SR metric is specified by the triple (Q, L, M) of smooth x, y, w-dependent matrices. 2) The general normal form: Fix a surface S as in Section ?? and a normal coordinate system ξ = (x, y, w) for a problem P. Theorem 9: (Normal form, [2]) There is a unique orthonormal frame F = (Q, L, M) for (∆, g) with the following properties: 1. Q(x, y, w) is symmetric, Q(0, y, w) = Id (the identity matrix), 2. Q(x, y, w)x = x, 3. L(x, y, w)x = 0, M(x, y, w)x = 0. 4. Conversely if ξ = (x, y, w) is a coordinate system satisfying conditions 1, 2, 3 above, then ξ is a normal coordinate system for the SR metric defined by the orthonormal frame F with respect to the parametrized surface {(0, y, w)}. Clearly, this normal form is invariant under the changes of normal coordinates (6). Let us write: Q(x, y, w) = Id + Q1 (x, y, w) + Q2 (x, y, w) + ..., L(x, y, w) = 0 + L1 (x, y, w) + L2 (x, y, w) + ..., M(x, y, w) = 0 + M1 (x, y, w) + M2 (x, y, w) + ..., where Qr , Lr , Mr are matrices depending on ξ = (x, y, w), the coefficients of which have order k w.r.t. x (i.e. they are in the rth power of the ideal of C ∞ (x, y, w) generated by the functions xr , r = 1, ..., n − p). In particular, Q1 is linear in x,PQ2 is quadratic, etc... Set u = k−1 (u1 , ..., up ) ∈ Rp . Then j=1 L1j (x, y, w)uj = L1,y,w (x, u) is quadratic in (x, u), and Rk−1 -valued. Its ith component is the quadratic expression denoted by L1,i,y,w (x, u). Similarly Pk−1 j=1 M1j (x, y, w)uj = M1,y,w (x, u) is a quadratic form in (x, u). The corresponding matrices are denoted by L1,i,y,w , i = 1, ..., k − 1, and M1,y,w . The following was proved in [2], [5] for corank 1: Proposition 10: 1. Q1 = 0, 2. L1,i,y,w , i = 1, ..., p − 1, and M1,y,w are skew symmetric matrices. A first useful very rough estimate in normal coordinates is the following: Proposition 11: If ξ = (x, y, w) ∈ Tε , then: ||x||2 ≤ ε, ||y||2 ≤ kε2 ,

for some k > 0. At this point, we shall split the problems under consideration into two distinct cases: first the 2-step bracket-generating case, and second, the 2-control case. 3) Two-step bracket-generating case: In that case, we set, in accordance to Proposition 11, that x has weight 1, and the ∂ have yi ’s and w have weight 2. Then, the vector fields ∂x i ∂ , ∂ have weight −2. weight -1, and ∂y i ∂w Inside a tube Tε , we write our control system as a term of order -1, plus a residue, that has a certain order w.r.t. ε. Here, O(εk ) means a smooth term bounded by cεk . We have, for a trajectory remaining inside Tε :

x˙ = u + O(ε2 ); (1) 1 y˙ i = x0 Li (w)u + O(ε2 ); i = 1, ..., k − 1; 2 1 w˙ = x0 M (w)u + O(ε2 ), 2

(8)

where Li (w), M (w) are skew-symmetric matrices depending smoothly on w. Remark 12: In 8, (1), the term O(ε2 ) can seem surprising. One should wait for O(ε). It is due to (1) in Proposition 10. In that case, we define the Nilpotent Approximation Pˆ along Γ of the problem P by keeping only the term of order -1:

ˆ (P)

x˙ = u; 1 y˙ i = x0 Li (w)u; 2 1 w˙ = x0 M (w)u. 2

(9) i = 1, ..., p − 1;

ˆ of P and Pˆ correspondConsider two trajectories ξ(t), ξ(t) ing to the same control u(t), issued from the same point on Γ, and both arclength-parametrized (which is equivalent to ||u(t)|| = 1). For t ≤ ε, we have the following estimates:

||x(t)−ˆ x(t)|| ≤ cε3 , ||y(t)−ˆ y (t)|| ≤ cε3 , ||w(t)−w(t)|| ˆ ≤ cε3 , (10) for a suitable constant c. ˆ Remark 13: It follows that the distance (either d or d-the distance associated with the nilpotent approximation) between ˆ is smaller than ε1+α for some α > 0. ξ(t), ξ(t) This fact comes from the estimate just given, and the standard ball-box Theorem ([15]). It will be the key point to reduce the motion planning problem to the one of its nilpotent approximation along Γ. 4) The 2-control case:

5

5) Normal forms: In that case, we have the following general normal form, in normal coordinates. It was proven first in [1], in the corank1 case. The proof holds in any corank, without modification. Consider Normal coordinates with respect to any surface S. There are smooth functions, β(x, y, w), γi (x, y, w), δ(x, y, w), such that P can be written as (on a neighborhood of Γ):

x˙ 1 = (1 + (x2 )2 β)u1 − x1 x2 βu2 ,

(11)

2

x˙ 2 = (1 + (x1 ) β)u2 − x1 x2 βu1 , x1 x2 x1 x2 y˙ i = γi ( u1 − u2 ), w˙ = δ( u1 − u2 ), 2 2 2 2 where moreover β vanishes on the surface S. The following normal forms can be obtained, on the tube Tε , by just changing coordinates in S in certain appropriate way. It means that a trajectory ξ(t) of P remaining in Tε satisfies: Generic 4 − 2 case (see [12]): x˙ 1 = u1 + 0(ε3 ), x˙ 2 = u2 + 0(ε3 ), x1 x2 y˙ = ( u1 − u2 ) + O(ε2 ), 2 2 x2 x1 w˙ = δ(w)x1 ( u1 − u2 ) + O(ε3 ). 2 2 We define the nilpotent approximation as: x2 x1 (Pˆ4,2 ) x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), 2 2 x2 x1 w˙ = δ(w)x1 ( u1 − u2 ). 2 2 ˆ Again, we consider two trajectories ξ(t), ξ(t) of P and ˆ P corresponding to the same control u(t), issued from the same point on Γ, and both arclength-parametrized (which is equivalent to ||u(t)|| = 1). For t ≤ ε, we have the following estimates:

||x(t)−ˆ x(t)|| ≤ cε4 , ||y(t)−ˆ y (t)|| ≤ cε3 , ||w(t)−w(t)|| ˆ ≤ cε4 . (12) ˆ between Which implies that, for t ≤ ε, the distance (d or d) ˆ is less than ε1+α for some α > 0, and this will ξ(t) and ξ(t) be also the keypoint to reduce our problem to the Nilpotent approximation. Generic 5 − 2 case (see [13]): x˙ 1 = u1 + 0(ε3 ), x˙ 2 = u2 + 0(ε3 ), x2 x1 y˙ = ( u1 − u2 ) + O(ε2 ), 2 2 x2 x1 z˙ = x2 ( u1 − u2 ) + O(ε3 ), 2 2 x2 x1 w˙ = δ(w)x1 ( u1 − u2 ) + O(ε3 ). 2 2 We define the nilpotent approximation as: x2 x1 (Pˆ5,2 ) x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), 2 2 x2 x1 z˙ = x2 ( u1 − u2 ), 2 2 x2 x1 w˙ = δ(w)x1 ( u1 − u2 ). 2 2

The estimates necessary to reduce to Nilpotent approximation are: ||x(t) − x ˆ(t)|| ≤ cε4 , ||y(t) − yˆ(t)|| ≤ cε3 , 4

(13)

4

||z(t) − zˆ(t)|| ≤ cε , ||w(t) − w(t)|| ˆ ≤ cε . Generic 6 − 2 case (proven in Appendix): x˙ 1 = u1 + 0(ε3 ), x˙ 2 = u2 + 0(ε3 ), (14) x1 x2 y˙ = ( u1 − u2 ) + O(ε2 ), 2 2 x2 x1 z˙1 = x2 ( u1 − u2 ) + O(ε3 ), 2 2 x2 x1 z˙2 = x1 ( u1 − u2 ) + O(ε3 ), 2 2 x1 x2 w˙ = Qw (x1 , x2 )( u1 − u2 ) + O(ε4 ), 2 2 where Qw (x1 , x2 ) is a quadratic form in x depending smoothly on w. We define the nilpotent approximation as: x1 x2 (15) (Pˆ6,2 ) x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), 2 2 x2 x1 x2 x1 z˙1 = x2 ( u1 − u2 ), z˙2 = x1 ( u1 − u2 ), 2 2 2 2 x2 x1 w˙ = Qw (x1 , x2 )( u1 − u2 ). 2 2 The estimates necessary to reduce to Nilpotent approximation are: ||x(t) − x ˆ(t)|| ≤ cε4 , ||y(t) − yˆ(t)|| ≤ cε3 , 4

(16)

5

||z(t) − zˆ(t)|| ≤ cε , ||w(t) − w(t)|| ˆ ≤ cε . In fact, the proof given in Appendix, of the reduction to this normal form, contains the other cases 4-2 and 5-2. 6) Invariants in the 6-2 case, and the ball with a trailer: Let us consider a one form ω that vanishes on ∆00 = [∆, [∆, ∆]]. Set α = dω|∆ , the restriction of dω to ∆. Set H = [F1 , F2 ], I = [F 1, H], J = [F2 , H], and consider the dω(F1 , I) dω(F2 , I) 2 × 2 matrix A(ξ) = . dω(F1 , J) dω(F2 , J) Due tu Jacobi matrix. It is  Identity, A(ξ) is a symmetric  ω([F1 , I]) ω([F2 , I]) also equal to , using the fact that ω([F1 , J]) ω([F2 , J]) 00 ω([X, Y ]) = dω(X, Y ) in restriction to ∆ . Let us consider a gauge transformation, i.e. a feedback that preserves the metric (i.e. a change of othonormal frame (F1 , F2 ) obtained by setting F˜1 = cos(θ(ξ))F1 +sin(θ(ξ))F2 , F˜2 = − sin(θ(ξ))F1 + cos(θ(ξ))F2 ). It is just a matter of tedious computations to check that ˜ the matrix A(ξ) is changed for A(ξ) = Rθ A(ξ)R−θ . On the other hand, the form ω is defined modulo muttiplication by a nonzero function f (ξ), and the same holds for α, since d(f ω) = f dω+df ∧ω, and ω vanishes over ∆00 . The following lemma follows: Lemma 14: The ratio r(ξ) of the (real) eigenvalues of A(ξ) is an invariant of the structure. Let us now consider the normal form (14), and compute the form ω = ω1 dx1 + ... + ω6 dw along Γ (that is, where x, y, z = 0). Computing all the brackets show that ω1 = ω2 =

6

... = ω5 = 0. This shows also that in fact, along Γ, A(ξ) is just the matrix of the quadratic form Qw . We get the following: Lemma 15: The invariant r(Γ(t)) of the problem P is the same as the invariant rˆ(Γ(t)) of the nilpotent approximation along Γ. Let us compute the ratio r for the ball with a trailer, Equation (5). We denote by A1 , A2 the two right-invariant vector fields over So(3, R) appearing in (5). We have: 1 ∂ ∂ + A1 − cos(θ) , ∂x1 L ∂θ ∂ 1 ∂ F2 = + A2 − sin(θ) . ∂x2 L ∂θ [A1 , A2 ] = A3 , [A1 , A3 ] = −A2 , [A2 , A3 ] = A1 . F1 =

∂ Then, we compute the brackets: H = A3− L12 ∂θ , 1 ∂ 1 ∂ I = −A2− L3 sin(θ) ∂θ , J = A1+ L3 cos(θ) ∂θ , [F1 , I] = ∂ ∂ , [F1 , J] = 0 = [F2 , I], [F2 , J] = −A3− L14 ∂θ . −A3− L14 ∂θ Then: Lemma 16: For the ball with a trailer, the ratio r(ξ) = 1. These two last lemmas are a key point in the section IV: theyl imply in particular that the system of geodesics of the nilpotent approximation is integrable in Liouville sense, as we shall see.

III. R ESULTS In this section, we summarize and comment most of the results obtained in the papers [8], [9], [10], [12], [13], [14]. A. General results We need the concept of an ε-modification of an asymptotic optimal synthesis. Definition 17: Given a one parameter family of (absolutely continuous, arclength parametrized) admissible curves γε : [0, Tγε ] → Rn , an ε-modification of γε is another one parameter family of (absolutely continuous, arclength parametrized) admissible curves γ˜ε : [0, Tγ˜ε ] → Rn such that for all ε and for some α > 0, if [0, Tγε ] is splitted into subintervals of length ε, [0, ε], [ε, 2ε], [2ε, 3ε], ... then: 1. [0, Tγ˜ε ] is splitted into corresponding intervals, [0, ε1 ], [ε1 , ε1 +ε2 ], [ε1 +ε2 , ε1 +ε2 +ε3 ], ... with ε ≤ εi < ε(1+εα ), i = 1, 2, ..., 2. for each couple of an interval I1 = [˜ εi , ε˜i + ε], (with ε˜0 = 0, ε˜1 = ε1 , ε˜2 = ε1 + ε2 , ...) and the respective interval d d (˜ γ ) and dt (γ) coincide over I2 , i.e.: I2 = [iε, (i + 1)ε], dt d d (˜ γ )(˜ εi + t) = (γ)(iε + t), for almost all t ∈ [0, ε]. dt dt Remark 18: This concept of an ε-modification is for the following use: we will construct asymptotic optimal syntheses for the nilpotent approximation Pˆ of problem P. Then, the asymptotic optimal syntheses have to be slightly modified in order to realize the interpolation constraints for the original (non-modified) problem. This has to be done ”slightly” for the length of paths remaining equivalent. In this section it is always assumed but not stated that we consider generic problems only. One first result is the following:

Theorem 19: In the cases 2-step bracket generating, 4-2, 52, 6-2, (without singularities), an asymptotic optimal synthesis [relative to the entropy] for P is obtained as an ε-modification of an asymptotic optimal synthesis for the nilpotent approxiˆ As a consequence the entropy E(ε) of P is equal mation P. ˆ ˆ to the entropy E(ε) of P. This theorem is proven in [12]. However, we can easily get an idea of the proof, using the estimates of formulas (10, 12, 13, 16). All these estimates show that, if we apply an ε-interpolating ˆ and the same controls to P, at time ε (or strategy to P, length ε-since it is always possible to consider arclengthparametrized trajectories), the enpoints of the two trajectories ˆ of order ε1+α , for are at subriemannian distance (either d or d) some α > 0. Then the contribution to the entropy of P, due to the correction necessary to interpolate Γ will have higher order. Also, in the one-step bracket-generating case, we have the following equality: Theorem 20: (one step bracket-generating case, corank k ≤ 3) The entropy is equal to 2π times the metric complexity: E(ε) = 2πM C(ε). The reason for this distinction between corank less or more than 3 is very important, and will be explained in the section III-C. Another very important result is the following logarithmic lemma, that describes what happens in the case of a (generic) singularity of ∆. In the absence of such singularities, as we shall see, we shall always have formulas of the following type, for the entropy (the same for the metric complexity): Z dt 1 , (17) E(ε) ' p ε χ(t) Γ

where χ(t) is a certain invariant along Γ. When the curve Γ(t) crosses tranversally a codimension-1 singularity (of ∆0 , or ∆00 ), the invariant χ(t) vanishes. This may happen at isolated points ti , i = 1, ...r. In that case, we always have the following: Theorem 21: (logarithmic lemma). The entropy (resp. the metric complexity) satisfies: r

E(ε) ' −2

ln(ε) X 1 , εp i=1 ρ(ti )

where ρ(t) = |

dχ(t) |. dt

On the contrary, there are also generic codimension 1 singularities where the curve Γ, at isolated points, becomes tangent to ∆, or ∆0 , ... At these isolated points, the invariant χ(t) of Formula 17 tends to infinity. In that case, the formula 17 remains valid (the integral converges). B. Generic distribution in R3 This is the simplest case, and is is important, since many cases just reduce to it. Let us describe it in details. Generically, the 3-dimensional space M contains a 2dimensional singularity (called the Martinet surface, denoted by M). This singularity is a smooth surface, and (except at

.

7 77

Fig. Fig.4.4. 3-dimensional 3-dimensionalMartinet Martinetcase case

Fig. Fig. 3. 3. 3-dimensional 3-dimensional contact contact case case

Fig. 4.

3-dimensional contact case

3-dimensional Martinet case

Figure Figure 44 concerns concerns the the case case r r 6=$= 00 (crossing (crossingMartinet Martinet isolated isolated points points on on M), M), the the distribution distribution ∆ !isisnot nottangent tangenttotoM. M. surface). At a Martinet point, the vector-field Xε has a limit r = $ 0 (crossing Martinet surface).4Atconcerns a Martinet the point,case the vector-field X Generically, the curve Γ crosses M transversally at a finite Figure ! has a limit Generically, curve " crosses a finite cycle, which is not tangent to the distribution. The asymptotic ted points on M), thethe distribution ! is M nottransversally tangent to atM. number of isolated points ti , i = 1, ..., r, . These points aresurface). cycle, which is not tangent to thethe distribution. The asymptotic At a Martinet point, vector-field X has ! X a limit number of"isolated points ttransversally , i = 1, ..., r, . at These points are optimal strategy consists of: a. following a trajectory of erically, thenot curve crosses a to finite ε the special isolated M pointsi where ∆ is tangent M (thiscycle, optimal strategy consists of: a. following a trajectory X which isthenotheight tangent to center the distribution. Theofasymptotic not thepoints special tisolated points where ! is tangent to are M (this till reaching of the of the limit cycle, b. ! ber of isolated , i = 1, ..., r, . These points would be not generic). They are called Martinet points. This i till reaching the height of the center of the limit cycle, b. optimal strategy consists a. following would be not generic). They are called Martinet points. This crossing the cylinder, with aof: neglectible cost 2ε, ac. trajectory Following of X! r can be zero. Also,! there are other isolated points τj , he special number isolated points where is tangent to M (this crossing the cylinder, with a neglectible cost 2$, c. Following number r can be zero. Also, there are other isolated points !jtill , areaching trajectory ofthe the height opposite of vector field −X . The strategy ε of the limitforcycle, b. the field center j = 1, ..., l,They at which Γcalled is tangent to ∆ (which means that Γ a trajectory of the but opposite vector %X d be not generic). are Martinet points. This 0strategy for ! . The .! c. Following entropy is similar, using the tangent cylinder j= 1, ...,admissible l, at whichin"aisneighborhood tangent to !of(which means that " crossing the cylinder, with atheneglectible costCCε2$, is almost τ ). Out of M, the j entropy is similar, but using tangent cylinder !. ber r can be zero. Also, there are other isolated points ! , j is almost admissible in a neighborhood !j ). Outproperty). of M, the distribution ∆ is a contact distribution (aofgeneric a trajectory of the opposite vector field %X! . The strategy for 1, ..., l, at which " is tangent tothat ! vanishes (which (aon means that " distribution a contact distribution generic Let ω be !a isone-form ∆ andproperty). that is 1entropy C. Theisone-step case similar,bracket-generating but using the tangent cylinder C!! . Let " be a one-form that vanishes on ! and that is 1 most admissible in a neighborhood of ! ). Out of M, the ˙ C. The one-step bracket-generating case on Γ, defined up to multiplication jby a function which is 1 ˙ defined up to multiplication by a function which is 1 For the corank k ≤ 3, the situation is very similar to the on a", bution ! along is contact (a generic property). Γ. Alongdistribution Γ, the restriction 2-form dω |∆ can be made For the corank 3, the very similar the 3-dimensional case.k It&can be situation competelyis reduced to it.toFor along ". Along ", the restriction 2-form d" can be made |! a skew-symmetric endomorphism A(Γ(t)) ∆ 1 (skewC. The t " be a into one-form that vanishes on ! and thatof is 3-dimensional case. It can be competely reduced to it. For one-step bracket-generating case into a skew-symmetric A("(t))over of !∆), (skew details, see [10]. symmetric with respect endomorphism to the scalar product , defined up to multiplication by a function which is !), 1 by details, [10].this strange fact appears: there is the limit At thissee point, symmetric with respect to the scalar product over by For duality: < A(Γ(t))X, Y >= dω(X, Y ). Let χ(t) denote the thethiscorank k &strange 3, thefactsituation very similar to the At point, appears: is there is the g ". Alongmoduli ", the restriction 2-form d" can be made corank k = 3. If kthis > 3 only, new phenomena appear. Letlimit us duality:ofIt3for only, newcompetely phenomena appear. Letto us it. For thisbe a skew-symmetric endomorphism A("(t)) !dtthe(skew moduli of the eigenvalues of A("(t)). Weof have following: explain Theorem 22: 1. If r = 0, M C(ε) ' ε22 ! κ(t) . At pointsdetails, see [10]. explain now the reason for this Let us consider the following mapping Bξ : ∆ξ × ∆ξ → metric with Theorem respect 22: to 1. theIf scalar product !), by 2 dt r = 0, M C($) ! over !2Γ !(t) . At points At Let following B%defined : ! !is% " thisusξpoint, strange there the limit T M/∆ , consider (X, Y )this →the [X, Y ] + ∆ξfact . mapping It isappears: a well % 'tensor x ty: < A("(t))X, Y→ >= d"(X, Y ). Let #(t) denote the where χ(t) +∞, the formula is convergent. " T M/! , (X, Y ) " [X, Y ] + ! . It is a well defined tensor mapping , which means that it actually applies to vectors (and P x % % corank k = 3. If k > 3 only, new phenomena appear. Let us r ln(ε) have 1 following: uli of the eigenvalues A("(t)). the where "of +#, the'formula is!convergent. 2. If #(t) r 6= 0, M C(ε) −2We where ρ(t) = not i=1 ρ(ti ) , ε2 " to vector fields, as expected from the definition). This is mapping , which means that it actually applies to vectors (and r explain now the reason for this 1 2. If|. r $= 0, M C($) ! %2 ln(!) | dχ(t) i=1dt following for a from one-form ω : dω(X, Y ) =is !2 2 "(ti ) , where %(t) = due not totothe vector fields,formula, as expected the definition). This

eorem 22: d#(t) 1. dt If r = 0, M C($) !

!(t) .

At points

Let us ]) consider following mapping Bd"(X, !Y%B)' !% " % : by E(ε) = 2πM C(ε). ω([X, +following ω(Y )Xthe −formula, ω(X)Y. Letaus call Iξ the | 3. due toYthe for one-form " :image ξ= dt |. " T M/! , (X, Y ) " [X, Y ] + ! . It is a well defined x % % the product of two balls Let in ∆us TheIfollowing holds: Let us describe asymptotic optimal syntheses. They are of"([X, 3. E($) = 2&Mthe C($). Y ]) + "(Y )X %unit "(X)Y. by B% tensor ξ . call % the image e #(t) " shown +#, the formula is convergent. mapping , which means actually vectors (and on describe Figuresln(!) 3, 23: of For a generic P,it in for 3, applies the sets Ito are "r ofTheorem the product two unitthat balls !k%≤ . The following holds: Let us the4.asymptotic optimal syntheses. They are Γ(t) If r $= 0, M C($) %2 !3,2 4. i=1 "(t1i ) , where %(t) = notconvex. toTheorem vector 23: fields, expected the shown on ! Figures For aasgeneric P, forfrom k & 3, thedefinition). sets I"(t) are This is ) is shown in [10],for with the consequences that Y ) = |. due convex. toThis thetheorem following formula, a one-form " : d"(X, we This will state just below. Figure 3 concerns the case r = 0 (everywhere contact type). is shown in [10],Let withusthecall consequences that by B% E($) = 2&M C($). "([X, Y ]) theorem + "(Y )X % "(X)Y. I% the image This isstate no just morebelow. true for k > 3, the first catastrophic TheFigure points3 concerns where thethedistribution ∆ is not transversal to Γ we will case r = 0 (everywhere contact type). product of two unit pballs in !% . Thein following holds: t us describe the asymptotic optimal syntheses. They are isof the being = R10 ). The are (they do not change anything). Hence ∆ is the no case more10-4 true(afor k 4>distribution 3, the first catastrophic Theomitted points whereagain the distribution ! is not transversal to " caseThis n on Figures 3, 4. Theorem 23: For a generic P, for k & 3, the sets I are cases k = 4, 5(ainp dimension 10 are ininteresting, also transversal the cylinders Cε , for ε small. Hence Therefore, case being the case 10-4 = 4 distribution R10 ). The"(t) are omitted (theytoagain do not change anything). ! is intermadiate since on somecases open ksubsets the convexity property may ∆ (up totosign) a vector field Xε $onsmall. Cε, tangent toconvex. intermadiate = 4, 5ofinΓ,dimension 10 are interesting, alsodefines transversal the cylinders C! , for Therefore, hold or not. These cases are studied in the paper [13]. ∆, that can be chosen of length 1. The asymptotic optimal is shown the consequences that sincetheorem on some open subsetsinof [10], ", thewith convexity property may ! defines (up to sign) a vector field X! on C$, tangent to This Theor main consequence of this convexity property is that synthesis consists of: 1. Reaching Cε from Γ(0), 2. Followwe will state just below. hold not. These cases are studied in the paper [13]. !, that can be chosen of length 1. The asymptotic optimal gure 3 concerns the case r = 0 (everywhere contact type). a trajectory of Xε , 3. Join Γ(t). The steps 1 and 3 cost 2ε, everything reduces (out of singularities where the logarithmic property iscatastrophic that synthesis distribution consists of: 1. Reaching Ctransversal 2. " Follow ThisTheis main no consequence more true of forthisk convexity > 3, case, the first ! from "(0), to points where notmetric to the 3-dimensional contact as is shown whichthe is neglectible w.r.t.!theis full complexity. To get lemma applies) 10 everythingthe reduces (out of singularities where the logarithmic a trajectorydo of not X! , change 3. Join "(t). The steps 1 and ! 3 cost 2$, case p = 4 distribution mitted (they anything). Hence in being the paper [10]. We10-4 briefly(asummarize the results. in R ). The the again optimal synthesis for the interpolation entropy, one is has tocase which is neglectible w.r.t. the full metric complexity. To get lemma applies) to the 3-dimensional contact case, as is shown cases = 4,ω5that in vanish dimension the same construction, but starting fromTherefore, a subriemannianintermadiate Consider the onekforms on ∆ 10 and are that interesting, are transversalmake to the cylinders C , for $ small. the optimal0 synthesis for!the interpolation entropy, one has to in the ˙paper [10]. We briefly summarize the results. cylinder C tangent to Γ. 1 on Γ, and again, by the duality w.r.t. the metric over since Consider on somethe open subsets" of ",vanish the convexity property may εa vector field X! on C$, tangent to efines (up make to sign) the same construction, but starting from a subriemannian oneYforms that Y !vector and that are In normal inThe that asymptotic case, the x-trajectories arehold ∆,ordefine dω ) =< AX, >,inon for fields |∆ (X,cases !ofcoordinates, not. These are studied the paper [13]. hat can becylinder chosen length 1. optimal ˙ C! and tangent ". 1 Yon in",∆.and again, duality the1)-parameter metric over just circles, the to corresponding optimal controls are just X, Now, we by havethe along Γ, w.r.t. a (k − The main consequence of this convexity property is that hesis consists of: 1. Reaching C from "(0), 2. Follow In normal coordinates, in that case, the x-trajectories are !, define d" (X, Y ) =< AX, Y >, for vector 2π ! trigonometric functions, with period ε . affine family of|! skew symmetric endomorphisms AΓ(t)fields of and"(t). the corresponding are just X, Y in reduces !. Now, (out we have along ", a (k % 1)-parameter everything of singularities where the logarithmic jectory of just X! ,circles, 3. Join The steps 1optimal and 3controls cost 2$, 2$ trigonometric with period affineapplies) family of skew3-dimensional symmetric endomorphisms A"(t) to the contact case, as of is shown h is neglectible w.r.t. functions, the full metric complexity. To get lemma ! . !2

ptimal synthesis for the interpolation entropy, one has to

in the paper [10]. We briefly summarize the results.

8 8

k!1 ! k−1 X !!(t). Say, A!(t) (!) = A0!(t) + !i Ai!(t) . Set "(t) = 0 i

∆Γ(t). Say, AΓ(t) (λ) = AΓ(t) +i=1 λi AΓ(t) . Set χ(t) =

i=1 inf ! ||A!(t) (!)|| = ||A!(t) (!" (t))||. inf λ ||AΓ(t) (λ)|| = ||AΓ(t) (λ∗ (t))||. OutOut of of isolated points isolated pointsofof" Γ(that (thatcount countfor fornothing nothing inin the the metric complexity metric complexityor orin inthetheentropy), entropy),the thet!one t−one parameter parameter ∗ can be smoothly block-diagonalized (with family A!(t) (!"(λ(t)) family AΓ(t) (t)) can be smoothly block-diagonalized (with 2 × 2 bloks), using gaugetransformation transformationalong along ". Γ. After After 2 " 2 bloks), using a agauge this gauge transformation, the 2-dimensional eigenspace correthis gauge transformation, the 2-dimensional eigenspace corresponding to the largest moduli)eigenvalue eigenvalueofofAAΓ(t)(! (λ"∗(t)), (t)), sponding to the largest (in(inmoduli) !(t) corresponds to the two first coordinates in the distribution, and corresponds to the two first coordinates in the distribution, and to the 2 first controls. In the asymptotic optimal synthesis, all to the 2 first controls. In tothezero asymptotic synthesis, other controls are put [here theoptimal convexity propertyallis other controls arepicture put to of zero the convexity used], and the the[here asymptotic optimal property synthesis isis used], and that the of picture of the asymptotic optimal synthesis is exactly the 3-dimensional contact case. We still have the formulas: exactly that of the 3-dimensional contact case. We still have Z the formulas: dt 2 , E(ε) = 2πM C(ε). M C(ε) ' "2 κ(t) 2 ε Γ dt MC(#) # 2 , E(#) = 2$MC(#). !(t) first treated in [12] in the 10The case k ># 3 was

!

dimensional case, and was completed in general in [14].

Fig. 5.

The dance of minimum entropy, for 3rd bracket.

Fig. 5. The dance of minimum entropy, for 3rd bracket.

of Section II-C5. In both cases, we change the variable dw of Section II-C5. cases, we look change variable . We for the arclengthw for w ˜ such that Indw ˜both = δ(w) dw parametrized nilpotent (i.e. w for w˜ suchtrajectories that dw˜ of= the#(w) . We approximation look for arclength2 2 (u ) + (u ) = 1), that start from Γ(0), and reach Γ 1 2 trajectories of the nilpotent approximation in parametrized (i.e. Zε 2 (u1 ) + (u2 )2 = 1), that start from "(0), and reach " in ˙ )dτ. Abnormal extremals do fixed time ε, maximizing "$w(τ 0 fixed timein#,themaximizing w(& ˙ )d&.curves Abnormal extremals no come picture, and optimal correspond to thedo

hamiltonian

0

p curves 2 2 correspond to the H = and (P Foptimal The case k > 3 was first treated in [12] in the 10- no come in the picture, 1 ) + (P F2 ) , hamiltonian In that case, case,and thewas situation does innot reduce to the 3dimensional completed general in [14]. ' dimensional contact case: the optimal controls, in the asymp- where P is the adjoint H = vector. (P F1It)2turns + (Pout F2 that, )2 , in our normal coordinates, the same trajectories are optimal for both the 4-2 totic optimal synthesis for the nilpotent approximation are In that case, the situation does not reduce to the 3- and the 5-2 case (one has just to notice that the solution of the still trigonometric controls, but with different periods that where P is the adjoint vector. It turns out that, in our normal dimensional contact case: the optimal controls, in the as- 4-2 case meets the extra interpolation condition corresponding are successive integer multiples of a given basic period. New coordinates, the same trajectories are optimal for both the 4-2 to the 5-2 case). ymptotic optimal synthesis nilpotent invariants λjθ(t) appear, andfor the the formula for theapproximation entropy is: and the 5-2 case (oneu1has to notice solution of the Setting as usual = just cos(ϕ) = Pthat F1 , uthe are still trigonometric controls, butPwith different periods that 2 = sin(ϕ) = Z T r j 4-2 case meets the extra interpolation condition , F2 ]]P F1 − jλθbasic period. New P F2 , we get ϕ˙ = P [F1 , F2 ], ϕ¨ = −P [F1 , [F1 corresponding 2π are successive integer multiples of aj=1 given E(ε) ' dθ, P j P [F , [F , F ]]P F . r to the 5-2 case). j 2 2 1 2 2 2 ε the0 formula invariants !"(t) appear, and j=1 (λfor θ ) the entropy is: At this point, we have notice that Setting as usual u1 =to cos(') = Ponly F1 , the u2 components = sin(') = # the optimal controls being " of j P , P of the adloint vector P are not constant (the hamilx x T the rform: 1 , we 2 P F get ' ˙ = P [F , F ], ' ¨ = !P [F , [F , F2 ]]P F1 ! j! 2 1 2 1 1 2$ j=1 " tonian in the nilpotent approximation depends only on the xE(#) v # 2 d%, #r P [F , [F , F ]]P F . 2 1 2 2 variables), therefore, P [F1 , [F1 , F2 ]] and P [F2 , [F1 , F2 ]] are (!j" )2 u # 0j u jλθ(t) j=1 2πjt At this point, we have to are notice only the components constant (the third brackets alsothat constant vector fields). t sin( ), (18) u2j−1 (t) = − Pr the optimal controls being of jλ thej form: P , P of the adloint vector P are not constant (the hamilε Hence, ϕ ¨ = α cos(ϕ) + β sin(ϕ) = α x ˙ + β x ˙ for appropriate x x 1 2 1 2 j=1 θ(t) v constants α, β. It follows that, for another constant wextonian in the nilpotent approximation depends only onk,the u j $ u jλ have, for the optimal curves of the nilpotent approximation, 2πjt variables), therefore, P [F , [F , F ]] and P [F , [F , F ]] θ(t) 1 1 2 2 1 2 are tP j ), j = 1, ..., r u2j (t) = % cos( % rj!"(t)j in normal coordinates x , x : 2$jt 1 2 ε u2j!1 (t) = !& #rj=1 jλθ(t) sin( ), (18) constant (the third brackets are also constant vector fields). j # Hence, '¨ = ( cos(') ) sin(') x˙ 1 + ) x˙ 2 for appropriate j! u2r+1 (t) = 0 if pj=1 is odd"(t) . x˙ 1 =+cos(ϕ), x˙ 2 = =( sin(ϕ), $ constants (, ). It ϕ˙follows that, for another constant k, we = k + λx1 + µx2 . These last % formulas hold in the free case only (i.e. the have, for the optimal % j!j"(t) curves of the nilpotent approximation, 2$jt p(p−1) &# caseu2j where corank kj = cos( , the (t) =the ), dimension j = 1, ..., rof he in Remark 2 r normal coordinates x1 , xthat 24: 1. It means 2 : we are looking for curves in # j! second homogeneous component of the free Lie-algebra with p j=1 "(t) generators). The non free case is more complicated (see [14]). uTo (t) = 0 if p is odd . 2r+1 prove all the results in this section, one has to proceed as follows: 1. use the to (i.e. nilpotent These last formulas holdtheorem in the of freereduction case only the approximation (19), and 2. use the Pontriaguin’smaximum p(p!1) caseprinciple where on thethecorank k = , the dimension of he normal form of 2the nilpotent approximation, second homogeneous component of the free Lie-algebra with p in normal coordinates

generators). The non free case is more complicated (see [14]). To all the results section, 4 D. prove The 2-control case, ininRthis and R5 . one has to proceed as follows: 1. use the theorem of reduction to nilpotent These cases correspond respectively to the car with a trailer approximation the (Example Pontriaguin’smaximum (Example 2) (19), and theand ball2.onuse a plate 3). principle on the normal form of the nilpotent approximation, We use also the theorem 19 of reduction to Nilpotent in normal coordinates approximation, and we consider the normal forms Pˆ4,2 , Pˆ5,2 D. The 2-control case, in R4 and R5 .

the x1 , x2 plane, whose curvature is an affine function of the position, x˙ 1 = cos('), x˙ 2 = sin('), 2. In the two-step case (contact case), '˙ =bracket k + !xgenerating 1 + µx2 . otimal curves were circles, i.e. curves of constant curvature, 3. the conditions of ε-interpolation of Γ say that these curves Remark 24: 1. It means that we are looking for curves in must be periodic (there will be more details on this point in the the an affine 1 , x2 plane, nextxsection), that whose the areacurvature of a loopismust be zerofunction (y(ε) =of0),the position, and finally (in the 5-2 case) that another moment must be zero. 2. In easily the two-step case these (contact case), It is seen thatbracket such a generating curve, meeting interpolation conditions, must be an elliptic curve of elastica-type. otimal curves were circles, i.e. curves of constant curvature, The periodicity and surface 3. the conditions of vanishing #-interpolation of "requirements say that theseimply curves that it is the only periodic elastic curve shown on Figure 5, must be periodic (there will be more details on this point in the parametrized in a certain way.

next section), that the area of a loop must be zero (y(#) = 0), and finally (in the 5-2 case) that another moment must be zero. It is easily seen that such a curve, meeting these interpo-

9

The formulas are, in terms of the standard Jacobi elliptic functions: 4t u1 (t) = 1 − 2dn(K(1 + ))2 , ε 4t 4t ϕ0 u2 (t) = −2dn(K(1 + ))sn(K(1 + )) sin( ), ε ε 2 where ϕ0=130◦ (following [22], p. 403) and ϕ0 = 130, 692◦ R , with k = sin( ϕ20 ) and K(k) is the following Mathematica quarter period of the Jacobi elliptic functions. The trajectory on the x1 , x2 plane, shown on Figure 5, has equations: 4Kt ε −4Kt [ + 2(Eam( + K) − Eam(K))], 4K ε ε ε 4Kt x2 (t) = k cn( + K). 2K ε On the figure 2, one can clearly see, at the contact point of the ball with the plane, a trajectory which is a ”repeated small deformation” of this basic trajectory. The formula for the entropy is, in both the 4-2 and 5-2 cases: Z dt 3 , E(ε) = 2σε3 Γ δ(t)

Remark 25: This integrability property is no more true in the general 6-2 case. It holds only for the ball with a trailer. As usual, we work in Poincar´e coordinates, i.e. we consider level 21 of the hamiltonian H, and we set: u1 = P F = sin(ϕ), Differentiating twice, we get ϕ˙ = P [F, G], ϕ¨ = −P F F G.P F − P GF G.P G, where F F G = [F, [F, G]] and GF G = [G, [F, G]]. We set λ = −P F F G, µ = −P GF G. We get that:

x1 (t) = −

where σ is a universal constant, σ ≈ 0.00580305. Details of computations on the 4-2 case can be found in [12], and in [13] for the 5-2 case. IV. T HE BALL WITH A TRAILER We start by using Theorem 19, to reduce to the nilpotent approximation along Γ : x2 x1 x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), (19) 2 2 x2 x1 x2 x1 z˙1 = x2 ( u1 − u2 ), z˙2 = x1 ( u1 − u2 ), 2 2 2 2 x2 x1 w˙ = Qw (x1 , x2 )( u1 − u2 ). 2 2 By Lemma 16, we can consider that

ϕ¨ = λ sin(ϕ) + µ cos(ϕ).

(20)

where δ(w) is the main invariant. In fact, it is the only invariant for the nilpotent approximation along Γ. Moreover, dw if we reparametrize Γ by setting dw := δ(w) , we can consider that δ(w) = 1. R Then, we want to maximaize wdt ˙ in fixed time ε, with the interpolation conditions: x(0) = 0, y(0) = 0, z(0) = 0, w(0) = 0, x(ε) = 0, y(ε) = 0, z(ε) = 0. From Lemma 27 in the appendix, we know that the optimal trajectory is smooth and periodic, (of period ε). Clearly, the optimal trajectory has also to be a length minimizer, then we have to consider the usual hamiltonian for length: H = 12 ((P.F1 )2 + (P.F2 )2 ), in which P = (p1 , ..., p6 ) is the adjoint vector. It is easy to see that the abnormal extremals do not come in the picture (cannot be optimal with our additional interpollation conditions), and in fact, we will show that the hamiltonian system corresponding to the hamiltonian H is integrable.

(21)

Now, we compute λ˙ and µ. ˙ We get, with similar notations as above for the brackets (we bracket from the left): λ˙ = P F F F G.P F + P GF F G.P G, µ˙ = P F GF G.P F + P GGF G.P G, and computing the brackets, we see that GF F G = F GF G = 0. Also, since the hamiltonian does not depend on y, z, w, we get that p3 , p4 , p5 , p6 are constants. Computing the brackets F F G and GF G , we get that 3 3 λ = p4 + p6 x1 , µ = p5 + p6 x2 , 2 2 and then, λ˙ = p6 sin(ϕ) and µ˙ = p6 cos(ϕ). Then, by (21), ϕ¨ =

˙ λλ p6

+

µµ˙ p6 ,

and finally: x˙ 1 = sin(ϕ), x˙ 2 = cos(ϕ), 1 ϕ˙ = K + (λ2 + µ2 ), 2p6 λ˙ = p6 sin(ϕ), µ˙ = p6 cos(ϕ).

(Pˆ6,2 )

Qw (x1 , x2 ) = δ(w)((x1 )2 + (x2 )2 )

u2 = P G = cos(ϕ).

Setting ω =

λ p6 , δ

=

µ p6 ,

(22)

we obtain:

ω˙ = sin(ϕ), δ˙ = cos(ϕ), p6 ϕ˙ = K + (ω 2 + δ 2 ). 2 It means that the plane curve (ω(t), δ(t)) has a curvature which is a quadratic function of the distance to the origin. Then, the optimal curve (x1 (t), x2 (t)) projected to the horizontal plane of the normal coordinates has a curvature which is a quadratic function of the distance to some point. Following the lemma (23) in the appendix, this system of equations is integrable. Summarizing all the results, we get the following theorem. Theorem 26: (asymptotic optimal synthesis for the ball with a trailer) The asymptotic optimal synthesis is an εmodification of the one of the nilpotent approximation, which has the following properties, in projection to the horizontal plane (x1 , x2 ) in normal coordinates: 1. It is a closed smooth periodic curve, whose curvature is a quadratic function of the position, and a function of the square distance to some point, R nd 2. The area R and the 2 order moments Γ x1 (x2 dx1 − x1 dx2 ) and Γ x2 (x2 dx1 − x1 dx2 ) are zero.

10 10

Fig. Fig. 7.7. Parking Parking the the ball ball with with aa trailer trailer

Fig. 7.

Parking the ball with a trailer

Fig. Fig.6.6. The Thedance danceofofminimum minimumentropy entropyfor forthe theball ballwith withaatrailer trailer

Fig. 6.

The dance of minimum entropy for the ball with a trailer

! ! R! dw σ 3.3.The E(!) dw , #(w) Theentropy entropyisisgiven givenby bythe theformula: formula:E(ε) E(!)== ε""44!4 Γ!!δ(w) #(w) , where "(w) is the main invariant from (20), and # is δ(w) σ# is aa= ! ! dw "(w) isisthegiven main invariant (20), andE(!) 3. Thewhere entropy by thefrom formula: , "4 ! #(w) universal constant. universal constant. where "(w) is the main invariant from (20), and # is a InInfact factwe wecan cangogoa alittle littlebit bitfurther furthertotointegrate integrateexplicitely explicitely universal constant. ¯$¯= cos(ϕ)λ the system (22). Set cos(%)$ !!sin(ϕ)µ, sin(%)µ, µ ¯µ ==sin(ϕ)λ sin(%)$ + λ − the system (22). Set $ = cos(%)$ sin(%)µ, ¯ sin(%)$ + In fact we can go a little bit further to integrate explicitely cos(%)µ. we get: cos(ϕ)µ. cos(%)µ. we get: ¯ the system (22). Set $ = cos(%)$ ! sin(%)µ, µ ¯ = sin(%)$ + ¯¯ dλ 1 ¯2 cos(%)µ. we get: 2 d$$ !¯ ¯ 2++µ ==−¯ µµ(K ++ 1 (λ ¯µ !¯ (K ($$ ¯2)), )), dtdt 2p 6 6 2p6 ¯ d$ 1 d¯ µ 1 ¯ 22($ ¯¯2222)). d¯ µ=! = ¯ ¯ 2 )), p µ(K + ¯$(K ++ 1 (λ ¯ + ¯ 2++µ $(K ($ $ µ ¯+ )). µ dt dtdt = p666 +λ(K 2p 6 2p 6 2p6

dµ ¯

6

1

¯ ¯ 2system. This isisaa a222dimensional dimensional (integrable) hamiltonian system. The This = p6 +(integrable) $(K +hamiltonian ($ +µ ¯ 2The )). Thisis dimensional (integrable) hamiltonian system. The dt 2p 6 hamiltonian is: hamiltonian hamiltonianis:is: This is a 2 dimensional system. The 2p 6(integrable) 1 ¯ 222 hamiltonian 222 222 ¯$$ !p !! 2p6 6(K $$ ¯− ¯ ++µ HH ++ 1 (λ ¯µ )))) . . 11 1==−p 66λ !p (K ( ¯ 6 4 2p hamiltonian is: Fig. 8. The universal movements in normal coordinates 4 2p666 Fig. Fig. 8. 8. The The universal universal movements movements in in normal normal coordinates coordinates 2p 1 This hamiltonian system is therefore integrable, and solutions 6 This hamiltonian system is therefore integrable, and solutions 2 2 2 This hamiltonian system ¯ ! is therefore ¯ solutions H = !pin (K integrable, + functions. (and $ +liitle µ ¯ )) . 6$ can bebe1expressed expressed terms ofof hyperelliptic A can functions. A 4hyperelliptic canbe expressedin interms termsof hyperelliptic2p functions. A liitle liitle 6 These figures are, in the two-step bracket generating case: 8. The universal movements in norm These the bracket generating case: These figures figures are, are, in inFig. the two-step two-step bracket generating case: numerics now allows totoshow, show, on figure 6,6, the the optimal xnumerics now allows to on figure 6, optimal xth numerics now allows show, on figure the optimal xth a circle, for the third bracket, the periodic elastica, for the 4 th aa circle, for the 44 This hamiltonian system is therefore integrable, and solutions circle, for the the third third bracket, bracket, the the periodic periodic elastica, elastica, for for the trajectory ininthe the horizontal plane ofofthe the normal coordinates. trajectory trajectoryin thehorizontal horizontalplane planeof thenormal normalcoordinates. coordinates. bracket, the plane curve of the figure 6. bracket, the plane curve of the figure 6. can be expressed in terms of hyperelliptic functions. A liitle bracket, the plane curve of the figure 6. On the figure 7,7,we we show the motion of the ball with On Onthe thefigure figure7, weshow showthe the motion motion of of the the ball ball with with aaa These figures are, in the two-s They are periodic plane curves whose curvature is respecrespecThey is respecThey are are periodic periodic plane curves curves whose whose curvature curvature is numerics now allows to of show, on point figure 6, the the optimal x- plane trailer on the plane (motion the contact between trailer traileron onthe theplane plane(motion (motionof ofthe thecontact contactpoint pointbetween between the the tively: a circle, for the third bracket, the tively: a constant, a linear function of of the position, a constant, a linear function of of the position, tively: a constant, a linear function of of the position, aaa trajectory in the horizontal planeis thealong normal ball and the plane).Here, the problem isisof totomove move along the xball the to the balland andthe theplane).Here, plane).Here, theproblem problem move along thexx- coordinates. quadratic function of the position. bracket, the plane curve of the fig quadratic function of the position. function axis, keeping constant the frame attached totothe the ball and the On the figure 7, we the motion of the quadratic ball with a of the position. axis, the frame to axis,keeping keepingconstant constant theshow frameattached attached the ball ball and and the the They are periodic plane curves angle ofofthe the trailer. trailer on the plane the angle of trailer. This is, as shown on Figure 8,a the theconstant, clear beginning beginning of angle the trailer.(motion of the contact point between This Figure clear This is, is, as as shown shown on on Figure 8, aa tively: a of linear func ball and the plane).Here, the problem is to move along the xseries. series. series. quadratic function of the position axis, keeping constant the frame attached to the ball and the V.V.E EEXPECTATIONS XPECTATIONS AND CONCLUSIONS V. XPECTATIONSAND ANDCONCLUSIONS CONCLUSIONS B. Robustness angle of the trailer. B. B. Robustness Robustness This is, as shown on Figure 8 Some movies ofofminimum minimum entropy for the ball rolling on Some Somemovies moviesof minimumentropy entropyfor for the the ball ball rolling rolling on on As one can see, in many cases (2 controls, controls, or or corank corank As one can many plane and the ball with trailer are visible on the website can see, see, in inseries. many cases cases (2 aa aplane planeand andthe theball ballwith withaaatrailer trailerare arevisible visibleon on the the website website k "As3),one our strategy is extremely robust in the following sense: k ≤ 3), our strategy is extremely robust in the following sense: ***************************. k " 3), our strategy is extremely V. E XPECTATIONS AND CONCLUSIONS ***************************. ***************************. the asymptotic optimal syntheses do not depend, from the the asymptotic optimal syntheses do not depend, from the the asymptotic optimalB.syntheses Robustness Some movies of minimum entropy for the ball rolling on qualitative point of view, of the metric chosen. They depend qualitative point of view, of the metric chosen. They depend qualitative point of view, of Astheneeded one to can see, A.A. Universality ofofsome some pictures ininnormal normal coordinates only on the number of brackets generate the in space.many ca a planeA. and the ball with a trailer visible on only theon Universality of pictures in coordinates Universality some pictures normalare coordinates the to generate the space. only onwebsite the number number of of brackets brackets needed k " 3), our strategy is extremely r ***************************. Our first conclusion isis the the following: there are certain Our Our first first conclusion conclusion is the following: following: there there are are certain certain the asymptotic optimal synthese C. The practical importance of normal coordinates coordinates universal pictures for the motion planning problem, in corank C. The The practical practical importance importance of of normal universal universalpictures picturesfor forthe themotion motionplanning planningproblem, problem,in incorank corank C. qualitative point of view, less ororequal equal toto3, 3,3,and and ininrank rank 2,2,with with brackets atatmost most (could The main practical problem of implementation of of our our stratstrat- of the m less 2, at The lessor equalto andin rank with444brackets brackets most(could (could The main main practical practical problem problem of of implementation A. Universality of some pictures in normal coordinates only on the number brackets n bebe555brackets brackets atataaasingularity, singularity, with the logarithmic lemma). egy comes with the !-modifications. How to compute computeofthem, them, egy to be bracketsat singularity,with withthe thelogarithmic logarithmiclemma). lemma). egy comes comes with with the the ε-modifications. !-modifications. How Our first conclusion is the following: there are certain universal pictures for the motion planning problem, in corank less or equal to 3, and in rank 2, with 4 brackets at most (could

C. The practical importance of n The main practical problem of

11

how to implement? In fact, the ε-modifications count at higher order in the entropy. But, if not applied, they may cause deviations that are not neglectible. The high order w.r.t. ε in the estimates of the error between the original system and its nilpotent approximation (Formulas 10, 12, 13, ??) make these deviations very small. It is why the use of our concept of a nilpotent approximation along Γ, based upon normal coordinates is very efficient in practice. On the other hand, when a correction appears to be needed (after a noneglectible deviation), it corresponds to brackets of lower order. For example, in the case of the ball with a trailer (4th bracket), the ε-modification corresponds to brackets of order 2 or 3. The optimal pictures corresponding to these orders can still be used to perform the ε-modifications. D. Final conclusion This approach, to approximate optimally nonadmissible paths of nonholomic systems, looks very efficient, and in a sense, universal. Of course, the theory is not complete, but the cases under consideration (first, 2-step bracket-generating, and second, two controls) correspond to many practical situations. But there is still a lot of work to do to in order to cover all interesting cases. However, the methodology to go ahead is rather clear. VI. A PPENDIX A. Appendix 1: Normal form in the 6-2 case We start from the general normal form (11) in normal coordinates: x˙ 1 = (1 + (x2 )2 β)u1 − x1 x2 βu2 , x˙ 2 = (1 + (x1 )2 β)u2 − x1 x2 βu1 , x1 x2 y˙ i = ( u1 − u2 )γi (y, w), 2 2 x2 x1 w˙ = ( u1 − u2 )δ(y, w) 2 2 We will make a succession of changes of parametriztion of the surrface S (w.r.t. which normal coordinates were constructed). These coordinate changes will always preserve tha fact that Γ(t) is the point x = 0, y = 0, w = t. Remind that β vanishes on S, and since x has order 1, we can already write on Tε : x˙ = u + O(ε3 ). One of the γi ’s (say γ1 ) has to be nonzero (if not, Γ is tangent to ∆0 ). Then, y1 has order 2 on Tε .Set for i > 1, y˜i = yi − γγ1i . Differentiating, yi γi 2 we get that d˜ ˜2 , z2 = y˜3 dt = y˙ i − γ1 y˙ 1 + O(ε ), and z1 = y δ have order 3. We set also w := w − γ1 , and we are at the following point: x2 x1 x˙ = u + O(ε3 ), y˙ = ( u1 − u2 )γ1 (w) + O(ε2 ), 2 2 x2 x1 z˙i = ( u1 − u2 )Li (w).x + O(ε3 ), 2 2 x2 x1 w˙ = ( u1 − u2 )δ(w).x + O(ε3 ), 2 2 where Li (w).x, δ(w).x are liner in x. The function γ1 (w) can be put to 1 in the same way by setting y := γ1 y(w) . Now let

T (w) be an invertible 2×2 matrix. Set z˜ = T (w)z. It is easy to see that we can chose T (w) for we get: x1 x2 x˙ = u + O(ε3 ), y˙ = ( u1 − u2 ) + O(ε2 ), 2 2 x2 x1 3 z˙i = ( u1 − u2 )xi + O(ε ), 2 2 x2 x1 w˙ = ( u1 − u2 )δ(w).x + O(ε3 ), 2 2 Another change of the form: w := w + L(w).x, where L(w).x is linear in x kills δ(w) and brings us to w˙ = ( x22 u1 − x1 2 2 2 u2 )O(ε ). This O(ε ) can be of the form Qw (x)+h(w)y + 3 O(ε ) where Qw (x) is quadratic in x. If we kill h(w), we get the expected result. This is done with a change of coordinates 2 of the form: w := w + ϕ(w) y2 . B. Appendix 2: Plane curves whose curvature is a function of the distance to the origin This result was known already, see [24]. However we provide here a very simple proof. Consider a plane curve (x(t), y(t)), whose curvature is a function of the distance from the origin, i.e.: x˙ = cos(ϕ), y˙ = sin(ϕ), ϕ˙ = k(x2 + y 2 ).

(23)

Equation 23 is integrable. Proof: Set x ¯ = x cos(ϕ) + y sin(ϕ), y¯ = −x sin(ϕ) + y cos(ϕ). Then k(¯ x2 + y¯2 ) = k(x2 + y 2 ). Just computing, one gets: d¯ x = 1 + y¯k(¯ x2 + y¯2 ), (24) dt d¯ y = −¯ xk(¯ x2 + y¯2 ). dt We just show that (24) is a hamiltonian system. Since we are in dimension 2, it is always Liouville-integrable. Then, we are looking for solutions of the system of PDE’s: ∂H = 1 + y¯k(¯ x2 + y¯2 ), ∂x ¯ ∂H = −¯ xk(¯ x2 + y¯2 ). ∂ y¯ But the Schwartz integrability conditions are satisfied: ∂2H xy¯k 0 . ∂ y¯∂ x ¯ = 2¯

∂2H ∂x ¯∂ y¯

=

C. Appendix 3: periodicity of the optimal curves in the 6-2 case Proof: We consider the nilpotent approximation Pˆ6,2 given in formula 15: (Pˆ6,2 )

x2 x1 x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), (25) 2 2 x2 x1 x2 x1 z˙1 = x2 ( u1 − u2 ), z˙2 = x1 ( u1 − u2 ), 2 2 2 2 x2 x1 w˙ = Qw (x1 , x2 )( u1 − u2 ). 2 2

12

We consider the particular case of the ball with a trailer. Then, according to Lemma 16, the ratio r(ξ) = 1. It follows that the last equation can be rewritten w˙ = δ(w)((x1 )2 + (x2 )2 )( x22 u1 − x21 u2 ) for some never vanishing function δ(w) (vanishing would contradict the full rank of ∆(4) ). We can change the coordinate w for w ˜ such that dw dw ˜ = δ(w) . We get finally: x1 x2 (26) x˙ 1 = u1 , x˙ 2 = u2 , y˙ = ( u1 − u2 ), 2 2 x1 x2 x1 x2 z˙1 = x2 ( u1 − u2 ), z˙2 = x1 ( u1 − u2 ), 2 2 2 2 x1 2 2 x2 w˙ = ((x1 ) + (x2 ) )( u1 − u2 ) 2 2 6 This is a right invariant system on R with cooordinates ξ = (ς, w) = (x, y, z, w), for a certain Nilpotent Lie group structure over R6 (denoted by G). It is easily seen (just expressing right invariance) that the group law is ot the form (ς2 , w2 )(ς1 , w1 ) = (ς1 ∗ ς2 , w1 + w2 + Φ(ς1 , ς2 )), where ∗ is the multiplication of another Lie group structure on R5 , with coordinates ς (denoted by G0 ). In fact, G is a central extension of R by G0 . Z Lemma 27: The trajectories of (26) that maximize wdt ˙ (Pˆ6,2 )

in fixed time ε, with interpolating conditions ς(0) = ς(ε) = 0, have a periodic projection on ς (i.e. ς(t) is smooth and periodic of period ε). Remark 28: 1. Due to the invariance with respect to the w coordinate of (26), it is equivalent to consider the problem with the more restrictive terminal conditions ς(0) = ς(ε) = 0, w(0) = 0, 2. The scheme of this proof works also to show periodicity in the 4-2 and 5-2 cases. The idea for the proof was given to us by A. Agrachev. Proof: Let (ς, w1 ), (ς, w2 ) be initial and terminal points of an optimal solution of our problem. By right translation by (ς −1 , 0), this trajectory is mapped into another trajectory of the system, with initial and terminal points (0, w1 +Φ(ς, ς −1 )) and (0, w1 + Φ(ς, Zς −1 )). Hence, this trajectory has the same value of the cost

wdt. ˙ We see that the optimal cost is in

fact independant of the ς-coordinate of the initial and terminal condition. Z Therefore, the problem is the same as maximizing wdt ˙ but with the (larger) endpoint condition ς(0) = ς(ε) (free). Now, we can apply the general transversality conditions of Theorem 12.15 page 188 of [4]. It says that the initial and terminal covectors (p1ς , p1w ) and (p2ς , p2w ) are such that p1ς = p2ς . This is enough to show periodicity. R EFERENCES [1] A.A. Agrachev, H.E.A. Chakir, J.P. Gauthier, Subriemannian Metrics on R3 , in Geometric Control and Nonholonomic Mechanics, Mexico City 1996, pp. 29-76, Proc. Can. Math. Soc. 25, 1998. [2] A.A. Agrachev, J.P. Gauthier, Subriemannian Metrics and Isoperimetric Problems in the Contact Case, in honor L. Pontriaguin, 90th birthday commemoration, Contemporary Maths, Tome 64, pp. 5-48, 1999 (Russian). English version: journal of Mathematical sciences, Vol 103, N◦ 6, pp. 639-663.

[3] A.A. Agrachev, J.P. Gauthier, On the subanalyticity of Carnot Caratheodory distances, Annales de l’Institut Henri Poincar´e, AN 18, 3 (2001), pp. 359-382. [4] A.A. Agrachev, Y. Sachkov, Contro Theory from the geometric view point, Springer Verlag Berlin Heidelberg, 2004. [5] G. Charlot Quasi Contact SR Metrics: Normal Form in R2n , Wave Front and Caustic in R4 ; Acta Appl. Math., 74, N◦ 3, pp. 217-263, 2002. [6] H.E.A. Chakir, J.P. Gauthier, I.A.K. Kupka, Small Subriemannian Balls on R3 , Journal of Dynamical and Control Systems, Vol 2, N◦ 3, , pp. 359-421, 1996. [7] F.H. Clarke, Optimization and nonsmooth analysis, John Wiley & Sons, 1983. [8] J.P. Gauthier, F.Monroy-Perez, C. Romero-Melendez, On complexity and Motion Planning for Corank one SR Metrics, 2004, COCV; Vol 10, pp. 634-655. [9] J.P. Gauthier, V. Zakalyukin, On the codimension one Motion Planning Problem, JDCS, Vol. 11, N◦ 1, January 2005, pp.73-89. [10] J.P. Gauthier, V. Zakalyukin, On the One-Step-Bracket-Generating Motion Planning Problem, JDCS, Vol. 11, N◦ 2, april 2005, pp. 215-235. [11] J.P. Gauthier, V. Zakalyukin, Robot Motion Planning, a wild case, Proceedings of the Steklov Institute of Mathematics, Vol 250, pp.5669, 2005. [12] J.P. Gauthier, V. Zakalyukin, On the motion planning problem, complexity, entropy, and nonholonomic interpolation, Journal of dynamical and control systems, Vol. 12, N◦ 3, July 2006. [13] J.P. Gauthier, V. Zakalyukin , Entropy estimations for motion planning problems in robotics, Volume In honor of Dmitry Victorovich Anosov, Proceedings of the Steklov Mathematical Institute, Vol. 256, pp. 62-79, 2007. [14] JP Gauthier, B. Jakubczyk, V. Zakalyukin, Motion planning and fastly oscillating controls, SIAM Journ. On Control and Opt, Vol. 48 (5), pp. 3433-3448, 2010. [15] M. Gromov, Carnot Caratheodory Spaces Seen from Within, Eds A. Bellaiche, J.J. Risler, Birkhauser, pp. 79-323, 1996. [16] F. Jean, Complexity of Nonholonomic Motion Planning, International Journal on Control, Vol 74, N◦ 8, pp 776-782, 2001. [17] F. Jean, Entropy and Complexity of a Path in SR Geometry, COCV, Vol 9, pp. 485-506, 2003. [18] F. Jean, E. Falbel, Measures and transverse paths in SR geometry, Journal d’Analyse Math´ematique, Vol. 91, pp. 231-246, 2003. [19] T. Kato, Perturbation theory for linear operators, Springer Verlag 1966, pp. 120-122. [20] J.P. Laumond, (editor), Robot Motion Planning and Control, Lecture notes in Control and Information Sciences 229, Springer Verlag, 1998. [21] L. Pontryagin, V. Boltyanski, R. Gamkelidze, E. Michenko, The Mathematical theory of optimal processes, Wiley, New-York, 1962. [22] A.E.H. Love, A Treatise on the Mathematical Theory of Elasticity, Dover, New-York, 1944. [23] H.J. Sussmann, G. Lafferriere, Motion planning for controllable systems without drift; In Proceedings of the IEEE Conference on Robotics and Automation, Sacramento, CA, April 1991. IEEE Publications, New York, 1991, pp. 109-148. [24] D.A. Singer, Curves whose curvature depend on the distance from the origin, the American mathematical Monthly, 1999, vol. 106, no9, pp. 835-841. [25] H.J. Sussmann, W.S. Liu, Lie Bracket extensions and averaging: the single bracket generating case; in Nonholonomic Motion Planning, Z. X. Li and J. F. Canny Eds., Kluwer Academic Publishers, Boston, 1993, pp. 109-148.