Lecture 13: Multivariable Control of Robot Manipulators · 2008. 10. 8. · Lecture 13: Multivariable Control of Robot Manipulators •PD-Control •Feedback Linearization •Robust

Lecture 13: Multivariable Control of Robot Manipulators

• PD-Control

c©Anton Shiriaev. 5EL158: Lecture 13 – p. 1/17


• PD-Control

• Feedback Linearization



• PD-Control


• Robust and Adaptive Motion Control


Improved Dynamical Model

Given a mechanical system with n-degrees of freedom

D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T




D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T

We suppose that each of degrees of freedom is controlled by ageared DC-motor

Jmkθk + Bkθk =

Kmk

Rk

Vk −1

rk

τk, k = 1, . . . , n

where• θk is the kth-motor angle;• rk is a gear ration;• Jmk

, Bk, Kmk, Rk are parameters or computed from

parameters of the kth DC-motor




D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T


Jmkθk + Bkθk =

Kmk

Rk

Vk −1

rk

τk, k = 1, . . . , n

and that the motor and the link angles are related by

θmk= rkqk, k = 1, . . . , n




D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T


Jmkθk + Bkθk =

Kmk

Rk

Vk −1

rk

τk, k = 1, . . . , n

and that the motor and the link angles are related by

θmk= rkqk, k = 1, . . . , n

Then the actuator equations are

r2kJmk

qk + r2kBkqk = rk

Kmk

Rk

Vk − τk, k = 1, . . . , n


Improved Dynamical Model (Cont’d)

The dynamical systems

D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T

r2kJmk

qk + r2kBkqk = rk

Kmk

Rk

Vk − τk, k = 1, . . . , n

can be rewritten as one, if we exclude τ !


Improved Dynamical Model (Cont’d)

The dynamical systems

D(q)q+C(q, q)q+g(q) = τ , q =[q1, . . . , qn

]T

, τ =[τ1, . . . , τn

]T

r2kJmk

qk + r2kBkqk = rk

Kmk

Rk

Vk − τk, k = 1, . . . , n

can be rewritten as one, if we exclude τ !

Indeed, it is

M(q)q + C(q, q)q + Bq + g(q) = u

where

• M(q) = D(q) + J with J = diag{r21Jm1

, . . . , r2nJmn

}

• B =[r21B1, r2

2B2, . . . , r2

nBn

]T

• u = [u1, u2, . . . , un]T with uk = rkKmk

Rk

Vk, k = 1, . . . , n


PD-Controller Design

Given a mechanical system

M(q)q + C(q, q)q + Bq + g(q) = u

we are interested• to design a controller to stabilize a particular configuration

of the robot: q = qd

• to analyze the closed-loop system




M(q)q + C(q, q)q + Bq + g(q) = u




Assume that B = 0 and g(p) = 0




M(q)q + C(q, q)q + Bq + g(q) = u




Assume that B = 0 and g(p) = 0

The first controller to analyze is

u = −Kp (q − qd)−Kdq

with Kp and Kd are diagonal matrices with positive elements.


PD-Controller Design (Cont’d)

To analyze the behavior of the closed loop system

M(q)q + C(q, q)q = u = −Kp (q − qd)−Kdq

consider a scalar function

V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)

Its time-derivative along solutions of the closed-loop system is

ddt

V = qT M(q)q + qT ddt

1

2[M(q)] q + qT Kp (q − qd)






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)


ddt


1

2[M(q)] q + qT Kp (q − qd)

= qT[u− C(q, q)q

]+ qT d

dt1

2[M(q)] q + qT Kp (q − qd)






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)


ddt


1

2[M(q)] q + qT Kp (q − qd)

= qT[u− C(q, q)q

]+ qT d

dt1

2[M(q)] q + qT Kp (q − qd)

= qT[u + Kp (q − qd)

]+ qT

{ddt

1

2[M(q)]− C(q, q)

}

q






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)


ddt


1

2[M(q)] q + qT Kp (q − qd)

= qT[u− C(q, q)q

]+ qT d

dt1

2[M(q)] q + qT Kp (q − qd)


]+ qT

{ddt

1

2[D(q) + J ]− C(q, q)

}

q︸︷︷︸

= 0






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)


ddt


1

2[M(q)] q + qT Kp (q − qd)

= qT[u− C(q, q)q

]+ qT d

dt1

2[M(q)] q + qT Kp (q − qd)


]

= qT[−Kp (q − qd)−Kdq + Kp (q − qd)

]= −qT Kdq






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)


ddt

V = −qT Kdq ≤ 0

Therefore• V is positive definite, V (q, q) = 0 ⇒ {q = qd, q = 0}• V (q(t), q(t)) is monotonically decreasing!

⇒ ∃ limt→+∞

