T - School of Computing & Intelligent Systems Magee Campus

Lecture
THREE
Robot Arm Equation: Direct Kinematics
3.1 The Direct Kinematics
Robot arm kinematics deals with the analytical study of the geometry of motion of a robot
arm with respect to a fixed reference coordinate system as a function of time without
regard to the force and moment. Kinematics deals with two fundamental problems of both
theoretical and practical interest: direct kinematics and inverse kinematics. In this chapter
we will discuss direct kinematics which is a systematic procedure for obtaining the robot
arm equation.
A robotic arm consists of several mechanical joints. To define a position and orientation
of the tool center point (TCP) of the end-effector in three dimensional spaces an
expression is to be determined as a function of joint angles. Since the links of the robot
arm rotates and/or translates with respect to a reference coordinate, a relative coordinate
or body attached frame is established along the joint axis for each link. A transformation
is needed for setting up the correspondence between the elements of two coordinate
systems that are rotated and/or translated relative to each other. The position and
orientation can be expressed in each of these coordinate systems. There are several
transformation techniques:
•
•
•
Rotation matrix technique
Quaternion and Rotation vectors
Homogeneous transformation matrices technique
We will discuss rotation matrix and homogeneous transformation matrix techniques
only in this lecture.
3.2.1 Rotation Matrix
In order to specify the position and orientation of the end-effector in terms of a coordinate
system attached to the fixed base, coordinate transformations involving both rotations and
translations are required. Let us consider two coordinate systems, shown in Figure 3.1,
namely, the XYZ coordinate and the UVW coordinate system. Both coordinate systems
have their origin at point O. XYZ coordinate system is fixed in the three-dimensional
space and considered to be the reference coordinates. UVW coordinate system is rotating
with respect to the reference coordinate system.
Pxyz and Puvw represent the same point P in the space. A point P can be represented by its
coordinates with respect to the either coordinates XYZ and UVW as
Puvw = [Pu , Pv , Pw ]T
(3.1)
[
Pxyz = Px , Py , Pz
]T
(3.2)
Z
P
W
V
Y
U
X
Figure 3.1: Coordinate system
We are interested in finding a rotation matrix R that will transform the coordinates of
Puvw to the coordinates expressed with respect to the XYZ coordinate system, after UVW
coordinate system has been rotated. That is,
Pxyz = RPuvw
(3.3)
Physically the point Puvw has been rotated together with the UVW coordinate system. The
coordinates of Puvw can be obtained from the coordinates of Pxyz using the definition in
(3.3).
Puvw = R −1 Pxyz
(3.4)
Where R −1 = R T . The goal is to find the rotation matrix that represents rotation of the
moving coordinate system UVW about the principal axes of the fixed reference
coordinate system XYZ. Let UVW coordinate system be rotated α angle about OX axis to
arrive at a new location transformation matrix R x,α , which can be derived from the above
concept as follows
Pxyz = Rx,α Puvw
(3.5)
Figure 3.2: Three basic rotations about x, y and z-axes.
Thus using the vector scalar product, the first basic rotation matrix for a positive rotation
α about X-axis is:
0
1
R x,α = 0 cos α
0 sin α
0 
− sin α 
cos α 
(3.6)
A similar analysis can be done to derive the second and third basic rotation matrix about
Y-axis with angle φ and about Z-axis with angle θ respectively.
 cos φ 0 sin φ 
R y ,φ =  0
1
0 
(3.7)
− sin φ 0 cos φ 
cos θ
R z ,θ =  sin θ
 0
− sin θ
cos θ
0
0
0
1
(3.8)
The three basic rotations about the three axes are shown in Figure 3.2. The matrices R x,α ,
R y ,φ , and R z ,θ are called the basic rotation matrices as the other finite rotation matrices
can be derived from them.
3.2.2. Composite Rotation Matrix
To establish an arbitrary orientation for the end-effector, we need a sequence of finite
rotations of basic form which is termed as composite rotation matrix, and it is obtained by
multiplying a number of basic rotation matrices together. The rotation can be about the
principal axis of the reference coordinate system as well as about its own principal axes.
Since, the operation of matrix multiplication is not commutative the order in which the
basic rotation matrices are performed makes difference in resulting composite rotation
matrix. Therefore, distinct rules must be given as to how a composite rotation matrix
should be constructed, and the following rules are used.
1. Initially both coordinate systems are coincident; hence the rotation matrix is a
3× 3 identity matrix. Initialize the rotation matrix to R = I 3 .
2. If the rotating coordinate system is to be rotated about one of the principal axes of
the reference coordinate system, then pre-multiply the previous rotation matrix with an
appropriate basic rotation matrix.
3. If the rotating coordinate system is rotating about its own principal axes, then postmultiply the previous rotation matrix with an appropriate basic rotation matrix.
Using the above rules the composite rotation matrix can be found representing a rotation
of α angle about OX axis followed by rotation of φ angle about OY axis followed by a
rotation of θ angle about OZ axis. The resulting composite rotation matrix is
R = R z ,θ R y ,φ R x ,α
cos θ
=  sin θ
 0
