Control

1Video SegmentBigDog Reflexes Boston Dynamics

Robot Control

PID Control Joint-Space Dynamic Control

C o n t r o l Natural Systems

Task-Oriented Control Force Control

Joint-Space Control

Task-Oriented Control

2Joint Space Control

Inv.Kin.

xd qd q

Control

Control

Control

Joint n

Joint 2

Joint 1q1

qn

q2 q2

qn

q1

Resolved Motion Rate Control (Whitney 72)

x J ( )Outside singularities

J x1( )Arm at Configuration

x f ( )x x xd J x1

Resolved Motion Rate Control

xJ -1

q q

Control

Control

Control

Joint n

Joint 2

Joint 1q1

qn

q2 q2

qn

q1

ForwardKinematics

xd x

Conservative Systemsx

km

Natural Systems

( ) ( )( ) 0ddt x

KV Vx

K 2

12

K mx

Conservative Forces

Fm xx k

x

km

Natural Systems

( )d K Kdt x x

Vx

0 21( )2x

V Work kx x kx Potential Energy of a spring

21( )2kx

x

Conservative Forces

Fm xx k

x

km

Natural Systems

( )d K Kdt x x

Vx

0 21( )2x

V Work kx x kx Potential Energy of a spring

21( )2kx

x

3Conservative Systems

0k xm x

x

km

Natural Systems

( ) ( )( ) 0ddt x

KV Vx

K

212

V kx

212

K mx

Conservative Systems

Natural Systems

0k xm x V kx 1

22

x

t

Frequency increaseswith stiffnessand inverse mass

n kmx xn 2 0

x t c tn( ) cos( )

Natural Frequency

Dissipative Systemsx

kmx x x x Friction

Viscous friction: f bxfriction mx bx kx 0

Natural Systems

( ) ( )( ) frictiond fdt x x

K VKV

Dissipative Systemsk

mx x x x Friction mx bx kx 0 x b

mx k

mx 0

2 nbm

x

tOscillatorydamped

x

tCriticallydamped

x

tOverdamped

Higher b/m

n2

2d order systems mx bx kx 0 x b

mx k

mx 0

bmn2

n2Natural damping ratio

Critically dampedwhenb/m=2n

n nbm

2

bkm

2

Critically damped system: n b km 1 2 ( )

Time Response x x xn n n 2 02 n km ; n

bkm

2

Naturalfrequency

Naturaldampingratio

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

dampedNaturalfrequency

x(t)

t

21 2

n n

21 nn

4kmx x x x b

x

mx bx kx 0m 2 0.b 4 8.k 8 0.what is the damped Natural frequency

n n1 2 n km 2 ; n

bkm

2

0 6.

2 1 0 36 1 6. .

Example

1-dof Robot Controlm f

x0 xd

mx f V(x)

x0 xdx

Potential Field

V x x xV x x x

d

d

( ) ,( ) ,

00

V x k x xp d( ) ( ) 122 ; f V x V

x ( )

mxx

k x xp d [ ( ) ] 12

2 ; mx k x xp d ( ) 0Position gain

12 Tgoal p g gV k x x x x System

d K Kdt x x

f goalVfX

Passive Systems (Stability)

0goal

Kd Kdt x x

V StableConservative Forces

Asymptotic Stability

is asymptotically stable if

0 ; 0Ts x fF or x 0s v vF k kx

p goalk x xF Control

a system

x

0goal

Kd Kdt x x

V goal

s

Kd Kdt x x

VF

vk x

sF

Proportional-Derivative Control (PD)mx f k x x k xp d v ( )

( ) 0v p dmx k x k x x

1. ( ) 0pv dkkx x x x

m m

21. 2 ( ) 0dx x x x k

mp k

k mv

p2closed loopdamping ratio

closed loopfrequency

Velocity gain Position gain

Gainsk m

k mp

v

2

2( )Gain Selection

set

FHGIKJ

k m

k mp

v

2

2( )

k

kp

v

2

2

Unit mass system m - mass systemk mk

k mkp

v

k

kp

v

5Control Partitioningmx f (1. )x fm m

( )v p df k x k x x

fm mx f 1. x f unit mass system

1. ( ) 0v p dx k x k x x 2 2

[ ( )]v p df k x k x x fm m

Non Linearities( , )mx b x fx

Control Partitioningf f

with

, ( , )( ) mf b x xmx b x x 1. x f

Systemf ( , )x x

++

( , )b x x

mf

Unit mass system

( , )

m

b x x

Motion Control

( , )mx b x x f 1. x ff mf b

Goal Position (xd):Control:Closed-loop System:

f k x k x xv p d ( )

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

Trajectory Trackingx t x t x td d d( ); ( ); ( )and

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

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

e k e k ev p 0

e k e k ev p 0

+-

+-

++

dx

dx

dxkpkv

f m Systemb x x( , )

xx

f

( , )mx b x x f Disturbance Rejection

+-

+-

++

dx

dx

dxkpkv

f m Systemb x x( , )

xx

f

