7.1 Time-Varying Problems

This section brings time into the motion planning formulation. Although the robot has been allowed to move, it has been assumed so far that the obstacle region and the goal configuration, qG free, are stationary for all time. It is now assumed that these entities may vary over time, although their motions are predictable. If the motions are not predictable, then some form of feedback is needed to respond to observations that are made during execution. Such problems are much more difficult and will be handled in Chapters 8 and throughout Part IV.

O ∈ C

311

• • 1. Problem Formulation

The formulation is designed to allow the tools and concepts learned so far to be applied directly. Let T ⊂ R denote the time interval, which may be bounded or

unbounded. If T is bounded, then T = [0, tf ], in which 0 is the initial time and tf is the final time. If T is unbounded, then T = [0, ). An initial time other than 0 could alternatively be defined without difficulty, but this will not be done here. Let the state space X be defined as X = T , in which is the usual C- space of the robot, as defined in Chapter 4. A state x is represented as x = (q, t), to indicate the configuration q and time t components of the state vector. The planning will occur directly in X, and in many ways it can be treated as any C-space seen to far, but there is one critical difference: Time marches forward. Imagine a path that travels through X. If it first reaches a state (q1, 5), and then later some state (q2, 3), some traveling backward through time is required! There is no mathematical problem with allowing such time travel, but it is not realistic for most applications. Therefore, paths in X are forced to follow a constraint that

∞

C × C

they must move forward in time.

Now consider making the following time-varying versions of the items used in Formulation 4.1 for motion planning.

Formulation 7.1 (The Time-Varying Motion Planning Problem)

• • • 1. A world in which either = R2 or = R3. This is the same as in Formulation 4.1.

W W W

• • • 1. A time interval T ⊂ R that is either bounded to yield T = [0, tf ] for some

final time, tf > 0, or unbounded to yield T = [0, ∞).

• • • 1. A semi-algebraic, time-varying obstacle region (t) for every t T . It is assumed that the obstacle region is a finite collection of rigid bodies that undergoes continuous, time-dependent rigid-body transformations.

O ⊂ W ∈

• • • 1. The robot (or 1, . . ., m for a linkage) and configuration space defini- tions are the same as in Formulation 4.1.

A A A C

• • • 1. The state space X is the Cartesian product X = T and a state x X is denoted as x = (q, t) to denote the configuration q and time t components. See Figure 7.1. The obstacle region, Xobs, in the state space is defined as

C × ∈

Xobs = {(q, t) ∈ X | A(q) ∩ O(t) /= ∅}, (7.1)

and Xfree = X \ Xobs. For a given t ∈ T , slices of Xobs and Xfree are obtained. These are denoted as Cobs(t) and Cfree(t), respectively, in which (assuming A is one body)

Cobs(t) = {q ∈ C | A(q) ∩ O(t) /= ∅} (7.2)

and Cfree = C \ Cobs.

Cfree(t_1) C_free(t_2) C_free(_t_3)

Figure 7.1: A time-varying example with piecewise-linear obstacle motion.

• • • 1. A state xI ∈ Xfree is designated as the initial state, with the constraint that xI = (qI , 0) for some qI free(0). In other words, at the initial time the robot cannot be in collision.

∈ C

• • • 1. A subset XG ⊂ Xfree is designated as the goal region. A typical definition is to pick some qG and let XG = (qG, t) Xfree t T , which means that the goal is stationary for all time.

∈ C { ∈ | ∈ }

• • • 1. A complete algorithm must compute a continuous, time-monotonic path, τ [0, 1] Xfree, such that τ (0) = xI and τ (1) XG, or correctly report that such a path does not exist. To be time-monotonic implies that for any

→ ∈

s1, s2 ∈ [0, 1] such that s1 < s2, we have t1 < t2, in which (q1, t1) = τ (s1)

and (q2, t2) = τ (s2).

Example 7.1 (Piecewise-Linear Obstacle Motion) Figure 7.1 shows an ex- ample of a convex, polygonal robot A that translates in W = R2. There is a single, convex, polygonal obstacle O. The two of these together yield a convex, polygonal

C-space obstacle, obs(t), which is shown for times t1, t2, and t3. The obstacle moves with a piecewise-linear motion model, which means that transformations applied to are a piecewise-linear function of time. For example, let (x, y) be a fixed point on the obstacle. To be a linear motion model, this point must transform as (x + c1t, y + c2t) for some constants c1, c2 R. To be piecewise-linear, it may change to a different linear motion at a finite number of critical times. Between these critical times, the motion must remain linear. There are two critical times in

C

O

∈

the example. If Cobs(t) is polygonal, and a piecewise-linear motion model is used,

then Xobs is polyhedral, as depicted in Figure 7.1. A stationary goal is also shown, which appears as a line that is parallel to the T -axis. ．

