Performing Concurrent Robot Path Execution and ... - CiteSeerX

Performing Concurrent Robot Path Execution and Computation in Real-Time John S. Zelek

currently at : Department of Computer Science and Engineering Wright State University, Dayton, Ohio 45435, e-mail: [email protected] formerly at : Centre for Intelligent Machines (CIM) & Dept. of Electrical Engineering McGill University, Montreal, Quebec, Canada, H3A 2A7, e-mail: [email protected] (1996 AAAI Fall Symposium on Flexible Computation, Nov. 9-11,1996, Cambridge, MA)

Abstract

Concurrent robot path planning and execution in real-time is dicult to accomplish with existing technology. A computational framework is proposed and implemented that accomplishes this task using an existing network of computational processor resources. The tradeos and decisions made in order to execute this computational framework in real-time are also examined. Path planning is performed using a potential eld approach. A speci c type of potential function a harmonic function - that has no local minima is used. Steepest gradient descent on this function is guaranteed to nd a path to the goal if such a path exists. The implementation is parallel across a network of SPARC and SGI workstations using a message-passing software package called PVM. The computation of the plan is performed independently of and concurrently with the execution of the plan. The path planner is an integral part of a mobile robot control architecture called SPOTT which performs navigational tasks in real-time. Control is transmitted via a radio ethernet link to the robot. In order to use the path planner for real-time scenarios, it has local extent, interacts with a global path planner to obtain global goal information, is computed at varying resolutions, and is provided with a reasonable good estimate to initiate path computation.

Introduction

Path planning is the guidance of an agent - a robot from a source to a destination, while avoiding all encountered obstacles. A robot must not only be able to create and execute plans, but must be willing to interrupt or abandon a plan when circumstances demand it. In traditional AI planning, a smart planning phase constructs a plan which is carried out in a mechanical fashion by a dumb executive phase. The use of plans regularly involves rearrangement, interpolation, disambiguation, and substitution of map information. Real situations are characteristically complex, uncertain, and immediate. These situations require that the

planning and the execution phases function in parallel, as opposed to the serial ordering found in traditional AI planning. Path planning can be categorized as either being static or dynamic, depending on the mode of available information (Hwang & Ahuja 1992). A static path planning strategy is used when all the information about the obstacles is known a priori. Most path planning methods are static. The path planning is dynamic when no or partial information is available about the obstacles a priori. The environment can also be unpredictable and time-varying. Dynamic path planning is problematic because the path needs to be continually recomputed as new information is discovered. Dynamic path planning is used as an integral part of the SPOTT (Zelek & Levine 1996) mobile robot control architecture. The control system is a real-time AI system which is responsible for dynamically adapting to changing environmental circumstances in order to successfully execute and complete a set of navigational tasks for an autonomous mobile robot. SPOTT (see Figure 1) consists of a behavioral controller, a local dynamic path planner, and a global path planner, as well as a map and a graphical user interface. The behavioral control formalism is called TR+ and is based on an adaptation and extension of the Teleo-Reactive (TR) formalism introduced by Nils Nilsson (1992; 1994). TR+ rules make decisions which aect actuator control and map database maintenance. The dynamic local path planner continually polls the map database in order to navigate around newly encountered obstacles. It is based on a potential eld method using harmonic functions, which are guaranteed to have no spurious local minima. The harmonic function is computed iteratively across a collection of heterogeneous processor resources. The robot controller solicits trajectory commands from the local path planner at the same time the path is being computed. Continuous navigation is workable if trajectory commands are issued concurrently with the plan computa-

USER

SPOTT PATH PLANNING

Map Database Global Path Planner

LOCAL PATH PLANNER

CONTROLLER

GOAL COMMANDS

TR+ Program Interpreter

(coarse resolution) steepest gradient descent

set & unset: 1) task goals, 2) intermediate goals, 3) obstacles, & 4) robot position,

(fine resolution) POTENTIAL FUNCTION

override potential field if required.

(done at most reliable resolution)

POTENTIAL FUNCTIONS

PERCEPTION MODELLING

