Optimal Paths for Some Wheeled Vehicles

For some of the wheeled vehicle models of Section 13.1.2, the shortest path between any pair of configurations was completely characterized. In this section, X = = R2 S1, which corresponds to the C-space for a rigid body in the plane. For each model, the path length in must be carefully defined to retain some physical

C

×

C

significance in the world = R2 in which the vehicle travels. For example, in the case of the simple car, the distance in traveled by the center of the rear axle will be optimized. If the coordinate frame is assigned appropriately, this

W

W

corresponds to optimizing the path length in the R2 subspace of while ignoring orientation. Keep in mind that the solutions given in this section depend heavily on the particular cost functional that is optimized.

C

Sections 15.3.1–15.3.3 cover the shortest paths for the Dubins car, the Reeds- Shepp car, and a differential-drive model, respectively. In each case, the paths can be elegantly described as combinations of a few motion primitives. Due to symmetries, it is sufficient to describe the optimal paths from a fixed initial con-

figuration qI = (0, 0, 0) to any goal configuration qG ∈ C. If the optimal path is desired from a different qI ∈ C, then it can be recovered from rigid-body transfor-

mations applied to qI and qG (the whole path can easily be translated and rotated without effecting its optimality, provided that qG does not move relative to qI ). Alternatively, it may be convenient to fix qG and consider optimal paths from all possible qI .

Once qI (or qG) is fixed, C can be partitioned into cells that correspond to sets

of placements for qG (or qI ). Inside of each cell, the optimal curve is described by a fixed sequence of parameterized motion primitives. For example, one cell for the Dubins car indicates “turn left,” “go straight,” and then “turn right.” The curves are ideally suited for use as an LPM in a sampling-based planning algorithm.

This section mainly focuses on presenting the solutions. Establishing their cor- rectness is quite involved and is based in part on Pontryagin’s minimum principle from Section 15.2.3. Other important components are Filipov’s existence theorem (see [903]) and Boltyanskii’s sufficient condition for optimality (which also justi- fies dynamic programming) [130]. Substantially more details and justifications of the curves presented in Sections 15.3.1 and 15.3.2 appear in [903, 904, 923]. The corresponding details for the curves of Section 15.3.3 appear in [64].

• • 1. Dubins Curves

Recall the Dubins version of the simple car given in Section 13.1.2. The system was specified in (13.15). It is assumed here that the car moves at constant forward speed, us = 1. The other important constraint is the maximum steering angle

φmax, which results in a minimum turning radius ρmin. As the car travels, consider the length of the curve in = R2 traced out by a pencil attached to the center of the rear axle. This is the location of the body-frame origin in Figure 13.1. The

W

task is to minimize the length of this curve as the car travels between any qI and

Figure 15.3: The three motion primitives from which all optimal curves for the Dubins car can be constructed.

qG. Due to ρmin, this can be considered as a bounded-curvature shortest-path problem. If ρmin = 0, then there is no curvature bound, and the shortest path follows a straight line in R2. In terms of a cost functional of the form (8.39), the criterion to optimize is

L(q˜, u˜) =

x˙ (t)2 + y˙(t)2dt, (15.42)

r tF /

0

in which tF is the time at which qG is reached, and a configuration is denoted as

q = (x, y, θ). If qG is not reached, then it is assumed that L(q˜, u˜) = .

∞

Since the speed is constant, the system can be simplified to

x˙ = cos θ

y˙ = sin θ θ˙ = u,

(15.43)

in which u is chosen from the interval U = [− tan φmax, tan φmax]. This implies that (15.42) reduces to optimizing the time tF to reach qG because the integrand reduces to 1. For simplicity, assume that tan φ = 1. The following results also

hold for any φmax (0, π/2).

∈

It was shown in [294] that between any two configurations, the shortest path for the Dubins car can always be expressed as a combination of no more than three motion primitives. Each motion primitive applies a constant action over an interval of time. Furthermore, the only actions that are needed to traverse the shortest paths are u 1, 0, 1 . The primitives and their associated symbols are shown in Figure 15.3. The S primitive drives the car straight ahead. The L and R primitives turn as sharply as possible to the left and right, respectively. Using these symbols, each possible kind of shortest path can be designated as a sequence of three symbols that corresponds to the order in which the primitives are applied. Let such a sequence be called a word . There is no need to have two consecutive primitives of the same kind because they can be merged into one. Under this observation, ten possible words of length three are possible. Dubins showed that only these six words are possibly optimal:

∈ {− }

{LRL, RLR, LSL, LSR, RSL, RSR}. (15.44)

The shortest path between any two configurations can always be characterized by one of these words. These are called the Dubins curves.

qI qG

Rα_S_d_Lγ RαLβ Rγ_

Figure 15.4: The trajectories for two words are shown in W = R2.

To be more precise, the duration of each primitive should also be specified. For L or R, let a subscript denote the total amount of rotation that accumulates during the application of the primitive. For S, let a subscript denote the total distance traveled. Using such subscripts, the Dubins curves can be more precisely characterized as

{Lα Rβ Lγ , Rα Lβ Rγ , Lα Sd Lγ , Lα Sd Rγ , Rα Sd Lγ , Rα Sd Rγ }, (15.45)

in which α, γ [0, 2π), β (π, 2π), and d 0. Figure 15.4 illustrates two cases. Note that β must be greater than π (if it is less, then some other word becomes optimal).

∈ ∈ ≥

It will be convenient to invent a compressed form of the words to group together paths that are qualitatively similar. This will be particularly valuable when Reeds- Shepp curves are introduced in Section 15.3.2 because there are 46 of them, as opposed to 6 Dubins curves. Let C denote a symbol that means “curve,” and represents either R or L. Using C, the six words in (15.44) can be compressed to only two base words:

{CCC, CSC}. (15.46)

In this compressed form, remember that two consecutive Cs must be filled in by distinct turns (RR and LL are not allowed as subsequences). In compressed form, the base words can be specified more precisely as

{Cα Cβ Cγ , Cα Sd Cγ }, (15.47)

in which α, γ [0, 2π), β (π, 2π), and d 0.

∈ ∈ ≥

Powerful information has been provided so far for characterizing the shortest paths; however, for a given qI and qG, two problems remain:

• • • 1. Which of the six words in (15.45) yields the shortest path between qI and

qG?

Figure 15.5: A slice at θ = π of the partition into word-invariant cells for the Dubins car. The circle is centered on the origin.

• • • 1. What are the values of the subscripts, α, β, γ, and d for the particular word?

To use the Dubins curves as an LPM, these questions should be answered effi- ciently. One simple approach is to try all six words and choose the shortest one. The parameters for each word can be determined by tracing out minimum-radius circles from qI and qG, as shown in Figure 14.23. Another way is to use the precise characterization of the regions over which a particular word is optimal. Suppose that qG is fixed at (0, 0, 0). Based on the possible placements of qI , the C-space can be partitioned into cells for which the same word is optimal. The cells and their boundaries are given precisely in [903]. As an example, a slice of the cell decomposition for θ = π is shown in Figure 15.5.

In addition to use as an LPM, the resulting cost of the shortest path may be a useful distance function in many sampling-based planning algorithms. This is sometimes called the Dubins metric (it is not, however, a true metric because it violates the symmetry axiom). This can be considered as the optimal cost-to-go G∗. It could have been computed approximately using the dynamic programming approach in Section 14.5; however, thanks to careful analysis, the exact values are known. One interesting property of the Dubins metric is that it is discontinuous; see Figure 15.6. Compare the cost of traveling π/2 using the R primitive to the cost of traveling to a nearby point that would require a smaller turning radius than that achieved by the R primitive. The required action does not exist in U, and the point will have to be reached by a longer sequence of primitives. The discontinuity in G∗ is enabled by the fact that the Dubins car fails to possess the STLC property from Section 15.1.3. For STLC systems, G∗ is continuous.

