Robot Control - Stanford School of Engineering - Stanford

14
1 Video Segment Juggling Robot, Dan Koditschek University of Michigan, ISRR’93 video proceedings 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

Transcript of Robot Control - Stanford School of Engineering - Stanford

Page 1: Robot Control - Stanford School of Engineering - Stanford

1

Video SegmentJuggling Robot, Dan KoditschekUniversity of Michigan,ISRR’93 video proceedings

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

Page 2: Robot Control - Stanford School of Engineering - Stanford

2

Joint Space Control

Inv.Kin.

xd qd q

Control

Control

Control

Joint n

Joint 2

Joint 1δq1

δ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 1δq1

δqn

δq2 q2

qn

q1

ForwardKinematics

xd

x

Conservative Systemsx

km

Natural Systems

( ) ( )( ) 0ddt x

KV Vx

K∂ ∂∂ ∂− −

− = 212

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( )2

kxx∂∂

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( )2

kxx∂∂

Page 3: Robot Control - Stanford School of Engineering - Stanford

3

Conservative Systems

0kxm x + =

x

km

Natural Systems

( ) ( )( ) 0ddt x

KV Vx

K∂ ∂∂ ∂− −

− =

212

V kx=

212

K mx=

Conservative Systems

Natural Systems

0kxm x + =

V kx=12

2

x

t

Frequency increaseswith stiffnessand inverse mass

ω nkm

=

x 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 bm

x km

x+ + = 0

2 nbm

ω=

x

tOscillatorydamped

x

tCriticallydamped

x

tOverdamped

Higher b/m

n2ω⋅

2d order systems mx bx kx+ + = 0x b

mx k

mx+ + = 0

bm

n2ωω n

2

Natural damping ratioCritically dampedwhenb/m=2ωn

ξωn

n

bm

=2

bkm

=2

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

Time Responsex x xn n n+ + =2 02ξ ω ω

ω nkm

= ; ξ nbkm

=2

Naturalfrequency

Naturaldampingratio

x t ce tn ntn n( ) cos( )= − +−ξ ω ω ξ φ1 2

ωdampedNaturalfrequency

x(t)

t

21 2

πω ξn n−

21 nnωω ξ−=

Page 4: Robot Control - Stanford School of Engineering - Stanford

4

kmx 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

ω nkm

= = 2 ; ξ nbkm

= =2

0 6.

ω = − =2 1 0 36 1 6. .

Example

Video SegmentTactile Sensing, H. Maekawa et al.MEL, AIST-MITI, Tsukuba, JapanICRA’93 video proceedings

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( ) ( )= −12

2 ; f V x Vx

= −∇ = −( ) ∂∂

mxx

k x xp d[ ( ) ]= − −∂∂

12

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

( ) ( )12

T

goal p g gV k x x x x= − −

Systemd K Kdt x x

f∂ ∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠goalV

fX

∂⇓ = −

Passive Systems (Stability)

( )0goalKd K

dt x xV∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠ Stable

Conservative 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

( )0goalKd K

dt x xV∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠

( )goals

Kd Kdt x x

VF

∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠

vk x−

sF

Page 5: Robot Control - Stanford School of Engineering - Stanford

5

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

( ) 0v p dmx k x k x x+ + − =

1. ( ) 0pvd

kkx x x xm m

+ + − =

21. 2 ( ) 0dx x x xξω ω+ + − =ω =

km

pξ = kk mv

p2closed loopdamping ratio

closed loopfrequency

Velocity gain Position gain

Gainsk m

k mp

v

=

=

ω

ξω

2

2( )Gain Selection

set ξω

ωξω

FHGIKJ →

==

k mk m

p

v

2

2( )

′ =

′ =

k

kp

v

ω

ξω

2

2

Unit mass system m - mass systemk mk

k mkp

v

=

=

k

kp

v

Control Partitioningmx f= (1. )x fm m ′=

( )v p df k x k x x= − − −

′fm mx f ′=

1. x f= ′ unit mass system1. ( ) 0v p dx k x k x x′ ′+ + − =

2ξω ω 2

[ ( )]v p df k x k x x fm m′ ′ ′= − − − =

Non Linearitiesmx b x x f( , )+ =

Control Partitioningf f= ′ +α β

with

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

Systemf ( , )x x

++

( , )b x x

m′f

Unit mass system

β = ( , )b x xα = m

Motion Controlmx b x x f( , )+ = ⇒ = ′ 1. x f

f 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 e x xd≡ −

e k e k ev p+ ′ + ′ = 0

e k e k ev p+ ′ + ′ = 0

+-

+-

++

xd

xd

xd

′kp

′kv

′f m System

b x x( , )

xx

f

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

Page 6: Robot Control - Stanford School of Engineering - Stanford

6

Disturbance Rejectionmx b x x f( , )+ =

+-

+-

++

xd

xd

xd

′kp

′kv

′f m fSystem

b x x( , )

xx

++

fdist

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

{ }∀ <t f adist

boundedf mf b x x= ′ + ( , )

e k e k e fmv pdist+ ′ + ′ =

Control

Closed loop

( )0 :e e= =

′ =k e fmpdist

e fmk

dist

p

=′=

fkdist

pClosed looppositiongain

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 fkddist

p

= +

Δ xf k xdist p= Δ

Δx fkdist

p

=Closed LoopStiffness

Steady-State Error - Example PID (adding Integral action)mx b x x f( , )+ = + fdist

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 fmv p idist+ ′ + ′ + ′ =z

e k e k e k ev p i+ ′ + ′ + ′ = 0

e = 0

cons

tant

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 m=

1

τη

θη

θm mL

m mL

