Yacine Chitour
email: yacine.chitour@lss.supelec.fr

Frédéric Jean
email: frederic.jean@ensta-paristech.fr

Ruixing Long
email: rlong@princeton.edu

‡ F Jean

A Global Steering Method for Nonholonomic Systems *

 for chained-form systems.

Introduction

Nonholonomic systems have been attracting the attention of the scientific community for several years, due to the theoretical challenges they offer and the numerous important applications they cover. From the point of view of control theory, a nonholonomic system is a driftless control-affine system and is written as

(Σ) ẋ = m i=1 u i X i (x), x ∈ Ω, u = (u 1 , . . . , u m ) ∈ R m , (1.1)
where Ω is an open connected subset of R n , and X 1 , . . . , X m are C ∞ vector fields on Ω. Admissible inputs are R m -valued measurable functions u(•) defined on some interval [0, T ] and a trajectory of (Σ), corresponding to some x 0 ∈ Ω and to an admissible input u(•), is the (maximal) solution x(•) in Ω of the Cauchy problem defined by ẋ(t) = m i=1 u i (t)X i (x(t)), t ∈ [0, T ], and x(0) = x 0 . In this paper, we address the motion planning problem (MPP for short) for (Σ), namely determine a procedure which associates with every pair of points (p, q) ∈ Ω × Ω an admissible input u(•) defined on some interval [0, T ], such that the corresponding trajectory of (Σ) starting from p at t = 0 reaches q at t = T . As for the existence of a solution to MPP, this is equivalent to the complete controllability of (Σ). After the works of Chow and Rashevsky in the thirties [START_REF] Chow | Über Systeme von linearen partiellen Differentialgleichungen erster Ordnung[END_REF][START_REF] Rashevsky | Any two points of a totally nonholonomic space may be connected by an admissible line[END_REF], and that of Sussmann and Stefan in the seventies [START_REF] Sussmann | Orbits of Families of Vector Fields and Integrability of Distributions[END_REF][START_REF] Stefan | Accessible sets, orbits, and foliations with singularities[END_REF], the issue of complete controllability for nonholomic systems is well-understood and it is usually guaranteed by assuming that the Lie Algebraic Rank Condition (also known as the Hörmander condition) holds for (Σ). This easily checkable condition is not only sufficient for complete controllability but also necessary when the vector fields are analytic. From a practical viewpoint, assuming the LARC is, in a sense, the minimal requirement to ensure complete controllability for (Σ) and this is what we will do for all the control systems considered hereafter.

As for the construction of the solutions of the MPP, we present, in this paper, a complete procedure solving the MPP for a nonholonomic system subject to the sole LARC. By "complete procedure", we mean that the following properties must be guaranteed by the proposed procedure.

1. Global character of the algorithm: for every pair of points (p, q) in Ω, the algorithm must produce a steering control. (Note that the core of many algorithms consists in a local procedure and turning the latter into a global one is not always a trivial issue.)

2. Proof of convergence of the algorithm.

3. Regarding numerical implementations, no prohibitive limitation on the state dimension n.

4. Usefulness for practical applications, e.g., robustness with respect to the dynamics, "nice" trajectories produced by the algorithm, (no cusps neither large oscillations), and possibility of localizing the algorithm in order to handle obstacles (i.e., reducing the working space Ω to any smaller open and connected subset of R n ).

There exist of course several algorithms addressing the MPP in different contexts but most of them fail to verify all the aforementioned properties.

At first, in the case of specific classes of driftless nonholonomic systems (i.e. where more is known than the sole LARC), effective techniques have been proposed, among which a Lie bracket method for steering nilpotentizable systems (see [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] and [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF]), sinusoidal controls for chained-form systems (see [START_REF] Murray | Nonholonomic motion planning: Steering using sinusoids[END_REF]), averaging techniques for left-invariant systems defined on a Lie group (see [START_REF] Leonard | Motion Control of Drift-Free, Left-Invariant Systems on Lie Groups[END_REF][START_REF] Bullo | Controllability and Motion Algorithms for Underactuated Lagrangian Systems on Lie Groups[END_REF]), and a trajectory generation method for flat systems (see [START_REF] Fliess | Flatness and defect of nonlinear systems: Introductory theory and examples[END_REF]). Depending on the applications, these methods turn out to be extremely efficient, especially when the system to be steered is shown to be flat with an explicit flat output.

However, the class of systems considered previously is rather restrictive: for 2-input nonholonomic systems (i.e. m = 2), under suitable regularity assumptions, a flat system admits a feedback chainedform transformation (cf. [START_REF] Murray | Nilpotent bases for a class of non-integrable distributions with applications to trajectory generation for nonholonomic systems[END_REF][START_REF] Martin | Flat Systems: open problems, infinite dimensional extension, symmetries and catalog[END_REF]) and thus is exactly nilpotentizable; on the other hand, when the dimension of the state space is large enough, exact nilpotentizability is clearly a non generic property among 2-input nonholonomic systems. Moreover, there exist standard nonholonomic systems whose kinematic model does not fall into any of the aforementioned categories. For instance, mobile robots with more than one trailer cannot be transformed in chained-form unless each trailer is hinged to the midpoint of the previous wheel axle, an unusual situation in real vehicles. Another similar example is the rolling-body problem: even the simplest model in this category, the so-called plate-ball system, does not allow any chained-form transformation and is not flat.

Regarding general nonholonomic systems, various steering techniques have been proposed in the literature, and we only mention three of them: the iterative method, the generic loop method, and the continuation method. The first one, introduced in [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] and improved in [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF], is an approximation procedure which is exact for nilpotent systems. This method is proved to be convergent with the sole assumption of the LARC and actually meets most of the requirements to be a complete procedure in the sense defined previously. However, either the resulting trajectories in [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF] contain a large number of cusps (exponential with respect to the degree of nonholonomy), or the computation of the steering control in [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] requires the inversion of a system of algebraic equations. The latter turns out to be numerically intractable as soon as the dimension of the state is larger than six. Let us also mention a less important limitation for practical use. The iterative method described in [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF][START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF] makes use of several nonlinear changes of coordinates, which must be performed by numerical integration of ODEs at each step of the iterative method, thus leading to spurious on-line computations.

The generic loop method, presented in [START_REF] Sontag | Control of systems without drift via generic loops[END_REF], is based on a local deformation procedure and requires an a priori estimate of some "critical distance" which is, in general, an unknown parameter in practice. That fact translates into a severe drawback for constructing a globally valid algorithm. The continuation method of [START_REF] Sussmann | A continuation method for nonholonomic path finding[END_REF] and [START_REF] Chitour | A continuation method for motion planning problems[END_REF] belongs to the class of Newton-type methods. Proving its convergence amounts to show the global existence for the solution of a non linear differential equation, which relies on handling the abnormal extremals associated to the control system. That latter issue turns out to be a hard one, see [START_REF] Chitour | Genericity results for singular curves[END_REF][START_REF] Chitour | Singular trajectories of control-affine systems[END_REF] for instance. This is why, in the current state of knowledge, the continuation method can be proved to converge only under restrictive assumptions (see [START_REF] Chelouah | On the motion planning of rolling surfaces[END_REF][START_REF] Chitour | Path planning on compact Lie groups using a homotopy method[END_REF][START_REF] Chitour | Line-integral estimates and motion planning using the continuation method[END_REF]).

The algorithm considered in the present paper takes as starting point the globally convergent algorithm for steering regular nonholonomic systems discussed in [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF]. As the iterative method of [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF][START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF], that algorithm can be casted in the realm of nonlinear geometric control and roughly works as follows: one first solves the motion planning problem for a control system which is nilpotent and "approximates" system (1.1) in a suitable sense; then, one applies the resulting input û to (1.1) and iterates the procedure from the current point. If we use x(t, a, û), t ∈ [0, T ] to denote a trajectory of the "approximate" control system starting from a, a local version of this algorithm is summarized below, where d is an appropriate distance (to be defined in the next section) and e is a fixed positive real number.

Algorithm 1 Local Steering Algorithm

Require: x 0 , x 1 , e k = 0;

x k = x 0 ; while d(x k , x 1 ) > e do Compute ûk such that x 1 = x(T, x k , ûk );

x k+1 = AppSteer (x k , x 1 ) := x(T, x k , ûk ); k = k + 1; end while

We note that Algorithm 1 converges locally provided that the function AppSteer is locally contractive with respect to the distance d, i.e., for x 1 ∈ Ω, there exists ε x 1 > 0 and c x 1 < 1 such that

d(AppSteer(x, x 1 ), x 1 ) ≤ c x 1 d(x 1 , x), (1.2)
for x ∈ Ω and d(x 1 , x) < ε x 1 .

Assume now that we have a uniformly locally contractive function AppSteer on a connected compact set K ⊂ Ω, i.e. there exists ε K > 0 and c K ∈ (0, 1) < 1 such that d(AppSteer(x, x 1 ), x 1 ) ≤ c K d(x 1 , x), (1.3) for x, x 1 ∈ K and d(x 1 , x) < ε K .

Based on the local algorithm, a global approximate steering algorithm on K can be built along the line of the following idea (a similar procedure is proposed in [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF]): consider a parameterized path γ ⊂ K connecting x 0 to x 1 . Then pick a finite sequence of intermediate goals {x d 0 = x 0 , x d 1 , . . . , x d j = x 1 } on γ such that d(x d i-1 , x d i ) < ε K /2, i = 0, . . . , j. One can prove that the iterated application of a uniformly locally contractive AppSteer(x i-1 , x d i ) from the current state to the next subgoal (having set x d i = x 1 , for i ≥ j) yields a sequence x i converging to x 1 .

To turn the above idea into a practically efficient algorithm, three issues must be successfully addressed:

(P-1) Construct a uniformly locally contractive local approximate steering method.

(P-2) The "approximate" control ûk must be exact for steering the "approximate system" from the current point x k to the final point x 1 . As this computation occurs at each iteration, it must be performed in a reasonable time.

(P-3) Since the knowledge of the "critical distance" ε K is not available in practice, the algorithm should achieve global convergence without explicit knowledge of ε K .

Issues (P-1) and (P-3) are solved in [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF] under the assumption that the control system is regular (cf. Definition 2.9 below). The solution proposed therein relies on the understanding of the geometry defined by the nonholonomic system (cf. [START_REF] Bellaïche | Geometry of Nonholonomic Systems[END_REF]). That geometry is a sub-Riemannian one and it endows the working space Ω with a sub-Riemannian metric d for which the aforementioned function AppSteer is contractive. Moreover, the approximation of the original system adapted to the motion planning turns out to be the approximation at the first order with respect to d (cf. [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF]). However, the regularity assumption for the control system is rather restrictive: in general, nonholonomic systems do exhibit singularities (cf. [START_REF] Vershik | Nonholonomic dynamical systems, geometry of distributions and variational problems[END_REF]). A solution also exists in the case of a non regular control system [START_REF] Vendittelli | Non-homogeneous nilpotent approximation for systems with singularities[END_REF], but only when the state dimension n is less than or equal to 5. In the present paper, we completely remove the regularity assumption and solve Issues (P-1) and (P-3) for every nonholonomic control system. The solution is based on an explicit desingularization procedure: adding new variables (thus augmenting the dimension of the state space), we construct a "lifted" control system which is regular and whose projection is the original control system. The solution of Issue (P-1) described in [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF] can thus be applied to the "lifted" control system, as well as the globally convergent motion planning algorithm solving Issue (P-3) proposed therein. Note that other desingularization procedures already exist [START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF][START_REF] Goodman | Nilpotent Lie groups[END_REF][START_REF] Jean | Uniform estimation of sub-Riemannian balls[END_REF][START_REF] Rothschild | Hypoelliptic differential operators and nilpotent groups[END_REF]], but we insist on the fact that the one we propose here involves only explicit polynomial transformations. It numerically translates to the fact that these changes of variables can be performed off-line once each local procedure is identified.

As regards Issue (P-2), several algorithms were proposed for computing û, i.e. for controlling nilpotent systems. In [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF], the authors make use of piecewise constant controls and obtain smooth controls by imposing some special parameterization (namely by requiring the control system to stop during the control process). In that case, the smoothness of the inputs is recovered by using a reparameterization of the time, which cannot prevent in general the occurrence of cusps or corners for the corresponding trajectories. However, smoothness of the trajectories is generally mandatory for robotic applications. Therefore, the method proposed in [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] is not adapted to such applications. In [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF], the proposed controls are polynomial (in time), but an algebraic system must be inverted in order to access to these inputs. The size and the degree of this algebraic system increase exponentially with respect to the dimension of state space, and there does not exist a general efficient exact method to solve it. Even the existence of solutions is a non trivial issue. Furthermore, the methods [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] and [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF] both make use of exponential coordinates which are not explicit and thus require in general numerical integrations of nonlinear differential equations. That prevents the use of these methods in an iterative scheme such as Algorithm 1. Let us also mention the path approximation method by Liu and Sussmann [START_REF] Liu | An approximation algorithm for nonholonomic systems[END_REF], which uses unbounded sequences of sinusoids. Even though this method bears similar theoretical aspects with our method (see especially the argument strategy in order to prove Lemma 5.8, which is borrowed from [START_REF] Liu | An approximation algorithm for nonholonomic systems[END_REF]), it is not adapted from a numerical point of view to the motion planning issue since it relies on a limit process of highly oscillating inputs. In the present paper, we present an exact steering algorithm for general nilpotent systems is provided, which uses sinusoidal inputs and which can be applied for controlling the approximate (nilpotent) system used in [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF]. Our method generalizes the one proposed in [START_REF] Murray | Nonholonomic motion planning: Steering using sinusoids[END_REF] for controlling chained-form systems, which is briefly recalled next: after having brought the system under a "canonical" form, the authors of [START_REF] Murray | Nonholonomic motion planning: Steering using sinusoids[END_REF] proceed by controlling component after component by using, for each component, two sinusoids with suitably chosen frequencies. In the present paper, we show for general nilpotent systems that, with more frequencies for each component, one can steer an arbitrary component independently on the other components. We are also able to construct inputs which give rise to C 1 -trajectories.

We now describe in a condensed manner the global motion planning strategy developed in this paper. The latter is presented as an algorithmic procedure associated with a given nonholonomic system (Σ) defined on Ω ⊂ R n . The required inputs are initial and final points x initial and x final belonging to Ω, a tolerance e > 0, and a compact convex set K ⊂ Ω (of appropriate size) equal to the closure of its interior which is a neighborhood of both x initial and x final . For instance, K can be chosen to be a large enough compact tubular neighborhood constructed around a curve joining x initial and x final . The global steering method is summarized in Algorithm 2.

Algorithm 2 Global Approximate Steering Algorithm: Global(x initial , x final , e, K)

1: Build a decomposition of K into a finite number of compacts sets V c J i , with i = 1, . . . , M (Section 3.3). 2: Construct the connectedness graph G := (N, E) associated with this decomposition and choose a simple path p := {j 0 , j 1 . . . , j M } in G such that x initial ∈ V c

J j 0 and x final ∈ V c J j M (Section 3.3). 3: Choose a sequence (x i ) i=1,..., M -1 such that x i ∈ V c J j i ∩ V c J j i+1 . 4: Set x := x initial . 5: for i = 1, . . . , M -1 do 6:
Apply the Desingularization Algorithm at a := x i with J := J i (Section 3.3). {the output is an m-tuple of vector fields ξ on V J i × R n which is free up to step r.}

7:

Let AppSteer be the LAS method associated to the approximation A ξ of ξ on V J i × R n defined in Section 4.1 and to its steering law Exact m,r constructed in Section 5.3).

8:

Set x 0 := (x, 0), x 1 := (x i , 0), and

V c := V c J i × B R (0)
with R > 0 large enough.

9:

Apply GlobalFree( x 0 , x 1 , e, V c , AppSteer) to ξ (Section 4.2).

{the algorithm stops at a point x which is e-close to x 1 ;} 10:

return x := π( x). {π : V J i × R n → V J i is the canonical projection.} 11: end for
The paper is devoted to the construction of the various steps of this algorithm. We will also show that each of these steps is conceived so that the overall construction is a complete procedure in the sense defined previously. In particular, the convergence issue is addressed in the following theorem.

Theorem 1.1. Let (Σ) be a nonholonomic system on Ω ⊂ R n satisfying the LARC. For every e > 0, every connected compact set K which is equal to the closure of its interior, and every pair of points (x initial , x final ) in the interior of K, Algorithm 2 steers, in a finite number of steps, the control system (Σ) from x initial to a point x ∈ K such that d(x, x final ) < e.

Before providing the structure of the paper, we mention possible extensions of our algorithm. The first one concerns the working space Ω. Since it is an arbitrary open connected set of R n , one can extend the algorithm to the case where the working space is a smooth connected manifold of finite dimension. From a numerical point of view, there would be the additional burden of computing the charts. A second extension deals with the stabilization issue. Indeed, at the heart of the algorithm lies an iterative procedure such as Algorithm 1, which can be easily adapted for stabilization tasks (cf. [START_REF] Oriolo | A framework for the stabilization of general nonholonomic systems with an application to the plate-ball mechanism[END_REF]). Another possible generalization takes advantage of devising from our algorithm a globally regular input, one can then address the motion planning of dynamical extensions of the nonholonomic control systems considered in the present paper. Finally, let us point out the modular nature of Algorithm 2: one can propose other approaches to obtain uniformly contractive local methods (other desingularization methods or different ways of dealing with singular points), or replace Exact m,r (•) by more efficient control strategies for general nilpotent systems.

The paper is organized as follows. In Section 2, we define properly the notion of first order approximation. We then propose, in Section 3, a purely polynomial desingularization procedure based on a lifting method. In Section 4, we describe in detail the globally convergent steering algorithm given in [START_REF] Jean | A global convergent steering algorithm for regular nonholonomic systems[END_REF] for regular systems together with a proof of convergence. In Section 5, we present an exact steering procedure for general nilpotent systems using sinusoids, and we gather, in the appendix, the proof of Theorem 1.1 and some additional comments.

Notations and Definitions

Let n and m be two positive integers. Let Ω and V F (Ω) be respectively an open connected subset of R n and the set of C ∞ vector fields on Ω. Consider m vector fields X 1 , . . . , X m of V F (Ω), and the associated driftless control-affine nonholonomic system given by

ẋ = m i=1 u i X i (x), x ∈ Ω, (2.1)
where

u = (u 1 , • • • , u m ) ∈ R m and the input u(•) = (u 1 (•), . . . , u m (•)
) is an integrable vector-valued function defined on [0, T ], with T a fixed positive real number. We also assume that (2.1) is complete, i.e., for every a ∈ Ω and input u(•), the Cauchy problem defined by (2.1) starting from a at t = 0 and corresponding to u(•) admits a unique (absolutely continuous) solution x(•, a, u) defined on [0, T ] and called the trajectory of (2.1) starting from a at t = 0 and corresponding to the input u(•). A point x ∈ Ω is said to be accessible from a if there exists an input u : [0, T ] → R m and a time t ∈ [0, T ] such that x = x(t, a, u). Then, System (2.1) is said to be completely controllable if any two points in Ω are accessible one from each other (see [START_REF] Sussmann | A general theorem on local controllability[END_REF]).

We next provide a classical condition ensuring that System (2.1) is controllable. We first need the following definition. Definition 2.1 (Lie Algebraic Rank Condition (LARC)). Let L(X) be the Lie algebra generated by the vectors fields X 1 , . . . , X m and L(x) be the linear subspace of R n equal to the evaluation of L(X) at every point x ∈ Ω (see [START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF]). If L(x) = R n , we say that the Lie Algebraic Rank Condition (LARC for short) is verified at x ∈ Ω. If this is the case at every point x ∈ Ω, we say that System (2.1) satisfies the LARC.

Chow's Theorem essentially asserts that, if System (2.1) satisfies the LARC then it is completely controllable (cf. [START_REF] Chow | Über Systeme von linearen partiellen Differentialgleichungen erster Ordnung[END_REF]).

Remark 2.1. For the sake of clarity, we assume through this paper that the control set is equal to R m . However, it is well-known that Chow's theorem only requires that the convex hull of the control set contains a neighborhood of the origin in R m (see for instance [START_REF] Jurdjevic | Geometric Control Theory[END_REF]Chapter 4,Theorem 2]). We will explain later how we can adapt our method to the case with constraints on the control set (see Appendix B). Moreover, it is worth recalling that complete controllability for (Σ) does not imply that LARC holds true for (Σ) if the vector fields X 1 , . . . , X m are only smooth, but this is the case if X 1 , . . . , X m are analytic (cf. [START_REF] Agrachev | Control theory from the geometric viewpoint[END_REF]Chapter 5]).

