Optimal Motion Planning

This section can be considered transitional in many ways. The main concern so far with motion planning has been feasibility as opposed to optimality. This placed the focus on finding any solution, rather than further requiring that a solution be opti- mal. In later parts of the book, especially as uncertainty is introduced, optimality will receive more attention. Even the most basic forms of decision theory (the topic of Chapter 9) center on making optimal choices. The requirement of optimality in very general settings usually requires an exhaustive search over the state space, which amounts to computing continuous cost-to-go functions. Once such functions are known, a feedback plan is obtained, which is much more powerful than having only a path. Thus, optimality also appears frequently in the design of feedback plans because it sometimes comes at no additional cost. This will become clearer in Chapter 8. The quest for optimal solutions also raises interesting issues about how to approximate a continuous problem as a discrete problem. The interplay between time discretization and space discretization becomes very important in relating continuous and discrete planning problems.

7.7.1 Optimality for One Robot

Euclidean shortest paths One of the most straightforward notions of optimal- ity is the Euclidean shortest path in R2 or R3. Suppose that A is a rigid body that translates only in either W = R2 or W = R3, which contains an obstacle region O ⊂ W. Recall that, ordinarily, Cfree is an open set, which means that any path, τ : [0, 1] → Cfree, can be shortened. Therefore, shortest paths for motion planning

must be defined on the closure cl( free), which allows the robot to make contact with the obstacles; however, their interiors must not intersect.

C

For the case in which obs is a polygonal region, the shortest-path roadmap method of Section 6.2.4 has already been given. This can be considered as a kind of multiple-query approach because the roadmap completely captures the structure needed to construct the shortest path for any query. It is possible to make a single-query algorithm using the continuous Dijkstra paradigm [443, 708]. This method propagates a wavefront from qI and keeps track of critical events

C

Figure 7.39: For a polyhedral environment, the shortest paths do not have to cross vertices. Therefore, the shortest-path roadmap method from Section 6.2.4 does not extend to three dimensions.

in maintaining the wavefront. As events occur, the wavefront becomes composed of wavelets, which are arcs of circles centered on obstacle vertices. The possible events that can occur are 1) a wavelet disappears, 2) a wavelet collides with an obstacle vertex, 3) a wavelet collides with another wavelet, or 4) a wavelet collides with a point in the interior of an obstacle edge. The method can be made to run in time O(n lg n) and uses O(n lg n) space. A roadmap is constructed that uses O(n) space. See Section 8.4.3 for a related method.

Such elegant methods leave the impression that finding shortest paths is not very difficult, but unfortunately they do not generalize nicely to R3 and a polyhe-

dral Cobs. Figure 7.39 shows a simple example in which the shortest path does not

have to cross a vertex of obs. It may cross anywhere in the interior of an edge; therefore, it is not clear where to draw the bitangent lines that would form the shortest-path roadmap. The lower bounds for this problem are also discouraging. It was shown in [172] that the 3D shortest-path problem in a polyhedral environ- ment is NP-hard. Most of the difficulty arises because of the precision required to represent 3D shortest paths. Therefore, efficient polynomial-time approximation algorithms exist [215, 763].

C

General optimality criteria It is difficult to even define optimality for more general C-spaces. What does it mean to have a shortest path in SE(2) or SE(3)? Consider the case of a planar, rigid robot that can translate and rotate. One path could minimize the amount of rotation whereas another tries to minimize the amount of translation. Without more information, there is no clear preference. Ulam’s distance is one possibility, which is to minimize the distance traveled by k fixed points [474]. In Chapter 13, differential models will be introduced, which lead to meaningful definitions of optimality. For example, the shortest paths for a slow-moving car are shown in Section 15.3; these require a precise specification of the constraints on the motion of the car (it is more costly to move a car sideways than forward).

This section formulates some optimal motion planning problems, to provide a smooth transition into the later concepts. Up until now, actions were used in Chapter 2 for discrete planning problems, but they were successfully avoided for

basic motion planning by directly describing paths that map into Cfree. It will be

convenient to use them once again. Recall that they were convenient for defining costs and optimal planning in Section 2.3.