ACTUATORS

ENVIRONMENT SENSORS

Figure 1: Path Planning Within SPOTT The goal

speci cation is given as input by the user. If an a priori CAD map exists, it is loaded into the Map Database. The role of the TR+ program is to provide the local path planner (i.e., potential eld) with the necessary information for computation, such as obstacle con gurations and the current robot position. The TR+ program continually computes this by 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 trajectory for the robot. In order to guarantee proper control, there is a collection of potential functions concurrently computed at varying resolutions. The nest resolution potential function that has converged to its solution is used for trajectory generation. The TR+ program can also override this trajectory generation. This usually occurs for safety reasons when a dangerous situation necessitates a quick response to sensory stimuli.

tion. This is possible if the plan computation is faster than the rate of change in the environment (i.e., realtime). SPOTT's local path planner concurrently computes and executes the plan. The path planner is local because computation time increases proportional to the number of grid elements. The size of a grid element corresponds to the closeness of objects in the environment. A global path planner provides global goal information to the local path planner. The robot controller solicits trajectory commands from the local path planner at the same time the path is being computed. There is no guarantee on the correctness of the trajectory commands before the computation has converged to the solution for the current con guration. In order to address the correctness of the trajectory, a type of \anytime algorithm" (Mouaddib & Zilberstein

1995) is used, which trades o accuracy1 of the solution versus computation time. A collection of local path planners at varying spatial resolutions are concurrently computed. The ner the resolution, the more time the computation requires. The coarsest resolution local path planner is used rst, and the ner resolution local path planners will eventually be used once their computations have converged to their respective solutions, provided that there have been no sensed environmental changes. It is assumed that the computation speed of the path planner at the coarsest resolution is faster than the rate of trajectory-command requests. This is not always possible and the computation of the harmonic function is provided with a good initial guess for its computation. This speeds up the iterative computation and provides reasonable local trajectories before convergence. The important contributions of the described harmonic function potential eld approach for local path planning are (1) the guaranteeing of proper control (i.e., smooth trajectory) with a collection of potential functions at varying resolutions, (2) limiting the extent of the potential function2 and sliding its bounds when the robot moves around the environment, (3) an implementation which separates the computation from the control3, and (4) an investigation into real-time performance using existing computational resources in a parallel implementation.

Path Planning Approach

Biologically plausible potential elds are used to perform dynamic local path planning. The potential eld approach applies a function over a discrete grid of obstacle and goal con gurations, and the path is determined by performing gradient descent on this function. A drawback of the potential eld method is that some potential functions have spurious local minima. These functions are heuristic and are usually found by summing together local potentials for each obstacle and goal in the con guration (Khatib 1986). An advantage of such heuristic functions is that they are computationally inexpensive. A speci c type of potential function - a harmonic 1 The path produced is based on visiting neighboring grid points in a discretized grid. The ner the grid spacing, the more accurate the path. 2 This is necessary because the harmonic function is a rapidly decaying function and its computation time increases exponentially in direct proportion to the number of grid elements. A global path planner provides global goal information and projects it onto the border of the potential eld. 3 Trajectory commands are solicited at the same time the potential function is computed.

function - is used to provide navigational commands. A harmonic function has been shown to have the desirable property of no local minima (Connolly 1994; Tarassenko & Blake 1991). If at least one path exists to a known destination location, the path planning strategy is guaranteed to nd it.

Harmonic Functions as Potential Functions

A harmonic function on a domain4  R is a function which satis es Laplace's equation: n

r2  =

X

2 

=1 2 x2 = 0

n i

(1)

i

The value of  is given on a closed domain , which is the potential function space C . The robot is modeled as a point in the potential eld. Since it has an actual size and geometry (i.e., cylindrical), all obstacles are padded accordingly5 (e.g., a line becomes a rectangle). A harmonic function satis es the Maximum Principle6 and the Uniqueness Principle7 (Doyle & Snell 1984). Navigational path planning is considered only on a 2D planar surface. The boundary consists of all obstacle goal model boundaries, and the closed domain boundary of .

