Didier Henrion

Edouard Pauwels

Linear conic optimization for nonlinear optimal control

Infinite-dimensional linear conic formulations are described for nonlinear optimal control problems. The primal linear problem consists of finding occupation measures supported on optimal relaxed controlled trajectories, whereas the dual linear problem consists of finding the largest lower bound on the value function of the optimal control problem. Various approximation results relating the original optimal control problem and its linear conic formulations are developed. As illustrated by a couple of simple examples, these results are relevant in the context of finitedimensional semidefinite programming relaxations used to approximate numerically the solutions of the infinite-dimensional linear conic problems.

Motivation

In [START_REF] Lasserre | Optimisation globale et théorie des moments[END_REF][START_REF] Lasserre | Global optimization with polynomials and the problem of moments[END_REF], J.-B. Lasserre described a hierarchy of convex semidefinite programming (SDP) problems allowing to compute bounds and find global solutions for finite-dimensional nonconvex polynomial optimization problems. Each step in the hierarchy consists of solving a primal moment SDP problem and a dual polynomial sum-of-squares (SOS) SDP problem corresponding to discretizations of infinite-dimensional linear conic problems, namely a primal linear programming (LP) problem on the cone of nonnegative measures, and a dual LP problem on the cone of nonnegative continuous functions. The number of variables (number of moments in the primal SDP, degree of the SOS certificates in the dual SDP) increases when progressing in the hierarchy, global optimality can be ensured by checking rank conditions on the moment matrices, and global optimizers can be extracted by numerical linear algebra. For more information on the moment-SOS hierarchy and its applications, see [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF].

This approach was then extended to polynomial optimal control in [START_REF] Lasserre | Nonlinear optimal control via occupation measures and LMI relaxations[END_REF]. Whereas the key idea in [START_REF] Lasserre | Optimisation globale et théorie des moments[END_REF][START_REF] Lasserre | Global optimization with polynomials and the problem of moments[END_REF] was to reformulate a (finite-dimensional) nonconvex polynomial optimization on a compact semi-algebraic set into an LP in the (infinite-dimensional) space of probability measures supported on this set, the key idea in [START_REF] Lasserre | Nonlinear optimal control via occupation measures and LMI relaxations[END_REF], also developed in [START_REF] Gaitsgory | Linear programming approach to deterministic infinite horizon optimal control problems with discounting[END_REF], was to reformulate an (infinite-dimensional) nonconvex polynomial optimal control problem with compact constraint set into an LP in the (infinite-dimensional) space of occupation measures supported on this set. Note that LP formulations of optimal control problems (on ordinary differential equations and partial differential equations) are classical, and can be traced back to the work by L. C. Young, Filippov, as well as Warga and Gamkrelidze, amongst many others. For more details and a historical survey, see e.g. [START_REF] Fattorini | Infinite dimensional optimization and control theory[END_REF]Part III]. We believe that what is innovative in [START_REF] Lasserre | Nonlinear optimal control via occupation measures and LMI relaxations[END_REF] is the observation that the infinite-dimensional linear formulations for optimal control problems can be solved numerically with a moment-SOS hierarchy of the same kind as those used in polynomial optimization.

The objective of this contribution is to revisit the approach of [START_REF] Lasserre | Nonlinear optimal control via occupation measures and LMI relaxations[END_REF] and to survey the use of occupation measures to linearize polynomial optimal control problems. This is an opportunity to describe duality in infinite-dimensional conic problems, as well as various approximation results on the value function of the optimal control problem. The primal LP consists of finding occupation measures supported on optimal relaxed controlled trajectories, whereas the dual LP consists of finding the largest lower bound on the value function of the optimal control problem. The value function is the solution (in a suitably defined weak sense) of a nonlinear partial differential equation called the Hamilton-Jacobi-Bellman equation, see e.g. [START_REF] Trélat | Contrôle optimal : théorie et applications[END_REF]Chapters 8 and 9] and [START_REF] Clarke | Functional analysis, calculus of variations and optimal control[END_REF]Chapters 19 and 24]. It is traditionally used for verification of optimality, and for explicit computation of optimal control laws, but we do not describe these applications here.

Polynomial optimal control

We consider polynomial optimal control problems (POCPs) of the form

v * (t 0 , x 0 ) := inf u T t 0 l(x(t), u(t))dt + l T (x(T )) s.t. ẋ(t) = f (x(t), u(t)), x(t 0 ) = x 0 x(t) ∈ X, t ∈ [t 0 , T ] u(t) ∈ U, t ∈ [t 0 , T ] x(T ) ∈ X T (1)
where the dot denotes time derivative,

l ∈ R[x, u] is a given Lagrangian (integral cost), l T ∈ R[x] is a given terminal cost, f ∈ R[x, u] n is a given dynamics (vector field), X ⊂ R n is a given compact state constraint set, U ⊂ R m is a given compact control constraint set,
X T ⊂ X is a given compact terminal state constraint set. Also given are the terminal time T ≥ 0, the initial time t 0 ∈ [0, T ] and the initial condition x 0 ∈ X. In POCP (1), the minimum is with respect to all control laws u ∈ L ∞ ([t 0 , T ]; U) which are bounded functions of time t with values in U, and the resulting state trajectories x ∈ L ∞ ([t 0 , T ]; X) which are bounded functions of time t with values in X.

Let A ⊂ [0, T ]×X denote the set of values (t 0 , x 0 ) for which there is a controlled trajectory (x, u) ∈ L ∞ ([t 0 , T ]; X × U) starting at x(t 0 ) = x 0 and admissible for POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. The function (t 0 , x 0 ) → v * (t 0 , x 0 ) defined in ( 1) is called the value function, and its domain is A .

LP formulation

As explained in the introduction, to derive an LP formulation of POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF] we have to introduce measures on trajectories, the so-called occupation measures. The first step is to replace classical controls with probability measures, and for this we have to define additional notations.

Given a compact set X ⊂ R n , let C (X) denote the space of continuous functions supported on X, and let C + (X) denote its nonnegative elements, the cone of nonnegative continuous functions on X. Let M + (X) = C + (X) ′ denote its topological dual, the set of all nonnegative continuous linear functional on C (X). By a Riesz representation theorem, these are nonnegative Borel-regular measures, or Borel measures, supported on X. The topology in C + (X) is the strong topology of uniform convergence, whereas the topology in M + (X) is the weak-star topology. The duality bracket v, µ :=

X v(x)µ(dx)
denotes the integration of a function v ∈ C + (X) against a measure µ ∈ M + (X). For background on weak-star topology see e.g. [12, Section 5.10] or [2, Chapter IV]. Finally, let us denote by P(X) the set of probability measures supported on X, consisting of Borel measures µ ∈ M + (X) such that 1, µ = 1.

Relaxed controls

In POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF], given (t 0 , x 0 ) ∈ A , let (x k , u k ) k∈N ∈ L ∞ ([t 0 , T ]; X ×U) denote a minimizing sequence of admissible controlled trajectories, i.e. it holds

