Toward a deeper understanding ofmotion alternatives via an equivalencerelation on local paths

The International Journal ofRobotics Research31(2) 167–186© The Author(s) 2011Reprints and permission:sagepub.co.uk/journalsPermissions.navDOI: 10.1177/0278364911430418ijr.sagepub.com

Ross A Knepper1, Siddhartha S Srinivasa2 and Matthew T Mason2

AbstractMany problems in robot motion planning involve collision testing a set of local paths. In this paper we propose a novelsolution to this problem by exploiting the structure of paths and the outcome of previous collision tests. Our approachcircumvents expensive collision tests on a given path by detecting that the entire geometry of the path has effectivelyalready been tested on a combination of other paths. We define a homotopy-like equivalence relation among local pathsto detect this condition, and we provide algorithms that (1) classify paths based on equivalence, and (2) circumventcollision testing on up to 90% of them. We then prove both correctness and completeness of these algorithms and provideexperimental results demonstrating a performance increase up to 300% in the rate of path tests. Additionally, we applyour equivalence relation to the navigation problem in a planning algorithm that takes advantage of information gainedfrom equivalence relationships among collision-free paths. Finally, we explore applications of path equivalence to severalother mechanisms, including kinematic chains and medical steerable needles.

KeywordsAI reasoning methods, cognitive robotics, mobile and distributed robotics, nonholonomic motion planning, path planningfor manipulators, SLAM.

1. Introduction

Planning bounded-curvature paths for mobile robots is anNP-hard problem (Reif and Wang, 1998). Many nonholo-nomic mobile robots thus rely on hierarchical planningarchitectures (Kelly et al., 2006; Allen et al., 2007; Knep-per and Mason, 2008; Marder-Eppstein et al., 2010), whichdecompose the problem into at least two layers: a slowglobal planner and fast local planner (Figure 1). We focuson the local planner (Algorithm 1 and Algorithm 2), whichiterates in a tight loop searching through a set of paths andselecting the best path among them for execution. Withineach loop, the planner tests many paths before makingan informed decision. The bottleneck in path testing iscollision checking (Sánchez and Latombe, 2002). In thispaper we introduce a novel approach that delivers a signif-icant increase in path set collision-testing performance byexploiting the fundamental geometric structure of paths.

We introduce an equivalence relation intuitively resem-bling the topological notion of homotopy. Two pathsare path homotopic if a continuous, collision-free defor-mation with fixed start and end points exists betweenthem (Munkres, 2000). Like any path equivalence rela-tion, homotopy partitions paths into equivalence classes.

Different homotopy classes make fundamentally differentchoices about their routes among obstacles. However, twoconstraints imposed by mobile robots translate poorly intohomotopy theory: limited sensing and constrained action.

The robot may lack a complete workspace map, whichit must instead construct incrementally from sensor data.Since robot perception is limited by range and occlu-sion, a robot’s understanding of obstacles blocking itsmovement evolves as it moves. A variety of sensor-based planning algorithms have been developed to han-dle such partial information. Obstacle avoidance methods,such as potential fields (Khatib, 1985), vector field his-tograms (Borenstein and Koren, 1991), and the curvature-velocity method (Simmons, 1996), are purely reactive. Thebug algorithm (Lumelsky and Stepanov, 1987), which gen-erates a path to the goal using only a contact sensor,

1 Computer Science and Artificial Intelligence Lab, Massachusetts Insti-tute of Technology, Cambridge, USA2 Robotics Institute, Carnegie Mellon University, Pittsurgh, USA

Corresponding author:Ross A. Knepper, Computer Science and Artificial Intelligence Lab, Mas-sachusetts Institute of Technology, 32 Vasser St, Cambridge, MA 02139,USA.Email: rak@csail.mit.edu

168 The International Journal of Robotics Research 31(2)

Fig. 1. An example hierarchical planning scenario. The localplanner’s path set expands from the robot, at center, and feedscommands to the robot based on the best path that avoids obstacles(black squares). The chosen local path and corresponding globalpath (lower left) combine to form a proposed path to the goal.

is complete in two-dimensional spaces. A planner usingthe hierarchical generalized Voronoi graph, a roadmapwith global line-of-sight accessibility (Choset and Burdick,1995), achieves completeness in higher dimensions usingrange readings of the environment. Yu and Gupta (2000)propose a planner that iteratively constructs a probabilis-tic roadmap in response to partial sensed information aboutthe world. Actions are selected on the basis of maximizinginformation gain for future plan steps. Our local plannerresembles these algorithms in that it reacts to local obsta-cles while receiving global guidance about the direction tothe goal.

If a robot is tasked to perform long-range navigation, thenit must plan a path through unsensed regions. A low-fidelityglobal planner (i.e. one ignoring constraints) generates thispath because we prefer to avoid significant investment inthis plan, which will likely be invalidated later. Path homo-topy, in the strictest sense, requires global knowledge ofobstacles because homotopy-equivalent paths must connectfixed start and goal points.

Relaxing the endpoint requirement of homotopy avoidsreasoning about the existence of far-away, unsensed obsta-cles. In naively relaxing a fixed endpoint, our paths mightbe permitted to freely deform around obstacles, makingall paths equivalent. To restore meaningful equivalenceclasses, we propose an alternate constraint based on pathshape. Such path shape constraints stem from the nonholo-nomic motion constraints inherent to many mobile robots.Laumond (1986) first highlighted the importance of non-holonomic constraints and showed that feasible paths existfor a mobile robot with such constraints. Barraquand andLatombe (1993) created a grid-based planner that innatelycaptures these constraints. LaValle and Kuffner (2001) pro-posed the first planner to incorporate both kinodynamicconstraints and random sampling. In contrast to nonholo-nomic constraints, true homotopy forbids restrictions onpath shape; two paths are equivalent if any path deformationexists between them. By restricting our paths to bounded

Fig. 2. Left: Paths from a few distinct homotopy classes betweenthe robot and the goal. The distinctions between some classesrequire information that the robot has not yet sensed (the grayarea is out of range or occluded). Middle: With paths restrictedto the sensed area, they may freely deform around visible obsta-cles. Right: After restricting path shape to conform to motionconstraints, we get a handful of equivalence classes that areimmediately applicable to the robot.

curvature, we represent only feasible motions while limit-ing paths’ ability to deform around obstacles. The resultingset of path equivalence classes is of immediate importanceto the planner (Figure 2). The number of choices repre-sented by these local equivalence classes relates to Farber’stopological complexity of motion planning (Farber, 2003).

Various planners have employed equivalence classes toreduce the size of the search space. In task planning, recentwork has shown that equivalence classes of actions canbe used to eliminate redundant search (Gardiol and Kael-bling, 2007). In motion planning, path equivalence oftenemploys homotopy. A recent paper by Bhattacharya et al.(2010) provides a technique based on complex analysisfor detecting homotopic equivalence among paths in 2D.Two papers employing equivalence classes to build proba-bilistic roadmaps (Kavraki et al., 1996) are Schmitzbergeret al. (2002) and Jaillet and Simeon (2008). The latter paperproposes the visibility deformation, a departure from truehomotopic equivalence that restricts continuous deforma-tion to line-of-sight visibility between paths. We proposehere a different variation on homotopy. Not only do werestrict continuous deformation between paths, but we alsofix path length to create a purely local path equivalencerelation.

Our key insights are twofold: first, that this local pathequivalence reveals shared outcomes among a set of paths;and second, that this relation enables a planner to reasoncollectively about such path sets instead of considering eachpath in isolation. These insights are based on the observa-tion that two equivalent neighboring paths represent sweptvolumes of the robot that cover some common ground in theworkspace. Between them lies a continuum of paths whoseswept volumes are covered by the two bounding paths.

This paper makes several contributions relating to localpath equivalence. We develop the mathematical foundationsto detect equivalence relations among all local paths basedon a finite precomputed path set. We utilize these toolsto devise an efficient algorithm for detecting equivalenceamong a discretely sampled path set. By mapping localequivalence of discrete paths to the underlying continuum,we give an algorithm for implicitly collision testing local

Knepper et al. 169

paths, thus circumventing the usual expensive test. Further-more, we propose a navigation algorithm for mobile robotsthat utilizes local path equivalence to improve overall exe-cution success rates by understanding route alternatives andselecting routes most likely to permit safe execution.

1.1. Outline

The remainder of the paper is organized as follows. Weprovide an implementation of the basic classification algo-rithm in Section 2 and present the fast collision testingtechnique. Next, Section 3 explores the implications oflocal path equivalence for improving mobile robot naviga-tion performance. Section 4 then delves into the theoreticalfoundations of our path equivalence relation. Section 5 pro-vides some experimental results. Finally, Section 6 brieflyexplores several promising applications to other roboticsystems, including kinematic chains such as manipulatorarms, and steerable needles used in medical procedures.

2. Path classification and safety algorithms

In this section we present two new algorithms for path clas-sification and implicit path collision testing. We also bor-row a path set generation routine from prior work. All ofthe algorithms presented here run in polynomial time withrespect to path count. Throughout this paper, we use p torefer to a path and P to refer to a set of paths.

Definition 1 (path space). The path space is a metric space(P , μ) in which the metric μ is used to measure the distancebetween a pair of paths in P . Paths may vary in shape andlength.

2.1. Path set generation

We use the greedy path set construction technique of Greenand Kelly (2007). The algorithm iteratively builds a pathset sequence {P1,P2, . . . ,PN } by drawing paths from adensely sampled source path set, PN ⊂ X . X , the contin-uum path space, is the set of all output paths correspondingto all possible control inputs. This set is never explicitlyrepresented.

Instead, we compute the set PN , which can be createdfor any continuous system by finely discretizing the inputparameters and varying them in combination. The requi-site granularity of input discretization depends on charac-teristics of the particular system’s mapping between inputparameters and output paths. Note that in the constructionof path set PN , no care need be given to uniformity ofsampling among path shapes; with sufficient initial density,the Green–Kelly algorithm will provide approximate uni-formity (based on some metric μ) at a range of samplingresolutions.

At step i, the Green–Kelly algorithm selects the pathp ∈ PN that minimizes the dispersion of Pi = Pi−1 ∪ {p}.Borrowing from Niederreiter (1992):

