Introduction to ROBOTICS
Review for Midterm Exam
Dr. John (Jizhong) Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
City College of New York
1
Outline
• Homework Highlights
• Course Review
• Midterm Exam Scope
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
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
ax
ay
sz
0
az
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 )
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
Z1
D12  1   h1 ( ,)   c1 ( ) 
    



D22   2  h2 ( , ) c2 ( )
x2
Z2
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
for
j i
j i
Physical meaning?
T
jkm
for
J jU ji )
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
City College of New York
5
Homework 4
L
Find the dynamic model of 2-link
robot with mass equally distributed
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
x2
Z2
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

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
City College of New York
7
Course Review
• What are Robots?
– Machines with sensing, intelligence and
mobility (NSF)
• Why use Robots?
– Perform 4A tasks in 4D environments
4A: Automation, Augmentation, Assistance, Autonomous
4D: Dangerous, Dirty, Dull, Difficult
City College of New York
8
Course Coverage
z
• Robot Manipulator
– Kinematics
– Dynamics
– Control
z
z
y
y
x
z
x
y
x
y
x
• Mobile Robot
–
–
–
–
Kinematics/Control
Sensing and Sensors
Motion planning
Mapping/Localization
City College of New York
9
Robot Manipulator
City College of New York
10
Homogeneous Transformation
Homogeneous Transformation Matrix
 R33
TB  
 0
A
P31 

1 
Rotation
matrix
Position
vector
Scaling
• Composite Homogeneous Transformation Matrix
• Rules:
– Transformation (rotation/translation) w.r.t. (X,Y,Z) (OLD
FRAME), using pre-multiplication
– Transformation (rotation/translation) w.r.t. (U,V,W) (NEW
FRAME), using post-multiplication
City College of New York
11
Composite Rotation Matrix
• A sequence of finite rotations
– matrix multiplications do not commute
– rules:
• if rotating coordinate O-U-V-W is rotating about
principal axis of OXYZ frame, then Pre-multiply
the previous (resultant) rotation matrix with an
appropriate basic rotation matrix
• if rotating coordinate OUVW is rotating about its
own principal axes, then post-multiply the
previous (resultant) rotation matrix with an
appropriate basic rotation matrix
City College of New York
12
Homogeneous Representation
• A frame in space (Geometric
Interpretation)
 R33
F 
 0
P31 

1 
 nx
n
F  y
 nz

0
sx
sy
ax
ay
sz
0
az
0
P( px , p y , pz )
z
a
px 
p y 
pz 

1
s
n
y
x
Principal axis n w.r.t. the reference coordinate system
City College of New York
13
Manipulator Kinematics
1 
 
 2
 3 
 
 4 
 5 
 
 6 
Forward
Kinematics
Inverse
Joint Space
x 
y
 
z 
 
 
 
 
 
Task Space
1 
 
 2 
3 
 
 4 
 
 5
6 
Jacobian
Matrix
 x 
 y 
 
 z 
 
 x 
 y 
 
 z 
Jacobian Matrix: Relationship between joint
space velocity with task space velocity
City College of New York
14
Manipulator Kinematics
• Steps to derive kinematics model:
– Assign D-H coordinates frames
– Find link parameters
– Transformation matrices of adjacent joints
Ti i 1  T ( zi 1 , d i ) R( zi 1 , i )T ( xi , ai ) R( xi ,  i )
– Calculate kinematics model
C i
 S
 i
 0

 0
 C i S i
C i C i
S i
0
S i S i
 S i C i
C i
0
ai C i 
ai S i 
di 

1 
• chain product of successive coordinate transformation
matrices
T0n  T01T12 Tnn1
 R0n

0
P0n  n s a P0n 


1  0 0 0 1 
– When necessary, Euler angle representation
City College of New York
15
Denavit-Hartenberg Convention
• Number the joints from 1 to n starting with the base and ending with
the end-effector.
• Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system ( X 0 , Y0 , Z 0 ) at the supporting base
with Z 0 axis lying along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or
sliding) of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of
the ith coordinate at the intersection of the Zi & Zi-1 or at the
intersection of common normal between the Zi & Zi-1 axes and the Zi
axis.
• Establish Xi axis. Establish X i  (Zi 1  Zi ) / Zi 1  Zi or along the
common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign Yi  (Zi  X i ) / Zi  X i to complete the righthanded coordinate system.
• Find the link and joint parameters
City College of New York
16
Denavit-Hartenberg Convention
1
2
Z1
3
O1
1.
Number the joints
2.
Establish base frame
3.
Establish joint axis Zi
4.
Locate origin, (intersect.
of Zi & Zi-1) OR (intersect
of common normal & Zi )
X1
Z2 Z6
5.
Y1
O
Y3 2
Z5
Z4
Y6
O3

X
6
Y2 2
5
O
6
Z0
Y5
Y
X3 4
O5
X5 X6
O4 Z 3
X4
Establish Xi,Yi
X i  (Zi 1  Zi ) / Zi 1  Zi
Yi  (Zi  X i ) / Zi  X i
4
City College of New York
17
Link Parameters
1
J
2
1
Z1
2
3
O1
3
i
1
2
3
4
5
6
i
ai d i
-90 0
13
0
8
0
90
0
-l
8
4
-90 0
X1
Z2 Z6
Y1
5
90 0 0
Y3O2
Z5
Z4 6
Y6
0
0 t
O3

