Robotics 2

Trajectory Tracking Control

Prof. Alessandro De Luca

Inverse dynamics control

given the robot dynamic model

+ n(q, q ) = u
B(q)q
c(q, q ) + g(q) + friction model

and a twice-differentiable desired trajectory for t[0,T]

qd (t) " q d (t), q d (t)
!
!
applying the feedforward torque in nominal conditions
!

ud = B(qd )q d + n(qd , q d )

yields exact reproduction of the desired motion

.
.
! provided that q(0)=qd(0), q(0)=qd(0) (initial matched state)
Robotics 2

In practice ...
a number of differences from the nominal condition
"
"
"

"
"

initial state is not matched to the desired trajectory qd(t)

disturbances on the actuators, truncation errors on data,
inaccurate knowledge of robot dynamic parameters (link
masses, inertias, center of mass positions)
unknown value of the carried payload
presence of unmodeled dynamics (complex friction
phenomena, transmission elasticity, )

Robotics 2

Introducing feedback
u d = B (qd )q d + n (qd , q d )

with B , n estimates of terms

(or coefficients) in the dynamic model

. ..

d can be computed off line [e.g., by NE!(qd,qd,qd)]

note: u

feedback is introduced to make the control scheme more robust

different possible implementations depending on
amount of computational load share
! OFF LINE (

open loop)

! ON LINE (

closed loop)

two-step control design:

1. compensation (feedforward) or cancellation (feedback) of nonlinearities
2. synthesis of a linear control law stabilizing the trajectory error to zero
Robotics 2

A series of trajectory controllers

1. inverse dynamics compensation + PD

u = u d + K P (qd " q) + K D (q d " q )

2. inverse dynamics compensation + variable PD

u = u d + B (qd )[K P (qd " q) + K D (q d " q )]

only local
stabilization of
trajectory
error
e(t)=qd(t)-q(t)

3. feedback linearization + PD + ffw = COMPUTED TORQUE

u = B (q)[q d + K P (qd " q) + K D (q d " q )] + n (q, q )

4. feedback linearization + PID + ffw

u = B (q) q d + K P (qd " q) + K D (q d " q ) + K I # (qd " q)dt + n (q, q )

the most robust to uncertainties, but also the most complex to implement in real time
Robotics 2

Feedback linearization control

ROBOT (or its DYNAMIC MODEL)

q d + K Dq d

a
B (q)

+ K Pqd

B (q)
"1

+
_

"

"

n(q, q )
symmetric and
!
positive definite
matrices KP, KD

in nominal
conditions

(B=B,
n=n)

!
K!
D

n (q, q )

KP

+ n(q,!
B(q)q
q ) = u = B(q)a + n(q, q )

nonlinear robot dynamics nonlinear control law

global asymptotic a = q
d + K D (q d " q ) + K P (qd " q)
stabilization

Robotics 2

= a
q
linear and
decoupled
system
6

Interpretation in the linear domain

d + K Dq d + K Pqd
q

a=q

"

+
-

KD

diagonal, > 0

"

KP

under feedback linearization control, !

