# CENTRE NATIONAL

DE LA RECHERCHE
SCIENTIFIQUE
A Globally Exponentially Convergent Immersion
and Invariance Speed Observer for Mechanical
Systems with Non–Holonomic Constraints
Romeo Ortega
Laboratoire des Signaux et Syst ` emes, Supelec
Gif–sur–Yvette, France
Collaboration with: Alessandro Astolﬁ (Imperial College, UK) and Aneesh Venkatraman (Groningen
University)
NESTER Meeting, Milano, July 4, 2009 – p. 1/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
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 ∈ R
n
generalized positions and velocities, u ∈ R
m
control input, Z(q)λ
constraint forces with Z : IR
n
→IR
n×k
, λ ∈ R
k
, G : IR
n
→IR
n×m
input matrix,
M : IR
n
→IR
n×n
, M = M

> 0 mass matrix, and U : IR
n
→IR potential energy
function. C(q, ˙ q) ˙ q vector of Coriolis and centrifugal forces, deﬁned via the Christoffel
symbols.
Assumptions
q measurable and
the system is forward complete.
Objective
Design a globally asymptotically convergent observer for ˙ q.
NESTER Meeting, Milano, July 4, 2009 – p. 2/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Observer Design: The I&I Approach
The dynamical system
˙ η = α(q, η),
with η ∈ R
n
, is called an I&I observer for the mechanical system if there exists a full
rank matrix T : R
n
→R
n×n
and a vector function β : R
n
→R
n
, such that the
manifold
M:= {(η, q, ˙ q) : β(q) = η +T

(q) ˙ q} ⊂ R
n
×R
n
×R
n
is positively invariant and attractive.
The asymptotic estimate of ˙ q is given by
T

(β −η).
NESTER Meeting, Milano, July 4, 2009 – p. 3/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Main Result
Proposition There exist smooth mappings A : IR
3n−2k+1
×IR
n
×IR
m
→IR
3n−2k+1
,
B : IR
n
→IR
(n−k)×(3n−2k+1)
and N : IR
n
→IR
n−k
×IR
n
, with the latter left invertible,
such that the dynamical system
˙ χ = A(χ, q, u) (O1)
with state χ ∈ IR
3n−2k+1
, inputs q and u, and output
η = B(q)χ, (O2)
has the following property
lim
t→∞
e
αt
[N(q) ˙ q(t) −η(t)] = 0,
for some α > 0 and for all initial conditions (q(0), ˙ q(0), χ(0)) ∈ IR
n
×IR
n
×IR
3n−2k+1
.
That is, (O1), (O2) is a globally exponentially convergent speed observer for the mechanical
system.
NESTER Meeting, Milano, July 4, 2009 – p. 4/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
A Suitable Hamiltonian Representation
The system can be written in port-Hamiltonian form as

˙ q
˙ p
¸
¸
=

0 I
−I 0
¸
¸

¸

q
H(q, p)

p
H(q, p)

+

0
G(q)
¸
¸
u +

0
Z(q)
¸
¸
λ,
Z

(q)∇
p
H(q, p) = 0,
where p = M(q) ˙ q, and
H(q, p) =
1
2
p

M
−1
(q)p +U(q).
Restricted to the constrained space, it can be represented as (van der Schaft’2000)

˙ q
˙
˜ p
¸
¸
=

0
˜
S(q)

˜
S

(q) J(q, ˜ p)
¸
¸

¸

q
H
c
(q, ˜ p)

˜ p
H
c
(q, ˜ p)

+

0
G
c
(q)
¸
¸
u,
with
H
c
(q, ˜ p) =
1
2
˜ p

˜
M
−1
(q)˜ p +U(q),
NESTER Meeting, Milano, July 4, 2009 – p. 5/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
cont’d
where
˜
M : IR
n
→IR
(n−k)×(n−k)
is deﬁned as
˜
M
−1
(q) = m
11
(q) −m
12
(q)m
−1
22
(q)m
12
(q).
with m
ij
the partition of M
−1
(induced by the constraints),
˜ p =
˜
S

(q)p,
˜
S : IR
n
→R
n×(n−k)
is a full–rank right annihilator of Z

and G
c
: IR
n
→R
(n−k)×m
is the
constrained input matrix G. The (ij)-th element of J : IR
n
×IR
n−k
→IR
(n−k)×(n−k)
is
given by
J
ij
(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.
NESTER Meeting, Milano, July 4, 2009 – p. 6/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Some Deﬁnitions
Introduce a factorization, (e.g., Cholesky)
˜
M
−1
(q) = T

(q)T(q), where
T : IR
n
→IR
(n−k)×(n−k)
is a full rank matrix.
Deﬁne
L : IR
n
→IR
n×(n−k)
, F : IR
n
×IR
m
→IR
n−k
, S : IR
n
×IR
n−k
→IR
(n−k)×(n−k)
L(q) =
˜
S(q)T

(q),
F(q, u) = T(q)[G
c
(q)u −
˜
S

(q)∇U(q)]
S(y, x) = TJT

+
n
¸
i=1
[((∇
q
i
T)T
−1
x)(L

e
i
)

−(L

e
i
)((∇
q
i
T)T
−1
x)

].
where
(y, x) = (q, T(q)˜ p),
and e
i
the i–th basis vector of R
n−k
.
Notice that, since q and u are measurable, L and F are known. Moreover, L is a
left-invertible matrix.
NESTER Meeting, Milano, July 4, 2009 – p. 7/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
A Key Lemma
The system admits a state space representation of the form
˙ y = L(y)x
˙ x = S(y, x)x +F(y, u), (♥)
and S veriﬁes the following properties.
(i) S is skew–symmetric.
(ii) S is linear in the second argument.
(iii) There exists a mapping
¯
S : IR
n
×IR
n−k
→IR
(n−k)×(n−k)
such that
S(y, x)¯ x =
¯
S(y, ¯ x)x,
for all y ∈ IR
n
, x, ¯ x ∈ IR
n−k
.
The lemma implies that the speed observer problem can be recast as an observer
problem for (♥) with output y.
NESTER Meeting, Milano, July 4, 2009 – p. 8/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Example 1: The Chaplygin Sleigh
NESTER Meeting, Milano, July 4, 2009 – p. 9/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
cont’d
Model
M(q) =

m 0 −ma sin(q
3
)
0 m ma cos(q
3
)
−ma sin(q
3
) ma cos(q
3
) I +ma
2
¸
¸
¸
¸
, Z(q) =

−sin(q
3
)
cos(q
3
)
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 ﬁxed 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 ˆ y(0) = (2, 5, 5)
k
1
= 0.8, 1.7 ˆ x(0) = (3, 7)
k
2
= 4 ξ
1
(0) = (2, 2)
k
3
= 5 r(0) = 3
NESTER Meeting, Milano, July 4, 2009 – p. 10/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
cont’d
q, ˆ y (top row) and of ˙ q,
ˆ
˙ q (bottom row), for k
1
= 0.8 (dashed lines) and k
1
= 1.7 (dotted
lines).
0 2 4 6 8
−100
−80
−60
−40
−20
0
20
t (sec)
0 2 4 6 8
3
4
5
6
7
8
9
t (sec)
0 2 4 6 8
1.5
2
2.5
3
3.5
4
4.5
5
t (sec)
0 2 4 6 8
−15
−10
−5
0
5
t (sec)
0 2 4 6 8
−2
0
2
4
6
8
10
12
t (sec)
0 2 4 6 8
−0.5
0
0.5
1
1.5
2
t (sec)
PSfrag replacements
q
1
q
2
q
3
˙
q
1
˙
q
2
˙
q
3
NESTER Meeting, Milano, July 4, 2009 – p. 11/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Example 2: A Walking Robot
NESTER Meeting, Milano, July 4, 2009 – p. 12/13
CENTRE NATIONAL
DE LA RECHERCHE
SCIENTIFIQUE
Conclusions
A deﬁnite afﬁrmative 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 difﬁculty stems from the fact that a key function is deﬁned
via an integral expression, whose explicit analytic solution cannot be guaranteed a
priori.
NESTER Meeting, Milano, July 4, 2009 – p. 13/13

u ∈ Rm control input. q)q + q ˙ ˙ U (q) = = G(q)u + Z(q)λ. q)q vector of Coriolis and centrifugal forces. July 4. and U : I n → I potential energy function. 2009 – p. G : IR n → IR n×m input matrix. deﬁned via the Christoffel ˙ ˙ symbols. Assumptions q measurable and the system is forward complete. 2/1 . q ∈ Rn generalized positions and velocities. 0. C(q. Objective Design a globally asymptotically convergent observer for q. ˙ CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting.Problem Formulation Consider general n-dof mechanical systems with non–holonomic constraints M (q)¨ + C(q. Z (q)q ˙ where q. R R M : IR n → IR n×n . Z(q)λ ˙ constraint forces with Z : IR n → IR n×k . M = M > 0 mass matrix. λ ∈ Rk . Milano.

Milano. q) : β(q) = η + T ˙ is positively invariant and attractive. July 4. 2009 – p. such that the manifold M := {(η.Observer Design: The I&I Approach The dynamical system η = α(q. ˙ with η ∈ Rn . The asymptotic estimate of q is given by ˙ T − (β − η). q. η). (q)q} ⊂ Rn × Rn × Rn ˙ CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. 3/1 . 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 .

and output R η = B(q)χ. has the following property t→∞ (O1) (O2) lim eαt [N (q)q(t) − η(t)] = 0.Main Result Proposition There exist smooth mappings A : I 3n−2k+1 × I n × I m → I 3n−2k+1 . CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. q. ˙ R R R That is. ˙ for some α > 0 and for all initial conditions (q(0). Milano. u) ˙ with state χ ∈ I 3n−2k+1 . R R R R R R R R B : I n → I (n−k)×(3n−2k+1) and N : I n → I n−k × I n . (O2) is a globally exponentially convergent speed observer for the mechanical system. R such that the dynamical system χ = A(χ. q(0). July 4. 4/1 . with the latter left invertible. (O1). χ(0)) ∈ I n × I n × I 3n−2k+1 . inputs q and u. 2009 – p.

2 Restricted to the constrained space. and ˙ p H(q. H(q. July 4. Milano. p) = ˜ CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE 0 q ˙ = ˜ ˙ −S (q) p ˜   ˜ S(q) J(q. p) = 1 p M −1 (q)p + U (q). Z (q) where p = M (q)q. 2009 – p. p) p H(q. 1 p p M −1 (q)˜ + U (q). p) =  +  0 G(q)  u +   0 Z(q)   λ. p) ˜  +  0 Gc (q)   u. 5/1 . ˜ ˜ 2 NESTER Meeting. p) = 0. p) ˜ p Hc (q. it can be represented as (van der Schaft’2000)   with Hc (q. p) ˜   ˜ q Hc (q.A Suitable Hamiltonian Representation The system can be written in port-Hamiltonian form as   q ˙ p ˙     0 −I I 0   q H(q.

˜ ˜ ˜ ˜ where Si is the i-th column of S. ˜ p = S (q)p. Sj ] = −[Sj . and [Si . ˜ ˜ R R S : I n → Rn×(n−k) is a full–rank right annihilator of Z and Gc : I n → R(n−k)×m is the R R constrained input matrix G. Si ] we conclude that J is skew–symmetric. Sj ] is the standard Lie bracket. Recalling that ˜ ˜ ˜ ˜ [Si . The (ij)-th element of J : I n × I n−k → I (n−k)×(n−k) is R given by ˜ ˜ ˜ (1) Jij (q. Sj ]. CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. p) = −p [Si . Milano. July 4. 6/1 .cont’d ˜ R where M : I n → I (n−k)×(n−k) is deﬁned as R ˜ M −1 (q) = m11 (q) − m12 (q)m−1 (q)m12 (q). 2009 – p. 22 with mij the partition of M −1 (induced by the constraints).

Milano. CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. Cholesky) M −1 (q) = T (q)T (q). where R T : I n → I (n−k)×(n−k) is a full rank matrix. S : I n × I n−k → I (n−k)×(n−k) R L(q) F (q. ˜ T (q)[Gc (q)u − S (q) U (q)] n T JT + i=1 [(( qi T )T −1 x)(L ei ) − (L ei )(( qi T )T −1 x) ]. L is a left-invertible matrix. x) = = = ˜ S(q)T (q).Some Deﬁnitions ˜ Introduce a factorization. July 4.g. x) = (q. L and F are known. since q and u are measurable. where (y. (e. Moreover. R Deﬁne R R R R R R R L : I n → I n×(n−k) . u) S(y. p and ei the i–th basis vector of Rn−k . Notice that. T (q)˜). 7/1 . F : I n × I m → I n−k . 2009 – p..

(ii) S is linear in the second argument. CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. u). x. 2009 – p. (i) S is skew–symmetric.A Key Lemma The system admits a state space representation of the form y ˙ x ˙ = = L(y)x S(y. 8/1 . x)x. Milano. x ¯ for all y ∈ I n . x)¯ = S(y. x ∈ I n−k . July 4. x)x + F (y. R ¯ R The lemma implies that the speed observer problem can be recast as an observer problem for (♥) with output y. ¯ R R R (iii) There exists a mapping S : I n × I n−k → I (n−k)×(n−k) such that ¯ S(y. (♥) and S veriﬁes the following properties.

2009 – p. July 4. 9/1 . Milano.Example 1: The Chaplygin Sleigh CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting.

2009 – p.8. Milano. 3. U (q) = 0 and u = 0. 1. 2) r(0) = 3 CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. July 4. I is the moment of inertia of the rigid body about its center of mass and a denotes the ﬁxed distance between the knife edge and the center of mass. Simulation parameters and IC’s m=1 a=2 I=5 k1 = 0. Z(q) =  ma cos(q3 )   I + ma2  . 10/1 . The body is moving on the ground. 7) ˆ ξ1 (0) = (2. 1.  m is the mass of the rigid body.5) x(0) = (5. 10) y (0) = (2. 5) ˆ x(0) = (3.cont’d Model  m 0 −ma sin(q3 ) 0 m ma cos(q3 ) −ma sin(q3 )   − sin(q3 ) cos(q3 ) 0   M (q) =      . 5. that is.7 k2 = 4 k3 = 5 y(0) = (1.

q (bottom row).8 (dashed lines) and k1 = 1. 20 0 −20 9 8 7 5 4.5 0 6 1 0 −15 0 2 CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE 4 t (sec) 6 8 −2 2 4 t (sec) 6 8 NESTER Meeting.5 2 1.5 4 q1 −40 −60 −80 −100 q2 6 5 4 3 q3 0 2 4 t (sec) 6 8 3.7 (dotted ˆ ˙ ˆ ˙ lines). July 4. 11/1 . for k1 = 0.5 0 2 4 t (sec) 6 8 0 2 4 t (sec) 6 8 5 12 10 2 ments q1 ˙ 0 1.5 0 0 2 4 t (sec) 6 8 −0. y (top row) and of q. 2009 – p. Milano.cont’d q.5 3 2.5 8 −5 q2 ˙ 4 −10 2 q3 ˙ 0.

12/1 . 2009 – p. Milano. July 4.Example 2: A Walking Robot CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting.

In some sense. i. algorithm.Conclusions A deﬁnite afﬁrmative answer has been given to the question of existence of a globally convergent speed observer for general mechanical systems with non–holonomic constraints. CENTRE NATIONAL DE LA RECHERCHE SCIENTIFIQUE NESTER Meeting. July 4. hence the result is applicable for robots with prismatic joints. Milano. The only requirement is that the system is forward complete. our contribution should be interpreted more as an existence result than an actual.e. whose explicit analytic solution cannot be guaranteed a priori. No assumption is made on the existence of an upperbound for the inertia matrix. Leaving aside the high complexity of the observer dynamics. 13/1 .. practically implementable. 2009 – p. the difﬁculty stems from the fact that a key function is deﬁned via an integral expression. that trajectories of the system exist for all times t ≥ 0—which is a rather weak condition.