Roadmap Methods for Multiple Queries

Previously, it was assumed that a single initial-goal pair was given to the planning algorithm. Suppose now that numerous initial-goal queries will be given to the algorithm, while keeping the robot model and obstacles fixed. This leads to a multiple-query version of the motion planning problem. In this case, it makes sense to invest substantial time to preprocess the models so that future queries can be answered efficiently. The goal is to construct a topological graph called a roadmap, which efficiently solves multiple initial-goal queries. Intuitively, the paths on the roadmap should be easy to reach from each of qI and qG, and the graph can be quickly searched for a solution. The general framework presented here was mainly introduced in [516] under the name probabilistic roadmaps (PRMs). The probabilistic aspect, however, is not important to the method. Therefore, we call this family of methods sampling-based roadmaps. This distinguishes them from combinatorial roadmaps, which will appear in Chapter 6.

• • 1. The Basic Method

Once again, let G(V, E) represent a topological graph in which V is a set of vertices and E is the set of paths that map into free. Under the multiple-query philosophy, motion planning is divided into two phases of computation:

C

Preprocessing Phase: During the preprocessing phase, substantial effort is invested to build in a way that is useful for quickly answering future

G

queries. For this reason, it is called a roadmap, which in some sense should be accessible from every part of Cfree.

Query Phase: During the query phase, a pair, qI and qG, is given. Each configuration must be connected easily to using a local planner. Following this, a discrete search is performed using any of the algorithms in Section

G

2.2 to obtain a sequence of edges that forms a path from qI to qG.

BUILD ROADMAP

1 .init(); i 0;

G ←

1. while i < N
2. if α(i) ∈ Cfree then
3. G.add vertex(α(i)); i ← i + 1;
4. for each q ∈ neighborhood(α(i),G)
5. if ((not G.same component(α(i), q)) and connect(α(i), q)) then
6. G.add edge(α(i), q);

Figure 5.25: The basic construction algorithm for sampling-based roadmaps. Note that i is not incremented if α(i) is in collision. This forces i to correctly count the number of vertices in the roadmap.

Figure 5.26: The sampling-based roadmap is constructed incrementally by at- tempting to connect each new sample, α(i), to nearby vertices in the roadmap.

Generic preprocessing phase Figure 5.25 presents an outline of the basic preprocessing phase, and Figure 5.26 illustrates the algorithm. As seen throughout this chapter, the algorithm utilizes a uniform, dense sequence α. In each iteration,

the algorithm must check whether α(i) ∈ Cfree. If α(i) ∈ Cobs, then it must

continue to iterate until a collision-free sample is obtained. Once α(i) free, then in line 4 it is inserted as a vertex of . The next step is to try to connect α(i) to some nearby vertices, q, of . Each connection is attempted by the connect function, which is a typical LPM (local planning method) from Section 5.4.1. In most implementations, this simply tests the shortest path between α(i) and

G

G

∈ C

1. Experimentally, it seems most efficient to use the multi-resolution, van der Corput–based method described at the end of Section 5.3.4 [379]. Instead of the shortest path, it is possible to use more sophisticated connection methods, such as the bidirectional algorithm in Figure 5.24. If the path is collision-free, then connect returns true.

The same component condition in line 6 checks to make sure α(i) and q are in different components of before wasting time on collision checking. This en- sures that every time a connection is made, the number of connected components

G

of is decreased. This can be implemented very efficiently (near constant time) using the previously mentioned union-find algorithm [243, 823]. In some imple- mentations this step may be ignored, especially if it is important to generate multiple, alternative solutions. For example, it may be desirable to generate so- lution paths from different homotopy classes. In this case the condition (not

G

.same component(α(i), q)) is replaced with .vertex degree(q) < K, for some fixed K (e.g., K = 15).

G G

Selecting neighboring samples Several possible implementations of line 5 can be made. In all of these, it seems best to sort the vertices that will be considered for connection in order of increasing distance from α(i). This makes sense because shorter paths are usually less costly to check for collision, and they also have a higher likelihood of being collision-free. If a connection is made, this avoids costly collision checking of longer paths to configurations that would eventually belong to the same connected component.

