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 2020-08-14