Incremental Sampling and Searching

• • 1. The General Framework

The algorithms of Sections 5.4 and 5.5 follow the single-query model, which means (qI , qG) is given only once per robot and obstacle set. This means that there are no advantages to precomputation, and the sampling-based motion planning problem can be considered as a kind of search. The multiple-query model, which favors precomputation, is covered in Section 5.6.

The sampling-based planning algorithms presented in the present section are strikingly similar to the family of search algorithms summarized in Section 2.2.4. The main difference lies in step 3 below, in which applying an action, u, is replaced by generating a path segment, τs. Another difference is that the search graph, , is undirected, with edges that represent paths, as opposed to a directed graph in which edges represent actions. It is possible to make these look similar by defining an action space for motion planning that consists of a collection of paths, but this is avoided here. In the case of motion planning with differential constraints, this will actually be required; see Chapter 14.

G

Most single-query, sampling-based planning algorithms follow this template:

• • • 1. Initialization: Let (V, E) represent an undirected search graph, for which

G

V contains at least one vertex and E contains no edges. Typically, V contains

qI , qG, or both. In general, other points in Cfree may be included.

• • • 1. Vertex Selection Method (VSM): Choose a vertex qcur V for expan- sion.

∈

• • • 1. Local Planning Method (LPM): For some qnew ∈ Cfree that may or may not be represented by a vertex in V , attempt to construct a path τs : [0, 1] → Cfree such that τ (0) = qcur and τ (1) = qnew . Using the methods of Section

5.3.4, τs must be checked to ensure that it does not cause a collision. If this step fails to produce a collision-free path segment, then go to step 2.

• • • 1. Insert an Edge in the Graph: Insert τs into E, as an edge from qcur to

qnew. If qnew is not already in V , then it is inserted.

• • • 1. Check for a Solution: Determine whether encodes a solution path. As in the discrete case, if there is a single search tree, then this is trivial; otherwise, it can become complicated and expensive.

G

• • • 1. Return to Step 2: Iterate unless a solution has been found or some ter- mination condition is satisfied, in which case the algorithm reports failure.

In the present context, is a topological graph, as defined in Example 4.6. Each vertex is a configuration and each edge is a path that connects two configurations. In this chapter, it will be simply referred to as a graph when there is no chance of confusion. Some authors refer to such a graph as a roadmap; however, we reserve the term roadmap for a graph that contains enough paths to make any motion planning query easily solvable. This case is covered in Section 5.6 and throughout Chapter 6.

G

A large family of sampling-based algorithms can be described by varying the implementations of steps 2 and 3. Implementations of the other steps may also vary, but this is less important and will be described where appropriate. For convenience, step 2 will be called the vertex selection method (VSM) and step 3 will be called the local planning method (LPM). The role of the VSM is similar to that of the priority queue, Q, in Section 2.2.1. The role of the LPM is to compute a collision-free path segment that can be added to the graph. It is called local because the path segment is usually simple (e.g., the shortest path) and travels a short distance. It is not global in the sense that the LPM does not try to solve the entire planning problem; it is expected that the LPM may often fail to construct path segments.

It will be formalized shortly, but imagine for the time being that any of the search algorithms from Section 2.2 may be applied to motion planning by ap- proximating with a high-resolution grid. The resulting problem looks like a multi-dimensional extension of Example 2.1 (the “labyrinth” walls are formed by obs). For a high-resolution grid in a high-dimensional space, most classical dis- crete searching algorithms have trouble getting trapped in a local minimum. There could be an astronomical number of configurations that fall within a concavity in obs that must be escaped to solve the problem, as shown in Figure 5.13a. Imagine a problem in which the C-space obstacle is a giant “bowl” that can trap the config- uration. This figure is drawn in two dimensions, but imagine that the has many dimensions, such as six for SE(3) or perhaps dozens for a linkage. If the discrete

C

C

C

C

planning algorithms from Section 2.2 are applied to a high-resolution grid approx- imation of C, then they will all waste their time filling up the bowl before being

