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
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∂∂
−
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ωω ξ−=
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
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
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
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, ≠ + +
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
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πτ 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
qτ
eΣ
+- eΣ
+
τ d
++
+
Task Oriented Control
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+
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
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
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ε
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Σ
+- 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
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ϑΩ = Ω
Top Related