modelingandcontrol robotswithelasticjointstypicalfrequencyresponseofasingleelasticjoint 10-1 10...
TRANSCRIPT
Scuola di Dottorato CIRA
Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione
Bertinoro (FC), 14–16 Luglio 2003
Robots with Elastic Joints:
Modeling and Control
Alessandro De Luca
Dipartimento di Informatica e Sistemistica (DIS)
Universita di Roma “La Sapienza”
Outline
• Motivation for considering joint elasticity
• Dynamic modeling of EJ robots
• Formulation of control problems and sensing requirements
• Controllers for regulation tasks
• Controllers for trajectory tracking tasks
• Some modeling and control extensions
• Open research issues
• References
2
Motivation
• in industrial robots, the presence of transmission elements such as
– harmonic drives and transmission belts (typically, Scara arms)
– long shafts (e.g., last 3-dofs of Puma)
introduce elasticity effects between actuating inputs and driven outputs
• the desire to increase mechanical compliance while keeping cartesian accuracy
leads to the use of elastic transmissions in research robots for human cooperation
– actuator relocation plus cables and pulleys
– harmonic drives and lightweight (but rigid) link design
– redundant (macro-mini or parallel) actuation
• these phenomena are captured by modeling the elasticity at the robot joints
• neglected joint elasticity limits dynamic performance of controllers (vibrations,
poor tracking, chattering during environment contact)
3
Robots with joint elasticity — DEXTER
• 8R-arm by Scienzia Machinale, available at DIS
• DC motors with reductions for joints 1,2
• DC motors with reductions, steel cables and
pulleys for joints 3–8 (all located in link 2)
• encoders on motor sides4
Robots with joint elasticity — DLR III
• 7R-arm by DLR Institute of Robotics and Mechatronics• DC brushless motors with harmonic drives• encoders on motor and link sides, joint torque sensors• weight 18 kg, payload 7 kg!
5
Robots with joint elasticity — DECMMA
• 2R and 4R prototype arms by Stanford University Robotics Laboratory
• parallel macro (at base, with elastic coupling)–mini (at joints) actuation
6
Robots with joint elasticity — UB Hand
• dextrous hand mounted as end-effector of a Puma robot
• tendon-driven (static compliance in the grasp)
7
Joint elasticity in harmonic drives — industrial robots
• compact, in-line, high reduction (1:200), power efficient transmission element
• teflon teeth of flexspline introduce small angular displacement
8
Typical frequency response of a single elastic joint
10-1
100
-60
-40
-20
0
20Motor velocity output
Frequency (rad/sec)
Mag
nitu
de (
dB)
10-1
100
-100
-50
0
50
100
Frequency (rad/sec)
Pha
se (
deg)
10-1
100
-40
-20
0
20Link velocity output
Frequency (rad/sec)
Mag
nitu
de (
dB)
10-1
100
-300
-200
-100
0
Frequency (rad/sec)P
hase
(de
g)
• antiresonance/resonance behavior on motor velocity output, pure resonance onlink velocity output (weak or no zeros)
9
Dynamic modeling
• open-chain robot with N (rotary or prismatic) elastic joints and N rigid links,driven by electrical actuators
• Lagrangian formulation using motor variables θ ∈ IRN (as reflected throughreduction ratios) and link variables q ∈ IRN as generalized coordinates
q2
Jm1
m2J
m1
2I
I1
r1 q1
r2
m2
r2m
θ1
2θ
θm1
m2θ
K2
1K
10
• standing assumptions
A1) small displacements at joints (linear elasticity domain)
A2) axis-balanced motors (i.e., center of mass of rotors on rotation axes)
• link kinetic energy and link potential energy due to gravity (including the mass
of each motor as additional mass of the carrying link)
T� =1
2qTM(q)q Ug = Ug(q)
with symmetric, positive definite inertia matrix M(q)
• potential energy due to joint elasticity
Ue =1
2(q − θ)TK(q − θ)
with diagonal, positive definite joint stiffness matrix K
11
• simplifying assumption (Spong, 1987)
A3) angular kinetic energy of each motor is due only to its own spinning
Tm =1
2θT Jθ
with diagonal, positive definite motor inertia matrix J (reflected through the
reduction ratios: Ji = Jmir2i , i = 1, . . . , N)
• system Lagrangian
L = T − U = (T� + Tm)− (Ug + Ue) = L(q, θ, q, θ)
12
Euler-Lagrange equations
• given the set of generalized coordinates p = (q, θ) ∈ IR2N , the Lagrangian L
satisfies the usual vector equation
d
dt
(∂L
∂p
)T
−(∂L
∂p
)T
= u
being u ∈ IR2N the non-conservative forces/torques performing work on p
• assuming no dissipative terms, since the motor torques τ ∈ IRN only perform
work on the motor variables θ we obtain
M(q)q + C(q, q)q + g(q) + K(q − θ) = 0
Jθ + K(θ − q) = τ
link equation
motor equation
with centrifugal/Coriolis terms C(q, q)q and gravity terms g(q) = (∂Ug/∂q)T
13
Model properties
• M(q)− C(q, q) is skew-symmetric
• nonlinear dynamic model, but linear in a set of coefficients a = (ar, aK, aJ)(including K and J)
Yr(q, q, q) ar + diag{q − θ} aK = 0
diag{θ} aJ + diag{θ − q} aK = τ
• for K → ∞ (rigid joints): θ → q and K(q − θ) → finite value, so that theequivalent rigid model is recovered (summing up link and motor equation)(
M(q) + J)q + C(q, q)q + g(q) = τ
• there exists a bound on the norm of the gravity gradient matrix∥∥∥∥∥∂g(q)∂q
∥∥∥∥∥ ≤ α ⇒ ‖g(q1)− g(q2)‖ ≤ α‖q1 − q2‖, ∀q1, q2 ∈ IRN
14
Control problems
• regulation to a constant equilibrium configuration (q, θ, q, θ) = (qd, θd,0,0)
– only the desired link position qd is assigned, while θd has to be determined
– qd may come from the kinematic inversion of a desired cartesian pose xd– direct kinematics of EJ robots is a function of link variables: x = kin(q)
• tracking of a sufficiently smooth trajectory q = qd(t)
– the associated motor trajectory θd(t) has to be determined
– again, qd(t) is uniquely associated to a desired cartesian trajectory xd(t)
• other relevant planning/control problems not considered here include
– rest-to-rest trajectory planning in given time T
– cartesian control (regulation or tracking directly defined in the task space)
– force/impedance/hybrid control of EJ robots in contact with the environment
15
Sensing requirements
• full state feedback requires sensing of four variables: q, θ (link/motor position)and q, θ (link/motor velocity)
• only motor variables (θ, θ) are available with standard sensing arrangements(encoder + tachometer on the motor axis)
• velocities also through numerical differentiation of high-resolution encoders
• more advanced systems have also measurements beyond elasticity of the joints(e.g., link position q and joint torque z = K(θ − q) sensors in DLR III arm)
16
Regulation— a simple linear example
• two elastically coupled masses (motor/link) driven on one side (Quanser LEJ)
• dynamic model
mq + k(q − θ) = 0 jθ + k(θ − q) = τ
and transfer function from force input τ to second mass position q
P (s) =q(s)
τ(s)=
k
mjs2 + (m + j)k· 1
s2
NOTE: unstable but controllable system (→ any pole assignment via full statefeedback) with no zeros!
17
Feedback strategies with reduced measurements
1) τ = u1 − (kP�q + kD�q) (link PD feedback)
W��(s) =q(s)
u1(s)=
k
mjs4 + (m + j)ks2 + kkD�s + kkP�
always unstable (spring damping/friction leads to small stability intervals)
2) τ = u2 − (kPmθ + kDmθ) (motor PD feedback)
Wmm(s) =k
mjs4 + mkDms3 + [m(k + kPm) + jk] s2 + kkDms + kkPm
asympotically stable for kPm > 0, kDm > 0 (Routh criterion) → as in rigid
systems!
18
3) τ = u3 − (kP�q + kDmθ) (link position and motor velocity feedback)
W�m(s) =k
mjs4 + mkDms3 + (m + j)ks2 + kkDms + kkP�
asympotically stable for 0 < kP� < k, kDm > 0 (limited proportional gain)
4) with τ = u4−(kPmθ+kD�q) (motor position and link velocity feedback) the
closed-loop system is always unstable
⇓
• caution must be used in dealing with different output measures
• generalization to a nonlinear multidimensional setting (under gravity) of the most
efficient scheme (motor PD feedback)
video
19
Regulation with motor PD + feedforward
• for regulation tasks, consider the control law
τ = KP (θd − θ)−KDθ + g(qd)
with symmetric (diagonal) KP > 0, KD > 0, and the motor reference position
θd := qd + K−1g(qd)
Theorem (Tomei, 1991) If
λmin(KE) := λmin
([K −K−K K + KP
])> α
then the closed-loop equilibrium state (qd, θd,0,0) is globally asymptotically stable
20
Lyapunov-based proof
• closed-loop equilibria (q = θ = q = θ = 0) satisfy
K(q − θ) + g(q) = 0
K(θ − q)−KP (θd − θ)− g(qd) = 0
• adding/subtracting K(θd − qd)− g(qd) (= 0, by definition of θd) yields
K(q − qd)−K(θ − θd) + g(q)− g(qd) = 0
−K(q − qd) + (KP + K)(θ − θd) = 0
or, in matrix form,
KE
[q − qdθ − θd
]=
[g(qd)− g(q)
0
]
21
• using the Theorem assumption, for all (q, θ) �= (qd, θd) we have∥∥∥∥∥KE
[q − qdθ − θd
]∥∥∥∥∥ ≥ λmin(KE)
∥∥∥∥∥[q − qdθ − θd
]∥∥∥∥∥> α
∥∥∥∥∥[q − qdθ − θd
]∥∥∥∥∥ ≥ α ‖q − qd‖
≥ ‖g(qd)− g(q)‖ =
∥∥∥∥∥[g(qd)− g(q)
0
]∥∥∥∥∥and hence (qd, θd) is the unique equilibrium configuration
• define the position-dependent energy function
P (q, θ) =1
2(q−θ)TK(q−θ)+
1
2(θd−θ)TKP (θd−θ)+Ug(q)−θTg(qd)
its gradient ∇P (q, θ) = 0 only at (qd, θd) (using the same above argument)
and ∇2P (qd, θd) > 0 ⇒ (qd, θd) is an absolute minimum for P (q, θ)
22
• the following is thus a candidate Lyapunov function
V (q, θ, q, θ) =1
2qTM(q)q +
1
2θT Jθ + P (q, θ)− P (qd, θd) ≥ 0
• its time derivative, evaluated along the closed-loop system trajectories, is
V = qTM(q)q + 12q
T M(q)q + θT Jθ +(q − θ
)TK (q − θ)
− θTKP (θd − θ) + qT(∂Ug(q)
∂q
)T− θT g(qd)
= qT(−C(q, q)q − g(q)−K(q − θ) + 1
2M(q)q + K(q − θ) + g(q))
+ θT (u−K(θ − q)−K(q − θ)−KP (θd − θ)− g(qd))
= θT(KP (θd − θ)−KDθ + g(qd)−KP (θd − θ)− g(qd)
)= − θTKDθ ≤ 0
where the skew-symmetry of M − 2C has been used
23
• since V = 0 ⇐⇒ θ = 0, the proof is completed using LaSalle Theorem
• substituting θ(t) ≡ 0 in the closed-loop equations yields
M(q)q + C(q, q)q + g(q) + Kq = Kθ = constant (∗)Kq = Kθ −KP (θd − θ)− g(qd) = constant (∗∗)
from (∗∗) it follows that q(t) ≡ 0, which in turn simplifies (∗) into
g(q) + K(q − θ) = 0 (∗ ∗ ∗)
• from the fist part of the proof, q = qd, θ = θd, q = θ = 0 is the unique
solution to (∗∗)–(∗ ∗ ∗) and thus the largest invariant set contained in the set
of states such that V = 0 ⇒ global asymptotic stability of the set point
24
Remarks on regulation control
• if stiffness K is large enough (non-pathological cases), the Theorem assumption
λmin(KE) > α can always be satisfied by increasing λmin(KP )
• in the presence of model uncertainties, the control law
τ = KP (θd − θ)−KDθ + g(qd) θd := qd + K−1g(qd)
provides asymptotic stability, but with a different (and still unique) equilibrium
configuration (q, θ)
• version with on-line gravity compensation (Zollo, De Luca, Siciliano, 2003)
τ = KP (θd − θ)−KDθ + g(θ)
where θ := θ −K−1g(qd) is a ‘biased’ motor position measurement
25
Trajectory tracking
• assuming that
– qd(t) ∈ C4 (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 closed-loop robot dynamics (bunch of input-output integrators)
• exponential stabilization of the tracking error is then performed on the linear
side of the transformed problem
⇓a generalized computed torque law
• original result (Spong, 1987), revisited without the need of state-space concepts
26
System inversion
• differentiate the link equation of the dynamic model (independent of input τ)
M(q)q + n(q, q) + K(q − θ) = 0 n(q, q) := C(q, q)q + g(q)
to obtain (notation: q[i] = diq/dti)
M(q)q[3] + M(q)q + n(q, q) + K(q − θ) = 0
still independent from τ
• differentiating once more (fourth derivative of q appears)
M(q)q[4] + 2M(q)q[3] + M(q)q + n(q, q) + K(q − θ) = 0
the input τ appears through θ and the motor equation
Jθ + K(θ − q) = τ
27
• substitution of θ gives
M(q)q[4] + c(q, q, q, q[3]) + KJ−1K(θ − q) = KJ−1τ
with
c(q, q, q, q[3]) := 2M(q)q[3] + (M(q) + K)q + n(q, q)
• the control law
τ = JK−1(M(q)a + c(q, q, q, q[3]) + KJ−1K(θ − q)
)leads to the closed-loop system
q[4] = a
N separate input-output chains of four integrators (linearization and decoupling)
28
• (q, q, q, q[3]) is an alternative globally defined 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 first differentiation 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 controller term c(q, q, q, q[3]) can be expressed in terms of (q, θ, q, θ),
with an efficient organization of computations
• the control law τ = τ(q, θ, q, θ, a) can be implemented as a nonlinear static
(instantaneous) feedback from the original state
29
Tracking error stabilization
• control design is completed on the linear side of the problem by choosing
a = q[4]d + K3(q
[3]d − q[3]) + K2(qd − q) + K1(qd − q) + K0(qd − q)
with q and q[3] expressed in terms of (q, θ, q, θ) and diagonal gain matrices
K0, . . . ,K3 chosen such that
s4 + K3is3 + K2is
2 + K1is + K0i i = 1, . . . , N
are Hurwitz polynomials
• the tracking errors ei = qdi − qi on each link coordinate satisfy
e[4]i + K3ie
[3]i + K2iei + K1iei + K0iei = 0
i.e., exponentially stable linear differential equations (with chosen eigenvalues)
30
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 flat output for EJ robots”
• a nominal feedforward torque can be computed off line
τd = Jθd + K(θd − qd)
where, using the previous developments,
θd = qd+K−1(M(qd)qd + n(qd, qd)) θd = K−1(M(qd)q
[4]d + c(qd, qd, qd, q
[3]d )
)
• a simpler tracking controller (of local validity around the reference trajectory) is
τ = τd + KP (θd − θ) + KD(θd − θ)
31
An extension of dynamic modeling
• what happens if we remove the simplifying assumption A3)?
• for a planar 2R EJ robot, the complete angular kinetic energy of the motors is
Tm1 =1
2Jm1r
21θ
21 Tm2 =
1
2Jm2(q1 + r2θ2)
2
with no changes at base motor and new terms for elbow motor; as a result
Tm = 12 [ qT θT ]
Jm2 0 0 Jm2r20 0 0 0
0 0 Jm1r21 0
Jm2r2 0 0 Jm2r22
[q
θ
]
= 12 [ qT θT ]
Jm2 0
0 0S
ST J
[q
θ
]
the blue terms contribute to M(q) (the diagonal 0 should not worry here!)
32
• for NR planar EJ robots, we obtain
M(q)q + Sθ + C(q, q)q + g(q) + K(q − θ) = 0
ST q + Jθ + K(θ − q) = τ
link equation
motor equation
with the strictly upper triangular matrix S representing inertial couplings between
motor and link accelerations
• in general, a non-constant matrix S may arise (see De Luca, Tomei, 1996)
S(q) =
0 S12(q1) S13(q1,q2) ··· S1N(q1,...,qN−1)
0 0 S23(q2) ··· S2N(q2,...,qN−1)... ... ... . . . ...
0 0 0 ··· SN−1,N(qN−1)
0 0 0 ··· 0
with new associated centrifugal/Coriolis terms in both link and motor equations
33
Control-oriented remarks
• specific kinematic structures with elastic joints (single link, polar 2R, PRP, . . . )
have S ≡ 0, so that the reduced model is also complete and feedback linearizable
• the presented controllers for regulation tasks work as such also for the complete
model (with a more complex LaSalle final analysis)
• as soon as S �= 0, exact linearization and input-output 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, θ) ∈ IR4N , dynamic compensator state ξ ∈ IRm
(yet to be defined), and new control input v ∈ IRN
34
A control extension — Dynamic feedback linearization of EJ robots
• a three-step design that achieves full linearization and input-output 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 state-space format is not needed
• collapses in the usual linearizing control by static state feedback when S = 0
• can be applied also to the case of joints of mixed type —some rigid, other elastic
(see De Luca, 1998)
35
∫∫
u
∫∫
unobservable
subsystem
⟩ θ
N x 2 states
Step 1: I-O decoupling w.r.t. θ
• apply the static control law
τ = Ju + ST q + K(θ − q)
or, eliminating link acceleration q (and dropping dependencies)
τ =(J − STM−1S
)u− STM−1
(Cq + g + K(q − θ)
)+ K(θ − q)
• the resulting system is
θ = uM(q)q = . . . (2N dynamics unobservable from θ)
36
⟩
∫∫u
w
N(N-1) states φ
Step 2: I-O decoupling w.r.t. f
• define 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 ui (i = 1, . . . , N) so as
to avoid successive input differentiation
37
∫∫
w
unobservable
subsystem
⟩ f∫∫
∫∫ ∫∫
∫∫
∫∫
N x (N+1) states
• differentiate 2i times the component fi (i = 1, . . . , N) and apply a linear static
control law (depending on K and S)
w = Fwφ + Gww
so that the resulting input-output system is
d2ifidt2i
= wi i = 1, . . . , N
38
⟩
∫∫
w
v
N(N-1) states ψ
Step 3: I-O decoupling w.r.t. q
• dynamic balancing: add 2(N − i) integrators on input wi (i = 1, . . . , N) so
as to avoid successive input differentiation
• differentiate 2(N−i) times the ith input-output equation (i = 1, . . . , N) after
Step 2, thus obtaining
d2Nf
dt2N=
d2N
dt2N(M(q)q + C(q, q)q + g(q) + Kq) = v
39
∫∫∫ ∫ ∫∫∫ ∫
∫∫∫ ∫
⟩
qv
N x 2(N+1) states
• apply the nonlinear static control law (globally defined)
v = M(q)v + n(q, q, . . . , q[2N+1]) + g[2N ](q) + Kq[2N ]
where
n =2N∑k=1
(2Nk
)M [k](q)q[2(N+1)−k] +
2N∑k=0
(2Nk
)C[k](q, q)q[2N+1−k]
• the final resulting system is fully linearized and decoupled
q[2(N+1)] = v
40
∫ ∫∫ ∫ ∫∫
∫ ∫∫ ∫ ∫∫
v1
v2
y = q 11
y = q 22
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 final system with 2 chains of 6 integrators
41
• for trajectory tracking purposes, given a (sufficiently smooth) qd(t) ∈ C2(N+1),
the tracking error ei = qdi − qi on each channel is exponentially stabilized by
vi = q[2(n+1)]di +
2N+1∑j=0
Kji
(q[j]di − q
[j]i
)i = 1, . . . , N
where K0i, . . . ,K2N+1,i are the coefficients of a desired Hurwitz polynomial
42
Open research issues
• kinetostatic calibration of EJ robots using only motor measurements
• unified dynamic identification of model parameters (including K and J)
• robust control for trajectory tracking in the presence of uncertainties
• adaptive control: available (and too complex) only for the reduced model with
S = 0 (Lozano, Brogliato, 1992)
• inclusion of spring damping (visco-elastic joints): static feedback linearization
becomes ill-conditioned (inclusion of viscous friction on motors and links is instead trivial)
• cartesian impedance control with proved stability
• fitting the results into parallel/redundant actuated devices with joint elasticity
• . . .
43
References
(De Luca, 1998) “Decoupling and feedback linearization of robots with mixed rigid/elastic joints,”
IJRNC, 8(11), 965–977
(De Luca, Lucibello, 1998) “A general algorithm for dynamic feedback linearization of robots with
elastic joints,” IEEE ICRA, 504–510
(De Luca, Tomei, 1996) “Elastic joints,” in Theory of Robot Control (Canudas de Wit, Siciliano,
Bastin Eds), Springer, 179–217
(Lozano, Brogliato, 1992) “Adaptive control of robot manipulators with flexible joints,” IEEE
TAC, 37(2), 174–181 (errata exists)
(Spong, 1987) “Modeling and control of elastic joint robots,” ASME JDSMC, 109, 310–319
(Tomei, 1991) “A simple PD controller for robots with elastic joints,” IEEE TAC, 36(10), 1208–
1213
(Zollo, De Luca, Siciliano, 2003) “A PD controller with on-line gravity compensation for robots
with elastic joints,” (in preparation)
44