Control

Natural Systems

Video Segment
BigDog Reflexes
Boston Dynamics

PID Control
Joint-Space Dynamic Control
Task-Oriented Control
Force Control

Joint-Space Control

Task-Oriented Control

Robot Control

Natural Systems

Joint Space Control

q1

xd

q2

Inv. qd
Kin.

qn

Control

Control

Control

Joint 1

Joint 2

Joint n

Conservative Systems

q1

q2

x J ( )

Resolved Motion Rate Control

q2

1 2
( kx )
x 2

Natural Systems
Conservative Forces

0
1
V Work (kx) x kx 2
x
2

-1

Potential Energy of a spring

J 1x

m
x F kx

x f ( )
x x d x

xd
x

1
K mx 2
2

d K K V
( )

dt x x
x

J 1 ( )x

Natural Systems
Conservative Forces

Outside singularities

q1

d (K V) (K V)
0
(
)
dt x
x

qn

Resolved Motion Rate Control (Whitney 72)

Arm at Configuration

Control

Joint 1

Control

Joint 2

q1

q2

k
m

d K K V
( )

dt x x
x

Potential Energy of a spring

qn

Control

Forward
Kinematics

Joint n

qn

0
1
V Work (kx) x kx 2
x
2

m
x F kx

1 2
( kx )
x 2

Dissipative Systems

Natural Systems
Conservative Systems

k
m

d (K V) (K V)
(
)
0
dt x
x

Conservative Systems

Frequency increases
with stiffness
and inverse mass
Natural Frequency

x 2n x 0

b
2 n
m

b
k
x x 0
m
m
b
m 2
n2
n
x

Natural damping ratio

b
2 n m

b
2 km

Critically damped system: n 1 ( b 2 km )

Natural Systems

Time Response

x 2 n n x 2n x 0

d (K V) (K V)
(
)
f friction
dt
x
x
Viscous friction:

Over
damped

mx bx kx 0

Critically
damped
when
b/m=2n

k
m

Higher b/m

t
Critically
damped

2d order systems

2 n

x (t ) c cos( n t )

Dissipative Systems

k
b
x x 0
m
m

1 2
kx
2

m
x kx 0

mx bx kx 0

Friction

x
Oscillatory
damped

Natural Systems

1
K mx 2
2
1
V kx 2
2

m
x kx 0

f friction bx

m
x

Friction

Natural
frequency

b
k
; n
2 km
m

Natural
damping
ratio

x (t ) ce n n t cos( n 1 2n t )

x(t)

mx bx kx 0
t
2
n 1 2n

damped
Natural
frequency

n 1 n2

Example
k

x
x

m
x

mx bx kx 0
what is the damped Natural frequency

n 1
n

k
2;
m

m 2.0
b 4.8

k 8.0

Asymptotic Stability
d K K Vgoal
a system
0Fs

dt x
x
is asymptotically stable if

2
n

FsT x 0 ; for x 0

b
0.6
2 km

Fs kv x kv 0

Control

F k p x xgoal kv x

2 1 0.36 1.6

Proportional-Derivative Control (PD)

1-dof Robot Control

m

Potential Field

mx f k p ( x x d ) kv x
mx kv x k p ( x xd ) 0

mx f

f
xd

x0

V(x)

Velocity gain

V ( x ) 0, x x d
x

x0
xd
V ( x ) 0, x x d
1
V
V ( x ) k p ( x x d ) 2 ; f V ( x )
x
2
1
2
mx [ k p ( x x d ) ] ; mx k p ( x x d ) 0
x 2

Position gain

Passive Systems (Stability)

1
k p x xg x xg
2
d K K
f
System

dt x x
Vgoal
f
X
Conservative Forces
K

V
goal 0
d K

Stable
dt x
x
Vgoal

Fs

x
1.

Position gain

kp
kv
x (x xd ) 0
m
m

1.
x 2 x 2 (x xd ) 0

kv
closed loop
2 k p m damping ratio