To avoid for now the complications of differential equations, consider making an approximate model of motion planning in which every path must be composed of a sequence of shortest-path segments in free. Most often these are line seg- ments; however, for the case of SO(3), circular arcs obtained by spherical linear interpolation may be preferable. Consider extending Formulation 2.3 from Section

C

2.3.2 to the problem of motion planning.

Let the C-space be embedded in Rm (i.e., Rm). An action will be defined shortly as an m-dimensional vector. Given a scaling constant ǫ and a configuration q, an action u produces a new configuration, q′ = q + ǫu. This can be considered as a configuration transition equation, q′ = f(q, u). The path segment represented

C C ⊂

by the action u is the shortest path (usually a line segment) between q and q′. Following Section 2.3, let πK denote a K-step plan, which is a sequence (u1, u2,

. . ., uK ) of K actions. Note that if πK and qI are given, then a sequence of states, q1, q2, . . ., qK+1, can be derived using f. Initially, q1 = qI , and each following state is obtained by qk+1 = f(qk, uk). From this a path, τ : [0, 1] , can be derived.

→ C

An approximate optimal planning problem is formalized as follows:

Formulation 7.4 (Approximate Optimal Motion Planning)

1. The following components are defined the same as in Formulation 4.1: W, O, A, C, Cobs, Cfree, and qI . It is assumed that C is an n-dimensional manifold.
2. For each q , a possibly infinite action space, U(q). Each u U is an

∈ C ∈

n-dimensional vector.

1. A positive constant ǫ > 0 called the step size.
2. A set of stages, each denoted by k, which begins at k = 1 and continues indefinitely. Each stage is indicated by a subscript, to obtain qk and uk.
3. A configuration transition function f(q, u) = q + ǫu in which q + ǫu is com- puted by vector addition on Rm.
4. Instead of a goal state, a goal region XG is defined.
5. Let L denote a real-valued cost functional, which is applied to a K-step plan, πK . This means that the sequence (u1, . . . , uK ) of actions and the sequence (q1, . . . , qK+1) of configurations may appear in an expression of L. Let F = K + 1. The cost functional is

K

-

L(πK ) = l(qk, uk) + lF (qF ). (7.26)

k=1

The final term lF (qF ) is outside of the sum and is defined as lF (qF ) = 0 if qF XG and lF (qF ) = otherwise. As in Formulation 2.3, K is not necessarily a constant.

∈ ∞

Figure 7.40: Under the Manhattan (L1) motion model, all monotonic paths that follow the grid directions have equivalent length.

Manhattan

Independent Joint

Euclidean

Figure 7.41: Depictions of the action sets, U(q), for Examples 7.4, 7.5, and 7.6.

1. Each U(q) contains the special termination action uT , which behaves the same way as in Formulation 2.3. If uT is applied to qk at stage k, then the action is repeatedly applied forever, the configuration remains in qk forever, and no more cost accumulates.

The task is to compute a sequence of actions that optimizes (7.26). Formu- lation 7.4 can be used to define a variety of optimal planning problems. The parameter ǫ can be considered as the resolution of the approximation. In many formulations it can be interpreted as a time step, ǫ = ∆t; however, note that no explicit time reference is necessary because the problem only requires constructing a path through free. As ǫ approaches zero, the formulation approaches an exact optimal planning problem. To properly express the exact problem, differential equations are needed. This is deferred until Part IV.

C

Example 7.4 (Manhattan Motion Model) Suppose that in addition to uT , the action set U(q) contains 2n vectors in which only one component is nonzero

and must take the value 1 or −1. For example, if C = R2, then

U(q) = {(1, 0), (−1, 0), (0, −1), (0, 1), uT }. (7.27)

When used in the configuration transition equation, this set of actions produces “up,” “down,” “left,” and “right” motions and a “terminate” command. This

