Optimal grid-free path planning across arbitrarily-contoured terrain with anisotropic friction and gravity effects

Neil C. Rowe and Ron S. Ross

Department of Computer Science

Code 52Rp, Naval Postgraduate School Monterey, CA 93943


Anisotropic (heading-dependent) phenomena arise in the "two-and-one-half-dimensional" path-planning problem of finding minimum-energy routes for a mobile agent across some hilly terrain. We address anisotropic friction and gravity effects, and ranges of impermissible-traversal headings due to overturn danger or power limitations. Our method does not require imposition of a uniform grid, nor average effects in different directions, but reasons about a polyhedral approximation of terrain. It reduces the problem to a finite but provably-optimal set of possibilities, and then uses A* search to find the cost-optimal path. However, the possibilities are not physical locations but path subspaces. Our method exploits the surprising insight that there are only four ways, mathematically simple, to optimally traverse an anisotropic homogeneous region: (1) straight across without braking, the standard isotropic-weighted-region traversal; (2) straight across without braking but as close as possible to a desired impermissible heading; (3) making impermissibility-avoiding switchbacks on the path across a region; and (4) straight across with braking. We prove specific optimality criteria for transitions on the boundaries of regions for each combination of traversal types. These criteria subsume previously published criteria for traversal of isotropic weighted-region terrain. Our method can take considerably less computer time and space than previous methods on some terrain; at the same time, we believe it is the first algorithm to provide truly optimal paths on anisotropic terrain.

This work was supported in part by the U. S. Army Combat Developments Experimentation Center under MIPR ATEC 88-86. Special thanks to Yutaka Kanayama and Robert McGhee. This paper appeared in IEEE Transactions on Robotics and Automation, 6, no. 5 (October 1990), 540-553.  The equations were redrawn in 2008.


We consider the problem of mobile-agent optimal-path planning between some start point and some goal point on terrain with arbitrary contours, using a simple physical model with gravity and friction forces. We address only "high-level" path planning, as for instance outdoors to a resolution of 100 meters or so, for which (1) the agent traversing the path is much smaller than path dimensions, and (2) terrain elevation and rough surface-covering information is available. We treat this as a two-dimensional problem in which we reason in the projection of the surface onto a two-dimensional map plane perpendicular to the direction of gravity. We assume we wish to minimize either energy cost or, when power is constant, traversal time (since then work is power times time). We allow that in certain regions of the terrain (whose boundaries will be defined by polygons), certain directions of travel are impermissible (of infinite cost).

Furthermore, we want a grid-free solution method, one that does not impose a feature-independent grid on the terrain and try to figure how a wavefront would propagate across this grid [9, 3], but tries to reason about terrain features like hills and ridges, only partitioning terrain as absolutely necessary. Grid-based methods are versatile, and preferable to the methods we propose here for a variety of applications, as on very heterogeneous terrain. But they can have several serious disadvantages: (1) potentially much wasted computation, since search for the goal is mostly blind; (2) potentially much wasted space, since homogeneous areas of the terrain require as much storage per unit area as heterogeneous areas; (3) directional biases in the propagation grid giving path-cost errors; (4) accumulation of roundoff errors; (5) digitalization effects causing unreasonable jagged paths where the optimal path should be straight (while jagged paths can be smoothed, there are many kinds of smoothing to choose among, and optimality or even feasibility of the smoothed path cannot be guaranteed); and (6) inability to converge to the optimal solution as grid size increases. [15, 6] discuss these disadvantages in more detail for a related problem. Note that disadvantage (4) is particularly serious for grid algorithms on anisotropic terrain, because repeated rounding to the same side of the recommended path direction, as in near-homogeneous terrain, will force the path further and further afield; on the other hand, increasing the number of angles of propagation effectively decreases the resolution of the grid quickly. Because of this, grid-propagation algorithms cannot provide guaranteed-optimal paths on anisotropic terrain.

Our previous work [15, 14, 12] and that of [8] successfully explored grid-free methods for regions of different internally-homogeneous cost per traversal distance, but assuming isotropism (no effect of travel direction on cost). These techniques can give guaranteed-optimal paths. We extend these approaches in this paper to the most important kinds of anisotropism that arise in overland path planning.

The physical model

The basic energy-cost model

We study minimum-energy paths for an agent (perhaps a mobile robot or army vehicle) moving across some hilly terrain in only a forward direction. (This problem should not be confused with the "geodesic" problem of finding the path that minimizes the surface-travel distance between two points [7].) Assuming that the agent has no net acceleration over the path, and that path turns altogether involve insignificant energy loss, the major external forces operating on the agent are gravity and friction [1, 16]. Then when the agent is traversing a slope of  (positive uphill, negative downhill) in the direction of the gradient (see Figure 1), the sum of the two forces is  using the classic model from physics of a body on an inclined plane, where mg is agent weight and  is the coefficient of friction or specific resistance that is working against the vehicle. (This is derived from a more complex model in [13]). This is the external work per unit of surface distance that must be expended by the vehicle to overcome nature, so the energy cost of a path is its integral over the path. (This formula was confirmed experimentally within 1% for wheeled vehicles on slopes of less than 20% in [16], with  interpreted as the ratio of drawbar pull to vehicle weight.)

However, we must make two changes to the formula to make it general. First, if we are not traveling straight uphill or straight downhill, the normal force is not coplanar with the plane of Figure 1, and depends only on the gradient. Letting  be the inclination angle of the gradient, the sum of the two forces becomes . Second, we can modify this formula to reason about paths in their two-dimensional azimuth projections onto a map plane, and thereby turn path planning into a two-dimensional problem. The perceived length of a path on a slope viewed from directly above is the actual length times the cosine of the slope angle. Then the cost per unit of perceived distance is . Because  and  is very close to 1 for most traversable natural terrain,  is very close to 1, and we will generally ignore it subsequently. Then the energy lost to friction depends only on the length of the path as viewed from above. Furthermore, the work expended against gravity is just the sum over all path segments of the perceived distance times the tangent of the slope for each path segment, or the sum of all the elevation changes; so the total work against gravity is independent of the path between two points, and can be ignored in finding the optimal path. Hence only distance and the coefficient of friction are significant in finding minimum-energy nonbraking paths.