Several useful implementations of neighborhood are

• 1. Nearest K: The K closest points to α(i) are considered. This requires setting the parameter K (a typical value is 15). If you are unsure which implementation to use, try this one.
  2. Component K: Try to obtain up to K nearest samples from each con- nected component of . A reasonable value is K = 1; otherwise, too many connections would be tried.

G

• 1. Radius: Take all points within a ball of radius r centered at α(i). An upper limit, K, may be set to prevent too many connections from being attempted. Typically, K = 20. A radius can be determined adaptively by shrinking the ball as the number of points increases. This reduction can be based on dispersion or discrepancy, if either of these is available for α. Note that if the samples are highly regular (e.g., a grid), then choosing the nearest K and taking points within a ball become essentially equivalent. If the point set is highly irregular, as in the case of random samples, then taking the nearest K seems preferable.
  2. Visibility: In Section 5.6.2, a variant will be described for which it is worth- while to try connecting α to all vertices in G.

Note that all of these require to be a metric space. One variation that has not yet been given much attention is to ensure that the directions of the neighborhood points relative to α(i) are distributed uniformly. For example, if the 20 closest points are all clumped together in the same direction, then it may be preferable to try connecting to a further point because it is in the opposite direction.

C

Figure 5.27: An example such as this is difficult for sampling-based roadmaps (in higher dimensional C-spaces) because some samples must fall along many points in the curved tube. Other methods, however, may be able to easily solve it.

Query phase In the query phase, it is assumed that G is sufficiently complete to answer many queries, each of which gives an initial configuration, qI , and a goal configuration, qG. First, the query phase pretends as if qI and qG were chosen from

α for connection to G. This requires running two more iterations of the algorithm

in Figure 5.25. If qI and qG are successfully connected to other vertices in G, then

a search is performed for a path that connects the vertex qI to the vertex qG. The path in the graph corresponds directly to a path in free, which is a solution to the query. Unfortunately, if this method fails, it cannot be determined conclusively whether a solution exists. If the dispersion is known for a sample sequence, α, then it is at least possible to conclude that no solution exists for the resolution of the planner. In other words, if a solution does exist, it would require the path to travel through a corridor no wider than the radius of the largest empty ball [600].

C

Some analysis There have been many works that analyze the performance of sampling-based roadmaps. The basic idea from one of them [69] is briefly presented here. Consider problems such as the one in Figure 5.27, in which the connect method will mostly likely fail in the thin tube, even though a connection exists. The higher dimensional versions of these problems are even more difficult. Many planning problems involve moving a robot through an area with tight clearance. This generally causes narrow channels to form in free, which leads to a challenging planning problem for the sampling-based roadmap algorithm. Finding the escape of a bug trap is also challenging, but for the roadmap methods, even traveling through a single corridor is hard (unless more sophisticated LPMs are used [479]). Let V (q) denote the set of all configurations that can be connected to q using the connect method. Intuitively, this is considered as the set of all configurations

C

that can be “seen” using line-of-sight visibility, as shown in Figure 5.28a The ǫ-goodness of Cfree is defined as

（ 1

ǫ(C

free

) = min

q∈Cfree

µ(V (q)) , (5.41)

µ(Cfree)

in which µ represents the measure. Intuitively, ǫ(Cfree) represents the small frac-

• • 1. Visibility definition (b) Visibility roadmap

Figure 5.28: (a) V (q) is the set of points reachable by the LPM from q. (b) A visibility roadmap has two kinds of vertices: guards, which are shown in black, and connectors, shown in white. Guards are not allowed to see other guards. Connectors must see at least two guards.

