An Inverse Kinematic Solution for

Kinematically Redundant Robot

Manipulators
Se-Young Oh
Department of Electrical Engineering and Computer Science, University of
Illinois at Chicago, Chicago, Illinois 60680

David Orin
Department of Electrical Engineering, The Ohio State University, Columbus,
Ohio 43210

Michael Each
Department of Electrical Engineering and Computer Science, University of
Illinois at Chicago, Chicago, Illinois 60680
Received May 25, 1984; accepted July 3, 1984

An inverse kinematic analysis addresses the problem of computing the sequence of joint motion
from the Cartesian motion of an interested member, most often the end effector. Although the
rates and accelerations are related linearly through the Jacobian, the positions go through a
highly nonlinear transformation from one space to another. Hence, the closed-form solution has
been obtained only for rather simple manipulator configurations where joints intersect or where
consecutive axes are parallel or perpendicular. For the case of redundant manipulators, the
number of joint variables generally exceeds that of the constraints, so that in this case the
problem is further complicated due to an infinite number of solutions. Previous approaches have
been directed to minimize a criterion function, taking into account additional constraints, which
often implies a time-consuming optimization process. In this article, a different approach is
taken to these problems. A Newton-Raphson numerical procedure has been developed based
on a composite Jacobian which now includes rows for all members under constraint. This
procedure may be applied to solve the inverse kinematic problem for a manipulator of any
mechanical configuration without having to derive beforehand a closed-form solution. The
technique is applicable to redundant manipulators since additional constraints on other members
as well as on the end effector may be imposed. Finally, this approach has been applied to a
seven degree-of-freedom manipulator, and its ability to avoid obstacles is demonstrated.
~ ~ P ~ ’ i ~ y 3 a ~ ~ ~ M . ~ ~ ~ ~
83 Q iI t: P di Q 0 IlilcDIIB, mais t:+%isSBO,t n5 t:@ma M . f 3 ti 7 2691
Q f i L T @ Q W A B t L T 4 k X Q # . QRIAIPktb,4Bl#B~Qbfi#!2SK.kSQljfiKB
l - 9 - Kni! 3
4Qb\. Q 3 7 ,% + P 7 ’ i .IY 3;3@MfB’)IRlb,l t w t P 3 l l w & 7 = ~ L/
7S8354aQ.