To facilitate mathematical analysis, we assume as in the Army Mobility Model [18] and our own earlier work based on it [12, 15] that the terrain is partitioned into polygonal regions within which the coefficient of friction can be assumed constant; any terrain can be sufficiently closely approximated by some such polygons [10]. For overland terrain, often this can be done from map overlays or classification of surface covering in aerial photography.

Internal energy losses in the agent are often proportional to external work done against gravity and friction (as the heat losses in an engine), multiplying the cost by a constant that can be ignored. Other such losses can be modeled as proportional to path distance (as the friction of wheels on an axle, and wind resistance when the wind is constant-speed parallel to the surface), so the minimum-distance path will minimize these too. The optimal paths in this paper require turns which do add extra energy cost, but it is usually negligible compared to the overall path cost, so we will call it a "second-order" cost. Specifically, if two paths have the same energy or "first-order" cost, the one with the fewest turns is preferred.

Anisotropic phenomena with braking

When the above cost formula becomes negative, the agent is gaining energy in going downhill. This would cause an agent to accelerate, and since there are limits to safe acceleration and safe speed of an agent, we assume braking must occur sufficient to give no net acceleration on any downhill path segment. Braking causes energy loss as heat, energy which cannot be recovered.

For braking-phenomena analysis, we will need reasonably detailed information about surface elevation, like the grid of elevation readings given in Defense Mapping Agency data. Such data can be fitted to an irregular polyhedron by least-squares fitting of planar patches (see Section 6). The projections of polyhedral faces to the two-dimensional map plane define boundaries of constant-gradient regions. These can be further subdivided by intersecting them with the constant-friction regions mentioned earlier, to give subregions with both constant gradient and constant coefficient of friction.

For a polyhedral-face region with gradient inclination (steepness angle) and gradient azimuth (direction in the map plane) , it follows by geometry that the vehicle inclination  at the azimuth  is . For a particular direction of travel  we can compare this with the "critical braking slope" at which braking starts; braking only occurs for downward inclinations more than this. The two symmetric azimuth directions corresponding to this critical slope we will call the "critical braking headings"; together they delimit the "braking range".

Braking requires negligible energy. However, if we use map distance as the cost for nonbraking traversal, we must assign a "virtual cost" to braking episodes to be consistent; braking increases the cost per distance predicted by the formula to zero from the negative number of , so the virtual cost is the negative of this. This is equivalent to replacement of a friction cost for the segment by . This is always positive since braking always occurs downhill, when  is negative. This change in cost formula must occur at the azimuth angles defined by , or when the inclination angle is . Hence the virtual cost of a traversal of distance D is (1)  for nonbraking traversal, and (2) , or the elevation difference, for braking traversal.

Lemma 2.1: The cost formula value for nonbraking traversal  compares to the cost formula value for braking traversal (  the first elevation,   the second) in the following way: (1) for a downhill braking slope, the second value is larger; (2) for a critical braking slope downhill, the two values are equal; (3) for any other slope (nonbraking), the first value is larger. Proof: from the discussion in Section 2, case (1) means , (2) means , and (3) means . By rearranging the inequalities and equation, and observing , the results follow.

Error analysis of the cost calculations

For the outdoor paths that are our primary application interest, Defense Mapping Agency information is quite detailed for the high-level planning we are concerned with. So for these paths, the major source of error in our cost computations will be variation in the coefficient of friction  within regions. These variations will tend to average out for long traversals and be negligible. Errors also arise in the polyhedral fit. This has an effect on heading ranges of permissible traverse, for which the cost error is hard to predict, and in the cost of braking episodes, for which the cost error is the cumulative elevation error and hence is small.

Time-minimizing paths

The nonbraking path that minimizes energy consumption between a start point and a goal point is also the minimum-time nonbraking path under constant power consumption, assuming power is sufficient for all uphill slopes. This is because the velocity and all forces acting against the agent are parallel to the surface, so work is Pt, P the power and t the time. During braking episodes, a time-minimizing agent can travel as fast as is comfortable, call it . It is straightforward to define a "virtual time cost" for braking episodes equal to , which has an extra term representing the nonzero actual time required. Note that the extra term depends on path length, so the turn "second-order" path cost referred to in energy-minizing paths is even more important for time-minimizing paths.

Anisotropic-obstacle phenomena

Anisotropism also arises with "anisotropic obstacles", regions sufficiently steep to absolutely rule out certain directions of travel. Such regions have pairs of "critical impermissibility headings", permissible headings delimiting a range of impermissible azimuth directions for traversal. Again, a polyhedral terrain fit greatly simplifies analysis because these ranges will be the same everywhere on a polyhedral facet.

[16] indicates four conditions can give anisotropic slope limitations. First, an agent may be limited in force expenditure in trying to counteract gravity and friction; call this . For our simple physics model, this means the slope angle cannot exceed . (This was experimentally confirmed within 2% for wheeled vehicles on shallow slopes in [16].) Second, there may be a speed or power limitation on the mobile agent. If we also have a minimum specified velocity for traversal, we can substitute  for  in the preceding.

Third, there may be a danger of loss of traction on a steep surface. Traction is controlled by the coefficient of static friction  which is different from the  coefficient of kinetic friction (or rolling friction for a wheeled vehicle). This static friction must equate all the force being used to push the agent forward, and its maximum is , so an anisotropic traction-loss phenomena will occur when the slope is greater than (after some algebraic rearrangement) approximately .

Fourth, we may experience a catastrophic overturn of the mobile agent on slopes more than a certain steepness. Such overturns occur when the projection of the center of gravity of the agent on the terrain surface lies outside the polygon formed by its support points [5], most often when the vehicle is "sideslope" or perpendicular to uphill. This is an anisotropic phenomenon because the distance of the projection of the center of gravity on a level surface from the boundaries of the support polygon determines the stability when the uphill gradient is oriented in that direction; for instance, a long truck is more stable going uphill than going sideslope. We cannot give a single formula for the anisotropism here because this is specific to the support pattern.