− sin θ
cos θ
0
 CφCθ
=  Sθ
− SφCθ
0  cos φ
0  0
1 − sin φ
0 sin φ  1
0
0 


1
0  0 cos α − sin α 
0 cos φ  0 sin α cos α 
SφSα − CφSθCα CφSθCα + SφCα 

CθCα
− CθSα
(3.9)


SφSθCα + CφSα CφCα − SφSθSα 
Where C (.) ≡ cos(.) and S (.) ≡ sin(.) .
3.2.3. Homogeneous Transformation Matrix
The most general relationship between the two coordinate systems X0Y0Z0 and X1Y1Z1 as
shown in Figure 3.3 can be expressed as the combination of a pure rotation and a pure
translation.
Z1
Z0
P1
P0
O1
O0
Y1
Y0
X1
X0
Figure 3.3: Relationship between two coordinate systems
Pure rotations are sufficient to characterize the orientation of a robotic manipulator but
translation is used to characterize the position of the manipulator relative to a coordinate
system. A [3× 3] matrix is not sufficient to represent translation. In robotics, we normally
use a scale factor of σ = 1 for convenience. Thus, four dimensional homogeneous
coordinates are obtained from three dimensional physical coordinates by simply
augmenting the vector with a unit fourth component. A homogeneous transformation
matrix can be considered to consists of the following four sub-matrices
R
T =  3×3
η 1×3
p 3×1 
σ 1×1 
(3.10)
 Rotation matrix

T =
 Perspective
 transformation

Position vector 



Scaling


(3.11)
Here the [3× 3] sub-matrix R in the upper left corner of T is a rotation matrix. It represents
the orientation of the mobile coordinate frame relative to the fixed reference frame. The
upper right [3×1] sub-matrix P is the position vector which represents the translation. It
specifies the position of the origin of the mobile coordinate frame relative to the fixed
reference frame. The lower left [1× 3] sub-matrix η T is a perspective vector and represents
perspective transformation which is used for computer vision and camera calibration.
Here, the elements of this matrix are set to zero to indicate null perspective
transformation. The scalar σ in the lower right corner of T is a nonzero scale factor which
is typically set to unity.
If a physical point P in a three dimensional space is expressed in terms of homogeneous
coordinates and we want to change from one coordinate to frame we extend the [3× 3]
rotation matrix to a [4 × 4] homogeneous transformation matrix. For pure rotation
equations (3.6), (3.7), (3.8) expressed as rotation matrices, become
T x ,α
T y ,φ
T z ,θ
0
1
0 cos α
=
0 sin α

0
0
 cos φ
 0
=
− sin φ

 0
cos θ
 sin θ
=
 0

 0
0
0
0

1
(3.12)
0
0
0

1
(3.13)
0 0
0 0
1 0

0 1
(3.14)
0
− sin α
cos α
0
0 sin φ
1
0
0 cos φ
0
− sin θ
cos θ
0
0
0
The equations (3.12), (3.13) and (3.14) are called the basic homogeneous rotation
matrices.
The [3×1] position vector of the homogeneous transformation matrix has the effect of
translating the X1Y1Z1 coordinate system which has axes parallel to the reference
coordinate system X0Y0Z0. The three basic homogeneous translation matrices are as
follows.
T x ,dx
1
0
=
0

0
0 0 dx 
1 0 0 
0 1 0

0 0 1
(3.15)
T y ,dy
1
0
=
0

0
0 0 0
1 0 dy 
0 1 0

0 0 1
(3.16)
T z ,dz
1
0
=
0

0
0
1 0 0 
0 1 dz 

0 0 1
(3.17)
0 0
The basic homogeneous translation matrix can be obtained from the three basic
translation matrices (3.15)-(3.17) as
Ttran
1
0
=
0

