# «The Robotics Institute Carnegie Mellon University Pittsburgh, PA 15213 May 13, 2005 Submitted in partial fulfillment of the requirements for the ...»

## RELATED WORK

The re-planning behaviors of D* and D* Lite are ideal for mission-directed path planning. However, neither enables global constraint satisfaction, nor are they optimized for use in state spaces of dimensionality greater than two.A large body of research addresses the associated problem of planning under uncertainty, both for path planning and more generally. Though a treatment of uncertainty is not the focus of this thesis, notable path planning related works include those by LaValle and Hutchinson [37], Takeda, Facchinetti and Latombe [69], Wellman, Ford and Larson [76], Roy et al [55], Ferguson and Stentz [14] and Gonzalez and Stentz [21].

2.6 Path Planning Under Global Constraints Standard heuristic search algorithms, like A*, assume the path metric can be specified in terms of a scalar-valued objective function that encodes cost over paths. However, in many applications, several disparate factors contribute to cost, promoting many to use a sum of weighted costs formulation. However, if any of the costs are non-linear, it is not immediately clear how to select weights in order to prioritize the factors to match the objectives of the search. A more natural approach is to specify constraints on these factors, and to plan paths that satisfy the constraints over the path. The following research is directed towards this approach.

Logan and Alechina [40] describe an extension of A* called A* with bounded costs, or ABC. ABC allows inequality constraints (i.e. cost X), equality constraints (feature = Y), and optimality constraints (cost optimum + ε ) on a list of path costs that are tracked through each path transition. Using a user-supplied priority on these constraints, and an admissible heuristic on costs, ABC follows an algorithm very similar to A* that is both complete and optimal1. A path p is preferred to path p’ if it has the same or better values for all of its cost functions. Distinct from A*, if both paths terminate in the same state, then p will dominate p’, and hence will always represent an equal or better solution than will p’. In such cases, the dominated states can be legally removed from consideration while preserving optimality and completeness. All undominated paths must be tracked through the search. In under-constrained problems, the algorithm will add slack in prioritized constraints. The advantage of this approach is a clear flexibility in specifying the requirements for path under a framework that is natural for many applications. However, it is not clear from the paper how the computational and storage complexity increases with numbers and types of constraints.

Stentz also addresses the global path constraint problem, and adds a rapid re-planning feature to address applications in environments with incomplete or uncertain information [66]. The CD* algorithm uses a weighting factor to balance an optimality cost and a feasibility (global constraint) cost. CD* performs a binary search on the weighting factor to find the path that is optimal in the weight space under the global constraint. At each depth in the tree CD* uses D* [65] to plan an optimal path that satisfies the feasibility cost to within a weight factor error that decreases by a facIn fact, in the degenerate case where ABC has a single optimization constraint, its operation is identical to that of A*

tor of two, on average, with each iteration. In re-planning, CD* re-uses the graphs created by D* in the initial binary search to find a new weight, taking advantage of the fast re-planning characteristics of D*. Changing the weight of a D* graph is equivalent to re-specifying all the costs in the graph, eliminating the benefits of D*. Often, very few of the weights change with updates to the costs, and so CD* is often far more efficient than planning from scratch with A*. At the worst case, the weight must be adjusted at the root of the binary tree, forcing CD* to plan the entire problem from scratch. The author expects that an extension to multiple constraints will add a factor of the binary search depth D in run-time complexity.

Finally, under an approach similar to D* itself, Stentz and Tompkins offer an algorithm called Incremental Search Engine (Chapter 3) that provides optimal planning and re-planning under multiple global constraints in high-dimensional state spaces, yet with greater predictability in performance than CD*. As with D*, ISE runs in a fashion very similar to A* for initial planning, and re-plans efficiently because it determines which portions of the search space are affected by new information and limits the recomputation to those portions. ISE is space efficient through the use of dynamic state generation, state dominance and pruning within resolution equivalence classes.

Modeling constraints is essential for managing resources. The ABC algorithm is attractive because it appears to allow a very flexible means of specifying constraints on a path planning problem, and achieving combinations of optimality, constraints and slack. However, because it mimics the A* algorithm, changes to underlying models will often entail full re-plans over the state space. In this regard, CD* is an interesting alternative. However, its current restriction to 2-D search and single constraints may limit its value for the high dimensionality of this problem. Of the three approaches, ISE is the most relevant to this research because it combines optimal and complete search, compatibility with multi-dimensional state spaces and fast re-planning. As a result, it was adopted for use in this research.

Chapter 3 describes ISE in full detail and demonstrates its performance under varying conditions.

2.7 Applied Path Planning: Natural Terrain Roboticists have applied path planning to the problem of finding safe and optimal paths through rough terrain, both for military and space applications. The majority of research has focused on the local terrain problem, though a few efforts have developed approaches for global, long-distance planning. Note that the primary research issue in these examples is to measure and represent natural terrain, and that finding the plan itself invokes a more general path search algorithm.

2.7.1 Local Path Planning In the local problem, knowledge of the terrain is typically limited to that derived from rover sensor measurements, and hence is limited to areas immediately around the rover path. Rover navigation schemes often interleave sensing, planning and execution at high frequencies to re-assess local rocks in the context of finding a minimum-cost path to a

## RELATED WORK

global goal position. Furthermore, local methods often represent the rover’s volume (as opposed to representing the rover as a point), kinematic constraints, and occasionally, dynamic constraints.Howard and colleagues present a fuzzy logic terrain classification methodology for mobile robot navigation [24].

