224
IEEE
JOURNAL
OF
ROBOTICS
AND
AUTOMATION,
VOL.
RA-3,
NO.
3,
JUNE
1987
A
Simple Motion-Planning Algorithm
for
General
Robot Manipulators
TOMAS LOZANO-PEREZ, MEMBER,
IEEE
Abstrct-A
simple and efficient algorithm is presented, using configu-
ration space, to plan collision-free motions for general manipulators.
An
implementation of the algorithm for manipulators made up of revolute
joints is also presented. The configuration-space obstacles for an
n
degree-of-freedom manipulator are approximated
by
sets of
n
-
1-
dimensional slices, recursively built up from one-dimensional slices. This
obstacle representation leads to an efficient approximation of the free
space outside
of
the configuration-space obstacles.
T
I.
INTRODUCTION
HIS PAPER presents an implementation of a new
motion-planning algorithm for general robot manipulators
moving among three-dimensional polyhedral obstacles. The
algorithm has a number of advantages: it is simple to
implement, it is fast for manipulators with few degrees of
freedom, it can deal with manipulators having many degrees
of freedom (including redundant manipulators), and it can deal
with cluttered environments and nonconvex polyhedral obsta-
cles. An example of a path obtained from an implementation of
the algorithm is shown in Fig. 1.
The ability to plan automatically collision-free motions for a
manipulator given geometric models of the manipulator and
the task is one of the capabilities required to achieve
task-leuel
robot programming
[lS].
Task-level programming is one of
the principal goals of research in robotics. It is the ability to
specify the robot motions required to achieve a task in terms
of
task-level commands, such as
‘
‘Insert pin-A in hole-B
,
’
’
rather
than robot-level commands, such as “move to 0.1, 0.35,
1.6.”
The motion-planning problem, in its simplest form, is to
find a path from a specified starting robot configuration to a
specified goal configuration that avoids collisions with a
known set of stationary obstacles. Note that this problem is
significantly different from, and quite a bit harder than, the
collision detection problem: detecting whether a known robot
configuration or robot path would cause a collision [1], [4].
Motion planning is also different from on-line obstacle
avoidance: modifying a known robot path
so
as to avoid
unforeseen obstacles [6], [9], [lo],
[
111.
Manuscript received May 19,
1986;
revised December
11,
1986. This work
was supported in part by the Office of Naval Research under Contract
N00014-82-K-0494, in part by a grant from the System Development
Foundation, in part by the Advanced Research Projects Agency under Office
of Naval Research Contracts N00014-85-K-0214 and N00014-82-K-0334, and
in part by a National Science Foundation Young Investigator Grant.
The author is with the Artificial Intelligence Laboratory and the Department
of Electrical Engineering and Computer Science, Massachusetts Institute of
Technology, Cambridge,
MA
02139,
USA.
IEEE
Log
Number 8714998.
Although general-purpose task-level programming is still
many years away, some of the techniques developed for task-
level programming are relevant to existing robot applications.
There is, for example, increasing emphasis among major robot
users on developing techniques for off-line programming, by
human programmers, using computer-aided design
(CAD)
models of the manipulator and the task. In many of these
applications motion planning plays a central role. Arc welding
is a good example; specifying robot paths for welding along
complex three-dimensional paths is a time-consuming and
tedious process. The development of practical motion-plan-
ning algorithms could reduce significantly the programming
time for these applications.
A great deal of research has been devoted to the motion-
planning problem within the last five to eight years, e.g.
,
[2],
[31,
[SI,
171, 181, [121-[141, U61, [17l,
WI,
POI.
However,
few of these methods are simple enough and powerful enough
to be practical. Practical algorithms are particularly scarce for
manipulators made up of revolute joints, the most popular type
of industrial robot. The author is aware of only two previous
motion-planning algorithms that are both efficient and reason-
ably general for revolute manipulators with three or more
degrees of freedom [2], [7]. Brook’s algorithm [2] has
demonstrated impressive results but is fairly complex. Faver-
jon’s algorithm [7], on the other hand, is appealingly simple.
The basic approach of the algorithm described here is closely
related to the method described by Faverjon. Many of the
details of the present algorithm, however, especially the
treatment of three-dimensional constraints and the free space
representation,
are
new and more general.
The approach taken in this algorithm is similar to that
of
[7],
[8], [12], [13] in that it involves quantizing joint angles. It
differs in this respect from exact algorithms such as
[
171,
[
191.
On the other hand, the quantization approach lends itself
readily to efficient computer implementation. The approach
taken here differs from most previous configuration-space
algorithms, for example, [4], [5],
[
131,
[
141, in that no attempt
is made to characterize the
surfaces
of the obstacles in the
configuration space. Instead, the approximate obstacles are
built up from a series of one-dimensional ranges of forbidden
values. As with many motion-planning algorithms, including
[5], [13], [14], [19], the free-space outside of the obstacles in
this algorithm is represented as a collection of cells. The
connectivity graph of the cells is used
to
search for a path. A
cellular representation of the free-space contrasts with the use
of a one-dimensional subset of the free-space such as the
Voronoi diagram; see, for example, [2], [17].
0882-4967/87/0600-0224$01
.OO
0
1987 IEEE
LOZANO-PEREZ: A SIMPLE MOTION-PLANNING ALGORITHM
225
4
Fig.
1.
Path for all six links
of
Puma obtained using algorithm described here. Three-fingered hand shown as end-effector does not
change configuration during motion, hut its shape
is
taken into account during motion planning.
Total
planning time
on
Symbolics
3600
was approximately
3
min. Joints were sampled at
k
3".
The purpose of this paper is to show that motion planning
for general manipulators can be both simple and relatively
efficient in most practical cases. There is no reason why
motion planning should be any less practical than computing
renderings
of three-dimensional solids in computer graphics.
In both cases, there are many simple numerical computations
that can benefit from hardware support. In fact, it is worth
noting that in the examples in Fig.
1
it took longer to compute
the hidden-surface displays in the figures than to compute the
paths.
II.
THE
BASIC
APPROACH:
SLICE
PROJECTION
The
configuration
of a moving object is any set of
parameters that completely specify the position of every point
on
the object. Configuration space
(C
space) is the space of
configurations of a moving object. The set of joint angles
of
a
robot manipulator constitute a configuration. Therefore, a
robot's joint space is a configuration space. The Cartesian
parameters of the robot's end effector, on the other hand, do
not usually constitute a configuration because of the multiplic-
ity
of solutions to a robot's inverse kinematics. It is possible to
IEEE
JOURNAL
OF
ROBOTICS
AND
AUTOMATION,
VOL. RA-3,
NO.
3,
JUNE
1987
226
I
(b)
Fig. 2. (a) Closeup of initial configuration of manipulator for path in Fig.
l(b)
(from different viewpoint).
(b)
Closeup
of
final
configuration of
manipulator
for
path
in
Fig.
l(b).
Note that full polyhedral descriptions of
arm
and end-effector are used
in
algorithm.
map the obstacles in the robot's workspace into its configura-
tion space [3j-[5], [13], [14j. These
C-space obstacles
represent those configurations of the moving object that would
cause collisions.
Free space is defined to be the complement
of the C-space obstacles.
Motion planning requires an explicit characterization of the
robot's free space. The characterization may not be complete,
for example, it may cover only a subset of the free space.
However, without a characterization of the free space, one is
reduced to trial and error methods to find a path. In this paper
we show how to compute approximate characterizations of the
free space for simple manipulators. By simple manipulators
we mean manipulators composed of a nonbranching sequence
of links connected by either revolute or prismatic joints (see
[18] for a treatment of the kinematics of simple manipulators).
We restrict the position of link zero of a simple manipulator to
be fixed. Most industrial manipulators (not including parallel-
jaw grippers) are simple manipulators in this sense.
The C-space obstacles for a manipulator with
n
joints are, in
general, n-dimensional volumes. Let
C
denote an n-dimen-
sional C-space obstacle for a manipulator with
n
joints. We
represent approximations of
C
by the union of
n
-
1-
dimensional slice projections
[
131,
[
141. Each
n
-
1-
dimensional configuration in a slice projection of
C
represents
a range of n-dimensional configurations (differing only in the
value of a single joint parameter) that intersects
C.
A slice projection of an n-dimensional C-space obstacle is
defined by a range of values for one of the defining parameters
of the C space and an
n
-
1-dimensional volume. Let
q
=
(ql,
*
,
qn)
denote a configuration, where each
qi
is a joint
parameter, which measures either angular displacement (for
revolute joints) or linear displacement (for prismatic joints).
Let
x,
be a projection operator for points, defined such that
x,(4)=(41,
***Y
qj-19
4j+l>
.**)
qn).
Let
IIla,,bjl(S)
be a projection operator for point sets
S,
defined
such that
n[a,,,](S)=
{Tj(q)lq
E
S
and
qj
E
[ai,
bjI}*
Then, the slice projection of the obstacle
C
for values of
qj
E
[aj,
b,]
is
%7j,bj](C).
The definition of slice projection is illustrated in Fig.
3.
In the
foregoing example, joint
j is called the slice joint while the
other joints are known as
free joints.
Note that a slice projection is a conservative approximation
of a segment of an n-dimensional C-space obstacle. An
approximation of the full obstacle is built as the union of a
number of
n
-
1-dimensional slice projections, each for a
different range of values of the same joint parameter (Fig. 3).
Each of the
n
-
1-dimensional slice projections, in turn, can
be approximated by the union of
n
-
2-dimensional slice
projections and
so
on, until we have a union of one-
dimensional volumes, that is, linear ranges. This process is
illustrated graphically in Fig. 3. Note that the slice projection
can be continued one more step until only zero-dimensional
volumes (points) remain, but this is wasteful.
Consider a simple two-link planar manipulator whose joint
parameters are
q1
and
42.
C-space obstacles for such a
manipulator are two-dimensional. The one-dimensional slice
projection of a C-space obstacle
C
for
41
E
[a,
b] is some set
of linear ranges
{Ri}
for
42.
The ranges must be such that if
there exists a value of
q2,
call it
d,
and a value
q1
E
[a,
bj,
call it
c,
for which
(cy
d)
E
C,
then
d
is in one of the
Ri
(Fig.
3).
A representation of a configuration space with obstacles is
illustrated in Fig. 4(b), for the two-link manipulator and
obstacles shown in Fig. 4(a). The actual configuration space is
the surface of a torus since the top and bottom edge of the
diagram coincide
(0
=
2a),
as do the left and right edge. The
obstacles are approximated as a set of
42
ranges (shown dark)
for a set of values of
ql.
The resolution is
2"
along the
qI
axis.
If the manipulator has three links, its configuration space
can be constructed as follows.
1) Ignore links beyond link
1.
Find the ranges of legal
values of
41
by considering rotations of link 1 around the
fixed base.
LOZANO-PEREZ: A SIMPLE MOTION-PLANNING ALGORITHM
P
t
I
I
I
I
T
227
2)
Sample the legal range of
q1
at the specified resolution.
Do
steps
3-5
for each of the value ranges of
ql.
3)
Ignore links beyond link
2.
Find the ranges of legal
values of
q2
by considering rotating link
2
around the
positions of joint
2
determined by the current value range
4)
Sample the legal range of
q2
at the specified resolution.
Do
step
5
for each of these value ranges of
q2.
5)
Find the ranges of legal values of
q3
by considering
rotating link
3
around the position of joint
3
determined
by the current value ranges of
q1
and 42.
of
41.
Some sample slices from a configuration space computed in
this way can be seen in Fig.
5.
Note that
the
process just described is an instance of the
following simple recursive process. To compute C space
(i),
1)
ignore links beyond link
i,
and find the ranges of legal
values of
qi
by considering rotating link
i
around the
positions of joint
i
determined by the current value
ranges of
ql,
*
e,
4;-
1;
2)
if
i
=
n,
then stop; else sample the legal range of
qi
at
the specified resolution. Compute C space
(i
+
1)
for
each of these value ranges of
qi.
Observe that the basic computation to be done is that of
determining the ranges of legal values for a joint parameter
given ranges of values of the previous joints. This computation
is the subject of Section
111.
The recursive nature of the C-space computation calls for a
recursive data structure to represent the C space. The current
implementation uses a tree whose depth is
n
-
1, where
n
is
the number of joints, and whose branching factor is the
number of intervals into which the legal joint parameter range
for each joint is divided (Fig.
6).
The leaves of the tree are
ranges of legal (or forbidden) values for the joint parameter
n.
Many of the internal nodes in the tree will have no descendants
because they produce a collision of some link
i
<
n.
The main advantage of a representation method built on
recursive slice projection is its simplicity. All operations on
the representation boil down to dealing with linear ranges, for
which very simple and efficient implementations are possible.
The disadvantages
are
the loss of accuracy, and the rapid
increase of storage and processing time with dimensionality of
the
C
space. Contrast this approach with one that represents
the boundaries of the obstacles by their defining equations
[4],
[5].
Using the defining equations is cleaner and more accurate,
but the algorithms for dealing with interactions between
obstacle boundaries are very complex.
I
believe that the
simplicity of slice projection outweighs its drawbacks. These
drawbacks can be significantly reduced by exercising care in
the implementation of the algorithms.
111.
SLICE
PROJECTIONS
FOR
POLYGONS
The key step in our approach is computing one-dimensional
slice projections of C-space obstacles, that is, determining the
range of forbidden values of one joint parameter, given ranges
of values for all previous joint parameters. We will illustrate
how these ranges may be computed by considering the case of
228
IEEE
JOURNAL
OF
ROBOTICS AND AUTOMATION, VOL.
RA-3,
NO.
3,
JUNE
1987
(b)
Fig.
4.
(a) Two-link revolute manipulator and obstacles.
(b)
Two-dimensional C space with obstacles approximated
by
list
of
one-
dimensional slice projections (shown dark). Initial and final position of manipulator are shown in input space and C space.
Fig.
5.
Configuration space slices for three link revolute manipulator.
Figures on right show samples of two-dimensional slice projections used to
approximate three-dimensional configuration space. Each slice shows
constraints on
q2
and
q,
for
different range of values of
q1
(note different
orientations of manipulator’s first link in right figures). On left, manipula-
tor is shown in number
of
configurations along path shown on slice
diagram; initial and final configurations of paths are indicated by circles.
planar revolute manipulators moving among planar obstacles.
We will first discuss this problem informally and then derive
the solution from the equations of
C surfaces.
A.
A
Geometric View
Assume that joint
k,
a revolute joint, is the free joint for a
one-dimensional slice projection and that the previous joints
are fixed at known values. Note that we assume, for now, that
the previous joints are fixed at
single values rather than ranges
of
values; we will see in Section
III-C
how to relax this
restriction. We require that the configuration of the first
k
-
1
links be safe, that is, no link intersects an obstacle. This is
guaranteed by the recursive computation we saw in Section
11.
Given these assumptions, we need to find the ranges of values
of the single joint parameter
qk
that are forbidden by the
presence of objects in the workspace.
The ranges of forbidden values for
qk will be bounded by
angles where link
k
is just touching an obstacle. For polygonal
links moving among polygonal obstacles, the extrema1 con-
tacts happen when a vertex of one object is in contact with an
edge of another object. Therefore, the first step in computing
the forbidden ranges for
qk is to identify those critical values
of
qk
for which some obstacle vertex is in contact with a link
edge or some link vertex is in contact with an obstacle edge
(Fig.
7).
The link is constrained to rotate about its joint; therefore,
every point on the link follows a circular path when the link
rotates. The link vertices, in particular, are constrained to
known circular paths. The intersection of these paths with
obstacle edges determine some of the critical values of
qk,
for
example,
B
in Fig.
7.
As
the link rotates, the obstacle vertices
also follow known circular paths relative to the link. The
intersection of these circles with link edges determine the
remaining critical values for
qk,
for example, A in Fig.
7.
Determining whether a vertex and an edge segment can
intersect requires first intersecting the circle traced out by the
vertex and the infinite line supporting the edge to compute the
potential intersection points. The existence of such an intersec-
tion is
a
necessary condition
for
a contact between link and
obstacle, but it is not
sufficient. Three additional constraints
must hold (Fig.
8).
1)
In-edge constraint is that where the
intersection point must be within the finite edge segment, not
just the line supporting the edge.
2)
For orientation con-
straint, the orientation of the edges at the potential contact
must be compatible, that is, the edges that define the contact
vertex must both be outside of the contact edge. For the
reachability constraint for nonconvex objects, there must not
be other contacts that prevent reaching this point.
The in-edge constraint can be tested trivially given the