Rapidly Exploring Dense Trees

This section introduces an incremental sampling and searching approach that yields good performance in practice without any parameter tuning.14 The idea is to incrementally construct a search tree that gradually improves the resolution but does not need to explicitly set any resolution parameters. In the limit, the tree densely covers the space. Thus, it has properties similar to space filling curves [842], but instead of one long path, there are shorter paths that are organized into a tree. A dense sequence of samples is used as a guide in the incremental con- struction of the tree. If this sequence is random, the resulting tree is called a rapidly exploring random tree (RRT). In general, this family of trees, whether the sequence is random or deterministic, will be referred to as rapidly exploring dense trees (RDTs) to indicate that a dense covering of the space is obtained. This method was originally developed for motion planning under differential constraints [608, 611]; that case is covered in Section 14.4.3.

14The original RRT [598] was introduced with a step size parameter, but this is eliminated in

the current presentation. For implementation purposes, one might still want to revert to this older way of formulating the algorithm because the implementation is a little easier. This will be discussed shortly.

SIMPLE RDT(q0)

1. .init(q0);

G

1. for i = 1 to k do
2. G.add vertex(α(i));
3. qn ← nearest(S(G), α(i));
4. G.add edge(qn, α(i));

Figure 5.16: The basic algorithm for constructing RDTs (which includes RRTs as a special case) when there are no obstacles. It requires the availability of a dense sequence, α, and iteratively connects from α(i) to the nearest point among

all those reached by G.

qn

• 1. (b)

Figure 5.17: (a) Suppose inductively that this tree has been constructed so far using the algorithm in Figure 5.16. (b) A new edge is added that connects from the sample α(i) to the nearest point in S, which is the vertex qn.

• • 1. The Exploration Algorithm

Before explaining how to use these trees to solve a planning query, imagine that the goal is to get as close as possible to every configuration, starting from an initial configuration. The method works for any dense sequence. Once again, let α denote an infinite, dense sequence of samples in . The ith sample is denoted by α(i). This may possibly include a uniform, random sequence, which is only dense with probability one. Random sequences that induce a nonuniform bias are also acceptable, as long as they are dense with probability one.

C

An RDT is a topological graph, (V, E). Let S free indicate the set of all points reached by . Since each e E is a path, this can be expressed as the swath, S, of the graph, which is defined as

G ∈

G ⊂ C

S = e([0, 1]). (5.40)

I

e∈E

In (5.40), e([0, 1]) free is the image of the path e.

⊆ C

The exploration algorithm is first explained in Figure 5.16 without any obsta- cles or boundary obstructions. It is assumed that C is a metric space. Initially,

a vertex is made at q0. For k iterations, a tree is iteratively grown by connecting

qn

q0 α(i)

Figure 5.18: If the nearest point in S lies in an edge, then the edge is split into two, and a new vertex is inserted into G.

45 iterations 2345 iterations

Figure 5.19: In the early iterations, the RRT quickly reaches the unexplored parts. However, the RRT is dense in the limit (with probability one), which means that it gets arbitrarily close to any point in the space.

Figure 5.20: If there is an obstacle, the edge travels up to the obstacle boundary, as far as allowed by the collision detection algorithm.

α(i) to its nearest point in the swath, S. The connection is usually made along the shortest possible path. In every iteration, α(i) becomes a vertex. Therefore, the resulting tree is dense. Figures 5.17–5.18 illustrate an iteration graphically. Suppose the tree has three edges and four vertices, as shown in Figure 5.17a. If

the nearest point, qn ∈ S, to α(i) is a vertex, as shown in Figure 5.17b, then an

edge is made from qn to α(i). However, if the nearest point lies in the interior of an edge, as shown in Figure 5.18, then the existing edge is split so that qn appears as a new vertex, and an edge is made from qn to α(i). The edge splitting, if required, is assumed to be handled in line 4 by the method that adds edges. Note that the total number of edges may increase by one or two in each iteration.

The method as described here does not fit precisely under the general frame- work from Section 5.4.1; however, with the modifications suggested in Section 5.5.2, it can be adapted to fit. In the RDT formulation, the nearest function serves the purpose of the VSM, but in the RDT, a point may be selected from anywhere in the swath of the graph. The VSM can be generalized to a swath-point

