14.6.3.3 The path-constrained phase space

Recall the approach in Section 14.4.1 that enabled systems of the form $ {\ddot q}= h(q,{\dot q},u)$ to be expressed as $ {\ddot q}= u'$ for some suitable $ U'(q,{\dot q}) \subseteq U$ (this was illustrated in Figure 14.15). This enabled many systems to be imagined as multiple, independent double integrators with phase-dependent constraints on the action space. The same idea can be applied here to obtain a single integrator.

Let $ {S}$ denote a 2D path-constrained phase space, in which each element is of the form $ (s,{\dot s})$ and represents the position and velocity along $ \tau$. This parameterizes a 2D subset of the original phase space $ X$. Each original state vector is $ x =
(q,{\dot q}) = (\tau(s),d\tau/ds\;{\dot s})$. Which accelerations are possible at points in $ {S}$? At each $ (s,{\dot s})$, a subset of $ U$ can be determined that satisfies the equations of the form (14.39). Each valid action yields an acceleration $ {\ddot s}$. Let $ U'(s,{\dot s}) \subseteq {\mathbb{R}}$ denote the set of all values of $ {\ddot s}$ that can be obtained from an action $ u \in U$ that satisfies (14.39) for each $ i$ (except the ones for which $ d\tau_i/ds = 0$). Now the system can be expressed as $ {\ddot s}=
u'$, in which $ u' \in U'(s,{\dot s})$. After all of this work, we have arrived at the double integrator. The main complication is that $ U'(s,{\dot s})$ can be challenging to determine for some systems. It could consist of a single interval, disjoint intervals, or may even be empty. Assuming that $ U'(s,{\dot s})$ has been characterized, it is straightforward to solve the remaining planning problem using techniques already presented in this chapter. One double integrator is not very challenging; hence, efficient sampling-based algorithms exist.

An obstacle region $ {S}_{obs} \subset {S}$ will now be considered. This includes any states that belong to $ X_{free}$. Given $ s$ and $ {\dot s}$, the state $ x$ can be computed to determine whether any constraints on $ X$ are violated. Usually, $ \tau$ is constructed to avoid obstacle collision; however, some phase constraints may also exist. The obstacle region $ {S}_{obs}$ also includes any points $ (s,{\dot s})$ for which $ U'(s,{\dot s})$ is empty. Let $ {S}_{free}$ denote $ {S}\setminus {S}_{obs}$.

Before considering computation methods, we give some examples.

Example 14..6 (Path-Constrained Double Integrators)   Consider the case of two double integrators. This could correspond physically to a particle moving in $ {\mathbb{R}}^2$. Hence, $ {\cal C}= {\cal W}= {\mathbb{R}}^2$. Let $ U = [-1,1]^2$ and $ {\ddot q}
= u$ for $ u \in U$. The path $ \tau$ will be chosen to force the particle to move along a line. For linear paths, $ d\tau/ds$ is constant and $ d^2\tau/ds^2 = 0$. Using these observations and the fact that $ h'(s,{\dot s},u) = u$, (14.39) simplifies to

$\displaystyle {\ddot s}= \frac{u_i}{d\tau_i/ds} ,$ (14.40)

for $ i = 1, 2$.

Suppose that $ \tau(s) = (s,s)$, which means that the particle must move along a diagonal line through the origin of $ {\cal C}$. This further simplifies (14.40) to $ {\ddot s}= u_1$ and $ {\ddot s}=
u_2$. Hence any $ u_1 \in [-1,1]$ may be chosen, but $ u_2$ must then be chosen as $ u_2 = u_1$. The constrained system can be written as one double integrator $ {\ddot s}=
u'$, in which $ u' \in [-1,1]$. Both $ u_1$ and $ u_2$ are derived from $ u'$ as $ u_1 = u_2 = u'$. Note that $ U'$ does not vary over $ {S}$; this occurs because a linear path is degenerate.

Now consider constraining the motion to a general line:

$\displaystyle \tau(s) = (a_1 s + b_1, \; a_2 s + b_2) ,$ (14.41)

