Introduction to ROBOTICS
Manipulator Control
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
The City College of New York
1
Outline
• Homework Highlights
• Robot Manipulator Control
– Control Theory Review
– Joint-level PD Control
– Computed Torque Method
– Non-linear Feedback Control
• Midterm Exam Scope
The City College of New York
2
Homework 2
Find the forward kinematics, Roll-Pitch-Yaw
representation of orientation
Joint variables ?
Why use atan2 function?
Inverse trigonometric functions have multiple solutions:
1
3
cos x 
x?
x?
2
2
tan(x)  tan(x  k ) Limit x to [-180, 180] degree
sin x 
1
 0    90
 

90    180
  a tan 2( y, x)  


  180    90
  90    0

sin (1/ 2)  30 ,150

cos1 ( 3 / 2)  30 ,30
The City College of New York
for  x and  y
for  x and  y
for  x and  y
for  x and  y
a tan(1/ 2, 3 / 2)  30
3
Homework 3
Find kinematics model of 2-link robot, Find the inverse kinematics solution
L
L
y1
y2
x1
2
Z1
Z2
x2
m2
1
C12  S12
 S12 C12
2
1 2
T0  T0 T1  
 0
0

0
 0
m1
Inverse: know position (Px,Py,Pz) and
orientation (n, s, a), solve joint variables.
 nx
n
T02   y
 nz

0
sx
sy
sz
0
0 C1  C12
0 S1  S12 

1
0

0
1

ax
ay
az
0
px 
p y 
pz 

1
cos( 1   2 )  n x 

sin(1   2 )  n y 
1  2  a tan2(ny , nx )
cos(1   2 )  cos1  px 

sin(1   2 )  sin 1  p y 
1  a tan2( py  ny , px  nx )
The City College of New York
4
Homework 4
L
Find the dynamic model of 2-link
robot with mass equally distributed
L
y1
•
x1
2
  D(q)q  H (q, q )  C (q)
 1   D11
    D
 2   21
y2
D12  1   h1 ( ,)   c1 ( ) 
    



D22   2  h2 ( , ) c2 ( )
Z1
Z2
x2
m2
1
m1
Calculate D, H, C terms directly
Dik 
n
T0i T0j 1Q jT ji1
U ij 

q j  0
Tr(U jk J jU ji )
T
j  max(i , k )
hikm 
n
Tr(U
j  max(i , k , m )
n
Ci   m j gU ji rj
j i
Physical meaning?
T
jkm
j
J jU ji )
for j  i
for j  i
U ij
qk
 U ijk
 T0 j 1Q jT jk11Qk Tki1

  T0k 1Qk Tk j11Q jT ji1
0
i j

ik j
i jk
or i  k
Interaction effects of motion of joints j & k on link i
The City College of New York
5
Homework 4
Find the dynamic model of 2-link
robot with mass equally distributed
L
L
y1
y2
x1
2
  D(q)q  H (q, q )  C (q)
Z1
•
Derivation of L-E Formula
LK P
d L
L
( )
 i
dt qi qi
 xi 
y 
i
ri   i 
 zi 
 
1
point at link i
Z2
x2
m2
1
m1
Erroneous answer
Velocity of point
i
i
T0i
d i
i
Vi  V  r0  (
q j )ri  (U ij q j )rii
dt
j 1 q j
j 1
L
0
rii   
0
 
1 
i
0
Kinetic energy of link i
Ji

1  i i
i iT
T
K i   dKi  Tr   U ip (  ri ri dm)U ir q p qr 
2  p 1 r 1

The City College of New York
6
Homework 4
Example: 1-link robot with point mass (m)
concentrated at the end of the arm.
L
Set up coordinate frame as in the figure
l 
0 
r11   
0 
 
1
According to physical meaning:
1 2 2
l m1
2
P  9.8m  l  S1
K
m
Y0
Y1
LK P
X1
1
X0
d L
L
  (  )
 l 2 m1  9.8m  l  C1
dt 1 1
The City College of New York
7
Manipulator Control
The City College of New York
8
Manipulator Dynamics Revisit
• Dynamics Model of n-link Arm
  D(q)q  H (q, q )  C (q)