produces a topological graph according to the 1-neighborhood model, (5.37), which was given in Section 5.4.2. The action set for this example and the following two examples are shown in Figure 7.41 for comparison. The cost term l(qk, uk) is defined to be 1 for all qk free and uk. If qk obs, then l(qk, uk) = . Note that the set of configurations reachable by these actions lies on a grid, in which the spacing between 1-neighbors is ǫ. This corresponds to a convenient special case in which time discretization (implemented by ǫ) leads to a regular space discretization. Consider Figure 7.40. It is impossible to take a shorter path along a diagonal because the actions do not allow it. Therefore, all monotonic paths along the grid produce the same costs.

∈ C ∈ C ∞

Optimal paths can be obtained by simply applying the dynamic programming algorithms of Chapter 2. This example provides a nice unification of concepts from Section 2.2, which introduced grid search, and Section 5.4.2, which explained how to adapt search methods to motion planning. In the current setting, only algo- rithms that produce optimal solutions on the corresponding graph are acceptable. This form of optimization might not seem relevant because it does not represent

the Euclidean shortest-path problem for R2. The next model adds more actions, and does correspond to an important class of optimization problems in robotics.

．

Example 7.5 (Independent-Joint Motion Model) Now suppose that U(q) includes uT and the set of all 3n vectors for which every element is either 1, 0, or 1. Under this model, a path can be taken along any diagonal. This still does not change the fact that all reachable configurations lie on a grid. Therefore, the standard grid algorithms can be applied once again. The difference is that now there are 3n 1 edges emanating from every vertex, as opposed to 2n in Example

−

−

• 1. This model is appropriate for robots that are constructed from a collection of links attached by revolute joints. If each joint is operated independently, then it makes sense that each joint could be moved either forward, backward, or held

stationary. This corresponds exactly to the actions. However, this model cannot nicely approximate Euclidean shortest paths; this motivates the next example. ．

Example 7.6 (Euclidean Motion Model) To approximate Euclidean shortest paths, let U(q) = Sn−1 uT , in which Sn−1 is the m-dimensional unit sphere centered at the origin of Rn. This means that in k stages, any piecewise-linear path in which each segment has length ǫ can be formed by a sequence of inputs.

∪ { }

Therefore, the set of reachable states is no longer confined to a grid. Consider taking ǫ = 1, and pick any point, such as (π, π) R2. How close can you come to this point? It turns out that the set of points reachable with this model is dense in Rn if obstacles are neglected. This means that we can come arbitrarily close to any point in Rn. Therefore, a finite grid cannot be used to represent the problem.

∈

Approximate solutions can still be obtained by numerically computing an optimal cost-to-go function over C. This approach is presented in Section 8.5.2.

One additional issue for this problem is the precision defined for the goal. If

the goal region is very small relative to ǫ, then complicated paths may have to be selected to arrive precisely at the goal. ．

Example 7.7 (Weighted-Region Problem) In outdoor and planetary naviga- tion applications, it does not make sense to define obstacles in the crisp way that has been used so far. For each patch of terrain, it is more convenient to associate a cost that indicates the estimated difficulty of its traversal. This is sometimes considered as a “grayscale” model of obstacles. The model can be easily captured in the cost term l(qk, uk). The action spaces can be borrowed from Examples 7.4 or

• 1. Stentz’s algorithm [913], which is introduced in Section 12.3.2, generates op-

timal navigation plans for this problem, even assuming that the terrain is initially unknown. Theoretical bounds for optimal weighted-region planning problems are

given in [709, 710]. An approximation algorithm appears in [820]. ．

7.7.2 Multiple-Robot Optimality

Suppose that there are two robots as shown in Figure 7.42. There is just enough room to enable the robots to translate along the corridors. Each would like to arrive at the bottom, as indicated by arrows; however, only one can pass at a time through the horizontal corridor. Suppose that at any instant each robot can either be on or off. When it is on, it moves at its maximum speed, and when it is off, it is stopped.4 Now suppose that each robot would like to reach its goal as quickly as possible. This means each would like to minimize the total amount of time that it is off. In this example, there appears to be only two sensible choices: 1)

A1 stays on and moves straight to its goal while A2 is off just long enough to let

A1 pass, and then moves to its goal. 2) The opposite situation occurs, in which

2 stays on and 1 must wait. Note that when a robot waits, there are multiple locations at which it can wait and still yield the same time to reach the goal. The only important information is how long the robot was off.