0
0 0 dx 
1 0 dy 
0 1 dz 

0 0 1
(3.18)
A [4 × 4] homogeneous transformation matrix maps a vector expressed in homogeneous
coordinates with respect to X1Y1Z1 coordinate system to the reference coordinate system
X0Y0Z0 as shown in Figure 3.3. That is
P0 = T01 P1
(3.19)
3.2.4 Composite Homogeneous Transformation Matrix
A homogeneous transformation represents both a rotation and translation of a mobile
coordinate system with respect to a fixed reference coordinate system. A sequence of
individual rotation and translation matrices can be multiplied together to obtain a
composite homogeneous transformation matrix. Since matrix multiplications are not
commutative operations, the order in which the rotations and translations are to be
performed is important. Furthermore, the mobile coordinate system can rotate or translate
about the unit vectors. Therefore, the effects of the different operations on the composite
homogeneous transformation matrix are summarized in the following rules.
1. Initially both the coordinate systems are coincident, hence the transformation matrix is
a [4 × 4] identity matrix, therefore, initialize it to T = I.
2. Represent rotations and transitions using separate homogeneous transformation
matrices.
3. Represent composite rotations as separate fundamental homogeneous rotation matrices.
4. If the mobile coordinate system is to be rotated about or translated along a unit vector
of the fixed reference coordinate, then pre-multiply the homogeneous transformation
matrix T by the appropriate basic homogeneous rotation or translation matrix.
5. If the mobile coordinate system is to be rotated about or translated along one of its own
coordinate system, then post-multiply the homogeneous transformation matrix T by the
appropriate basic homogeneous rotation or translation matrix.
Thus composite homogeneous transformation matrices are built up starting with the
identity matrix and a rotation of α angle about OX axis, followed by rotation of φ angle
about OY axis followed by a rotation of θ angle about OZ axis as shown in the following
equations.
T R = T z ,θ T y ,φ T x ,α
cos θ
 sin θ
TR = 
 0

 0
− sin θ
cos θ
0
0
0 0  cos φ
0 0  0
1 0 − sin φ

0 1  0
0 sin φ
1
0
0 cos φ
0
0
(3.20)
0 1
0


0 0 cos α
0 0 sin α

1  0
0
0
− sin α
cos α
0
0
0
0

1
(3.21)
The homogeneous translation matrix for the three-dimensional space is
1
0
TT = 
0

0
0 0 dx 
1 0 dy 
0 1 dz 

0 0 1
(3.22)
Now the composite homogeneous transformation matrix is obtained by pre-multiplying
the composite rotation matrix by the composite translation matrix as
T = TT T R
(3.23)
The homogeneous transformation matrix can be considered to consist of the four submatrices as given in equation (3.24).


T =



R
0

P 


1
(3.24)
R is a [3× 3] rotation matrix and P is a [3×1] position vector. The geometric
interpretation of the homogeneous transformation matrix T is that it provides the
orientation and position of a mobile coordinate system with respect to a fixed reference
coordinate system. In robotic manipulator analysis of these two coordinate systems can be
assigned to two consecutive links of a robot arm, say link i − 1 and link i respectively. The
link i − 1 coordinate system is the fixed reference coordinate and the link i coordinate is
the mobile coordinate system. Using the homogeneous transformation matrix we can
specify a point Pi in the mobile coordinate i in terms of the fixed reference coordinate
i −1 .
Pi −1 = Tii−1 Pi
(3.25)
Where Tii−1 is a [4 × 4] homogeneous transformation matrix relating two coordinate i and
i − 1 . Pi is a [4 ×1] augmented position vector representing a point in the link i coordinate
system. Pi −1 is a [4 ×1] augmented position vector representing a point in the link
i − 1 coordinate system. Sometimes, we also want to express the position and orientation of
a point in the fixed reference coordinate system in terms of the mobile coordinate system.
( )
Pi = Tii−1
−1
(3.26)
Pi −1
Then, we need to find the inverse of the homogeneous transformation matrix T. The
inverse of a rotation sub-matrix R is equivalent to its transpose. However, the inverse of
the homogeneous transformation matrix is not equivalent to its transpose. The position of
the fixed reference coordinate system with respect to the mobile coordinate systems can
only be found after the inverse of Tii−1 is determined. In general, if η = 0 and σ = 1 , then
( )
inverse of
( ) can be found to be
Tii−1
(T )
i −1
i −1


=



