A Globally Exponentially Convergent Immersion

and Invariance Speed Observer for Mechanical

Systems with Non–Holonomic Constraints
Romeo Ortega
Laboratoire des Signaux et Systèmes, Supelec
Gif–sur–Yvette, France

Collaboration with: Alessandro Astolfi (Imperial College, UK) and Aneesh Venkatraman (Groningen
University)

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 1/1

Problem Formulation

Consider general n-dof mechanical systems with non–holonomic constraints

M (q)q̈ + C(q, q̇)q̇ + ∇U (q) = G(q)u + Z(q)λ,

Z > (q)q̇ = 0,

where q, q̇ ∈ Rn generalized positions and velocities, u ∈ Rm control input, Z(q)λ

constraint forces with Z : IR n → IR n×k , λ ∈ Rk , G : IR n → IR n×m input matrix,
M : IR n → IR n×n , M = M > > 0 mass matrix, and U : IRn → IR potential energy
function. C(q, q̇)q̇ vector of Coriolis and centrifugal forces, defined via the Christoffel
symbols.
Assumptions
q measurable and
the system is forward complete.
Objective
Design a globally asymptotically convergent observer for q̇.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 2/1

Observer Design: The I&I Approach

The dynamical system

η̇ = α(q, η),

with η ∈ Rn , is called an I&I observer for the mechanical system if there exists a full
rank matrix T : Rn → Rn×n and a vector function β : Rn → Rn , such that the
manifold
M := {(η, q, q̇) : β(q) = η + T > (q)q̇} ⊂ Rn × Rn × Rn

is positively invariant and attractive.

The asymptotic estimate of q̇ is given by

T −> (β − η).

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 3/1

Main Result

Proposition There exist smooth mappings A : IR 3n−2k+1 × IRn × IRm → IR3n−2k+1 ,

B : IRn → IR(n−k)×(3n−2k+1) and N : IRn → IRn−k × IRn , with the latter left invertible,
such that the dynamical system

χ̇ = A(χ, q, u) (O1)

with state χ ∈ IR3n−2k+1 , inputs q and u, and output

η = B(q)χ, (O2)

has the following property

lim eαt [N (q)q̇(t) − η(t)] = 0,
t→∞

for some α > 0 and for all initial conditions (q(0), q̇(0), χ(0)) ∈ IR n × IRn × IR3n−2k+1 .
That is, (O1), (O2) is a globally exponentially convergent speed observer for the mechanical
system.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 4/1

A Suitable Hamiltonian Representation

The system can be written in port-Hamiltonian form as

        
q̇ 0 I ∇q H(q, p) 0 0
  =   + u +   λ,
ṗ −I 0 ∇p H(q, p) G(q) Z(q)

Z > (q)∇p H(q, p) = 0,

where p = M (q)q̇, and

1 > −1
H(q, p) = p M (q)p + U (q).
2

Restricted to the constrained space, it can be represented as (van der Schaft’2000)

      
q̇ 0 S̃(q) ∇q Hc (q, p̃) 0
 =  +  u,
p̃˙ −S̃ > (q) J(q, p̃) ∇p̃ Hc (q, p̃) Gc (q)

with
1 > −1
Hc (q, p̃) = p̃ M̃ (q)p̃ + U (q),
2
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 5/1

cont’d

where M̃ : IRn → IR(n−k)×(n−k) is defined as

M̃ −1 (q) = m11 (q) − m12 (q)m−1

22 (q)m12 (q).

with mij the partition of M −1 (induced by the constraints),

p̃ = S̃ > (q)p,

S̃ : IRn → Rn×(n−k) is a full–rank right annihilator of Z > and Gc : IRn → R(n−k)×m is the
constrained input matrix G. The (ij)-th element of J : IR n × IRn−k → IR(n−k)×(n−k) is
given by
Jij (q, p̃) = −p> [S̃i , S̃j ], (1)

where S̃i is the i-th column of S̃, and [S̃i , S̃j ] is the standard Lie bracket. Recalling that

