MOTION CONTROL Joint space control Decentralized ... ROBOTICS Prof. Bruno SICILIANO MOTION CONTROL...

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