A A

Thus, the two interesting plans are that either A2 is off for some amount of time, toff > 0, or A1 is off for time toff . Consider a vector of costs of the form

(L1, L2), in which each component represents the cost for each robot. The costs of the plans could be measured in terms of time wasted by waiting. This yields (0, toff ) and (toff , 0) for the cost vectors associated with the two plans (we could equivalently define cost to be the total time traveled by each robot; the time on is the same for both robots and can be subtracted from each for this simple example). The two plans are better than or equivalent to any others. Plans with this property

are called Pareto optimal (or nondominated). For example, if A2 waits 1 second too long for A1 to pass, then the resulting costs are (0, toff + 1), which is clearly

4This model allows infinite acceleration. Imagine that the speeds are slow enough to allow this approximation. If this is still not satisfactory, then jump ahead to Chapter 13.

Figure 7.42: There are two Pareto-optimal coordination plans for this problem, depending on which robot has to wait.

worse than (0, toff ). The resulting plan is not Pareto optimal. More details on Pareto optimality appear in Section 9.1.1.

Another way to solve the problem is to scalarize the costs by mapping them to a single value. For example, we could find plans that optimize the average wasted time. In this case, one of the two best plans would be obtained, yield- ing toff average wasted time. However, no information is retained about which robot had to make the sacrifice. Scalarizing the costs usually imposes some kind of artificial preference or prioritization among the robots. Ultimately, only one plan can be chosen, which might make it seem inappropriate to maintain multiple solutions. However, finding and presenting the alternative Pareto-optimal solu- tions could provide valuable information if, for example, these robots are involved in a complicated application that involves many other time-dependent processes. Presenting the Pareto-optimal solutions is equivalent to discarding all of the worse plans and showing the best alternatives. In some applications, priorities between robots may change, and if a scheduler of robots has access to the Pareto-optimal solutions, it is easy to change priorities by switching between Pareto-optimal plans without having to generate new plans each time.

Now the Pareto-optimality concept will be made more precise and general. Suppose there are m robots, A1, . . ., Am. Let γ refer to a motion plan that gives the paths and timing functions for all robots. For each A , let Li denote

i

its cost functional, which yields a value Li(γ) [0, ] for a given plan, γ. An

∈ ∞

m-dimensional vector, L(γ), is defined as

L(γ) = (L1(γ), L2(γ), . . . , Lm(γ)). (7.28)

Two plans, γ and γ′, are called equivalent if L(γ) = L(γ′). A plan γ is said

to dominate a plan γ′ if they are not equivalent and Li(γ) Li(γ′) for all i such that 1 i m. A plan is called Pareto optimal if it is not dominated by any others. Since many Pareto-optimal plans may be equivalent, the task is to determine one representative from each equivalence class. This will be called finding the unique Pareto-optimal plans. For the example in Figure 7.42, there are two unique Pareto-optimal plans, which were already given.

≤ ≤

≤

Scalarization For the motion planning problem, a Pareto-optimal solution is also optimal for a scalar cost functional that is constructed as a linear combination of the individual costs. Let α1, . . ., αm be positive real constants, and let

m

-

l(γ) = αi_L_i(γ). (7.29)

i=1

It is easy to show that any plan that is optimal with respect to (7.29) is also a Pareto-optimal solution [606]. If a Pareto optimal solution is generated in this way, however, there is no easy way to determine what alternatives exist.

Computing Pareto-optimal plans Since optimization for one robot is already very difficult, it may not be surprising that computing Pareto-optimal plans is even harder. For some problems, it is even possible that a continuum of Pareto-optimal solutions exist (see Example 9.3), which is very discouraging. Fortunately, for the problem of coordinating robots on topological graphs, as considered in Section 7.2.2, there is only a finite number of solutions [386]. A grid-based algorithm, which is based on dynamic programming and computes all unique Pareto-optimal coordination plans, is presented in [606]. For the special case of two polygonal robots moving on a tree of piecewise-linear paths, a complete algorithm is presented in [212].

Further Reading