selection method, SSM. This generalization will be used in Section 14.3.4. The LPM tries to connect α(i) to qn along the shortest path possible in C.

Figure 5.19 shows an execution of the algorithm in Figure 5.16 for the case in which = [0, 1]2 and q0 = (1/2, 1/2). It exhibits a kind of fractal behavior.15 Several main branches are first constructed as it rapidly reaches the far corners of the space. Gradually, more and more area is filled in by smaller branches. From the pictures, it is clear that in the limit, the tree densely fills the space. Thus, it can be seen that the tree gradually improves the resolution (or dispersion) as the iterations continue. This behavior turns out to be ideal for sampling-based motion planning.

C

Recall that in sampling-based motion planning, the obstacle region obs is not explicitly represented. Therefore, it must be taken into account in the construction of the tree. Figure 5.20 indicates how to modify the algorithm in Figure 5.16 so that collision checking is taken into account. The modified algorithm appears in Figure

C

• 1. The procedure stopping-configuration yields the nearest configuration possible to the boundary of Cfree, along the direction toward α(i). The nearest

15If α is uniform, random, then a stochastic fractal [586] is obtained. Deterministic fractals can be constructed using sequences that have appropriate symmetries.

RDT(q0)

• • 1. .init(q0);

G

• • 1. for i = 1 to k do
    2. qn ← nearest(S, α(i));
    3. qs ← stopping-configuration(qn,α(i));
    4. if qs qn then
    5. G.add vertex(qs);
    6. G.add edge(qn, qs);

Figure 5.21: The RDT with obstacles.

point qn ∈ S is defined to be same (obstacles are ignored); however, the new edge might not reach to α(i). In this case, an edge is made from qn to qs, the last point possible before hitting the obstacle. How close can the edge come to the obstacle boundary? This depends on the method used to check for collision, as explained in Section 5.3.4. It is sometimes possible that qn is already as close as possible to the boundary of free in the direction of α(i). In this case, no new edge or vertex is added that for that iteration.

C

• • 1. Efficiently Finding Nearest Points

There are several interesting alternatives for implementing the nearest function in line 3 of the algorithm in Figure 5.16. There are generally two families of methods: exact or approximate. First consider the exact case.

Exact solutions Suppose that all edges in are line segments in Rm for some dimension m n. An edge that is generated early in the construction process will be split many times in later iterations. For the purposes of finding the nearest

≥

G

point in S, however, it is best to handle this as a single segment. For example, see the three large branches that extend from the root in Figure 5.19. As the number of points increases, the benefit of agglomerating the segments increases. Let each of these agglomerated segments be referred to as a supersegment. To implement nearest, a primitive is needed that computes the distance between a point and a line segment. This can be performed in constant time with simple vector calculus. Using this primitive, nearest is implemented by iterating over all supersegments and taking the point with minimum distance among all of them. It may be possible to improve performance by building hierarchical data structures that can eliminate large sets of supersegments, but this remains to be seen experimentally.

In some cases, the edges of may not be line segments. For example, the shortest paths between two points in SO(3) are actually circular arcs along S3. One possible solution is to maintain a separate parameterization of for the purposes of computing the nearest function. For example, SO(3) can be represented

G

C

as [0, 1]3/ ∼, by making the appropriate identifications to obtain RP3. Straight-

Figure 5.22: For implementation ease, intermediate vertices can be inserted to avoid checking for closest points along line segments. The trade-off is that the number of vertices is increased dramatically.

line segments can then be used. The problem is that the resulting metric is not consistent with the Haar measure, which means that an accidental bias would result. Another option is to tightly enclose S3 in a 4D cube. Every point on S3

can be mapped outward onto a cube face. Due to antipodal identification, only

four of the eight cube faces need to be used to obtain a bijection between the set of all rotation and the cube surface. Linear interpolation can be used along the cube faces, as long as both points remain on the same face. If the points are on different faces, then two line segments can be used by bending the shortest path around the corner between the two faces. This scheme will result in less distortion

than mapping SO(3) to [0, 1]3/ ∼; however, some distortion will still exist.

Another approach is to avoid distortion altogether and implement primitives that can compute the distance between a point and a curve. In the case of SO(3), a primitive is needed that can find the distance between a circular arc in Rm

