14.6.2 Plan and Transform

For the decoupled approach in this section, assume that $X = {\cal C}$, which means there are only velocity constraints, as covered in Section 13.1. The system may be specified as ${\dot q}=f(q,u)$ or implicitly as a set of constraints of the form $g_i(q,{\dot q}) = 0$. The ideas in this section can easily be extended to phase spaces. The method given here was developed primarily by Laumond (see [596]) and was also applied to the simple car of Section 13.1.2 in [587]; other applications of the method are covered in [596].

Figure 14.21: A general outline of the plan-and-transform approach.

An outline of the plan-and-transform approach is shown in Figure 14.21. In the first step, a collision-free path $\tau: [0,1] \rightarrow {\cal C}_{free}$ is computed by ignoring differential constraints. The path is then iteratively modified until it satisfies the constraints. In each iteration, a subinterval $[s_1,s_2] \subseteq [0,1]$ is selected by specifying some $s_1,s_2 \in [0,1]$ so that $s_1 < s_2$. These points may be chosen using random sequences or may be chosen deterministically. The approach may use binary subdivision to refine intervals and gradually improve the resolution on $[0,1]$ over the iterations.

For each chosen interval $[s_1,s_2]$, an LPM is used to compute a path segment $\gamma: [0,1] \rightarrow {\cal C}_{free}$ that satisfies the conditions $\gamma(0) = \tau(s_1)$ and $\gamma(1) = \tau(s_2)$. It might be the case that the LPM fails because it cannot connect the two configurations or a collision may occur. In this case, another subinterval is chosen, and the process repeats. Each time the LPM succeeds, $\tau$ is updated to $\tau '$ as

$$\tau'(s) = \left\{ \begin{array}{ll} \tau(s) & \mbox{ if $s < s_1... ... if $s \in [s_1,s_2]$} \\ \tau(s) & \mbox{ if $s > s_2$.} \\ \end{array}\right.$$ (14.30)

The argument to $\gamma$ reparameterizes it to run from $s_1$ to $s_2$, instead of 0 to $1$.

Example 14..5 (Plan-and-Transform for the Dubins Car)   For a concrete example, suppose that the task is to plan a path for the Dubins car. Figure 14.22 shows a path $\tau$ that might be computed by a motion planning algorithm that ignores differential constraints. Two sharp corners cannot be traversed by the car. Suppose that $s_1$ and $s_2$ are chosen at random, and appear at the locations shown in Figure 14.22. The portion of $\tau$ between $\tau(s_1)$ and $\tau(s_2)$ needs to be replaced by a path that can be executed by the Dubins car. Note that matching the orientations at $\tau(s_1)$ and $\tau(s_2)$ is important because they are part of the configuration.

Figure 14.22: An initial path that ignores differential constraints.

A replacement path $\gamma$ is shown in Figure 14.23. This is obtained by implementing the following LPM. For the Dubins car, a path between any configurations can be found by drawing circles at the starting and stopping configurations as shown in the figure. Each circle corresponds to the sharpest possible left turn or right turn. It is straightforward to find a line that is tangent to one circle from each configuration and also matches the direction of flow for the car (the circles are like one-way streets). Using $\gamma$, the path $\tau$ is updated to obtain $\tau '$, which is shown in Figure 14.24, and satisfies the differential constraints for the Dubins car. This problem was very simple, and in practice dozens of iterations may be necessary to replace path segments. Also, if randomization is used, then intervals of the form $[0,s]$ and $[s,1]$ must not be neglected.

Figure 14.23: A path for the Dubins car can always be found by connecting a bitangent to two circles generated by the minimum turning radius. The path is not necessarily optimal; see Section 15.3.1 for optimal paths.

Figure 14.24: Upon replacement, the resulting path $\tau '$ can be followed by the Dubins car.

$\blacksquare$

Example 14.5 seemed easy because of the existence of a simple local planner. Also, there were no obstacles. Imagine that $\tau$ instead traveled through a narrow, zig-zagging corridor. In this case, a solution might not even exist because of sharp corners that cannot be turned by the Dubins car. If there had been an single obstacle that happened to intersect the loop in Figure 14.24, then the replacement would have failed. In general, there is no guarantee that the replacement segment is collision-free. It is important for the LPM to construct path segments that are as close as possible to the original path. For the Dubins car, this is not possible in many cases. For example, moving the Dubins car a small distance backward requires moving along the circles shown in Figure 14.23. Even as the distance between two configurations is reduced, the distance that the car needs to travel does not approach zero. This is true even if the shortest possible paths are used for the Dubins car.

What property should an LPM have to ensure resolution completeness of the plan-and-transform approach? A sufficient condition is given in [596]. Let $\rho$ denote a metric on $X$. An LPM is said to satisfy the topological property if and only if the following statement holds: For any $\epsilon > 0$, there exists some $\delta > 0$ such that for any pair $q,{q}' \in {\cal C}_{free}$ having $\rho(q,q') < \delta$ implies that $\rho(\tau(s),q) < \epsilon$ for all $s \in [0,1]$. If an LPM satisfies the topological property, then any collision-free path through ${\cal C}_{free}$ can be transformed into one that satisfies the differential constraints. Suppose that a path $\tau$ has some clearance of at least $\epsilon$ in ${\cal C}_{free}$. By dividing the domain of $\tau$ into intervals so that the change in $q$ is no more than $\delta$ over each interval, then the LPM will produce collision-free path segments for replacement.

It turns out that for the Reeds-Shepp car (which has reverse) such an LPM can be designed because it is small-time locally controllable, a property that will be covered in Sections 15.1.3 and 15.4. In general, many techniques from Chapter 15 may be useful for analyzing and designing effective LPMs.

An interesting adaptation of the plan-and-transform approach has been developed for problems that involve $k$ implicit constraints of the form $g_i(q,{\dot q}) = 0$. An outline of the multi-level approach, which was introduced in [859], is shown in Figure 14.25 (a similar approach was also introduced in [333]). The idea is to sort the $k$ constraints into a sequence and introduce them one at a time. Initially, a path is planned that ignores the constraints. This path is first transformed to satisfy $g_1(q,{\dot q}) = 0$ and avoid collisions by using the plan-and-transform method of Figure 14.21. If successful, then the resulting path is transformed into one that is collision-free and satisfies both $g_1(q,{\dot q}) = 0$ and $g_2(q,{\dot q}) = 0$. This process repeats by adding one constraint each time, until either the method fails or all $k$ constraints have been taken into account.

Figure 14.25: The multi-level approach considers implicit constraints one at a time.