Figure 15.6: Level sets of the Dubins metric are shown in the plane. Along two circular arcs, the metric is discontinuous (courtesy of Philippe Sou`eres).

• • 1. Reeds-Shepp Curves

Now consider the shortest paths of the Reeds-Shepp car. The only difference in comparison to the Dubins car is that travel in the reverse direction is now allowed. The same criterion (15.42) is optimized, which is the distance traveled by the center of the rear axle. The shortest path is equivalent to the path that takes minimum time, as for the Dubins car. The simplified system in (15.43) can be enhanced to obtain

x˙ = u1 cos θ y˙ = u1 sin θ θ˙ = u1u2,

(15.48)

in which u1 ∈ {−1, 1} and u2 ∈ [− tan φmax, tan φmax]. The first action variable, u1, selects the gear, which is forward (u1 = 1) or reverse (u1 = −1). Once again, assume for simplicity that u2 ∈ [−1, 1]. The results stated here apply to any

φmax (0, π/2).

∈

It was shown in [814] that there are no more than 48 different words that describe the shortest paths for the Reeds-Shepp car. The base word notation from Section 15.3.1 can be extended to nicely express the shortest paths. A new symbol, “ ”, is used in the words to indicate that the “gear” is shifted from forward to reverse or reverse to forward. Reeds and Shepp showed that the shortest path for their car can always be expressed with one of the following base words:

|

{C|C|C, CC|C, C|CC, CSC, CCβ |Cβ C, C|Cβ Cβ |C,

C|Cπ/_2SC, CSCπ/2|C, C|Cπ/2SCπ/_2|C}.

(15.49)

As many as five primitives could be needed to execute the shortest path. A subscript of π/2 is given in some cases because the curve must be followed for precisely π/2 radians. For some others, β is given as a subscript to indicate that it must match the parameter of another primitive. The form given in (15.49)

Figure 15.7: The interval ranges are shown for each motion primitive parameter for the Reeds-Shepp optimal curves.

Figure 15.8: The six motion primitives from which all optimal curves for the Reeds-Shepp car can be constructed.

is analogous to (15.46) for the Dubins car. The parameter ranges can also be specified, to yield a form analogous to (15.47). The result is shown in Figure 15.7. Example curves for two cases are shown in Figure 15.9.

Now the base words will be made more precise by specifying the particular motion primitive. Imagine constructing a list of words analogous to (15.44) for the Dubins car. There are six primitives as shown in Figure 15.8. The symbols S, L, and R are used again. To indicate the forward or reverse gear, + and superscripts will be used as shown in Figure 15.8.6

−

Figure 15.10 shows 48 different words, which result from uncompressing the base words expressed using C, S, and “ ” in (15.49). Each shortest path is a word with length at most five. There are substantially more words than for the Dubins car. Each base word in (15.49) expands into four or eight words using the motion primitives. To uncompress each base word, the rule that R and L cannot be applied consecutively is maintained. This yields four possibilities for the first

|

6This differs conceptually from the notation used in [903]. There, r− corresponds to L− here. The L here means that the steering wheel is positioned for a left turn, but the car is in reverse. This aids in implementing the rule that R and L cannot be consecutive in a word.

Figure 15.9: An example of the R+L−R+ curve. This uses reverse to generate a

α β γ

curve that is shorter than the one in Figure 15.4b for the Dubins car.

six compressed words. The remaining three involve an intermediate S primitive, which allows eight possible sequences of Rs and Ls for each one. Two of the 48 words were eliminated in [923]. Each of the remaining 46 words can actually occur for a shortest path and are called the Reeds-Shepp curves.

For use as an LPM, the problem appears once again of determining the partic- ular word and parameters for a given qI and qG. This was not difficult for Dubins curves, but now there are 46 possibilities. The naive approach of testing every word and choosing the shortest one may be too costly. The precise cell boundaries in over which each word applies are given in [903]. The cell boundaries are un- fortunately quite complicated, which makes the point location algorithm difficult to implement. A simple way to prune away many words from consideration is to use intervals of validity for θ. For some values of θ, certain compressed words are impossible as shortest paths. A convenient table of words that become active over ranges of θ is given in [903]. Once again, the length of the shortest path can serve as a distance function in sampling-based planning algorithms. The resulting Reeds-Shepp metric is continuous because the Reeds-Shepp car is STLC, which will be established in Section 15.4.

C

• • 1. Balkcom-Mason Curves

In recent years, two more families of optimal curves have been determined [64, 211]. Recall the differential-drive system from Section 13.1.2, which appears in many mobile robot systems. In many ways, it appears that the differential drive is a special case of the simple car. The expression of the system given in (13.17) can be made to appear identical to the Reeds-Shepp car system in (15.48). For example, letting r = 1 and L = 1 makes them equivalent by assigning uω = u1 and

Figure 15.10: The 48 curves of Reeds and Shepp. Sussmann and Tang [923] showed that (L−R+L−) and (R−L+R−), which appear in the first row, can be eliminated. Hence, only 46 words are needed to describe the shortest paths.

uψ = u1u2. Consider the distance traveled by a point attached to the center of the differential-drive axle using (15.42). Minimizing this distance for any qI and qG is trivial, as shown in Figure 13.4 of Section 13.1.2. The center point can be made to

travel in a straight line in W = R2. This would be possible for the Reeds-Shepp

car if ρmin = 0, which implies that φmax = π/2. It therefore appeared for many years that no interesting curves exist for the differential drive.

The problem, however, with measuring the distance traveled by the axle cen- ter is that pure rotations are cost-free. This occurs when the wheels rotate at the same speed but with opposite angular velocities. The center does not move; however, the time duration, energy expenditure, and wheel rotations that occur are neglected. By incorporating one or more of these into the cost functional, a challenging optimization arises. Balkcom and Mason bounded the speed of the differential drive and minimized the total time that it takes to travel from qI to qG. Using (13.16), the action set is defined as U = [ 1, 1] [ 1, 1], in which the maximum rotation rate of each wheel is one (an alternative bound can be used without loss of generality). The criterion to optimize is

− × −

r tF /

L(q˜, u˜) =

x˙ (t)2 + y˙(t)2 + |θ˙(t)|dt, (15.50)

0

0

which takes θ into account, whereas it was neglected in (15.42). This criterion is once again equivalent to minimizing the time to reach qG. The resulting model will be referred to as the Balkcom-Mason drive. An alternative criterion is the total amount of wheel rotation; this leads to an alternative family of optimal curves [211].

Figure 15.11: The four motion primitives from which all optimal curves for the differential-drive robot can be constructed.

It was shown in [64] that only the four motion primitives shown in Figure

15.11 are needed to express time-optimal paths for the differential-drive robot. Each primitive corresponds to holding one action variable fixed at its limit for an interval of time. Using the symbols in Figure 15.11 (which were used in [64]), words can be formed that describe the optimal path. It has been shown that the word length is no more than five. Thus, any shortest paths may be expressed as a piecewise-constant action trajectory in which there are no more than five pieces. Every piece corresponds to one of the primitives in Figure 15.11.

It is convenient in the case of the Balkcom-Mason drive to use the same sym- bols for both base words and for precise specification of primitives. Symmetry transformations will be applied to each base word to yield a family of eight words that precisely specify the sequences of motion primitives. Nine base words describe the shortest paths:

{r.,, ⇓, ⇓r.,, r.,⇓r.,, ⇑-IIπ ⇓, -II⇓r.,, ⇓r.,r.,, -II⇓r.,⇑, ⇑-II⇓r.,⇑}. (15.51)

This is analogous to the compressed forms given in (15.46) and (15.49). The motions are depicted in Figure 15.12.

Figure 15.13 shows 40 distinct Balkcom-Mason curves that result from apply- ing symmetry transformations to the base words of (15.51). There are 72 entries in Figure 15.13, but many are identical. The transformation T1 indicates that

the directions of ⇑ and ⇓ are flipped from the base word. The transformation T2

reverses the order of the motion primitives. The transformation T3 flips the direc- tions of -II and r.,. The transformations commute, and there are seven possible ways to combine them, which contributes to a row of Figure 15.13.

To construct an LPM or distance function, the same issues arise as for the Reeds-Shepp and Dubins cars. Rather than testing all 40 words to find the shortest path, simple tests can be defined over which a particular word becomes active [64]. A slice of the precise cell decomposition and the resulting optimal cost-to-go (which can be called the Balkcom-Mason metric) are shown in Figure 15.14.