IEEE Robotics & Automation Magazine - June 2011 - 114

*
constant direction inside of each triangle. The task in
each triangle is to induce a flow that carries the robot into a
triangle that is a step closer to the goal. A navigation function can likewise be constructed on continuous spaces.
Figure 12 shows the level sets of a navigation function that
sends the robot on the shortest path to the goal.
These examples produce piecewise linear trajectories
that are usually inappropriate for execution because the
velocity is discontinuous. One weak form of differential
constraint is that the resulting plan is smooth along all trajectories to the goal. The method shown in Figure 11 can
be adapted to produce smooth vector fields by using bump
functions to smoothly blend neighboring field patches
[Lin13]. Smooth versions of navigation functions can also
be designed for most environments if the obstacles in X are
given [16].
Optimal Feedback Planning
In many contexts, we may demand an optimal feedback
plan. In the discrete-time case, the goal is to design a plan
that optimizes a cost functional,
L(x1 , . . . , xKþ1 , u1 , . . . , uK ) ¼

K
X

l(xk , uk ) þ lKþ1 (xKþ1 ),

k¼1

from every possible start state x1 . Each l(xk , uk ) > 0 is the
cost-per-stage and lKþ1 (xKþ1 ) is the final cost, which is
zero if xKþ1 2 XG , or 1 otherwise. In the special case of
l(xk , uk ) ¼ 1 for all xk and uk , (6) simply counts the number of steps to reach the goal.

xG

(b)

Figure 12. (a) A point goal in a simple polygon. (b) The level
sets of the optimal navigation function (Euclidean cost-to-go
function).

Stage k + 1
Possible Next States
Stage k

xk

Figure 13. Even though xk is a sample point, the next state,
xkþ1 , may land between sample points. For each uk 2 U,
interpolation may be needed for the resulting next state,
xkþ1 ¼ f ðxk , uk Þ.

114

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

Z
~) ¼
L(~x, u

tF

~(t))dt þ lF (~x(tF )),
l(~x(t), u

JUNE 2011

(7)

0

in which tF is the termination (or final) time.
Consider a function GÃ : X ! ½0, 1 called the optimal
cost-to-go, which gives the lowest possible cost GÃ (x) to
get from any x to XG . If x 2 XG , then GÃ (x) ¼ 0, and if XG
is not reachable from x, then GÃ (x) ¼ 1. Note that GÃ is a
special form of a navigation function / as defined in the
"Feasible Feedback Planning" section. In this case, the
optimal plan is executed by applying
uÃ ¼ argmin fl(x, u) þ GÃ (f (x, u))g:
u2U(x)

(8)

If the term l(x, u) does not depend on the particular u
chosen, then (8) actually reduces to (5) with GÃ ¼ /.
The key challenge is to construct the cost-to-go GÃ . Fortunately, because of the dynamic programming principle,
the cost can be written as (see [12]):
GÃk (xk ) ¼ min fl(xk , uk ) þ GÃkþ1 (xkþ1 )g:
uk 2U(xk )

(6)

(a)

The continuous-time counterpart to (6) is

(9)

The equation expresses the cost-to-go from stage k, GÃk ,
in terms of the cost-to-go from stage k þ 1, GÃkþ1 . The classical method of value iteration [2] can be used to iteratively
compute cost-to-go functions until the values stabilize as a
stationary GÃ . There are also Dijkstra-like [12] and policy
iteration methods [2].
When moving to a continuous state space X, the main
difficulty is that GÃk (xk ) cannot be stored for every
xk 2 X. There are two general approaches. One is to
approximate GÃk using a parametric family of surfaces,
such as polynomials or nonlinear basis functions derived
from neural networks [3]. The other is to store GÃk over a
finite set of sample points and use interpolation to
obtain its value at all other points [11] (see Figure 13).
As an example for the one-dimensional case, the value of
GÃkþ1 in (9) at any x 2 ½0, 1 can be obtained via linear
interpolation as
GÃkþ1 (x) % aGÃkþ1 (si ) þ (1 À a)GÃkþ1 (siþ1 ),

(10)

in which the coefficient a 2 ½0, 1.
Computing such approximate, optimal feedback plans
seems to require high-resolution sampling of the state
space, which limits their application to lower dimensions
(less than five in most applications). Although the planning algorithms are limited to lower-dimensional problems, extensions to otherwise difficult cases are straight
forward. For example, consider the stochastic optimal
planning problem in which the state-transition equation
is expressed as P(xkþ1 jxk , uk ). In this case, the expected
cost-to-go satisfies
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2011
