An Equivalence Relation for Local Path Sets

We propose a novel enhancement to the task of collision-checking a set of local paths. Our approach exploits the fundamental structure of paths to identify regions of the workspace in which pairs of known collision-free paths permit us to declare a continuum of paths collision-free without an explicit test. We deﬁne an equivalence relation similar to homotopy among local paths. We provide algorithms to classify sets of paths and implicitly collision check up to 90% of them. We prove both correctness and completeness of our algorithms. Applied to the collision-checking task, our algorithms deliver a performance increase of up to 250%.


Introduction
Planning bounded-curvature paths for mobile robots is an to NP-hard problem [22].Many nonholonomic mobile robots thus rely on hierarchical planning architectures [1,12,19], which split responsibility between at least two layers (Fig. 1): a slow global planner and fast local planner.We focus here on the local planner (Algorithms 1 and 2), which iterates in a tight loop: searching through a set of paths and selecting the best path for execution.During each loop, the planner must test many paths in order to make an informed decision.The bottleneck in path testing is collision-checking [24].In this paper, we introduce a novel approach that delivers a significant increase in path set collision-checking performance by exploiting the fundamental geometric structure of paths.
Our equivalence relation intuitively resembles the topological notion of homotopy.Two paths are path homotopic if a continuous, collision-free deformation with fixed start and end points exists between them [20].Like any path equivalence rela-Fig.1 An example hierarchical planning scenario.The local planner's path set expands from the robot, at center, and feeds commands to the robot based on the best path that avoids obstacles (black squares).The chosen local path (green) and global path (red) combine to form a proposed path to the goal.tion, homotopy partitions paths into equivalence classes.Different homotopy classes make fundamentally different choices about their route amongst obstacles.However, two mobile robot concepts translate poorly into homotopy theory: limited sensing and constrained action.
One motivation for hierarchical planning is that the robot may lack a complete workspace map, which must instead be constructed from sensor data.Robot perception is limited both by range and occlusion, meaning that a robot's understanding of obstacles constraining its movement evolves with its vantage point.A variety of sensor-based planning algorithms have been developed to handle the partial information caused by occlusion and range limits.Obstacle avoidance methods, such as potential fields [11], are purely reactive.The bug algorithm [18], which generates a path to the goal using only a contact sensor, is complete in 2D.Choset and Burdick [5] present the hierarchical generalized Voronoi graph, a roadmap with global line-of-sight accessibility that achieve completeness in higher dimensions using range readings of the environment.Though sensor-based, such algorithms do not fit the hierarchical planning framework because they do not accept guidance from a global planner.
If a robot is tasked to perform long-range navigation, then it must plan a path through unsensed regions.The low-fidelity global planner generates this path because we prefer to avoid significant investment in this plan, which will likely later be invalidated.Path homotopy, in the strictest sense, requires global knowledge of obstacles because homotopy equivalent paths must connect fixed start and goal points.Relaxing the endpoint requirement avoids reasoning about the existence of far-away, unsensed obstacles.
Naively relaxing a fixed endpoint, our paths might be permitted to freely deform around obstacles, making all paths equivalent.To restore meaningful equivalence classes, we propose an alternate constraint based on path shape.This is in keeping with the nonholonomic constraints that limit mobile robots' action.Laumond [15] first highlighted the importance of nonholonomic constraints and showed that feasible paths exist for a mobile robot with such constraints.Barraquand and Latombe [2] created a grid-based planner that innately captures these constraints.LaValle and Kuffner [17] proposed the first planner to incorporate kinodynamic constraints while using random sampling to avoid the discretization issues associated with grids.Despite the importance of nonholonomic constraints, true homo-Fig.2 left: Paths from a few distinct homotopy classes between the robot and the goal.The distinctions between some classes require information that the robot has not yet sensed (the dark area is out of range or occluded).middle: With paths restricted to the sensed area, they may freely deform around visible obstacles.right: After restricting path shape to conform to motion constraints, we get a handful of equivalence classes that are immediately applicable to the robot.topy forbids restrictions on path shape; it considers two paths equivalent if any path deformation-however baroque-exists between them.By restricting our paths to bounded curvature, we represent only feasible motions while limiting paths' ability to deform around obstacles, resulting in a set of path equivalence classes that are of immediate importance to the planner (Fig. 2).
Equivalence classes have been employed to aid in various planning tasks.In discrete planning, identical objects (multiple grippers, balls, baskets, etc.) gives rise to redundant search.For instance, in naive planning the difficulty of the command "put a ball in a basket" scales with the number of grippers, balls and baskets available.Recent work has shown that equivalence classes of actions can be established to eliminate such redundant search [6].
In the realm of motion planning, path equivalence often employs homotopy.A recent paper by Bhattacharya, Kumar, and Likhachev [3] provides a technique based on complex analysis for detecting homotopic equivalence among paths in a graph.Two papers employing equivalence classes to build probabilistic roadmaps (PRMs) are by Schmitzberger, et al. [25] and Jaillet and Simeon [9].The latter paper departs from true homotopy by proposing the visibility deformation, a simplified alternative to homotopic equivalence based on line-of-sight visibility between paths.
Our key insight is that local path equivalence is an expressive and powerful tool that reveals shared outcomes in collision checking.Specifically, two equivalent neighboring paths cover some common ground in the workspace, and between them lies a continuum of covered paths.We develop the mathematical foundations to detect equivalence relations among all local paths based on a finite precomputed path set.We then utilize these tools to devise efficient algorithms for detecting equivalence and implicitly collision-checking local paths.
The remainder of the paper is organized as follows.Section 2 explores the theoretical foundations of our path equivalence relation.We provide an implementation of the basic algorithm in Section 3. Finally, Section 4 briefly describes some applications of this equivalence relation, including fast collision-checking.