The Acceleration-related Inertia term, Symmetric Matrix
The Coriolis and Centrifugal terms
The Gravity terms
 1  Driving torque
     applied on each link
 n 
Non-linear, highly coupled , second order differential equation
Joint torque
Robot motion
The City College of New York
9
Jacobian Matrix Revisit
Forward Kinematics
n s a p 
T 

0
0
0
1

 44
6
0
 h1 (q ) 
h (q )
Y61  h(q )   2 
  


h
(
q
)
 6 
Y  J q
61
6n n1
 x 
 y 
 
 z 
Y     
 
 
 
 
 x   h1 ( q ) 
p   y   h2 ( q ) 
 z   h3 ( q ) 
 (q )  h4 (q )
{n, s, a}   (q )    h5 (q ) 
 (q ) h6 (q ) 
 dh(q) 

J  
 dq  6n
 q1 
 
 dh(q )  q 2 
 dq    

 6n
 
q n  n1
The City College of New York
 h1
 q
 1
 h2
  q1
 
 h
 6
 q1
h1
q2
h2
q2

h6
q2
h1 
qn 

h2 

qn 

 
h6 


qn  6n

10
Jacobian Matrix Revisit
• Example: 2-DOF planar robot arm
– Given l1, l2 , Find: Jacobian
 x  l1 cos1  l2 cos(1   2 )  h1 (1 , 2 ) 
 y    l sin   l sin(   )   h ( , )
  1
1
2
1
2 
 2 1 2 
1 
 x 

Y     J 

 y 
 2 
 h1
 
J  1
 h2
 1
(x , y)
2
l2
1 l1
h1 
 2   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )

h2   l1 cos1  l2 cos(1   2 ) l2 cos(1   2 ) 
 2 
The City College of New York
11
Robot Manipulator Control
D(q)q  H (q, q )  C (q)  

Y  h(q)

• Robot System:
e  qd  q
• Joint Level Controller
Trajectory q d q d e e
Controller
qd _
Planner
Find a control input (tor), q  qd
q q
tor
Robot
as t  
• Task Level Controller
e  Y Y
d
Task level Yd Yd e e
Controller
Planner
_
Find a control input (tor),
tor
Y  Yd
Y Y
Robot q q Forward
Dynamics
Kinematics
as t  
The City College of New York
e  Yd  Y  0
12
Robot Manipulator Control
• Control Methods
– Conventional Joint PID Control
• Widely used in industry
– Advanced Control Approaches
•
•
•
•
•
Computed torque approach
Nonlinear feedback
Adaptive control
Variable structure control
….
The City College of New York
13
Control Theory Review (I)
PID controller: Proportional / Integral / Derivative control
e= d  a
V = Kp • e + Ki ∫ e dt + Kd
Error signal e
d  a
desired d
-
d )e
dt
Closed Loop Feedback Control
compute V using
PID feedback
V
Motor
actual a
actual a
Reference book: Modern Control Engineering, Katsuhiko Ogata, ISBN0-13-060907-2
The City College of New York
14
Evaluating the response
overshoot
steady-state error
ss error -- difference from the
system’s desired value
settling time
overshoot -- % of final value
exceeded at first oscillation
rise time -- time to span from
10% to 90% of the final value
settling time -- time to reach
within 2% of the final value
How can we eliminate
rise time
the steady-state error?
The City College of New York
15
Control Performance, P-type
Kp = 20
Kp = 50
Kp = 200
Kp = 500
The City College of New York
16
Control Performance, PI - type
Kp = 100
Ki = 50
Ki = 200
The City College of New York
17
You’ve been integrated...
Kp = 100
unstable &
oscillation
The City College of New York
18
Control Performance, PID-type
Kd = 2
Kd = 5
Kd = 10
Kd = 20
The City College of New York
Kp = 100
Ki = 200
19
PID final control
The City College of New York
20
Control Theory Review (II)
• Linear Control System
– State space equation of a system
x  Ax  Bu
– Example: a system:
 x1  x2

 x 2  u