X
6
2
Y2
5  : angle from X to X
O
i-1
i
6
Z0
i
Y5
about Zi-1
X 3 Y4
O5
X5 X6
 i : angle from Zi-1 to Zi
O4 Z 3
about X
i
X4
Joint distance
4 ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
di : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
City College of New York
18
Example: Puma 560
City College of New York
19
Jacobian Matrix
Kinematics:
Y61  h(qn1 )
 x 
 y 
Jacobian is a function of
 q1 
 
q, it is not a constant!
q 
 z   dh(q)   2 
    dq    
 6n
 x  
 
 y 
h1 
q n  n1
 h1 h1
 
 q q  q 
 z 
2
n
 1

 h2 h2  h2 
 dh(q) 
   q q
J  
qn 
1
2
dq

 6n  


 
 h h
h6 
6
6



qn  6n
 q1 q2
City College of New York
20
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  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
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

21
Trajectory Planning
Tasks
Task Plan
• Motion Planning:
– Path planning
Action Plan
Path Plan
Trajectory
Plan
Controller
Robot
Sensor
• Geometric path
• Issues: obstacle avoidance, shortest
path
– Trajectory planning,
• “interpolate” or “approximate” the
desired path by a class of
polynomial functions and generates
a sequence of time-based “control
set points” for the control of
manipulator from the initial
configuration to its destination.
City College of New York
22
Trajectory planning
Joint i
Final
q(tf)
q(t2)
• Path Profile
• Velocity Profile
Set down
q(t1)
q(t0)
Lift-off
Initial
• Acceleration Profile
t0
t1
t2
tf Time
Speed
t0
Acceleration
t0
City College of New York
t1
t1
t2
t2
tf
Time
tf Time
23
Trajectory Planning
• n-th order polynomial, must satisfy 14 conditions,
• 13-th order polynomial
a13t 13    a2t 2  a1t  a0  0
• 4-3-4 trajectory
h1 (t )  a14t 4  a13t 3  a12t 2  a12t  a10
t0t1, 5 unknow
h2 (t )  a23t 3  a22t 2  a21t  a20
t1t2, 4 unknow
hn (t )  an 4t 4  an 3t 3  an 2t 2  an 2t  an 0
t2tf, 5 unknow
• 3-5-3 trajectory
City College of New York
24
Manipulator Dynamics
Joint torques
Robot motion, i.e. position velocity,
• Lagrange-Euler Formulation
d L
L
( )
 i
dt qi
qi
– Lagrange function is defined
L K P
• K: Total kinetic energy of robot
• P: Total potential energy of robot
• qi : Joint variable of i-th joint
i: first time derivative of qi
• q
•  i : Generalized force (torque) at i-th joint
City College of New York
25
Manipulator Dynamics
• Dynamics Model of n-link Arm
  D(q)q  h(q, q )  C (q)
 D11  D1n 
 The Acceleration-related Inertia
D  

 matrix term, Symmetric
 Dn1  Dnn 
 h1 
h(q, q )     The Coriolis and Centrifugal terms
hn 
 C1 
C (q)    
The Gravity terms
Cn 
 1 
     Driving torque
applied on each link
 n 
City College of New York
26
Example
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
City College of New York
27
Manipulator Dynamics
• Potential energy of link i
r0i : Center of mass
w.r.t. base frame
Pi  mi gr0i  mi g (T0i ri i )
ri i : Center of mass
w.r.t. i-th frame
g  ( g x , g y , g z ,0)
g  9.8m / sec 2
g
: gravity row vector
expressed in base frame
• Potential energy of a robot arm
n
n
P   Pi   [mi g (T0i ri i )]
i 1
i 1
City College of New York
Function of
qi
28
Robot Motion Control
• 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
City College of New York
29
Joint Level Controller
• Computed torque method
e  qd  q
Trajectory q d q d e e
Controller
qd _
Planner
q q
tor
Robot
– 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  kv e  k p e  0
Advantage: compensated for the dynamic effects
Condition: robot dynamic model is known exactly
City College of New York
30
Robot Motion Control
Error dynamics
e  kv 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
City College of New York
2
k 0
v
One of a
selections: k p  0
31
Task Level Controller
• 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)  
City College of New York
32
Task Level Controller
• 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
Nonlinear feedback controller:
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
Y  U
U  Yd  kv (Yd  Y )  k p (Yd  Y )
Linear Controller:
Error dynamic equation:
e  kv e  k p e  0
City College of New York
33
Midterm Exam Scope
• Study lecture notes
• Understand homework and examples
• Have clear concept
• 2.5 hour exam
• close book, close notes
• But you can bring one-page cheat sheet
City College of New York
34
Thank you!
Next class: Oct. 23 (Tue): Midterm Exam
Time: 6:30-9:00
z
z
z
y
y
x
z
x
y
x
y
x
City College of New York
35