ẋk (t) = f (x k (t), u k (t)), x(t 0 ) = x 0 and lim k→∞ T t 0 l(x k (t), u k (t))dt + l T (x k (T )) = v * (t 0 , x 0 ).
In general the infimum in POCP (1) is not attained, so our next step is to assume that, at each time t ∈ [t 0 , T ], the control is not a vector u(t) ∈ U, but a time-dependent probability measure ω(du | t) ∈ P(U) which rules the distribution of the control in U. We use the notation ω t := ω(. | t) to emphasize the dependence on time. This is called a relaxed control, or stochastic control, or Young measure in the functional analysis literature. POCP (1) is then relaxed to

v * R (t 0 , x 0 ) := min ω T t 0 l(x(t), .), ω t dt + l T (x(T )) s.t. ẋ(t) = f (x(t), .), ω t , x(t 0 ) = x 0 x(t) ∈ X, t ∈ [t 0 , T ] ω t ∈ P(U), t ∈ [t 0 , T ] x(T ) ∈ X T (2)
where the minimization is w.r.t. a relaxed control. Note that we replaced the infimum in POCP (1) with a minimum in relaxed POCP [START_REF] Barvinok | A Course in Convexity[END_REF]. Indeed, it can be proved that this minimum is always attained using (weak-star) compactness of the space of probability measures with compact support.

Since classical controls u ∈ L ∞ ([t 0 , T ]; U) are a particular case of relaxed controls ω t ∈ P(U) corresponding to the choice ω t = δ u(t) for a.e. t ∈ [t 0 , T ], the minimum in relaxed POCP ( 2) is smaller than the infimum in classical POCP (1), i.e.

v * (t 0 , x 0 ) ≥ v * R (t 0 , x 0 ). Contrived optimal control problems (e.g. with overly stringent state constraints) can be cooked up such that the inequality is strict, i.e. v * (t 0 , x 0 ) > v * R (t 0 , x 0 ), see e.g. the examples in [7, Appendix C]. We do not consider that these examples are practically relevant, and hence the following assumption will be made.

Assumption 1 (No relaxation gap) For any relaxed controlled trajectory (x, ω t ) admissible for relaxed POCP (2), there is a sequence of controlled trajectories (x k , u k ) k∈N admissible for POCP (1) such that

lim k→∞ T t 0 v(x k (t), u k (t))dt = T t 0 v(x(t), .), ω t dt for every function v ∈ C (X × U). Then it holds v * (t 0 , x 0 ) = v * R (t 0 , x 0 ) for every (t 0 , x 0 ) ∈ A .
Note that this assumption is satisfied under the classical controllability and/or convexity conditions used in the Filippov-Ważewski Theorem with state constraints, see [START_REF] Frankowska | Filippov's and Filippov-Ważewski's theorems on closed domains[END_REF] and the discussions around Assumption I in [START_REF] Gaitsgory | Linear programming approach to deterministic infinite horizon optimal control problems with discounting[END_REF] and Assumption 2 in [START_REF] Henrion | Convex computation of the region of attraction of polynomial control systems[END_REF]. However, let us point out that Assumption 1 does not imply that the infimum is attained in POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. Conversely, if the infimum is attained, the values of POCP (1) and relaxed POCP (2) coincide, and Assumption 1 is satisfied.

Occupation measure

Given initial data (t 0 , x 0 ) ∈ A , and given a relaxed control ω t ∈ P(U), the unique solution of the ODE ẋ

(t) = f (x(t), .), ω t , x(t 0 ) = x 0 (3)
in relaxed POCP (2) is given by

x(t) = x 0 + t t 0 f (x(s), .), ω s ds (4) for every t ∈ [t 0 , T ]. Let us then define µ(dt, dx, du) := dt δ x(t) (dx) ω t (du) ∈ M + ([t 0 , T ] × X × U) (5)
as the occupation measure concentrated uniformly in time on the state trajectory starting at x 0 at time t 0 , for the given relaxed control ω t . An analytic intepretation is that integration w.r.t. the occupation measure is equivalent to time-integration along system trajectories, i.e.

T t 0 v(t, x(t))dt = T t 0 X U v(t, x)µ(dt, dx, du) = v, µ given any test function v ∈ C ([t 0 , T ] × X).
Let us define the linear operator L :

C 1 ([t 0 , T ] × X) → C ([t 0 , T ] × X × U) by v → Lv := ∂v ∂t + n i=1 ∂v ∂x i f i = ∂v ∂t + grad v • f. Given a continuously differentiable test function v ∈ C 1 ([t 0 , T ] × X), notice that v(T, x(T )) -v(t 0 , x(t 0 )) = T t 0 dv(t, x(t)) = T t 0 v(t, x(t))dt = T t 0 Lv(t, x(t))dt = Lv, µ
which can be written more concisely as

v, µ T -v, µ 0 = Lv, µ (6)
upon defining respectively the initial and terminal occupation measures

µ 0 (dt, dx) := δ t 0 (dt)δ x(t 0 ) (dx), µ T (dt, dx) := δ T (dt)δ x(T ) (dx). ( 7
)
Let us define the adjoint linear operator

L ′ : C ([t 0 , T ] × X) ′ → C 1 ([t 0 , T ] × X × U) ′ by the relation v, L ′ µ := Lv, µ for all µ ∈ M ([t 0 , T ] × X) and v ∈ C 1 ([t 0 , T ] × X).
More explicitly, this operator can be expressed as

µ → L ′ µ = - ∂µ ∂t - n i=1 ∂(f i µ) ∂x i = - ∂µ ∂t -div f µ
where the derivatives of measures are understood in the weak sense, i.e. via their action on smooth test functions, and the change of sign comes from integration by parts. Equation ( 6) can be rewritten equivalently as v, µ T -v, µ 0 = v, L ′ µ and since this equation should hold for all test functions v ∈ C 1 ([t 0 , T ] ×X), we obtain a linear partial differential equation (PDE) on measures L ′ µ = µ T -µ 0 that we write

∂µ ∂t + div f µ + µ T = µ 0 . (8)
This linear transport equation is classical in fluid mechanics, statistical physics and analysis of PDEs. It is called the equation of conservation of mass, or the continuity equation, or the advection equation, or Liouville's equation. Under the assumption that the initial data (t 0 , x 0 ) ∈ A and the control law ω t ∈ P(U) are given, the following result can be found e.g. in [START_REF] Villani | Topics in optimal transportation[END_REF]Theorem 5.34] or [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF].

Lemma 1 (Liouville PDE = Cauchy ODE)

There exists a unique solution to the Liouville PDE [START_REF] Lasserre | Optimisation globale et théorie des moments[END_REF] which is concentrated on the solution of the Cauchy ODE (3), i.e. such that ( 5) and ( 7) hold.

In our context of conic optimization, the relevance of the Liouville PDE ( 8) is its linearity in the occupation measures µ, µ 0 and µ T , whereas the Cauchy ODE (3) is nonlinear in the state trajectory x(t).

Primal LP on measures

The cost in relaxed POCP (2) can therefore be written

