Video Segment

Juggling Robot, Dan Koditschek University of Michigan, ISRR93 video proceedings

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

Robot Control

Joint-Space Control

Task-Oriented Control

Joint Space Control

q1 Control Joint 1 q1

Resolved Motion Rate Control (Whitney 72)

x = J ( )
Outside singularities
q2

xd

Inv. qd Kin.

q2

Control

Joint 2

= J 1 ( )x
Arm at Configuration

qn

Control

Joint n

qn

x = f ( ) x = x d x

= J 1x

+ = +

Resolved Motion Rate Control

q1 Control Joint 1 q1

Natural Systems
Conservative Systems
k
m x

xd x

J -1

q2

Control

Joint 2

q2

qn

Control

Joint n

qn

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

1 K = mx 2 2

Forward Kinematics

Natural Systems
Conservative Forces
k
m x

Natural Systems
Conservative Forces
k
m x

d K K V ( ) = dt x x x

d K K V ( ) = dt x x x
Potential Energy of a spring
0 1 V = Work = (kx) x = kx 2 x 2

m x = F = kx
Potential Energy of a spring

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

1 2 ( kx ) x 2

m x = F = kx

1 2 ( kx ) x 2

Natural Systems
Conservative Systems
k
m x

Natural Systems
Conservative Systems

V=

1 2 kx 2

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

m x + kx = 0
Frequency increases with stiffness and inverse mass Natural Frequency

m x + kx = 0

1 K = mx 2 2 1 V = kx 2 2

n =

x + 2x = 0 n

k m

x (t ) = c cos( n t + )

Natural Systems
Dissipative Systems k
x

Dissipative Systems
k
m
x x x

Friction

m
x x

Friction

mx + bx + kx = 0

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

x+

k b x+ x=0 m m
x t
Critically damped

f friction = bx
t
Oscillatory damped

x t
Over damped

mx + bx + kx = 0

Higher b/m

b = 2 n m

2d order systems

mx + bx + kx = 0
x+

Time Response
Natural frequency

2 n

b k x+ x=0 m m b m 2 n2 n
Natural damping ratio

x + 2 n n x + 2 x = 0 n
n =
b k ; n = 2 km m
Natural damping ratio

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

Critically damped when b/m=2n

n =

b 2 n m

b 2 km

t
2 n 1 2 n

Critically damped system: n = 1 ( b = 2 km )

damped Natural frequency

= n 1 n2

Example
k
x

m
x x

mx + bx + kx = 0
what is the damped Natural frequency
2 = n 1 n

m = 2.0 b = 4.8

k = 8.0

Video Segment
Tactile Sensing, H. Maekawa et al. MEL, AIST-MITI, Tsukuba, Japan ICRA93 video proceedings

n =

k =2; m

n =

b = 0.6 2 km

= 2 1 0.36 = 1.6

1-dof Robot Control

m
x0

f
xd

mx = f

Potential Field

V(x)

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)

T 1 k p ( x xg ) ( x xg ) 2 d K K = f System dt x x Vgoal f = X Conservative Forces ( K Vgoal ) d K =0 dt x x Stable

Vgoal =

Asymptotic Stability d K ( K Vgoal ) a system = 0s F dt x x

is asymptotically stable if

FsT x < 0 ; for x 0

Control

Fs

Fs = kv x kv > 0

F = k p ( x xgoal ) kv x

Proportional-Derivative Control (PD)

Gains

mx = f = k p ( x x d ) kv x mx + kv x + k p ( x xd ) = 0
Velocity gain Position gain

k p = m 2 kv = m(2 )

Gain Selection

1. x +

kp kv x + (x xd ) = 0 m m
=
k p closed loop m frequency

set

FG IJ k = m H K k = m(2 )
2 p v

1. x + 2 x + 2 (x xd ) = 0
kv closed loop = 2 k p m damping ratio

Unit mass system

kp =

m - mass system

k p = mkk p kv = mkkv

kv = 2

Control Partitioning

mx = f

m (1. x ) = m f

Non Linearities mx + b ( x , x ) = f
Control Partitioning
with

f = kv x k p ( x xd )
f = m[kv x k (x xd )] = m f p

mx = m f

f = f + =m = b( x, x )

1. x = f
2

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

unit mass system

mx + b( x , x ) = mf + b ( x , x ) 1. x = f
f

+ +

System
b( x, x )

( x, x )

Unit mass system

Motion Control

mx + b ( x , x ) = f 1. x = f
f = mf + b

Disturbance Rejection mx + b( x , x ) = f x
d

Goal Position (xd):

Control: f = k v x k p ( x x d ) 1. x + kv x + k p ( x x d ) = 0 Closed-loop System:

xd

+ + -

kp

f
+ System

x
x

xd

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 ) Closed-loop System: ( x x d ) + k v ( x x d ) + k p ( x x d ) = 0

with e x x d

