I RDAi4i779
A MOBILE ROBOT(U) PLANNING FOR PATH PITTSBURGH RELAXATION UNIY PATH CARNEGIEMELLON PA ROBOTICS INST
UNCLAS7DF/G
12/1
I
C ETHORPE APR 84 CMURITR845 N814SiK0503
i/ NL
EEEEEEEEEEEEEE Eu.'..'ll~l
.
*
j& .
.8
.

L1.
ILI
111.6
III,
1111
I~i 34r0 L
6
"2,
11.411.6 IHIIL== MICROCOPY RESOLUTION TEST CHART NATIONAL BUREAU Of STANDARDS 1%3 A
44
L% r. ,,...,..,' ' ' ,,.
, ."
. ,
,
.
5. .
.
.
5
5, ... *,
*5 .
. .55.*
. .
. . '.. ....
*5
..
,.
. .. 5 .. ."5
.
",
",
,"
" "
.3"
,:
'
PahReja)xation: Piih Planiin,l Pahfor a Mobile Robot
INR oI) (~~~~~~
I  A A,
*14AFI.
Path Relaxation: Path Planning for a Mobile Robot Charles E.Thorpe
CMURI'I'R845
The Robotics Institute CarnegieMellon University Pittsburgh, Pennsylvania 15213
1984
JUN 5
April 1984
Copyright © 1984 Mobile Robot Laboratory, CarnegieMellon University The CMU Rover has been supported at the CarnegieMellon University Robotics Instituitc since 1931 by the Office of Naval Research under contract number N0001481K0503. The views and conclusions contained in this document are those of the author and should not be interpreted as representing the official policies, either expressed or implied, of the Office of Naval Research or the U. S. Government.
rThis dcy
at o, been approved
rtp''b~i¢ .,
~
**
* ..
.
',,,, .ale, its
~**
\.
.;.
*.
.w ..
.*
.:
.T_.T.
4/.,
li4t1
~
*
o~ W7 l
. Z
77
7 •7
77 77
'P c..

. ......

r~ aa"'Ified 3
.SECU RITY CLASSIFt.ArTO4
.
Is PhA.[ '% en D...
't'eJ
REPORT DOCUMENTATION PAGE R 1jPOikT NUMt4BER

I
2. GOVI
READ I STRUCTIONS !!FFOPE COMPLETING FO",V ACCESSION NO.J 3.
;C:ENTS CATALOG NUWa=J
CMURITR845 4.
TITLE (and Subttl)
S.
TYPE OF REPORT 6
Path Relaxation: Path Planning for a Mobile Robot *
.
AUTNOR(a)
7.
,
Interim S.
PERFOR14ING ORG. REPORT NUMBER
8.
CONTRACT OR GRANT NUMBER(.J
Charles E. Thorpe
C
e
ToONR
N0001481K0503
PERFORMING ORGANIZATION NAME AND ADDRESS
2.
SO.
CarnegieMellon University
PROCRAM ELEMENT. PROJECT. TA,<
AREA
wORK UNIT NUMBERS
The Robotics Institute Pittsburgh, PA. 15213 
t'. CONT.Rfff.,NG OFFICE NAME AND ADDRESS
12. REPORT DATE
Office of Naval Research Arlington, VA 22217
April 1984 13.
NUMBER OF PAGES
27
14. MONITORING AGENCY NAME a ADDRESS(If dillerent tram Controlling Office)
IS. SECURITY CLASS. (of Chia report)
UNCLASSIFIED 1S1.
DECL ASSI FICATION/DOWNGRADING SCHEDuLE
t6. i)ISTRIBUTION
STATEMENT (orthta
Repo.rt
Approved fci publicrelease; distribution unlimi'ted
17. DISTRIBUTION STATEMENT (of the abotract entered In Block 20. if di!ferortroa Report)
Approved for public release; distribution unlimited
1O.
SUPPLEMENTARY NOTES
IS.
KEY WORDS (Continue on towers* *tao i neceary and identity by block ntmbet)
20.
ABSTRACT (Continue on
re.erse olde If necaeeaz" and idenetty by block nt.nber)
Path Relaxation is a method of planning safe paths around obstacles for mobile robots. It works in two steps: a global grid search that finds a rough path, followed by a local relaxation step that adjusts each node on the path to lower the overall path cost. The representation used by Path Relaxation allows an explicit tradeoff among length of path, clearance away from obstacles, and distance traveled through unmapped areas.
DD ,
1473
coro c, 1NO0CI 1s ETN
OETE UNCIASSIFIED
0102SO6OLET1
s/N 0102o1,600, ,
S9CURITY
"". S
CLASSIFICATION
OF THIS PAGE (b ben Dat.
1%
"nteoed)
Table of Contents 1. Introduction
1
2. Constraints
2
3. Approaches to Path Planning 4. Path Relaxation
4 6
4.1 Grid Size 4.2 Grid Search
6 8
4.3 Relaxation
10
5. Additions to the Basic Scheme 6. Remaining Work
12 13
Acknowledgements
15
Example Runs
16
.1
6:"
.1. ..
4
.
4"
.4..
4,
4,
,
*
~.. ~d ~ *~
!
List of Figures Figure 1: To find open spaces ol a II) line big Cnough to hold a robot of sizc r, it is sulj'fici(lt to sample the line at intervals of r. Figure 2: On a 4connected 21) grid of size r. it is possible to miss diagonal paths. I lere, there is
6 7
enough room foir the robot to go from (1,1) to (2,2), hut there is no clear path on the grid. Figure 3: One fix for the problen in figure 2 is to shrink the grid by a fictor of'sqrt(2). Then, if the robot will fit, there will be a grid path. Here the robot could go from (1.1) to (2.1) to (2,2) to (3.2) and on. Figure 4: Another fix is to use an 8connectcd grid. Now there is a diagonal line directly connecting (1,1) with (2,2).
7 8
1~1 I'a%
o
4, 'aJ
'U ,.. 5
,
,
,
.
,:
.,.
..
..
',..
.....
_"
'.";:
.....
'.'.
" "
;.<,
:.".
,
.
,",' :',,,,
,,.'''
.
',
''
:'',:,
,
IsI
Path Relaxation is a method oCiplanning sild
paths around tohstacles tor mobile robots. It works in two steps:
a global grid search that finds a ()o'ugh padl.
fbillwed by a local relaxation Step that adjusts each node on tlhe
paul to lower the overall path cost. The representation used by Path Relaxation allows an explicit tradeoff' aiong length or"path, clearance away f'rom obstacles, and distance traveled t11"rough tinnl)l)ed areas.
,,,
!, 4°
,.
.
.
. ..
,
.
.. .
. ..
.
. ,
. . .
. .; . .
. .. .
. .. .. :
.
: :.0.
.
I ''',,'..,,,,. ,.,',.,., ,,'.. ,.,..,. .... ..,. .,'..,
,.T: 
......
1. Introduction Path Relaxation is a twostep process hor mobile robots. It finds a sal'e path for a robot to traverse a field of obstacles and arrive at its destination. The first sLep of path relaxation finds a preliminary path on an cightconnectcd grid of points. The second step adjusts, or "relaxes", the position of each preliminary path point to improve the path. One advantage of path relaxation is that it allows many dillT'rent ftiCtOS to he considered in choosing a path.
*.
Typical path planning algorithms evalkte tile cost o" alternative laths solely on tile basis of path length. The cost l'unction used by Path Relaxation, in contrast, liso includes how close the palh coimcs to objects (the further away, the lower the cost) and penalties for traveling through areas out of the field of view. The effect is to produce paths that neithcr clip the corners of obstacles nor make wide deviations around isolatcd ohjects, and that prefer to stay in mapped terrain unless a path through tIlmlapped regions is substantially shorter. Other factors, such as sharpness of corners or visibility of landmarks, could also he added for a particular robot or mission. Path Relaxation is part of Fido, the vision and navigation system of the CM L Rover mol)ile robot. [29, 411 The Rover, under Fido's control, will navigate solely by stereo 'ision.
It will pick about 40 points to track,
find them in a pair of stereo images, and calculate their 31) positions relative to the Rover. The Rover will then move about half a meter, take a new pair of pictires, find the 40 tracked points in each of the new pictures and recalculate their positions. The change in position of those points relativc to tlie robot gives the actual changc or tie robot's position in the stationary world. 1ido's modcl of the world is not suitable for most existing pathplanning algorithms.
Algorithms for
plaming paths usually assume a completely known world model composed of planarfaced objects. Fido's world model, on the other hand, contains only the 40 points it is tracking. Ior each point the model records its position, the uncertainty in that position, and the appearance of a small patch of the image around that point. Furthermore, Fido only knows about what it has seen: points that have never been within its field of view are not listed in the world model. Also, the vision system may fail to track points correctly, so there may be phantom objects in the world model that have been seen once hut are no longer being tracked. All this indicates the need for a data structure that call represent uncertainty and inaccuracy, and for algorithms that
K'
can use such data. Section 2 of this report outlines the constraints aailable to lido's path planner. Section 3 discusses some common types of path planners, and shows how they are inadequate lojr our application. The Path Relaxation algorithm is explained in detail in Section 4. and some additions to the basic SCheme arc presented in Section 5. Finally, Section 6 discusses shortcomings of Path Rclaxation and some possible extensions.
'
)"=%
2
2. Constraints An intelligent path planner needs to bring muLch information to bear on the problem. This section discusses some
of
tile information useful for mobile ro)ot path planning, and shows how tie constIraints for m)lobile
robot paths differ fion those Ibr maniptlator trajectories. OW limesioIality. A groundhased robot vehicle is constrained to three degrees of freedom: x and y position and Orientation. InI particular, the CM U Rover has a circular crosssection, So 116r tile purposes Of path planniig the orientation does not matter. "[his makes path planning onl1y a two)dinensional problem. In contrast, typical robot anns have fronn 3 to 6 joints. A path planner fl)r a1manipulator", it it takes into
account the position of each joint, would have up to a 6dimensional search space.
...wheels.
Imprecise control. Even tlnder the best of circunstances, a mobile robot is not likely to be very acirate. Its precision will depend on the smoothness of the ground, the accuracy of its Controller, and the traction of the Typical manipulators. ol the other hand. have repeata)ilities of a k w thousandths of an inch over their entire reach. The implication for path planning is that it is much less iIportaIt to worry albolt exact fits for mobile robot I)aths. If the robot could, theoretically, just barely lit through a certain opening, then in practice that's probably not a good way to go. Computational resources are better spent exploring alternate paths rather than worrying about highly accurate motion calculations. ('umulative error. Frrors in a deadreckoning system tend to accumulate: a small error in heading, for instance, can give rise to a large error in position as the vehicle moves. rThe only wily to Icduce C'error is to periodically measure position against some global standard, which can be timcconstiming. The Rover, for example, docs its measurement by stereo vision, taking a few minutes to compute its exact position. So a
slightly longer path that slays farther away from obstacles, and allows longer motion between stops for
measurement. may take less time to travel than a shorter path that requires more frequent stops. In contrast, a manipulator can reach a location with approximately the same error regardless of what path is Liken to arrive there. 'There is no cumulative error, and no time spent in reorientation. Unknown areas. Robot manipulator trajectory planners usually know about all the obstacles. The Rover knows only about those that it has seen. This leaves unknown areas outside its field of view and behind obstacles. It is usually preferable to plan a path that traverses only known empty regions, but ir that path is much longer than the shortest possible path then it may he worth while looking at the unknown regions.
Perhaps Some "curiosity factor", that changed depending on whether this was I inapping run or a IProduction run. could dcterniine the tendency to look around.
,
Fuzzy objects. Not only do typical manipulator pathplanners know about all the objects, they know precisely where each object is. This informnation might comme, for instance, from the CA) svteni that designed the robot workstation. Mobile robots, on the other hand, tIsually sCsC the world ats they go. The Rover. instead ofthaving precise bounds for )bjects, knows only about fuzzy poinls. The location orta point is only known to the precision or the stereo vision system, and tile extent of' an object beyond the point is
entirely unknown.
1
,
.
.
.
o
•
.
.
.
.
,
o