[S̃i , S̃j ] = −[S̃j , S̃i ]

we conclude that J is skew–symmetric.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 6/1

Some Definitions

Introduce a factorization, (e.g., Cholesky) M̃ −1 (q) = T > (q)T (q), where

T : IRn → IR(n−k)×(n−k) is a full rank matrix.
Define
L : IRn → IRn×(n−k) , F : IRn × IRm → IRn−k , S : IRn × IRn−k → IR(n−k)×(n−k)

L(q) = S̃(q)T > (q),

F (q, u) = T (q)[Gc (q)u − S̃ > (q)∇U (q)]
n
X
S(y, x) = T JT > + [((∇qi T )T −1 x)(L> ei )> − (L> ei )((∇qi T )T −1 x)> ].
i=1

where
(y, x) = (q, T (q)p̃),

and ei the i–th basis vector of Rn−k .

Notice that, since q and u are measurable, L and F are known. Moreover, L is a
left-invertible matrix.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 7/1

A Key Lemma

The system admits a state space representation of the form

ẏ = L(y)x
ẋ = S(y, x)x + F (y, u), (♥)

and S verifies the following properties.

(i) S is skew–symmetric.
(ii) S is linear in the second argument.
(iii) There exists a mapping S̄ : IRn × IRn−k → IR(n−k)×(n−k) such that

S(y, x)x̄ = S̄(y, x̄)x,

for all y ∈ IRn , x, x̄ ∈ IRn−k .

The lemma implies that the speed observer problem can be recast as an observer
problem for (♥) with output y.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 8/1

Example 1: The Chaplygin Sleigh

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 9/1

cont’d
Model
   
m 0 −ma sin(q3 ) − sin(q3 )
   
M (q) = 
 0 m ma cos(q3 ) 
 , Z(q) = 
 cos(q3 ) ,

−ma sin(q3 ) ma cos(q3 ) I + ma2 0

m is the mass of the rigid body, I is the moment of inertia of the rigid body about its
center of mass and a denotes the fixed distance between the knife edge and the
center of mass. The body is moving on the ground, that is, U (q) = 0 and u = 0.
Simulation parameters and IC’s

m=1 y(0) = (1, 3, 1.5)

a=2 x(0) = (5, 10)
I=5 ŷ(0) = (2, 5, 5)
k1 = 0.8, 1.7 x̂(0) = (3, 7)
k2 = 4 ξ1 (0) = (2, 2)
k3 = 5 r(0) = 3

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 10/1

 cont’d

q, ŷ (top row) and of q̇, q̇ˆ (bottom row), for k1 = 0.8 (dashed lines) and k1 = 1.7 (dotted
lines).

20 9 5

0 8 4.5

4
−20 7
3.5

q3
q2
q1

−40 6
3
−60 5
2.5

−80 4 2

−100 3 1.5
0 2 4 6 8 0 2 4 6 8 0 2 4 6 8
t (sec) t (sec) t (sec)

5 12 2

10
1.5
placements 0
8

6 1

q̇3
q̇2
q̇1

−5
4 0.5
2
−10
0
0

−15 −2 −0.5
0 2 4 6 8 0 2 4 6 8 0 2 4 6 8
t (sec) t (sec) t (sec)
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 11/1

Example 2: A Walking Robot

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 12/1

Conclusions

A definite affirmative answer has been given to the question of existence of a globally
convergent speed observer for general mechanical systems with non–holonomic
constraints.
No assumption is made on the existence of an upperbound for the inertia matrix,
hence the result is applicable for robots with prismatic joints.
The only requirement is that the system is forward complete, i.e., that trajectories of
the system exist for all times t ≥ 0—which is a rather weak condition.
In some sense, our contribution should be interpreted more as an existence result than
an actual, practically implementable, algorithm. Leaving aside the high complexity of
the observer dynamics, the difficulty stems from the fact that a key function is defined
via an integral expression, whose explicit analytic solution cannot be guaranteed a
priori.

CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE

NESTER Meeting, Milano, July 4, 2009 – p. 13/1