and a point in Rm. This might not be too difficult, but if the curves are more

complicated, then an exact implementation of the nearest function may be too

expensive computationally.

Approximate solutions Approximate solutions are much easier to construct, however, a resolution parameter is introduced. Each path segment can be approx- imated by inserting intermediate vertices along long segments, as shown in Figure

• 1. The intermediate vertices should be added each time a new sample, α(i), is inserted into . A parameter ∆q can be defined, and intermediate samples are inserted to ensure that no two consecutive vertices in are ever further than ∆q from each other. Using intermediate vertices, the interiors of the edges in are ignored when finding the nearest point in S. The approximate computation of nearest is performed by finding the closest vertex to α(i) in . This approach is by far the simplest to implement. It also fits precisely under the incremental sampling and searching framework from Section 5.4.1.

G

G

G

G

When using intermediate vertices, the trade-offs are clear. The computation time for each evaluation of nearest is linear in the number of vertices. Increasing the number of vertices improves the quality of the approximation, but it also dramatically increases the running time. One way to recover some of this cost is to insert the vertices into an efficient data structure for nearest-neighbor searching.

7

14

Figure 5.23: A Kd-tree can be used for efficient nearest-neighbor computations.

One of the most practical and widely used data structures is the Kd-tree [264, 365, 758]. A depiction is shown in Figure 5.23 for 14 points in R2. The Kd-tree can be considered as a multi-dimensional generalization of a binary search tree. The Kd-tree is constructed for points, P, in R2 as follows. Initially, sort the points with respect to the x coordinate. Take the median point, p P, and divide

∈

P into two sets, depending on which side of a vertical line through p the other points fall. For each of the two sides, sort the points by the y coordinate and find their medians. Points are divided at this level based on whether they are above or below horizontal lines. At the next level of recursion, vertical lines are used again, followed by horizontal again, and so on. The same idea can be applied in

Rn by cycling through the n coordinates, instead of alternating between x and y, to form the divisions. In [52], the Kd-tree is extended to topological spaces that

arise in motion planning and is shown to yield good performance for RRTs and sampling-based roadmaps. A Kd-tree of k points can be constructed in O(nk lg k) time. Topological identifications must be carefully considered when traversing the tree. To find the nearest point in the tree to some given point, the query algorithm descends to a leaf vertex whose associated region contains the query point, finds all distances from the data points in this leaf to the query point, and picks the closest one. Next, it recursively visits those surrounding leaf vertices that are further from the query point than the closest point found so far [47, 52]. The nearest point can be found in time logarithmic in k.

Unfortunately, these bounds hide a constant that increases exponentially with the dimension, n. In practice, the Kd-tree is useful in motion planning for problems of up to about 20 dimensions. After this, the performance usually degrades too much. As an empirical rule, if there are more than 2n points, then the Kd-tree should be more efficient than naive nearest neighbors. In general, the trade-offs must be carefully considered in a particular application to determine whether exact solutions, approximate solutions with naive nearest-neighbor computations, or approximate solutions with Kd-trees will be more efficient. There is also the issue of implementation complexity, which probably has caused most people to prefer the approximate solution with naive nearest-neighbor computations.

• • 1. Using the Trees for Planning

So far, the discussion has focused on exploring free, but this does not solve a planning query by itself. RRTs and RDTs can be used in many ways in plan- ning algorithms. For example, they could be used to escape local minima in the randomized potential field planner of Section 5.4.3.

C

Single-tree search A reasonably efficient planner can be made by directly using the algorithm in Figure 5.21 to grow a tree from qI and periodically check whether it is possible to connect the RDT to qG. An easy way to achieve this is to start with a dense sequence α and periodically insert qG at regularly spaced intervals. For example, every 100th sample could be qG. Each time this sample is reached, an attempt is made to reach qG from the closest vertex in the RDT. If the sample sequence is random, which generates an RRT, then the following modification works well. In each iteration, toss a biased coin that has probability 99/100 of being heads and 1/100 of being tails. If the result is heads, then set α(i), to be the next element of the pseudorandom sequence; otherwise, set α(i) = qG. This forces the RRT to occasionally attempt to make a connection to the goal, qG. Of course, 1/100 is arbitrary, but it is in a range that works well experimentally. If the bias is too strong, then the RRT becomes too greedy like the randomized potential field. If the bias is not strong enough, then there is no incentive to connect the tree to qG. An alternative is to consider other dense, but not necessarily nonuniform sequences in . For example, in the case of random sampling, the probability density function could contain a gentle bias towards the goal. Choosing such a bias is a difficult heuristic problem; therefore, such a technique should be used with caution (or avoided altogether).