Algorithm 1 Test All Paths(w, P)
Input: w -a costmap object; P -a fixed set of paths Output: P f ree , the set of paths that tested collision-free 1: Execute Path On Robot( j)

Equivalence Relation Foundations
In this section, we establish the foundations of our equivalence relation.We then provide a correctness proof for our approach to detecting equivalence.
We begin with some conventions on notation and terminology.In this work, we are only concerned with a kinematic description of paths.All paths start at a common position and heading.Let κ i (s) describe the curvature of path i as a function of arc length, with max 0≤s≤s f |κ i (s)| ≤ κ max .Typical expressions for κ i include polynomials, piecewise constant functions, and piecewise linear functions.The robot motion produced by path i is given by Next, we offer some general definitions.
Definition 1.A feasible path has bounded curvature and fixed length.The set F(s f , κ max ) contains all feasible paths of length s f and curvature |κ| ≤ κ max .
Definition 2. A segment is a closed interval describing all or part of a path.Definition 3. A swath is the workspace area of ground or volume of space swept out as the robot traverses a path.
Definition 4. We say a path is safe if its swath contains no obstacles.
Definition 5. Path space is a metric space (P, µ) describing the distance between pairs of paths in P defined by metric µ.Paths can vary in shape and length.
Throughout this paper, we use lowercase p to refer to a path (a set of points in the workspace), while P is a set of paths (each one a point in path space).

Metric
We use the Hausdorff metric to define our path space.The Hausdorff metric returns the greatest amount of separation between two paths in the workspace.We borrow from Munkres [20], regarding the paths as closed point sets X and Y : where (p) r denotes dilation of p by r: {t ∈ R 2 : µ d (t p ,t) ≤ r for some t p ∈ p}.In this case, we use the Euclidean metric for µ d .Note that µ H satisfies all properties of a metric [8].In Section 2.2, we employ the Hausdorff metric to define a sufficient condition for path equivalence.Other metrics may also suffice, but the Hausdorff distance has many well-known properties and is easy to compute.