R
T
0

− R P 


1 
T
(3.27)
3.2.5 Joint and Link Parameters
A robotic arm is a chain of rigid links interconnected joints. The joints can be revolute or
prismatic joints. Each joint link pair constitutes one DOF. For an n-degree of freedom or
n-axis robotic arm, there are n joint-link pairs. The link θ is attached to a base where a
reference coordinate frame is assigned and the remaining links are assigned to one
coordinate frame, and the last with the tool or end-effector. The joint and links are
numbered outwardly from the base. Only six different joints are possible: revolute,
prismatic, cylindrical, spherical, screw, and planar. Of these, only revolute and prismatic
joints are common in robotic manipulators. The PUMA arm consists of only revolute
joints as shown in Figure 3.4. The relative position and orientation of two successive links
can be specified by two joint parameters. This can also be seen in Figure 3.4.
A joint axis (x i , y i , z i ) is established at the connection of two links. The joint i connects
link (i − 1) to link i . The parameters associated with joint i are defined with respect to
z i −1 , which is aligned with axis of joint i . The relative position of two connected links is
given by the parameter d i called joint distance. It is the translation along z i −1 needed to
bring axis x i −1 onto axis x i . The second parameter is θ i called the joint angle. It is the
rotation about z i −1 needed to bring axis x i −1 parallel to axis x i . Thus θ i is a rotation about
the axis of joint i and d i is the translation along the axis of joint i . For each joint one of
these parameters will be fixed and the other is variable. The variable joint parameter
depends on the type of joints. For a revolute joint, the joint angle θ i is variable and the
joint distance d i is fixed. For a prismatic joint, the joint distance d i is variable and the
joint angle θ i is fixed.
A link is connected to two other links. Thus two joint axes are established at both ends of
the connection. The relative position and orientation of the axes of the successive joints
can be specified by two link parameters: link length and twist angle. The structure of the
link can be determined by the link length a i and the twist angle α i . a i is the shortest
distance measured along the common normal between the axes of joints i and i + 1 and
α i is the angle between the joint axes measured in a plane perpendicular to a i . The link
parameters are always constant and are specified as part of the mechanical design. Thus,
these four parameters come in pairs: link parameters (a i , α i ) determine the structure of the
link and the joint parameters (d i , θ i ) determine the relative position of the two successive
links.
For an N -axis robotic arm, there are 4 × N kinematic parameters that constitute minimal
set to specify the configuration of a robot. For each axis, three of these parameters are
fixed and depend on the mechanical design and the fourth parameter is the joint variable.
The variable joint and link parameters depend on the type of the joint. The Table 3.1
shows the parameters with different type of joints. Unlike the joint parameters, the two
link parameters are constant and are specified by the mechanical design of the robot. For
industrial robots, the link twist angle is usually multiple of π / 2 radians.
Figure 3.4: Links and joints of PUMA arm.
Table 3.1: Joint and link parameters
Parameters
Symbols Revolute
Prismatic
joint
joint
(θ i )
Joint angle
Variable
Fixed
(d i )
Joint distance
Fixed
Variable
(a i )
Link length
Fixed
Fixed
(α i )
Link twist angle
Fixed
Fixed
In general, a robot manipulator consists of a set of links connected together by various
joints. The joints can either be very simple, such as a revolute joint or a prismatic joint, or
they can be more complex joints. This is called a kinematic chain. Now the problem is to
determine the cumulative effect of the entire set of joint variables in a kinematic chain.
Moreover, the kinematic analysis of an N -link manipulator can be extremely complex
and a systematic approach is required that simplify the analysis considerably. In the
following section we will develop a set of conventions that provide a systematic
procedure for performing this analysis.
3.2.6 Denavit-Hartenberg Representation
A systematic technique to establish the translation and rotational relationship for each
adjacent links was proposed by Denavit and Hartenberg (1955). This approach is known
as D-H representation. Once the link-attached coordinate systems are assigned,
transformation between adjacent coordinate systems can be represented by a
[4 × 4] homogeneous transformation matrix (See section 3.2.3).
An orthonormal Cartesian coordinate system (Xi, Yi, Zi) can be assigned to each link i
where i = 1,2,3, K , N . Each (Xi, Yi, Zi) coordinate of a robotic arm corresponding to joint
0
0
i + 1 and is fixed in link i . The base coordinate is defined as the 0-th coordinate (X , Y ,
0
n
n
n
Z ) and the N-th coordinate (X , Y , Z ) as the tool tip or the end-effector. The coordinate
systems are assigned to the links by using the following D-H representation.
1. Number the links and joints starting with the base and ending with end-effector. The
starting base is denoted as link 0 and the end-effector is link n . Link i moves in respect
to link i − 1 around (for revolute) or along (for prismatic) joint i . Assign a right handed
coordinate system (X0, Y0, Z0) to the robot base and align Z0 with the axis of joint 1. X0
and Y0 axes can be normal to Z0 axis.
2. Establish link's coordinate system for each of the joints:
a) The Zi-1 axis is chosen along the axis of motion of the i -th joint.
b) The Xi axis is chosen orthonormal to both Zi-1 and Zi. If Zi and Zi-1 are parallel,
point Xi away from Zi-1.
c) The Yi axis is chosen to from a right-handed coordinate system.
3. Define the joint parameters which are the four geometric quantities θ i , d i a i and α i .
a) Compute θ i as the angle of rotation from Xi-1 to Xi measured about Zi-1 axis.
b) Compute d i as the distance from origin of the (i − 1) th coordinate system to the
intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. For a prismatic joint
d i is variable and for a revolute joint d i is fixed.
c) Compute a i as the distance from intersection of the Zi-1 axis and Xi axis to the origin
of the i -th coordinate system along the Xi axis.
d) Compute α i as the angle of rotation from Zi-1 measured about Xi axis.
4. Assign the N-th coordinate (Xn,Yn,Zn) to the end effector. Establish the origin conveniently along Zn, preferably at the center of the gripper or at tool tip point as shown in
Figure 3.5.
Yi
Z0
Xi
Zi
Yn
Xn
Zn
Y0
X0
Figure 3.5: Assignments of coordinates.
Once the D-H coordinate system has been established for each link, a homogeneous
transformation matrix can easily be developed relating to i -th coordinate to (i − 1) th
coordinate. A point in the coordinate system can be expressed in (i − 1) th coordinate
system by performing the following transformations:
1. Rotate about the Zi-1 axis an angle of θ i to align the Xi-1 axis with the Xi axis.
2. Translate along the Zi-1 axis a distance of d i to bring the Xi-1 and the Xi axes into
coincidence.
3. Translate along the Xi axis a distance of a i to bring the two origins as well as the x axis into coincidence.
4. Rotate about the Xi axis an angle of α i to bring the two coordinate systems into
coincidence.
Each of these four operations can be expressed by a homogeneous transformation matrix
Aii−1 as a product of four basic transformation matrices. Aii−1 is known as the D-H
transformation matrix for adjacent coordinate system, i and i − 1 . Thus we obtain
Aii−1 = T R ( z ,θ )TT ( x,d )TT ( x,a )T R ( x,α )
Aii−1
cos θ i
 sin θ