In the general formulation, there are no additional constraints on the path, τ , which means that the robot motion model allows infinite acceleration and un- bounded speed. The robot velocity may change instantaneously, but the path through must always be continuous. These issues did not arise in Chapter 4 because there was no need to mention time. Now it becomes necessary.1

C

• • 1. Direct Solutions

Sampling-based methods Many sampling-based methods can be adapted from to X without much difficulty. The time dependency of obstacle models must be taken into account when verifying that path segments are collision-free; the tech- niques from Section 5.3.4 can be extended to handle this. One important concern is the metric for X. For some algorithms, it may be important to permit the use of a pseudometric because symmetry is broken by time (going backward in time

C

is not as easy as going forward).

For example, suppose that the C-space C is a metric space, (C, ρ). The metric

can be extended across time to obtain a pseudometric, ρX , as follows. For a pair of states, x = (q, t) and x′ = (q′, t′), let

 0 if q = q′

ρX (x, x′) =

∞ if q /= q′ and t′ ≤ t

 ρ(q, q′) otherwise.

(7.3)

Using ρX , several sampling-based methods naturally work. For example, RDTs from Section 5.5 can be adapted to X. Using ρX for a single-tree approach ensures that all path segments travel forward in time. Using bidirectional approaches is more difficult for time-varying problems because XG is usually not a single point. It is not clear which (q, t) should be the starting vertex for the tree from

1The infinite acceleration and unbounded speed assumptions may annoy those with mechanics and control backgrounds. In this case, assume that the present models approximate the case in which every body moves slowly, and the dynamics can be consequently neglected. If this is still not satisfying, then jump ahead to Part IV, where general nonlinear systems are considered. It is still helpful to consider the implications derived from the concepts in this chapter because the issues remain for more complicated problems that involve dynamics.

the goal; one possibility is to initialize the goal tree to an entire time-invariant segment. The sampling-based roadmap methods of Section 5.6 are perhaps the most straightforward to adapt. The notion of a directed roadmap is needed, in which every edge must be directed to yield a time-monotonic path. For each pair

of states, (q, t) and (q′, t′), such that t t′, exactly one valid direction exists for

making a potential edge. If t = t′, then no edge can be attempted because it would require the robot to instantaneously “teleport” from one part of to another. Since forward time progress is already taken into account by the directed edges, a symmetric metric may be preferable instead of (7.3) for the sampling-based roadmap approach.

W

Combinatorial methods In some cases, combinatorial methods can be used to solve time-varying problems. If the motion model is algebraic (i.e., expressed with polynomials), then Xobs is semi-algebraic. This enables the application of general planners from Section 6.4, which are based on computational real alge- braic geometry. The key issue once again is that the resulting roadmap must be directed with all edges being time-monotonic. For Canny’s roadmap algorithm, this requirement seems difficult to ensure. Cylindrical algebraic decomposition is

straightforward to adapt, provided that time is chosen as the last variable to be considered in the sequence of projections. This yields polynomials in Q[t], and R is nicely partitioned into time intervals and time instances. Connections can then

be made for a cell of one cylinder to an adjacent cell of a cylinder that occurs later in time.

If Xobs is polyhedral as depicted in Figure 7.1, then vertical decomposition can be used. It is best to first sweep the plane along the time axis, stopping at the critical times when the linear motion changes. This yields nice sections, which are further decomposed recursively, as explained in Section 6.3.3, and also facilitates the connection of adjacent cells to obtain time-monotonic path segments. It is not too difficult to imagine the approach working for a four-dimensional state space, X, for which obs(t) is polyhedral as in Section 6.3.3, and time adds the fourth dimension. Again, performing the first sweep with respect to the time axis is preferable.

C

If X is not decomposed into cylindrical slices over each noncritical time interval, then cell decompositions may still be used, but be careful to correctly connect the cells. Figure 7.2 illustrates the problem, for which transitivity among adjacent cells is broken. This complicates sample point selection for the cells.

Bounded speed There has been no consideration so far of the speed at which the robot must move to avoid obstacles. It is obviously impractical in many applications if the solution requires the robot to move arbitrarily fast. One step toward making a realistic model is to enforce a bound on the speed of the robot. (More steps towards realism are taken in Chapter 13.) For simplicity, suppose

2 2

C = R , which corresponds to a translating rigid robot, A, that moves in W = R .

A configuration, q ∈ C, is represented as q = (y, z) (since x already refers to the

q

t

Figure 7.2: Transitivity is broken if the cells are not formed in cylinders over T . A time-monotonic path exists from C1 to C2, and from C2 to C3, but this does not imply that one exists from C1 to C3.

whole state vector). The robot velocity is expressed as v = (y˙, z˙) ∈ R2, in which y˙ = dy/dt and z˙ = dz/dt. The robot speed is v = y˙2 + z˙2. A speed bound, b, is a positive constant, b (0, ), for which v b.