Properties of Paths
In this section, we establish a small set of conditions under which we can quickly determine that two paths are equivalent.We constrain path shape through two dimensionless ratios, which relate three physical parameters.We may then detect equivalence through a simple test on pairs of paths using the Hausdorff metric.The constraints will forbid paths from crossing themselves, and they seek to minimize the extent to which a path turns back on itself.In so doing, these constraints ensure a continuous deformation between neighboring paths.Many important classes of action sets obey these general constraints, including the line segments common in RRT [17] and PRM [10] planners, as well as constant curvature arcs.Fig. 1 illustrates a more expressive action set [12] that adheres to our constraints.
The three physical parameters are: d, the diameter of the robot; s f , the length of each path; and r min , the minimum radius of curvature allowed for any path.Note that 1/r min = κ max , the upper bound on curvature.For non-circular robots, d reflects the minimal cross-section of the robot's swath sweeping along a path.We express relationships among the three physical quantities by two dimensionless parameters: Fig. 3 provides some intuition on the effect of these parameters.Due to the geometry of paths, only certain choices of v and w are appropriate.The value of v affects the "curviness" allowed in paths, while w affects their length.Definition 6.A path conforming to appropriate values of v and w from the proof of Lemma 2 is an appropriate path, Fig. 3 previews the permissible values.
After selecting appropriate v and w, the final condition is When this condition is met, the two paths' swaths overlap, resulting in a continuum of coverage between the paths.This coverage, in turn, ensures the existence of a continuous deformation, as we shall show in Theorem 1, but first we formally define a continuous deformation between paths.
Definition 7. A continuous deformation between two safe, feasible paths p i and p j in F(s f , κ max ) is a function f : [0, 1] → F(s e , 4 3 κ max ), with s e slightly less than s f .f (0) is the initial segment of p i , and f (1) is the initial segment of p j .We write p i ∼ p j to indicate that a continuous deformation exists between paths p i and p j , and they are therefore equivalent.
The length s e depends on v and w, but for typical values, s e is fully 95-98% of s f .For many applications, this is sufficient, but an application can quickly test the remaining path length if necessary.Nearly all paths f (c) are are bounded by curvature κ max , but it will turn out that in certain geometric circumstances, the maximum curvature through a continuous deformation is up to 4  3 κ max .
Definition 8. Two safe, feasible paths that define a continuous deformation are called guard paths because they protect the intermediate paths.
In the presence of obstacles, it is not trivial to determine whether a continuous deformation exists between two feasible paths.Rather than trying to find a deformation between arbitrary paths, we propose a particular condition under which we prove that a bounded-curvature, fixed-length, continuous path deformation exists, Another way to view this guarantee is to consider collision checking as a coverage problem.The overlapping swaths of p 1 and p 2 cover a continuum of intermediate swaths between the two paths.Eqn. ( 4) is a proper equivalence relation because it possesses each of three properties: • reflexivity.µ H (p, p) = 0; p is trivially deformable to itself.
• symmetry.The Hausdorff metric is symmetric.
• transitivity.The continuous deformation from p 1 to p 3 proceeds via p 2 .