i
=
 0

 0
− sin θ i
Aii−1
cos θ i
0
0
cos θ i
 sin θ
i
=
 0

 0
0 0  1
0 0 0
1 0  0

0 1  0
0  1
1 0 0  0
0 1 d i  0

0 0 1  0
0 0
0 0 a i  1
0


1 0 0  0 cos α i
0 1 0  0 sin α i

0 0 1  0
0
− cos α i sin θ i
sin α i sin θ i
cos α i cos θ i
− sin α i cos θ i
sin α i
cos α i
0
0
a i cos θ i 
a i sin θ i 
di 

1

(3.28)
0
− sin α i
cos α i
0
0
0
(3.29)
0

1
(3.30)
Using the equations (3.30), we obtain the inverse of this transformation according to
(3.27).
cos θ i
sin θ i
0
− ai


− cos α sin θ

α
θ
α
α
cos
cos
sin
d
Sin
−
−
1
i
i
i
i
i
i
i 
Aii−1 = Aii −1 = 
(3.31)
 sin α i sin θ i
− sin α i cos θ i cos α i − d i Cosα i 
( )


0
0
0


1
Where α i , a i , d i are fixed and θ i is the joint variable for a revolute joint. For a prismatic
joint, the joint variable is d i and α i , a i , and θ i are constant and the transformation
matrix Aii−1 becomes
Aii−1 = T z ,θ T z ,d T x ,α
cos θ i
 sin θ
i
=
 0

 0
− cos α i sin θ i
sin α i sin θ i
cos α i cos θ i
− sin α i cos θ i
sin α i
cos α i
0
0
0
0 
di 

