See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/303095146

slides iros08 2

Data · May 2016

CITATIONS READS

0 7

1 author:

F. Aghili
Canadian Space Agency
167 PUBLICATIONS   1,706 CITATIONS   

SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Automated Laser Tracking Evaluation and Scan Constraint Analysis for Pose Estimation View project

Dr. Murad Al Shibli View project

All content following this page was uploaded by F. Aghili on 14 May 2016.

The user has requested enhancement of the downloaded file.

 A Gauge-Invariant Formulation for Constrained
Robotic Systems Using Square-Root Factorization
and Unitary Transformation

Farhad Aghili

Space Technologies, Spacecraft Engineering

Canadian Space Agency (CSA)

2008 IEEE/RSJ Int. Conference on Intelligent Robots & Systems

Nice, France

logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 1 / 19

Outline
1 Introduction
2 Simplified Lagrangian of Mechanical Systems
Square-Root Factorization
Quasi-Variables
Unitary Transformation
3 Constrained Mechanical Systems
Dynamics of Reduced Quasi-Velocities
Gauge-Invariant Formulation
4 Control
Properties of the Dynamics Formulation
Control Law
5 Simulation
6 Conclusions logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 2 / 19

Introduction
Modeling in terms of generalized coordinates

Dynamics equations of MBS are cornerstone for simulation and control

of robotic systems
Efficient modeling techniques to derive the equations of motion of
MBS in novel forms.
Describing the equations in terms of joint variables and their
time-derivatives
In classical mechanics of constrained MBS
The tangential space does not possess a natural metric
The generalized coordinates or force may contains translational
and rotational components
Dynamic formulation in not invariant—a solution depends on
measure units
logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 3 / 19

Introduction (cont’d)
Modeling in terms of quasi-velocities

Other techniques to describe equations of motion in terms of

quasi-velocities
If the square-root factorization of mass matrix is integrable, robot
dynamics can be significantly simplified [D. E. Kodistchek, 1985]
Closed-form derivation of mass matrix factorization of MBS [G.
Rodriguez et al., 1992]
Set-point control of manipulator [A. Jain et al. 1995] (tracking
problem using quasi-velocity feedback remains unsolved)
Extending the notion of quasi-velocity for constrained MBS
Quasi-velocity is invariant under unitary transformation
Gauge-invariance
The equations of motion are decoupled from those of constrained
force
Tracking control: A composite feedback on general coordinates and
logo
quasi-velocity

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 4 / 19

Simplified Lagrangian of Mechanical Systems
Square-Root Factorization

Dynamics of MBS obeys Euler-Lagrange equations:

!
d ∂T ∂T 1
− = f, with T (q, q̇) = q̇ T M (q)q̇
dt ∂ q̇ ∂q 2
Square-root factorization of a symmetric p.d. matrix M
M = WWT

Cholesky decomposition M = LLT (lower triangular matrix):

i−1
X
2

1/2
lii = mii − lik ∀i = 1, · · · , n
k=1
i−1
X

lji = mji − ljk lik /lii ∀j = i + 1, · · · , n
k=1

T 
d

 ∂ W T q̇ logo
Substitution =⇒ f = W dt W T q̇ + Ẇ − ∂q W T q̇

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 5 / 19

Simplified Lagrangian of Mechanical Systems
Quasi-Variables

Define vectors of quasi-velocity and quasi-force as

v(q, q̇) , W T (q)q̇ and u(q, f ) , W −1 (q)f.

Dynamics equation in term of quasi-variables

 ∂v T 
v̇ + Γv = u, where Γ , W −1 Ẇ −
∂q

The quasi-coordinate ξ(q) ∋ ξ˙ = v may not exist.

W (q) must be a Jacobian matrix
State-space representation:
   −T   
d q W 0
= v+ u.
dt v −Γ I logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 6 / 19

Simplified Lagrangian of Mechanical Systems
Unitary Transformation

Consider orthogonal matrix V , i.e., V V T = V T V = I. Then

W̄ = W V =⇒ W̄ W̄ T = M

New quasi-variables.

v̄ = V T v and ū = V T u

Time-derivative of orthogonal matrix satisfies:

V̇ = −ΩV,

where skew-symmetric matrix Ω is the angular rate; analogous to

Ṙ = −[ω×]R in the 3-D.
Dynamics equations of the transformed variables

v̄˙ + Γ̄v̄ = ū, Γ̄ = V T Γ − Ω V

logo
where

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 7 / 19

Constrained Mechanical Systems
Quasi-Variables

EL equations of constrained MBS

 
d ∂T ∂T
− = f − AT λ,
dt ∂ q̇ ∂q
A(q)q̇ = 0

Define Λ , AW −T , then the above equations are equivalent to:

v̇ + Γv = u − ΛT λ
Λv = 0

According to singular value decomposition

 
S 0
Λ = U ΣV T where Σ= ,
0 0
logo
U = [U1 U2 ] ∈ Rm×m and V = [V1 V2 ] ∈ Rn×n are orthogonal matrices

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 8 / 19

Constrained Mechanical Systems
Reduced Variables

Equations of the transformed (unitary) quasi-variables

v̄˙ + Γ̄v̄ = ū − Λ̄T λ,

Λ̄v̄ = 0

.
where Λ̄ , ΛV = [Λr .. 0m×(n−r) ] and Λr , U1 S. The structure of Λ̄
specifies:
 
