Introduction

• • 1. Problem Formulation

Motion planning under differential constraints can be considered as a variant of classical two-point boundary value problems (BVPs) [440]. In that setting, initial and goal states are given, and the task is to compute a path through a state space that connects initial and goal states while satisfying differential constraints. Mo- tion planning involves the additional complication of avoiding obstacles in the state space. Techniques for solving BVPs are unfortunately not well-suited for motion planning because they are not designed for handling obstacle regions. For some methods, adaptation may be possible; however, the obstacle constraints usually cause these classical methods to become inefficient or incomplete. Throughout this chapter, the BVP will refer to motion planning with differential constraints and no obstacles. BVPs that involve more than two points also exist; however, they are not considered in this book.

It is assumed that the differential constraints are expressed in a state transition

equation, x˙ = f(x, u), on a smooth manifold X, called the state space, which

may be a C-space or a phase space of a C-space. A solution path will not be directly expressed as in Part II but is instead derived from an action trajectory via integration of the state transition equation.

C

Let the action space U be a bounded subset of Rm. A planning algorithm computes an action trajectory u˜, which is a function of the form u˜ : [0, ) U. The action at a particular time t is expressed as u(t). To be consistent with standard notation for functions, it seems that this should instead be denoted

∞ →

as u˜(t). This abuse of notation was intentional, to make the connection to the discrete-stage case clearer and to distinguish an action, u U, from an action

∈

trajectory u˜. If the action space is state-dependent, then u(t) must additionally satisfy u(t) ∈ U(x(t)) ⊆ U. For state-dependent models, this will be assumed by

default. It will also be assumed that a termination action uT is used, which makes it possible to specify all action trajectories over [0, ∞) with the understanding that

at some time tF , the termination action is applied.

The connection between the action and state trajectories needs to be formu- lated. Starting from some initial state x(0) at time t = 0, a state trajectory is

derived from an action trajectory u˜ as

t

r

x(t) = x(0) +

0

f(x(t′), u(t′))dt′, (14.1)

which integrates the state transition equation x˙ = f(x, u) from the initial con-

dition x(0). Let x˜(x(0), u˜) denote the state trajectory over all time, obtained by integrating (14.1). Differentiation of (14.1) leads back to the state transition equa- tion. Recall from Section 13.1.1 that if u is fixed, then the state transition equation defines a vector field. The state transition equation is an alternative expression of (8.14) from Section 8.3, which is the expression for an integral curve of a vector field. The state trajectory is the integral curve in the present context.

The problem of motion planning under differential constraints can be formu- lated as an extension of the Piano Mover’s Problem in Formulation 4.1. The main differences in this extension are 1) the introduction of time, 2) the state or phase space, and 3) the state transition equation. The resulting formulation follows.

Formulation 14.1 (Motion Planning Under Differential Constraints)

• • • 1. A world , a robot (or 1, . . ., m for a linkage), an obstacle region , and a configuration space , which are defined the same as in Formulation 4.1.

C

W A A A O

• • • 1. An unbounded time interval T = [0, ∞).
      2. A smooth manifold X, called the state space, which may be X = C or it may be a phase space derived from C if dynamics is considered; see Section

13.2. Let κ : X → C denote a function that returns the configuration q ∈ C

associated with x ∈ X. Hence, q = κ(x).

• • • 1. An obstacle region Xobs is defined for the state space. If X = C, then

Xobs = Cobs. For general phase spaces, Xobs is described in detail in Section

14.1.3. The notation Xfree = X Xobs indicates the states that avoid collision and satisfy any additional global constraints.

\

• • • 1. For each state x ∈ X, a bounded action space U(x) ⊆ Rm ∪ {uT }, which includes a termination action uT and m is some fixed integer called the

number of action variables. Let U denote the union of U(x) over all x ∈ X.

• • • 1. A system is specified using a state transition equation x˙ = f(x, u), defined

for every x X and u U(x). This could arise from any of the differential models of Chapter 13. If the termination action is applied, it is assumed that f(x, uT ) = 0 (and no cost accumulates, if a cost functional is used).

∈ ∈

• • • 1. A state xI ∈ Xfree is designated as the initial state.
      2. A set XG ⊂ Xfree is designated as the goal region.
      3. A complete algorithm must compute an action trajectory u˜ : T → U, for