,
...
.
...
.
=
o
.
.
.
,

o
•
.,
..
;.
..... .t.
..
,....,......
.......
"
.:.
Field of view. It may be important flor a mobile robot to plan paths that keep it as ftr away from obstacles as possible, so that it can see more distant objects. Or it may be important to kcep it behind objects, so that it cannot be seen from lntr off: imagine a robot watchman, or a robot Lank, sneaking up on somC hostile forcC.
Sithis
In summary, a good system for mobile robot path planning will be quite diflflrent from a maniplulator path planner. Mobile robot path planners need to handle uncertainty in the :;ensed world model and errors in path execution. They do not have to worry about high dimensionality or extremely high accuracy. Section 3 of" report discusses sone existing path planning algorithms and their shortcomings. Section 4 then presents the algorithms used by Path Relaxation, and shows how they address these problems.
.
5'.
''p?,
'""::" ';:
,","'..
.V""
"""
"""
.
.
" ""
""
""
"" "
'
""
""
""
'"
""
? '"
"
"
'
4,.
4
3. Approaches to Path Planning 'liis SCtiOin outlines several approaches to path plalning and some of the drawbacks of each approach. All of these methods except the potential flelds approach abstract tile search space to a graph of possible paths. This graph is then searched by some standard search techniqie. such as breadthfirst or A* [30. 38, 391. and thc shortcst path is returned. The important thing to note in tile Ibllowing is tLe intformation made explicit by each representation and the in frolmation thrown away. .
Free Space methods. [3. 13, 31. 32] One type Of'path planner explicitly deals with the free space rather than with the space occupicd by obstaclcs, and forces path segments to run down the middle of the corridors
*
betwecn Obstacles. One such method fits generalized cylinders to the free space. Another calculates the free space's Voronoi diagram. The spine of thc Voronoi diagram or the axes of tie generali/cd cylinders form a network of possible paths. Some oflthcsc paths may pass through places narrower than the robot: these pall segments can be detected and deleted. The remaining segments form the graph of' possiblc paths which is searched to find the shortest path. Free space algorithms sulTer from two related problems, both resulting from a data abstraction that throws away too much information. The first problem is that paths always run down the middle of corridors. In a narrow space. this is desirable, since it allows the maximum possible robot error without hitting an object. But in some cases paths may go much further out or their way than necessary. The second iproblem is that the algorithms do not use clearance in formation. The shortest path is always selected, even if it involves much closer tolerances than a slightly longer path. Vertex Graiphs. [24, 40, 28] Another class of algorithms is based on a graph connecting pairs of vertices. In the first step, each obstacle is cxpandcd by the size of the robot and thc robot conceptually shrunk to a point. 'he problem of finding a path for the point through the grown obstacles is exactly the same as finding a path for the whole robot through the original objects. The graph of possible paths is built by considering every pair of vertices of the expanded obstacles: if the line between two vertices does not intersect any of the expanded obstacles, it is a candidate path segment and is added to the graph. As in the Free Space methods. the graph is searched by some standard graph search algorithm, and the shortest path is returned. Variations of this algorithm use schemes that interleave generation and testing of the graph, hoping to avoid building parts of the graph. Vertex graph algorithms suffer from the "too close" problem: in their concern for the shortest possible path, they find paths that clip tile corners of obstacles and even run along the edges of some objects. It is, of course, possible to build in a margin of error by growing the obstacles by an extra amnount: (his may, however, block some paths.
Both free space and vertex graph methods throw away too much inlormation too soon. All obstacles are modeled as polygons, all paths are considered either open or blocked, and the shortest path is always best. There is no mechanism for trading a slightly longer path for more clearance, or for making local path adjustments. There is also no clean way to deal with unmapped regions, other than to close them off entirely.
.:
'ThePotential Fields [1, 20] approach tries to make those tradeofrs explicit. Conceptually. it turns the robot into a marble, tilts ie floor towards the goal, and watches to see which way tile marble rolls. Obstacles are represented as hills with sloping sides, so the marble will roll a prudent distance away from them but not too
.
*
.
'q.
St.t
far, and will seek the passes between adjacent hills. Sophisticated algorithms even give the marble momentum, taking into account the energy needed to accelerate, decelerate, and turn. The problem with potential Field paths is that they tend to get caught in dead ends: once the marble rolls into a box canyon, the algorithm has to invoke specialcase mechanisms to cut ol that route, backtrack, and start again. Mi rcover, the path with the lowest threshold might turn out to be a long and winding road. while a path that A limb a small ridge at the start and then has an easy run to the goal Miiight neer be in'estigated. Sc of these problems can be avoided if there are no concave objects or collections of*objects, but this is i "ug and unrealistic restriction.
the Regular Grid method. This covers the world with a regular grid of points, each connected \,ith its 4 or 8 neighbors to form a graph. In existing regular grid implementations, the only inflonnation stored at a node is whether it is inside an object or not. Then the graph is searched, and the shortest grid path returned. This straightforward grid search has many of the same "too close" l)roblems as the vertex graph approaches. A more sophisticated approach could assign weights to the nodcs depending on how close they were to objects or other factors, and avoid the "too close" and "too far" problems. But the remaining path would still be jagged, and might miss the best path bccause of the grid's coarse resolution.
6
4. Path Relaxation Path Relaxation combines [he best fetires of grid search ind potential fields. Lsing the rolling marble analogy, (lie irst step is a global grid search that finds a good valley for the path to lollow. The second step is a local relaxation step, similar to the potcntial Held approach, that moves the nodes in the pai to the bottom of tile valley in which they lie. The terrain (cost tunction) consists ot a gradual slope towirds tie goal, hills with sloping sides for obstacles, and plateaus for unexplored regions. The height or the hills has to do with the confidence that there really is an object there. I lill diameter depends on robot precisioln: a more precise robot can drive closer to an object, so the hills will be tall and narrow, while a less accurate %chicie will need more clearance, requiring wide, gradually tapering hillsides. This section first presents results onl how large the grid size can be withotit missing paths. It next discusses the inechanism lbr assigning cost to the iodes and searching the grid. linally, it )IeenlS the rclaxation step that adjusts the positions of path nodes.
4.1 Grid Size Ilow large can a grid be and still not miss any possible paths? That depends on the nmber of dimensions of the problem, on the connectivity of the grid, and on the size of the vehicle. It also depends oil the vehicle's shape: in this section. we discuss the simplest shape, which is a vehicle with a circular crosssection.
Robot
Figure 1: To Find open spaces on a 11) line big enough to hold a robot of size r, it is sufficient to sample the line at intervals of r. First consider the problem of placing a circle on a 11) line (see figure I). Given a line segment with some sections covered with I I) obstacles, and a robot of size r, we wish to Find all clear places on the line where the robot can be placed. It is stirficicnt in this case to sample the segment at intervals ot' r an1d test only the unblocked points. A 11) grid spacing of r guarantees that any open section l'length r or greater will totich at least one oh" the grid points. An open point does not guarantee that it is part of an open interval or size I" it merely says that point is worth investigating.
*
We can extend this reasoning [iom one dimension to a more useful case, a 21) area with 21) obstacles. The area can be covered with a grid in which each niode is connected to either its IUr or its eight nearest neighbors. For a fourconnected grid, if dhe spacing were r, there would be a chance o' missing diagonal
k%
I".
7
Robot
1
2
Figure 2: On a 4conncccd 21) grid of size r, it is possible to miss diagonal paths. Here. thcre is enough room for the robot to go from (1.1) to (2.2), but there is no clear path on the grid.
Robot 3