The first three cases above rule out a range of uphill traversal, and the fourth a range of sideslope traversal (while braking phenomena affect a range of downhill traversal). Figure 2 gives an example of the heading ranges concerned. Note the impermissibility constraints can rule out unconnected ranges of headings. Or some of their impermissible ranges may overlap, in which case two critical-impermissibility headings disappear and one larger impermissibility range is created. In some cases all possible headings may be impermissible, giving an isotropic obstacle, the familiar kind in path-planning work.

Somewhat similar anisotropisms arise with time-minimal paths when power is assumed constant. Now the maximum-power constraint does not apply but the velocity-minimum does. Slippage and overturn phenomena are identical to those with energy-minimizing paths.

Related work

The most similar major approach to this problem with which we are familiar is the grid-based one of [3]. Their cost function is much more arbitrary: it is discontinuous with heading, which is hardly reasonable, and the discontinuity, which represents the transition to braking, occurs exactly at sideslope, which is not reasonable on off-road terrain when friction is high. They also seem to ignore the viewing-angle cosine correction described earlier, so their results may be wrong on terrain with nonnegligible slopes.

[9] reports an integrated grid-based path-planning system that uses an anisotropic cost function incorporating a variety of factors. But it seems ad hoc, and no justification for it is given in the paper, nor are any other justifying papers cited.

The Army Mobility Model [18] uses a complicated formula for assigning traversal costs to terrain, and slope is a parameter. However, the Model averages slope in all directions to obtain an average figure for a terrain region, resulting in an isotropic model.

Turn criteria for optimal paths

We now specify local criteria for optimal paths. We will show there are only four ways an optimal path can traverse a homogeneous-friction-coefficient planar anisotropic region (henceforth called just a "region"): type I, straight across at a permissible nonbraking non-critical heading, turning on boundaries according to standard weighted-region constraints [15]; type II, straight across at a critical-impermissibility heading to follow as close as possible to a desired direction; type III, "switchbacks" or "slaloming" alternating arbitrarily between a matched pair of critical-impermissibility headings; and type IV, straight across with braking at a non-critical heading, with the associated braking cost function. Other key results of this section are summarized in Figures 3, 4, and 5. Figure 3 illustrates the four traversal types; Figure 4 gives their properties; and Figure 5 summarizes the transition constraints between traversal types on the boundary between two polyhedral facets (two regions of different surface orientation), the most complicated aspect of our analysis.

Basic definitions and results

Definition: a shortcut is a straight-line segment across a turn on a two-line-segment path P connecting a point on one segment of P with a point on the other segment of P.

In what follows we will exploit the following necessary condition for optimal paths ("Shortcut Suboptimality Condition"): whenever the optimal path turns, it could never be improved by a shortcut across the inside the turn. This is illustrated in Figure 6: if the optimal path from P to Q makes a single turn at R, then the path that starts at P and takes a shortcut must either be impossible due to constraints in the terrain traversed by the shortcut, or the shortcut must cost more than its detour through R. Note any shortcut must have a heading between the two path headings, the heading from P to R and the heading from R to Q. We will also consider more complex ways of improving on a path that turns, as in Lemmas 3.1 and 3.2.

Definition: Let  be the Snell's-Law mapping of heading when going from a region of cost-per-distance to one of , where  and  are angles with respect to the normal of the boundary crossed; that is, the  if it exists. (Note that  and the function is reversible by interchanging the last two arguments: .)

Lemma 3.1: Suppose a path P turns from a heading (with respect to the boundary normal) of  in region 1 to a heading of  in region 2, and at least one part of P is nonbraking. This path cannot be optimal if there exists a heading  between  and   such that both is permissible and nonbraking in region 1, and   is permissible and nonbraking in region 2. Proof: If such a  exists, and the portions of path P in the two regions are nontrivial, there must exist two points A and B on P, A in region 1 and B in region 2, such that traveling from A at heading , and then turning to heading  on the region border, will reach B. That is because if   is left of , a path with starting heading infinitesimally to the right of  and turning by Snell's Law on the boundary must have a heading to the left of  in region 2, and hence can intersect P arbitrarily close to the boundary. The argument is analogous for  to the right. Thus by picking A and B sufficiently close to the region boundary, it is always possible to find a locally-optimal path obeying Snell's Law and departing at  that connects A and B on the - path. Since Snell's-Law paths are the single local optimum among isotropic-weighted-region paths as proved in [11] and [15] the  path must be optimal if the - path is totally nonbraking. Otherwise, if either segment of the latter path is braking, then its cost can only be worse by Lemma 2.1, so the  path is still optimal.

Lemma 3.2: Suppose a path P turns from a heading (with respect to the boundary normal) of  in region 1 to a heading of  in region 2, and at least one part of P is nonbraking. This path cannot be optimal if there exist headings  and  such that ,  ,  is braking in region 1, and  is braking in region 2. It also cannot be optimal if , ,  is braking in the first region, and  is braking in the second region. Proof: Under either of the specified conditions, P could not be any better than a detour path Q that starts on P infinitesimally close to the boundary in region 1, follows heading  to the boundary, and then follows  back to P, because such a path would be braking its entire length. Consider the portion of P that is detoured by Q. If it were entirely braking it would cost the same as Q. Otherwise, if part of it were nonbraking, its total cost would be more than the all-braking cost by Lemma 2.1.

Lemma 3.3: If an optimal path turns within a region, it must turn from a critical impermissibility heading to its matching critical impermissibility heading on the other side of the impermissible heading range. Proof: Use the Shortcut Suboptimality Condition. Any shortcut across such turns must be either impossible or higher-cost-per-distance. It could not be higher-cost if it were braking, by the same argument used in Lemma 3.2: (1) if the detour around the shortcut were braking, it would have the same regular (first-order) cost, but have a turn and hence be undersirable in second-order cost; (2) if the detour is partly nonbraking, that can only increase the cost over the case (1) cost by Lemma 2.1. Hence turns are not desirable on an optimal path within a region unless all shortcuts across the turn are at impermissible headings. This can only be if the entire heading range between the two turn headings is impossible. Hence the two headings of the turn must be a matched pair of critical-impermissibility headings.

Isotropic traversals and anisotropic traversals of type I

