14.4.1.1 A double-integrator lattice

First consider the double integrator from Example 13.3. Let ${\cal C}= {\cal C}_{free}= {\mathbb{R}}$ and ${\ddot q} = u$. This models the motion of a free-floating particle in ${\mathbb{R}}$, as described in Section 13.3.2. The phase space is $X = {\mathbb{R}}^2$, and $x = (q,{\dot q})$. Let $U = [-1,1]$. The coming ideas can be easily generalized to allow any acceleration bound $a_{max} > 0$ by letting $U = [-a_{max},a_{max}]$; however, $a_{max} = 1$ will be chosen to simplify the presentation.

The differential equation ${\ddot q} = u$ can be integrated once to yield

$${\dot q}(t) = {\dot q}(0) + u t ,$$ (14.21)

in which ${\dot q}(0)$ is an initial speed. Upon integration of (14.21), the position is obtained as

$$q(t) = q(0) + {\dot q}(0) \, t + \begin{matrix}\frac{1}{2}\end{matrix} u t^2 ,$$ (14.22)

which uses two initial conditions, $q(0)$ and ${\dot q}(0)$.

Figure 14.12: The reachability graph will be obtained by switching between these vector fields at every $\Delta t$. The middle one produces horizontal phase trajectories, and the others produce parabolic curves.

Figure: The reachability graph from the origin is shown after three stages (the true state trajectories are actually parabolic arcs when acceleration or deceleration occurs). Note that a lattice is obtained, but the distance traveled in one stage increases as $\vert{\dot q}\vert$ increases.

A discrete-time model exists for which the reachability graph is trapped on a lattice. This is obtained by letting $U_d = \{-1,0,1\}$ and $\Delta t$ be any positive real number. The vector fields over $X$ that correspond to the cases of $u = -1$, $u = 0$, and $u=1$ are shown in Figure 14.12. Switching between these fields at every $\Delta t$ and integrating yields the reachability graph shown in Figure 14.13.

This leads to a discrete-time transition equation of the form $x_{k+1} = f_d(x_k,u_k)$, in which $u_k \in U_d$, and $k$ represents time $t = (k-1) \Delta t$. Any action trajectory can be specified as an action sequence; for example a six-stage action sequence may be given by $(-1,1,0,0,-1,1)$. Start from $x_1 = x(0) = (q_1,{\dot q}_1)$. At any stage $k$ and for any action sequence, the resulting state $x_k = (q_k,{\dot q}_k)$ can be expressed as

$$\begin{split}q_k & = q_1 + i \begin{matrix}\frac{1}{2}\end{ma... ...ta t)^2 \\ {\dot q}_k & = {\dot q}_1 + j \Delta t , \end{split}$$ (14.23)

in which $i,j$ are integers that can be computed from the action sequence. Thus, any action sequence leads to a state that can be expressed using integer coordinates $(i,j)$ in the plane. Starting at $x_1 = (0,0)$, this forms the lattice of points shown in Figure 14.13. The lattice is slanted (with slope $1$) because changing speed requires some motion. If infinite acceleration were allowed, then ${\dot q}$ could be changed instantaneously, which corresponds to moving vertically in $X$. As seen in (14.21), ${\dot q}$ changes linearly over time. If $q \not = 0$, then the configuration changes quadratically. If $u = 0$, then it changes linearly, except when ${\dot q}= 0$; in this case, no motion occurs.

The neighborhood structure is not the same as those in Section 5.4.2 because of drift. For $u = 0$, imagine having a stack of horizontal conveyor belts that carry points to the right if they are above the $q$-axis, and to the left if they are below it (see Figure 14.12b). The speed of the conveyor belt is given by ${\dot q}$. If $u = 0$, the distance traveled along $q$ is ${\dot q} \Delta t$. This causes horizontal motion to the right in the phase plane if ${\dot q}> 0$ and horizontal motion to the left if ${\dot q}< 0$. Observe in Figure 14.13 that larger motions result as $\vert{\dot q}\vert$ increases. If ${\dot q}= 0$, then no horizontal motion can occur. If $q \not = 0$, then the ${\dot q}$ coordinate changes by $\pm \frac{1}{2} u (\Delta t)^2$. This slowing down or speeding up also affects the position along $q$.

For most realistic problems, there is an upper bound on speed. Let $v_{max} > 0$ be a positive constant and assume that $\vert{\dot q}\vert \leq v_{max}$. Furthermore, assume that ${\cal C}$ is bounded (all values of $q \in {\cal C}$ are contained in an interval of ${\mathbb{R}}$). Since the reachability graph is a lattice and the states are now confined to a bounded subset of ${\mathbb{R}}^2$, the number of vertices in the reachability graph is finite. For any fixed $\Delta t$, 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 ${x_{I}}$. 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 $\Delta t$ 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.

Figure 14.14: The initial and goal states can be connected to lattice points that call within cones in $X$ that represent time-limited reachable sets.

Recall the problem of connecting to grid points, which was illustrated in Figure 5.14b. If the goal region ${X_{G}}$ contains lattice points, then exact arrival at the goal occurs. If it does not contain lattice points, as in the common case of ${X_{G}}$ 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 ${x_{G}}$ within time $\Delta t$ lie within a cone, as shown in Figure 14.14a. Lattice points that fall into the cone can be easily connected to ${x_{G}}$ by applying a constant action in $U$. Likewise, ${x_{I}}$ does not even have to coincide with a lattice point. Thus, it is straightforward to connect ${x_{I}}$ to a lattice point, obtain a trajectory that arrives at a lattice point near ${x_{G}}$, and then connect it exactly to ${x_{G}}$.