Algorithm 1 Local_Planner_Algorithm(w, x, h, P)

Input: w – a costmap object;x – initial state;h – a heuristic function for selecting a path to execute;P – a fixed set of paths

Output: Moves the robot to the goal if possible1: while not at goal and time not expired do2: Pfree ← Test_All_Paths( w, x,P)3: j← h.Best_Path( x,Pfree)4: Execute_Path_On_Robot( j)5: x← Predict_Next_State( x, j)6: end while

Algorithm 2 Pfree ←Test_All_Paths(w, x, P)

Input: w – a costmap object;P – a fixed set of paths

Output: Pfree, the set of paths that passed collision test1: Pfree ← ∅2: while time not expired and untested paths remain do3: p← Get_Next_Path(P)4: collision← w.Test_Path( x, p) // collision is boolean5: if not collision then6: Pfree ← Pfree ∪ {p} // non-colliding path set7: end if8: end while9: return Pfree

Definition 2 (dispersion). Given a bounded metric space(X , μ) and a set P = {x1, . . . , xn} ∈ X , the dispersion ofP in X is defined by

δ(P ,X )= supx∈X

minp∈P

μ( x, p) . (1)

The dispersion of P in X equals the radius of the biggestopen ball in X containing no points in P . By minimiz-ing dispersion, we ensure that there are no large voids inpath space. Thus, dispersion reveals the quality of P as an“approximation” of X because it guarantees that for anyx ∈ X , there is some point p ∈ P such that μ( x, p)≤δ(P ,X ). Note that all paths in X are of fixed length andshare a start state. This condition is sufficient to assure thatX is bounded for a wide variety of path metrics.

The Green–Kelly algorithm generates a sequence of pathsets Pi, for i ∈ {1, . . . , N}, that has monotonically decreas-ing dispersion. In seeking a path to execute, the local plan-ner algorithm (Algorithm 1) searches paths in this order,thus permitting early termination while ensuring that a low-dispersion set of paths is collision tested. Note that althoughthe source set PN is of finite size—providing a lower boundon dispersion at runtime—it can be chosen with arbitrarilylow dispersion in X a priori.

170 The International Journal of Robotics Research 31(2)

Fig. 3. A simple path set, in which obstacles (black) eliminatecolliding paths. The collision-free path set has three equivalenceclasses (red, green, and blue). In the corresponding graph rep-resentation, at right, adjacent nodes represent proximal paths.Connected components indicate equivalence classes of paths.

2.2. Path classification

We next present Algorithm 3, which classifies collision-freemembers of a path set. The Hausdorff metric is central to thealgorithm. Intuitively, this metric returns the largest amountof separation between two paths in the workspace. FromMunkres (2000):

μH( pi, pj)= infε{pi ⊂( pj)ε and pj ⊂( pi)ε }, (2)

where ( p)r denotes dilation of p by r: {t ∈ R2 : ‖tp− t‖L2 ≤

r for some tp ∈ p}. Note that μH satisfies all properties of ametric (Henrikson, 1999). We similarly define a normalizedHausdorff metric as

μH( pi, pj, d)= infε{pi ⊂( pj)εd and pj ⊂( pi)εd }. (3)

These two Hausdorff metric notations are equivalent fornow, but we exploit the normalized format in Section 6,where we replace the constant d with a function of pathlength. For our fixed path set generated by Green–Kelly anda given d, we precompute each pairwise path metric value of(3) and store them in a lookup table for rapid online access.

Algorithm 3 performs path classification on a set of pathsthat have already tested collision-free at runtime. We forman equivalence graph G = ( V , E) in which node vi ∈ Vcorresponds to path pi. Edge eij ∈ E exists, joining nodes vi

and vj, when this relation holds:

μH( pi, pj, d)≤ 1, (4)

where d is the diameter of the robot. This condition is truewhen two paths are separated by at most one robot diameter.Taking the transitive closure of this relation, two paths pa

and pb are equivalent if nodes va and vb are in the sameconnected component of G (Figure 3).

In effect, this algorithm constructs a probabilisticroadmap (PRM) in the path space instead of the conven-tional configuration space. A query into this PRM tellswhether two paths are equivalent. As with any PRM, a

Algorithm 3 D←Equivalence_Classes(Pfree, d)

Input: Pfree – a set of safe, appropriate paths; d – thediameter of the robot

Output: D – a partition of Pfree into equivalence classes (aset of path sets)

1: Let G =( V , E)←(∅,∅)2: D← ∅ // Partition of paths into classes (represented

by a set of sets)3: for all pi ∈ Pfree do // This loop discovers adjacency4: V .add( pi) // Add a graph node corresponding to

path pi

5: for all pj ∈ V \ {pi} do6: if μH( pi, pj, d)≤ 1 then7: E.add( i, j) // Connect nodes i and j with an

unweighted edge8: end if9: end for

10: end for11: S ← Pfree // Unclassified paths12: while S = ∅ do // This loop finds the connected

components13: C ← ∅ // Next connected component14: p← a member of S15: L← {p} // List of nodes to be expanded in this

class16: while L = ∅ do17: p← a member of L18: C ← C ∪ {p} // Commit p to class19: S ← S \ {p}20: L←(L ∪ V .neighbors( p) )∩S21: end while22: D← D ∪ {C}23: end while24: return D

query is performed by adding two new graph nodes vs

and vg corresponding to the two paths. We attempt to jointhese nodes to other nodes in the graph based on (4).The existence of a path connecting vs to vg indicates pathequivalence.

2.3. Implicit path safety test

There is an incessant need in motion planning to acceleratecollision testing, which may take up to 99% of total CPUtime (Sánchez and Latombe, 2002). During collision test-ing, the planner must verify that a given swath is free ofobstacles.

Definition 3 (swath). A swath is the workspace area ofground or volume of space swept out as the robot traversesa path.

Definition 4 (safe). A path is safe if its swath contains noobstacles.

Knepper et al. 171

Algorithm 4 b←Test_Path_Implicit(p, w, S, d)

Input: p is a path to be tested;w is a costmap object; // used as a backup when pathcannot be implicitly testedS is the set of safe paths found so far;d is the diameter of the robot

Output: b – boolean indicating whether path is safe1: for all pi, pj ∈ S such that μH( pi, pj, d)≤ 1 do2: if p.Is_Between( pi, pj) then // p’s swath has been

tested previously3: sf ← p.Get_End_Point( )4: collision← w.Test_Point( sf ) // endpoint may

not be covered by swaths5: return collision6: end if7: end for8: return w.Test_Path( p) // Fall back to explicit path

test

In testing many swaths of a robot passing through space,most planners effectively test the free workspace manytimes by testing overlapping swaths. We may test a pathimplicitly with significant computational savings by recall-ing recent collision testing outcomes and circumventingnew collision tests whenever possible. We formalize theidea in Algorithm 4, which is designed to be invoked fromAlgorithm 2, line 4 in lieu of the standard path test routine.

The implicit collision test condition resembles the neigh-bor condition (4) used by Algorithm 3, but it has an addi-tional Is_Between check, which indicates that the swath ofthe path under test is fully covered by two collision-freeneighboring swaths. The betweenness trait can be precom-puted and stored in a lookup table. Given a set of safepaths, we can quickly discover whether any pair covers thepath under test. By precomputing eligible paths in the pathset and efficiently tracking collision-free paths, the plannerpromises to realize significant performance gains, produc-ing several times as many paths per second as conventionalcollision test algorithms.

3. Route selection

Thus far, we have shown that local path equivalence canproduce significantly more collision-free paths per unittime than traditional collision testing. However, an earlierresult (Knepper and Mason, 2008) found surprisingly thatif a planner is given additional safe paths, its performancecould actually decline. Given a coarsely sampled path set Cand a densely sampled path set D, we believe this effect isdue to the fact that D is expected to contain a more optimalpath than C that approaches closer to obstacles. In particu-lar, D is likely to find risky narrow corridors that C mightmiss entirely. Therefore, we must establish that the addi-tional safe paths can in fact be put to use in increasing

navigation performance. We use path equivalence to helpachieve this goal.

Since the local planner has a limited horizon, the result-ing planned route is a concatenation of paths from the localand global planners. Only the local paths are feasible to exe-cute directly on the robot, so the local planner must replan atregular intervals to allow continued progress. Thus, Algo-rithm 1 outputs a sequence of paths, the concatenation ofwhich forms the true route. At the end of each replan cycle,the planner executes the beginning of the route representingthe least cost to the goal, a heuristic known as Best_Path.Using this heuristic, traditional hierarchical planners pro-duce strongly goal-directed behavior that comes with twodrawbacks: temporal incoherence and excessive obstacleproximity.

3.1. The temporal incoherence problem

Temporal incoherence occurs because Best_Path does notgenerate consistent behavior between replan cycles, mean-ing that there is no deliberate process to maintain cer-tain decisions throughout navigation. Often, in hierarchicalplanning (Kelly et al., 2006; Allen et al., 2007), the ultimateroute executed by the local planner algorithm is an emergentbehavior because the planner lacks any continuity of intentbetween consecutive replan cycles. We propose local pathequivalence as a means of representing such continuity. Inchoosing a sequence of local paths, local planners implicitlyalso select a sequence of equivalence classes. This obser-vation provides another perspective in which to view localpath equivalence: based on information available to thelocal planner within a given replan cycle, the planned routesof all equivalent paths are homotopic. We propose a newalgorithm to improve navigation performance by explicitlyconsidering continuity within each replan cycle.

In general, we would like each replan cycle to select anew path that closely resembles the previous path, but suchis not always the case. In prior work (Knepper and Mason,2009), we proposed increasing the chance of such an out-come with the Best_Path heuristic by preserving the unex-ecuted remainder of the previous path as a continuation,which is considered along with the ordinary path set withinsubsequent cycles. Even so, on some occasions, consecutivereplan cycles may switch equivalence classes, thus select-ing a new planned route. Best_Path does not distinguishbetween classes, so such switches may happen arbitrarilyoften. Frequent switching is typically associated with per-ception noise. In especially noisy systems, or where twoplanned routes are about equally costly, the planner mayrapidly alternate between routes, thus effectively follow-ing an unplanned and undesirable path directly toward theobstacle separating the two routes.