T t 0 l(x(t), .), ω t dt + l T (x(T )) = l, µ + l T , µ T
and we can now define a relaxed optimal control problem as an LP in the cone of nonnegative measures:

p * (t 0 , x 0 ) := min µ,µ T l, µ + l T , µ T s.t. ∂µ ∂t + div f µ + µ T = δ t 0 δ x 0 µ ∈ M + ([t 0 , T ] × X × U) µ T ∈ M + ({T } × X T ) (9)
where the minimization is w.r.t. the occupation measure µ (which includes the relaxed control ω t , see [START_REF] Gaitsgory | Linear programming approach to deterministic infinite horizon optimal control problems with discounting[END_REF]) and the terminal measure µ T , for a given initial measure µ 0 = δ t 0 δ x 0 which is the right-hand side in the Liouville equation constraint.

Note that in LP (9) the infimum is always attained since the admissible set is (weakstar) compact and the functional is linear. However, since classical trajectories are a particular case of relaxed trajectories corresponding to the choice (5), the minimum in LP ( 9) is smaller than the minimum in relaxed POCP (2) (this latter one being equal to the infimum in POCP (1), recall Assumption 1), i.e.

v * (t 0 , x 0 ) ≥ p * (t 0 , x 0 ).

The following result, due to [START_REF] Vinter | Convex duality and nonlinear optimal control[END_REF], essentially based on convex duality, shows that there is no gap occuring when considering more general occupation measures than those concentrated on solutions of the ODE.

Lemma 2 It holds v * (t 0 , x 0 ) = p * (t 0 , x 0 ) for all (t 0 , x 0 ) ∈ A .

Dual LP on functions

Primal measure LP (9) has a dual LP in the cone of nonnegative continuous functions:

d * (t 0 , x 0 ) := sup v v(t 0 , x 0 ) s.t. l + ∂v ∂t + grad v • f ∈ C + ([t 0 , T ] × X × U) l T -v(T, .) ∈ C + (X T ) ( 11
)
where maximization is with respect to a continuously differentiable function v ∈ C 1 ([t 0 , T ]× X) which can be interpreted as a Lagrange multiplier of the Liouville equation in [START_REF] Lasserre | Global optimization with polynomials and the problem of moments[END_REF].

In general the supremum in dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF] is not attained, and weak duality with the primal LP (9) holds p * (t 0 , x 0 ) ≥ d * (t 0 , x 0 ) but it can be shown that there is actually no duality gap:

Lemma 3 (No duality gap) It holds p * (t 0 , x 0 ) = d * (t 0 , x 0 ) for all (t 0 , x 0 ) ∈ A .

Proof : The proof follows along the same lines as the proof of [7, Theorem 2]. First we observe that (t 0 , x 0 ) ∈ A and Assumption 1 imply that p * (t 0 , x 0 ) is finite. Second, we use the condition that the cone

{( l, µ + l T , µ T , ∂µ ∂t + div f µ + µ T ) : µ ∈ M + ([t 0 , T ] × X × U), µ T ∈ M + ({T } × X T )}
is closed in the weak-star topology. This is a classical sufficient condition for the absence of a duality gap in infinite-dimensional LPs, see e.g. [2, Chapter IV, Theorem 7.2].

Approximation results

Primal LP [START_REF] Lasserre | Global optimization with polynomials and the problem of moments[END_REF] and dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF] are infinite-dimensional conic problems. If we want to solve them with a computer, we invariably have to use discretization and approximation schemes. The aim of this section is to derive various approximation results that prove useful when designing numerical methods based on moment-SOS hierarchies.

Lower bound on value function

First, observe that there always exists an admissible solution for dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF]. For example, choose v(t, x) := a + b(T -t) with a ∈ R such that l T (x) ≥ a on X T and b ∈ R such that l(x, u) ≥ b on X × U. Moreover, by construction, any admissible function for dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF] gives a global lower bound on the value function:

Lemma 4 (Lower bound on value function)

If v ∈ C 1 ([t 0 , T ] × X) is admissible for dual LP (11), then v * ≥ v on [t 0 , T ] × X. Proof : If (t 1 , x 1 ) / ∈ A , then v * (t 1 , x 1
) is unbounded and the statement holds because v(t 1 , x 1 ) must be finite. Let (t 1 , x 1 ) ∈ A be given. If v is admissible for dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF], then for any admissible trajectory (x, u) ∈ L ∞ ([t 0 , T ]; X × U), starting at x(t 1 ) = x 1 , it holds

T t 1 (l(x(t), u(t))+Lv(t, x(t), u(t)))dt = T t 1 l(x(t), u(t))dt+v(T, x(T ))- v(t 1 , x 1 ) ≥ 0 since l + L ≥ 0 on [t 0 , T ] × X × U. Moreover T t 1 l(x(t), u(t))dt + l T (x(T )) ≥ T t 1 l(x(t), u(t))dt + v(T, x(T )) since l T -v(T, .
) ≥ 0 on X T . Combining the two inequalities yields T t 1 l(x(t), u(t))dt + l T (x(T )) ≥ v(t 1 , x 1 ) and the expected inequality follows by taking the infimum over admissible trajectories.

The relation between the dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF] and the original POCP (1) is given by the following result:

Lemma 5 (Maximizing sequence) Given (t 0 , x 0 ) ∈ A , there is a sequence (v k ) k∈N admissible for the dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF] such that v * (t 0 , x 0 ) ≥ v k (t 0 , x 0 ) and lim k→∞ v k (t 0 , x 0 ) = v * (t 0 , x 0 ).

Proof : From Lemma 4, it holds v * (t 0 , x 0 ) ≥ v k (t 0 , x 0 ) for every function v k admissible for dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF]. By Assumption 1 and Lemma 3, it holds d * (t 0 , x 0 ) = p * (t 0 , x 0 ) = v * (t 0 , x 0 ) and hence there exists a maximizing sequence v k ∈ C 1 ([t 0 , T ] × X) for LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF].

Uniform approximation of the value function along trajectories

In this section, we investigate the properties of maximizing sequences given by Lemma 5, and in particular their convergence to the value function of POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. We first demonstrate the lower semicontinuity of the value of LP ( 9). This leads to the lower semicontinuity of the value of POCP (1), by considering Assumption 1 and Lemma 2. Note that lower semicontinuity is readily ensured when the set

{(f (x, u), l(x, u) + a) : u ∈ U, a ≥ 0} is convex in R n+1 for
) k∈N such that p * (t k , x k ) = µ k , l + µ T k , l T and ∂µ k ∂t + div f µ k + µ T k = δ t k δ x k . Convergence of (t k , x k ) to (t, x) implies weak-star convergence of δ t k δ x k to δ t δ x .
Using the same closedness argument as in the proof of Lemma 3, we can consider that, up to a subsequencce, µ k and µ T k converge to some measures µ and µ T in the weak-star topology and that ∂µ ∂t + div f µ + µ T = δ t δ x . Hence, we have lim inf k→∞ p * (t k , x k ) = µ, l + µ T , l T and the pair (µ, µ T ) is feasible for problem p * (t, x). Therefore lim inf k→∞ p * (t k , x k ) ≥ p * (t, x) which proves the result when LP ( 9) is feasible for (t, x). Using similar arguments, one can show that if (t, x) is such that LP (9) is not feasible, there cannot be infinitely many k such that LP ( 9) is feasible for (t k , x k ).

