Abstract�This paper describes an efficient stochastic algorithm for planning near-optimal paths for a point agent moving through two-dimensional weighted-region terrain from a specified start point to a specified goal point.� Weighted-region terrain consists of polygonal regions with a constant traversal cost within each region, and models differences in vegetation and terrain that affect traversal.� Our algorithm combines heuristic search with probabilistic optimization by simulated annealing.� A key advantage of our approach is that it can be more easily implemented efficiently by distributed processing than other algorithms.� It finds constrained random perturbations to the sequence of region edges that a class of paths cross, and for each sequence, opti�mizes a convex function to find the locally-optimal path.� Test results show an implementation of our algorithm even on a single processor runs faster than representative implementations of the three major algorithms for this problem, with similar space requirements and a minimal penalty in optimality.
Keywords�path planning, simulated annealing, weighted regions, minimum cost, stochastic algorithms
This paper appeared in the International Workshop on Bio and Intelligent Computing, Fukuoka, Japan, March 2012.
We consider efficient planning of near-minimum-cost paths for a point agent moving through planar weighted-region terrain, the "weighted-region problem" (WRP) [1].� Solving it enables real-time selection of good routes not confined to roads where costs such as danger and difficulty of traversal are important.� The WRP is an important problem for large-grain path planning for autonomous vehicles [2], for overland route-recommendation systems for people, and for laying pipelines and electrical cables.
We assume a path is a connected sequence of line segments between a start point and a goal point.� Weighted-region terrain consists of non-overlapping polygonal regions where each region has a fixed cost ("weight") per unit distance of movement anywhere within that region; obstacles are regions of infinite weight.� Regions are assumed isotropic so traversal cost is independent of the direction of movement.� (Algorithms can be extended to minimum-energy and minimum-time paths on tilted terrain [3]; the change in potential energy between a fixed start and goal� and can usually be ignored in planning a path between them, and traversal friction energy loss is proportional to horizontal distance independent of tilt with the classic coefficient-of-friction model.)� Cost is usually energy consumption, but can be traversal time, cumulative danger, or amount of thinking necessary during traversal.� Recent work on the WRP has explored linear fractionals [4], anisotropic path planning [5], and routing in networks [6].
There is a misperception that the WRP has been "solved" since it has been discussed for a long time, but this is incorrect because current practice heavily uses a particular suboptimal algorithm.� Due to its non-convex solution space, the WRP requires combinatorial search. Most robotic applications of the WRP use "wavefront propagation" which approximates the input map with a grid of a finite number of points. It searches through the grid from the start in a best-first manner until the goal is reached; backpointers give the solution path.� One problem is that a high-resolution grid may be necessary to capture key features of the terrain, and then execution times suffer.�� Speed can be improved for obstacle-avoiding paths by using nonuniform grids like quadtrees and "framed" subspaces [7] but it is unclear how much improvement is possible for the WRP.� Another problem is that the requirement that all paths be fixed to the grid introduces digital bias.� This stair-stepping effect results in unnaturally jagged paths costlier than the optimal solutions [8]; attempts to smooth them may make them still costlier or even infeasible.� [9] and [10] improve on wavefront propagation by searching an irregular grid between points on the borders of weighted regions.� [10] searched by varying the starting direction, but performance can vary widely with the terrain.� Similar methods have used on weighted polyhedral surfaces [11].
Polygonal and grid representation of terrain entails approximation.� However, better accuracy can be obtained with a polygonal model than a grid model with a fixed grid size by subdividing polygons as necessary.� Many factors affecting accuracy of the terrain representation, such as thickness of vegetation and unevenness of the ground, tend to be randomly distributed and will tend to cancel out on long paths.
�Some approaches have tried to obtain an exact algorithm for the WRP using geometric reasoning.� [12] presents a "continuous Dijkstra" algorithm for the WRP that runs in a costly O(n8L) time using O(n3) space, where n is the total number of vertices of the regions and L is the number of bits of the input. [8] developed a similar approach using Snell�s Law and A* search to find the exact solution.� Although [8] had an exponential worst-case time complexity, empirically it had better average performance than [12].
Previous methods for the WRP have rarely used the intuitive human approach of quickly finding a reasonable path and adjusting it to improve it.� We explore this here with simulated annealing [13].� It is a kind of optimization that makes use of analogies to thermodynamics in exploring "low-energy" perturbations to find a solution in a large state space.� It has mostly been applied to NP-hard problems, and it can have large running times.� But our experiments indicate that the WRP is well suited for annealing and it finds near-optimal paths quickly, consistent with other stochastic algorithms for path planning [14].� We have adapted it to create a new algorithm, "path annealing".
Simulated annealing is a stochastic adaptive search algorithm inspired by what molecules do when cooling from a liquid state to a solid state [15].� Random perturbations of states create an agenda of possibilities.� Rather than the best option being chosen from the agenda at each step as with A* search, a random choice is made among the best options with probability increasing with the quality of the option.� As time proceeds, the bias towards choosing the best state on the agenda increases, aiding convergence.� Choosing other than the best state helps optimization to better escape local minima than in A* search, and permits surprise discoveries similar to those of genetic algorithms.� Stochastic algorithms like simulated annealing have done better than deterministic algorithms for problems such as sorting, the traveling-salesman problem, and optimization of military battle plans.
We can turn the WRP into a mostly combinatorial problem with the idea of a "window sequence", a set of cost-region edges ("windows") that a set of paths crosses in sequence between a start point and a goal point (Figure 1).� An infinite set of paths can traverse a given window sequence by crossing the edges at different places, but a unique path of locally minimum cost must exist and can be found by convex optimization [8].� Our approach will consider window sequences as states in annealing and search for the best one, then return the optimal path through it.
Figure 1. Example window sequence.
Without loss of generality, we assume that nonconvex weighted regions are partitioned into convex ones along artificial edges, and we assume obstacle edges bound the terrain.� For calculation, we store for each edge its identifier, defining vertices, linear equation, midpoint, length, direction, two associated regions, and associated-region weights.� A vertex edge-list stores a clockwise ordered list of edges incident on each vertex; an obstacle edge-list stores the edges incident on the obstacle; a region edge-list stores the boundary edges for each region; and a region vertex-list stores the vertices around each region.
Combinatorial optimization benefits from an upper bound on cost.� Since a straight path from start to goal may be high in cost, we obtain a path by an A* search on a graph of the midpoints of every traversable edge, using as lower-bound cost to the goal the straight-line distance times the map�s lowest weight.� This cost of this path provides a guaranteed bounding ellipse for the optimal WRP path with the start and goal points as its foci [8].
Path annealing requires assigning costs to window
sequences.� We use the costs of the locally optimal paths within them.� Snell�s
Law from optics applies to the WRP and entails that the optimal path through a
window sequence is straight except for turns on boundaries between weighted
regions, where the turn is a function of the ratio of the costs.� Finding a
path that obeys Snell's Law within a sequence is a continuous optimization
problem with the variables being the turn points 
�along each region boundary.�
One method is a coordinate descent method of "single-path relaxation"
(SPR).� Beginning with a path through midpoints of the edges, we adjust �to their
locally optimal locations along alternate region-boundary edges from start to
goal.� This process repeats, alternating the edges and direction considered,
until no point needs to be adjusted more than some tolerance.� This is guaranteed
to converge because it is optimizing a convex function.� To adjust each �we could
do iterative search [16] but chose table lookup to maximize speed.� We
parameterize this table with the ratio of region weights along the region boundary,
the angle the line between 
�and 
�makes with respect
to the region boundary, and the relative location of the crossing point of this
line.� The path must go through an endpoint of the region-boundary edge when
the line between �and �lies outside the
edge.
An alternative for approximating the optimal path through a window sequence is "uniform discrete-point" (UDP) search, branch-and-bound search restricted to points uniformly spaced along the region-boundary edges like in [9].� We had best results when the resulting subdivisions of the edge were approximately the average length of a weighted-region edge in the entire map so that feature resolution was roughly constant.� We use UDP first to find an approximately optimal path, and then a few cycles of SPR to iteratively improve it.� We found this markedly improved the accuracy of UDP alone while greatly improving the total speed from that of SPR alone since SPR could start much closer to the true optimum.� UDP took most of the time when used with SPR.
"Reentry" behavior occurs when a path travels from a high-cost region to the edge of a low-cost region, turning along it at the Snell�s-Law critical angle, traveling along the edge while incurring the lower weight, and reentering the high-cost region at the critical angle.� The critical angle is associated with total internal reflection in optics and is the arccosine of the ratio of the cost of the low-cost region to the cost of the high-cost region.� Such behavior can be part of globally optimal paths, particularly when start and goal lie in the same high-cost region.� Reentry presents no special problem for UDP since we allow paths along an edge.� For SPR, entry and exit of the reentry edge can be inferred by trigonometry using the critical angle without iteration.
A key to simulated annealing is its "move generator".� Path annealing generates window sequences semi-randomly with a move generator that modifies a few edges of one window sequence to get another.� The most useful move is "vertex rotation", replacing an edge E in a window sequence by the ordered list of the other edges meeting at one of E's endpoints, provided there are no obstacles or map boundaries there. This routes the path around the "other side" of the endpoint.� We then eliminate any subsequences of the new window sequence that are bounded by the same pair of edges to prevent the new window sequence from crossing itself.� We generalize vertex rotation to "obstacle rotation" by treating the obstacles as big vertices: We replace an edge incident on the obstacle with the list of all other edges incident on the obstacle.� [17] proves that all window sequences that do not enter the same cost region more than once are obtainable by some sequence of vertex and obstacle rotations from any of their members.
Another possible move is "reentry insertion" along an edge (see Figure 2).� An optimal path cannot reenter successively on two boundary edges since [12] proves that between any two critical-angle turns the path must pass through at least one vertex.� But an optimal path can cross several boundary edges, "reflect" at the critical angle, then double back across the same edges in reverse order (Figure 3).� This can be achieved with annealing by installing reentry paths within an edge on which a reentry already occurs.� Optimal paths with reentry occurred only 5% of the time in our experiments, but reentry introduction can significantly reduce path cost.�
Figure 2. Example of installation of a reentry window sequence.
Figure 3. A reentry path that crosses edges multiple times.
Vertex rotation and reentry installation are not sufficient to find globally optimal paths in a few rare situations involving narrow regions along which the weights vary considerably [17], but such cases can be eliminated by� partitioning narrow regions.� Figure 4 shows an example where an optimal path needs to visit an edge more than once.
Figure 4. An optimal path can follow an edge more than once (numbers are costs per unit distance).
Our algorithm is well suited for distributed implementation with a shared map.� This is not true of wavefront propagation which requires significant communication between adjacent cells to determine the best path to each cell.� Our algorithm is recursively splitting the space of all paths departing from the start point, and each partition can be assigned a processor, then boundaries and cost data reported at a central location when each partition is done.� Since path-space partition is done in a geometrically justified way, partitions rarely intersect.� Exceptions occur only when a partition terminates because its boundary paths cross due to obstacles and reflection as discussed above, in which case its two adjacent partitions need to be checked for intersection.� That means that the processors for different partitions rarely need to communicate.� Processors will need to request help from new processors when splitting the path space around a terrain feature, but this is a one-way transmission of information (a broadcast).� Uniform grids where each processor is assigned to a terrain cell of the same size tend to have unbalanced loads because some areas of terrain tend to have many more path-relevant features than others.� Our algorithm focuses on terrain features, and will automatically allocate more processing resources to areas of the terrain that have more of them, so it will tend to have good load balancing.
Annealing requires varying "temperature" over time, the degree of randomness in the search.� For path annealing, a newly found window sequence of less cost than the current sequence is always accepted, and otherwise is accepted with probability P = e-DC/T� where DC is the new cost minus the old cost and T is the temperature.� Initially, it is desirable that new sequences be explored frequently.�� To do this we set an initial temperature that will accept 90% of them [14]; for our test maps this was around 50% more than the cost of the initial path.� More sophisticated "cooling" schemes have been devised [18], but we found adequate the simpler approach in [13].� The rate at which temperature decreases is made inversely proportional to the number of choices available [14], so that search is uniformly thorough on all sizes of problems.� This size should be roughly the number of edges in an average window sequence, which can be initially estimated from the average number of region edges crossed by a straight segment whose length is the diameter of the map.� We adjusted the constant of proportionality from sample maps so that the optimal path was found 99% of the time when runs continued indefinitely.� Then for that 99%, we chose a stopping temperature that terminated runs with the correct answer with a 99% success rate.
To test our path annealing algorithm, we used a single processor, seven test maps, and many start and goal points within each map (avoiding points very close to the map border where the initial branching factor is unrealistically low).� Each map had around 200 regions; [17] provides more details of the experiments.� Maps 1 and 2 were manually designed to suggest natural terrain; Map 1 was used in [8], permitting performance comparison.� Maps 3 and 4 were constructed from a 1000 by 1000 meter square of terrain at Fort Hunter-Liggett, California, from data provided by the U.S. National Imagery and Mapping Agency.� Map 3 represented a wet season during which a reservoir is full, and Map 4 represented a dry season.� The geometry and weights of these maps were confirmed by a site visit in which we rode in an all-terrain vehicle over a sampling of the terrain in the map area.� Weights represent the cost in energy of traversing the terrain and incorporate factors of vegetation and surface covering.� To forestall the criticism that such weights were especially favorable to path annealing, we also created Maps 5, 6, and 7 by assigning random weights to the regions in Map 4; the new weights were drawn from� integers 1 to 10 (Map 5), odd digits (Map 6), and {1, 4, 7} (Map 7).
���� We implemented our algorithms in Prolog with some mathematical computations in C.� We also implemented two control algorithms.� One was a wavefront propagation on an 8-neighbor graph corresponding to a uniform square grid of 100 by 100 cells.� The other was "uniform-discrete-point global A* search" (UDPGA*), a version of the UDP algorithm discussed earlier that did A* search between points uniformly spaced at distance d along each weighted-region edge within the bounding ellipse for the start and goal point; we experimented with different values of d to determine its effect.� For each weighted region, the points on its edges were interconnected by straight-line segments, to form the search graph; the bounding ellipse for simulated annealing was used as described in section II.B.� We did not experimentally compare path annealing to the exact solution algorithms of [12], [7], and the algorithm for the generalization of the WRP to any-start problems of [19] since all were in our experience much slower than wavefront prop�agation due to the required large number of operations in computational geometry.
���� The number of vertices or edges in the map is not a good metric of WRP problem size since the choice of start and goal point is critical.� We used instead the total number of boundary edges reached by the initial A* search through midpoints, which approximates the number of boundary edges inside an initial bounding ellipse.� Execution time was measured during search only, and did not include time to preprocess the map and create initial data structures, which we assume need only be done once for many path-findings in the same terrain area and thus can be ignored.
Our simple implementation of wavefront propagation ran much slower than both UDPGA* and path annealing even with a relatively coarse 100 by 100 grid.� It ran around 60 times slower than UDPGA* for a 40-edge problem on the average, and still worse than path annealing, for tests on Map 1 not even including the cost of building the search graph.� The performance disadvantage of wavefront propagation in�creased for larger problems too.� Some of this is due to the inability of wavefront propagation to use a bounding ellipse, since it had no initial solution, but mostly it is due to the uniform grid having too much detail for most of the map and too little detail in a few places; it also required considerable storage for the graph.� A nonuniform grid could help, but would have difficulty improving speed by a factor of 60.� In nearly every case UDPGA* got a better solution than wavefront did because of the latter�s "stair-stepping" (Figure 5; gray regions are obstacles).
���� We compared performance of path annealing with UDPGA* on 66 distinct start-goal pairs for each map.� On Map 2, path annealing got the same answer as UDPGA* in 55 cases, and its paths averaged about 0.5% more in cost, a negligible amount.� At the same time, path annealing ran an average of 2.5 times faster than UDPGA* except for two cases.� Computer memory requirements were similar for both algorithms.� Tests on real terrain of Maps 3 and 4 were consistent: Costs averaged 0.5% more than UDPGA*, path annealing ran 3 times faster, and memory requirements were identical.� These maps had about three times as many edges as Map1 so a slower rate of annealing was calculated.� Note again that the time expended by annealing grows significantly more slowly with problem size than the time expended by UDPGA*, and appeared near linear with problem size in our experiments (see [17] for details).
���� On the maps with weights of higher variability like maps 5, 6, and 7, the more numerous and deeper local minima did not tax the ability of path annealing to find good solutions, contrary to our supposition.� We saw more variation in performance on Map 7 (Figure 6) but the points below the horizontal axis indicate that UDPGA* also had trouble finding good solutions for this unusually variable map.
Figure 5. Example routes found by path annealing (thick lines) with comparison paths from wavefront propagation (thinner lines); numbers indicate region traversal costs per unit distance.
Figure 6. Map7 solution cost ratio versus time ratio (path annealing / UDPGA*)
Our path annealing algorithm for the WRP showed far superior performance to that of a direct algorithm like [8].� It is also much better than the simplest form of the most popular algorithm for the WRP in robotics, grid-based wavefront propagation, in both speed and accuracy.� However, improvements to wavefront propagation like hierarchical searching and nonuniform grids [7] were not tested.� Compared to graph search on an irregular graph with points at terrain features, path annealing still is faster by a factor of at least 3 in our experiments with real outdoor terrain while using the same amount of space and having the same accuracy.� This may reflect its use of additional geometric concepts like "long straight boundary".
��� While path annealing like many stochastic algorithms is NP-hard in complexity and not guaranteed to find the optimal solution, our experiments showed it had little difficulty finding it most of time for a wide range of� parameters and problems including real terrain.� ��Although we have not yet tested it, a distributed implementation of our algorithm should permit significant speed improvements with multiple processors sharing terrain data, something more difficult with graph search [20].� Further speed improvements are also possible by adjusting the annealing schedule.� Path annealing thus appears to be a good way to solve the WRP for real-world applications.
This work was supported by the Navy Center for Applied Research in Artificial Intelligence and by the Naval Postgraduate School under funds provided by the Chief for Naval Operations.�� Views expressed are those of the authors and do not represent those of the U.S. Government.
[1] J. Mitchell, "Geometric Shortest Paths and Network Optimization," in J.-R. Sack and J. Urrutia, (Eds.), Handbook of Computational Geometry (Amsterdam: North-Holland, 1998, pp. 633-701).
[2] C. Goerzen, Z. Kong, and B. Mettler, "A Survey of Motion Planning Algorithms from the Perspective of Autonomous UAV Guidance," Journal of Intelligent and Robotic Systems, Vol. 57, pp. 1-4, January 2010.
[3] N. Rowe, "Obtaining Optimal Mobile-Robot Paths With Non-Smooth Anisotropic Cost Functions Using Qualitative-State Reasoning," International Journal of Robotics Research, Vol. 16, No. 3, 1997, pp. 375-399.
[4] Y. Cheung, O. Daescu, and A. Kurdia, "A New Modeling for Finding Optimal Weighted Distances," Proc. Intl. Conf. on Biocomputation, Bioinformatics, and Biomedical Technologies, June-July 2008, pp. 41-46.
[5] S.-W. Cheng, H.-S. Na, A. Vigneron, and Y. Wang, "Approximate Shortest Paths in Anisotropic Regions," SIAM Journal on Computing, Vol. 38, No. 3, June 2008.
[6] O. Daescu, Y. Cheung, and J. Palmer, "Parallel Optimal Weighted Links," in Transactions on Computational Science III, Berlin: Springer-Verlag, 2009.
[7] R. Szczerba, D. Chen, and J. Uhran, "Planning Shortest Paths Among 2-D And 3-D Weighted Regions Using Framed-Subspaces," �International Journal of Robotics Re�search, Vol. 17, No. 5, 1998, pp. 531-546.
[8] N. Rowe and R. Richbourg, �An Efficient Snell's-Law Method For Optimal-Path Planning Across Multiple Two-Dimensional Irregular Homogeneous-Cost Regions," International Journal of Robotics Research, Vol. 9, No. 6, 1990,� pp. 48-66.
[9] C. Mata and J. Mitchell, �A New Algorithm For Computing Shortest Paths In Weighted Planar Subdivisions," Proc. 13th Symposium on Computational Geometry, 1997, pp. 264-273.
[10] J. Reif and Z. Sun, "An Efficient Approximation Algorithm Of Weighted Region Shortest Path Problem," Proc. 4th International Workshop on the Foundations of Robotics, 2000, pp. 191-203.
[11] M. Lanthier, A. Maheshwari, and J.-R. Sack, "Approximating Weighted Shortest Paths on Polyhederal Surfaces," Proc. 13th Symposium on Computational Geometry, 1997, pp. 274-283.
[12] J. Mitchell and C. Papadimitriou, C. H., "The Weighted Region Problem: Finding Shortest Paths Through A Weighted Planar Subdivision,"� Journal of the ACM, Vol. 38, No. 1, 1991, pp. 18-73.
[13] D. Johnson, C. Aragon, L. McGeoch, and C. Schevon, "Optimization By Simulated Annealing: An Experimental Evaluation, Part I, Graph Partitioning," Operations Research, Vol. 37, pp. 865-892.
[14] S. Chien, Z. Yang, and E. Zhou, "Genetic Algorithm Approach For Transit Route Planning And Design," Journal of Transportation Engineering, Vol. 127, No. 3, 2001, pp. 200-207.
[15] E. Aarts and J. Korst, Simulated Annealing And Botzmann Machines (New York: Wiley, 1989).
[16] T. Smith, G. Peng, and P. Gahinet, "Asynchronous, Iterative, And Parallel Procedures For Solving The Weighted Region Least Cost Path Problem," Geographical Analysis, Vol. 21, 1989, pp. 147-166.
[17] M. Kindl, "A Stochastic Approach To Path Planning In The Weighted-Region Problem," doctoral dissertation, U.S. Naval Postgraduate School, Monterey, CA, 1991.
[18] M. Park and Y. Kim, "A Systematic Procedure For Setting Parameters In Simulated Annealing Algorithms," Computers and Operations Research, Vol. 25, 1998, pp. 207-217.
[19] N. Rowe and R. Alexander, "Finding Optimal-Path Maps For Path Planning Across Weighted Regions," International Journal of Robotics Research, Vol. 19, No. 2, 2000, pp. 83-95.
[20] R. Murphy, K. Hughes, and E. Noll, "An Explicit Path Planner To Facilitate Reactive Control And Terrain Preferences," Proc. IEEE International Conference on Robotics and Automation, Minneapolis, MN, April 1996, Vol. 3, pp. 2067-2072.