Dynamic Discovery and Path Planning
for a Mobile Robot at a Cocktail Party
John S. Zelek
Engineering Systems & Computing,
S&od of Engineering, University of Guelph,
Guelph, ON, N1G 2W1, Canada
jzelek@uoguelph.ca,http://www.eos.noguelph.ca/webfles/zelek
Abstract
Sensor-based
discovery
(i.e., dynamic) path planning
is problematic because the path needs to be contin-
ually recomputed as new information is discovered.
A pmcers-based client-server approach has been succesfully deployed to solve this problem, thus permitting concurrent sensor-based map and localizationcorrection updates as well as concurrent path compntation and execution. A potential function is created by
solving Laplare’s equation (Le., a harmonic function)
using an iteration kernel convolved with an occupancygrid representation of the current free space. The path
produced (i.e., by performing steepest gradient descent
on the harmonic function) is optimal in the sense of
minimiing the distanee to the goal in addition to mini m i ~ b the
g hit+&g probabi!ity. This &ips deviate the
uncertainty iufiuence on path planning. In this paper
the approach is extended to include a specification on
the target approach: appropriately referred to as the
&g
component of the cocktail party problem. The
extension is unique in that no additional computational
complexity is necessary because of the inherent prop
erty of harmonic functions encoding more than one descending directional derivative from any single point is
exploited. Biasing the entire path produce, an unique
path:when compared to the steepest descending gradent but the arrival approach requirement may not be
met. the path is only biased when open space is detected between the current robot position and the goal.
It takes on the form of an opposite direction bias to POsition the robot before slowly changing, constrained by
a turning radius constraint, to the required approach
direction.
Keywon&- autonomous mobile robot, navigation,
dynamic path planning, harmonic functions
Introduction
Sensor-based discovery path planning is the guidance of
an agent - a robot - without a complete a priori map,
by discovering and negotiating the environment so as
to reach a goal location while avoiding all enmintered
obstacles.
The dynamic path planning problem extends the basic navigation planning problem (Latombe 1991). The
preliminaries and assumptions of the dynamic path
planning problem are as follows:
Let R be a single rigid object - the robot - moving
in a Euclidean space E, called the environment, r e p
resented as PN,with N = 2. Only 2D motion is
considered, however the concepts apply to higher dimensions, where N > 2.
Let A,...,Onbe either fixed (or moving rigid) objects
distributed in E. The positions [and dynamics) of an
object pj may or may not be known a priori. The free
space of the robot is the allowable space for navigation and is obtained by subtracting all the pi models
from the environmental space P.
Both the geometry of R,B,..., and the locations
of the p,’s in E are assumed to be approximated to
within a certain predefined degree of slack or tolerance. In addition, it is assume that the only kine
matic constraint that limits the motions of R is the
two-dimensional plane on which it navigates.
Given the above definitions and assumptions, the dynamic path planning problem is defined as follows:
Given an initial position and orientation (qatnrt =
(zi,yi,&)) and a goal position and orientation
(ggonl= (zg,
y,, 0,)) ofR in E (both defined in terms
of a global coordinate system), generate a path T
specifying a continuous sequence of positions and orientations of R, avoiding contact with the ,&’s, starting at ( z i , y i r B i ) and terminating at (z,,yg,O,). As
the robot navigates, the pi’s may be hown a priori, appear suddenly, disappear suddenly, remain mchanged, or be dynamic. It is a s m e d that R is
circular. Failure is reported if the goal is unattah
able in the a m e n t environment, or if the position of
R is lost, or if the obstacles pj’s in the environment
E cannot be monitored with &dent detail.
Typically, the orientation of the god does not necessarily need to be attained but for the cocktattail seming
problem, it is required. The orientation necessary for
initiating the robot is achieved by an appropriate rotation ofthe pIatform before commencing execution.
The cocktad serving problem is used to describe the
destination orientation requirement because it draws on
on
0-7803-5655-1/99/$10.00 0 1999 IEEE
285
a direct analogy with a crowded room of people min.
gling about with the robot playing the role of the waiter
approaching potential clients in a direction dependent
on a person's global orientation. This problem be a p
plied to other service-based applications where the a p
proach position is important.
Background
One type of heuristic algorithm for p e r f d n g dynamic
path planning is the potential field technique based
on the summation of potentials (Khatib 1986). This
technique offers speedup in performance but canpot offer any performance guarantee. In addition, these algorithms typically get stuck at local minima. Techniques have been devised for escaping these local minima such as the randomized path planner (Barraquand
& Latombe 1991) which escapes. a local minimum by
executing random walks, but this is at a cost of computational efficienq.
A complete algorithm for perform% dynamic path
planning is the D
' algorithm (Stentz 1995). D' (i.e.,
dynamic A*) performs the A' algorithm initially on
the entire workspace. As new information is discovered, the algorithm incrementally repairs the necessary
components of the path. This algorithm computes the
shortest distance path to the goal with the information
available a t any time instance. One problem with the
D' algorithm is that no allowances are made for uncertainty in the position and orientation of the sensed
obstacles (i.e., sue is also applicable for obstacles) and
robot.
An approach that approximates being a complete algorithm is called pmbabilistic planning (Barraquand et
al. to appear). Probabilistic planning is generally used
for multi-link manipulators and when the configuration
space has more than five degrees of freedom. In this
approach, the fkst step is to ei3dently sample the configuration space at random and retain the free configurations as milestones. The next step checks if each
milestone can be circumvented by a collision free path,
producing a probabilistic roadmap.
It is argued that the harmonic function potential field
can be used for complete 2D path planning provided
that its spatial extent is limited and it is integrated
with a global path planner to compensate for the imposed myopia. The presented technique makes stronger
completeness claims than the pmbabilistic approach. In
addition, in contrast to the D
' technique, the optimality properties (i.e., minimizing the hitting probability)
make allowances for the position and size uncertainties
associated with sensed obstacles.
Approach and Context
Approach
A path planning framework has been proposed and irndemented as p q of the SPOTT mobile robot control
architecture (Zelek 1996). Within SPOTT's framework
there are two concurrently executing planning modules.
286
Planning is defined along a natural division: (1) local,
high resolution, and quick response, versus (2) global,
low resolution, and slower response. The local planning module executes a path based on what is currently
known about the environment as encoded in an occupancy grid (Elfes 1987). A potential field technique
using harmonic functions is used, which are guaranteed
to have no spurious local minima (Connolly & Grupen
1993). A harmonic function is the solution to Laplace's
equation. Any path - determined by performing pradient descent on the harmonic function -has the desirable
property that if there is a path to the goal, the path will
terminate there (Doyle & Snell 1984). In addition, the
steepest descent gradient path is also optimal in the
sense that it provides the minimum probability of hitting obstacles while minimizing the traversed distance
to a goal location (Doyle & Snell 1984). This optimality condition permits modeling uncertainty with an error tolerance (i.e., for the robot and obstades) provided
that the robot is frequently localized and the obstacles'
physical properties are validated via sensor information.
This may appear to he heuristic, but it is based on not
permitting the robot's position mor to grow beyond a
wtain k e d limit (i.e., by re-localizing).
The computation of the harmonic function is expensive, being linear in the number of grid points. The
technique proposed is a complete dgorithrn (i.e., also
referred to as an ezact algorithm): guaranteed to find a
path between two wntigurations if it exists. A complete algorithm usually requires exponential time in
the number of degreesof-freedom (Halperin, Kavra!+
& Latomhe 1997). Methods-of-relaxation techniques
were used to achieve harmonic function convergence in
O(rn),where rn is the number of data points. Since m
is a square matrix of size n by n, then convergence is
achieved in O(n2),where 2 is the degree of freedom.
To generahze, O(nd)is the convergence rate where d
is the degree of freedom (i.e., exponential in the degree of freedom). The approach presented is confined
to a 2D workspace. An emphasis is placed on obtaining
real-ttme performance in this dimensional space wirh
future prospects of adaptation to higher dimensionalities. Real-time is defined as responding to environmental stimuli faster than events change in the world.
A client-server process model is used to concurrently
compute the potential field (Le., encode all the potential paths to the goal) and execute the local path. A
global planning module (Zelek 1996) uses a search algorithm (i.e., Dijkstra's algorithm) on a graph structure representation of the map'. The role of the global
planning module is to provide global information to the
local path planner. This is necessary because the spa
tial extent of the local path planner is k e d to limit
computation times,
A typical operational scenario for SPOTT (&lek
'This is only possible if a global map is available a priglobal map only contains the permanent
features of the environment (e.g., walk).
ori. Initially, the
1996) ran be presented in the following manner. The
goal spedfication is given as input by the user. If an
a priori CAD map eldsts, it is loaded into the mnp
database. The role of the control programs (i.e., b e
havioral controller) is to provide the local path planner
(i.e., potential field) with the necessary sensor-based
information for computation, such as obstacle configurations, cun’ent robot position, sub-gods if necessary, in
addition to performingreactive control. The behavioral
controller continually computes mapping and localization information hy processing sensor data. As the path
is being computed, another module is responsible for
concurrently performing steepest gradient descent on
the potential function in order to determine the local
robot path. If the ultimate goal is outside of the local
bounds, then the global path planner is responsible for
determining the local sub-goal. If the robot is about to
move outside the local bounds, the local map boundaries are shifted so as to position the robot to the center.
Computing the Harmonic Function
SPOTT’s local path planner is b a e d on a potential field
method using harmonic functions, which are guaranteed
to have no spurious local minima (Connolly & Grnpen
1993). A harmonic function on a domain Cl c R” is a
function which satisfies Laplace’s equation:
The value of 4 is given on a closed domain s1 in the
configuration space C. For mobile robot navigation,
C corresponds to a planar set of coordinates (z,y).
A barrhonic function satisfies the Maximum Principle
and ‘Uniqueness Principle (Doyle & Snell 1984). The
Maximum Principle states that a ‘harmonic function
f(z,y) defined on Cl takes on its maximum value M
and its minimum value rn on the boundary and guarantees that there are no local minima in the harmonic
function. The Uniqueness Principle stipulates that if
f(z,y) and g(z,y) are harmonic functions on R such
that f(z,y) = g(z,g) for all houndaq.points, then
f(z,y) = g(z,Y) for all z , ~ .
Iterative applications of a finite difference operator
are used to compute the harmonic function because it
is sufficient to only consider the exact solutionat a finite
number of mesh points. This is in contrast to finite element methods which compute an approximationof an
exact solution by continuous piecewise polynomiil functions. The obstacles and grid boundary form boundary
conditions. The harmonic function is computed in the
free space using an iteration kernel, which is formed
by taking advantage of the harmonic function’s inherent averaging property, where a point is the average of
its neighboring points. A fivepoint iteration kernel for
solving Laplace’s equation is used:
The boundaries of the obstacles and the mesh is modeled using Dirichlet boundary conditions (i.e., where U
is given at each point of the boundary). The path - determined by performing steepest gradient descent on a
harmonic function using Dirichlet boundary conditions
-is optimal in the sense that it is the minimumprobability of hitting obstacles while minimizing the traversed
distance to a goal location (Doyle & SneU 1984). In
contrast, Neumann boundary conditions cause such a
path to graze obstacle boundaries. The Dirichlet optimality condition permits modeling uncertainty with an
error tolerance (i.e., obstacle’s position and size, robot’s
position) provided that the robot is frequentlylocalized
using sensor data. The robot is modeled as a point but
all obstacles are compensated to account for the size of
the robot in addition to an uncertainty tolerance.
The computation of the harmonic function is performed over an occupancy grid representation of the
local map. Harmonic function convergence is linear
(as reported earlier) in the number of gnd points b e
cause “Methods-of-Relaation” (4mes 1992) techniques
such as Gauss-Seidel iteration with Successive O w Relaxation, combined with the “Alternatcng Direction”
(ADI) method were used. Cornputation time is r e
stricted by using only a local window. This also ensures
proper encoding of the harmonic function which rapidly
decays m narrow corridors (Zelek 1996). A global path
planner (Zelek 1996) provides information about goals
outside the local hounds.
Another approach for speeding up performame is to
provide an initial guess to the solution, namely the simpler heuristic potential field (Khatib 1986) (i.e.>summation of potentials). This m a k s the local path planner
an iterative-refmement anytime approach (Zilhemtein
1996), where after a short period after a map change,
response is reactive but not optimal or guaranteed. After convergence IS achieved, the response is, of course,
opbmal.
The time for convergence will vary depending on the
resolution of the image. For the configuration given m
Figure 1, times for convergence were measured for varying resolutions. Convergence was based on observing
the error rate decrease to an insignificant value. The
error rate was based on the maximum change in any
gnd element value between successive iterations. An
error of 2.7x10-’ was attained on a 16 hy 16 grid in
,162 mRops (million floatmg point operations). An er~ reached on a 32 by 32 grid in ,927
ror of 3 . 2 5 ~ 1 0 -was
mflops. An error of 1.88x10-‘ was achieved on a 64 by
64 grid in 14.43 mflops. Typically, SPOTT’s local map
was 36 by 36, making convergence in less than a second
or two highly realizable.
A quad tree representation (Pavlidis 1982) vras also
used to reduce the number of grid elements and effectively speed up convergence (Zelek 1998). For any irregular grid sampliig (including quad tree), the iteration
kernel can be rewritten as shown in Equation 3, where
281
(b) Poll to see if a new robot position &ate
is available.
If so, correct the accumulated erro:.
(c) Poll to see if any newly sensed data is available. If so,
update the map.
(d) Update the robot's position based on theodometer sen601.
( e ) Poll to see if the executionerpre module requests a
4
Figure I:
new steering direction for the robot. If 50, compute it
by calculating the steepest dexent gradient vector at
the w e n t estimate of the robot's position.
( f ) If the robot is near the edge of the border of the potential field window,move the window so that the robot
is at the center and go to Loop 1; else go to Loop 2.
ia
L
ab X& i
a
b
d
Paths Generated at 64 by 64. She fig-
ure shows the equal-potential contoun generated (solid lines)
and the mrious paths computed (dashed lines) from varying
starting positions using steepest gradient descent for the above
configuration. The resolution of the grid was 64 by 64.
U, are neighboring points to U i J ,R, distance away.
n=d U.
En=-&
= Xf&
(3)
Again, linear interpolation is used to approximate gradients when the position of the robot is in between grid
points. This introduces some error in the minimal hitting path but the benefit in significantly less computations provides adequate justification for t h i s trade-off.
In addition, the introduced error has minimal effect in
increasing the chance of collision because the error is
in the large grid locations which are void of obstacles
and goals. As stated earlier, since computation time is
linear O(m)in the number of grid points m,then a reduction of k (i.e., where k is a fraction less than 1)in m
equivalently reduces the computation time to O(km).
Computation and Execution
The cornputation of she harmonic function and the execution of the path are executed as separate processes.
The path executioner requests trajectories which are
computed by performing steepest gradient descent on
the barmonic function. Linear interpolation is used to
approximate gradients when the position of the robot is
between mesh points. The process computing the iteration kernels, servicing requests from the executioner
and providing map updates based on current sensor
data uses following algorithm:
1. Loop 1 Load initial map if available.
the goal is outside the bounds of the potentid field
window, project the goal onto the window border. The
border location is based on information provided by the
global path planner.
3. Loop 2:
(a) Compute an averaging iteration kernel over the entire
local grid space.
2. If
288
This computation has the desirable property that
map features can be ridded or subtracted dynamically
(Le., as they are sensed) alongside with the correction
of the robot's position. Implied is the assumption that
step 3(a) is executed more more frequently than compared to steps 3(b) through to 3(f). In addition, these
events are independent of the computation and the execution of the path.
Cocktail Party Problem
One method of solving the problem where orientation
matters is to inmease the dimensionality of
for example, to N = 3 from N = 2. This course of action is
extremely costly because the time complexity increases
exponentially with additional dimensions.
At any point P ( z , y ) on the potential function 4,
there is a vector that is the direction in which 4 undergoes the greatest rate of deuease and that is referred
to as the steepest descent gmdient. In addition to this
direction, there is actually a whole family of directions
that are of descending value. Let As be a vector that
has a magnitude As and points from P to PI. Since
A s = iAz+jAyandthegradientof+isV+=
it follows that A4 = (As).V# .... Let As = a s ,
and Ab = (ii.VO)Vs + .. ., so that
=
.. . .
Taking the limit of this equation gives us $$ = %Vg.
The quantity
is called the directional deriuatiue of
6. The maximal value of As is the direction of the
steepest descent gradient - As, - and is the trajectory
we have used~whenthe target approach orientation was
not specified. However, there is a family of functions
bounded by the equi-potential contour at P. Let t'he
two directions Asl and Asz define the directions of the
equi-potential contour emanating from P. The minimal
hitting urobability path is defined by the steepest descen&g gradientbut other paths can. be systematically
chosen between the equi-potential contours. The subsequentdiscrete trajectoq i n w d S are limited by the
of the robot. ~n
our particnon.ho]onomic
ular experimental operating scenario it is not an issue
because we are using a Nomadics Scout mobile platform (i.e., holonomic robot) hut it will be an issue in
the future as we plan on using the technique for a yetto-beacquired outdoor four-wheel drive robot.
e,
+
2
ig+j'$$,
2
+
Figure 2 lkajectory Preposition Bias on Steepest
Gradient Descent. The role of the trajectcq prepositions
(e.g., left) is to bias the steepest gradient descent operation
that is performed on the potential function by the local path
planner.
The idea for choosing different trajectoriesother than
the one b a d on steepest gradient descent stems from
the task command language the SPOTT robot architecture uses. SPOTT uses a language lexicon (Zelek
1997) for defining robot tasks and takes on the form of
a minimal spanning subset for human 2D navigational
tasks (Landau & Jackendoff1993). The task command
lexicon consists of a verb, destination, direction and a
speed. The role of the trajectory prepositions (i.e., direction category in the task command) is to bias the
steepest gradient descent operation that is performed
on the potential function by the local path planner.
Rather than using steepest gradient descent, the trajectory is a descending gradient that is biased to be as close
as possib:e to the direction specified by the trajectory
preposition (see Figure 2). Based on this strategy of bi-
potential directional derivatives does not come at any
computational cost but only requires a slight change in
the decision process of step 3'1.) in the algorithm presented earlier. As seen in Figure 3, when some of the
trajectories are based in the opposite direction of the
steepest descendinggradient, there is a tendency for the
trajectory to oscillate (see the S and W biases in Figure 3). In addition, biasing the entire path does not
guarantee achieving the approach direction. Another
approach was to bias the trajectory off-center from the
steepest descending gradient (see Figure 4). Again,
there was no certainty in meeting the desired approach
direction.
Figure 4 Family offunctions. The steepest descending
gradient is illustrated by a solid line. The dashed lines illustrates other trajectories formed by the differential derivative
either being left or right of the steepest dewending gradient
but confined by the qui-potential contour lines.
The one conclusion gained &om these investigations
was that biasing the entire trajectory was not viable for
x-
Figure 3:
Bias Results. The solid line represents the
steepest descending gradient trajectory. The trajectwy biased
in the South direction is illustrated by the dotted line. The
trajectwy biased in the South=West direction is drawn with a
dash-dot pattern and the trajectwy biased in the West direction is shown with a dashed pattern. In addition, the.NorthWen biased trajectory is shown by a dotted line with circles
as samples.
asing the steepest descendinggradient, we examined the
validity of this approach for meeting the requirement of
approach direction (see Figure 3). Choosing amongst
trying to achieve an approach direction. Alternatively,
it was decided to apply the bias only when the current
robot position and goal were in open space, and the
goal was visible and within sensing range of the robot.
Given that the target position is known and that the
robot's position is approximatelyknown,the open space
conmtion is computed using the quad-tree representation: if the cell a t a particular scale neighbors a cell
containing the goal, then activate tbe trajectory bias.
In general, the open space condition 1s active when the
target is situated in the unoccluded visibility region of
the robot and within sensor range. As shown in Figure
5, even biasing the trajectory in open space does not
necessarily guarantee the target approach direction to
be that of the imposed bias, The trajectones in Figure
5 show that the bias forces the trajectory as far as it
can go (while maintaining a negative directionalderivative) and then the trajectory oscillates back towards the
goal. This is especially true when the desired approach
direction is in the opposite direction of the steepest d e
scending gradient. This leads us to conclude that in
open space to set the approach direction, we should bias
the trajectory in the opposite drection of the desired
289
putational costs in computing apatb to the target when
compared to the case where the constraint is not spedfied. The approach is applied in an environment where
the existence of obstacles and their locations is not necessarily known a priori. We are currently investigating
addressing the inclusion and representation of moving
obstacles and goals into our mobile robot path planning
strategy.
References
Figure 5
Ames, W. F. 1992. Numerical Methods for Partial Differential Equati-.
Academic Press Inc.
Barraqoand, J., and Latombe, J.-C. 1991. Robot motion
planning: A distributed representation approach. Internn-
Wen, North-West.North-East. East. Notice that the path
tries to go as far as it can based on the bias then there is an
oscillating pull back t o the goal.
Bamaquand, J.; Kavraki, L.; Latombe, J.; Li, T.; Motw i , R.; and Raghavan, P. to appear. A random sampling scheme for path planning. Internotionol Journal of
Bias Tests in Open Space In moving from
left to right. the biases in an open space configuration are
tiowl Journal of Robotics Resennh 10628449.
Robotics Reseanh
approach (as long as the steepest descending gradient is
not approximately equal t o the desired approach), and
when the trajectory is about the osdate, the bias is
slowly changed back towards the desired approach (see
Figure 6). Note that there was no incurred cost in the
Connolly, C. I., and Gmpen, R A. 1993. On the applications of harmonic fmctions to robotis. Journal of Robotic
Systems 10(7):931-946.
Doyle, P. G., and Snell, J. L. 1984. Rnndom Walk and
Electric Networks. TheMathematicalAssociation of America.
Elf-, A. 1987. Sonar-based real-world mapping and navigation. IEEE Journnl of Robotics and Automation 3249265.
Halperin, D.; Kavraki, L.; and Latombe, J.-C. 1997.
Robotics. Boca Raton, FL CRC Press. chapter 41, 755778.
Khatib, 0. 1986. Real-time obstacle avoidance for manip
ulators and mobile robots. The Internotional Journal of
Robotiw Research 5(1):90-98.
Landau. B.. and Jackendoff,R 1993. what and where
in spatial l&guage and spatial cognition. Behavioral and
Bmin Sciences 16217-265.
Latombe, J.-C.. 1991. Robot Motion Planning. Kluwer
Figure 6
!Trajectories in Open Space with Bias
Strategy. (a) The left path's intent was to approach the
target in the East direction. This was achieved by first biasing
the path in the West direction and when the path was about t o
oscillate, to slowly change the bias towards the East direction.
(b) Similarly.the right path's intent was to approach the target
in the West direction and a similar strategy was adopted.
computing of the harmonic function when compared to
the situation where the approach direction constraint
is not specified. The only negligible additional computational cost was in the decision making process for
choosing which local directional derivative to steer the
trajectory. Since the bias is only applied in open space
areas, the afiects of deviating from the minimal hitting
probability path t o achieve the approach constraint are
not salient.
Discussions
The paper demonstrates an approach for introducing a
target approach constraint and not increasing the com-
290
Academic Publishers.
Pavlidis, T. 1982. Algorithm for Graphics and Image
Pmceriing. Computer Science Press.
Stentz, A. 1995. The focussed d' algorithm for real-time
replanning. In Proeeedings of the Znternntionnl Joint Conference on Artificial Intelligence
Zelek, J. S. 1996. SPOTT: A Real-time Distributed and
Scalable Architecture for Autonomous Mobile Robot ConhoL Ph.D. Dissertation, McGiU University, Dept. of El=-
trical Engineering.
Zelek, J. 1997. Human-robot interaction with a minimal spanning naturd language template for autonomous
and telwperated control. In Pmceedings of the Tenth
IEEE/RSJ Internntimol Conference on Zntelligent RoboO.
and Systems, 29S305.
Zelek, J. 1998. Complete real-time path planning during
sets01 discovery. In Proceedings of the 11 'th IEEE/RSJ Internotiowl Conference on Intelligent Robots and Systems,
1399-1404.
Zilberstein, S. 1996. Using anytime algorithms in intelligent systems. Artificial Intelligence Magnzine 73-83.