Correctness
Having presented the the set of conditions under which (4) holds, we now prove that they are sufficient to ensure the existence of a continuous deformation between two neighboring paths.Our approach to the proof will be to first describe a continuous path deformation, then show that paths along this deformation are feasible and safe.Given two feasible guard paths p i and p j with common origin, let p e be the shortest curve connecting their endpoints that does not cross either path.The set B = p i ∪ p j ∪ p e creates one or more closed loops (the paths may cross each other).By the Jordan curve theorem, each loop partitions R 2 into two sets, only one of which is compact.Let I, the interior, be the union of these compact regions.Definition 9. A path p c is between paths p i and p j if they share common initial position and p c ⊂ I.
Lemma 1.Given appropriate paths p i , p j ⊂ F(s f , κ max ) with µ H (p i , p j ) ≤ d, there exists a path sequence in the form of a continuous deformation between p i and p j .
Proof.We provide the form of a continuous deformation from p i to p j such that each intermediate path is between them.Let t be a point in the workspace and p a path.Let Each level set g(t) = c for c ∈ [0, 1] defines a weighted generalized Voronoi diagram (GVD), as in Fig. 4. For general shapes in R 2 , the GVD forms a set of Fig. 4 Continuous deformation between paths, as defined by the level sets of (6).The upper bounds on curvature, shown at right, vary along the deformation.κ/κ bound max curves meeting at branching points [23].However, we will show that appropriate v and w restrict the GVD to a simple curve between the origin and p e .We give the form of a continuous deformation using level sets g −1 (c) as follows.Each path is parametrized starting at the origin and extending for a length s e in the direction of p e .Let us now pin down the value of s e .Every point t i in p i forms a line segment projecting it to its nearest neighbor t j on p j (and vice versa).Their collective area is shown in Fig. 5. Eqn.(3) constrains each segment to have length d or less.s e is the greatest value such that no intermediate path of length s e departs from the region covered by these projections.
The center of curvature of the guard path at t j must lie somewhere along the line defined by t i and t j .However, curvature constraints prevent it from lying between p i and p j since d < r min .Consequently, no GVD cusps or branching points occur in any intermediate path [4].Therefore, each level set defines a path through the origin.
Now we show bounds on curvature.Each path's curvature function is piecewise continuous and everywhere bounded.A small neighborhood of either guard path approximates constant curvature.A GVD curve generated by two constant-curvature sets forms a conic section [27].Where curvature discontinuities occur in a guard path, the GVD's curvature remains bounded across two adjacent continuous curvature segments.Table 1 reflects that the curvature of p c is everywhere bounded with the maximum possible curvature being 4  3 κ max .Fig. 5 Hausdorff coverage (delta shape in center) is a conservative approximation of swath coverage (gray).The Hausdorff distance between paths p i and p j is equal to the maximum-length projection from any point on either path to the closest point on the opposite path.Each projection implies a line segment.The set of projections from the top line (blue) and bottom line (red) each cover a solid region between the paths.These areas, in turn, cover a slightly shorter intermediate path p c , in white, with its swath in cyan.This path's length, s e is as great as possible while remaining safe.
nearest neighbor of point t on path p.The two swaths form a safe region, U = sl(p i , d) ∪ sl(p j , d).
Suppose that U contains a hole, the set h, which might contain an obstacle.Now, consider the shape of the paths that could produce such a hole.Beginning with equal position and heading, they must diverge widely enough to separate by more than d.To close the loop in U, the paths must then bend back towards each other.Since the paths separate by more than d, there exist two open intervals p h i ⊂ p i and p h j ⊂ p j surrounding the hole on each path such that (at this point) p h i ⊂ (p j ) d and p h j ⊂ (p i ) d .To satisfy (3), there must exist later intervals p e i ⊂ p i such that p h j ⊂ (p e i ) d and likewise p e j ⊂ p j such that p h i ⊂ (p e j ) d , as in Fig. 6a.How long must a path be to satisfy this condition?Consider the minimum length solution to this problem under bounded curvature.For each point t ∈ p h j , the segment p e i must intersect the open disc D = int((t) d ), as in Fig. 6b.Since p h j grows with the width of h, and p e i must intersect all of these open neighborhoods, the path becomes longer with larger holes.We will therefore consider the minimal small-hole case.
Vendittelli [26] solves the shortest path problem for a Dubins car to reach a line segment.We may approximate the circular boundary of D by a set of arbitrarily small line segments.One may show from this work that given the position and slope of points along any such circle, the shortest path to reach its boundary (and thus its interior) is a constant-curvature arc of radius r min .In the limit, as v approaches one and the size of h approaches zero, the length of arc needed to satisfy (3) approaches π/2 from above, resulting in the condition that w > 0.48.Thus, for w ≤ 0.48 and v ∈ [0, 1), p c is safe.For smaller values of v, D shrinks relative to r min , requiring longer paths to reach, thus allowing larger values of w as shown in the plot in Fig. 3.
We have shown that there exist appropriate choices for v and w such that (3) implies that U contains no holes.Since U contains the origin, any path p c ∈ I emanating from the origin passes through U and is safe.6 (a) With bounded curvature, there is a lower bound on path lengths that permit a hole, h, while satisfying (3).Shorter path lengths ensure the existence of a continuous deformation between paths.(b) We compute the maximal path length that prevents a hole using Vendittelli's solution to the shortest path for a Dubins car.Starting from the dot marked s, we find the shortest path intersecting the circle D. The segment p e i illustrates path lengths permitting a hole to exist.
Theorem 1.Given safe, appropriate guard paths p i and p j , µ H (p i , p j ) ≤ d implies that a continuous deformation exists between p i and p j .
Proof.Lemma 1 shows that (6) gives a continuous deformation between paths p i and p j such that each intermediate path p c ⊂ I is feasible.Lemma 2 shows that any such path is safe.Therefore, a continuous deformation exists between p i and p j .This proves the validity of the Hausdorff metric as a test for path equivalence.