This chapter covered some of the most direct extensions of the basic motion planning problem. Extensions that involve uncertainties are covered throughout Part III, and the introduction of differential constraints to motion planning is the main focus of Part

IV. Numerous other extensions can be found by searching through robotics research publications or the Internet.

The treatment of time-varying motion planning in Section 7.1 assumes that all mo- tions are predictable. Most of the coverage is based on early work [153, 506, 818, 819]; other related work includes [367, 368, 532, 812, 875, 905]. To introduce uncertainties into

this scenario, see Chapter 10. The logic-based representations of Section 2.4 have been extended to temporal logics to allow time-varying aspects of discrete planning problems (see Part IV of [382]).

For more on multiple-robot motion planning, see [15, 34, 41, 319, 320, 345, 364, 408, 606, 780, 886]. Closely related is the problem of planning for modular reconfigurable robots [180, 209, 385, 552, 990]. In both contexts, nonpositive curvature (NPC) is an

important condition that greatly simplifies the structure of optimal paths [139, 385, 386]. For points moving on a topological graph, the topology of Cfree is described in [5]. Over

the last few years there has also been a strong interest in the coordination of a team or swarm of robots [165, 228, 274, 300, 305, 336, 361, 665].

The complexity of assembly planning is studied in [398, 515, 733, 972]. The problem is generally NP-hard; however, for some special cases, polynomial-time algorithms have been developed [9, 429, 973, 974]. Other works include [186, 427, 453, 458, 542].

Hybrid systems have attracted widespread interest over the past decade. Most of this work considers how to design control laws for piecewise-smooth systems [137, 634]. Early sources of hybrid control literature appear in [409]. The manipulation planning framework of Section 7.3.2 is based on [16, 17, 166]. The manipulation planning frame- work presented in this chapter ignores grasping issues. For analyses and algorithms for grasping, see [256, 487, 681, 781, 799, 800, 826, 827, 920]. Manipulation on a microscopic scale is considered in [126].

To read beyond Section 7.4 on sampling-based planning for closed kinematic chains, see [244, 246, 432, 979]. A complete planner for some closed chains is presented in [697]. For related work on inverse kinematics, see [309, 693]. The power of redundant degrees of freedom in robot systems was shown in [158].

Section 7.5 is a synthesis of several applications. The application of motion planning techniques to problems in computational biology is a booming area; see [24, 25, 33, 245, 514, 589, 601, 654, 1001] for some representative papers. The knot-planning coverage is based on [572]. The box-folding presentation is based on [661]. A robotic system and planning technique for creating origami is presented in [65].

The coverage planning methods presented in Section 7.6 are based on [222] and [372, 373]. A survey of coverage planning appears in [217]. Other references include [6, 7, 164, 374, 444, 468, 976]. For discrete environments, approximation algorithms for

the problem of optimally visiting all states in a goal set (the orienteering problem) are

presented and analyzed in [115, 188].

Beyond two dimensions, optimal motion planning is extremely difficult. See Section

• • 1. for dynamic programming-based approximations. See [215, 763] for approximate shortest paths in R3. The weighted region problem is considered in [709, 710]. Pareto- optimal coordination is considered in [212, 386, 606].

Exercises

• • • 1. Consider the obstacle region, (7.1), in the state space for time-varying motion planning.
         1. To ensure that Xobs is polyhedral, what kind of paths should be allowed? Show how the model primitives Hi that define O are expressed in general,

using t as a parameter.

• • • • 1. Repeat the exercise, but for ensuring that Xobs is semi-algebraic.
      • Propose a way to adapt the sampling-based roadmap algorithm of Section 5.6 to solve the problem of time-varying motion planning with bounded speed.

Figure 7.43: Two translating robots moving along piecewise-linear paths.