able to escape to qG. The number of grid points in this bowl would typically be on the order of 100n for an n-dimensional C-space. Therefore, sampling-based motion planning algorithms combine sampling and searching in a way that attempts to overcome this difficulty.

As in the case of discrete search algorithms, there are several classes of algo- rithms based on the number of search trees.

qG

• • • • 1. (b)

(c) (d)

Figure 5.13: All of these depict high-dimensional obstacle regions in C-space. (a) The search must involve some sort of multi-resolution aspect, otherwise, that al- gorithm may explore too many points within a cavity. (b) Sometimes the problem is like a bug trap, in which case bidirectional search can help. (c) For a double bug trap, multi-directional search may be needed. (d) This example is hard to solve even for multi-directional search.

Unidirectional (single-tree) methods: In this case, the planning ap- pears very similar to discrete forward search, which was given in Figure 2.4. The main difference between algorithms in this category is how they imple- ment the VSM and LPM. Figure 5.13b shows a bug trap9 example for which forward-search algorithms would have great trouble; however, the problem might not be difficult for backward search, if the planner incorporates some kind of greedy, best-first behavior. This example, again in high dimensions, can be considered as a kind of “bug trap.” To leave the trap, a path must be found from qI into the narrow opening. Imagine a fly buzzing around through the high-dimensional trap. The escape opening might not look too difficult in two dimensions, but if it has a small range with respect to each configuration parameter, it is nearly impossible to find the opening. The tip of the “volcano” would be astronomically small compared to the rest of the bug trap. Examples such as this provide some motivation for bidirectional

9This principle is actually used in real life to trap flying bugs. This analogy was suggested

by James O’Brien in a discussion with James Kuffner.

algorithms. It might be easier for a search tree that starts in qG to arrive in the bug trap.

Bidirectional (two-tree) methods: Since it is not known whether qI or qG might lie in a bug trap (or another challenging region), a bidirectional approach is often preferable. This follows from an intuition that two prop- agating wavefronts, one centered on qI and the other on qG, will meet after covering less area in comparison to a single wavefront centered at qI that must arrive at qG. A bidirectional search is achieved by defining the VSM to alternate between trees when selecting vertices. The LPM sometimes gen- erates paths that explore new parts of free, and at other times it tries to generate a path that connects the two trees.

C

Multi-directional (more than two trees) methods: If the problem is so bad that a double bug trap exists, as shown in Figure 5.13c, then it might make sense to grow trees from other places in the hopes that there are better chances to enter the traps in the other direction. This complicates the problem of connecting trees, however. Which pairs of trees should be selected in each iteration for possible connection? How often should the same pair be selected? Which vertex pairs should be selected? Many heuristic parameters may arise in practice to answer these questions.

Of course, one can play the devil’s advocate and construct the example in Figure 5.13d, for which virtually all sampling-based planning algorithms are doomed. Even harder versions can be made in which a sequence of several narrow corridors must be located and traversed. We must accept the fact that some problems are hopeless to solve using sampling-based planning methods, unless there is some problem-specific structure that can be additionally exploited.

• • 1. Adapting Discrete Search Algorithms

One of the most convenient and straightforward ways to make sampling-based planning algorithms is to define a grid over and conduct a discrete search using the algorithms of Section 2.2. The resulting planning problem actually looks very similar to Example 2.1. Each edge now corresponds to a path in free. Some edges may not exist because of collisions, but this will have to be revealed incrementally during the search because an explicit representation of obs is too expensive to construct (recall Section 4.3).

C

C

C

Assume that an n-dimensional C-space is represented as a unit cube, = [0, 1]n/ , in which indicates that identifications of the sides of the cube are made to reflect the C-space topology. Representing as a unit cube usually requires a reparameterization. For example, an angle θ [0, 2π) would be replaced with θ/2π to make the range lie within [0, 1]. If quaternions are used for SO(3),

∈

C

∼ ∼

C

then the upper half of S3 must be deformed into [0, 1]3/ ∼.

Discretization Assume that C is discretized by using the resolutions k1, k2,. . .,

and kn, in which each ki is a positive integer. This allows the resolution to be different for each C-space coordinate. Either a standard grid or a Sukharev grid can be used. Let

