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 
 0    90
 

90    180
  a tan 2( y, x)  


  180    90
  90    0

sin 1 (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.
cos(1   2 )  nx 

sin( 1   2 )  n y 
cos(1   2 )  cos 1  px 

sin( 1   2 )  sin 1  p y 
 nx
n
2
T0   y
 nz

0
0 C1  C12
0 S1  S12 

1
0

0
1

sx
sy
sz
ax
ay
az
0
0
px 
p y 
pz 

1
1   2  a tan 2(n y , nx )
1  a tan 2( p y  n y , p x  nx )
The City College of New York
4
Homework 4
L
Find the dynamic model of 2-link
robot with mass equally distributed
L
•
x1
2
  D(q)q  H (q, q )  C (q)
 1   D11
    D
 2   21
y2
y1
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
j
J jU ji )
for
j i
j i
Physical meaning?
T
jkm
for
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
Ki   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
T 
0 0 0
6
0
p
1  44
 h1 (q) 
h (q )
Y61  h(q)   2 
  


h6 (q) 
Y61  J 6n q 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
I  A 
 2  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
 x1 
k 2  
 x2 
u  k1
(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:
tor  D(q)[qd  kv (q d  q )  k p (q d  q)]  H (q, q )  C(q)
(qd  q)  kv (q d  q )  k p (q d  q)  0
Error dynamics
How to chose
Kp, Kv ?
e  k v 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  k v e  k p e  0
Define states:
x1  e
x2  e
In matrix form:
 x1   0
 x    k
 2  p
Characteristic equation:
How to chose Kp, Kv
to make the system
stable?
x1  x2
x2  kv x2  k p x1
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
k 0
v
One of a
selections: k p  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  k v e  k p e  0
The City College of New York
28
Project
• Simulation study of Non-linear Feedback Control
e  Yd  Y
Task level Yd
Planner Yd
Linear System
q Forward Y
e Linear U Nonlinear tor Robot
Dynamics q Kinematics Y
 Controller Feedback
_e
The City College of New York
29
Thank you!
HWK 5 posted on the web, Due: Oct. 23 before class
Next Class (Oct. 23): 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