/

∈ ∞ I I ≤

I I

In terms of Figure 7.1, this means that the slope of a solution path τ is bounded. Suppose that the domain of τ is T = [0, tf ] instead of [0, 1]. This yields τ : T → X

and τ (t) = (y, z, t). Using this representation, dτ1/dt = y˙ and dτ2/dt = z˙, in which τi denotes the ith component of τ (because it is a vector-valued function). Thus, it can seen that b constrains the slope of τ (t) in X. To visualize this, imagine that only motion in the y direction occurs, and suppose b = 1. If τ holds the robot fixed, then the speed is zero, which satisfies any bound. If the robot moves at speed 1, then dτ1/dt = 1 and dτ2/dt = 0, which satisfies the speed bound. In Figure 7.1 this generates a path that has slope 1 in the yt plane and is horizontal in the zt√plane. If dτ1/dt = dτ2/dt = 1, then the bound is exceeded because the

speed is 2. In general, the velocity vector at any state (y, z, t) points into a cone

that starts at (y, z) and is aligned in the positive t direction; this is depicted in Figure 7.3. At time t + ∆t, the state must stay within the cone, which means that

y(t + ∆t) − y(t) 2 + z(t + ∆t) − z(t) 2 ≤ b2(∆t)2. (7.4)

（ ） （ ）

This constraint makes it considerably more difficult to adapt the algorithms of Chapters 5 and 6. Even for piecewise-linear motions of the obstacles, the problem has been established to be PSPACE-hard [818, 819, 928]. A complete algorithm is presented in [819] that is similar to the shortest-path roadmap algorithm of Section 6.2.4. The sampling-based roadmap of Section 5.6 is perhaps one of the easiest of the sampling-based algorithms to adapt for this problem. The neighbors of point q, which are determined for attempted connections, must lie within the cone that represents the speed bound. If this constraint is enforced, a resolution complete or probabilistically complete planning algorithm results.

y

t

Figure 7.3: A projection of the cone constraint for the bounded-speed problem.

• • 1. The Velocity-Tuning Method

An alternative to defining the problem in T is to decouple it into a path planning part and a motion timing part [506]. Algorithms based on this method are not complete, but velocity tuning is an important idea that can be applied elsewhere. Suppose there are both stationary obstacles and moving obstacles. For the stationary obstacles, suppose that some path τ : [0, 1] free has been computed using any of the techniques described in Chapters 5 and 6.

C ×

→ C

The timing part is then handled in a second phase. Design a timing function (or time scaling), σ : T → [0, 1], that indicates for time, t, the location of the robot along the path, τ . This is achieved by defining the composition φ = τ ◦ σ, which

maps from T to free via [0, 1]. Thus, φ : T free. The configuration at time

C → C

t T is expressed as φ(t) = τ (σ(t)).

∈

A 2D state space can be defined as shown in Figure 7.4. The purpose is to convert the design of σ (and consequently φ) into a familiar planning problem. The robot must move along its path from τ (0) to τ (1) while an obstacle, (t), moves along its path over the time interval T . Let S = [0, 1] denote the domain of τ . A state space, X = T S, is shown in Figure 7.4b, in which each point (t, s) indicates the time t T and the position along the path, s [0, 1]. The obstacle region in X is defined as

O

∈ ∈

×

Xobs = {(t, s) ∈ X | A(τ (s)) ∩ O(t) /= ∅}. (7.5) Once again, Xfree is defined as Xfree = X \ Xobs. The task is to find a continuous path g : [0, 1] Xfree. If g is time-monotonic, then a position s S is assigned for every time, t T . These assignments can be nicely organized into the timing function, σ : T S, from which φ is obtained by φ = τ σ to determine where the robot will be at each time. Being time-monotonic in this context means that the path must always progress from left to right in Figure 7.4b. It can, however, be nonmonotonic in the positive s direction. This corresponds to moving back and forth along τ , causing some configurations to be revisited.

→ ◦

∈

→ ∈

Any of the methods described in Formulation 7.1 can be applied here. The dimension of X in this case is always 2. Note that Xobs is polygonal if and are both polygonal regions and their paths are piecewise-linear. In this case, the

A O

1

s

0 t

1. (b)

Figure 7.4: An illustration of path tuning. (a) If the robot follows its computed path, it may collide with the moving obstacle. (b) The resulting state space.

vertical decomposition method of Section 6.2.2 can be applied by sweeping along the time axis to yield a complete algorithm (it is complete after having committed to τ , but it is not complete for Formulation 7.1). The result is shown in Figure

7.5. The cells are connected only if it is possible to reach one from the other by traveling in the forward time direction. As an example of a sampling-based approach that may be preferable when Xobs is not polygonal, place a grid over X and apply one of the classical search algorithms described in Section 5.4.2. Once again, only path segments in X that move forward in time are allowed.