1
(3.32)
The inverse becomes
cos θ i
sin θ i
0
0


− cos α sin θ
cos α i cos θ i sin α i − d i Sinα i 
−1
i
i
i
i −1

Ai −1 = Ai =
(3.33)
 sin α i sin θ i
− sin α i cos θ i cos α i − d i Cosα i 


0
0
0
1


Using the transformation matrix, we can specify a point Pi in the coordinate i in terms of
the fixed reference coordinate i − 1 .
Pi −1 = Aii−1 Pi
(3.34)
( )
Where Pi −1 = (x i −1 , y i −1 , z i −1 ,1)T and Pi = (x i , y i , z i ,1)T .
3.2.7 The Arm Matrix
Once a D-H transformation matrix is obtained, we can arrive at a composite
transformation matrix which transforms end-effector coordinates into robot’s base
coordinates by multiplying successive transformation matrices starting at tool tip and
ending at the base. This composite homogeneous coordinate transformation matrix is
termed as arm matrix. There are four steps involved in constructing the homogeneous
transformation matrix. On performing the steps on a two consecutive coordinate frames,
and we can get the transformation matrix.
1. Rotate coordinate frame (i − 1) about Z i −1 by θ i
2. Translate frame (i − 1) along Z i −1 by d i
3. Translate frame (i − 1) about X i −1 by a i
4. Rotate frame (i − 1) about X i −1 by α i
By convention, the joints and links of a robotic arm are numbered outward starting with
the fixed base, which is link 0, and ending with the tool, which is link n. In order to
systematically assign coordinate frames to the links of an n-axis robotic arm special
attention must be taken the last link, i.e. the tool. The orientation of the tool can be
expressed in rectangular coordinates by a rotation matrix R , where the three components
corresponds to the normal, sliding and approach vectors as shown in Figure 3.6. The
approach vector (a ) is aligned with the tool roll axis. The sliding vector (s ) is aligned with
tool pitch (open-close) axis and orthogonal to approach vector. The normal vector (n ) is
aligned with tool yaw and orthogonal to plane defined by approach-sliding vectors.
tool
If Tbase
represent a transformation from tool tip coordinate of link n to base coordinate at
link 0 , then the homogeneous matrix is expressed as
i
tool
Tbase
= A01 A12 L Aii−1 =
∏A
j
j −1
for i = 1,2, L , n
(3.35)
j =1
The equation (3.35) can be written in terms of orientation matrix, position vector,
perspective and scaling factor as shown in (3.36).


 R tool
tool 
Pbase 
base
tool
(3.36)
Tbase
=




0
1 

Specifically, if i = 6 we have a 6 link manipulator and we obtain T = A06 . This T matrix
is used so frequently in robot kinematics that it is called the arm matrix.
Figure 3.6: Normal (n), sliding (s) and approach (a) vector.
3.2.8. Arm Equations of Robot Manipulators
Example 3.1: A three-axis spherical wrist
The spherical wrist configuration is shown in Figure 3.7, in which the joint axes z3, z4,
z5 intersect at o. The Denavit-Hartenberg parameters are shown in Table 3.2. The
Stanford manipulator is an example of a manipulator that possesses a wrist of this type. In
fact, the following analysis applies to virtually all spherical wrists.
We show now that the final three joint variables, {θ 4 , θ 5 , θ 6 } are the rotation angles
(Euler angles) {α , φ , θ } respectively, with respect to the coordinate frame (x3 y 3 z 3 ) . To
see this we need only compute the matrices A4 , A5 and A6 using Table 3.2 and the
expression (3.29). This gives
Figure 3.7: Schematic diagram of a three-axis spherical wrist.
Table 3.2: D-H parameters for spherical wrist.
Joint
4
5
6
(θ i )
(d i )
(a i )
(α i )
θ4
θ5
θ6
0
0
0
0
− 90 o
0
d6
0
0
cos θ 4
 sin θ
4
A34 = 
 0

 0
cos θ 5
 sin θ
5
A45 = 
 0

 0
cos θ 6
 sin θ
5
A56 = 
 0

 0
0
− sin θ 4
0
−1
cos θ 4
0
0
0
0
sin θ 5
0
0
0

1
(3.37)
0
0
0

1
(3.38)
0
0 0 
1 d6 