3.2. The obstacle proximity problem

Obstacle proximity, the second drawback incurred byBest_Path, risks the safety of the robot in cases of outside

172 The International Journal of Robotics Research 31(2)

Fig. 4. A navigating robot faces both discrete decisions (left)about which corridor to follow and continuous optimization (right)over where in the corridor to drive. A planner or controller shouldbe able to consider these choices separately.

disturbance or internal prediction error. From a planningperspective, nearby obstacles also substantially reduce thequantity and diversity of safe paths available in subsequentreplan cycles.

Two related approaches to the problem of decreasingrobot proximity to obstacles have been in use for manyyears. The first approach involves “growing” the obstaclesusing a hard buffer (Buhmann et al., 1995), which runsthe risk of closing off narrow openings. This problem ispartially ameliorated by making the obstacle growth radiusvary in proportion to robot speed.

The second approach involves placing a soft bufferaround each obstacle in the form of a gradient of elevatedcost (Thorpe, 1984), such that cost varies inversely withobstacle proximity. Although this approach does not elim-inate options from consideration, it is difficult to predicthow a given cost function will affect decisions betweencorridors.

The drawback of both approaches is that they couple twodistinct decisions: which route (equivalence class) to fol-low, and how to proceed (which path in the class) along thatroute. These decisions are of qualitatively different char-acter because continual fine-tuning is possible throughoutthe traverse of a corridor, but the choice of corridor to betraversed requires a discrete decision that soon becomesirreversible (Figure 4).

We introduce a new multi-stage path selection algorithmthat separates these two decisions, thus allowing them tobe weighed individually and traded off against one another.This process in turn improves planning and control flex-ibility, increasing continuity of plans, and retaining goal-directedness. Through application of a set of rules basedon path equivalence (applied both within and across replantime steps), the algorithm selects paths for execution that

guide the robot sufficiently far from obstacles while movingconsistently toward the goal.

3.3. Logical succession path relation

Having already demonstrated the value of path equivalencein a single replan cycle, we now introduce a relation onpath equivalence classes to detect logical succession acrossmultiple replan cycles.

Definition 5 (logical succession). Logical succession is astrict partial ordering among equivalence classes A � Bsuch that some paths pA ∈ A and pB ∈ B exist for whichμH( pA, pB, d)≤ 1 and A was generated in an earlier replantime step than B.

This definition establishes that two paths covering largelythe same terrain but produced by different replan cycles canbe said to follow the same route. The definition assumes asmall time increment between replan cycles, such that littleground is covered in the interim. For larger steps, the defi-nition would instead need to compare the end of pA to thestart of pB. Of course, the pairwise logical succession prop-erty can be precomputed for our fixed path set in order tooptimize performance.

In considering a new path for execution, logical succes-sion provides a powerful tool for a planner to distinguishbetween paths that represent major and minor alterationsto the prior plan. Suppose the planner just executed pathpi at time step t − 1. We may initially choose to con-sider at time t only those paths pj such that [pi] � [pj],where [p] describes the equivalence class containing p. Thisrestriction provides continuity of plan. Often, each equiva-lence class has only one logical successor at the followingreplan time step. However, merges and splits may occur atcritical points along the robot’s traverse (Figure 5). Whenthe planner detects a split, it is important to select thebranch that maximizes success, given the locally availableinformation.

3.4. Multistage path selection algorithm

We introduce the multistage local planner algorithm (Algo-rithm 7) to make principled path selections that trade offbetween the issues of logical succession, safety, and esti-mated path length to the goal. At a high level, the algorithmconsists of two stages. Stage One selects for consideration asubset of Pfree comprising one or more equivalence classesin order to ensure progress, safety, and consistency. StageTwo selects from among the chosen subset one path forexecution that trades off safety and cost of the path, whileretaining goal-directedness.

3.4.1. Stage One: Solving the decision problem In gen-erally preferring to execute a new path that is a logicalsuccessor to the previously chosen equivalence class, welargely eliminate sensitivity to noisy perception data. Two

Knepper et al. 173

Fig. 5. Between replan cycles, safe paths are associated by a logical succession of equivalence classes. This strict partial orderingrelation is represented by a directed acyclic graph. Graph edges represent the relationship P1 � P2. Graph node colors (and matchingequivalence class path colors) are conserved in consecutive replan cycles only for the largest logical successor class. Between cycles,we may detect a termination, split, merge, or continuation of the previous equivalence classes. By preferring the logical successor to thepreviously-commanded path, subsequent path selections give better performance.

Fig. 6. These four equivalence classes are annotated as to whethereach is narrow or wide and progressing or non-progressing. Widthis measured by the number of paths in the class as a fraction of allsurviving paths, whereas progressivity describes whether its pathsmake progress toward the goal at left.

exceptions arise in which the algorithm will not executea logical successor path. First, if a non-successor equiva-lence class predicts a significantly lower cost to the goal,then the planner switches classes on the assumption thatthe magnitude of the change exceeds that of likely per-ception noise. Second, we allow the algorithm to considerbroader alternatives—whether more or less costly—if alllogical successor classes terminate or become narrow.

Definition 6 (narrow, wide). A narrow equivalence classcontains few paths. We employ path count as a proxy forthe measure of a corridor in path space. Thus, a low path

count indicates little space to maneuver the robot through anarrow corridor. Non-narrow classes are called wide.

We define a constant fraction, MIN_PATH_THRESH,which adaptively selects the cutoff in corridor width as apercentage of the number of paths in Pfree. Thus, the moredensely we sample the space of paths, proportionately morepaths are required to constitute a wide corridor. Even whendensely sampling the path space, a highly cluttered envi-ronment may eliminate all but a few paths through collisionwith obstacles. In such a case, a passage containing rela-tively few paths may still be to be considered “wide” incomparison to others with fewer paths.

This concept of wide and narrow corridors closely resem-bles that of Borenstein and Koren (1991). Their vector fieldhistogram represents obstacle density projected down toone dimension corresponding to heading. Sparse regionsof the histogram indicate corridors but, due to the pro-jection, only the component of corridor width perpendic-ular to that projection is recorded. Our approach to corri-dor detection and width estimation is more general sinceit closely approximates the full capabilities of the robotand is not limited to a particular obstacle configuration orobservational perspective.

Although Algorithm 7 displays a preference for wide log-ical successor classes, it strictly selects only progressingpaths within a class for consideration.

Definition 7 (progressing). A progressing path is one forwhich both of the following two points are nearer to the goalthan is the current robot position, according to the globalplanner:

1.the point one replan time step in the future; and2.the end point of the local path.

174 The International Journal of Robotics Research 31(2)

Algorithm 5 (W ,N )←Divide_Wide_Narrow(C, t)

Input: C – candidate set of classes; t – threshold size ofclass

Output: W – set of paths in wide classes; N – set of pathsin narrow classes

1: W ← ∅2: N ← ∅3: for all C ∈ C do4: if |C| > t then5: W ←W ∪ C // Paths in wide classes6: else7: N ← N ∪ C // Paths in narrow classes8: end if9: end for

10: W ← Cull_Nonprogressing_Paths(W) // Omit pathsthat move robot away from the goal

11: N ← Cull_Nonprogressing_Paths(N )12: return (W ,N )

Figure 6 illustrates equivalence classes that are wide,narrow, progressing, and non-progressing. The progress-ing property is often shared by all paths in an equivalenceclass, but certain large classes in the absence of clutter canhave mixed progressivity. By executing only progressingpaths, we ensure that the robot monotonically approachesthe goal, thus guaranteeing termination. Furthermore, byeliminating non-progressing paths during Stage One, weare free to ignore goal-directedness in Stage Two while stillguaranteeing progress.

When testing progressivity in a real implementation, itmay be preferable to consider only criterion 2, a path’s end-point, and ignore the next step. Recognizing that curvature-constrained local paths need more space to maneuver thanglobal grid paths, this relaxation provides the planner addi-tional safety and flexibility when navigating around sharpcorners, at the expense of termination guarantees.

In our implementation, Algorithm 5 is used to eliminatenonprogressing paths and divide the rest according to thenarrow/wide dichotomy. Algorithm 7 uses it as a helperfunction in establishing an order of preference in select-ing S, the set of paths for consideration. The net order ofpreference is:

1. All wide, progressing, logical successor classes;2. Any wide, progressing class;3. All narrow, progressing, logical successor classes;4. Any narrow, progressing class;5. Return failure.

After choosing a preliminary S, we must check if the plan-ner has found a highly suboptimal subset of paths; the algo-rithm compares the best path in S against the best pathin Pfree. A difference above a certain SCORE_THRESHprovokes a mid-course correction. Such a switch of equiva-lence class should be a rare event.

Algorithm 6 p←Optimize_Path(x, h, e, p)

Input: x – initial state;h – a heuristic function for selecting a path to execute;e – equivalence objectp – seed path for optimization

Output: Return a path similar to p but safer1: repeat2: N ← e.Get_Neighbors( p)∪{p}3: p← h.Farthest_Obstacle_Path( x,N ) // Select path

in set farthest from nearest obstacle4: until p converges or p.obstacle_proximity > 3

2robot_diameter

5: return p

Note that we are making a choice with global implica-tions based on unreliable information from a low-fidelityglobal planner. Lacking detailed knowledge of the com-plete path to the goal, we instead consider a calculationbased solely on the statistics of this environment’s aver-age obstacle density, which predict that a narrow corridor“pinch point” should occur periodically at some frequencyduring traversal. Given a distance remaining to reach thegoal, we can estimate an expected number of risky narrowcorridors remaining. SCORE_THRESH should be chosenso that the decreased risk (stemming from the shorter pathlength) of getting stuck in a future narrow corridor out-weighs the immediate risk involved in the current routechange, which may itself jump to a narrower corridor.

3.4.2. Stage Two: Solving the optimization problem Afterestablishing a final set of candidate paths S, the algorithmmoves on to Stage Two, which selects a single path forexecution. Initially, it finds the greedy Best_Path option inS, but this path may come unsafely close to an obstacle.Within the equivalence class containing the shortest path,the subroutine Optimize_Path performs a local, gradient-descent-type optimization in path space by traversing theequivalence graph. This optimization, which generates asoft safety buffer around obstacles, seeks to maximizethe distance to the one nearest obstacle as described inAlgorithm 6.

