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

$${\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:

$$\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

$$\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

$${\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

$$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

$$\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$