INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
MOTION CONTROL
Joint space control
Decentralized control
Computed torque feedforward control
Centralized control
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
THE CONTROL PROBLEM
• Joint space control
• Operational space control
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
JOINT SPACE CONTROL
• Dynamic model
B(q)q + C(q, q)q + Fvq + g(q) = τ
• Control ≡ find τ :
q(t) = qd(t)
⋆ transmissions
Krq = qm τm = K−1
r τ
⋆ drives
K−1
r τ = Ktia
va = Raia + Kvqm
va = Gvvc
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Voltage-controlled manipulator
B(q)q + C(q, q)q + F q + g(q) = u
F = Fv + KrKtR−1
a KvKr
u = KrKtR−1
a Gvvc
KrKtR−1
a Gvvc = τ + KrKtR−1
a KvKrq
τ = KrKtR−1
a (Gvvc − KvKrq)
⋆ Kr with elements≫ 1
⋆ Ra with small elements
⋆ τ not too large⇓
⋆ decentralized control
Gvvc ≈ KvKrq
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Torque-controlled manipulator
⋆ reduction of sensitivity to parameter variations
Kt, Kv, Ra
ia = Givc
⇓
⋆ centralized control
τ = u = KrKtGivc
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
DECENTRALIZED CONTROL
• Dynamic model at motor side
K−1
r BK−1
r qm+K−1
r CK−1
r qm+K−1
r FvK−1
r qm+K−1
r g = τm
⋆ average inertia
B(q) = B + ∆B(q)
K−1
r BK−1
r qm + Fmqm + d = τm
⋆ viscous friction
Fm = K−1
r FvK−1
r
⋆ disturbance
d = K−1
r ∆B(q)K−1
r qm + K−1
r C(q, q)K−1
r qm + K−1
r g(q)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Manipulator + drives
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
INDEPENDENT JOINT CONTROL
• Manipulator≡ n independent systems (joint drives)
⋆ control of each joint axis as asingle–input/single–outputsystem
⋆ coupling effects between joints treated as disturbance inputs
• Effective rejection of the effects ofd onθm
⋆ large value of amplifier gain before point of intervention ofdisturbance
⋆ presence of integral action in the controller so as to cancelthe effect of gravitational component on output at steadystate (constantθm)
⇓
• Proportional-integral(PI) control
C(s) = Kc
1 + sTc
s
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• General structure
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position feedback
CP (s) = KP
1 + sTP
sCV (s) = 1 CA(s) = 1
kTV = kTA = 0
⋆ transfer function of forward path
C(s)G(s) =kmKP (1 + sTP )
s2(1 + sTm)
⋆ transfer function of return path
H(s) = kTP
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ root locus analysis
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ closed-loop input/output transfer function
Θm(s)
Θr(s)=
1
kTP
1 +s2(1 + sTm)
kmKP kTP (1 + sTP )
W (s) =
1
kTP
(1 + sTP )(
1 +2ζs
ωn
+s2
ω2n
)(1 + sτ)
⋆ closed-loop disturbance/output transfer function
Θm(s)
D(s)= −
sRa
ktKP kTP (1 + sTP )
1 +s2(1 + sTm)
kmKP kTP (1 + sTP )
XR = KP kTP TR = max
{TP ,
1
ζωn
}
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position and velocity feedback
CP (s) = KP CV (s) = KV
1 + sTV
sCA(s) = 1
kTA = 0
⋆ transfer function of forward path
C(s)G(s) =kmKP KV (1 + sTV )
s2(1 + sTm)
⋆ transfer function of return path
H(s) = kTP
(1 + s
kTV
KP kTP
)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ root locus analysis
⋆ choice of zeroTV = Tm
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ closed-loop input/output transfer function
Θm(s)
Θr(s)=
1
kTP
1 +skTV
KP kTP
+s2
kmKP kTP KV
W (s) =
1
kTP
1 +2ζs
ωn
+s2
ω2n
⋆ design requirements
KV kTV =2ζωn
km
KP kTP KV =ω2
n
km
⋆ closed-loop disturbance/output transfer function
Θm(s)
D(s)= −
sRa
ktKP kTP KV (1 + sTm)
1 +skTV
KP kTP
+s2
kmKP kTP KV
XR = KP kTP KV TR = max
{Tm,
1
ζωn
}
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position, velocity and acceleration feedback
CP (s) = KP CV (s) = KV CA(s) = KA
1 + sTA
s
G′(s) =km
(1 + kmKAkTA)
1 +
sTm
(1 + kmKAkTA
TA
Tm
)
(1 + kmKAkTA)
⋆ transfer function of forward path
C(s)G(s) =KP KV KA(1 + sTA)
s2G′(s)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ transfer function of return path
H(s) = kTP
(1 +
skTV
KP kTP
)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ root locus analysis
⋆ choice of zero
TA = Tm
kmKAkTATA ≫ Tm kmKAkTA ≫ 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ closed-loop input/output transfer function
Θm(s)
Θr(s)=
1
kTP
1 +skTV
KP kTP
+s2(1 + kmKAkTA)
kmKP kTP KV KA
⋆ closed-loop disturbance/output transfer function
Θm(s)
D(s)= −
sRa
ktKP kTP KV KA(1 + sTA)
1 +skTV
KP kTP
+s2(1 + kmKAkTA)
kmKP kTP KV KA
XR = KP kTP KV KA TR = max
{TA,
1
ζωn
}
⋆ design requirements
2KP kTP
kTV
=ωn
ζ
kmKAkTA =kmXR
ω2n
− 1
KP kTP KV KA = XR
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ reconstruction of acceleration
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Decentralized feedforward compensation
• High values of velocities and accelerations
⋆ degraded tracking capabilities
• Adoption of feedforward action
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position feedback
Θ′r(s) =
(kTP +
s2(1 + sTm)
kmKP (1 + sTP )
)Θmd(s)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position and velocity feedback
Θ′r(s) =
(kTP +
skTV
KP
+s2
kmKP KV
)Θmd(s)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Position, velocity and acceleration feedback
Θ′r(s) =
(kTP +
skTV
KP
+(1 + kmKAkTA)s2
kmKP KV KA
)Θmd(s)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Control with torque-controlled drive and current feedfoward
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
COMPUTED TORQUE FEEDFORWARDCONTROL
• At output of PIDD2
a2e + a1e + a0e + a−1
∫ t
e(ς)dς +Tm
km
θmd +1
km
θmd −Ra
kt
d
=Tm
km
θm +1
km
θm
⇓
a′2e + a′
1e + a′
0e + a′
−1
∫ t
e(ς)dς =Ra
kt
d
E(s)
D(s)=
Ra
kt
s
a′2s3 + a′
1s2 + a′
0s + a′
−1
⋆ adoption of too high loop gains
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Computed torque
⋆ feedforwardaction (inverse model)
dd = K−1
r ∆B(qd)K−1
r qmd+K−1
r C(qd, qd)K−1
r qmd+K−1
r g(qd)
⋆ reduction of disturbance rejection effort (limited gains)
⋆ off-line/on-line computation
⋆ partial compensation
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
CENTRALIZED CONTROL
• Manipulator ≡ Coupled nonlinear multi-input/multi-outputsystem
B(q)q + C(q, q)q + F q + g(q) = u
PD control with gravity compensation
• Regulation toconstantequilibrium postureqd
• Lyapunov direct method
⋆ state[ qT qT ]T q = qd − q
⋆ Lyapunov function candidate
V (q, q) =1
2qT B(q)q +
1
2qT KP q > 0 ∀q, q 6= 0
V = qT B(q)q +1
2qT B(q)q − qT KP q
=1
2qT
(B(q) − 2C(q, q)
)q − qT F q + qT
(u − g(q) − KP q
)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ choice of control
u = g(q) + KP q − KDq
⇓
V = −qT (F + KD)q
V = 0 q = 0, ∀q
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ dynamics of controlled system
B(q)q + C(q, q)q + F q + g(q) = g(q) + KP q − KDq
⋆ at equilibrium (q ≡ q ≡ 0)
KP q = 0 =⇒ q = qd − q ≡ 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Inverse dynamics control
• Dynamic model
B(q)q + n(q, q) = u
n(q, q) = C(q, q)q + F q + g(q)
• Nonlinear state feedback (global linearization)
⋆ linear structure inu
⋆ (B(q)) = n ∀q
u = B(q)y + n(q, q)
⇓
q = y
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Choice of stabilizing controly
y = −KP q − KDq + r
r = qd + KDqd + KP qd
⇓
¨q + KD˙q + KP q = 0
⋆ perfect cancellation
⋆ constraints on hardware/software architecture of controlunit
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Robust control
• Imperfectcompensation
u = B(q)y + n(q, q)
⋆ uncertainty
B = B − B n = n − n
⇓
Bq + n = By + n
q = y + (B−1B − I)y + B−1n = y − η
η = (I − B−1B)y − B−1n
• Choice of control
y = qd + KD(qd − q) + KP (qd − q)
⇓
¨q + KD˙q + KP q = η
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
¨q = qd − y + η
⋆ first-order differential equation
ξ = Hξ + D(qd − y + η)
• Estimate on range of variation of uncertainty
supt≥0
‖qd‖ < QM < ∞ ∀qd
‖I − B−1(q)B(q)‖ ≤ α ≤ 1 ∀q
‖n‖ ≤ Φ < ∞ ∀q, q
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Choice of control
y = qd + KD˙q + KP q + w
⇓
ξ = Hξ + D(η − w)
H = (H − DK) =
[O I
−KP −KD
]
• Lyapunov method
V (ξ) = ξT Qξ > 0 ∀ξ 6= 0
V = ξT Qξ + ξT Qξ
= ξT (HT Q + QH)ξ + 2ξT QD(η − w)
= −ξT Pξ + 2ξT QD(η − w)
= −ξT Pξ + 2zT (η − w)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Control law
w =ρ
‖z‖z ρ > 0
⇓
zT (η − w) = zT η −ρ
‖z‖zT z
≤ ‖z‖‖η‖ − ρ‖z‖
= ‖z‖(‖η‖ − ρ)
• Choice ofρ
ρ ≥ ‖η‖ ∀q, q, qd
⋆ estimate of uncertainty
‖η‖ ≤ ‖I − B−1B‖(‖qd‖ + ‖K‖ ‖ξ‖ + ‖w‖
)+ ‖B−1‖ ‖n‖
≤ αQM + α‖K‖ ‖ξ‖ + αρ + BMΦ
ρ ≥1
1 − α(αQM + α‖K‖‖ξ‖ + BMΦ)
⇓
V = −ξT Pξ + 2zT
(η −
ρ
‖z‖z
)< 0 ∀ξ 6= 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
⋆ (attractive)slidingsubspace
• Elimination of high-frequency components
w =
ρ
‖z‖z for ‖z‖ ≥ ǫ
ρ
ǫz for ‖z‖ < ǫ
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Adaptive control
• Dynamic model (linear in the parameters)
B(q)q + C(q, q)q + F q + g(q) = Y (q, q, q)π = u
• Control
u = B(q)qr + C(q, q)qr + F qr + g(q) + KDσ
qr = qd + Λq
qr = qd + Λ ˙q
σ = qr − q = ˙q + Λq
⇓
B(q)σ + C(q, q)σ + Fσ + KDσ = 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Lyapunov method
V (σ, q) =1
2σT B(q)σ +
1
2qT Mq > 0 ∀σ, q 6= 0
V = σT B(q)σ +1
2σT B(q)σ + qT M ˙q
= −σT (F + KD)σ + qT M ˙q
= −σT Fσ − ˙qT
KD˙q − qT ΛKDΛq
⋆ V = 0 only for q = ˙q ≡ 0 =⇒ [ qT σT ]T = 0
globally asymptotically stable
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Control based on parameter estimates
u = B(q)qr + C(q, q)qr + F qr + g + KDσ
= Y (q, q, qr, qr)π + KDσ
⇓
B(q)σ + C(q, q)σ + Fσ + KDσ
= −B(q)qr − C(q, q)qr − F qr − g(q)
= −Y (q, q, qr, qr)π
• Modification ofV
V (σ, q, π) =1
2σT B(q)σ + qT ΛKDq +
1
2πT Kππ > 0
∀σ, q, π 6= 0
V = −σT Fσ − ˙qT
KD˙q − qT ΛKDΛq
+ πT(Kπ
˙π − Y T (q, q, qr, qr)σ)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
• Adaptive law
˙π = K−1
π Y T (q, q, qr, qr)σ
⇓
V = −σT Fσ − ˙qT
KD˙q − qT ΛKDΛq
⋆ q → 0
⋆ Y (q, q, qr, qr)(π − π) → 0