Computing the Harmonic Function

The harmonic function can be iteratively computed by taking advantage of its inherent averaging property, where a point is the average of its neighboring points. A nine-point iteration kernel can be used to solve Laplace's equation (van de Vooren & Vliegenthart 1967): = 51 (U +1 + U ?1 + U +1 + U ?1 ) 1 + 20 (U +1 +1 + U ?1 +1 + U ?1 ?1 + U +1 ?1 ) Ui;j

i

i

;j

;j

i

i

;j

;j

i;j i

i;j

;j

i

;j

(2)

Path Generation

The eld of arti cial velocities V~ (e), is produced from a dierentiable potential function U : C ! i. All grids are given the same initial obstacle and goal con guration and are all updated with newly acquired sensor information as it becomes available. The harmonic functions are all computed iteratively and concurrently for each grid resolution. The steepest descent gradient is taken at the current estimated position of the robot for each grid computation: fv~1 ; v~2 ; :::; v~ g. If grid g has converged to the harmonic function for the current con guration, and there are no grids g , where j > i, that have also converged, then v~ is used as the local trajectory control. It is assumed that the computation time for convergence at the coarsest grid resolution is faster than the rate of change in the environment (i.e., real-time). Therefore, if there are no environmental changes sensed, eventually grid g (i.e., the nest grid) will converge and its trajectory vector v~ will be used to specify the local trajectory for the robot. For normal operation, the trajectory vectors are used in the order of ascending grid resolution fv~1 ; v~2 ; :::; v~ g. This progression switches from grid g to grid g +1 (i.e., control from v~ to v ~+1 ) when grid g +1 has converged to its solution. The progression is restarted (i.e., sometimes before grid g 's vector v~ has had a chance to be used for control) when the local map is updated (e.g., newly sensed data arrives, or a new goal is set). At this time, v~1 will again be used for control, and control will again switch in ascending order until v~ is reached or newly sensed data arrives and control is switched back to v~1 . The switching between levels depends on determining if a particular grid resolution g has converged to its harmonic function. Convergence can be found by measuring the change in value of the harmonic function grid elements between successive iteration steps. It is achieved when this value is a minimum (i.e., approximately zero). However, convergence monitoring requires additional computational resources, which will slow down the computation of the harmonic function. An alternative approach is to compare all the trajectory vectors fv~1 ; v~2 ; :::; v~ g in ascending order, assuming that v~1 , the direction computed at the coarsest resolution, is correct. If v~2 is approximately equal to v~1 , then v~2 is used, otherwise v~1 is used. This comparison continues between successive grid resolution levels until it fails or v~ is reached. n

g1

RESOLUTION HIGH

n

i

n

0.5

0

j

−0.5

−1 60

n

i

i

i

i

n

n

n

i

n

n

hierarchy in a single level is not addressed in this paper and is left as a future research topic.

20 0

10 0

OBSTACLES 1

0.5

GOAL

ROBOT TRAJECTORY

0

−0.5

ROBOT POSITION

−1 30 30

20

25 20 15

10

10 0

j

n

50 40 30

20

i

n

60

40

5 0

1

0.5

i

i

SLOW TO CONVERGE 1

0

−0.5

−1 15 15

10

RESOLUTION LOW

10 5

5 0

0

FAST TO CONVERGE

MULTI−RESOLUTION POTENTIAL FIELD COMPUTATION

Figure 4: Multi-Resolution Potential Fields. The trajectory of the robot is continuously available by taking the gradient at the nest resolution that has converged to its solution (i.e., harmonic function) at a particular time instance.

Trajectory Commands Before Convergence

The previous section assumes that the coarsest resolution converges in real-time. Real-time is de ned as reacting (i.e., converging) in a time faster than changes in the environment. Another way of speeding up the computation of the harmonic function is to provide the computation with a good initial guess for a given obstacle and goal con guration. A good initial estimate can be provided by the \summation of potentials approach" (Khatib 1986). In the summation of potentials approach, the potential function is created by summing together attractive potential functions for each goal model and a repulsive potential function for each obstacle model. The elementary potential function for an obstacle is in the form of a decaying function emanating from the obstacle's geometric model. Examples of such a function are a Gaussian, 1 , or 12 (where r is the distance from the object model). The attraction potential function is a mirror image of the repulsive potential function. The \summation of potentials approach" produces a function which may have a local minima or maxima. It serves as a good initial guess not only for speeding up the path computation, but also for providing an estimate of the potential function during the time the computation is performed for the current con guration. r

r

Interaction with Global Path Planning

A global path planner is used in conjunction with the local path planner to permit global goal information to in uence the local trajectory. The two planners function in parallel at dierent time-scales. The local path planner is in a control feedback loop with the robot and environment, and its response time to sensory input is crucial to the real-time operation of the robot. It dynamically reacts to changes in the map database which is continually updated by the behavioral controller. On the other hand, the global path planner uses states which change at a slower rate. These states are the current and potential physical locations of the robot with respect to nodes in an abstract graph structure, where a node is a room or a hallway portion. The global planning module advises the local planning module of the local eects of a global goal. The global planning module performs classical AI search using Dijkstra's algorithm (Aho, Hopcroft, & Ullman 1983) through a graph structure of nodes (i.e., rooms) and arcs (i.e., access ways). The global graph planner determines a path based on start and stop nodes de ned by the current location and desired destination of the robot. If the current goal is outside the extent of the local path planner, then the global path planner projects the goal onto the border of the local map used by the local path planner. The projection of the goal onto the border is based on the path produced at the abstract graph level (Zelek 1996).

Implementation

The use of PVM (Geist et al. 1994) to distribute the processing across a heterogeneous collection of processors makes the tradeo issue between the quality of the results (i.e., computation time) and the computational resources mute and transparent. PVM tries to balance the computation across the set of computational resources. The degradation of performance will depend on the number of processors made available and the total number of processes. Performance will not deteriorate if the number of available processors is greater than the number of master and slave processes. The computation time of the harmonic function is directly proportional to the number of grid elements (see Figure 5). Typically SPOTT uses a potential function with a grid size of 35 by 35 points. Using this grid size results in convergence to the harmonic function in less than 2 seconds. It was found that the limiting performance factor for the master-slave con guration using PVM was the network speed (i.e., Ethernet, 10 Mbps was used). Communication speeds can improve drastically if other protocols such as Fast Ethernet or ATM are used instead of Ethernet.

Each slave process iterates over a parcel of the entire data set. The optimum number of slave processes to use is dependent on the size of the entire data set as well as the overhead associated with the packing and sending of the data in a message passing paradigm. Figure 6 shows the relationship between the computation times for a single iteration with respect to the number of slave processes used (i.e., empirical results). Time taken for each iteration for different grid sizes.

300

200 m s e c

msec/iteration 100

a)