tion of free that is visible from any point. In terms of ǫ and the number of vertices in , bounds can be established that yield the probability that a solution will be found [69]. The main difficulties are that the ǫ-goodness concept is very conserva- tive (it uses worst-case analysis over all configurations), and ǫ-goodness is defined in terms of the structure of free, which cannot be computed efficiently. This result and other related results help to gain a better understanding of sampling- based planning, but such bounds are difficult to apply to particular problems to determine whether an algorithm will perform well.

G

C

C

• • 1. Visibility Roadmap

One of the most useful variations of sampling-based roadmaps is the visibility roadmap [885]. The approach works very hard to ensure that the roadmap repre- sentation is small yet covers free well. The running time is often greater than the basic algorithm in Figure 5.25, but the extra expense is usually worthwhile if the multiple-query philosophy is followed to its fullest extent.

C

The idea is to define two different kinds of vertices in G:

Guards: To become a guard, a vertex, q must not be able to see other guards. Thus, the visibility region, V (q), must be empty of other guards.

Connectors: To become a connector, a vertex, q, must see at least two guards. Thus, there exist guards q1 and q2, such that q ∈ V (q1) ∩ V (q2).

The roadmap construction phase proceeds similarly to the algorithm in Figure

5.25. The neighborhood function returns all vertices in G. Therefore, for each new sample α(i), an attempt is made to connect it to every other vertex in G.

The main novelty of the visibility roadmap is using a strong criterion to deter- mine whether to keep α(i) and its associated edges in . There are three possible cases for each α(i):

G

1. The new sample, α(i), is not able to connect to any guards. In this case,

α(i) earns the privilege of becoming a guard itself and is inserted into G.

1. The new sample can connect to guards from at least two different connected components of . In this case, it becomes a connector that is inserted into along with its associated edges, which connect it to these guards from

G

G

different components.

1. Neither of the previous two conditions were satisfied. This means that the sample could only connect to guards in the same connected component. In this case, α(i) is discarded.

The final condition causes a dramatic reduction in the number of roadmap vertices. One problem with this method is that it does not allow guards to be deleted in favor of better guards that might appear later. The placement of guards depends strongly on the order in which samples appear in α. The method may perform poorly if guards are not positioned well early in the sequence. It would be better to have an adaptive scheme in which guards could be reassigned in later iterations as better positions become available. Accomplishing this efficiently remains an open problem. Note the algorithm is still probabilistically complete using random sampling or resolution complete if α is dense, even though many samples are

rejected.

• • 1. Heuristics for Improving Roadmaps

The quest to design a good roadmap through sampling has spawned many heuris- tic approaches to sampling and making connections in roadmaps. Most of these exploit properties that are specific to the shape of the C-space and/or the partic- ular geometry and kinematics of the robot and obstacles. The emphasis is usually on finding ways to dramatically reduce the number or required samples. Several of these methods are briefly described here.

Vertex enhancement [516] This heuristic strategy focuses effort on vertices that were difficult to connect to other vertices in the roadmap construction algo- rithm in Figure 5.25. A probability distribution, P(v), is defined over the vertices v V . A number of iterations are then performed in which a vertex is sampled from V according to P(v), and then some random motions are performed from v to try to reach new configurations. These new configurations are added as ver- tices, and attempts are made to connect them to other vertices, as selected by the neighborhood function in an ordinary iteration of the algorithm in Figure 5.25. A recommended heuristic [516] for defining P(v) is to define a statistic for each v as nf /(nt + 1), in which nt is the total number of connections attempted for

∈

• • • 1. (b)

Figure 5.29: (a) To obtain samples along the boundary, binary search is used along random directions from a sample in obs. (b) The bridge test finds narrow corridors by examining a triple of nearby samples along a line.

C

v, and nf is the number of times these attempts failed. The probability P(v) is assigned as nf /(nt + 1)m, in which m is the sum of the statistics over all v V (this normalizes the statistics to obtain a valid probability distribution).

∈

Sampling on the Cfree boundary [22, 26] This scheme is based on the intu- ition that it is sometimes better to sample along the boundary, ∂Cfree, rather than

