Searching

The algorithm fits directly into the framework of Section 14.3.4. A search graph is constructed incrementally from ${x_{I}}$ 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 $\Delta t$ and a finite set $U_d$ of actions.

Figure 14.16: (a) The first four stages of a dense reachability graph for the Dubins car. (b) One possible search graph, obtained by allowing at most one vertex per cell. Many branches are pruned away. In this simple example, there are no cell divisions along the $\theta$-axis.

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 $D$ is called visited if the search graph that has been constructed so far contains a vertex in $D$; otherwise, $D$ is called unvisited. Initially, only the cell that contains ${x_{I}}$ 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.

Figure 14.17: Searching by using a cell decomposition of $X$.

The basic algorithm outline is shown in Figure 14.17. Let $Q$ represent a priority queue in which the elements are vertices of the search graph. If optimization of a cost functional is required, then $Q$ may be sorted by the cost accumulated along the path constructed so far from ${x_{I}}$ to $x$. This cost can be assigned in many different ways. It could simply represent the time (number of $\Delta t$ 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 $Q$ 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 $u \in U_d$ over time $\Delta t$ and reporting only those states reached without violating the constraints (including collision avoidance).

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