in which $ a_1$ and $ a_2$ are nonzero. In this case, (14.40) yields $ {\ddot s}= u_1/a_1$ and $ {\ddot s}=
u_2/a_2$. Since each $ u_i \in [-1,1]$, each equation indicates that $ {\ddot s}\in [-1/a_i,1/a_i]$. The acceleration must lie in the intersection of these two intervals. If $ \vert a_1\vert \geq \vert a_2\vert$, then $ {\ddot s}\in [-1/a_1,1/a_1]$. We can designate $ u' = u_1$ and let $ u_2 = u' a_2/a_1$. If $ \vert a_1\vert > \vert a_2\vert$, then $ {\ddot s}\in
[-1/a_2,1/a_2]$, $ u' = u_2$, and $ u_1 = u' a_1/a_2$.

Suppose that $ a_1 = 0$ and $ a_2 \not = 0$. The path is

$\displaystyle \tau(s) = (q_1, \; a_2 s + b_2) ,$ (14.42)

in which $ q_1$ is fixed and the particle is constrained to move along a vertical line in $ {\cal C}= {\mathbb{R}}^2$. In this case, only one constraint, $ {\ddot s}=
u_2$, is obtained from (14.40). However, $ u_1$ is independently constrained to $ u_1 = 0$ because horizontal motions are prohibited.

If $ n$ independent, double integrators are constrained to a line, a similar result is obtained. There are $ n$ equations of the form (14.40). The $ i \in
\{1,\ldots,n\}$ for which $ \vert a_i\vert$ is largest determines the acceleration range as $ {\ddot s}\in [-1/a_i,1/a_i]$. The action $ u'$ is defined as $ u' = u_i$, and the $ u_j$ for $ j \not = i$ are obtained from the remaining $ n-1$ equations.

Now assume $ \tau$ is nonlinear, in which case (14.39) becomes

$\displaystyle {\ddot s}= \frac{u_i}{d\tau_i/ds} - \frac{d^2\tau_i}{ds^2} \; {\dot s}^2 ,$ (14.43)

for each $ i$ for which $ d\tau_i/ds \not =
0$. Now the set $ U'(s,{\dot s})$ varies over $ {S}$. As the speed $ {\dot s}$ increases, it becomes less likely that $ U'(s,{\dot s})$ is nonempty. In other words, it is less likely that a solution exists to all equations of the form (14.43). In a physical system, that means that staying on the path requires turning too sharply. At a high speed, this may require an acceleration $ {\ddot q}$ that lies outside of $ [-1,1]^n$. $ \blacksquare$

The same ideas can be applied to systems that are much more complicated. This should not be surprising because in Section 14.4.1 systems of the form $ {\ddot q}= h(q,{\dot q})$ were interpreted as multiple, independent double integrators of the form $ {\ddot q}= u'$, in which $ u' \in U'(q,{\dot q})$ provided the possible accelerations. Under this interpretation, and in light of Example 14.6, constraining the motions of a general system to a path $ \tau$ just further restricts $ U'(q,{\dot q})$. The resulting set of allowable accelerations may be at most one-dimensional.

The following example indicates the specialization of (14.39) for a robot arm.

Example 14..7 (Path-Constrained Manipulators)   Suppose that the system is described as (13.142) from Section 13.4.2. This is a common form that has been used for controlling robot arms for decades. Constraints of the form (14.39) can be derived by expressing $ q$, $ {\dot q}$, and $ {\ddot q}$ in terms of $ s$, $ {\dot s}$, and $ {\ddot s}$. This requires using (14.32), (14.33), and (14.34). Direct substitution into (13.142) yields

$\displaystyle M(\tau(s)) \left(\frac{d^2\tau}{ds^2} {\dot s}^2 + \frac{d\tau}{d...
...au(s),\frac{d\tau}{ds}{\dot s}\Big) \frac{d\tau}{ds} {\dot s}+ g(\tau(s)) = u .$ (14.44)

This can be simplified to $ n$ equations of the form

$\displaystyle \alpha_i(s) {\ddot s}+ \beta_i(s) {\dot s}^2 + \gamma_i(s) {\dot s}= u_i .$ (14.45)

Solving each one for $ {\ddot s}$ yields a special case of (14.39). As in Example 14.6, each equation determines a bounding interval for $ {\ddot s}$. The intersection of the intervals for all $ n$ equations yields the allowed interval for $ {\ddot s}$. The action $ u'$ once again indicates the acceleration in the interval, and the original action variables $ u_i$ can be obtained from (14.45). If $ d\tau_i/ds = 0$, then $ \alpha_i(s) = 0$, which corresponds to the case in which the constraint does not apply. Instead, the constraint is that the vector $ u$ must be chosen so that $ {\dot q}_i = 0$. $ \blacksquare$

Steven M LaValle 2020-08-14