0 1
(3.39)
0 − cos θ 5
−1
0
0
0
− sin θ 6
cos θ 6
0
0
0
The arm equation obtained by multiplying the successive transformation matrices together
according to (3.35), which yields
T36 = A34 A45 A56
(3.40)
C 4 C 5 C 6 − S 4 S 6
S C C + C S
4 6
T36 =  4 5 6

− S5C6

0

− C 4C5 S 6 − S 4 S 6
− S 4C5 S 6 + C 4C6
C4 S5
S 4 S5
S5S6
0
C5
0
C4 S5d 6 
S 4 S 5 d 6 
C5 d 6 

1 
(3.41)
where C i = cos θ i and S i = sin θ i
Comparing the rotational part of T36 with the rotation angle (Euler angle) transformation
shows that {θ 4 , θ 5 , θ 6 } can indeed be identified as rotation angles {α , φ , θ } with respect to
the coordinate frame (x3 y 3 z 3 ) .
Example 3.2: Four-axis SCARA arm
Consider the SCARA manipulator, which consists of two revolute and one prismatic joint.
It has a one DOF wrist, whose motion is a roll about the vertical axis. The first step is to
locate and label the joint axes as shown in Figure 3.8. Since all joint axes are parallel we
have some freedom in the placement of the origins. The origins are placed as shown for
convenience. This is completely arbitrary and only affects the zero configuration of the
manipulator, i.e., the position of the manipulator when θ1 = 0 . The joint parameters are
shown in Table 3.3.
Figure 3.8: SCARA arm showing joints and links.
Figure 3.9: Schematic diagram of 4-axis SCARA arm
Table 3.3: Kinematic parameters of 4-axis SCARA arm
(θ i )
(d i )
(a i )
(α i )
Link
d1=fixed
a1
π
2
θ1
θ2
0
a2
0
3
0
θ3 = d3
0
0
4
θ4
d4=fixed
0
0
1
The Ai matrices are given in (3.42)-(3.45)
C1 − S1 0 a1C1 
S
C1 0 a1 S1 
A1 =  1
0
0
1
0 


0
0
1 
0
S2
0 a2C2 
C 2
S
− C 2 0 a 2 S 2 
A2 =  2
0
0
−1
0 


0
0
1 
0
1 0 0 0 
0 1 0 0 

A3 = 
0 0 1 d 3 


0 0 0 1 
C 4 − S 4 0 0 
S
C 4 0 0 
A2 =  4
0
0
1 d4 


0
0 1
0
The forward kinematic equations of the SCARA arm are given by
(3.42)
(3.43)
(3.44)
(3.45)
C1 2 C 4 + S1 2 S 4 − C1 2 S 4 + S1 2 C 4
S C − C S
− S 1 2 S 4 − C1 2 C 4
12 4
T04 = A1 A2 A3 A4 =  1 2 4

0
0

0
0

Where C ij = Cos (θ i + θ j ) and S ij = Sin(θ i + θ j ) .
a1C1 + a 2 C1 2 
a1 S1 + a 2 S1 2 
(3.46)
−1
− d3 − d4 

0
1

0
0
References
J.J. Craig (2005). Introduction to Robotics- Mechanics and Control, 3rd Edition, Pearson
Prentice Hall, Upper Saddle, NJ.
K. S. Fu; R. C. Gonzalez; C. S. G. Lee, (1987). Robotics: Control, Sensing, Vision, and
Intelligence, McGraw-Hill Inc., International Edition.
Y. Korem (1985). Robotics for Engineers, McGraw-Hill Book Company, New York.
D. McCloy and D.M. J. Harris (1986). Robotics: An Introduction, Open University Press,
Milton Keynes.
R. M. Murray, Z. Li and S.S. Sastry (1994). A Mathematical Introduction to Robotic
Manipulator, CRC Press.
R.P. Paul (1982). Robot Manipulators: Mathematics, Programming and Control, The MIT
Press, Cambridge.
R. J. Schilling (1990). Fundamentals of Robotics: Analysis and Control, Prentice Hall,
Englewood Cliffs, New Jersey.
W. E. Snyder (1985). Industrial Robots: Computer Interfacing and Control, Prentice Hall
Inc., Englewood Cliffs, New Jersey.
M. W. Spong; M. Vidyasagar (1989). Robot Dynamics and Control, John Wiley & Sons;
New York, Chichester, Brisbane, Toronto, Singapore.
M. Xie (2003). Fundamentals of Robotics: Linking Perceptions to Action, World
Scientific, New Jersey, London, Singapore, Hong Kong.