14.3.4 General Framework Under Differential Constraints

The framework presented here is a direct extension of the sampling and searching framework from Section 5.4.1 and includes the extension of Section 5.5 to allow the selection of any point in the swath of the search graph. This replaces the vertex selection method (VSM) by a swath-point selection method (SSM). The framework also naturally extends the discrete search framework of Section 2.2.4. The components are are follows:

1. Initialization: Let ${\cal G}(V,E)$ represent an undirected search graph, for which the vertex set $V$ contains a vertex for ${x_{I}}$ and possibly other states in ${X_{free}}$, and the edge set $E$ is empty. The graph can be interpreted as a topological graph with a swath $S({\cal G})$.
2. Swath-point Selection Method (SSM): Choose a vertex ${x_{cur}}\in S({\cal G})$ for expansion.
3. Local Planning Method (LPM): Generate a motion primitive ${\tilde{u}}^p : [0,t_F] \rightarrow {X_{free}}$ such that $u(0) = {x_{cur}}$ and $u(t_F) = x_r$ for some $x_r \in {X_{free}}$, which may or may not be a vertex in ${\cal G}$. Using the system simulator, a collision detection algorithm, and by testing the phase constraints, ${\tilde{u}}^p$ must be verified to be violation-free. If this step fails, then go to Step 2.
4. Insert an Edge in the Graph: Insert ${\tilde{u}}^p$ into $E$. Upon integration, ${\tilde{u}}^p$ yields a state trajectory from ${x_{cur}}$ to $x_r$. If $x_r$ is not already in $V$, it is added. If ${x_{cur}}$ lies in the interior of an edge trajectory for some $e \in E$, then $e$ is split by the introduction of a new vertex at ${x_{cur}}$.
5. Check for a Solution: Determine whether ${\cal G}$ encodes a solution path. In some applications, a small gap in the state trajectory may be tolerated.
6. Return to Step 2: Iterate unless a solution has been found or some termination condition is satisfied. In the latter case, the algorithm reports failure.

The general framework may be applied in the same ways as in Section 5.4.1 to obtain unidirectional, bidirectional, and multidirectional searches. The issues from the Piano Mover's Problem extend to motion planning under differential constraints. For example, bug traps cause the same difficulties, and as the number of trees increases, it becomes difficult to coordinate the search.

Figure 14.11: (a) Forward, unidirectional search for which the BVP is avoided. (b) Reaching the goal precisely causes a BVP. (c) Backward, unidirectional search also causes a BVP. (d) For bidirectional search, the BVP arises when connecting the trees.

The main new complication is due to BVPs. See Figure 14.11. Recall from Section 14.1.1 that for most systems it is important to reduce the number of BVPs that must be solved during planning as much as possible. Assume that connecting precisely to a prescribed state is difficult. Figure 14.11a shows the best situation, in which forward, unidirectional search is used to enter a large goal region. In this case, no BVPs need to be solved. As the goal region is reduced, the problem becomes more challenging. Figure 14.11b shows the limiting case in which ${X_{G}}$ is a point $\{{x_{G}}\}$. This requires the planning algorithm to solve at least one BVP.

Figure 14.11c shows the case of backward, unidirectional search. This has the effect of moving the BVP to ${x_{I}}$. Since ${x_{I}}$ is precisely given (there is no ``initial region''), the BVP cannot be avoided as in the forward case. If an algorithm produces a solution ${\tilde{u}}$ for which $x(0)$ is very close to ${x_{I}}$, and if ${X_{G}}$ is large, then it may be possible to salvage the solution. The system simulator can be applied to ${\tilde{u}}$ from ${x_{I}}$ instead of $x(0)$. It is known that ${\tilde{x}}(x(0),{\tilde{u}})$ is violation-free, and ${\tilde{x}}({x_{I}},{\tilde{u}})$ may travel close to ${\tilde{x}}(x(0),{\tilde{u}})$ at all times. This requires $f$ to vary only a small amount with respect to changes in $x$ (this would be implied by a small Lipschitz constant) and also for $\Vert{x_{I}} - x(0)\Vert$ to be small. One problem is that the difference between points on the two trajectories usually increases as time increases. If it is verified by the system simulator that ${\tilde{x}}({x_{I}},{\tilde{u}})$ is violation-free and the final state still lies in ${X_{G}}$, then a solution can be declared.

For bidirectional search, a BVP must be solved somewhere in the middle of a trajectory, as shown in Figure 14.11d. This complicates the problem of determining whether the two trees can be connected. Once again, if the goal region is large, it may be possible remove the gap in the middle of the trajectory by moving the starting state of the trajectory produced by the backward tree. Let ${\tilde{u}}_1$ and ${\tilde{u}}_2$ denote the action trajectories produced by the forward and backward trees, respectively. Suppose that their termination times are $t_1$ and $t_2$, respectively. The action trajectories can be concatenated to yield a function ${\tilde{u}}: [0,t_1+t_2] \rightarrow U$ by shifting the domain of ${\tilde{u}}_2$ from $[0,t_2]$ to $[t_1,t_1+t_2]$. If $t \leq t_1$, then $u(t) = u_1(t)$; otherwise, $u(t) = u_2(t - t_1)$. If there is a gap, the new state trajectory ${\tilde{x}}({x_{I}},{\tilde{u}})$ must be checked using the system simulator to determine whether it is violation-free and terminates in ${X_{G}}$. Multi-directional search becomes even more difficult because more BVPs are created. It is possible in principle to extend the ideas above to concatenate a sequence of action trajectories, which tries to remove all of the gaps.

Consider the relationship between the search graph and reachability graphs. In the case of unidirectional search, the search graph is always a subset of a reachability graph (assuming perfect precision and no numerical integration error). In the forward case, the reachability graph starts at ${x_{I}}$, and in the backward case it starts at ${x_{G}}$. In the case of bidirectional search, there are two reachability graphs. It might be the case that vertices from the two coincide, which is another way that the BVP can be avoided. Such cases are unfortunately rare, unless ${x_{I}}$ and ${x_{G}}$ are intentionally chosen to cause this. For example, the precise location of ${x_{G}}$ may be chosen because it is known to be a vertex of the reachability graph from ${x_{I}}$. For most systems, it is difficult to force this behavior. Thus, in general, BVPs arise because the reachability graphs do not have common vertices. In the case of multi-directional search, numerous reachability graphs are being explored, none of which may have vertices that coincide with vertices of others.