waste samples on large areas of free that might be free of obstacles. Figure 5.29a shows one way in which this can be implemented. For each sample of α(i) that falls into obs, a number of random directions are chosen in ; these directions can

C

C C

be sampled using the Sn sampling method from Section 5.2.2. For each direction, a binary search is performed to get a sample in Cfree that is as close as possible to Cobs. The order of point evaluation in the binary search is shown in Figure 5.29a. Let τ : [0, 1] denote the path for which τ (0) ∈ Cobs and τ (1) ∈ Cfree. In

the first step, test the midpoint, τ (1/2). If τ (1/2) free, this means that ∂ free lies between τ (0) and τ (1/2); otherwise, it lies between τ (1/2) and τ (1). The next iteration selects the midpoint of the path segment that contains ∂ free. This will be either τ (1/4) or τ (3/4). The process continues recursively until the desired resolution is obtained.

C

∈ C C

Gaussian sampling [132] The Gaussian sampling strategy follows some of the

same motivation for sampling on the boundary. In this case, the goal is to obtain points near ∂Cfree by using a Gaussian distribution that biases the samples to be closer to ∂Cfree, but the bias is gentler, as prescribed by the variance parameter of the Gaussian. The samples are generated as follows. Generate one sample, q1 ∈ C, uniformly at random. Following this, generate another sample, q2 ∈ C, according

to a Gaussian with mean q1; the distribution must be adapted for any topological identifications and/or boundaries of C. If one of q1 or q2 lies in Cfree and the other

lies in obs, then the one that lies in free is kept as a vertex in the roadmap. For some examples, this dramatically prunes the number of required vertices.

C C

Figure 5.30: The medial axis is traced out by the centers of the largest inscribed balls. The five line segments inside of the rectangle correspond to the medial axis.

Bridge-test sampling [465] The Gaussian sampling strategy decides to keep a point based in part on testing a pair of samples. This idea can be carried one step further to obtain a bridge test, which uses three samples along a line segment. If the samples are arranged as shown in Figure 5.29b, then the middle sample becomes a roadmap vertex. This is based on the intuition that narrow corridors are thin in at least one direction. The bridge test indicates that a point lies in a thin corridor, which is often an important place to locate a vertex.

Medial-axis sampling [455, 635, 971] Rather than trying to sample close to the boundary, another strategy is to force the samples to be as far from the boundary as possible. Let (X, ρ) be a metric space. Let a maximal ball be a ball B(x, r) X such that no other ball can be a proper subset. The centers of all maximal balls trace out a one-dimensional set of points referred to as the medial

⊆

axis. A simple example of a medial axis is shown for a rectangular subset of R2 in Figure 5.30. The medial axis in Cfree is based on the largest balls that can be inscribed in cl(Cfree). Sampling on the medial axis is generally difficult, especially

because the representation of free is implicit. Distance information from collision checking can be used to start with a sample, α(i), and iteratively perturb it to increase its distance from ∂ free [635, 971]. Sampling on the medial axis of W

C

C \ O

has also been proposed [455]. In this case, the medial axis in is easier to compute, and it can be used to heuristically guide the placement of good roadmap

W \ O

vertices in Cfree.

Further Reading

Unlike the last two chapters, the material of Chapter 5 is a synthesis of very recent research results. Some aspects of sampling-based motion planning are still evolving. Early approaches include [70, 144, 193, 280, 282, 329, 330, 658, 760]. The Gilbert- Johnson-Keerthi algorithm [388] is an early collision detection approach that helped inspire sampling-based motion planning; see [472] and [588] for many early references.

In much of the early work, randomization appeared to be the main selling point; however, more recently it has been understood that deterministic sampling can work at least as well while obtaining resolution completeness. For a more recent survey of sampling-based motion planning, see [640].

Section 5.1 is based on material from basic mathematics books. For a summary of basic theorems and numerous examples of metric spaces, see [696]. More material appears in basic point-set topology books (e.g., [451, 496]) and analysis books (e.g., [346]). Metric issues in the context of sampling-based motion planning are discussed in [21, 609]. Measure theory is most often introduced in the context of real analysis [346, 425, 546, 836, 837]. More material on Haar measure appears in [425].

