Inverse dynamics control
given the robot dynamic model
!
B(q)˙ ̇ q + n(q, ˙ q ) = u
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)
!
c(q, ˙ q ) + g(q) + friction model
Robotics 2 2
. .
In practice ...
" 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, …)
a number of differences from the nominal condition
Robotics 2 3
different possible implementations depending on amount of computational load share
! OFF LINE ( open loop)
! ON LINE ( closed loop)
Introducing feedback
!
ˆ u d = ˆ B (qd)˙ ̇ q d + ˆ n (qd, ˙ q d) with estimates of terms (or coefficients) in the dynamic model
note: ud can be computed off line [e.g., by NE!(qd,qd,qd)]
feedback is introduced to make the control scheme more robust
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 4
. ..
!
ˆ B , ˆ n
ˆ ˆ
A series of trajectory controllers
1. inverse dynamics compensation + PD
2. inverse dynamics compensation + variable PD
3. feedback linearization + PD + ffw = “COMPUTED TORQUE”
4. feedback linearization + PID + ffw
!
u = ˆ u d + KP qd "q( ) + KD˙ q d " ˙ q ( )
!
u = ˆ u d + ˆ B (qd) KP qd "q( ) + KD˙ q d " ˙ q ( )[ ]
!
u = ˆ B (q) ˙ ̇ q d + KP qd "q( ) + KD˙ q d " ˙ q ( )[ ] + ˆ n (q, ˙ q )
!
u = ˆ B (q) ˙ ̇ q d + KP qd "q( ) + KD˙ q d " ˙ q ( ) + KI qd "q( )dt#[ ] + ˆ n (q, ˙ q )
the most robust to uncertainties, but also the most complex to implement in real time
only local stabilization of
trajectory error
e(t)=qd(t)-q(t)
Robotics 2 5
Feedback linearization control
+
!
"_
!
˙ q
!
˙ ̇ q
!
n(q, ˙ q )
+
!
ˆ n (q, ˙ q )
+
u
KD
KP
+
ROBOT (or its DYNAMIC MODEL)
in nominal conditions (B=B, n=n)
!
B(q)˙ ̇ q + n(q, ˙ q ) = u = B(q)a + n(q, ˙ q )
nonlinear robot dynamics nonlinear control law
!
˙ ̇ q = a
!
a = ˙ ̇ q d + KD˙ q d " ˙ q ( ) + KP qd "q( )
a
!
" q
!
˙ ̇ q d + KD˙ q d
+ KPqd
!
ˆ B (q)
!
B"1(q)_
linear and decoupled
system global asymptotic stabilization
symmetric and positive definite matrices KP, KD
Robotics 2 6
ˆ ˆ
Interpretation in the linear domain
!
"
!
˙ q
!
˙ ̇ q
!
˙ ̇ q d + KD˙ q d + KPqd
KD
KP
- +
under feedback linearization control, the robot has a dynamic behavior that is invariant, linear and decoupled in its whole workspace ( )
diagonal, > 0
!
" q, ˙ q
each joint coordinate qi evolves independently from the others, forced by ai
a =
!
" q
error transients ei = qdi-qi → 0 exponentially (prescribed by choice of KDi, KPi)
linearity
decoupling
a unitary mass (m=1) in the joint space !!
Robotics 2 7
!
˙ ̇ e + KD˙ e + KP e =0 " ˙ ̇ e i + KDi
˙ e i + KPiei =0
" desired joint trajectory can be generated from Cartesian data
" real-time computation by Newton-Euler algo: uFL=NE!(q,q,a)!! " simulation of feedback linearization control
Remarks
!
˙ ̇ p d(t),pd(0), ˙ p d(0)
!
˙ ̇ q d(t) = J"1(q) ˙ ̇ p d(t) " ˙ J (q)˙ q [ ]
!
qd(0) = f "1 pd(0)( )˙ q d(0) = J"1 qd(0)( )˙ p d(0)
!
" qd(t)
!
˙ q d(0)
!
˙ q d(t)
feedback linearization
robot qd(t)
!
˙ q
!
q
estimated parameters
!
ˆ "
true 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
!
ˆ " ="
qd(t), qd(t), .
..
!
qd(0)
!
˙ ̇ q d(t)
Robotics 2 8
. ^
Further comments
" choice of the diagonal elements of KP , KD (and KI) " for shaping the error transients, with an eye to motor saturations...
" parametric identification " 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 " 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, ...
critically damped transient e(t)=qd(t)-q(t)
t
e(0)
Robotics 2 9
Other example of feedback linearization " dynamic model of robots with elastic joints
" ! = motor position (after reduction gears) " q = link position " Bm = diagonal matrix (>0) of the inertia of the (balanced) motors " K = diagonal matrix (>0) of (finite) stiffness of the joints
" is there a control law that exactly linearizes via feedback?
2N generalized coordinates
!
u ="(#,q, ˙ # , ˙ q ) + $(#,q, ˙ # , ˙ q )a
!
B(q)˙ ̇ q + c(q, ˙ q ) +g(q) +K(q"#) = 0
Bm˙ ̇ # + K(# "q) = u
(1)
(2)
4N state variables
hint: differentiate (1) w.r.t. time until motor acceleration appears; substitute this from (2); choose u so as to cancel all nonlinearities …
YES and it yields
!
d4qi
dt4 = ai i =1,...,Nlinear and decoupled system:
N chains of 4 integrators (to be stabilized by linear
control desing)
Robotics 2 10
Alternative global trajectory controller
" 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
" 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...
!
u = B(q)˙ ̇ q d + S(q, ˙ q )˙ q d + g(q) + FV˙ q d + KPe + KD
˙ e
symmetric and positive definite matrices
SPECIAL factorization such that B-2S is skew-symmetric .
.
Robotics 2 11
. ..
Analysis of asymptotic stability of the trajectory error - 1
" Lyapunov candidate and its time derivative
!
B(q)˙ ̇ q + S(q, ˙ q )˙ q + g(q) + FV˙ q = u
!
u = B(q)˙ ̇ q d + S(q, ˙ q )˙ q d + g(q) + FV˙ q d + KPe + KD
˙ e robot dynamics (including friction)
control law
!
V = 12
˙ e TB(q) ˙ e + 12 eTKPe " 0
!
˙ V = 12
˙ e T ˙ B (q) ˙ e + ˙ e TB(q)˙ ̇ e + eTKP˙ e
" the closed-loop system equations yield
!
B(q)˙ ̇ e = "S q, ˙ q ( ) ˙ e "KPe " (KD + FV)˙ e " substituting and using the skew-symmetric property
!
˙ V = " ˙ e T KD + FV( ) ˙ e # 0" direct application of LaSalle is not allowed, since the system is
time-varying (because of qd(t))
!
˙ V = 0 " ˙ e = 0
!
q = qd(t) - e, ˙ q = ˙ q d(t) - ˙ e " V = V(e, ˙ e ,t) = V(x,t)
Robotics 2 12
⇒
error state
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
... is this bounded?
" from i) + ii), V is bounded
!
˙ ̇ e = "B-1(q) S q, ˙ q ( ) ˙ e +KP e + (KD+FV) ˙ e ( )
" assume that the desired trajectory has bounded velocity qd
!
˙ ̇ V = "2 ˙ e T KD +FV( ) ˙ ̇ e
!
t"#lim ˙ V = 0
bounded
Robotics 2 13
⇒
.
!
e and ˙ e are bounded. ⇒ q is
bounded
.
" using the following two properties of dynamic model terms
then also e will be bounded (in norm) since
!
0 <m" B-1(q) "M<#
!
S(q, ˙ q ) " #S ˙ q ..
bounded bounded bounded in norm by M
bounded in norm by
⇒
!
"S ˙ q bounded
Analysis of asymptotic stability of the trajectory error – end of proof
" we can now conclude by proceeding as in LaSalle Theorem
!
B(q)˙ ̇ e = "KPe
!
" ˙ ̇ e = 0 # e = 0
!
e, ˙ e ( ) = 0,0( )is the largest
invariant set in V=0 .
Robotics 2 14
!
˙ V = 0 " ˙ e = 0" the closed-loop dynamics in this situation is
(global) asymptotic tracking will be achieved
Regulation as a special case
" what happens to the control laws designed for trajectory tracking when qd is constant? are there simplifications?
" feedback linearization
" no special simplifications
" however, this is a solution to the regulation problem with exponential stability (and decoupled transients at each joint!)
" alternative global controller
" we recover the PD + gravity cancellation control law!!
!
u = KP(qd "q) "KD˙ q + g(q)
!
u = B(q) KP qd "q( ) "KD˙ q [ ] + c(q, ˙ q ) + g(q)
Robotics 2 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
Robotics 2 17
!
w(s)= y(s)yd(s)
= p(s)c(s)1+p(s)c(s)
!
uk(s) = " u k(s)+ vk(s)=c(s)ek(s)+ vk(s)
“plug-in” signal
!
yk(s) = w(s)yd(s)+ p(s)1+p(s)c(s)
vk(s)
closed-loop system without learning (c(s) is, e.g., a PD control law)
control law at iteration k
system output at iteration k
p(s) c(s)
Update law and convergence
" the update of the feedforward term is designed as
Robotics 2 18
!
vk+1(s) ="(s) # u k(s) + $(s)vk(s)with ! and " suitable filters
(also non-causal, of the FIR type)
recursive expression of feedforward term
recursive expression of error e=yd-y
!
vk+1(s) ="(s)c(s)
1+p(s)c(s)yd(s) + #(s) $"(s)w(s)( )vk(s)
!
ek+1(s) =1- "(s)
1+p(s)c(s)yd(s) + "(s) #$(s)w(s)( )ek(s)
" if a contraction condition can be enforced
then convergence is obtained for k→"
!
"(s) #$(s)w(s) < 1
!
v"(s) =yd(s)p(s)
#(s)w(s)1$%(s) +#(s)w(s)
!
e"(s) =yd(s)
1+p(s)c(s)1#$(s)
1#$(s) +%(s)w(s)
(for all s=j# frequencies such that ...)
Update law and convergence (cont)
" if the choice "=1 allows to satisfy the contraction condition, then convergence to zero tracking error is obtained
and the inverse dynamics command has been learned
Robotics 2 19
" 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)
!
v"(s) =yd(s)p(s)
!
e"(s) = 0
Application to robots
" for N-dof robots modeled as
we choose as (initial = pre-learning) control law
and design the learning filters (at each joint) using the linear approximation
Robotics 2 20
!
Bm + B(q)[ ]˙ ̇ q + FV + S(q, ˙ q )[ ]˙ q + g(q) = u
!
" u (t) = KP(qd #q) #KD (˙ q d # ˙ q ) + ˆ g (q)
!
wi(s) = qi(s)qd,i(s)
= KDi s +KPiˆ B mi s
2 + ( ˆ F vi +KDi)s +KPi
i=1,…,N
" initialization of feedforward uses the best estimates
or simply v1 = 0 (in the worst case), at first trial k=1
!
v1 = ˆ B m + ˆ B (qd)[ ]˙ ̇ q d + ˆ F V + ˆ S (qd, ˙ q d)[ ]˙ q d + ˆ g (qd)
Experimental set-up " joints 2 and 3 of 6R MIMO CRF robot prototype @DIS
Robotics 2 21
50°/s 160°
desired velocity/position for both joints
$90% gravity balanced
through springs
Harmonic Drives transmissions
with ratio 160:1
resolvers and tachometers
DC motors with current amplifiers
DSP Tc = 400µs D/A = 12 bit
R/D = 16 bit/2$%A/D = 11 bit/(rad/s)%
high level of dry friction
De Luca, Paesano, Ulivi: IEEE Trans Ind Elect, 1992