Video Segment

Juggling Robot, Dan Koditschek

University of Michigan,
ISRR’93 video proceedings

Control
• Natural Systems
• PID Control
• Joint-Space Dynamic Control
• Task-Oriented Control

Robot Control • Force Control

Joint-Space Control Task-Oriented Control

1
Joint Space Control Resolved Motion Rate Control (Whitney 72)
δx = J (θ )δθ
δq1 q1
Control Joint 1
Outside singularities
δq2
Control Joint 2
q2
δθ = J −1 (θ )δx
xd Inv. qd q
Kin. Arm at Configuration θ
δqn
Control Joint n
qn
x = f (θ )
δx = x d − x
δθ = J −1δx
θ + = θ + δθ

Resolved Motion Rate Control Natural Systems

k
δq1
Control Joint 1
q1 Conservative Systems m
x
δq2 q2
xd δx δq Control Joint 2
q
J -1
x d ∂ (K −V) ∂ (K −V) 1
( )− =0 K = mx 2
δqn
Control Joint n
qn
dt ∂ x ∂x 2
Forward
Kinematics

Natural Systems Natural Systems

k k
Conservative Forces m Conservative Forces m
x x
d ∂ K ∂ K ∂V d ∂ K ∂ K ∂V
( )− =− ( )− =−
dt ∂ x ∂ x ∂x dt ∂ x ∂ x ∂x
Potential Energy of a spring
m x = F = − kx 0 1
−
∂ 1 2
( kx ) V = Work = ∫ (−kx)δ x = kx 2
Potential Energy of a spring ∂x 2 x 2
1
m x = F = − kx
0
V = Work = ∫ (−kx)δ x = kx 2
x 2 ∂ 1 2
− ( kx )
∂x 2

2
 Natural Systems Natural Systems V=
1 2
kx
k 2
Conservative Systems m Conservative Systems
x
m x + kx = 0 x
d ∂ (K −V) ∂ (K −V) 1
( )− =0 K = mx 2 Frequency increases
dt ∂ x ∂x 2 with stiffness
and inverse mass
1
V = kx 2 k
m x + kx = 0 2 Natural Frequency ωn =
m
x + ω 2n x = 0 t
x (t ) = c cos(ω n t + φ )

Natural Systems Dissipative Systems

k k
Dissipative Systems x m
x x x Friction
x
x m
x x x Friction
mx + bx + kx = 0
d ∂ (K −V) ∂ (K −V) b k
( )− = f friction x+ x+ x=0
dt ∂x ∂x m m

Viscous friction: f friction = − bx x x x

mx + bx + kx = 0 t
Oscillatory
t
Critically
Higher b/m
Over
t
b
= 2ω n
damped damped damped

2d order systems Time Response

mx + bx + kx = 0 x + 2ξ nω n x + ω 2n x = 0
b k
x+x+ x=0 k b Natural
ωn = ; ξn =
Natural
m m damping
frequency
b m 2 km ratio
m ⋅ 2ω ω n2
2ω n x (t ) = ce − ξ nω n t cos(ω n 1 − ξ 2n t + φ )
n

Natural damping ratio

Critically
damped
ξn =
b
=
b x(t) ω
2ω n m
when damped
b/m=2ωn 2 km Natural
t frequency
Critically damped system: ξ n = 1 ( b = 2 km )
2π ω = ω n 1 − ξ n2
ω n 1 − ξ 2n

3
 Example
k x
x
m
x x x b m = 2.0
mx + bx + kx = 0 b = 4.8
k = 8.0
Video Segment
what is the “damped Natural frequency”
ω = ω n 1 − ξ n2
k b
ωn = =2; ξn = = 0.6 Tactile Sensing, H. Maekawa et al.
m 2 km
MEL, AIST-MITI, Tsukuba, Japan
ω = 2 1 − 0.36 = 1.6 ICRA’93 video proceedings

1-dof Robot Control

m f mx = f
x0 xd
V(x)
Potential Field
V ( x ) > 0, x ≠ x d
x
V ( x ) = 0, x = x d x0 xd
1 ∂V
V ( x ) = k p ( x − x d ) 2 ; f = −∇ V ( x ) = −
2 ∂x
∂ 1
mx = − [ k p ( x − x d ) ] ; mx + k p ( x − x d ) = 0
2

∂x 2
Position gain

Passive Systems (Stability) Asymptotic Stability