C

Balanced, bidirectional search Much better performance can usually be ob- tained by growing two RDTs, one from qI and the other from qG. This is particu- larly valuable for escaping one of the bug traps, as mentioned in Section 5.4.1. For a grid search, it is straightforward to implement a bidirectional search that ensures that the two trees meet. For the RDT, special considerations must be made to ensure that the two trees will connect while retaining their “rapidly exploring” property. One additional idea is to make sure that the bidirectional search is balanced [560], which ensures that both trees are the same size.

Figure 5.24 gives an outline of the algorithm. The graph G is decomposed

into two trees, denoted by Ta and Tb. Initially, these trees start from qI and qG, respectively. After some iterations, Ta and Tb are swapped; therefore, keep in mind that Ta is not always the tree that contains qI . In each iteration, Ta is grown exactly the same way as in one iteration of the algorithm in Figure 5.16. If a new vertex, qs, is added to Ta, then an attempt is made in lines 10–12 to extend Tb. Rather than using α(i) to extend Tb, the new vertex qs of Ta is used. This causes Tb to try to grow toward Ta. If the two connect, which is tested in line 13, then a solution has been found.

RDT BALANCED BIDIRECTIONAL(qI , qG)

1. Ta.init(qI ); Tb.init(qG);
2. for i = 1 to K do
3. qn ← nearest(Sa, α(i));
4. qs ← stopping-configuration(qn,α(i));
5. if qs /= qn then
6. Ta.add vertex(qs);
7. Ta.add edge(qn, qs);
8. qn′
9. qs′

← nearest(Sb, qs);

← stopping-configuration(qn′ ,qs);

1. if qs′ /= qn′ then
2. Tb.add vertex(qs′ );
3. Tb.add edge(qn′ , qs′ );
4. if qs′

= qs then return SOLUTION;

1. if Tb > Ta then SWAP(Ta, Tb);

| | | |

1. return FAILURE

Figure 5.24: A bidirectional RDT-based planner.

Line 14 represents an important step that balances the search. This is partic- ularly important for a problem such as the bug trap shown in Figure 5.13b or the puzzle shown in Figure 1.2. If one of the trees is having trouble exploring, then it makes sense to focus more energy on it. Therefore, new exploration is always performed for the smaller tree. How is “smaller” defined? A simple criterion is to use the total number of vertices. Another reasonable criterion is to use the total length of all segments in the tree.

An unbalanced bidirectional search can instead be made by forcing the trees to be swapped in every iteration. Once the trees are swapped, then the roles are reversed. For example, after the first swap, Tb is extended in the same way as an integration in Figure 5.16, and if a new vertex qs is added then an attempt is made to connect Ta to qs.

One important concern exists when α is deterministic. It might be possible that even though α is dense, when the samples are divided among the trees, each may not receive a dense set. If each uses its own deterministic sequence, then this problem can be avoided. In the case of making a bidirectional RRT planner, the same (pseudo)random sequence can be used for each tree without encountering such troubles.

More than two trees If a dual-tree approach offers advantages over a single tree, then it is natural to ask whether growing three or more RDTs might be even better. This is particularly helpful for problems like the double bug trap in Figure 5.13c. New trees can be grown from parts of that are difficult to reach. Controlling the number of trees and determining when to attempt connections

C

between them is difficult. Some interesting recent work has been done in this direction [82, 918, 919].

These additional trees could be started at arbitrary (possibly random) configu- rations. As more trees are considered, a complicated decision problem arises. The computation time must be divided between attempting to explore the space and attempting to connect trees to each other. It is also not clear which connections should be attempted. Many research issues remain in the development of this and other RRT-based planners. A limiting case would be to start a new tree from every sample in α(i) and to try to connect nearby trees whenever possible. This approach results in a graph that covers the space in a nice way that is independent of the query. This leads to the main topic of the next section.