In especially wide corridors, the robot should be freeto follow a reasonably short path, so the obstacle proxim-ity penalty decays to zero beyond 1.5 robot diameters. Theproximity penalty function is only defined with respect tothe one nearest obstacle, so in a narrow corridor the penaltyis locally minimized by the path most nearly following thecenter of the corridor, thus maximizing both safety andfuture planning options. Note that the algorithm will followeven an extremely narrow corridor, provided that the routerepresents the best means to progress toward the goal.

Ultimately, the algorithm we describe here improves onthe original local planner algorithm by executing an actionthat is safe, maximizes future planning/control options, and

Knepper et al. 175

Algorithm 7 ( p,L)←Multistage_Local_Planner_Algorithm (w, x, h, e, P , L)

Input: w – a costmap object;x – initial state;h – a heuristic function for selecting a path to execute;e – equivalence object;P – a fixed set of paths;L – equivalence class of path selected in prior call(initially ∅)

Output: p – a path progressing safely toward the goal;L – equivalence class of p

1: Pfree ← Test_All_Paths( w, x,P) // May invokeimplicit path test

2: b← h.Best_Path( x,Pfree) // Greedy shortest path// Stage 1: select equivalence classes for consideration;trade off succession and corridor width

3: if L = ∅ then // Compute successor path candidates4: C← e.Get_Logical_Successor_Classes(L)

// Returns a set of classes5: (Ws,Ns)← Select_Classes(C, MIN_PATH_THRESH× |Pfree|)

6: else7: (Ws,Ns)←( ∅,∅)8: end if9: E← e.Compute_Equivalence_Classes(Pfree)

10: (W ,N )← Select_Classes(E, MIN_PATH_THRESH×|Pfree|) // Non-successor classes

11: if Ws = ∅ then12: S ←Ws // Consider the set of wide successor

classes if some exist13: else if W = ∅ then14: S ←W // Prefer any wide, progressing class over a

narrow successor15: else if Ns = ∅ then16: S ← Ns // Narrow successors are better than other

narrow classes17: else if N = ∅ then18: S ← N // Last resort: take any path19: else20: return failure21: end if22: p← h.Best_Path( x,S)23: if p.score− b.score > SCORE_THRESH then24: S ← Pfree // Jump equivalence classes to a

significantly shorter route25: end if //

// Stage 2: Select one path from the set, trading off pathlength with safety

26: p← h.Best_Path( x,S)27: p← Optimize_Path( x, h, e, p) // Find safe enough

path in selected path set28: L← e.Class_Of ( p)29: return ( p,L)

remains consistent across replan cycles, all while retaininggoal-directedness.

4. Foundations

In this section we establish the foundations of an equiv-alence relation on path space based on continuous defor-mations between paths. We then provide correctness proofsfor our algorithms for classification and implicit collisiontesting.

We are given a kinematic description of paths. All pathsare parametrized by a common initial pose, common fixedlength, and individual curvature function. Let κi( s) describethe curvature of path i as a function of arc length, withmax0≤s≤sf |κi( s) | ≤ κmax. Typical expressions for κi includepolynomials, piecewise constant functions, and piecewiselinear functions. The robot motion produced by control i isa feasible path given by

⎡⎣ θi

xi

yi

⎤⎦ =

⎡⎣ κi

cos θi

sin θi

⎤⎦ . (5)

Definition 8 (feasible). A feasible path has bounded curva-ture (implying at least C1 continuity) and fixed length. Theset F( sf , κmax) contains all feasible paths of length sf andcurvature |κ( s) | ≤ κmax.

As an aside, one may also consider piecewise C1 con-tinuous paths, such as paths with cusps as followed by themotion model of Reeds and Shepp (1990). Such paths maybe equivalent provided that each follows the same sequenceof forward and backward motions. Then, a conservativemeans of testing equivalence of the whole path is to per-form pairwise tests on each C1-continuous path segment.Thus, the following proofs may assume full C1 continuity.

4.1. Properties of paths

In this section we establish a small set of conditions underwhich we can quickly determine that two paths are equiv-alent. We constrain path shape through two dimension-less ratios relating three physical parameters. We may thendetect equivalence through a simple test on pairs of pathsusing the Hausdorff metric.

These constraints ensure a continuous deformationbetween neighboring paths while permitting a range ofuseful actions. Many important classes of action setsobey these general constraints, including the line seg-ments common in RRT (LaValle and Kuffner, 2001) andPRM (Kavraki et al., 1996) planners, as well as con-stant curvature arcs. Figure 1 illustrates a more expressiveaction set (Knepper and Mason, 2008) that adheres to ourconstraints.

The three related physical parameters are: d, the diam-eter of the robot; sf , the length of each path; and rmin, theminimum radius of curvature allowed for any path. Notethat 1/rmin = κmax, the upper bound on curvature. Fornon-circular robots, d reflects the minimal cross-sectionof the robot’s swath sweeping along a path. We express

176 The International Journal of Robotics Research 31(2)

Fig. 7. At top: several example paths combining different valuesof v and w. Each path pair obeys (4). The value of v affects the“curviness” allowed in paths, whereas w affects their length.At bottom: this plot, generated numerically, approximates the setof appropriate choices for v and w. The gray region at top rightmust be avoided, as we show in Lemma 2. Such choices wouldpermit an obstacle to occur between two safe paths that obey (4). Apath whose values fall in the white region is called an appropriatepath.

relationships among the three physical quantities by twodimensionless parameters:

v = d

rminw = sf

2πrmin.

We only compare paths with like values of v and w. Figure 7provides some intuition on the effect of these parameterson path shape. Due to the geometry of paths, only certainchoices of v and w are appropriate.

Definition 9 (appropriate path). An appropriate path is afeasible path conforming to appropriate values of v andw from the proof of Lemma 2. Figure 7 previews thepermissible values.

When the condition in (4) is met, the two paths’ swathsoverlap, resulting in a continuum of coverage between thepaths. This coverage, in turn, ensures the existence of a con-tinuous deformation, as we show in Theorem 1, but first weformally define a continuous deformation between paths.

Definition 10 (continuous deformation). A continuousdeformation between two safe, feasible paths pi and pj

in F( sf , κmax) is a continuous function f : [0, 1] →F( s−f , κ+max), with s−f slightly less than sf and κ+max slightlymore than κmax. f ( 0) is the initial interval of pi, and f ( 1) isthe initial interval of pj, both of length s−f .

Definition 11 (equivalent). We write pi ∼ pj to indicatethat a continuous deformation exists between paths pi andpj, and they are therefore equivalent.

The length s−f depends on v and w, but for typical values,

s−f is fully 95–98% of sf . For many applications, this is suffi-cient, but an application can quickly test the remaining pathlength if necessary. Nearly all paths f ( c) are bounded bycurvature κmax, but it turns out that in certain geometric cir-cumstances, the maximum curvature through a continuousdeformation is up to κ+max = 4

3κmax.

Definition 12 (guard paths). Two safe, feasible paths thatdefine a continuous deformation are called guard pathsbecause they protect the intermediate paths.

In the presence of obstacles, it is not trivial to deter-mine whether a continuous deformation is safe, thus main-taining equivalency. Rather than trying to find a defor-mation between arbitrary paths, we propose a particularcondition under which we show that a bounded-curvature,fixed-length, continuous path deformation exists,

μH( p1, p2, d)≤ 1 =⇒ p1 ∼ p2. (6)

This statement, which we prove in the next section, is thebasis for Algorithm 3 and Algorithm 4. The overlappingswaths of appropriate paths p1 and p2 cover a continuumof intermediate swaths between the two paths. The equiv-alence relation, of which (6) detects local instances, is aproper equivalence relation because it possesses each ofthree properties:

• reflexivity μH( p, p, d)= 0; p is trivially deformable toitself.

• symmetry The Hausdorff metric is symmetric.• transitivity Given μH( p1, p2, d)≤ 1 and

μH( p2, p3, d)≤ 1, a continuous deformation canbe constructed from p1 to p3 passing through p2.

4.2. Equivalence relation

We now prove (6); that is, we show that shape constraintsindicated by v and w combined with Hausdorff distanceconstraints are sufficient to ensure the existence of a con-tinuous deformation between two neighboring paths. Ourapproach to the proof will be to first describe a feasiblecontinuous deformation, then show that paths along thisdeformation are safe.

Given appropriate guard paths pi and pj with commonorigin, let pe be the shortest curve in the workspace con-necting their endpoints without crossing either path (pe maypass through obstacles). The closed path B = pi + pe + pj

creates one or more closed loops (the paths may cross eachother). By the Jordan curve theorem (Munkres, 2000), eachloop partitions R

2 into two sets, only one of which is com-pact. Let I , the interior, be the union of these compact setswith B, as in Figure 8.

Knepper et al. 177

Fig. 8. Paths pi, pj, and pe form boundary B. Its interior, I , con-tains all paths in the continuous deformation from pi to pj. Theset of paths in I illustrates the betweenness trait described inSection 2.3.

Definition 13 (between). A path pc is between paths pi andpj if pc ⊂ I.

Lemma 1. Given appropriate paths pi, pj ⊂ F( sf , κmax)with μH( pi, pj, d)≤ 1, a path sequence exists in the form ofa feasible continuous deformation between pi and pj.

Proof. We provide the form of a continuous deformationfrom pi to pj such that each intermediate path is betweenthem. With t a workspace point and p a path, let

γ ( t, p) = infε

t ∈( p)ε (7)