As in our previous work [14, 15], we will specify local criteria that an optimal path must obey. First consider isotropic regions. As we have shown in previous work [12, 15, 11], optimal paths in such regions must be straight line segments, with turns only possible on the boundaries of such regions such that the turn obeys Snell's Law, with special modifications necessary at boundary vertices. Identical conditions apply to type-I traversal of anisotropic regions, which we will define as traversal at a non-critical nonbraking heading. Lemma 3.3 requires that a type-I traversal cannot be followed on an optimal path by a turn within a region, or be preceded by such a turn, because the only possible optimal turns involve type-II headings. The following theorem establishes conditions on region boundaries.

Theorem 3.1: A transition between two type-I traversals on the boundary of a region must obey the standard Snell's Law conditions of the isotropic-region case. Proof: the type-I headings by definition must not be infinitesimally close to impermissible or braking headings on both sides. Thus, no matter which direction the turn would be, it could be improved upon in the manner of Lemma 3.1 unless either (1) the turn followed Snell's Law or (2) the path went through a boundary vertex and followed the modification of Snell's Law for there.

Paths in isotropic regions and type-I anisotropic traversals can also "graze" regions, turning at a region vertex to avoid some adjacent region. The Shortcut Suboptimality Condition requires that such avoidance turns enclose either (1) a higher-cost isotropic region or (2) an anisotropic region in which all headings between those on the path before and after the turn are impermissible or braking.

Anisotropic-region traversals of type II

But suppose that obeying Snell's Law on a path entering an anisotropic region would mean an impermissible heading in that region (see Figure 7). The next best thing would seem to be "rounding" that impermissible heading to either one of the matched pair of critical-impermissibility headings surrounding the originally postulated heading. A path could traverse the region at that critical-impermissible heading, and then turn back in the direction it wanted to go originally--a small detour, but sometimes not much. That is a type II traversal. Note type-II paths can be braking as well.

Theorem 3.2: Transitions between type-I and type-II traversal can only occur (1) on region boundaries; (2) when the Snell's-Law mapping of the type-I heading is impermissible or braking in the type-II region; (3) when every heading between starting heading and the Snell's Law reverse mapping of the ending heading is either (a) impermissible or braking in the first region, or (b) its Snell's-Law mapping is impermissible or braking in the second region; and (4) when there are no braking headings  for the first region and  for the second region such that  and  are in opposite senses (clockwise vs. counterclockwise) with respect to their corresponding entry and exit path headings. Proof: Such turns cannot occur within regions by Lemma 3.3, which explains condition (1). On a boundary, there must be some reason a Snell's-Law detour as described in Lemma 3.1 is not possible, so either one segment of such a detour must be impermissible or else one segment must be braking. Hence condition (3) follows, and condition (2) is the special case of (3) due to the definition of a type-I heading. Condition (4) follows from Lemma 3.2.

Theorem 3.3: Transitions between two type-II traversals can only occur subject to conditions (1), (3), and (4) in Theorem 3.2 if at least one heading is nonbraking. If both headings are braking, the conditions are given by Theorem 3.8. Proof: Use the same arguments in Theorem 3.2 for the first case, noting now the properties of type-I traversal cannot be exploited to prove condition (2). For the all-braking second case, the conditions on Theorem 3.8 are satisfied.

Type-II paths can cross boundaries at vertices. However, type-I with type-II transitions through vertices can be handled by the methods for type-I paths through vertices discussed in [11] and [15], and type-II with type-II transitions require such a rare coincidence in two numeric quantities that we will ignore them.

Anisotropic-region traversals of type III

Optimal paths can also turn within anisotropic regions, what we call type-III traversals. For instance, to go up a hill too steep to assault head-on, one can "zigzag" up it in a series of switchbacks. This is possible when transitory occurrence of an impermissible heading in the mobile agent is not dangerous, as when the agent's momentum counters the danger. Lemma 3.3 says that the only optimal such turns are from one critical impermissibility (or type-II) heading to its matching critical impermissibility heading. Since a vector sum is involved, the heading segments may be interleaved arbitrarily without changing the cost (note the three paths in Figure 8 have identical cost), so an infinite number of type-III paths are optimal with respect to first-order cost. To minimize second-order cost, we should use the path with the fewest turns, but that refinement is not essential. Note that if a region has more than one disjoint range of impermissible headings, there can be multiple kinds of type-III traversals, one for each; and the switchback turns can be on region boundaries ("reflection" switchbacks). Note also one heading in a type-III traversal can be braking.

Theorem 3.4: Transitions on region boundaries involving type-III traversals are constrained in the same way as those involving type-II traversals, but where the type-II headings referred to previously can be either of the defining matched-pair headings in its region. Proof: the type-III traversal(s) can terminate at the boundary with either of their defining headings, so either may be taken as the incident heading in a type-II transition. Note that all the conditions on type-II turns refer to the headings immediately adjacent to the boundary, and are not affected by what happens elsewhere on the path.

Anisotropic-region traversals of type IV

Anisotropic-region traversals with braking but without critical impermissibility are type IV. The following are the necessary results.

Theorem 3.5: Optimal paths including Type-IV traversals can not turn within a region. Proof: This follows immediately from Lemma 3.3, since a type-IV heading can not be a critical impermissibility one.