The following result extends the convergence properties of the maximizing sequence.

Theorem 1 (Uniform convergence along relaxed trajectories) For any sequence (v k ) k∈N admissible for the dual LP [START_REF] Lasserre | Moments, positive polynomials and their applications[END_REF], for any solution (x, ω t ) of relaxed POCP (2), and for any t ∈ [t 0 , T ], it holds

0 ≤ v * (t, x(t)) -v k (t, x(t)) ≤ v * (t 0 , x 0 ) -v k (t 0 , x 0 ) → k→∞ 0.
Proof : Let (x j , u j ) j∈N be an approximating sequence for (x, ω t ), whose existence is guaranteed by Assumption 1. For any j ∈ N, k ∈ N, and t ∈ [t 0 , T ], we have l T (x j (T ))v k (T, x j (T ))+ T t (l(x j (s), u j (s))+L(s, x j (s), u j (s)))ds = l T (x j (T ))+ T t l(x j (s), u j (s))dsv k (t, x j (t)). Both the first term and the integrand are positive in the left-hand side. Therefore, the right-hand side is a decreasing function of t. Moreover, the trajectory is suboptimal, and v k is a lower bound on the value function. It holds that 0

≤ v * (t, x j (t)) - v k (t, x j (t)) ≤ l T (x j (T ))+ T t l(x j (s), u j (s))ds-v k (t, x j (t)) ≤ l T (x j (T ))+ T t 0 l(x j (s), u j (s))ds- v k (t 0 , x 0
). Letting j tend to infinity, using the lower semicontinuity of v * , we conclude that 0

≤ v * (t, x(t)) -v k (t, x(t)) ≤ lim inf j→∞ v * (t, x j (t)) -v k (t, x j (t)) ≤ lim j→∞ (l T (x j (T )) + T t 0 l(x j (s), u j (s))ds -v k (t 0 , x 0 )) = v * (t 0 , x 0 ) -v k (t 0 , x 0 ).
It is important to notice that Theorem 1 holds for any trajectory realizing the minimum of POCP (2) and therefore, for all of them simultaneously. In addition, these trajectories are identified with limiting trajectories of POCP (1) by Assumption 1.

Optimal control over a set of initial conditions

Liouville equation ( 8) is used as a linear equality constraint in POCP ( 9) with a Dirac right-hand side as an initial condition. However, this right-hand side can be replaced by more general probability measures. The linearity of the constraint allows to extend most of the results of the previous section to this setting. It leads to similar convergence guarantees regarding a (possibly uncountable) set of optimal control problems. These guarantees hold for solutions of a single infinite-dimensional LP.

Suppose that we are given a set of initial conditions X 0 ⊂ X, such that (t 0 , x 0 ) ∈ A for every x 0 ∈ X 0 . Given a probability measure ξ 0 ∈ P(X 0 ), let µ 0 (dt, dx) = δ t 0 (dt)ξ 0 (dx) and consider the following average value v * (µ 0 ) :=

X 0 v * (t, x)µ 0 (dt, dx) = v * , µ 0 (12)
where v * is the value of POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. Under Assumption 1, by linearity this value is equal to the value of POCP ( 9) with µ 0 as the right-hand side of the equality constraint, namely the primal averaged LP p * (µ 0 ) := min µ,µ T l, µ + l T , µ T s.t.

∂µ ∂t + div f µ + µ T = µ 0 µ ∈ M + ([t 0 , T ] × X × U) µ T ∈ M + ({T } × X T ) (13)
with dual averaged LP d * (µ 0 ) :

= sup v v, µ 0 s.t. l + ∂v ∂t + grad v • f ∈ C + ([t 0 , T ] × X × U) l T -v(T, .) ∈ C + (X T ). ( 14
)
The absence of duality gap is justified in the same way as in Lemma 3. Moreover, Lemma 4 also holds, and, as in Lemma 5, we have the existence of maximizing lower bounds v k such that lim

k→∞ v k , µ 0 = v * (µ 0 ) = p * (µ 0 ) = d * (µ 0 ).
Intuitively, primal LP (13) models a superposition of optimal control problems. The LP formulation allows to express it as a single program over measures satisfying a transport equation. A relevant question here is the relation between solutions of averaged measure LP [START_REF] Trélat | Contrôle optimal : théorie et applications[END_REF] and optimal trajectories of the original problem POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. The intuition is that measure solutions of LP ( 13) represent a superposition of optimal trajectories of the relaxed POCP [START_REF] Barvinok | A Course in Convexity[END_REF]. These trajectories are themselves limiting trajectories of the original POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. The superposition principle of [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]Theorem 3.2] allows to formalize this intuition and to extend the result of Theorem 1 to this setting.

Theorem 2 (Uniform convergence on support of optimal measure) For any solution (µ, µ T ) of primal averaged LP [START_REF] Trélat | Contrôle optimal : théorie et applications[END_REF], there are parametrized measures ξ t ∈ P(X) (for the state) and ω t ∈ P(U) (for the control) such that µ(dt, dx, du) = dt ξ t (dx)ω t (du), µ 0 (dt, dx) = δ t 0 (dt)ξ t 0 (dx) and µ T (dt, dx) = δ T (dt)ξ T (dx). In addition, if (v k ) k∈N is a maximizing sequence for dual averaged LP [START_REF] Villani | Topics in optimal transportation[END_REF], for any t ∈ [t 0 , T ], it holds

0 ≤ X (v * (t, x) -v k (t, x))ξ t (dx) ≤ X (v * (t 0 , x 0 ) -v k (t 0 , x 0 ))ξ 0 (dx 0 ) → k→∞ 0.
Proof : The decomposition is given by Lemma 3 in [START_REF] Henrion | Convex computation of the region of attraction of polynomial control systems[END_REF]. It asserts the existence of a measure σ ∈ M (C ([t 0 , T ], X)) supported on trajectories admissible for relaxed POCP [START_REF] Barvinok | A Course in Convexity[END_REF] and such that for any measurable function w : X → R, it holds X w(x)ξ t (dx) =

C ([t 0 ,T ],X) w(x(t))σ(dx(.)). By Assumption 1, all trajectories of the support of σ are pointwise limits of sequences of feasible trajectories of POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF]. Hence σ-almost all of these sequences must be minimizing sequences for POCP [START_REF] Ambrosio | Transport equation and Cauchy problem for non-smooth vector fields[END_REF], otherwise, that would contradict optimality of (µ, µ T ). The result follows by discarding the trajectories which are not limits of minimizing sequences. This does not change σ or ξ t . Theorem 1 applies to σalmost all these trajectories and we have 0