V (q(t), q(t)) = V∞ and ∃ limt→+∞

q(t) = q∞(t) = 0






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)

If we substitute this limit trajectory into the dynamics, we obtain

M(q∞) q∞︸︷︷︸

= 0

+C(q∞, q∞) q∞︸︷︷︸

= 0

= −Kp (q∞− qd)−Kd q∞︸︷︷︸

= 0

that is0 = −Kp (q∞− qd)






V (q, q) = 1

2qT M(q)q + 1

2(q − qd)

T Kp (q − qd)

If we substitute this limit trajectory into the dynamics, we obtain

M(q∞) q∞︸︷︷︸

= 0

+C(q∞, q∞) q∞︸︷︷︸

= 0

= −Kp (q∞− qd)−Kd q∞︸︷︷︸

= 0

that is0 = −Kp (q∞− qd)

Kp = diag {Kp1, Kp2, . . . , Kpn} > 0 ⇒ q∞ = qd


PD-Controller Design with Gravity Compensation


M(q)q + C(q, q)q + Bq + g(q) = u

How to modify the controller


if B 6= 0 and g(p) 6= 0?


PD-Controller Design with Gravity Compensation


M(q)q + C(q, q)q + Bq + g(q) = u

How to modify the controller


if B 6= 0 and g(p) 6= 0?

The controller

u = −Kp (q − qd)−Kdq + g(q)

is stabilizing, if Kp and Kd are diagonal matrices such that

Kp > 0 Kd −B > 0



• PD-Control




Feedback Linearization


M(q)q + C(q, q)q + Bq + g(q) = u

How to define a control variable

u = α(q, q) + β(q, q)v

so that the closed loop system is:• linear? I.e. is equivalent to

x = Ax + Bv

• linear and stabilizable? I.e. the pair (A, B) is stabilizable




M(q)q + C(q, q)q + Bq + g(q) = u


u = α(q, q) + β(q, q)v


x = Ax + Bv


What ifu = M(q)v + C(q, q)q + Bq + g(q)?




M(q)q + C(q, q)q + Bq + g(q) = u


u = α(q, q) + β(q, q)v


x = Ax + Bv


ThenM(q)q = M(q)v ⇒ q = v




M(q)q + C(q, q)q + Bq + g(q) = u


u = α(q, q) + β(q, q)v


x = Ax + Bv


Then

q = v ⇒ x =

[

0n In

0n 0n

]

x +

[

0n

In

]

v, x = [qT , qT ]T




M(q)q + C(q, q)q + Bq + g(q) = u

and the desired trajectory qd = qd(t), introduce the controller

u = M(q)v + C(q, q)q + Bq + g(q)

v = qd(t)−Kp(q − qd(t))−Kd(q − qd(t))




M(q)q + C(q, q)q + Bq + g(q) = u


u = M(q)v + C(q, q)q + Bq + g(q)


Then the closed loop system is

q = v = qd(t)−Kp(q − qd(t))−Kd(q − qd(t))




M(q)q + C(q, q)q + Bq + g(q) = u


u = M(q)v + C(q, q)q + Bq + g(q)


Then the closed loop system is

q = v = qd(t)−Kp(q − qd(t))−Kd(q − qd(t))

It can be rewritten in error-variables as

e + Kde + Kpe = 0, e = q − qd(t)



• PD-Control




Robust and Adaptive Motion Control


M(q)q + C(q, q)q + g(q) = u

and the desired trajectory qd = qd(t), the controller

u = M(q)v + C(q, q)q + g(q)


cannot be implemented!




M(q)q + C(q, q)q + g(q) = u

and the desired trajectory qd = qd(t), the controller

u = M(q)v + C(q, q)q + g(q)


cannot be implemented!

The approximation might be used

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]




The uncertainty in parameters of controller

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]


motivates two approaches

• Robust Control:Design Kp, Kd and qd(t) such that the error signal

e(t) = q(t)− qd(t) ≈ 0 ∀ {△M,△C,△g} ∈ W



The uncertainty in parameters of controller

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]


motivates two approaches

• Robust Control:Design Kp, Kd and qd(t) such that the error signal

e(t) = q(t)− qd(t) ≈ 0 ∀ {△M,△C,△g} ∈ W

• Adaptive Control: Improve estimates for

M(q), C(q, q), g(q)

in the course of regulating the system


Robust Motion Control Based on Feedback Linearization

Given a trajectory q = qd(t), consider the closed loop system

M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]





M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]


M(q)q+C(q, q)q+g(q) =

= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g

⇒ q = M(q)−1 [M(q) +△M ] v + M(q)−1 [△Cq +△g]




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g

q = M−1 [M +△M ] v + M−1 [△Cq +△g]± M−1Mv




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g