d ⎛ ∂K ⎞ ∂ ( K − Vgoal )
k p ( x − xg ) ( x − xg )
1 a system ⎟− = 0Fs
T
Vgoal = ⎜
2 dt ⎝ ∂x ⎠ ∂x
d ⎛ ∂K ⎞ ∂K is asymptotically stable if
System ⎜ ⎟− = f Fs
dt ⎝ ∂x ⎠ ∂x
∂Vgoal FsT x < 0 ; for x ≠ 0 x
⇓ f =−
∂X Fs = − kv x → kv > 0
d ⎛ ∂K ⎞ ∂ ( goal ) = 0
K − V Conservative Forces Control
⎜ ⎟− F = −k p ( x − xgoal ) −kv x
dt ⎝ ∂x ⎠ ∂x Stable

4
Proportional-Derivative Control (PD) Gains
mx = f = − k p ( x − x d ) − kv x k p = mω 2
mx + kv x + k p ( x − xd ) = 0 kv = m(2ξω )
Gain Selection
FG ξ IJ → k = mω
Velocity gain Position gain
2
kv kp
1. x + x + (x − xd ) = 0
Hω K k = m(2ξω )
p
set
m m v

1. x + 2ξω x + ω2 (x − xd ) = 0 Unit mass system m - mass system

kv k p closed loop k p′ = ω 2
k p = mkk p′
ξ= closed loop ω=
2 k p m damping ratio m frequency kv′ = 2ξω kv = mkkv′

Control Partitioning Non Linearities

mx = f m (1. x ) = m f ′ mx + b ( x , x ) = f
Control Partitioning
f = −kv x − k p ( x − xd ) f = αf ′ + β
α=m
f = m[−kv′x − k′p (x − xd )] = m f ′ with
β = b( x, x )
mx = m f ′ f′
mx + b( x , x ) = mf ′ + b ( x , x )
1. x = f ′ unit mass system 1. x = f ′
1.x + kv′ x + k ′p ( x − xd ) = 0 f′ + f
m System ( x, x )
+

2ξω ω2
b( x, x )
Unit mass system

Motion Control Disturbance Rejection

mx + b ( x , x ) = f ⇒ 1. x = f ′ mx + b( x , x ) = f
f = mf ′ + b xd
Goal Position (xd):
Control: f ′ = − k v′ x − k p′ ( x − x d ) xd f′ f x
+ k p′ m +
System
Closed-loop System: 1. x + k v′ x + k p′ ( x − x d ) = 0
- + x
xd +
kv′ b( x , x )
Trajectory Tracking -

x d (t ); x d (t ); and x d (t )
Control: f ′ = x d − kv′ ( x − x d ) − k p′ ( x − x d ) e + kv′e + k p′ e = 0
Closed-loop System:
( x − x d ) + k v′ ( x − x d ) + k p′ ( x − x d ) = 0
with e ≡ x − x d
e + kv′e + k p′ e = 0

5
Disturbance Rejection Steady-State Error
mx + b( x , x ) = f fdist
xd fdist e + kv′e + k p′ e =
m
f′
( e = e = 0) :
xd f++ x
+ k p′ m +
System The steady-state
- + x
xd +
kv′ b( x , x )
fdist
k p′ e =
-

mx + b ( x , x ) = f + fdist m
f f
Control f = mf ′ + b ( x , x )
bounded
{∀ t fdist < a} e = dist = dist
Closed loop mk p′ kp
fdist Closed loop
e + kv′e + k p′ e = position
m gain

Steady-State Error - Example PID (adding Integral action)

f fdist
m System mx + b ( x , x ) = f + fdist
mx + kv x + k p ( x − x d ) = 0 Control f = mf ′ + b ( x , x )
kp
x
m
x x x kv
fdist
k p ( x − x d ) = fdist z
f ′ = xd − kv′ ( x − xd ) − k p′ ( x − xd ) − ki′ ( x − xd )dt

z
f Closed-loop System
x = x d + dist fdist
kp e + kv′e + k p′ e + ki′ edt =
fdist = k p Δx m
Δx e + kv′e + k p′ e + ki′e = 0

nt
ta
f

ns
Δx = dist

co
kp Closed Loop Steady-state Error e=0
Stiffness

Gear Reduction R Effective Inertia