≤ C ([t 0 ,T ],X) (v * (t, x(t)) -v k (t, x(t)))σ(dx(.)) = X (v * (t, x) -v k (t, x))ξ t (dx) ≤ X (v * (t 0 , x 0 ) -v k (t 0 , x 0 ))ξ 0 (dx 0 ).
A remarkable practical implication of this result is that maximizing sequences of averaged dual LP [START_REF] Villani | Topics in optimal transportation[END_REF] provide an approximation to the value function of POCP (1) that is uniform in time and almost uniform in space along limits of optimal trajectories starting from X 0 .

Numerical illustrations

In Sections 3 and 5 we reformulated nonlinear optimal control problems as abstract linear conic optimization problems that involve manipulations of measures and continuous functions in their full generality. The results presented in Section 4 are related to properties of minimizing or maximizing elements, or sequences of elements for these problems. From a practical point of view, it is possible to construct these sequences using the same numerical tools as in static polynomial optimization. On the primal side, this allows to approximate the minimizing elements of measure LP problems with a converging hierarchy of moment SDP problems. On the dual side, we can construct numerically maximizing sequences of polynomial SOS certificates for the continuous function LP problems. The convergence properties that we investigated hold in particular for these solutions of the moment-SOS hierarchy.

This section illustrates convergence properties of the sequence of approximations of value functions computed using moment-SOS hierarchies. We consider simple, but largely spread, optimal control problems for which the value function (or optimal trajectories) are known.

Uniform approximation along an optimal trajectory

Consider the one-dimensional turnpike POCP analyzed in Section 22.2 of [START_REF] Clarke | Functional analysis, calculus of variations and optimal control[END_REF]:

v * (t 0 , x 0 ) := inf u 2 t 0 (x(t) + u(t))dt s.t. ẋ(t) = 1 + x(t) -x(t)u(t) x(t 0 ) = x 0 u(t) ∈ [0, 3]. (15)
Figure 1: Optimal trajectory t → x * (t) starting at (t 0 , x 0 ) = (0, 0) for the turnpike POCP [START_REF] Vinter | Convex duality and nonlinear optimal control[END_REF].

For this problem, the infimum is attained at a unique optimal control which is piecewise constant. The optimal trajectory t → x * (t) starting at (t 0 , x 0 ) = (0, 0) is presented in Figure 1. The uniform convergence of approximate value functions t → v k (t, x * (t)) to Figure 2: Differences t → v * (t, x * (t)) -v k (t, x * (t)) between the actual value function and its polynomial approximations of increasing degrees k = 3, 5, 7, 9 along the optimal trajectory t → x * (t) starting at (t 0 , x 0 ) = (0, 0) for the turnpike POCP [START_REF] Vinter | Convex duality and nonlinear optimal control[END_REF]. We observe uniform convergence along this trajectory, as well as time decrease of the difference, as predicted by the theory.

the true value function t → v * (t, x * (t)) along this optimal trajectory, stated by Theorem 1, is illustrated in Figure 2. Moreover, the difference t → v * (t, x * (t)) -v k (t, x * (t)) is a decreasing function of time as we observed in the proof of Theorem 1.

Uniform approximation over a set

Consider the classical linear quadratic regulator problem:

v * (t 0 , x 0 ) := inf u (16)

For each (t 0 , x 0 ), the infimum is attained and the value of the problem can be computed by solving a Riccati differential equation. To illustrate Theorem 2 we are interested in the average value [START_REF] Luenberger | Optimization by vector space methods[END_REF] for an initial measure µ 0 concentrated at time 0 and uniformly distributed in space in X 0 = [-1, 1]. We approximate this value with primal and dual solutions of LPs ( 13) and [START_REF] Villani | Topics in optimal transportation[END_REF]. The countour lines (in decimal logarithmic scale) of the difference between the true value function (t, x) → v * (t, x) and a polynomial approximation of degree 6 (t, x) → v 6 (t, x) is represented in Figure 3. We also show the support of optimal trajectories starting from X 0 . This illustrates the fact that the approximation of the value function is correct in this region, as stated by Theorem 2. It is noticeable Figure 3: Countour lines (at 0, -1, -2, -3, . . .) of the decimal logarithm of the difference (t, x) → v * (t, x) -v 6 (t, x) between the actual value function and its polynomial approximation of degree 6 for LQR POCP (16). The dark area represents the set of optimal trajectories starting from x 0 ∈ X 0 = [-1, 1] at time t 0 = 0. We observe that the difference is smaller in this area, as predicted by the theory. . that this is computed by a single linear program and provides approximation guarantees uniformly over an uncountable set of optimal control problems.

1 0(

 1 10x(t) 2 + u(t) 2 )dt s.t. ẋ(t) = x(t) + u(t)x(t 0 ) = x 0 .

CNRS, LAAS, 7 avenue du colonel Roche, F-31400 Toulouse, France.

Université de Toulouse, LAAS, F-31400 Toulouse, France.

Faculty of Electrical Engineering, Czech Technical University in Prague, Technická 2, CZ-16626 Prague, Czech Republic


Cyril Roussillon

Simon Lacroix

High rate-localization for high-speed all-terrain robots

Localization plays a central role in autonomous robot navigation, and a vast number of contributions can be found in the robotics literature. High speed all terrain robots raise new challenges for localization, that must run at higher rates, while being more accurate than for walking speed robots. The article presents a SLAM setup that satisfies these requirements, using vision and low-cost inertial sensors as its core. Other localization sensors can also be incorporated, such as GPS or odometry, yielding a system that estimate the robot pose and velocity parameters at 100 Hz, with translation errors on the pose of the order of 1m for 500m long trajectories.

I. INTRODUCTION

Most field robotics applications would benefit from faster operations, which implies faster robots. Augmenting the speed of all-terrain robots raises several challenges, the main one being the fact that the vehicle dynamics must be considered for both path planning and path tracking processes, which is all the more difficult with hardly predictable vehicle/ground interactions. Moving at higher speeds also impacts the perception processes: the robot must perceive the terrain further away in order to assess feasible paths, and process the gathered data at a speed compatible with its motions. One of the key to cope with computation limitations, is to exploit approaches that comply with an economy of means principle [START_REF] Kelly | Minimum throughput adaptive perception for high speed mobility[END_REF].

But high motion speeds also imply stringent constraints on the rate and precision of the localization processes. Localization is essential to autonomous navigation, and is notably required to ensure (i) the spatial consistency of the terrain model built to plan paths and (ii) the proper tracking of the paths. Both functions require precise pose estimates: for instance, a 1 • error on the robot pitch angle yields a 17cm error on the elevation of a point detected at 10.0m, a distance at which precise information on the terrain is required for speeds above a few m/s, and the robot position and heading must be precisely known to track paths at such speeds. Both functions also require a given frequency of the pose estimates. Path tracking does not require very high rate localization (e.g. in [START_REF] Lenain | High-speed mobile robot control in off-road conditions: a multi-model based adaptive approach[END_REF] a 10Hz rate is sufficient to control a robot at 8m/s: the response time of the actuators and the robot dynamics are such that a higher rate is not necessary), but terrain modelling processes do require high rate localization. Since every data perceived on the environment must be precisely localized to be incorporated in the model that represents the environment geometry, the robot pose must be known at the exact time data are gathered.