the robot has!
a dynamic behavior that is
)
invariant, linear and decoupled in its whole workspace ( " q, q
a unitary mass (m=1) in the joint space !!

linearity

error transients ei = qdi-qi 0 exponentially (prescribed by choice of KDi, KPi)

decoupling

each joint coordinate qi evolves independently from the others, forced by ai

Robotics 2

+ K D e + K P e = 0 " e
i + K Di e i + K Pi e i = 0
e

Remarks
"

desired joint trajectory can be generated from Cartesian data

d (t), p d (0), p d (0)
"1
p
q
(0)
=
f
p
(0)
(
)

qd (0)

d (t)
q
!

"

"

"

qd (0)

q d (t)

"

qd(t)

q d (0) = J"1 (qd (0))p d (0)

q d (t) = J"1 (q) p

d (t) " J(q)q

^
.
!
real-time
computation
Newton-Euler algo: uFL=NE!(q,q,a)!!
!
simulation
of feedback!linearization control
!
!

.
qd(t), qd(t),
..
qd(t)

!
by

true parameters "

feedback
linearization

q
q

robot

estimated parameters "

Note: there is no sense in simulating the ideal case (" = " ), since the robot behavior
!
will be identical to the linear and decoupled case of stabilized double integrators
Robotics 2

Further comments
"

choice of the diagonal elements of KP , KD (and KI)

"

for shaping the error transients, with an eye to motor saturations...

e(t)=qd(t)-q(t)

"

parametric identification
"

"

critically damped transient

to be done in advance, using the property of linearity in the dynamic

coefficients of the robot dynamic model

choice of the sampling time of a digital implementation

"

"

e(0)

compromise between computational time and tracking accuracy,

typically Tc = 0.5 10 ms

exact linearization by (state) feedback is a general technique

of nonlinear control theory
can be used for robots with elastic joints, wheeled mobile robots, ...
" non-robotics applications: satellites, induction motors, helicopters, ...
"

Robotics 2

Other example of feedback linearization

"

dynamic model of robots with elastic joints

"
"
"
"

! = motor position (after reduction gears)

2N generalized
coordinates
q = link position
Bm = diagonal matrix (>0) of the inertia of the (balanced) motors
K = diagonal matrix (>0) of (finite) stiffness of the joints

4N state
variables

+ c(q, q ) + g(q) +K(q " # ) = 0

B(q)q
B # + K(# " q) = u
m

(1)
(2)

is there a control law that exactly linearizes via feedback?

u = " (#,q,#, q ) + $ (#,q,#, q )a
linear and decoupled system:
!
d4qi
N chains of 4 integrators
YES and it yields
= ai
i = 1,...,N
4
"

dt

(to be stabilized by linear

control desing)

hint: differentiate (1) w.r.t. time until motor acceleration appears;

substitute this from (2); choose u so as to cancel all nonlinearities
!

Robotics 2

10

Alternative global trajectory controller

u = B(q)q d + S(q, q )q d + g(q) + FVq d + K Pe + K De
SPECIAL
factorization such that
.
B-2S is skew-symmetric

"
"
"

global stabilization of (e,e) to zero (i.e., trajectory tracking)

asymptotic stability proven by Lyapunov+Barbalat+LaSalle
does not produce a complete cancellation of nonlinearities
"

"

"
"

symmetric and
positive definite matrices

.
..
the q and q that appear linearly in the model are evaluated on the
desired trajectory

does not induce a linear and decoupled behavior of the

trajectory error e(t)=qd(t)-q(t) in the closed-loop system
lends itself more easily to an adaptive version
cannot be directly computed by the standard NE algorithm...

Robotics 2

11

Analysis of asymptotic stability

of the trajectory error - 1

B(q)q + S(q, q )q + g(q) + FVq = u robot dynamics (including friction)

u = B(q)q d + S(q, q )q d + g(q) + FVq d + K Pe + K De
control law
Lyapunov candidate and its time derivative
+ e TK Pe
V = 12 e TB(q)e + 12 e TK Pe " 0 V = 12 e TB (q)e + e TB(q)e
!
" the closed-loop system equations yield
"

= "S(q, q )e " K Pe " (K D + FV )e

B(q)e

"

substituting and using the!skew-symmetric property

V = " e T (K D + FV )e # 0

"

V = 0 " e = 0

direct
! application of LaSalle is not allowed, since the system is
time-varying (because of qd(t))

!
Robotics 2

q = qd (t) - e, q = q d (t) - e " V = V(e, e ,t) = V(x,t)

!
error state

12

Analysis of asymptotic stability

of the trajectory error - 2
.

"

since i) V is lower bounded and ii) V ! 0, we can check condition iii) in

order to apply Barbalat lemma

= "2 e T (K + F ) e

V
D
V

... is this bounded?

e and e are bounded

"

from i) + ii), V is bounded

"

assume that the desired trajectory has bounded velocity qd

.
q is
bounded

!" using the following two properties of dynamic model terms

!

0 < m " B -1 (q) " M < #

..

S(q, q ) " # S q

then also e will be bounded (in norm) since

!

= "B -1 (q) (S(q, q )!e + K P e + (K D+ FV ) e )

e
bounded
in norm by M

bounded bounded
bounded

in norm by " S q

Robotics 2

bounded

bounded

!
!

V = 0
tlim
"#

13

Analysis of asymptotic stability

of the trajectory error end of proof

"

we can now conclude by proceeding as in LaSalle Theorem

V = 0 " e = 0
"

the closed-loop dynamics in this situation is

= "K Pe
B(q)e
!
= 0 # e = 0
" e

(e, e ) = (0,0)

is the largest.
invariant set in V=0

!
!

Robotics 2

(global) asymptotic tracking

will be achieved

14

Regulation as a special case

"

"

what happens to the control laws designed for trajectory

tracking when qd is constant? are there simplifications?
feedback linearization