( , )mx b x x f Disturbance Rejection

fdist

( , ) distmx b x x ff ( , )f mf b x x

e k e k e fmv pdist

Control

Closed loop

6 0 :e e k e f

mpdist

e fmkdist

p

fkdist

pClosed looppositiongain (stiffness)

Steady-State Error

The steady-state

e k e k e fmv pdist

mx k x k x xv p d ( ) 0m

f fdist

kpmx x x x kv

fdistk x x fp d dist( ) x x f

kddist

p

xf k xdist p

x fkdist

p

Closed LoopStiffness

Steady-State Error - Example

PID (adding Integral action)

( , )mx b x fx distf( , )f mf b x x

zf x k x x k x x k x x dtd v d p d i d ( ) ( ) ( ) e k e k e k edt f

mv p idist z

e k e k e k ev p i 0e 0

System

Control

Closed-loop System

Steady-state Error

Rr

Motor

m LIm

IL

Linkr

R

Gear ratio

( )

L m

L m

1

m m m L L m m L LI I b b ( ) 1 1

L m1

m mL

m mL

mII b b ( ) ( ) 2 2

L L m L L m LI I b b ( ) ( ) 2 2Effective Inertia Effective Damping

Gear Reduction

Effective InertiaI I Ieff L m 2

for a manipulatorI I qL L ( )

1Direct Drive

Gain Selection

k I I k

k I I kp L m p

v L m v

( )

( )

2

2

Time Optimal Selection

( )min max

I I IL L L 142

Manipulator Control

M V G( ) ( , ) ( )

m mm m

m mm

GG

11 12

21 22

1

2

1121 2

122

11212

22

1

2

1

20

0

20

FHG

IKJFHGIKJ FHGIKJ

FHG

IKJFHGIKJ FHGIKJ FHGIKJ

d i

m m m m G

m m m G

11 1 12 2 112 1 2 122 22

1 1

22 2 21 1112

12

2 22

m2

l2 2m1l1

1

7+- d1 kp1 Link1(m11)++

kv1

Link1(m11) 1++

d2 kp2 Link2(m22)++- +kv2

2

G1

G2

1

2 2

1m m m12 2 122 22 122 1 2 m m21 1 112 1

2

2

Joint 1PD m11q d1 q q1 1,

Joint nPD mnnqnd q qn n,


Joint 1PD m11q d1 q q1 1, D

YNAMIC

COUPLING

i j ij jm q b g, 1 1 1 q1

Joint nPD mnnqnd q qn n,

qn


q2 i j ij jm q b g, 2 2 2

i j n ij j n nm q b g,

2( ) ( )[ ] ( )[ ] ( )M q q B q qq C q q G ( )p d vk q q k q

( ) sVd K Kdt q q q

PD Control Stability

d vV k qq

21/2 ( )d p dV k q q

2( ) ( )[ ] ( )[ ] ( )M q q B q qq C q q G ( )p d vk q q k q

( )( ) s dV Vd K Kdt q q q

PD Control Stability

s

1/2 ( ) ( )Td p d dV k q q q q

s vk q with 0 0; 0Ts vq for q k

PerformanceHigh Gains better disturbance rejection

Gains are limited bystructural flexibilitiestime delays (actuator-sensing)sampling rate

nres

ndelay

nsampling rate

2

3

5

lowest structural flexibility

largest delay2 delayFHGIKJ

8[( ) ( )] M V V G G1 ( )M M11.

Nonlinear Dynamic DecouplingM V G( ) ( , ) ( )

( ) ( , ) ( )M V G

with perfect estimates( ) t1.

: input of the unit-mass systems ( ) ( )d v d p dk k

Closed-loop

( )t E k E k Ev p 0

-

( )M q Arm

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

kvkp

qdqdqd

q

q

e+

- e

+

d+

++

Task Oriented Control

Joint Space Control

Joint Space Control

F

( )GoalV xF

( )GoalV x

TJ F

Operational Space Control

9Unified Motion & Force Control

motion contactF F F contactF

motionF

F

[ , , , ( )]x x x GoalM V G V xF F

x

xM x Fx xV G

Operational Space Dynamics

Task-Oriented Equations of Motion

Non-Redundant Manipulator ; n m 1 2 Tmx x x x 1 2 Tnq q q q

{0}{ 1}n

10n

Equations of Motiond L L Fdt x x

with

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

xyz

x

( ) ( , ) ( )x x xM x x V x x G x F Operational Space Dynamics

End-Effector Centrifugal and Coriolis forces

( , ) :xV x x( ) :xG x End-Effector Gravity forces:F End-Effector Generalized forces

( ) :xM x End-Effector Kinetic Energy Matrix

:x End-Effector Position andOrientation

Joint Space/Task Space Relationships

( , )xK x x 1 ( ) 1 ( )

22TT

xx M x x q M q q ( , )qK q q

Kinetic Energy

( )x J q q Using ( )1 1

2 2T TT

x MJ M Jq q q q

10

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

1( ) () ( )( )xT M qM qq Jx J