The use of rotating Lidar that continuously stream range data are becoming popular in field robotics (for instance the Velodyne Lidar simultaneously fires 64 lasers at about 4kHz): in the absence of "continuous localization", the produced images are warped because of the robot translations and of the attitude variations, that can not be neglected for all-terrain robots. Finally, locomotion monitoring to detect faults such as slippages may also benefit from precise high rate pose estimates. SLAM processes have shown to be elegant and robust solutions to the localization problem, and the robotics literature now abounds with analyses of the problem, proposals of theoretical solutions and practical setups (see e.g. [START_REF] Bailey | Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms[END_REF], [START_REF] Durrant-Whyte | Simultaneous Localisation and Mapping (SLAM): Part II State of the Art[END_REF] for a rather complete synthesis). In this paper we present a SLAM solution that estimates the pose and velocities of a robot at 100 Hz, thus making it usable for high speed all-terrain robots. It mainly relies on a low-cost inertial measurement unit (IMU) to predict the robot motions, and on vision to observe landmarks of the environment. These two sensors form the backbone of our approach, according to a classical motion prediction / landmark observation scheme, implemented with an extended Kalman filter. To improve the consistency of the system, and in particular to avoid the scale drift factor inherent to monocular SLAM approaches and that is not perfectly corrected with low cost inertial sensors, direct observations of the robot state provided by other sensors (namely odometry and GPS) are incorporated in the filter.

Our solution relies on RT-SLAM, an open-source software framework originally presented in [START_REF] Roussillon | Rt-slam: a generic and real-time slam architecture[END_REF], whose architecture is summarized in section II. Section III depicts the way the images are processed so as to optimize computation time, while ensuring that no outliers are integrated within the filter. Section IV briefly depicts the way the IMU is used to predict the robot motions, and section V introduces how odometry and GPS pose measurements are introduced within the filter for a better robustness and precision over time. Finally section VI depicts some results, and in particular the benefits of using the additional sensors.

II. OVERALL RT-SLAM ARCHITECTURE

RT-SLAM is a generic, efficient and flexible SLAM applications development tool, which is tailored to yield on-board robust and real-time solutions. RT-SLAM stands for "Real-Time SLAM", and is available as open source software at http://rtslam.openrobots.org.

Fig. 1 presents the main objects defined in RT-SLAM. They encompass the basic concepts of a SLAM solution: the world or environment contains maps; maps contain robots and landmarks; robots have sensors; sensors can make observations of landmarks. Each of these objects is abstract and can have different implementations. They can also contain other objects that may themselves be generic. The maps contain an optimization or estimation engine: for now RT-SLAM uses a standard formulation of EKF-SLAM. Since this solution is very well documented in the literature [START_REF] Davison | Monoslam: Real-time single camera slam[END_REF], it is not detailed in depth here. Indirect indexing within Boost's ublas C++ structures is intensively used to exploit the sparsity of the problem and the symmetry of the covariances matrices.

b) Robot: Robots can be of different type according to the way their state is represented and their prediction model. The latter can be either a simple kinematic model (constant velocity, constant acceleration, . . . ) or a proprioceptive sensor (odometry, inertial, . . . ), as illustrated section IV. The proprioceptive sensor is an example of generic object contained in robot objects, as different hardware can provide the same function.

c) Sensor: We distinguish two kinds of sensors. The first kind are exteroceptive sensors, that observe landmarks in the environment. Similarly to robots, they can also have different models (perspective camera, panoramic catadioptric camera, laser, . . . ), and contain a generic exteroceptive sensor hardware object (firewire camera, USB camera, . . . ). The second kind are proprioceptive sensors, that directly observe the pose of the robot and help for localization. Note that this acceptation of the term "proprioceptive" is here quite different from its original meaning in physiology: proprioceptive sensors are the ones that directly observe the state of the robot (or a part of the state), as opposed to exteroceptive sensors that observe the environment. Thus we list as proprioceptive sensors inertial sensors, odometry, GPS, magnetometers, barometers etc, even if some actually observe properties of the environment -accelerometers observe earth gravity, magnetometers observe magnetic field, barometers observe air pressure.

In addition, as sensors belong to the map, their state can be estimated: this opens the possibility for estimating other parameters such as extrinsic calibration, time delays, biases and gain errors, and the like.

d) Landmark: Landmarks can be of different type (points, lines, planes, . . . ), and each type can have different state parametrization (Euclidean point, inverse depth point, . . . ). Moreover the parametrization of a landmark can change over time, as explained section III-B. A landmark also contains a descriptor dedicated to data association, which is a dual description to the state representation.

As shown Fig. 1(a), it is worth noticing that landmark objects are common to the different sensors, all of them being able to observe the same landmark (provided they have compatible descriptors for this landmark of course). This allows to greatly improve the observability of landmarks compared to a system where the sensors are strictly independent. In the particular case of two cameras for instance, landmarks can be used even if they are only visible from one camera or if they are too far away for a stereovision process to observe their depth (this process was introduced in [START_REF] Sola | Bicamslam: Two times mono is more than stereo[END_REF] as BiCam SLAM).

e) Observation: In RT-SLAM, the notion of observation plays a predominant role. An observation is a real object containing both methods and data. One observation is instantiated for every sensor-landmark pair, regardless of the sensor having actually observed the landmark or not, and has the same lifetime as the associated landmark. The methods it contains are the conventional projection and backprojection models (that depend on the associated sensor and landmark models), while the stored data consist of results and intermediary variables such as Jacobian matrices, predicted and observed appearances, innovations, event counters and others, that allow to greatly simplify and optimize computations.

f) Managers: In order to achieve full genericity wrt landmark types, in particular to allow the concurrent use of different landmark types for one sensor, two different manager objects are added: data manager and map manager. Their implementations define a given management strategy, while their instantiations are dedicated to a certain landmark type. The data manager processes the sensors raw data, providing observations of the external landmarks. For this purpose, it exploits some raw data processors (for feature detection and tracking), and decides which observations are to be corrected and in which order, according to the quantity of information they bring and their quality. For example it can apply an active search strategy and try to eliminate outliers as described in section III-A. The map manager keeps the map clean, with relevant information, and at a manageable size, by removing landmarks according to their quality and the given policy (e.g. visual odometry where landmarks are discarded once they are not observed, or multimap slam where maps are "closed" according to given criteria). These managers communicate together: for example, the data manager may ask the map manager if there is enough space in the map to start a new initialization process, and to allocate the appropriate space for the new landmark.

III. VISUAL PROCESSING WITHIN RT-SLAM

Vision is currently the only exteroceptive sensor implemented in RT-SLAM. The processing of images for a SLAM setup is quite complex, and calls for the definition of several key processes that are summarized here.

A. Active search and one-point RANSAC

The strategy currently implemented in RT-SLAM data manager to deal with observations is an astute combination of active search [START_REF] Davison | Monoslam: Real-time single camera slam[END_REF] and outliers rejection using one-point RANSAC [START_REF] Civera | 1-point ransac for ekf-based structure from motion[END_REF].