which the state trajectory x˜, resulting from (14.1), satisfies: 1) x(0) = xI , and 2) there exists some t > 0 for which u(t) = uT and x(t) ∈ XG.

Additional constraints may be placed on u˜, such as continuity or smoothness

over time. At the very least, u˜ must be chosen so that the integrand of (14.1) is integrable over time. Let denote the set of all permissible action trajectories over T = [0, ). By default, is assumed to include any integrable action trajectory. If desired, continuity and smoothness conditions can be enforced by introducing new phase variables. The method of placing integrators in front of action variables, which was covered in Section 13.2.4, can usually achieve the desired constraints. If optimizing a criterion is additionally important, then the cost functional given by (8.39) can be used. The existence of optimal solutions requires that U is a closed set, in addition to being bounded.

∞ U

U

A final time does not need to be stated because of the termination action uT . As usual, once uT is applied, cost does not accumulate any further and the state remains fixed. This might seem strange for problems that involve dynamics because momentum should keep the state in motion. Keep in mind that the termination action is a trick to make the formulation work correctly. In many cases, the goal corresponds to a subset of X in which the velocity components are zero. In this case, there is no momentum and hence no problem. If the goal region includes states that have nonzero velocity, then it is true that a physical system may keep moving after uT has been applied; however, the cost functional will not measure any additional cost. The task is considered to be completed after uT is applied, and the simulation is essentially halted. If the mechanical system eventually collides due to momentum, then this is the problem of the user who specified a goal state that involves momentum.

The overwhelming majority of solution techniques are sampling-based. This is motivated primarily by the extreme difficultly of planning under differential constraints. The standard Piano Mover’s Problem from Formulation 4.1 is a special case of Formulation 14.1 and is already PSPACE-hard [817]. Optimal planning is also NP-hard, even for a point in a 3D polyhedral environment without differential constraints [172]. The only known methods for exact planning under differential constraints in the presence of obstacles are for the double integrator system q¨ = u,

for = R [747] and = R2 [171].

C C

Section 14.1.2 provides some perspective on motion planning problems under

differential constraints that fall under Formulation 14.1, which assumes that the initial state is given and future states are predictable. Section 14.5 briefly addresses the broader problem of feedback motion planning under differential constraints.

• • 1. Different Kinds of Planning Problems

There are many ways to classify motion planning problems under differential con- straints. Some planning approaches rely on particular properties of the system; therefore, it is helpful to characterize these general differences. The different kinds

of problems described here are specializations of Formulation 14.1. In spite of dif- ferences based on the kinds of models described below, all of them can be unified under the topic of planning under differential constraints.

One factor that affects the differential model is the way in which the task is decomposed. For example, the task of moving a robot usually requires the consideration of mechanics. Under the classical robotics approach that was shown in Figure 1.19, the motion planning problem is abstracted away from the mechanics of the robot. This enables the motion planning ideas of Part II to be applied. This decomposition is arbitrary. The mechanics of the robot can be considered directly in the planning process. Another possibility is that only part of the constraints may be considered. For example, perhaps only the rolling constraints of a vehicle are considered in the planning process, but dynamics are handled by another planning module. Thus, it is important to remember that the kinds of differential constraints that appear in the planning problem depend not only on the particular mechanical system, but also on how the task is decomposed.

• • • 1. Terms from planning literature

Nonholonomic planning The term nonholonomic planning was introduced by Laumond [593] to describe the problem of motion planning for wheeled mobile robots (see [595, 633] for overviews). It was informally explained in Section 13.1 that nonholonomic refers to differential constraints that cannot be completely integrated. This means they cannot be converted into constraints that involve no derivatives. A more formal definition of nonholonomic will be given in Section

15.4. Most planning research has focused on velocity constraints on , as opposed to a phase space X. This includes most of the models given in Section 13.1, which are specified as nonintegrable velocity constraints on the C-space . These are often called kinematic constraints, to distinguish them from constraints that arise due to dynamics.

C

C

In mechanics and control, the term nonholonomic also applies to nonintegrable velocity constraints on a phase space [112, 113]. Therefore, it is perfectly rea- sonable for the term nonholonomic planning to refer to problems that also involve dynamics. However, in most applications to date, the term nonholonomic planning is applied to problems that have kinematic constraints only. This is motivated pri- marily by the early consideration of planning for wheeled mobile robots. In this