q = M−1 [M +△M ] v + M−1 [△Cq +△g]± M−1Mv

⇒ q = v + M−1(q) [△M(q)v +△C(q, q)q +△g(q)]




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g

q = M−1 [M +△M ] v + M−1 [△Cq +△g]± M−1Mv

q = v + η(q, q, v)




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]



= [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

⇒ M(q)q = [M(q) +△M ] v +△Cq +△g

q = M−1 [M +△M ] v + M−1 [△Cq +△g]± M−1Mv

q = v + η(q, q, v)




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]

v = qd(t)−Kp(q − qd(t))−Kd(q − qd(t)) +w




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]


It can be rewritten as

q = v + η(q, q, v)or

(q − qd) + Kd (q − qd) + Kp (q − qd) = w + η(q, q, v)




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]





or

ddt

e =

[

0n In

−Kp −Kd

]

e+

[

0n

In

]

(w + η) , e =

[

(q − qd)

(q − qd)

]




M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]





or

ddt

e = Ae + B (w + η) , e =

[

(q − qd)

(q − qd)

]



To continue with design of w for the system

ddt

e = Ae + B[w + η(e, w)

]

we need to impose some assumptions on η(·), namely

‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1




ddt

e = Ae + B[w + η(e, w)

]


‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

Matrix A is stable, therefore ∀Q > 0, ∃P = P T > 0 such that

AT P + PA = −Q




ddt

e = Ae + B[w + η(e, w)

]


‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1


AT P + PA = −Q

Consider a Lyapunov function candidate as V = eT Pe, then

ddt

V = ddt

eT Pe + eT P ddt

e

= eT (AT P + PA)e + 2eT PB[w + η(e, w)

]




ddt

e = Ae + B[w + η(e, w)

]


‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1


AT P + PA = −Q

Consider a Lyapunov function candidate as V = eT Pe, then

ddt

V = −eT Qe + 2eT PB[w + η(e, w)

]

≤ 0 ← How to achieve this by choosing w?




ddt

e = Ae + B[w + η(e, w)

]


‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

Let us look at the second term

ddt


]

when w has the form

w = −ρ(·) z√

zT z, z = BT Pe , ρ is a function to choose




ddt

e = Ae + B[w + η(e, w)

]


‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

Let us look at the second term

ddt


]

when w has the form

w = −ρ(·) z√

zT z, z = BT Pe , ρ is a function to choose

zT

(

−ρz√

zT z+ η

)

≤ −ρ‖z‖+ ‖z‖‖η‖ = ‖z‖ (−ρ + ‖η‖)



To sum up, we search for a scalar function ρ(·) such that

(−ρ + ‖η‖) ≤ 0 ⇔ ‖η‖ ≤ ρ

and

‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

withw = −ρ(·) z

√zT z




(−ρ + ‖η‖) ≤ 0 ⇔ ‖η‖ ≤ ρ

and

‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

withw = −ρ(·) z

√zT z

These two inequalities imply the next one

α

∥∥∥∥ρ(·) z√

zT z

∥∥∥∥

+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3 ≤ ρ(·)




(−ρ + ‖η‖) ≤ 0 ⇔ ‖η‖ ≤ ρ

and

‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

withw = −ρ(·) z

√zT z


αρ(·) + γ1 ‖e‖+ γ2 ‖e‖2 + γ3 ≤ ρ(·)




(−ρ + ‖η‖) ≤ 0 ⇔ ‖η‖ ≤ ρ

and

‖η(e, w)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

withw = −ρ(·) z

√zT z


αρ(·) + γ1 ‖e‖+ γ2 ‖e‖2 + γ3 ≤ ρ(·)

ρ(e) ≥ 1

1− α

[

γ1 ‖e‖+ γ2 ‖e‖2 + γ3

]


Final Form of the Controller


M(q)q + C(q, q)q + g(q) = u

u = [M(q) +△M ] v + [C(q, q) +△C] q + [g(q) +△g]


w =

{

−ρ(e) z√

zT z, if z = BT Pe 6= 0

0, if z = BT Pe = 0

where ρ(·) is any function that satisfies to inequality

ρ(e) ≥ 1

1− α

[

γ1 ‖e‖+ γ2 ‖e‖2 + γ3

]

where constants α, γ1-γ2 are from the inequality

‖η(·)‖ ≤ α ‖w‖+ γ1 ‖e‖+ γ2 ‖e‖2 + γ3, α < 1

andη(q, q, v) = M−1 [△Mv +△Cq +△g] , e =

[

(q − qd)

(q − qd)

]


Documents

Lecture 13: Multivariable Control of Robot Manipulators · 2008. 10. 8. · Lecture 13: Multivariable Control of Robot Manipulators •PD-Control •Feedback Linearization •Robust