Implementation
In this section, we present two algorithms: path set generation, and discrete path classification using the equivalence relation from Definition 7. The contents of this section will be the foundation for an implicit collision-checking algorithm in Section 4.1.All of the algorithms in this paper run in polynomial time.Before proceeding, we must define dispersion [21]: Definition 10.Given a bounded metric space (X, µ) and point set P = {x 1 , . . ., x N } ∈ X, the dispersion of P in X is defined by The dispersion of P in X equals the radius of the biggest open ball in X containing no points in P. By minimizing dispersion, we ensure that there are no large voids.Thus, dispersion reveals the quality of P as an "approximation" of X because it guarantees that for any x ∈ X, there is some point p ∈ P such that µ(x, p) ≤ δ (P, X).

Path Set Generation
We use the greedy path set construction technique described by Green and Kelly [7] and outlined in Algorithm 3. The algorithm draws paths from a densely-sampled source path set, X.It produces a sequence of path sets, P n for n ∈ {1, . . ., N}, with monotonically decreasing dispersion.We will search the paths in this order at runtime, thus permitting early termination while retaining near-optimal results.Note that while the source set X is of finite size-thus providing a lower bound on dispersion online-it can be chosen with arbitrarily low dispersion a priori.

Algorithm 3 Green Kelly(X, N)
Input: X -a densely-sampled, low-dispersion path set; N ≤ |X| -the target path set size Output: path sequence P N of size N 1:

Path Classification
We describe an equivalence relation that classifies paths based on the route by which they avoid obstacles.We now present Algorithm 4, which uses a finite set of appropriate paths to approximate the equivalence classes for a given configuration; each one is a continuum set of connected components of the safe, feasible path space.
Briefly, this algorithm takes as input a set of paths that have already tested collision-free at runtime.We form a graph G = (V, E) in which path p i corresponds to a node v i ∈ V .If (3) holds true, then an edge e i j ∈ E joins nodes v i and v j .Due to transitivity, any two paths p a and p b are in the same equivalence class if nodes v a and v b are in the same connected component of G (Fig. 7).
Effectively, this algorithm constructs a probabilistic roadmap (PRM) in path space instead of the conventional configuration space.A query into this PRM tells whether a continuous deformation exists between two paths.As with any PRM, a query is performed by adding two new graph nodes, v s and v g , corresponding to the start and goal paths.We attempt to join these nodes to other nodes in the graph based on (4).We are not necessarily interested in finding the actual path in path space, which would describe how p s may be deformed into p g .Instead, it is sufficient for our purposes to know that p s and p g are equivalent under deformation.

