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
– Workspace, Payload
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
Robot Links and Joints
Links
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
Link 1
Joint 1
Joint 3
O0 X0
Joint 2
X3
d2
Link 2
O1 X1 O2 X2
Y2
a0
a1
19
Link Coordinate Frames
• Assign Link Coordinate Frames:
– 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
Link 1
Joint 1
Joint 3
O0 X0
Joint 2
X3
d2
Link 2
O1 X1 O2 X2
Y2
a0
a1
20
Link Coordinate Frames
– 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
Link Coordinate Frames
– 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
D-H Link Parameter Table
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
Joint and Link Parameters
LINK length: ai-1
Distance along a line that is mutually perpendicular to
both axes (i & i-1). Unique or infinite-Parallel axes.
LINK twist: i-1
The relative location of the two
axes (i & i-1) .
LINK offset: di
Neighboring links have a common axis between them.
Distance along this common axis from one link to the
next (Link i & i-1).
JOINT angle:  i
Amount or rotation about the common axis between
one link and the next (Link i & i-1).
24
Link and Joint Parameters
• 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
about the Xi axis.
25
Summary: DH Link Parameter Table
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 .
Link i
αi
ai
di
Θi
1
Angle between zo
and z1 measured
about x1
Distance from zo to
z1 measured along
x1
Distance from xo
to x1 measured
along zo
Angle between xo
and x1 measured
about zo
2
Angle between z1
and z2 measured
about x2
Distance from z1 to
z2 measured along
x2
Distance from x1
to x2 measured
along z1
Angle between x1
and x2 measured
about z1
n
Angle between zn
and zn+1 measured
about xn+1
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
about zn
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
D-H Link Parameter Table
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
Link Parameters
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
about Zi-1
 i : angle from Zi-1 to Zi
about Xi
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
(e.g., link length, link twist,
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.
Link i
α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:
Link i
α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
Find link parameters
Transformation matrices of adjacent joints
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
Link Coordinate Parameters
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