q
2
J
m1
m2
J
m
1
2
I
I
1
r
1 q
1
r
2
m
2
r2
m
1
2
m1
m2
K
2
1
K
10
standing assumptions
A1) small displacements at joints (linear elasticity domain)
A2) axisbalanced motors (i.e., center of mass of rotors on rotation axes)
further assumption on location of actuators in the kinematic chain
A3) each motor is mounted on the robot in a position preceding the driven link
L
1
L
2
L
N  1
Link 0
Base
Link 1
Joint 1
Link N  1
Link 2
Link N
Joint 2
World
Frame
Joint N
L
N
11
link (linear + angular) kinetic energy
T
=
N
i=1
T
i
=
N
i=1
_
T
L,
i
+T
A,
i
_
=
N
i=1
1
2
_
m
i
v
T
c,
i
v
c,
i
+
T
i
I
i
_
=
1
2
q
T
M
(q) q
motor linear kinetic energy the mass m
m
i
= m
s
i
+m
r
i
of each motor (stator
+ rotor) is just an additional mass of the carrying link
T
L,m
=
N
i=1
T
L,m
i
=
N
i=1
1
2
m
m
i
v
T
c,m
i
v
c,m
i
=
1
2
q
T
M
m
(q) q
summing up, a symmetric inertia matrix M(q) > 0 results
T
+T
L,m
=
1
2
q
T
(M
(q) +M
m
(q)) q =
1
2
q
T
M(q) q
12
link and motor potential energy due to gravity
U
g
= U
g,
+U
g,m
=
N
i=1
_
U
g,
i
(q) +U
g,m
i
(q)
_
= U
g
(q)
A2) both M, U
g
are independent from
potential energy due to joint elasticity
U
e
=
1
2
(q )
T
K(q )
with diagonal, positive denite N N matrix K of joint stiness
13
simplifying assumption reduced dynamic model of (Spong, 1987)
A4) angular kinetic energy of each motor is due only to its own spinning
T
A,m
=
1
2
T
B
+T
L,m
+T
A,m
) (U
g
+U
e
)
=
1
2
q
T
M(q) q +
1
2
T
B
U
g
(q)
1
2
(q )
T
K(q )
= L(q, , q,
)
14
EulerLagrange equations
given the set of generalized coordinates p = (q
T
T
)
T
IR
2N
, the Lagrangian
L satises the usual vector equation
d
dt
_
L
p
_
T
_
L
p
_
T
= u
being u IR
2N
the nonconservative forces/torques performing work on p
assuming no dissipative terms and no external forces (acting on links), since the
motor torques IR
N
only perform work on the motor variables we obtain
M(q) q +C(q, q) q +g(q) +K(q ) = 0
B
+K( q) =
link equation
motor equation
with centrifugal/Coriolis terms C(q, q) q and gravity terms g(q) = (U
g
/q)
T
15
Coriolis/centrifugal terms
being the generalized coordinates p = (q
T
T
)
T
, these quadratic terms in the
generalized velocity p are computed by (symbolic) dierentiation of the elements
of the 2n 2N robot inertia matrix
/(p) =
_
M(q) 0
0 B
_
=
_
/
1
/
2
.
.
. /
2N
_
(columns)
using the Christoel symbols (of the second type):
((p, p) p = col p
T
(
i
(p) p
(
i
(p) =
1
2
_
_
_
/
i
p
_
+
_
/
i
p
_
T
_
/
p
i
_
_
_
i = 1, 2, . . . , 2N
thanks to the simple structure of /= /(q), the computation is relevant only
for the upper left block M(q) only C(q, q) q in link equation
16
Model properties
M(q) 2C(q, q) is skewsymmetric
nonlinear dynamic model, but linear in a set of coecients a = (a
r
, a
K
, a
B
)
(including K and B)
Y
r
(q, q, q) a
r
+diagq a
K
= 0
diag
a
B
+diag q a
K
=
for K (rigid joints): q and K(q ) nite value, so that the
equivalent rigid model is recovered (summing up link and motor equation)
_
M(q) +B
_
q +C(q, q) q +g(q) =
there exists a bound on the norm of the gravity gradient matrix
_
_
_
_
_
g(q)
q
_
_
_
_
_
g(q
1
) g(q
2
) q
1
q
2
, q
1
, q
2
IR
N
17
. . . work out the dynamic model for a case study
q
2
J
m1
m2
J
m
1
2
I
I
1
r
1 q
1
r
2
m
2
r2
m
1
2
m1
m2
K
2
1
K
planar 2R arm with elastic joints (without or with gravity)
18
Singularly perturbed dynamic model
if joint stinesses K = diagK
1
, . . . , K
N
are very large ( rigid joints), the
system exhibits a twotime scale dynamic behavior in terms of link position (q)
and joint deformation torque (z) that can be used for simpler control design
to show this, we use a linear change of coordinates
_
q
z
_
=
_
q
K( q)
_
and rewrite the motor acceleration and the second time derivative of the joint
deformation torque as
= B
1
( z)
z = K(
q) = K
_
B
1
( z) +M
1
(q) (C(q, q) q +g(q) z)
_
= KB
1
+KM
1
(q) (C(q, q) q +g(q)) K
_
B
1
+M
1
(q)
_
z
19
from K, we can extract a common large scalar factor
1
2
1 so that
K =
1
2
K =
1
2
diag
K
1
, . . . ,
K
N
with
K
i
of similar (moderate) amplitude
the second dynamic equation becomes
2
z =
KB
1
+
KM
1
(q) (C(q, q) q +g(q))
K
_
B
1
+M
1
(q)
_
z ()
and represents the fast dynamics associated with the elastic joints, while
M(q) q +C(q, q) q +g(q) = z
represents the slow dynamics of the links
time scaling is made explicit by introducing the fast time variable =
t
in ()
2
z =
2
d
2
z
dt
2
=
d
2
z
d
2
20
Inverse dynamics
given a suciently smooth link trajectory q
d
(t), together with a number of its
time derivatives, compute the required motion torque
d
(t)
the associated motor trajectory
d
(t) is needed
the motor position is computed from the link equation as
d
= q
d
+K
1
(M(q
d
) q
d
+C(q
d
, q
d
) q
d
+g(q
d
))
motor velocity is computed from the rst time derivative of link equation
d
= q
d
+K
1
_
M(q
d
)q
[3]
d
+
M(q
d
) q
d
+
C(q
d
, q
d
) q
d
+C(q
d
, q
d
) q
d
+ g(q
d
)
_
using the notation x
[i]
=
d
i
x
dt
i
21
motor acceleration is computed from the second time derivative of link equation
d
= q
d
+ K
1
_
M(q
d
)q
[4]
d
+2
M(q
d
)q
[3]
d
+
M(q
d
) q
d
+
C(q
d
, q
d
) q
d
+2
C(q
d
, q
d
) q
d
+C(q
d
, q
d
)q
[3]
d
+g(q
d
)
_
nally, the needed torque is computed from the motor equation by substitution
d
= B
d
+K(
d
q
d
) = BK
1
_
M(q
d
)q
[4]
d
+. . . +g(q
d
)
_
+(M(q
d
) +B) q
d
+C(q
d
, q
d
) q
d
+g(q
d
)
this Lagrangianbased scheme may become computationally heavy for large N
a recursive O(N) NewtonEuler inverse dynamics algorithm may be set up,
by including in the forward recursions also the linear/angular link jerks (third
derivatives) and snaps (fourth derivatives) and in the backward recursions also
the rst and second derivatives of the link forces/torques
22
Sensing requirements
full state feedback requires sensing of four variables: q, (link/motor position)
and q,
(link/motor velocity) 4N state variables for a Ndof EJ robot
only motor variables (,
) are available with standard sensing arrangements
(encoder + tachometer on the motor axis)
velocities also through numerical dierentiation of highresolution encoders
advanced systems have also measures beyond the elasticity of the joints (e.g.,
link position q and joint torque
J
= K(q )(= z) sensors in DLR LWRs)
23
Exploded view of a DLR LWRIII joint
24
Control problems
regulation to a constant equilibrium conguration (q, , q,
) = (q
d
,
d
, 0, 0)
only the desired link position q
d
is assigned, while
d
has to be determined
q
d
may come from the kinematic inversion of a desired cartesian pose x
d
direct kinematics of EJ robots is a function of link variables: x = kin(q)
tracking of a suciently smooth trajectory q = q
d
(t)
the associated motor trajectory
d
(t) has to be determined
again, q
d
(t) is uniquely associated to a desired cartesian trajectory x
d
(t)
other relevant planning/control problems not considered here include
resttorest trajectory planning in given time T
Cartesian control (regulation or tracking directly dened in the task space)
force/impedance/hybrid control of EJ robots in contact with the environment
25
Regulation
a simple linear example
two elastically coupled masses (motor/link) driven on one side (Quanser LEJ)
dynamic model (without damping/friction eects)
m q +k(q ) = 0 b
+k( q) =
using Laplace transform, we can dene two inputoutput transfer functions of
interest from the force input to . . .
the position of the rst mass (collocated), representing the motor
the position q of the second mass (noncollocated), representing the link
26
Transfer functions of interest
motor transfer function
P
motor
(s) =
(s)
(s)
=
ms
2
+k
mbs
2
+(m+b)k
1
s
2
unstable system with zeros, but passive (zeros always precede poles on the
imaginary axis) stabilization can be achieved via output () feedback
link transfer function
P
link
(s) =
q(s)
(s)
=
k
mbs
2
+(m+b)k
1
s
2
unstable but controllable system as before ( any pole assignment via full
state feedback), but now without zeros!
with damping, pole/zero pairs are moved to the lefthand side of Cplane
27
Typical frequency response of a single elastic joint
10
1
10
0
60
40
20
0
20
Motor velocity output
Frequency (rad/sec)
M
a
g
n
i
t
u
d
e
(
d
B
)
10
1
10
0
100
50
0
50
100
Frequency (rad/sec)
P
h
a
s
e
(
d
e
g
)
10
1
10
0
40
20
0
20
Link velocity output
Frequency (rad/sec)
M
a
g
n
i
t
u
d
e
(
d
B
)
10
1
10
0
300
200
100
0
Frequency (rad/sec)
P
h
a
s
e
(
d
e
g
)
antiresonance/resonance behavior on motor velocity output, pure resonance on
link velocity output (weak or no zeros)
28
Feedback strategies with reduced measurements
1) = u
1
(k
P
q +k
D
q) (link PD feedback)
W
(s) =
q(s)
u
1
(s)
=
k
mbs
4
+(m+b)ks
2
+kk
D
s +kk
P
always unstable (spring damping/friction leads to small stability intervals)
2) = u
2
(k
Pm
+k
Dm
) (motor PD feedback)
W
mm
(s) =
k
mbs
4
+mk
Dm
s
3
+[m(k +k
Pm
) +bk] s
2
+kk
Dm
s +kk
Pm
asympotically stable for k
Pm
> 0, k
Dm
> 0 (Routh criterion) as in rigid
systems!
29
3) = u
3
(k
P
q +k
Dm
+g(q
d
)
with symmetric (diagonal) K
P
> 0, K
D
> 0, and the motor reference position
d
:= q
d
+K
1
g(q
d
)
Theorem (Tomei, 1991) If
min
(K
E
) :=
min
__
K K
K K +K
P
__
>
then the closedloop equilibrium state (q
d
,
d
, 0, 0) is globally asymptotically stable
31
Lyapunovbased proof
closedloop equilibria ( q =
= q =
= 0) satisfy
K(q ) +g(q) = 0
K( q) K
P
(
d
) g(q
d
) = 0
adding/subtracting K(
d
q
d
) g(q
d
) (= 0, by denition of
d
) yields
K(q q
d
) K(
d
) +g(q) g(q
d
) = 0
K(q q
d
) +(K
P
+K)(
d
) = 0
or, in matrix form,
K
E
_
q q
d
d
_
=
_
g(q
d
) g(q)
0
_
32
using the Theorem assumption, for all (q, ) ,= (q
d
,
d
) we have
_
_
_
_
_
K
E
_
q q
d
d
__
_
_
_
_
min
(K
E
)
_
_
_
_
_
_
q q
d
d
__
_
_
_
_
>
_
_
_
_
_
_
q q
d
d
__
_
_
_
_
q q
d

g(q
d
) g(q) =
_
_
_
_
_
_
g(q
d
) g(q)
0
__
_
_
_
_
and hence (q
d
,
d
) is the unique equilibrium conguration
dene the positiondependent energy function
P(q, ) =
1
2
(q )
T
K(q )+
1
2
(
d
)
T
K
P
(
d
)+U
g
(q)
T
g(q
d
)
its gradient P(q, ) = 0 only at (q
d
,
d
) (using the same above argument)
and
2
P(q
d
,
d
) > 0 (q
d
,
d
) is an absolute minimum for P(q, )
33
the following is thus a candidate Lyapunov function
V (q, , q,
) =
1
2
q
T
M(q) q +
1
2
T
B
+P(q, ) P(q
d
,
d
) 0
its time derivative, evaluated along the closedloop system trajectories, is
V = q
T
M(q) q +
1
2
q
T
M(q) q +
T
B
+
_
q
_
T
K(q )
T
K
P
(
d
) + q
T
_
U
g
(q)
q
_
T
T
g(q
d
)
= q
T
_
C(q, q) q g(q) K(q ) +
1
2
M(q) q +K(q ) +g(q)
_
+
T
(u K( q) K(q ) K
P
(
d
) g(q
d
))
=
T
_
K
P
(
d
) K
D
+g(q
d
) K
P
(
d
) g(q
d
)
_
=
T
K
D
0
where the skewsymmetry of
M 2C has been used
34
since
V = 0
= 0, the proof is completed using LaSalle Theorem
substituting
(t) 0 in the closedloop equations yields
M(q) q +C(q, q) q +g(q) +Kq = K = constant ()
Kq = K K
P
(
d
) g(q
d
) = constant ()
from () it follows that q(t) 0, which in turn simplies () into
g(q) +K(q ) = 0 ( )
from the rst part of the proof, the unique solution to ()( ) is q = q
d
,
=
d
and thus the largest invariant set contained in the set of states such
that
V = 0 global asymptotic stability of the set point
35
Remarks on regulation control
if the (minimum) joint stiness min
i=1,...,N
K
i
> , the Theorem assumption
min
(K
E
) > can always be satised by increasing
min
(K
P
)
since the symmetric matrices K and K
P
are assumed diagonal, it is sucient
to analyze the scalar case (N = 1)
det(I K
E
) =
2
(2K +K
P
) +KK
P
min
(K
E
) = K +
K
P
2
_
K
2
+
_
K
P
2
_
2
set K = +, for arbitrary small > 0: the assumption is satised if
K
P
> 2 +
2
for 0 K
P
36
in the presence of model uncertainties, the control law (with K
P
large enough)
= K
P
(
d
) K
D
+g(q
d
)
d
:= q
d
+
K
1
g(q
d
)
provides asymptotic stability, for a dierent (still unique) equilibrium ( q,
)
a version with online gravity compensation (De Luca, Siciliano, Zollo, 2005)
= K
P
(
d
) K
D
+g(
)
where
:= K
1
g(q
d
) is a biased motor position measurement
proof uses a modied Lyapunov candidate with
P
t
=
1
2
(q )
T
K(q ) +
1
2
(
d
)
T
K
P
(
d
) +U
g
(q) U
g
(
)
however, the available proof does not relax the assumptions on a minimum
K (structural) and on the need of an associated lower bound involving K
P
(minimum positional control gain)
37
Comparative simulation results on a 2R robot
K
P
= diag180, 180 K
D
= diag80, 80 ( 133)
0 2 4 6 8 10
0.5
0
0.5
1
1.5
2
[s]
[
r
a
d
]
LINK POSITIONS
0 2 4 6 8 10
100
0
100
200
300
400
[s]
[
N
m
]
MOTOR TORQUES
0 2 4 6 8 10
0.5
0
0.5
1
1.5
2
[s]
[
r
a
d
]
LINK #1 POSITION ERROR
0 2 4 6 8 10
0.05
0
0.05
0.1
0.15
[s]
[
r
a
d
]
LINK #2 POSITION ERROR
online (solid) vs. constant (dashed) gravity compensation
38
Comparative simulation results on a 2R robot
K
P
= diag150, 150 K
D
= diag50, 50 (suciency is violated)
0 2 4 6 8 10
0.5
0
0.5
1
1.5
2
[s]
[
r
a
d
]
LINK POSITIONS
0 2 4 6 8 10
100
0
100
200
300
400
[s]
[
N
m
]
MOTOR TORQUES
0 2 4 6 8 10
0.5
0
0.5
1
1.5
2
[s]
[
r
a
d
]
LINK #1 POSITION ERROR
0 2 4 6 8 10
0.1
0.05
0
0.05
0.1
0.15
0.2
0.25
[s]
[
r
a
d
]
LINK #2 POSITION ERROR
online (solid) vs. constant (dashed) gravity compensation
39
Further remarks on regulation control
a stronger result is obtained using online quasistatic gravity cancellation (Kugi,
Ott, AlbuSchaer, Hirzinger, 2008)
= K
P
(
d
) K
D
+g( q)
where q = q() is obtained by solving iteratively, at any given position
g( q) +K( q ) = 0 q
i
= K
1
g(q
i1
)
the sequence q
0
= , q
1
, q
2
, . . . converges (in few iterations) to q thanks to
a contraction mapping result structural assumption min
i=1,...,N
K
i
> is
kept, while any K
P
> 0 is sucient
an even stronger result can be obtained using a nonlinear PD law, including
dynamic gravity cancellation on the link dynamics (De Luca, Flacco, 2011),
based on the feedback equivalence principle any K > 0 and K
P
> 0 will
be sucient
40
Trajectory tracking
assuming that
q
d
(t) C
4
(fourth derivative w.r.t. time exists)
full state is measurable
we proceed by system inversion from the link position output q
a nonlinear static state feedback will be obtained that decouples and exactly
linearizes the closedloop dynamics (set of inout integrators) for any value K
exponential stabilization of the tracking error is then performed on the linear
side of the transformed problem
) = 0
the input appears through
and the motor equation
B
+K( q) =
42
substitution of
gives
M(q)q
[4]
+c(q, q, q, q
[3]
) +KB
1
K( q) = KB
1
with
c(q, q, q, q
[3]
) := 2
M(q)q
[3]
+(
M(q) +K) q + n(q, q)
the control law
= BK
1
_
M(q)a +c(q, q, q, q
[3]
)
_
+K( q)
leads to the closedloop system
q
[4]
= a
N separate inputoutput chains of four integrators (linearization and decoupling)
43
(q, q, q, q
[3]
) is an alternative globally dened state representation
from the link equation
q = M
1
(q) (K( q) n(q, q))
i.e., link acceleration is a function of (q, , q)
from the rst dierentiation of the link equation
q
[3]
= M
1
(q)
_
K(
q)
M(q) q n(q, q)
_
i.e., link jerk is a function of (q, , q,
) (using the above expression for q)
the control term c(q, q, q, q
[3]
) can be expressed as a function of (q, , q,
),
with an ecient organization of computations
the control law = (q, , q,
, a) can be implemented as a nonlinear static
(instantaneous) feedback from the original state
44
Tracking error stabilization
control design is completed on the linear side of the problem by choosing
a = q
[4]
d
+K
3
(q
[3]
d
q
[3]
) +K
2
( q
d
q) +K
1
( q
d
q) +K
0
(q
d
q)
with q and q
[3]
expressed in terms of (q, , q,
) and diagonal gain matrices
K
0
, . . . , K
3
chosen such that
s
4
+K
3i
s
3
+K
2i
s
2
+K
1i
s +K
0i
i = 1, . . . , N
are Hurwitz polynomials
the tracking errors e
i
= q
di
q
i
on each link coordinate satisfy
e
[4]
i
+K
3i
e
[3]
i
+K
2i
e
i
+K
1i
e
i
+K
0i
e
i
= 0
i.e., exponentially stable linear dierential equations (with chosen eigenvalues)
45
Remarks on trajectory tracking control
the shown feedback linearization result is the nonlinear/MIMO counterpart of
the transfer function q being without zeros (no zero dynamics)
the same result can be rephrased as q is a at output for EJ robots
a nominal feedforward torque ( inverse dynamics!) can be computed o line
d
= B
d
+K(
d
q
d
)
using the previous developments, where
K(
d
q
d
) = M(q
d
) q
d
+n(q
d
, q
d
)
d
= K
1
_
M(q
d
)q
[4]
d
+c(q
d
, q
d
, q
d
, q
[3]
d
)
_
a simpler tracking controller (of local validity around the reference trajectory) is
=
d
+K
P
(
d
) +K
D
(
)
46
Twotime scale control design
for high stiness K the system is singularly perturbed may use a simpler
composite control law, combining a slow and a fast component
=
s
(q, q, t) +
f
(q, q, z, z, t)
where
s
= [
=0
depends only on the slow dynamics of link motion (time t in
the arguments may come from the reference trajectory q
d
(t) to be tracked)
the slow control
s
is designed on the equivalent rigid dynamics (e.g., a feedback
linearization/computed torque or a PD law) obtained by setting = 0 in the
singularly perturbed model, whereas the fast control
f
is used for stabilization
of fast dynamics due to elasticity around the manifold of equivalent rigid motion
the control design is thus split in two parts that work at dierent time scales: we
should avoid to mix back these through feedback (
f
should not contain terms
of order 1/ or higher)
47
use the input =
s
+
f
in the fast dynamics of the singularly perturbed
model (see slide 20), set = 0 (in the limit), and solve for z
z =
_
B
1
+M
1
(q)
_ _
B
1
s
+M
1
(q) (C(q, q) q +g(q))
_
()
= h(q, q,
s
(q, q, t)) a controldependent manifold in the
4Ndimensional state space of the robot
replacing in the slow dynamics (M(q) q +. . . = z) yields, after simplications
(M(q)+B) q + C(q, q) q + g(q) =
s
slow reduced (2Ndim) system
which is the equivalent rigid robot dynamics (obtained for K !)
for tracking a reference trajectory q
d
(t) C
2
, choose, e.g., a slow control law
based on feedback linearization
s
= (M(q) +B) ( q
d
+K
D
( q
d
q) +K
P
(q
d
q)) +C(q, q) q +g(q)
=
s
(q, q, t)
48
substitute the (partially designed) control law =
s
(q, q, t) +
f
in the fast
dynamics of the singularly perturbed model
2
z =
K
_
B
1
_
s
(q, q, t) +
f
_
_
B
1
+M
1
(q)
_
z
+M
1
(q) (C(q, q) q +g(q))
_
due to time scale separation, the slow variables in the fast dynamics are assumed
to stay constant to their values at t =
t (q = q(
t) = q, q = q(
t) =
q), so
2
z =
K
_
B
1
f
_
B
1
+M
1
( q)
_
z
_
+w( q,
q,
t)
where
w( q,
q,
t) =
K
_
B
1
s
( q,
q,
t) +M
1
( q)
_
C( q,
q)
q +g( q)
__
which in turn, when compared with the manifold dened by (), is rewritten as
w( q,
q,
t) =
K
_
B
1
+M
1
( q)
_
z
z will be treated as a constant parameter in the fast dynamics
49
dening the error on the fast variables as = z z, its dynamics is
(=
2
z) =
K
_
B
1
f
+
_
B
1
+M
1
( q)
_
( z z)
_
=
K
_
B
1
f
_
B
1
+M
1
( q)
_
_
the fast control law should stabilize this linear error dynamics so that the fast
variable z converges to its boundary layer z
with a diagonal K
f
> 0 (but such that
max
(K
f
) _
1
), the choice
f
= K
f
=
f
(q, q, z, z, t)
leads to the exponentially stable error dynamics
+
_
KB
1
K
f
_
+
_
K
_
B
1
+M
1
( q)
__
= 0
(being all matrices positive denite)
the nal control law is =
s
(q, q, t) K
f
z
50
An extension Invariant manifold control design
in the previous analysis, the slow control component
s
works correctly, i.e., it
tracks the reference trajectory q
d
(t) on the equivalent rigid manifold, only for
= 0
to improve the local behavior around an equivalent reduced (2Ndim) manifold
in the IR
4N
state space, we add corrective terms
s
=
0
+
1
+
2
2
+. . .
(in the previous control law,
0
=
s
)
proceed as before for the slow control design, but using a similar expansion of
the resulting manifold (compare with ())
z = h(q, q, z, z, , t)
= h
0
(q, q, z, z, t) +h
1
(q, q, z, z, t) +
2
h
2
(q, q, z, z, t) +. . .
51
in particular, by using up to the secondorder correction term, it can be shown
that the resulting manifold can be made invariant
if the initial state starts on the (integral) manifold, the controlled trajectories
will remain on it unless disturbances occur
this result is similar to the one obtained by feedback linearization, but restricted
to the integral manifold
the fast control law is then needed only for recovering from initial state mismatch
and/or disturbances
see (Spong, Khorasani, Kokotovic, 1987)
52
Robots with mixed rigid/elastic joints
consider an Ndof robot having R rigid joints, characterized by q
r
IR
R
, and
N R elastic joints, characterized by link variables q
e
IR
NR
and motor
variables
e
IR
NR
the state dimension is 2R+4(N R) = 4N 2R
under assumption A4), the dynamic model can be rewritten in a partitioned way
(possibly, after reordering of joint variables) as
_
M
rr
(q) M
re
(q)
M
T
re
(q) M
ee
(q)
__
q
r
q
e
_
+
_
n
r
(q, q)
n
e
(q, q)
_
+
_
0
K
e
(q
e
e
)
_
=
_
r
0
_
B
e
e
+K
e
(
e
q
e
) =
e
where q=(q
T
r
q
T
e
)
T
IR
N
, the 2N2N inertia matrix M(q) and its diagonal blocks M
rr
(q)
and M
ee
(q) are invertible for all q, the 2Nvector n(q, q)=(n
T
r
(q, q) n
T
e
(q, q))
T
collects all
centrifugal/Coriolis and gravity terms, K
e
is the diagonal (NR)(NR) stiness matrix of
the elastic joints, and =(
T
r
T
e
)
T
IR
N
are the input torques
53
for the link accelerations (i.e., applying the system inversion algorithm to the
output y = q, after two time derivatives)
_
q
r
q
e
_
=
_
_
_
_
M
rr
M
re
M
1
ee
M
T
re
_
1
0
_
M
ee
M
T
re
M
1
rr
M
re
_
1
M
T
re
M
1
rr
0
_
_
_
_
r
0
_
+
_
r
(q, q,
e
)
e
(q, q,
e
)
_
= ,(q) +(q, q,
e
)
it is easy to see that ,(q) is the decoupling matrix of the system (i.e., all its
rows should be nonzero) as soon as M
re
, 0
if this is the case, ,(q) is always singular (due to the last columns of zeros)
the necessary (and sucient) condition for inputoutput decoupling by static
state feedback is violated
thus, if M
re
, 0, the more general class of dynamic state feedback may be
needed for obtaining exact linearization of the closedloop system
54
consider then the case M
re
0; moreover, assume that n
e
= n
e
(q, q
e
) (i.e.,
is independent from q
r
)
this happens if and only if M
rr
= M
rr
(q
r
), M
ee
= M
ee
(q
e
) (using the
Christoel symbols for the derivation of the Coriolis/centrifugal terms from the
robot inertia matrix)
the latter implies also n
r
= n
r
(q, q
r
) a complete inertial separation between
the dynamics of the rigidly driven and of the elastically driven links follows
_
M
rr
(q
r
) q
r
+n
r
(q, q
r
) =
r
M
ee
(q
e
) q
e
+n
e
(q, q
e
) +K
e
(q
e
e
) = 0
B
e
e
+K(
e
q
e
) =
e
55
Theorem 1 (De Luca, 1998) Robots having mixed rigid/elastic joints can be input
output decoupled (with y = q) and exactly linearized by static state feedback if and
only if there is complete inertial separation in the structure, i.e.
1. M
re
0
2. M
rr
= M
rr
(q
r
), M
ee
= M
ee
(q
e
)
The resulting closedloop linear system is in the form q
r
= a
r
, q
[4]
e
= a
e
Under the hypotheses of the Theorem, the feedback linearization control law is
r
= M
rr
(q
r
)a
r
+n
r
(q, q
r
)
e
= BK
1
_
M
ee
(q
e
)a
e
+c
e
(q, q, q
e
, q
[3]
e
)
_
where c
e
() := 2
M
ee
(q
e
)q
[3]
e
+(
M
ee
(q
e
) +K
e
) q
e
+ n
e
(q, q
e
) and
q
e
= M
1
ee
(q
e
) (K
e
(
e
q
e
) n
e
(q, q
e
))
q
[3]
e
= M
1
ee
(q
e
)
_
K
e
(
e
q
e
)
M
ee
(q
e
) q
e
n
e
(q, q
e
)
_
56
Theorem 2 (De Luca, 1998) When Theorem 1 cannot be applied, robots having
mixed rigid/elastic joints can always be inputoutput decoupled (with y = q) and
exactly linearized by a dynamic state feedback law of order m = 2R. The resulting
closedloop linear system is in the form q
[4]
r
= a
r
, q
[4]
e
= a
e
The following linear dynamic compensator, with state = (
T
r
T
r
)
T
IR
2R
B
r
r
+K
r
(
r
q
r
) =
re
r
= K
r
(
r
q
r
)
having arbitrary (diagonal) B
r
> 0, K
r
> 0 and new input
re
IR
R
, extends the
original mixed rigid/elastic dynamics to the same structure with all elastic joints
_
M
rr
(q) M
re
(q)
M
T
re
(q) M
ee
(q)
__
q
r
q
e
_
+
_
n
r
(q, q)
n
e
(q, q)
_
+
_
K
r
(q
r
r
)
K
e
(q
e
e
)
_
=
_
0
0
_
_
B
r
0
0 B
e
__
e
_
+
_
K
r
(
r
q
r
)
K
e
(
e
q
e
)
_
=
_
re
e
_
feedback linearizable by a static feedback from the extended state . . .
57
A more complete dynamic model of EJ robots
what happens if we remove the simplifying assumption A3)?
for a planar 2R EJ robot, the complete angular kinetic energy of the motors is
T
m1
=
1
2
J
m1
r
2
1
2
1
T
m2
=
1
2
J
m2
( q
1
+r
2
2
)
2
with no changes at base motor and new terms for elbow motor; as a result
T
m
=
1
2
( q
T
T
)
_
_
_
_
_
_
J
m2
0 0 J
m2
r
2
0 0 0 0
0 0 J
m1
r
2
1
0
J
m2
r
2
0 0 J
m2
r
2
2
_
_
_
_
_
_
_
q
_
=
1
2
( q
T
T
)
_
_
_
J
m2
0
0 0
S
S
T
B
_
_
_
_
q
_
the blue terms contribute to M(q) (the diagonal 0 should not worry here!)
58
for NR planar EJ robots, we obtain
M(q) q +S
+K( q) =
link equation
motor equation
with the strictly upper triangular matrix S representing inertial couplings between
motor and link accelerations
in general, a nonconstant matrix S may arise, see (De Luca, Tomei, 1996)
S(q) =
_
_
0 S
12
(q
1
) S
13
(q
1
,q
2
) S
1N
(q
1
,...,q
N1
)
0 0 S
23
(q
2
) S
2N
(q
2
,...,q
N1
)
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
0 0 0 S
N1,N
(q
N1
)
0 0 0 0
_
_
with new associated centrifugal/Coriolis terms in both link and motor equations
59
Controloriented remarks
specic kinematic structures with elastic joints (single link, polar 2R, PRP, . . . )
have S 0, so that the reduced model is also complete and feedback linearizable
for the inverse dynamics solution, see (De Luca, Book, 2008)
as soon as S ,= 0, exact linearization and inputoutput decoupling both fail if
we rely on the use of a nonlinear but static state feedback structure
in order to mimic a generalized computed torque approach (linearization and
decoupling for tracking tasks), we need a dynamic state feedback controller
= (x, ) +(x, )v
= (x, ) +(x, )v
with robot state x = (q, , q,
) IR
4N
, dynamic compensator state IR
m
(yet to be dened), and new control input v IR
N
60
A control extension Dynamic feedback linearization of EJ robots
a threestep design that achieves full linearization and inputoutput decoupling,
incrementally building the compensator dynamics through the solution of two
intermediate subproblems DFL algorithm in (De Luca, Lucibello, 1998)
presented for constant S ,= 0, works also for S(q)
transformation of the dynamic equations in statespace format is not needed
collapses in the usual linearizing control by static state feedback when S = 0
can be applied also to the complete model of robots with joints of mixed type
some rigid, other elastic, see (De Luca, Farina, 2004)
61
Step 1: IO decoupling w.r.t.
apply the static control law
= Bu +S
T
q +K( q)
or, eliminating link acceleration q (and dropping dependencies)
=
_
J S
T
M
1
S
_
u S
T
M
1
_
C q +g +K(q )
_
+K( q)
the resulting system is
= u
M(q) q = . . . (2N dynamics unobservable from )
62
Step 2: IO decoupling w.r.t. f
dene as output the generalized force
f = M(q) q +C(q, q) q +g(q) +Kq
the link equation, after Step 1, is rewritten as f(q, q, q) +Su K = 0
dynamic extension: add 2(i 1) integrators on input u
i
(i = 1, . . . , N) so as
to avoid successive input dierentiation
63
dierentiate 2i times the component f
i
(i = 1, . . . , N) and apply a linear static
control law (depending on K and S)
w = F
w
+G
w
w
so that the resulting inputoutput system is
d
2i
f
i
dt
2i
= w
i
i = 1, . . . , N
64
Step 3: IO decoupling w.r.t. q
dynamic balancing: add 2(N i) integrators on input w
i
(i = 1, . . . , N) so
as to avoid successive input dierentiation
dierentiate 2(Ni) times the ith inputoutput equation (i = 1, . . . , N) after
Step 2, thus obtaining
d
2N
f
dt
2N
=
d
2N
dt
2N
(M(q) q +C(q, q) q +g(q) +Kq) = v
65
apply the nonlinear static control law (globally dened)
v = M(q)v + n(q, q, . . . , q
[2N+1]
) +g
[2N]
(q) +Kq
[2N]
where
n =
2N
k=1
_
2N
k
_
M
[k]
(q)q
[2(N+1)k]
+
2N
k=0
_
2N
k
_
C
[k]
(q, q)q
[2N+1k]
the nal resulting system is fully linearized and decoupled
q
[2(N+1)]
= v
66
Comments on the DFL algorithm
using recursion, the output q and all its derivatives (linearizing coordinates) can
be expressed in terms of the robot + compensator states
[ q q q q
[3]
q
[2N+1]
] = T(q, , q,
, , )
the overall nonlinear dynamic feedback for the original torque
= (q, , q,
, , v)
=
_
_
= . . .
is of dimension m = 2N(N 1)
for a planar 2R EJ robot, m = 4 and nal system with 2 chains of 6 integrators
67
when some of the elements in the upper triangular part of S are zero (depending
on the arm kinematics), then the needed dynamic controller has a dimension m
that is lower than 2N(N 1) the dynamic extensions at steps 2 and 3 are
required only for some joints
for trajectory tracking purposes, given a (suciently smooth) q
d
(t) C
2(N+1)
,
the tracking error e
i
= q
di
q
i
on each channel is exponentially stabilized by
v
i
= q
[2(n+1)]
di
+
2N+1
j=0
K
ji
_
q
[j]
di
q
[j]
i
_
i = 1, . . . , N
where K
0i
, . . . , K
2N+1,i
are the coecients of a desired Hurwitz polynomial
68
Final remarks
for the complete dynamic model of EJ robots, all proposed control laws for
regulation tasks are still valid
under the same conditions, using the same Lyapunov candidates in the proof,
with a more complex nal LaSalle analysis
addition of viscous friction terms on the lhs of the link (D
q
q) and the mo
tor (D