Gear ratio η =
θm Link
r
Ieff = I L + η 2 I m
1
Motor Im θ L = ( )θ m
θL
r
η η =1
for a manipulator
IL τ L = ητ m
I L = I L (q)
R
Direct Drive
1 1 Gain Selection
τ m = I mθ m + ( I Lθ L ) + bmθ m + bLθ L
η
θL = θm
1
η k p = ( I L + η 2 Im )k p′
η

τ m = ( Im +
IL
)θ m + ( bm +
bL
)θ m kv = ( I L + η 2 I m )kv′
η2 η2
Time Optimal Selection
τ L = ( I L + η 2 I m )θ L + ( bL + η 2 bm )θ L
1
Effective Inertia Effective Damping IL = ( I Lmin + I Lmax ) 2
4

6
 Video Segment
On the Run, Marc Raibert, MIT
ISRR’93 video proceedings

Manipulator Control
m2

M (θ )θ + V (θ , θ ) + G (θ ) = τ G1
l2 θ2 kv1
l1
m1
θd - + τ1 θ1
θ1
1 + k p1 + Link1(m11)

m12θ 2 + m122θ 22 + m122θ 1θ 2

θ1

FG m IJ FG θ IJ + FG m IJ dθ F 0 m122 I Fθ I F G I F τ I m21θ 1 −
m112 2
θ1
i GH − m JK GHθ JK + GH G JK = GH τ JK
2
m12
θ2 + θd τ2 θ2
K Hθ K H 0 K
11 1 1

Hm
1 112 1
1 112 2
m22 0 2
+ +
21 2 2 2 2 2 2
k p2 Link2(m22)

θ2
- +

m11θ 1 + m12θ 2 + m112θ 1θ 2 + m122θ 22 + G1 = τ 1 kv2

G2
m112 2
m22θ 2 + m21θ 1 − θ 1 + G2 = τ 2
2

Σ i , j ≠1 m ij q j + b1 + g1 q1
q1d q1 , q1 q1d q1 , q1
PD m11 Joint 1 m11
D
PD Joint 1 Y
N
A
M
q2 d q2 , q2 Σ i , j ≠ 2 m ij q j + b2 + g 2 q2
I
PD m22 Joint 2 C
q2 d q2 , q2
PD m22 Joint 2
C
O
U
Σ i , j ≠ n m ij q j + bn + g n P
qn L
qnd qn , qn I
PD mnn Joint n qnd qn , q n
mnn N
PD Joint n G

7
 PD Control Stability PD Control Stability

M(q)q + B(q)[qq] +C(q)[q2 ] +G(θ) =τ M(q)q + B(q)[qq] +C(q)[q2 ] +G(θ) =τ

τ = −k p (q − qd ) − kv q τ = −k p (q − qd ) − kv q
Vd = 1/ 2kp (q − qd )2 Vd = 1/ 2kp (q − qd )T (q − qd )

d ∂K ∂ K ∂ Vs ∂ Vd d ∂K ∂ K ∂ (V s − V d )
( )− + =τ
− − kvq ( )− + =τs
dt ∂ q ∂q ∂q ∂q dt ∂ q ∂q ∂q
τ s = − k v q with τ sT q < 0 for q ≠ 0; k v > 0

Performance Nonlinear Dynamic Decoupling

High Gains better disturbance rejection M (θ )θ + V (θ , θ ) + G (θ ) = τ
Gains are limited by τ = M (θ )τ ′ + V (θ , θ ) + G (θ )
structural flexibilities
1. θ = ( M M )τ ′ + M −1 [(V − V ) + ( G − G )]
−1
time delays (actuator-sensing)
with perfect estimates
sampling rate
ω res 1. θ = τ ′ + ε (t )
ωn ≤ lowest structural flexibility
2 τ ′ : input of the unit-mass systems
ω delay F 2π I
largest delay G
τ ′ = θ d − kv′ (θ − θ d ) − k p′ (θ − θ d )
ωn ≤
3 H τ JK delay Closed-loop
ω sampling − rate E + kv′ E + k p′ E = 0 + ε (t )
ωn ≤
5

τd
qd
+ q
qd - e
kv + τ
Σ Σ M (q) Σ Arm q
+ +
- e
qd Σ
+ kp

a f
B(q, q ) + C q, q + G (q )

Task Oriented Control

8
 Joint Space Control Joint Space Control

Operational Space Control Unified Motion & Force Control

V ( xGoal )
F
Fmotion

F = Fmotion + Fcontact
F = −∇V ( xGoal ) Fcontact
τ = JTF

Operational Space Dynamics Task-Oriented Equations of Motion

