Constraint Reformulation in a Hierarchical Path Planner
David Zhu and Jean-Claude Latombe
Robotics Laboratory
Department of Computer Science, Stanford University
Stanford, CA 94305, USA
A b t r c t : One of ihe mort popular approacher i o wbot path plon-
ning consirts of approrimating the ret of f r e e configuration. of the
wboi by a collection of rectangloid cells ai succerrive level* of ap-
protimation and, ai each level, rearching the graph representing the
adjacency relation among ihese cells. We have developed a r e i of new
algoriihmr which rpecifically address ihe main iwo computaiional is-
suer underlying this approach: cell decomposition and graph rearch-
ing. We have implemented a planner which incorporates these algo-
rithms and we have eoperimented with it on many eoamples. These
ezperimentr show that our planner i4 significantly fader than pre-
vious plannerr based on the same general approach. In this paper,
we focus on the planner’s cell decomposition algorithms. They use a
”conriraint reformulation” technique that consists of approximating
the obrtacles intersecting the cell to be decompored by a collection
of rectangloids and computing their complementr in ihe cell. Two
iyper of approrimations are used, “bounding” and “bounded” approz-
imations. This iechnique produces much lers cell8 than previously
proposed iechniquer, renulling in rmaller search graphs. We provide
detailed d a h on our ezperimenial results.
Keywords: Spatial reasoning, Robot motion planning, Path plan-
ning, Constraint reformulation
Acknowledgements: This research w a s funded by DARPA contract
DAAA21-89C0002 (Army), CIS (Center of Integrated Systems), and
CIFE (Center for Integrated Facility Engineering).
1 Introduction
One of the most popular approaches to motion planning is hierarchical
appron’mate cell decomposition, which was first introduced in [Brooks
and LozM~PCrez,19821, with subsequent contnbutions by other au-
thors (e.g., [Laugier and Germain, 19851 [Faverjon, 19861 [Kambham-
pati and Davis, 1986)). This approach consists of decomposing the
robot’s configuration space into rectangloid cells at successive levela
of approximation. Cells are classified to be EMPTY or FULL, de-
pending on whether they lie entirely outside or entirely inside the
obstacles. If they are neither EMPTY, nor FULL, they are labeled
MIXED. At each level of approximation, the planner searches the con-
nectivity graph representing the adjacency relation among the cells
for a sequence of adjacent EMPTY cells connecting the initial con-
figuration of the robot to the goal configuration. If no such sequence
is found, it decomposes some MIXED cells into smaller cells, labels
them appropriately, and searches again for a sequence of EMPTY
cells. The procem ends when a solution has been found, or it is guar-
anteed that no solution can be found, or all MIXED cells are smaller
than some prespecified size.
This motion planning approach presents a rather unique combination
of attractive features. It is relatively easy to implement. It is rea-
sonably efficient when the number of degrees of freedom of the robot
is small. It is “resolution-complete”. Finally, it produces “channels”
(sequences of adjacent cells), rather than individual paths. At execu-
tion time, this leaves the controller with some freedom, for instance,
to optimize dynamic behavior or avoid unexpected obstacles [Choi,
Zhu and Latombe, 19891. These features makes the approach spe-
c i d y attractive when the robot is a moving rigid object, for instance
an omnidirectional robot that can both translate and rotate.
The two main computational issues in the approach are the following:
1918
CH2876-1/90/oooo/1918$01.00 0 1990 IEEE
- The decompcsition of a MIXED cell should maximize the volume
of the EMPTY a d FULL cells rasulting from the decomposition,
in order to make it poosible to find a path (or the absence of path)
ea quickly M possible.
- The search of a connectivity graph should take advantage of un-
successful search work done at prior levels of approximation, since
most of the search graph remains the same from one level to the
next.
We have developed new algorithms that specifically address those two
issues. The cell decompositionalgorithms use a new “constraint refor-
mulation” technique, which consists of approximating the obstacles
intersecting with the MIXED cell to be decomposed by a collection
of rectangloids, and computing the complement of these rectangloids
in the cell. The search algorithms record failure conditions in a way
inspired from the dependency-directed backtracking techniques pre-
sented in [Latombe, 19791, in order to avoid making the same mistakcs
severd times. We have implemented these algorithms in a planner,
which we ran on a variety of planning problems. Taking into account
the relative speed of the computers, our planner is significantly (ap-
proximately 10 times) faster than previous planners using the same
approach.
In this paper we present the constraint reformulation technique in
detail. We also report on the experimental results obtained with
the planner and we compare them with those obtained with previous
planners. The graph searching algorithms used by the planner are
described in [Zhu and Latombe, 19891.
2 Background and Overview
2.1 Configuration space
Let A be a rigid object moving in a Euclidean workspace W = R N
among fixed obstacles B,, i = 1, ...,q . Both A and the l3,’s are closed
regions in RN. A Cartesian coordinate frame, 3 w , is embedded in
W . Another Cartesian frame, 3 d , is embedded in A. The origin of
F A , denoted by 0.4,is the reference point of A.
A configuration of A is aspedfication of the position and orientation
of FA with respect to 3 w . The configuration space of A is the
space C of all the possible configurations of A. The subset of W
occupied by A at configuration q is denoted by A(q).
The obstacles B,’s are mapped into C as closed regions denoted by
CB, which are d e d C-obstacles. These regions are defined by
CBI = {q E C / 4 s ) n B, # 0). The region Cfree = C\ u,E[l,ql
Cut
is called the free space. A collision-free path (more simply, a free
T : [0,1] -
path) between two configurations q1 and 9 2 is any continuous map
Cfree such that ~ ( 0=)q1 and ~ ( 1 =
) qz.
In this paper we only consider the case where A is a two-dimensional
object, which translates and rotates in W = R2.In this case, C is a
manifold diffeomorphic to RZx S’ , where S’ denotes the unit circle.
We parameterize a configuration in this manifold by a triplet (z, y, e ) ,
where I,y E R2 are the coordinates of od in F w , and 0 E [ 0 , 2 a ) IS
the angle (modulo 27r) between the I axes of Fw and F A .
2.2 C-obstacles in Polygonal Case
Both A and the B,’s are modeled as polygons. Under this condition,
the cross-section of each CB, at any orientation 0 is also a polygon
defined by CBi = Bi 8 A(O,O,O), where A(O,O,O) denotes the region
occupied by A at configuration ( 0 , 0 , 8 ) and e is the symbol for the
MInkowski difference [Lozano-PCrez, 19831. Each edge of this polygon
is the locus of od when A transbta at fixed orientation 8, in such
a way that an edge (resp. a vertex) of A stays in contact with a
vertex (resp. an edge) of Bi. A contact between an edge of A and
a vertex of Bi is called a Type A contact. When A rotates slightly,
the corresponding edge of CB; rotates by the wune angle. A contact
between a vertex of A and an edge of Bi is called a T y p e B contact.
When A rotates slightly, the corresponding edge of CB; translates.
The C-obstacle C B corresponding to an obstacle B is a three-
dimensionalvolume without hole which is bounded by patches of ruled
mufaca called C-facets. E d Cfacet is swept out by a straight line
segment of variable length which remains parallel to the zy plane.
A C-facet cnated by a contact of Type A (rap. Type B) b called
a Type A (reap. Type B) C-facet. Each Cfacet is comprised be-
tween two limit orientations, beyond which the contact that creates
the C-facet is no longer fessible.
The geometry of CB, when A and B are polygons, is studied in depth
in various publications, e.g. [Lozan-PCrez, 19831 [Avnaim and Bois-
somat, 19881 prost, 19891. If A and B are both convex polygons, C B
is bounded by o(ndn&3)C-facets, where n A and n&3M the number
of edga in A and 0, respectively. If A and B are non-convex, C D has
n(n:n.",) C-facets [Avnaim and Bois"&, 19881.
2.3 Rectangloid Decomposition
We assume without practical loes of generality that the range of
y i b l e values for 2 and are dosed interval. [ ~ m i n t ~ m a zand
]
[y,in, ~mo+].We represent C as a closed rectangloid
K = [zmint zmaz] x [Ymini mot] x [OB 2r] C R3
with the croessections at 8 = 0 and 8 = 2 r procedurally identified.
Let x [vi, m]x
be a rectangloid, i.e. a region of the form [zi,iz)
R the first one. This problem arises from the fact that we constrain
[el, 821 1.A rectangloid decomposition of R is a collection of
rectangloids, { ~ j } j = l , . .,.,,, such that:
1. R is equal t o the union of the 'cj, i.e. R = U;-, ~ j .
2. The ~j an non-overlapping', i.e. Vj1,jz E [l,n],j1 #
int(sjl) n int(q,) = 0.
Each rectmgloid ~j is called a cell of R in the decomposition. Two
celb sj1 and R 'a M adjacent iff their intemectionis a set of non-zero
measure in ad.
A cell ~j is classified M EMPTY, if int(rcj) n U , C B ; = 0; FULL,
if sj C uiCB;; MIXED otherwise. Given an initial configura-
tion qinit =
(zinitiyinitieinit) and a goal configuration qgoaI =
(Zgooi~ygoatr@goo1)t anctangloid decompaeition of K , {Kj}j=l ,...,n,
is admissible iff it contains a sequence of EMPTY cells K k , k =
lv-.,~, such that Qinit E ~ l 99-1
r E ~ p and
r - 11 : s k
Vk E [ I ~ P
and Rk+1 are adjacent. This sequence of EMPTY cells is called an
EMPTY channel.
Let ( x k ) k , = l , , , , , p be an EMPTY channel. Any path joining q;,,it to
qo-r and lying entirely in the interior of R k is a free path. Such
a path can easily be constructed.
A rectangloid decomposition is said to be =mi-admissible if it con-
tains a sequence of EMPTY and MIXED cells R k , k = 1, p , called ...,
MIXED channel, with the same three properties as above. A MIXED
channel may contain a free path connecting the initial to the goal con-
figuration, but there is no guarantee that this is the case.
2.4 Hierarchical Path Planning
HierartAical path planning consists of constructing successive rect-
angloid decompositions of K , until an admissible decomposition has
been generated and an EMPTY channel has been extracted. Let
'PI, 1 = O , l , ...,
denote the successive decompositions of K , with
Po = { K } . 'PI, I > 0 , is obtained from 'Pr-1 by decomposing one
or several MIXED cells, the other cells being unchanged.
'Throughout this paper, we say that two closed sets are non-
overlapping iff their interiors do not intersect.
Whenever a decomposition 'Pi, 1 2 0 , is generated, a non-directed
graph denoted by CCQl i8 constructed. The nodes of CCFl are the
EMPTY and MIXED cells in 'Pt.Any two nodes are connected by a
link iff they are adjacent. This graph is called the cell-connectivity
graph of the decomposition 'Pl. It is searched for an EMPTY or
MIXED channel. Three outcomes are possible:
1. An EMPTY channel is found. Then, the planner returns success.
2. No EMPTY or MIXED channel is found. Then, the planner
returns failure.
3. A MIXED -el is found, but no EMPTY channel.
In the third cam, the planner proceeds racunively by decomposing the
MIXED cells contained in the MIXED channel. Hence, the planner
iteratively refines the "interating" areas. In order to bound its worst-
case time complexity, the planner may label FULL every MIXED cell
that is smaller than some predefined size.
3 Cell Decomposition
The subproblem considered here is the generation of a rectangloid de-
composition {tcj}j=', ...,,, of a given MIXED cell ti. For the efficiency
of the whole planning process, the generated decomposition should
simultaneously satisfy the following two goals2:
1. The number of EMPTY and MIXED cells in the decomposition
should be reasonably small, in order to keep the size of the search
graph as tractable as possible.
2. The volume of the EMPTY and FULL cells should be large rel-
atively to the total volume of R , in order to reduce as quickly as
possible the "uncertain area".
These goals AE confiicting since one obvious way to achieve the second
god is to produce many small cells, which is in contradiction with
the cells to be rectangloids. Allowing cells of more complex shapes
makes it possible to exactly decompose the free space [Schwartz and
Sharir, 19831. However, it makes other components of the planner
more delicate to implement and eventually less efficient to run.
j2 : One simple method of decomposing a M E E D cell is to partition it
into 2'" cells of equal size, where m is the dimension of configura-
tion space (3 in our case). The overall decomposition of configuration
space obtained with this method is called "quadtree" when m = 2
[Kambhampati and Davis, 19861 and "octree" when m = 3 [Faverjon,
19861. The advantage of this method is that it leads to a decom-
position easily representable in a tree structure of degree 2m. The
drawback is that most of the time none or a few of the newly gen-
erated cells are EMPTY or FULL. The method tends to produce a
huge number of cells.
Another method described in [Brooks and Lozano-PQez, 19821 con-
sists of considering potential cuts of the cell, score them, and choose
the best. The potential cuts are chosen wherever a C-surface3 will go
through a vertex of one of the new cells generated by the decompo-
sition. The scoring function favors cuts which do not generate small
cells, i.e. the cuts closer from the mid-points of each edge are pre-
ferred. The scoring function also attempts to minimize the number of
C-surfaces intersected by each new cell, in order to reduce future com-
putations. This method has the drawback of treating each C-surface
individually and to combine the effect of the various C-surfaces in a
global scoring function. Although certainly better than a Zm-tree,the
resulting decomposition may still be far from optimal. It may also
conservatively label a cell that intersects no C-obstacle as MIXED.
We propose a new method for decomposing MIXED cells. It con-
sists of first approximating each C-obstacle lying in the cell ti to be
decomposed as a collection of non-overlapping rectangloids. The com-
plement of a union of rectangloids within a rectangloid region is also
lAnother goal is that decomposition be as fast as possible. How-
ever, since producing extra cells ultimately requires more cells to be
decomposed and larger connectivity graphs to be searched, it is worth
spending some extra time generating better decompositions.
3A C-aurjace is the infinite ruled surface that support a C-facet.
1919
a union of rectangloids, which can easily be computed. This yields a
rectsngloid decomposition of K . We call this technique contraint re-
fomuktion, aince it basically consists of reformulating the constraints
imposed by the C-obstacles into a form directly compatible with the
format of the decomposition of K into rectangloid cells.
4 Constraint Reformulation
Let K = [ z l , z 2 ] x [ y l , ~ x~ [el
] ,821 be the MIXED cell to be decom-
posed. For every i E [l, q], let CBi[s] = CBi n K .
Our planner generatea and uses two types of approximation of C-
obstacles, bounding and bounded approximations:
1. A bounding approximation of C&[K] is a collection of non-
overlapping rectangloids ' R i k ' S , k = 1,...,p, with Vk E [I,p] :
Z i k c andCBi[*] UkP1,.,,,P'Rik.
2. A bounded approximation of CB,[K] is a collection of
non-overlapping rectangloids R:,'S, k = 1,...,PI, with
U~=l,...,p~
'R:k E csi[~I*
puting the complement of uiU,
The EMPTY cells of the decomposition of K are obtained by com-
?& in K . The FULL cells are the
'R:k's. The MIXED cells are obtained by computing the complement
of Ui U, R:, in every 'Rik.
n n
BOUNDING
BOUNDED
(4 (b)
Figure 1: Bounding and Bounded Approximations
Figure 1 illustrates the notion of bounding and bounded approxima-
tions in a two-dimensional space. Figure 2b illustrates the rectangloid
decomposition of a cell K into EMPTY, FULL and MIXED celle, us-
ing these two approximations. Compared with the q u a d t m decom-
position with similar resolution IYI shown in Figure 2a, we see that
the decomposition using bounding and bounded approximations has
much smaller MIXED area and larger EMPTY and FULL area.
n n
0 EMPTY
MIXED
FULL
(a) (b)
Figure 2: Decomposition of a Two-Dimensional Cell
There are infinitely many ways to generate bounding and bounded
approximations of a C-obstacle CB in a cell K . Our method computes
"outer" and "inner" projections of CB on the z y plane, next on the x
or y axis. Then, it lifts badc the projection into rectangloid cells. It
consists of the following two steps:
1. Projection onto xy plane: The [81,8,] interval iscut intonon-
...,
overlapping subintervals [ y u , y u + l ) ,U = 1, r, r 2 1, with y1 =
81 and yr+i = 02. We denote by d' the rectangloid [z1,zz] x
[ Y l t Y 2 1 x [7u,ru+1].
For every U E [l,r], we compute the o u t e r projection and the
inner projection of C B [ K ~on
] the z y plane. These two projec-
tions, which we respectivelydenote by OCB,,[s'] and TCB,,[tc'],
are defined by:
o c B r ~ [ ~ "=l t ( x , ~ ) / 3 8E [ ~ ~ , r ~ +( xl, yl ,:e ) E cB[tcuIh
1920
T C B ~ , [ K ~=I t ( z , m e E [ Y ~ , Y ~: (z,Y,e)
+ ~ I E CBIKul).
Clearly, we have: TCB,,[lc"] C OCBzy[su].
2. Projection onto y (or z) axis: For every U E [l,r], the interval
[zi ,121 or [vi, 921, whichever is longer, is cut into non-overlapping
subintervals.
Let us assume that [x1,zz] is subdivided (a similar presentation
would be made in the case where [yi,yz] was decomposed). The
generated subintervals are [a,,a,+l], U = 1,..., s, s 2 1 with
ai = 11 and a.+i = 22. We denote by the rectangloid
[au,au+il x [yi,v21 x [ru,ru+il.
For every U E [ l , 4 we compute the outer projection and the inner
projection of o c B , y [ ~ u ]and the inner projection of ~ c & y [ ~ u ]
onto the y axis, They are denoted by OCB,[suu and TCB,[K""
respectively and are delined as follows:
ocB,[Kuu] = ( 9 / 32 E [au,Qu+l] : (Z,y) E ocazy[tcu]}
TCB,[K""] = {y / Vx E [au,au+i]: ( x , ~ E) ICBry[s"]).
Both ocBy[Kuu] and I C B , [ K ~ ~ ]are sets of intervals.
Tc&[Kuu] ocB,[Suu].
E d rectangloid [ ~ u , Q , + I x
] [b,b'l x [ T U , ~ ~ + Iwhere
], [hb'] E
0 C B y [ ~ " " ](reap. TCB,[K""]) is a rectangloid Rik (resp. RI,.) in
the bounding (resp. bounded) approximation of C&[K].
The choice of the yu's and the a,'s is empirical. Various heuristics
can be used, but most of them seem to have limited effect on the
average efficiency of the method. The only useM heuristic guideline
is to keep the three dimensions of every MIXED cell approximately
"homogeneous". Let 62, 6y and 68 be these dimensions. We say
they are "homogeneous" ifT 62 fi: 6y fi: p68, where p is the maximal
distance between O A and the points in the boundary of A .
In the next two sections, we describe the computation of the outer
and inner projections of ~ B [ Kon ] the z y plane. we propose two
different methods. The first, called "projection" method, consists
of computing the projection of the surface of CB comprised between
yu and y u + l rand clipping the subset of the projection contained in
[zi,221 X [yi, M]. The second method, called "swept-area" method,
consists of computing the "outer" and the "inner" swept areas of
A when it rotates around the reference point from orientation yu to
orientation ~ , , + l and
, growing the obstacle B by these two areas. The
projection method is preferred when the interval [yuryu+l] is small.
The swept-area method run.9 significantly faster when this interval is
large; however, it does not exactly compute ICBz,[tcu], but a subset
of it. The computation of OCBy[su"] and TCBy[tcuu]is very simple
and not desmibed here.
In the following description of the two methods, we assume that the
reference point O A is chosen in the interior of A. In fact, the choice
of O A has some impact on the efficiency of the decomposition [Zhu
and Latombe, 19891.
5 Projection Method
Principle. Let us h t assume that both A and 13 are convex
polygons. Consider the point (10,yo) in the z y plnne and the segment
((zo,yo,0) / 8 E [yu,yy+l]} above this point. If the segment pierces
a C-facet, then (zo,yo) is in CXB,,[s"], but not in I C f ? , y [ ~ u ] . If it
piercea no C-facet, then either (20,yo) is not in OCB,,[K~],or it is
in TCBZ,[~"]. The segment {(zo,yo,O) / 8 E [yu,yu+l]} pierces a
C-facet e iff (x0,yo) lies in the projection of e in the z y plane.
We show below that each C-facet comprised between yu and yu+l
projects on the z y plane according to a generalized polygon'. The
projection of the boundary of CB that is comprised between yu and
yu+i is the union of generalized polygons, each being the projection of
a C-facet. This union is a "donut" shaped region (see Figure 3), with
an outer boundary ri and an inner boundary r2. The region bounded
by ri is the outer projection OCBzY[su].The region bounded by rz
is the inner projection Ica,,[~"].
~ ~~~
'A generalized polygon is a two-dimensional region homeomorphic
to the closed disc whose boundary consists of straight and circular
edges.
 fixed orientations 41 and 4 2 from the position where p and b coincide
to the position where a1 and b coincide. The region in Figure 4 c
is the locull of O A , when A translates and rotates while the edge
segment pclz stays in contact with the vertex b.
The case of eA projecting for the orientation range [yU,yu+1]C
[+I, b]can be treated in the same way, by drawing fictitious edges
F1 and Fz from vertex b (see Figure 4 e), so that, when E is aligned
with F'1 (resp. F2), A's orientation is yu (resp. ?,,+I).

Figure 3: Outer and Inner Projections

Therefore, the projection method consists of: (1) projecting all the C-
facets (to the extent they are contained in the interval [yu,ru+l])on
the ry plane; (2) dipping the union of the projections by the rectangle
[r1.r2]x [yl,yz] and, within this rectangle, tracking rl and r2.

The projection method may be made faster by associating with each

interval [yu,yU+1] the list of all the C-obstaclem having a non-empty
intersection with the interval, and for each of these C-obstacles the
list of the C-facets that intersect the interval. If the interval gets
decomposed further, only these C-facets have to be considered. The
projection method is scient when the number of C-facets which
have to be projected is reasonably small, that is when the interval
[Tu,r"+llis small.
V
(dl

Figure 5: Projection of a Type B C-Facet

Projection of a Type B C-Facet. Let e g be a C-facet of
Type B comprised between the limit orientations 41 and 4 2 . The
contact that generates e g is illustrated in Figure 5 a. It occurs be-
tween a vertex a of d and an edge F of 0. a is the extremity of two
edges of A, E1 and E2. F connects two vertices of D , b i and b z . Since
we assumed A to be convex, O A Lles within the convex angular sector
bounded by the two half-lines supporting E1 and fi and erected from
a. The orientation 41 (resp. 42) is achieved when the edge E2 (resp.
E l ) is aligned with the edge F . Assuming that [+I, 421 C [ru, ru+l],
the projection of e g on the ry plane is shown in Figure 5 d. It is
obtained as the union of two regions, shown in Figures 5 b and c.

(d) (e)
Figure 4: Projection of a Type A C-Facet
Projection of Type A C-Facet. Let eA be a C-facet of
Type A comprised between the limit orientations 41 and 4 2 . The
contact that generates e A is illustrated in Figure 4 a. It occuls be-
tween an edge E of d and a vertex b of B. E connects two vertices of
A, a1 and al. b is the extremity of two edges of 0 , F1 and F2. O A
projects on the supporting line of E at the point p. We assume below
that p i s located between a1 and a2. (The case where p is outside the
segment Olalis treated in a very similar fashion.) The orientation 41
(resp. Q)2) is achieved when the edge E is aligned with the edge F1
(resp. Fz).Assuming that [&,&] 2 [yy1yu+1], the projection of e A
on the z y plane is shown in Figure 4 d. It is obtained as the union
of two regions shown in Figures 4 b and c.
The region in Figure 4 b is the locus of o d , when A translates and
rotates while the edge segment crlp stays in contact with the vertex b.
This region is bounded by two circular arcs and two straight segments.
The two arcs an centered at b . The smaller one is the locus of O a
when p coincides with b and A rotates from 41 to 42. The larger arc
is the locus of 04 when a1 coincides with b and A rotates from $1
to 62. The straight segments are the loci of OA when A translates at
+
Let be the orientation of A when a lies in F and the segment
is perpendicular to F. The region in Figure 5 b is the locus of O d ,
when A translates and rotates with the vertex a staying in F and
the orientation 0 ranging over [+I, $1. (If $J < 61, then the region is
empty.) This region is bounded by two circular arcs and two straight
segments. The two arcs are centered at b l and b z , respectively, and
have the same radius equal to the distance between a and o d . The
region in Figure 5 c is the locus of O A , when A translates and rotates
with Q staying in F and the orientation 0 ranging over [$J,&]. (If
+ > 42, the region is empty.)
The case of eg projecting for the orientation range [ru,yu+l] C
[ + I , 421 can be treated in the same way, by drawing fictitious edges
E1 and E2 from vertex a (see Figure 5 e and f), so that, when El
(resp &) is aligned with F , A's orientation is yu (resp. y U + l ) .
Computation of ~ C & , [ K " ] and ZcB [ K " ] . The projec-
tion of every C-facet is a generalized polygon w?h a small number of
edges - two to four straight segments and two or three circular arcs.
The union of aU the generalized polygons forms a "donut" shaped
region (see Figure 3). The contours rland rz can be extracted by
a sweepline technique ([Preparata and Shamos, 1985]), and clipped
by the rectangle [q,zz] x [yi,yz]. The projections OCB,,[K~"]a n d
lCBy[tc""] are easily computed by determining the points of rl and

1921
rz a t abwissae a.,, v = 1, ...,s + 1, and the other extremal points of
rl and r2 within each interval [ a u r a v + i ] ,U = 1, ...,a. In fact, AU
these computations can be done during line sweeping. If the inter-
val [q,t 2 ) ia decomposed into subintervals, the sweepline hss to be
pardlel to the t axis, otherwise it should be parallel to the y axis.
+
takes time O((n m) logn), where n
T h e overall sweep-lineprocao
k the number of C-facet. that intersect with the interval [rur-y~+l]
and m ia the number of intunectiona of the generalized polygons.
We know that n 5 nAnlp (see Subsection 2.2). On the other hand,
m E O(n2), but it M usually much amaller.
Figure 6: Outer Swept Areas
Generalization. If A is a non-convex polygon that uu1 be d e
composed into convex components, such that the interiors of all these
components have a non-zmpty inturection, then the reference point
can be selected within this intersectionand the above m e t h g directly
Computation of osA[r,, ~ , , + l ] . The outer swept area
applies to ead~component taken separately. o S A [ r ~ , r ~is+ai ]generalized polygon (see Figure 6). The straight
edges of this polygon are portions of edges of A(O,O,y,) and
If A is non-convex and cannot be repreaented as the union of overlap A(O,O,y,+l). Each circular edge is the locus of a vertex of A, when
ping convw( components (for instance, it is a Dohaped object), the A rotates from ru to ru+l about the reference point. The contour
above method can still be applied, but with some changes. Since the of OSA[ru,ru+l] is traced out in time O ( n 5 ) starting at the vertex
reference point will be outside some of the convex components, the that is the most distant from Od.
shape of the projection of the corresponding C-facets will be different,
but not difficult to establish.
Avnaim and Boismnnat [Avnaim and Boissonuat, 19881 described an
algorithm of time complexity O(n3 nilognAnB) for computing the
description of the boundary of C d , when both A and 0 axe non-
convex. In the case where A is non-convex, using thia algorithm firat
and projecting the C-obstaclecl on the xy plane next, would probably
be an &dent method. However, we have not implemented it.

6 Swept-Area Method
Principle. The swept-area method consists of first computing two
arean swept out by A when it rotates about its reference point from
orientation ru to orientation rU+l:the outer rwept area denoted by
0SA[-yu,.vu+t] and the inner rwepl area denoted by I S A [ r u , r u + l ] .
They are defined follows:
Figure 7: Inner Swept Areas
Computation of 1SA[7,,Tu+l]. The inner swept area
?SA[yu,ru+l] is a generalized polygon (see Figure 7). The straight
edges of this polygon are portions of the edges of A(O,O,rU)and
A(O,O,r,+l). Et& circular edge is the locus of a point obtained
by projecting the pefepence point on an edge of A, if that projec-
tion falls between the two extremities of the edge. The contour of
ZSA[-yu,ru+l]can be traced out in time O ( n i ) . The starting point
is the first intersection of a ray (any one) drawn from the refence point
with the potential edges of ISA[r,, yu+l]. (Since the reference point
is in the interior of A, it is also in the interior of ISA[ru,ru+l].)

Then, the method regards both OSA[ru,ru+l]and ISA[rU,rU+l] 7 Experimental Results

CM moving objects, which can only translate in the plane, and it maps
the nbet&.de 13 in the configuration spaces, Rz,of t h a e objects. The
mapping is obtained by "growing" B inversely to the shape of these
two objects, leading to two regions OB[7u1ru+l]and I B [ ~ u , ~ u + I ]
formally defined as follows (see Subsection 2.2):
oB[r,,rt,+i] = B e OSA[ru,ru+i]
IB[qu,ru+i] = B e ISA[ru,yu+iJ.
The Minlrowski s u m ( m p . difference) of two generalized polygons is a
generalid polygon, which can be computed in time O(n1 +n2), if the
two input polygons are convex, and O(n:n:)logn1n2, otherwise. (n1
and ng denote the number of edges of the input generalized polygons.)
We have implementeda path planner that incorporatss the algorithms
described in the previous sections. This planner is written in Allegro
Common Lisp and runs on an Apple Macintosh I1 computer. We
experimented with it on many examples including both convex and
non-convex moving objects. Eight of these examples are illustrated
in Figure 8. For each example, the figure displays a path extracted
from the generated channel. Statistics - CPU time, total number of
EMPTY and MIXED cells generated (Nrotol), number of EMPTY
cella generated (NEMPTY), and the number of EMPTY cells used
-
in the output channel (Nuacd) characterizing the efficiency of the
p h e r are given in Table 1. Additional experimental results are
given in [Zhuand Latombe, 19891.

The swept-- method returns two regions:

The first is exactly OCU+~[K"], as shown in [Lourn*PCrez, 19831.
The second is strictly included in ICBSp[(cu], which leads the overall
decompositionalgorithm to generate a set of FULL cells of less total
volume than with the projection method.
Iq practice, the swept-area method is significantly mope efficient than
the projection method when the intervals [ru,yu+l] are large. When
these intervals become small enough, the projection method is pre-
ferred because it exactly computes TCB,,[s"].
Examples 3 and 6 have been previously reported in [Brooks and
Lozano-Perez, 19821 (BLP planner), the best previous planner known
to the authors using the hierarchid approximate cell decomposition
approach. In Table 2, we give the comparison of the statistics on
these two examples with our planner and BLP planner (CPU times
are in minutes)". We should point out that for example 3 the path
generated by our planner is different from the path generated by BLP
planner, ;.e, it does not have a backing-up maneuver.
5The BLP planner was implemented on a Lisp machine, which
runs Lisp approximately 5 times faster than the Macint.osl1 I1 runs
Allegro Common Lisp (according to our own benchmark).
 Ezample 3 Example 6
Planner C P U time Ntotoi CPU time Ntotai
0.9 140 74 18 O m 5.5 293 2.5 205
5.5 293 96 36 BLP tens 644 tens 782
5 .O 218 116 29

I : 7
0.9
2.5
11.0
160
205
31 2
77
88
72
22
17
21
8 10.5 369 121 18 Nb. of cells ezample2 ezample4 ezample7 exampled
’ ours 140 218 312 389
Octre-e >500 >2OOO >SO00 >5000
Table 1: Statistics for the Eight Examples
- x

that it produces significantly better results than earlier decomposition

techniques.
We currently use the planner in a mobile robot system capable of
dealing with some un-expected obstacles. The planner is used to
construct channels using the available knowledge about the robot’s
workspace. The controller guides the robot in the channels using a
4 potential field technique. The non-anticipated obstacles are detected
by proximity sensors. This system is described in [Choi, Zhu and
Latombe, 19891.

References
Avnaim, F. and Boissonnat, J.D. (1988) Polygon Placement Under
Translation and R o f a f i o n ,Technical Report No. 889, INRIA, Sophia-
Antipolis, France.
Brooks, R.A., and Lozano-PCra, T. (1982) A Subdivision Algorithm
in Configuration Space for Findpath with Roiation, AI Memo 684, AI
Laboratory, MIT, Cambridge, MA.
Brost, R.C. (1989) “Computing Metric and Topological Properties
of Conciguration-SpaceObstacles,” Proceedings of the IEEE Interna-
tional Conference on Roboiics and Automation, Scottsdale, Arizona,
170-1 76.
Choi, W., Zhu, D.J. and Latombe, J.C. (1989) “Contingency-Tolerant
Motion Planning and Control,” Proceedings of the IEEE Workshop
o n Intelligent Robots and Systems, IROS’89, Tsukuba, Japan, 78-86.
Faverjon, B. (1986) “Object Level Programming Using an Octree in
the Configuration Space of a Manipulator,” Proceedings of the I E E E
International Conference o n Robotics and Automation, Sa11 1?ra11-
cisco, 1406-1412.
Kambhampati, S. and Davis, L.S. (1986) “Multiresolution Path Plan-
ning for Mobile Robots,” I E E E Journal of Robotics and Automalion,
RA-2 (3), 135-145.
Latombe, J.C. (1979) “Failure Processing in a System for Designing
Complex Assemblies,” Proceedings of the Sixth Internalional Joanf
Figure 8: Planning Examples Conference on Artificial Intelligence, Tokyo, Japan.
Laugier, C. and Germain, F. (1985) “An Adaptative Collision-Free
Trajectory Planner,” Proceedings of the International Conference on
For examples 2, 4, 7, and 8, we compared the number of cells gen- Advanced Robotics, Tokyo, Japan.
erated by our planner with the number of cells that a similar plan- Lozano-PCrez, T. (1983) “Spatial Planning: A Configuration Space
ner using the octree decomposition technique would generate. Based
Approach,” I E E E Transactions on Computers, C-32(2), 108-120.
on partial implementation, we obtained a conservative estimate (i.e.,
lower bound) of the number of cells generated by the octree decom- Preparata, F.P. and Sham-, M.I. (1985) Computational Geometry:
position technique. The data are reported in Table 3. Example 7 was A n Introduciion. Springer-Verlag, New York.
extracted from [Avnaim, Boissonnat and Faverjon, 19881. According Schwartz, J.T. and Sharir, M. (1983) “On the Piano Movers’ Prob-
to this paper, the planner described in [Faveion, 19861, which uses lem: I. The Case if a Two-Dimensional Rigid Polygonal Body Moving
the octree decomposition tedmique, generates more than 10,000 cells. Amidst Polygonal Barriers,” Communications on Pure and Applied
Mathematics, 36, 345-398.
8 Conclusion Zhu, D. and Latombe J.C. (1989) N e w Heuristic Algorithms f o r Ef-
We have described a new constraint reformulation technique for effi- ficient Hierarchical P a t h Planning, Report No. STAN-CS-89-1279,
ciently decomposing cells in a path planner based on the hierarchical Department of Computer Science, Stanford Univcrsity.
approximate cell decomposition approach. This technique is aimed
at minimizing the total volume of the generated MIXED cells, while
produang a reasonably small number of cells. Experiments showed

1923