book, it will be assumed that nonholonomic planning refers to planning under nonintegrable velocity constraints on C or any phase space X.

For the purposes of sampling-based planning, complete integrability is actually not important. In many cases, even if it can be theoretically established that constraints are integrable, it does not mean that performing the integration is practical. Furthermore, even if integration can be performed, each constraint may be implicit and therefore not easily parameterizable. Suppose, for example, that constraints arise from closed kinematic chains. Usually, a parameterization is not available. By differentiating the closure constraint, a velocity constraint is

obtained on . This can be treated in a sampling-based planner as if it were a nonholonomic constraint, even though it can easily be integrated.

C

Kinodynamic planning The term kinodynamic planning was introduced by Canny, Donald, Reif, and Xavier [290] to refer to motion planning problems for which velocity and acceleration bounds must be satisfied. This means that there

are second-order constraints on . The original work used the double integrator model q¨ = u for = R2 and = R3. A scalar version of this model appeared Example 13.3. More recently, the term has been applied by some authors to

C C

C

virtually any motion planning problem that involves dynamics. Thus, any problem that involves second-order (or higher) differential constraints can be considered as a form of kinodynamic planning. Thus, if x includes velocity variables, then kinodynamic planning includes any system, x˙ = f(x, u).

Note that kinodynamic planning is not necessarily a form of nonholonomic planning; in most cases considered so far, it is not. A problem may even involve both nonholonomic and kinodynamic planning. This requires the differential con- straints to be both nonintegrable and at least second-order. This situation often results from constrained Lagrangian analysis, covered in Section 13.4.3. The car with dynamics which was given Section 13.3.3 is both kinodynamic and nonholo- nomic.

Trajectory planning The term trajectory planning has been used for decades in robotics to refer mainly to the problem of determining both a path and velocity function for a robot arm (e.g., PUMA 560). This corresponds to finding a path in the phase space X in which x X is defined as x = (q, q˙). Most often the problem is solved using the refinement approach mentioned in Section 1.4 by first computing a path through free. For each configuration q along the path, a velocity q˙ must be computed that satisfies the differential constraints. An inverse control problem may also exist, which involves computing for each t, the action u(t) that results in the desired q˙(t). The refinement approach is often referred to as time scaling of a path through [456]. In recent times, trajectory planning seems synonymous with kinodynamic planning, assuming that the constraints are second- order (x includes only configuration and velocity variables). One distinction is that

∈

C

C

trajectory planning still perhaps bears the historical connotations of an approach that first plans a path through Cfree.

• • • 1. Terms from control theory

A significant amount of terminology that is appropriate for planning has been developed in the control theory community. In some cases, there are even conflicts with planning terminology. For example, the term motion planning has been used to refer to nonholonomic planning in the absence of obstacles [156, 727]. This can be considered as a kind of BVP. In some cases, this form of planning is referred to as the steering problem (see [596, 725]) and will be covered in Section 15.5. The

term motion planning is reserved in this book for problems that involve obstacle avoidance and possibly other constraints.

Open-loop control laws Differential models, such as any of those from Chapter 13, are usually referred to as control systems or just systems, a term that we have used already. These are divided into linear and nonlinear systems, as described in Sections 13.2.2 and 13.2.3, respectively. Formulation 14.1 can be considered in control terminology as the design of an open-loop control law for the system (sub- jected to nonconvex constraints on the state space). The open-loop part indicates that no feedback is used. Only the action trajectory needs to be specified over time (the feedback case is called closed-loop; recall Section 8.1). Once the initial state is given, the state trajectory can be inferred from the action trajectory. It may also be qualified as a feasible open-loop control law, to indicate that it satis- fies all constraints but is not necessarily optimal. It is then interesting to consider designing an optimal open-loop control law. This is extremely challenging, even for problems that appear to be very simple. Elegant solutions exist for some re- stricted cases, including linear systems and some wheeled vehicle models, but in the absence of obstacles. These are covered in Chapter 15.

Drift The term drift arose in Section 13.2.1 and implies that from some states it is impossible to instantaneously stop. This difficulty arises in mechanical systems due to momentum. Infinite deceleration, and therefore infinite energy, would be required to remove all kinetic energy from a mechanical system in an instant of time. Kinodynamic and trajectory planning generally involve drift. Nonholonomic planning problems may be driftless if only velocity constraints exist on the C-space; the models of Section 13.1.2 are driftless. From a planning perspective, systems with drift are usually more challenging than driftless systems.