{n + 1}
{0}
0n+1

F
x
Non-Redundant Manipulator ; n = m
x = ( x1 x2 … xm )
T
M x x+ Vx + Gx= F
q = ( q1 q2 … qn )
T

F = F[Mˆ x ,Vˆx , Gˆ x ,V ( xGoal )]

9
 Equations of Motion ⎛ x⎞
Operational Space Dynamics
⎜ ⎟
d ⎛ ∂L ⎞ ∂L ⎜ y⎟
M x ( x) x + Vx ( x, x) + Gx ( x) = F
⎜ ⎟− =F ⎜z⎟
x=⎜ ⎟
dt ⎝ ∂x ⎠ ∂x ⎜α ⎟
⎜β ⎟ x: End-Effector Position and
⎜⎜ ⎟⎟
with ⎝γ ⎠ Orientation
L ( x , x ) = K ( x, x ) − U ( x ) M x ( x) : End-Effector Kinetic Energy Matrix
Vx ( x, x) : End-Effector Centrifugal and
Coriolis forces
Gx ( x) : End-Effector Gravity forces
F: End-Effector Generalized forces

Joint Space/Task Space Joint Space/Task Space

Relationships Relationships
Kinetic Energy M x ( x) = J −T (q) M (q) J −1 (q)
K x ( x, x ) ≡ K q ( q , q )
1 T 1 Vx ( x, x) = J −T (q) V (q, q ) − M x (q ) h(q, q)
x M x ( x) x ≡ qT M ( q )q
2 2
Using x = J (q )q Gx ( x ) = J − T ( q ) G ( q )
1 T T 1
q ( J M x J ) q ≡ qT M q where h(q, q) J (q )q
2 2

Example ⎡ −d s1 c1⎤
0
J =⎢ 2 ⎥
⎣ d 2 c1 s1⎦
q2 = d 2
1
J

⎛ c1 − s1⎞ ⎛ 0 1 ⎞
⎡ d 2 c1⎤
0
J =⎜ ⎟⎜ ⎟
x=⎢ ⎥ ⎝ s1 c1 ⎠⎝ d 2 0 ⎠
⎣ d 2 s1⎦ ⎛ 0 1 d2 ⎞
1 −1
J =⎜ ⎟ ;
⎡ −d s1 c1⎤ ⎝1 0 ⎠
0
J =⎢ 2 ⎥
⎣ d 2 c1 s1⎦ ⎛ 0 1 ⎞⎛ m11 0 ⎞ ⎛ 0 1 d 2 ⎞
1
Mx = ⎜ ⎟⎜ ⎟⎜ ⎟
⎝1 d 2 0 ⎠⎝ 0 m22 ⎠ ⎝ 1 0 ⎠

10
 ⎛m 0 ⎞ ⎛ c1 − s1⎞ ⎛ m2 0 ⎞ ⎛ c1 s1 ⎞
Mx = ⎜ 2
0
Mx = ⎜ ⎟⎜ ⎟
m2 + ⎠ ⎜⎝ − s1 c1 ⎟⎠
1
⎟
⎝ 0 m2 + m2′ ⎠ ⎝ s1 c1 ⎠ ⎝ 0
m2 + m2′
m2
• +
m2 = m2 + m2′
y1 m2
x
• 1
m1
⎛ m2 + m2′ s12 −m2′ s c1 ⎞
0
Mx = ⎜ ⎟
I 331 + I 332 + m l ⎝ −m2′ s c1 m2 + m2′ c12 ⎠
2
m2′ = 11
d22

End-Effector Control

m2 + m2′
m2
F
τ = J T (q) F
⎛ m + m2′ s1 2
− m2′ s c1 ⎞
0
Λ=⎜ 2 ⎟
⎝ −m2′ s c1 m2 + m2′ c12 ⎠

Passive Systems (Stability) Asymptotic Stability

d ⎛ ∂K ⎞ ∂ ( K − Vgoal )
k p ( x − xg ) ( x − xg )
1 a system ⎟− = 0Fs
T
Vgoal = ⎜
2 dt ⎝ ∂x ⎠ ∂x
d ⎛ ∂( K − V ) ⎞ ∂( K − V ) is asymptotically stable if
System ⎜ ⎟− =F Fs
dt ⎝ ∂x ⎠ ∂x
∂ FsT x < 0 ; for x ≠ 0 x
⇓ F =−
∂X
(
Vgoal − Vˆ ) Fs = − kv x → kv > 0
d ⎛ ∂K ⎞ ∂ ( K − Vgoal ) Conservative Forces Control
− =
⎜ ⎟ F = − k p ( x − xgoal ) + Gˆ x −kv x
0
dt ⎝ ∂x ⎠ ∂x Stable