∆qi = [0 · · · 0 1/ki 0 · · · 0], (5.35)

in which the first i − 1 components and the last n − i components are 0. A grid point is a configuration q ∈ C that can be expressed in the form

10

n

-

ji∆qi, (5.36)

i=1

in which each ji 0, 1, . . . , ki . The integers j1, . . ., jn can be imagined as array indices for the grid. Let the term boundary grid point refer to a grid point for which ji = 0 or ji = ki for some i. Due to identifications, boundary grid points might have more than one representation using (5.36).

∈ { }

Neighborhoods For each grid point q we need to define the set of nearby grid points for which an edge may be constructed. Special care must be given to defining the neighborhood of a boundary grid point to ensure that identifications and the C-space boundary (if it exists) are respected. If q is not a boundary grid point, then the 1-neighborhood is defined as

N1(q) = {q + ∆q1, . . . , q + ∆qn, q − ∆q1, . . . , q − ∆qn}. (5.37)

For an n-dimensional C-space there at most 2n 1-neighbors. In two dimensions, this yields at most four 1-neighbors, which can be thought of as “up,” “down,” “left,” and “right.” There are at most four because some directions may be blocked by the obstacle region.

A 2-neighborhood is defined as

N2(q) = {q ± ∆qi ± ∆qj | 1 ≤ i, j ≤ n, i /= j} ∪ N1(q). (5.38)

Similarly, a k-neighborhood can be defined for any positive integer k n. For an n-neighborhood, there are at most 3n 1 neighbors; there may be fewer due to boundaries or collisions. The definitions can be easily extended to handle the boundary points.

−

≤

Obtaining a discrete planning problem Once the grid and neighborhoods have been defined, a discrete planning problem is obtained. Figure 5.14 depicts the process for a problem in which there are nine Sukharev grid points in [0, 1]2.

Using 1-neighborhoods, the potential edges in the search graph, G(V, E), appear in Figure 5.14a. Note that G is a topological graph, as defined in Example 4.6,

because each vertex is a configuration and each edge is a path. If qI and qG do not

10Alternatively, the general lattice definition in (5.21) could be used.

(a) (b)

(c) (d)

Figure 5.14: A topological graph can be constructed during the search and can successfully solve a motion planning problem using very few samples.

coincide with grid points, they need to be connected to some nearby grid points, as shown in Figure 5.14b. What grid points should qI and qG be connected to? As a general rule, if k-neighbors are used, then one should try connecting qI and qG to any grid points that are at least as close as the furthest k-neighbor from a typical grid point.

Usually, all of the vertices and edges shown in Figure 5.14b do not appear in G

because some intersect with obs. Figure 5.14c shows a more typical situation, in which some of the potential vertices and edges are removed because of collisions. This representation could be computed in advance by checking all potential vertices and edges for collision. This would lead to a roadmap, which is suited for multiple queries and is covered in Section 5.6. In this section, it is assumed that is revealed “on the fly” during the search. This is the same situation that occurs for the discrete planning methods from Section 2.2. In the current setting, the potential edges of are validated during the search. The candidate edges to evaluate are given by the definition of the k-neighborhoods. During the search,

C

G

G

any edge or vertex that has been checked for collision explicitly appears in a data structure so that it does not need to be checked again. At the end of the search, a path is found, as depicted in Figure 5.14d.

Grid resolution issues The method explained so far will nicely find the solution to many problems when provided with the correct resolution. If the number of points per axis is too high, then the search may be too slow. This motivates selecting fewer points per axis, but then solutions might be missed. This trade-off is fundamental to sampling-based motion planning. In a more general setting, if other forms of sampling and neighborhoods are used, then enough samples have to be generated to yield a sufficiently small dispersion.

There are two general ways to avoid having to select this resolution (or more generally, dispersion):

• • • 1. Iteratively refine the resolution until a solution is found. In this case, sam- pling and searching become interleaved. One important variable is how frequently to alternate between the two processes. This will be presented shortly.
      2. An alternative is to abandon the adaptation of discrete search algorithms and develop algorithms directly for the continuous problem. This forms the basis of the methods in Sections 5.4.3, 5.4.4, and 5.5.