g( t) ={

[0, 1] if γ ( t, pi)= γ ( t, pj)= 0{γ (t, pi)

γ (t, pi)+ γ (t, pj)

}otherwise,

(8)

where g( t) is a set-valued function to accommodate inter-secting paths. Each level set g( t)= c for c ∈ [0, 1] definesa weighted generalized Voronoi diagram (GVD) forming apath as in Figure 9. We give the form of a continuous defor-mation using level sets g−1( c); each path is parametrizedstarting at the origin and extending for a length s−f in thedirection of pe.

Let us now pin down the value of s−f , the length of inter-mediate paths pc. Every point ti on pi forms a line segmentprojecting it to its nearest neighbor tj on pj (and vice versa).Their collective area is shown in Figure 10. Equation (4)bounds each segment’s length at d. s−f is the greatest value

such that no intermediate path of length s−f departs from theregion covered by these projections.

For general-shaped generators in R2, the GVD forms a

set of curves meeting at branching points (Sampl, 2001).In this case, no GVD cusps or branching points occur inany intermediate path. Since d < rmin, no center of cur-vature along either guard path can fall in I (Blum, 1967).Therefore, each level set defines a unique path through theorigin.

Each path’s curvature function is piecewise continuousand everywhere bounded. A small neighborhood of eitherguard path approximates constant curvature. A GVD curvegenerated by two constant-curvature sets forms a conic sec-tion (Yap, 1987). Table 1 reflects that the curvature of pc iseverywhere bounded with the maximum possible curvaturebeing bounded by 4

3κmax. For the full proofs, see Knepperet al. (2010a). Thus, each intermediate path pc is a feasiblepath.

Fig. 9. In a continuous deformation between paths pi and pj, asdefined by the level sets of (8), each path takes the form of aweighted GVD. Upper bounds on curvature vary along the defor-mation, with the maximum bound of 4

3κmax occurring at themedial axis of the two paths.

Fig. 10. Hausdorff coverage (overlapping shapes in center) is aconservative approximation of swath coverage (gray). The Haus-dorff distance between paths pi and pj is equal to the maximum-length projection from any point on either path to the closestpoint on the opposite path. Each projection implies a line seg-ment. The set of projections from the top line and bottom lineeach cover a solid region between the paths. These areas, in turn,cover a slightly shorter intermediate path pc, in white, with itslight-colored swath. This path’s length, s−f , is as great as possiblewhile remaining safe, with its swath inside the gray area.

Table 1: Conic sections form the weighted Voronoi diagram. κ1and κ2 represent the curvatures of the two guard paths, withκ1 the lesser magnitude. Let κm = max( |κ1|, |κ2|). For details,see Knepper et al. (2010a).

Type Occurrence Curvature boundsof intermediate paths

line κ1 = −κ2 |κ| ≤ κmparabola κ1 = 0, κ2

= 0 |κ| ≤ κm

hyperbola κ1κ2 < 0, κ1

= −κ2 |κ| ≤ κm

ellipse κ1κ2 > 0 |κ| < 43κm

Lemma 2. Given safe, appropriate guard paths pi, pj ∈F( sf , κmax) separated by μH( pi, pj, d)≤ 1, any path pc ⊂F( s−f , 4

3κmax) between them is safe.

178 The International Journal of Robotics Research 31(2)

Proof. We prove this lemma by contradiction. Assume anobstacle lies between pi and pj. We show that this assump-tion imposes lower bounds on v and w. We then concludethat for lesser values of v and w, no such obstacle can exist.

Let sl( p, d)= {t ∈ R2, tp = nn( t, p) : tpt ⊥ p and ‖t −

tp‖L2 ≤ d2 } define a conservative approximation of a swath,

obtained by sweeping a line segment of length d with itscenter along the path. tpt is the line segment joining tp to tand nn( t, p) is the nearest neighbor of point t on path p. Thetwo swaths form a safe region, U = sl( pi, d)∪sl( pj, d).

Suppose that U contains a hole, denoted by the set h,which might contain an obstacle. Now, consider the shapeof the paths that could produce such a hole. Beginningwith equal position and heading, they must diverge widelyenough to separate by more than d. To close the loop in U ,the paths must then bend back toward each other. Since thepaths separate by more than d, there exist two open intervalsph

i ⊂ pi and phj ⊂ pj surrounding the hole on each path such

that (at this point) phi ⊂( pj)d and ph

j ⊂( pi)d . To satisfy (4),

there must exist later intervals pei ⊂ pi such that ph

j ⊂( pei )d

and likewise pej ⊂ pj such that ph

i ⊂( pej )d , as in Figure 11a.

How long must a path be to satisfy this condition? Con-sider the minimum length solution to this problem underbounded curvature. For each point t ∈ ph

j , the inter-val pe

i must intersect the open disk D = int( ( t)d ), asin Figure 11b. Since ph

j grows with the width of h, andpe

i must intersect all of these open neighborhoods D, thepath becomes longer with larger holes. We will thereforeconsider the minimal small-hole case.

Vendittelli et al. (1999) solve the shortest path problemfor a Dubins car to reach a line segment. We may approxi-mate the circular boundary of D by a set of arbitrarily smallline segments. One may show from this work that giventhe position and slope of points along any such circle, theshortest path to reach its boundary (and thus its interior)is a constant-curvature arc of radius rmin. In the limit, asv approaches one and the size of h approaches zero, thelength of arc needed to satisfy (4) approaches π/2 fromabove, resulting in the condition that w > 0.48. Thus, forw ≤ 0.48 and v ∈ [0, 1), pc is safe. For smaller values of v,D shrinks relative to rmin, requiring longer paths to reach,thus allowing larger values of w as shown in the plot inFigure 7.

We have shown that there exist appropriate choices for vand w such that (4) implies that U contains no holes. SinceU contains the origin, any path pc ∈ I emanating from theorigin passes through U and is safe.

Theorem 1. Given safe, appropriate guard paths pi, pj ∈F( sf , κmax), and given μH( pi, pj, d)≤ 1, a safe continuousdeformation exists between pi and pj.

Proof. Lemma 1 shows that (8) gives a continuous defor-mation between paths pi and pj such that each intermediatepath pc ⊂ I is feasible. Lemma 2 shows that any such pathis safe. Therefore, a continuous deformation exists between

Fig. 11. (a) With bounded curvature, there is a lower bound onpath lengths that permit a hole, h, while satisfying (4), indicatedby pe

i , the blue highlight. Shorter path lengths ensure the existenceof a safe continuous deformation between paths. (b) We computethe maximal path length that prevents a hole using Vendittelli’ssolution to the shortest path for a Dubins car. Starting from thedot marked s, we find the shortest path intersecting the circle Dof radius rmin. The interval pe

i illustrates path lengths permitting a

hole to exist. Shorter paths leave some part of phj uncovered.

pi and pj. This proves the validity of the Hausdorff metricas a test for path equivalence.

By chaining together continuous deformations betweenneighboring paths, we can demonstrate that a continuousdeformation exists between any pair of paths within anequivalence class by following the correct sequence ofedges of the equivalence graph. This property holds for anypaths in our discretely sampled set. It also applies for anyother pair of paths satisfying the shape constraints, providedthat the discrete sampling is sufficiently dense. The exis-tence of a sufficiently dense path sampling is the subject ofthe next section.

4.3. Resolution completeness of path classifier

In this section, we show that Algorithm 3 is resolution-complete. Resolution completeness commonly shows thatthere exists a sufficiently high discretization of each dimen-sion of the search space such that the planner finds a pathexactly when one exists in the continuum space. We insteadshow that there exists a sufficiently low dispersion samplingin the infinite-dimensional path space such that the approx-imation given by Algorithm 3 has the same connectivity asthe continuum safe, feasible path space.

Let F be the continuum feasible path space and Ffree ⊂F be the set of safe, feasible paths. Using the Green–Kellyalgorithm, we sample offline from F a path sequence PN ofsize N . At runtime, using Algorithm 2, we test members ofPN in order to discover a set Pfree ⊂ PN of safe paths.

The following lemma is based on the work of LaValleet al. (2004), who prove resolution completeness of deter-ministic roadmap (DRM) planners, which are PRM plan-ners that draw samples from a low-dispersion, determin-istic source. Since we use a deterministic sequence pro-vided by Green–Kelly, the combination of Algorithm 2 andAlgorithm 3 generates a DRM in path space.

Knepper et al. 179

Lemma 3. For any given configuration of obstacles andany path set PN generated by the Green–Kelly algorithm,there exists a sufficiently large N such that any two pathspi, pj ∈ Pfree are in the same connected component of Ffree

if and only if Algorithm 3 reports that pi ∼ pj.

Proof. LaValle et al. (2004) show that by increasing N , asufficiently low dispersion can be achieved to make a DRMcomplete in any given C-Space. By an identical argument,given a continuum connected component C ⊂ Ffree, allsampled paths in C∩PN are in a single partition of Pfree. If qis the radius of the narrowest corridor in C, then for disper-sion δN < q, our discrete approximation exactly replicatesthe connectivity of the continuum freespace.

Lemma 4. Under the same conditions as in Lemma 3, thereexists a sufficiently large N such that for any continuumconnected component C ⊂ Ffree, Algorithm 2 returns a Pfree

such that Pfree ∩ C = ∅. That is, every component in Ffree

has a corresponding partition returned by Algorithm 3.

Proof. Let Br be the largest open ball of radius r in C. WhenδN < r, Br must contain some sample p ∈ PN . Since C isentirely collision-free, p ∈ Pfree. Thus, for dispersion lessthan r, Pfree contains a path in C.

There exists a sufficiently large N such that after N sam-ples, PN has achieved dispersion δN < min( q, r), whereq and r are the dispersion required by Lemmas 3 and4, respectively. Under such conditions, a bijection existsbetween the connected components of Pfree and Ffree.

Theorem 2. Let D = {D1| . . . |Dm} be a partition of Pfree

as defined by Algorithm 3. Let C = {C1| . . . |Cm} be a finitepartition of the continuum safe, feasible path space intoconnected components. A bijection f : D → C exists suchthat Di ⊂ f (Di).

Proof. Lemma 3 establishes that f is one-to-one, whereasLemma 4 establishes that f is onto. Therefore, f is bijective.This shows that by sampling at sufficiently high density, wecan achieve an arbitrarily good approximation of the con-nectedness of the continuum set of collision-free paths inany environment.

Finally, we move on to show that we can detect pathsafety while circumventing a collision test.

Theorem 3. A path interval pc may be implicitly tested safeif it is between paths pi and pj such that μH( pi, pj, d)≤ 1and a small region at the end of pc has been explicitly tested.

Proof. By Lemma 2, the initial interval of pc is safe becauseits swath is covered by the swaths of the guard paths. Sincethe small interval at the end of pc has been explicitly tested,the whole of pc is collision-free.

5. Evaluation

We present some experimental results involving equiva-lence class detection and implicit path collision testing.All tests were performed in simulation on planning prob-lems of the type described in previous work (Knepper andMason, 2008). Briefly, planning problems consist of a com-bination of an environment and planning query. Environ-ments were randomly generated within a room measuringtwenty meters on a side, in which 10 cm diameter obsta-cles were randomly positioned until reaching the desiredworkspace obstacle coverage fraction. A planning queryasks the 0.412 m diameter Nomad Scout robot to navigatefrom a start to a goal configuration, each chosen randomlyso as to be separated by 14 m. Finally, candidate problemswere rejected if a 10 cm-resolution 8-connected grid plan-ner was unable to solve the planning problem. Note that thisgrid planner also serves as global guidance to the local plan-ner. Unless otherwise stated, the local planner receives 0.1 sper replan cycle. Each reported result is an average over 100runs on a fixed set of different planning problems.

During navigation, the local planner is permitted to runfor a fixed amount of time within each replan cycle beforeexecuting its chosen path. A variable number of paths willbe tested each cycle depending on factors such as obstacleclutter and implicit path testing. In some experiments, wevary the planning time allotted, whereas other experimentsexplore the effects of obstacle density.

5.1. Classification performance overhead

Path classification imposes a computational overhead due tothe cost of searching for neighboring collision-free paths.Collision rate in turn relates to the density of obstacles inthe environment. Figure 12 shows that the computationaloverhead of our classification implementation is nearly 20%in an empty environment but drops to 0.3% in dense clut-ter. It is in precisely such high-clutter environments that theusefulness of classification is maximized since two arbitrarypaths are less likely to be equivalent amongst many obsta-cles. We now proceed to weigh these and other benefits ofpath classification against its costs.

5.2. Collision testing

Regardless of obstacle density, implicit collision testingmore than compensates for the overhead of path classifi-cation. In comparing collision test algorithms, the baselinecollision tester is among the most efficient for a rigid bodymoving along a path. The algorithm performs successivestatic collision tests at positions along the path. In orderto determine the interval between tests, the algorithm com-putes the distance to the nearest obstacle and moves thatdistance (or less) along the path. The algorithm thus simul-taneously guarantees correctness (never missing a collision)and efficient performance (computing a small number ofstatic collision tests). Note that this same idea can also be

180 The International Journal of Robotics Research 31(2)

Fig. 12. Path classification overhead is minimized in exactly thosedensely cluttered problems where its contribution is most valuable.In this plot, a constant time of 0.1 s is allotted to collision test andclassify paths at a range of obstacle densities. Note that with sucha fixed deadline, the planner finds more safe paths in lower-densityscenarios. If a fixed quantity of safe paths must be generatedregardless of elapsed time, this curve becomes significantly flatter.

extended to multibody kinematic chains (Schwarzer et al.,2004).

Figure 13 shows the effect of implicit path testing on totalpaths tested in the absence of obstacles. We compare theimplicit collision tester of Algorithm 4 against traditionalexplicit collision testing. As the time allotment for testingpaths increases, the number of paths collision tested underthe traditional algorithm increases linearly at a rate of 8,300paths per second. With implicit testing, the initial test rateover small time allotments (thus small path set sizes) is over22,500 paths per second. The marginal rate declines overtime due to the aforementioned overhead, but implicit pathtesting still maintains its speed advantage until the entire2,401-member path set is collision tested. Note that thisresult occurs in the empty world case, where overhead ismost severe.

Figure 14 presents implicit collision testing performancein the presence of clutter. As obstacle density increases,we expect overhead to drop, but it simultaneously becomesmore difficult to satisfy the necessary conditions for implic-itly testing a path. Fixing the replan rate at an intermediatevalue of 10 Hz, we see that implicit path evaluation main-tains an expected advantage across all navigable obstacledensities. In high clutter, this advantage is statistically lessclear, yet implicit path testing still outperforms explicit pathtesting with over 90% confidence at the maximum testedobstacle density.

5.3. Route selection

We tested the multistage local planner algorithm (Algo-rithm 7) over a variety of environments at a range of

Fig. 13. Paths tested per time-limited replan step in an obstacle-free environment. Path testing performance improves by up to 3×with the algorithms we present here. Note that an artificial ceilingcurtails performance at the high end due to a maximum path set ofsize 2,401.

Fig. 14. Paths tested per 0.1 s time step at varying obstacle densi-ties. Implicit collision testing allows significantly more paths to betested per unit time. Even in extremely dense clutter, implicit pathtesting considers an extra six paths on average. The right edge ofthe graph represents the maximum density at which environmentsremain navigable. Error bars indicate 95% confidence.

obstacle densities in order to evaluate the effects on plan-ner performance of awareness of local equivalence classes.Experimental setup was the same as before, except thathere we tested the planner on 500 different planning prob-lems for each obstacle density. Replan cycle times for theseexperiments is 0.1 s.

Figure 15 shows success rate for the local planneralgorithm (greedy) and multistage local planner algorithm(equivalence-aware). The latter produces a statistically sig-nificant improvement of 7.6% at solving planning problemsin dense clutter. Path length increases only negligibly, and

Knepper et al. 181

Fig. 15. Improvement in planner success rate at solving queries.In high clutter, Algorithm 7 performs significantly better thanAlgorithm 1 at successfully completing navigation tasks.

Fig. 16. Ratio of Best_Path cost to multistage local planner algo-rithm cost. Lower is better. The cost of an individual path fromeither planner represents the path integral of the reciprocal ofobstacle proximity. The overall mean cost improvement (solidline) is about a factor of two, meaning that during navigationthe robot stays twice as far from obstacles, on average. The scat-ter plot (blue crosses) shows the individual results for the 1838experiments in which both planners successfully reached the goal.

despite the extra path length, we find a decreased pathcost, expressed as

c( p)=∫

p

ds

od( p( s) ), (9)

where od( s) is the distance to the nearest obstacle from thegiven point along the path. Figure 16 shows the change incost between the two planners. The overall mean change,shown with the solid line, indicates that the robot staysabout twice as far from obstacles. We compared perfor-mance on each test problem separately so the individ-ual data points (shown with blue crosses) represent costimprovement normalized to the difficulty of the problem.

Fig. 17. Two paths of a spherical robot (gray) in a 3D workspaceare insufficient to establish equivalence. The bottom figure depictsa cross-sectional slice through the swept volumes of the paths.Although two paths may be separated by less than one robot diam-eter, obstacles (black) may still prevent a continuous deformationbetween the paths.

In contrast to the overall trend, a small fraction of theexperiments showed cost worsening. In roughly 2% ofcases, skewed heavily toward the less dense environments,obstacle proximity increased with the equivalence-awareplanner. We attribute this phenomenon to the interplaybetween choice and lookahead distance. In more open envi-ronments, the increased choice makes it more tempting tostray from a coherent plan. At a different lookahead dis-tance, the planner may have understood its choices bet-ter, but there are of course significant costs to increasedlookahead. Making this tradeoff dynamically presents aninteresting opportunity for future work.

6. Extensions and future work

Having thus far examined our equivalence relation in thecase of a mobile robot in the plane, we now briefly turn tomore complex systems and applications. Each system con-sidered in this section has at least one of two attributes:motion in three dimensions, and internal articulation (i.e.it can change shape).

6.1. A rigid body in 3D

First, we address a rigid body in a 3D workspace, such as anaircraft or spacecraft. Let us suppose this rigid body takesthe form of a sphere (or can be approximated as one). Twodifferent paths, no matter how close together, are never suf-ficient to establish path equivalence, as is the case in twodimensions. Figure 17 illustrates this fact.

182 The International Journal of Robotics Research 31(2)

Fig. 18. Three paths are required to establish path equivalence in3D. The swept volumes of the three paths are shown. To estab-lish equivalence, we require that the Hausdorff distance betweeneach pair of paths must not exceed the robot’s radius. Given suchproximity, one path may be continuously deformed to another byfollowing the dotted lines, without risk of intersecting the blackobstacles. Upon inspection, one might think that a looser boundcould be found because the radius–circles do not meet perpendic-ularly in this condition. In the limiting case as depicted, the pathpoint and radius–circle intersections are collinear. Any relaxationof the inter-path distance would result in a case where the obsta-cles could squeeze into the corners at the intersection points andprevent a continuous deformation from occurring. Implicit colli-sion testing may be performed on any fourth path discovered to beentirely inside the gray star-shaped region at center.

In three dimensions, three paths are equivalent if each oftheir pairwise Hausdorff separations is not more than theradius of the robot. Figure 18 depicts such a configurationof three paths and also shows how to construct a continu-ous deformation between any pair. As in the 2D case, withthree paths in 3D we can perform implicit collision testingon a variety of paths. The region in which a fourth pathmust reside to implicitly declare it collision-free is shownin Figure 18.

One who is familiar with topology might question thevalue of path equivalence in three dimensions because ordi-nary bounded obstacles do not induce additional homotopyclasses. However, this is where local path equivalence really

Fig. 19. Mobile robot paths with variable radius. The nearestpoint on the opposite path depends on both the position and radiusof each point along the path.

shines. Since the planner has only local knowledge, it can-not distinguish between a finite, long, skinny obstacle andan infinite, skinny obstacle. For all practical purposes, thefinite obstacle might as well be infinite. Given that thehierarchical planner has no knowledge of paths that per-form an end-run around the object, such choices are suf-ficiently costly that the partition of local paths into separateclasses represents a more accurate depiction of the availablechoices than the single class supplied by a true homotopyrelation.

In the application of local path equivalence to 3D prob-lems, one concern arises in the relatively uncluttered natureof most 3D environments. Such environments put the classi-fication algorithm at the left end of the graph in Figure 12—a high-overhead situation. However, the situation is not asbad as it first appears. The data in that figure was gatheredwith a very efficient 2D collision tester. By comparison,collision testing in 3D is significantly more complex andcostly, whereas the complexity of the classification algo-rithm increases only slightly. Classification becomes pro-portionately much more efficient in three dimensions, thusthe gain from avoiding explicit path tests in 2D (Figure 13and Figure 14) becomes more dramatic in 3D.

6.2. Variable-size robots in 2D

We begin by introducing a variant of the basic 2D mobilerobot in which the robot’s size—still approximated as adisk—varies as a function of path length. This scenariooccurs in the mobile manipulation problem, in which amounted manipulator arm may extend out beyond therobot’s own footprint. For example, the elastic strips ofBrock and Khatib (2002), when projected onto the floor,closely resemble this variable-width robot. A radius func-tion ri( �) expresses the robot’s radius with respect to pointpi( �) along path pi. We introduce the concept of propor-tional dilation, in which the path width grows in accordancewith its radius function:

( p)ri = {t ∈ R2 : ‖p( �)−t‖L2 ≤ ri( �) for some � ∈ IL},

(10)

where IL = [0, L], with L the path length. Now a pair ofnearby points on two neighboring paths may possess

Knepper et al. 183

distinct diameters, as in Figure 19, thus giving rise to a newnormalized Hausdorff metric,

μvH( pi, pj, ri, rj)= max( argmin

ε

(∀�a ∈ IL, ∃�b ∈ IL

: ( pi( �a) )εri(�a) ∩( pj( �b) )εrj(�b) = ∅) ,

argminε

(∀�a ∈ IL, ∃�b ∈ IL

: ( pj( �a) )εrj(�a) ∩( pi( �b) )εri(�b) = ∅) ) . (11)

Intuitively, this variant of the Hausdorff metric finds theminimal scale factor for the two paths’ radius functionssuch that no gap remains between the two paths follow-ing proportional dilation. Following dilation, each point oneither path is replaced by a disk. In order to ensure the abovecondition, each disk on one path must intersect some diskon the opposite path.

Given some appropriate constraints on the shape of pathsas well as their radius functions, the equivalence relation onpaths of variably-sized mobile robots then follows directlyfrom the fixed-diameter case:

μvH( p1, p2, r1, r2)≤ 1 =⇒ p1 ∼ p2. (12)

Here, curvature bounds must apply to the boundary of thevariable-diameter robot swath in addition to the path itself.However, in the case of a mobile manipulator, where thereach of an arm is on the same order of magnitude as themobile base diameter, this distinction is rarely critical.

6.3. Articulated robots

We also apply path classification to the trajectories ofmanipulator arms in 3D. At a high level, the situationclosely parallels the 2D mobile robot case we present inSection 2. Two paths, separated by at most the diameter ofthe arm, are equivalent under certain shape and proximityconstraints. In contrast to a 2D rigid body, the medial axis ofthe arm sweeps out a 2D manifold or sheet in the workspace(Figure 20 and Figure 21), so points along our path arenow parametrized by ( s, �), where s ∈ IS = [0, sf ], a dis-tance along the arm’s motion in the configuration space, and� ∈ IL = [0, L], a distance along the axis of the arm frombase to end-effector. Thus, p( s, �) corresponds to a particu-lar workspace location along the arm’s axis while the arm isin a certain configuration.

A function r( �) describes the radius of a disk circum-scribing a cross-section of the arm at a point along itslength. Note that radius is now a function of arm lengthrather than trajectory position. Note also that the disk is nownormal to the arm axis, whereas in the 2D shape-changingrobot, it is coplanar with the path. Though nearly identicalto (10), we define proportional dilation in the context of atwo-parameter path function,

( p)ri = {t ∈ R2 : ‖p( s, �)−t‖L2 ≤ ri( �)

for some s ∈ Is and � ∈ IL}. (13)

Fig. 20. A kinematic chain such as a robot arm (gray) can belikened to a mobile robot path with varying radius as in Figure 19.When executing a trajectory, the arm’s medial axis (black) sweepsout a two-dimensional sheet with varying radius in the spatialdimension and fixed radius in the temporal dimension.

As in Section 6.2, we define a variant of the Hausdorffdistance in the context of arm paths,

μaH( pi, pj, ri, rj)= max( argmin

ε

(∀( sal�a) , ∃( sb, �b) :

( pi( sa, �a) )εri(�a) ∩( pj( sb, �b) )εrj(�b) = ∅) ,

argminε

(∀( sa, �a) , ∃( sb, �b) :

( pj( sa, �a) )εrj(�a) ∩( pi( sb, �b) )εri(�b) = ∅) ) ,

where sa, sb ∈ IS and �a, �b ∈ IL. Note that this is a con-servative expression for the distance separating two armtrajectories.

In considering the possibility that an obstacle divides twoarm trajectories, the semantics of the application come intoplay. For example, objects in human spaces do not levi-tate, so in the absence of highly dynamic objects such asa thrown ball, we may relax the tight constraints imposedby μa

H. Instead, the two arm trajectories need only to com-pletely surround a pocket of space, meaning that theirend-effector trajectories and end states overlap.

Next, we address constraints on arm path shape andlength. Such concepts are inherently much more nebu-lous than their mobile-robot equivalents due to the arm’sarticulation—especially for arms with revolute joints. It isdifficult to pin down general, meaningful constraints on armpath length and shape.

In principle, a useful measure of path length could beobtained by computing swept volume of the arm. After all,in the case of a rigid body mobile robot, all swaths of a givenlength that do not cross over themselves have equal length.

184 The International Journal of Robotics Research 31(2)

(a) (b)

Fig. 21. When moving an arm between two configurations, many trajectories are possible. Each trajectory traces a unique path throughthe workspace. (a) For two different trajectories represented by solid and dotted lines, the paths of the elbow and wrist are shown. (b)Two sheets correspond to two distinct arm trajectories. These 2D manifolds embedded in a 3D workspace share many properties withthe 1D mobile robot paths embedded in a 2D workspace discussed earlier.

In the case of manipulator arms (especially those with revo-lute joints), many useful motions do involve swept volumesthat “cross over themselves,” so an alternate formulation ofpath length is needed. Just as the length of a mobile robotpath is found by integrating velocity, the length of an armpath may be found by integrating a form of velocity as well.Given an arm path pi executed with unit C-Space speed, letvi( t, �) be the workspace velocity of point pi( t, �) along theaxis of the arm at time t. We propose two alternative pathlength measures:

Lmean( i) = 1

L

∫ L

0

∫ sf

0vi( s, �) ds d�, (14)

Lmax( i) =∫ sf

0max�∈IL

vi( s, �) ds. (15)

In the case of a rigid body under arbitrary motion in R3, the

mean and max path lengths are always related by a factorbetween one and two. For an articulated chain, the factormay be greater.

We now move on to address path shape constraints for anarm. This issue is both complex and mechanism-dependent.In previous work (Knepper et al., 2010b), we utilized pathsets comprising straight lines in C-Space. Of course, such“straight” trajectories can involve arbitrarily high curva-ture of some point on the arm within the workspace. Givena sampling of such paths dense enough to establish pathequivalence, it is not clear that the planner would often dis-cover multiple equivalence classes. It would therefore beleft to a given manipulation application to further constrainpath sampling to a set of tasks useful for a given problem.

A few approaches worth exploring further includebounding the energy consumed in executing a given path(after subtracting out torques associated with gravity com-pensation), and retraction-based approaches. In the lattercase, we propose to compare paths by utilizing retraction-like reductions in dimensionality of a search space, suchas those proposed by Sun and Lumelsky (1991) and

Choset and Burdick (1995). Under this reduction, any givenpath in the free configuration space maps to a path in aone-dimensional set, which is the deformation retract of thefreespace. We can then employ such retracts as a graph-like roadmap and compare only paths whose correspond-ing graph paths are similar. For general articulated sys-tems, this approach raises some challenges of its own, suchas the fact that in three or more dimensions, these one-dimensional retracts are necessarily either not connected orhave extra loops not corresponding to topological featuresof the original freespace (Choset and Rizzi, 2005).

Despite these challenges, our path equivalence relationholds promise in the domain of motion planning for artic-ulated robots, such as manipulator arms. For instance, evenlacking constraints on path shape, it is possible to utilize μa

Hto accelerate collision testing. Although arms pose greaterchallenges for satisfying the necessary conditions on prox-imity and betweenness, the cost of each collision test issignificantly higher for articulated robots than it is for rigidbodies, so the gains remain potentially significant.

6.4. Steerable needles

Steerable needles are long, flexible, hollow needles with abevel tip. Such needles can be used in medical proceduresto reach soft-tissue anatomical features that would other-wise be inaccessible due to obstructing anatomy of a hard(bone) or delicate (nerve, artery) nature. During insertion,the bevel tip causes the needle to follow a constant curvaturepath. Steerable needles are interesting from a motion plan-ning perspective because they are underactuated, havingonly two velocity controls. One may vary the rate of inser-tion and the rate of rotation about the axis of the needle.This twisting motion alters the plane in which the needlebends (Webster III et al., 2006).

During needle motion, uncertainty is introduced to thepath primarily in the form of a random variable added to the

Knepper et al. 185

needle’s curvature. This uncertainty derives from the com-plexities of interaction with human tissue. Instantaneously,the uncertainty in position increases in the direction of thevector normal to the needle within the plane of curvature.Meanwhile, the instantaneous control inputs act primarilyin vectors along the needle and normal to the plane of cur-vature. By duty-cycling the needle, steering control can beapplied to cancel out errors in curvature, thus the needle cantrack arbitrary paths of bounded curvature (Minhas et al.,2007).

Planners have been proposed for needle steering toaccount for uncertainty in both motion (Alterovitz et al.,2008) and localization (van den Berg et al., 2010). Itshould be possible to extend many steerable needle plan-ners using path equivalence. The equivalence algorithms inthis paper assume that any path under consideration maybe chosen deterministically. Thus, the distribution of pathsis purely a function of control inputs. In the steerable nee-dles context, the distribution among paths is generated bya combination of control and uncertainty. Consequently,we must find equivalence between entire groups of pathsacross a probability distribution instead of between indi-vidual paths. Thus, steerable needles constitute both a the-oretical extension and a promising application for futurework.

7. Conclusion

In this paper we propose an equivalence relation on localpaths based on the following constraints: fixed start posi-tion and heading, fixed length, and bounded curvature. Wedescribe an algorithm for easily classifying paths using theHausdorff distance between them. Path classification is atool that permits collective reasoning about paths, leadingto more efficient collision testing.

We present a variety of applications for local path equiva-lence, including implicit path testing to accelerate collisiontesting. In this area, we demonstrate performance increasesof over 300% in paths collision tested per unit time. In addi-tion to the strict implicit path-testing formalism we present,many generalizations are possible such as implicitly testingpaths of varying length.

Of course, we must be able to make use of all thoseadditional paths, so our second application is in the areaof navigation. Equivalence classes are a powerful tool todistinguish between discrete decision problems and contin-uous optimization problems and to balance choices betweenthe two. In navigation, we show an increase in navigationsuccess of up to 7.6% for one class of long-range planningproblem. Meanwhile, our navigation algorithm reduces byhalf the cost of obstacle proximity, indicating that thesepaths are significantly safer.

Finally, we explore some extensions and real-world appli-cations. Local path equivalence, when applied to three-dimensional planning problems, is actually more meaning-ful in a hierarchical planning context than is true homotopy

because it reflects choices that are locally discernible. Wediscuss the strengths and weaknesses of local path equiv-alence applied to three-dimensional kinematic chains. Thepath equivalence formulation seems particularly well-suitedto the bevel-tipped steerable needle problem, since thatmechanism is naturally curvature-constrained.

Surveying these applications, we conclude that local pathequivalence is a formalism that provides valuable tools toaddress motion planning problems involving hierarchicalplanning, collision testing, and navigation for a variety oftwo- and three-dimensional systems.

Acknowledgments

The authors thank Matthew Tesch, Laura Lindzey, AlbertoRodriguez, Mehmet Dogar, and M. Jenae Lowe for valuable com-ments and discussions. Thanks extend also to the anonymousreviewers for their thoughtful and supportive feedback.

Funding

This work is sponsored by the Defense Advanced ResearchProjects Agency under contract HR0011-07-1-0002. This workdoes not necessarily reflect the position or the policy of theGovernment. No official endorsement should be inferred.

Conflict of interest

The authors declare that they have no conflicts of interest.

References

Allen T, Underwood J and Scheding S (2007) A path planning sys-tem for autonomous ground vehicles operating in unstructureddynamic environments. In: Proceedings of the AustralasianConference on Robotics and Automation.

Alterovitz R, Branicky M and Goldberg K (2008) Motion plan-ning under uncertainty for image-guided medical needle steer-ing. International Journal of Robotics Research 27: 1361–1374.

Barraquand J and Latombe JC (1993) Nonholonomic multibodymobile robots: Controllability and motion planning in thepresence of obstacles. Algorithmica 10: 121–155.

Bhattacharya S, Kumar V and Likhachev M (2010) Search-based path planning with homotopy class constraints. In: Pro-ceedings of the Twenty-Fourth AAAI Conference on ArtificialIntelligence.

Blum H (1967) A transformation for extracting new descriptorsof shape. In: Whaters-Dunn W (ed.) Proceedings of the Sympo-sium on Models for the Perception of Speech and Visual FormCambridge, MA: MIT Press, 362–380.

Borenstein J and Koren Y (1991) The vector field histogram—fast obstacle avoidance for mobile robots. IEEE Transactionson Robotics and Automation 7: 278–288.

Brock O and Khatib O (2002) Elastic strips: A framework formotion generation in human environments. International Jour-nal of Robotics Research 21: 1031–1052.

Buhmann J, Burgard W, Cremers AB, Fox D, Hofmann T, Schnei-der FE, et al. (1995) The mobile robot RHINO. AI Magazine16: 31–38.

186 The International Journal of Robotics Research 31(2)

Choset H and Burdick J (1995) Sensor based planning, part I: Thegeneralized Voronoi graph. In: Proceedings of the InternationalConference on Robotics and Automation, pp. 1649–1655.

Choset H and Rizzi AA (2005) Topology in motion planning.In: Proceedings of the Eleventh International Symposium onRobotics Research, pp. 90–99.

Farber M (2003) Topological complexity of motion planning.Discrete & Computational Geometry 29: 211–221.

Gardiol NH and Kaelbling LP (2007) Action-space partitioningfor planning. In: Proceedings of the National Conference onArtificial Intelligence.

Green C and Kelly A (2007) Toward optimal sampling in thespace of paths. In: Proceedings of the Thirteenth InternationalSymposium of Robotics Research.

Henrikson J (1999) Completeness and total boundedness of theHausdorff metric. MIT Undergraduate Journal of Mathematics1.

Jaillet L and Simeon T (2008) Path deformation roadmaps: Com-pact graphs with useful cycles for motion planning. Interna-tional Journal of Robotics Research 27: 1175–1188.

Kavraki L, Svestka P, Latombe JC and Overmars M (1996) Proba-bilistic roadmaps for path planning in high-dimensional config-uration spaces. In: Proceedings of the International Conferenceon Robotics and Automation, pp. 566–580.

Kelly A, Stentz A, Amidi O, Bode MW, Bradley D, Diaz-CalderonA, et al. (2006) Toward reliable off road autonomous vehiclesoperating in challenging environments. International Journalof Robotics Research 25: 449–483.

Khatib O (1985) Real-time obstacle avoidance for manipulatorsand mobile robots. In: Proceedings of the IEEE InternationalConference on Robotics and Automation, pp. 500–505.

Knepper RA and Mason MT (2008) Empirical sampling of pathsets for local area motion planning. In: Proceedings of theInternational Symposium on Experimental Robotics.

Knepper RA and Mason MT (2009) Path diversity is only part ofthe problem. In: Proceedings on the International Conferenceon Robotics and Automation.

Knepper RA, Srinivasa SS and Mason MT (2010a) Curvaturebounds on the weighted Voronoi diagram of two proximal pathswith shape constraints. Technical Report CMU-RI-TR-10-25.Robotics Institute, Carnegie Mellon University.

Knepper RA, Srinivasa SS and Mason MT (2010b) Hierarchicalplanning architectures for mobile manipulation tasks in indoorenvironments. In: Proceedings of the International Conferenceon Robotics and Automation, pp. 1985–1990.

Laumond JP (1986) Feasible trajectories for mobile robots withkinematic and environment constraints. In: Proceedings of theInternational Conference on Intelligent Autonomous Systems,pp. 346–354.

LaValle SM, Branicky MS and Lindemann SR (2004) On therelationship between classical grid search and probabilisticroadmaps. International Journal of Robotics Research 23:673–692.

LaValle SM and Kuffner JJ (2001) Randomized kinodynamicplanning. International Journal of Robotics Research 20:378–400.

Lumelsky V and Stepanov A (1987) Automaton movingadmist unknown obstacles of arbitrary shape. Algorithmica 2:403–430.

Marder-Eppstein E, Berger E, Foote T, Gerkey B and Kono-lige K (2010) The office marathon: Robust navigation in an

indoor office environment. In: Proceedings of the InternationalConference on Robotics and Automation, pp. 300–307.

Minhas DS, Engh JA, Fenske MM, and Riviere CN (2007) Mod-eling of needle steering via duty-cycled spinning. In: Proceed-ings of the Conference of the IEEE Engineering in Medicineand Biology Society, pp. 2756–2759.

Munkres JR (2000) Topology. Upper Saddle River, NJ: PrenticeHall.

Niederreiter H (1992) Random Number Generation and Quasi-Monte-Carlo Methods. Philadelphia: Society for IndustrialMathematics.

Reeds JA and Shepp LA (1990) Optimal paths for a car that goesboth forwards and backwards. Pacific Journal of Mathematics145: 367–393.

Reif J and Wang H (1998) The complexity of the two dimensionalcurvature-constrained shortest-path problem. In: Third Interna-tional Workshop on Algorithmic Foundations of Robotics, pp.49–57.

Sampl P (2001) Medial axis construction in three dimensions andits application to mesh generation. Engineering with Comput-ers 17: 234–248.

Sánchez G and Latombe JC (2002) On delaying collision check-ing in PRM planning: Application to multi-robot coordination.International Journal of Robotics Research 21: 5–26.

Schmitzberger E, Bouchet J, Dufaut M, Wolf D and Husson R(2002) Capture of homotopy classes with probabilistic roadmap. In: Proceedings of the International Conference on Intel-ligent Robots and Systems, pp. 2317–2322.

Schwarzer F, Saha M and Latombe J (2004) Exact collision check-ing of robot paths. In: Proceedings of the Workshop on theAlgorithmic Foundations of Robotics, pp. 25–41.

Simmons R (1996) The curvature-velocity method for local obsta-cle avoidance. In: Proceedings of the IEEE International Con-ference on Robotics and Automation, pp. 3375–3382.

Sun K and Lumelsky VJ (1991) Motion planning for three-link robot arm manipulators operating in an unknown three-dimensional environment. In: Proceedings of the ThirtiethIEEE Conference on Decision and Control, pp. 1019–1026.

Thorpe CE (1984) Path relaxation: Path planning for a mobilerobot. In: Proceedings of the National Conference on ArtificialIntelligence, pp. 318–321.

van den Berg J, Patil S, Alterovitz R, Abbeel P and Goldberg K(2010) LQG-based planning, sensing, and control of steerableneedles. In: Proceedings of the Ninth International Workshopon the Algorithmic Foundations of Robotics, pp. 373–389.

Vendittelli M, Laumond JP and Nissoux C (1999) Obstacle dis-tance for car-like robots. IEEE Transactions on Robotics andAutomation 15: 678–691.

Webster III RJ, Kim JS, Cowan NJ, Chirikjian GS and Oka-mura AM (2006) Nonholonomic modeling of needle steering.International Journal of Robotics Research 25: 509–525.

Yap CK (1987) An O( n log n) algorithm for the Voronoi diagramof a set of simple curve segments. Discrete & ComputationalGeometry 2: 365–393.

Yu Y and Gupta K (2000) An information theoretic approachto view point planning for motion planning of eye-in-handsystems. In: Proceedings of the International Symposium onRobotics, pp. 306–311.