11
Example 2-d.o.f arm:
Non-Dynamic Control ( m c 12 + m ) x + m
1
* 2
2 1
*
y + Vx1 = − k p ( x − xg ) − kv x

( m c 12 + m ) y + m
* 2 *
x + Vx 2 = − k p ( y − y g ) − kv y
M x ( x) x + Vx ( x, x) + Gx ( x) = F 1 2 1

q2 Closed loop behavior

F = −k p ( x − xg ) − kv x + Gˆ ( x) l2
l1
q1 (
m11 (q ) x + kv x + k p ( x − xg ) = − m1* y + Vx1 )
m22 (q) y + kv y + k p ( y − y g ) = −(m 1
*
x + Vx 2)

Nonlinear Dynamic Decoupling

Perfect Estimates
Model I x=F'
M x ( x) x + Vx ( x, x) + Gx ( x) = F F ' input of decoupled end-effector
Control Structure
Goal Position Control
F = Mˆ ( x) F '+ Vˆx ( x, x) + Gˆ x ( x)
F ' = −kv' x − k p' ( x − xg )
Decoupled System
I x=F' Closed Loop

with τ = JTF I x + kv' x + k p' ( x − xg ) = 0

Trajectory Tracking In joint space

Trajectory: xd , xd , xd
ε q + kv' ε q + k p' ε q = 0
F ' = I xd − k ( x − xd ) − k ( x − xd )
'
v
'
p

with ε q = q − qd
( x − xd ) + kv' ( x − xd ) + k p' ( x − xd ) = 0
or ε x + kv' ε x + k p' ε x = 0
with ε x = x − xd

12
 Task-Oriented Control Compliance I x = F′
⎛ k ′p 0 0 ⎞
⎜ x ⎟
xd
af x F′= −⎜ 0 k ′p y 0 ⎟ ( x − x d ) − k v′ x
a fτ
q
F Kin q
xd - e
kv JT q ⎜ ⎟
⎜ 0 k ′p z ⎟⎠
Σ Σ M x (q ) Σ Arm q
J aq f
+ x
xd -
Σ
e
kp ⎝ 0
+
set to zero

Vx ( q , q ) + G x ( q )
x + k x + k ( x − xd ) = 0
'
v
'
px

y + k v' y + k py
'
( y − yd ) = 0
z + k v' z = 0
Compliance along Z

Stiffness Force Control

z + kv′ z + k ′pz ( z − zd ) = 0 1-d.o.f. mx = f
f fd
m
determines stiffness along z set f = fd
Closed-Loop Stiffness: Mˆ k′ = k
x p p Problem
F = Kx (x − xd ) friction Visc
ous
Coulomb friction
10(N.m)

τ = J T F = J T K x Δ x = ( J T K x J ) Δ θ = Kθ Δ θ fd = 1Nm

Kθ = J T (θ ) K x J (θ )
output~0
Break away

Force Sensing Dynamics

fs = k s x
fs
mx + k s x = f
f fd fs = k s x
m s fs
m
fs + fs = f fs = k s x
s ≡ ; fs = k s x ks Control
mx + k s x = f m
fs fd + ( − k p′ f ( fs − fd ) − kv′f fs )
ks
At static Equilibrium Closed Loop
fs = fd ⇒ f = fd m
Dynamics [ fs + kv′f fs + k p′ f ( fs − fd )] + fs = fd
mx + k s x = fd + f Dynamic
ks

13
 Steady-State error
m Task Description
( fs + kv′f fs + k p′ f ( fs − fd )) + ( fs − fd ) = 0
ks
fdist
f s = fs = 0
mk p′ f
( + 1)e f = fdist
ks
fdist
ef =
mk p′ f
1+
ks

Task Specification Unified Motion & Force Control

F = Ω Fmotion + Ω Fforce

Selection matrix

⎛ 1 0 0⎞
⎜ ⎟
Ω = ⎜ 0 1 0⎟; Ω = I − Ω Two decoupled Ω ϑ = Ω Fmotion
*

⎜ 0 0 0⎟ Subsystems
⎝ ⎠ Ω ϑ = ΩFforce
*

14