Throughout this paper, we will only consider driftless control-affine nonholonomic systems of the type (2.1) verifying the LARC, and thus completely controllable. In that context, the motion planning problem will be defined as follows: find a procedure which furnishes, for every two points x 0 , x 1 ∈ Ω, an input u steering (2.1) from x 0 to x 1 , i.e., x(T, x 0 , u) = x 1 .

Our solution to this problem relies heavily on the underlying geometry, which is a sub-Riemannian geometry. We provide in Section 2.1 the useful definitions and refer the reader to [START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF] for more details. We then introduce in Section 2.2 a notion of approximate steering method related to this geometry.

Basic facts on sub-Riemannian geometry

2.1.1 Sub-Riemannian distance and Nonholonomic order Definition 2.2 (Length of an input). The length of an input u is defined by

ℓ(u) = T 0 u 2 1 (t) + • • • + u 2 m (t)dt,
and the length of a trajectory x(•, a, u) is defined by ℓ(x(•, a, u)) := ℓ(u).

The appropriate notion of distance associated with the control system (2.1) and closely related to the notion of accessibility is that of sub-Riemannian distance, also called control distance.

Definition 2.3 (Sub-Riemannian distance). The vector fields X 1 , . . . , X m induce a function d on Ω, defined by d(x 1 , x 2 ) := inf u ℓ(x(•, x 1 , u)), for every points x 1 , x 2 in Ω, where the infimum is taken over all the inputs u such that x(•, x 1 , u) is defined on [0, T ] and x(T, x 1 , u) = x 2 . We say that the function d is the sub-Riemannian distance associated with X 1 , . . . , X m .

Remark 2.2. The function d defined above is a distance in the usual sense, i.e., it verifies (i) d(x 1 , x 2 ) ≥ 0 and d(x 1 , x 2 ) = 0 if and only if

x 1 = x 2 ; (ii) symmetry: d(x 1 , x 2 ) = d(x 2 , x 1 ); (iii) triangular inequality: d(x 1 , x 3 ) ≤ d(x 1 , x 2 ) + d(x 2 , x 3
). Notice that one always has d(x 1 , x 2 ) < ∞ since the control system is assumed to be completely controllable. Definition 2.4 (Nonholonomic derivatives of a function). If f : Ω → R n is a smooth function, the first-order nonholonomic derivatives of f are the Lie derivatives X i f of f along X i , i = 1, . . . , m. Similarly, X i (X j f ), i, j = 1, . . . , m, are called the second-order nonholonomic derivatives of f , and more generally,

X i 1 • • • X i k f , i 1 , . . . , i k ∈ {1, . . . , m} are the k th -order nonholonomic derivatives of f ,
where k is any positive integer. Proposition 2.1 ([2, Proposition 4.10, page 34]). Let s be a non-negative integer. For a smooth function f defined near a ∈ Ω, the following conditions are equivalent:

(i) f (x) = O(d s (x, a)) for x in a neighborhood of a;
(ii) all the nonholonomic derivatives of order ≤ s -1 of f vanish at a. Definition 2.5 (Nonholonomic order of a function). Let s and f be respectively a non-negative integer and a smooth real-valued function defined on Ω. If Condition (i) or (ii) of Proposition 2.1 holds, we say that f is of order ≥ s at a ∈ Ω. If f is of order ≥ s but not of order ≥ s + 1 at a, we say that f is of order s at a. The order of f at a will be denoted by ord a (f ). Definition 2.6 (Nonholonomic order of a vector field ). Let q be an integer. A vector field Y ∈ V F (Ω) is of order ≥ q at a ∈ Ω if, for every non-negative integer s and every smooth function f of order s at a, the Lie derivative Y f is of order ≥ q + s at a. If Y is of order ≥ q but not ≥ q + 1, it is of order q at a. The order of Y at a will be denoted by ord a (Y ). Definition 2.7 (Nonholonomic first order approximation at a). An m-tuple X a := { X a 1 , . . . , X a m }, defined on B(a, ρ a ) := {x ∈ Ω, d(x, a) ≤ ρ a } with ρ a > 0, is said to be a nonholonomic first order approximation of X := {X 1 , . . . , X m } at a ∈ Ω, if the vector fields X i -X a i , for i = 1, . . . , m, are of order ≥ 0 at a. The positive number ρ a is called the approximate radius at a. Remark 2.3. As a consequence of Definition 2.7, one gets that the nonholonomic order at a defined by the vector fields X a 1 , . . . , X a m coincides with the one defined by X 1 , . . . , X m .

Privileged coordinates

The changes of coordinates take an important place in this paper, whether it is to estimate the sub-Riemannian distance, or to compute the order of functions and vector fields, or to transform a control system into a normal form. To avoid heavy notations, we will need some conventions and simplifications that we fix now for the rest of the paper.

A point in Ω ⊂ R n is denoted by x = (x 1 , . . . , x n ) and the canonical basis of R n by (∂ x 1 , . . . , ∂ xn ). Even though x is a point, we will sometimes refer to (x 1 , . . . , x n ) as the original coordinates. A system of local coordinates y = (y 1 , . . . , y n ) at a point a ∈ Ω is defined as a diffeomorphism ϕ between an open neighborhood N a ⊂ Ω of a and an open neighborhood N ϕ(a) ⊂ R n of ϕ(a), ϕ : x → y = (y 1 , . . . , y n ). If the diffeomorphism ϕ is defined on Ω, then y = (y 1 , . . . , y n ) is said to be a system of global coordinates on Ω. A system of global coordinates is said to be affine (resp. linear ) if the corresponding diffeomorphism ϕ is affine (resp. linear). If f is a function defined on N a , the function f • ϕ -1 defined on N ϕ(a) will be called f (expressed) in coordinates (y 1 , . . . , y n ). If X ∈ V F (Ω) is a vector field, the push-forward

ϕ * X = dϕ • X • ϕ -1 ∈ V F (N ϕ(a)
) will be called X (expressed) in coordinates (y 1 , . . . , y n ).

For the sake of simplicity, we will in general not introduce the notation ϕ and, with a slight abuse of the notation, replace it by y. Thus we write y(x) or (y 1 (x), . . . , y n (x)) instead of ϕ(x). The function f • ϕ -1 will be denoted by f (y), and the vector field ϕ * X by X(y) or y * X. The values at a point ȳ ∈ N ϕ(a) will be denoted respectively by f (y) |y=ȳ and X(y) |y=ȳ .

A special class of coordinates, called privileged coordinates and defined below, turns out to be a useful tool to compute the order of functions and vector fields, and to estimate the sub-Riemannian distance d.

We will use L s (X) to denote the Lie sub-algebra of elements of length (cf. Definitions 3.1 and 3.4) not greater than s ∈ N. Take x ∈ Ω and let L s (x) be the vector space generated by the values at x of elements belonging to L s (X). Since System (2.1) verifies the LARC at every point x ∈ Ω, there exists a smallest integer r := r(x) such that dim L r (x) = n. This integer is called the degree of nonholonomy at x. Definition 2.8 (Growth vector ). For a ∈ Ω, let n s (a) := dim L s (a), s = 1, . . . , r. The sequence (n 1 (a), . . . , n r (a)) is the growth vector of X at a. Definition 2.9 (Regular and singular points). A point a ∈ Ω is said to be regular if the growth vector remains constant in a neighborhood of a and, otherwise, a is said to be singular. The nonholonomic System (2.1) (or the m-tuple X) is said to be regular if every point in Ω is regular.

Note that regular points form an open and dense set in Ω.

Definition 2.10 (Weight). For a ∈ Ω and j = 1, . . . , n, let w j := w j (a) be the integer defined by setting w j := s if n s-1 < j ≤ n s , with n s := n s (a) and n 0 := 0. The integers w j , for j = 1, . . . , n are called the weight at a. Remark 2.4. The meaning of Definition 2.10 can be understood in another way. Choose first some vector fields W 1 , . . . , W n 1 in L 1 (X) such that W 1 (a), . . . , W n 1 (a) form a basis of L 1 (a). Choose then other vectors fields W n 1 +1 , . . . , W n 2 in L 2 (X) such that W 1 (a), . . . , W n 2 (a) form a basis of L 2 (a) and, for every positive integer s, choose W n s-1 +1 , . . . , W ns in L s (X) such that W 1 (a), . . . , W ns (a) form a basis of L s (a). We obtain in this way a sequence of vector fields W 1 , . . . , W n such that

W 1 (a), . . . , W n (a) is a basis of R n , W i ∈ L w i (X), i = 1, . . . , n.
(2.2)

A sequence of vector fields verifying Eq. (2.2) is called an adapted frame at a. The word "adapted" means "adapted to the flag L 1 (a) ⊂ L 2 (a) ⊂ • • • ⊂ L r (a) = R n ", since the values at a of an adapted frame contain a basis W 1 (a), . . . , W ns (a) of every subspace L s (a) of the flag. The values of W 1 , . . . , W n at a point b close to a also form a basis of R n . However, if a is singular, this basis may be not adapted to the flag

L 1 (b) ⊂ L 2 (b) ⊂ • • • ⊂ L r(b) (b) = R n .
Definition 2.11 (Privileged coordinates at a). A system of privileged coordinates at a ∈ Ω is a system of local coordinates (z 1 , . . . , z n ) centered at a (the image of a is 0) such that ord a (z j (x)) = w j , for j = 1, . . . , n.

Remark 2.5. For every system of local coordinates (y 1 , . . . , y n ) centered at a, we have, up to a reordering, ord a (y j ) ≤ w j or, without re-ordering, n j=1 ord a (y j ) ≤ n j=1 w j . The order at a ∈ Ω of functions and vector fields expressed in a system of privileged coordinates (z 1 , . . . , z n ) centered at a can be evaluated algebraically as follows:

• the order of the monomial z α 1 1 . . . z αn n is equal to its weighted degree w(α

) := w 1 α 1 + • • • + w n α n ;
• the order of a function f (z) at z = 0 is the least weighted degree of the monomials occurring in the Taylor expansion of f (z) at 0;

• the order of the monomial vector field z α 1 1 . . . z αn n ∂ z j is equal to its weighted degree w(α)w j , where one assigns the weight -w j to ∂ z j at 0;

• the order of a vector field W (z) = n j=1 W j (z)∂ z j at z = 0 is the least weighted degree of the monomials occurring in the Taylor expansion of W at 0. Definition 2.12 (Continuously varying system of privileged coordinates). A continuously varying system of privileged coordinates on Ω is a mapping Φ taking values in R n , defined and continuous on a neighborhood of the set {(x, x), x ∈ Ω} ⊂ Ω × Ω, and so that the partial mapping z := Φ(a, •) is a system of privileged coordinates at a. In this case, there exists a continuous function ρ : Ω → (0, +∞) such that the coordinates Φ(a, •) are defined on B(a, ρ(a)). We call ρ an injectivity radius function of Φ. Definition 2.13 (Pseudo-norm). Let a ∈ Ω and w 1 , . . . , w n the weights at a. The application from R n to R defined by

z a := |z 1 | 1/w 1 + • • • + |z n | 1/wn , z = (z 1 , . . . , z n ) ∈ R n , is called the pseudo-norm at a.

Distance and error estimates

Privileged coordinates provide estimates of the sub-Riemannian distance d, according to the following result.

Theorem 2.2 (Ball-Box Theorem [START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF]). Consider (X 1 , . . . , X m ) ∈ VF (Ω) m , a point a ∈ Ω, and a system of privileged coordinates z at a. There exist positive constants C d (a) and ε d (a) such that, for every x ∈ Ω with d(a, x) < ε d (a), one has

1 C d (a) z(x) a ≤ d(a, x) ≤ C d (a) z(x) a . (2.3)
If Ω contains only regular points and if Φ is a continuously varying system of privileged coordinates on Ω, then there exist continuous positive functions C d (•) and ε d (•) on Ω such that Eq. (2.3) holds true with z = Φ(a, •) at all (x, a) satisfying d(x, a) < ε d (a).

Corollary 2.3. Let K be a compact subset of Ω. Assume that K only contains regular points and there exists a continuously varying system of privileged coordinates Φ on K. Then, there exist positive constants C K and ε K such that, for every pair (a, x) ∈ K × K verifying d(a, x) < ε K , one has

1 C K Φ(a, x) a ≤ d(a, x) ≤ C K Φ(a, x) a . (2.4)
Privileged coordinates also allow one to measure the error obtained when X is replaced by an approximation X.

Proposition 2.4 ([2, Prop. 7.29]). Consider a point a ∈ Ω, a system of privileged coordinates z at a, and an approximation X of X at a. Then, there exist positive constants C e (a) and ε e (a) such that, for every x ∈ Ω with d(a, x) < ε e (a) and every integrable input function u(•) with ℓ(u) < ε e (a), one has

z(x(T, x, u)) -z(x(T, x, u)) a ≤ C e (a) max z(x) a , ℓ(u) ℓ(u) 1/r , (2.5)
where r is the degree of nonholonomy at a, x(•, x, u) and x(•, x, u) are respectively the trajectories of ẋ = m i=1 u i X i (x), and ẋ = m i=1 u i X i (x).

Approximate steering method

Definition 2.14 (Nonholonomic first order approximation on Ω). A nonholonomic first order approximation of X on Ω is a mapping A which associates, with every a ∈ Ω, a nonholonomic first order approximation of X at a defined on B(a, ρ a ), i.e., A(a) := X a on B(a, ρ a ). The approximation radius function of A is the function ρ : Ω → (0, ∞) which associates, with every a, its approximate radius ρ a , i.e., ρ(a) := ρ a .

In the sequel, nonholonomic first-order approximations will simply be called approximations. Useful properties of approximations are continuity and nilpotency.

Definition 2.15 (Continuity and nilpotency of an approximation). Let A : a → X a be an approximation on Ω.

• We say that A is continuous if (i) the mapping (a, x) → A(a)(x) is well-defined and, for every a ∈ Ω, is continuous on a neighborhood of (a, a) ∈ Ω × Ω;

(ii) the approximation radius function ρ of A is continuous.

• We say that A is nilpotent of step s ∈ N if, for every a ∈ Ω, the Lie algebra generated by X a is nilpotent of step s, i.e. every Lie bracket of length larger than s is equal to zero. (For a definition of the length of a Lie bracket, see Definitions 3.1 and 3.4.)

Consider a m-tuple of vector fields

X = {X 1 , . . . , X m } in V F m (Ω).
Definition 2.16 (Steering law of an approximation). Let A : a → X a be an approximation of X on Ω and ρ its approximation radius function. A steering law of A is a mapping which, to every pair (x, a) ∈ Ω × Ω verifying d(x, a) < ρ(a), associates an integrable input function û : [0, t] → R m , henceforth called a steering control, such that the trajectory x(•, x, û) of the approximate control system

ẋ = m i=1 u i X a i (x), (2.6)
is defined on [0, T ] and satisfies x(T, x, û) = a. In other words, û(•) steers (2.6) from x to a.

A steering law of an approximation is intended to be used as an approximate steering law for the original system. For that purpose, it is important to have a continuity property of the steering control: the closest are x and a, the smaller is the length of û. We introduce the stronger notion of sub-optimality (which is a sort of Lipschitz continuity of the steering law). Definition 2.17 (Sub-optimal steering law ). Let A be an approximation of X on Ω and, for every a ∈ Ω, let da be the sub-Riemannian distance associated to A(a). We say that a steering law of A is sub-optimal if there exists a constant C ℓ > 0 and a continuous positive function ε ℓ (•) such that, for any a, x ∈ Ω with d(a, x) < ε ℓ (a), the control û(•) steering (2.6) from x to a satisfies:

ℓ(û) ≤ C ℓ da (x, a) = C ℓ da (x(0, x, û), x(T, x, û)).
Due to the definition of the sub-Riemannian distance da , sub-optimal steering laws always exist. Given an approximation A of X and a steering law for A, we define a local approximate steering method for X as follows.

Definition 2.18 (Local approximate steering). The local approximate steering (LAS for short) method associated to A and its steering law is the mapping AppSteer(•, •) which associates, with every pair (x, a) ∈ Ω × Ω verifying d(x, a) < ρ(a), the point x(T, x, û), i.e., AppSteer(x, a) := x(T, x, û), where û(•) is the steering control of A(a) associated to (x, a) and ρ is the approximation radius function of A.

Definition 2.19 (Local contractions and uniform local contractions).

A LAS method is locally contractive if, for every a ∈ Ω, there exist ε a > 0 and c a < 1 such that one has:

d(a, x) < ε a =⇒ d(a, AppSteer(x, a)) ≤ c a d(a, x).
A LAS method is uniformly locally contractive on a compact set K ⊂ Ω if it is locally contractive, and if ε a and c a are independent of a, i.e., there exists ε K > 0 and c K < 1 such that, for every pair (a, x) ∈ K × K, the following implication holds true:

d(a, x) < ε K =⇒ d(a, AppSteer(x, a)) ≤ c K d(a, x).
Remark 2.6. We will show that if X is an approximation of X at a, the corresponding AppSteer function is locally contractive in a neighborhood of a. By the Fixed Point Theorem, one gets local convergence of Algorithm 1 (LAS). However, in order to obtain a globally convergent algorithm from LAS, one needs AppSteer to be uniformly locally contractive. In other words, the mapping A needs to be continuous in the sense of Definition 2.15.

As a direct consequence of Proposition 2.4, we obtain sufficient conditions for a LAS method to be uniformly locally contractive.

Corollary 2.5. Let K be a compact subset of Ω. Assume that:

(i) all points in K are regular;

(ii) there exists a continuously varying system of privileged coordinates Φ on K;

(iii) there exists a continuous approximation A of X on K;

(iv) A is provided with a sub-optimal steering law.

Then, the LAS method AppSteer associated to A and its steering law is uniformly locally contractive. Moreover, up to reducing the positive constant ε K occurring in Corollary 2.3, one has, for every pair

(a, x) ∈ K × K verifying d(a, x) < ε K , d(AppSteer(x, a), a) ≤ 1 2 d(x, a), (2.7
) (2.4) and (2.8) and based on privileged coordinates do not hold true uniformly. In particular, if (a n ) is a sequence of regular points converging to a singular point a (this is possible since regular points are dense in Ω), the sequences ε d (a n ) and ε e (a n ) tend to zero whereas ε d (a) and ε e (a) are not equal to zero.