Underactuation Action variables, the components of u, are often referred to as actuators, and a system is called underactuated if the number of actuators is strictly less than the dimension of . In other words, there are less independent action variables than the degrees of freedom of the mechanical system. Under- actuated nonlinear systems are typically nonholonomic. Therefore, a substantial amount of nonholonomic system theory and planning for nonholonomic systems involves applications to underactuated systems. As an example of an underactu-

C

ated system, consider a free-floating spacecraft in R3 that has three thrusters. The amount of force applied by each thruster can be declared as an action variable;

however, the system is underactuated because there are only three actuators, and the dimension of is six. Other examples appeared Section 13.1.2. If the system is not underactuated, it is called fully actuated, which means that the number of actuators is equal to the dimension of . Kinodynamic planning has mostly addressed fully actuated systems.

C

C

Figure 14.1: An obstacle region Cobs ⊂ C generates a cylindrical obstacle region

Xobs ⊂ X with respect to the phase variables.

Symmetric systems Finally, one property of systems that is important in some

planning algorithms is symmetry.1 A system x˙

= f(x, u) is symmetric if the

following condition holds. If there exists an action trajectory that brings the system from some xI to some xG, then there exists another action trajectory that brings the system from xG to xI by visiting the same points in X, but in reverse time. At each point along the path, this means that the velocity can be negated by a different choice of action. Thus, it is possible for a symmetric system to reverse any motions. This is usually not possible for systems with drift. An example of a symmetric system is the differential drive of Section 13.1.2. For the simple car, the Reeds-Shepp version is symmetric, but the Dubins version is not because the car cannot travel in reverse.

• • 1. Obstacles in the Phase Space

In Formulation 14.1, the specification of the obstacle region in Item 4 was inten- tionally left ambiguous. Now it will be specified in more detail. If X = C, then

Xobs = obs, which was defined in (4.34) for a rigid robot and in (4.36) for a robot with multiple links. The more interesting case occurs if X is a phase space that includes velocity variables in addition to configuration information.

C

Any state for which its associated configuration lies in Cobs must also be a member of Xobs. The velocity is irrelevant if a collision occurs in the world W. In

most cases that involve a phase space, the obstacle region Xobs is therefore defined

1Sometimes in control theory, the term symmetry applies to Lie groups. This is a different

concept and means that the system is invariant with respect to transformations in a group such as SE(3). For example, the dynamics of a car should not depend on the direction in which the car is pointing.

NASA/Lockheed Martin X-33 Re-entry trajectory

Figure 14.2: In the NASA/Lockheed Martin X-33 re-entry problem, there are complicated constraints on the phase variables, which avoid states that cause the craft to overheat or vibrate uncontrollably. (Courtesy of NASA)

as

Xobs = {x ∈ X | κ(x) ∈ Cobs}, (14.2)

in which κ(x) is the configuration associated with the state x ∈ X. If the first n variables of X are configuration parameters, then Xobs has the cylindrical structure shown in Figure 14.1 with respect to the other variables. If κ is a complicated mapping, as opposed to simply selecting the configuration coordinates, then the structure might not appear cylindrical. In these cases, (14.2) still indicates the correct obstacle region in X.

• • • 1. Additional constraints on phase variables

In many applications, additional constraints may exist on the phase variables. These are called phase constraints and are generally of the form hi(x) 0. For example, a car or hovercraft may have a maximum speed for safety reasons. There- fore, simple bounds on the velocity variables will exist. For example, it might be specified that q˙ q˙max for some constant q˙max (0, ). Such simple bounds are often incorporated directly into the definition of X by placing limits on the velocity variables.

≤

I I ≤ ∈ ∞

In other cases, however, constraints on velocity may be quite complicated. For example, the problem of computing the re-entry trajectory of the NASA/Lockheed Martin X-33 reusable spacecraft2 (see Figure 14.2) requires remaining within a complicated, narrow region in the phase space. Even though there are no hard obstacles in the traditional sense, many bad things can happen by entering the wrong part of the phase space. For example, the craft may overheat or vibrate uncontrollably [160, 201, 662]. For a simpler example, imagine constraints on X