mI I b b= + + +( ) ( )2 2

τ η θ η θL L m L L m LI I b b= + + +( ) ( )2 2

Effective 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= +14

2

Page 7: Robot Control - Stanford School of Engineering - Stanford

7

Video SegmentOn the Run, Marc Raibert, MITISRR’93 video proceedings

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 θ2

m1l1θ1

+-θ d1 kp1

Link1(m11)+

+

kv1

Link1(m11)τ 1+

+

θ d2 kp2Link2(m22)

++- +

kv2

τ 2

G1

G2

θ1

θ 2

θ 2

θ1m m m12 2 122 22

122 1 2θ θ θ θ+ +

m m21 1

11212

2θ θ−

Joint 1PD m11

q d1 q q1 1,

Joint nPD mnn

qnd q qn n,

Joint 2PD m22

q d2 q q2 2,

Joint 1PD m11

q d1 q q1 1, DYNAMIC

COUPLING

Σ i j ij jm q b g, ≠ + +1 1 1 q1

Joint nPD mnn

qnd q qn n,qn

Joint 2PD m22

q d2 q q2 2,q2

Σ i j ij jm q b g, ≠ + +2 2 2

Σ i j n ij j n nm q b g, ≠ + +

Page 8: Robot Control - Stanford School of Engineering - Stanford

8

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

τ dv

V 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

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πτ delay

FHGIKJ

[( ) ( )]+ − + −−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 kClosed-loop

( )t+ εE k E k Ev p+ ′ + ′ = 0

-

Σ Σ( )M q Arm

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

kv

kp

qd

qd

qd

q

+- eΣ

+

τ d

++

+

Task Oriented Control

Page 9: Robot Control - Stanford School of Engineering - Stanford

9

Joint Space Control Joint Space Control

F

( )GoalV xF = −∇

( )GoalV x

TJ Fτ =

Operational Space Control Unified Motion & Force Control

motion contactF F F= +contactF

motionF

F

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

x

xM x F=x xV G+ +

Operational Space Dynamics Task-Oriented Equations of Motion

Non-Redundant Manipulator ; n m=

( )1 2T

mx x x x= …

( )1 2= … Tnq q q q

{0}{ 1}n +

10n+

Page 10: Robot Control - Stanford School of Engineering - Stanford

10

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 qKinetic Energy

( )x J q q=Using

( )1 12 2

T TTx MJ M Jq q q q≡ 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 d=

20

2

1 11 1

d s cJ

d c s−⎡ ⎤

= ⎢ ⎥⎣ ⎦

21 1 0 1;

1 0d

J − ⎛ ⎞= ⎜ ⎟⎝ ⎠

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

Page 11: Robot Control - Stanford School of Engineering - Stanford

11

21

2 2

00x

mM

m m⎛ ⎞

= ⎜ ⎟′+⎝ ⎠

2331 332 1 1

2 22

I I m lmd

+ +′ =

2 2′+m m

• 1x1y•

1m

2m

2m

20

2

01 1 1 101 1 1 1x

mc s c sM

ms c s c+

− ⎛ ⎞⎛ ⎞ ⎛ ⎞= ⎜ ⎟⎜ ⎟ ⎜ ⎟−⎝ ⎠ ⎝ ⎠⎝ ⎠

2 2 2m m m+ ′= +

20 2 2 2

22 2 2

1 11 1x

m m s m s cM

m sc m m c′ ′⎛ ⎞+ −

= ⎜ ⎟′ ′− +⎝ ⎠

2m2 2m m′+

20 2 2 2

22 2 2

1 11 1

m m s m s cm s c m m c

⎛ ′ ′ ⎞+ −Λ = ⎜ ⎟′ ′− +⎝ ⎠

End-Effector Control

( )T FJ qτ =F

( ) ( )12

T

goal p g gV k x x x x= − −

System( ) ( )d K K

dt xV V F

x∂ − ∂ −⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠

Passive Systems (Stability)

( )0goalKd K

dt x xV∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠ Stable

Conservative Forces

( )ˆgoalF V V

X∂

⇓ = − −∂

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

( )0goalKd K

dt x xV∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠

( )goals

Kd Kdt x x

VF

∂ −∂⎛ ⎞ − =⎜ ⎟∂ ∂⎝ ⎠

vk x−

sF

Page 12: Robot Control - Stanford School of Engineering - Stanford

12

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

( ) ˆ ( )p g vk x x k GF x x= − − − +1q1l

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 = − −+ −+ +

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ε

Page 13: Robot Control - Stanford School of Engineering - Stanford

13

Task-Oriented Control

-

Σ Σ( )M qx J qT a f ArmKin qa f

J qa f

( , ) ( )V q q G qx x+

kv

kp

xd

xd

xd

q

q

x

xτF

+- 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

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 Tx

TxΔ Δ Δ( )

K J K JTxθ θ θ= ( ) ( )

mf

Force Control1-d.o.f. mx f=

fdset f fd=

Problem

f Nmd = 1

output~0

friction

Break away

Coulomb friction10(N.m)

Viscous

Force Sensing

mfdf

sfs

s ≡ ; f k xs s=mx k x fs+ =

fs

At static Equilibriumf fd⇒ =f fs d=

Dynamicsf fd Dynamic+mx 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

Page 14: Robot Control - Stanford School of Engineering - Stanford

14

Steady-State errormk

f k f k f f f fs

s v s p s d s df f( ( )) ( )+ ′ + ′ − + − = 0

fdistf fs s= = 0

( )mk

ke fp

sf dist

f′+ =1

e fmk

k

fdist

p

s

f

=+

′1

Task Description

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ϑΩ = Ω