(Equ. 1)
 x1  0 1  x1  0
 x   0 0  x   1u
 2   
 2 
– Eigenvalue of A are the root of characteristic equation
 1 2
I  A 
 0
0 
I  A  0
– Asymptotically stable
real part
all eigenvalues of A have negative
The City College of New York
21
Control Theory Review (II)
– Find a state feedback control u   K  x such that the
closed loop system is asymptotically stable
u  k1
 x1 
k 2  
 x2 
(Equ. 2)
– Closed loop system becomes
u
x  ( A  BK ) x
A
B
x

x
-K
– Chose K, such that all eigenvalues of A’=(A-BK) have
negative real parts
I  A' 

1
k1   k2
 2  k2  k1  0
The City College of New York
22
Control Theory Review (III)
• Feedback linearization
– Nonlinear system X  f ( x)  G( x)U
U  [G 1 ( x) f ( x)  G 1 ( x)V ]
X  V
Linear System
– Example:
V
Original system:
Nonlinear
Feedback
U
Dynamic
System
x
x  cos x  U
Nonlinear feedback:
U  cos x  V
Linear system:
The City College of New York
x  V
23
Robot Motion Control (I)
• Joint level PID control
– each joint is a servo-mechanism
– adopted widely in industrial robot
– neglect dynamic behavior of whole arm
– degraded control performance especially in
high speed
– performance depends on configuration
e  qd  q
Trajectory q d q d e e
Controller
qd _
Planner
q q
tor
Robot
The City College of New York
24
Robot Motion Control (II)
• Computed torque method
– Robot system:
D(q)q  H (q, q )  C (q)  

Y  h(q)

– Controller:
d  kv (q d  q)  k p (qd  q)]  H (q, q)  C(q)
tor  D(q)[q
d  q
)  kv (q d  q)  k p (qd  q)  0
(q
Error dynamics
How to chose
Kp, Kv ?
e  kv e  k p e  0
Advantage: compensated for the dynamic effects
Condition: robot dynamic model is known
The City College of New York
25
Robot Motion Control (II)
Error dynamics
e  kv e  k p e  0
Define states:
x1  e
x2  e
x1  x2
x2  kv x2  k p x1
x
 0
In matrix form:  1   
 x 
 2
Characteristic equation:
How to chose Kp, Kv
to make the system
stable?
 k p
1   x1 
 AX



 kv   x2 
I  A 

1
kp
  kv
 2  kv   k p  0
 k v  k v  4k p
2
The eigenvalue of A matrix is:
1, 2 
Condition:  have negative real part
The City College of New York
2
One of a
selections:
kv  0
kp  0
26
Robot Motion Control (III)
• Non-linear Feedback Control
Linear System
e  Yd  Y
Task level Yd
Planner Yd
q Forward Y
e Linear U Nonlinear tor Robot
Dynamics q Kinematics Y
 Controller Feedback
_e
Robot System:
D(q)q  H (q, q )  C (q)  

Y  h(q)

Jocobian: Y 
d
[h(q)] q  Jq
dq
Y  Jq  Jq
q  J 1 (Y  Jq )
D(q) J 1 (Y  Jq )  H (q, q )  C(q)  
The City College of New York
27
Robot Motion Control (III)
• Non-linear Feedback Control
Linear System
e  Yd  Y
Task level Yd
Planner Yd
q Forward Y
e Linear U Nonlinear tor Robot
Dynamics q Kinematics Y
 Controller Feedback
_e
Design the nonlinear feedback controller as:
tor  D(q) J 1 (U  Jq )  H (q, q)  C(q)
Then the linearized dynamic model:
D(q) J 1Y  D(q) J 1U
Design the linear controller:
Error dynamic equation:
Y  U
U  Yd  kv (Yd  Y )  k p (Yd  Y )
e  kv e  k p e  0
The City College of New York
28
Midterm Exam Scopes
The City College of New York
29
Thank you!
HWK 5 posted on the web,
Next Class: Midterm Exam
Time: 6:30-9:00, Please on time!
z
z
z
y
y
x
z
x
y
x
y
x
The City College of New York
30