5.5.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 ${\alpha }$ denote an infinite, dense sequence of samples in ${\cal C}$. The $i$th sample is denoted by ${\alpha }(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.

An RDT is a topological graph, ${\cal G}(V,E)$. Let $S \subset {\cal C}_{free}$ indicate the set of all points reached by ${\cal G}$. Since each $e \in E$ is a path, this can be expressed as the swath, $S$, of the graph, which is defined as

$$S = \bigcup_{e \in E} e([0,1]) .$$ (5.40)

In (5.40), $e([0,1]) \subseteq {\cal C}_{free}$ is the image of the path $e$.

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, ${\alpha }$, and iteratively connects from ${\alpha }(i)$ to the nearest point among all those reached by ${\cal G}$.

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 ${\alpha }(i)$ to the nearest point in $S$, which is the vertex $q_n$.

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 ${\cal G}$.

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.

The exploration algorithm is first explained in Figure 5.16 without any obstacles or boundary obstructions. It is assumed that ${\cal C}$ is a metric space. Initially, a vertex is made at $q_0$. For $k$ iterations, a tree is iteratively grown by connecting ${\alpha }(i)$ to its nearest point in the swath, $S$. The connection is usually made along the shortest possible path. In every iteration, ${\alpha }(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, $q_n \in S$, to ${\alpha }(i)$ is a vertex, as shown in Figure 5.17b, then an edge is made from $q_n$ to ${\alpha }(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 $q_n$ appears as a new vertex, and an edge is made from $q_n$ to ${\alpha }(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 framework 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 ${\alpha }(i)$ to $q_n$ along the shortest path possible in ${\cal C}$.

Figure 5.19 shows an execution of the algorithm in Figure 5.16 for the case in which ${\cal C}= [0,1]^2$ and $q_0 = (1/2,1/2)$. It exhibits a kind of fractal behavior.^5.15Several 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.

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.

Figure 5.21: The RDT with obstacles.

Recall that in sampling-based motion planning, the obstacle region ${\cal C}_{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 5.21. The procedure STOPPING-CONFIGURATION yields the nearest configuration possible to the boundary of ${\cal C}_{free}$, along the direction toward $\alpha(i)$. The nearest point $q_n \in S$ is defined to be same (obstacles are ignored); however, the new edge might not reach to ${\alpha }(i)$. In this case, an edge is made from $q_n$ to $q_s$, 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 $q_n$ is already as close as possible to the boundary of ${\cal C}_{free}$ in the direction of ${\alpha }(i)$. In this case, no new edge or vertex is added that for that iteration.