The most straightforward approach is to iteratively improve the grid resolution. Suppose that initially a standard grid with 2n points total and 2 points per axis is searched using one of the discrete search algorithms, such as best-first or A∗. If the search fails, what should be done? One possibility is to double the resolution, which yields a grid with 4n points. Many of the edges can be reused from the first grid; however, the savings diminish rapidly in higher dimensions. Once the resolution is doubled, the search can be applied again. If it fails again, then the resolution can be doubled again to yield 8n points. In general, there would be a full grid for 2ni points, for each i. The problem is that if n is large, then the rate of growth is too large. For example, if n = 10, then there would initially be 1024 points; however, when this fails, the search is not performed again until there are over one million points! If this also fails, then it might take a very long time to reach the next level of resolution, which has 230 points.

A method similar to iterative deepening from Section 2.2.2 would be preferable. Simply discard the efforts of the previous resolution and make grids that have in points per axis for each iteration i. This yields grids of sizes 2n, 3n, 4n, and so on, which is much better. The amount of effort involved in searching a larger grid is insignificant compared to the time wasted on lower resolution grids. Therefore, it seems harmless to discard previous work.

A better solution is not to require that a complete grid exists before it can be searched. For example, the resolution can be increased for one axis at a time before attempting to search again. Even better yet may be to tightly interleave

searching and sampling. For example, imagine that the samples appear as an infinite, dense sequence α. The graph can be searched after every 100 points are added, assuming that neighborhoods can be defined or constructed even though the grid is only partially completed. If the search is performed too frequently, then searching would dominate the running time. An easy way make this efficient is to apply the union-find algorithm [243, 823] to iteratively keep track of connected components in instead of performing explicit searching. If qI and qG become part of the same connected component, then a solution path has been found. Every time a new point in the sequence α is added, the “search” is performed in nearly11 constant time by the union-find algorithm. This is the tightest interleaving of the sampling and searching, and results in a nice sampling-based algorithm that requires no resolution parameter. It is perhaps best to select a sequence α that contains some lattice structure to facilitate the determination of neighborhoods in each iteration.

G

What if we simply declare the resolution to be outrageously high at the outset? Imagine there are 100n points in the grid. This places all of the burden on the search algorithm. If the search algorithm itself is good at avoiding local minima and has built-in multi-resolution qualities, then it may perform well without the iterative refinement of the sampling. The method of Section 5.4.3 is based on this idea by performing best-first search on a high-resolution grid, combined with random walks to avoid local minima. The algorithms of Section 5.5 go one step further and search in a multi-resolution way without requiring resolutions and neighborhoods to be explicitly determined. This can be considered as the limiting case as the number of points per axis approaches infinity.

Although this section has focused on grids, it is also possible to use other forms of sampling from Section 5.2. This requires defining the neighborhoods in a suit- able way that generalizes the k-neighborhoods of this section. In every case, an infinite, dense sample sequence must be defined to obtain resolution completeness by reducing the dispersion to zero in the limit. Methods for obtaining neighbor- hoods for irregular sample sets have been developed in the context of sampling- based roadmaps; see Section 5.6. The notion of improving resolution becomes generalized to adding samples that improve dispersion (or even discrepancy).

• • 1. Randomized Potential Fields

Adapting the discrete algorithms from Section 2.2 works well if the problem can be solved with a small number of points. The number of points per axis must be small or the dimension must be low, to ensure that the number of points, kn, for k points per axis and n dimensions is small enough so that every vertex in g can be reached in a reasonable amount of time. If, for example, the problem requires 50 points per axis and the dimension is 10, then it is impossible to search all of

11It is not constant because the running time is proportional to the inverse Ackerman function, which grows very, very slowly. For all practical purposes, the algorithm operates in constant time. See Section 6.5.2.

Initialization (i=1)

Figure 5.15: The randomized potential field method can be modeled as a three- state machine.