k p closed loop
m frequency

Gains

k p m 2
kv m(2 )

Gain Selection

F I k m
set G J
H K k m(2 )
2

Unit mass system

k p

kv 2

m - mass system

k p mkk p
kv mkkv

Control Partitioning

m (1.
x) m f

mx f

x d kv ( x x d ) k p ( x x d )
Control: f
Closed-loop System:

f kv x k p ( x xd )
f m[kvx kp (x xd )] m f

1. x f

( x xd ) k v ( x x d ) k p ( x x d ) 0

m
xm f

Trajectory Tracking
x d (t ); x d (t ); and xd (t )

with e x xd

e kve k p e 0

unit mass system

1.
x kv x k p ( x xd ) 0
2

Disturbance Rejection

Non Linearities
mx b( x, x ) f
Control Partitioning

f f

xd +

with

mx b ( x , x ) f

xd

x d

b ( x , x )
b ( x , x )
mx b ( x , x ) mf

k p

System

kv

+
-

1. x f
f

b( x , x)

e kve k p e 0
+
+

System

( x , x)

b ( x , x )

Unit mass system

Motion Control

Disturbance Rejection

mx b( x, x ) f

1. x f
f mf b

Goal Position (xd):

f kv x k p ( x x d )
Control:
Closed-loop System:

1. x kv x k p ( x x d ) 0

mx b ( x , x ) f

xd

xd +

x d

k p

fdist
f

kv

+
-

x
System

b( x , x)

m
x b ( x , x ) f f dist
Control

f mf b ( x , x )

Closed loop

e kve k p e

fdist
m

Steady-State Error

Gear Reduction

f
e kve k p e dist
m
The steady-state

m
Motor Im

e e 0 :

fdist
m
f
f
e dist dist
kp
mk p

m I m m

m ( Im

m
x

kv

fdist k p x
f
x dist
kp

Control

k p ( x x d ) fdist
f
x x d dist
kp

Effective Damping

I L I L (q)

Direct Drive

Gain Selection

k p ( I L 2 Im )k p
kv ( I L 2 I m )kv

Time Optimal Selection

1
IL ( I Lmin I Lmax ) 2
4

Closed Loop
Stiffness

f m f b ( x , x )

Manipulator Control
m2
l2

f
xd kv ( x x d ) k p ( x xd ) ki ( x xd )dt

f
e kve k p e ki edt dist
m

e kve k p e kie 0

Steady-state Error

I L
b
) m ( bm L2 ) m

for a manipulator

mx b ( x , x ) f f dist

Closed-loop System

L m

Ieff I L 2 I m

PID (adding Integral action)

System

1
( I L L ) bm m bL L

Effective Inertia

fdist

fdist

Effective Inertia

mx kv x k p ( x x d ) 0

kp

L ( ) m

L m

IL

R
r

L ( I L 2 I m ) L ( bL 2 bm ) L

Closed loop
position
gain (stiffness)

Steady-State Error - Example

Link

r
R

k p e

Gear ratio

e0

M ( ) V ( , ) G ( )

2
m1

l1
1

FG m
Hm

11
21

m12
m22

IJ FG IJ FG m IJ d
K H K H 0 K
1
2

112

F 0
i GH m
2

112

m122
0

I F I F G I F I
JK GH JK GH G JK GH JK
2
1
2
2

m111 m12 2 m112 1 2 m122 22 G1 1

m
m22 2 m211 112 12 G2 2
2

PD Control Stability
d

G1

kv1
+

k p1

Link1(m11)

m12 2 m122 22 m122 1 2

+
-

k p2

m
m211 112 12
2

+
+

Link2(m22)

kv2

