INVERSE KINEMATIC ANALYSIS
OF ROBOT MANIPULATORS
By
RACHID MANSEUR
A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY
UNIVERSITY OF FLORIDA
1988
To those who fought for
the freedom and education
of all Algerians.
ACKNOWLEDGMENTS
This work would not have been possible without the
teachings, the constant guidance, and the expert advice I
received from my advisor, Professor Keith L. Doty. I am
especially grateful for his encouragements, his enthusiasm,
his great sense of humor, and his friendliness which made
this work so enjoyable.
I would like to thank Dr. Herman Lam, Dr. Gerhard X.
Ritter, Dr. Ralph G. Selfridge, and Dr. Jose C. Principe for
their excellent service on my final committee. Thanks are
also due to Dr. Giuseppe Basile and Dr. Eginhard J. Muth for
their previous service on my committee.
I also wish to express my appreciation to Dr. Carl D.
Crane III, for his help with the computer graphics
simulations and the photographs of Figure 9.4 of this text,
and to Dr. Joseph Duffy for taking the time to answer my
questions.
The financial support I received, throughout my
doctoral studies, from the Mathematics Department and from
the Electrical Engineering Department of the University of
Florida, in the form of teaching assistantships, is
gracefully acknowledged. Special thanks are due to
iii
Dr. Peyton Z. Peebles and to Dr. Robert L. Sullivan for
their timely help and understanding.
I would like to take this opportunity to thank Ms.
Greta Sbroco for being such a great graduate secretary, for
caring, and for her constant encouragements.
All the students of the Machine Intelligence Laboratory
also deserve recognition for creating such a friendly and
productive working environment. In particular, I would like
to thank Dr. Subbian Govindaraj and Mr. David Alderman for
their help in the laboratory.
I would also like to express my gratitude and my love
to my family, specially my parents, Omar Amokrane Manseur
and Dhia Yamina SiAhmed, for their support, their
understanding and their patience throughout the long years
of my education.
My gratitude also goes to my parentsinlaw, Tahar
Zbiri and Faiza Houasnia for their help and support, and for
entrusting me with their daughter.
A very special and sweet thought goes to my wife,
Zohra, for her support, her patience throughout our common
student life, and for my beautiful children, Mehdi and Maya
Nibal, for being there and making it all worthwhile.
Finally, I wish to thank all our friends in Gainesville
for making it such a great city to live in.
TABLE OF CONTENT
Page
ACKNOWLEDGEMENTS.......... ............................. iii
ABSTRACT............................................... vii
CHAPTERS
1 INTRODUCTION.................................. ,1
2 THE INVERSE KINEMATICS PROBLEM............... 5
Notation and Mathematical Preliminaries. 5
Problem Definition...................... 10
3 EXISTING SOLUTIONS ............................ 13
ClosedForm Architectures............... 13
Record and Playback..................... 14
Numerical Techniques.................... 15
4 NEW APPROACH.................................. 18
LinkFrames Assignment.................. 18
The Reduced System of Equations.......... 19
Additional Inverse Kinematic Equations.. 24
Solving Inverse Kinematic Equations..... 25
Exchanging Base and EndEffector Frames. 27
5 SOLVING 4DOF MANIPULATORS................... 29
Reduced System of Equations............. 29
Special 4DOF Manipulator Geometries.... 35
6 SOLVING 5DOF MANIPULATORS................... 51
OneDimensional Iterative Method........ 51
FiveDOF Robots with ClosedForm
Solution............................... 63
7 SOLVING 6DOF MANIPULATORS................... 70
Reduction to a 4DOF Problem............ 70
TwoDimensional Iterative Technique...... 72
OneDimensional Method................... 82
ClosedForm Solution.................... 83
8 ORTHOGONAL MANIPULATORS..................... 87
9 APPLICATION EXAMPLES......................... 92
Example l:.The PUMA 560................. 92
Example 2: The GP66..................... 98
Example 3: The OM25 Manipulator.........108
Example 4: OM37 Manipulator.............122
Example 5: A General Geometry 6DOF
Manipulator ...........................124
10 CONCLUSION AND FUTURE WORK...................126
APPENDIX SOLVING FOR 8 ...............................133
REFERENCES... ..........................................136
BIOGRAPHICAL SKETCH.....................................139
vi
Abstract of Dissertation Presented
to the Graduate School of the University of Florida in
Partial Fulfillment of the Requirements for the Degree of
Doctor of Philosophy
INVERSE KINEMATIC ANALYSIS OF
ROBOT MANIPULATORS
By
RACHID MANSEUR
August 1988
Chairman: Dr. Keith L. Doty
Major Department: Electrical Engineering
Computercontrolled robot manipulators are becoming an
important part of automated manufacturing plants thereby
creating a need for reliable and fast control algorithms
that can improve the performance of robot manipulators in
industrial applications. An important part of such control
algorithms is the inverse kinematics portion which consists
of computing the values of the robotic joint variables
corresponding to a desired endeffector position and
orientation. This work is based on a new approach that uses
orthogonality of rotation matrices to reduce the problem to
a simpler form. The reduction techniques are first used to
analyze the kinematics of fourdegreeoffreedom (DOF)
robots. The results obtained are then applied to the study
vii
of five and sixdegreeoffreedom manipulators. Fast one
and twodimensional numerical techniques for solving five
and sixDOF arms of arbitrary geometry are developed. These
new methods provide a large reduction in computational
complexity and can be easily implemented in realtime
applications. Another contribution of this work is a
classification of robot geometries in terms of inverse
kinematic complexity. Some new sufficient structural
conditions for the possibility of closedform solutions for
five and sixDOF robot manipulators are described. In the
case of sixDOF arms, structural conditions for the
applicability of a onedimensional iterative technique are
also provided. Finally, in the example applications of the
techniques presented here, we describe a sixdegreeof
freedom manipulator capable of achieving a particular end
effector pose in sixteen distinct configurations.
viii
CHAPTER 1
INTRODUCTION
An important part of computer control algorithms for
open serial kinematic chains is the inverse kinematics
section. In any robotic application, the hand or end
effector of the robot may move along a trajectory specified
as a sequence of points at which the endeffector pose
(orientation and position) is known. While this trajectory
is specified in Cartesian coordinates, the motion of the
robot is controlled through individual joint actuators that
produce the necessary rotation in revolute joints, or the
translation in prismatic joints. The robot controller must,
therefore, be supplied the values of the joint variables
corresponding to the endeffector pose, i.e., the
coordinates in joint space of the robot hand for each point
along the trajectory must be computed. The conversion of
trajectory locations from Cartesian coordinates to joint
coordinates is referred to as the inverse kinematics
problem.
A desirable inverse kinematic algorithm is one capable
of producing the joint coordinates in realtime. While the
robot hand is at, or approaching, one location along the
trajectory, the algorithm must be able to produce the joint
coordinates for the next pose. In tasks where speed and
precision are important, the realtime requirement puts
heavy constraints on the computation time of the inverse
kinematic algorithm.
The forward kinematics problem, the conversion from
joint space to Cartesian space, is a much simpler problem
that has a unique closedform solution. In most cases a
robot manipulator can achieve a desired endeffector pose in
more than one configuration. The question of just how many
distinct solutions there are to the inverse kinematics
problem of general sixdegreeoffreedom (DOF) robot
manipulators has interested a few researchers. Roth,
Rastegar, and Scheinman (1973) put an upper bound of 32 on
the degree of a polynomial equation (in one joint variable)
that can be derived from the inverse kinematics problem of
sixDOF manipulators. A similar result was obtained by
Duffy and Crane (1980), using the equivalence between an
open 6revoluteDOF kinematic chain and the 7revolute
singleloop spatial mechanism. Therefore, the number of
inverse kinematic solutions for 6revoluteDOF manipulators
could be at most 32. More recently, Lee and Liang (in
press), using Duffy's method, were able to reduce the degree
of the inverse kinematic polynomial equation to 16, thereby
reducing the upper bound on the number of inverse kinematic
solutions to 16. Tsai and Morgan (1984), illustrating a new
inverse cinematic method capableof producingQall solutions,
found a robot manipulator and an endeffector pose with 12
possible solutions. Manseur and Doty (in press) described a
simple manipulator geometry capable of achieving a
particular endeffector pose in 16 distinct configurations,
thereby closing the proof that 16 is indeed the maximum
achievable number of inverse kinematic solutions for sixDOF
robot manipulators. The manipulator and the pose for which
the sixteen solutions were found and the inverse kinematic
solution search algorithm used are discussed in Chapter 9,
Example 3 of this dissertation.
Another desirable property of an inverse kinematic
algorithm is the capability of computing more than one
solution, so that a solution can be chosen according to some
optimality or collision avoidance criteria. Although for
manipulators with simple geometries, such as the PUMA 560
industrial robot, several possible solutions can be obtained
in closedform, this multiple solution property conflicts
with the realtime requirement discussed earlier for many
other robots that must rely on iterative techniques.
After introducing the notation and some mathematical
preliminaries and a brief discussion of existing inverse
kinematic methods, we present a new approach to theinverse
kinematic problem based on a reduced set of nonlinear
equations. This new approach is then used to analyze the
kinematics of four, five, and sixdegreeoffreedom
manipulators. Some simple and efficient iterative
4
techniques are described and sufficient manipulator
structural conditions for the applicability of these methods
are determined. All the methods developed in this
dissertation are illustrated by examples in Chapter 9.
Chapter 10 summarizes the final results, discusses the
contributions of this work to the field of robotics, and
presents related topics and areas of future research.
CHAPTER 2
THE INVERSE KINEMATICS PROBLEM
Notation and Mathematical Preliminaries
The DH Parameters
A robot manipulator is modelled as an open kinematic
chain of rigid bodies (links) connected by joints. A
reference frame is assigned to each link along the chain
starting with the base frame F0, assigned to the fixed link,
up to the end effector frame Fn, for a manipulator with n
degrees of freedom (DOF). The position and orientation of
frame Fi=(xi, yi, zi), with respect to the preceding frame
Fi_1, are entirely described by the four DHparameters di,
Gi, ai and ai (Denavit and Hartenberg 1955). These
parameters are illustrated in Figure 2.1 and defined as:
di = distance between the common normal to axes zi1 and zi
and the common normal to zi and zi+1 measured along
axis zi.
9i = the angle of rotation about zi so that xi becomes
parallel to xi_1 when 8i = 0.
ai = the length of the common normal to axes zi_1 and zi.
ai = the angle of rotation about xi so that zi becomes
parallel to zi_1 when ai = 0.
I/
di
zi
Fl 2in k (i l)ink i
Figure 2.1. The DHparameters./
Figure 2.1. The DHparameters.
When joint i is revolute, parameter ei is the joint variable
and, if joint i is prismatic, the joint variable is di. When
applicable, di measures the translation along axis zi1.
Homogeneous Matrices
If a vector iu = [iux, iy, iuz]T is expressed in frame
Fi, its expression with respect to frame Fi_, ilu,
satisfies
ilu Ci Siri Siai aiCi x
iluy Si CiTi Ciai aiSi iuy
(2.1)
i1a 1
i z 0 i i di
1 0 0 0 1 1
or in a compact notation,
iiu u
= Ai (2.2)
1 1
where Ti=cos(ai), ai=sin(ai), Ci=cos(9i), and Si=sin(9i) and
Ai is the homogeneous frametransform matrix (Paul 1981).
The leading superscript indicates the frame of expression.
The homogeneous matrix transform merely expresses the
fact that frame Fi can be obtained from frame Fi1 by the
following sequence of basic transforms:
1. Rotation about zi_1 of angle ei whose homogeneous
matrix is
Rz (i) =
Ci Si
Si Ci
1 1
0 0 1 0
0 0 0 1
2. Translation
the matrix
1
0
Trz(di) =
0 0
0 0
of di units along axis zi1 described by
0 0 0
1 0 0
0 u 1 ai
0 0 0 1
3. Translation of
homogeneous transform
Trx(ai) =
ai units along axis xi, with
1 0 0 ai
0 1 0 0
0 0 1 0
0 0 0 1
4. Rotation about xi of angle ai,
Rx (i) =
1 0 0 0
0 a1 i
0 ai ri
0 0 0 1
The matrix A1 of Eq. (2.1) is obtained by the product
(Paul, 1981),
(2.3)
(2.4)
(2.5)
(2.6)
Ai = Rz(9i) Trz(di) Trx(ai) Rx(ai).
A useful decomposition of matrix Ai is
Ai = Ai Bi (2.7)
with the definitions
Ai = Rz(ei) (2.8)
and
Bi = Trz(di) Trx(ai) Rx(ai). (2.9)
Explicitly, matrix Bi is
1 0 0 ai
0 ri Ci 0 Gi ki
Bi 1 (2.10)
0 ai 7i di 0 0 0 1
0 0 0 1
where Gi is the upper left 3 x 3 in Bi and ki is the upper
right 3 x 1 vector of Bi. The upper left 3 x 3 matrix in Ai
is the rotation matrix Ri necessary to align the unit
vectors of Fi with their counterparts in Fi_, while vector
aiCi
ii = aiSi
di
positions the origin of Fi with respect to Fi_,.
A compact and useful expression for Ai is
(2.11)
Rotation matrices are orthogonal, so Ril= RiT, where
the superscript T denotes the transpose operation, and the
inverse of matrix Ai can be expressed as
Ci
Siri
Sii
0
Si
ciri
Ciai
0
ai
aidi
ridi
1
RiT (RiTli)
(2.12)
0 0 0 1
Problem Definition
If the orientation of the endeffector is specified by
the rotation matrix R, necessary to align the unit vectors
of the endeffector frame Fn with the corresponding vectors
of base frame FO, and the position of the origin of the end
effector frame is given as a vector p with respect to the
base frame F0, then the endeffector pose is adequately
described by the 4 x 4 matrix
nx bx tx
ny by ty
nz bz tz
0 0 0
n b t p R p
0 0 0 1 0 0 0 1
(2.13).
Ai1
where
n b tx Px
n = ny b = by t = ty P = Py
nz bz tz pz
and
nx bx tx
R = "y by ty
nz bz tz
The inverse kinematics problem for a ndegreeof
freedom manipulator consists of finding a set of joint
variables values, called a solution set, that will satisfy
the equation
Al A2 A3 A4 A5...An = P. (2.14)
This matrix equation gives rise to a system of
nonlinear equations whose complexity depends on the
manipulator geometry, as described by the DHparameters.
At least six degrees of freedom are required to
arbitrarily position and orient a rigid body in space.
Therefore, when n is larger than six, the manipulator is
redundant and the system of equations implied by Eq. (2.14)
is underconstrained. If n is less than 6,' the system
becomes overconstrained and when n is equal to 6, the
inverse kinematic problem is exactly specified. In this
research we will address the inverse kinematics problem of
nonredundant robot manipulators.
Most existing industrial manipulators are 5 or 6
degreeoffreedom robots, hence, it is of practical
importance to solve Eq. (2.14) for n=5 and n=6. The
numerical techniques developed in this text are based on a
complete inverse kinematic analysis of fourdegreeof
freedom manipulators. Therefore, this research will aim at
solving Eq. (2.14) for robots with four, five, and six joint
axes.
Although the techniques described in this text can be
applied to manipulators having prismatic joints (Manseur and
Doty 1988), we concentrate on allrevolute sixDOF
manipulators; n is six and all joints are assumed revolute.
CHAPTER 3
EXISTING SOLUTIONS
ClosedForm Architectures
The ability to compute the coordinates in joint space
of an endeffector pose given in Cartesian space is an
important criterion in the design of computercontrolled
manipulators. A desirable property for an industrial
manipulator is the possibility of computing the joint
variables necessary to position and orient the endeffector
as specified in Cartesian space, in closedform. Pieper
(1968) has shown that a closedform solution is possible
when the manipulator has three adjacent joint axes
intersecting at a common point. The inverse kinematic
problem reduces then to a quartic polynomial equation in one
of the joint variables. Manipulators with the last three
joint axes intersecting are said to be "wristpartitioned".
Computationally efficient methods for computing the
position, velocity, and acceleration inverse kinematics for
this type of manipulators have been presented by
Featherstone (1983), Hollerbach and Sahar (1983), Paul and
Zhang (1986), and Low and Dubey (1986). Several industrial
six and fiveDOF manipulators such as the PUMA series
robots are of the wristpartitioned type. If, on top of
having a wrist, the manipulator has some added structural
feature such as two parallel or intersecting joint axes
then closedform solutions may be obtained in a simpler form
than a quartic polynomial equation. This is the case of the
PUMA 560 robot whose inverse kinematics are discussed in
Example 1 of Chapter 9. An algebraic method for solving the
inverse kinematics of the PUMA 560 can be found in Craig
(1986) and a geometric approach is described in Fu,
Gonzalez, & Lee (1987). Another sufficient condition for
closedform solutions is that three adjacent joint axes be
parallel (Duffy 1980, Fu, Gonzalez, & Lee 1987).
Record and Playback
An industrial robot manipulator is usually equipped
with sensors that can measure information such as joint
variable values and rates of change of those values. A
method that avoids the computational complexity of the
inverse kinematic problem altogether consists of remotely
guiding a robot endeffector trajectory by activating each
joint separately while storing joint space coordinates and
information from the sensors at selected' points along the
trajectory. The robot can then indefinitely repeat the
recorded motion. Should the robot be needed for a different
task or should a change in the workcell occur that requires
different endeffector trajectories, the motion of the robot
will have to be recorded again.
Numerical Techniques
Many six and fiveDOF kinematic structures lack the
necessary architectural simplicity for closedform inverse
kinematic solutions. Solving such manipulators requires the
use of numerical iterative techniques. For sixDOF robots,
equation (2.14) can be expressed as a system of six
nonlinear equations in the six joint variables of the form
fl(01' e2' 83' e4' e5' E6) = Px
f2( 1l 02' S3' O4, 85' 86) = Py
f3( 1' 82' 83' 84' 05' 86) = Pz
f4(8e1, 82, 83, 84, 85, e6) =
f54l' 82' 03' 4' O5' 6) = a
f5(81, 82, e3, e4, 85, G6) = E
f6(11, e2' e3' e4' e5' e6) = 0
where px, Py, and pz are the coordinates of the origin of
the endeffector frame and a, 8, and 0 are either the Euler
angles or the rollpitchyaw angles derived from the
orientation matrix R of the endeffector frame (Paul 1981).
The sixdimensional equation is then solved by use of a
direct or' modified NewtonRaphson or 'similar technique.
Multidimensional iterative techniques for solving the
inverse kinematics problem of manipulators of arbitrary
architecture are described by Angeles (1985, 1986),
Goldenberg, Benhabib', & Fenton (1985)'' Goldenbergaind
Lawrence (1985). The computational efficiency of these
methods is hindered by the need to compute the inverse of
the manipulator Jacobian at several points.
Linares & Page (1984) and Kazerounian (1987) describe
techniques that solve the inverse kinematic problem by
varying one joint variable at a time so as to minimize the
difference, measured by a defined norm, between the end
effector pose as computed from the current joint variables
values and the desired pose. This technique has the
advantages of guaranteed convergence and reliability even at
a singular position. This method requires computation of
the forward kinematics at each iteration and it has a
computational complexity comparable to that of a Modified
multidimensional NewtonRaphson.
After reducing the problem to a polynomial system of
four equations in only four of the joint variables, Tsai and
Morgan (1984) used a homotopy map method, for solving
systems of polynomial equations in several variables, to
find the solutions of the inverse kinematics problem of
revolute five and six degreeoffreedom manipulators of
arbitrary architecture. The method finds all solutions but
its computational complexity renders it impractical for many
applications.
Lumelsky (1984) presented an iterative algorithm that
finds estimates for three of the joint variables and solves
in closed form for the remaining three variables at each
iteration. The method applies to a particular type of arm
17
geometry (that of the GP66 robot discussed in Example 2,
Chapter 9, of this dissertation) and converges to an
accurate endeffector position, but to a less accurate
approximation of the endeffector orientation.
CHAPTER 4
NEW APPROACH
LinkFrames Assignment
Some simplification in the mathematical description of
the inverse kinematics problem can be obtained if certain
simple rules for assigning the linkframes are applied.
In selecting frame Fi, the direction of vector zi is
always chosen so that twist angle ai is in the interval
[O,r). If ai = 0, then vectors zi1 and zi are parallel and
the common normal can be arbitrarily located along both
axes. In this case the position of Frame Fi should be
chosen so that di is equal to zero.
For an nDOF robot, frame Fn, attached to the end
effector, can be chosen so that it differs from link frame
Fn1 only by a rotation of angle en about Znl. In other
words, Fn can be selected so that dn = an = an = 0, without
loss of generality. We prove this point for n = 6, but it
is valid for any relevant value of n. Let us assume that
Eq. (2.14) is to be solved with a 6DOF manipulator for
which d6, a6, or a6 is not equal to 0, then the homogeneous
matrix transform A6 decomposes into
A6 = A6 B6
as given in Eq. (2.7). Equation (2.14) is then equivalent
to
A1 A2 A3 A4 A5 6 P B6
where the right hand side of this last equation is seen to
be a constant pose matrix for a manipulator described by the
left hand side (i.e. one for which d6=a6=a6=0 so that
A6=A6)
When joint 1 is not prismatic, dI is constant and the
origin of the base frame FO can be positioned so that dI is
equal to 0.
The Reduced System of Equations
For a 6DOF arm, Eq. (2.14) becomes
Al A2 A3 A4 A5 A6 = P (4.1)
and it yields twelve non trivial scalar equations in the six
unknown variables. It is desirable to reduce this system to
a minimal number of equations involving as few of the joint
variables as possible. For allrevolute, 6DOF
manipulators, Tsai and Morgan (1984) have established~ that
with respect to frame F3, the zcomponent of the position
vector 3p and that of vector 3t along with the inner
products (3t.3p) and (3p.3p) provide 4 equations in only 4
of the unknowns, thereby reducing the complexity of the
problem. The process of obtaining these four equations
involved multiplying the Amatrices and simplifying the
expressions obtained for the elements of 3t and 3p. Besides
being lengthy, this method does not allow insight into the
mechanisms that make the simplifications possible. The
approach presented here provides the same results with much
less effort and greater insight by taking advantage of the
properties of rotation transformations.
By writing the product of two A matrices in the form
RiRj (Rilj + 1i)
Ai Aj =
0 0 0 1
we divide Eq. (4.1) into a position equation
p = R1(R2(R3(R4(R516+15)+14)+13)+12)+11 (4.2)
and an orientation equation
R = R1 R2 R3 R4 R5 R6. (4.3)
With the frame assignment conventions discussed, 16=0 when
joint 6 is revolute. Equation (4.2) then simplifies to
p = RI(R2(R3(R415+l4)+13)+l2)+11. (4.4)
Three independent scalar equations for px, Py, and pz
can be obtained from Eq. (4.4) and more equations can be
selected out of the 9 scalar equations implied by Eq. (4.3).
Since rotations are orthogonal transformations, they
leave inner products invariant, hence
R u R v = u v (4.5)
for any rotation matrix R and any vectors u and v. A
special case of (4.5) that is very useful is
R u v = u RIv. (4.6)
These properties are extremely efficient in eliminating
algebraic terms and unnecessary joint variables when applied
to Eqs. (4.3) and (4.4) if it is further recognized that
Rilli = [ai,dii,dii]T (4.7)
and
Rilz = [0,ai,7i], (4.8)
where z = [ 0, 0, 1]T, are always independent of ei. Also,
due to the frame assignments discussed above,
l
R6 z = R61 z = z
in all cases since frame F6 is chosen to force a6 = 0.
By repeated use of Eqs. (4.5) and (4.6), we obtain four
reduced equations from Eqs. (4.3) and (4.4).
tz equation.
tz = t z = (R z) z
t = (R1 R2 R3 R4 R5 R6 z) z
tz = (R1 R2 R3 R4 R5 Z) z
t = z (R51 R41 R31 R21 R11 z) (4.9)
pz equation
p = R1 R2 R3 R4 q
with
q = 15 + R41(14 + R31(13 + R21(12 + R111))),
so that
pz = p z = q (R41 R31 R21 R11) z. (4.10)
p.t equation.
p t = R51q z (4.11)
p.p equation.
p.p = q.q = q2. (4.12)
Since Ri1i1 and R11z are independent of ei (Eqs.
(4.7) and (4.8)), vector q and Eqs. (4.9)(4.12) are easily
seen to be independent of the first and last joint variables
and therefore form a system of 4 equations in 4 unknowns.
Figure 4.1 illustrates this discussion. With 16=0, vector
t, which coincides with z5, and the position vector p of the
origin of frame F6 are invariant in the rotation R6
(rotation about z5 which can only affect the endeffector
orientation). Rotation about z0 has no effect on the z
component of any vector expressed in frame F0. Hence, p and
t are independent of 81 as well. Finally, since rotation
about z0 moves all the robotic structure as a bloc, it does
not affect the length of vector p or the inner product of t
and p..
I
e6
I
SZ5
e, n
I P
Zo
20 F
/Yo
F
xo
Figure 4.1. Rotations about z0 or z5 do not affect
:tz, Pz, t.p, and p.p.
The reduced system of equations (4.9)(4.12) determines
candidate solutions for joint variables 2, 3, 4, and 5.
Once this system of equations is solved, the remaining two
variables can be found by using more equations from (4.1)
and then tested for consistency. The power of this approach
will become apparent for specific manipulators as further
simplification using Eqs. (4.5)(4.8) becomes obvious.
Furthermore, simplification by use of rotation innerproduct
invariance is computationally economical and provides
greater insight into the structure and properties of the
inverse kinematic equations.
Additional Inverse Kinematics Equations
Equations (4.9)(4.12) are necessary, but not
sufficient. Although they are satisfied by all solution
sets of Eq. (4.1), they are also, in general, satisfied by
extraneous solutions. This problem was reported by Tsai and
Morgan (1984) as well..
Another problem with considering Eqs. (4.9)(4.12)
alone is the presence of sign ambiguities. In many
practical situations, one of the equations will allow a
closedform solution for either the sine or the cosine
function of a revolute variable 8. The other function needs
to be computed using the Pythagorean identity, which offers
two values opposite in sign. Although both signs can be
tried in the search for a solution, in some cases the number
of sign ambiguities can be reduced by considering more
constraints from Eqs. (4.3) and (4.4). Additional equations
will also help filter out extraneous solutions and in some
cases will ease the solutionfinding process rather than
complicate it. The x and ycomponents of vectors t and p
provide convenient additional constraints at the cost of
introducing the variable 81. Equations
tx = R1 R2 R3 R4 R5 z x, (4.13)
ty = R1 R2 R3 R4 R5 z y, (4.14)
Px = (R1(R2(R3(R415+14)+13)+12)+11) x, (4.15)
and
py = (R1(R2(R3(R415+14)+13)+12)+11) y, (4.16)
where
1 0
x = 0 and y = 1
0 0
are the usual canonical unit vectors, are still independent
of 96'
Solving Inverse Kinematic Equations
Once the reduced set of equations (4.9)(4.12) and the
additional equations (4.13)(4.16) have been expanded, the
problem becomes that of extracting the values of the joint
angles from the equations which are in terms of the sines
and cosines of the angles. In this section, we describe
some of the techniques that can be used for this task.
Certain simple arm geometries allow a closed form
solution. For such arms, one of the equations will have the
form
aS+bC=d
where S and C are the sine and cosine, respectively, of some
angle e. If the constants a, b, and d are known, then there
are two possible solutions when a2 + b2 d2
e = atan2[d,J(a2+b2d2)] atan2(b,a)
where atan2(v,w) returns the angle arctan(v/w) adjusted to
the proper quadrant according to the sign of the real
numbers v and w.
A special case occurs when a = 0 or b = 0. The
equation can then be solved for S or C separately. The
other variable can be obtained from the Pythagorean identity
S2 + C2 = 1 (4.17)
with a sign ambiguity. Again, this leads to two possible
values for the angle 8,
e = atan2(S, /(1 S2)) if S is computed or
9 = atan2( J(1 C2), C) if C is the known
variable.
A value of 8 can be directly and uniquely obtained when
two linear equations in the sine and cosines of one angle
are obtained. In this case the values of S and C are
computed and the angle 9 is then given by
8 = atan2(S, C).
Exchanging Base and EndEffector Frames
The inverse kinematics problem consists of finding
joint variables that realize a given relationship between
two frames, the base frame F0 and the endeffector frame Fn.
The roles of these two frames are in fact interchangeable as
we illustrate in Figure 4.2. This means that the problem
can be viewed as finding the joint variables necessary for
the robot to achieve the base frame as viewed from the end
effector frame. This problem reversal requires that the DH
parameters be rearranged and intermediate frames be
reassigned as illustrated in Figure 4.1 but it can be useful
in many ways. For example, several computationally
efficient inverse kinematic techniques have been developed
for robots with the last three joint axes intersecting at a
common point (Featherstone 1983, Hollerbach and sahar 1983,
Paul and Zhang 1986, Low and Dubey 1986). The same
techniques can be used for a robot whose first three axes
intersect by reversing the roles of endeffector and base
frame. In the next chapters, we will use this problem
reversal technique to avoid repetitious developments.
ende ffector
base
base
endeffector
6x4
Z5 t Z6 `
Figure 4.2. Interchanging base and endeffector
frames.
CHAPTER 5
SOLVING 4DOF MANIPULATORS
Reduced System of Equations
For 4DOF robot arms, the inverse kinematic problem is
solved when 4 joint values are found that satisfy the
equation
A1 A2 A3 A4 = P. (5.1)
Equation (5.1) decouples into a position equation
R1 (R2 (R3 14 + 13) + 12) + 11 = p, (5.2)
and an orientation equation given by
R1 R2 R3 R4 = R. (5.3)
When the fourth joint .is revolute,_14=0 is obtained by
proper choice of frame F4 and Eq. (5.2) simplifies to
R1 (R2 13 + 12) + 11 = p. (5.4)
A reduced system of four equations in the sines and
cosines of joint angles e1 and e3 can be derived by
considering the quantities tz, pz and the inner products
t.p and p.p expressed in; frame F1
Vector t is given by
t = R z = R1 R2 R3 R4 z
where z is the third canonical unit vector z = [ 0, 0, 1]T
Since twist angle a4 is equal to 0,
R4 z = [04 S4, U4 C4, T4]T = [0, 0, 1]T = ,
and the expression for t simplifies to
t = R1 R2 R3 z. (5.5)
Multiplying by R1I yields
R1t
R1 t = R2 R3 z
and the inner product of each side of this equality with
vector z provides
z (Ri1 t) = z (R2 R3 z).
Eq. (4.5) applied to both sides of this last equation gives
R1 t = R21 z. R3 z
or
(R1 z).t (R21 z).(R3 z) = 0. (5.6)
Since R21 z = [0, a2, T2] does not depend on 82, this
last equation is independent of joint variables 2 and 4.
Subtracting vector 11 from both sides of Eq. (5.4) and
multiplying by: R1 yields;
R2 13 + 12 = R11 (p 11) (5.7)
and taking the innerproduct with vector z provides
(z R2 13) + (z 12) = (z R11 p) (z RI1 11).
Applying (4.5) to the first term of both sides of this
equation gives (after rearranging terms)
1 1
R1 z p R1 z 13 = z RI1 11 + z 12. (5.8)
The right hand side of Eq. (5.8) is constant since
ai
Ri1 ii = diai and z.li=di are independent of ei.
diri
Multiplying Eq. (5.7) by R21 gives
13 + R2 12 R2 RI1 (p 11) (5.9)
and multiplication of Eq. (5.5) by R21 R11 yields
R3 z = R21 R11 t. (5.10)
The inner product of corresponding sides of equations
(5.9) and (5.10) produces
(13 + R21 12).(R3 z) = [R21 R11 (p 11)].[R21 RI1 t].
Repeated use of properties (4.4) and (4.5) and reordering
simplifies this last equation to
11.t [R21 12 R3 z] = t.p [R31 13 z]. (5.11)
Equation (5.11) is also independent of 82 and E4.
Using Eq. (5.4), the innerproduct p.p satisfies
p.p = [R1 (R2 13 + 12) + 11].[R1 (R2 13 + 12) + 11].
Expanding the left hand side, using innerproduct invariance
of rotations where needed, and rearranging terms yield
13.R21 12 + p.11 = [p.p + 11.11 12.12 13.13]/2. (5.12)
Equations (5.11), (5.2), (5.18), and (5.12) form a
linear system in the variables S1, C1, S3 and C3. The four
equations obtained are
alty S1 + altx C1 + a2C3 S3 a2a3d2 C3 = r1 (5.13)
0atx S1 Olty C1 + a2a3 C3 = r2 (5.14)
alpx S1 alPy C1 a2a3 S3 = r3 (5.15)
alPy S1 + alpx C1 + a2a3d2 S3 + a2a3 C3 = r4 (5.16)
with
r = t.p 3d3 dltz T23d2 (5.17)
r2 = T23 r1 t (5.18)
r3 = T(d tz) + d2 + T2d3 (5.19)
r4 = (p.p + a12+d2a22d22a d32)/2
dlPz 2d2d3. (5.20)
The linear system of equations formed by Eqs. (5.13)(5.16)
will be referred "'to as "the reduced system for a fourDOF
manipulator. Although di can be assumed zero without loss
of generality for a 4DOF manipulator, this system of
equations will be used for 4DOF sections of larger
manipulators (next chapters) for which the parameter
corresponding to di will, in general, not be zero. Hence,
dl is assumed not equal to zero at this point.
A unique solution to the reduced system is given by
S1 ri
C1 H r2 (5.21)
SH (5.21)
S3 r3
C3 r4
where
alty altx a2a3 a2a3d2
altx alty 0 a2a3
H = 1 (5.22)
alPx alPy a2a3 0
alPy alPx a2a3d2 a2a3
when matrix H is nonsingular. Unique values of 81 and 83
are thus obtained from the values of Sl, Cl, S3, and C3.
The case where H is not invertible is discussed in the next
sections because of its interesting implications.
With 81 and 83 known, Eq. (5.7) provides a'way to solve
for 82. Indeed, when expanded, the first 2 components yield
(a2d372a3S3) S2 + (a2+a3C3) C2 = ClPx + SlPy al (5.23)
and
(a2+a3C3) S2 (a2d3r2a3S3) C2 =
71SlPx + T1ClPy + al(Pzdl). (5.24)
When the determinant of this linear system of equations in
S2 and C2 is not 0, a unique value of 82 can be computed.
Otherwise, we can obtain e2 uniquely from another linear
system of equations in S2 and C2,
(r2a3C3+a2r3) S2 + a3S3 C2 = Cltx + Slty (5.25)
and
a3S3 S2 (T2a3C3+a2T3) C2
= 71Sltx + T1Clty + 1itz, (5.26)
derived from Eq. (5.5). Note that 92 can also be computed
using a system of two equations formed by Eq. (5.23) or Eq.
(5.24) and one of Eqs. (5.25) and (5.26). The Appendix
shows that the determinants of the two systems of equations
above are simultaneously zero only when joint axis 2 aligns
with another joint axis which puts the arm in a degenerate
configuration.
To complete the 4DOF solution set, we use Eq. (5.2)
which can be rewritten as
R = R11 R21 R1 R.
The first column vector of R4, obtained by multiplying both
sides by the first canonical unit vector x = [1,0,0]T,
C4 n1
R = S4 R31 R21 1R x = R3 R2 1 R1
0 nz
can be used to compute the last variable G4. This shows
that a 4DOF inverse kinematic problem will, in general
(general in the sense that matrix H is nonsingular), yield a
unique solution set. However, for some manipulator
geometries and/or some particular endeffector poses, the
problem may have more than one solution.
Special 4DOF Manipulator Geometries
Equation (5.21) is valid only when matrix H is
invertible. The determinant of matrix H, computed from Eq.
(5.22), is given by
2 2
dH = 61 62 al[a2 (a32 wI + 32 W2) + 2 02 03 a3 d2 w3]
2 2 +2 2 2 2
+ 03 a3 1[1 (a2 + 022 d2 ) + 02 a12] w4 (5.27)
where the quantities wl, w2, w3, and w4 are defined, in
terms of the components of pose vectors t and p, as
w tx2 + ty2
W2 = x2 + y2
w3 = Px tx '" 
Sw4=~Px ty :y tx'. 
W 4 x
Analyzing dH
Equation (5.27) shows that the value of dH depends on
the seven robot parameters al, a2, a3, al, a2, a3, and d2 as
well as the pose quantities wl, w2, w3, and w4. However,
for certain robot structures dH is equal to zero no matter
what the endeffector pose is. The expression of dH above
provides us with a way to find all such 4DOF robot
geometries. Due to our link frames assignment, the only
robot parameter in the expression of dh that can be negative
is d2. By expanding Eq. (5.27) we get
dH = aI o2 al a2 a32 wI + 1 "2 032 a a2 w2
+ 2 01 02 a3 al a3 d2 w3 + a12 03 a22 a3 w4
+ 02 022 03 a3 d22 W4 + 22 3 al2 a3 w4 (5.28)
where only the quantities d2, w3, and w4 can be negative.
If an arm structure is such that dH is zero for every
possible endeffector pose, then dH will be zero even for a
pose with positive w3 and w4.
If we assume w3, and w4 non negative, then with d2
negative, dH can be zero if the equality
2 a0 022 03 a a3 d2 w3 =
C01 2 al a2 a32 1 + 01 02 032 al a2 2
+ 012 3 a22 a3 W4 + al2 022 03 a3 d2 W4 + 022 03 a2 a3 W4
holds. However, such an equality is actually a condition on
pose quantities wl, w2, w3, and w4. We conclude that robot
structures for which dH is always zero (independent of the
endeffector pose) have DHparameters al, 02, 03, al, a2,
a3, and d2 for which each of the six terms in Eq. (5.28) is
individually zero. These terms, in turn, are zero when
particular structure parameters are equal to zero. For
example, a1=a2=0 will make the determinant zero. In order to
enumerate the minimum number of distinct combinations of
zero parameters that make dH = 0, we examine all possible
cases when a particular parameter is zero. We get seven
simpler expressions of dH, listed in Table 51., by
separately assuming each relevant parameter to be equal to
zero.
Table 51 provides a simple mean for finding all (pose
independent) 4DOF robot geometries for which matrix H will
be singular. In the next section, we show that the inverse
kinematics problem for such robots can still be solved by
use of the reduced system of equations (5.13)(5.16).
Special 4DOF Arm Structures
A trivial condition occurs when two consecutive joint
axes coincide somewhere along the arm. Such a degenerate
condition is detected by ai = ai = 0 for some joint i. In
this case, the manipulator loses one degree of freedom and
becomes a redundant 3DOF arm. If a solution set exists for
such an arm, there will be an infinite number of solution
sets. A careful analysis of Table 51 shows that there are
only ten minimal, nontrivial, conditions on the arm
Table 51. Special expressions for dH.
Condition dH
1 01 = 0 022 a3 a12 a3 w4
2 C2 = 0 12 o3 a22 a3 w4
3 03 = 0 1 o02 al a2 a32 w1
4 al = 0 a12 03 a3 (a2 + 22 d22) w4
5 a2 = 0 22 03 a3[2 01 al d2 w3 + (a12 + o12 d2)W4]
6 a3 = 0 1 02 032 a1 a2 w2
7 d2 = 0 01 02 al a2 (032 W2 + a32 W1)
+ 03 a3 (022 a12 + 12 a22) w4
geometry (poseindependent) for dH = 0. All ten conditions
are listed and described in Table 52 and illustrated in
Figure 5.1.
The first three conditions in Table 52 follow from the
first entry of Table 51. Conditions 4 and 5 are derived
from the second entry in Table 51 after dropping duplicate
conditions already established. Continuing in this fashion,
all of Table 52 can be completed. Observe that entry 7 in
Table 51 does not add any new conditions into Table 52
since all minimal sets of zero parameters implied by dH=0 in
entry 7 have already been accounted for. 
It must be noted that dH can still be zero for 4DOF
arm geometries not= listed in the preceding. Table. However
from the discussionabove, we see that such a situation can
only happen at particular endeffector poses whereas dH will
Table 52. Special structures of 4DOF manipulators
Condition Description
1 01=02=0 First 3 joint axes (1,2,3) are parallel
2 al=a3=0 First 2 axes (1,2) are parallel and
Last 2 axes (3,4) are parallel
3 al=a3=0 First 2 axes (1,2) are parallel and
last 2 axes (3,4) intersect
4 02=a3=0 Last three axes (2,3,4) are parallel
5 a2=a3=0 Middle axes (2,3) are parallel and
last 2 axes (3,4) intersect
6 C3=al=0 First 2 axes (1,2) intersect & last 2
axes (3,4) are parallel
7 a3=a2=0 Middle axes (2,3) intersect & last 2
axes (3,4) are parallel
8 al=a3=0 First 2 axes (1,2) intersect & last 2
axes (3,4) intersect
9 al=a2=d2=0 First 3 axes (1,2,3) intersect
10 a2=a3=0 Middle 2 axes (2,3) intersect and last
2 axes (3,4) intersect
be zero for the geometries described in Table 52 at any
pose. We now examine in detail the inverse kinematics of
each of the special 4DOF robot architectures described in
Table 52 and illustrated in Figure 5.1.
1. First three joint axes
are parallel.
3.Axes 1 and 2 are parallel,
axes 3 and 4 intersect.
5.Axes 2 and 3 are parallel,
3 and 4 intersect.
2.Axes 1 and 2 are parallel,
3 and 4 are parallel.
4.Axes 2, 3, and 4 are
parallel.
6.Axes 1 and 2 intersect
2 and 3 are parallel.
Figure 5.1. Special 4DOF structures.
7.Axes 2 and 3 intersect,
3 and 4 are parallel.
9.Axes 1 and 2 intersect,
3 and 4 intersect.
8.Axes 1, 2, and 3 intersect.
10.Axes 2 and 3 intersect,
3 and 4 intersect.
Figure 5.1Continued. 
The reduced system of equations (5.13)(5.16) can still
be efficiently used to find all the solution sets of Eq.
(5.1) when matrix H is singular.
Case 1: al=a2=0. First three joint axes are parallel
(Entry 1 in Table 52).The reduced system of equations
becomes
alty S1 + altx C1 + a2a3 S3 = r1 (5.29)
0 = r2 (5.30)
0 = r3 (5.31)
alPy S1 + alPx C1 + a2a3 C3 = r4. (5.32)
Equations (5.30) and (5.31) are constraints on pose
parameters tz and pz respectively. Only endeffector poses
that satisfy p = dl + d2 + d3 and tz = 73 (Eqs. (5.18) and
(5.19)) are solvable with this arm geometry.
Equations' (5.29) and (5.32) still allow a solution in
the style of Pieper (1968) by first eliminating S3 and C3
from the equations. This can be done by solving for S3 and
C3 and substituting in the Pythagorean identity (4.17) to
get
{[rl (alty S1 + altx C1)]/a2a3)2 +
{[rl (alPy S1 + alPx Cl)]/a2a23 = 1. (5.33)
With the trigonometric identities
Sl = 2 tl/(l + t12) and C1 = (1 t12)/(l + t12)
where tI = tan(81/2), Eq. (5.33) yields a quartic polynomial
equation in tI. With ti computed, a value of 81 is obtained
and 83 can be computed uniquely from Eqs. (5.29) and (5.32).
The remaining angles (e2 and 84) can be computed as
indicated earlier.
We propose a method that allows better insight without
the complexity of a quartic polynomial equation. For
simplicity, the sine and cosine of a sum of angles will be
represented according to Cijk=cos(ei+9j) and Sij=sin(ei+9j).
As described in chapter 4, a set of inverse kinematic
equations can be obtained by expressing the components of
vectors t and p and the inner products t.p and p.p in terms
of the joint variables ei, i=l, . 3. The equations
obtained are
t = a3 S123 (5.34)
ty = a3 C123 (5.35).
Px = a3 C123 + a2 C12 + al C1 (5.36)
Py = a3 S123 + a2 S12 + al S1 (5.37)
t.p= al C23 + a2a3 S3 + T3(dl+d2) + d3 (5.38)
p.p = 2(a1a3 C23 + a2a3 C3 + ala2 C2) + ct (5.39)
where
ct = a12 + a22 + a32 + d12 + d22 + d32
+ 2(d1d2 + d1d3 + d2d3).
Equations (5.34) and (5.35) yield S123 and C123
directly, so .a unique value of 8123 = 1+82+3 is obtained.
With 8123 known, Eqs. (5.36) and (5.37) become (elbow
equations)
Px a3 C123 = a2 C12 + al C1 (5.36')
py a3 S123 = a2 S12 + al S1 (5.37')
and can be solved for C2 by
C2 = [(Px a3 C123)2 + (Py a3 S123)2
a22 a12] / (2 al a2)
which is obtained by applying the cosine law to the triangle
having links 1 and 2 as its sides. Two values of 92 follow
from e2=atan2( J(1C22), C2)
and a unique value of 81 can then be computed from Eqs.
(5.36') and (5.37') which yield a linear system in S1 and C1
when S12 and C12 are expanded using sum of angles
trigonometric identities. Joint variable 3, 83 is given by
83 = 6123 81 82
and the solution set is completed when the last angle 04 is
computed as shown earlier. This development proves that
there can be at most 2 solution sets for a 4DOF arm with
this particular geometry.
Case 2: al=a3=0. The first two joint axes are parallel
and the last two joint axes are parallel. The reduced
system is
aty S1 + altx Cl = rI (5.40)
0 = r2 (5.41)
2a3 S3 = r3 (5.42)
alPy S1 + alPx C1 + a2a3d2 S3 + a2a3 C3 = r4. (5.43)
Equation (5.41) imposes a constraint on pose parameter tz,
tz = T2T3/T1. When this constraint is satisfied, Eq. (5.40)
can be solved and yields two distinct values for e1. Then
Eqs. (5.42) and (5.43) form a linear system in S3 and C3
which can be solved uniquely for e3. With 91 and e3
computed, 82 and 84 can be uniquely obtained as shown
earlier. Here again we find at most two solution sets.
Case 3: al=a3=0. First two joint axes are parallel and
last two joint axes intersect. The reduced system becomes
alty S1 + altx C1 + a2a3 S3 a2a3d2 C3 = r1 (5.44)
a2a3 C3 = r2 .(5.45)
0 = r3 (5.46)
alPy S1 + alPx C1 = r4. (5.47)
From Eq. (5.19), the pose constraint r3 = 0 translates to
Pz = dl + d2 + T2d3*
For a pose matrix that satisfies this constraint, two
possible values of 83 can be obtained from Eq. (5.45). For
each of those 83 values, a unique value of el is computed
from the linear system in S1 and C1 formed by Eqs. (5.44)
and (5.47). The two solution sets are then completed as
shown previously.
Case 4: a2=a3=0. The last three joint axes are
parallel. The reduced system simplifies to
alty S1 + altx C1 = r1 (5.48)
altx S1 alty C1 = r2 (5.49)
l~px S1 alPy C1 = r3 (5.50)
alPy S1 + alPx C1 + a2a3 C3 = r4. (5.51)
Two out of the first three equations (Eqs. (5.48)(5.50))
can be used to solve uniquely for e1. The third (unused
equation) becomes a realizability constraint on the pose.
With 81 known, Eq. (5.51) yields a value for C3 which in
turn gives two possible values for 93. Two solution sets
can be obtained after computing e2 and e4.
Case 5: a2=a3=0. The intermediate joint axes are
parallel and the last two axes intersect. The reduced system
becomes
alty S1 + altx C1 + a2a3 S3 = rl (5.52)
altx S1 alty C1 = r2 (5.53)
'lPx S1 alPy C1 = r3 (5.54)
alPy S1 + alPx C1 =r4. (5.55)
Two out of the last three equations (5.53)(5.55) can be
solved uniquely for el. The third equation becomes a
realizability constraint on pose element tz or pz depending
on the chosen equation. With el known, two values of 83
can be computed from the value of S3 derived from Eq.
(5.52).
Case 6: a3=al=0. The last two joint axes are parallel
and the first two axes intersect. The reduced system is
0 = r1 (5.56)
altx S1 alty C1 = r2 (5.57)
a1Px S1 alPy C1 a2a3 S3 = r3 (5.58)
a2a3d2 S3 + a2a3 C3 = r4. (5.59)
Equation (5.56) is a realizability constraint on pose
parameter tz (Eq. (5.17)). For an endeffector pose that
satisfies this constraint, Eq. (5.57) yields two values for
e1. Equations. (5.58) and (5.59) can then be solved for 63
uniquely.
Case 7: a3=a2=0. The last two joint axes are parallel
and the intermediate two axes intersect. The reduced system
is
alty S1 + altx C1 = rl (5.60)
altx S1 alty C1 = r2 (5.61)
CaPx S1 aiPy C1 2a3 S3 = r3 (5.62)
alPy S1 + alPx C1 + a2a3d2 S3 = r4. (5.63)
Here, Eqs. (5.60) and (5.61) yield a unique value for 81,
then one of Eqs. (5.62) or (5.63) can be used to solve for
S3 thereby providing two values for e3, the remaining
equation is a pose constraint.
Case 8: al=a2=d2=0. The first three joint axes
intersect and the reduced system becomes
0 = rl (5.64)
it x S1 alty C1 + 0a23 C3 = r2 (5.65)
1iPx S1 alPy C1 a2a3 S3 = r3 (5.66)
0 = r4. (5.67)
Equations (5.64) and (5.67) impose constraints on pose
parameters tz and pz. Here again a solution can be obtained
in form of a quartic polynomial equation in tl=tan(81/2) by
eliminating 83 from Eqs. (5.65) and (5.66) as we did earlier
in case 1.
With 81 known, 83 can be uniquely obtained from Eqs.
(5.65) and (5.66) and the solution set completed as usual.
This method puts an upper bound of 4 on the number of
solution sets since at most four distinct values of 81 can
be obtained from the quartic polynomial equation in ti.
An easier inverse kinematic analysis of this structure
can be obtained if the roles of endeffector frame and base
frame are reversed and the intermediate linkframes are
reassigned accordingly. This will put the three
intersecting axes at the endeffector position instead of at
the base. The 4DOF structure is seen to be equivalent to
one that has a2 = a3 = 0 which is discussed in case 10.
With this analysis, we find that there can be at most two
solution sets.
Case 9: al=a3=0. The first two joint axes intersect
and the last two joint axes intersect. The reduced system
is
a2a3 S3 a2a3d2 C3 = r1 (5.68)
a1tx S1 alty C1 + a2a3 C3 = r2 (5.69)
alPx S1 alPy C1 = r3 (5.70)
0 = r4. (5.71)
Here, r4 = 0 poses a constraint on pose parameter pz.
Equation (5.70) yields two distinct values of el, then Eqs.
(5.68) and (5.69) will form a linear system in S3 and C3
which can be solved for a unique value of 83 for each value
of e1. Two solution sets are thus obtained.
Case 10: a2=a3=0. The intermediate two joint axes
intersect and the last two joint axes intersect. The reduced
system becomes
alty S1 + altx C1 a2a3d2 C3 = r1 (5.72)
altx S1 alty C1 + 0203 C3 = r2 (5.73)
alpx S1 alPy C1 = r3 (5.74)
alPy S1 + alPx C1 = r4. (5.75)
Equations (5.74) and (5.75) yield a unique solution for e1.
The value of el obtainedcan be substituted in Eq. (5.72) or
(5.73) to solve for C3 which provides two possible values
for 83. The unused equation is a constraint on the end
effector pose parameter tz.
When a3 = 0 or d2 = 0, the conditions for dH=0 obtained
have all been already discussed, hence there are only ten
minimal poseindependent arm geometry conditions for which
the reduced system is singular. In all ten cases, we found
at most two distinct inverse kinematic solution sets.
To summarize the above cases, we find that a fourDOF
robot manipulator will in general have a unique inverse
kinematic solution set. At most two solution sets can be
found when the arm has one of the following special
structures:
1. Three consecutive joint axes that are parallel.
2. Three consecutive joint axes intersect.
3. Two consecutive pairs of parallel or intersecting
joint axes.
4. Three consecutive joint axes such that two
intersect and two are parallel.
5. Three consecutive joint axes such that the first
two intersect and the last two intersect.
. .,
CHAPTER 6
SOLVING FIVEDOF MANIPULATORS
OneDimensional Iterative Technique
With five degrees of freedom, Eq. (2.14) takes the form
Al A2 A3 A4 A5 = P. (6.1)
and after multiplying both sides of this equation by A11
we obtain
A2 A3 A4 A5 = Q (6.2)
with
1
Q = A11 P. (6.3)
When 81 is known, matrix Q is fully determined and can be
viewed as a pose matrix for a 4DOF arm whose structure is
described by the left hand side of Eq. (6.2) which merely
expresses a 4DOF problem. In Chapter 5, we have seen that
a 4DOF problem can always be solved in closedform, hence
the remaining joint variables can be computed as shown
earlier.
Since we only need to know one of the joint variables
to solve for the whole solution set, the inverse kinematics
problem of fiveDOF manipulators reduces to finding the
value of the first joint variable only, and getting closed
form values for the remaining variables.
Let the column vectors of pose matrix Q of Eq. (6.3) be
given by m, c, u, and q, in order, so that
mx cx u
my Cy Uy
mz cz uz
0 0 0
m c u q
0 0 0 1
u = R1 R z = R1 t
q = R11 p 
q = R1 p
R11 11 = R1l(p 11).
From the left hand side of Eq. (6.2), two vectors
corresponding to vectors u and q, are given by
(6.6)
UL and qL'
uL = R2 R3 R4 z
qL = R2(R3 14 + 13) + 12.
A nonlinear function of e1 can be defined as a difference
between corresponding quantities from the left and the right
side of Eq. (6.2). For example, the difference between the
innerproducts (uL.qL) and (u.q) yields the function
f(e1) = uL.L u.q _.,
then
(6.4)
and
(6.5)
and
(6.7)
(6.8)
(6.9)
If the value of e1 used to compute pose matrix Q in Eq.
(6.3) does correspond to a solution set, then Eq. (6.2) will
hold, vectors uL and qL will be exactly equal to u and q,
respectively, and function f will equal zero. In other
words solution sets of Eq. (6.1) correspond to zeros of
function f defined in Eq. (6.9). Hence, the inverse
kinematics problem of 5DOF robot manipulators reduces to
solving the onedimensional equation
f(e1) = 0.
The zeros of f can be found by use of any suitable one
dimensional technique such as NewtonRaphson or the secant
method. Once 81 is known, the solution set can be completed
by solving Eq. (6.2) in closed form as we showed in Chapter
5. The solution set can then be checked for consistency
with Eq. (6.1) to determine whether the one found is
extraneous or not because the zeros of f are not always part
of a solution set of the manipulator.
Computing f(e)1. Using Eqs. (6.7) and (6.8), the inner
product uL.qL is given by
UL qL = (R2 R3 R4 z) (R2(R3 14 + 13) + 12).
If we apply properties (4.5) and (4.6) repeatedly, this.last
equation becomes
uL.qL = z.(R4114) + z.(R41R3113) + z.(R41R31R2112)
or after computing the zcomponents of the terms in
parentheses,
uL.qL = r4d4 + a3a4 S4 a3d304 C4 + r4d3
+ 04 S4 (a2 C3 + 72d2 S3)
04 C4 (a2r3 S3 + O2d2T3 C3 + r2d2a3)
+ 74 (a2a3 S3 o2d2a3 C3 + T2d272). (6.10)
This last equation shows that e3 and 84 must be known before
we can compute uL.qL. With e1 known, 83 and 84 can be
obtained by solving Eq. (6.2) as described in Chapter 5.
The coordinates of vectors u and q and the innerproducts
u.q, and q.q are necessary for the 4DOF inverse kinematic
method of Chapter 5. Equation (6.5) yields
tx C1 + ty S1
u = RI1 t = 71 tx S1 + TitY C1 + lt z (6.11)
altx S1 lty C1 + Tltz
and from Eq. (6.6),
Px C1 + Py S1 al
q = 'px S1 + TlPy C1 + alPz (6.12)
alPx S1 aly C1 + rpz
where we have assumed (R1 11) = [al, 0, 0]T since dl=0 by
proper positioning of frame Fo.
The innerproducts u.q and q.q can then be easily
computed by
u.q = uxqx + uyqy + Uzqz
and
q.q = qx2 + qy2 + qz2
when the numeric values of the components of u and q have
been obtained. These inner products can be obtained from
Eqs. (6.5) and (6.6) as well,
u.q = (RI1 t) (R1 (p 11)) = t (p 11),
u.q = tx (px al Cl) + ty (py al S) (6.13)
and
q.q = R1(p 1i) R1(p 11) = (P 11).(p 11),
q.q = p.p + 11.11 2(p.l1)
q.q = p.p + a12 2 (pxal C1 + Pyal S1). (6.14)
Equations (6.11)(6.14) clearly show that all
components of u and q, and the innerproducts u.q and q.q
are linear functions of S1 and C1, a result that will prove
useful in the next section.
To summarize, f(e1) can be computed for a given value
of 81 according to the following steps:
Step 1. From the current estimate of 81, Compute the
components of u and q and the inner products u.q and q.q as
shown in Eqs. (6.11)(6.14).
Step 2. Compute e2 and 83 from the reduced system of
equations
a2Uy S2 + a2ux C2 + a3o4 S4 o3"4d3 C4 = rI (6.15)
a2ux S2 a2Uy C2 + 0304 C4 = r2 (6.16)
o2qx S2 92qy C2 3a4 S4 = r3 (6.17)
a2qy S2 + a2qx C2 + 33a4d3 S4 + a3a4 C4 = r4 (6.18)
with
rl = q.u 74d4 d2uz 73T4d3 (6.19)
r2 = 34 2Uz (6.20)
r3 = 2(d2 qz) + d3 + T3d4 (6.21)
r4 = (q.q+a22+d22a32d32a42d42)/2
d2qz r3d3d4, (6.22)
derived from Eqs. (5.13)(5.20) by proper index substitution
(the indexes are incremented to fit the 4DOF problem of Eq.
(6.2)). Vectors u and q play the roles of vectors t and p
respectively. The last system of equations gives the values
of e2 and 84. Equations (5.23) and (5.24), with the proper
index changes,
(73d4T3a4S4) S3 + (a3+a4C4) C3
= C2qx + S2y a2 (6.23)
and
(a3+a4C4) S3 (a3d4T3a4S4) C3
= 72S2qx + r2C2qy + a2(qzd2), (6.24)
can be solved for 83. Another way to obtain 83 is by using
the equations
(73"4C4+a 34) S3 + 4S4 C3 = C2ux + S2uy (6.25)
and
04S4 S3 (73a4C4+a3T4) C3 =
72S2Ux + T2C2Uy + C2uz, (6.26)
derived from Eqs. (5.25) and (5.26) by incrementing the
indexes. With 81, 82, and 83 known, uL.qL can be computed
as in Eq. (6.10) and f(91) is then given by Eq. (6.8).
The ability to compute f(81) when 0e is given is
sufficient to implement a practical NewtonRaphson algorithm
for finding the zeros of function f. The algorithm can be
programmed according to the following steps:
Step 1. Obtain an initial estimate for 01. As for all
iterative methods, the closer the initial estimate of 8' is
to a true solution, the faster the convergence will be. If
the endeffector of the robot is tracking a trajectory given
as a finite set of endeffector poses, a good estimate for
finding the solution set for a pose along the trajectory is
the value of 81 corresponding to the preceding pose on the
trajectory.
Step 2. Compute 83 and 84 and then f(81) as described
earlier.
Step 3. Compute the derivative df/d01 of f with
respect to 81. A numeric approximation of this derivative
is given by
df/del = [f(81+681)f(E1)]/681, (6.27)
where 681 is a small increment of 81. Note that this
approximation requires another function evaluation at
(91+601).
Step 4. Obtain a new estimate for 01 by the one
dimensional NewtonRaphson method, i.e.
e1(new) = 81 f(01)/(df/d91). (6.28)
Steps 2 to 4 must be repeated until 01 is obtained to
the desired accuracy. The solution set can then be
completed by using the values of 02, 03 and 04 as computed
at the last iteration and by computing 85 uniquely from
C5
R5 x = S5 = R41 R31 R21 RI1 m.
0
and
5 = atan2(S5,C5).
The onedimensional method just described is flexible
in terms of the choice of function f to be used. A
different function can be implemented. The only
requirements are that f(81) be computable for any value of
81 and some known relationship between the zeros of f and
the solution sets of Eq. (6.1). For example, another choice
of f may be
f(E1) = qL.qL q.q
(6.29)
or any difference between corresponding quantities from the
left and right side of Eq. (6.2). The function choice is
important in terms of minimal computation complexity and
filtering of extraneous solutions which are discussed next.
In all practical experiments the function defined in Eq.
(6.8) has given good results.
Extraneous solutions. An extraneous solution set is
one that the iterative method converges to, i.e. it
satisfies the reduced system of equations (6.15)(6.18!) and
f(e1) = 0 but yet it is not a solution for Eq. (6.1). The
iterative method just described may converge to such a' set.
This problem was also reported by Tsai and Morgan (1984) who
developed a different inverse kinematic method that makes
use of a similar reduced system of equations.
In deriving the reduced system of equations (5.13)
(5.16) in chapter 5, vectors u and q,and the innerproducts
u.q and q.q are the only pose related quantities that were
involved. This means that a solution set obtained by
convergence of the method just described does not
necessarily satisfyother pose requirements from Eq. (6.1).
Extraneous solutions can be filtered out by a choice. of
function f that constrains more of the endeffector pose
elements at the expense of computation time or by checking
all solutions found for consistency with one or more end
effector pose elements.
Iterating on 95. An equivalent onedimensional
iterative technique can be implemented based on a function
of 85 instead of e1. Recall from Chapter 2 that the
homogeneous matrix A4 decomposes into
A4 = A4 B4
where B4 is a homogeneous matrix fully determined by
parameters a4, d4, and a4 and independent of 94. Right
multiplication of Eq. (6.1) by (A51B41) yields
A1 A2 A3 A4 = Q (6.30)
with
Q = P A51B4. (6.31)
When 85 is given, matrix Q becomes a known pose matrix for
the 4DOF problem expressed by equation (6.30). Vectors u
and q are given by
u = R Rg51G1 z (6.32)
and
q = R R51(G41k4) + p, (6.33)
where G4 is the rotation part of homogeneous matrix.B4,
B4 =
1 0
0 74
0 04
0 0
0
04
74
0
a4
, k4 = 0 is the position vector
d4
of B4, and R and p are the usual rotation matrix and
position vector of endeffector pose P. Explicitly, we get
nxa4 S5 + bx 4 C5 + tx74
u = ny 4 S5 + by 4 C5 + tyT4 (6.34)
nzx4 S5 + bza4 C5 + t T4
and
(nxa4d4 + bxa4) S5
(nxa4 + bxa4d4)
(nya4d4 + bya4) S5
(nya4 + bya4d4)
(nza4d4 + bza4) S5
(nza4 + bzo4d4)
C5 txT4d4 + Px
C5 tyT4d4 + Py
C5 tzT4d4 + Pz
Expressions of innerproducts u.q and q.q in terms of
S5 and C5 can be obtained from Eqs. (6.32) and (6.33),
u.q =[R R51G415 z] [R (R5 (G4 k4) + p]
and
(6.35)
q.q = [R (R51(G41k4) + p] [R (R51(G41k4) + p].
With the use of properties (4.5) and (4.6) as necessary and
rearranging terms, the equations yield
u.q = 04 [(n.p) S5 + (b.p) C5] + 74(t.p) d4 (6.36)
and
q.q = 2[a4d4 (n.p) a4 (b.p)] S5
2[a4 (n.p) + a4d4 (b.p)] C5
2r4d4 (t.p) + a42 + d42 + p.p, (6.37)
where we used the fact that
n.p nxPx + nypy + nzp
R1 p = b.p = bxPx + bypy + bzpz
t.P txpx + tyPy + tzpz
Here again, we note that uz, Pz, u.q, and q.q are linear
functions of S5 and C5.
With the components of u and q and the inner products
u.q and q.q computed, a onedimensional iterative method can
be implemented as described earlier with a function f(85)
given by
f(85) = uL.qL u.q (6.38)
which will converge to a value of e5.
5DOF Robots with ClosedForm Solution
Certain Fivedegreeoffreedom robots with simple
geometries may yield inverse kinematic equations that can be
solved directly and without need for a numeric technique
such as NewtonRaphson. In Chapter 5, some particular 4DOF
robot structures were found for which the reduced system of
equations (5.13)(5.16) was overspecified i.e. the matrix H
of the linear system was singular. The analysis of these
special geometries showed that one or two of the four
equations of the reduced system became constraint equations
on pose elements, particularly, elements tz, pz, t.p, and
p.p.
In the case of 5DOF robots, the quantities uz, qz,
u.q, and q.q (u playing the role of t and q that of p) are
either linear functions of S1 and C1 as shown in Eqs.
(6.11)(6.14) or linear functions of S5 and C5 as shown in
Eqs. (6.34)(6.37). Either way, the constraint equations
described in the ten cases of chapter 5 can be used to solve
for the correct value of e1 or 95 directly without need for
an iterative technique. This result means that if a 5DOF
robot manipulator has a 4DOF section with one of the
special geometries discussed in Chapter 5, then the arm can
be solved in closed form. We now proceed to prove this
point.
The 5DOF inverse kinematics problem of Eq. (6.1) can
be reduced to the 4DOF one of Eq. (6.2). In this case, the
reduced system of equations (6.15)(6.18) must be solved.
By substituting the expressions of uz, qz, u.q, and q.q from
Eqs. (6.11)(6.14) into Eqs. (6.19)(6.22) and rearranging,
we get
rI = (alty aid2tx) S1 + (altx + d2alty) C1
+ txPx typy rld2tz T3d3T4 74d4, (6.39)'
r2 = al_2tx S1 + a1 2ty C1 Tl2tz + T3T4, (6.40)
r3 = a2Px S1 + a1 2Py C1
72PZ + T2d2 + d3 + T3d4, (6.41)
and
4 = (alPy ald2Px) S1 + (alPx + d2alPy) C1
Tld2Pz + 73d3d4
+ (p.p +a12+a22+d22a32d32a42d42). (6.42)
These last four equations prove that the terms rl, r2, r3,
and r4 are all of the form
ri = ril S1 + ri2 C1 + ri3, i=l, . ., 4,
where the constants rij are fully determined by the arm
parameters and the endeffector pose elements.
Another choice is to use Eq. (6.30). The reduced
system of equations is given by Eqs. (5.13)(5.16) with all
elements of pose matrix P replaced by corresponding elements
of matrix Q of Eq. (6.31). The ri quantities become linear
expressions in S5 and C5 and have the form
ri = ril S5 ri2 C5 + ri3, i=l, . 4.
Indeed, if we replace uz, qg, u.q, and q.q by their
expressions in terms of S5 and C5, as given by Eqs. (6.34)
(6.38) and substitute in Eqs. (5.17)(5.20) (substitute for
tz, pz, t.p, and p.p and let d1=0 ), we obtain
rI = a4(n.p) S5 + a4(b.p) C5 + r4(t.p)
73d3 72d2r3 d4, (6.43)
r2 = rla4nz S5 Tl_4b C5 Tr4tz + r2T3, (6.44)
r3 = (la4d4nz Tla4b) S5 + (rla4nz + r714d4bz) C5
+ (T714d4tz 1TpZ) + d2 + T2d3, (6.45)
and
r4 = 2[a4d4 (n.p) a4 (b.p)] S5
2[a4 (n.p) + a4d4 (b.p)] C5 2r4d4 (t.p) 2d2d3
+ (p.p + a2 a22 d22 a32
d32 + a42 + d42)/2. (6.46)
In the analysis of special fourDOF geometries in
Chapter 5, we found cases where the reduced system of
equations included a constraint of the form ri = 0. Such a
constraint applied to one of Eqs. (6.39)(6.46) will usually
yield a value of 81 or 95 which in turn makes the 5DOF
inverse kinematics problem solvable in closed form.
Case 1: Three joint axes are parallel. When the
parallel axes are the first three (i.e. axes 1, 2 and 3),
Eq. (6.30) can be used. Case 1 of Chapter 5 shows that
r2=0 and r3=0. These two constraints and Eqs. (6.44) and
(6.45) yield a system of equations in S5 and C5,
r21 S5 + r22 C5 = r23
r31 S5 + r32 C5 = r33'
which can be solved for 05 when the determinant given by
(r21r32 r31r22) is not zero, otherwise there is no
solution. With 85 known, the remaining angles can be
obtained in closedform.
If the last three axes are parallel, a similar result
is obtained by exchanging the roles of base and endeffector
frames. When the intermediate axes are parallel, Eq. (6.2)
should be used. The constraints r2 = r3 = 0 then yield a
value of 01 and the inverse kinematic problem can be solved
in closed form as well.
Case 2: two consecutive sets of two parallel axes. If
axes 1 and 2 are parallel and axes 3 and 4 are parallel, Eq.
(6.30) and Chapter 5, case 2 yield r2 = 0 which can be used
to solve for 85 from Eq. (6.44). If axes 2 and 3 are
parallel and axes 4 and 5 are parallel, then using Eq. (6.2)
and Eq. (6.40) will yield a value of 01.
Case 3: Two parallel axes followed by two intersecting
axes. When this special geometry concerns the first four
joint axes of the 5DOF arm, using Eq. (6.30) and Chapter 5,
case 3 yields r3 = 0. This constraint applied to Eq. (6.45)
gives a value of 05. If the upper part of the 5DOF robot
has the special structure, Eq. (6.2) can be used and the
constraint r3 = 0 applies to Eq. (6.41). Angle 1, can be
directly computed.
Case 4: Two intersecting axes followed by two parallel
axes. This structure corresponds to Chapter 5, case 6.
Here, the constraint is rI = 0 and, as in the preceding
cases, 81 or 85 can be directly computed from Eq. (6.39) or
(6.43), respectively.
Case 5: Three intersecting axes. Pieper (1968) has
shown that a sixDOF manipulator with three intersecting
axes can always be solved in closed form. This result
applies to the simpler case of fiveDOF robots. This
structure corresponds to case 8 of Chapter 5 where the
constraints are r1=0 and r4=0. If the three intersecting
axes are the first three, Equation (6.30) should be used.
With Eqs. (15) and (6.46), a value of a5 can be obtained
directly. This same method can be used when the last three
axes intersect by first exchanging the roles of endeffector
and base frames. When the intermediate three axes are
intersecting, use of Eqs. (6.2), (6.39) and (6.42) will
yield a value of el.
Case 6: Two consecutive sets of two intersectingaxes.
This structure is analyzed in case 9 of Chapter 5. The
constraint equation is r4 = 0. Depending on where this
special structure is located along the fiveDOF arm, 61 or
85 can be directly computed by use of Eqs. (6.42) or (6.46)
respectively.
In the special geometries described in Chapter 5, cases
5, 7, and 10, we did not find a constraint of the form ri=0,
yet a fiveDOF arm having one of these particular geometries
can still be solved in closed form. We now study these
special cases as they apply to fivedegreeoffreedom
robots.
Case 7: Three joint axes are such that they either
intersect or they are parallel two at a time. This type of
structure is studied in cases 5, 7, and 10 of Chapter 5.
Assuming this geometry concerns axes 3, 4, and 5 of the
fiveDOF arm, Eq. (6.2) should be used. From Chapter 5,
case 5 and case 10, we see that the last two equations of
the reduced system, Eqs. (6.17) and (6.18) have the form
a2(qx S2 qy C2) = r3
a2(qy S2 + qx C2) = r4
where qx, qy, r3, and r4 are all linear expressions in S1
and C1. A quartic polynomial equation in ti = tan(91/2) is
readily obtained by squaring and adding the last two
equations,
q2 + qy2 = (r3/2)2 + (r4/a2)2
and substituting S1 = 2t/(l+t 12) and C1 = (1t12)/(l+t12).
This polynomial can be solved for 81 and the solution set
completed as described earlier. Similarly, from case 7 of
Chapter 5, we get the equations
a2(Uy S2 + ux C2) = r1
o2(ux S2 y C2) = r2
corresponding to Eqs.(6.15) and (6.16) of the reduced system
of equations. Here again a quartic polynomial equation in
ti is obtained by eliminating S2 and C2.
When the three axes with the special geometry are
located elsewhere along the fiveDOF structure, a similar
result can be obtained by using equation (6.30) or by
exchanging the roles of base and endeffector frames.
To summarize the above cases, we find that a 5DOF
robot manipulator will allow closedform solutions if any of
the following conditions is satisfied:
1. Three consecutive joint axes are parallel.
2. Three consecutive joint axes intersect.
3. There are two consecutive sets of two joint axes
that are either parallel or intersecting.
4. Three consecutive joint axes are such that two
intersect and two are parallel.
5. Three consecutive joint axes are such that the
first two intersect and the last two intersect.
Note that these conditions are not exclusive of one
another. For example, arms that satisfy condition 5 include
those thatsatisfy condition 2.
CHAPTER 7
SOLVING SIXDOF MANIPULATORS
Reduction to.a 4DOF Problem
At least six degrees of freedom are required for a
robot manipulator to be able to arbitrarily position and
orient its endeffector within its workspace. Equation
(2.14), with n equal to six, yields
A1 A2 A3 A4 A5 A6 = P. (7.1)
If both sides of this equality are multiplied by (A1 A2)1
the equation becomes
A3 A4 A5 A6 = Q (7.2)
with
Q = A21 A11 P. (7.3)
When 81 and 82 are known, matrix Q is fully determined and
can be viewed as a pose matrix for a 4DOF arm whose
structure is described by the left hand side of Eq. (7.2)
which merely expresses a 4DOF problem. In Chapter 5, we
have seen that a 4DOF problem can always be solved in
closedform, hence the remaining joint variables can be
computed from Eq. (7.2).
First we show that a similar result can be obtained if
85 and 86 are known or if 81 and 06 are known instead of the
first two joint variables.
In the development of the 4DOF inverse kinematics
solution, we have used the simplifying assumption that the
last frame had DHparameters d, a, and a all equal to zero.
Although this assumption is obviously correct in the case of
Eq. (7.2), we must show that it can be obtained in other
cases. As shown in Eq. (2.7), a homogeneous matrix Ai
decomposes into Ai = Ai Bi where Ai and Bi are given by Eqs.
(2.8) and (2.9) and Ai is a homogeneous matrix for which DH
parameters a, d, and a are zero. If the values of 81 and 86
are known, Equation (7.1) now reduces to the 4DOF problem
A2 A3 A4 A5 = Q (7.4)
where
Q = AI1 P A1 B51. (7.5)
Similarly, If 85 and 86 are known, the inverse kinematic
task becomes that of solving the 4DOF case
A1 A2 A3 A4 = Q, (7.6)
with
Q = P A61 A51 B41. (7.7)
In the following section, we will show how a two
dimensional iterative technique can be implemented to solve
the inverse kinematics problem of sixDOF robot
manipulators. Although this technique can equally be
developed using Eqs. (7.4) or (7.6), it will be based on Eq.
(7.2) for convenience.
TwoDimensional Iterative Technique
Since we only need to know 2 of the joint variables to
solve for the whole solution set, the inverse kinematics
problem of sixDOF manipulators can be reduced to finding
the values of the first two joint variables only, and
getting closedform values for the remaining variables. A
numerical technique aimed at finding the values of 81 and E2
can be implemented by defining a system of two nonlinear
equations in 81 and 82,
f(81'82) = 0 (7.8)
g(e1,e2) = 0, (7.9)
that can be solved using an iterative method such as a two
dimensional NewtonRaphson.
From the left hand side of Eq. (7.2), two vectors uL
and qL, corresponding to vectors u and q, (vectors u and q
relate to pose Q as shown in Eq. (6.4)), are given by
uL = R3 R4 R5 z (7.10)
and
(7.11):
qL = R3(R4 15 + 14) + 13.
We define two nonlinear functions of 91 and 82 as
differences between the innerproducts uL.qL, qL.qL and the
innerproducts u.q and q.q, respectively;
f(81,82) = uL.qL u.q, (7.12)
g(81,82) = qL'L q.q. (7.13)
If the values of 81 and 82 used to compute pose matrix Q in
Eq. (7.3) do correspond to a solution set, then Eq. (7.2)
will hold and vectors uL and qL will be exactly equal to u
and q forcing both functions f and g to be equal to zero.
In other words solution sets of Eq. (7.1) correspond to
zeros of the functions f and g defined in Eqs. (7.12) and
(7.13).
Computing f(81e2) and q(91,81.
In order to compute the values of f and g for a given
pair (81,82), the components of vectors u, q and the inner
products u.q and q.q are needed to solve the 4DOF equation
(7.2) which in turn allows computation of inner products
uL.qL and qL.qL and finally the values of f and g.
Vectors u and q, computed from Eq. (7.3), are
u = R21 R 1 R z = R1 R t (7.14)
2 1. 2 1
and
q = R21 [RI1 (p 11) 12]. (7.15)
If we consider the components of vector t as expressed with
respect to frame F1,
tx C1 tx + S1 t
t = it = R1 t t= + T1C1 t + a1 t
tz S1 tx olC1 ty + T1 tzJ
then vector u is given by
C2 tx + S2 ty
u = R21 t 72S2 itx + T2C2 It + a2 t (7.16)
a2S2 itx 2C2 ty + 2 itz
To obtain the components of vector q, first we rewrite Eq.
(7.15) as
q = R21(R1 p R 11) R21 12
and we define
S x C1 Px + S1 Py
ip lp = R11 p = _ISI Px + riCI py + 01 Pz
Pz 018 Px 01C1 Py + T1 Pz
vector q is then given by
C2 (px al) + S2 1y a2
q = 2S2 1 + r2C2 y + 02 1P o2d2 (7.17)
S2S2 1Px 2C2 1Py + 2 Pz T2d2
The inner product u.q can be derived from Eqs. (7.14) and
(7.15),
u.q = (R21 R11 t) R21R11 [(p 11) R1 12]
Using (4.4) and (4.5) as needed and expanding yields
u.q = t.(p 11) R1t 12,
or
u.q = t.p t.11 t.12
which gives
u.q = t.p altx C alty S1 a2 tx C2
2 ity S2 d2 Itz. (7.18)
Similarly, the square of the length of vector q, q.q, is
given by
q.q = R21R1 (P I1)R1 12] R21R1 [(p 1)R1 123
where we factored out R21R11 in the expression of q from
Eq. (7.15). Using (4.5) and (4.6) and expanding again leads
to
q.q = p.p + 11.11 + 12.12 2(p.11 + R 1p.12 + R1 11.12)
or
q.q = 2a2 [(al + 1px) C2 + 1py S2] 2d2 iPz
2al 1Px + p.p + a ++ a22 + d2. (7.19)
Equation (7.2) gives rise to a reduced system similar to
that of Eqs. ((5.13)(5.16) with the required shift in
indexes,
a3Uy S3 + a3ux C3 + a4o5 S5 a4o5d4 C5 = r1 (7.20)
U3ux S3 a3Uy C3 + u045 C5 =r2 (7.21)
3q9x S3 a3qy C3 a4a5 S5 = r3 (7.22)
a3qy S3 + a3qx C3 + C4a5d4 S5 + a4a5 C5 = r4 (7.23)
with
rl = q.u 75d5 d3uz 7475d4 (7.24)
2 = 45 3uz (7.25)
r3 = T3(d3 qz) + d4 + '4d5 (7.26)
4 = (q.q + a32 + d32 a42 d42 a52 d52)/2
d3qz 4d4d5. (7.27)
Solving this system of equations will yield the values of 83
and 85. The value of 04 can then be computed from the two
equations
(a4d5 4a5S5) S4 + (a4+a5C5) C4 =
C3qx + S3qy a3 (7.28)
and
(a4+a5C5) S4 (4d54a5S5) C4 =
T3S3qx + r3C3qy + a3(qzd3) (7.29) (7.29)
derived from Eqs. (5.23) and (5.24), or from the equations
(T45C5+4r75) S4 + g5S5 C4 = C3Ux + S3uy (7.30)
and
a5S5 S4 (r4a5C5+a45) C4 =
T3S3ux + 73C3Uy + 03uz, (7.31)
corresponding to Eqs. (5.25) and (5.26).
We can now compute the inner products uL.qL and qL.qL
By incrementing the indexes in Eq. (6.10), we derive
uL'L = T5d5 + a4o5 S5 a4d4a5 C5 + r5d4
+ 05 S5 (a3 C4 + c3d3 S4)
05 C5 (a3 4 S4 + a3d3 4 C4 + T3d374)
+ 75 (a3a4 S4 a3d304 C4 + T3d3 3). (7.32)
Vector qL, obtained from the left hand side of Eq. (7.2), is
qL = R3(R4 15 + 14) + 13 (7.33)
and the square of its length is given by
qL9qL = (R3(R4 15 + 14) + 13) (R3(R4 15 + 14) + 13)
or
qL.qL = (15+R4114+R41R3113) .(15+R4114+R41R3113),
after factoring out (R3 R4) and using inner product
invariance of rotations. Multiplying out the terms in
parentheses and using (4.5) and (4.6) where necessary, the
last equation yields
qL.L = 2 [a5 C5(a4 + a3 C4 + a3d3 S4)
+ a5 S5(a3r4S4 + a3d3 4 C4 + 73d3C4 + C4d4)
+ d5 (T4d4 +a3a4 S4 a3d304 C4 + d373T4)
r a3a4 C4 + C3d3a4 S4 + T3d3d4]
+ a32 + d32 + a42 + d42 + a52 +d52. (7.34)
Given a pair (81, e2), the corresponding values of f(el,e2)
and g(81,82) are obtained by the following steps:
Step 1. For initial values of 81 and 82, compute the
coordinates of vectors u and q as given by Eqs. (7.16) and
(7.17). The inner products u.q and q.q can be computed
using the regular inner product formula,
u.q = uxqx + Uyqy + Uzqz
and
q.q = q2 + q2 + q
Step 2. Solve the reduced system of Eqs. (7.20)(7.23)
for 03 and 85.
Step 3. Compute the value of 84 from Eqs. (7.28) and
(7.29) or Eqs. (7.30) and (7.31).
Step 4. Compute the inner products uL.qL and qL.qL,
given by Eqs. (7.32) and (7.34), respectively, and compute
the values of f and g as given by Eqs. (7.12) and (7.13).
TwoDimensional NewtonRaphson
The zeros of f and g can be iteratively computed and
checked for consistency with Eq. (7.1). If a computer
program for evaluating the two functions is available, the
partial derivatives of f and g with respect to 81 and 02,
denoted fl, f2 and gl, g2 respectively, can be numerically
approximated by
fl(e1,e2)= Bf/9l = [f(Ql+681, 2)f (l,Q2) ]/6el, (7.35)
f2 (,1'2)= Bf/aE2 = [f (E1,2+62)f (1,82) ]/602, (7.36)
and
91(e1,e2)= 1g/ae1 = [g(Q1+6e81,2)g(e1'2) ]/681, (7.37)
g2(,1'E2)= 2g/aE2 = [g(,1'E2+682)g(E1'92)]/692, (7.40)
where 681 and 682 are small increments of 81 and 82
respectively.
The twodimensional NewtonRaphson technique for
solving the inverse kinematics problem for a sixrevolute
DOF robot arm of arbitrary architecture proceeds according
to the following steps:
Step 1. Assume an initial estimate of 81 and E2 and
compute 83, 84, and 95
Step 2. .From the values of e1, 82, 83, 84, and E5
compute f(91,,2) and g(91,e2) as in Eqs. (7.12) and (7.13).
Step 3. Compute the partial derivatives of f and g
with respect to 91 and 02 by numeric approximations as shown
earlier.
Step 4. Obtain a new estimate for 1 and E2 by the
twodimensional NewtonRaphson method, i.e.
1
81 81 fl 2 f(112)
)2 82 9g1 2 g(91, 2)
new
Step 5. Repeat steps 2 to 5 until desired accuracy is
attained.
Step 6. Complete the solution set by uniquely
computing G6 from
C6 nx
R6 x = S6 = R51 R41 R31 R21 R1 n (7.41)
0 nz
Step 7. Check the solution set for consistency with
Eq. (7.1).
Choice of functions f and q. The functions f and g
defined by Eqs. (7.12) and (7.13) are computationally
economical since they do not require computation of the
forward kinematics or the inverse jacobian of the
manipulator. In fact, even the value of 86 is not required
to compute f and g since Eq. (7.41) is considered only after
convergence. Unfortunately, a pair (81, 82) for which both
f and g are zero is not guaranteed to correspond to a
solution set of Eq. (7.1). Extraneous solution sets can be
converged to as well. These are joint values that will
yield an endeffector pose that is not exactly the desired
one.
Other functions that fully constraint the endeffector
pose can be defined at the cost of greater computational
complexity. Wu and Paul (1982) have shown that the
difference between actual and desired endeffector poses can
be expressed as a 6 x 1 vector xe given by
xI nL (P PL)
2 bL (P PL)
x3 bL (P PL)
xe = (7.42)
X4 (tL.b t.bL)/2
x5 (nL.t n.tL)/2
x6 (bL.n b.nL)/2
where n, b, t, and p are the vectors that describe the
desired endeffector pose P as defined in (2.12) and vectors
nL, bL, tL, and PL are their corresponding vectors from the
left hand side of equation (7.1). Two functions can be
defined as
f(81,e2) = 12 + x2 + x32 (7.43)
22 (7.44)
g(1',e2) = 42 + x52 + x62. (7.44)
A pair (81,e2) that is a zero of both f and g is guaranteed
to correspond to a solution set of Eq. (7.1) so that the
iterative method described above will only converge to a*
true solution. However, now, the forward kinematics must be
computed at each iteration since the components of vectors
nL, bL, tL, and PL are all needed to evaluate functions f
and g as defined by Eqs. (7.42) and (7.43).
OneDimensional Method
The inverse kinematic problem for sixDOF manipulators
reduces to a fiveDOF one when the first or the last joint
variable is known. Equation (7.1) becomes
A2 A3 A4 A5 A6 = Q, (7.45)
with
Q = Al1 P (7.46)
when 81 is known, and
Al A2 A3 A4 A5 = Q, (7.47)
with
l 
Q = P A61B1 (7.48)
if 86 is known. In both cases, a fiveDOF problem is
obtained. When the resulting fiveDOF problem is solvable
in closed form, knowledge of 81 or 86 is then sufficient to
yield a complete solution set. The inverse kinematic
problem then reduces to finding one joint angle which can be
accomplished by a onedimensional iterative technique.
In chapter 6, we found that a sufficient condition for
closed form solutions of 5DOF manipulators is that they
have one of the special structures listed at the end of
Chapter 6. SixDOF arms that include a fiveDOF segment
with this type of geometry can then be solved using a one
dimensional iterative method. This iterative technique can
be implemented in much the same way as described in Chapter
6 for fivedegreeoffreedom arms. Assuming Eq. (7.45) is
to be solved, we define a function f of 9i by
f(91) = uL9L u.q (7.49)
where vectors uL, qL, u, and q are defined as earlier.
Given a value of 01, vectors u and q are computed from Eq.
(7.46), the values of the remaining joint variables are
computed in closed form from Eq. (7.45) as indicated in
Chapter 6 and Appendix B, the inner product uL.qL can then
be obtained as in Eq. (6.10) with the proper index
adjustments, and the value of f is then given by Eq. (7.49).
As we have seen before, the ability to compute the function
f for a given value of e1 allows the implementation of a
practical onedimensional NewtonRaphson algorithm.
Therefore, we can conclude that a sixdegreeoffreedom
manipulator with two consecutive pairs of intersecting or
parallel joint axes or three consecutive joint axes that are
parallel and/or intersecting two at a time can be solved by
use of a onedimensional iterative technique instead of the
twodimensional method required for an arm of arbitrary
architecture.
ClosedForm Solution
Some sixdegreeoffreedom manipulators with simple
geometries do not require any iterative method since they
can be solved in closedform. Pieper (1968) has shown that
a sufficient condition for closedform solutions is that
three consecutive axes be intersecting. The inverse
kinematics problem then reduces to finding the zeros of a
quartic polynomial. In the literature, It seems to be
common knowledge that three consecutive joint axes that are
parallel is another sufficient geometric condition for
closed form solutions.
The analysis of Chapter 5 and Appendix A showed that
under certain conditions, the reduced system of equations
(7.20)(7.23) included constraint equations of the form
ri = 0. (7.50)
The quantities ri, i=l, .. ,4, are functions of 61
and 82, as we have seen earlier. By looking for conditions
under which a joint variable value can be directly obtained
from an equation having the form of Eq. (7.50), we find two
more sufficient sixDOF robot structure conditions for
closedform solutions (excluding the already known
conditions of three parallel or three intersecting axes).
When the first two joint axes of a manipulator are
parallel so that al=0, then a1=0, Tl=I, and the zcomponents
of vectors u and q, given by Eqs. (7.16) and (7.17), become
uz = 2 (ty C12 + tx S12) + T2tz (7.51)
and
(7.52)
z = 2' (PX S12 Py C12) + 2 (PZd2):
This shows that r2 and r3, as given in (7.25) and (7.26),
become linear functions of S12 and C12.
When joint axes 3 and 4 are parallel and joint axes 5
and 6 are parallel, the reduced system oe equations (7.20)
(7.23) becomes similar to that of case 2 of Chapter 5,
a3Uy S3 + a3ux C3 = rI (7.53)
0 = r2 (7.54)
a4a5 S5 = r3 (7.55)
a3qy S3 + a3qx C3 + a4a5d4 S5 + a4a5 C5 = r4. (7.56)
Equation (7.54) yields two possible values for G12, each of
which will provide two possible values of 05 from Eq. (7.55)
when substituted in the expression of r3. The remaining
joint values can then be computed in closed form. A similar
development occurs when axes 3 and 4 are parallel and axes 5
and 6 intersect.
To summarize, a sixDOF manipulator has a closedform
solution if one of the following conditions is satisfied:
1. Three consecutive joint axes are parallel.
2. Three consecutive joint axes intersect at one
point.
3. The arm is formed of three sets of two parallel
axes. This structure is illustrated in Figure 7.1(a).
4. The arm has two sets of two parallel joint axes
followed or preceded by two intersecting axes. These
structures are illustrated in Figure 7.1(b) and 7.1(c).
a. 3 pairs of parallel joint axes.
b. 2 pairs of parallel joint axes followed by 2
intersecting joint axes.
c. 2 pairs of parallel joint axes preceded by 2
intersecting joint axes.
Figure 7.1. 6DOF manipulators with closedform
inverse kinematics.
CHAPTER 8
ORTHOGONAL MANIPULATORS
Definition: An naxes, serial kinematic chain of
revolute or prismatic joints is orthogonal if all twist
angles ai, i=l, . n, along the chain are 0 or 7/2. An
open orthogonal kinematic chain will be called an orthogonal
manipulator (Doty 1986).
SixDOF orthogonal manipulators can be classified in
terms of the values of their twist angles ai, i=l, ,5.
Twist angle a6 is always assumed to be zero in this text.
In fact, the value of a6 can be chosen arbitrarily since z6
is not a joint axis. Therefore, there are only 25 = 32
distinct classes of orthogonal manipulators, as listed in
Table 81, 8 of which have 4 or more adjacent parallel joint
axes which reduces their capability to less than six
degrees of freedom. As a result, there are only 24 types of.
sixjoint orthogonal manipulators with full spatial position
and orientation capability.
A convenient notation for this classification of
orthogonal manipulators is obtained by assigning a 5bit
binary number to each of these 24 types in which bit i is 0
if ai= 0 and bit i is 1 if ai=7r/2. For example, a
manipulator with twist angles"
a5=7/2, a4=7/2, a3=0, a2=0, and al=7r/2
belongs to the class 11001 of orthogonal manipulators.
Since most industrial robot arms are orthogonal, it is
worthwhile to consider the inverse kinematics problem with
respect to these manipulators. The Amatrices associated
with orthogonal arms have one of the two following forms
Ai(a=0)
Si
Ci
0
0
aiCi
aiSi
di
1
(8.1)
Ai(a=7/2) =
Si aiCi
Ci aiSi
0 di
0 1
Further computational simplification is obtained in the
inverse kinematic equations with orthogonal manipulators
since
Riz = Ri1 = z if ai= 0
and .
Riz = y if ai= 7/2.
Doty (1986) has shown that, of the 24 classes of
nontrivial orthogonal manipulators, those with 2 nonzero
(8.2)
twist angles (classes 01001, 01010, 01100, 10100 and 10
010) have closedform solutions. The inverse kinematic
analysis of Chapter 7 shows that the most complex sixDOF
robot structure can be solved by use of a twodimensional
iterative technique. Simpler structures only require a one
dimensional numerical technique and some even simpler
structures can be solved in closedform.
In Table 81, we provide a list of all thirtytwo
orthogonal manipulator classes in which we indicate the
degenerate geometries and, for the twenty four non
degenerate classes, we indicate a suitable inverse kinematic
method necessary for solving the most complex arm structure
within that class. It must be understood that intersecting
axes cannot be considered according to a classification
based on the values of the twist angles alone. The choice
of inverse kinematic method indicated in Table 81 is based
solely on the presence of parallel axes within a given
class. Simpler inverse kinematic methods can be used if any
of the special structures discussed in chapters 5, 6, and 7
are present.
In Chapter 9, the inverse kinematics of four orthogonal
manipulators are described *in more detail.
Table 81. Inverse kinematics of orthogonal manipulators
Class Method Justification
1 00000. D All six axes are parallel
2 00001 D Five consecutive parallel axes
3 00010 D Four consecutive parallel axes
4 00011 D Four consecutive parallel axes
5 00100 CF Three consecutive parallel axes
6 00101 CF Three consecutive parallel axes
7 00110 CF Three consecutive parallel axes
8 00111 CF Three consecutive parallel axes
9 01000 D Four consecutive parallel axes
10 01001 CF Three consecutive parallel axes
11 01010 CF Three pairs of parallel axes
12 01011 1D Two pairs of parallel axes
13 01100 CF Three consecutive parallel axes
14 01101 2D
15 01110 2D
16 01111 2D
Table 81.
Class
Continued
Method
Justification
Five consecutive parallel axes
Four consecutive parallel axes
Three consecutive parallel axes
Three consecutive parallel axes
Three consecutive parallel axes
Two pairs of parallel axes
Four consecutive parallel axes
Three consecutive parallel axes
Two pairs of parallel axes
Three consecutive parallel axes
Notation: D
CF
1D
2D
= Degenerate geometry
= ClosedForm
= One Dimensional iterative method
= TwoDimensional iterative method
10000
10001
10010
10011
10100
10101
10110
10111
11000
11001
11010
11011
11100
11101
11110
11111
D
D
CF
CF
CF
1D
2D
2D
D
CF
1D
2D
CF
2D
2D
2D
CHAPTER 9
APPLICATION EXAMPLES
Example 1: The PUMA 560
A popular orthogonal manipulator geometry, the PUMA
560, is described by the kinematic parameters given in Table
91 and illustrated in Figure 9.1. This manipulator has a
spherical wrist and therefore allows closedform solutions
(Pieper 1968). Inverse kinematic solutions have been
proposed by numerous authors for this type of arm (Lee and
Ziegler 1984; Craig 1986; Paul and Zhang 1986).
Table 91. PUMA 560 kinematic parameters
Joint d G a a (rd)
81
E2
e23
83 
94
04
85
E6
7/2
0
0/2
7/2
0
This example is
utility of the approach
included here to demonstrate the
already outlined and to contrast it