kv

b( x , x )

e + kve + k p e = 0

e + kve + k p e = 0

Disturbance Rejection mx + b( x , x ) = f x
d

Steady-State Error
fdist
System

e + kve + k p e =
x
x

xd

+ + -

kp

f++
+

xd

The steady-state

( e = e = 0) :

fdist m

kv

b( x , x )

mx + b ( x , x ) = f + fdist
Control Closed loop

f = mf + b ( x , x )

{ t fdist < a}

bounded

e + kve + k p e =

fdist m

fdist m f f e = dist = dist kp mk p k pe =

Closed loop position gain

Steady-State Error - Example

PID (adding Integral action)

System Control

f
m

fdist

mx + kv x + k p ( x x d ) = 0
x

mx + b ( x , x ) = f + fdist

f = mf + b ( x , x )

kp

m
x x

fdist
x

kv

fdist = k p x f x = dist kp

k p ( x x d ) = fdist f x = x d + dist kp
x
Closed Loop Stiffness

f = xd kv ( x xd ) k p ( x xd ) ki ( x xd )dt
Closed-loop System

e + kve + k p e + ki edt =

Steady-state Error

e=0

Gear Reduction
m
Motor Im
r R

Gear ratio = Link

R r

Effective Inertia

IL
1

L = ( ) m L = m
1

Ieff = I L + 2 I m
for a manipulator

=1
Direct Drive

I L = I L (q)

m = I m m +

( I L L ) + bm m +
L = m
1

bL L

Gain Selection

k p = ( I L + 2 I m )k p kv = ( I L + 2 I m )kv
Time Optimal Selection

m = ( Im +

IL

) m + ( bm +

bL

) m

L = ( I L + 2 I m ) L + ( bL + 2 bm ) L
Effective Inertia Effective Damping

IL =

1 ( I Lmin + I Lmax ) 2 4

co ns

e + kve + k p e + kie = 0

ta

nt

fdist m

Video Segment
On the Run, Marc Raibert, MIT ISRR93 video proceedings

Manipulator Control
m2 l2 l1 1 2 m1

M ( ) + V ( , ) + G ( ) =

kv1
+

G1
+ +

k p1

1
m112 2 1 2

1
Link1(m11)

FG m Hm

11 21

m12 m22

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

2 +

F 0 i GH m 2

m122 0

112

I F I F G I F I JK GH JK + GH G JK = GH JK
2 1 2 2 1 2 1 2

2 m12 2 + m122 2 + m122 1 2

1
2

m21 1
2

+ -

k p2
kv2

+ +

Link2(m22)

m11 1 + m12 2 + m112 1 2 + m122 2 + G1 = 1 2 m22 2 + m21 1 m112 2 1 + G2 = 2 2

G2

i , j 1 m ij q j + b1 + g1

q1d

PD

m11

Joint 1

q1 , q1

q1d

PD

m11

Joint 1

q1 q1 , q1

q2 d

PD

m22

Joint 2

q2 , q2
q2 d

i , j 2 m ij q j + b2 + g 2

q2

D Y N A M I C

PD

m22

Joint 2

q2 , q2
C O U P L I N G

qnd

PD

mnn

Joint n

qn , qn

i , j n m ij q j + bn + g n

qn

qnd

PD

mnn

Joint n

qn , q n

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
Vd = 1/ 2kp (q qd )2

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

Vd d K K Vs kvq ( ) + = q dt q q q

d K K (V s V d ) ( ) + =s dt q q q s = k v q with sT q < 0 for q 0; k v > 0

Performance
High Gains better disturbance rejection Gains are limited by structural flexibilities time delays (actuator-sensing) sampling rate

Nonlinear Dynamic Decoupling M ( ) + V ( , ) + G ( ) =

= M ( ) + V ( , ) + G ( )
1. = ( M M ) + M 1 [(V V ) + ( G G )]
1

with perfect estimates

n n n

res
2

1. = + (t )

lowest structural flexibility

delay
3

sampling rate
5

F 2 I largest delay G H JK
delay

: input of the unit-mass systems = d kv ( d ) k p ( d )

Closed-loop

E + kv E + k p E = 0 + (t )

qd qd qd

d
+
e + -

kv

M (q)

Arm

e +

kp

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

a f

Task Oriented Control

Joint Space Control

Joint Space Control

Operational Space Control

Unified Motion & Force Control

V ( xGoal )

Fmotion

F = V ( xGoal ) = JTF

F = Fmotion + Fcontact Fcontact

Operational Space Dynamics

Task-Oriented Equations of Motion

{n + 1}

{0}

0n+1

F x M x x+ Vx + Gx= F
F = F[M x ,Vx , Gx ,V ( xGoal )]
Non-Redundant Manipulator ; n = m

q = ( q1 q2 qn )

x = ( x1 x2 xm )

Equations of Motion
d L L =F dt x x
with

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