Resolution Completeness
In this section, we show that our discrete approximation of the continuum path space is resolution complete, meaning that for a sufficiently high sampling resolutionmeasured by dispersion-the approximation has the same connectivity as the continuum safe, feasible path space.
Let F be the continuum feasible path space and F f ree ⊂ F be the set of safe, feasible paths.Using the Green-Kelly algorithm, we sample offline from F a path sequence P of size N.At runtime, using Algorithm 1, we test members of P in order to discover a set P f ree ⊂ P of safe paths.
The following lemma is based on the work of LaValle, Branicky, and Lindemann [16], who prove resolution completeness of deterministic roadmap (DRM) planners, which are PRM planners that draw samples from a low-dispersion, deterministic source.Since we use a deterministic sequence provided by Green-Kelly, the combination of Algorithms 1 and 4 generates a DRM in path space.Lemma 3.For any given configuration of obstacles and any path set P N generated by the Green-Kelly algorithm, there exists a sufficiently large N such that any two paths p i , p j ∈ P f ree are in the same connected component of F f ree if and only if Algorithm 4 reports that p i ∼ p j .
Proof.LaValle, et al. [16], show that by increasing N, a sufficiently low dispersion can be achieved to make a DRM complete in any given C-Space.By an identical argument, given a continuum connected component C ⊂ F f ree , all sampled paths in C ∩ P N are in a single partition of D. If q is the radius of the narrowest corridor in C, then for dispersion δ N < q, our discrete approximation exactly replicates the connectivity of the continuum freespace.Lemma 4.Under the same conditions as in Lemma 3, there exists a sufficiently large N such that for any continuum connected component C ⊂ F f ree , Algorithm 1 returns a P f ree such that P f ree ∩ C = / 0. That is, every component in F f ree has a corresponding partition returned by Algorithm 4.
Proof.Let B r be the largest open ball of radius r in C. When δ N < r, B r must contain some sample p ∈ P. Since C is entirely collision-free, p ∈ P f ree .Thus, for dispersion less than r, P f ree contains a path in C.
After N samples, P has achieved dispersion δ N < min(q, r).There exists a sufficiently large N such that a bijection exists between the connected components of P f ree and F f ree .Proof.Lemma 3 establishes that f is one-to-one, while Lemma 4 establishes that f is onto.Therefore, f is bijective.This shows that by sampling at sufficiently high density, we can achieve an arbitrarily good approximation of the connectedness of the continuum set of collision-free paths in any environment.

Applications
The local path set equivalence classes we have described promise a number of enhancements to the hierarchical planning process.Here, we briefly present two applications.For in-depth empirical analyses of them, see [14].

Implicit Path Safety Test
There is an incessant need in motion planning to accelerate collision-checking, which may take 99% of total CPU time [24].In checking many swaths of a robot passing through space, most planners effectively test the free workspace many times by testing overlapping swaths.We may test a path implicitly, at significant computational savings, by recalling which parts of the space have been covered already.Earlier, we used Lemma 2 to prove equivalence between neighboring paths, but it also allows us to implicitly test new paths without the cost of collision checking the entire path length.We formalize the idea in Algorithm 5, which is designed to be invoked from Algorithm 1, line 4 in lieu of the standard path test routine.
Note that considerable portions of the work described in Algorithm 5 can be precomputed and stored in lookup tables.Since we are planning with a precomputed path set, we know the relationships between paths and can find exactly the pairs of paths that cover any path in our set.It is then a simple matter to discover whether any of those pairs is safe.In [14], we show that this algorithm allows us to test up to 90% of paths implicitly, thus increasing the path evaluation rate by 250%.Theorem 3. A path segment p may be implicitly tested safe if it is between paths p i and p j such that µ H (p i , p j ) ≤ d and a small region at the end of p c has been explicitly tested.
Proof.By Lemma 2, the initial segment of p c is safe because its swath is covered by the swaths of the guard paths.Since the small segment at the end of p c has been explicitly tested, the whole of p c is collision-free.

Path Selection
Another application for equivalence comes in the selection of a path for execution during each replan step.As stated, the Local Planning Algorithm lacks any notion of continuity between cycles.A heuristic objective function selects each path independently, leading to emergent behavior that can be difficult to understand or engineer.
We therefore introduce a two-stage path selection process.Stage one uses path equivalence classes to select a subset of paths that make progress toward the goal along the preferred route.Stage two then applies the objective function to pick the best path within the desired route.
Just as (4) identifies spatially equivalent path pairs, the same rule also finds temporal equivalence among paths.Between iterations, the robot's motion may induce splits and merges among the classes.By recalling the previous cycle's choice of class, successive cycles may give preference to a successor class.However, narrow corridors should be avoided, as the planner may have difficulty sampling repeatedly from a small volume of path space with a fixed path set.