u = B(q)[K P (qd " q) " K Dq ] + c(q, q ) + g(q)

"
"

no special simplifications

however, this is a solution to the regulation problem with

exponential stability (and decoupled transients at each joint!)

!alternative global controller

"

u = K P (qd " q) " K Dq + g(q)

"

Robotics 2

we recover the PD + gravity cancellation control law!!

15

Trajectory execution without a model

"

"

is it possible to accurately reproduce a desired smooth

joint-space reference trajectory with reduced or no
information on the robot dynamic model?
this is feasible in case of repetitive motion tasks over a
finite interval of time
"

"

"

"

trials are performed iteratively, storing the trajectory error

information of the current execution [k-th iteration] and
processing it off line before the next trial [(k+1)-iteration] starts
the robot should be reinitialized in the same initial position at the
beginning of each trial
the control law is made of a non-model based part (typically, a
decentralized PD law) + a time-varying feedforward which is
updated at every trial

this scheme is called iterative trajectory learning

Robotics 2

16

Scheme of iterative trajectory learning

"

control design can be illustrated on a SISO linear system

in the Laplace domain

plug-in
signal
c(s)

w(s) =

y(s)
p(s)c(s)
=
y d (s) 1+ p(s)c(s)

p(s)

closed-loop system without learning

(c(s) is, e.g., a PD control law)

uk (s) = u"k (s) + v k (s) = c(s)e k (s) + v k (s)

!

y k (s) = w(s) y d (s) +

Robotics 2

p(s)
v k (s)
1+ p(s)c(s)

control law at iteration k

system output at iteration k
17

Update law and convergence

"

the update of the feedforward term is designed as

v k+1 (s) = " (s) u#k (s) + $ (s) v k (s)

with ! and " suitable filters

(also non-causal, of the FIR type)

recursive expression
of feedforward term

" (s)c(s)
v k+1 (s) =
y (s) + (# (s) $ " (s)w(s) ) v k (s)
1+ p(s)c(s) d

recursive expression
of error e=yd-y
!

1 - " (s)
e k+1 (s) =
y (s) + (" (s) # $ (s)w(s) ) e k (s)
1+ p(s)c(s) d

"

if a contraction condition can be enforced

(for all s=j# frequencies such that ...)
" (s) #!$ (s) w(s) < 1
then convergence is obtained for k"

v " (s) =

Robotics 2

y d (s)
# (s)w(s)
p(s) 1 $ % (s) + # (s)w(s)

e" (s) =

y d (s)
1 # $ (s)
1+ p(s)c(s) 1 # $ (s) + % (s)w(s)
18

Update law and convergence

"

(cont)

if the choice "=1 allows to satisfy the contraction condition,

then convergence to zero tracking error is obtained
e" (s) = 0

and the inverse dynamics command has been learned

!
"
"

"

v " (s) =

y d (s)
p(s)

in particular, for !(s)=1/w(s) convergence is in 1 iteration!

the use of filter "(s)#1 allows to obtain convergence (with
!
residual tracking
error) despite the presence of unmodeled
high-frequency dynamics
the two filters can be designed from very poor information
on system dynamics, using classic tools (e.g., Nyquist plots)

Robotics 2

19

Application to robots
"

for N-dof robots modeled as

[Bm + B(q)]q + [FV + S(q, q )]q + g(q) = u

we choose as (initial = pre-learning) control law

u"(t) = K P (qd # q) # K D (q d # q ) + g (q)
!and design the learning filters (at each joint) using
the linear approximation
K Di s + K Pi
! w (s) = qi(s) =
i
qd,i (s) B mi s 2 + (Fvi + K Di )s + K Pi
"

!
Robotics 2

i =1,,N

initialization of feedforward uses the best estimates

v1 = B m + B (qd ) q d + FV + S (qd , q d ) q d + g (qd )

] [

or simply v1 = 0 (in the worst case), at first trial k=1

20

Experimental set-up
"

joints 2 and 3 of 6R MIMO CRF robot prototype @DIS

$90% gravity
balanced
through springs

50/s

160

high level of
dry friction
Harmonic Drives
transmissions
with ratio 160:1

desired velocity/position for both joints

DC motors with
current amplifiers

DSP Tc = 400s
D/A = 12 bit
R/D = 16 bit/2$%
A/D = 11 bit/(rad/s)%

Robotics 2

resolvers and
tachometers

De Luca, Paesano, Ulivi: IEEE Trans Ind Elect, 1992

21

Experimental results

Robotics 2

feedforward vk for k = 3, 6, 12 (zero at k=1)

feedback uk for k = 1, 3, 6, 12

joint 3

joint 2

error for k = 1, 3, 6, 12

22