q1d

] C(q)[q2 ] G()
M(q)q B(q)[qq
1

k p (q qd ) kv q

Vd 1/ 2kp (q qd )2

Vd
d K
K Vs

k v q
(
)

q
d t q
q
q

G2

m 11

PD

q1 , q1

Joint 1

PD Control Stability

] C(q)[q2 ] G()
M(q)q B(q)[qq
q2 d

m 22

PD

Joint 2

q2 , q 2

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

qnd

m nn

PD

Joint n

i , j 1 m ij q j b1 g1

q1d

PD

m 11

Joint 1

i , j 2 m ij qj b2 g 2

q2 d

PD

m 22

Joint 2

i , j n m ij q j bn g n

qnd

PD

m nn

Joint n

qn , q n

q1
q1 , q1

q2

Performance
D
Y
N
A
M
I
C

q2 , q2

qn

qn , q n

d K
K (V s V d )
(
)

s
d t q
q
q
s k v q with sT q 0 for q 0; k v 0

C
O
U
P
L
I
N
G

High Gains

better disturbance rejection

Gains are limited by

structural flexibilities
time delays (actuator-sensing)
sampling rate

n
n
n

res

lowest structural flexibility

delay
3

largest delay

F 2 I
GH JK
delay

sampling rate
5

Joint Space Control

Nonlinear Dynamic Decoupling

M ( ) V ( , ) G ( )
M ( ) V ( , ) G ( )
1. ( M 1 M ) M 1 [(V V ) ( G G )]
with perfect estimates

1. (t )

: input of the unit-mass systems

d kv ( d ) k p ( d )
Closed-loop

E kv E k p E 0 (t )

Joint Space Control

d

qd
q d
qd

e
+
-

kv

e
+

(q )
M

+
+

Arm

kp

a f

( q, q) C q, q G
( q)
B

Operational Space Control

V ( xGoal )

Task Oriented Control

F V ( xGoal )
JTF

Unified Motion & Force Control

Equations of Motion
d L L
F

dt x x
with

Fmotion

L( x, x ) K ( x, x ) U ( x)

x

y
z
x

F Fmotion Fcontact
Fcontact

Operational Space Dynamics

Operational Space Dynamics

M x ( x)
x Vx ( x, x ) Gx ( x) F

M x x Vx Gx F
F F[M x ,Vx , G x ,V ( xGoal )]

Task-Oriented Equations of Motion

{n 1}

{0}

0n1

Non-Redundant Manipulator ; n m

x x1 x2 xm

q q1 q2 qn

End-Effector Position and

Orientation
M x ( x) : End-Effector Kinetic Energy Matrix
Vx ( x, x ) : End-Effector Centrifugal and
Coriolis forces
Gx ( x) : End-Effector Gravity forces
End-Effector Generalized forces
F:

x:

Joint Space/Task Space

Relationships
Kinetic Energy

K x ( x, x ) K q (q, q )
1 T
1
x M x ( x) x q T M (q )q
2
2
x J (q)q
Using
1 T T
1
q ( J M x J ) q q T M q
2
2

Joint Space/Task Space

Relationships
T

IC2

M x ( x ) J (q ) M (q ) J (q )

l1

Vx ( x, x ) J T (q) V (q, q ) M x (q ) h(q, q )

where h(q, q ) J (q )q

d 2 c1
x

d 2 s1
0

IC2

q2 d 2

l1

m2

m2
1

m1

IC1

d2

LMm l
N

2
11

I zz1 m2 d22 I zz 2
0

0
m2

m2 m2 y1

LMm l
N

2
11

I zz1 m2 d22 I zz 2

OP
Q

m2 m2

m2

x
1
m1
0

OP
Q

y1
g

IC2
m1

IC1

m2

d2

1 m11 0 0 1 d 2

0 0 m22 1 0

c1 s1 m2
Mx

s1 c1 0

Task Space
0

m2
x
1
m1
0 c1 s1
m2 m2 s1 c1

m m2 s12
Mx 2
m2 s c1

Joint Space

LMm l I
N
2
11

zz1

m2

m2

0
Mx
1 d 2

d s1 c1
J 2

d 2 c1 s1 1 J

0 1
c
1

s
1

0
J
l1

s1 c1 d 2 0
y
d
0
1

2
1 1

J
;
1 0

d2

I zz1 I zz 2 m1l12
d22

m
Mx 2
0

d s1 c1
J 2

d 2 c1 s1

m1

IC1

Gx ( x ) J T ( q ) G ( q )

Example

m2

m2 s c1

m2 m2 c12

m2d22 Izz2 0
0
m2

OP
Q

10

m
Mx 2
0

m2 m2

m2 m2

m2

Asymptotic Stability
d K K Vgoal
a system
0Fs

x
dt x
is asymptotically stable if

FsT x 0 ; for x 0
0

m2 m2 s12

m2 s c1

Fs

Fs kv x kv 0

m2 s c1

m2 m2 c12

Control

F k p x xgoal G x kv x

End-Effector Control

Example 2-d.o.f arm:

Non-Dynamic Control

M x ( x)
x Vx ( x, x ) Gx ( x) F
F

J T (q) F

Passive Systems (Stability)

1
Vgoal k p x xg x xg
2
d ( K V ) ( K V )
F
System

x
x
dt

F
Vgoal V
X
Conservative Forces
d K K Vgoal
0

Stable
dt x
x
T

F k p x xg kv x G ( x)

l2
l1

q2

q1

m c 12 m x m

y Vx1 k p x xg kv x

m c 12 m y m

x Vx 2 k p y y g kv y

* 2
1

* 2
1

*
1

*
1

Closed loop behavior

m11 ( q) x kv x k p x x g m1*
y Vx 1
m22 ( q) y kv y k p y y g m1*
x Vx 2

11

Nonlinear Dynamic Decoupling

In joint space

Model

M x ( x)
x Vx ( x, x ) Gx ( x) F

q kv' q k p' q 0

Control Structure

F M ( x) F ' Vx ( x, x ) G x ( x)

q q qd

with

Decoupled System

I
xF'
with

JTF

Perfect Estimates

Task-Oriented Control

I
xF'

F ' input of decoupled end-effector

Goal Position Control

xd
x d xd

e
+
-

kv

e
+

M x ( q )

a f

JT q

Arm

kp

F ' kv' x k p' x xg

af
J aq f

Kin q
q

Vx ( q , q ) G x ( q )

Closed Loop

I
x kv' x k p' ( x xg ) 0

Trajectory Tracking
xd
Trajectory: xd , xd ,
F ' I
xd kv' ( x xd ) k p' ( x xd )

(
x
xd ) kv' ( x xd ) k p' ( x xd ) 0
or

x k k 0
'
v x

with

'
p x

x x xd

Compliance
k p
x

F 0

I
x F
0
k p y
0

0 ( x x d ) k v x

k p z
set to zero

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

12

Stiffness

Dynamics

mx k s x f

z kv z k pz ( z zd ) 0
determines stiffness along z

M xkp k p
F Kx (x xd )

Closed-Loop Stiffness:

fd

J T F J T K x x ( J T K x J ) K
K J T ( ) K x J ( )

Force Control
1-d.o.f.
f

fd

mx f
set

f fd

Problem
friction

Coulomb friction

10(N.m)

fd 1Nm
output~0

Break away

Force Sensing
m

fs

Closed Loop

m
( k p f ( fs fd ) kvf fs )
ks

m
[ fs kvf fs k p f ( fs fd )] fs fd
ks

Steady-State error

fs
m
fs fs f
ks
Control

fs k s x
fs ks x
f k
x

m
( fs kvf fs k p f ( fs fd )) ( fs fd ) 0
ks
fdist
f f 0
s
s

mk p f
ks

1)e f fdist
ef
1

fdist
mk p f
ks

Task Description
fd

; fs k s x

mx k s x f
fs
At static Equilibrium

fs fd f fd

Dynamics

mx k s x fd f Dynamic

13

Task Specification
F Fmotion Fforce
Selection matrix

1 0 0

0 1 0; I
0 0 0

Unified Motion & Force Control

Two decoupled
Subsystems

*
Fmotion
*
Fforce

14