Discussion and Future Work
In this paper, we propose an equivalence relation on local paths based on the following constraints: fixed start position and heading, fixed length, and bounded curvature.We describe an algorithm for easily classifying paths using the Hausdorff distance between them.Path classification permits collective reasoning about paths, leading to smarter decisions regarding obstacle avoidance and collision checking.
There are many other applications for path equivalence.One example uses path class knowledge to swing wide around occluding corners, thus improving visibility and safety.Another avenue of future work involves generalizing path equivalence to higher dimensions.For instance, an implicit path test for a robot floating in 3D requires three neighboring paths, while a manipulator arm needs only two.

Fig. 3
Fig. 3 At top: several example paths combining different values of v and w.Each path pair obeys(3).The value of v affects the "curviness" allowed in paths, while w affects their length.At bottom: this plot, generated numerically, approximates the set of appropriate choices for v and w.The gray region at top right is to be avoided, as we prove in Lemma 2. A path whose values fall in the white region is called an appropriate path.
Fig.6(a) With bounded curvature, there is a lower bound on path lengths that permit a hole, h, while satisfying(3).Shorter path lengths ensure the existence of a continuous deformation between paths.(b) We compute the maximal path length that prevents a hole using Vendittelli's solution to the shortest path for a Dubins car.Starting from the dot marked s, we find the shortest path intersecting the circle D. The segment p e i illustrates path lengths permitting a hole to exist.

Fig. 7 A
Fig.7A simple path set, in which obstacles (black) eliminate colliding paths.The surviving path set has three equivalence classes (red, green, and blue).In the corresponding graph representation, at right, adjacent nodes represent proximal paths.Connected components indicate equivalence classes of paths.In general, components need not be complete.

Algorithm 4
Equivalence Classes(P f ree , d) Input: P f ree -a set of safe, appropriate paths; d -the diameter of the robot Output: D -a partition of P f ree into equivalence classes (a set of path sets) 1: Let G = (V, E) ← ( / 0, / 0) 2: D ← / 0 3: for all p i ∈ P f ree do // This loop discovers adjacency 4: V.add(p i ) // Add a graph node corresponding to path p i 5: for all p j ∈ V \ p i do 6: if µ H (p i , p j ) < d then 7: E.add(i, j) // Connect nodes i and j with an unweighted edge 8: S ← P f ree 9: while S = / 0 do // This loop finds the connected components 10: C ← / 0 11: p ← a member of S 12: L ← {p} // List of nodes to be expanded in this class 13: while L = / 0 do 14: p ← a member of L 15: C ← C ∪ {p} // Commit p to class 16: S ← S − {p} 17: L ← (L ∪V.neighbors(p)) ∩ S 18: D ← D ∪ {C} 19: return D

Theorem 2 .
Let D = {D 1 | . . .|D m } be a partition of P f ree as defined by Algorithm 4. Let C = {C 1 | . . .|C m } be a finite partition of the continuum safe, feasible path space into connected components.A bijection f : D → C exists such that D i ⊂ f (D i ).
Given safe, appropriate guard paths p i , p j ∈ F(s f , κ max ), any path p c ⊂ F(s e ,43 κ max ) between them is safe.Proof.Let sl(p, d) = {t ∈ R 2 ,t p = nn(t, p) : t p t ⊥ p and t − t p L2 ≤ d 2 } define a conservative approximation of a swath, obtained by sweeping a line segment of length d along the path.t p t is the line segment joining t p to t and nn(t, p) is the Algorithm 5 Test Path Implicit(p, w, S, d)Input: p is a path to be tested Input: w is a costmap object // used as a backup when path cannot be implicitly tested Input: S is the set of safe paths found so far Input: d is the diameter of the robot 1: for all p i , p j ∈ S such that µ H (p i , p j ) ≤ d do