From: AAAI-86 Proceedings. Copyright ©1986, AAAI (www.aaai.org). All rights reserved.
A SIMPLE MOTION PLANNING
ALGORITHM
FOR GENERAL
ROBOT MANIPULATORS
Tom&s
MIT
Lozano-P&ez
Artificial
Intelligence
545
Technology
Cambridge,
Abstract:
This paper
configuration
space,
lators.
We describe
tors made
presents
to plan
This
up of revolute
slices,
obstacle
free space
and efficient
motions
an implementation
joints.
an n degree-of-freedom
dimensional
a simple
collision-free
recursively
outside
obstacles
are approximated
built
leads
up from
USA
the last
five to eight
years,
14, 16, 17, 19, 201. But,
within
few of these
methods
powerful
for
by sets of n-
1
scarce
enough
for manipulators
of industrial
algorithms
slices.
planning
approximation
of the
for revolute
7, 121.
obstacles.
but
Brooks’s
is fairly
This
paper
gorithm
presents
for general
sional
polyhedral
tages:
degrees
of a new motion
moving
manipulators
obstacles.
it is simple
few degrees
an implementation
robot
The
of freedom,
of freedom
(including
cluttered
cles.
An example
path
obtained
rithm
is shown
in Figure
1.
environments
geometric
one of the capabilities
[151. Task-level
in robotics.
to achieve
pin-A
programming
a task
complex.
rather
of task-level
than
motions
the
previous
motion-
reasonably
general
and
or more
degrees
[2] h as demonstrated
basic
algorithm
approach
to the method
algorithm,
constraints
most
of freedom
impressive
17 , on the other
of the
described
however,
popu-
hand,
described
by Faverjon.
Many
especially
goals
and the free space representation.
general.
it can
is
of research
required
such
commands,
as “Insert
such
as “Move
to 0.1,0.35,1.6”.
The
path
a specified
configuration
that
obstacles.
quite
Note
a bit
whether
lision
that
this
than,
a known
avoidance:
starting
avoids
harder
[l, 4j.
obstacles
problem,
motaon-plannzng
from
robot
Motion
problem
a known
robot
path
from,
problem:
or a path
different
goal
set of stationary
different
detectzon
configuration
is also
modifying
a known
is significantly
planning
is to find a
to a specified
would
from
and
detecting
cause
so as to avoid
a colobstacle
on-lzne
unforeseen
16, 9, 10, 11:.
Although
general-purpose
task-level
away, some of the techniques
ming
are relevant
ple, increasing
to existing
emphasis
for off-line
of the manipulator
motion
planning
specifying
plays
robot
paths
A great
time
role.
for welding
for these
deal of research
I SCIENCE
robot
and the task.
There
programmers.
In many
of these
and tedious
algorithms
is. for exam-
process.
could
using
techCA11
applications
is a good
complex
many
prograrn-
users on developing
Arc welding
along
is still
for tdsk-level
applications.
major
by human
a central
paths is a time-consuming
practical motion-planning
programming
programming
developed
robot
among
programming,
models
626
with
cofliszon
the
form,
configuration
collisions
years
niques
in its simplest
robot
example;
three-dimensional
The development
reduce
significantly
Figure
hand.
applications.
has been devoted
of
the
to the motion-planning
1.
A path
obtained
for all SIX link \ of a I’rlma.
using
the algorithm
plus
dr+t r ibed here
of
the treatment
for a ma-
motions
is
algorithm
robot programming
robot
‘2,
results.
obsta-
and the task
commands,
robot-level
joints,
three
of the algo-
of the manipulator
the
efficient
and
with
and
an implementation
to specify
new and more
three
of only
enough
are particularly
many
polyhedral
task-level
related
of the present
of three-dimensional
both
Faverjon’s
The
algorithms
of advan-
having
is one of the principal
ability
in terms
in hole-B”,
manipulators
to achieve
al-
manipulators
plan collision-free
models
required
It is the
a number
for
non-convex
from
planning
three-dimen-
manipulators),
and
to automatically
given
with
redundant
with
The ability
has
it is fast
it can deal
deal
nipulator
algorithm
to implement,
among
are
[2, 3, 5, 7, 8, 12, 13,
are simple
up of revolute
1 know
with
algorithm
simple.
here is closely
the details
that
e.g.,
Practical
made
robot.
manipulators
appealingly
1. Introduction
to be practical.
lar type
one dimensional
to an efficient
of the configuration-space
manipu-
for manipula-
The configuration-space
02139
problem
using
for general
of the algorithm
manipulator
representation
algorithm,
Mass.
Laboratory
Square
a three-fingered
are
The
approach
12, 131 in that
respect
the
from
taken
in this
it involves
exact
algorithms
quantization
algorithm
quantizing
such
approach
lends
is similar
to that
of 17, 8,
angles.
It differs
in this
joint
as [17. 19j.
itself
On the
readily
other
to efficient
joints
are known
as JTFC JOZ?~!$.
hand,
computer
implementation.
The
purpose
general
of this
manipulators
most
practical
cases.
than
in computer
numerical
fact,
it is worth
that
motion
relatively
motion
renderings
can benefit
planning
for
efficient
planning
of three
cases.
in
should
dimensional
there
are many simple
hardware
support.
In
from
in the examples
to compute
as to compute
that
and
why
In both
that
noting
time
simple
computing
graphics.
computations
the same
is to show
be both
I see no reason
be any less practical
solids
paper
can
in Figure
the hidden-surface
1 it took
displays
about
Figure
list
2.
Slice
dimensional
that
the full obstacle
Slice Projection
completely
of a moving
specify
figuratzon
space
object.
The
the
of joint
angles
other
Cartesian
hand,
of solutions
urations
Motion
planning
free space.
The
II may cover
trrlzation
only
characterizations
is built
we mean
of links
restrict
the position
industrial
simple
The
space
obstacle
imations
I4
a range
.4 slice projection
by a range
and
of values
an n -
measures
either
displacement
Then,
that
only
ranges.
union
we have
This
a union
process
the slice projection
zero dimensional
slice projections,
of n -
2 dimensional
of one dimensional
is illustrated
graphically
can be continued
volumes
(points)
one more
remain,
but this
is
wasteful.
a simple
two-link
planar
are q1 and 92. C-space
two dimensional.
The
The
ranges
must
and
a value
q1 E [a,/?],
since
top
bottom
2 degrees
projection
set of linear
if there
exists
and
right
(shown
joint
paare
of a C-space
{RE}
ranges
a value
call it c, for which
of a configuration
actual
of 92 rauges
slice
for 92.
of 42, call it w,
(c, w) E C, then
space
with
3b, for the two link manipulator
3a. The
the
that
whose
for such a manipulator
w is in
2).
in Figure
do the left and
is some
be such
in Figure
manipulator
obstacles
one dimensional
C for q1 E [a,@;
obstacle
configuration
edge
edge.
space
dark)
is the surface
of the diagram
The obstacles
obstacles
is illus-
and obstacles
coincide
shown
of a torus
(0 = 2~),
are approximated
as
as a set
of 41. The resolution
for a set of values
is
the qr axis.
d]OJIg
displacement
obstacle
only
in
is defined
of the C-space
q = (ql, .
parameter,
, qn) denote
each
a)
of which
joints)
or linear
b)
{q j q3 E [a,/3 } be the set
Let
qJ t
113,
of C
C.
(for revolute
joints).
projection
parameters
the
approx-
(differing
C-space
Let
m
:a,8
and
let rJ
be a projection
Figure
Two
= (Sl.“‘)
the slice projection
of the same joint
C-
slzce proJec2zons
intersects
qt is a joint
for which
such that
Qn..‘..qn)
of values
of
1 dimensional
are
are,
We represent
configurations
volume.
each
angular
of n-
to be fixed.
grippers)
an n dimensional
in a slice
of an n dimensional
(for prismatic
of all configurations
operator
configuration
for one of the defining
where
(see
We
n joints
with
n joints.
with
parameter)
I dimensional
a configuration,
se-
joints
is, linear
A representation
By simple
manipulator
Let C denote
of n dimensional
joint
step
trated
approximate
parallel-jaw
of n - 1 dimensional
n - 1 dimensional
of a single
2. Note that
a charac-
manipulators).
for a manipulator
volumes.
for a manipulator
the value
that
one of the R, (Figure
methods
by the
and so on, until
volumes,
until
of a number
range
of a
An approximation
sense.
obstacles
represents
without
or prismatic
(not including
of C by the union
Each
of the robot’s
for example,
and error
of simple
a
by one-
approximation
obstacle.
as the union
be approximated
Consider
config-
of a non-branching
revolute
zero of a simple
in this
C-space
composed
of link
n-dimensional
But.
into
represented
2). Each of the n - 1 dimensional
in Figure
rameters
J+ce space
collisions.
how to compute
kinematics
of the
those
for sample manipulators.
by either
on the
obstacles.
to tridl
of the
manipulators
manipulators
general,
we show
manipulators
connected
181 for a treatment
represent
can
C-space
each for a different
(Figure
slice projections
It is possible
be complete,
one is reduced
in turn,
its configuration
characterization
may not
paper
effector,
because
into
cause
an explicit
of the free space
manipulators
quence
would
Con-
constitute
kinematics.
obstacles
parameter
is a configuration
end
workspace
that
of a moving
a configuration
of the free space
of the free spate.
In this
robot’s
of the C-space
requires
a subset
space
inverse
that
characterization
to find a path.
Most
object
object.
manipulator
joint
C-spacr
to be the complement
on the
of configurations
constitute
in the robot’s
of the moving
is defined
point
of the
to a robot’s
the obstacles
set of parameters
of a robot
parameters
‘3. 4; 5. 13. 14,. These
space
space
a robot’s
do not usually
multiplicity
to map
Therefore,
is any
of every
is the
set
The
position
(C-space)
a configuration.
space.
object
obstacle
are in turn
is a conservatave
a slice projection
of an n dimensional
slice projections,
configuratzon
that
slices.
Note
segment
The
of a three-dimensional
slices
in the figures
the paths.
2. The Basic Approach:
Projection
of two-dimensional
q3-1*q3-1
of the obstacle
.“‘)
C for values
qn)
of q3 c
0, /3] is
(a)
3.
Two
dimensional
link
revolute
manipulator
and
with
obstacles
one dimensional
slice projections
(shown
dark).
The
positions
manipulator
are shown
in the
input
of the
C-space
approximated
obstacles.
(b)
by a list
initial
space
of
and final
and
the
C-space
The
definition
example
above.
of slice
joint
projection
3 aGo\e
is illustrated
i< called
the
in Figure
.silrf ~oznt while
2.
In the
the other
I’or general
can be c onst rutted
manipulators
with
z links,
the
configuration
space
as follows:
PERCEPTION
AND ROBOTICS
/ 627
To compute
1
in Section
C-space(z):
Ignore
links
beyond
qz by considering
determined
link
1.
rotating
Find
link
by the current
the
ranges
2 around
value
of legal
the positions
values
of
of joint
z
presence
of objects
n then
specified
value
Observe
else
resolution.
Compute
the basic
the
legal
C-space(z
computation
of legal values
of the previous
sample
of qz at the
range
I
1) for each
of these
of qz.
ranges
that
the ranges
stop,
joints.
to be done is that
for a joint
This
parameter
computation
of determining
given
ranges
of values
is the subject
of Section
computation
calls for a re-
3.
among
polygonal
vertex
of one
cursive
tion
recursive
data
nature
structure
branching
legal joint
parameter
leaves
joint
of the
tree
parameter
n.
no descendants
tation
range
for each joint
is divided
of legal
with
~4. 5;.
the
boundaries
the first
those
edge
increase
of storage
are very
built
Contrast
complex.
by exercising
(Figure
The
intersection
by their
IS cleaner
and
interactions
that
These
can be
of the
Figure
Contact
5.
projections:
rotates
configurations
for the robot
C-space
leaves
first
key step
projections
in our
leads
to a recursive
represent
legal
ranges
points.
The
existence
between
point
edges
the edges
values
joint
be computed
obstacles.
of one joint
parameters.
That
parameter,
We will
by considering
one
dimensional
is. determining
given
ranges
the
range
of values
how these
the case of planar
slice
revolute
of
for all
ranges
may
manipulators
and obstacles.
Assume
dimensional
at known
joints
slice
projection
and
Note
we assume,
values.
see in Section
an obstacle.
628
joint
are fixed
configuration
k: a revolute
that
that
at szngle values
3.3 how to relax
is guaranteed
/ SCIENCE
that
rather
this
of the first k - 1 links
This
joint,
is the free joint
the
previous
for now,
than
ranges
restriction.
be safe, that
by the recursive
joints
that
the
of values;
We require
for a oneare fixed
that
must
relative
detcrmrne
i\ in Figure
5
that
the
can
be done
endpoints.
obstacle
vertices,
the position
be rotated
around
joint
at the contact.
angle
the joint
The orientation
required
outside
that
they
computation
Therefore,
is that
of the
that
constraint
the
two edges
contact
edge.
revolve
trivially
the
given
the potential
Since
all that
of the
defined
link edges
computed
vertex
are typically
direction
value
simply.
the contact
about
is on the right
to
of the edge.
of the link edge
by the
edges
we know
remains
y coordinates
can also be tested
forming
of the polygon
there
involving
of the endpoints
Polygon
objects,
of T and y coordinates
position
is,
point.
the endpoints
for contacts
that
of the
edge.
r and
the
be outside
this
line of the edge.
in a counterclockwise
the outside
both
reaching
- the
not just
- the orientation
- for non-convex
that
Three
be compatible.
must
of the contact
the range
Note
constraznt
it lies between
by ensuring
condition
constraznt
in-edge
edge segment,
can be tested
is on the
are within
the edge
is, no link intersects
we saw
point
and the
intersection
is a necessary
6):
prevent
the endpoints
as
can intersect
by the vertex
the potential
must
vertex
that
is whether
point
out
but it is not suficzenl.
contact
constraznt
constraint
and
be determined
previous
we will
point
traced
the finite
the contact
of link
of the vertices
and an edge segment
orzentataon
potential
slice
(b) vertex
the path
(Figure
be within
contacts
one dimensional
an intersection
hold
rFachabzlzty
the contact
This
of such
define
edge:
contact
a vertex
the edge;
The in-edge
illustrate
paths
joint.
link and obstacle,
at the
that
indicate
the edge to compute
must
not be other
for Polygons
circles
the circle
constraints
contact
is computing
some
,4s the link
link edges
and edge of link
the specified
whether
for a contact
of the
of
manipulator.
approach
of C-space
forbidden
previous
for computing
of obstacle
intersecting
line supporting
contact
The
conditions
The
infinite
must
3. Slice Projections
5.
circular
with
for qk. for example.
around
the line supporting
of the
known
circles
values
(a) Vertex
intersection
tree whose
determine
B in Figure
also follow
of these
edges
paths.
the simplicity
drawbacks
care in the implementation
nature
obstacle
The
circular
more
additional
an n-level
to known
ever!
rotates.
between
Determining
recursive
with
therefore
the link
with
requires
The
with
defining
61
4.
paths
vertices
and edge of obstacle.
structure:
its joint,
when
of qk, for example,
critical
vertex
is in contact
are
the link
Figure
about
path
are constrained
The intersection
the remaining
vertex
for qk is
obstacle
and processing
algorithms
data
to rotate
a circular
of these
the obstacle
to the link.
ranges
some
a
object.
5).
follows
values
link
when
of another
the forbidden
or some
moving
happen
an edge
of qk for which
by angles
links
very simple
this approach
1 believe
its drawbacks.
edge
in particular,
of the critical
ranges
by the
on recur-
disadvantages
of the obstacles
with
a link
contacts
with
in computing
is constrained
vertices,
the
link z < n.
for which
equations
have
step
For polygonal
extremal
is in contact
values
on the link
rotates,
the
crztzcal
with
link
to find
are forbidden
for qk will be bounded
an obstacle.
obstacles,
to identify
point
qk that
on the represen-
The
for dealing
outweighs
redured
met,hod
ranges,
defining
algorithms
The
for the
in the tree will
of some
the
4).
values
possible.
of the C-space.
Using
of slice projection
significantl!
linear
are
the boundaries
the
(Figure
All operations
and the rapid
dimensionality
but
nodes
a collision
of a representation
to dealing
accurate.
internal
produce
implementations
equations
obstacle
of the
they
of joints,
into which
(or forbidden)
is its simplicity.
represents
n is the number
are ranges
the loss of accuracy,
time with
1, where
In my implementa-
of intervals
advantage
boil down
is n-
C-space.
is the number
Many
projection
efficient
one that
depth
the
factor
because
The main
sive slice
and
to represent
I use a tree whose
and whose
of the C-space
values
touching
Therefore,
an obstacle
we need
parameter
in the workspace.
object
is in contact
assumptions,
joint
of forbidden
link k is just
The link
The
these
single
The ranges
where
2. If z =
Given
of the
of ql(. . . qa- 1.
ranges
2.
of values
by
and
must
of the
All that
is
be on the
oriented
so
the boundary.
of the edge as we
travrrce
the
boundarv.
<Given this.
the feaslbillty
of a tontact
can be
4.
Slice
The
In\ 01~ rd 1n the (on: a( t
Projections
basic
three
for
dpprodch
described
dimensional
dirnensional
A) x’ertcx
d11d fate
of rjbstacle,
and
actc>r1/ctl
tli,ll
\lt)]c
Figure
6
Given
the following
doe< not
not.
must
edges
(c) The
contact
contact
3 does
contact
the
contact
must
not
define
I satisfies
condition
and
contact:
vertex
this
must
but
1’
both
be
2 does
1 satisfies
reievant
line,
\<liur.
q
o! tht, link V.II tr ri “1\er1 obstdclC.
c’intact~
of ‘lj
gJf’dtP1
itldn
rolliilon
TdUSf’
collision
1‘~r ed(h
(Section
direction:
ran
for qk. This
values
p
c~~ntn 1
(dllif’
(~~!!l-l~~n
‘i.2).
Thr
be merged
process
nrlglfs
q
or
bsf’
this.
but
contact
angles
the
sdtibf)
\alucs
in Figure
the
Icss
together
ranges
all of the link edges meeting
and reachability
dra. there
not be any earlier
must
in-face
constraint
for testing
constraint
except
is positive
The
and the collision
axis of rotation
type
discussion
the joints
except
the potential
line (supporting
the
thfa simpler
\ve haxc
link
a range.
not just
511ce projectIon
problem
troscczction
alrcddb
under
dvf1nmg
slice
cases.
we can use a very
instead
simple
is posItioned
all the joint
Lalue
the, magnitude.
area
their
ranges
k 1n response
of Joint
value5
link
approximation
specified
to an)
represents
the
method.
for the
carteslan
out
vaiue
exact
read]])
values)
Joint
the
the specified
configuration
If Ne ’ grow”
11nh includes
it bleeps
link within
rk. of the largest
tingle
to
values)
shape
when
of
one free
Me can
of joint
(for
specified
definition
save
value.
is to replate
of computing
at the
on link
grown
problem
where’ nil
The
(for ranges
of the expanded
of the original
In most
the
within
values.
all the joints.
a single
idea
b> the
mole
placement
displacements
ulator
The
consideration
the
Any safe
proje( tion
d;scu\sed
fixed
a lln\vc
IO situations
of the
1:;. 14:
a range
of legal
joint
Assume
area.
using
any of the existing
that
from
The
orienta-
the dot products
the contact
vertex
constraint
of
to ad-
is enforced
the forbidden
angle
both
Free
slitr
are ultimately
represents
the
of 41.
of the
paths
rep1esented
There
is also
to derive
difficult
than
to check
free
of legal
but
the
Since
as sets
of linear
ranges.
not
very
near]\.
idfantica!
for. one that
Each
small
range
representa-
If we were
a great
deal
slices.
capture<
to try
of time
-4 more
iorne
for
slices.
convenient
compact.
slices
the
free space,
of q2 for some
values
a reasonabl>
individual
through
iun i+ railed
obsta-
A two dimensional
is 1n itself
space
of the C-space
of all the obstacles.
as a list of one dimensional
ranges
Learching
reprcx4entat
an intersection
difficult
approximation
to compute.
This
through
bc Lrnsted
Of course.
but not particularly
of a
axis and a sta-
to be feasible.
the complement
~111 be represented
t ion
edge).
the joint
is a bit rnore
a toniervative
is tr1vial
fired
about
joint.
the intersection
Representation
obtained
of Lalues
the face to a
see ,5 ).
Space
example.
detecting
edge segments
A and B contacts
B contacts
and
of the manipulator
rotating
those
for t!pe
that
M hich
to type
to an obstacle
require
constraint
The
rf’gion
ranges.
<licci
volumes.
region
the manip-
alold
displacement
ITI a;,
of any point
free
space
1s made
(l‘igurtb
rcprt+entat1on
up out
8)
some
vrr\
and
rninlmum
ndrrou
Free space
a <implr
to
would
compact
of the coherence
overlap
these
kernel
point
the
overlap
region’s
between
slices
up of rfqzons
from
A
a set of adjacent
of all the
kernel
slices
in a
In practice,
we
in the same
regions
to
kernels.
regions
rnt~thod
each
ranges
of common
called
are non-convex
not alwaS s be connect
from
I use 1s made
of olerlapplng
‘I‘hzs arca
I\ ret tangular
require
by the midpoint
of
Compute
slice projection
defined
displacement
within
the specified
range
each link by its corresponding
radius Ek.
the snt’pt
polyhe-
+he joint?
ranges
swept
be outside
reaching this
is in a polygon.
analogously
an obstacle
be inside
the obstacles
limited
known
:l~ce projection\
to be withIn
convert.
far ha? been
is still
<‘ contacts
(les. t h<kfree +pace is simpl\
directions.
the lact have
onf--dimensional
joint.
thus
must
prevent
reachability
belongs
a hnk edge)
complement
Our
that
case by merging
are handled
no& the vertex
must
Having
t angle5
in-face
- for non-convex
by checking
The
an orient ation
5.
(ontac
a point
5
.4 contacts
that
link.
using
at the vertex
contacts
as in the two-dimensional
Type
the
of forbidden
7.
value3
ca5e. pos-
the c)t,>t nc 16’fat c. orli’rltdti0n
each of the vectors
(for the derivation.
of forbidden
fncri define>
ranges.
point
ranges
intersec-
to b<’ fe‘d-lt)l?
can be checked
whether
with
vertices
exactly
thdrl
with
jacent
tionary
Ccjnstruct1ng
Lerticcs
The
an ob<tncle
constraint
can be enforced
11ne (supporting
7
lmk
axis.
one.
Detecting
Figure
rotates.
thrrxe c~>riitrnirlt~
of the obstacle.
The
v.ht,thtlr
tictfbrnlinr~
Mhcthcr
to forrn
is illustrated
that
of obstacle.
Jn1nt 15 char-
supporting
t,c within
are
of link
Each revolutr
As in thi, t~~~o-dlrrlerlsi~,nal
constraint
the face normal
1v.C’c ~‘I1-t ralnl.
satlsf?
rJJu>t
types
B) vertex
is the joint
t ilc c ontnc t must
algorithms
dll the
plane
possible
contact
(type
As the Joint
(cjnst rdint
tion
!Ir\t
the
of contacts
three
to
of link and edge
first
normal
po1ntc of contact.
(~~rit~~~ti
for non-convex
ObJCTtS)
lJl6
C) edge
directly
is, houever.
(a) The
this
but contact
contact
is only
an edge
1 satisfies
rontart
be reachable.
(this
circle
for a feasible
segment.
that
nf the con~ac t edge.
of a vertex
be met
be in the edge
(b) The
outFide
intersection
conditions
must
contact
the
(type
whose
( 1r( I(’ with
tMo candidate
types
The
over
There
and face of link.
of rotation.
1n a plane
of tllii
dre three
t>‘pe B contacts
b:, ~11 d\is
trrl( (8 circle\
3 carries
obstacles.
polyhedra.
of obstacle
I,ct 11~consider
and
there
difference
bet\vecn
(type
in Section
manipulators
one significant
three
Polyhedra
for moving
along
points
and so points
able by a straight
between
points
line.
within
its slice to the edge of the
with
a straight
within
There
the region
is, however.
the region:
kernel
and
move
connect
lme.
PERCEPTION
AND
ROBOTICS
/ 629
To search
representing
where
for a path
the nodes
mon
neighbors
with
each
each link records
at the common
regions
indicate
regions
are
for these
boundary
with
com-
a set of links
the area of overlap
region
requires
a regaon graph
We build
region
in the q1 direction:
primarily
of qz values
in different
and the links
Associated
regions.
points
of the regions.
are regions
boundary
adjacent
between
the connectivity
Regions
neighbors,
is stored
C-space to be recomputed.
In rapidly
changing
environments,
it may
not be appropriate
to compute
the complete
C-space since only small
sections
have
of the C-space
The
to
path
hcuristrcs
to subset
and
three
lrnks. the end-effector
path
arc chosen
and
bounding
to be the
Having
goal.
of decoupling
compute
only
of the origin
First
box for the
the
computed
plan
rest
The origin
closest
in free space
points
such
degrees
two
a path,
(the
last
and goal for this
there
to the
remains
actual
finding
the actual origin (resp.
This strategy
has the
of freedom.
of the C-space
simple
for the first 3 links
of the manipulator
C-space between
goal) of the path.
the portion
using
a path
and the load).
found
pdt h\ III the full-dimonsional
goal) and the origin (resp.
effect
1 was
the C-space:
the link.
origin
be traversed
in Figure
the range
with
a simple
will ever
shown
For all these
bounded
paths,
by the joint
we
values
and goal configurations.
a)
7. Discussion
The main
advantages
to implement,
dom,
8. (a) R e g ion definition
Figure
regions
are the region
Region
graph
indicate
the
for two link C-space.
kernels.
corresponding
existence
The shaded
to the regions
of a common
The rectangular
area shows
in part
boundary
Rx.
region
A. The link labels
q1 and/or
in the
it can deal
including
(b)
q2
directions.
regions
only
at the 0 = 2n bound-
algorithm,
is bounded
and
an interactive
above
below
by obsta-
shown
but they
with
could
many
obstacles.
obstacles
floating-point
are already
quite
system
cluttered
plan
operations
a bit faster
en-
wall-clock
a path
for the
on a Symperformed
by carefully
(on-line
of free-
of freedom
10 is six seconds
be improved
programming
degrees
The total
and then
3 and
it is simple
few degrees
and it can deal with
in Figure
times
here are:
with
having
polyhedral
3600 Lisp Machine
have q2 neighbors
else the region
manipulators
the C-space
These
described
manipulators
manipulators,
example
software.
By construction,
for
and non-convex
to compute
two-link
bolics
ary, anywhere
with
redundant
vironments
time
of the algorithm
it is fast
in
re-coding
than
the
a human
using
or off-line).
cles.
In general,
each
n - 1 dimensional
of joint
values.
structed
seen that
neighboring
we could
two dimensional
dimensional
C-spaces
range
connectivity
among
connectivity
is determined
nels in neighboring
incrementing
exists.
graph.
This
by considering
the two dimensional
the region
is associated
as neighbors
searching
containing
graph
that
slices
for three
represent
(Figure
9). The
region
is, slices
obtained
When
kerby
overlap
the corresponding
obtained
for
the
between
link in
to n dimensional
extended
n - 2 joint
In
regions
parameter.
with
is readily
are con-
Instead,
and
overlap
slices,
of
by joining
dimensional
in the region
the first joint
one of the first
regions
parameter
by detecting
method
regions
so on
two
joint
regions
or decrementing
decrementing
Path
first
two dimensional
the area of overlap
the region
slices
these
and
build
as a list
are a list of ranges
slice-projections.
dimensional
regions,
of the
slices
two dimensional
three
we simply
of values
is represented
one dimensional
construct
neighboring
each
slice
and one dimensional
We have
by joining
principle,
n dimensional
slices
by incrementing
parameters
used
or
to define
slice.
is done
by an A’ search
the start
point
in the region
to the region
graph
containing
from
the goal
point.
Figure
10.
tween
start
(a) Regions
(1)
and
for example
goal
in Figure
(4) configuratrons
3 (b) Path
(c) Some
found
be-
intermediate
configurations.
The
main
tions
introduced
legal
paths
cution
Figure
9.
can have
Region
connectivity
for three
dimensional
slices;
regions
bound
in q1 direction.
neighbors
for building
believe
more
Having
paths.
630
built
Changes
a C-space,
it may
to the environment.
/ SCIENCE
be searched
however.
repeatedly
will cause
number
performance
that
time
enough
in many
applications
than
for different
In fact:
the planning
times
parts
required
to perform
hidden
of the
motion
algorithm
off-line
will probably
surface
that
enough
automatic
approximato miss
growth
last
in exe-
drawback
the worst-case
of degrees
shows
and simple
interactive
This
planner:
in the number
of this
can be fast
effective
joints.
the
the algorithm
and the rapid
of robot
in any general
are:
may cause
environments,
will be exponential
The
the C-space
the
inherent
algorithms
6. Heuristics
with
tight
algorithm
of the
by the quantization
in very
time
probably
disadvantages
of freedom
motion
planning
19
planning
for practical
motion
is
time
use.
programming
of robots.
be on the order
of the times
elimination
in graphics
I
will be
systems.
Acknowledgments.
ficial
Intelligence
Support
part
for
by
the
from
System
Projects
NCICiO14-85-K-0214
of Navnl
is also
Research
by
NSF
Intelligence
research
Office
The
Young
in
by
Research
in part
NOOO14-82-K-0494.
Arti-
is provided
in part
of Nnvnl
nnd
at the
Technology.
of
Foundation,
under
Presidential
done
Institute
NO0014-82-K-0334
contract
an
research
Development
Agency
rend
under
supported
describes
Mnssnchusetts
Artificial
the
Research
report
of the
Laboratory’s
R grant
Advanced
tracts
This
Laboratory
by
con-
the
author’s
Investigator
the
Office
research
grant.
Bibliography
1. J. W.
Boyse,
“Interference
2. R. A. Brooks,
Place
Detection
Among
of ACM, Vol. 22, No.
Comm.
faces”,
“Planning
Operations”,
Solids
1, Jan.
Collision-Free
Motions
Sur-
for Pick-and-
J. Robotacs Research,
Intl.
and
1979.
Vol.
2, No.
4,
1983.
3. R. A. Brooks
and
in Configuration
Eighth
Int.
Joint
on SMC, Vol.
MIT
European
Published
Rep.
791, May
Avoidance
by MIT
7. B. Faverjon,
Space
Robotics,
Atlanta,
8. L. Gouzenes,
Press,
1985.
Also
Proc.
806, Oct.
1984.
of Freedom”,
1984.
in Multi-Robot
Research,
Cambridge,
Avoidance
March
Polyhedra”,
Six Degrees
Systems”,
Proc.
August
1984.
Kyoto,
Mass.
Using
of a Manipulator”,
an Octree
Proc.
in the Con-
IEEE
Intl.
Conf.
1984.
“Strategies
for Mobile
Mar/Apr
AI Memo
with
Robotics
“Obstacle
figuration
Problems
Planning
Symp.
in Proc.
IEEE Trans.
Also
for Moving
Also MIT
“Collision
Intl.
1983.
2, 224 -233,
Detection
“Motion
Algorithm
Rotation”,
1983.
A I, 1984.
AI Tech.
Second
No.
“Collision
6. E. Freund,
with
on A I, Aug.
684, Feb.
5. B. R. Donald,
MIT
Conf.
Conf.
“A Subdivision
for Findpath
SMC-15,
AI Memo
4. J. F. Canny,
T. Lozano-Pkrez,
Space
for Solving
and Manipulator
Collision-Free
Trajectory
Robots”,
J. Robotacs
Intl.
Research, Vol. 3, No. 4, 1984.
9. N. Hogan,
Amer.
10. 0.
“Impedance
Control
Khatib
ulators
and
jectory
Italy,
“Feedback
Allerton
An Approach
Conf.,
and
“Dynamic
Proc.
Avoidance
of Ill., Oct.
“An
Int.
Conf.
of ManipProc.
Third
Control”,
Proc.
1978.
Obstacle
Univ.
Control
Environment”,
Sept.
F. Germain,
Planner”,
to Manipulation”,
1984.
in a Complex
Udine,
11. B. H. Krogh,
12. C. Laugier
June
J. F. Le Maitre,
Operating
CISM-IFToMM.
21st
Control:
Conf.,
1983.
Adaptive
Adv.
Collision-Free
Robotics,
Tra-
Tokyo,
Sept.
1985.
13. T. Lozano-Perez,
“Automatic
1981.
14. T. Lozano-Pbrez,
proach”,
Feb.
Also MIT
of Manipulator
“Spatial
AI Memo
Planning:
1983.
Also MIT
Vol 71: No.
AI Memo
“Robot
Vol C-32,
605, Dec.
July
Transfer
No.
606, Dec.
1983.
10, 681
1980.
Space
Ap-
No. 2, 108 - 120,
1980.
Proceedzngs of the IEEE,
Programming”.
7, 821 - 841,
ShlC-11,
A Configuration
IEEE Trans. on Computers,
15. T. Lozano-Perez,
Dec.
Planning
IEEE Trans. on SMC, Vol.
Movements”,
- 698. Oct.
Also
MIT
AI Memo
698,
1982.
16. T. Lozano-PCrez
Collision-Free
and M. A. Wesley,
Paths
Among
the ACM. Vol. 22: No.
17. C. O’Dtinlaing.
Approach
and C. K. Yap,
Comm.
of
1979.
“Retraction:
15th ACM
Planning’.
for Planning
Obstacles”1
10. 560 - 570. October
M. Sharir.
to Motion
“An Algorithm
Polyhedral
STOC.
A New
207
220,
1983.
18. R. P. Paul.
Robot Manipulators.
\1IT Press.
19. J. Schv.artz
and
the> Pinno
M. Sharir.
“On
1981.
ILIover’s
Problem
Courant
Inst. of Math. Sci. Tech.
Rep. 41. Feb. 1982.
20. S. Udupa, “Collision
Detection
and Avoidance
in Computer
trolled
Manipulators”,
bridge,
1977.
Proc.
Fifth
Intl.
Joint
Conf.
II”.
Con-
AI, Cam-
PERCEPTION
AND ROBOTICS
/ 63 1