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


THE CONTROL PROBLEM

• Joint space control

• Operational space control


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


• 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


• Torque-controlled manipulator

⋆ reduction of sensitivity to parameter variations

Kt, Kv, Ra

ia = Givc

⇓

⋆ centralized control

τ = u = KrKtGivc


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)


• Manipulator + drives


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


• General structure


• 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


⋆ root locus analysis


⋆ 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

}


• Position and velocity feedback

CP (s) = KP CV (s) = KV

1 + sTV

sCA(s) = 1

kTA = 0


C(s)G(s) =kmKP KV (1 + sTV )

s2(1 + sTm)


H(s) = kTP

(1 + s

kTV

KP kTP

)



⋆ choice of zeroTV = Tm



Θ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


Θ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

}


• 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)


C(s)G(s) =KP KV KA(1 + sTA)

s2G′(s)



H(s) = kTP

(1 +

skTV

KP kTP

)



⋆ choice of zero

TA = Tm

kmKAkTATA ≫ Tm kmKAkTA ≫ 1



Θm(s)

Θr(s)=

1

kTP

1 +skTV

KP kTP

+s2(1 + kmKAkTA)

kmKP kTP KV KA


Θ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


⋆ reconstruction of acceleration


Decentralized feedforward compensation

• High values of velocities and accelerations

⋆ degraded tracking capabilities

• Adoption of feedforward action


• Position feedback

Θ′r(s) =

(kTP +

s2(1 + sTm)

kmKP (1 + sTP )

)Θmd(s)


• Position and velocity feedback

Θ′r(s) =

(kTP +

skTV

KP

+s2

kmKP KV

)Θmd(s)


• Position, velocity and acceleration feedback

Θ′r(s) =

(kTP +

skTV

KP

+(1 + kmKAkTA)s2

kmKP KV KA

)Θmd(s)


• Control with torque-controlled drive and current feedfoward


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


• 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


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

)


⋆ choice of control

u = g(q) + KP q − KDq

⇓

V = −qT (F + KD)q

V = 0 q = 0, ∀q


⋆ 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


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


• 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


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 = η


¨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


• 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)


• 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


⋆ (attractive)slidingsubspace

• Elimination of high-frequency components

w =

ρ

‖z‖z for ‖z‖ ≥ ǫ

ρ

ǫz for ‖z‖ < ǫ


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


• 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


• 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


+ πT(Kπ

˙π − Y T (q, q, qr, qr)σ)


• Adaptive law

˙π = K−1

π Y T (q, q, qr, qr)σ

⇓

V = −σT Fσ − ˙qT


⋆ q → 0

⋆ Y (q, q, qr, qr)(π − π) → 0

motion control joint space control decentralized ... robotics prof. bruno siciliano motion control...

Documents