Using vision sensing, the approach loosely quantifies several terrain characteristics - terrain roughness, slope, discontinuity, and hardness - using fuzzy variables. The method composes the resulting vector of fuzzy variables and applies a set of rules to classify the traversability of the terrain. The authors claim a greater stability in classifying terrain traversability using noisy sensors than with analytical approaches.

Morphin, a local path planner presented originally by Simmons et al., and in refined form by Singh et al. [59], uses data from stereo vision or other 3D sensors to track local terrain traversability. A terrain classifier uses stereo range data to derive a “goodness” measure comprising ground plane orientation and surface roughness, and measurement certainty, over grid cells approximately the size of the robot (see Figure 2-1). Morphin merges goodness maps over time, by taking the weighted average of the goodness values, scaled by the certainty measures. It de-values older data by reducing their certainty measures as a function of distance traveled since the measurements. It projects a predetermined set of discrete drive arcs over this local map and computes the goodness and certainty of each arc. Arcs intersecting obstacles or entering entirely unknown regions are vetoed. The overall value of an arc is computed as the multiplication of its goodness and certainty. This local scheme has been successfully demonstrated, in conjunction with a D*-based global path planner, on a CMU ATRV robot [59], on the CMU Hyperion rover [78], and for tens of

kilometers on a Honda ATV automated for the DARPA PerceptOR program [32]. A modified version of Morphin, called GESTALT, provides local navigational autonomy for the NASA Mars Exploration Rovers [19].

Both RoverBug and the Morphin-based approaches coarsely categorize terrain for traversability, but generally avoid the roughest terrain. Other approaches are working to plan paths over rough terrain, and consider a greater number of factors including vehicle kinematics and dynamics. LAAS has developed a method for planning over rough terrain in terms of rover kinematic constraints, described in Hait and Simeon [22]. Assuming an a priori model of the terrain, the method estimates how a rover will sit on terrain by “placing” the rover over the entire discretized terrain map.

Given estimated wheel contact, the software checks for violations in joint limits and under-body clearance. Those cells where placement yields no violations are used for planning using terrain roughness and distance metrics.

Kinematics often impose constraints that must be considered in planning local paths. With automated fork trucks in mind, Kelly and Nagy developed an approach for generating non-holonomic trajectories reactively [31]. Rather than solve the general non-holonomic path planning problem, their software operates on polynomial spiral trajectories.

The spiral primitive has the advantage of roughly spanning the space of feasible steering controls while being described by only two parameters. Optimal control laws on the polynomial coefficient parameters transform to a nonlinear programming problem which can be solved very quickly on the spiral primitives. Their results display an ability to generate and evaluate trajectories in under one millisecond. This approach was successfully adapted for use in rough terrain navigation for the DARPA PerceptOR program [32].

Researchers at MIT are developing physics-based motion planners for planetary rovers that consider vehicle statics, kinematics and dynamics and soil mechanical properties [25]. This approach hopes to provide a physics-grounded means of planning paths in rough terrain such as steep embankments where simple heuristic classifications of terrain might fail. Research by Urmson is integrating randomized kinodynamic projections of the rover state to path planning to enable travel over rough terrain that requires vehicle inertia to succeed [72].

Wellington and Stentz present an adaptive technique for local navigation through vegetated terrain [75]. In the presence of tall grass, bushes and other vegetation on terrain in front of a vehicle, laser scanning and radar data cannot easily deduce the height of the weight bearing surface. Some measurements reflect the full height of the vegetation, while others may represent mid-level branches and possibly the ground. The approach demonstrates both off-line and on-line learning to determine the height of the ground from this ambiguous data. With an estimated map of the ground height derived from learned experience, software samples from the range of possible future control actions, taking into account steering dynamics, to project a set of possible drive arcs. The software “places” a kinematic model of the vehicle onto the terrain model at regular intervals along the arcs to test for violations of roll and pitch

## RELATED WORK

limits, mechanism limits, and underbody ground contact (similar to [22]). A simple planner selects the non-violating arc that produces the lowest cost to a given goal.All the planners mentioned above are solutions to varying forms of the local path planning problem. Their sensorbased models of rover-scale terrain are of limited use in over-the-horizon navigation or in sensing, modeling or identifying large-scale terrain. Though some might address obstacles of arbitrary size, perhaps only [25] and [72] considers large-scale terrain in a more general framework of slopes and their effect on static stability, traction and locomotion energy, as a function of the rover. None of the local planners address time-varying parameters, for example sunlight, in path selection. Furthermore, these approaches attempt to reduce energy expense by minimizing path length and avoiding rough terrain, but ignore the non-monotonicity introduced by energy collection and the global constraint imposed by limits in storage capacity. Finally, each of these methods only partially address uncertainty.

For example, the D*-based method interleaves sensing, re-planning and action at a high frequency to restrict execution of plans to the immediate proximity of the rover, where data is most certain and complete. The LAAS approach anticipates uncertainty in rover state by ensuring that configurations over a corridor of positions and orientations about the path also satisfy kinematic constraints. However, in avoiding probabilistic or worst-case uncertainty growth over the path, its plan may not adequately prepare for the range of possible outcomes.

2.7.2 Global Path Planning Long-distance path planning is characterized by a greater a priori knowledge of terrain over vast distances than for local planning, albeit at very low resolution. At large scales with low-resolution terrain data, it becomes impractical to consider obstacles at the scale of the rover, but becomes more convenient to model travel more abstractly. Terrain map grid cells define regions of homogeneous terrain properties, and the robot is reduced to a point object travelling from cell to cell.

Shiller and Gwo [58] present a vehicle path planning problem in which path and vehicle speed are optimized. The approach ignores vehicle size relative to terrain, but does model basic vehicle steering kinematics and dynamics.