Complete Methods for Continuous Spaces
A complete feedback planning algorithm must compute a feedback solution if one exists; otherwise, it must report failure. Section 8.4.1 parallels Section 8.2 by defining feedback plans and navigation functions for the case of a continuous state space. Section 8.4.2 indicates how to define a feasible feedback plan from a cell complex that was computed using cell decomposition techniques. Section 8.4.3 presents a combinatorial approach to computing an optimal navigation function
and corresponding feedback plan in R2. Sections 8.4.2 and 8.4.3 allow the feedback plan to be a discontinuous vector field. In many applications, especially those in
which dynamics dominate, some conditions need to be enforced on the naviga- tion functions and their resulting vector fields. Section 8.4.4 therefore considers constraints on the allowable vector fields and navigation functions. This coverage includes navigation functions in the sense of Rimon-Koditschek [829], from which the term navigation function was introduced.
- Feedback Motion Planning Definitions
Using the concepts from Section 8.3, we are now ready to define feedback mo- tion planning over configuration spaces or other continuous state spaces. Recall Formulation 4.1, which defined the basic motion planning problem in terms of con- figuration space. The differences in the current setting are that there is no initial condition, and the requirement of a solution path is replaced by a solution vector
field. The formulation here can be considered as a continuous-time adaptation to Formulation 8.1.
Formulation 8.2 (Feedback Motion Planning)
- A state space, X, which is a smooth manifold. The state space will most often be Cfree, as defined in Section 4.3.1.
9
- For each state, x ∈ X, an action space, U(x) = Tx(X). The zero velocity,
0 Tx(X), is designated as the termination action, uT . Using this model, the robot is capable of selecting its velocity at any state.10
∈
- An unbounded time interval, T = [0, ∞).
- A state transition (differential) equation,
x˙ = u, (8.38)
which is expressed using a coordinate neighborhood and yields the velocity, x˙ , directly assigned by the action u. The velocity produced by uT is 0 Tx(X) (which means “stop”).
∈
- A goal set, XG ⊂ X.
A feedback plan, π, for Formulation 8.2 is defined as a function π, which pro- duces an action u ∈ U(x) for each x ∈ X. A feedback plan can equivalently be considered as a vector field on X because each u ∈ U(x) specifies a velocity vector
(uT specifies zero velocity). Since the initial state is not fixed, it becomes slightly more complicated to define what it means for a plan to be a solution to the prob-
lem. Let Xr ⊂ X denote the set of all states from which XG is reachable. More precisely, a state xI belongs to Xr if and only if a continuous path τ : [0, 1] → X exists for which τ (0) = xI and τ (1) = xG for some xG ∈ XG. This means that a
solution path exists from xI for the “open-loop” motion planning problem, which was considered in Chapter 4.
- Solution concepts
A feedback plan, π, is called a solution to the problem in Formulation 8.2 if from all xI Xr, the integral curves of π (considered as a vector field) arrive in XG, at which point the termination action is applied. Some words of caution must be given about what it means to “arrive” in XG. Notions of stability from control theory [523, 846] are useful for distinguishing different cases; see Section 15.1. If
∈
9Note that X already excludes the obstacle region. For some problems in Part IV, the state space will be X = , which includes the obstacle region.
10This allows discontinuous changes in velocity, which is unrealistic in many applications.
C
Additional constraints, such as imposing acceleration bounds, will also be discussed. For a complete treatment of differential constraints, see Part IV.
XG is a small ball centered on xG, then the ball will be reached after finite time using the inward vector field shown in Figure 8.5b. Now suppose that XG is a single point, xG. The inward vector field produces velocities that bring the state closer and closer to the origin, but when is it actually reached? It turns out that convergence to the origin in this case is only asymptotic; the origin is reached in the limit as the time approaches infinity. Such stability often arises in control theory from smooth vector fields. We may allow such asymptotic convergence to the goal (if the vector field is smooth and the goal is a point, then this is unavoidable). If any integral curves result in only asymptotic convergence to the goal, then a solution plan is called an asymptotic solution plan. Note that in general it may be impossible to require that π is a smooth (or even continuous) nonzero vector field. For example, due to the hairy ball theorem [834], it is known that no such
vector field exists for Sn for any even integer n. Therefore, the strongest possible requirement is that π is smooth except on a set of measure zero; see Section 8.4.4.
We may also allow solutions π for which almost all integral curves arrive in XG.
However, it will be assumed by default in this chapter that a solution plan converges to xG in finite time. For example, if the inward field is normalized to produce unit speed everywhere except the origin, then the origin will be reached in finite time. A constraint can be placed on the set of allowable vector fields without affecting the existence of a solution plan. As in the basic motion planning problem, the speed along the path is not important. Let a normalized vector field be any vector field for which either f(x) = 1 or f(x) = 0, for all x X. This means that all velocity vectors are either unit vectors or the zero vector, and the speed is no longer a factor. A normalized vector field provides either a direction of motion or no motion. Note that any vector field f can be converted into a normalized vector field by dividing the velocity vector f(x) by its magnitude (unless the magnitude is zero), for each x X.
∈
I I ∈
In many cases, unit speed does not necessarily imply a constant speed in some true physical sense. For example, if the robot is a floating rigid body, there are many ways to parameterize position and orientation. The speed of the body is sensitive to this parameterization. Therefore, other constraints may be preferable instead of f(x) = 1; however, it is important to keep in mind that the constraint is imposed so that f(x) provides a direction at x. The particular magnitude is assumed unimportant.
I I
So far, consideration has been given only to a feasible feedback motion planning problem. An optimal feedback motion planning problem can be defined by intro- ducing a cost functional. Let x˜t denote the function x˜t : [0, t] X, which is called the state trajectory (or state history). This is a continuous-time version of the state history, which was defined previously for problems that have discrete stages.
→
Similarly, let u˜t denote the action trajectory (or action history), u˜t : [0, t] → U.
Let L denote a cost functional, which may be applied from any xI to yield
r tF
L(x˜tF , u˜tF ) =
l(x(t), u(t))dt + lF (x(tF )), (8.39)
0
L(x˜tF , u˜tF ) =
l(x(t), u(t))dt + lF (x(tF )), (8.39)
0
in which tF is the time at which the termination action is applied. The term
l(x(t), u(t)) can alternatively be expressed as l(x(t), x˙ (t)) by using the state tran- sition equation (8.38). A normalized vector field that optimizes (8.39) from all initial states that can reach the goal is considered as an optimal feedback motion plan.
Note that the state trajectory can be determined from an action history and initial state. In fact, we could have used action trajectories to define a solution path to the motion planning problem of Chapter 4. Instead, a solution was defined there as a path τ : [0, 1] free to avoid having to introduce velocity fields on smooth manifolds. That was the only place in the book in which the action space seemed to disappear, and now you can see that it was only hiding to avoid inessential notation.
→ C
- Navigation functions
As in Section 8.2.2, potential functions can be used to represent feedback plans, assuming that a local operator is developed that works for continuous state spaces. In the discrete case, the local operator selects an action that reduces the potential value. In the continuous case, the local operator must convert the potential func- tion into a vector field. In other words, a velocity vector must be defined at each state. By default, it will be assumed here that the vector fields derived from the navigation function are not necessarily normalized.
Assume that π(x) = uT is defined for all x XG, regardless of the potential function. Suppose that a potential function φ : X R has been defined for which the gradient
→
∈
∇ _
· · · 『
φ = ∂φ
∂x1
∂φ
∂x2
∂φ
(8.40)
∂xn
exists over all of X \ XG. The corresponding feedback plan can then be defined
as π(x) = φ x. This defines the local operator, which means that the velocity is taken in the direction of the steepest descent of φ. The idea of using potential functions in this way was proposed for robotics by Khatib [525, 526] and can be considered as a form of gradient descent, which is a general optimization technique. It is also possible to work with potential functions for which the gradient does not exist everywhere. In these cases, a continuous-space version of (8.4) can be
−∇ |
defined for a small, fixed ∆t as
u∗ = argmin φ(x′) , (8.41)
J ＼
u∈U (x)
in which x′ is the state obtained by integrating velocity u from x for time ∆t. One problem is that ∆t should be chosen to use the smallest possible neighborhood around φ. It is best to allow only potential functions for which ∆t can be made arbitrarily small at every x without affecting the decision in (8.41). To be precise, this means that an infinite sequence of u∗ values can be determined from a sequence of ∆t values that converges to 0. A potential function should then be chosen to ensure after some point in the sequence, u∗, exists and the same u∗ can be chosen
to satisfy (8.41) as ∆t approaches 0. A special case of this is if the gradient of φ
exists; the infinite sequence in this case converges to the negative gradient.
A potential function, φ, is called a navigation function if the vector field that is derived from it is a solution plan. The optimal cost-to-go serves as an optimal navigation function. If multiple vector fields can be derived from the same φ, then every possible derived vector field must yield a solution feedback plan. If designed appropriately, the potential function can be viewed as a kind of “ski slope” that guides the state to XG. If there are extra local minima that cause the state to become trapped, then XG will not be reached. To be a navigation function, such local minima outside of XG are not allowed. Furthermore, there may be additional requirements to ensure that the derived vector field satisfies additional constraints, such as bounded acceleration.
Example 8.17 (Quadratic Potential Function) As a simple example, sup- pose X = R2, there are no obstacles, and qgoal = (0, 0). A quadratic function
φ(x, y) = 1 x2 + 1 x2 serves as a good potential function to guide the state to the
2 1 2 2
−∇ − −
goal. The feedback motion strategy is defined as f = φ = [ x1 x2], which
is the inward vector field shown in Figure 8.5b.
If the goal is instead at some (x′1, x′2) ∈ R2, then a potential function that guides the state to the goal is φ(x1, x2) = (x1 − x′1)2 + (x2 − x′2)2. ．
Suppose the state space represents a configuration space that contains point obstacles. The previous function φ can be considered as an attractive potential because the configuration is attracted to the goal. One can also construct a repul- sive potential that repels the configuration from the obstacles to avoid collision. Let φa denote the attractive component and φr denote a repulsive potential that is summed over all obstacle points. A potential function of the form φ = φa + φr can be defined to combine both effects. The robot should be guided to the goal while avoiding obstacles. The problem is that it is difficult in general to ensure that the potential function will not contain multiple local minima. The configuration could become trapped at a local minimum that is not in the goal region. This was an issue with the planner from Section 5.4.3.
- Vector Fields Over Cell Complexes
This section describes how to construct a piecewise-smooth vector field over a cell complex. Only normalized vector fields will be considered. It is assumed that each cell in the complex has a simple shape over which it is easy to define a patch of the vector field. In many cases, the cell decomposition techniques that were introduced in Chapter 6 for motion planning can be applied to construct a feedback plan.
Suppose that an n-dimensional state space X has been decomposed into a cell complex, such as a simplicial complex or singular complex, as defined in Section
6.3.1. Assume that the goal set is a single point, xG. Defining a feedback plan π
over X requires placing a vector field on X for which all integral curves lead to
xG (if xG is reachable). This is accomplished by defining a smooth vector field for each n-cell. Each (n 1)-cell is a switching boundary, as considered in Section
−
- This leads directly to solution trajectories in the sense of Filipov. If π is allowed to be discontinuous, then it is actually not important to specify values on any of the cells of dimension n 1 or less.
−
A hierarchical approach is taken to the construction of π:
- Define a discrete planning problem over the n-cells. The cell that contains xG is designated as the goal, and a discrete navigation function is defined over the cells.
- Define a vector field over each n-cell. The field should cause all states in the cell to flow into the next cell as prescribed by the discrete navigation function.
One additional consideration that is important in applications is to try to reduce the effect of the discontinuity across the boundary as much as possible. It may be possible to eliminate the discontinuity, or even construct a smooth transition between n-cells. This issue will not be considered here, but it is nevertheless quite important [235, 643].
The approach will now be formalized. Suppose that a cell complex has been
defined over a continuous state space, X. Let Xˇ
denote the set of n-cells, which
can be interpreted as a finite state space. A discrete planning problem will be
defined over
Xˇ . To avoid confusion with the original continuous problem, the
prefix super will be applied to the discrete planning components. Each superstate
xˇ ∈ Xˇ corresponds to an n-cell. From each xˇ, a superaction, uˇ ∈ Uˇ (xˇ) exists for
each neighboring n-cell (to be neighboring, the two cells must share an (n − 1)-
dimensional boundary). Let the goal superstate xˇg be the n-cell that contains xG. Assume that the cost functional is defined for the discrete problem so that every
action (other than uT ) produces a unit cost. Now the concepts from Section 8.2 can be applied to the discrete problem. A discrete navigation function, φˇ : Xˇ R, can be computed using Dijkstra’s algorithm (or another algorithm, particularly if
→
optimality is not important). Using the discrete local operator from Section 8.2.2, this results in a discrete feedback plan, πˇ : Xˇ Uˇ .
→
Based on the discrete feedback plan, there are two kinds of n-cells. The first is the goal cell, xˇg , for which a vector field needs to be defined so that all integral curves lead to Xg in finite time.11 A termination action can be applied when xG is actually reached. The remaining n-cells are of the second kind. For each cell
xˇ, the boundary that is shared with the cell reached by applying uˇ = πˇ(xˇ) is
called the exit face. The vector field over the n-cell xˇ must be defined so that all integral curves lead to the exit face. When the exit face is reached, a transition will occur into the next n-cell. If the n-cells are convex, then defining this transition is straightforward (unless there are additional requirements on the field, such as
11This is possible in finite time, even if Xg is a single point, because the vector field is not continuous. Otherwise, only asymptotic convergence may be possible.
Figure 8.10: A triangulation is used to define a vector field over X. All solution trajectories lead to the goal.
smoothness at the boundary). For more complicated cells, one possibility is to define a vector field that retracts all points onto a single curve in the cell.
A simple example of the approach is illustrated for the case of X = Cfree ⊂ R2,
in which the boundary of free is polygonal. This motion planning problem was considered in Section 6.2, but without feedback. Suppose that a triangulation of
C
X has been computed, as described in Section 6.3.2. An example was shown in Figure 6.16. A discrete feedback plan is shown for a particular goal state in Figure
8.10. Each 2-cell (triangle) is labeled with an arrow that points to the next cell.
For the cell that contains xG, a normalized version of the inward vector field shown in Figure 8.5b can be formed by dividing each nonzero vector by its magni- tude. It can then be translated to move its origin to xG. For each remaining 2-cell, a vector field must be constructed that flows into the appropriate neighboring cell. Figure 8.11 illustrates a simple way to achieve this. An outward vector field can be made by negating the field shown in Figure 8.5b to obtain f = [x y]. This field can be normalized and translated to move the origin to the triangle vertex that is not incident to the exit edge. This is called the repulsive vertex in Figure 8.11. This generates a vector field that pushes all points in the triangle to the ext edge. If the fields are constructed in this way for each triangle, then the global vector field represents a solution feedback plan for the problem. Integral curves (in the sense of Filipov) lead to xG in finite time.
- Optimal Navigation Functions
The vector fields developed in the last section yield feasible trajectories, but not necessarily optimal trajectories unless the initial and goal states are in the same
convex n-cell. If X = R2, then it is possible to make a continuous version of Dijkstra’s algorithm [708]. This results in an exact cost-to-go function over X
based on the Euclidean shortest path to a goal, xG. The cost-to-go function serves
Figure 8.11: A vector field can be defined for each triangle by repelling from a vertex that opposes the exit edge.
(a) (b)
Figure 8.12: (a) A point, x, in a simple polygon. (b) The visibility polygon, V (x).
as the navigation function, from which the feedback plan is defined by using a local steepest descent.
Suppose that X is bounded by a simple polygon (no holes). Assume that only normalized vector fields are allowed. The cost functional is assumed to be the Euclidean distance traveled along a state trajectory. Recall from Section 6.2.4 that for optimal path planning, X = cl( free) must be used. Assume that free and cl( free) have the same connectivity.12 This technically interferes with the definition of tangent spaces from Section 8.3 because each point of X must be contained in an open neighborhood. Nevertheless, we allow vectors along the boundary, provided that they “point” in a direction tangent to the boundary. This can be formally defined by considering boundary regions as separate manifolds.
C
C C
Consider computing the optimal cost-to-go to a point xG for a problem such as that shown in Figure 8.12a. For any x ∈ X, let the visibility polygon V (x) refer
C
12This precludes a choice of free for which adding the boundary point enables a homotopically distinct path to be made through the boundary point. An example of this is when two square obstacles in R2 contact each other only at a pair of corners.
(a) (b) (c) (d)
Figure 8.13: The optimal navigation function is computed in four iterations. In each iteration, the navigation function is extended from a new way point.
to the set of all points visible from x, which is illustrated in Figure 8.12b. A point x′ lies in V (x) if and only if the line segment from x′ to x is contained in X. This implies that the cost-to-go from x′ to x is just the Euclidean distance from x′ to x. The optimal navigation function can therefore be immediately defined over V (xG) as
φ(x) = Ix − x_G_I. (8.42)
Level sets at regularly spaced values of this navigation function are shown in Figure 8.13a.
How do we compute the optimal cost-to-go values for the points in X V (xG)? For the segments on the boundary of V (x) for any x X, some edges are contained in the boundary of X, and others cross the interior of X. For the example in Figure 8.12b, there are two edges that cross the interior. For each segment that crosses the interior, let the closer of the two vertices to x be referred to as a way point. Two way points are indicated in Figure 8.12b. The way points of V (xG) are places through which some optimal paths must cross. Let W (x) for any x X denote the set of way points of V (x).
∈
\
∈
A straightforward algorithm proceeds as follows. Let Zi denote the set of points over which φ has been defined, in the ith iteration of the algorithm. In the first iteration, Z1 = V (xG), which is the case shown in Figure 8.13a. The way points of V (xG) are placed in a queue, Q. In each following iteration, a way point x is removed from Q. Let Zi denote the domain over which φ is defined so far. The domain of φ is extended to include all new points visible from x. These new points
are V (x)\ Zi. This yields Zi+1 = Zi ∪V (x), the extended domain of φ. The values of φ(x′) for x′ ∈ Zi+1 \ Zi are defined by
φ(x′) = φ(x) + Ix′ − xI, (8.43)
in which x is the way point that was removed from Q (the optimal cost-to-go value of x was already computed). The way points of V (x) that do not lie in Zi+1 are added to Q. Each of these will yield new portions of X that have not yet been
seen. The algorithm terminates when Q is empty, which implies that Zk = X for some k. The execution of the algorithm is illustrated in Figure 8.13.
The visibility polygon can be computed in time O(n lg n) if X is described by n edges. This is accomplished by performing a radial sweep, which is an adapta- tion of the method applied in Section 6.2.2 for vertical cell decomposition. The difference for computing V (x) is that a ray anchored at x is swept radially (like a radar sweep). The segments that intersect the ray are sorted by their distance from x. For the algorithm that constructs the navigation function, no more than O(n) visibility polygons are computed because each one is computed from a unique way point. This implies O(n2 lg n) running time for the whole algorithm. Unfor- tunately, there is no extension to higher dimensions; recall from Section 7.7.1 that computing shortest paths in a 3D environment is NP-hard [172].
The algorithm given here is easy to describe, but it is not the most general, nor the most efficient. If X has holes, then the level set curves can collide by arriving from different directions when traveling around an obstacle. The queue, Q, described above can be sorted as in Dijkstra’s algorithm, and special data structures are needed to identify when critical events occur as the cost-to-go is propagated outward. It was shown in [443] that this can be done in time O(n lg n) and space O(n lg n).
- A Step Toward Considering Dynamics
If dynamics is an important factor, then the discontinuous vector fields considered so far are undesirable. Due to momentum, a mechanical system cannot instanta- neously change its velocity (see Section 13.3). In this context, vector fields should be required to satisfy additional constraints, such as smoothness or bounded ac- celeration. This represents only a step toward considering dynamics. Full consid- eration is given in Part IV, in which precise equations of motions of dynamical systems are expressed as part of the model. The approach in this section is to make vector fields that are “dynamics-ready” rather than carefully considering particular equations of motion.
A framework has been developed by defining a navigation function that satisfies some desired constraints over a simple region, such as a disc [829]. A set of transformations is then designed that are proved to preserve the constraints while adapting the navigation function to more complicated environments. For a given problem, a complete algorithm for constructing navigation functions is obtained by applying the appropriate series of transformations from some starting shape.
This section mostly focuses on constraints that are maintained under this transformation-based framework. Sections 8.4.2 and 8.4.3 worked with normalized vector fields. Under this constraint, virtually any vector field could be defined, pro- vided that the resulting algorithm constructs fields for which integral curves exist in the sense of Filipov. In this section, we remove the constraint that vector fields must be normalized, and then consider other constraints. The velocity given by the vector field is now assumed to represent the true speed that must be executed
when the vector field is applied as a feedback plan.
One implication of adding constraints to the vector field is that optimal so- lutions may not satisfy them. For example, the optimal navigation functions of Section 8.4.3 lead to discontinuous vector fields, which violate the constraints to be considered in this section. The required constraints restrict the set of allowable vector fields. Optimality must therefore be defined over the restricted set of vector fields. In some cases, an optimal solution may not even exist (see the discussion of open sets and optimality in Section 9.1.1). Therefore, this section focuses only on feasible solutions.
- An acceleration-based control model
To motivate the introduction of constraints, consider a control model proposed in [235, 830]. The action space, defined as U(x) = Tx(X) in Formulation 8.2,
produces a velocity for each action u ∈ U(x). Therefore, x˙ = u. Suppose instead
that each action produces an acceleration. This can be expressed as x¨ which x¨ is an acceleration vector,
_
『
= u, in
x¨ =
dx˙
=
dt
d2x1 dt2
d2x2
dt2 · · ·
d2xn dt2
. (8.44)
The velocity x˙ is obtained by integration over time. The state trajectory, x˜ : T X, is obtained by integrating (8.44) twice.
→
Suppose that a vector field is given in the form x˙ = f(x). How can a feedback plan be derived? Consider how the velocity vectors specified by f(x) change as x varies. Assume that f(x) is smooth (or at least C1), and let
∇x˙ f(x) = [∇x˙ f1(x) ∇x˙ f2(x) · · · ∇x˙ fn(x)] , (8.45)
in which ∇x˙ denotes the unnormalized directional derivative in the direction of x˙ :
∇fi · x˙ . Suppose that an initial state xI is given, and that the initial velocity is
x˙ = f(xI ). The feedback plan can now be defined as
u = ∇x˙ f(x). (8.46)
This is equivalent to the previous definition of a feedback plan from Section 8.4.1; the only difference is that now two integrations are needed (which requires both
x and x˙ = f(xI ) as initial conditions) and a differentiability condition must be
satisfied for the vector field.
Now the relationship between x˙
and f(x) will be redefined. Suppose that x˙
is the true measured velocity during execution and that f(x) is the prescribed velocity, obtained from the vector field f. During execution, it is assumed that x˙ and f(x) are not necessarily the same, but the task is to keep them as close to each other as possible. A discrepancy between them may occur due to dynamics that have not been modeled. For example, if the field f(x) requests that the velocity must suddenly change, a mobile robot may not be able to make a sharp turn due to its momentum.
Using the new interpretation, the difference, f(x) x˙ , can be considered as a discrepancy or error. Suppose that a vector field f has been computed. A feedback plan becomes the acceleration-based control model
−
u = K(f(x) − x˙ ) + ∇x˙ f(x), (8.47)
in which K is a scalar gain constant. A larger value of K will make the control system more aggressively attempt to reduce the error. If K is too large, then
acceleration or energy constraints may be violated. Note that if x˙
u = ∇x˙ f(x), which becomes equivalent to the earlier formulation.
= f(x), then
- Velocity and acceleration constraints
Considering the acceleration-based control model, some constraints can be placed on the set of allowable vector fields. A bounded-velocity model means that Ix˙ I <
vmax, for some positive real value vmax called the maximum speed. This could indicate, for example, that the robot has a maximum speed for safety reasons. It is also possible to bound individual components of the velocity vector. For example, there may be separate bounds for the maximum angular and linear velocities of an aircraft. Intuitively, velocity bounds imply that the functions fi, which define the vector field, cannot take on large values.
A bounded-acceleration model means that x¨ amax, in which amax is a pos- itive real value called the maximum acceleration. Intuitively, acceleration bounds imply that the velocity cannot change too quickly while traveling along an integral
I I ≤
curve. Using the control model x¨ = u, this implies that IuI ≤ amax. It also im-
poses the constraint that vector fields must satisfy I∇x˙ f(x)I ≤ amax for all x˙ and
x X. The condition u amax is very important in practice because higher accelerations are generally more expensive (bigger motors are required, more fuel
∈ I I ≤
is consumed, etc.). The action u may correspond directly to the torques that are applied to motors. In this case, each motor usually has an upper limit.
As has already been seen, setting an upper bound on velocity generally does not affect the existence of a solution. Imagine that a robot can always decide to travel more slowly. If there is also an upper bound on acceleration, then the robot can attempt to travel more slowly to satisfy the bound. Imagine slowing down in a car to make a sharp turn. If you would like to go faster, then it may be more difficult to satisfy acceleration constraints. Nevertheless, in most situations, it is preferable to go faster.
A discontinuous vector field fails to satisfy any acceleration bound because it essentially requires infinite acceleration at the discontinuity to cause a discontinu- ous jump in the velocity vector. If the vector field satisfies the Lipschitz condition (8.16) for some constant C, then it satisfies the acceleration bound if C < amax.
In Chapter 13, we will precisely specify U(x) at every x X, which is more general than imposing simple velocity and acceleration bounds. This enables vir- tually any physical system to be modeled.
∈
- Navigation function in the sense of Rimon-Koditschek
Now consider constructing a navigation function from which a vector field can be derived that satisfies constraints motivated by the acceleration-based control model, (8.47). As usual, the definition of a navigation function begins with the consideration of a potential function, φ : X R. What properties does a potential function need to have so that it may be considered as a navigation function as defined in Section 8.4.1 and also yield a vector field that satisfies an acceleration bound? Sufficient conditions will be given that imply that a potential function
→
will be a navigation function that satisfies the bound.
To give the conditions, it will first be important to characterize extrema of multivariate functions. Recall from basic calculus that a function f : R R has a critical point when the first derivative is zero. At such points, the sign of the
→
second derivative indicates whether the critical point is a minimum or maximum.
These ideas can be generalized to higher dimensions. A critical point of φ is one for which ∇φ = 0. The Hessian of φ is defined as the matrix
∂2φ
∂2φ
1
∂2x2
∂2φ
∂x1∂x2
∂2φ
2
· · ·
∂2φ
∂x1∂x
∂2φ
n
H(φ) = ∂x2∂x1
∂x2
· · ·
∂x2∂xn . (8.48)
∂xn∂x1
...
∂2φ
...
∂2φ
∂xn∂x2 · · ·
...
∂2φ
∂x2
n
∂x2
n
At each critical point, the Hessian gives some information about the extremum. If the rank of H(φ) at x is n, then the Hessian indicates the kind of extremum. If (8.48) is positive definite,13 then the φ achieves a local minimum at x. If (8.48) is negative definite,14 then the φ achieves a local maximum at x. In all other cases, x is a saddle point. If the rank of H(φ) at x is less than n, then the Hessian is degenerate. In this case the Hessian cannot classify the type of extremum. An example of this occurs when x lies in a plateau (there is no direction in which φ increases or decreases. Such behavior is obviously bad for a potential function because the local operator would not be able to select a direction.
Suppose that the navigation function is required to be smooth, to ensure the existence of a gradient at every point. This enables gradient descent to be per- formed. If X is not contractible, then it turns out there must exist some critical
points other than xG at which ∇φ(x) = 0. The critical points can even be used
13Positive definite for an n n matrix A means that for all x Rn, xT Ax > 0. If A is symmetric (which applies to H(φ)), then this is equivalent to A having all positive eigenvalues. 14Negative definite means that for all x Rn, xT Ax < 0. If A is symmetric, then this is
∈
× ∈
equivalent to A having all negative eigenvalues.
to infer the topology of X, which is the basic idea in the subject of Morse theory [701, 234]. Unfortunately, this implies that there does not exist a solution naviga- tion function for such spaces because the definition in Section 8.4.1 required that the integral curve from any state that can reach xG must reach it using the vector field derived from the navigation function. If the initial state is a critical point, the integral curve is constant (the state remains at the critical point). Therefore, under the smoothness constraint, the definition of a navigation function should be modified to allow critical points at a small number of places (only on a set that has measure zero). It is furthermore required that the set of states from which the integral curves arrive at each critical point (i.e., the domain of attraction of each critical point) has measure zero. From all possible initial states, except from a set of measure zero, the integral curves must reach xG, if it is reachable. This is ensured in the following definition.
A function φ : X R is called a navigation function in the sense of Rimon- Koditschek if [829]:
→
- It is smooth (or at least C2).
- Among all values on the connected component of free that contains xG, there is only one local minimum, which is at xG.15
C
- It is maximal and constant on ∂Cfree, the boundary of Cfree.
- It is a Morse function [701], which means that at each critical point x (i.e.,
16
∇ |
φ x = 0), the Hessian of φ is not degenerate. Such functions are known
to exist on any smooth manifold.
If φ is smooth in the C∞ sense, then by Sard’s Theorem [234] the set of critical points has measure zero.
Methods for constructing navigation functions are outlined in [829] for a gen- eral family of problems in which free has a semi-algebraic description. The basic idea is to start with simple shapes over which a navigation function can be easily defined. One example of this is a spherical subset of Rn, which contains spheri- cal obstacles. A set of distorting transformations is then developed to adapt the navigation functions to other shapes while ensuring that the four properties above are maintained. One such transformation extends a ball into any visibility region
C
(in the sense defined in Section 8.4.3). This is achieved by smoothly stretching out the ball into the shape of the visibility region. (Such regions are sometimes called star-shaped.) The transformations given in [829] can be combined to define navigation functions for a large family of configuration spaces. The main problem is that the configuration space obstacles and the connectivity of free are repre- sented only implicitly, which makes it difficult to correctly apply the method to
C
15Some authors do not include the global minimum as a local minimum. In this case, one would say that there are no local minima.
16Technically, to be Morse, the values of the function must also be distinct at each critical
point.
complicated high-dimensional problems. One of the advantages of the approach is that proving convergence to the goal is simplified. In many cases, Lyapunov stability analysis can be performed (see Section 15.1.1).
- Harmonic potential functions
Another important family of navigation functions is constructed from harmonic functions [236, 239, 240, 481, 529]. A function φ is called a harmonic function if it satisfies the differential equation
n ∂2φ
∇ φ = - ∂x2 = 0. (8.49)
2
i=1 i
There are many possible solutions to the equation, depending on the conditions along the boundary of the domain over which φ is defined. A simple disc-based example is given in [235] for which an analytical solution exists. Complicated navigation functions are generally defined by imposing constraints on φ along the boundary of free. A Dirichlet boundary condition means that the boundary must be held to a constant value. Using this condition, a harmonic navigation function can be developed that guides the state into a goal region from anywhere in a simply connected state space. If there are interior obstacles, then a Neumann boundary condition forces the velocity vectors to be tangent to the obstacle boundary. By solving (8.49) under a combination of both boundary conditions, a harmonic nav- igation function can be constructed that avoids obstacles by moving parallel to their boundaries and eventually landing in the goal. It has been shown under general conditions that navigation functions can be produced [240, 239]; however, the main problems are that the boundary of free is usually not constructed ex- plicitly (recall why this was avoided in Chapter 5) and that a numerical solution to (8.49) is expensive to compute. This can be achieved, for example, by using Gauss-Seidel iterations (as indicated in [240]), which are related to value iteration (see [96] for the distinction). A sampling-based approach to constructing naviga- tion functions via harmonic functions is presented in [124]. Value iteration will be used to produce approximate, optimal navigation functions in Section 8.5.2.
C
C