Академический Документы
Профессиональный Документы
Культура Документы
i =1
A
i
(q
i
) = G,
where
A
i
(q
i
), G SE(3)
A
i
(q
i
) =
_
R(q
i
) p
0 1
_
.
Since G denes six constraints, the problem is well posed only if the number of inde-
pendent joint variables is equal to 6. If dim(q) < 6, the problem is overconstrained and in
1
Strictly speaking, the knee is not revolute: it has a small but nontrivial sliding component. The approximation
as a revolute joint will sufce for graphical purposes.
356 TOLANI, GOSWAMI, AND BADLER
TABLE 1
Number of Analytic Solutions for
6-Degree-of-Freedom Systems
n Upper bound on solutions
<6 0
>6
6R, 5RP 16
4R2P, 6R with S joint 8
3R3P 2
general no solution is possible. Conversely, if dim(q) > 6, the problem is said to be under-
constrained or to possess redundant degrees of freedom. In this case, an innite number of
solutions may exist. In a redundant system, for a xed posture of the end effector the joints
of the robot are still free to move along a constraint surface in joint space. This surface is
called the self-motion manifold. Redundant systems are useful because the extra degrees
of freedom can be used for optimizing a cost function, avoiding collision, or keeping clear
from joint limits or Jacobian singularities.
2.1.1. Theoretical Results
For 6-degree-of-freedom systems, precise upper bounds on the number of solutions have
been established. Let R denote a revolute (rotating) joint and P denote a prismatic (trans-
lating) joint. For a general 6R or 5RP manipulator, there are at most 16 possible solutions.
For a 4R2P or a 6R manipulator with a spherical joint, the number of possible solutions
drops to 8. Finally, a 3R3P system can have at most 2 solutions. See Table 1 for a summary
of available results.
2.1.2. Taxonomy of Inverse Kinematics Algorithms
Broadly speaking, inverse kinematics algorithms can be characterized as analytical or
numerical. Analytical methods are said to be complete since they nd all possible solutions.
Analytical methods can be further subdivided into closed-form and algebraic-elimination-
based methods. In a closed-form method, the solution to the joint variables can be directly
expressed as a set of closed-form equations. In general, closed-form solutions can only be
obtained for 6-degree-of-freedom systems with special kinematic structure. Methods based
on algebraic elimination express the joint variables as solutions to a systemof multivariable
polynomial equations, or alternatively express a single joint variable as the solution to a
very-high-degree polynomial and determine the other joint variables using closed-form
computations. Since the degree of these polynomials will be greater than 4, algebraic-
elimination-based methods still require the use of numerical subroutines. However, since
numerical methods exist for solving all the roots of a polynomial equation, the algebraic
elimination methods are still classied as analytical in nature.
In contrast to the analytical methods, numerical approaches iteratively converge to a sin-
gle solution based on an initial guess. In general, analytical methods are preferable to their
numerical counterparts because analytical methods yield all solutions and are computation-
ally faster and more reliable. The primary advantage of numerical algorithms is they can
be used in cases where the system is ill-posed. There are three popular numerical methods
REAL-TIME INVERSE KINEMATICS 357
used in solving inverse kinematics problems. The simplest method is the straightforward
application of the NewtonRaphson algorithm for solving systems of nonlinear equations.
Alternatively, the inverse kinematics problem can be converted into a differential equation
in terms of q and q. Finally, the third category of numerical algorithms is based on recasting
the inverse kinematics problem into a nonlinear optimization problem.
2.2. Numerical Algorithms
In the following we list the frequently used numerical inverse kinematics algorithms:
1. NewtonRaphson methods: The solution to an inverse kinematics problemis the roots
to the system of nonlinear equations
F(q) f(q) g = 0.
Here f is the forward kinematics map, g is the desired position and orientation of the end
effector, and q is the joint angle vector. Because NewtonRaphson methods use a rst-order
approximation to the original equations, convergence can be slow when the equations are
highly nonlinear. Moreover, near the vicinity of a singularity the inverse of the Jacobian is
ill-conditioned and may cause the algorithm to fail.
2. Piepers methods: Pieper [17] was one of the rst authors to adapt the Newton
Raphson method for solving inverse kinematics equations. Pieper derived two alternative
methods with different interpretations of the forward kinematics mapping function f(q). In
the rst method, f(q) is viewed as a homogeneous transformation. In the second method an
alternative version of the NewtonRaphson method is used in which the forward kinematics
map is viewed as a screw motion instead of a homogeneous transformation.
3. Methods based on differential equationsresolved motion rate control [26]: The joint
velocities can then be integrated from 0 to t
f
to produce the joint angles corresponding to
the solution
q(t
f
) = q
0
_
t
f
0
q(t ) dt. (1)
This technique is sometimes called resolved motion rate control. If a xed value of t and
a rst-order integration scheme are used, this method is virtually identical to the Newton
Raphson method. However, it is possible to utilize more accurate and robust integration
techniques. In particular, Cheng and Gupta [4] have proposed a modied predictorcorrector
algorithm for performing the joint velocity integration. They have demonstrated that their
method is more efcient and stable than the best NewtonRaphson methods.
4. For redundant manipulators, J is not square, but both the NewtonRaphson and
differential-equations-based approaches can be extended to redundant manipulators using
J
a
(x
d
Ke) yields the linear system
e Ke = 0. J
a
is the analytical Jacobian with respect to the Euler angles. Suitable values
for K can then be chosen ensure covergence and to weight the units of orientation relative
to position.
358 TOLANI, GOSWAMI, AND BADLER
6. Methods based on J
T
[21]: Interpreting a tiny displacement in the joint vector as a
torque and the error vector as a force suggests the update law
q = J
T
Ke.
This equation has the physical interpretation of a generalized spring of stiffness constant K
that pulls the end effector toward the goal state. This approach is computationally inexpen-
sive and does not require inversion of the Jacobian. On the other hand, if Ke lies in N(J
T
),
q will be zero and no further progress toward the goal state can be made.
7. Optimization approaches: Inverse kinematics can also be regarded as a nonlinear
optimization problem. Let f(q) denote the forward kinematics map from joint space to
Cartesian space, and let x
goal
denote the desired end-effector position. Consider the scalar
potential function dened by
P(q) = (f(q) x
goal
)
T
(f(q) x
goal
). (2)
Clearly the function is nonnegative and has a global minimum for any q
w
i
P
i
(q),
where w
i
are weight factors. Moreover, goals can also be satised disjunctively. In this case
the goal function is dened as
G(q) = min
i
{P
i
(q)].
2.5. Quality Criteria
We will nowattempt to dene a set of criteria for assessing the strengths and shortcomings
of an inverse kinematics algorithm.
Efciency: Since inverse kinematics must often be performed in real time, efciency
or speed is of paramount concern.
Reliability: An algorithm is reliable if it can consistently nd a solution when one
exists and if it correctly detects instances of the problem that are unsolvable.
Completeness: Additionally, in some applications it is desirable to nd the entire set
of solutions; algorithms satisfying this requirement are termed complete.
Stability: Numerical stability refers to the algorithms robustness when degenerate or
ill conditioned cases arise.
Generality: Finally, each algorithm will be evaluated on its generality, that is whether
it can be adapted to redundant manipulators or other ill-posed problems where an innite
number of solutions may exist.
Efciency is difcult to determine because it depends upon the quality of the imple-
mentation. Additionally, some algorithms may perform well on certain types of problems
and poorly on others. In general, we would expect the NewtonRaphson methods to be the
slowest and the analytical algorithms to be the fastest.
Not all of the algorithms are reliable. For example, the Jacobian transpose method and
the optimization-based approaches can stop in local minima. Numerical instability can also
prevent an algorithmfromconverging to a solution, and almost all of the numerical routines
suffer from poor reliability near singularities of the Jacobian. Analytical methods do not
suffer from singularities in the Jacobian, but can be susceptible to numerical problems
unless special precautions are taken.
The most signicant advantage of numerical algorithms is that they can be generalized
to accommodate additional constraints and objective functions, whereas the analytical ap-
proaches are restricted to 6-degree-of-freedomsystems. Optimization methods in particular
provide a convenient framework for incorporating a wide variety of criteria.
3. AN ANALYTICAL ALGORITHM FOR A 7-DOF LIMB
In this section we present the inverse kinematics algorithm for the human arm. The
kinematic chain of our interest contains seven joint variables; i.e., it has one redundant
degree of freedom. Our algorithm uses the extra of freedom to avoid joint limits or to place
the elbow as close as possible to a desired position. Our algorithm is purely analytical
REAL-TIME INVERSE KINEMATICS 361
and has no problems with Jacobian singularities or local minima. We also give empirical
evidence to demonstrate that our algorithm is more efcient and reliable than numerical
approaches.
3.1. Forward Kinematics
Consider the kinematic chain shown in Fig. 1. Let
T
1
=
_
R
1
(
1
,
2
,
3
) 0
0 0 0 1
_
denote the rotation matrix from the proximal to the distal site of S
1
as a function of
1
,
2
,
and
3
. Similarly,
T
2
=
_
R
2
(
5
,
6
,
7
) 0
0 0 0 1
_
represents the rotation matrix from the proximal to the distal site of S
2
and
T
y
=
_
R
y
(
4
) 0
0 0 0 1
_
=
_
_
c
4
0 s
4
0
0 1 0 0
s
4
0 c
4
0
0 0 0 1
_
_
FIG. 1. A 7-degree-of-freedom limb.
362 TOLANI, GOSWAMI, AND BADLER
is the rotation produced by revolute joint F. Without loss of generality, we have assumed that
the coordinate frames about the revolute joint have been dened so that rotation produced
by
4
is about the local y axis of the proximal frame of F. Finally,
A =
_
R
a
t
a
0 0 0 1
_
and
B =
_
R
b
t
b
0 0 0 1
_
are the constant transformation matrices from the distal frame of S
1
to the proximal frame
of F, and the distal frame of F to the proximal frame of S
2
.
3.2. Inverse Kinematics
To solve the inverse kinematics problem for a desired goal
G =
_
R
g
t
g
0 0 0 1
_
we must solve the equation
T
1
AT
y
BT
2
= G (3)
for the unknowns R
1
, R
2
, and
4
. The values of
1
,
2
,
3
,
5
,
6
, and
7
are then determined
by extracting the Euler angles from the rotation matrices R
1
and R
2
.
3.2.1. Solving for
4
Since
4
is the only joint variable that affects the distance of S
2
, relative to S
1
,
4
may
be computed independently. If the normal vector of the plane containing S
1
, S
2
, and F is
parallel to the axis of rotation of F, then
4
can be computed trivially using the law of
cosines. We consider the more general case where the axis of rotation of the revolute joint
is not necessarily parallel to the normal. We rst note that the position of S
2
relative to S
1
does not depend on R
2
and is given by
T
1
AT
y
BT
2
[0, 0, 0, 1]
T
= R
1
R
a
R
y
t
b
R
1
t
a
. (4)
Taking the dot product of Eq. (4) with itself and setting it equal to the square of the
distance of the goal gives
2t
T
a
R
a
R
y
t
b
= t
T
g
t
g
t
T
a
t
a
t
T
b
t
b
, (5)
which is a trigonometric equation of the form a cos(
4
) b sin(
4
) = c and can be solved
using straightforward trigonometric methods. In general there are two solutions to (5), but
only one solution is physically realizable because of joint limits on the knee or elbow.
REAL-TIME INVERSE KINEMATICS 363
3.2.2. Characterizing the Extra Degree of Freedom
Because Eq. (3) has seven unknowns but only six constraints, the system has one degree
of redundancy. Asimple physical interpretation of the redundant degree of freedomis based
on the observation that if the wrist is held xed, the elbow is still free to swivel about a
circular arc whose normal vector is parallel to the axis from the shoulder to the wrist.
The workspace of this mechanism was rst systematically analyzed by Korein [12].
Korein observed that the rst two shoulder joints along with their joint limits restrict the
tip of the elbow to lie on a spherical polygon. By interesecting the elbow circle with the
polygon it is possible to determine the legal elbow congurations as a function of the joint
limits of the rst two joints. Additionally, the twist induced by the third joint also restricts
the elbowto lie on a circular arc. It is also possible to determine the restrictions on the elbow
position as a function of the wrist joints. By taking the intersection of all sets of valid elbow
arcs, Korein derived the restrictions on the elbow position induced by the joint limits.
Our approach to solving this problem is based on the same observation as Koreins.
Koreins algorithm is derived from a geometric analysis of the problem. By contrast, our
method is purely algebraic and gives an exlicit formula for the joint angles and their deriva-
tives as a function of the swivel angle. This is an advantage when an objective function is
used to select an appropriate value of since it is often necessary to express the objective
function in terms of the joint angles.
In Fig. 2, s, e, and t
g
dene the positions of the shoulder, the elbow, and the goal location
of the wrist. The origin (0, 0, 0) is coincident with the shoulder position. The scalars L
1
,
L
2
, and L
3
denote the lengths of the upper arm, lower arm, and the distance from the goal
position to the shoulder. The origin of the coordinate system is taken as s. As the swivel
angle varies, the elbowtraces an arc of a circle lying on a plane whose normal is parallel to
the wrist-to-shoulder axis. To mathematically describe the circle we rst dene the normal
vector of the plane by the unit vector in the direction from the shoulder to the wrist,
n =
t
g
|t
g
|
.
Additionally, we need two unit vectors u and v that form a local coordinate system for the
plane containing the circle. We set uto be the projection of an arbitrary axis a (corresponding
FIG. 2. For a given goal, the elbow is free to move on a circle.
364 TOLANI, GOSWAMI, AND BADLER
FIG. 3. Finding the elbow position that is the closest possible to a desired position.
to = 0), selected by the user, onto the plane of the circle
u =
a ( a n) n
| a ( a n) n|
and set v = n u. The center of the circle c and its radius R can be computed from simple
trigonometry:
cos() =
L
2
3
L
2
1
L
2
2
|t
g
|L
1
c = cos()L
1
n
R = sin()L
1
.
Finally the equation of the elbow position is given by
e() = c R(cos() u sin() v). (6)
The swivel angle gives a useful way of characterizing the space of solutions. The swivel
angle has a meaningful physical interpretation to the user and for a given value of , Eq. (6)
can be evaluated to give an additional constraint on the position of the elbow so that the
solution to the inverse kinematics problem is uniquely dened.
Sometimes the user wants to give a desired position of the elbow e
d
instead of a swivel
angle; in this case, it is a simple matter to compute the corresponding that minimizes
|e
d
e()|. Dene the vectors p = (e
d
c) and p
= p (p n) n. Note that p
is merely
the projection of p onto the plane containing the elbow circle. As shown in Fig. 3 the value
of that minimizes |e
d
e()| is the angle between p
u|
|p
|
and
cos() =
p
u
|p
|
, = a tan 2(|p
u|, p
u).
3.2.3. Solving for R
1
and R
2
In this section we show how to compute R
1
and R
2
efciently given the values of and
4
. Figure 4 shows the initial conguration of the arm corresponding to an elbow rotation
of
4
and the goal conguration for a desired swivel angle . As before, we have taken
the position of the shoulder joint s as the origin. e, e
g
(), w, w
g
denote the positions of
REAL-TIME INVERSE KINEMATICS 365
FIG. 4. Calculating R
1
.
the elbow and wrist in the rest and goal congurations. Assuming that the elbow is not
completely outstretched, the shoulder, elbow, and wrist locations form a reference triangle.
The rotation matrix R
1
is merely the rigid body transformation of .sew onto .se
g
()w
g
.
To nd R
1
we rst dene a local coordinate system [
x y z 0
0 0 0 1
] associated with .sew by
x =
e
|e|
y =
w (w x) x
|w (w x) x|
z = x y.
Analogouslywe dene the coordinate system[
x
g
() y
g
() z
g
() 0
0 0 0 1
] associatedwith.se
g
()w
g
.
R
1
is then given by
R
1
=
_
x
g
() y
g
() z
g
() 0
0 0 0 1
__
x y z 0
0 0 0 1
_
T
. (7)
Finally, R
2
is obtained by rearranging Eq. (3)
T
2
= (T
1
AT
y
B)
1
G
R
2
= (R
1
R
a
R
y
R
b
)
1
R
g
.
It is straightforward to extend the algorithm to problems that involve only position con-
straints. In this case the wrist angles are chosen by the user and only R
1
and
4
are computed.
3.2.4. Joint Limits
In practice, it is necessary to consider joint limits to ensure plausible looking solutions.
In this section, we describe an analytical algorithm that computes all possible values of
that satisfy the joint limits. The algorithm also determines when a solution is not possible.
366 TOLANI, GOSWAMI, AND BADLER
FIG. 5. Decomposing R
1
into R
0
and R
.
We will rst derive the relationship between the joint variables
1
,
2
,
3
,
5
,
6
,
7
and the
swivel angle and show how this relationship can be used to determine the valid ranges of
for each joint.
Computing the valid ranges for
1
,
2
,
3
. As shown in Fig. 5 R
1
can be expressed as a
sequence of two rotations R
1
= R
0
R
, where R
0
is the rotation matrix that moves the elbow
to e(0) from the rest position by setting = 0 in Eq. (7) and R
into its scalar components yields one of two possible systems of equations of the form
sin(
i
) = f
1
()
cos(
i
) cos(
j
) = f
2
()
cos(
i
) sin(
j
) = f
3
()
cos(
i
) cos(
k
) = f
4
()
cos(
i
) sin(
k
) = f
5
()
or
cos(
i
) = f
1
()
sin(
i
) cos(
j
) = f
2
()
sin(
i
) sin(
j
) = f
3
()
sin(
i
) cos(
k
) = f
4
()
sin(
i
) sin(
k
) = f
5
(),
REAL-TIME INVERSE KINEMATICS 367
where i, j , and k are any permutation of the sequence {1, 2, 3], f
l
=
l
sin()
l
cos()
l
, and
l
,
l
, and
l
are constants that depend upon R
0
. Without loss of generality, we will
assume that we have a set of equations of the form
sin(
1
) = f
1
()
cos(
1
) cos(
2
) = f
2
()
cos(
1
) sin(
2
) = f
3
()
cos(
1
) cos(
3
) = f
4
()
cos(
1
) sin(
3
) = f
5
().
The other cases can be handled using a straightforward generalization of the techniques
that we are about to describe. We rst consider how to determine the valid ranges of
that satisfy the joint limits for
1
. There are two families of solutions for
1
of the
equation sin(
1
) = k ([k[ 1). Family one corresponds to values of
1
that lie in the range
(
2
,
2
) and family two corresponds to values that lie in the range (
2
,
3
2
). To distinguish
between these two families we will use the notation
1
1
to indicate a value of
1
that
belongs to the rst family and
1
2
to denote a value of
1
in the second family. Obviously,
within each family the relationship between sin(
1
) and
1
is monotonic. Given a set of
joint limits
1 min
< <
1 max
, we can nd a corresponding set of valid joint limit ranges
for each family. There will be at most two such ranges. For example, if
min
=
4
and
max
=
7
4
, then any value of in family two satises the joint limits, but solutions in
family one are restricted to lie in the interval (
4
,
2
) or in the interval (
3
2
,
7
4
) as illustrated in
Fig. 6.
Suppose (a, b) is a valid range for
1
in family i . Because the relationship between
1
i
and sin(
1
i
) is monotonic in a given family i , if
1
i
(a, b), then we must have
FIG. 6. Joint limits on
1
.
368 TOLANI, GOSWAMI, AND BADLER
FIG. 7. Finding the values of that satisfy the joint limits of
1
.
sin(
1
i
) (sin(a), sin(b)). From Eq. (8), the relationship between sin(
1
) and is given
by the equation
sin(
1
) =
1
sin()
1
cos()
1
.
As shown in Fig. 7, to nd the corresponding ranges of that yield values of
1
in the range
(a, b) we intersect the
1
sin()
1
cos()
1
curve with the straight line segments
sin() = a and sin() = b and determine if the sections of the curve lying between two
consecutive intersectionpoints are inthe range (sin(a), sin(b)). This test canbe accomplished
by checking the sign of the derivative of the curve to see if the function is increasing or
decreasing.
The valid ranges of for
1
are stored in two sets
1
1
= {(a, b) [
1 min
<
1
1
() <
1 max
, a < < b]
1
2
= {(a, b) [
1 min
<
1
2
() <
1 max
, a < < b].
As with
1
, there are two families of solutions for
2
corresponding to whether
1
is in
the range (
2
,
2
) or (
2
,
3
2
).
2
1
() = a tan 2( f
3
, f
2
)
2
<
1
<
2
2
() = a tan 2(f
3
, f
2
)
2
<
1
<
3
2
.
REAL-TIME INVERSE KINEMATICS 369
To determine the valid set of such that
2
lies within the joint limits, we compute the
intersections of the
2
i
curves with
2 min
and
2 max
and use these intersection points to
partition the curves into piecewise components. If a component lies within
2 min
and
2 max
,
then the corresponding interval is valid. To determine the intersections of the
2
1
and
2
2
curves with
2 min
and
2 max
we note that since tan() = tan( ) the solution of for
the equation
f
2
()
f
3
()
= tan() (8)
implies that either
a tan 2( f
2
, f
3
) =
or
a tan 2( f
2
, f
3
) = a tan 2(f
2
, f
3
) = .
Thus to compute the desired intersection points, we only need solve Eq. (8) with
2 min
and
2 max
substituted for . We then determine if the intersection point corresponds to family
one or family two by checking if
2
1
() = .
The algorithm for determining the valid ranges of proceeds as follows. For each curve
2
i
()
compute the associated intersection points and store them into a sorted sequence
along with the end points 0 and 2. For each set of adjacent intersection points (
j
,
j 1
)
in the sequence determine if the corresponding curve segment
2
i
()
j
<<
j 1
lies within
the range
2 min
and
2 max
. This test can be accomplished by checking the derivative of
2
i
or by evaluating the curve at a randomly chosen point inside the interval (
i
,
i 1
). Also,
because an angle can wrap around from 0 to 2 the algorithm merges two intervals of the
form (0,
i
) and (
i
, 2) into a single interval. As with
1
, the valid intervals are stored in
two sets
2
1
and
2
2
. The analysis of
3
is identical to that of
2
.
Computing the valid joint ranges for
5
,
6
,
7
. Recalling R
2
= (R
1
R
a
R
y
R
b
)
1
R
g
and
substituting R
1
= R
0
R
i =1
i
1
A
2
=
3
i =1
i
2
.
Analogously, dene B
1
and B
2
for joints
5
,
6
, and
7
:
B
1
=
7
i =5
i
1
B
2
=
7
i =5
i
2
.
370 TOLANI, GOSWAMI, AND BADLER
V, the set of valid psi intervals that satisfy all six sets of joint limits, is given by
V = (A
1
B
1
) (A
1
B
2
) (A
2
B
1
) (A
2
B
2
).
Given V there are at least two useful ways of selecting an appropriate value of . The rst
method is to nd the largest interval in V and to select its midpoint. This will tend to keep
the arm in a posture that maximizes clearance from the joint limits. Another possibility is
to choose the value of that lies in a valid interval and that is closest to a desired swivel
angle
d
.
More generally, we can nd the value of that minimizes an arbitrary objective function
f (
1
, . . . ,
7
). In order to nd a minimizing value for f , we need to solve
d
d
f = 0. Apply-
ing the chain rule gives
d
d
f (
1
(), . . . ,
7
()) =
_
d
1
d
, . . . ,
d
7
d
_
f.
If the relationship between and is of the form sin() = e(), where e() = cos()
sin() , the derivatives
d
d
can be computed as
d
d
sin() =
d
d
e()
cos()
d
d
= e
/
()
d
d
=
e
/
()
cos()
=
e
/
()
_
1 sin
2
()
=
e
/
()
_
1 e
2
()
.
For family 1,
2
< <
2
cos() > 0
d
d
=
e
/
()
_
1 e
2
()
.
For family 2,
cos() < 0
d
d
=
e
/
()
_
1 e
2
()
.
If the relationship between and is of the form
sin() cos() = e
1
()
cos() cos() = e
2
()
sin() = e
3
()
e
i
() =
i
cos()
i
sin()
i
,
where is another joint variable, the derivative
d
d
is slightly more complicated. We rst
note that this system of equations implies that
= tan
1
_
e
1
()
e
2
()
_
or = tan
1
_
e
1
()
e
2
()
_
.
REAL-TIME INVERSE KINEMATICS 371
Taking the derivative of either equation gives
d
d
=
e
/
1
e
2
e
1
e
/
2
e
2
1
e
2
2
=
e
/
1
e
2
e
1
e
/
2
cos
2
()
=
e
/
1
e
2
e
1
e
/
2
_
1 e
2
3
.
Note that in this case the derivative does not depend upon what family belongs to.
Since f can be arbitrarily complex, there will in general be no closed-form solution to
d
d
f = 0, and a numerical method must be used instead. However, this approach still has
a number of signicant advantages compared to the approach of minimizing with respect
to the joint variables
1
, . . . ,
7
as in Zhao and Badler [27], for example. In traditional
optimization methods, there are two ways of solving for the minimum of f while satisfying
the inverse kinematics and joint limit constraints. In the approach favored in [27], the
problem is cast as
min(w
1
f (
1
, . . . ,
7
) w
2
(|g(
1
, . . . ,
7
) g
d
|
2
)
subject to
i min
<
i
<
i max
; i = 1 . . . 7, (9)
where g(
1
, . . . ,
7
) is the forward kinematics mapping function, g
d
is the desired end-
effector position and orientation, and w
1
and w
2
are weights that rank the importance of
minimizing the objective function relative to satisfying the inverse kinematics constraint.
In this formulation, the optimization problem is in a seven-dimensional space and because
the inverse kinematics task is merely part of an augmented objective function there is
no guarantee that the inverse kinematics constraint will be satised. Another traditional
approach is to pose the problem as
min f (
1
, . . . ,
7
)
subject to
g(
1
, . . . ,
7
) g
d
= 0
i min
<
i
<
i max
; i = 1 . . . 7.
A minimum of the problem above is guaranteed to solve the inverse kinematics problem,
but now the constraints are nonlinear, which makes the optimization problem very difcult
to solve.
In contrast, our method is a hybrid of analytical and numerical techniques. The analytical
phase is used to simplify the dimension of the problemto a single variable and to establish
the feasible set of solutions by nding linear inequality limits on . Anumerical method can
then be used to nd the solution to the one-dimensional optimization problemof minimizing
f () instead of a seven-dimensional function f (
1
, . . . ,
7
). Unlike the case of multivariate
optimization, fast and reliable techniques exist for nding the minimum of a function of
one variable.
Unsolvable problems. Sometimes it is not possible to nd a value of that satises
the limits of all the joint variables. In these cases, it is often useful to give the user an
372 TOLANI, GOSWAMI, AND BADLER
approximate solution that satises the joint limits. The optimization method used by Zhao
and Badler [27] incorporates this case implicitly. We use an optimization-based method to
handle this case as well, but we take advantage of properties of the problem to simplify the
optimization task.
We rst notice that we can usually solve for the desired position of the wrist provided
that the goal position is close enough to the shoulder. This allows us to compute
4
and
the valid ranges of for variables
1
,
2
, and
3
. Let V = A
1
A
2
denote the set of that
satisfy the joint limits for the two families of solutions for
1
,
2
, and
3
. Since the location
of the wrist does not depend on the other joints, any V satises the joint limits for the
shoulder angles and is a solution for the goal position. Thus, the problem has been reduced
to nding a suitable value of V that minimizes the orientation error between the wrist
frame and the goal frame.
There are many possible ways of measuring orientation error between two rotation ma-
trices R
d
= [ n
d
, s
d
, a
d
] and R = [ n, s, a]. The approach used by Zhao and Badler [27] uses
the term |( n
d
n)| |( s
d
s)|. Another approach is to convert the rotations into their
unit quaternion representations and take the absolute value of their dot product. A value of
1 means that the two rotations are identical, and a value of 0 indicates that the two rotations
are far apart. Another approach is to nd the axis u and angle of equivalent rotation of
the matrix
R
u
() = R
d
R
T
,
2
2
and to take the orientation error as sin()
2
. It is a simple matter to show that
4 sin(v)
2
= (r
32
r
23
)
2
(r
13
r
31
)
2
(r
21
r
12
)
2
,
where r
i j
are the components of R
u
().
The method used to calculate orientation error can be written as an objective function for
a nonlinear optimization problemwith linear inequality constraints in terms of the variables
,
5
,
6
, and
7
. This approach has several advantages compared with the approach used in
[27]. The objective function is simpler and the problemis only four-dimensional rather than
seven-dimensional. Additionally, the objective function used in [27] constrains both position
and orientation terms and requires scale factors to weight the relative importance of these
terms. In contrast, the objective function used in our algorithm contains only orientation
constraints, the position constraint is always satised.
3.3. Partial Orientation Constraints
It is often the case that a user wants to pose an inverse kinematics solution for only one of
the columns of the orientation component of the goal matrix. Consider the case of a person
hammering a nail. In this example, it is not necessary to solve completely for the orientation
of the end effector to obtain a plausible-looking solution. The only requirements are that
the position of the tip of the hammer coincide with the nail and the axis pointing out of the
head of the hammer align with the shaft of the nail. In this case, the system has an extra
degree of freedom parameterized by the angle of rotation about the axis of the hammer. For
these types of problems, the inverse kinematics problem has the form
T
1
AT
y
BT
2
E = G(),
REAL-TIME INVERSE KINEMATICS 373
FIG. 8. A partial orientation problem is parameterized by and .
where E = [
R
e
t
e
0 0 0 1
] species the constant transformation from the proximal frame of the
wrist to the end effector. As shown in Fig. 8, the orientation component of the goal matrix
G depends on three vectors a, u
g
, v
g
, where a is the axis that the user wants to align with
the end effector and the u
g
and v
g
vectors are dened by constructing a local coordinate
system about the plane whose normal is given by a,
u
g
=
p ( p a) a
| p ( p a) a|
v
g
= a u
g
,
where p is the projection axis dened in the previous section. The structure of G depends
on which column of the goal matrix a corresponds to:
a x G() =
_
a u
g
cos() v
g
sin() v
g
cos() u
g
sin() t
g
0 0 0 1
_
a y G() =
_
v
g
cos() u
g
sin() a u
g
cos() v
g
sin() t
g
0 0 0 1
_
a z G() =
_
u
g
cos() v
g
sin() v
g
cos() u
g
sin() a t
g
0 0 0 1
_
.
The parameter plays a role for the wrist position similar to the role plays for the
elbowlocation. Changing the value of moves the wrist relative to the end-effector position
about a circle that lies on a plane whose normal is a.
We nowcompute the wrist and elbowpositions as function of and . The wrist position
w is given by
_
w
1
_
= T
1
AT
y
BT
2
[0, 0, 0, 1]
T
= G()E
1
[0, 0, 0, 1]
T
.
Expanding the right-hand side of the equation reveals that w is of the form
w() =
_
1
cos
1
sin
1
2
cos
2
sin
2
3
cos
3
sin
3
_
_
, (10)
374 TOLANI, GOSWAMI, AND BADLER
where
i
,
i
, and
i
are constants that depend on Gand E. Equation (10) gives a convenient
way of solving for a suitable value of that optimizes for a criterion that involves the
wrist position. For example, if the vertical axis is along the z direction, the values of that
minimize and maximize the height of the wrist can be computed by solving
d
d
(
3
cos
3
sin
3
) = 0, (11)
which has a straightforward, trigonometric analytical solution. There are two solutions
to Eq. (11) corresponding to the minimizing and maximizing values of . They can be
distinguished by checking whether the second derivative is positive or negative.
Similarly, the value of that minimizes the distance from a desired wrist position w
d
is
given by the solution to the equation
d
d
((w() w
d
) (w() w
d
)) = 0. (12)
Equation (12) has the form
a cos
2
b sin
2
c(cos sin ) d cos e sin f = 0.
We can convert the trigonometric terms into a polynomial by making the half angle
substitution
u = tan
2
cos =
1 u
2
1 u
2
sin =
2u
1 u
2
.
The equation with the substituted variable is a quartic polynomial in u which has a straight-
forward, trigonometric analytical solution. Finally, once the wrist position is known, the
elbow equation can be computed using the method described in the previous section. How-
ever, the elbow position now depends upon the two variables and .
Given a goal matrix G and the parameters and , we can derive the wrist and elbow
positions from Eqs. (10) and (6) with w substituted for t
g
.
4
and R
0
depend upon both G
and , and the R
we can always
nd a system of equation of the form
sin(
i
) = f
1
(, )
cos(
i
) cos(
j
) = f
2
(, )
REAL-TIME INVERSE KINEMATICS 375
cos(
i
) sin(
j
) = f
3
(, )
cos(
i
) cos(
k
) = f
4
(, )
cos(
i
) sin(
k
) = f
5
(, )
or
cos(
i
) = f
1
(, )
sin(
i
) cos(
j
) = f
2
(, )
sin(
i
) sin(
j
) = f
3
(, )
sin(
i
) cos(
k
) = f
4
(, )
sin(
i
) sin(
k
) = f
5
(, ),
where i, j , and k are a permutation of the sequence {1, 2, 3] and f
l
(, ), l = 1, . . . , 5, are
lengthy trigonometric expression in , which have to be determined through the use of
a symbolic mathematics package. Assume without loss of generality that we have a system
of the rst form and that i = 1, j = 2, and k = 3. We can calculate the partial derivatives
of
1
,
2
, and
3
,
=
_
_
_
f
1
_
_
_
1 f
2
1
family 1
_
f
1
_
_
_
1 f
2
1
family 2
=
_
_
_
f
1
_
_
_
1 f
2
1
family 1
_
f
1
_
_
_
1 f
2
1
family 2
and
=
__
f
3
_
f
2
f
3
_
f
2
___
_
1 f
2
1
=
__
f
3
_
f
2
f
3
_
f
2
___
_
1 f
2
1
=
__
f
5
_
f
4
f
5
_
f
4
___
_
1 f
2
1
=
__
f
5
_
f
4
f
5
_
f
4
___
_
1 f
2
1
.
By expanding the equation R
2
= (R
0
R
R
a
R
y
R
b
)
1
R
g
()R
1
e
, we can obtain similar ex-
pressions for the partials of
5
,
6
, and
7
.
We can now cast the problem of nding a solution to the partial orientation constraint
with joint limits into an optimization problem in terms of and . Dene the objective
function
F(, ) =
7
i =1,i ,=4
(
i
(, )
i
)
2
,
376 TOLANI, GOSWAMI, AND BADLER
where
i
= (
max
i
min
i
)/2 is the middle of the joint limit range of joint i . Minimizing F
will tend to keep the joint variables as close as possible to the middle of their valid ranges.
A local minimum of F can be found using a standard nonlinear optimization procedure to
nd where the gradient of F vanishes. The gradient of F is given by
F =
_
_
7
i =1,i ,=4
2(
i
(, )
i
i
)
7
i =1,i ,=4
2(
i
(, )
i
i
)
_
_
.
The expressions for
i
(, ),
i
, and
i
2
= a sin
_
L
3
L
4
sin
_
= ( ).
The center of the hand circle c and its radius R are given by
c = L
3
cos
p
|p|
R = L
3
sin .
Finally to obtain h() we dene a local u, v coordinate system by projecting an arbitrary
378 TOLANI, GOSWAMI, AND BADLER
vector p onto the plane of the circle as we did in the previous two sections.
h() = c R(cos u sin v). (13)
In order to solve R
1
we rst consider the following subproblem. Given two points x, y such
that |x| = |y| and a unit vector z passing through the origin we want to nd the angle of
rotation about z that rotates x onto y. We also make the assumption that x and y are not
collinear and that they do not lie along the z axis. Let x
= x z
T
zx and y
= y z
T
zy be
the projections of x and y onto the plane perpendicular to z. We can nd by calculating
the angle between x
and y
. Since
x
= z sin()|x
||y
| (14)
and
x
= cos()|x
||y
| (15)
we have = a tan 2( z
T
(x
), x y).
For a specied
4
and h() we nd R
1
by expressing it as the product of two rotation
matrices S
1
and S
2
(Fig. 11). Let h
o
denote the position of the hand after the elbow joint
rotates by
4
. S
1
is the rotation about the vector h
o
h()/|h
o
h()| that moves the
hand from h
o
to h(). Now let a
1
denote the orientation of the aim vector after we apply
S
1
. S
2
is the rotation about the
sh /|