0r×1
v̄ =  · · ·  where vr , V2T v ∈ Rn−r
vr

is the reduced quasi-velocity.

One-to-one correspondence

q̇ = W −T V2 vr , and vr = V2T W T q̇. logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 9 / 19

Constrained Mechanical Systems
Dynamics of Reduced Quasi-Velocities

The vector of quasi-force is partitioned accordingly

 
uo
uo , V1T W −1 f
ū = · · · , where
ur , V2T W −1 f
ur

Equations of motion and force constraint force

ΛTr λ + Γo vr = uo uo λ
v̇r + Γr vr = ur
ur v̇r
where Γr = V2T
Γ − Ω V2 and

Γo = V1T Γ − Ω V2 . motion
State-space representation
   −T   
d q W V2 0
= vr + ur logo
dt r v −Γ r I

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 10 / 19

Constrained Mechanical Systems
Unified Formulation

Reduced square-root factorization

Wr , W V 2

Coriolis terms:
∂vr

Γr = Wr+ Ẇr −
∂q
∂vr

Γo = Wr− Ẇr −
∂q
where
Wr+ Wr = I left inverse of Wr
Wr− Wr =0 annihilator of Wr
logo
Unified formulation for constrained and unconstrained systems

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 11 / 19

Constrained Mechanical Systems
Gauge-Invariant Formulation

The space of generalized velocity does not possess natural metric

Inconsistent results; they depend on the measure unit
The quasi-variable have always homogenous units
√
Since kvr k = kvk = 2T , then
p
vr : [ kg/s]
p
ur : [ kg/s2 ]

Even if the generalized coordinates or the constraint equations

have both rotational and translational components

logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 12 / 19

Control
Properties of the Dynamics Formulation

Conservation of energy: T = 21 kvr k2 =⇒ d

dt kvr k
2
= 0 if ur ≡ 0, i.e.,

vrT Γr vr = 0

Coriolis terms are radially bounded, i.e., ∃γ > 0 s.t.

kΓr k, kΓo k ≤ γkvr k

Reduced square-root matric

If c1 I ≤ M (q) ≤ c2 I, then
1 1
√ ≤ kWr+ k ≤ √
c2 c1
Lipschitz condition
logo
kWrT (q) − WrT (q ∗ )k ≤ c2 kq − q ∗ k ∀q, q ∗ ∈ Rn

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 13 / 19

Control
Control Law

Assume independent variable θ ∈ Rn−r so that q = q(θ). Then

 ∂q(θ) 
vr = B θ̇ where B, V2T W T .
∂θ
Motion controller
ur = ṡ + Γr s − Kd ǫ
where s = vrd − WrT Kp q̃ and composite error
ǫ , ṽr + B(q)Kp q̃
Exponential stability if λmin (Kp ) > c.
Force controller
uo = ΛTr λd + Γo vr

Minimum control input: kuk2 = kuo k2 + kur k2

logo
uo = 0 =⇒ kuk 7−→ min

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 14 / 19

Simulation
A Constrained P-R-R Robot

P-R-R robot:
   
q1 (m) f1 (N) q3
q = q2 (rad) f = f2 (Nm)
q3 (rad) f3 (Nm) y
q2 λ2
λ1
l = 1 (m) and m = 5 (kg). θ
q1
r θ
Constraint equation: A(q)q̇ = 0,
d x
 
c l(c3 + 1) l
A(q) = s23 ls3 .
r +1 1
23
r
Figure: Constrained motion of a
Identity θ̇ = q̇2 + q̇3 trivially yields
P-R-R manipulator.
∂θ  
= 0 1 1 .
∂q logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 15 / 19

Simulation
Results

3 2.5
6

2 2 4
q3

vr ( kgm/s)
2
q2 , q3 (rad)

1 1.5

q1 (m)
q1 0

√
0 1
−2

−1 q2 0.5 −4

−6
−2 0
0 1 2 3 4 0 1 2 3 4
time (sec) time (sec)

Figure: Configuration variables. Figure: Reduced quasi-velocity.

logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 16 / 19

 Simulation
Results (cont’d)

15

10 20 20
λ2
ur , uo ( kgm/s )

uo1
2

5 10 10

ylab1

ylab2
0 0 0
√

−5 −10 −10
ur
uo2 λ1
force
−10 −20 −20

0 1 2 3 4
−15
0 1 2 3 4
time (sec)
time (sec)
Figure: Lagrangian multipliers without
Figure: Motion and force control (blue) and with (red) force control.
inputs (quasi-forces).
logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 17 / 19

Simulation
Results (cont’d)

15

force control

kuk ( kgm/s )
10
√ 2

no force control

0
0 1 2 3 4
time (sec)

Figure: Euclidean norm of quasi-forces.

logo

F. Aghili (CSA) A Gauge-Invariant Formulation ... IROS’08 18 / 19

Conclusions

An gauge-invariant formulation for constrained MBS based on the notion

reduced quasi-velocity
The equations of motion and those of constraint forces are
decoupled such that each system has its own independent force
input.
The invariance problems rising in hybrid force/motion control of
constrained MBS involving both rotational and translational
components becomes a nonissue
Tracking control based on the composite feedback on the vectors of
quasi–velocity and configuration–variables

logo

View publication
F. Aghili (CSA)stats A Gauge-Invariant Formulation ... IROS’08 19 / 19