( , ) ( )) ( )( ,( ) ,Tx xV qV x x M qqJ q h q q

( )( () )TxG J q G qx

Joint Space/Task Space Relationships

Example

2

2

11

d cx

d s

20

2

1 11 1

d s cJ

d c s

2 2q dg

l1y

1CI

2CI

m1

m2

d2

x

20

2

1 11 1

d s cJ

d c s

21 1 0 1 ;1 0

dJ

11 21

2 22

0 1 0 0 11 0 0 1 0x

m dM

d m

0

2

0 11 101 1

c sJ

ds c

1 J g

l1y

1CI

2CI

m1

m2

d 2

x

Mm l I m d I

mzz zz L

NMOQP

1 12

1 2 22

2

2

00

g

l1y

1CI

2CI

m1

m2

d2

x

21

2 2

00xm

Mm m

21 2 1 1

2 22

zz zzI I m lmd

2 2m m

1x1y

1m2m

2m

Mm l I m d I

mzz zz L

NMOQP

1 12

1 2 22

2

2

00

20

2 2

01 1 1 101 1 1 1xmc s c s

Mm ms c s c

2

0 2 2 22

2 2 2

1 11 1x

m m s m s cM

m sc m m c

Mml I m d I

mzz zz L

NMOQP

1 12

1 2 22

2

2

00

Task Space

Joint Space

1x1y

1m2m

11

2m2 2m m

20 2 2 2

22 2 2

1 11 1

m m s m s cm sc m m c

21

2 2

00xm

Mm m

End-Effector Control

( )T FJ q F

12 Tgoal p g gV k x x x x System

( ) ( )d K Kdt x

V V Fx

Passive Systems (Stability)

0goal

Kd Kdt x x

V StableConservative Forces

goalF V VX

Asymptotic Stability

is asymptotically stable if

0 ; 0Ts x fF or x 0s v vF k kx

p goal xx GF k x Control

a system

x

0goal

Kd Kdt x x

V goal

s

Kd Kdt x x

VF

vk x

sF

Example2-d.o.f arm:Non-Dynamic Control

( )p g vk x x k GF x x 1q1

l

2q2l

( ) ( , ) ( )x x xM x x V x x Fx G

Closed loop behavior

111 * 1( ) v p g xm q x k x k x Vx m y 122 * 2( ) v p g xm q y k y k y Vy m x

* 2 *1 2 1 112 p gx vk x x k xm c m x m y V * 2 *1 2 1 212 p gx vk y y k ym c m y m x V

12

Nonlinear Dynamic Decoupling

with T FJ

( ) ( ' , ) ( )x xx xF M F V x xG Control Structure

'I Fx Decoupled System

Model( ) ( , ) ( )x x xM x x V x x Fx G

Perfect Estimates'I Fx

input of decoupled end-effector'F

Goal Position Control

' ' 'v p gF k x k x x Closed Loop

' ' ( ) 0v p gI x k x k x x

Trajectory TrackingTrajectory: , , d d dx x x

' '' ( ) ( )d v d p dF I x k x x k x x ' '( ) ( ) ( ) 0d v d p dx x k x x k x x

with

or

x dx x ' ' 0x v x p xk k

In joint space

' ' 0q v q p qk k

with q dq q

Task-Oriented Control

-

( )M qx J qT a f ArmKin qa f

J qa f

( , ) ( )V q q G qx x

kvkp

xdxdxd

q

qx

xF

e

+- e

+

Compliance I x F 0 0

0 0 ( )

0 0

x

y

z

p

p d v

p

k

k x x k x

k

F

set to zero

' '

' '

'

( ) 0

( ) 0

0

v px d

v py d

v

x k x k x x

y k y k y y

z k z

Compliance along Z

13

Stiffness( ) 0

zv p dz k z k z z

x p pM k k

determines stiffness along z

Closed-Loop Stiffness:

( )x dF K x x J F J K x J K J KT T x T x ( )

K J K JT x ( ) ( )

mf

Force Control1-d.o.f. mx f

fdset f fdProblem

f Nmd 1

output~0

friction

Break away

Coulomb friction10(N.m)

Force Sensing

mfdf s

fs

s ; f k xs smx k x fs

fsAt static Equilibrium

f fd f fs dDynamics

f fd Dynamicmx k xs

Dynamicsmx k x fs mk

f f fs

s s

f mk

k f f k fds

p s d v sf f ( ( ) )

Closed Loopmk

f k f k f f f fs

s v s p s d s df f[ ( )]

f k x

f k x

f k x

s s

s s

s s

Control

sf

Steady-State errormk

f k f k f f f fs

s v s p s d s df f( ( )) ( ) 0

fdist f fs s 0( )mkk

e fps

f distf 1e fmk

k

fdist

p

s

f

1

Task Description

14

Task Specification

motion forceF F F

1 0 00 1 0 ;0 0 0

I

Selection matrix

Unified Motion & Force Control

Two decoupled Subsystems

*motionF *forceF

Documents

Control