Theorem 3.6: Let  be the coefficient of friction for a type-I traversal, and let  be the elevation inclination angle of the facet edge at the meeting of two polyhedral facets as viewed from the type-I side. Then transitions on a boundary between the type-I and a type-IV traversal require that the type-I heading be either (1) permissible, nonbraking, and form angle  with the normal to the boundary (that angle being to the same side of the normal as the type-IV heading); or (2) only if item (1) does not exist, a heading passing through a boundary endpoint; or (3) only if item (1) does not exist, a permissible critical braking heading. Furthermore, either the type-IV heading is critical-braking or else no path at a braking heading could cross the boundary from the type-I region. Proof: Consider the cost of a path from some point P a distance "a" from the boundary in the non-braking region to some other point Q in the braking region at elevation "h" (see Figure 9). Let the elevation of the edge line be , where d is elevation of the projection of P onto the edge, and x is distance along the edge when viewed from above. Let  be the path angle in the nonbraking region with respect to the normal to the edge when viewed from above. Then the cost of the path from P to Q (necessarily straight except for a turn on the boundary) is . Its derivative with respect to  is , which is zero only when , a single value. This value is a strict minimum because the second derivative there is  which is always positive. If such a heading does not exist, i.e. , or the heading is not a type-I heading, the optimal path must represent an extremum of possibilities, which means case (2) or (3) in the theorem statement. (Note the Snell's-Law mapping function is not involved here because the arcsin formula supplants it.) The last condition in the theorem comes from Lemma 3.2: If the type-IV heading were not critical-braking, then there must exist headings both just to the left and just to the right of it that are braking in the type-IV region. Hence the path could be improved by some detour from a point on the path in the type-I region infinitesimally close to the boundary to a point on the path in the type-IV region infinitesimally close to the boundary, an all-braking detour using a braking heading for the type-I region and a heading just to the left or just to the right of the original type-IV heading for the type-IV region.

Theorem 3.7: For a transition on a boundary between a type-II and a type-IV traversal, the type-II heading (nonbraking by definition) must bound the impermissible range of headings that includes the  heading. Proof: since the proof of Theorem 3.6 showed that special heading is the single local minimum of the cost formula, if that heading is not possible the true minimum must occur at the limit of a feasibility region adjacent to the special heading, which here is the type-II headings bounding the impermissibility region of that special heading.

Theorem 3.8: At a turn on a boundary between two type-IV traversals, (1) both headings must be impermissible or nonbraking in the opposite region, (2) all intermediate headings must be impermissible or nonbraking in at least one region, (3) some intermediate heading must be a critical-braking or braking-critical-impermissible (braking and type-II) for each region, and (4) the aforementioned critical heading for the entry region must be closer to the entry heading than the aforementioned critical heading for the exit region is to the entry heading. Proof: Use the Shortcut Suboptimality Condition. If a shortcut across the two type-IV segments consists of two (as created by the boundary) braking segments itself, then the elevation-difference cost is identical on both paths, but the detour is worse in second-order cost and hence undesirable. So type-IV to type-IV transition turns are optimal only if every shortcut across them is partly nonbraking or partly impermissible, giving condition (2) in the theorem statement. Taking that to the limit proves condition (1), since it is known that the path headings themselves are braking. Hence if heading  in either region is known to be braking and heading  in that same region is known to be nonbraking or impermissible, there must exist some heading between  and  that is a critical braking heading or a braking-critical-impermissibility heading, by their definitions, proving condition (3). Condition (4) occurs because the two braking-heading ranges for the two regions cannot overlap by condition (2).

Completeness of the traversal types

Theorem 3.9: The four traversal types described are the only ways an optimal path can traverse polyhedral terrain subdivided into regions of homogeneous cost-per-distance, given the model of Section 2. Proof: A straight path segment in a region must be either braking (type IV or braking type II by definition) or nonbraking; nonbraking traversal must either avoid critical impermissibility headings (type I) or not. Lemma 3.3 requires that type-I or type-IV paths cannot turn within regions. Theorems 3.2, 3.3, and 3.4 require that the only way critical impermissibility headings can be used on an optimal path is either by paths not turning in the region (type II) or turning between a matched pair of such headings (type III).

Isotropic obstacles

Isotropic obstacles, regions in which no traversal is possible at any heading, have been studied extensively in the context of many robot path-planning problems. It is straightforward to include them in our approach, modeling their boundaries as polygons just like all other regions. By the Shortcut Suboptimality Condition, any optimal path that turns on the boundary of an obstacle must enclose an obstacle vertex in the turn angle. A path can also touch an obstacle without turning if it follows along an obstacle edge.

Reducing path-finding to A* search

For our now-formalized problem, the optimal path between a start and a goal point can be found by A* search. This search need only consider a finite number of possibilities when region boundaries are polygonal, as with the polyhedral-obstacle algorithms of [4] and [17], thanks to the results of the last section and the convexity proof given in the Appendix. This proof shows that for certain well-behaved subspaces of the space of paths between start and goal, there is at most one local-optimum path; hence it can be found by simple iterative optimization with bisection. The well-behaved path subspace required for this theorem is any set of paths crossing the same region-boundary-edge "windows" in the same order and with the same traversal types and subtypes; a "window" can be any region edge or region vertex, and a traversal subtype is a type qualified by the particular heading(s) involved in types II or III (there are no subtypes of I and IV). For example, for an anisotropic region characterized by Figure 2, with three impermissible-heading ranges and a braking range that does not overlap the impermissible-heading range, there are eleven types and subtypes: one type I, six type II, three type III, and one type IV.

The A* search is over all window sequences in the same way as [12, 15]. For those window sequences that terminate at the goal, we can compute the locally-optimal path, and take the minimum-cost of these as the global optimum. This may seem much work for complicated terrain, but many pruning criteria follow from the boundary constraints discussed in Section 3. Any feasible path between start and goal provides an upper bound on the cost of the optimal path, and that prunes high-cost window sequences; we can obtain a feasible path by running a straight line between start and goal, and locally perturbing segments of that line until it avoids obstacles and does not violate anisotropic-region constraints. We can also rank window sequences by the sum of a lower bound on their cost plus a cost-to-goal lower bound from the last window in the sequence. A lower bound on the cost of a window sequence is obtained by adding the region's  times the minimum distance across the region for type I, II, and III traversals, or the minimum elevation difference across the region for type-IV traversals. The cost-to-goal lower bound is just distance to the goal when the window is a point; for an edge, it is the distance to the goal of the closest of three points: the two endpoints, and the perpendicular projection of the goal onto the edge if any.

Visibility analysis

But the most powerful pruning tools are the implicit vertex-visibility and edge-visibility graphs [4] for the anisotropic regions, graphs that indicate when straight lines can connect region vertices or region-boundary edges without intersecting others. Such graphs are straightforward to make once all regions are subdivided into convex subregions. The artificial boundaries introduced in subdivision can be flagged as special and treated as permitting no turns when encountered in the path search.

Visibility analysis can be further improved by considering the possible headings with which an optimal path could cross a window. For each window in a window sequence created during A* search, we can calculate its "shadow" on the next, based on the possible range of headings of the current and previous region traversals. If the next window is an edge, the intersection of the edge with the shadow is the only part of the edge reachable, and this smaller edge narrows the subsequent window sequence; if the intersection is empty, the entire window sequence is impossible. In computing shadows, Type II traversal is easy because the traversal heading is fixed, making the shadow on the exit line a simple linear mapping of the shadow on the entrance edge. For Type III traversal, the path displacement between the two edges must represent a weighted sum of two fixed vectors, so project the leftmost of those vectors from the left side of the entry window, and project the rightmost of those vectors from the right side of the entry window, to obtain the shadow. For Type IV, project the bounding headings of the braking range. For Type I, a variety of shadow constraints apply depending on the traversal types of the adjacent regions; for instance, if two adjacent regions are both traversed by type-I paths, the Snell's Law mapping of the first heading range is the heading range of the second.

Shadows can propagate backward as well as forward, leading to constraint propagation as the term is used in artificial intelligence. For instance, when a type I traversal is followed by a type IV, the type I is restricted to at most four distinct headings, as is the traversal just before the type I, and so on back to the start point. Such backward propagation is particularly helpful when started from the goal point.


We are testing a Common Lisp implementation [13] of our algorithm, using data about three military vehicles, two light trucks and an armored personnel carrier (a tracked vehicle). For such vehicles, isotropic soil failure occurs before anisotropic power limitations, so the only significant anisotropic phenomena are braking and sideslope-overturn failure. Furthermore, for these particular vehicles, turns are slow and hence type-III turns are unsafe. Thus, our implementation only needed to consider traversals of types I, II, and IV in optimal paths. Also note the impermissibility of sideslope traversal on even small inclines means that the  cost term is especially negligible, since the eliminated headings are those in which the term most matters.

Figures 10, 11, and 12 show sample program output screens, for problems on artificial terrain with a uniform coefficient of friction. The terrain contains two pyramidal hills of varying slopes, as illustrated in the side views in Figure 13. The vehicles assumed were two kinds of cargo truck, and the "vehicle profile" box in the upper right shows data on them; the "gradient slope" is the maximum slope in degrees on which truck can travel in some direction, the "contour slope" is the minimum sideways (sideslope) inclination of the truck that can cause sideslope overturn, and the "coasting slope" is the minimum slope at which braking is necessary when heading straight downhill. Reasoning times averaged 23 minutes, which is reasonable considering we have not attempted an efficient implementation. The implementation is described in detail in [13].

As formulated, our path-planning problem is another instance of what [17] calls the "combinatorial shortest path problem." Such problems occur when an optimal permutation of edge or vertex sequences in a graph must be found to solve a problem. Though some special subcases are polynomial-time, as well as some key components like finding line intersections [2], the time for the general problem is  in time where n is the number of vertices in the terrain area under consideration. (This result assumes the iterative solution of the optimization through every window sequence requires constant time.) For these problems, the space complexity is similar to the time complexity. This result is for worst-case analysis; the multitude of pruning heuristics we have given means that our algorithm can be much better on the average than this pessimistic bound.

Thus the algorithm of this paper can only be desirable on a time or space basis compared to grid-based methods if a significant data compression of the terrain can be done in polyhedral modeling, so that n is far smaller than the number of cells in a sufficient-resolution grid. This means terrain with usually-low values of the second derivative of the elevation, as for instance, terrain consisting of ridges with sharp curvatures only at their crests. But even when time and space for an implementation of our algorithm are large, it may still be highly desirable on the basis of solution quality. As we discussed in Section 1, grid-based methods when used with anisotropic cost functions will round headings to a elements of a fixed set, and these errors can accumulate; our paths follow the exactly optimal headings. We also observed in computer experiments that grid-based methods have a more serious problem of failing to find any path at all for goals reachable only at headings close to impermissible. For instance, if there are eight directions of grid propagation, and the sideslope-impermissibility headings ranges in a planar region are between 90 and 180 degrees each, the grid propagation will be unable to find any paths except those straight uphill or downhill; and even if the sideslope-impermissibility heading ranges are between 0 and 90 degrees, an average of one quarter of all possible goal headings will be achievable only by paths that we would classify as type-III, paths that cannot be smoothed because the result would be an impermissible path. Thus we feel our algorithm will be more appropriate than grid-based methods whenever sufficient time is available, as in planning an army operation in advance.

Constructing a polyhedral terrain model

Our methods require a polyhedral approximation of terrain. The graphics literature contains much discussion of surface modeling, and this is not our research focus, so we will only discuss polyhedral modeling briefly here. Algorithms to obtain good, as opposed to adequate, polyhedral approximations (that is, exhibiting significant data compression over the raw elevation data) are complex and not always reliable. Finding a good polyhedral model can be viewed as an optimization problem where one strives to minimize both the number of polyhedral faces and the least-squares deviations of the fits on each face, but there are few heuristics to guide the optimization, and usually many local optima [19]. Testing all possible groupings of known-elevation points into polyhedral faces is impossibly slow. Mere clustering of the terrain based on the gradient gets into trouble on terrain with gradual curvature. And the intersection line of two well-fit planes may not lie in azimuth projection between the two sets of points defining the planes. We achieved best success with the following four-step algorithm.

1. We only need to polyhedrally model terrain sufficiently steep to create anisotropic effects by our model, but not so steep as to be untraversable in any direction. This means terrain with a gradient magnitude between the braking inclination and the isotropic-failure inclination. So a first step extracts all such terrain points and clusters them; only these are considered in subsequent steps.

2. We classify these terrain points by curvatures in the X and Y directions into four classes: convex, concave, flat, or "other" (the curvature threshold being an adjustable parameter). We then cluster similarly-marked points into subregions of the regions in step 1. This improves the chances that the intersection planes created for adjacent sets of points will lie between the points, and is based on greater ease in finding a polyhedral fit for a convex or concave surface, thanks to the two-dimensional extension of Rolle's Theorem.

3. We then perform a split-and-merge algorithm (like Algorithm 6.6 in [10]) using the planar least-squares fit to the elevations as criterion. We subdivide the subregions recursively, approximately in half each time, until the region fit is below a threshold. We then merge adjacent regions wherever the fit remains below the threshold.

4. We then compute the intersection lines of all the least-squares planes for each pair of adjacent regions. We then iteratively adjust the orientations of the planes to minimize a weighted sum of the planar fits and the deviations of the intersection lines from their ideal positions; ideal positions are computed from region-border least-squares lines partitioning the original data points.

Figure 14 shows some hilly terrain near Lower Stony Reservoir, Fort Hunter-Liggett, California, as modeled polyhedrally by our method. (The user interface used with real data is different from that shown previously.) A 40-by-40 grid of elevation readings (obtained from digitizing a map) at 12.5-meter spacings was used, plus some vegetation data. The figure shows portions of two hills (bottom center and upper right) with a flat area between. There are 26 polyhedral facets, of which four are isotropic (meaning that their slopes are all less than the braking slope), one is an obstacle (the vertically shaded area at the bottom right, where the vegetation was dense and the slope was steep), and five are high-cost but passable (the spotted regions, where vegetation is a well-grazed oak forest; their traversal cost rate is assumed to be 1.5 times the cost rate of the non-shaded regions). The gradient of the anisotropic passable regions is indicated by the line segments within each region: the length of each segment indicates gradient magnitude, and the direction indicates gradient direction, with the square indicating the uphill end of the segment. The slope at which sideslope danger occurs was 10 degrees, and the slope at which braking occurs was 2 degrees. The thick solid line crossing the figure is a sample optimal path; S is the start and G is the goal.


We have developed and demonstrated a new grid-free approach to finding optimal paths on terrain with arbitrary elevation variation. Our model addresses the two major forces acting along such paths, gravity and friction, and shows a surprisingly elegant way in which their complexities can be simplified. Our grid-free approach means that terrain features like hills and ridges are the basic units of our analysis, not arbitrarily-placed grid cells, so we may be able to save much time and space over grid methods on terrain whose features can be simply modeled. Reference to terrain features also makes our paths easier to follow in the real world. And furthermore, our methods can solve problems that grid methods cannot, and can give lower-cost paths than those of grid methods, even when the resolution of the grid is made arbitrarily fine. But a truly efficient implementation of our methods remains future work.


[1] M. G. Bekker, Introduction to Terrain-Vehicle Systems, Ann Arbor, MI: University of Michigan Press, 1969.

[2] J. L. Bentley and T. A. Ottmann, "Algorithms for reporting and counting geometric intersections," IEEE Transactions on Computers, C-28, 9, 643-647, September 1979.

[3] D. Gaw and A. Meystel, "Minimum-time navigation of an unmanned mobile robot in a 2-1/2D world with obstacles", Proceedings of the IEEE Conference on Robotics and Automation, 1670-1677, April 1986.

[4] T. Lozano-Perez and M. A. Wesley, "An algorithm for planning collision-free paths among polyhedral obstacles", Communications of the ACM, 22, 10, 560-570, October 1979.

[5] McGhee, R. and Iswandhi, G., "Adaptive Locomotion of a Multilegged Robot over Rough Terrain", IEEE Transactions Systems, Man, and Cybernetics, SMC-9, 4, 176-182, April 1979.

[6] J. S. B. Mitchell and D. Keirsey, "Planning Strategic Paths Through Variable Terrain Data", SPIE, Applications of Artificial Intelligence, volume 485, 172-179, 1984.

[7] J. S. B. Mitchell, D. M. Mount, and C. H. Papadimitriou, "The discrete geodesic problem", SIAM Journal on Computing, 16, 4, 647-668, August 1987.

[8] J. S. B. Mitchell and C. H. Papadimitriou, "The weighted region problem", to appear in Journal of the ACM, 1990.

[9] A. M. Parodi, "Multi-goal real-time global path planning for an autonomous land vehicle using a high-speed graph search processor", Proceedings of the 1985 IEEE Conference on Robotics and Automation, 161-167, 1985.

[10] T. Pavlidis, Graphics and Image Processing, Rockville, MD: Computer Science Press, 1982.

[11] R. Richbourg, "Solving a Class of Spatial Reasoning Problems: Minimal-Cost Path Planning in the Cartesian Plane", Ph. D. Thesis, U. S. Naval Postgraduate School, June 1987 (also Technical Report NPS52-87-021).

[12] R. Richbourg, N. Rowe, M. Zyda, and R. McGhee, "Solving Global Two-Dimensional Routing Problems Using Snell's Law and A* Search", Proceedings of the 1987 IEEE Conference on Robotics and Automation, Raleigh, North Carolina, 1631-1636, February 1987.

[13] R. S. Ross, "Planning Minimum-Energy Paths in an Off-Road Environment with Anisotropic Traversal Costs and Motion Constraints", Ph. D. Thesis, U. S. Naval Postgraduate School, June 1989.

[14] N. C. Rowe, "Roads, Rivers, and Rocks: Optimal Two-Dimensional Route Planning around Linear Features for a Mobile Agent", accepted to International Journal of Robotics Research, to appear 1990.

[15] N. C. Rowe and R. F. Richbourg, "An Efficient Snell's-Law Method for Optimal-Path Planning Across Multiple Homogeneous-Cost Regions", accepted to International Journal of Robotics Research, to appear 1990.

[16] A. A. Rula and C. J. Nuttall, An Analysis of Ground Mobility Models (ANAMOB)", Technical Report M-71-4, U.S. Army Engineer Waterways Experiment Station, Vicksburg, Miss., July 1971.

[17] M. Sharir and A. Schorr, "On shortest paths in polyhedral spaces," SIAM Journal of Computing, 15, 1, 193-215, February 1986.

[18] G. W. Turnage and J. L. Smith, "Adaptation and condensation of Army Mobility Model for cross-country mobility mapping," Technical Report GL-83-12, U. S. Army Corp of Engineers, Waterways Experiment Station, September 1983.

[19] D. Yee, "Three Algorithms for Planar-Path Terrain Modeling", M. S. thesis, U.S. Naval Postgraduate School, technical report NPS52-89-018, June 1988.

Appendix: The convexity theorem

Assume a two-dimensional world consisting of anisotropic and isotropic polygonal regions, each with a uniform coefficient of friction. We prove, over the set of all paths between some start and goal points that cross the same region-boundary edges in the same order, that path cost is a convex function of parameters sufficient to uniquely specify the path. The parameters will be distances along the region-boundary edges. We will first show that the cost of each subpath between region boundaries is convex. (We will do this proof for minimizing energy cost, but the analogous proof for minimizing time cost under constant power is very similar.) Since the coefficient of friction is constant for each subpath, we will ignore it. There are five cases to consider.

Case 1, beginning and ending segments of the path: first suppose the segment traverses an isotropic region or does a type-I traversal of an anisotropic region. Let x represent the distance along the first or last region boundary from the projection of the start or goal point onto its line extension, and assume this projection has length c. The cost between the start or goal and this boundary is  which has a second derivative with respect to every other variable besides x of zero, and with respect to x of , a nonnegative quantity.

For type III nonbraking traversal in either the beginning or ending path segment, let  and  be the two headings, D the distance of the start or goal point from the boundary, and x the distance from the projection of the start or goal on the boundary to the intersection of the path with the boundary. Then the cost (if both headings are nonbraking) is

which is convex because it is a linear function of x. If one heading is braking, that just multiplies its subcost by a constant factor, the ratio of the negative of the slope tangent to , so linearity is preserved.

Type II traversal for the first or last path segment need not be considered in the convexity analysis because its cost is fixed by the critical heading chosen. Type IV or type II braking-traversal cost is just a function of the elevation of the intersection point, which on a planar polyhedral face is a linear function of distance along the boundary, and hence is convex.

Case 2, general isotropic-region and anisotropic-type-I segments: if the two boundaries are not parallel, let x and y be distances along the lines containing the boundary line segments, distances from the intersection of those lines. The law of cosines says that the subpath distance is . Convexity is proved from

by squaring both sides, cancelling terms, rearranging, squaring again, and cancelling terms to get a nonnegative sum of squares; we skip the tedious details. If the region boundaries are parallel at distance D, measure the distances x and y along them relative to some perpendicular line. Then subpath distance is . Convexity follows from


which can be shown by the same methods as the general case.

Case 3, general type-II segments: if the boundaries are not parallel, then by the law of sines the ratio of this subdistance to the distance of the region entry point from the point of intersection of the linear extensions of the two boundary segments is a constant since it is equal to the ratio of constants for the triangle formed by the path and the two region boundaries: the sine of the angle between the two boundary segments and the sine of the angle at the second (exit) boundary. Hence whether or not the heading is braking, the cost can be expressed as ax+b, where x is distance along the first boundary line, a convex function of x; hence cost is a convex function of x. (If the boundaries are parallel, the cost is a constant independent of entry point, so its derivatives are zero.)

Case 4, general type-III segments: Tthe distance across the region does not depend on the locations of the turn points, just the total subpath length allocated to each of the two headings; call these  and . As in case 2, let x and y be distances of the entry and exit points from the intersection point of the extension of the boundaries into lines. Let  and  be the headings of the two region boundaries, and  and  the two (known) subpath headings. Then:


We can solve these equations for  and , and the total distance of the optimal path in the anisotropic region is:

This is a linear function of both x and y, and hence is convex with respect to all boundary-distance variables. (That also covers parallel boundaries, when .) If one subpath heading is braking, that just multiplies its cost by a constant as in case 1, so linearity is preserved.

Case 5, general type-IV segments: These segments must be straight, so we can use the same variables as case (2), distance along the boundaries. The virtual cost is proportional to the difference between the starting and ending elevations. But on a planar polyhedral face, the elevation along a boundary line segment is a linear function of distance along it. Therefore the difference in elevations is just the difference between two linear functions of two variables, and it must be convex in those variables. Parallelism of the boundary segments does not affect this argument.

Hence all the subcosts in a well-behaved subspace of paths are convex functions of the variables (distances along boundaries). Hence their sum is convex. It is true that some of the variables are functions of others (with several isotropic regions in succession, the first two boundary intersections determine all the others, by Snell's Law; with a type II anisotropic traversal, the first entry point determines the exit). However, a restriction of the domain of a convex function that makes some of the variables monotonic functions of the other variables is still convex. The functions here are monotonic because in the case of isotropic or type I anisotropic traversal, Snell's Law gives a monotonic nonlinear mapping of entry angle into exit angle, and hence, entry point onto exit point; and in the case of type II anisotropic traversal, the fixed heading of traversal forces a linear mapping of entry point onto exit point.

Convexity of total path cost means there is at most one path in the set of possible paths between start and goal that is a local minimum with respect to total cost. Hence the local minimum (if any) is the global minimum, and can be found by a simple bisection iteration.

Captions for figures

Figure 1: Summary of our physical model of the path-following agent

Figure 2: Example of impermissible and braking heading ranges at a point on a sloped plane; the six upper rays represent critical- impermissibility headings, the lower two critical-braking headings

Figure 3: The four types of anisotropic-region traversal, demonstrated across a quadrilateral sloped-planar region with friction coefficient homogeneous and identical to its surroundings

Figure 4: Properties of the four types of anisotropic-region traversal

Figure 5: Turn constraints on the optimal path at the boundary of two regions where the path goes from one region to another

Figure 6: Reasoning for the Shortcut Suboptimality Condition

Figure 7: Two type-II traversals, detours from the Snell's-Law mapping (shaded region is anisotropic with a higher coefficient of friction)

Figure 8: Some type-III traversals between two points (shaded region is anisotropic)

Figure 9: Diagram illustrating type-IV traversal (shaded region is anisotropic, assumed traversal type-IV)

Figure 10: Example program output on artificial terrain with two pyramidal hills of varying slopes (S is the start point, G the goal point)

Figure 11: Another example program output

Figure 12: Another example program output

Figure 13: Side view of the hills used in Figures 11, 12, and 13 (lower right hill is viewed from southwest; slopes in other directions on same hill are the same). Numbers are slopes in degrees. Vertical scale exaggerated by a factor of 2.

Figure 14: Polyhedrally-modeled real terrain in Fort Hunter-Liggett, California, with an example optimal path superimposed. S is start, G is goal, and Roman numerals are traversal types.

Go up to paper index