the 5010 samples. Planners that exploit best-first heuristics might find the answer without searching most of them; however, for a simple problem such as that shown in Figure 5.13a, the planner will take too long exploring the vertices in the bowl.12 The randomized potential field [70, 72, 588] approach uses random walks to attempt to escape local minima when best-first search becomes stuck. It was one of the first sampling-based planners that developed specialized techniques beyond classical discrete search, in an attempt to better solve challenging motion planning problems. In many cases, remarkable results were obtained. In its time, the approach was able to solve problems up to 31 degrees of freedom, which was well beyond what had been previously possible. The main drawback, however, was that the method involved many heuristic parameters that had to be adjusted for each problem. This frustration eventually led to the development of better approaches, which are covered in Sections 5.4.4, 5.5, and 5.6. Nevertheless, it is worthwhile to study the clever heuristics involved in this earlier method because they illustrate many interesting issues, and the method was very influential in the

development of other sampling-based planning algorithms.13

The most complicated part of the algorithm is the definition of a potential function, which can be considered as a pseudometric that tries to estimate the distance from any configuration to the goal. In most formulations, there is an attractive term, which is a metric on that yields the distance to the goal, and a repulsive term, which penalizes configurations that come too close to obstacles. The construction of potential functions involves many heuristics and is covered in great detail in [588]. One of the most effective methods involves constructing cost-to-go functions over and lifting them to [71]. In this section, it will be sufficient to assume that some potential function, g(q), is defined, which is the same notation (and notion) as a cost-to-go function in Section 2.2.2. In this case, however, there is no requirement that g(q) is optimal or even an underestimate of the true cost to go.

C

W C

When the search becomes stuck and a random walk is needed, it is executed for some number of iterations. Using the discretization procedures of Section 5.4.2, a

12Of course, that problem does not appear to need so many points per axis; fewer may be used instead, if the algorithm can adapt the sampling resolution or dispersion.

13The exciting results obtained by the method even helped inspire me many years ago to work

on motion planning.

high-resolution grid (e.g., 50 points per axis) is initially defined. In each iteration, the current configuration is modified as follows. Each coordinate, qi, is increased or decreased by ∆qi (the grid step size) based on the outcome of a fair coin toss. Topological identifications must be respected, of course. After each iteration, the new configuration is checked for collision, or whether it exceeds the boundary of (if it has a boundary). If so, then it is discarded, and another attempt is made from the previous configuration. The failures can repeat indefinitely until a new

C

configuration in free is obtained.

C

The resulting planner can be described in terms of a three-state machine, which is shown in Figure 5.15. Each state is called a mode to avoid confusion with earlier state-space concepts. The VSM and LPM are defined in terms of the mode. Initially, the planner is in the best first mode and uses qI to start a gradient descent. While in the best first mode, the VSM selects the newest vertex, v V . In the first iteration, this is qI . The LPM creates a new vertex, vn, in a neighborhood of v, in a direction that minimizes g. The direction sampling may be performed using randomly selected or deterministic samples. Using random samples, the sphere sampling method from Section 5.2.2 can be applied. After some number of tries (another parameter), if the LPM is unsuccessful at reducing g, then the mode is changed to random walk because the best-first search is stuck in a local minimum of g.

∈

In the random walk mode, a random walk is executed from the newest ver- tex. The random walk terminates if either g is lowered or a specified limit of iterations is reached. The limit is actually sampled from a predetermined ran- dom variable (which contains parameters that also must be selected). When the random walk mode terminates, the mode is changed back to best first. A counter is incremented to keep track of the number of times that the random walk was attempted. A parameter K determines the maximum number of attempted random walks (a reasonable value is K = 20 [71]). If best first fails after K random walks have been attempted, then the backtrack mode is entered. The backtrack mode selects a vertex at random from among the vertices in V that were obtained during a random walk. Following this, the counter is reset, and the mode is changed back to best first.

