The algorithm fits directly into the framework of Section 14.3.4. A search graph is constructed incrementally from by applying any systematic search algorithm. It is assumed that the system has been discretized in some way. Most often, the discrete-time model of Section 14.2.2 is used, which results in a fixed and a finite set of actions.

In the basic search algorithms of Section 2.2.1, it is
important to keep track of which vertices have been explored. Instead
of applying this idea to vertices, it is applied here to cells. A
cell is called *visited* if the search graph that has been
constructed so far contains a vertex in ; otherwise, is called
*unvisited*. Initially, only the cell that contains is
marked as *visited*. All others are initialized to *unvisited*. These labels are used to prune the reachability graph
during the search, as shown in Figure 14.16.

The basic algorithm outline is shown in Figure 14.17.
Let represent a priority queue in which the elements are vertices
of the search graph. If optimization of a cost functional is
required, then may be sorted by the cost accumulated along the
path constructed so far from to . This cost can be
assigned in many different ways. It could simply represent the time
(number of steps), or it could count the number of times the
action has changed. As the algorithm explores, new candidate vertices
are encountered. They are only saved in the search graph and placed
into if they lie in a cell marked *unvisited* and are
violation-free. Upon encountering such a cell, it becomes marked as
*visited*. The REACHED function generates a set of
violation-free trajectory segments. Under the discrete-time model,
this means applying each over time and
reporting only those states reached without violating the constraints
(including collision avoidance).

As usual, the BVP issue may arise if is small relative to the cell size. If is large enough to include entire cells, then this issue is avoided. If is a single point, then it may only be possible to approximately reach . Therefore, the algorithm must accept reaching to within a specified tolerance. This can be modeled by defining to be larger; therefore, tolerance is not explicitly mentioned.

Steven M LaValle 2012-04-20