Motion Planning with Timevarying Polyhedral Obstacles Based on
Graph Search and Mathematical Programming
C. L. Shih T.T. Lee
Center for Robotics and Manufacturing Systems
University of Kentucky
Lexington, Kentucky 40506
ABSTRACT
This work treats a method for path planning of a
point robot moving among polyhedra1 obstacles with
piecewise constant translating velocity. By representing
the obstacles as polyhedra in the space-time domain, a
collision-free, minimum-time path is obtained using
graph search and linear-programming. The planner
views the space-time configuration of free space as
disjoint polytopes where each point in the space-time
domain is mapped into a unique polytope. The
planner searches connected polytopes between the start
and goal polytopes that satisfy the speed and time
constraints. A near-optimal trajectory is determined by
constrained optimization. In the proposed approach,
the obstacle is allowed to move faster than the point
robot, and the methodology can be easily extended to
motion planning in higher dimensional spaces.
1. Introduction
Path planning method that consider moving
obstacles and multiple robots are of great important for
applications. If robot motion could be easily planned to
accommodate moving obstacles, multiple devices
could be operated safely in the same workspace by
planning a task for one robot with all other motions
regarded as obstacles. The abiliy to avoid moving
obstacles leads to increased mobility of the robot for
navigation. It also results in higher productivity for
manufacturing automation. By allowing the obstacles
to move with piecewise constant speed and faster than
the planned robot, we can consider a larger class of
applications. For example, suppose two robots are
working together and then move away after the task
has been completed. In such a case, the robot can be
considered as an obstacle which can move
independently, and may move faster than other objects
in the workspace.
Collision-free path planning for robot motion has
CH2876-1/90/oooO/0331$01.00 0 1990 IEEE
W. A. Gruver
been studied from several different viewpoints
[5,7,8,10,11,12,13]. While previous work on this topic
has been mainly concerned with algorithms that
generate a collision-free path for stationary obstacles,
these techniques may not be applicable to time-varying
environments. For example, the visibility graph
approach [8] cannot be utilized directly with moving
obstacles because the free space changes. Penalty
functions [7] provide a method for local collision
avoidance but suffer from local extrema. Moreover,
the choice of a penalty function is not obvious.
Methods for path planning with moving obstacles,
however, differ from those with stationary obstacles.
Previous work directly dealing with moving
obstacles includes the following. Reif and Sharir [91
investigated the computational complexity of planning
the motion of a body in 2D/3D space clustered with
moving obstacles. They showed that 3D dynamic
movement is intractable even when the body has only
a constant number of degrees of freedom of
movement. Canny and Reif 111 showed that motion
planning for a point in the plane with a bounded
velocity is NP-hard, even when the moving obstacles
are convex polygons moving at constant linear
velocity without rotation. Kant and Zucker [61
proposed a two-level hierarchy for planning collision-
free paths in time-varying environments in which a
higher planning level specifies a desired path in space
domain and the lower level realizes a trajectory by
planning its velocity along the predetermined path.
Their approach results in a significant reduction in the
complexity of the problem; however, a limitation of
this approach is that it permits only velocity
alternations, not path alternations. Erdmann and
Lozano-Perez [2] proposed a planner for moving
objects that constructs a configuration space each time
an object in the scene changes its velocity. Their
method is based on stacking two-dimensional planes,
where each plane represents a configuration space at
some time. Fujimura and Samet [31 presented an
approach to incorporate time dependence in an octree
hierarchical structure to represent the time-varying
331
polygonal obstacles. Recently, Fujimura and Samet [41 where
made use of the "accessibility graph" to find a time- q(t) = q(t0) + v (t-to) , to5 t I tl
minimal path for a moving point in a 2D time-varying
environment filled with convex polygonal obstacles Then we have
which move at constant speed.

In this paper, we derive a method for collision-free

p(t) = { XE R r : NT(x- v t) + d - NT(q(to)- v to) I0 1 (3)
path planning with stationary obstacles and obstacles
moving with piecewise constant velocities. By By augmenting the state,
augmenting the state vector with time as a new state,
the moving obstacles are represented by polyhedra in
the space-time domain. For a polyhedron translating at
z =[: 3 eRr+l

a constant speed the volume swept by the polyhedron and rewriting

in the space-time domain becomes a polyhedron.
When a polyhedron rotates, its trajectory is no longer a
to< t I tl
polyhedron in the space-time domain. For this reason,
we restrict the motion of objects to be simple
as
translations. The free space is then partitioned into a
collection of polytopes which can be easily checked for -t+toSO, t - tlS 0,
existence by linear programming. Then, a family of
feasible piece-wise linear trajectories can be generated we obtain
by searching for connected polytopes using graph
search techniques. Velocity constraints for the planned P(v,to.tl) = { E R'+1 : N ( v ) ~ z+ d(v,to,tl) I O } (4)
robot become additional constraints for the graph
search. Finally, a near-optimal trajectory is determined where
by linear /nonlinear programming. The performance
functional can be minimum time, fixed final time, or
shortest distance. The proposed approach can be easily
extended to spaces of dimension three and higher.

d(v,to,tl)T = [(d - NT(q(to)-vto) )T, to, -ti] E R'X(m+2).

2. Time-varying Polyhedra and Free Space Polytopes

A stationary polyhedron P with m faces can be
represented by the set
P = { x c R r : NTx + d 5 0)
where
N = [ n l , ... ,nm] E RrXm
dT= [dl ,...,dmIT=[-nTpl, ... ,-ngpJ,
Thus, as a polyhedron with m faces in x-space (Rr)
translates at a constant speed without rotation, the
volume swept by the polyhedron in the space-time
domain becomes a polyhedron of (m+2) faces in z-
space (Rr+l). The stationary polyhedron is a special
case of the object with zero speed on the infinite time
interval. If the speed of the polyhedron P is piecewise
constant, we represent the trajectory of P by several
d E Rm polyhedra in z-space as l'(1),P(2);-, where

N is a r X m matrix whose ith column ni is the = {zE R'+l : N(VdTz + d(vi,ti-l.ti) S 0

P(i)(Vi7ti-1,ti) } (5)
outward normal unit vector of the ith face of the
polyhedron; d is an m-dimensional vector whose ith and vi is the constant velocity &+ween ti-1 and ti. It is
element is the magnitude of the vector from the origin noted that p(i) and p(i+l) are touching each other at
perpendicular to the ith face measured in the negative time ti,
ni direction; and pi (i=l;-,m) is a point in the ith face
of P. The set Q outside of a single polyhedron P can be
represented by

The constant translating polyhedron P(t) can be Q = {XE Rr : max(NTx + d) 2 0 }

represented by (6)

where max(y) is defined as the maximum value of

P(t) = { XE R' : NT(x-q(t)) + d I 0) (2) elements in vector y. We observe that the set Q can be
divided into a number of disjoint polytopes (Fig. 1)

332
 -
Q = Q1 U-.UQ,, , interiorQ) n inte.rior(Q,)= 0,
for i#j Therefore, to locate a free-space polytope C(i1,*-,in)
containing a free-space point x becomes a simple task.
and
We remark that the above results are also applicable
a =( X E R ~ : A T X + ~2 0 ) . i = l , .... m (7) to represent and determine free-space polytopes in z-
space. To check for the existence of a nonempty free-
where space polytope C(il,-*,in), it must be established that
Ai = ni[l,. ...1] - [nl,. . .,ni-l,Oml,ni+l,-.
.,nm]E R- there is a solution x in Rr satisfying the linear
inequality constrained problem as shown in (8). This
by= di[l,.*.,1lT- [dl,...,di.l,O,di+l,...,dmlT, bie R m . problem can be solved directly by linear programming.

3. Planning a Piecewise Linear Trajectory

In the following, P(i) represents the ith polyhedral
object. All the symbols or parameters associated with In this section, we formulate the basic problem for
are denoted by a Superscript (i)8 for example the point robot trajectory planning with time-varying
Now we the in which the environment polyhedral obstacles. We shall follow a decomposition
is filed with n polyhedral Sets p(l)t".tp(n)t and each p(') approach [12]using the frespace polytopes C(i1;..,in)
with faces. The free space i'2 outside of n developed in the previous section to find a collision-
polyhedral sets P(1);-,P(n) is free path.

...nQb
Q=Q(l)n ) = ( U Q::))
il=l
n...n( U
m(@

i.= 1
Qt)) We define a free-space graph G whose nodes,
labeled by the n-tuple (i1,**-,in),correspond to the
= U (Q!:)n.. .n Qc") ) = C(i1,.. .,in) nonempty free-space polytopes, labeled by C(il,-.,in),
. .
11.. .,I. il,....in and whose edges connect a pair of adjacent freespace
polytopes according to the following:
Definition 1: Let C1 = C(il,-,in) and Cp= C(jl,-,jn> be
A freespace polytope in an envhnment filled with two nonempty free-space polytopes, The two nodes
polyhedral Sets p(l),.*-,p(n), each P(i) With m(i) ( i l l...,in) and (j1, ...,jn) in graph G are adjacent if
can be represented by C(i1;-,in) where ij denotes the
ith face of the jth polyhedron, and (Ci n C2) is not an empty set.

Proposition I:
C(i1,. .., in)= { x E RI : A;..,X + bi ,...in 2o ) (8) Suppose the free starting point xs is in the polytope
where CS and the free goal point xg is in the polytope Cg.
Ai,...in=[

b. .. - [ z] E
, A y ) ]E
~

R(""+ ... +"9

RTX(m'"+... +In'"') There is a collision-free motion from xs to xg if and
only if there is a path from Cs to Cg in the free-space
graph G.

An example of C(i1,ip) in a two-dimensional space
filled with two obstacles is shown in Fig. 2b). We call
C(il,--,in) a free-space polytope. In general, C(il,-,in)
is a polytope, and it may be an empty set. C(il,-;in) is
uniquely determined by the n-tuple (il,-.,in). C(il,-,in)
ProoE
Suppose there is a motion from xs to xg. Then it
traverses a sequence of polytopes. It is clear that two
consecutive polytopes are adjacent, hence there is a
path from Cs to Cg in the free-space graph G.
Suppose there is a path in the free-space graph G
from Cs to Cgl denoted by CO,CI,-, Ck, where CO = CS
and COcontains xs, and Ck = Cg and Ck contains xg.
Then Ci-1 and Ci are adjacent for i = l,-,k. Let x0 = xs
and xk+l = XR.It is clear that there exists a point xi in

this distance is greater thin zero. The index i for the n-
tuple (il,-*,in) containing the freespace point x can be
obtained by
i, = arg( max[ (NGPx+du))2 01 ) for j=1,. ..,n.
(9)
in the free space from xo to xk+l , Q.E.D.
The determination of adjacency is clearly a problem
of linear programming. Proposition 1 leads to a
method for finding a line segment, collision-free path.

333
The above definition of adjacency, however, is too
strict, and therefore requires more computation and
less freedom for intermediate points. For this reason,
we provide another simplification of adjacency for the
free-space graph G. Consider the set :
S(il,-.,in) is the set of all points x such that its distance
to the jth object in the ith face is greater than zero.
Both S(il;-,in) and C(il;-,in) are uniquely determined
by the n-tuple (il,-,in). It is also clear, for a given n-
tuple (iI;-,in), that
C(il,-..,in) c S(i1,.. .,in) c free space (10)
An example of S(i1, i2) in a two-dimensional space
filled with two obstacles is shown in Fig. 2(b).
From (lo), it follows that Proposition 1 still holds for
the free-space graph G with node S(il,-;in) rather than
C(il ,+..,in). The determination of adjacency again
involves a linear programming problem. However,
this simplification largely reduces the number of
constraints for the adjacency test from 2(m(l)+-+m(n))
linear inequalities to 2n linear inequalities. Another
significant difference between (cln c2) and (SI n S d is
that (CI n c2) C Rr-1 but (si n s2) c R r . According to
Proposition 1, the latter definition of adjacency allows
more freedom for the choice of the intermediate points
(xi, i=l;..,k) as well as the final, line-segments
trajectories. As one may expect, the number of
adjacent pairs in graph G increases.
In practical situations, we may only consider the
adjacent pairs (iI;..,in) and (jl;..,jn) in the free-space
graph G such that there is only one different element
between n-tuples (ii ;..,in) and ( j i ;..,jn). With this
property, the maximum number of adjacent nodes for
a given node in the free-space graph G is limited to
(m(l)+-+m(n)). Geometrically, this means that we
consider the path by moving around one obstacle at a
time. As a result, Proposition 1 still holds. We remark
that the adjacency test is reduced to (n+l) linear
inequality constraints. Examples of free-space polytopes
in a 2-dimensional space filled with two obstacles are
shown in Fig. 3(a). The free-space graph with node
C(il,-.,in) for the free space in Fig. 3(a) is shown in
Fig. 3(b). The free-space graph with node S(il,.-,in) for
the free space in Fig. 3(a) is shown in Fig. 3(c). We see
that the graph in Fig. 3(c) becomes more connected
than that of Fig. 3(b). After adding the one-element
difference constraint mentioned above, the graph
becomes that shown in Fig. 3(d).
334
Because we are basically working in the space-time
domain, Proposition 1 is not directly applicable to
moving obstacle environments (2-space). In order to
avoid choosing an unrealistic path, e.g., traveling in
the negative time direction or exceeding velocity
limits, the linear trajectory from zi-1 to Zi must
satisfy the following speed constraint:
(11)
We note that the inequality (11) implies that ti 2 ti-1.
Then, the free-space polytopes pair Ci-1 and Ci in Z-
space are feasible and adjacent if there exist (Xi-1, ti-1)
and (Xi, ti) such that zi-1 is located in Ci-1, Zi is located
in Ci, and inequality (11)is satisfied.
To plan a feasible trajectory from zoto zg (or xg), we
first search a feasible path in the free-space gra.ph as
follows. First, locate CO which contains zo. Then find
an adjacent node C1 of COsuch that zl is located in C1
and inequality (11) is not violated. Repeat this
procedure until a node Ck containing the goal point zg
(or x,) has been found and the linear trajectory from zk
to zg is feasible. If there is a collision-free trajectory
from zo to zg, then it will travel a set of feasible nodes
in the free-space graph. Therefore, we always can find a
solution, if one exists. The above procedure basically is
a global graph search problem. Standard graph search
techniques (A*, best-first, breadth-first, depth-first ) can
be used to find a set of connected and feasible polytopes
from the start free-space polytope to the final free-space
polytope in the free-space graph. Now, we can
formulate the final trajectory minimization problem
for a moving point robot as follows:
Given ZS, xg, and a feasible path in the search
graph G, denoted CO;-, Ck, determine zo,-, zk,
and Zk+l that minimizes the objective function
f = tk+l- to, subject to
-vmm 5 Xi - - max
ti - ti.1 , for i= 1,..., k+l.
The final trajectory is found by two steps: graph
search and constrained optimization. As a result, the
solution may not be optimal. Two different graph
paths, however, may lead to the same final trajectory.
In addition to the minimum time trajectory, other
optimality criteria, such as shortest distance and fixed
final time, can also be easily accommodated by the
proposed approach.
 We now summarize the algorithm for point robot
motion planning with time-varying polyhedral
obstacles. The input is a set of stationary or moving
polyhedral obstacles (with piecewise constant
translating speed and known trajectories) and the
initial and the final states zs and z g (or x g ) ,
respectively. The procedure is as follows:
Step 1. Represent trajectories of obstacles in the space-
time domain.
If the speed of the obstacle is constant or zero, we
represent its trajectory by a single polyhedron in z-
space according to Eq. (3). If the speed of the obstacle is
piecewise constant, we represent its trajectory by
several polyhedra in z-space according to Eq. (5).
Step 2. Locate initial and final nodes.
Locate the polytope Csand Cg that contains the initial
point zs and the final point zg, respectively. For the
case in which the final time is free (only xg is known),
the goal points in z-space lie on a straight line and
hence we may have multiple goal nodes in the graph
search.
Step 3. Graph path search.
Generate a family of feasible trajectories by searching
the shortest graph path from the starting node to the
goal node in the free-space graph G.
S t e p 4. Final trajectory optimization by linear
programming.
In the above algorithm, Step 3 (global graph search)
is most time consuming with regard to computation
for determining the first near-optimal trajectory. The
other steps are relatively easy to compute. In the search
graph, there are at most (m(l)-m(n)) nodes. Although
the number of nodes increases when the number of
polyhedra increases, most of these nodes constitute an
empty set. Hence, in many situations the number of
nodes in the search graph is far less than (m(l)-m(n)).
Furthermore, for a given node in the free-space graph,
there are at most (m(l)+-+m (n)) adjacent nodes.
However, few are feasible when the number of
polyhedra (n) increases. Roughly speaking, the
computation time increases linearly with the number
of degrees of freedom and exponentially with the
number of faces and the number of polyhedra.
Therefore, the proposed approach still suffers from
large search space size when the number of obstacles
becomes large.
4. Examples
Figure 4 shows a point robot in two-dimensional
space with three moving square obstacles A, B, and C.
Assume the initial time t=O and their initial positions
are shown in Figure 4(a). The motion of obstacles A, B,
335
and C, which have a maximum speed of fl m/sec in
both coordinate directions, are shown in the Figure
4(h). The initial position of the point robot is located at
(5,01, the goal position is located at (021, and the near-
minimum-time trajectory is to be planned. In the first
example, the point robot has a maximum speed of f1.5
m/sec in both coordinate directions, the planned
trajectory (denoted by black dots) is 4 seconds in
duration as shown in Figure 4(h). In the second
example, the point robot has a maximum speed of fl
m/sec in both coordinate directions, the planned
trajectory (marked by gray dots) is 7 seconds in
duration as shown in Figure 4(h). In the last example,
the point robot has a maximum speed of *0.5 m/sec in
both coordinate directions, the planned trajectory
(marked by small circles) is 14 seconds in duration as
shown in Figure 4(h). Seven views of the robot's
motion for these three examples at different times are
shown in Figure 4(a)-(h). As shown in the last
example, the obstacles may move faster than the
planned robot. The computation time for this example
on a Sun 3/50 workstation was about 50 seconds. The
algorithm was coded in Fortran 77.
5. Conclusions
We have presented an approach suitable for path
planning of robots with moving polyhedral obstacles.
The approach is based on establishing a proper free
space representation by polytopes. By augmenting the
state with time as a new state vector, trajectories of
moving obstacles can be represented by polyhedra in
the space-time domain. Velocity constraints for
planned motion of objects become additional
constraints for the free space graph search problem.
After a global graph search has been employed, a
constrained optimization problem can be formulated
to obtain the final near-optimal trajectory.
Most path planning algorithms reduce path
planning to a graph search problem. After a path in the
graph has been determined, a path/trajectory in
obstacle space is fixed. In this paper, a family of feasible
trajectories is generated after a global graph search has
been employed. As a result, the solution may not be
optimal. Specifically, the final trajectory is chosen from
a family of piece-wise linear trajectories. However,
once a good initial feasible path is known, other
methods 151 can be used to smooth the trajectory and
optimize the path.
Advantages of the proposed approach are (1) the
polytope representation for both objects and free space
is simple and efficient, (2) different optimization
criteria can be accommodated, (3) no discretization is
needed, (4) a near-optimal solution can be found
without exploring all the free space, (5) the ability to
perform motion planning in spaces of dimension
three and higher, (6) obstacles do not necessarily move
slower than the planned robot, and (7)the speed of
obstacles is piecewise constant. As a result, the
approach may be applied in many practical situations,
such as multiple robot motion planning. However, the
algorithm has a disadvantage that the search space
increases significantly when the number of obstacles
becomes large.
1111 J. T. Schwartz, M. Sharir, and J. Hopcroft (Eds.),
Planning, Geometry, and Complexity of Robot Motion,
Norwood, NJ, Ablex Company, 1987.
[12] C. K. Yap, "Algorithmic Motion Planning," in
Advances in Robotics 1, Hillsdale, NJ, Erlbaum Co.,
1987.
[131 S. Buckley "Fast Motion Planning for Multiple
Moving Robots," IEEE Int. Conf. on Robotics and
Automation, pp.322-326, 1989.

References

[l] J. Canny and J. Reif, "New lower bound techniques

for robot motion planning problem," Proceedings of
28th Annual I E E E Symp. on Foundation of Computer QI
Sci.ence, Los Angeles, CA, pp 49-60, October 1987.

[2] M. Erdmann and T. Lozano-Perez, "On Multiple

Moving Objects," Algorithmica, Vol. 2, No. 4, pp. 477-
521,1987.

[3] K. Fujimura and H. Samet, "A Hierarchical Strategy

for Path Planning Among Moving Obstacles," l E E E
Trans. on Robotics and Automation, Vol. 5, No. 1, pp. Figure 1: Decomposition of space outside of a single
61-69, Feb., 1989. polyhedron P by disjoint polytopes Q, 42, a,
and Q4 .
[4] K. Fujimura and H. Samet, "Time-Minimal Paths
Among Moving Obstacles," l E E E l n t . Conf. on
Robotics and Automation, pp.1110-1115, 1989.

[5] E. G. Gilbert and P. W. Johnson, "Distance Functions

and Their Application to Robot Path Planning in the
Presence of Obstacles," I E E E I. Robotics and
Aufomafion,Vol. RA-1, No. 1, pp. 21-30, 1985.

[6] K. Kant and S. Zucker, "Toward Efficient Trajectory

Planning: The Path-Velocity Decomposition," lnt. I.
Robotics Research, Vol. 5, pp. 72-89, No. 3, Fall 1986.

[7] 0. Khatib, "Real-Time Obstacles Avoidance for

Manipulators and Mobile Robots," l n t . I. Robotics I'
(a) free-space polytope C
Research, Vol. 5, No. 1, pp. 90-98, Spring 1986.

[8] T. Lozano-Perez, "Spatial Planning: A

Configuration Space Approach," I E E E Trans. on
Computers, Vol. 32, No. 2, pp. 108-120, 1983.

[9] J. Reif and M. Sharir, "Motion planning in the

presence of moving obstacles," Proceedings of 26th
Annual l E E E Symp. on Foundation of Computer
Science, Portland, OR, pp.144-154, October 1985.

[lo] J. T. Schwartz and M. Sharir, "On the Piano

Movers' Problem: I. The Special Case of Rigid
Polygonal Body Moving Amidst Polygonal Barriers," Figure 2: Free-space polytope and free-space set in an
Comm. Pure Appl. Math., Vol. XXXVI, pp. 345-398, environment filled with two obstacles. (a) Free-space
1983. polytope C h , i d , and (b) free-space set S(i1,iz) .

336
 ;I
4
3
5
a r
--
--
%p
0 1 2 3 4 5 x
(c) positions at t = 2.0

e
IAI
II
Y 1
0; L

3
y 2
1
0 1

tt-"
0

(d) positions at t = 3.0

0 0 1 2 3 4 5 x

(e) positions at t = 4.0 I1 (f) positions at t = 5.0

(c) free-space graph 2 1 4 --

3 -- 0
2--
1 -
0 q,,n 1 1 .
B 1

1 (g) positions at t = 6.0 I

Figure 3: (a) An example of decomposition of free-space

by free-space polytopes in two-dimensional space,
(b) the corresponding freespace graph, with node C(i,j),
(c) the corresponding free-spce graph with node S(i,j),
and (d) the corresponding free-space graph with node
S(i,j) and with one-element difference (neighboring)
constraint.

Kl 1 (h) trajectories in x-y space

5
Figure 4 An illustrated 2-dimensional example
consisting of three moving square obstacles, A, B, and
y 2 ;& ; C.Three point robots considered in this example have
1 a maximum speed of 3 . 5 m/sec (marked by O),
0 1 2 3 4 5 x +1m/sec (marked by e), and f0.5 m/sec (marked by 0
0 in both coordinate directions, respectively. (a) the initial
0 1 2 3 4 5 x configuration, (b)-(g)intermediate configurations
I
(a) initial positions at t=Ol (b) positions at t = 1.0 I during motion, and (h) trajectories for point robots and
obstacles A, B, and C.

337