Due to the random walks, the resulting paths are often too complicated to be useful in applications. Fortunately, it is straightforward to transform a computed path into a simpler one that is still collision-free. A common approach is to iteratively pick pairs of points at random along the domain of the path and attempt to replace the path segment with a straight-line path (in general, the shortest path

in C). For example, suppose t1, t2 ∈ [0, 1] are chosen at random, and τ : [0, 1] → Cfree is the computed solution path. This path is transformed into a new path,

 τ (t) if 0 ≤ t ≤ t1

τ ′(t) =

aτ (t1) + (1 − a)τ (t2) if t1 ≤ t ≤ t2

 τ (t) if t2 ≤ t ≤ 1,

(5.39)

in which a ∈ [0, 1] represents the fraction of the way from t1 to t2. Explicitly,

a = (t2 − t)/(t2 − t1). The new path must be checked for collision. If it passes,

then it replaces the old path; otherwise, it is discarded and a new pair t1, t2, is chosen.

The randomized potential field approach can escape high-dimensional local minima, which allow interesting solutions to be found for many challenging high- dimensional problems. Unfortunately, the heavy amount of parameter tuning caused most people to abandon the method in recent times, in favor of newer methods.

• • 1. Other Methods

Several influential sampling-based methods are given here. Each of them appears to offer advantages over the randomized potential field method. All of them use randomization, which was perhaps inspired by the potential field method.

Ariadne’s Clew algorithm This approach grows a search tree that is biased to explore as much new territory as possible in each iteration [688, 687]. There are two modes, search and explore, which alternate over successive iterations. In the explore mode, the VSM selects a vertex, ve, at random, and the LPM finds a new configuration that can be easily connected to ve and is as far as possible from the other vertices in . A global optimization function that aggregates the distances to other vertices is optimized using a genetic algorithm. In the search mode, an attempt is made to extend the vertex added in the explore mode to the goal configuration. The key idea from this approach, which influenced both the next approach and the methods in Section 5.5, is that some of the time must be spent exploring the space, as opposed to focusing on finding the solution. The greedy behavior of the randomized potential field led to some efficiency but was also its downfall for some problems because it was all based on escaping local minima with respect to the goal instead of investing time on global exploration. One disadvantage of Ariadne’s Clew algorithm is that it is very difficult to solve the optimization problem for placing a new vertex in the explore mode. Genetic algorithms were used in [687], which are generally avoided for motion planning because of the required problem-specific parameter tuning.

G

Expansive-space planner This method [467, 844] generates samples in a way that attempts to explore new parts of the space. In this sense, it is similar to the explore mode of the Ariadne’s Clew algorithm. Furthermore, the planner is made more efficient by borrowing the bidirectional search idea from discrete search

algorithms, as covered in Section 2.2.3. The VSM selects a vertex, ve, from G with a probability that is inversely proportional to the number of other vertices of G

that lie within a predetermined neighborhood of ve. Thus, “isolated” vertices are more likely to be chosen. The LPM generates a new vertex vn at random within a predetermined neighborhood of ve. It will decide to insert vn into with a probability that is inversely proportional to the number of other vertices

G

of that lie within a predetermined neighborhood of vn. For a fixed number of iterations, the VSM repeatedly chooses the same vertex, until moving on to another vertex. The resulting planner is able to solve many interesting problems by using a surprisingly simple criterion for the placement of points. The main drawbacks are that the planner requires substantial parameter tuning, which is problem-specific (or at least specific to a similar family of problems), and the performance tends to degrade if the query requires systematically searching a long labyrinth. Choosing the radius of the predetermined neighborhoods essentially amounts to determining the appropriate resolution.

G

Random-walk planner A surprisingly simple and efficient algorithm can be made entirely from random walks [179]. To avoid parameter tuning, the algorithm adjusts its distribution of directions and magnitude in each iteration, based on the success of the past k iterations (perhaps k is the only parameter). In each iteration, the VSM just selects the vertex that was most recently added to . The LPM generates a direction and magnitude by generating samples from a multivariate Gaussian distribution whose covariance parameters are adaptively tuned. The main drawback of the method is similar to that of the previous method. Both have difficulty traveling through long, winding corridors. It is possible to combine adaptive random walks with other search algorithms, such as the potential field planner [178].

G