The goal of active search is to minimize the quantity of raw data processing by constraining the search in the area where the landmarks are likely to be found. Observations outside of this 3σ observation uncertainty ellipse would be anyway considered incompatible with the filter and ignored by the gating process. In addition active search gives the possibility to decide anytime to stop matching and updating landmarks with the current available data, thus enabling hard real-time constraints. We extended the active search strategy to landmark initialization: each sensor strives to maintain features in its whole field of view using a randomly moving grid of fixed size, and feature detection is limited to empty cells of the grid. Furthermore the good repartition of features in the field of view ensures a better observability of the motions.

Outliers can come from matching errors in raw data or mobile objects in the scene. Gating is not always discriminative enough to eliminate them, particularly right after the prediction step when the observation uncertainty ellipses can be quite large -unfortunately at this time the filter is very sensitive to faulty corrections because it can mistakenly make all the following observations incompatible. To prevent faulty observations, outliers are rejected using a one-point RANSAC process. It is a modification of RANSAC, that uses the Kalman filter to obtain a whole model with less points than otherwise needed, and provides a set of strongly compatible observations that are then readily corrected. Contrary to [START_REF] Civera | 1-point ransac for ekf-based structure from motion[END_REF] where data association is assumed given when applying the algorithm, we do the data association along with the one-point RANSAC process: this allows to look for features in the very small strongly compatible area rather than the whole observation uncertainty ellipse, and to save additional time for raw data processing.

B. Landmark parametrization and re-parametrization

In order to solve the problem of adding to the EKF a point with unknown distance and whose uncertainty cannot be represented by a Gaussian distribution, point landmarks parametrization and initialization strategies for monocular EKF-SLAM have been well studied [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF] [13] [START_REF] Montiel | Unified inverse depth parametrization for monocular slam[END_REF]. The solutions now widely accepted are undelayed initialization techniques with inverse depth parametrization. Anchored Homogeneous Point [START_REF] Sola | Consistency of the monocular ekf-slam algorithm for three different landmark parametrizations[END_REF] parametrization is currently used in RT-SLAM.

The drawback of inverse depth parametrization is that they describe a landmark by at least 6 variables in the stochastic map, compared to only 3 for an Euclidean point (x y z)

T . Memory and temporal complexity being quadratic with the map size for EKF, there is a factor of 4 to save in time and memory by reparametrizing landmarks that have converged enough [START_REF] Civera | Inverse Depth to Depth Conversion for Monocular SLAM[END_REF]. The map manager uses the linearity criterion proposed in [START_REF] Montiel | Unified inverse depth parametrization for monocular slam[END_REF] to control this process.

C. Image processing g) Point extraction:

Point extraction is based on Harris detector with several optimizations. Some of them are approximations: a minimal derivative mask [-1, 0, 1] is used, as well as a square and constant convolution mask, in order to minimize operations. This allows the use of integral images [START_REF] Viola | Rapid object detection using a boosted cascade of simple features[END_REF] to efficiently compute the convolutions. Additional optimizations are related to active search (section III-A): only one new feature is searched in a small region of interest, which eliminates the costly steps of thresholding and submaxima suppression.

h) Point matching: Point matching is based on Zeromean Normalized Cross Correlation (ZNCC), also with several optimizations. Integral images are used to compute means and variances, and a hierarchical search is made (two searches at half and full resolution are sufficient). We also implemented bounded partial correlation [START_REF] Stefano | Zncc-based template matching using bounded partial correlation[END_REF] in order to interrupt the correlation score computation when there is no more hope to obtain a better score than the threshold or the best one up to now. To be robust to viewpoint changes and to track landmarks longer, tracking is made by comparing the initial appearance of the landmark with its current predicted appearance [START_REF] Davison | Monoslam: Real-time single camera slam[END_REF].

IV. MOTION PREDICTION

A. Motion model-based prediction

In the absence of any additional sensor, a motion model is required to predict the state at the time observations are integrated in the filter. A constant velocity model is often used in vision-based SLAM systems. Velocities must then be included in the robot state to be estimated: R = (p q v w) T where p and q are respectively the position and quaternion orientation of the robot, and v and w are its linear and angular velocities. The predict equation is then:

p + = p+v.dt q + = q * w.dt v + = v w + = w
The covariance matrix is predicted with the Jacobians of the predict equations for p and q, and with the uncertainty of the model for v and w, that represents how much the robot actual dynamic may differ from the model. For a constant velocity model, it is defined by the maximal linear and angular accelerations that the robot can experience.

The advantage of the constant velocity model is its simplicity, but it rapidly fails when the system is subject to dynamic motions. A more realistic motion model can be provided by the robot dynamic model parametrized by the applied control u, but this is hardly feasible with a high speed ground robot, for which the terrain irregularities generate unpredictable perturbations.

B. IMU-based prediction

The pure visual SLAM approach described in the previous section suffers from several drawbacks that can be lessened by using additional sensors:

• The scale factor is not estimated. This can be solved by using a second camera with a known baseline, or by using a proprioceptive sensor that provides velocity information.

• Search areas for landmark matching in the image can be large as a kinematic model is not very precise. Using a proprioceptive sensor reduces the landmark uncertainty ellipses and hence speeds up image processing.

• Linearisation points are not very accurate, again as a consequence of the poor precision of the kinematic model. Using another sensor at a higher rate will allow to exploit the kinematic model at a shorter term where it will be more precise, improving SLAM precision or permitting to reduce the frame-rate to decrease CPU load for equivalent quality.

• Very high motion dynamics are difficult to track because the uncertainty of the kinematic model has to be increased even more, and the very large uncertainty ellipses implied increase the matching time and the risk of wrong matching. The use of sensors for the prediction step is computationally cheap, as only the covariance matrix part that depends on the robot state is updated, which is significantly smaller than the whole filter state covariance matrix that contains the landmarks. The sensor must nevertheless fulfill the following constraints to be eligible:

1) It is periodic with a higher frequency than any other sensor integrated in the filter, 2) It provides complete data, i.e. that enables to predict all the robot state parameters -otherwise a kinematic or dynamic model is still needed to complete the prediction, 3) It has a reasonably low level of noise, so that it provides a good linearisation point, 4) It is never faulty, i.e. its errors are always consistent with its uncertainty, and it has a Gaussian probability. 5) It does not measure a quantity that is redundant with an other sensor, otherwise it would constantly override its measures instead of being fused with. An IMU satisfies all these requirements: it is fast and its instantaneous measures are precise, thus yielding predictions around which linearisations do not induce errors. It also stabilizes the attitude angles thanks to the observation of the earth gravity, and it complements well vision in the SLAM problem, as it provides better observability of the scale factor.

When using an IMU to predict motions, the robot state becomes: R = (p q v a b w b g)

T
where a b and w b are the accelerometers and gyrometer biases, and g the norm of the gravity vector. Estimating only the norm of the gravity vector requires a correct initialization of the attitude of the robot, which can easily be done with initial acceleration measures acquired when the robot is static, which is not a strong operational constraint.