Figure 3: One fix for the problem in figure 2 is to shrink the grid by a factor of sqrt(2). "'hen,if the robot will fit, there will be a grid path. Flere the robot could go from (1,1) to (2,1) to (2,2) to (3,2) and on.
"'."..
,*'.
. ',.." .. "q,'~ ,*
'.,,,,;,  "" .
;. *" ..
f.. *.; .;'.
:
~,*.C~*
**
.".
'. '.V ,.X ',.
8
v,
o.5
1 \
2
Figure 4: Another fix is to use aln 8con nccited grid. Now there is a diagonal line directly connecting (1,1) with (2.2).
paths: there might be just enough room between two obstacles for the rohot to move Iron (x, y) to (x + 1,y+ 1), yet both node (x, y+ 1)and node (x + 1, y) might he covered. To guarantee that no paths are i gttlahl'flCcs th't if. missed, the grid spacing must be rediced to rV'2 /2. That is the largest size allowable tha diagonally opposite nodes are covered, there is not enough room between tlhem for the robot to safely pass. It the grid is eightconnected (each node connected to its diagonal, as well as orthogonal, neighbors), the problem with diagonal paths disappears. As in the 11)case, the grid spacing can be a full t; while guaranteeing that if there is a path it will be found.
4.2 Grid Search