0 0

100 200 grid size = n, for n x n

Number of iterations to converge for different grid sizes.

# 1000 o f

900 800 700

i t e r a t i o n s

600 500 it. to converge

400 300 200

b)

100 0 0

50 100 150 grid size = n, for n x n

200

Total time for convergence for different grid sizes.

300

s 200 e c o n d 100 s

seconds to converge

c)

0 0 100 200 grid size = n, for n x n

Figure 5: Potential Field Computation Times Versus Grid Size. (a) The computation time of a single itera-

tion increases proportionately with the number of grid points (i.e., sampling resolution). (b) In addition, the number of iterations required for convergence also increases. (c) The total time for convergence appears to be exponential with respect to the number of grid points.

Conclusions

The contribution of this work is a technique to guarantee proper concurrent robot path planning and execution in real-time using an existing heterogeneous collection of networked workstations. A potential eld approach using a harmonic function guarantees that a path to the goal will be found if one exists. The control is performed o-board the robot and transmitted via a radio ethernet link to a Nomadics 200 robot. This path planning system is an integral part of a mobile robot control architecture called SPOTT. SPOTT has been successfully used to control the mobile robot to complete various navigational tasks in an oce and

tional Centres of Excellence Program through IRIS (Institute for Robotics and Intelligent Systems).