A special care has to be taken for the conversion of the noise from continuous time (provided by the manufacturer in the sensor's datasheet) to discrete time. As the perturbations are continuous white noise, the variance of the discrete noise grows linearly with the integration period.

V. ADDITIONAL OBSERVATIONS

The IMU does not directly measures velocities, but accelerations that are biased: it can only provide a correct scale factor observation if these biases are properly estimated. Good observability of these biases is a difficult task: it requires the trajectory to stimulate all the degrees of freedom, and the filter must remain consistent in the long term. These conditions are hardly met with a typical robot trajectory, that usually mainly goes forward with limited turns, and because it covers large distances with landmarks that are often renewed, easing a drift of the scale factor estimation. And when the camera loses track of all the landmarks, the scale factor drift biases the new landmarks position estimates, leading the filter to become inconsistent.

A. Odometry

Odometry is an ambivalent sensor: it provides most of the time a precise and stable estimation of the robot velocity, but it can also deliver very faulty data, e.g. when the robot experiences slippages.

There are two common flaws in odometry integration: 1) Using it in the prediction step of the filter. When faulty measures are not identified as such, observation are not consistent with the prediction and either rejected by a gating process or badly integrated in the filter. 2) Assuming that odometry provides the complete velocity vector, oriented along the forward axis of the robot. This is wrong whenever the robot is not rigid: because of tires or suspension, the robot body (on which observation sensors are rigidly tied) is not actually Fig. 2. The robot Mana moving along the estimated direction. This discrepancy all the more important when the robot is moving at high speeds. Thus in RT-SLAM odometry is integrated by observing only the norm of the robot velocity. This allows to properly estimate the scale factor by providing correct velocities, and to reject faulty data by gating out inconsistent values.

Our robot Mana is built upon a Segway RMP400 platform (figure 2), and the odometry provides the independent velocities and torques of the four wheels. This enables to dynamically compute an estimation of the uncertainty of odometry according to the consistency of these measures. For now this estimation is made by analysing the homogeneity of rear and front wheels velocities and torques, and the absolute values of the acceleration, the turn radius and the torques -a machine learning approach to derive this estimation should give better results. However it is important to notice that this uncertainty that takes into account the fault risk can not be used by the gating process, because it would render the faulty data consistent. It is better to use the basic Gaussian uncertainty for the gating, so as to completely reject faulty measures.

Another purpose of odometry integration is to stabilize the system when vision is not working. For instance if the environmental conditions prevent the camera from observing landmarks (masking, low light, sun blinded, etc), then the IMU is not stabilized anymore and quickly starts drifting: odometry help stabilizing the system in such cases. For more efficiency, it is possible to consider again that the odometry observes the complete velocity vector but with higher uncertainty. In particular one can assume that the lateral component of the velocity is usually null, and use gating to detect when it is not the case.

B. GPS

All the sensors described above will not prevent the estimation of the robot's pose to drift on large scale trajectories (except the attitude angles that are stabilized by the IMU). If GPS signal is available, even occasionally, it will not only bound the position error, but also the heading error by comparing the direction of the local velocity estimated by vision and IMU to the direction of the global velocity provided by the GPS measures (even if GPS provides only position measures).

The easiest way to integrate GPS is known as loose integration, and consists in directly using the position measures computed by the GPS. On the contrary, tight integration uses the raw observations of pseudo-distances to the satellites and reimplements the GPS equations in the filter, which is more precise but also a lot more complex. Thus we only perform loose integration in RT-SLAM.

This naturally works very well with RTK-GPS, because its noise has a Gaussian distribution as required by the filter. However the error of natural GPS is highly biased, mainly by perturbations in the atmosphere that change very slowly. Consequently it must be observed at a limited frequency to avoid being affected too much by this bias.

VI. RESULTS

Videos illustrating the test sequences can be downloaded at http://rtslam.openrobots.org/Material/ CCCA2012. As this sequences taken by our robot Mana cover quite long distances, it is not possible to recall all observed landmarks -otherwise the system would be far from real-time. A solution to this problem is to build several submaps, but as this is not implemented yet in RT-SLAM we are running "short-term SLAM", immediately forgetting landmarks when they are lost.

A. First sequence

The first sequence represents Mana navigating autonomously along a 450m trajectory at 1.5m/s. It is driving half on a road and half on an irregular grassy terrain, reaching successive waypoints while avoiding obstacles thanks to a digital terrain map built from its Velodyne sensor data integrated with our localization (see section VI-C).

Figure 3 shows the trajectory for different combinations of sensors. The trajectory which is using RTK-GPS (with centimetre accuracy at 20Hz) can be considered as ground truth for the position. The trajectory with only camera and IMU illustrates the difficulties it to correctly estimate the scale factor. Adding odometry bounds the position error to a few meters, as shown in more detail figure 4.

B. Second sequence

The second sequence represents Mana navigating autonomously at 3m/s, again on grassy and concrete terrains. A this speed our basic control algorithms reach their limits, so the trajectory is quite jerky.

Figures 5 and6 show two things. First as there are a lot of accelerations and stops, the scale factor is better observed by the system with only camera and IMU. Second, as the robot is constantly turning left and right it renews very often its landmarks, which makes the heading drift faster. That is why there is no significant difference in accuracy with and without odometry.

C. Attitude angles estimation

It is difficult to evaluate the precision of the attitude angle estimation in the absence of ground truth. Nevertheless, the "quality" of the digital terrain model built using range data acquired while to robot is moving provides insights on the precision of RT-SLAM. Figure 7 compares two digital terrain maps built with the Xsens MTi configured as an INS to provide pitch and roll to a 3D odometry localization process, and with RT-SLAM integrating the same Xsens MTi configured as an IMU with camera and odometry.

VII. SUMMARY

We focused in this paper on the fact that high speed and precise localization is necessary for faster autonomous robots, and presented how this can achieved on the basis of vision/INS SLAM approach, complemented by additional proprioceptive sensors for better consistency of the position estimates on the long run. Results show that this 100 Hz localization system allows to build precise terrain models from range data at speeds up to several meter per seconds, and 1m accuracy position estimates overs several hundreds meters long trajectories. Higher rates are easily reachable with a faster CPU: RT-SLAM requires a single-core 3GHz processor to process INS data at 100 Hz and VGA images at 50Hz.

Fig. 1 .

 1 Fig. 1. The main objects in RT-SLAM

Fig. 3 .Fig. 4 .

 34 Fig. 3. Trajectories estimated on the first sequence with different combinations of sensors

Fig. 5 .Fig. 6 .

 56 Fig. 5. Trajectories of the robot Mana on the second sequence with different set of sensors

Fig. 7 .

 7 Fig. 7. A digital terrain map of a flat grassy area, built from Velodyne lidar range data using attitude angles provided by an INS (left) and by RT-SLAM (right), while the robot is moving at 2m/s. Erroneous and delayed attitude angles estimates of the INS corrupt significantly the range data, and hinders the building of a precise terrain model.