Section 5.2 is mainly inspired by literature on Monte Carlo and quasi–Monte Carlo methods for numerical integration and optimization. An excellent source of material is [738]. Other important references for further reading include [191, 540, 682, 937, 938]. Sampling issues in the context of motion planning are considered in [380, 559, 600, 639, 987]. Comprehensive introductions to pure Monte Carlo algorithms appear in [341, 502]. The original source for the Monte Carlo method is [695]. For a survey on algorithms that compute Voronoi diagrams, see [54].

For further reading on collision detection (beyond Section 5.3), see the surveys in [488, 637, 638, 703]. Hierarchical collision detection is covered in [406, 638, 702]. The incremental collision detection ideas in Section 5.3.3 were inspired by the algorithm [636] and V-Clip [247, 702]. Distance computation is covered in [167, 306, 387, 406, 413, 702, 807]. A method suited for detecting self-collisions of linkages appears in [653]. A combinatorial approach to collision detection for motion planning appears in [855]. Numerous collision detection packages are available for use in motion planning research. One of the most widely used is PQP because it works well for any mess of 3D triangles [948].

The incremental sampling and searching framework was synthesized by unifying ideas from many planning methods. Some of these include grid-based search [71, 548, 620] and probabilistic roadmaps (PRMs) [516]. Although the PRM was developed for multiple queries, the single-query version developed in [125] helped shed light on the connection to earlier planning methods. This even led to grid-based variants of PRMs [123, 600]. Another single-query variant is presented in [845].

RDTs were developed in the literature mainly as RRTs, and were introduced in [598, 610]. RRTs have been used in several applications, and many variants have been developed [82, 138, 150, 200, 224, 244, 265, 362, 393, 495, 499, 498, 528, 631, 641, 642,

918, 919, 949, 979, 986]. Originally, they were developed for planning under differen- tial constraints, but most of their applications to date have been for ordinary motion planning. For more information on efficient nearest-neighbor searching, see the recent survey [475], and [46, 47, 48, 52, 99, 230, 365, 476, 538, 758, 908, 989].

Section 5.6 is based mainly on the PRM framework [516]. The “probabilistic” part is not critical to the method; thus, it was referred to here as a sampling-based roadmap. A related precursor to the PRM was proposed in [390, 391]. The PRM has been widely

used in practice, and many variants have been proposed [1, 23, 61, 62, 125, 161, 181, 244,

479, 544, 600, 627, 628, 740, 784, 792, 885, 900, 950, 971, 979, 995]. An experimental

comparison of many of these variants appears in [380]. Some analysis of PRMs appears in [69, 467, 573]. In some works, the term PRM has been applied to virtually any

sampling-based planning algorithm (e.g., [467]); however, in recent years the term has been used more consistently with its original meaning in [516].

Many other methods and issues fall outside of the scope of this chapter. Several interesting methods based on approximate cell decomposition [144, 328, 646, 658] can be considered as a form of sampling-based motion planning. A sampling-based method of

developing global potential functions appears in [124]. Other sampling-based planning algorithms appear in [194, 348, 417, 418, 463]. The algorithms of this chapter are gener- ally unable to guarantee that a solution does not exist for a motion planning problem. It is possible, however, to use sampling-based techniques to establish in finite time that

no solution exists [75]. Such a result is called a disconnection proof. Parallelization

issues have also been investigated in the context of sampling-based motion planning

[82, 177, 183, 257, 795].

Exercises

1. Prove that the Cartesian product of a metric space is a metric space by taking a linear combination as in (5.4).
2. Prove or disprove: If ρ is a metric, then ρ2 is a metric.
3. Determine whether the following function is a metric on any topological space: X:

ρ(x, x′) = 1 is x /= x′; otherwise, ρ(x, x′) = 0.

