First consider the double integrator from Example 13.3. Let and . This models the motion of a free-floating particle in , as described in Section 13.3.2. The phase space is , and . Let . The coming ideas can be easily generalized to allow any acceleration bound by letting ; however, will be chosen to simplify the presentation.
The differential equation can be integrated once to yield
A discrete-time model exists for which the reachability graph is trapped on a lattice. This is obtained by letting and be any positive real number. The vector fields over that correspond to the cases of , , and are shown in Figure 14.12. Switching between these fields at every and integrating yields the reachability graph shown in Figure 14.13.
This leads to a discrete-time transition equation of the form , in which , and represents time . Any action trajectory can be specified as an action sequence; for example a six-stage action sequence may be given by . Start from . At any stage and for any action sequence, the resulting state can be expressed as
The neighborhood structure is not the same as those in Section 5.4.2 because of drift. For , imagine having a stack of horizontal conveyor belts that carry points to the right if they are above the -axis, and to the left if they are below it (see Figure 14.12b). The speed of the conveyor belt is given by . If , the distance traveled along is . This causes horizontal motion to the right in the phase plane if and horizontal motion to the left if . Observe in Figure 14.13 that larger motions result as increases. If , then no horizontal motion can occur. If , then the coordinate changes by . This slowing down or speeding up also affects the position along .
For most realistic problems, there is an upper bound on speed. Let be a positive constant and assume that . Furthermore, assume that is bounded (all values of are contained in an interval of ). Since the reachability graph is a lattice and the states are now confined to a bounded subset of , the number of vertices in the reachability graph is finite. For any fixed , the lattice can be searched using any of the algorithms of Section 2.2. The search starts on a reachability graph for which the initial vertex is . Trajectories that are approximately time-optimal can be obtained by using breadth-first search (Dijkstra's algorithm could alternatively be used, but it is more expensive). Resolution completeness can be obtained by reducing by a constant factor each time the search fails to find a solution. As mentioned in Section 5.4.2, it is not required to construct an entire grid resolution at once. Samples can be gradually added, and the connectivity can be updated efficiently using the union-find algorithm [243,823]. A rigorous approximation algorithm framework will be presented shortly, which indicates how close the solution is to optimal, expressed in terms of input parameters to the algorithm.
Recall the problem of connecting to grid points, which was illustrated in Figure 5.14b. If the goal region contains lattice points, then exact arrival at the goal occurs. If it does not contain lattice points, as in the common case of being a single point, then some additional work is needed to connect a goal state to a nearby lattice point. This actually corresponds to a BVP, but it is easy to solve for the double integrator. The set of states that can be reached from some state within time lie within a cone, as shown in Figure 14.14a. Lattice points that fall into the cone can be easily connected to by applying a constant action in . Likewise, does not even have to coincide with a lattice point. Thus, it is straightforward to connect to a lattice point, obtain a trajectory that arrives at a lattice point near , and then connect it exactly to .
Steven M LaValle 2012-04-20