• • • 1. Develop an efficient algorithm for computing the obstacle region for two translating polygonal robots that each follow a linear path.
      2. Sketch the coordination space for the two robots moving along the fixed paths shown in Figure 7.43.
      3. Suppose there are two robots, and each moves on its own roadmap of three paths. The paths in each roadmap are arranged end-to-end in a triangle.
         1. Characterize the fixed-roadmap coordination space that results, including a description of its topology.
         2. Now suppose there are n robots, each on a triangular roadmap, and charac- terize the fixed-roadmap coordination space.
      4. Consider the state space obtained as the Cartesian product of the C-spaces of n identical robots. Suppose that each robot is labeled with a unique integer. Show that X can be partitioned nicely into n! regions in which Xobs appears identical and the only difference is the labels (which indicate the particular robots that are in collision).
      5. Suppose there are two robots, and each moves on its own roadmap of three paths. The paths in one roadmap are arranged end-to-end in a triangle, and the paths in the other are arranged as a Y. Characterize the fixed-roadmap coordination space that results, including a description of its topology.
      6. Design an efficient algorithm that takes as input a graph representation of the connectivity of a linkage and computes an active-passive decomposition. Assume that all links are revolute. The algorithm should work for either 2D or 3D linkages (the dimension is also an input). Determine the asymptotic running time of your algorithm.
      7. Consider the problem of coordinating the motion of two robots that move along precomputed paths but in the presence of predictable moving obstacles. Develop a planning algorithm for this problem.
      8. Consider a manipulator in W = R2 made of four links connected in a chain by revolute joints. There is unit distance between the joints, and the first joint is

attached at (0, 0) in W = R2. Suppose that the end of the last link, which is position (1, 0) in its body frame, is held at (0, 2) ∈ W.

• • • • 1. Use kinematics expressions to express the closure constraints for a configu- ration q ∈ C.
        2. Convert the closure constraints into polynomial form.
        3. Use differentiation to determine the constraints on the allowable velocities that maintain closure at a configuration q ∈ C.

Implementations

• • • 1. Implement the vertical decomposition algorithm to solve the path-tuning problem, as shown in Figure 7.5.
      2. Use grid-based sampling and a search algorithm to compute collision-free motions of three robots moving along predetermined paths.
      3. Under the conditions of Exercise 12, compute Pareto-optimal coordination strate- gies that optimize the time (number of stages) that each robot takes to reach its goal. Design a wavefront propagation algorithm that keeps track of the com- plete (ignoring equivalent strategies) set of minimal Pareto-optimal coordination strategies at each reached state. Avoid storing entire plans at each discretized state.
      4. To gain an appreciation of the difficulties of planning for closed kinematic chains, try motion planning for a point on a torus among obstacles using only the implicit torus constraint given by (6.40). To simplify collision detection, the obstacles can be a collection of balls in R3 that intersect the torus. Adapt a sampling-based planning technique, such as the bidirectional RRT, to traverse the torus and solve planning problems.
      5. Implement the spanning-tree coverage planning algorithm of Section 7.6.
      6. Develop an RRT-based planning algorithm that causes the robot to chase an un- predictable moving target in a planar environment that contains obstacles. The algorithm should run quickly enough so that replanning can occur during execu- tion. The robot should execute the first part of the most recently computed path while simultaneously computing a better plan for the next time increment.
      7. Modify Exercise 16 so that the robot assumes the target follows a predictable, constant-velocity trajectory until some deviation is observed.
      8. Show how to handle unexpected obstacles by using a fast enough planning algo- rithm. For simplicity, suppose the robot is a point moving in a polygonal obstacle region. The robot first computes a path and then starts to execute it. If the obsta- cle region changes, then a new path is computed from the robot’s current position. Use vertical decomposition or another algorithm of your choice (provided it is fast enough). The user should be able to interactively place or move obstacles during plan execution.
      9. Use the manipulation planning framework of Section 7.3.2 to develop an algorithm that solves the famous Towers of Hanoi problem by a robot that carries the rings

[166]. For simplicity, suppose a polygonal robot moves polygonal parts in W = R2

and rotation is not allowed. Make three pegs, and initially place all parts on one

peg, sorted from largest to smallest. The goal is to move all of the parts to another peg while preserving the sorting.

• • • 1. Use grid-based approximation to solve optimal planning problems for a point robot in the plane. Experiment with using different neighborhoods and metrics. Characterize the combinations under which good and bad approximations are obtained.