z(AppSteer(x, a)) a ≤ 1 2 z(x) a . ( 2
Remark 2.8. A similar discontinuity issue occurs of course for the approximate system. Indeed, if a is a singular point, the growth vector and the weights of the associated privileged coordinates at a change around a, implying a change of the truncation order in the Taylor expansion of the vector fields. Therefore, the approximate vector fields cannot vary continuously in any neighborhood of a singular point.

Desingularization by Lifting

As it appears in Corollary 2.5, the absence of singular points is one of the key features in order to construct uniformly locally contractive LAS method. As a matter of fact, we will show in Section 4.2 how to construct a globally convergent motion planning algorithm for a regular nonholonomic system (i.e., when all points in Ω are regular). However, in general, nonholonomic systems do have singular points. For such systems, attempts have been made to construct specific LAS methods (see [START_REF] Vendittelli | Non-homogeneous nilpotent approximation for systems with singularities[END_REF][START_REF] Jean | An approximate algorithm for nonholonomic motion planning[END_REF]), but additional conditions on the structure of the singularities are required. Our approach here is different: we present in this section a desingularization procedure of the system, in such a way to replace a MPP for a non regular system by a MPP for a regular one.

The strategy consists in "lifting" the vector fields {X 1 , . . . , X m } ∈ V F m (Ω) defining the control system to some extended domain Ω := Ω × R ñ, with n ∈ N to be defined later. The lifted vector fields {ξ 1 , . . . , ξ m } ∈ V F m ( Ω) are constructed so that:

(i) for i = 1, . . . , m, ξ i has the following form,

ξ i (x, y) = X i (x) + n j=1 b ij (x, y)∂ y j , (x, y) ∈ Ω × R ñ,
where the functions b ij , j = 1, . . . , ñ, are smooth;

(ii) the Lie algebra generated by {ξ 1 , . . . , ξ m } is free up to step r (see Def. 3.6 below).

Point (ii) guarantees that the nonholonomic system defined by {ξ 1 , . . . , ξ m } is regular, since its growth vector is constant on Ω. Point (i) guarantees that one obtains X 1 , . . . , X m by projecting ξ 1 , . . . , ξ m on R n . Indeed, let π be the canonical projector from Ω to Ω defined by π( x) = x, where x = (x, y) ∈ Ω. Then, denoting dπ x the differential of π at x, one has

dπ x (ξ i ( x)) = X i (π( x)).
As a consequence, the projection by π of a trajectory x(•, x 0 , u) of the control system

˙ x = m i=1 u i ξ i ( x), x ∈ Ω, ( 3.1)
is a trajectory of (2.1) associated to the same input, i.e., π x(•, x 0 , u) = x(•, π( x 0 ), u). Therefore, any control u steering System (3.1) from a point x 0 := (x 0 , 0) to a point x 1 := (x 1 , 0) also steers System (2.1) from x 0 to x 1 . It then suffices to solve the MPP for the regular System (3.1).

Note that distinguished desingularization procedures already exist, cf. [START_REF] Rothschild | Hypoelliptic differential operators and nilpotent groups[END_REF][START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF][START_REF] Jean | Uniform estimation of sub-Riemannian balls[END_REF]. However, an important property of the desingularization procedure presented here is that all the changes of coordinates and intermediate constructions involved in it are explicit and purely algebraic. Note also that, during the lifting process, we obtain, as a byproduct, a nonholonomic first order approximation of {ξ 1 , . . . , ξ m } in a "canonical" form, which can be exactly controlled by sinusoids (see Section 5).

We start this section by presenting some general facts on free Lie algebras, namely the P. Hall basis in Section 3.1, and the canonical form of a nilpotent Lie algebra of step r in Section 3.2. We then give one desingularization procedure in Section 3.3. The proofs of the results stated in Section 3.3 will be gathered in Section 3.4.

P. Hall basis on a free Lie algebra and evaluation map

In this section, we present some general facts on free Lie algebras. The reader is referred to [START_REF] Bourbaki | Groupe et Algèbre de Lie[END_REF] for more details. Consider I := {1, . . . , m}, and the free Lie algebra L(I) generated by the elements of I. Recall that L(I) is the R-vector space generated by the elements of I and their formal brackets [ , ], together with the relations of skew-symmetry and the Jacobi identity enforced (see [START_REF] Bourbaki | Groupe et Algèbre de Lie[END_REF] for more details). We note that, by construction, for every I ∈ L(I), there exists ( (3.2)

I 1 , I 2 ) ∈ L(I) × L(I) such that I = [I 1 , I 2 ].
|I| := |I 1 | + |I 2 |, for I = [I 1 , I 2 ], with I 1 , I 2 ∈ L(X). (3.3)
We use L s (I) to denote the subspace generated by elements of L(I) of length not greater than s. Let n s be the dimension of L s (I).

The P. Hall basis of L(I) is a totally ordered subset of L(I) defined as follows. (H4) an element I k ∈ L(I) of length greater than 3 belongs to

H if I k = [I k 1 , [I k 2 , I k 3 ]] with I k 1 , I k 2 , I k 3 , and [I k 2 belonging to H, I k 2 ≺ I k 3 , I k 2 ≺ I k 1 or I k 2 = I k 1 , and I k 1 ≺ [I k 2 , I k 3 ].
The elements of H form a basis of L(I), and " ≺ " defines a strict and total order over the set H. In the sequel, we use I k to denote the k th element of H with respect to the order " ≺ ". Let H s be the subset of H of all the elements of length not greater than s. The elements of H s form a basis of L s (I) and Card(H s ) = n s . The set G s := H s \ H s-1 contains the elements in H of length equal to s. Its cardinal will be denoted by k s = Card(G s ). By (H1)-(H4), every element I j ∈ H can be expanded in a unique way as

I j = [I k 1 , [I k 2 , • • • , [I k i , I k ] • • • ]], (3.4)
with

k 1 ≥ • • • ≥ k i , k i < k, and k ∈ {1, . . . , ñ1 }.
In that case, the element I j is said to be a direct descendent of I k , and we write φ(j) := k. For I j ∈ H r , the expansion (3.4) also associates with I j ∈ H a sequence α j = (α 1 j , . . . , α nr j ) in Z nr defined by α ℓ j := Card {s ∈ {1, . . . , i}, k s = ℓ}.

By construction, one has α ℓ j = 0 for ℓ ≥ j, and α j = (0, . . . , 0) for 1 ≤ j ≤ ñ1 . Consider now a family of m vector fields X = {X 1 , . . . , X m } and the Lie algebra L(X) they generate. The P. Hall basis H induces, via the evaluation map, a family of vector fields spanning L(X) as a linear space. Definition 3.3 (Evaluation map). The evaluation map E X defined on L(I), with values in L(X), assigns to every I ∈ L(I) the vector field X I = E X (I) obtained by plugging in X i , i = 1, . . . , m, for the corresponding letter i. Definition 3.4 (Length of the elements of L(X)). With the notations of Definition 3.3, if X I = E X (I), the length of X I , denoted by ∆(X I ), is set to be equal to |I|, . Definition 3.5 (P. Hall family). The P. Hall family H X associated with the vector fields X = {X 1 , . . . , X m } is defined by H X := {E X (I), I ∈ H}, where E X is the evaluation map and H is the P. Hall basis of the free Lie algebra L(I). The family H X inherits the ordering and the numbering of the elements in H induced by (H1)-(H4).

Note that H X is a spanning set of L(X), but not always a basis. Definition 3.6 (Free up to step s). Let s be a positive integer such that 1 ≤ s ≤ r. A family of vector fields ξ = {ξ 1 , . . . , ξ m } defined on a subset Ω of R nr is said to be free up to step s if, for every x ∈ Ω, the growth vector (n 1 ( x), . . . , n s ( x)) is equal to ( n 1 , . . . , n s ). Remark 3.1. If ξ defined on Ω ⊂ R nr is free up to step r, then every point of Ω is regular. Definition 3.7 (Free weights). Let ξ = {ξ 1 , . . . , ξ m } be free up to step r on Ω ⊂ R nr . The integers w 1 , . . . , w nr , where w j = s if n s-1 ( x) < j ≤ n s ( x) for every x ∈ Ω, are called the free weights of step r.

Canonical form

We present in this section the construction of a canonical form for nilpotent systems proposed by Grayson and Grossman in [START_REF] Grayson | Nilpotent Lie algebras and vector fields[END_REF] and [START_REF] Grayson | Models for Free Nilpotent Lie Algebra[END_REF]. Similar results were also obtained by Sussmann in [START_REF] Sussmann | A general theorem on local controllability[END_REF].

The construction takes place in R nr , where r is a positive integer and n r the dimension of L r (I). We denote by v = (v 1 , . . . , v nr ) the points in R nr , and by (∂ v 1 , . . . , ∂ v nr ) the canonical basis of R nr . For j = 1, . . . , n r , we assign to the coordinate function v j the weight w j and to the vector ∂ v j the weightw j . The weighted degree of a monomial

v α 1 1 • • • v α nr nr is then defined as w(α) := w 1 α 1 + • • • + w nr α nr ,
where α = (α 1 , . . . , α nr ), and the weighted degree of a monomial vector field v α 1 1 . . . v

α nr nr ∂ v j is defined as w(α) -w j .
For every I j ∈ H r , let α j be the sequence associated with I j (see Section 3.1). Define the monomial P j (v) associated with I j by

P j (v) := v α j α j ! , (3.5)
where

v α j := ℓ v α ℓ j
ℓ , and α j ! := ℓ α ℓ j !. Note that P j satisfies the following inductive formulas.

P j (v) = 1 if I j ∈ H 1 ; P j (v) = v j 1 α j 1 j 2 + 1 P j 2 (v) if I j = [I j 1 , I j 2 ]. (3.6)
Theorem 3.1 ( [START_REF] Grayson | Nilpotent Lie algebras and vector fields[END_REF][START_REF] Grayson | Models for Free Nilpotent Lie Algebra[END_REF]). We define the vector fields D 1 , . . . , D m on R nr as follows:

D 1 = ∂ v 1 , D 2 = ∂ v 2 + 2≤|I j |≤r φ(j)=2 P j (v)∂ v j ,
. . .

D m = ∂ vm + 2≤|I j |≤r φ(j)=m P j (v)∂ v j .
Then, the Lie algebra L(D) generated by (D 1 , . . . , D m ) is free up to step r, and one has

D I j (0) = ∂ v j , for I j ∈ H r ,
where D I j := E D (I j ) is defined through the evaluation map E D with values in L(D).

The proof of Theorem 3.1 goes by induction on the length of elements in L(D). The reader is referred to [START_REF] Grayson | Models for Free Nilpotent Lie Algebra[END_REF] for a complete development.

Corollary 3.2. For all I k ∈ H r , D I k has the following form

D I k = ∂ v k + I j ∈H r , |I j |>|I k | P k j (v)∂ v j , (3.7)
where every non zero polynomial P k j is homogeneous of weighted degree equal to

|I j | -|I k |.
Remark 3.2. The explicit expression of the polynomials P k j (v) as functions of the monomials P j (v) is obtained through an induction formula.

Corollary 3.3. For i = 1, . . . , m, we define m vector fields Ďi as follows:

Ďi := ∂ v i + 2≤|I k |≤H r-1 φ(k)=i P k (v)∂ v k + I j ∈S φ(j)=i P j (v)∂ v j ,
where S is an arbitrary non-empty subset of G r . Then,

• if I k ∈ H r-1 ∪ S, we have ĎI k = ∂ v k + I j ∈H r-1 ∪S, |I j |>|I k | P k j (v)∂ v j ; • if I k ∈ G r \ S, we have ĎI k = 0.
Definition 3.8 (Canonical form). Let X 1 , . . . , X m be m vector fields on an open subset Ω of R nr and v a local system of coordinates on Ω. The control system associated to {X 1 , . . . , X m } is said to be in canonical form in the coordinates v if one has v * X i = D i , for i = 1, . . . , m, where we use v * X i to denote the push-forward of X i by v.

Consider now the control system given by

v = m i=1 u i D i (v), v ∈ R nr . (3.8) Writing (3.8) component by component, one has, for j = 1, . . . , n r , vj = P j (v 1 , . . . , v j-1 )u i , where i = φ(j), (3.9)
or inductively, vj = v j 1 α j 1 j 2 + 1 vj 2 , where

I j = [I j 1 , I j 2 ]. (3.10)
More explicitly, one has

vj = 1 k! v k j 1 vj 2 , if X I j = ad k X I j 1 X I j 2 , (3.11)
where ad k X I j 1

X I j 2 := [X I j 1 , [X I j 1 , • • • , [X I j 1 k times , X I j 2 ]
, with X I j 2 = [X I j 3 , X I j 4 ] and I j 3 ≺ I j 1 . The inductive formula (3.11) will be used in Chapter 5.

A particular system of coordinates, called canonical coordinates (a terminology arising from Lie group theory), allows one to obtain canonical forms. Consider m vector fields X 1 , . . . , X m on Ω ⊂ R nr , let v ∈ Ω, and W = {W 1 , . . . , W n } be a set of vector fields in L(X) such that W 1 (v), . . . , W n (v) is a basis of R nr . The canonical coordinates of the second kind at v associated with W are the system of local coordinates at v defined as the inverse of the local diffeomorphism

(z 1 , . . . , z nr ) -→ e z nr W nr • • • • • e z 1 W 1 (v), (3.12)
where we use e zW i to denote the flow of W i . When the system (X 1 , . . . , X m ) is nilpotent, the above diffeomorphism defines global coordinates on Ω for every v ∈ Ω.

Theorem 3.4 ([37]

). Assume that the vector fields (X 1 , . . . , X m ) generate a Lie algebra which is both nilpotent of step r and free up to step r. Then, in the canonical coordinates of the second kind associated with the P. Hall basis H r X , the control system defined by (X 1 , . . . , X m ) is in canonical form. Remark 3.3. The canonical coordinates of the second kind require to determine the flow of the control vector fields i.e., to integrate some differential equations. In general, there does not exist algebraic change of coordinates between an arbitrary system of coordinates and the canonical coordinates of the second kind.

Desingularization algorithm

Let X = {X 1 , . . . , X m } ⊂ V F (Ω) be a family of m vector fields on Ω ⊂ R n , and K be a compact subset of Ω. We assume that the LARC is satisfied at every point of K. Therefore, the degree of nonholonomy of X is bounded on K and we denote by r its maximal value.

Recall that H r denote the elements of the P. Hall basis of length smaller or equal to r. For every n-tuple J = (I 1 , . . . , I n ) of elements of H r , we define the domain V J ⊂ Ω by

V J := { p ∈ Ω such that det(X I 1 (p), . . . , X In (p)) = 0 }, (3.13)
where X I j = E X (I j ). Such a set V J is open in Ω (possibly empty) and for every p ∈ V J , the vectors

X I 1 (p), . . . , X In (p) form a basis of R n .
Since K is compact, there exist a finite family of n-tuples J 1 , . . . , J M of elements of H r such that

K ⊂ M i=1 V J i . (3.14)
One deduces from (3.14) a compact covering of K in the form

K ⊂ M i=1 V c J i , (3.15)
where, for i = 1, . . . , M, the set

V c J i ⊂ V J i is compact.
Definition 3.9. Let (S i ) i∈I be a finite set of subsets of Ω. The connectedness graph G := (N, E) associated with (S i ) i∈I is defined as as follows:

-the set of nodes N := I;

-a pair (i, j) with i and j in N belongs to the set of edges E if S i ∩ S j = ∅.

A simple path on G is a subset p := {i 1 , . . . , i L } of two by two distinct elements of N such that, for j = 1, . . . , L -1, the pair (i j , i j+1 ) belongs to E.

Remark 3.4. With the notations of Definition 3.9, if we assume that all the sets S i are open or, all of them are closed, and the set S := ∪ i∈I S i is connected, then, for every (x 0 , x 1 ) ∈ S × S, there exists a simple path on G denoted by p := {i 1 , . . . , i L } such that x 0 ∈ S i 1 and x 1 ∈ S i L .

Take J = (I 1 , . . . , I n ) among J 1 , . . . , J M , and pick a point a in V J . In the sequel, we construct, by induction on the length of elements in a free Lie algebra, a family of m vector fields ξ = {ξ 1 , . . . , ξ m } defined on V J × R ñr-n , which is free up to step r and has its projection on V J equal to X. At the same time, we give an approximation of ξ at ã := (a, 0) ∈ V J × R nr-n in canonical form. We define J s := {I j ∈ J , with |I j | = s}, for s ≥ 1, and G s := H s \ H s-1 , for s ≥ 2. We denote by k s the cardinal of J s , and by ks the cardinal of G s . We are now ready to describe in details our desingularization algorithm.

Desingularization Algorithm (DA)

• Step 1:

(1-1) Set V 1 := V J × R k1 -k 1 , a 1 := (a, 0) ∈ V 1 , K 1 := H 1 ∪ (J \ J 1 ). (1-2) Define {ξ 1 1 , . . . , ξ 1 m } on V 1 as follows: ∀(x, v 1 ) ∈ V 1 , ξ 1 i (x, v 1 ) := X i (x) + 0, for i ∈ J 1 , ∂ v 1 i , for i ∈ G 1 \ J 1 .
(1-3) Compute the coordinates y 1 on V 1 defined as the unique affine system of coordinates on V 1 such that ∂ y 1 j = ξ 1 I j (a 1 ), for I j ∈ K 1 , and y 1 (a 1 ) = 0. (1-4) Construct the system of global coordinates z 1 on V 1 by

z 1 j := y 1 j , for j ∈ H 1 , z 1 j := y 1 j - ñ1 k=1 (ξ 1 k • y 1 k )(y 1 ) |y 1 =0 y 1 k , for I j ∈ K 1 \ H 1 ,
where I j denotes the j th element in K 1 .

• Step s, 2 ≤ s ≤ r:

(s-1) Set V s := V s-1 × R ks-ks , a s := (a, 0) ∈ V s , and K s := K s-1 ∪ (G s \ J s ). Denote by v s the points in R ks-ks .
(s-2) Define {ξ s 1 , . . . , ξ s m } as the vector fields on V s which write in coordinates (z s-1 , v s ) as:

ξ s i (z s-1 , v s ) = ξ s-1 i (z s-1 ) + I k ∈G s \J s φ(k)=i P k (z s-1 )∂ v s k .
(s-3) Compute the system of global coordinates y s on V s as the unique isomorphism (z s-1 , v s ) → y s such that ∂ y s φ(I) = ξ s I (a s ) for every I ∈ K s . (s-4) Construct the system of global coordinates zs on V s by the following recursive formulas:

(s-4)-(a) for I j ∈ H s , zs j := y s j + |I j |-1 k=2 r k (y s 1 , . . . , y s j-1 ), (3.16)
where, for k = 2, . . . ,

|I j | -1, r k (y s 1 , . . . , y s j-1 ) = - |β|=k ω(β)<|I j | (ξ s I 1 ) β 1 • • • (ξ s I j-1 ) β j-1 • (y s j + k-1 q=2 r q ) (y s ) |y s =0 (y s 1 ) β 1 β 1 ! • • • (y s j-1 ) β j-1 β j-1 ! ; (s-4)-(b) for I j ∈ K s \ H s , zs j := y s j + s k=2 r k (y s 1 , . . . , y s ns ), (3.17)
where, for k = 2, . . . , s,

r k (y s 1 , . . . , y s ns ) = - |β|=k ω(β)≤s (ξ s I 1 ) β 1 • • • (ξ s I ns ) β ns • (y s j + s q=2 r q ) (y s ) |y s =0 (y s 1 ) β 1 β 1 ! • • • (y s j-1 ) β ns β ns ! .
(s-5) Construct the system of global coordinates z s as follows:

(s-5)-(a) for j > n s , set z s j := zs j ; (s-5)-(b) for j = 1, . . . , n s , set z s j := Ψ s j (z s 1 , . . . , zs j-1 ), where all Ψ s j are polynomials such that the two following conditions are satisfied:

-if we impose the weight of z s j to be w j for j = 1, . . . , n s , then every Ψ s j is homogeneous of weighted degree equal to w j ; -denote by ord s a s (•) the nonholonomic order defined by (ξ s 1 , . . . , ξ s m ) at a s , and by ξ s i,j (z s ) the j th component of ξ s i (z s ); then one has

ξ s i,j (z s ) = δ i,φ(j) P j (z s 1 , . . . , z s j-1 ) + R i,j (z s ), j = 1, . . . , n s , (3.18)
where ord s a s (R i,j ) ≥ w j (δ i,k denotes the Kronecker symbol). Note that ord s a s (P j ) = w j -1.

Theorem 3.5. Let ξ i := ξ r i , for i = 1, . . . , m, and z := z r , where ξ r i and z r are given by the desingularization algorithm. Then, the family of vector fields ξ = {ξ 1 , . . . , ξ m } defined on Ω × R nr-n is free up to step r. Moreover, the system of coordinates z = (z 1 , . . . , z nr ) is a system of privileged coordinates at ã for ξ, and the family of vector fields ξ = { ξ 1 , . . . , ξ m } defined in the coordinates z by the canonical form:

ξ i = ∂ z i + 2≤|I j |≤ nr φ(j)=i P j (z 1 , . . . , z j-1 )∂ z j , for i = 1, . . . , m, (3.19)
is a nonholonomic first order approximation of ξ at ã. Remark 3.5. We note that the desingularization procedure does not a priori require that (a) the coordinates z are privileged coordinates;

(b) the system ξ is a first order approximation of ξ at a.

However, (a) and (b) can be used directly at the first step of the motion planning algorithm presented in Section 4.

Remark 3.6. If we assume that the original system X = {X 1 , . . . , X m } is nilpotent, then, by adapting the proof of Theorem 3.5 presented in Section 3.4, one can show that the corresponding "lifted" system ξ = {ξ 1 , . . . , ξ m } given by the Desingularization Algorithm proposed in this section remains nilpotent with the same order of nilpotency. Moreover, when expressed in the privileged coordinates z, the system ξ is equal to its own first order approximation in the canonical form. In other words, for any nilpotent systems of step k, the Desingularization Algorithm constructs a nilpotent system of step k and free up to step k which is in the canonical form in coordinates z.

Proof of Theorem 3.5

The proof of Theorem 3.5 is based on the following proposition.

Proposition 3.6. The desingularization algorithm is feasible from s = 1 to s = r. At each step s of the construction (s = 1, . . . , r), the following properties hold true:

(A1) the vectors {ξ s I (a s ) : I ∈ K s } are linearly independent;

(A2) if |I j | ≤ s, then ord s a s (z s j ) = |I j |, and ord s a s (z s j ) = |I j |; (A3) if |I j | > s, then ord s a s (z s j ) > s;
(A4) the change of coordinates (Ψ s j ) j=1,..., ns is well defined;

(A5) for I k ∈ K s , the vector fields ξ s I k has the following form in coordinates z s ,

ξ s I k (z s ) = I j ∈H s (P k j (z s ) + R k j (z s ))∂ z s j + I ℓ ∈K s \H s Q k ℓ (z s )∂ z s ℓ , (3.20)
with ord s a s (R k j ) > |I j | -|I k |, ord s a s (Q k ℓ ) > s -|I k |
, and P k j given by Eq. (3.7). More precisely, if one defines ξs i :=

I j ∈H s φ(j)=i P j (z s )∂ z s j , then, one has ξs I k = I j ∈H s P k j (z s )∂ z s j
, where the polynomials P k j verify the following properties:

• if I k ∈ H s , then -for |I j | < |I k |, P k j = 0; -for |I j | = |I k |, P j j = 1, and P k j = 0 if k = j; -for |I j | > |I k |, ord s a s (P k j ) = |I j | -|I k |; • if I k ∈ K s \ H s , P k j =
0 for all j = 1, . . . , n s .

Remark 3.7. Property (A1) implies that Step (s-3) is feasible, which, in turn, guarantees that Steps (s-4)-(a) and (s-4)-(b) are well defined, and zs is a system of coordinates because the differential of the application y s → zs at 0 is equal to the identity map. Property (A4) guarantees that Step (s-5)-(b) is feasible. Property (A2) ensures that, at the end of the algorithm, the system of coordinates z r is a system of privileged coordinates. Property (A5) finally ensures that for s = r, the approximation ξ of ξ is in canonical form.

By Remark 3.7, Theorem 3.5 is a consequence of Proposition 3.6 by induction on s.

Proof of Proposition 3.6. We begin by showing that Properties (A1)-(A5) hold true for s = 1.

Claim 1. The family of vectors {ξ 1 I (a 1 )} I∈K 1 is linearly independent, i.e., Property (A1) holds true for s = 1.

Proof of Claim 1. By construction, for every I ∈ J , one has ξ 1 I (a 1 ) = X I (a), which belongs to R n ×{0}.

For i ∈ G 1 \ J 1 , the vector ξ 1 i (a 1 ) belongs to R n × R k 1 -k 1
, and the family of vectors {ξ 1 i (a 1 )} i∈G 1 \J 1 is linearly independent. Therefore, the family of vectors {ξ 1 I (a 1 )} I∈K 1 is linearly independent and Claim 1 holds true.

Claim 2. For j = 1, . . . , n 1 , one has ord 1 a 1 (z 1 j ) = 1, i.e., Property (A2) holds true for s = 1.

Proof of Claim 2. For j = 1, . . . , n 1 , one has by construction ξ 1 j • z 1 j (a 1 ) = 1. Thus, one has ord 1 a 1 (z 1 j ) ≤ 1. Since z 1 is a system of coordinates centered at a 1 , one has z 1 j (a 1 ) = 0, which implies that ord 1 a 1 (z 1 j ) > 0. Therefore, one gets ord 1 a 1 (z 1 j ) = 1 and Claim 2 holds true.

Claim 3. For I j ∈ K 1 with |I j | > 1, one has ord 1 a 1 (z 1 j ) > 1, i.e., Property (A3) holds true for s = 1. Proof of Claim 3. For |I j | ≥ 2, i.e. I j ∈ K 1 \ J 1 , one computes ξ 1 k • z 1 j at a 1 for every k ∈ {1, . . . , n 1 }.

ξ 1 k • z 1 j (a 1 ) = ξ 1 k • y 1 j (a 1 ) - n 1 i=1 (ξ 1 i • y 1 j )(a 1 )(ξ 1 k • y 1 i )(a 1 ) = ξ 1 k • y 1 j (a 1 ) -ξ 1 k • y 1 j (a 1 ) = 0.
Then, by definition, one has ord 1 a 1 (z 1 j ) > 1 for |I j | > 1 and Claim 3 holds true.

Claim 4. For i = 1, . . . , m, and j = 1, . . . , n 1 , the j th -component of ξ 1 i in coordinates z 1 is equal to 1 if i = j, and equal to 0 otherwise. In other words, for i = 1, . . . , m, the n 1 first components of ξ 1 i verify Eq. (3.18). Properties (A4) and (A5) hold true for s = 1.

Proof of Claim 4. By Claim 1, ξ 1 1 (a 1 ), . . . , ξ 1 n 1 (a 1 ) is a basis of R n 1 , and thus the linear change of coordinates y 1 exists. As ∂ y 1 j = ξ 1 j (a 1 ), and z 1 j = y 1 j for j = 1, . . . , n 1 , Claim 4 holds true.

Therefore, Properties (A1)-(A5) hold true for s = 1. Let 1 ≤ s ≤ r. Let us now assume that Properties (A1)-(A5) hold true for 1 ≤ s ′ ≤ s. We will show that they still hold true for s + 1.

P k (z s ) = z s k 1 α k 1 k 2 + 1 P k 2 (z s ).
Since

(P k ) = |I k 1 | + |I k 2 | -1 = s. Claim 6. For I k ∈ K s+1 with |I k | ≤ s + 1, one has ξ s+1 I k (z s , v s+1 ) = ξ s I k (z s ) + I j ∈G s+1 \J s+1 P k j (z s )∂ v s+1 j , (3.21)
where P k j (z s ) = P k j (z s 1 , . . . , z s ns ) + Rk j (z s ), with ord

ξ s+1 k (z s , v s+1 ) = ξ s k (z s ) + I j ∈G s+1 \J s+1 φ(j)=k P j (z s )∂ v s+1 j . By Claim 5, if φ(j) = k, then ord s a s+1 (P j ) = s = |I j | -|I k |. Claim 6 holds true for |I k | = 1.
Assume that Claim 6 holds true for every I ∈ K s+1 of length less than or equal to s 1 . Consider

I k ∈ K s+1 with |I k | = s 1 + 1.
In coordinates (z s , v s+1 ), one has

ξ s+1 I k = [ξ s+1 I k 1 , ξ s+1 I k 2 ] = [ξ s I k 1 + I i ∈G s+1 \J s+1 (P k 1 i + Rk 1 i )∂ v s+1 i , ξ s I k 2 + I i ∈G s+1 \J s+1 (P k 2 i + Rk 2 i )∂ v s+1 i ] = [ξ s I k 1 , ξ s I k 2 ] + I i ∈G s+1 \J s+1 {ξ s I k 2 • (P k 1 i + Rk 1 i ) -ξ s I k 1 • (P k 2 i + Rk 2 i )}∂ v s+1 i .
Since (A5) holds true up to step s, one has

ξ s I k 1 • (P k 2 i + Rk 2 i ) =   I j ∈H s (P k 1 j + R k 1 j )∂ z s j + I ℓ ∈K s \H s Q k 1 ℓ ∂ z s ℓ   • (P k 2 i + Rk 2 i ) = I j ∈H s (P k 1 j + R k 1 j )∂ z s j P k 2 i + I ℓ ∈K s \H s Q k 1 ℓ ∂ z s ℓ P k 2 i + I j ∈H s (P k 1 j + R k 1 j )∂ z s j Rk 2 i + I ℓ ∈K s \H s Q k 1 ℓ ∂ z s ℓ Rk 2 i = I j ∈H s P k 1 j ∂ z s j P k 2 i +   I j ∈H s R k 1 j ∂ z s j P k 2 i + I j ∈H s (P k 1 j + R k 1 j )∂ z s j Rk 2 i + I ℓ ∈K s \H s Q k 1 ℓ ∂ z s ℓ Rk 2 i   := I j ∈H s P k 1 j ∂ z s j P k 2 i + R i,1 .
We first show that every term in R i,1 has, at a s+1 , an order strictly greater than s

+ 1 -|I k |. Indeed, for I j ∈ H s , since ord s a s+1 (z j ) = |I j |, ord s a s+1 (P k 2 i ) = |I i | -|I k 2 |, and ord s a s+1 (R k 1 j ) > |I j | - |I k 1 |, one has ord s a s+1 (R k 1 j ∂ z s j P k 2 i ) > |I j |-|I k 1 |+(|I i |-|I k 2 |)-|I j | = |I i |-|I k |, with |I i | = s+1. Note that ord s a s+1 ((P k 1 j + R k 1 j )∂ z s j Rk 2 i ) = ord s a s+1 (P k 1 j ∂ z s j Rk 2 i ). Since ord s a s+1 (P k 1 j ) = |I j | -|I k 1 |, and ord s a s+1 ( Rk 2 i ) > |I i | -|I k 1 |, then, one has ord s a s+1 (P k 1 j ∂ z s j Rk 2 i ) > |I j | -|I k 2 | + |I i | -|I k 1 | -|I j | = |I i | -|I k |.
Recall that, by definition, all the functions have positive order. Therefore, one gets ord s a s+1 (Q

k 1 ℓ ∂ z s ℓ Rk 2 i ) ≥ ord s a s+1 (Q k 1 ℓ ) > s -|I k 1 | ≥ s + 1 -|I k |.
In conclusion, one gets ord s a s+1 (R i,1 ) > s + 1 -|I k |. A similar computation shows that

ξ s I k 2 • (P k 1 i + Rk 1 i ) = I j ∈H s P k 2 j ∂ z s j P k 1 i +   I j ∈H s R k 2 j ∂ z s j P k 1 i + I j ∈H s (P k 2 j + R k 2 j )∂ z s j Rk 1 i + I ℓ ∈K s \H s Q k 2 ℓ ∂ z s ℓ Rk 1 i   := I j ∈H s P k 2 j ∂ z s j P k 1 i + R i,2 , with ord s a s+1 (R i,2 ) > s + 1 -|I k |.
Therefore, one gets

ξ s+1 I k = ξ s I k + I i ∈G s+1 \J s+1 { I j ∈H s (P k 1 j ∂ z s j P k 2 i -P k 2 j ∂ z s j P k 1 i )}∂ v i + I i ∈G s+1 \J s+1 (R i,1 + R i,2 )∂ v i , with ord s a s+1 (R i,1 + R i,2 ) ≥ min (ord s a s+1 (R i,1 ), ord s a s+1 (R i,2 )) > s + 1 -|I k |. Since Corollary 3.3 implies that I j ∈H s (P k 1 j ∂ z s j P k 2 i -P k 2 j ∂ z s j P k 1 i ) = P k i , and ord s a s+1 (P k i ) = |I i | -|I k |, one gets ξ s+1 I k (z s , v s+1 ) = ξ s I k (z s ) + I i ∈G s+1 \J s+1 (P k i (z s ) + Rk i (z s ))∂ v i , with ord s a s+1 (P k i ) = s + 1 -|I k | and ord s a s+1 ( Rk i ) > s + 1 -|I k |.
Therefore, Claim 6 still holds true for I k ∈ K s+1 with |I k | = s 1 + 1. This terminates the induction, and Claim 6 is now proved.

Claim 7. The family of vectors {ξ s+1

I k (a s+1 )} I k ∈K s+1 is linearly independent, i.e., Property (A1) holds true at Step s + 1.

Proof of Claim 7. Claim 6 implies that for every I k ∈ K s , one has ξ s+1 I k (a s+1 ) = ξ s I k (a s ) ∈ R ns × {0}. Corollary 3.3 implies that for every I k ∈ G s+1 \ J s+1 , one has

ξ s+1 I k (a s+1 ) = ξ s I k (a s ) + ∂ v k ∈ R ns × R k s+1 -k s+1 .
Therefore, by (A1) at step s, the vectors {ξ s+1 I k (a s+1 )} I k ∈K s+1 are linearly independent. Claim 8. After performing (s+1)-4-(a) and (s+1)-4-(b) in the Desingularization Algorithm, one has, for every I j ∈ H s+1 , ord s+1 a s+1 (z s+1 j ) = |I j |, and for every

I j ∈ K s+1 \ H s+1 , ord s+1 a s+1 (z s+1 j ) > s + 1.
The proof of Claim 8 is based on the following result due to Bellaïche [2, Lemma 4.12].

Lemma 3.7. Let {X 1 , . . . , X m } be a family vector fields defined on Ω. Consider {W 1 , . . . , W n } a frame adapted to the flag

L 1 (a) ⊂ • • • ⊂ L r (a) = R n at a ∈ Ω ( Remark 2.

4).

A function f is of order strictly greater than s at a is and only if

(W α 1 1 • • • W αn n f )(a) = 0, for all α = (α 1 , . . . , α n ) such that w(α) ≤ s. Proof of Claim 8. Claim 7 guarantees that {ξ s+1 I k } I k ∈H s+1 is a basis adapted to the flag L 1 (a s+1 ) ⊂ • • • ⊂ L s+1 (a s+1 ). Complete {ξ s+1 I k } I k ∈H s+1
by other elements of the Lie algebra generated by {ξ s+1 i } i=1,...,m in order to get a basis adapted to the flag

L 1 (a s+1 ) ⊂ • • • ⊂ L s+1 (a s+1 ) ⊂ • • • ⊂ L r (a s+1 ).
For I j ∈ K s+1 \H s+1 , Formula (s+1)-4-(b) ensures that ((ξ s+1 I 1 )

β 1 • • • (ξ s+1 I n s+1 ) β n s+1 • zs+1 j
)(a s+1 ) = 0, for all β = (β 1 , . . . , β n s+1 ) such that w(β) ≤ s + 1. By Lemma 3.7, one has ord s+1 a s+1 (z s+1 j ) > s + 1, for I j ∈ K s+1 \ H s+1 .

For I j ∈ H s+1 , Formula (s+1)-4-(a) implies that ((ξ s+1 I 1 )

β 1 • • • (ξ s+1 I j-1 ) β j-1 • zs+1 j )(a s+1 ) =
ξ s+1 i (z s+1 ) = ∂ zs+1 i + I j ∈H s+1 |I j |≥2 ( Pi,j (z s+1 ) + Ri,j (z s+1 ))∂ zs+1 j + I ℓ ∈K s+1 \H s+1 Qi,ℓ (z s+1 )∂ zs+1 ℓ ,
where Pi,j , Ri,j , and Qi,ℓ are polynomials with ord s+1 a s+1 ( Pi,j ) = w j -1, ord s+1 a s+1 ( Ri,j ) ≥ w j , and ord s+1 a s+1 ( Qi,ℓ ) > s. Since ord s+1 a s+1 (z s+1 j ) = w j , for I j ∈ H s+1 , and ord s+1 a s+1 (z s+1 j ) > s + 1, for I j ∈ K s+1 \ H s+1 , the polynomials Pi,j contain only variables zs+1 k with w k ≤ w j -1. Let us now show that there exists a change of coordinates Ψ s+1 which transforms coordinates zs+1 into new coordinates z s+1 such that ord s+1 a s+1 (z s+1 j ) = w j , for I j ∈ H s+1 ,

ord s+1 a s+1 (z s+1 j ) > s + 1, for I j ∈ K s+1 \ H s+1 ,
and in the new coordinates, the n s+1 first components ξ s+1 i,j (z s+1 ) of ξ s+1 i (z s+1 ) are in the form ξ s+1 i,j (z s+1 ) = δ i,φ(j) P j (z s+1 1 , . . . , z s+1 j-1 ) + R i,j (z s+1 ), j = 1, . . . , n s+1 , with ord s+1 a s+1 (R i,j ) ≥ w j . We first note that, once one has ord s+1 a s+1 (z s+1 j ) = w j for I j ∈ H s+1 , and ord s+1 a s+1 (z s+1 j ) > s + 1 for I j ∈ K s+1 \ H s+1 , then, the order of P i,j will be equal to its weighted degree, and thus automatically equal to w j -1 by construction of these polynomials.

Consider now ξi s+1 defined in coordinates zs+1 by

ξs+1 i (z s+1 ) = ∂ zs+1 i + I j ∈H s+1 Pi,j (z s+1 )∂ zs+1 j .
Recall that, by construction, the vector fields { ξi } i=1,...,m generate a free nilpotent Lie algebra of step s + 1. Moreover, in the canonical coordinates of the second kind (z s+1 1 , . . . , z s+1 n s+1 ) associated with { ξs+1

I k } I k ∈H s+1 , the vector fields ξs+1 i are in the canonical form, i.e.

ξs+1 i = ∂ zs+1 i + I j ∈H s+1 φ(j)=i P j (z s+1 )∂ z s+1 j .
By definition of a system of coordinates, there exist n s+1 smooth functions (Ψ s+1 1 , . . . , Ψ s+1 n s+1 ) such that, for j = 1, . . . , n s+1 , one has z s+1 j = Ψ s+1 j (z s+1 1 , . . . , zs+1 n s+1 ). Expand now Ψ s+1 j in Taylor series. Since ord s+1 a s+1 (z s+1 ) = w j , the Taylor expansion of Ψ s+1 j is a polynomial of weighted degree equal to w j . Claim 9 is now proved.

Remark 3.8. The change of coordinates (Ψ s+1 j ) j=1,..., n s+1 is computed by identification. Indeed, since ord s+1 a s+1 (z s+1 j ) = w j , and the nonholonomic order does not depend on any system of coordinates, then Ψ s+1 j is a function of order w j at a s+1 , i.e., the Taylor expansion of Ψ s+1 j at a s+1 contains only monomials of weighted degree equal to w j , and there is a finite number of such monomials. Therefore, the function Ψ s+1 j is necessarily in the following form

Ψ s+1 j (z s+1 ) = w(α)= w j β α j (z s+1 1 ) α 1 . . . (z s+1 n s+1 ) α n s+1 , (3.22)
where β α j are real numbers. Eq. (3.22) is a finite sum and therefore the scalar coefficients (ϕ α j ) can be obtained by identification. Claim 9 guarantees that such a set of real numbers (ϕ α j ) exists. Note also that, due to the constraint on the weight, Eq. (3.22) only involves variables zs+1 k of weight less than w j , implying that the change of coordinates (Ψ s+1 j ) j=1,..., n s+1 is naturally triangular. Remark 3.9. Let us now illustrate Remark 3.8 with a simple example. Consider here a nilpotent system of step 2 generated by two vector fields (ξ 1 , ξ 2 ). We have ξ

I 1 = ξ 1 , ξ I 2 = ξ 2 and ξ I 3 = [ξ 1 , ξ 2 ].
In coordinates z = (z 1 , z2 , z3 ), ξ 1 and ξ 2 are necessarily in the form ξ 1 = (1, 0, α 1 z1 + α 2 z2 ), and ξ 2 = (0, 1, β 1 z1 + β 2 z2 ), where α 1 , α 2 , β 1 and β 2 are real numbers verifying β 1α 2 = 1. As mentioned in Remark 3.8, in the change of coordinates (Ψ 1 , Ψ 2 , Ψ 3 ), every Ψ j is a homogeneous polynomial of weighted degree equal to w j . Set z = (Ψ 1 (z), Ψ 2 (z), Ψ 3 (z)) =: (z 1 , z2 , z3 + az 1 z2 + bz 2 1 + cz 2 2 ), with a, b, and c to be determined. One imposes that ξ 2 (z) = (0, 1, z 1 ). After computation, one gets

(α 1 + 2b)z 1 + (α 2 + a)z 2 = 0, (β 1 + a)z 1 + (β 2 + 2c)z 2 = z 1 = z1 . By identification, one gets a = -α 2 , b = -α 1 2 , c = -β 2 2
, and in that case, β 1 + a = β 1α 2 = 1 is automatically verified. Then, the triangular change of coordinates

(z 1 , z 2 , z 3 ) = (z 1 , z2 , z3 -α 2 z1 z2 - α 1 2 z2 1 - β 2 2 z2 )
puts ξ 1 and ξ 2 into the canonical form.

Claim 10. Property (A5) holds true at step s + 1.

Proof of Claim 10. The proof goes by induction on the length of I k ∈ K s+1 . It is similar to the one of Claim 6. For |I k | = 1, one has

ξ s+1 i (z s+1 ) = I j ∈H s+1 φ(j)=i (P j (z s+1 ) + R i,j (z s+1 ))∂ z s+1 j + I ℓ ∈K s+1 \H s+1 Q i,ℓ (z s+1 )∂ z s+1 ℓ , with ord s+1 a s+1 (P j ) = |I j | -1, ord s+1 a s+1 (R i,j ) > |I j | -1, ord s+1 a s+1 (Q i,ℓ ) > s.
Claim 10 holds true for |I k | = 1. Assume that Claim 10 holds for brackets of length less than s 1 . We show that it still holds true for brackets of length s 1 + 1. Consider I k ∈ K s+1 with |I k | = s 1 + 1. Then, one has

ξ s+1 I k = [ξ s+1 I k 1 , ξ s+1 I k 2 ] = [ I j ∈H s+1 (P k 1 j + R k 1 j )∂ z s+1 j + I ℓ ∈K s+1 \H s+1 Q k 1 ℓ ∂ z s+1 ℓ , I j ∈H s+1 (P k 2 j + R k 2 j )∂ z s+1 j + I ℓ ∈K s+1 \H s+1 Q k 2 ℓ ∂ z s+1 ℓ ] = I j ∈H s+1 [ I i ∈H s+1 P k 1 i ∂ z s+1 i P k 2 j -P k 2 i ∂ z s+1 i P k 1 j ]∂ z s+1 j + I j ∈H s+1 [ I i ∈H s+1 {R k 1 i ∂ z s+1 i (P k 2 j + R k 2 j ) -R k 2 i ∂ z s+1 i (P k 1 j + R k 1 j )} + {P k 1 i ∂ z s+1 i R k 2 j -P k 2 i ∂ z s+1 i R k 1 j } + I ℓ ∈K s+1 \H s+1 Q k 1 ℓ ∂ z s+1 ℓ (P k 2 j + R k 2 j ) -Q k 2 ℓ ∂ z s+1 ℓ (P k 1 j + R k 1 j )]∂ z s+1 j + I j ∈K s+1 \H s+1 [ I i ∈H s+1 (P k 1 i + R k 1 i )∂ z s+1 i Q k 2 j -(P k 2 i + R k 2 i )∂ z s+1 i Q k 1 j + I ℓ ∈K s+1 \H s+1 Q k 1 ℓ ∂ z s+1 ℓ Q k 2 j -Q k 2 ℓ ∂ z s+1 ℓ Q k 1 j ]∂ z s+1 j .
By the inductive hypothesis, one can proceed as follows.

• Taking into account the relation

ord s+1 a s+1 (R k 1 i ) > |I i | -|I k 1 |, and ord s+1 a s+1 (∂ z s+1 i (P k 2 j + R k 2 j )) ≥ |I j | -|I k 2 | -|I i |, one deduces that ord s+1 a s+1 R k 1 i ∂ z s+1 i (P k 2 j + R k 2 j ) > |I j | -|I k |. By a similar argument, ord s+1 a s+1 R k 2 i ∂ z s+1 i (P k 1 j + R k 1 j ) > |I j | -|I k |. Therefore, one gets ord s+1 a s+1 (R k 1 i ∂ z s+1 i (P k 2 j + R k 2 j ) -R k 2 i ∂ z s+1 i (P k 1 j + R k 1 j )) > |I j | -|I k |.
• Since ord s+1 a s+1 (P

k 1 i ) = |I i |-|I k 1 | and ord s+1 a s+1 (∂ z s+1 i R k 2 j ) > |I j |-|I k 2 |-|I i |, then ord s+1 a s+1 (P k 1 i ∂ z s+1 i R k 2 j ) > |I j | -|I k |. By a similar argument, ord s+1 a s+1 (P k 2 i ∂ z s+1 i R k 1 j ) > |I j | -|I k |. One thus obtains ord s+1 a s+1 (P k 1 i ∂ z s+1 i R k 2 j -P k 2 i ∂ z s+1 i R k 1 j ) > |I j | -|I k |.
• Using the fact that ord s+1 a s+1 (∂ z s+1 ℓ

(P k 2 j +R k 2 j )) > |I j |-|I k 2 |-(s+1) and ord s+1 a s+1 (Q k 1 ℓ ) > s+1-|I k 1 |, then ord s+1 a s+1 (Q k 1 ℓ ∂ z s+1 ℓ (P k 2 j + R k 2 j )) > |I j | -|I k |. By a similar argument, ord s+1 a s+1 Q k 2 ℓ ∂ z s+1 ℓ (P k 1 j + R k 1 j ) > |I j | -|I k |. One deduces ord s+1 a s+1 (Q k 1 ℓ ∂ z s+1 ℓ (P k 2 j + R k 2 j ) -Q k 2 ℓ ∂ z s+1 ℓ (P k 1 j + R k 1 j )) > |I j | -|I k |.
• Recall that ord s+1 a s+1 (P

k 1 i + R k 1 i ) = |I i | -|I k 1 |, and ord s+1 a s+1 (∂ z s+1 i Q k 2 j ) > s + 1 -|I k 2 | -|I i |, then ord s+1 a s+1 ((P k 1 i + R k 1 i )∂ z s+1 i Q k 2 j ) > s + 1 -|I k |.
By a similar argument, ord s+1 a s+1 ((P

k 2 i + R k 2 i )∂ z s+1 i Q k 1 j ) > s + 1 -|I k |. Therefore, it yields ord s+1 a s+1 ((P k 1 i + R k 1 i )∂ z s+1 i Q k 2 j -(P k 2 i + R k 2 i )∂ z s+1 i Q k 1 j ) > s + 1 -|I k |. • Since ∂ z s+1 ℓ Q k 2 j is a function, one knows by definition that ord s+1 a s+1 (∂ z s+1 ℓ Q k 2 j ) ≥ 0. As ord s+1 a s+1 (Q k 1 ℓ ) > s + 1 -|I k 1 |, one has ord s+1 a s+1 (Q k 1 ℓ ∂ z s+1 ℓ Q k 2 j ) > s + 1 -|I k 1 | = s + 1 -(|I k | -|I k 2 |) > s + 1 -|I k |. By a similar argument, ord s+1 a s+1 (Q k 2 ℓ ∂ z s+1 ℓ Q k 1 j ) > s + 1 -|I k |. One hence derives ord s+1 a s+1 (Q k 1 ℓ ∂ z s+1 ℓ Q k 2 j - Q k 2 ℓ ∂ z s+1 ℓ Q k 1 j ) > s + 1 -|I k |.
Summing up the above terms, one gets, for I k ∈ K s+1 of length s 1 + 1, that the bracket ξ s+1 I k can be written in the form

ξ s+1 I k (z s+1 ) = I j ∈H s+1 (P k j (z s+1 ) + R k j (z s+1 ))∂ z s+1 j + I j ∈K s+1 \H s+1 Q k j (z s+1 )∂ z s+1 j , with ord s+1 a s+1 (P k j ) = |I j | -|I k |, ord s+1 a s+1 (R k j ) > |I j | -|I k |, and ord s+1 a s+1 (Q k ℓ ) > s + 1 -|I k |. Claim 10 is now proved.
In conclusion, Properties (A1)-(A5) still hold true at step s + 1 in the Desingularization Algorithm. The induction step is established, which terminates the proof of Proposition 3.6.

Global Steering Method for Regular Systems

By taking into account the Desingularization Algorithm presented in Chapter 3, we assume in this chapter and without loss of generality that the family of vectors fields X = {X 1 , . . . , X m } is free up to step r (cf. Definition 3.6). Recall that, in that case, every point x ∈ Ω is regular and the growth vector is constant on Ω. We present in Section 4.1 an algebraic construction of privileged coordinates and a nonholonomic first order approximation of X under canonical form. For regular systems, this construction also provides a continuously varying system of privileged coordinates. We then propose in Section 4.2 a global motion planning algorithm for regular systems.

Construction of the approximate system A X

For every point a in Ω, we construct the first order approximate system A X (a) of the system X at a (cf. Definition 2.14) as follows:

Step (1) Take {X I j } I j ∈H r . Set w j = w j for j = 1, . . . , n.

Step (2) Construct the linear system of coordinates y = (y 1 , . . . , y n ) such that ∂ y j = X I j (a).

Step (3) Build the system of privileged coordinates z = ( z 1 , . . . , z n ) by the following iterative formula: for j = 1, . . . , n,

z j := y j + w j -1 k=2 h k (y 1 , . . . , y j-1 ), (4.1)
where, for k = 2, . . . , w j -1,

h k (y 1 , . . . , y j-1 ) = - |α|=k w(α)<w j X α 1 I 1 . . . X α j-1 I j-1 • (y j + k-1 q=2 h q )(y)| y=0 y α 1 1 α 1 ! • • • y α j-1 j-1 α j-1 ! , with |α| := α 1 + • • • + α n .
Step (4) For i = 1, . . . , m, compute the Taylor expansion of X i ( z) at 0, and express every vector field as a sum of vector fields which are homogeneous with respect to the weighted degree defined by the sequence (w j ) j=1,...,n :

X i ( z) = X (-1) i ( z) + X (0) i ( z) + • • • , where we use X (k)
i ( z) to denote the sum of all the terms of weighted degree equal to k. Set

X a i ( z) := X (-1) i ( z).
Step (5) For j = 1, . . . , n, identify homogeneous polynomials Ψ j of weighted degree equal to w j such that, in the system of privileged coordinates z := (z 1 , . . . , z n ) defined by z j := Ψ j ( z 1 , . . . , z j-1 ), for j = 1, . . . , n, the approximate system X a (z) = {z * X a 1 ( z), . . . , z * X a m ( z)} is in the canonical form.

Step (6) Set A X (a) := X a and Φ X (a, •) := the mapping x → z. Remark 4.1. Steps ( 1)-(3) construct a system of privileged coordinates z. The proof that z is a system of privileged coordinates is essentially based on Lemma 3.7. Roughly speaking, the idea to obtain z j from y j goes as follows: for every α = (α 1 , . . . , α n ) with w(α) < w j (so

α j = • • • = α n = 0), compute X α 1 I 1 • • • X α j-1 I j-1 • y j (y)| y=0 .
If it is not equal to zero, then replace y j by

y j -(X α 1 I 1 • • • X α j-1 I j-1 • y j )(y)| y=0 y α 1 1 α 1 ! • • • y α j-1 j-1 α j-1 ! .
With that new value of y j , one gets

X α 1 I 1 • • • X α j-1 I j-1 • y j (y)| y=0 = 0.
Therefore, by Lemma 3.7, one has ord a ( z j ) ≥ w j for j = 1, . . . , n. On the other hand, since Step (3) of the construction does not modify the linear part, the system of coordinates z remains adapted. By Remark 2.5, one also has ord a ( z j ) ≤ w j , and therefore, ord a ( z j ) = w j .

Remark 4.2. The existence of Ψ j in Step ( 5) is guaranteed by a simple modification of Claim 9, page 24, see also Remarks 3.8 and 3.9. The key point is, in the current case, the exponential coordinates are algebraic.

Remark 4.3. We will propose in Section 5 an effective and exact method for steering general nilpotent systems given in the canonical form.

It results from [START_REF] Bellaïche | The tangent space in sub-Riemannian geometry[END_REF] that, for regular systems, the mapping Φ X : (a, x) → z is a continuously varying system of privileged coordinates on Ω. Note also that the coordinates z are obtained from y by expressions of the form

z 1 = y 1 , z 2 = y 2 + pol 2 (y 1 ),
. . .

z n = y n + pol n (y 1 , . . . , y n-1 ),
where, for j = 1, . . . , n, the function pol j (•) is a polynomial which does not contain constant nor linear terms. Due to the triangular form of this change of coordinates, the inverse change of coordinates from z to y bears exactly the same form. Therefore, the mapping z = Φ X (a, •) is defined on the whole Ω, i.e., Φ X has an infinite injectivity radius. We also note that, by construction, A X is a nonholonomic first order approximation (cf. Definition 2.14) and its continuity results from the continuity of the mapping Φ X : (a, x) → z. In summary, we have the following proposition.

Proposition 4.1. The mapping Φ X is a continuously varying system of privileged coordinates on Ω and the mapping A X is a continuous approximation of X on Ω.

The following theorem is a consequence of Proposition 4.1 and Corollary 2.5.

Theorem 4.2. Let V c be a compact subset of Ω. If A X is provided with a sub-optimal steering law (cf. Definitions 2.16 and 2.17), then the LAS method AppSteer associated with A X and its steering law (cf. Definition 2.18) is uniformly locally contractive on V c . Remark 4.4. Due to Step (5) in the construction procedure, the approximate system A X (a) is under canonical form in a system of privileged coordinates z. Therefore, A X (a) has always the same form, regardless of the control system X or the approximate point a ∈ Ω. The specificity of each system or each approximate point is hidden in the change of coordinates Φ X .

Remark 4.5. It is important to notice that the approximate system used in the LAS method is a nonholonomic first order approximation at the goal point a (cf. Definition 2.18). Therefore, the steering control always displaces A X (a) from some position (which is the image by Φ X (a, •) of the current point of the original system) to 0 (which is Φ X (a, a) by construction) in coordinates z. The latter fact plays a crucial role in getting the sub-optimality for the steering law (see Section 5.3 for more details).

Approximate steering algorithm

Let V c ⊂ Ω be a connected compact set equal to the closure of its interior and (x initial , x final ) ∈ V c × V c . We devise, under the assumptions of Theorem 4.2, an algorithm (Algorithm 3 below) which steers System (2.1) from x initial to x final . That algorithm does not require any a priori knowledge on the critical distance ε V c . Note that this algorithm bears similarities with trust-region methods in optimization (see [START_REF] Bonnans | Numerical Optimization -Theoretical and Practical Aspects[END_REF] for more details).

Recall first that the family of vectors fields X = {X 1 , . . . , X m } is assumed to be free up to step r. As a consequence the weights (w 1 , . . . , w n ) are equal at every point a ∈ V c to ( w 1 , . . . , w n ), the free weights of step r. Hence the pseudo-norm • a (see Definition 2.13) does not depend on a ∈ V c and will be denoted as • r .

The parameterized path t → δ t (x) is defined by δ t (x) := (t w 1 z 1 (x), . . . , t wn z n (x)), for x ∈ Ω, where z := Φ X (x final , •). Note that δ t is the (weighted) dilatation in privileged coordinates at x final with parameter t. In particular, z(δ t (x)) r = |t| z(x) r . We also define the function Subgoal as follows.

Subgoal(x, η, j)

1. t j := max(0, 1 -jη z(x) r ); 2. Subgoal(x, η, j) := δ t j (x)
We note that the formula for generating t j guarantees that z(Subgoal(x, η, j))z(Subgoal(x, η, j -1)) r ≤ η, and that x d = x final for j large enough. Algorithm 3 GlobalFree (x initial , x final , e, V c , AppSteer)

1: i := 0; j := 1; 2: x i := x initial ; x := x initial ; 3: η := z(x initial ) r ; {initial choice of the maximum step size;} 4: while z(x i ) r > e do 5:

x d := Subgoal (x, η, j);

6:

x := AppSteer (x i , x d );

7: if Φ X (x d , x) r > 1 2 Φ X (x d , x i ) r
then {if the system is not approaching the subgoal,} 8:

η := η 2 ; {reduce the maximum step size,} 9:

x := x i ; j := 1; {change the path δ 0,t (x).} 10:

else 11:

i := i + 1; j := j + 1;

12:

x i := x; x d i := x d ; 13:
end if 14: end while 15: return x i .

The global convergence of Algorithm 3 is established in the following theorem. For the sake of clarity, we first assume that the sequences (x i ) i≥0 and (x d i ) i≥0 constructed by Algorithm 3 both stay within V c . This assumption being of a purely numerical nature, we explain at the end of this section how we can remove it by adding suitable intermediate steps to Algorithm 3. Theorem 4.3. Let V c ⊂ Ω be a connected compact set equal to the closure of its interior. Assume that (i) the approximate system system A X is provided with a sub-optimal steering law;

(ii) the LAS method AppSteer is associated with A X and its steering law; Then, ∀ (x initial , x final ) ∈ V c × V c , Algorithm 3 terminates in a finite number of steps for any choice of the tolerance e > 0 provided that the sequences (x i ) i≥0 and (x d i ) i≥0 both belong to V c . Proof of Theorem 4.3. Note first that, if the conditional statement of Line 7 is not true for every i greater than some i 0 , then x d i = x final after a finite number of iterations. In this case, the error z(x i ) r is reduced at each iteration and the algorithm stops when it becomes smaller than the given tolerance e. This happens in particular if d(x i , x d ) < ε V c for all i greater than i 0 because condition (2.8) is verified. Another preliminary remark is that, due to the continuity of the control distance and of the function z(•) r , there exists η > 0 such that, for every pair (

x 1 , x 2 ) ∈ V c × V c , one has z(x 1 ) -z(x 2 ) r < η =⇒ d(x 1 , x 2 ) < ε V c 2 . (4.2)
In the following, we will prove by induction that if, at some step i 0 , one has η < η, then, for all i > i 0 , one has d(

x i-1 , x d i ) < (1/2 + • • • + (1/2) i-i 0 )ε V c < ε V c .
We assume without loss of generality that i 0 = 0 and x = x 0 . For i = 1, by construction,

x d = Subgoal(x 0 , η, 1) and z(x 0 ) -z(x d ) r ≤ η < η.
In view of (4.2), one then has d(x 0 , x d ) < ε V c /2. In view of (2.8), the conditional statement of Line 7 is not true, therefore x d 1 = x d and one has d(x 0 , x d 1 ) < ε V c /2. Assume now that for i > 1 one has:

d(x i-2 , x d i-1 ) < (1/2 + • • • + (1/2) i-1 )ε V c . (4.3)
The subgoal x d i-1 is of the form Subgoal(x, η, j). Let x d = Subgoal(x, η, j + 1). One can write:

d(x i-1 , x d ) ≤ d(x i-1 , x d i-1 ) + d(x d i-1 , x d ). By construction, it is z(x d i-1 ) -z(x d ) r ≤ η < η, which implies d(x d i-1 , x d ) < ε V c /2. The induction hypothesis (4.3) implies that d(x i-1 , x d i-1 ) ≤ 1 2 d(x i-2 , x d i-1
). Finally, one gets

d(x i-1 , x d ) ≤ 1 2 d(x i-2 , x d i-1 ) + d(x d i-1 , x d ) ≤ (1/2 + • • • + (1/2) i )ε V c .
In view of (2.8), the conditional statement of Line 7 is not true, and so x d i = x d . This ends the induction.

Notice that, at some step i, η ≥ η, the conditional statement of Line 7 could be false. In this case, η is decreased as in Line 8. The updating law of η guarantees that after a finite number of iterations of Line 8, there holds η < η. This ends the proof.

When the working space Ω is equal to the whole R n , the assumption that the sequences (x i ) i≥0 and (x d i ) i≥0 constructed by Algorithm 3 both stay within a compact set V c can be removed. This requires a simple modification of Lines 11 and 12 of Algorithm 3.

We choose a real number R close to one, precisely ( 1 2 ) 1/(r+1) 2 < R < 1, where r is the maximum value of the degree of nonholonomy of System (2.1) on V c . For every non-negative integer k, we set

R k = 1 + R + • • • + R k .
The algorithm is modified as follows. Introduce first a new variable k, and add the initialization k := 0. Replace then Lines 11 and 12 of Algorithm 3 by the procedure below.

This procedure guarantees that the sequences (x i ) i≥0 and (x d i ) i≥0 of the algorithm both belong to the compact set

K = {x ∈ R n : z(x) r ≤ 1 1 -R z(x initial ) r }. 1: if z(x) r ≥ R k+1 z(x initial ) r then 2: η := η 2 ; 3: else if R k z(x initial ) r ≤ z(x) r < R k+1 z(x initial ) r then 4:
i := i + 1; j := j + 1;

5:

x i := x; x d i := x d ;

6:

η := η 2 ;

7:

k := k + 1; 8: else if z(x) r ≤ R k z(x initial ) r then 9:
i := i + 1; j := j + 1; 10:

x i := x; x d i := x d ; 11: end if
Moreover, at each iteration of the algorithm, the new variable k is such that

z(x i ) r ≥ R k z(x initial ) r ⇒ η ≤ z(x initial ) r 2 k .
For the sake of clarity, we state here the complete modified algorithm named as Algorithm 4.

Algorithm 4 GlobalFree Modified (x initial , x final , e, V c , AppSteer) 1: i := 0; j := 1; 2: x i := x initial ; x := x initial ; 3: η := z(x initial ) r ; {initial choice of the maximum step size;} 4: while z(x i ) r > e do 5:

x d := Subgoal (x, η, j);

6:

x := AppSteer (x i , x d );

7: if Φ X (x d , x) r > 1
2 Φ X (x d , x i ) r then {if the system is not approaching the subgoal,}

η := η 2 ; {reduce the maximum step size,} 9:

x := x i ; j := 1; {change the path δ 0,t (x).} 10:

else if z(x) r ≥ R k+1 z(x initial ) r then 11:

η := η 2 ;

12:

else if R k z(x initial ) r ≤ z(x) r < R k+1 z(x initial ) r then 13:
i := i + 1; j := j + 1;

14:

x i := x; x d i := x d ;

15:

η := η 2 ;

16:

k := k + 1; 17: else if z(x) r ≤ R k z(x initial ) r then 18:
i := i + 1; j := j + 1;

19:

x i := x; x d i := x d ; 20:
end if 21: end while 22: return x i . Proposition 4.4. Let V c ⊂ Ω be a connected compact set equal to the closure of its interior. Under the assumptions (i) and (ii) of Theorem 4.3, ∀ (x initial , x final ) ∈ V c × V c , Algorithm 4 terminates in a finite number of iterations for any choice of the tolerance e > 0.

Proof of Proposition 4.4. Notice that Lines 17, 18, and 19 in Algorithm 4 are identical to Lines 10, 11, and 12 in Algorithm 3. Therefore, it is enough to show that, after a finite number of iterations, the condition of Line 17 in Algorithm 4 holds true. Another preliminary remark is that the distance z(x)-z(y) r give a rough estimate of the sub-Riemannian distance. Indeed it follows from Theorem 2.3 that, for every pair of close enough points (x, y) ∈ V c × V c , one has

1 C 0 z(x) -z(y) r+1 r ≤ d(x, y) ≤ C 0 z(x) -z(y) 1/(r+1) r , (4.4)
where C 0 is a positive constant. As a consequence, Eq. (4.2) holds true if η ≤ (ε

V c /(2C 0 )) r+1 .
Let us choose a positive η smaller than (ε V c /(2C 0 )) r+1 . We next show that if, at some step i 0 , η < η, then the case of Line 10 and the one of Line 12 occur only in a finite number of iterations. Recall first that, from the proof of Theorem 4.3, one gets, for every i > i 0 ,

z(x d i ) r ≤ z(x i 0 ) r and d(x i , x d i ) ≤ ε V c .
In view of Eq. (4.4), an obvious adaptation of the latter proof yields, for every i > i 0 , d(x i , x d i ) ≤ 2C 0 η 1/(r+1) , and thus

z(x i ) -z(x d i ) r ≤ (2C 2 0 ) 1/(r+1) η 1/(r+1) 2 . Finally one gets z(x i ) r ≤ z(x d i ) r + z(x i ) -z(x d i ) r ≤ z(x i 0 ) r + (2C 2 0 ) 1/(r+1) η 1/(r+1) 2 . (4.5)
On the other hand, there exists an integer k 0 such that η ≥ z(x initial ) r 2 k 0

. This implies that z(x i 0 ) r ≤ R k 0 z(x initial ) r . Up to reducing η, and so increasing k 0 , assume

(2C 2 0 ) 1/(r+1) ( z(x initial ) r 2 k 0 ) 1/(r+1) 2 ≤ R k 0 +1 z(x initial ) r ,
since one has chosen R > ( 1 2 ) 1/(r+1) 2 . Using Eq. (4.5), it holds, for every i

≥ i 0 , z(x i ) r ≤ R k 0 z(x initial ) r + R k 0 +1 z(x initial ) r = R k 0 +1 z(x initial ) r .
Therefore, the case of Line 10 and the one of Line 12 occur in at most k 0 + 1 iterations. Applying again the arguments of the proof of Theorem 4.3, the conclusion follows.

Remark 4.6. It is worth pointing out that the additional steps involved in Algorithm 4 are designed to prevent the sequences (x i ) i≥0 and (x d i ) i≥0 from accumulating toward the boundary of the compact V c . There exist other numerical artifacts of probabilistic nature which solve this problem. One also deduces from the proof of Proposition 4.4 that if the points x initial and x final are far enough from the boundary of V c , the sequences (x i ) i≥0 and (x d i ) i≥ will remain in V c .

Exact Steering Method for Nilpotent Systems

In this chapter, we devise an exact steering method for general nilpotent systems. Without loss of generality, we assume that the system X = {X 1 , . . . , X m } is nilpotent of step r, free up to step r, and given in the canonical form in coordinates x. Recall that, under this assumption, the dynamics is written as follows ẋi = u i ,

if i = 1, . . . , m; ẋI = 1 k! x I L ẋI R , if X I = ad k X I L X I R , I L , I R ∈ H r , (5.1)
where the components of x are numbered by the elements of H r , i.e., for I ∈ H r , the component x I corresponds to the element X I . We also assume that we want to steer the System (5.1) from any point x ∈ R nr to the origin 0 of R nr .

Remark 5.1. Note that these two assumptions are not restrictive since, for general nilpotent systems, in order to steer from x initial to x final , it suffices to apply the Desingularization Algorithm at the final point x final (see also Remark 3.6).

This method can also be applied for the construction of a sub-optimal steering law for the approximate system A X defined in Section 4.1. For practical uses, we require that the inputs give rise to regular trajectories (i.e., at least C 1 ), which are not too "complex" in the sense that, during the control process, we do not want the system to stop too many times or to make a large number of maneuvers.

Several methods were proposed in the literature for steering nilpotent systems. In [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF], the authors make use of piecewise constant controls and obtain smooth controls by imposing some special parameterization (namely by requiring the control system to stop during the control process). In that case, the regularity of the inputs is recovered by using a reparameterization of the time, which cannot prevent in general the occurrence of cusps or corners for the corresponding trajectories. However, regularity of the trajectories is generally mandatory for robotic applications. Therefore, the method proposed in [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF] is not adapted to such applications. In [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF], the proposed inputs are polynomial functions in time, but an algebraic system must be inverted in order to access to these inputs. Moreover, the size and the degree of this algebraic system increase exponentially with respect to the dimension of state space, and there does not exist a general efficient exact method to solve it. Even the existence of solutions is a non trivial issue. Furthermore, the methods [START_REF] Lafferriere | A general strategy for computing steering controls of systems without drift[END_REF] and [START_REF] Lafferriere | A differential geometry approach to motion planning[END_REF] both make use of exponential coordinates which are not explicit and thus require in general numerical integrations of nonlinear differential equations. That prevents the use of these methods in an iterative scheme such as Algorithm 1. Let us also mention the path approximation method by Liu and Sussmann [START_REF] Liu | An approximation algorithm for nonholonomic systems[END_REF], which uses unbounded sequences of sinusoids. Even though this method bears similar theoretical aspects with our method, it is not adapted from a numerical point of view to the motion planning issue since it relies on a limit process of highly oscillating inputs.

Steering by sinusoids

We consider input functions in the form of linear combinations of sinusoids with integer frequencies. In [START_REF] Murray | Nonholonomic motion planning: Steering using sinusoids[END_REF], authors used this family of inputs to control the chained-form systems.

We first note that if every component of the input u = (u 1 , . . . , u m ) in Eq. (5.1) is a linear combination of sinusoids with integer frequencies, then the dynamics of every component in Eq. (5.1) is also a linear combination of sinusoids with integer frequencies, which are themselves linear combinations of frequencies involved in the input u. One may therefore expect to move some components during a 2π time-period without modifying others if the frequencies in u are properly chosen. Due to the triangular form of Eq. (5.1), it is reasonable to expect to move the components of x one after another according to the order "≺" induced by the P. Hall basis. In that case, one must ensure that all the components already moved to their preassigned values return to the same values after each 2π-period of control process, while the component under consideration arrives to its preassigned position. However, all the components cannot be moved independently by using sinusoids. For that purpose, we introduce the following notion of equivalence. Definition 5.1 (Equivalence). Two elements X I and X J in the P. Hall family are said to be equivalent if ∆ i (X I ) = ∆ i (X J ) for i = 1, . . . , m, where we use ∆ i (X I ) to denote the number of times X i occurs in X I . We write X I ∼ X J if X I and X J are equivalent and equivalence classes will be denoted by

E X (ℓ 1 , . . . , ℓ m ) := {X I | ∆ i (X I ) = l i , for i = 1, . . . , m}.
We say that the components x I and x J are equivalent if the corresponding brackets X I and X J are equivalent and equivalent classes for components are defined as follows,

E x (ℓ 1 , . . . , ℓ m ) := {x I |X I ∈ E X (ℓ 1 , . . . , ℓ m )}.
Remark 5.2. We will see in the following subsections that the frequencies occurring in the dynamics of x I only depend on the equivalence class of x I , and not on the structure of the bracket X I . Therefore, the equivalent components (in the sense of Definition 5.1) cannot be moved separately by using sinusoids. Definition 5.2 (Ordering of equivalence classes). Let E x (ℓ 1 , . . . , ℓ m ) and E x ( l1 , . . . , lm ) be two equivalence classes. E x (ℓ 1 , . . . , ℓ m ) is said to be smaller than E x ( l1 , . . . , lm ) if the smallest element (in the sense of "≺") in E x (ℓ 1 , . . . , ℓ m ) is smaller than the one in E x ( l1 , . . . , lm ), and we write (by abuse of notation) E x (ℓ 1 , . . . , ℓ m ) ≺ E x ( l1 , . . . , lm ).

Let {E 1

x , E 2 x , . . . , E N x } be the partition of the set of the components of x induced by Definition 5.1. Assume that, for every pair (i, j) ∈ {1, . . . , N } 2 with i < j, one has E i

x ≺ E j x . Our control strategy consists in displacing these equivalence classes one after another according to the ordering "≺" by using sinusoidal inputs. For every j = 1, . . . , N, the key point is to determine how to construct an input u j defined on [0, 2π] such that the two following conditions are verified: (C1) under the action of u j , every element of E j

x reaches its preassigned value at t = 2π;

(C2) under the action of u j , for every i < j, every element of E i x returns at t = 2π to its value taken at t = 0.

Once one knows how to construct an input u j verifying (C1) and (C2) for every j = 1, . . . , N, it suffices to concatenate them to control the complete system. Definition 5.3 (Concatenation). The concatenation of u 1 , . . . , u N is defined on the interval [0, 2 Nπ] by

u 1 * • • • * u N (t) := u j (t -2(j -1)π), (5.2)
for t ∈ [2(j -1)π, 2jπ] and j ∈ {1, . . . , N }.

Remark 5.3. As we will show later (see Remark 5.8), for every positive integer k, it is possible to make C k concatenations such that the inputs are globally of class C k and the corresponding trajectories are not only piecewise smooth, but also globally of class C k+1 .

Choice of frequencies

In this section, we fix an equivalence class E j x . We choose frequencies in u j such that Conditions (C1) and (C2) are verified. For sake of clarity, we first treat the case m = 2 in Subsections 5.2.1 and 5.2.2., and we show, in Subsection 5.2.3, how to adapt the method to greater values of m.

5.2.1

A simple case: m = 2 and Card (E j x ) = 1 Let x I be the only element of E j

x , and X I the corresponding bracket. Let m 1 := ∆ 1 (X I ), and m 2 := ∆ 2 (X I ).

Proposition 5.1. Consider three positive integers ω 1 , ω 2 , ω 3 , and ε ∈ {0, 1} such that

ω 3 = m 1 ω 1 + (m 2 -1)ω 2 , ε = m 1 + m 2 -1 (mod 2), (5.3)
and ω 2 > (m 1 + m 2 )m 1 .

(5.4)

By choosing properly ζ, the control

u 1 = cos ω 1 t, u 2 = cos ω 2 t + ζ cos(ω 3 t -ε π 2 ), (5.5)
steers, during [0, 2π], the component x I from any initial value to any preassigned final value without modifying any component x J , with J ≺ I. Moreover, x I (2π)x I (0) gives rise to a non zero linear function of ζ, where ζ is the coefficient in front of cos(ω 3 tε π 2 ) in Eq. (5.5). The key point is to understand the frequencies occurring in the dynamics ẋI . Lemma 5.2. For J ≤ I, the dynamics ẋJ is a linear combination of cosine functions of the form

cos{(ℓ 1 ω 1 + ℓ 2 ω 2 + ℓ 3 ω 3 )t -(ℓ 3 ε + ℓ 1 + ℓ 2 + ℓ 3 -1) π 2 }, (5.6)
where

ℓ 1 , ℓ 2 , ℓ 3 ∈ Z satisfy |ℓ 1 | ≤ m 1 , |ℓ 2 | + |ℓ 3 | ≤ m 2 .
In particular, the term

cos[(m 1 ω 1 + (m 2 -1)ω 2 -ω 3 )t -(-ε + m 1 + m 2 -1) π 2 ]
occurs in ẋI with a zero coefficient depending linearly on ζ.

Proof of Lemma 5.2. The proof goes by induction on |J|.

• |J| = 1, the result is true since ẋI 1 = u 1 and ẋI 2 = u 2 .

• Inductive step: Assume that the result holds true for all J such that | J| < s. We show that it remains true for J such that |J| = s.

By construction, we have

X J = ad k X J 1 X J 2 with |J 1 | < s and |J 2 | < s. Then, ẋJ = 1 k! x k J 1 ẋJ 2 , (5.7)
ẋJ 2 is given by the inductive hypothesis and x I 1 is obtained by integration of Eq. (5.6). By using product formulas for cosine function, the result still holds true for J of length s. This ends the proof of Lemma 5.2.

Let us take ω 3 = -ω 2 . It results from Lemma 5.3 that, for every J ≤ I, one has

α J = α J 1 + α J 2 , if X J = [X J 1 , X J 2 ].
Since α 1 = 0 and α 2 = 1, then, over the hyperplane of R 3 defined by ω 3 = -ω 2 , the function α J is a strictly positive number independent of ω 1 and ω 2 .

Let us show now that α J (ω 1 , ω 2 , ω 3 ) is not identically equal to zero over the hyperplane of R 3 defined by

ω 3 = m 1 ω 1 + (m 2 -1)ω 2 . Let ω2 := -m 1 ω 1 /m 2 . One has m 1 ω 1 + (m 2 -1)ω 2 = -ω 2 . It implies that α I (ω 1 , ω2 , m 1 ω 1 + (m 2 -1)ω 2 ) = α I (ω 1 , ω2 , -ω 2 ).
Since the function α I (ω 1 , ω 2 , -ω 2 ) is never equal to zero, and it coincides with the function α I (ω 1 , ω 2 , m 1 ω 1 + (m 2 -1)ω 2 ) at the point (ω 1 , ω2 ), which is not identically equal to zero. Therefore, f I (ω 1 , ω 2 , ω 3 ) is not identically equal to zero over the hyperplane ω 3 = (m 2 -1)ω 2 + m 1 ω 1 . Moreover, as it is a non trivial rational function, it eventually vanishes at a finite number of integer points. Then, we obtain a non zero linear function of ζ, and (1) is now proved. Proposition 5.1 results from (1) and (2).

Proof of Lemma 5.3. The proof goes by induction on |I|. Since ẋ1 = u 1 and ẋ2 = u 2 , by Eq. ( 5.5), one has α 1 = 0 and α 2 = 1. Assume that |J| ≥ 2. By construction, one has

X J = [X J 1 , X J 2 ] with |J 1 | ≤ |J 2 | < |J|.
According to the inductive hypothesis, one has

ẋJ 1 = g J 1 cos{(m J 1 1 ω 1 + m J 1 2 ω 2 )t -(m J 1 1 + m J 1 2 -1) π 2 } +f J 1 cos{(m J 1 1 ω 1 + (m J 1 2 -1)ω 2 -ω 3 )t -(m J 1 1 + m J 1 2 -1 -ε) π 2 } + R J 1 , ẋJ 2 = g J 2 cos{(m J 2 1 ω 1 + m J 2 2 ω 2 )t -(m J 2 1 + m J 2 2 -1) π 2 } +f J 2 cos{(m J 2 1 ω 1 + (m J 2 2 -1)ω 2 -ω 3 )t -(m J 2 1 + m J 2 2 -1 -ε) π 2 } + R J 2 .
This implies that

ẋJ = 1 m J 1 1 ω 1 + m J 1 2 ω 2 g J 1 cos{(m J 1 1 ω 1 + m J 1 2 ω 2 )t -(m J 1 1 + m J 1 2 ) π 2 } + 1 m J 1 1 + (m J 1 2 -1)ω 2 -ω 3 f J 1 a cos{(m J 1 1 ω 1 + (m J 1 2 -1)ω 2 -ω 3 )t -(m J 1 1 + m J 2 2 -ε) π 2 } + R J 1 g J 2 cos{(m J 2 1 ω 1 + m J 2 2 ω 2 )t -(m J 2 1 + m J 2 2 -1) π 2 } +f J 2 a cos{(m J 2 1 ω 1 + (m J 2 2 -1)ω 2 -ω 3 )t -(m J 2 1 + m J 2 2 -1 -ε) π 2 } + R J 2 = 1 2 g J 1 g J 2 m J 1 1 ω 1 + m J 1 2 ω 2 cos{(m J 1 ω 1 + m J 2 ω 2 )t -(m J 1 + m J 2 -1) π 2 } + 1 2 g J 1 f J 2 m J 1 1 ω 1 + m J 1 2 ω 2 + g J 2 f J 1 m J 1 1 ω 1 + (m J 1 2 -1)ω 2 -ω 3 cos{(m J 1 ω 1 + m J 2 ω 2 -ω 3 )t -(m J 1 + m J 2 -1 -ε) π 2 } + R J = g J cos{(m J 1 ω 1 + m J 2 ω 2 )t -(m J 1 + m J 2 -1) π 2 } +f J cos{(m J 1 ω 1 + m J 2 ω 2 -ω 3 )t -(m J 1 + m J 2 -1 -ε) π 2 } + R J .
Therefore, one obtains

α J = m J 1 1 ω 1 +m J 1 2 ω 2 m J 1 1 ω 1 +(m J 1 2 -1)ω 2 -ω 3 α J 1 + α J 2 .

5.2.2

A more general case: m = 2 and Card (E j x ) > 1 In general, given a pair (m 1 , m 2 ), the equivalence class E x (m 1 , m 2 ) contains more than one element. This situation first occurs for Lie brackets of length 5. For instance, given the pair (3, 2), one has both

X I = [X 2 , [X 1 , [X 1 , [X 1 , X 2 ]]]] and X J = [[X 1 , X 2 ], [X 1 , [X 1 , X 2 ]]]
. By Lemma 5.2, if one chooses a 4-tuple verifying the resonance condition (5.3) for ẋI , the same resonance occurs in ẋJ . Such two components cannot be independently steered by using resonance. The idea is to move simultaneously these components. For instance, one can choose (u 1 , u 2 ) as follows:

u 1 (t) = cos ω 1 t, u 2 (t) = cos ω 2 t + a I cos ω 3 t + cos ω 4 t + a J cos ω 5 t,
where ω 1 = 1, ω 2 is chosen according to Eq. (5.4), ω 3 = (m 2 -1)ω 2 +m 1 ω 1 , and ω 5 = (m 2 -1)ω 4 +m 1 ω 1 , with ω 4 large enough to guarantee Condition (C2). After explicit integration of Eq. ( 5.1), one obtains

f I (ω 1 , ω 2 ) f I (ω 1 , ω 4 ) f J (ω 1 , ω 2 ) f J (ω 1 , ω 4 ) a I a J = A a I a J = x I (2π) -x I (0) x J (2π) -x J (0) ,
where f I and f J are two rational functions of frequencies. Thus, the pair (u 1 , u 2 ) controls exactly and simultaneously x I and x J , provided that the matrix A is invertible. We generalize this strategy in the following paragraphs. Assume that

E j x (m 1 , m 2 ) = {x I 1 , . . . , x I N }.
} belonging to N m 1 N × N m 2 N such that      ∀ j = 1 . . . N, ω * 2j = m 1 i=1 ω i 1j + m 2 -1 i=1 ω i 2j , ε = m 1 + m 2 -1 (mod 2), (5.11)
and

∀j = 1 . . . N -1,            ω 1 11 ∈ N ; ω i+1 1j > m 1 ω i 1j ; i = 1 . . . m 1 , ω 1 2j > m 1 ω m 1 1j ; ω i 2j > m 2 ω i-1 2j + m 1 ω m 1 1j ; i = 2 . . . m 2 -1, ω 1 1j+1 > m 2 ω m 2 -1 2j + m 1 ω m 1 1j .
(5.12)

Then, the input u := (u 1 , u 2 ) defined by

           u 1 := N j=1 m 1 i=1 cos ω i 1j t, u 2 := N j=1 m 2 -1 i=1 cos ω i 2j t + a j cos(ω * 2j t -ε π 2 ), (5.13)
steers the components (x I 1 , . . . , x I N ) from an arbitrary initial condition (x I 1 (0), . . . , x I N (0)) to an arbitrary final one (x I 1 (2π), . . . , x I N (2π)), without modifying any other component having been previously moved to its final value.

This result generalizes Proposition 5.1. The proof is decomposed in two parts as follows:

Part I: we show that, if (5.12) holds and the control functions are of the form (5.13), then (5.11) is the only resonance occurring in ( ẋI 1 , . . . , ẋI N );

Part II: as the resonance gives rise to a system of linear equations on (a 1 , . . . , a N ), we recover the invertibility of this system by choosing suitable frequencies in the control function (5.13).

Part I Frequencies and Resonance

Consider inputs of the form (5.13). Generalizing Lemma 5.2, we give a general form of frequencies involved in ẋJ .

Lemma 5.5. The dynamics ẋJ is a linear combination of cosine functions of the form

(ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 )t -(ℓ 1 + ℓ 2 + m * 2 -1 + ℓ * 2 ε) π 2 , (5.14)
where

ℓ 1 • ω 1 = N j=1 m 1 i=1 ℓ i 1j ω i 1j , ℓ 2 • ω 2 = N j=1 m 2 -1 i=1 ℓ i 2j ω i 2j , ℓ * 2 • ω * 2 = N j=1 ℓ * 2j ω * 2j , ℓ 1 = N j=1 m 1 i=1 ℓ i 1j , ℓ 2 = N j=1 m 2 -1 i=1 ℓ i 2j , ℓ * 2 = N j=1 m * 2j , with (ℓ i 1j , ℓ i 2j , ℓ * 2j ) ∈ Z 3 . Let |ℓ 1 | = N j=1 m 1 i=1 |ℓ i 1j |, |ℓ 2 | = N j=1 m 2 -1 i=1 |ℓ i 2j |, and |ℓ * 2 | = N j=1 |ℓ * 2j |, then, one has |ℓ 1 | ≤ ∆ 1 (X J ), |ℓ 2 | + |ℓ * 2 | ≤ ∆ 2 (X J ).
Proof of Lemma 5.5. The proof goes by induction on |J|.

• |J| = 1: the result is true since ẋ1 = u 1 and ẋ2 = u 2 .

• Inductive step: Assume that the result holds true for all x J such that 1 ≤ | J| < s. We show that it remains true for x J with |J| = s. By construction, we have X Then, by the inductive hypothesis, we have

J = ad k X J 1 X J 2 , and ẋJ = 1 k! x k J 1 ẋJ 2 , ( 5
ẋJ 1 = LinCom cos{(ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 )t -(ℓ 1 + ℓ 2 + ℓ * 2 -1 + ℓ * 2 ε) π 2 } , (5.16) ẋJ 2 = LinCom cos{( l1 • ω 1 + l2 • ω 2 + l * 2 • ω * 2 )t -( l1 + l2 + l * 2 -1 + l * 2 ε) π 2 } , (5.17)
where LinCom{•} stands "linear combination".

Eq. (5.16) implies that

x J 1 = LinCom cos{(ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 )t -(ℓ 1 + ℓ 2 + ℓ * 2 -1 + ℓ * 2 ε) π 2 - π 2 } = LinCom cos{(ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 )t -(ℓ 1 + ℓ 2 + ℓ * 2 + ℓ * 2 ε) π 2 } . (5.18)
For notational ease, we will only write down the case ẋJ = x J 1 ẋJ 2 . Using product formulas for cosine function, one has

ẋJ = LinCom cos{[(ℓ 1 ± l1 ) • ω 1 + (ℓ 2 ± l2 ) • ω 2 + (ℓ * 2 ± l * 2 ) • ω * 2 ]t -[(ℓ 1 ± l1 ) + (ℓ 2 ± l2 ) + (ℓ * 2 ± l * 2 ) -1 + (ℓ * 2 ± l * 2 )ε] π 2 } . (5.19)
Moreover, according to the inductive hypothesis, one has

|ℓ 1 | ≤ ∆ 1 (X J 1 ), |ℓ 2 | + |ℓ * 2 | ≤ ∆ 2 (X J 1 ), and | l1 | ≤ ∆ 1 (X J 2 ), | l2 | + | l * 2 | ≤ ∆ 2 (X J 2 ). Then, one gets | l1 ± l1 | ≤ ∆ 1 (X J ), and | l2 ± l2 | + |ℓ * 2 ± l * 2 | ≤ ∆ 2 (X J
). This concludes the proof of Lemma 5.5. By Lemma 5.5, one gets a non trivial contribution for x J if the resonance condition

ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 = 0, ℓ * 2 ε + ℓ 1 + ℓ 2 + ℓ * 2 -1 ≡ 0 (mod 2), (5.20)
is verified by the frequencies of some cosine functions involved in ẋJ .

Lemma 5.6. Under conditions (5.11) and (5.12) in Proposition 5.4, one gets a non trivial contribution on x I j depending linearly on a j for all j = 1 . . . , N.

Proof of Lemma 5. , ω * 2N }, and ε ∈ {0, 1} verifying (5.11). We show that it is the only resonance occurring in ẋI j . Indeed, by Lemma 5.5, the integer part of frequencies in ẋI j is in the following form

ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 = N j=1 m 1 i=1 ℓ i 1j ω i 1j + N j=1 m 2 -1 i=1 ℓ i 2j ω i 2j + N j=1 ℓ * 2j ω * 2j = N j=1 m 1 i=1 ℓ i 1j ω i 1j + N j=1 m 2 -1 i=1 ℓ i 2j ω i 2j + N j=1 ℓ * 2j m 1 i=1 ω i 1j + m 2 -1 i=1 ω i 2j = N j=1 m 1 i=1 (ℓ i 1j + ℓ * 2j )ω i 1j + N j=1 m 2 -1 i=1 (ℓ i 2j + ℓ * 2j )ω i 2j .
(5.21)

By Condition (5.12), Eq. (5.21) is equal to zero if and only if

ℓ i 1j + ℓ * 2j = 0, for i = 1, . . . , m 1 , ℓ i 2j + ℓ * 2j = 0, for i = 1, . . . , m 2 -1.
Then, one has

|ℓ 1 | = N j=1 m 1 i=1 |ℓ i 1j | = N j=1 m 1 i=1 |ℓ * 2j | = m 1 N j=1 |ℓ * 2j |, |ℓ 2 | = N j=1 m 2 -1 i=1 |ℓ i 2j | = N j=1 m 2 -1 i=1 |ℓ * 2j | = (m 2 -1) N j=1 |ℓ * 2j |.
However, by Lemma 5.5, one knows that

|ℓ 1 | ≤ m 1 and |ℓ 2 | + |ℓ * 2 | ≤ m 2 .
Then, one necessarily has m * 2j = 0 for all j = 1, . . . , N. In that case, one obtains

ℓ * 2 ε + ℓ 1 + ℓ 2 + ℓ * 2 -1 = -1 = 0 (mod 2).
In conclusion, the resonance condition (5.20) does not hold for any 4-tuple (ℓ

1 , ℓ 2 , ℓ * 2 , ε) different from (m 1 , m 2 -1, -1, m 1 + m 2 -1 (mod 2)).
By Eq. (5.13), the power of a j is equal to the number of times ω * 2j occurs in the resonance condition (5.3). Since the latter is equal to 1, we obtain a linear function of a j . This ends the proof of Lemma 5.6. Lemma 5.7. If x J ∈ E i x and i < j, then x J (2π)x J (2π) = 0.

Proof of Lemma 5.7. We first note that Eq. (5.21) still holds true. Recall its expression here.

ℓ 1 • ω 1 + ℓ 2 • ω 2 + ℓ * 2 • ω * 2 = N j=1 m 1 i=1 (ℓ i 1j + ℓ * 2j ) • ω i 1j + N j=1 m 2 -1 i=1 (ℓ i 2j + ℓ * 2j ) • ω i 2j (5.22)
By condition (5.12) in Proposition 5.4, Eq. (5.22) is equal to zero if and only if ℓ i 1j + ℓ * 2j = 0 for i = 1, . . . , m 1 , j = 1, . . . , N and ℓ i 2j + ℓ * 2j = 0 for i = 1, . . . , m 2 -1, j = 1, . . . , N. In that case, one has

|ℓ 1 | = m 1 N j=1 |ℓ * 2j |, |ℓ 2 | + |ℓ * 2 | = m 2 N j=1 |ℓ * 2j |.
One also knows that

|ℓ 1 | ≤ ∆ 1 (X J ), |ℓ 2 | + |ℓ * 2 | ≤ ∆ 2 (X J ), with ∆ 1 (X J ) < m 1 or ∆ 2 (X J ) < m 2 .
Therefore, one has ℓ * 2j = 0 for all j = 1, . . . , N. This implies that

ℓ * 2 ε + ℓ 1 + ℓ 2 + ℓ * 2 -1 = -1 = 0 (mod 2).
In conclusion, the resonance condition (5.20) does not hold true. This ends the proof of Lemma 5.7.

Part II Invertibility

As a consequence of Lemma 5.6, one has   

x I 1 (2π)x I 1 (0) . . .

x I N (2π) -x I N (0)    = A(ω 1 11 , . . . , ω m 2 -1 2N , ω * 2N )    a 1 . . . a N    (5.23) =    f X I 1 (ω
)       a 1 . . . a N    ,
where f X I j : R m → R are rational functions of frequencies, and every ω * 2j verifies Eq. (5.11) for j = 1, . . . , N. Definition 5.4 (Control matrix and control vector ). The matrix A and the vector (a 1 , . . . , a N ) occurring in Eq. (5.23) are called respectively control matrix and control vector associated with the equivalence class E j

x .

We show in the sequel that it is possible to choose integer frequencies {ω , ω * 2N }, so that the invertibility of the control matrix A is guaranteed, as well as the non-resonance of every component x J belonging to a class smaller than E j

x . For j = 1, . . . , N, we use P j to denote the hyperplane in R M with M := m 1 + m 2 defined by Eq. (5.11), for which we recall the expression next,

ω * 2j = m 1 i=1 ω i 1j + m 2 -1 i=1 ω i 2j .
We start by showing that the function detA(ω 1 11 , . . . , ω * 2N ) is not identically equal to zero on ∩ N j=1 P j . This is a consequence of the following lemma.

Lemma 5.8. The family of functions

{f X I 1 (ω 1 1 , . . . , ω m 2 -1 2 , ω * 2 ), . . . , f X I N (ω 1 1 , . . . , ω m 2 -1 2 , ω * 2 )}
is linearly independent on the hyperplane P in R M defined by the equation

ω * 2 = m 1 i=1 ω i 1 + m 2 -1 i=1 ω i 2 .
Proof of Lemma 5. If we apply one control of the form {v i = cos ν i t} i=1...M , with ν m = m-1 i=1 ν i , to System (5.24), then, by explicit integration, there exists, for each component y J j , a fractional function f Y J j : R m → R such that

y J j (2π) -y J j (0) = f Y J j (ν 1 , . . . , ν M ), for ν M = M -1 i=1 ν i . (5.25) Claim 11. The family of functions {f Y J 1 , . . . , f Y I Ñ } is linearly independent on the hyperplane in R M defined by ν M = M -1 i=1 ν i . Proof of Claim 11. We first define f Y J j by f Y J j (ν 1 , . . . , ν M ) = f Y J j (ν 1 , . . . , -ν M ). (5.26)
Then, it is easy to see that f Y J j verifies the following inductive formula:

1. for J = i = 1 . . . M, f Y J (ν i ) = 1 ν i ; 2. for |J| > 1, Y J = [Y J 1 , Y J 2 ]
, there exists an injective function

σ J : {1, . . . , m J } → {1, . . . , M} such that f Y J (ν σ J (1) , . . . , ν σ J (m J ) ) = f Y J 1 (ν σ J (1) , . . . , ν σ J (m J 1 ) ) m J 1 i=1 ν σ J (i) f Y J 2 (ν σ J (m J 1 +1) , . . . , ν σ J (m J ) ), (5.27)
where m J := ∆(Y J ), m J 1 := ∆(Y J 1 ), and m J 2 := ∆(Y J 2 ).

We note that the family of rational functions f Y J is well defined for all the Lie brackets Y J such that ∆ i (Y J ) ≤ 1, i = 1, . . . , M. The algebraic construction could be extended to all the Lie brackets, but it is not necessary for our purpose. We also note that Claim 11 is equivalent to the fact that the family of rational functions { f Y J j (ν 1 , . . . , ν M )} j=1,..., Ñ is linearly independent over the hyperplane M i=1 ν i = 0. Recall that every element Y J j in the family {Y J 1 , . . . , Y J Ñ } writes uniquely as

Y J j = [Y J j 1 , Y J j 2 ].
(5.28) Definition 5.5 (Left and right factors). For J ∈ {J 1 , . . . , J Ñ }, the left factor L(J) and the right factor R(J) of J are defined in such a way that

Y J = [Y L(J) , Y R(J) ].
Let L * be defined by L * := max j=1,..., Ñ {L(J j )}.

(5.29)

The integer L * is well defined since a P. Hall family is totally ordered. Thus, there exists J * ∈ {J 1 , . . . , J Ñ } such that L * = L(J * ). Then, define R * := R(J * ) and set m * = |L * |. Let Λ = Λ L ∪ Λ R and Λ = {1, . . . , Ñ} \ Λ, with Λ L and Λ R defined by

Λ L := {j ∈ {1, . . . , Ñ}, such that Y L(J j ) ∼ Y L }, (5.30)
Λ R := {j ∈ {1, . . . , Ñ}, such that Y L(J j ) ∼ Y R }.

(5.31)

Then, for all j ∈ Λ, there exists an injection function

σ j : {1, . . . , M} → {1, . . . , M} such that one has f Y J j (ν 1 , . . . , ν M ) = f Y L(J j ) (ν σ j (1) , . . . , ν σ j (m * ) ) m * i=1 ν σ j (i) f Y R(J j ) (ν σ j (m * +1) , . . . , ν σ j (M ) ), if j ∈ Λ L , (5.32) f Y J j (ν 1 , . . . , ν M ) = f Y L(J j ) (ν σ j (m * +1) , . . . , ν σ j (M ) ) M i=m * +1 ν σ j (i) f Y R(J j ) (ν σ j (1) , . . . , ν σ(m * ) ), if j ∈ Λ R . (5.33)
Note that, for all j 1 and j 2 in Λ L , one has {ν σ j 1 (1) , . . . , ν σ j 1 (m * ) } = {ν σ j 2 (1) , . . . , ν σ j 2 (m * ) }. Denote by Ξ L the set of variables involved in f Y L(J j ) with j ∈ Λ L . A similar property holds for Λ R . For all j 1 and j 2 in Λ R , one has {ν σ j 1 (m * +1) , . . . , ν σ j 1 (M ) } = {ν σ j 2 (m * +1) , . . . , ν σ j 2 (M ) }. Denote by Ξ R the set of all variables occurring in f Y L(J j ) with j ∈ Λ R . Then one has Ξ L ∪ Ξ R = {ν 1 , . . . , ν M }. By abuse of notation, we re-write Eqs. (5.32) and (5.33) in the following form:

f Y J j (ν 1 , . . . , ν M ) = f Y L(J j ) (Ξ L ) νk ∈Ξ L νk f Y R(J j ) (Ξ R ), if j ∈ Λ L ;
(5.34)

f Y J j (ν 1 , . . . , ν M ) = f Y L(J j ) (Ξ R ) νk ∈Ξ R νk f Y R(J j ) (Ξ L ), if j ∈ Λ R .
(5.35)

Moreover, by the resonance condition M i=1 ν i = 0, Eq. (5.35) becomes

f Y J j (ν 1 , . . . , ν M ) = f Y L(J j ) (Ξ R ) -νk ∈Ξ L νk f Y R(J j ) (Ξ L ), if j ∈ Λ R . (5.36)
We now prove that the family of rational functions { f Y J j (ν 1 , . . . , ν M )} j=1,..., Ñ is linearly independent over the hyperplane M i=1 ν i = 0. The proof goes by induction over the length of the Lie brackets under consideration. For the brackets of length 1, the result is obviously true. Assume that the result holds for all brackets of length smaller than M -1, M ≥ 2.

Assume that there exist ℓ j ∈ R Ñ such that

= j∈Λ L ℓ j f Y L(J j ) (Ξ L ) νk ∈Ξ L νk f Y R(J j ) (Ξ R ) - j∈Λ R ℓ j f Y L(J j ) (Ξ R ) νk ∈Ξ L νk f Y R(J j ) (Ξ L ) + j∈ Λ ℓ j f Y J j (ν 1 , . . . , ν M ) = 0.
Multiplying Eq. (5.38) by the factor νk ∈Ξ L νk , one gets

j∈Λ L ℓ j f Y L(J j ) (Ξ L ) f Y R(J j ) (Ξ R ) - j∈Λ R ℓ j f Y L(J j ) (Ξ R ) f Y R(J j ) (Ξ L ) + ( νk ∈Ξ L νk ) j∈ Λ ℓ j f Y J j (
j∈Λ L ℓ j f Y L(J j ) (Ξ L ) f Y R(J j ) (Ξ R ) - j∈Λ R ℓ j f Y L(J j ) (Ξ R ) f Y R(J j ) (Ξ L ) = 0.
(5.40)

Fixing variables belonging to Ξ R , Eq. (5.40) is a linear combination of elements of the family

{ f Y L(J j ) (Ξ L )} j∈Λ L ∪ { f Y R(J j ) (Ξ L )} j∈Λ R
associated with elements of length m * in the P. Hall family. By the inductive hypothesis, this family is linearly independent over the hyperplane of R m * defined by νk ∈Ξ L νk = 0. We therefore obtain that

ℓ j f Y R(J j ) (Ξ R ) = 0, for all j ∈ Λ L , (5.41)
ℓ j f Y L(J j ) (Ξ R ) = 0, for all j ∈ Λ R . Consider now the maximum left factor for j ∈ Λ and iterate the same reasoning used for Eq. (5.37). We deduce that ℓ j = 0 for every j ∈ Λ. Therefore, the family { f Y J j (ν 1 , . . . , ν M )} j=1,..., Ñ is linearly independent over the hyperplane M i=1 ν i = 0 and this concludes the proof of Claim 11. We are now in a position to proceed with the argument of Lemma 5.8. Let X I be an element of E X (m 1 , m 2 ), M := m 1 + m 2 and N :=Card E X (m 1 , m 2 ). Consider also another family of M indeterminates Y = {Y 1 , . . . , Y M } and let H Y be the P. Hall family over Y . Finally, consider all the elements of the class E Y (1, . . . , 1) = {Y J 1 , . . . , Y J Ñ } in H Y .

Let Π be the algebra homomorphism from L(Y ) to L(X) defined by Π(Y i ) = X 1 , for i = 1, . . . , m 1 , (5.44) Π(Y i ) = X 2 , for i = m 1 + 1, . . . , M.

(5.45)

Note that the map Π is surjective from E Y onto E X . Consider the following vector fields

V Y = {v 1 Y 1 + • • • + v M Y M },
where v i = cos ω i t, for i = 1 . . . M -1, and v M = cos(ω M t + ε π 2 ), (5.46) with ω M = M -1 i=1 ω i , and ω i verifying the non-resonance conditions.

Then, the non-autonomous flow of V Y between 0 and 2π is given by

-→ exp(V Y )(0, 2π) = e f Y J 1 Y J 1 • • • • • e f Y J Ñ Y J Ñ • J>J 1 e f Y J Y J .
(5.47)

Let us now apply Π to V Y , we get

Π(V Y ) := V X = {v 1 Π(Y 1 ) + • • • + v m Π(Y m )} = {u 1 X 1 + u 2 X 2 }, (5.48)
where

u 1 = m 1 i=1 v i = m 1 i=1
cos ω i t, (5.49)

u 2 = m i=m 1 +1 v i = m-1 i=m 1 +1
cos ω i t + cos(ω m t + ε π 2 ).

(5.50)

Then, the non-autonomous flow of V X between 0 and 2π is given by

-→ exp(V X )(0, 2π) = e f Y J 1 Π(Y J 1 ) • • • • • e f Y J Ñ Π(Y J Ñ ) • J>J 1 e f Y J Π(Y J ) = e Ñ j=1 f Y J j Π(Y J j ) • J>J 1 e f Y J Π(Y J ) . (5.51)
We also know that -→ exp(V X )(0, 2π) = e f X I 1 Recall that Π is surjective from E Y (1, . . . , 1) onto E X (m 1 , m 2 ). Therefore, by identifying Eqs. (5.51) and (5.52), we obtain that, for all j = 1, . . . , N, f X I j is a linear combination of f Y J i with i = 1, . . . , Ñ , i.e.,

X I 1 • • • • • e
f X I j = Ñ i=1 α j i f Y J i .
(5.53)

Since the family (f Y J i ) i=1,..., Ñ is linearly independent and the matrix A := (α j i ) i=1,...,N ;j=1,..., Ñ is surjective, we conclude that the family (f X I j ) j=1,...,N is also linearly independent. This ends the proof of Lemma 5.8.

A consequence of Lemma 5.8 is the following. Corollary 5.9. With the above notations, the function detA is not identically equal to zero on ∩ N j=1 P j .

Proof of Corollary 5.9. For j = 1, . . . , N, we define the vector L j by L j = f X I j (ω 1 11 , . . . , ω m 2 -1

21

, ω * 21 ), • • • , f X I j (ω 1 1N , . . . , ω m 2 -1

2N

, ω * 2N )

T .

sinusoids so that the frequencies are Z-linear combinations of the frequencies occurring in the control functions. The proof is the same as that of Lemma 5.5, up to extra notation. Since Lemma 11 depends only on the length of the Lie brackets, but not on the number of vector fields, the proof of Lemma 5.8 does not depend on m, either. In order to prove a similar result for m > 2, we just need to re-project Eqs. (5.44) and (5.45) to m vector fields instead of 2.

Exact and sub-optimal steering law

In this section, we explain how we can devise, from Proposition 5.4, an exact and sub-optimal steering law (cf. Definition 2.17) Exact m,r for the approximate system, which is already in the canonical form and how Exact m,r can be incorporated into the global approximate steering algorithm (cf. Section 4.2). Note that Exact m,r only depends on the number of controlled vector fields m and on the maximum degree of nonholonomy r.

Recall that the components of x ∈ R n r are partitioned into equivalence classes {E 1

x , E 2 x , . . . , E N x } according to Definition 5.1 in such a way that E i

x ≺ E j x for (i, j) ∈ {1, . . . , N} 2 and i < j. For every equivalence class E j x , Proposition 5.4 and Subsection 5.2.3 guarantee that we can choose frequencies such that the corresponding control matrix A j (cf. Definition 5.4) is invertible and the corresponding control function u j obtained by Eq. (5.13) steers all the elements of E j

x from an arbitrary initial value to the origin 0 (see Remark 4.5) without modifying any elements belonging to smaller classes.

Let x initial ∈ R n r . Let B j := A -1 j and N j := Card(E j x ), j = 1, . . . , N. For x ∈ R n r , we will use [x] i,...,k with 1 ≤ i < k ≤ n r to denote the vector (x i , . . . , x k ), and x to denote the pseudo-norm of x defined by the free weights (cf. Definition 2.13 and Definition 3.7). We also define an intermediate function Position(u) as follows: if System (5.1) starts from x = 0 and evolves under the action of u, then Position(u) returns its position at t = 2π. Algorithm 5 Exact Steering Law: Exact m,r (x initial ) Require: B 1 , . . . , B N , and N 1 , . . . , N N ;

1: λ := x initial 0 ; 2: x new := δ 0, 1 λ (x initial ); 3: ûnorm := 0; 4: i := 0; 5: for j = 1, . . . , N do 6:

x := [x new ] i+1,...,i+N j ; 7:

a j := B j x; 8: construct u j from a j by Eq. (5.13); 9:

x new := x new + Position(u j ); i = i + N j ; 12: end for 13: return û := λû norm . Proposition 5.11. For every x initial ∈ R n r , the input given by Exact m,r (x initial ) steers System (5.1) from x initial to 0 exactly. Moreover, there exists a constant C > 0 such that ℓ(Exact m,r (x initial )) ≤ Cd(x initial , 0), ∀ x initial ∈ R n r , (5.58)

where we use d to denote the sub-Riemannian distance defined by the family X.

Remark 5.9. Using the idea presented in Remark 5.8 together with Remark 5.7, it is easy to conclude that, for general nilpotent systems, the resulting trajectories are globally C 1 curves and the regularity does not depend on the time-parameterization of the trajectories.

For the sake of completeness, we give in this short appendix the proof of Theorem 1.1 together with some comments on Algorithm 2.

A Proof of Theorem 1.1

We first note that Steps 1 through 5 in Algorithm 2 are straightforward.

Theorem 3.5 guarantees that the Desingularization Algorithm (Section 3.3) provides us with a new family of vectors fields ξ = {ξ 1 , . . . , ξ m }, which is regular and free up to step r with r denoting the maximum value of the degree of nonholonomy of the original system X = {X 1 , . . . , X m }, on the corresponding compact set V c J i . Then, we construct the approximate system A ξ using the procedure presented in Section 4.1 and provide it with the sub-optimal steering law Exact m,r defined in Algorithm 5. The sub-optimality of Exact m,r is guaranteed by Theorem 5.12. Therefore, by Theorem 4.2, the LAS method AppSteer associated with A ξ and its steering law is uniformly contractive on the compact set V c J i × BR (0). Then, by Theorem 4.3, GlobalFree ( x 0 , x 1 , e, V c , AppSteer) provided by Algorithm 3 terminates in a finite number of steps and stops at a point x such that d( x, x 1 ) < e. Since there is a finite number of compacts to be explored, we conclude that Algorithm 2 terminates in a finite number of steps and steers the system (Σ) from x initial to some point x such that d(x, x final ) < e. This ends the proof of Theorem 1.1.

B About the control set

Let U ⊂ R m be any neighborhood of the origin. Then every trajectory of (2.1) corresponding to the inputs produced by Algorithm 2 can be time-reparameterized so that the resulting trajectory of (2.1) is associated with an input taking values in U.

C Getting trajectories of class C 1 for the original control system

We can slightly modify Algorithm 2 to get trajectories of class C 1 for the original control system (Σ). This is equivalent to ask for continuous inputs produced by the algorithm. According to Remark 5.8, inputs can be made continuous within each iteration step in Algorithm 3 since they are computed based the nilpotent approximate system. By using the same interpolation technique as presented in Remark 5.8, we can still produce inputs which remain continuous from one step to another in Algorithm 3. Therefore, trajectories of class C 1 for the control system (Σ) are obtained.

. 8 )Remark 2 . 7 .

 827 Proof of Corollary 2.5. Under the hypotheses (i) -(iv), one immediately extends Proposition 2.4 and obtains that there exist continuous positive functions C e (•) and ε e (•) such that inequality (2.5) holds true, with z = Φ(a, •) and X = A(a), for every pair (x, a) ∈ Ω × Ω with d(x, a) < ε e (a) and every integrable input function u(•) with ℓ(u) < ε e (a). The remaining argument is standard and one conclude easily. Since the growth vector and the weights do not remain constant in any open neighborhood of a singular point, privileged coordinates z cannot vary continuously in any open neighborhood of that singular point. Therefore, around a singular point, the distance estimations provided in Eqs.

Definition 3 . 1 (

 31 Length of the elements of L(I)). The length of an element I of a free Lie algebra L(I), denoted by |I|, is defined inductively by |I| := 1, for I = 1, . . . , m;

Definition 3 . 2 (

 32 P. Hall basis). A subset H = {I j } j∈N of L(I) is the P. Hall basis of L(I) if (H1), (H2), (H3), and (H4) are verified. (H1) If |I i | < |I j |, then I i ≺ I j ; (H2) {1, . . . , m} ⊂ H, and we impose that 1 ≺ 2 ≺ • • • ≺ m; (H3) every element of length 2 in H is in the form [I i , I j ] with (I i , I j ) ∈ I × I and I i ≺ I j ;

Claim 5 .

 5 The vector fields {ξ s+1 i } i=1,...,m are well defined. Moreover, one has ord s a s+1 (P k ) = s. Proof of Claim 5. Consider I k ∈ G s+1 \ J s+1 , then one has I k = [I k 1 , I k 2 ]. By Eq. (3.6), one has

  s a s+1 (P k j ) = |I j | -|I k | and ord s a s+1 ( Rk j ) > |I j | -|I k |. Proof of Claim 6. The proof goes by induction on the length |I k |. For |I k | = 1, one has

Claim 9 .

 9 0, for all β = (β 1 , . . . , β j-1 ) such that w(β) ≤ |I j | -1. Using again Lemma 3.7, one has ord s+1 a s+1 (z s+1 j ) > |I j | -1, for I j ∈ H s+1 . By construction, one already has that ord s+1 a s+1 (z s+1 j ) ≤ w j = |I j |. Therefore, one finally gets ord s+1 a s+1 (z s+1 j ) = |I j |, for I j ∈ H s+1 . Claim 8 is now proved. The change of coordinates (Ψ s+1 j ) j=1,..., n s+1 is well defined, i.e., Property (A4) holds true. Proof of Claim 9. After performing Steps (s+1)-4-(a) and (s+1)-4-(b), one obtains a new system of coordinates zs+1 . In this system of coordinates, one can write ξ s+1 i as

. 15 )

 15 with |J 1 | < |J|, |J 2 | < |J|, and k|J 1 | + |J 2 | = |J|.

  j (ν 1 , . . . , ν M ) = j∈Λ ℓ j f Y J j (ν 1 , . . . , ν M ) + j∈ Λ ℓ j f Y J j (ν 1 , . . . , ν M ) (5.38)

  ν 1 , . . . , ν M ) = 0. (5.39) Since L * is the maximal element among the left factors of Lie brackets of length M, the fraction f Y J j does not contain the factor νk ∈Ξ L νk for all j ∈ Λ. Therefore, on the hyperplane of R m * defined by νk ∈Ξ L νk = 0, one has

(5. 42 )

 42 Since Eqs.(5.41) and (5.42) hold true over the whole hyperplane of R M -m * defined by νk ∈Ξ R νk = 0, one has ℓ j = 0 for every j ∈ Λ. Therefore, Eq. (5.38) becomes j∈ Λ ℓ j f Y J j (ν 1 , . . . , ν M ) = 0.(5.43)

10 :

 10 ûnorm := ûnorm * u j (cf. Definition 5.3); 11:

  |I k 1 | ≤ s and |I k 2 | ≤ s, we have k 1 ≤ n s and k 2 ≤ n s , thus the right-hand side of the above equation is well defined in coordinates z s = (z s 1 , . . . , z s ns ). Therefore, the new vector fields {ξ s+1 i } i=1,...,m are also well defined. Since ord s a s+1 (z s k 1 P k 2 ) = ord s a s+1 (z s k 1 )+ord s a s+1 (P k 2 ), and by inductive hypothesis (namely (A2) holds true at step s), one has ord s a s+1 (z s k 1 ) = |I k 1 |, and ord s a s+1 (P k 2 ) = |I k 2 |-1. Therefore, one has ord s a s+1

  The main result is given next.

	Proposition 5.4. Consider
	{ω 1 11 , . . . , ω m 1 11 }, . . . , {ω 1 1N , . . . , ω m 1 1N },
	{ω 1 21 , . . . , ω m 2 -1 21	, ω * 21 }, . . . , {ω 1 2N , . . . , ω m 2 -1 2N	, ω * 2N

  [START_REF] Bourbaki | Groupe et Algèbre de Lie[END_REF]. It is clear that the resonance condition (5.20) holds for

	{ω 1 11 , . . . , ω m 1 11 }, . . . , {ω 1 1N , . . . , ω m 1 1N },
	{ω 1 21 , . . . , ω m 2 -1 21	, ω * 21 }, . . . , {ω 1 2N , . . . , ω m 2 -1 2N

  [START_REF] Chitour | Path planning on compact Lie groups using a homotopy method[END_REF]. The first part of the argument consists in considering a family of M indeterminates Y = {Y 1 , . . . , Y M } and the associated control system Let H Y be a P. Hall family over Y . Consider the elements {Y J 1 , . . . , Y J Ñ } in H Y of length M such that ∆ i (Y J j ) = 1, for i = 1 . . . M, and j = 1, . . . , Ñ, and the corresponding components {y J 1 , . . . , y J Ñ } in exponential coordinates.

	M
	ẏ =	v i Y i .	(5.24)
	i=1

* This work was supported by grants from Digiteo and Région Ile-de-France, by the ANR project GCM, program "Blanche", project number NT09 504490, and by the Commission of the European Communities under the 7th Framework Program Marie Curie Initial Training Network (FP7-PEOPLE-2010-ITN), project SADCO, contract number 264735. † Y. Chitour is with L2S,

Proof of Proposition 5.1. First note that integrating between 0 and 2π a function of the form cos(γt + γ π

2 ) with (γ, γ) ∈ N 2 almost always gives 0 except for γ = 0 and γ = 0 (mod 2) at the same time. Therefore, in order to obtain a non trivial contribution for x I , ẋI must contain some cosine functions verifying the following condition ℓ 1 ω 1 + ℓ 2 ω 2 + ℓ 3 ω 3 = 0, ℓ 3 ε + ℓ 1 + m 2 + ℓ 3 -1 ≡ 0 (mod 2), (5.8) and this condition must not be satisfied by J ≺ I in order to avoid a change in the component x J .

Under conditions (5.3) and (5.4), we claim that

(1) (m 1 , m 2 -1, -1, ε) is the only 4-tuple verifying (5.8) for x I , and x I (2π)x I (0) is a non zero linear function of ζ;

(2) Eq. (5.8) is never satisfied for x J with J < I.

One has

(5.9)

Assume that ω 2 > (m 1 + m 2 )m 1 ω 1 . Then, except for the 4-tuple (m 1 , m 2 , m 3 , ε) verifying Eq. ( 5.3), the only possibility to have the right-hand side of Eq. (5.9) equal to 0 is ℓ 1 = ℓ 2 = ℓ 3 = 0. In that case,

Then, Eq. (5.8) is not satisfied, and (2) is proved. Due to Eq. (5.5), the power of ζ is equal to the number of times ω 3 occurs in the resonance condition (5.3). The latter is clearly equal to 1. Thus, x I (2π)x I (0) gives rise to a linear function of ζ. It remains to show that the coefficient in front of ζ is not equal to zero. By Lemma 5.2, one knows that

where we gathered all other terms into R. Note that the numerical coefficients f I and g I depend on the frequencies ω 1 , ω 2 , and ω 3 . The goal is to show that f I is not equal to zero if we want to move the component x I , i.e., when ω 3 = (m 2 -1)ω 2 + m 1 ω 1 . If we consider f I as a function of ω 1 , ω 2 , and ω 3 , it suffices to show that this function is not identically equal to zero over the hyperplane of R 3 defined by the resonance condition ω 3 = (m 2 -1)ω 2 + m 1 ω 1 . We assume that the next lemma holds true, and we will provide an argument immediately after finishing the proof of Proposition 5.1.

, and g J the one in front of the term cos{(m

Then, the quotient α J := f J /g J verifies the following inductive formula.

where m

Assume that N j=1 ℓ j L j = 0 with l j ∈ R. Then, for all i = 1, . . . , N, we have

(5.54) By Lemma 5.8, we have l j = 0 for j = 1, . . . , N. Then, the family (L j ) j=1,...,N is linearly independent. We conclude that det A is not equal to zero. This ends the proof of Corollary 5.9.

We still need another technical lemma which guarantees that there exist integer frequencies such that Eq. (5.12) is satisfied and the matrix A in Eq. (5.23) is invertible.

Lemma 5.10. There exists integer frequencies such that (5.12) is satisfied and det A is not equal to zero.

Proof of Lemma 5.10. For j = 1, . . . , N, we set

then, we have detA =

where P and Q are two polynomials of (m -1)N variables. We first note that Q never vanishes over integer frequencies. Assume, by contradiction, that P is always equal to zero for integer frequencies verifying Eq. (5.12). Consider P as a polynomial in one variable ω m 2 -1

2N

, i.e.,

), if Eq. (5.57) is not identically equal to zero, then this polynomial in the variable ω m 2 -1 2N most has a finite number of roots. However, for a given choice of (m -1)N -1 first frequencies, there exist an infinite number of ω m 2 -1 2N verifying (5.12). Then, P j = 0 over all integer frequencies, and P M is not identically equal to zero. We note that all P j are polynomials of (m -1)N -1 variables. Proceeding by induction on the number of variables, it is easy to see that, at the end, we obtain a polynomial in the variable ω 1 11 which is equal to zero over all integer ω 1 11 , and which is not identically equal to zero according to Corollary 5.9. That contradiction ends the proof of Lemma 5.10.

General case: m > 2

Notice that the proof of Theorem 5.4 does not really depend on the number of vector fields involved in the control system (2.1). Indeed, for m > 2, if the control functions are linear combination of sinusoids with integer frequencies, then the state variables in the canonical form are also linear combinations of Proof of Proposition 5.11. The fact that the procedure described by the Lines 5 -12 in Algorithm 5 produces an input ûnorm steering System (5.1) from δ 0, 1 λ (x initial ) to 0 is a consequence of Proposition 5.4 and Subsection 5.2.3. We also note that, due to the homogeneity of System (5.1), if an input u steers (5.1) from x to 0, then, for every λ > 0, the input λu steers (5.1) from δ 0,λ (x) to 0. Therefore, the input computed by Exact m,r (x initial ) steers System (5.1) from x initial to 0. Let us now show (5.58). In the sequel, the application Exact m,r : x → Exact m,r (x) will be simply denoted by û : x → û(x). Let S(0, 1) := {y, y 0 = 1} and x ∈ R n r . Then, there exists x norm ∈ S(0, 1) such that x = δ 0,λ (x norm ) with λ := x 0 . We have:

ℓ(û(y)).

We also know that, since the sub-Riemannian distance d(0, •) from 0 and the pseudo-norm • 0 at 0 are both homogeneous of degree 1 with respect to the dilation δ 0,t (•), there exists a constant C > 0 such that Cλ ≤ d(0, x). Since the application y → û(y) is continuous from S(0, 1) to R m and S(0, 1) is compact, then, sup y∈S(0,1) ℓ(û(y)) is bounded, thus the inequality (5.58) holds true.

The following theorem is a consequence of Proposition 5.11 and Remark 4.4.

Theorem 5.12. The function Exact m,r (•) constructed by Algorithm 5 provides the approximate system A X defined in Section 4.1 with a sub-optimal steering law.

Proof of Theorem 5.12. It suffices to note that, for every a ∈ Ω, A X (a) has the same form (cf. Remark 4.4), thus defines the same sub-Riemannian distance d. Therefore, the inequality (5.58) holds uniformly with respect to the approximate point a, and this terminates the proof of Theorem 5.12.

Remark 5.4. Frequencies choices and the construction of the corresponding control matrix A j , as well as its inverse B j , translate to off-line computations. We note that Proposition 5.4 only gives sufficient conditions to prevent resonance (by choosing widely spaced frequencies, cf. Eq. (5.12)) and guarantee the invertibility of the corresponding matrix (by using a sufficiently large number of independent frequencies). These conditions tend to produce high frequencies while it is desirable to find smaller ones for practical use. We can prove that two independent frequencies suffice to steer one component (cf. Section 5.2.1), and we conjecture that 2N independent frequencies suffice to control one equivalence class of cardinal N by producing an invertible matrix. One can implement a searching algorithm for finding the optimal frequencies for each equivalence class such that they prevent all resonances in smaller classes and produce an invertible matrix for the class under consideration. Proposition 5.4 guarantees the finiteness of such an algorithm. Moreover, one can construct, once for all, a table containing the choice of frequencies and the corresponding matrices for each equivalence class in the free canonical system. Remark 5.5. Recall that the key point in our control strategy consists in choosing suitable frequencies such that, during each 2π-period, the corresponding input function displaces components of one equivalence class to their preassigned positions while all the components of smaller classes (according to the ordering in Definition 5.2) return at the end of this control period to the values taken at the beginning of the period. In order to achieve the previous task, special resonance conditions must be verified by the appropriate components, and these conditions must not hold for all the other smaller components (according to the ordering in Definition 5.2). Note that two categories of frequencies have been picked in Proposition 5.4: the basic frequencies {ω k ij }, and the resonance frequencies {ω * ij }. Since frequencies occurring in the dynamics of the state variables are just Z-linear combinations of {ω k ij } ∪ {ω * ij }, and the resonance frequencies {ω * ij } are chosen to be special Z-linear combinations of basic frequencies (resonance condition), the frequencies in the dynamics of the state variables are special Z-linear combinations of {ω k ij }. Remark 5.6. Once the frequencies and matrices are obtained, the on-line computation Exact m,r is only a series of matrix multiplications which can be performed on-line without any numerical difficulty.

Remark 5.7. The Desingularization Algorithm presented in Section 3.3 (see also Remarks 3.6 and 5.1) together with Algorithm 5 provides general nilpotent control systems with an exact steering method, which is also sub-optimal.

Remark 5.8. We note that the inputs constructed in this section are piecewise C ∞ during each time interval [2iπ, 2(i + 1)π], for i = 1, . . . , N -1, but they are not globally continuous during the entire control period [0, 2 Nπ], due to discontinuity at t = 2π, 4π, . . . , 2( N -1)π. However, it is not difficult to devise (globally) continuous inputs using interpolation techniques. We illustrate the idea with a simple example. Assume that we use u i = (u i 1 , u i 2 ) and u j = (u j 1 , u j 2 ) defined by

to steer two consecutive classes E i x and E j x (i.e. j = i + 1) which are both of cardinal equal to 1. If we require their concatenation u i * u j to be continuous, i.e. u i 1 (2π) = u j 1 (2π), (5.59) u i 2 (2π) = u j 2 (2π), (5.60) we can proceed as follows. For Eq. (5.59), it suffices to modify slightly u j 1 . We take u j 1 (t) = u i 1 (2π) cos ω 1J t.

(5.61) For Eq. (5.60), we distinguish two cases:

• if ε j = 1, we can take

= u i 2 (2π) cos ω 2j t + a j sin ω * 2j t;

(5.62)

• if ε j = 0, we add a frequency ω c to u j 2 , large enough to avoid any additional resonances, u j 2 (t) = cos ω 2j t + a j cos ω * 2j t + (u i 2 (2π)a j -1) cos ω c t.

(5.63) Let u j := ( u j 1 , u j 2 ). Then, by construction, the new input u i * u j is continuous over the time interval [2iπ, 2jπ].

It is clear that this idea of interpolation by adding suitable frequencies can be used to construct continuous inputs over the entire control period [0, 2 Nπ]. In fact, by using more refined interpolations, one can get inputs of class C k for arbitrary finite integer k.