|2||for k = 1 to K|
Step 3 chooses a random configuration, qrand, in . Alternatively, one could replace RAND_CONF with RAND_FREE_CONF, and sample configurations in (by using a collision detection algorithm to reject samples in ).
It is assumed that a metric has been defined over . Step 4 selects the vertex, xnear, in the RRT, G, that is closest to qrand. This can be implemented as shown below.
|4||vnew = v; ;|
In Step 5 of BUILD_RRT, NEW_CONF selects a new configuration, qnew, by moving from qnear an incremental distance, , in the direction of qrand. This assumes that motion in any direction is possible. If differential constraints exist, then inputs are applied to the corresponding control system, and new configurations are obtained by numerical integration. Finally, a new vertex, qnew, and is added, and a new edge is added from qnear to qnew.
To gain a better understanding of RRTs, consider the special case in which is a square region in the plane. Let represent the Euclidean metric. The frames below show the construction of an RRT for the case of , , and qinit = (50,50):
The RRT quickly expands in a few directions to quickly explore the four corners of the square. Although the construction method is simple, it is no easy task to find a method that yields such desirable behavior. Consider, for example, a naive random tree that is constructed incrementally by selecting a vertex at random, an input at random, and then applying the input to generate a new vertex. Although one might intuitively expect the tree to ``randomly'' explore the space, there is actually a very strong bias toward places already explored (simulation experiments yield an extremely high density of vertices near qinit, with little other exploration). A random walk also suffers from a bias toward places already visited. An RRT works in the opposite manner by being biased toward places not yet visited. This can be seen by considering the Voronoi diagram of the RRT vertices. Larger Voronoi regions occur on the ``frontier'' of the tree. Since vertex selection is based on nearest neighbors, this implies that vertices with large Voronoi regions are more likely to be selected for expansion. On average, an RRT is constructed by iteratively breaking large Voronoi regions into smaller ones.