```Introduction to ROBOTICS
Robot Kinematics I
Dr. Alex Ramirez-Serrano
Department of Mech. & Manuf Engineering
University of Calgary
[email protected]
1
Outline
• Review
– Manipulator Specifications
• Precision, Repeatability
– Homogeneous Matrix
• Denavit-Hartenberg (D-H)
Representation
• Kinematics Equations
• Inverse Kinematics
2
Review
• Manipulator, Robot arms, Industrial robot
– A chain of rigid bodies (links) connected by
joints (revolute or prismatic)
• Manipulator Specification
– DOF, Redundant Robot
How accurately a specified point can be reached
– Precision
accurately the same position can be reached
– Repeatability How
if the motion is repeated many times
3
Review
• Manipulators:
Cartesian: PPP
Cylindrical: RPP
Spherical: RRP
Hand coordinate:
Articulated: RRR
SCARA: RRP
n: normal vector; s: sliding vector;
(Selective Compliance
Assembly Robot Arm)
a: approach vector, normal to the
tool mounting plate
4
Review
• Basic Rotation Matrix
 px   i x  i u
Pxyz   p y    jy  i u
 p z  k z  i u
Pxyz  RPuvw
i x  jv
jy  jv
k z  jv
i x  k w   pu 
jy  k w   pv   RPuvw
k z  k w   pw 
z
w
P v
Puvw  QPxyz
Q  R 1  RT
u
y
x
5
Basic Rotation Matrices
– Rotation about x-axis with 
1 0
Rot ( x, )  0 C
0 S
0 
 S 
C 
– Rotation about y-axis with 
 C
Rot ( y, )   0
 S
– Rotation about z-axis with 
Pxyz  RPuvw
C
Rot ( z, )   S
 0
S 
1 0 
0 C 
0
 S
C
0
0
0
1
6
Review
• Coordinate transformation from {B} to {A}
r  ARB B r P  Ar o'
A P
 A r P   A RB


 1   013
B P


r
r 


1  1 
A o'
• Homogeneous transformation matrix
 RB
TB  
 013
A
A
r   R33

1   0
A o'
P31 

1 
Rotation
matrix
Position
vector
Scaling
7
Review
• Homogeneous Transformation
– Special cases
1. Translation
 I 33
A
TB  
013
A o'
r 

1 
A

RB
A
TB  
 013
031 

1 
2. Rotation
8
Review
• 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
9
Review
• Homogeneous Representation
– A point in R 3 space
 px 
p 
P   y   Homogeneous coordinate of P w.r.t. OXYZ
 pz 
 
P( p x ,
z
1
 
a
s
– A frame in R space
3
 nx

n s a P  n y
F 


0 0 0 1   nz

0
p y , pz )
sx
sy
ax
ay
sz
0
az
0
px 
p y 
pz 

1
n
y
x
10
Review
• Orientation Representation
(Euler Angles)
– Description of Roll-Pitch-Yaw
• A rotation of  about the OX
axis ( Rx , ) -- yaw
• A rotation of  about the OY
axis ( R y , ) -- pitch
• A rotation of  about the OZ
axis ( Rz , ) -- roll
Z



Y
X
11
Quiz 1
• How to get the resultant rotation matrix for YPR?
T  Rz , Ry , Rx,
C
 S

0

0
 S
C
0
0
0 0   C
0 0  0
1 0  S

0 1  0
Z

X
0
1
S
0
0
0
0

1
0 C
0 0
1 0
0 C

0 S

0 0
0
 S
C
0
0
0
0

1


Y
12
Quiz 2
• Geometric Interpretation?
 R33
T 
 0
P31 

1 
Orientation of OUVW coordinate
frame w.r.t. OXYZ frame
Position of the origin of OUVW
coordinate frame w.r.t. OXYZ frame
• Inverse Homogeneous Matrix?
T
T
Inverse of the rotation submatrix


R

R
P
1
is equivalent to its transpose
T 
 Position of the origin of OXYZ
1  reference frame w.r.t. OUVW frame
0
T

R
T 1T  
0
 R T P   R P   R T R 0


  I 44

1   0 1   0 1
13
Kinematics Model
• Forward (direct) Kinematics
q  (q1 , q2 ,qn )
Joint
variables
Direct Kinematics
z
Position and Orientation
of the end-effector
y
x
Inverse Kinematics
Y  ( x, y, z,  , , )
• Inverse Kinematics
14
KINEMATICS
The science of motion that treats the subject without regards to
the forces that cause it.
Here we study the: Position, Velocity, Acceleration and other
high order derivatives…..
ai-1
vi-1
vi
ai
Dr. A. Ramirez-Serrano
Types of joints in manipulators
Basic distinction: what kinds of joints extend from base to end.
“RR” or “2R”
“PR” arm
All manipulators can be represented as
chains of P (prismatic) and R (rotational)
joints. Joints with singly DOF !
The six possible robot lower-pair joint types
Dr. A. Ramirez-Serrano
17
Denavit-Hartenberg (D-H) 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 i-th coordinate system. Locate the origin
of the i-th 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
right-handed coordinate system.
• Find the link and joint parameters
18
Example:
• 3 Revolute Joints
Z3
Z1
Z0
Y0
O3
Y1
Joint 1
Joint 3
O0 X0
Joint 2
X3
d2
O1 X1 O2 X2
Y2
a0
a1
19
– To describe the geometry of robot motion, we assign a Cartesian
coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:
• establish a right-handed orthonormal coordinate frame O0 at
the supporting base with Z0 lying along joint 1 motion axis.
• the Zi axis is directed along the axis of motion of joint (i + 1),
that is, link (i + 1) rotates about or translates along Zi;
Z3
Z1
Z0
Y0
O3
Y1
Joint 1
Joint 3
O0 X0
Joint 2
X3
d2
O1 X1 O2 X2
Y2
a0
a1
20
– 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.
– the Xi axis lies along the common normal from the Zi-1
axis to the Zi axis X i  (Zi 1  Zi ) / Zi 1  Zi , (if Zi-1 is
parallel to Zi, then Xi is specified arbitrarily, subject only
to Xi being perpendicular to Zi);
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
21
– Assign Yi  (Zi  X i ) / Zi  X i to complete the righthanded coordinate system.
• The hand coordinate frame is specified by the geometry On
of the end-effector. Normally, establish Zn along the
direction of Zn-1 axis and pointing away from the robot;
establish Xn such that it is normal to both Zn-1 and Zn
axes. Assign Yn to complete the right-handed coordinate
system.
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
22
Example I
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
 i : rotation angle from Zi-1 to Zi about Xi
ai : distance from intersection of Zi-1 & Xi
Joint i
i
ai
di
i
1
0
a0
0
0
2
-90
a1
0
1
3
0
0
d2
2
to origin of i coordinate along Xi
di
: distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
i
: rotation angle from Xi-1 to Xi about Zi-1
City College of New York
23
Distance along a line that is mutually perpendicular to
both axes (i & i-1). Unique or infinite-Parallel axes.
The relative location of the two
axes (i & i-1) .
Neighboring links have a common axis between them.
Distance along this common axis from one link to the
JOINT angle:  i
Amount or rotation about the common axis between
24
• Joint angle  i : the angle of rotation from the Xi-1 axis to the Xi axis about
the Zi-1 axis. It is the joint variable if joint i is rotary.
• Joint offset d i : the distance from the origin of the (i-1) coordinate system
to the intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. It is the
joint variable if joint i is prismatic.
• Link length ai : the distance from the intersection of the Zi-1 axis and the
Xi axis to the origin of the ith coordinate system along the Xi axis.
• Link twist angle  i : the angle of rotation from the Zi-1 axis to the Zi axis
25
The Denavit-Hartenberg convention (DH convention) is commonly used to define
reference frames for robotic serial arms. This convention is based on two
assumptions:
DH1 : The z-axis will lie along the joint. According to many authors, axis zi-1 lies
along joint i. For example, the axis x1 is perpendicular to the axis zo .
DH2 : The x-axes lie along the common normals between the joint axes.
According to many authors, axis zi-1 lies along joint i. For example, the axis x 1
intersects the axis z0 .
αi
ai
di
Θi
1
Angle between zo
and z1 measured
Distance from zo to
z1 measured along
x1
Distance from xo
to x1 measured
along zo
Angle between xo
and x1 measured
2
Angle between z1
and z2 measured
Distance from z1 to
z2 measured along
x2
Distance from x1
to x2 measured
along z1
Angle between x1
and x2 measured
n
Angle between zn
and zn+1 measured
Distance from zn to
Distance from xn
zn+1 measured along to xn+1 measured
xn+1
along zn
Angle between xn
and xn+1 measured
26
Example:
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
 i : rotation angle from Zi-1 to Zi about Xi
ai : distance from intersection of Zi-1 & Xi
Joint i
i
ai
di
i
1
0
a0
0
0
2
-90
a1
0
1
3
0
0
d2
2
to origin of i coordinate along Xi
di
: distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
i
: rotation angle from Xi-1 to Xi about Zi-1
27
Example II: PUMA 260
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
O2
Y3
Z
Z4
O3
X 2 5  6 Y6
Y2
5
O
6
Z0
Y5
Y
X3 4
t
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
PUMA 260
City College of New York
28
1
J
2
1
Z1
2
3
O1
3
X1
Z2 Z6
Y1
O
Y3 2
Z
Z4
O3
X 2 5  6 Y6
Y2
5
O
6
Z0
Y5
Y
X3 4
O5
X5 X6
O4 Z 3
X4
Joint distance
4
4
5
6
i
1
2
3
4
5
6
i
ai d i
-90 0
13
0
8
0
90
0
-90 0
-l
8
90
0
0
0
0
t
 i : angle from Xi-1 to Xi
 i : angle from Zi-1 to Zi
ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
d i : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
City College of New York
29
Transformation between i-1 and i
• Four successive elementary transformations
are required to relate the i-th coordinate frame
to the (i-1)-th coordinate frame:
– Rotate about the Z i-1 axis an angle of i to align the X i-1 axis
with the X i axis.
– Translate along the Z i-1 axis a distance of di, to bring Xi-1 and Xi
axes into coincidence.
– Translate along the Xi axis a distance of ai to bring the two
origins Oi-1 and Oi as well as the X axis into coincidence.
– Rotate about the Xi axis an angle of αi ( in the right-handed
sense), to bring the two coordinates into coincidence.
30
Transformation between i-1 and i
• D-H transformation matrix for adjacent coordinate
frames, i and i-1.
– The position and orientation of the i-th frame coordinate can be
expressed in the (i-1)th frame by the following homogeneous
transformation matrix:
Source coordinate
Ti i 1  T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi ,  i )
Reference
Coordinate
C i
 S
 i
 0

 0
 C i S i
C i C i
S i S i
 S i C i
S i
0
C i
0
ai C i 
ai S i 
di 

1 
31
Kinematic Equations
q  (q1 , q2 ,qn )
• Forward Kinematics
– Given joint variables
– End-effector position & orientation Y  ( x, y, z,  , , )
n
• Homogeneous matrix T0
– specifies the location of the ith coordinate frame w.r.t.
the base coordinate system
– chain product of successive coordinate transformation
matrices of Ti i 1
T  T T T
n
0
Orientation
matrix
 R0n

0
1 2
0 1
n
n 1
Position
vector
P0n  n s a P0n 


1  0 0 0 1 
32
Forward Kinematics Exercise:
Problem: Obtain the kinematic equations of
motion for the two joint arm show on the left.
The robot has on revolute joint at the base,
followed by a prismatic joint, followed by a
revolute joint for the hand.
Figure: Robot Manipulator
NOTE: For a revolute joint, the joint is the
axis of rotation. For a prismatic joint, the joint
is on the motion path of the joint.
Step # 1: Assign the
robot’s joint and frames of
reference and obtain the
corresponding parameters
etc).
Figure 2: Robot with Joint & Frame Assignments
33
Forward Kinematics Exercise:
Figure 2: Robot with Joint & Frame Assignments
Step # 2: From the joints
and frames obtained in
Step 1 build the robot’s
DH Table.
αi
ai
di
Θi
Ti i-1
1
90
0
d1
Θ 1*
T 1o
2
0
0
d2*
0
T 21
3
0
0
d3
Θ 3*
T 32
Note: * denotes variable joint parameters. For each link, only
one unknown joint variable can exist for each joint axis.
34
Forward Kinematics Exercise:
αi
di
Θi
Ti i-1
1
90 0
d1
Θ 1*
T 1o
2
0
0
d2* 0
3
0
0
d3
ai
Θ 3*
T 21
T 32
Step # 3: From the DH table
obtain the corresponding
homogeneous transformations.
The resulting transformation
matrices for the manipulator are
given below:
35
Kinematics Equations
• Other representations
– reference from, tool frame
0
Treftool  Bref
T0n H ntool
– Roll-Pitch-Yaw representation for orientation
T  Rz , Ry , Rx,
C
 S

0

0
 S
C
0
0
0 0   C
0 0  0
1 0  S

0 1  0
0
1
S
0
0 C
0 0
0
0
0

1
1 0
0 C

0 S

0 0
0
 S
C
0
0
0
0

1
36
Solving forward kinematics
• Forward kinematics
1 
 
 2
 3 
 
 4 
 5 
 
 6 
 px 
p 
 y
 pz 
 
 
 
 
 
• Transformation Matrix
 nx
n
T  y
 nz

0
sx
sy
ax
ay
sz
0
az
0
px 
p y 
pz 

1
37
Solving forward kinematics
• Roll-Pitch-Yaw representation for orientation
CC CSS  SC
 SC SSS  CC
T0n  
  S
CS

0
 0
 nx
n
T0n   y
 nz

0
Problem?
sx
sy
ax
ay
sz
0
az
0
px 
p y 
pz 

1
CSC  SS
SSC  CS
CC
0
  sin 1 (nz )
px 
p y 
pz 

1
az
  cos (
)
cos 
nx
1
  cos (
)
cos 
1
Solution is inconsistent and ill-conditioned!!
38
atan2(y,x)
y
x
 0    90
for  x and  y



for  x and  y
 90    180
  a tan 2( y, x)  



180




90
for  x and  y

  90    0
for  x and  y
39
Roll-Pitch-Yaw Representation
T  Rz , Ry , Rx,
C
 S

0

0
 S
C
 nx
n
 y
 nz

0
sx
sy
ax
ay
sz
0
az
0
0
0
0 0   C
0 0  0
1 0  S
 
0 1  0
0
1
S
0
0 C
0 0
0
0
0

1
1 0
0 C

0 S

0 0
0
 S
C
0
0
0
0

1
0
0
0

1
40
Roll-Pitch-Yaw Representation
1
z ,
R T  Ry , Rx,
 C
 S

 0

 0
 C
 0

 S

 0
S
C
0
0
0
1
0 0  n x
0 0 n y

1 0  n z
 
0 1  0
S
0
0 C
0 0
0
0
0

1
sx
sy
ax
ay
sz
0
az
0
1 0
0 C

0 S

0 0
0
 S
C
0
0
0
0

1
(Equation A)
0
0
0

1
41
Roll-Pitch-Yaw Representation
• Compare LHS and RHS of Equation A, we have:
 sin   nx  cos   ny  0
  a tan 2(ny , nx )
cos   nx  sin   n y  cos 

nz   sin 

  a tan 2(nz , cos   nz  sin   ny )
  sin   s x  cos   s y  cos

 sin   ax  cos   a y   sin
  a tan 2(sin   ax  cos   a y , sin   sx  cos   s y )
42
Kinematic Model
• Steps to derive kinematics model:
–
–
–
–
–
Assign D-H coordinates frames
Calculate Kinematics Matrix
When necessary, Euler angle representation
43
Example:
Z3
Z1
Z0
Y0
Joint 3
O3
Y1
X3
d2
Joint 1
O0 X0
Joint 2
O1 X1 O2 X2
Y2
a0
a1
Joint i
i
ai
di
i
1
0
a0
0
0
2
-90
a1
0
1
3
0
0
d2
2
44
Example:
Joint i
i
ai
di
i
1
0
a0
0
0
Ti i 1
2
-90
a1
0
1
3
0
0
d2
2
C i
 S
 i
 0

 0
 C i S i
C i C i
S i S i
 S i C i
S i
0
C i
0
T03  (T 01)(T 21)(T 23)
ai C i 
ai S i 
di 

1 
cosθ 0

1  sinθ 0
T 0
 0

 0
cosθ1

2  sinθ1
T 1
 0

 0
cosθ 2

3  sin  2
T 2
 0

 0
0 a0 cos  0 
0 a0 sin  0 
1
0 

0
1 
 sinθ 0
cosθ 0
0
0
0
0
 sin 1
cos 1
1
0
0
0
 sinθ 2
cos  2
0
0
a1 cos 1 
a1 sin 1 
0 

1 
0
0 
1 d2 

0 1
0
0
45
Example: Puma 560
46
Example: Puma 560
47
PUMA 560 robot arm link coordinate parameters
Joint i
i
i
1
1
-90
2
2
0
431.8 149.09
3
3
90
-20.32
0
4
5
4
5
-90
90
0
0
433.07
0
6
6
0
0
56.25
ai(mm) di(mm)
0
0
48
Example: Puma 560
49
Example: Puma 560
50
Inverse Kinematics
• Given a desired position (P)
& orientation (R) of the endz
effector
q  (q1 , q2 ,qn )
• Find the joint variables
which can bring the robot to
the desired configuration
y
x
51
Inverse Kinematics
• More difficult
– Systematic closed-form
solution in general is not
available
– Solution not unique
(x , y)
• Redundant robot
• Elbow-up/elbow-down
configuration
– Robot dependent
52
Inverse Kinematics
• Transformation Matrix
 nx
n
T  y
 nz

0
sx
sy
ax
ay
sz
0
az
0
px 
p y 
 T01T12T23T34T45T56
pz 

1
1 
 
 2
 3 
 
 4 
 5 
 
 6 
Special cases make the closed-form arm solution possible:
1.
Three adjacent joint axes intersecting (PUMA, Stanford)
2.
Three adjacent joint axes parallel to one another (MINIMOVER)
53
Thank you!
Homework # 2 posted on the web.
Due: Feb. 27, 2014
Next class: Jocobian Matrix, Trajectory planning
z
z
z
y
y
x
z
x
y
x
y
x
54
```