Journal of Robotic Systems, 1(3), 235-249 (1984

8 1984 by John Wiley & Sons, Inc. C!CC 0471-2223/84/0302~-15$04.00
236 Journal of Robotic Systems-1 984

1. INTRODUCTION
In order to simulate and control industrial robots, it is often necessary to specify
the commanded trajectory in Cartesian workspace coordinates. Computer vision sys-
tems, for example, give positions in workspace coordinates, and it is also usually
simpler from a robot programming language standpoint to specify manipulator positions
in a workspace frame. On the other hand, overall movement is usually achieved by
simultaneously controlling each joint servo, which uses the joint angle as its set point.
This calls for the ability to transform trajectory specifications between Cartesian space
and joint space.
Usually, this transformation involves complicated trigonometries and generally does
not yield a simple solution. For any specific manipulator with six degrees of freedom
or fewer and with some consecutive axes intersecting or parallel, it is often possible
to obtain a closed-form analytical solution, and this is quite appropriate for real-time
control.'-" However, considerable time may be spent to derive the equations, and,
furthermore, there exist a number of mechanical configurations which render such an
approach inapplicable. Piepe? has taken a different, numerical approach to compute
joint positions given the position and orientation constraints of a general six degree-
of-freedom manipulator, and the present work is based on his approach.
In all these problems, the number of end-effector constraints equals the number of
joint variables and there exists a finite set of solutions. While a six degree-of-freedom
manipulator is capable of reaching any position and orientation in its workspace, a
human arm possesses additional degrees of freedom resulting in greater dexterity and
flexibility. This is important since there are times when an obstacle may block the
only route of a six degree-of-freedom manipulator. An arrn exhibiting this type of
additional flexibility is called redundant since there are more degrees of freedom than
is necessary to arbitrarily orient and position the end effector. The underspecified
nature of the resulting problem leads to an infinite number of solutions and some
method must be developed to choose among these.
Liegeois6 has formulated a mathematical programming approach to provide a so-
lution for redundant manipulators by attempting to minimize the deviations from the
mean joint positions under joint limit constraints. A similar procedure was applied by
Benati, Morasso, and Tagliasco' to a three-link, anthropomorphic arm possessing up
to nine degrees of freedom. This approach does not necessarily solve for more general
Oh, Orin, and Each: Robot Manipulators 237

configurations, however. In this case, the distance travelled in the joint configuration
space is minimized. In Klein’s work,s the joint rates were controlled for given Cartesian
rates by applying the generalized inverse method for the minimum norm solution of
joint velocities. This also means that the instantaneous power is minimized and sin-
gularities are avoided. However, the inverse solution for position is not implied. An
added criterion function approximating a minimax solution for joint angle distribution
is combined with the minimum norm solution to improve performance.
In this article, all the aspects of an inverse kinematic analysis are considered, namely,
determining not only joint positions but rates and accelerations as well. First, a more
efficient formulation of Pieper’s numerical approach is introduced. The procedure
makes explicit use of the Jacobian matrix and also uses a simple scheme to compute
the error between the desired and actual positiodorientation of the end effector. Second,
the method has been extended to include the case of a redundant manipulator. The
extra degrees of freedom are used to place additional constraints on the motion of
other members of the mechanism such as the elbow. For instance, the elbow height
as well as the end-effector positiodorientation may be constrained. This may be useful
for a task where the manipulator reaches over a “wall” in order to perform the desired
operation. The Jacobian now includes rows for all members constrained; thus it is
named the composite Jacobian.
Furthermore, since the solution provided in this article is general, this approach
could provide the basis for a simulation package useful in designing manipulators for
a hypothetical manufacturing workcell or in evaluating different control schemes.
Deriving and storing multiple closed-form equations, one for each configuration, may
not be as useful as a general method of simulation which works for any conceived
configuration.
Normally, only the position, and possibly the velocity, of the end effector is con-
strained. The method presented in this article allows constraints on position, velocity,
or acceleration of any combination of links. The only requirement is that the number
of constraints equal the number of degrees of freedom.
Besides joint positions, resolved rate control9 schemes require computation of the
desired joint rates. Furthermore, dynamic control technique^'^'' often require the
desired joint accelerations. Such additional use of rate servos or torque feedforward
terms in the control may enhance the system stability and reduce the steady-state error
for a given trajectory.
A numerical procedure is used in this work to compute the joint rates and accel-
erations for redundant manipulators. As in the position case, the constraints may be
split among different members of the system. The method is purely numerical and
based on successively applying direct kinematics to obtain the required coefficient
matrix.

II. DIRECT KINEMATICS

A modeling procedure proposed by Denavit and HartenbergI4was used to represent
a general manipulator configuration. A manipulator is comprised *Jf the base, N links
coming out of this base, and an end effector rigidly attached to the last (Nth) link.
The links are consecutively numbered from 0 to N +
1. The base is link 0 and the
238 Journd of Robotic Systems-1 984

end effector is link N + 1. The position of the ith joint is given with respect to the
zcl joint axis. Thus, joint i connects link i - 1 with link i.
A coordinate system [xi, yi, iti] is rigidly attached to each link. Furthermore, each
link is characterized by four parameters which describe the relationship between suc-
cessive joint and link pairs. These are the joint angle (Oi), the offset distance (di),the
link length (ai),and the twist (ai)(see Fig. 1). These parameters that describe the
relationship between successive joint frames define a 4 x 4 homogeneous transfor-
mation Ai.I This produces the position components of a point in frame i - 1 given
the components in frame i. For a rotational joint the A matrix is'

- sO,cai sOisai
- ceisai
Aj =
cai
0 0

where ce and se are used for cose and sine, respectively. The results for a sliding
joint are also given in ref. 1.
The relationship between other links may be described by multiplication of inter-
mediate A matrices to produce the T matrices:

The matrix 'Tigives the transform relationship between links i and j. For T matrices
without a leading superscript, it is assumed that the base frame (0) is being referenced.
The topleft 3 X 3 part of a &'Timatrix gives orientation information and will be
denoted by & I l l i . The first three elements of the right-hand column give the relative
position information and is denoted by "'p;. The position vector h m the origin of
the end-effector coordinate system to that of link j is denoted by rj. Similarly, the

j o i n t i+l
I,- ri. 2
r -i
joint i I -?Y

Figure 1. Link parameters associated with link i.

Oh,Orin, and Bach: Robot Manipulators 239

Figure 2. Depiction of vectors p,, rj, pi and transform &IUi for a manipulator.

position vector from the base to linkj is denoted by pi. These are all illustrated in
Figure 2.
The motion of a manipulator is completely specified by three vectors q, q, q,
representing the relative positions, rates, and accelerations at the joints, respectively:

Note that a qi term is either Oi for a rotational joint or di for a sliding joint. The
Cartesian movement of the links of the manipulator is specified by the appropriate
homogeneous transformation as well as by the linear and angular link rate and accel-
eration vectors. The latter are denoted by p, o,p, b, respectively. The expressions
relating link rates and accelerations to correspondingjoint variables may be found in
~ 1 .

111. JOINT POSITION DETERMINATION FROM CONSTRAINTS IN

CARTESIAN COORDINATES
Let X2 be a homogeneous transformation giving the constrained position and on-
entation of the end effector in the base frame. It may be represented ad6
240 Joumd of Robotic Systems-1 984

It is assumed that X2 is given as well as the N - 6 additional constraints specifying

position components of links other than the end effector. These are the constraints due
to the redundancy and will be denoted as c2 where

Furthermore, the position and orientation of the end effector as well as the position
of all other links may be determined using the present estimate of the joint angles
along with direct kinematics. This is obtained recursively starting at the base by
expressing the ith link frame with respect to the (i - 1)st link frame [see Eq. (2)].
When the last link of the end-effector frame has been expressed in terms of the base
frame, one has the XIhomogeneous transformation matrix. This gives the estimated
position and orientation of the end effector in Cartesian space and will be denoted by

Similarly, the present estimate of the (N - 6)-element constraint vector is given by

The differences between the constrained (X2,c2) and estimated (X1, cl) position of
the manipulator may be used to determine a better estimate of the joint position vector
q. This leads to the use of the Newton-Raphson method to iteratively find q, and this
approach is outlined in the following paragraphs.
First of all, the relationship between joint velocities and end-effector or constrained
link velocity components may be written as
Oh, Orin, and Each: Robot Manipulators 241

(8)

The vectors oEand pE are the angular and translational velocities of the end effector,
respectively. The components of the vector c are just the N - 6 rates of the constrained
link components. The matrix J is the N x N composite Jacobian given by

J = [;:I (9)

The 6 X N Jacobian matrix JE relates end-effector velocities to joint velocities while

Jc is the (N - 6) x N Jacobian relating rates of constrained link components to joint
rates. A method for computing the composite Jacobian is given in the Appendix.
The expression given in Eq. (8) may be used to relate the small deviations in positions
Ax by multiplying both sides by At:

By computing Ax (from the differences between constrained and estimated positions)

and premultiplying by the inverse Jacobian matrix, Aq is obtained:

The vector Aq is a linear approximation and may be added to the present estimated
q vector to obtain an updated value. At this point, direct kinematics is used to find a
new estimate of the Cartesian position and orientation of the end-effector (Xi) and
position values for the link constraints (cl). These estimates will be subtracted from
the constrained values (Xz,c2) to obtain a new value for A x and the process is repeated
until Ax is considered to be small enough. A method for computing A x follows.
First, the error matrix A between constrained and estimated positiodorientation of
the end effector, with the error expressed in the base frame, is defined through the
following equations:

or

where I is the identity matrix. Furthermore, assuming a differential position and

orientation error, A has been shown to have the following form:'
242 Journal of Robotic Systems-1 984

The six elements of differentialrotation (6) and translation ( d ) error in the A matrix
may be computed by equating Eqs. (13) and (14). The results, arranged in vector
form, are given as follows:

The second subscript indicates either a constrained (2) or an estimated (1) component.
The difference between constrained and estimated link position components is simply
given as follows:

Thus, Ax for a redundant manipulator may be formulated as

Ax = [i].
Assuming that Ax is too large to ensure convergence, the next step involves limiting
the step size. This is accomplished as follows. Compute a total distance )dl and a total
angle (6)for the end effector and a total distance )c(for the link constraints:

If (dl > d,,, then scale down the Ax values:

Ax + Ax d,,J(d(.
Oh, Orin, and Bach: Robot Manipulators 243

In a similar manner, if 161 > ti,, then

Finally, if (cI > c,,,, then

Once A x has been computed and appropriately scaled, then the inverse of the
Jacobian is used to transform these differential values from Cartesian space to joint
space. There are various approaches to computing the Jacobian.” The error vector A x
was computed in the base frame so the Jacobian is also determined with respect to
the base frame, according to Olson and Ribble’s method.” See the Appendix for
details of the composite Jacobian calculation.
Once the Jacobian has been computed, A q is determined through Eq.(1 I). However,
it may be necessary to limit A q for stability of convergence. The largest element of
Aq, Aq,, is determined. If Aqk > Aq,.,, then

Once A q has been appropriately scaled, a new value for q is computed:

Direct kinematics is then used with the new value of q to compute the estimated
position and the process is repeated until [Ax[ is small. It is important to limit the
number of iterations to avoid excessive computation in case the method is slow or
unable to converge. A flow chart of the process is given in Figure 3.

IV. JOINT RATBACCELERATION DETERMINATION FROM

CONSTRAINTS IN CARTESIAN COORDINATES
A position servo controller needs the desired joint position as input. However, there
are many cases in which a hybrid position-rate controller may be used to achieve a
better response. For this, both the desired joint position and the desired joint rate must
be supplied as inputs to the servo. The joint rate vector q is related to the Cartesian
rate constraint vector x (which is given) as follows:

The joint acceleration vector q may also be computed if needed. Given q and q,
the joint Cartesian accelerations are related through
244 Journal of Robotic Systems-1 984

(2 Start

w
4 - l
Initialize 9

Scale Az:
Compute Estimated
P o s i t i o n and O r i e n t a t i o n
through
D i r e c t Kinematice :
X1 and c
- -1

from C o n s t r a i n t s :
A& = 1 a , a. 5 1
r-5
Compute Jacobian

Compute A qk

-73 Return I 4 + 4 + 4n

Figure 3. Flow chart for the solution of the inverse kinematic problem (position only).

a.
A numerical procedure may be applied to obtain First of all, according to Eq. (27),
if q is set equal to zero, then direct kinematics (for acceleration) may be used to
compute x which in this case is equal to j6. Then the actual joint accelerations are
given through the following equation:

In evaluating Eqs. (26) and (28), the inverse of the composite Jacobian is needed.
A method for obtaining the Jacobian is given in the Appendix. However, the method
for obtaining %, suggests a simple, alternate method for computing J . Let q = ek,
Oh, Orin, and Bach: Robot Manipulators 245

where ek is an N X 1 vector with the kth element equal to 1 and 0 elsewhere, and Jk
is the kth column of J. Then the following equation may be used to compute the kth
column of the Jacobian:

Jk = ifor q = ek.

Note that in computing Jk through direct kinematics (for velocity), the analysis is
simplified due to the presence of many zero values for the joint velocities (compare
with Olso~dRibble'~).

V. SIMULATION EXAMPLE
In an industrial environment, it is frequently necessary to perform manipulation
tasks while avoiding obstacles in the work area. One example might be painting the
interior of an automobile through a door window opening. In this case, the elbow
position may have to be coordinated with the end-effector position in order to avoid
contacting the door. This may provide an example where it is required to constrain
the position component of a certain link in addition to the position and the orientation
of the end effector.
In this article, a seven degree-of-freedom manipulator, illustrated in Figure 4, was
used to demonstrate the use of a redundant degree of freedom to avoid an obstacle.
This manipulator has three degrees of freedom at the shoulder, one at the elbow, and
three at the wrist. Here, the seventh degree of freedom provides for shoulder twist.
These seven degrees of freedom allow the gripper to be positioned and oriented
arbitrarily and also provide a redundant degree of freedom to vertically position the
elbow in order to avoid contact with obstacles in its path.
Figure 4 shows a particular task to which the manipulator has been assigned. The
manipulator is supposed to lift an object off a conveyor through a slit in a vertical
wall without contactingthe wall. The shoulder twist was provided to give the additional
flexibility in order to avoid contact with the wall. The Newton-Raphson numerical
method as discussed in this article was used to compute the sequence of joint angles
to drive the manipulator through a desired trajectory. The initial joint angle estimates
were taken from the previous point solution. (The manipulator was assumed to be
initially positioned at point 1 so that the inverse kinematic analysis was not necessary
for this point.) The differential rotation and translation for each iteration step was
limited to 0.25 rad and 4 in., respectively. The joint angle change for each iteration
step was limited to a maximum value of 0.85 rad for convergence stability. Two to
three iterations were required to converge to the next point which is a few inches
away. The solution was assumed to have converged when the error was within 0.01
rad for the angle and 0.002 in. for the distance.

VI. CONCLUSION
An iterative numerical method has been developed to solve the inverse kinematic
problem of redundant, or nonredundant, robot manipulators. This involves not only
determining the joint positions but also joint rates and accelerations. Providing set
246 Journal of Robotic Systems-1984

Figure 4. An example task for a seven degree-of-Mommanipulator.

points for joint rates and accelerations may be necessary for resolved rate and accel-
eration control schemes. The inverse algorithm for determining rates and accelerations
is based on successively applying direct kinematics to obtain coefficient matrices and
vectors.
In the case of redundant manipulators, the extra degrees of freedom are specified
by constraining position components of links in addition to those of the end effector.
An example seven degree-of-freedom manipulator given in this article demonstrates
the use of a single additional constraint to avoid an obstacle.
There are outstanding questions that remain to be addressed in future work. These
include further considerations of possible limitations due to computation time, a sys-
tematic approach to choosing constraints for redundant systems, and problems with
Oh, Orin, and Bach: Robot Manipulators 247

singularities of the composite Jacobian. Although problems with the singularity may
be removed by redefining points along the path or by using the Jacobian for the previous
point, this still needs further investigation. While there is considerable work yet to be
addressed, this article presents a useful approach to control of the future generation
of redundant manipulators which possess far more dexterity and flexibility.

APPENDIX
Jacobian Formulation
As explained in Section III, J is partitioned as follows:

J = [:I,
where JE is a 6 X N Jacobian matrix for the end effector and Jc is (N - 6) X N
Jacobian associated with link constraints. The components for JE must be expressed
in the base coordinate system (0) since the end-effector constraints are in this frame.
The method of Olson and Ribble given in ref. 17 is chosen for this reason.
The form of JE is given as follows:

r ; r z ... r""
V s ' % ... rKr
rf Y2z
... r"
p.f P; ... I%
P'; w ...
pz1 Pi ... fi,

The components of JE may be computed through use of the following eq~ations:'~

o U i = o U i - I i - l U i , i =1 , 2 ,..., N + 1. (A41

, i = 1 , 2 ,..., N . (A51
248 Journal of Robotic Systems-1 984

Note that I is the 3 X 3 identity matrix, and ‘pf gives the components of the vector
pi’ in the ith coordinate system.
The Jc matrix has the following form:

It consists of a single row for each constrained component ( k ) of link 01 position. The
value for k may be 1, 2, or 3 for a position constraint along the q,,yo, or ~0 axis,
respectively. The value for j may be from 1 to N. The elements of each row of Jc are
then computed through use of the following equations:

ti = p.I - p.I ’ i = 0, 1, ... ,j - 1. (A1 1)

ti=O,i=j,j+ 1,..., N - 1. (A13

P: = (yi X - ti - I)k, i = 1, 2, ... , N . (A131

References
1. R. P. Paul, Robot Manipulators: Mathematics, Programming and Control, MIT Press,
Cambridge, MA, 1981.
2. C. S. Lee, “Robot Arm Kinematics, Dynamics and Control,” IEEE Comput., 15(12),
62-80 (December 1982).
3. R. Featherstone, “Position and Velocity Transformations Between Robot End-Effector
Coordinates and Joint Angles,” Int. J. Robot. Res., 2(2), 35-45 (Summer 1983).
4. J. Duffy, Analysis of Mechanisms and Robot Manipulators, Edward Arnold Ltd., London,
1980.
5 . D. L. Pieper, “Kinematics of Manipulators under Computer Control,” Ph.D. thesis, Stan-
ford University, 1968.
6. A. Liegeois, “Automatic Supervisory Control of the Configuration and Behaviour of
Multibody Mechanisms,”IEEE. Trans. Syst. Man Cybernet.. SMC-7(12), 86-71 (1972).
7. M. Benati, P. Morasso, and V. Tagliasco, “The Inverse Kinematic Problem for Anthro-
pomorphic Manipulator Arms,” ASME J. Dynam. Syst. Measure. Control, 104, 110-1 13
(1982).
8. C. A. Klein and C. H. Huang, “Review of Pseudoinverse Control for Use with Kine-
matically Redundant Manipulat&,” IEEE Trans.Syst. Man Cybernet., SMC-13(2),245-250
(1983).
9. D. E. ’Whitney,“Resolved Motion Rate Control of Manipulators and Human Prostheses,”
IEEE. Trans. Man-Machine Syst., MMS-10, 47-53 (1969).
10. R. P. Paul, “Modeling, Trajectory Calculation and Servoing of a Computer Controlled
Arm,” Stanford Artificial Intelligence Lab., AI Memo 177, 1972.
11. A. K. Bejczy, “Robot Arm Dynamics and Control,” Jet Propulsion Lab., Tech. Memo
33-669, 1974.
12. J. Y. S. Luh, M. W. Walker, and R. P. Paul, “On-Line Computational Scheme for
Mechanical Manipulators,”ASME J. Dynam. Syst. Measure. Control, 102,69-76 (1980).
Oh, Orin, and Each: Robot Manipulators 249

13. D. E. Orin, R. B. McGhee, M. Vukobratovic, and G. Hartoch, “Kinematic and Kinetic

Analysis of Open-Chain Linkages Utilizing Newton-Euler Methods,” Math. Biosci., 43,
107-130 (1979).
14. J. Denavit and R. B. Hartenberg, “A Kinematic Notation for Lower-Pair Mechanisms
Based on Matrices,” ASME J . Appl. Mech., 23, 215-221 (1955).
15. M. W. Walker and D. E. Orin, “Efficient Dynamic Computer Simulation of Robotic
Mechanisms,” ASME J. Dynam. Syst. Measure. Control, 104, 205-21 1 (1982).
16. C. H. Wu and R. P. Paul, “Resolved Motion Force Control of Robot Manipulators,”
IEEE. Trans. Sysr. Man Cybernet., SMC-12(3), 2 6 2 7 5 (1982).
17. D. E. Orin and W. W. Schrader, “Efficient Jacobian Determination for Robot Manipu-
lators,” Proc. Sixth IFTOMM Congress, New Delhi, India, 1983.