1. State and prove whether or not (5.28) yields a metric space on C = SE(3), as- suming that the two sets are rigid bodies.
2. The dispersion definition given in (5.19) is based on the worst case. Consider defining the average dispersion:

δ¯(P ) = 1

r

µ(X)

min ρ(x, p) dx. (5.42)

X p∈P

{ }

Describe a Monte Carlo (randomized) method to approximately evaluate (5.42).

1. Determine the average dispersion (as a function of i) for the van der Corput sequence (base 2) on [0, 1]/ ∼.
2. Show that using the Lebesgue measure on S3 (spreading mass around uniformly on S3) yields the Haar measure for SO(3).
3. Is the Haar measure useful in selecting an appropriate C-space metric? Explain.
4. Determine an expression for the (worst-case) dispersion of the ith sample in the base-p (Figure 5.2 shows base-2) van der Corput sequence in [0, 1]/ ∼, in which 0

and 1 are identified.

1. Determine the dispersion of the following sequence on [0, 1]. The first point is

α(1) = 1. For each i > 1, let ci = ln(2i − 3)/ ln 4 and α(i) = ci − ⌊ci⌋. It turns

out that this sequence achieves the best asymptotic dispersion possible, even in terms of the preceding constant. Also, the points are not uniformly distributed. Can you explain why this happens? [It may be helpful to plot the points in the sequence.]

1. Prove that (5.20) holds.
2. Prove that (5.23) holds.
3. Show that for any given set of points in [0, 1]n, a range space R can be designed so that the discrepancy is as close as desired to 1.
4. Suppose is a rigid body in R3 with a fixed orientation specified by a quaternion,

A

h. Suppose that h is perturbed a small amount to obtain another quaternion, h′

(no translation occurs). Construct a good upper bound on distance traveled by points on A, expressed in terms of the change in the quaternion.

1. Design combinations of robots and obstacles in W that lead to C-space obstacles resembling bug traps.
2. How many k-neighbors can there be at most in an n-dimensional grid with 1 ≤

k ≤ n?

1. In a high-dimensional grid, it becomes too costly to consider all 3n −1 n-neighbors. It might not be enough to consider only 2n 1-neighbors. Determine a scheme for selecting neighbors that are spatially distributed in a good way, but without requiring too many. For example, what is a good way to select 50 neighbors for a grid in R10?
2. Explain the difference between searching an implicit, high-resolution grid and growing search trees directly on the C-space without a grid.
3. Improve the bound in (5.31) by considering the fact that rotating points trace out a circle, instead of a straight line.
4. (Open problem) Prove there are n +1 main branches for an RRT starting from the center of an “infinite” n-dimensional ball in Rn. The directions of the branches align with the vertices of a regular simplex centered at the initial configuration.

Implementations

1. Implement 2D incremental collision checking for convex polygons to obtain “near constant time” performance.
2. Implement the sampling-based roadmap approach. Select an appropriate family of motion planning problems: 2D rigid bodies, 2D chains of bodies, 3D rigid bodies, etc.
   1. Compare the roadmaps obtained using visibility-based sampling to those obtained for the ordinary sampling method.
   2. Study the sensitivity of the method with respect to the particular neigh- borhood method.
   3. Compare random and deterministic sampling methods.
   4. Use the bridge test to attempt to produce better samples.
3. Implement the balanced, bidirectional RRT planning algorithm.
   1. Study the effect of varying the amount of intermediate vertices created along edges.
   2. Try connecting to the random sample using more powerful descent functions.
   3. Explore the performance gains from using Kd-trees to select nearest neigh- bors.
4. Make an RRT-based planning algorithm that uses more than two trees. Carefully resolve issues such as the maximum number of allowable trees, when to start a tree, and when to attempt connections between trees.
5. Implement both the expansive-space planner and the RRT, and conduct compar- ative experiments on planning problems. For the full set of problems, keep the algorithm parameters fixed.
6. Implement a sampling-based algorithm that computes collision-free paths for a rigid robot that can translate or rotate on any of the flat 2D manifolds shown in Figure 4.5.