10000 t i m e ( m s e c )

300 by 300

1000

150 by 150 120 by 120

100 0

4

8 12 # of processors

16

20

500 t i 400 m e 100 by 100

( 300 m s e 200 c )

75 by 75 60 by 60

100 0

4

8 12 # of processors

16

20

Computation Times for a Single Iteration: The computation time (in milliseconds) of a single

Figure 6:

iteration for dierent grid sizes is plotted against the number of slave processes. Each set of data corresponds to a dierent overall square grid size. Each slave processor processes an equal amount of data. The optimum number of processors to use depends on the overall grid size. The experiments were performed so as to ensure that each master and slave process were executed on a unique processor (i.e., workstation).

laboratory environment (Zelek 1996). Computational constraints restrict the extent of the path planner. A global path planner is used to provide information on how to project the global goal onto the border of the potential eld. Proper control (i.e., smooth trajectory) is guaranteed by computing a collection of harmonic functions at varying resolutions. A coarse-to- ne anytime algorithm (Mouaddib & Zilberstein 1995) is used to determine which resolution to use for the current local path control. This assumes that the coarsest resolution converges in real-time. If this is not the case, a good initial guess is provided to the computation. A message-passing paradigm called PVM is used to distribute the processing across a heterogeneous collection of processors. Processor resource allocation and loading is transparent to the application. This paper addressed the minimal requirements to make a concurrent path planning and execution strategy execute in real-time. When real-time performance is not possible, a heuristic initial guess is used to provide a reasonable estimate from which to extract trajectory commands.

Acknowledgements

This research is partially supported by the Natural Sciences and Engineering Research Council and by the Na-

References

Aho, A. V.; Hopcroft, J. E.; and Ullman, J. D. 1983. Data Structures and Algorithms. Addison-Wesley Publishing Co. Connolly, C. I. 1994. Harmonic Functions As A Basis For Motor Control And Planning. Ph.D. Dissertation, University of Massachusetts, Amherst, Massachusetts. Doyle, P. G., and Snell, J. L. 1984. Random Walks and Electric Networks. The Mathematical Association of America. Geist, A.; Beguelin, A.; Dongarra, J.; Jian, W.; Manchek, R.; and Sunderam, V. 1994. PVM: Parallel Virtual Machine - A Users' Guide and Tutorial for Networked Parallel Computing. MIT Press. Hwang, Y. K., and Ahuja, N. 1992. Gross motion planning - a survey. ACM Computing Surveys 24(3):220{291. Khatib, O. 1986. Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research 5(1):90{98. Mouaddib, A. I., and Zilberstein, S. 1995. Knowledgebased anytime computation. Proceedings of the Fourteenth International Joint Conference on Arti cial Intelligence 1:775{781. Nilsson, N. J. 1992. Toward agent programs with circuit semantics. Technical Report STAN-CS-92-1412, Department of Computer Science, Stanford University, Stanford, California 94305. Nilsson, N. J. 1994. Teleo-reactive programs for agent control. Journal of Arti cial Intelligence Research 1:139{ 158. Sabersky, R.; Acosta, A.; and Hauptmann, E. 1971. Fluid Flow. MacMillan Publishing Company, 2nd edition. Tarassenko, L., and Blake, A. 1991. Analogue computation of collision-free paths. In Proceedings of the 1991 IEEE International Conference on Robotics and Automation. IEEE. 540{545. van de Vooren, A., and Vliegenthart, A. 1967. On the 9-point dierence formula for laplace's equation. Journal of Engineering Mathematics 1(3):187{202. Zelek, J. S., and Levine, M. D. 1996. Spott: A mobile robot control architecture for unknown or partially unknown environments. In AAAI Spring Symposium on Planning with Incomplete Information for Robot Problems. Zelek, J. S. 1996. SPOTT: A Real-time Distributed and Scalable Architecture for Autonomous Mobile Robot Control. Ph.D. Dissertation, McGill University, Dept. of Electrical Engineering.