Code CS/Rp, Department of Computer Science
We present an efficient algorithm for finding least-cost paths for an agent of negligible size across an important special case of two-dimensional terrain, terrain consisting of (1) a single isotropic homogeneous-cost-per-distance background region, (2) "roads" or narrow transportation corridors of low cost-per-distance, (3) "rivers" or narrow features of high crossing cost, and (4) untraversable "obstacles". This work extends (Mitchell 1987) by including rivers and new pruning heuristics for roads; our algorithm remains in time complexity. We also show results of a first computer implementation for this class of algorithms, and analyze average-case performance and error sensitivity. This work applies primarily to high-level path planning for robots, but also can help plan military maneuvers and guide construction of roads and pipelines.
* This work was supported in part by the U. S. Army Combat Developments Experimentation Center under MIPR ATEC 88-86. The views and conclusions contained in this document are those of the author and should not be interpreted as representative of the official policies of the Army or the U.S. Government. Thanks to Robert Richbourg and Robert McGhee. This paper appeared in International Journal of Robotics Research, 9, no. 6 (December 1990), pp. 67-74. Equations were redrawn in 2008.
We address finding the best path for an agent of negligible size across a known two-dimensional area or terrain, if the path can go anywhere except "obstacle" regions. "Best" can mean minimizing cost, effort, time, the probability of accident, or anything else proportional to distance traversed within homogeneous terrain. The general problem requires the calculus of variations, but when isotropic terrain is homogeneous except for obstacles (Lozano-Perez and Wesley 1979) or consists of polygonal homogeneous-cost regions (Richbourg et al 1987; Rowe and Richbourg 1990; Mitchell 1988), the problem can be provably reduced to a graph search over a finite graph. "Roads" (Mitchell 1987; Gewali et al 1988), low-cost corridors of negligible width, represent a special case of the latter subproblem in which special efficient algorithms can be devised. But no implementations for the latter case have yet been done.
The work mentioned, and the algorithm of this paper, reason from intrinsic terrain geometry. An alternative is reasoning from a terrain grid as in dynamic-programming "wavefront propagation" methods (Chavez and Meystel 1984; Mitchell and Keirsey 1984) that then search the grid as a graph. While wavefront-propagation algorithms are preferable in many applications, they have several disadvantages (discussed further in (Rowe and Richbourg 1990)) that suggest exploring other algorithms. First, the grid is not likely to correspond to natural terrain features, causing approximation errors which can accumulate. Second, the best grid size may vary considerably over the terrain, and is hard to choose. Third, the algorithms consistently overvalue travel in certain directions; more complex propagation formulae which decrease this bias also reduce resolution. Fourth, the approach is computationally wasteful, as it explores blindly in every possible direction. Thus, we have not used wavefront-propagation methods in our work.
Finding optimal paths across terrain modeled by isotropic homogeneous-cost-per-distance polygonal regions (the "weighted region problem") necessarily requires considerable analysis once a start and goal point are selected. But we show in this paper that when otherwise-uniform terrain contains three special cases of weighted regions--"roads", "rivers", and "obstacles"--considerably more simple preanalysis of the terrain can be done, greatly simplifying subsequent searches. Roads are the limiting case of a low-cost region as its width decreases to zero while its cost-per-distance is held constant; rivers are the limiting case as the width decreases and the cost to cross remains constant (i.e., an impulse function); obstacles are the limiting case as cost increases and dimensions are held constant. The three cases cover many important overland terrain phenomena including highways, paths, transportation lines, power lines, walls and fences, and boundaries of areas of impenetrable vegetation or unsafe surface.
Our approach is to exploit local optimality criteria that optimal paths must follow on such special terrain features. Our basic principle we call the Shortcut Meta-Heuristic, and is the discrete-mathematics analog of the basic principle of the calculus of variations: At any turn on an optimal path from some start point to some goal point, any shortcut across that turn must be impossible or undesirable compared to the optimal-path detour. An immediate corollary is that optimal paths must run straight in the interiors of homogeneous regions because cost accrued there depends only on distance, and a straight line is the shortest distance between two points. We show now that the Shortcut Meta-Heuristic applied to piecewise-linear roads, rivers, and obstacle boundaries says that their vertices are nearly the only places where turns can occur on optimal paths. (We ignore the problem of obtaining piecewise-linear models of terrain features in this paper, as this is a well-studied problem in computational geometry.)
Specifically, let offroad "background" terrain have a cost per unit distance ; let roads have a cost per unit distance ; and let the cost of crossing a river segment be a fixed constant k. (Section 7 generalizes the algorithm to terrain of variable-such parameters.) We prove three important heuristics for optimal paths on roads. We first prove Road Heuristic 1: any turn offroad to onroad or vice versa on an optimal path cannot turn more than the critical angle . Consider a path segment of length L from off a road to a road, a segment that is followed by a turn of angle θ onto the road. If it would be worse to instead go directly to a point some infinitesimal amount ε further down the road, from the same offroad point, . Since ε is small, we can ignore the term under the square root, then take the first two terms of the binomial series for the square root, obtaining , and hence .
We next prove Road Heuristic 2: any onroad-offroad transition on an optimal path cannot form an angle less than C with any road segment (not necessarily the one followed by the path) with an endpoint at the point of transition. Now if the path is truly optimal, it must cost less than shortcutting to some point ε closer on that road segment, and then proceeding from there to the original transition point. The mathematics is the same as above except that ε is subtracted rather than added, giving , and hence .
We finally prove Road Heuristic 3: any turn from one road segment to another road segment on an optimal path cannot be more than 2C, C the critical angle. Consider a shortcut across a road turn that creates an isosceles triangle with the turn vertex. Let θ be the angle of the road turn (so 180-θ is the angle at the vertex of the triangle), and let x be the distance of the turn vertex from either shortcut endpoint. Then the shortcut cost does not improve on the two-part road path between its endpoints if , which implies after simplification.
These three heuristics have four immediate important corollaries. Heuristics 1 and 2 imply that any optimal-path onroad-offroad transition at the middle of a straight road segment must involve a turn of exactly C (Corollary 1, also proved in (Mitchell 1987)). Heuristics 1 and 2 imply that no onroad-offroad transition can occur at the inside of a road bend (two-segment road vertex), since any entry angle that satisfies Heuristic 1 would need to violate Heuristic 2 and vice versa (Corollary 2). Heuristic 1 implies that any offroad-onroad transition at a road "dead end" (a road vertex with only one associated road segment) must involve a turn of less than C (Corollary 3, also proved in (Mitchell 1987)). Finally, Heuristic 2 implies that no optimal path can make an offroad-onroad transition at a road vertex at which all road segments make an angle less than 2C with at least one other segment (Corollary 4).
Now let us turn to rivers and obstacles. With piecewise-linear modeling of them, the Shortcut Meta-Heuristic implies that the optimal path between any two points (not necessarily river or obstacle points) can only turn on rivers and obstacles at their vertices. This is since away from roads in order to be undesirable or impossible, any shortcut across a turn on the optimal path must be guaranteed to cross either an obstacle or a river segment not crossed by the detour on the optimal path. So the turn point P must be infinitesimally close and hence equivalent to an obstacle or river vertex, because the obstacle or river that crosses the shortcut must turn or end to avoid crossing the optimal path.
Furthermore, if P is an obstacle vertex, the turn must enclose the obstacle in its bend (so any shortcut would be sure to cross it), which we will call the Obstacle Heuristic. And if P is a river vertex, the turn must enclose more of the river segments meeting at P than it does not enclose, which we will call the River Heuristic. (Enclosure can include coincidence with river segments.) So at a river endpoint (as at a "bridge"), an optimal-path turn must enclose the endpoint; at a bend in a river, a turn must enclose the vertex and both edges; and at a junction of three or more river edges, a turn must enclose the majority of them. (Let optimal paths always pass slightly to one side of a vertex, so we avoid the issue of how many segments are crossed going right through the vertex.) So for the particularly common case of river-bend vertices, the only headings an optimal path turning there can have are headings between and including those of the two river segments.
It would seem that when following along a river, we would need to keep track of which bank of the river we were on, because we would cross different "tributaries" when following each. But surprisingly, it does not matter to total path cost. The Shortcut Meta-Heuristic requires that anytime an optimal path makes a right turn at a river vertex it must pass clockwise around the vertex, and counterclockwise for a left turn. So to avoid unnecessary crossings, the number of rivers crossed at any turn of the optimal path at a river vertex must be the number of river segments touching at that vertex and lying strictly outside the angle of the turn--except where a clockwise turn to follow a river segment is immediately followed by a counterclockwise turn, or vice versa, because then is it necessary to cross the river segment followed between. (Note the crossing place does not matter to the cost.) So the number of rivers crossed at a river vertex is the number of river segments incident at that vertex but strictly to the outside of the turn at the vertex, plus a "correction factor" of k if the river vertex was reached by following along a river segment and the previous path turn was in the opposite sense (clockwise versus counterclockwise) of the turn at this vertex.
The results of the previous section require that optimal paths over isotropic homogeneous terrain with embedded piecewise-linear roads, rivers, and obstacle boundaries must be themselves piecewise-linear, turning only at road, river, or obstacle vertices, or onto roads at the critical angle determined by the ratio of road and offroad costs. These restrictions mean we can apply a two-phase algorithm to do optimal-path planning on such terrain, the first phase preprocessing the terrain map, and the second phase finding the path between given start and goal points.
Phase 1 Step 1: Model all roads, rivers, and obstacle boundaries by line segments.
Phase 1 Step 2: Augment this graph by all valid optimal "shortcuts" between these features by iterating over all feature vertices, and using the local optimality criteria of section 2. This is the most complicated step; see the discussion below.
Phase 1 Step 3: Prune further the set of candidate road shortcuts by removing all which form an angle less than the critical angle with any road segment at a road vertex (by Road Heuristic 2), and those that do not form an angle more than180-C with some segment at the road vertex (by Road Heuristic 1).
Phase 2 Step 1 (to be done once start and goal points are given): Augment the graph made in phase 1 with start and goal points, and construct optimal shortcuts between them and all road segments, road vertices, river vertices, and obstacle vertices, using the standard pruning criteria.
Phase 2 Step 2: Remove from the graph all those edges and edge portions that lie outside the ellipse whose foci are the start and goal points, whose major axis is the distance between the foci F times the ratio (call it R) of the average cost of the shortest path between the foci to the road cost per distance , and whose minor axis is . (The shortest path is a straight line unless it intersects obstacles, in which case we must do a search like (Lozano-Perez and Wesley 1979.)) The ellipse represents how far the optimal path could travel from the start and goal points at cost rate .
Phase 2 Step 3: Now the problem has been reduced to weighted graph search. Use the A* algorithm. The cost lower bound is the distance to the goal times . The cost upper bound is the cost of the shortest start-to-goal path. Use the road heuristics and corollaries, the Obstacle Heuristic, and the River Heuristic of section 2 to restrict path transitions. Use the obstacle-turn restriction that the turn enclose the obstacle, and the river-turn restriction that the turn enclose more of the river segments meeting at the turn vertex than it excludes. When crossing rivers, add an additional penalty of k for each river crossed, plus the correction factor discussed in section 2.
In all these steps, if mixtures of roads, rivers, and obstacles meet at a vertex, treat the vertex as two or three coincident nonmixture vertices. For efficiency we can combine phase 2 steps 1 and 2, but not phase 1 steps 2 and 3 because later shortcuts can allow pruning of earlier shortcuts. (Mitchell 1987) provides an additional heuristic we could exploit, concerning crossing roads at shallow angles. Bidirectional A* search (de Champeaux and Sint 1977) could help in the phase 2 step 3 since problem is reversible. If still more speed in required in phase 2, we can prune in phase 1 step 3 each candidate shortcut for which a shorter path between its endpoints exists; an A* search can determine this, but this can be computationally expensive to do for every shortcut.
Phase 1 step 2 requires an inner and outer loop. In the outer loop we consider in turn each road, river and obstacle vertex V. Then in an inner loop for V a river or obstacle vertex, we consider in turn each road edge, river vertex, and obstacle vertex to which vertex V might connect directly in an optimal path; in an inner loop for V a road vertex, we consider each road segment as a start of a shortcut to V. Whenever a proposed shortcut is found in either inner loop, it must be confirmed not to intersect an obstacle.
In the rivers-obstacles inner loop, we can eliminate possibilities with the River Heuristic and the Obstacle Heuristic. This means that there can be no shortcuts to the inside of river bends. It also means that no shortcut to the outside of a river bend can occur if a straight-line continuation of the shortcut crosses the river there, and no shortcut to an obstacle vertex can occur if a straight-line continuation of the shortcut enters the obstacle. These are strong restrictions.
Handling of road vertices is more complex. For a non-road vertex V and a road segment E, let the range of headings from V to E be . Then if is one of the headings of E and C is the critical angle, there is a shortcut at heading if it is between and , and at heading if thtat is between them. There is also a shortcut to endpoint vertex W of E at heading if another road segment incident on W has heading and either or is between and (taking in the direction consistent with , i.e. if approaches W along E then leaves W along W1). For a road vertex V and a road segment E, additional constraints must be imposed at V. First, obtain the candidate shortcuts valid according to the preceding mathematics. Then for each shortcut endpoint on E, apply the criteria of the last paragraph in reverse, from the shortcut end on E to V, to further rule out candidates.
We implemented all except phase 2 step 2 of the above algorithm in Edinburgh C-Prolog (for simplicity, we picked test cases where all the data would fall into a phase 2 step 2 ellipse). Figure 1 shows an example of performance in planning between a point in the lower left and a point in the upper right. The offroad cost was 1.4 per inch, the onroad cost was 1 per inch, and the river-crossing cost was 0.2, where in the original scaling the terrain map was about 7 inches high.
Figure 2 shows the results of more extensive test runs on the same terrain. Here the road cost was 2, the offroad cost 1, and the river-crossing cost 1.5. The goal point was on the right side toward the bottom. 1656 (=36x46) evenly-spaced start points were tested. Figure 2 shows the initial direction of the optimal path for each, providing a picture of the optimal-path field for this terrain and goal. Superficially the picture resembles the back-pointer array created by wavefront-propagation path-planning methods, but note the subtle phenomena recorded by permitting an infinity of possible vector directions instead of the typical eight directions of wavefront propagation. Note also that initial path directions only change abruptly in vicinity of strongly competing influences. Such optimal-path fields are investigated further in (Alexander 1989).
(Mitchell, 1987) gives the time complexity of his algorithm for terrain with roads and obstacles of where n is the number of road and obstacle vertices, for an algorithm including phase 1 step 2, phase 2 step 1, and phase 2 step 3. Our algorithm does the same visibility-graph construction and similar examination of river vertices as he does with road vertices, and all our pruning criteria use the same information (edges incident at a vertex) that he does, so our algorithm is also . But this result is not very informative, so we give here an average-case analysis of our algorithm.
The average running time of phase 1 step 1 will be highly terrain-dependent. But the key step 2 of phase 1 is more analyzable. Suppose the result of phase 1 step 1 is E road segments, road vertices, river vertices, and obstacle vertices. Then the inner loop is executed times. Each pass requires some fixed heading checks, plus lookups to find the road and river segments adjacent at vertices. The lookups can be indexed in advance, then done with approximately operations for roads, for rivers. So the time for phase 1 step 2, not counting obstacle-intersection checking, will be proportional to .
The number of shortcuts created in phase 1 step 2 can be estimated by a few reasonable assumptions. Suppose the great majority of road and river vertices are two-segment bends, as is common in modeling real-world roads and rivers. Suppose at a typical bend the road, river, or obstacle boundary turns A degrees. A randomly chosen shortcut heading for a river or obstacle vertex will have a chance of 2A/360 of being permissible there (since it must lie outside the bend, and permit enclosure of the bend by the optimal path). A randomly chosen shortcut heading for a road vertex will have a similar probability of permissibility at that end of the shortcut (for ranges of headings of departure for a shortcut, for the two directions of travel through the turn). A randomly chosen shortcut heading for a road edge will have a success probability proportional to the average angular width, call it W, of a random edge from a random vertex. So the expected number of shortcuts created in phase 1 step is , considerably better for small A than the worst case of .
Now we can complete our analysis of phase 1 step 2: The S shortcuts found will require calls to an intersection routine to check whether they intersect obstacles, with the simplest possible approach. The phase 1 step 3 pruning criteria require negligible effort since they affect only to the rare road junctions at which other than two road segments meet. Both steps 1 and 2 of the phase 2 of the algorithm require iterating over all the vertices to do some simple checks, for operations. Step 1 will add approximately new shortcuts. Step 2 will delete a highly terrain-dependent and problem-dependent number, leaving a number we will call T. Then step 3 is an A* search over the remaining T edges, requiring effort proportional to T log T.
Execution time trades off with resolution of the piecewise-linear modeling. We can assume that during linearization of terrain features, terrain topology is maintained, model road and river vertices are always placed in the middle of road and river locations, obstacle-boundary vertices always placed on obstacles, and junction vertices always placed at the middle of junctions. Nonetheless, the linearization of terrain features causes road, river, and obstacle-boundary segments to be lengthened or shortened compared to the real world, changing estimated path costs. Suppose our algorithm finds an n-segment "optimal" path; for each path segment i following a road, river, or obstacle-boundary segment, we can define (1) , the maximum deviation (nonnegative) of the segment from the real-world feature between its endpoints, and (2) , the excess length of the real-world feature between its endpoints. Many phase 1 step 1 algorithms upper-bound both for the entire map. We can take the cost error of all other path segments to be zero (it does not matter where you cross a road, and we can assume path-followers are smart enough to take the better side of river and obstacle vertices). Again let be the background cost rate, the onroad cost rate, and C the critical angle.
Wherever the path found by the algorithm goes from road to offroad or vice versa, the real-world location of the road might be displaced a maximum of to the far side. And the excess length of the real-world road might be all concentrated in the part of the road segment used by the path. With a river or obstacle boundary, there can only be an error of the second (feature-following) type, but then there is an "detour" alternative path: we could start at one endpoint of the segment, move perpendicular to it, travel parallel to the segment, and then reach the other endpoint by returning distance perpendicularly to the segment; by the definition of , this is guaranteed not to cross a river or actual obstacle boundary provided no other rivers or obstacles are within distance of the model segment. Simlar detours may be necessary when the optimal path merely touches a vertex of a highly curving river or obstacle boundary. In summary, the maximum underestimate in the cost of the optimal path that can be made by our algorithm is just the sum for all path segments of either (1) , for segment i a road segment followed up to a road endpoint; (2) for segment i a road segment used only partially, then followed by an offroad segment not departing from an endpoint; (3) for segment i an offroad segment shortcutting to the interior of a road segment; (4) for segment i a river or obstacle-boundary segment; (5) for segment i touching a river or obstacle vertex but not following a river or obstacle segment, where j ranges over the river or obstacle segments at that vertex; or (6) 0 otherwise.
Wherever the actual road reaches an extreme on the near side of the straight-line model road segment, then the offroad distance saved could be at most (when the true optimal path turns onto a true road when it gets within of the model road segment); and no distance can be saved over a straight line along the model road segment. Following or touching of river and obstacle-boundary segments cannot cost less than the algorithm-computed cost. So the maximum overestimate in the cost of the optimal path that can be made by our algorithm is the summation for all path segments of either (1) , for segment i a road segment used only partially and adjacent to an offroad segment not departing from an endpoint; (2) , for segment i an offroad segment shortcutting to the interior of a road segment; (3) or otherwise 0.
It is straightforward to extend our algorithm to road segments of differing costs . Now the critical angle depends on the segment. In the mathematics of section 3.1, at a road vertex now where segment E has cost and segment W1 has cost , there is a shortcut to the vertex if either is between and or is between and . If river segments have different crossing costs , then the Shortcut Meta-Heuristic requires that an optimal-path turn enclose segments incident on the turn vertex whose sum of is greater than that of those incident but not enclosed. Modified corollaries follow from these modified heuristics.
Alexander, R. 1989 (September). Construction of optimal-path maps for homogeneous-cost-region path-planning problems. Ph.D. Thesis, U.S. Naval Postgraduate School, Department of Computer Science.
de Champeaux, B., and Sint, L. 1977. An improved bi-directional heuristic search algorithm. Journal of the ACM 24: 1777-1791.
Chavez, R. and Meystel, A. 1984 (Atlanta, Ga.). Structure of intelligence for an autonomous vehicle. IEEE International Conference on Robotics and Automation. New York: IEEE, pp. 584-591.
Gewali, L., Meng, A., Mitchell, J., and Ntafos, S. 1988 (June, Urbana Ill.). Path planning in 0/1/infinity weighted regions with applications. Proceedings of the Fourth Annual ACM Symposium on Computational Geometry. New York: ACM, pp. 266-278.
Lozano-Perez, T. and Wesley, M. A. 1979 (October). An algorithm for planning collision-free paths among polyhedral obstacles. Communications of the ACM 22(10): 560-570.
Mitchell, J. S. B. and Keirsey, D. 1984. Planning strategic paths through variable-terrain data. SPIE Conference on Applications of Artificial Intelligence. New York: SPIE, volume 485, pp. 172-179.
Mitchell, J. S. B. 1987 (May). Shortest paths among obstacles, zero-cost regions, and roads. Paper delivered at the Joint National Meeting of TIMS/ORSA, New Orleans, La.
Mitchell, J. S. B. 1988. An algorithmic approach to some problems in terrain navigation. Artificial Intelligence 37: 171-201.
Richbourg, R., Rowe, N., Zyda, M., and McGhee, R. 1987 (March, Raleigh, North Carolina). Solving global two-dimensional routing problems using Snell's Law and A* search. Proceedings of the IEEE International Conference on Robotics and Automation. New York: IEEE, pp. 1631-1636.
Rowe, N. C. and Richbourg, R. F. 1990. An efficient Snell's-law method for optimal-path planning across multiple two-dimensional irregular homogeneous-cost regions. To appear in International Journal of Robotics Research. Also Technical Report NPS52-88-017, Monterey, Calif.: U. S. Naval Postgraduate School, Department of Computer Science.