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