x y z x=

Operational Space Dynamics

M x ( x) x + Vx ( x, x) + Gx ( x) = F
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

Joint Space/Task Space Relationships

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

M x ( x) = J T (q) M (q) J 1 (q)

Vx ( x, x) = J T (q) V (q, q ) M x (q ) h(q, q) Gx ( x ) = J T ( q ) G ( q )
where h(q, q)

J (q )q

Example
q2 = d 2 d 2 c1 x= d 2 s1
0

d s1 c1 J = 2 d 2 c1 s1

d s1 c1 J = 2 d 2 c1 s1

c1 s1 0 1 J = s1 c1 d 2 0 0 1 d2 1 1 J = ; 1 0 1 m11 0 0 1 d 2 0 1 Mx = 1 d 2 0 0 m22 1 0
0

10

m Mx = 2 0

m2 + m2 0
y1

m2 + m2

c1 s1 m2 Mx = s1 c1 0
m2 = m2 + m2
+

0 c1 s1 m2 + s1 c1

m2
m2
0

x 1 m1
2 11

m2 =

I 331 + I 332 + m l d22

m2 + m2 s12 Mx = m2 s c1

m2 s c1 m2 + m2 c12

End-Effector Control
m2 + m2

m2

F
0

m + m2 s1 = 2 m2 s c1

m2 s c1 m2 + m2 c12

= J T (q) F

Passive Systems (Stability)

T 1 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 dt x x Stable

Vgoal =

Asymptotic Stability d K ( K Vgoal ) a system = 0s F x dt x

is asymptotically stable if

FsT x < 0 ; for x 0

Control

Fs

Fs = kv x kv > 0

F = k p ( x xgoal ) + Gx kv x

11

Example 2-d.o.f arm: Non-Dynamic Control

( m c 12 + m ) x + m
* 2 1 2

* 1

y + Vx1 = k p ( x xg ) kv x x + Vx 2 = k p ( y y g ) kv y

M x ( x) x + Vx ( x, x) + Gx ( x) = F
F = k p ( x x g ) kv x + G ( x )
l2 l1
q2

( m c 12 + m ) y + m
* 2 1 2

* 1

Closed loop behavior

q1

m11 (q ) x + kv x + k p ( x xg ) = m1* y + Vx1

m22 (q) y + kv y + k p ( y y g
* 1

( ) = (m

x + Vx 2

) )

Nonlinear Dynamic Decoupling

Model

Perfect Estimates
I x=F'

M x ( x) x + Vx ( x, x) + Gx ( x) = F
Control Structure

F ' input of decoupled end-effector

Goal Position Control
' F ' = kv' x k p ( x xg )

F = M ( x) F '+ Vx ( x, x) + Gx ( x)
Decoupled System

I x=F'
with

Closed Loop
' I x + kv' x + k p ( x xg ) = 0

= JTF

Trajectory Tracking
Trajectory: xd , xd , xd

In joint space
' q + kv' q + k p q = 0

F ' = I xd k ( x xd ) k ( x xd )
' v ' p ' ( x xd ) + kv' ( x xd ) + k p ( x xd ) = 0

with

q = q qd

or

' x + kv' x + k p x = 0

with

x = x xd

12

Task-Oriented Control

Compliance
k p x F= 0 0 0 ky p 0
' v

I x = F
0 0 ( x x d ) k v x kz p
set to zero
' px

xd xd xd

e + -

kv

M x (q )

JT q

a f

Kin q
q

Arm

e +

kp

af J aq f

x
x

Vx ( q , q ) + G x ( q )

x + k x + k ( x xd ) = 0
' y + k v' y + k py ( y y d ) = 0

z + k v' z = 0
Compliance along Z

Stiffness

z + kv z + k z ( z zd ) = 0 p
determines stiffness along z

Force Control 1-d.o.f. f

m

fd

mx = f
set

Closed-Loop Stiffness: M x k = k p p F = Kx (x xd )

f = fd

Problem
friction
10(N.m)
Visc ous

Coulomb friction

= J T F = J T K x x = ( J T K x J ) = K
K = J T ( ) K x J ( )
Break away

fd = 1Nm
output~0

Force Sensing f
m

Dynamics fd
; fs = k s x

fs

mx + k s x = f

fs = k s x fs = k s x fs = k s x

mx + k s x = f fs At static Equilibrium
Dynamics

fs m fs + fs = f ks Control
fd +
Closed Loop

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

fs = fd f = fd

mx + k s x = fd + f Dynamic

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

13

Steady-State error

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

Task Description

mk p f ks

+ 1)e f = fdist
ef = 1+ fdist mk p f ks

Task Specification
F = Fmotion + Fforce
Selection matrix

Unified Motion & Force Control

1 0 0 = 0 1 0; = I 0 0 0

Two decoupled Subsystems

* = Fmotion * = Fforce

14