2This project was canceled in 2001, but similar crafts have been under development.

to ensure that an SUV or a double-decker tour bus (as often seen in London, for example) will not tumble sideways while turning.

The additional constraints can be expressed implicitly as hi(x) ≤ 0. As part

of determining whether some state x lies in Xfree or Xobs, it must be substituted into each constraint to determine whether it is satisfied. If a state lies in Xfree, it will generally be called violation-free, which implies that it is both collision-free and does not violate any additional phase constraints.

• • • 1. The region of inevitable collision

One of the most challenging aspects of planning can be visualized in terms of the region of inevitable collision, denoted by Xric. This is the set of states from which entry into Xobs will eventually occur, regardless of any actions that are applied. As a simple example, imagine that a robotic vehicle is traveling 100 km/hr toward a large wall and is only 2 meters away. Clearly the robot is doomed. Due to momentum, collision will occur regardless of any efforts to stop or turn the vehicle. At low enough speeds, Xric and Xobs are approximately the same; however, Xric grows dramatically as the speed increases.

Let U∞ denote the set of all trajectories u˜ : [0, ∞) → U for which the termina-

tion action uT is never applied (we do not want inevitable collision to be avoided by simply applying uT ). The region of inevitable collision is defined as

Xric = {x(0) ∈ X | for any u˜ ∈ U∞ , ∃t > 0 such that x(t) ∈ Xobs}, (14.3)

in which x(t) is the state at time t obtained by applying (14.1) from x(0). This does not include cases in which motions are eventually blocked, but it is possible to bring the system to a state with zero velocity. Suppose that the Dubins car from Section 13.1.2 is used and the car is unable to back its way out of a dead-end alley. In this case, it can avoid collision by stopping and remaining motionless. If it continues to move, it will eventually have no choice but to collide. This case appears more like being trapped and technically does not fit under the definition of Xric. For driftless systems, Xric = Xobs.

Example 14.1 (Region of Inevitable Collision) Figure 14.3 shows a simple illustration of Xric. Suppose that = R, and the robot is a particle (or point mass) that moves according to the double integrator model q¨ = u (for mass, as- sume m = 1). For simplicity, suppose that u represents a force that must be

W

chosen from U = [ 1, 1]. The C-space is = R, the phase space is X = R2, and a phase (or state) is expressed as x = (q, q˙). Suppose that there are two obstacles in

− C

C: a point and an interval. These are shown in Figure 14.3 along the q-axis. In the cylinder above them, Xobs appears. In the slice at q˙ = 0, Xric = Xobs = Cobs. As

q˙ increases, Xric becomes larger, even though Xobs remains fixed. Note that Xric only grows toward the left because q˙ > 0 indicates a positive velocity, which causes momentum in the positive q direction. As this momentum increases, the distance

required to stop increases quadratically. From a speed of q˙ = v, the minimum

q˙ q˙ > 0

q˙ = 0

q˙ < 0

Figure 14.3: The region of inevitable collision grows quadratically with the speed.

distance required to stop is v2/2, which can be calculated by applying the action

u = −1 and integrating q¨ = u twice. If q˙ > 0 and q is to the right of an obstacle,

then it will safely avoid the obstacle, regardless of its speed. If q˙ < 0, then Xric

extends to the right instead of the left. Again, this is due to the required stopping distance. ．

In higher dimensions and for more general systems, the problem becomes sub- stantially more complicated. For example, in R2 the robot can swerve to avoid small obstacles. In general, the particular direction of motion becomes important. Also, the topology of Xric may be quite different from that of Xobs. Imagine that a small airplane flies into a cave that consists of a complicated network of corridors. Once the plane enters the cave, there may be no possible actions that can avoid collision. The entire part of the state space that corresponds to the plane in the cave would be included in Xric. Furthermore, even parts of the state space from which the plane cannot avoid entering the cave must be included.

In sampling-based planning under differential constraints, Xric is not computed because it is too complicated.3 It is not even known how to make a “collision de- tector” for Xric. By working instead with Xobs, challenges arise due to momentum. There may be large parts of the state space that are never worth exploring because they lie in Xric. Unfortunately, there is no practical way at present to accurately determine whether states lie in Xric. As the momentum and amount of clutter increase, this becomes increasingly problematic.

3It may, however, be possible to compute crude approximations of Xric and use them in

planning.