Once the grid size has been fixed, the next step is to assign costs to paths on the grid and then to search for the best path along the grid from the start to the goal. "Best", in this case, has three conflicting requirements: shorter path length, greater margin away from obstacles, and less distance in uncharted areas. Thcse three are explicitly balanced by the way path costs arc calculated. A path's cost is the sull of the costs o"the nlodes through which it passes, each multiplied by the distance to the adjacent nodes. (In at4connected graph all lengths are the samne, but in an 8connected graph we have to distinguish between orthogonal and diagonal links.) The node costs consist of three parts to explicitly represent the three conflicting criteria. 1.Cost for distance. l'.ach node starts out with a cost of one ulit, for length traveled.
..
..
2. Cost for near objects. Each object near a node adds to that node's cost. The nearcr the obstacle, the more cost it adds. The exact slope of the cost function will depend on the accuracy or the vehicle (a more accurate vehicle can afford to come closcr to objects). and the vehicle's speed (a faster vehicle can afford to go farther out of its way), among othcr actors.
"%,
S'
~%'
% 1 ,.5
~
*.
%.
*
: *
5
~
*
9 3. Cost for within or near an unmapped region. The cost ror traveling in an uniiapi1aped region will depend on the vehicle's mission. If this is primarily an exploration trip. for example, the cost might be relatively low. There is also a cost added lor being near an unmapped region, using die same sort o "function of distance as is used for obstacles. This provides a buller to keep paths from coming too close to potentially unniapped hazards. The first step of Path Relaxation is it set up the grid and construct the list of obstacles and the Nehicle's current position and Iield of view.' [he system can then calculate the cost at each node, based on the distances to nearby obstacles and whether that node is within the field of view or not. The next Step is to create links lroin each node to its 8 neighbors. The start and goal locations do not necessarily lie on grid points, so special nodes need to be created for them and linked into tile gralph. The system then searches this graph for the minimumcost path front the start to the goal. The search itself is a standard A* (301 search. [he estimated total cost of"a path, used by A* to pick which node to expand next, is the sum of die cost so far plus the straightline distance fiom the current location to the goal. This has the effect, in regions of equal cost, of finding the path that most closely approximates the straightline path to the goal. The path found is guaranteed to be the lowestcost path on the grid, but this is not necessarily the overall optlial path. First of all, even in areas with no obstacles the grid path may be Iminger than a strtightline path simply because it has to follow grid lines. For a 4connected grid. the worst case is diagonal lines, where the grid path is VT times as long as the straightline path. For an 8conneCted grid. tIccqu i1atCnt worst case is a path that goes equal distances forward and diagonally. This gives a path about 1.08 times as long as the straighiline path. In cases where the path curves around several obstacles, the extra pathl length can be even more significant. Secondly, if the grid path goes between two obstacles, it may be nonoptimal because a node is placed closer to one obstacle than to the other. A node placed exactly half way between the two obstacles would, for most types of cost functions, have a lower cost. The placement of the node that minimizes die overall path cost will depend both on node cost and on path length, but in any case is unlikely to be exactly on a grid point. If tie grid path is topologically equivalent to the optimal path (i.e. goes on the same side of each object), the grid path can be iteratively improved to approximate the optimal path (see Section 5). But if the grid path at any point goes on the "wrong" side otan obstacle, then no amount of local adjustment will yield ihe optimal path. The chance of going on the wrong side of an obstacle is related to the size or the grid and the shape of the cost vs. distance function. For a given grid si/c and cost Function, it is possible to put a limit on how much worse the path found could possibly he than the optimal path. If the result is too imprecise, the grid size can be decreased until the additional computation lite is no longer worth the improved pad. Up to this point, it has been assumned that the path is passable, that is that there is enough clearance lior the vehicle at all points. At this point that assumption can Ietested. If any path link goes hetween two objects
in this impleicntalion. there are two types of obstacles: polygonal and circular. ('irrenilly. the circular obstacles are u'sed fIr points found by Fido's vision system, each bounded by a circular error limit, and the polygolIs arc Used for IhIc111ld Of %iew. Ihe %ision Ns'tenm will eventually Vise polygonal obstacles, at which poilu both the obslacles and i(le field olf view will be piv'slcslled a,POlC.eii ild the
circular obstacles will no longcr he needed.
...
10 separated by less than the robot dianeter, the robot cannot SqlUCeZC throUgh that gap and the path is not passable. The link that was crossed can be removed, and the grid search redone. In practice, if the best grid path is not passable. it is likely that no other path will be, either. lowever it is possible For the lowestcost path to have a single spot that is just barely impassable, while the next lowest cost path has a series of just barely passable spots that add up to higher total cost. The prohaility ot this happening depends on tile shape o"thc cost j'unction, the size of the grid, the length of the path, and the amtount ofcLtter present. A few details oin the shale of' the cost function deserve mention. Many (lifirent cost 'iinctituis will work, but some shapes are harder to handle properly. The irst shape we tried was linear. This had the advantage of being easy to calculate quickly. hut gave problems when two objects were close together. The sutI Of tile costs 'roni two nearby objects was cqual to a linear Function of the sum otthe distances to the objects. This creates ellipses of equal cost, including the degenerate ellipse on the line between the two objects. In that case, there was ni) reason for the path to pick a spot midway between the objects, as we had (incorrectly) expected. Instead, the only change in cost came froitm changing distance, so tile path went wherever it had to to minimize path length. In our first attempt to remedy the situation we replaced de linear slope with an exponentially decaying value. This had the desired efect of creating a saddle between the two peaks, and forcing the path towards the midpoint between the objects. The problem with exponentials is that they never reach zero. For a linear function, therc was a quick test to see if a given object was close enough to a given point to have any influencc. If it wastoo Far away, tie function did not have to be evaluated. F[or the exponential cost flunction, on the other hand, the cost Function had to be calculated for every obstacle for each point. We tried ctting off the size of the exponential, hut this left a small ridge at the ex.,tremum of the function, and patis fended to run in nice circular arcs along those ridges. A good compronise, and the function we finally settled on. is a cubic function that ranges from 0 at some maximum distance, set by the user, to the obstacle's nmaximum cost at 0 distance. This has both the advantages of having a good saddle between neighboring obstacles and of being easy to compute and bounded in a local area.
4.3 Relaxation Grid search finds an approximate path; the next step is an optimization step that finetunes the location of each node on the path to minimize the total cost. One way to do this would be to precisely define the cost of the path by a set of nonlinear equations and solve them simultaneously to analytically determine the optimal position of each node. '[his approach is not, in general, compttationally feasible. The approach used here is a relaxation method. each node's position is adjusted in turn, using only local in formation to nininize the cost of the path sections on either side of that node. Since moving one node may affect the cost Of its neigho)mN's. the entire procedure is repeated until no node moves rarther than some small amount. Node motion has to be restricted. If nodes were allowed to move in any direction, they would all end tip at low cost points, with many nodes btnched together and a flew long links between then. This would not give a very good picture of the actual cost along the path. So in order to keel) the nodes spread ot, a nitnode's motion is restricted to be perpendicular to a line between the preceding and following nodes. Furthermore, at any one step a node is allowed to move no more dan one unit. As a node moves, all three factors of cost are affected: distance traveled (from the preceding node, via this
irJll .
. r+ . .
"
+ + +. (
c " r' : .r' 
,i
, Ir
II+
L

"
+

'.
'
'

+ +'
. 1. .
node, to die next node), proximity to objects, and relalionship to unmapped regions. The cotbinaition of these flctors makes itdillicult to directly solve for minimum Cost node position. Instead, a binary search is used to Find that position to whatever accuracy is desired. The relaxation step has thc effect or turning jagged lines into straight ones where possible, of finding the "saddle" in the cost function between two objects, and of ctirving Mirund isolated objetts. It also does ie "right thing" at region houndarics. The least cost pathl crossing a border between ditffrent cost regions will follow the same path as a ray of light rel'racting at a boundary hcteen media with diffirent transmission velocities. The relaxed paith will approach that path.
.,
.
4
JJ
j;:,,..4,,3 .:.,..,.,..,
.'. '.. 4 .  . ".'':.".'. . < ',',".'" ,.);, ..",:;.'.;.'
r
12
5. Additions to the Basic Scheme There are several uscful additions to the basic path relaxation approach. This section descrilbes those additions that have been implemented and tested. Section 6 describes other possible extensions. One extension is to vary the costs of individual obstacles. The vision system is not always accurate it sometimes reports phantom objects that do not really exist, and sometimes loses real objects that it had tracked correctly for several steps. If the path planner kept all the objects the vision system eer thought it saw, the world map could get hopelessly jammed. Oil the other hand, it'an object were only used for path planning when it was currently being tracked, the robot could collide with ohjects that had heen picked ip by the vision system at one point. The solution is to decrease the cost af objects thit are within the field of view and should be visible, but are not reported by the vision system. If ile cost goes below zero. that object gets deleted. Typically, the cost isdecreased by 10% of its original value rbr each turn it is within the field of view but not seen. So 10 turns after tile vision system loses an object, it disappears From the path planner's world model. This is a costeffective, but not very sophisticated approach to the problem. As the vision system improves and gives more reliable data, this will become a more interesting problem and will require more work. Another extension implemented is to rcuse cxisting paths whenever possible.
Fido invokes the path
planner at the beginning of each step. The planner adds new points to its world map. adjusts costs of existing points, and calculates the current field of view. It then takes the previously c;alculated path, trims off nodes that tie vehicle has moved through, and splices in the current position as the new start node. It checks this path to see if it is still usable; each node and each line between nodes is checked against all obstacles to see if it comes close enough to cause a collision. If the entire path isclear, it is used again for the next step. Only if it is blucked at some point does the pathplanner have to start from scratch. A more sophisticated scheme would also consider the chance that a new, shorter path had opened tip because of deleted obstacles or new area in the field of view. It would then estimate the amount of time that could be saved by finding a new path, and the chance that a new path could be found, before it decided whether to reuse the existing path or to replan from scratch. 'he relaxation step can be greatly speeded uP if it runs in parallel on several computers. Although an actual parallel implementation has not yet been done, a simulation has been written and tested. The major difference is that by the time a node's position is to be adjusted in the serial implementation, the previous node's position has alr'eady been updated, so the adjustments ripple down the chain of nodes. If all nodes are to be adjusted in parallel, though, a node's position has to updated based on its predecessor's current, rather new, position. One effect of this is that changes may take miore iterations to propagate (lown tile chain. Another effect isthat certain instabilities can occur. For example, a zigzagging path can Flip from the original path to its mirror image on alternate iterations. 'This can be fixed with appropriate damping.
than
'3 ;. '" " '
"
'
.
, .
.. .
. " .. . "
,. ' ' . ' .. ' " """.
. . , , ,", . . , , .
" , . ,", , ,,
.
13
6. Remaining Work Path Relaxation would be easy to extend (o higher dimensions. It could be used. for example, fr a threedimensional search to be used by underwater vehicles mancuvering through a drilling platibrm. Another use for higherdimensional searchcs would be to include rotations for asymmetric vehicles. Yet another application would he to model moving obstacles; then the third dimension becomes time, with the cost of a grid point having to do with distance to all objects at that time. This would have a slightly difTrent Ilavor than the other higherdinensional extensions: it is possible to go both directions in x, y. z.and theta. Nut only one direction in the time dimension. Another possible extension has to do with smoothing out sharp corners. All wheels on the CM U Rover steer, so it can frollow a path with sharp corners if necessary. Many other vehicles are not so maneuverable: they may steer like a car. with a minimum possible turning radius. In order to accommodate those vehicles, it would be necessary to restrict both the graph search and relaxation steps. A related problem is to use a smoothly curved path rather than a series ol" linear segments. Occlusions could be taken into account in fading out objects. The current rules decrease the weight of an object if it has not been seen on the current step. but is within the field of' view of the camera. Sometimcs an object may not be seen because it is behind a nearer object. So a better t,ding algorithm would take into account nearby objects that may be blocking further points from view. This itrategy becomres much more interesting with a more sophisticated vision system, flor example one2 that reports surf',cvs rzlher than points.
,
*
***
In the current system, the error function is the same lfr cachi object. One possible extension is to increase the size of obstacles further away from the start position, reflecting the increased positional error of" the vehicle as it moves. This has not been done for the Fido/Rover system, since a typical move will probably be less than a meter long. But other systems, interested in longer moves, may want to increase their margin of error along the path. Another possibility would be to use a constant error ftinction. btt to determine the distance to be traveled in one step by seeing how far along the current path the vehicle can move before its worstcase position error exceeds the clearance available. Another area that will require more work is time vs. accuracy tradeofts. Some algorithms have a time complexity that depends only on the number of objects or the number of object vertices. The runtime of path relaxation, in contrast, depends on the size of the area to be covered and on the grid spacing. So situatiolns with lots of empty space and only a fIew large objects tvor other algorithms. Part of that can be overcome with a strategy that automatically uses a larger grid size in clear areas. h'is brings up problems of representation of nonhomogeneous grids and of deciding when to switch grid size. Neither of these appears to be theoretically difficult, but both will take some thought to do elegantly. A related problem is use of path relaxation for paths with tight clearances. If'it is really necessary to sqlueez between two obstacles, the current algorithm may not provide a detailed enough path. If' multiple grid sizes are implemented. it will be possible to get smaller, as well as larger spacings, and to find good paths cven in tight spots.
.
.
.
.
.
.
.
.
.o
°
.
.
.

.
7
91
7
71
14 Path relaxation, ats well its almost all existing path planncrs, deals only with geomectic infrmtiln Algc part of a robots world knowledge, however. may be in partially symbolic lorm. [or example. atmap assembled by the vehicle itself may have very precise local patches. each meaismred from onC robot location. The relations betwccn patches, though, will probably be inuch less precise. since they depend on robot motion fromi onc step to thc next. Using such atmixture of constraints isathard problem.
15
Acknowledgements Many thanks to tlans Moravec, ILarry Matthies, and Richard Wallace, all of the CM U Rover Project, for
convemtious, advice, and encouragentent. In particular, Larry is the coauthor t)f lFido: he also gets credit for suggesting the name "Path lRelaxation". Richard suggested thc names "tooncar" and "toofar" used to ,:
2a
describe problems with various other palihplanning algorithms, and let me use some of his L.isp code 'or producing the drawings. Larry. Richard. and l lans all made helpflil comments on early versions of the mantsript. Thanks also to Nancy Serviou For stylistic comments.
.%
., I p" ,".,",,
,
_
.
.
.•..
".
'
......
,."
"
'
"
' .
' ..
"'"" ...
,,
'''''
,,
''''
',,
'
,
= "% ,'%
t'=I
16
Example Runs Figures 5, 6, and 7 form a set. Figure 5 is a ruln from scratch, using real data extracted fIrom the stored images by the Fido vision and navigation system. Objects are represented as little circles, where the size of the circle is the positional uncertainty of the stereo system. The nunber are not all consecutive. bCcauise some of the points being tracked are on the floor or arc high oflthe grouind, and therefore aren't obstacles. [he dotted lines surround the area not in the field of view: this should extend to negative infinity. The start position of" the robot is approximately (0. .2) and the goal is(0, 14.5). The grid path fbund is marked by 0's. After one iteration of relaxation, the path is marked by l's. After the second relaxation, the path is marked by 2's. The greatest change from I to 2 was less than .3 meters, the threshold, so the process stopped. The size of' the "hills" in the cost function is I meter, which means that the robot will try to stay I meter away from obstacles unless that causes it to go ot far out of its way. After the robot's first move, it tries to reuse the existing path. The robot is now at about (0.1, 0.8). It deletes de old start node and the nodes at (0,0) and (0,1), and links its current position into the path. In this case it had no new objects to add, so the path was still passablc. It then uses the old path without having to plan from scratch. After the next step, there are some new objects added. In particular, object #61 is too close to the old path, so a new one has to be planned. Again, the 0's are the grid path, and the l's and 2's mark steps of the relaxation algorithm. Figures 8, 9. and 10 are the same run as shown in figures 5. 6, and 7, except that the relaxation step is simulated to be in parallel. The biggest difference is near the end of the path in figure 8, where the path is somewhat jagged. The path flips between one state on evennumbered iterations (0 and 2) and the opposite state on otldnumbered steps (1). This could be fixed with damping, at the expense of slower convergence on other parts of the path.
4 4'
,.S
: ' ,
a
7 ' '.
, j . .:
, . . , . , , . ' ' ' ' ' ' ' , '
'
. , . .  .
. ' . . . 2 ' " ' , "
,
. " : . "  ", . . ' . " 
1*d
17
I. 'o1
References (11
J.Randolph Andrews. Impedance Control as a Framework fbr Implcnenting Obstacle Avoidance in a Manipulator. Master's thesis, M IT, 1983.
[21
G. Bauzil, M. Briot, and P. Ribes. A Navigation SubSystem Using Ultrasonic Sensors For the Mobile Robot IIII.A R:. In Isi Ilternatinal (onference on Robot Vision and .'enso , ("onliols. April. 1981.
[31
Rodney Brooks. Sol'ing the IFindIath Problem by Representing I'ree .Space cs G;''ralize' Cones. Al Memo 674, MassachUsetts Institute of Teclnology, May, 1982. Rodney Brooks.
14]
PhitanngCollision .ree Motionsf!: Pick and Pacc Operations. Al Memo 719, Massachusctts Institute ofTCchilology, May, 1983.
151 [61
[71
Raja Chatila. Path Finding and E'nvironment ILearning in a Mobile Robot System. In !i,'uropetn('onjerence on /11. Orsay, France, July. 1982.
[81
R. Chattergy. Some Ileuristics for /ie Navigation of a Robot. Technical Report, University of Hawaii at Manoa, 1983.
[9]
Bruno l)ufay and JeanClaude ILatombc. Robot Programming by InductivC learn;ng. In IJCAI83. August, 1983.
[101
Pierre I)ufresne. Represcnrtation and Processing of"Production Rule Interactions in a Robot Planning System. In I'C Symposium on Al. I.enningrad. LISSR, October, 1983.
fill
M. Ferrer. M. IBriot, and J. C.Taiou. Study of a Video Image Treatment System for the Mobile Robot I11 ,ARt. In Ist lnterniationalC'onrrenceoi Robot Vision ad .Sensory Controls. April. 1981.
[121
Richard Fikes. Peter Ilart, and Nils Nilsson. I earning and Execting Generalized Robot Plans. Technical Report 70, Stanford Research Institute. July, 1972.
[13]
Georges Giralt, Ralph Sobek. and Raja Chatila. A Multilevel Planning and Navigation System for a Mobile Robot: A I irst Approach to Hilare. In 'roceedings of JCAI6. August, 1979.

V'
Rodney Brooks and 'Ihomas I.o,.anoPercz. A Subdivision Algorithm Configuration Space fir IFindpath with Rotation. Il IJCAI83. August, 1983. B. B~ullock. 1). Keirscy, J. Mitchiell, T. Nussineier, I). "seng. Autonomous Vehicle Control: An Overview of the 1Iughes Project. In Trends andApplicalons 1983. IEEE Computer Society Press, May, 1983.
. .. €€ ";' *,¢
. ,",€ ,".",_
.. "' .'' ''''.'", ..:'..". ." ." .".. ".". . .""'
".."""". ."''''",..
18
S.,
1141
Georges Giralt. Raja Chatila, and Marc Vaisset. An Integrated Navigation and Motion Control Systern for AutonmoLs Multisensory Mohile Robots. In Ist lniernciomd ,yiposiui qf Robotic Research. Bretton Woods, NIL. September, 1983.
1151
'ugene Grcchanovshk and 1.S. Pinsker. An Algorithm Ibr Moving a ComputerControlled Manipulator While Avoiding Obstacles. In IC,f183. August, 1983.
1161
Leo Guibas, Lyle Ramshaw. and Jorge Stolli. A Kinetic Iramework for Computational Geometry. In Fouti onis mx f('ompiler Scienlce. IlI E.,1983.
1171
John Hallam. Resolving Observer Motion by Object Tracking. In tIdW,,183. August, 1983.
(181
J. Ilopcroft. J. Schwartz, and M. Sharir. hi'iel Deteclion of Inlerseclions hiong Spheres. Technical Report 59. Courant Institute. FIebruary, 1983.
[191
Scott Kaultfinan.
*".,," *
i
An Algorithmic Approach to Intelligent Robot Mobility. Robotics Age, May/June, 1983. [201
Bruce Krogh. Feedback Obstacle Avoidance Control. 1983. The author is with the I)epartment of ilectrical Fnginecring, CarnegicMclon University.
1211
R. E.. arson and A. J. Korsak. A )ynamic Programming Successive Approximation Techniqtc with Convergence Proof., Parts I and II. Ifutomalica 6. 1970.
[221
JeanPaul ILaumond. Model Structuring and Concept Recognition: Two Aspects of Learning for a Mobile Robot. In JCAI83. 1983.
[231
Thomas I .ozaoPerez, Matthew Mason. and RLisscll Taylor. ,IaIelmalic .S'nlhsis J',FintAiolionSlralegiewJbrRobot. A[ Memo, Massachusetts Institute ofTechnology. August, 1983.
[241
Tomas I .oanoPercz and Michael A. Wesley. An Algorithm for Planning CollisionFree Paths Among Polyhedral Obstacles. C('Il 22(10), October, 1979.
[251
Matthew Mason. Awomatic I'hluing of Fine Molions: Correctness and (ompleteness. Technical Report, CarnegieMellon University, October, 1983.
[261
J.A. Miller. Autonomous Guidance and Control ofa Roving Robot.
In Pro'eedintgs ofJILA'S. 1977. I,..
.4,.
4..,.
,,,
,
.
.
 ..
 .
..
 , .
 ,.,
 .
.
.. .
..  .
.
.. ...% . " ....
"'."'..,
'''.
. '.'.
.
19
[271
1281
)avid Miller. Robot Travel: Planning for Frors. 1984. Suhmitted to CSCSI84. The author is at the Coniuter _ience I)e11tlCllt, Yalc University.
Ians Moravec. Obstacle A voidance and Navigation in the Real JJ"orldby a Seeing Robot Rover. Technical Report CMLIRTR3, CarnegieMcllon U nivesity Robo)tics Institute, September, 1980.
[291
[301
ilans Moravec. The CM U Rover. iiI Proceedings' .f,,i 1.
August, 1982.
N. Nilsson. Problem Solving Alelhods in McGrawHill, 1971.
rliicialIntelligence.
[311
Cohm O')unlaing, Micha Sharir, and Chec Yap. Retraiction:a new approach to motionplaning. Technical Report, Courant Institute, November, 1982.
[321
C. O')unlaing and C. Yap. The Voroirun ethod Ior AlotionI'lanning: L The ('ase OJ'A Disc. Technical Report 53, CoUritt Institutc, March, 1983.
(331
R. S. Rownberg and P. V. Rowat. Spatial I'roblems for a Simulated Robot. In LI(,It81. August. 1981.
[341
Peter Rowat. ReiresentingSpatial P'xperieinceandSolvingSpatial Problems in ai SimhltedRobot Environment. Phi) thesis, University of British Columbia, October, 1979.
[351
Jacob Schwartz and Micha Sharir. On the Tano Alovers' Problem L The case of a Twodimensional Rigid Polygonal Body Moving ,Imidst Polygonal Barriers. 'Technical Report 39, Courant Institute, October, 1981.
1361
Micha Sharir. Inltersection and ClosestpairProblemsfor a Set ol PhnarObjects. Technical Report 56, Courant Institute. February, 1983.
[37]
Paiul Spirakis and Chee Yap. On The (ombinational Complexity Of Ql lotion Coordination. Technical Report 76, Courant Institute, April, 1983.
[381
Robert Tarjan. A Unified Approach to Path Problems. J.C31 28(3), July. 1981.
1391
Rlobert T'arjan. Fast Algorithms i or Solving Path Problems.
[
JACAI 28(3), July, 1981.
'
7
j5 7
7.
20 [401
Alan M. Thompson. The Navigation System of the JPI. Robot. In Proceedings i JC'A5. 1977.
[411
Charles Thorpe. The CM U Rover and the FI)O Vision and Navigation System. In Third ym.p').'osiuimi on AIuItonomotus Underwater I/ehicles. Marinc Systems Engincering Laboratory. Univcrsity of New IlampshirC, 1983.
[421
Charles Thorpe. A"n naIls'is o"lnires Operatorsim," 1"7,)0. Technical Rcport CM LUII IR8319, Robotics Institute, CarnegieMellon University, I)ccember, 1983.
[431
Shrirarn M. Udupa. Collision I)etection and Avoidance in Computcr Controlled Manipulators. In Proceedingsq II('1 15. 1977.
[441
Richard Wallace. TwoI)imensional Path Planning and Collision Avoidance iorl'hrccl)imcnsional Robot Manipulators. In Representation and Processing ofSpalial Kinowledge. University of Maryland. May, 1983.
[451
C. Witkowski.
,
A Parallel Processor Algorithm for Robot Route Planning. In IJCAI83. August, 1983. [461
Chee Yap. Hlolion (oordinalionfir Two Discs. TeLhnical Report. Courant Institute, January, 1982.
". 4..
.* 4.
,'
" ¢ ;
";
*:'
'.
;?
',""
" "
"
'
" ""
"'
" ""
""
" " "
""""
"
'
"
" "
" "
Figure 5
14
... __
_
__ _ _
__ _ _
__ _
_
JQ~J
10
"
*'*
/ %%
.•
5__k/
.3
4_
.2 _
1 2 .1 2..
L, S.,
2
qq
Figure 6
_
_
___

.4 .1
.
,
9.0
14.. __
_
___ ___
___
__
_
___
'Jo
11... __
_
____
____ _
7
4%d
.
5
44
2.
2se
___
____
_l__o_
Figure 7
0_
4
6'_
_
_
.5
4__ __
3_ __
2_
0_1_2_ _1_
4
Figure 8
K/
15i
".,.._
__ _ _
_ __
__
_
Cip *
411
1.
4_
.3_
.2
1
0
2..
";
*
°.
'"
.
'...
•
•
,

1
L"
3
r"'t""%.." q ,.'t. ¢ o """'.,

.a
1
". '." €" ","."
," ,"''
"
. ".."' ' ,.." .,v";.' .  ",","
Figure 9
14._
_
__
_
_
_
d3
12.
_
_
__
_
_
_
N.
% .U %1 %
~f%
*
U
.%%.J
JU
*
.54.
.
.
12
Figure 10
414
'
it._____
____
___
"S
S....
v/ de!
Ve[
'SII
S,.


3

1
2
3
4

'S
dl
Si'
~
:*~~ .z.i
_
~~
~Am
i~

tel
4 O
i't.l4
IIV