JOURNAL OF ?, VOL. ?, NO. ? ? ?

Optimal trajectories of curvature constrained

motion in the Hamilton-Jacobi formulation
Ryo Takei and Richard Tsai

Abstract
We propose a Hamilton-Jacobi equation approach for computing time-optimal trajectories of a
vehicle which travels under certain curvature constraints. We derive a class of Hamilton-Jacobi equations
which models such motions; it unifies two well-known vehicular models, the Dubins and Reeds-Shepps
cars, and gives further generalizations. We show that the value function of the control problem solves
the Hamilton-Jacobi equation in the viscosity sense. Numerical schemes and results that illustrate the
versatility of the Hamilton-Jacobi approach are presented for the two dimensional case.
Index Terms
Optimal path planning, curvature constrained paths, Dubins car, Reeds-Shepps car, reachability
sets, Hamilton-Jacobi equations, finite difference scheme, semi-Lagrangian scheme, viscosity solutions.

I. I NTRODUCTION
We present a Hamilton-Jacobi formulation that models the time optimal motion of a vehicle
that traces paths with an upper bound on curvature. It can be shown that a simple car (see
Figure 1) with axes that are L length apart traces a path with a curvature bounded from above
by max = (tan max )/L, where max the maximum angle that the (front) wheels can tilt.
Equivalently, the simple car has a minimum turning radius = 1/max . There are two wellRyo Takei is with the Department of Mathematics, UCLA, Box 951555 Los Angeles, CA 90095-1555, phone: (310) 825-4701,
fax: (310) 206-6673, e-mail: rrtakei@ucla.edu
Richard Tsai is with the Department of Mathematics and the Institute for Computational Engineering and Sciences at
University of Texas, Austin, 2515 Speedway, RLM 8.100 Austin, TX 78712, phone: (512) 232-7757, fax: (512) 471-9038,
e-mail: ytsai@math.utexas.edu
Manuscript received -; revised .
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

Fig. 1.

A simple car.

known prototypical models of a simple car. The first is the Dubins car [1], a forward-moving
(irreversible) car on a plane traveling at unit speed with a prescribed minimum turning radius.
The second is the Reeds-Shepps car [2], a reversible version of Dubins car. At time t > 0,
we write x(t), y(t) to be the spatial coordinates in two dimensions representing the position of
the car and (t) to be the direction of motion. Then the equations that model the Dubins and
Reeds-Shepps cars are
x(t)

= (t) cos (t)

y(t)
= (t) sin (t)


  1
(t) .

(1)

Here, > 0 is the (minimum) turning radius of the vehicle and the control () determines
whether to move forward or backward. If (t) 1, Dubins car is obtained, and if (t) {1},
we have the Reeds-Shepps car. This can be further generalized to allow for different, spatially
variable forward and reverse speeds and turing radii. Generalizations could be made to higher
dimensions, for instance, steering a simple car on a terrain or modeling an aircraft in three
dimensional space (a Dubins plane [3], also see Example 5 below). Throughout this article, we
shall refer to the motions of such vehicles as curvature constrained motions.
In this article, we describe a Hamilton-Jacobi formulation that unifies a wide range of curvature
constrained motions in an Eulerian framework. This formulation computes the value function,
or the cost-to-go, (see (8) below) by solving a first order partial differential equation, called
a Hamilton-Jacobi equation, and naturally handles the aforementioned generalizations. There
are several advantages to this approach. First, the value function contains nearly all relevant
information pertaining to a path planning problem, such as minimum arrival times (cost-toDecember 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

go), reachable sets (see Definition 27) and optimal paths. Second, the theory of weak solutions
to Hamilton-Jacobi equations known as viscosity solutions is well understood [4], [5]. Finally,
convergent and efficient numerical schemes can be exploited from the well-established numerical
theory for Hamilton-Jacobi equations. These schemes are easy to implement, can be solved on
uniform cartesian grids (or other grid geometries), and can handle domains with complicated
geometries (e.g. obstacles) with ease.
The article is organized as follows. We close the introduction by mentioning other works
concerning curvature constrained motions and Hamilton-Jacobi equations. In Section II we
formally derive the Hamilton-Jacobi equation for curvature constrained motions in two and
three dimensions, and present the equations for some well known models. A recurring theme in
this article is the striking difference between Dubins and Reeds-Shepps car; the value function
for the latter is continuous and the former is not. In Section III, we prove that the value function
of the Reeds-Shepps car is continuous via small-time local controllability, and therefore fits
in to the standard viscosity solution framework. For the Dubins car, a more general class
of weak solutions (called e-solutions) are exploited to establish the connection between the
(discontinuous) value function and the Hamilton-Jacobi equation. In section IV, approximation
schemes for computing the value function and the optimal trajectories are presented. While
standard monotone finite difference scheme are sufficient for problems with continuous value
functions, the performance deteriorates for solutions with discontinuities. Thus, we apply a semiLagrangian scheme to solve for discontinuous value functions. Finally, numerical results are
presented for the two dimensional cases to justify the convergence of the schemes and the
versatility of the formulation.
A. Previous Work
One of the earliest relevant work in curvature constrained motions is the article of Dubins [1],
where it was shown that curvature constrained geodesics in R2 could be classified into just six
types of paths. Later, Reeds and Shepp [2] generalized Dubins work to allow for cusps in the
curve, and showed that such geodesics can be classified into 68 types. The pioneering works of
Dubins, Reeds and Shepp were of theoretical interest, but provided no explicit formulas.
Traditionally, numerical algorithms for curvature constrained path planning have been combinatorial in a Lagrangian framework. Barraquad and Latombe [6] constructed a reachability
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

tree assuming that, for example, Dubins car would locally only move straight, fully left or
fully right for a short distance. This assumption is known as the bang-bang principle [7]. In
the presence of polygonal obstacles, the authors in [8], [9] developed algorithms to compute
optimal -robust paths; informally, a path is -robust if perturbations by /2 distance is still
feasible. Typically, the complexity of such algorithms are inversely proportional to . Others
have considered building optimal curvature constrained paths among obstacles based on obstacle
free optimal paths [10], way-points known a priori [11], or optimal paths with unconstrained
curvatures [12]. Our approach is similar to that of [13]: first determine the value function, then
compute individual trajectories. Their approach (which is Eulerian) involves approximating the
value function by discretizing the dynamic programming principle on tetrahedrons constructed
using barycentric coordinates. For a comprehensive treatment of path planning algorithms and
relevant bibliography, see [14]. We also mention a related work [15], where reachability sets of
Dubins and Reeds-Shepps cars (and others) are geometrically investigated.
Our approach approximates the value function on a uniform mesh, via numerically solving
a Hamilton-Jacobi equation with appropriate boundary conditions. It is well known [16], [17]
that a monotone, consistent scheme for a Hamilton-Jacobi equation converges to the unique
continuous viscosity solution; further techniques for constructing monotone finite difference
schemes were studied in [18]. Standard monotone, consistent numerical methods for solving static
Hamilton-Jacobi equations can be roughly classified into two types: finite difference schemes and
discretization of the dynamic programming principle. The former includes the fast marching and
sweeping methods; both solve the same system of nonlinear equations derived from discretizing
the Hamilton-Jacobi equation. The fast marching method [19], [20] is based on the monotonicity
of information along characteristics. The solutions are constructed using a variant of Dijkstra
algorithm and a heap-sort data structure. The fast sweeping method is based on a Gauss-Seidel
iterative strategy with different ordering of grid nodes. Rouy and Tourin [21] showed that
iteratively sweeping a monotone discretization of the eikonal equation solved for the unique
viscosity solution. Efficient and easy-to-implement Gauss-Seidel type update sweeping schemes
for Hamilton-Jacobi equations with convex Hamiltonians were proposed in [22], [23], [24].
Similar methods for more general, non-convex Hamiltonian was proposed by Kao et. al. [25].
The discretization of the dynamic programming principle on Eulerian grids is often referred
to as semi-Lagrangian schemes [26]. The main advantage of the semi-Lagrangian scheme over
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

finite difference schemes is the formers convergence properties for discontinuous solutions, see
Section IV-B. We also mention that the techniques of discontinuous Galerkin finite element
methods have been applied to Hamilton-Jacobi equations [27], [28].
This article is a generalization of the results in [29], where the authors (joint with H. Shen
and Y. Landa) studied the Hamilton-Jacobi equation and finite difference numerical scheme for
the Dubins car problem.
II. T HE H AMILTON -JACOBI F ORMULATION
In this section we formally derive the Hamilton-Jacobi equation that models a class of curvature
constrained motions in free space. For exposition, we shall formulate the prototypical case of the
minimum exit-time problem with spatially constant speeds and turning radii. Modifications to
accommodate terminal costs, variable running costs and spatially varying speeds and turning radii
can be handled accordingly. Readers interested in the derivation of Hamilton-Jacobi equations
arising in optimal control problems are referred to [5].
A. Notations and setup
We begin by developing the necessary notations used throughout the article. For convenience,
we provide a list of commonly used notations in section II-A1.
Consider a bounded, connected, open set 0 R2 representing the free space. We call a
pose to be point in 0 coupled with an angle (or phase) [0, 2) representing the direction of
forward motion of the car, in radians. We write := 0 [0, 2) as the set of all allowable poses
for the given free space. In three dimensions the free space is 0 R3 , and using spherical
coordinates the set of all allowable poses become = 0 [/2, /2] [0, 2).
We now derive a more general version of the dynamical system (1) for two dimensional
curvature constrained motion. Define a path or trajectory as a piecewise differentiable curve
z : t [0, ) ,

x(t)
,
z(t) =
(t)

x(t) 0 , (t) [0, 2).

(2)

The parameter t represents the time elapsed from the start of the motion. We assume the vehicle
travels at a given finite positive speed:

|x(t)|
= v(z(t), (t)),
December 2, 2010

(3)
DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

where (t) {1} is a control which determines whether to move forward or reverse. Then
the spatial dynamics can be written as

cos (t)
.

x(t)
= (t) v(z(t), (t))
sin (t)

(4)

Moreover, the curvature of the path is

d
d
=
ds
dt

ds
1

=
(t).
dt
v(z(t), (t))

(5)

d
Here, s = s(t) is the arclength parameter. Thus the constraint on curvature | ds
(t)| 1/

translates to |(t)|
v(z(t), (t))/. Introduce another control a(t) [1, 1] to write the phase

dynamics as
= a(t) v(z(t), (t)) ,
(t)

a(t) [1, 1].

(6)

(We shall see later that for trajectories in free space, the control a(t) only attains 1, see equation
(12).) We combine (4) and (6) to obtain the equations and boundary condition:
T

1

z(t)
= v(z(t), (t)) (t) cos (t), (t) sin (t), a(t)

=: f (z(t), (t), a(t))

(7)

z(0) = y0 .
The function f is called the dynamics associated with (7). We say that a path z() is feasible if
z(t) for all t 0. We call the admissible paths from the pose y0 , denoted by Ay0 , the set
of feasible paths z() satisfying (7).
Finally, given a (closed) target set T , we define the value function u : R as
u(y) = min { t | z() Ay , z(t) T }.

(8)

In other words, u(y) is the minimum arrival time to T from y under the speed and curvature
constraints. By definition, the value function is non-negative, and u(y) = 0 if and only if y T .
1) Summaries of notation: As a reference, we list notations used frequently throughout this
article. For concreteness, the notation is for the two dimensional case; the three dimensional
case, using spherical coordinates, are analogous.

0 R2 is the spatial domain.

= 0 [0, 2) is the set of all poses.

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

x = (x, y) 0 is a generic point in space.

y = (x, y, ) is a generic pose.

z() = (x(), ()) is a generic (feasible) path in , parametrized by time t 0.

(), a() are controls (parametrized by time t 0) associated with the car maneuverability
properties.

T is the target set, typically a final location or a final pose.

f is the dynamics of the control problem.

Ay represent the set of feasible paths from the pose y associated with given dynamics.

u is the value function associated with given dynamics and target set.

w(y) denotes the gradient of a function w : R at the point y .

wx , wy , w denotes the partial derivatives of a function w in the variable x, y, , respectively.

The forward and reverse speeds and turning radii in (7) as a function of the pose y are
denoted by
v(y, ) =

(y, ) =

v1 (y) = forward speed ( = 1),

v1 (y) = reverse speed ( = 1),

1 (y) = forward turning radius ( = 1),

(9)

1 (y) = reverse turning radius ( = 1).

B. Formal derivations
For notational simplicity we suppress the dependence of the speeds and turning radii on the
pose, i.e. v1 (y) = v1 , 1 (y) = 1 , etc.
1) Two dimensions: Recall the equations of motion (7). We start the derivation with the
dynamic programming principle [30] for path planning problems with unit running cost (or
equivalently, the minimum arrival time formulation):
u(y) = inf {u(z(t)) + t}.
z()Ay

December 2, 2010

(10)

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

If u is smooth, a Taylor series expansion and a division by t formally implies that, in the limit
t 0,

1,
0 = sup {u(y) z(0)}
z()Ay

, a
)} 1.
0 = sup {u(y) f (y,

{1}
a
[1,1]

This partial differential equation is known as the Hamilton-Jacobi-Bellman equation associated

with the dynamics f . The supremum is actually a maximum since the formula is continuous
with respect to the controls invoked over compact sets:



u
0 = max v
(cos ux + sin uy ) + a

1.

{1}

a
[1,1]

(11)

The maximum over a

can be evaluated by choosing
a
[1, 1] = {1}.

(12)

Thus, we arrive at the Hamilton-Jacobi equation for two dimensional curvature constrained
motions, with constant speeds and turning radii (9):


v
0 = max v
(cos ux + sin uy ) + |u | 1,

{1}

(13)

in \T . The boundary condition is

u(y) = 0,

yT,

(14)

which follows directly from (8).

2) Three dimensions: We derive the three dimensional analogue to (13) in spherical coordinates. For a path z(t) = (x(t), (t), (t)), where (t) [/2, /2] and (t) [0, 2) are the
inclination and azimuth angles, respectively, the velocity is

sin (t) cos (t)

x(t)
= (t)v(t) sin (t) sin (t)

cos (t)
and the curvature is


q
 d x(t)


2
2
2

= 1
 ds |x(t)|
 v(t) (t) + sin (t) (t)




=: (t),(t) (t),
(t)
,
(t) = 1.
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

Thus, the equations for forward and reverse motions become

v(t) sin (t) cos (t)

v sin (t) sin (t)

(t)

z(t)
= f(t) (z(t), a
(t), b(t)) :=
v(t) cos (t)

(t)

b(t)
with controls a
(), b() restricted under the curvature constraint
q
1
a
2 (t) + sin2 (t) b2 (t)
(t),(t) (
a(t), b(t)) =
v(t)
1

(t)

(15)

for (t) {1}. Note that if sin 6= 0, then , (, ) is a norm in R2 , with a dual norm

r
x
a

y2
, (x, y) := max = v x2 +
sin2
,
a,b)1
(
b
y
The derivation of the Hamilton-Jacobi equation from the dynamic programming principle is
nearly identical to the two dimensional case. In three dimensions, (11) becomes
n
v (sin cos ux +sin sin uy +cos uz )
0 = max

{1}
,
a,
b)1/
(

o
a
u bu ] 1
n
= max
v (sin cos ux +sin sin uy +cos uz )

{1}
)

a
u
1.
+
max
,
a,b)1/
(
b
u
If sin 6= 0, the last term is equivalent to the (scaled) dual norm expression of , :
(
0 =max
v (sin cos ux +sin sin uy +cos uz )

{1}

s
+

u2 +

u2
2

sin

(16)

)
1,

in \T . The boundary conditions are the same as (14).

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

10

Remark 1. In general, if the Hamilton-Jacobi-Bellman equation is linear in the controls, the

maximizing controls should be evaluated at the extremal points. This is known as the bang-bang
principle [7]. For example, (11) is linear in a
, so (12) is invoked. Intuitively, choosing a
to be
1 or +1 is equivalent to making a full right or a full left turn, respectively; in this sense,
the Hamilton-Jacobi formulation described above can be seen as an Eulerian counterpart to the
Lagrangian scheme proposed in [6] (see section I-A).
Remark 2. While the solution sought by invoking the bang-bang principle is optimal in free
space ([1], [2]), it may be suboptimal near the obstacle boundaries. For example, consider the
two dimensional case in the presence of an obstacle with a boundary of curvature less than
1/1 . If the true optimal path traces along this boundary, the control must satisfy |
a| < 1,
hence does not obey the bang-bang principle. Indeed, the value function near such obstacle
boundaries would be correctly characterized by the Hamilton-Jacobi-Bellman equation, which
does not assume the bang-bang principle. See section IV-F6 for numerical implications of this
issue.
C. Examples
In light of the equations (13) and (16) we present the Hamilton-Jacobi equations for several
well known models of curvature constrained motion, in two and three dimensions.
Example 3. Dubins Car [1], [29]. The parameters are v1 = 1, v1 = 0, 1 = > 0 (1 is
irrelevant). By virtue of the equation, the maximum in (13) must be evaluated at = 1 (note if
= 1, then the equation becomes 0 = 1). Thus, (13) becomes
1
0 = cos()ux sin()uy + |u | 1.

(17)

Example 4. Reeds-Shepps Car [2]. The parameters are v1 = v1 = 1, 1 = 1 = > 0.

Thus, (13) becomes
1
0 = max {(cos ux + sin uy )} + |u | 1
=1

1
= | cos ux + sin uy | + |u | 1.

(18)

For the Reeds-Shepps car (18), since

| cos ux + sin uy | = | cos( + ) ux + sin( + ) uy |,
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

11

the value function u(x, y, ) is -periodic. Indeed, a vehicle with equal forward and reverse
turning radii and speeds, will have the same optimal arrival times from the poses (x, y, ) and
(x, y, + ).
Example 5. Dubins Plane [3]. The turning radius constraint is enforced as in the Dubins car,
but has an additional independent bounded constraint over speed in the z direction: |z|
c for
some c > 0. This is a simple generalization to (17), with an additional state variable z:
1
0 = cos ux sin uy + c|uz | + |u | 1.

Example 6. Dubins Missile. We can model a vehicle that moves forward in space at unit speed,
with a turning radius constraint in all directions. This is another three dimensional counterpart
to Dubins car. Thus, for sin 6= 0,
0 = sin cos ux sin cos uy cos uz
s
u2
1
u2 +
+
1.

sin2
D. Curvature constrained shortest paths on manifolds
The Hamilton-Jacobi formulation also offers a framework for computing shortest paths of
simple cars on manifolds (i.e. terrains).
Suppose a manifold is represented by a smooth function M : 0 R. In addition to the
usual variables (x, y, ) of a pose, we introduce a new variable z for the height. For paths on an
arbitrary function M , the velocity projected onto the (x, y) plane will be scaled depending on
the gradient of M . We denote this scaling factor by (t) = (x(t), y(t), (t)), to be determined
later; thus,
x(t)

= (t)v(t) cos (t),

(19)
y(t)
= (t)v(t) sin (t).
(For simplicity we have suppressed the spatial dependence on of v(t) .) Furthermore, we have
z(t)
= (x(t),

y(t))

M (x(t), y(t)).

(20)

The velocity constraint of the car, now in three-dimensions, becomes

p

December 2, 2010

x 2 (t) + y 2 (t) + z 2 (t) = v(t) (x(t), y(t)),

(21)

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

12

for (t) {1}. Then, the scaling factor (t) can be solved by substituting (19) and (20) into
(21):
(t) = 1 + [Mx cos (t) + My sin (t))]2

1/2

(22)

where Mx = Mx (x(t), y(t)) and My = My (x(t), y(t)).

To compute the modified equations for (t),

we derive the curvature M (t) of the trajectory
at (x(t), y(t)) on the tangent planes of the manifold z = M (x, y). We fix an orientation by
selecting the unit normal at (x, y, M (x, y))
(t) = [Mx , My , 1]T /(M 2 + M 2 + 1)1/2 .
N
x
y
The unit tangent to the trajectory at (x(t), y(t)) is

cos
(t)

(t) = (t)
.
sin (t)

Mx cos (t) + My sin (t)

d

Then, the curvature of the trajectory on the tangent plane is (t) =  ds
(t) , where s is the ar
clength parameter. It can be shown by elementary calculations that (t) = 4 (t)|(t)|
kG((t))k ,
where

G() =

sin (1 + My2 + 2Mx My sin cos )

.
cos (1 +
+ 2Mx My sin cos )

2
2
Mx sin (My cos 2 1)+My cos (1 +Mx cos 2)
Mx2

Thus, the curvature constraint (t) v(t) / is equivalent to

||

(t)v(t)
,

(23)

where := 5 (t) kG((t))k. Note that is a function of (x(t), y(t), (t)). Since (19) and (23)
are in the form of the equations for the dynamics presented in Section II-A, the corresponding
Hamilton-Jacobi equation can be sought. Note, while the third dimension z(t) appears in the
derivation, it is irrelevant in the computation of the value function. Indeed, z(t) is implicitly
defined M by the relation z(t) = M (x(t), y(t)).

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

13

III. V ISCOSITY SOLUTIONS

In this section, we present a theoretical treatment of the value functions as weak solutions to
the Hamilton-Jacobi equation (13). The value function is in general a non-smooth function, so a
unique weak solution, called a viscosity solution [4], is sought. See section III-A for the definition
of a viscosity solution. We show that the value function to the Reeds-Shepps car problem is
continuous, while the value function of Dubins car is not. Consequently, we take different routes
for describing the solutions, see sections III-B and III-C. We refer to [31] for most of the proofs
in this section. For a more rigorous and comprehensive treatment of Hamilton-Jacobi equations
for control problems, see [5].
A. Definition of viscosity solutions
In this section, we keep the equation in the form prior to invoking (12):
0 = H(y, w(y)) = max {f (y, , a) w(y)} 1,
{1}
a[1,1]

(24)

in \T . While the proofs work for a general class of dynamics, for presentation purposes we
assume the curvature constrained motion dynamics in two dimensions:
T

a
,
f (y, , a) = v (y) cos , sin ,
(y)

(25)

where y = (x, y, ). The boundary condition coupling the PDE (24) is

w(y) = 0

for y T .

(26)

We now define viscosity solutions to the Hamilton-Jacobi equation (24).

Definition 7. We say that a continuous function w is a viscosity subsolution of (24) if for any
C 1 () such that y is a local maximum of w , then
H(y, (y)) 0.
We say that a function w is a viscosity supersolution of (24) if for any C 1 () such that y
is a local minimum of w , then
H(y, (y)) 0.
If w is both a viscosity subsolution and a viscosity supersolution of (24) then we say that w is
a viscosity solution of (24).
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

14

B. Small time local controllability

The key difference between a Dubins car and a Reeds-Shepps car is the former lacks the
maneuverability of the latter. This intuition will be made precise, by means of reachability sets
and small-time local controllability (STLC). Define the reachability set from y within time
t > 0 as
R(y; t) := {z( ) | z() Ay , 0 < t}

t > 0.

(27)

Definition 8. The dynamics f is small-time local controllable, or STLC, at the point y if

y int R(y; t)

for all t > 0.

In other words, the given dynamics is STLC if the vehicle can be steered to anywhere in an
small enough neighborhood about the starting pose. The main result for the STLC dynamics
problem is that the value function u solves (24) in the viscosity sense, provided STLC holds at
each point in the domain (see [31] for a proof, which uses Lemma 10 below):
Theorem 9. If the dynamics f is STLC in , then the value function u is the viscosity solution
to (24) in .
Reeds-Shepps car is STLC [32], therefore its value function is the unique, continuous viscosity
solution to (18). Furthermore, the same argument can be applied to a curvature constrained
motions with v1 , v1 > 0. For velocities that vary in , as long as inf y v1 (y), inf y v1 (y) > >
0, STLC still holds. We will see in the next section that characterizing a unique solution to (24)
and (26) becomes subtle if STLC fails and u is discontinuous.
We close this section by showing that STLC implies the continuity of the value function,
which is of both theoretical and numerical interest.
Lemma 10. If the dynamics f is STLC, then the value function u is continuous.
Proof: Fix y . By the definition of STLC, for each t > 0, there exists r(t) > 0 such
that
Br(t) (y) R(y; t),

for all t > 0,

where Br (y) denotes a ball of radius r centered at y. Choose an arbitrary y0 so that

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

15

|y y0 | < r(t). Then y0 is reachable from y within time t. Thus,

u(y0 ) u(y) + t.
Swapping the roles of y and y0 , we have |u(y0 ) u(y)| < t.
C. Discontinuous value function: Dubins car
If one of v1 or v1 is zero, STLC no longer holds. For instance, for  > 0 small, a Dubins
car with minimum turning radius starting at pose (0, , 0) is required to make a full turn to
arrive at T = {(0, 0, 0)}, so u(0, , 0) > 2 for all  > 0. But since u(0, 0, 0) = 0, the value
function u for the Dubins car is discontinuous at T and the dynamics is not STLC. Without
continuity, u cannot be a viscosity solution as per Definition 7.
While there are several notions for discontinuous viscosity solutions [33], [34], [35], [36], we
chose to exploit e-solutions [5], which are particularly suited for control problems. We begin by
defining subsolutions and supersolutions of a boundary value problem.
Definition 11. We say that a bounded upper semicontinuous (resp. lower semicontinuous) function w is a subsolution (resp. supersolution) of the boundary value problem (24) and (26) if w
is a viscosity subsolution (resp. viscosity supersolution) and w 0 (resp. w 0) at T .
Let S and Z be the set of all subsolutions and supersolutions, respectively, to (24) and (26).
Since a continuous function is both lower and upper semicontinuous, a viscosity solution w of
(24) that satisfies the boundary condition (26) is a subsolution and a supersolution. Thus, in
this case, w S Z. However, for discontinuous value functions u (e.g. for dynamics that are
not STLC), it is too strong to demand that u be a supersolution and u be a subsolution; for
example, for the Dubins car with T = {(0, 0, 0)}, 2 u  0 at T .
To achieve both existence and uniqueness of a weak solution for such a boundary value
problems, we use the notion of e-solutions:
Definition 12. We say w is an e-subsolution of (24) and (26) if there exists =
6 S(w) S such
that
w(y) = sup g(y)

y .

(28)

gS(w)

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

16

We say w is an e-supersolution of (24) and (26) if there exists =

6 Z(w) Z such that
w(y) = inf g(y)

y .

gZ(w)

(29)

Furthermore, if w is an e-subsolution and an e-supersolution of (24) and (26), we say it is an

e-solution of (24) and (26).
The main claim for non-STLC dynamics (25) is the value function u is the unique e-solution
to the boundary value problem (24) and (26). Again, we refer to [31] for a proof.
Theorem 13. The value function u associated with the dynamics (25) is the e-solution of the
problem (24) and (26).
IV. N UMERICAL I MPLEMENTATION
In this section, we describe two numerical schemes to solve the Hamilton-Jacobi equation for
curvature constrained motions in two dimensions (13). We validate the implementation of the
schemes with numerical results. The three dimensional settings works analogously.
First, in Section IV-A, a monotone finite difference scheme is described for solving continuous
value functions. Next, in Section IV-B we present a semi-Lagrangian scheme for the discontinuous case. This is followed, in section IV-C, by a description of how optimal paths are computed
from a given (numerical) value function. We dedicate Sections IV-D and IV-E on the treatment
of boundary conditions and obstacles, respectively. Finally, in Section IV-F, we demonstrate the
performance of the aforementioned schemes under various settings.
A. Continuous value functions: finite difference scheme
In order to approximate the solution of (13), we exploit the results of [16], [17] that monotone,
consistence schemes converge to the (continuous) viscosity solution.
Set up a three dimensional uniform Cartesian grid {(xi , yj , k )}, i = 1, . . . , Nx , j = 1, . . . , Ny , k =
1, . . . , N , with grid refinements (hx, , hy , h ). Let ui,j,k be the approximation of the solution
at the grid node (xi , yj , k ). Write k := sgn(cos k ), k := sgn (sin k ), Ck := | cos k | and
Sk := | sin k |. The finite difference construction of the scheme is as follows: the spatial derivative

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

17

terms in x and y are discretized in the upwind sense,

ui+k ,j,k uijk
,
(cos ux )ijk = Ck
hx
ui+k ,j,k uijk
(sin uy )ijk = Sk
,
hy
and the derivative in with a monotone discretization


uijk ui,j,k+1 uijk ui,j,k1
(|u |)ijk = max
,
,0 .
h
h

(30)

(31)

To approximate (13) with the discretization (30) and (31), we solve for uijk for the cases = 1
and (|u |)ijk zero or non-zero. Assume for simplicity that hx = hy =: h. For the case = 1, if
(|u |)ijk 6= 0,
uijk =

h
v1

+ Ck ui+k ,j,k + Sk ui,j+k ,k +

Ck + Sk +

h min{ui,j,k1 }
1 h

h
1 h

(32)

=: G+1
ijk
and if (|u |)ijk = 0,
uijk =

h
v1

+ Ck ui+k ,j,k + Sk ui,j+k ,k

Ck + Sk

+1
=: Fijk
.

(33)

For the case = 1, if (|u |)i,j,k 6= 0,

uijk =

h
v1

+ Ck uik ,j,k + Sk ui,jk, ,k +

Ck + S k +

h min{ui,j,k1 }
1 h

h
1 h

(34)

=: G1
ijk ,
and if (|u |)ijk = 0,
uijk =

h
v1

+ Ck uik ,j,k + Sk ui,jk ,k

Ck + Sk

1
=: Fijk
.

(35)

We solve the system of nonlinear equations (32)-(35) for {uijk } via the fast sweeping method [23],
[22]. Initially set
u0ijk =

if (xi , yj , k ) T

otherwise,
for all (i, j, k), and compute unijk for n = 1, 2, . . . iteratively according to the update scheme
 1 1 n
un+1
=
min
Gijk , Fijk , uijk .
(36)
ijk
by a Gauss-Seidel sweeping scheme; in each iteration, the grid nodes are visited by eight
different orderings, ascending and descending in each coordinate i, j, k. Then, unijk converges
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

18

to the solution of the nonlinear equations (32)-(35) as n . The above discretization is

monotone in the sense that the update formula for un+1
ijk is a non-decreasing function of the
neighboring values uni1,j+1,k , uni1,j1,k , uni,j,k1 . Thus, the computed approximation converges to
the viscosity solution of (13) as hx , hy , h 0.
B. Discontinuous value functions: semi-Lagrangian scheme
While a similar finite difference discretization gives somewhat reasonable approximations of
the value function (c.f. [29]), we resort to a semi-Lagrangian scheme, which is known to have
convergence properties for e-solutions, see Remark 14.
The idea behind semi-Lagrangian schemes is to discretize the dynamic programming principle (10) on an Eulerian (in our case Cartesian) grid, for a short time comparable to the grid
refinement. We tailor the formulation described in [37] for a Dubins-type (i.e. forward moving)
vehicle with speed v1 = v > 0 and turning radius 1 = . The value function is approximated at
the grid node yi,j,k = (xi , yi , k ). Denote za() (t) to be an optimal path with control a(t) [1, 1]:
T

a(t)
,
(37)
z a() (t) = v cos (t), sin (t),

za() (0) = yi,j,k .

(38)

For a time step > 0 small enough, we assume that za() (t) is in free space for 0 t , and
the optimal control is

aL = 1,
(left bang)

a(t) = aR = 1, (right bang)

a0 = 0,
(no bang, move straight).
The dynamic programming principle at the point yi,j,k becomes,
u(yi,j,k ) = +

min

a{aL ,aR ,a0 }

u(za ( ))

(39)

The value of u(za ( )) is approximated by a (three dimensional) interpolation of nearby grid

values of u.
In our implementation, we choose
=

December 2, 2010

h
.
v

(40)

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

19

so that the -components of zaL ( ), za0 ( zaR ( ) are k+1 = k + h , k and k1 = k h ,

respectively. Since za ( ) for each a {aL , a0 , aR } lie exactly on grid nodes, an interpolation
is required only in the x, y directions.
For completeness, we explicitly write za ( ) for each a {aL , aR , a0 } in the case is chosen as
in (40). Denote Rot() to be the rotation matrix by radians in two dimensions. For simplicity,
we consider the case hx = hy =: h. For the choice of time step (40), we have


zaL ( ) = xi + h
xL , yi + h
y L , k+1 ,


zaR ( ) = xi + h
xR , yi + h
y R , k1 ,


za0 ( ) = xi + h
x0 , yi + h
y 0 , k ,
where,

L
x
sin h
= Rot(k )
,
yL
(cos h 1)

R
sin h
x
,
= Rot(k )
(1 cos h )
yR


0
h
x
= Rot(k ) .
0
y0

(41)

(42)

We implemented the simplest case, linear interpolation using (xi , yj ) and its eight closest
neighboring grid nodes for each k , k1 . For each point in (41), we approximate u at the point
using the nearest four grid nodes (in (x, y)). For stability, we must choose h and h so that the
points in (41) be within the eight neighboring grid nodes about (xi , yj ), that is,
h h.
For problems with spatially varying turning radius (x), the restriction is
sup {(x)} h h.
x

Remark 14. Bardi, Falcone and Soravia [38], [39] have shown that the semi-Lagrangian scheme
converges as /h 0, on compact sets where the e-solution is continuous. In [40], however,
numerical tests of control problems with discontinuous value functions have shown that convergence can still be obtained for /h c, for a constant c > 0. We have found that this property
holds for discontinuous value functions in curvature constrained motions (e.g. Dubins car), see
section IV-F1.
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

20

C. Computing optimal paths

Once the value function has been approximated by uijk , individual optimal paths can be
computed by tracing the characteristic curves backwards towards the target set T . Suppose the
initial pose is (x(0), y(0), (0)) = (x0 , y0 , 0 ). Note that the characteristic curves are precisely
the optimal trajectories, but in opposite directions. For the general case in two dimensions (13),
the optimal trajectories z(t) = (x(t), y(t), (t)) satisfy

x(t)

= v (z(t)) cos (t)

y(t)
= v (z(t)) sin (t)

(t)
= v (z(t)) sgn (u (z(t))) .
(z(t))

(43)

Recall that = (z(t)) {1} determines whether to move the vehicle forward or reverse.
In the discrete setting, one can compute ijk {1} at each grid node (i, j, k) in the process
0
of computing the value function: initially set ijk
= 1, for all grid nodes (i, j, k), and update

according to

n+1
ijk
= 1

n
ijk



 1 1 n
+1
if min G+1
ijk , Fijk min Gijk , Fijk , uijk


 +1 +1 n
1
if min G1
ijk , Fijk < min Gijk , Fijk , uijk
1
if unijk < G1
ijk , Fijk .

We solve (43) by the explicit forward Euler method, until (x(t), y(t), (t)) reaches within a
prescribed tolerance distance from T . The updated pose (x(t + t), y(t + t), (t + t)) from
(x(t), y(t), (t)) is computed by first approximating
x , y(t), (t))
u(x(t)+h
ux (x(t), y(t), (t))

x , y(t), (t))
u(x(t) h
x
2h

and likewise for uy and u . The value function at non-grid nodes is approximated via linear
approximation. To ensure that {1} at non-grid locations, we implement a nearest-neighbor
approximation for (x(t), y(t), (t)). Where the value function is continuous, the computed paths
x, h
y, h
, t} O(hx , hy , h ) 0 by virtue
converge to the analytical characteristic curves as {h
of the approximation schemes.

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

21

D. Treatment of boundary conditions

Inaccuracies near the boundary condition is a known phenomenon for finite difference solutions, for instance, of Eikonal equations. We have found that the numerical approximation
computed using methods of sections (IV-A), (IV-B) can be inaccurate near T . The inaccuracy
is particularly noticeable for T that are singular in space and/or phase.
There are several ways to alleviate this problem. An obvious way is the increase the number
of grid points. Another way is to use a wider stencil which computes the gradient in more grid
directions [29]. We chose another approach, which is to enlarge the target set to be T := {y |
dist(y, T ) }, i.e. a morphological dilation of T with a ball of radius . Typically, we found
= 5 max{h, h } to be sufficient for reasonable accuracy.
E. Treatment of obstacles
It is standard practice in the PDE optimal control community to implement obstacles in
domains by setting the value function to be a very large number at grid nodes inside the obstacle.
This technique produces decent results for typical problems in optimal control, such as the
eikonal equation, see for example [41]. This property does not necessarily carry over for (13).
We have found that, for curvature constrained motions with STLC (e.g. Reeds-Shepps car),
the aforementioned approach performs well provided a wider stencil is implemented [29] in the
freespace near the obstacles.
Curvature constrained motions without STLC requires a more delicate approach. Rather than
forcing the value function to be infinite in the obstacles, we take a more natural and versatile
approach to modeling state constraints: we interpret obstacles as regions where the speed is zero
(or close to zero) or, equivalently, where the local cost-to-go is infinite (or very large), thus
extending the Hamilton-Jacobi equation into the obstacles. Assume that the speed v = v(x) only
varies in the spatial domain 0 . We introduce the slowness function r : 0 R as the inverse
of the speed: for a predefined large number rmax  1, set

1/v(x) if x is not in an obstacle,

r(x) =

rmax
if x is in an obstacle.

(44)

The key idea is to regularize r by smoothing out the discontinuities over a narrow band of
thickness  = O(h). Let us write the regularized slowness function as r . We implemented the
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

22

regularization

r(x)

r (x) = rmax

 (x)

if w(x) > 0
if w(x) < 2
if w(x) [2, 0],

where
 


rmax r(x)
w(x) + 2
(x) =
cos
+ 1 +r(x)
2
2


and w is the signed distance function to the obstacles:

w(x) =

inf
r(x0 )=rmax

|x x0 |

inf
r(x0 )<rmax

|x x0 |.

For the update scheme (39) at the grid node (xi , yj , k ), we set according to (40) and (44):
= h r (xi , yj , k ).
Indeed, the numerical solution will be influenced by  and rmax ; we have found that certain
choices of these parameters noticeably improves the accuracy of the computed optimal trajectories, see section IV-F6.
F. Numerical results
We give results of the numerical schemes described above. All test cases (except for convergence tests) were computed on the a grid of size Nx = Ny = 200 over the numerical spatial
domain = [1, 1]2 , and N = 200. All numerical optimal paths (43) were computed using the
time step t = min{h, h }/10.
1) Dubins car: Recall that Dubins car is the case v1 = 1, v1 = 0 and 1 > 0. For all test
cases, we used 1 = 0.2. We computed the value function in the domain = [1, 1]2 using
the semi-Lagrangian scheme (see section IV-B). Two targets were considered: a final pose target
TPose = {(0, 0, 0)} and a final location target TLoc = {(0, 0, ) | [0, 2)}.
Level sets of u(x, y, ) for the target TPose is shown in Figure 2 (left) and for TLoc in Figure 3
(left).
As a convergence test, we compared the numerical solution of u(x, y, ) for the case TPose =
{(0, 0, 0)} along the line segment {(x, 0, 0) | x [1, 1]}. The exact solution is

x
for x 0
u(x, 0, 0) =

2 + x for x > 0.
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

Fig. 2.

23

Numerical solution to for TPose = {(0, 0, 0)}, for = 1/5. Left: Dubins car (0.9 level set). Right: Reeds-Shepps car

(0.7 level set).

Fig. 3.

Numerical solution to for TLoc = {(0, 0, ) | [0, 2)}, for = 1/5. Left: Dubins car (0.9 level set). Right:

Reeds-Shepps car (0.7 level set).

Table I shows a O(h1/2 ) convergence rate in the L2 norm for the semi-Lagrangian scheme. Note
that O(h1/2 ) is a standard rate of convergence for numerical schemes of Hamilton-Jacobi equations [16]. As a comparison, we show the same test results for the finite difference scheme [29],
which gave a significantly slower convergence rate.
We also performed a convergence test for the case TLoc = {(0, 0, ) | [0, 2)} on the same
line segment {(x, 0, 0) | x [1, 1]}. The exact solution is

x
for x 0
u(x, 0, 0) =

2( tan1 (x/)) for x > 0.

Table II shows the L2 norm convergence of both the semi-Lagrangian and finite difference
schemes for the case TLoc . Note how the semi-Lagrangian scheme is more accurate than the
finite difference scheme by a small margin.
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

24

error SL

rate

error FD

rate

100

0.7582

0.6985

200

0.5542

0.4523

0.6450

0.1150

300

0.4508

0.5093

0.6087

0.1429

400

0.3665

0.7193

0.5832

0.1485

TABLE I
C ONVERGENCE RATES FOR THE CASE TP OSE = {(0, 0, 0)} FOR THE SEMI -L AGRANGIAN (SL) AND FINITE DIFFERENCE (FD)
SCHEMES .

error SL

rate

error FD

rate

100

0.3759

0.3754

200

0.2754

0.4488

0.2779

0.4337

300

0.2270

0.4764

0.2343

0.4209

400

0.1969

0.4948

0.2082

0.4105

TABLE II
C ONVERGENCE RATES FOR THE CASE TL OC = {(0, 0, ) | [0, 2)} FOR THE SEMI -L AGRANGIAN (SL)
DIFFERENCE

AND FINITE

(FD) SCHEMES .

Let
U (x, y) := min u(x, y, ),
[0,2)

the minimum arrival time to the target from (x, y) over all initial directions. Figure 4 shows
a sample numerical path and a contour plot of U (x, y) for a Dubins car in a domain with
obstacles.
2) Reeds-Shepps car: Recall that the Reeds-Shepps car is the case v1 = 1, v1 = 1 and
1 = 1 =: > 0. Since the dynamics is STLC and the value function is continuous, we
employ the finite difference approximation as described in section IV-A. Also, note that the
value function is computed only on [0, ) in the domain, with periodic boundary conditions
(see discussion after Example 4).
Figures 2 (right) and 3 (right) show the level sets of u(x, y, ) for the target pose TPose =
{(0, 0, 0)} and target location TLoc = {(0, 0, ) | [0, 2)}, respectively. The turning radius is
= 0.2.

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

25
U(x,y)
Obstacle bdry
Optimal path
start
end

Length = 4.9386
0.8
0.6

8
7
6

0.4
5

0.2
0

0.2

0.4
2
0.6
1

0.8
0.8 0.6 0.4 0.2

Fig. 4.

0.2

0.4

0.6

0.8

Numerical solution to Dubins car in a domain among obstacles. The contour plot represents U (x, y). Parameters are:

initial pose (x0 , y0 , 0 ) = (0.8, 0.8, /2), target pose T = {(0.8, 0.7, 0)}, turning radius = 0.2.
Length = 1.4019
0.8
0.7

Length = 0.77362

U(x,y)
Obstacle bdry
Optimal path

0.8
0.7

0.6

0.6

0.5

0.5

0.4

0.4

0.3

0.3

0.2

0.2

0.1

0.1

0.1

0.1

0.2
0.2

0.2

0.4

0.6

0.8

0.2
0.2

U(x,y)
Obstacle bdry
Optimal path

0.2

0.4

0.6

0.8

Fig. 5. Optimal paths for a reversible simple car for initial pose (x0 , y0 , 0 ) = (0.6, 0.8, /2) and target pose T = {(0, 0, /2)}.
Left: a Reeds-Shepps car. Right: a car with v1 = 2, v1 = 1, 1 = 1/6 and 1 = 1/10.

3) Different forward and reverse speeds and turning radii: To demonstrate the versatility of
our algorithm, we consider the steering of a vehicle with different forward and reverse speeds
and turning radii, see Figure 5 (right). Since the forward-reverse symmetry (as in the ReedsShepps car) no longer holds, the domain is set to [0, 2). For both examples in Figure 5, we
implemented a 16-node wide stencil in the freespace, see Section IV-E.
4) Spatially varying turning radius: We tested our algorithm on a problem of steering a
Dubins car along a sequence of contiguous rectangles, each with difference turning radii, see
Figure 6. This is a problem posed in [42], where it was solved using purely geometrical
arguments. While explicit geometrical calculations become increasingly difficult in compliDecember 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

26

cated environments, our algorithm requires only a trivial modification: replace in (42) by
i,j,k = (xi , yj , k ).
Length = 3.8547
8
0.8
0.6

U(x,y)
Obstacle bdry
Optimal path
start

= 1/10

= 1/8

0.4

0.2
0

= 1/10

= 1/8

0.2

0.4
2
0.6
0.8

= 1/8
0.8 0.6 0.4 0.2

= 1/5
0

0.2

0.4

0.6

0.8

Fig. 6. Dubins car problem for a spatially varying turning radius. The thin dotted lines are where (x, y) changes value. The
initial position of the optimal path is (x0 , y0 , 0 ) = (0.15, 0.85, 3/4). The target is T = {(1, y, ) | y [0.2, 0.2],
[0, 2)}.

5) Optimal paths on manifolds: We solve the shortest path problem on a manifold M (x, y) =
1/(x2 + y 2 + 0.6) for the Dubins car using the technique of Section II-D, see Figure 7.

Fig. 7. An optimal trajectory for a Dubins car on the manifold M (x, y) = 1/(x2 + y 2 + 0.6). The target is T = {(0.5, 0, ) |
[0, 2)} and the starting pose is (0.5, 0, ). Optimal paths for = 0.2 (solid), and = 0.002 (dotted) are shown. Left:
paths projected onto the (x, y) plane, with an intensity plot of M (x, y). Right: three dimensional plot of optimal paths on the
manifold.

6) Obstacle implementation and limitations of the algorithm: As mentioned in Remark 2,

the Hamilton-Jacobi equations (13) assume the bang-bang principle, which is valid only away
December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

27

from the obstacles. In the absence of obstacles, the works of Dubins and Reeds-Sheep prove
that the steering must obey the bang-bang principle. However, for steering in domains amongst
obstacles, the bang-bang principle may not yield exact solutions. A simple example is when the
optimal path traces along a obstacle boundary of curvature less than , the turning radius. Our
algorithm forces the bang-bang principle in the grid directions of the underlying stencil. While
a wider stencil may partially alleviate this issue (see wide stencil implementation in [29]), a
convergence in the limit hx , hy , h 0 requires a full implementation of the minimization over
the control a [1, 1]. While this is possible in principle, it will significantly complicate and
slow down the current implementation.

(see secHowever, we have found that employing the regularized slowness function rijk

tion IV-E) can also improve the solution near low curvature obstacles. Figure 8 (left) shows computed optimal paths of a Dubins car around a large circular obstacle with radius R = 0.5 > = 0.2
for regularization parameters  = 0 (no regularization) and  = 23h. The maximum slowness is
set to rmax = 100, and the target location is (0, 0.8). Note how the computed optimal path
for  = 23h is clearly more accurate. In Figure 8 (right) we plot the accuracy of u(0.5, 0.5, 0)
with the regularizations  = nh for a range of integer n from 0 to 25. Note that for small n,
the regularized solution overestimates u, while for large n, it underestimates. For the grid size
Nx = Ny = N = 200, the optimal regularization appears to be n = /h 22.5.
ucomp uexact
0.8
0.6

0.1

0.4
0.2

0.05

0
0.2

0.4
0.6
0.8

0.05
0
0.5

0.5

10

15

20

25

!/h

Fig. 8. The effects of regularization to optimal paths around obstacles (shaded). The target is T = {(0, 0.8, ) | [0, 2)}.
Left: the computed optimal path from (0.5, 0.5, 0) with no regularization (solid) and with  = 23h regularization (dotted) of
the obstacle. Right: a plot of ucomp (0.5, 0.5, 0) uexact (0.5, 0.5, 0), for regularizations with  = nh (n ranging between 0
and 25).

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

28

V. C ONCLUSION
We presented a Hamilton-Jacobi formulation for computing optimal trajectories with lower
bounds on curvature. It models the optimal path planning problem of simple cars, such as the
Dubins and Reeds-Shepps cars. In addition to formally deriving the Hamilton-Jacobi equations
for such problems, we show that the value functions solve the equations in the viscosity sense.
Furthermore, approximation schemes are presented, as well as numerical results verifying the
theory.
VI. ACKNOWLEDGEMENT
The authors thank Professors S. Osher, P. Souganidis and A. Vladimirsky for helpful discussions, and Dr. Y. Landa and Mr. H. Shen for taking part in the early stages of this project. Takei
was supported by ONR grants N00014-07-1-0810 and N00014-08-1-1119. Tsai was supported
by a Sloan Fellowship, National Science Foundation grants DMS-0914465, DMS-0914840, and
a MURI sub-contract from Univ. of S. Carolina grant No. W911NF-07-1-0185.
R EFERENCES
[1] L. E. Dubins, On curves of minimal length with a constraint on average curvature, and with prescribed initial and
terminal positions and tangents, American Journal of Mathematics, vol. 79, no. 3, pp. 497516, 1957. [Online].
Available: http://www.jstor.org/stable/2372560
[2] J. A. Reeds and L. A. Shepp, Optimal paths for a car that goes both forwards and backwards, Pacific J. Math., vol.
145, no. 2, pp. 367393, 1990. [Online]. Available: http://projecteuclid.org/getRecord?id=euclid.pjm/1102645450
[3] H. Chitsaz and S. M. LaValle, Time-optimal paths for a Dubins airplane, in Proceedings IEEE Conference Decision and
Control, 2007, to appear.
[4] M. G. Crandall and P.-L. Lions, Viscosity solutions of Hamilton-Jacobi equations, Trans. Amer. Math. Soc., vol. 277,
no. 1, pp. 142, 1983. [Online]. Available: http://dx.doi.org/10.2307/1999343
[5] M. Bardi and I. Capuzzo-Dolcetta, Optimal control and viscosity solutions of Hamilton-Jacobi-Bellman equations, ser.
Systems & Control: Foundations & Applications.

Boston, MA: Birkhauser Boston Inc., 1997, with appendices by

Maurizio Falcone and Pierpaolo Soravia. [Online]. Available: http://dx.doi.org/10.1007/978-0-8176-4755-1

[6] J. Barraquand and J.-C. Latombe, Nonholonomic multibody mobile robots: Controllability and motion planning in the
presence of obstacles, Algorithmica, vol. 10, no. 2, pp. 121155, October 1993.
[7] L. Sonneborn and F. Van Vleck, The bang-bang principle for linear control systems, SIAM J. Control, vol. 2, pp. 151159,
1965.
[8] H. Wang and P. K. Agarwal, Approximation algorithms for curvature-constrained shortest paths, in SODA 96:
Proceedings of the seventh annual ACM-SIAM symposium on Discrete algorithms.

Philadelphia, PA, USA: Society

for Industrial and Applied Mathematics, 1996, pp. 409418.

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

29

[9] P. Jacobs and J. Canny, Planning smooth paths for mobile robots. Norwell, MA: Kluwer Academic, 1992, pp. 271342.
[10] J. H. Reif and H. Wang, Nonuniform discretization for kinodynamic motion planning and its applications, SIAM J.
Comput., vol. 30, no. 1, pp. 161190, 2000.
[11] J. Backer and D. Kirkpatrick, Finding curvature-constrained paths that avoid polygonal obstacles, in SCG 07: Proceedings
of the twenty-third annual symposium on Computational geometry.

New York, NY, USA: ACM, 2007, pp. 6673.

[12] J.-P. Laumond, P. Jacobs, M. Taix, and R. Murray, A motion planner for nonholonomic mobile robots, Robotics and
Automation, IEEE Transactions on, vol. 10, no. 5, pp. 577 593, oct 1994.
[13] P. Konkimalla and S. M. Lavalle, Efficient computation of optimal navigation functions for nonholonomic planning, in
In Proc. First IEEE Intl Workshop on Robot Motion and Control, 1999, pp. 187192.
[14] S. M. LaValle, Planning Algorithms.

Cambridge, U.K.: Cambridge University Press, 2006, also available at

http://planning.cs.uiuc.edu/.
[15] V. S. Patsko and V. L. Turova, From dubins car to reeds and shepps mobile robot, Computational Visul Science, vol. 12,
pp. 345364, 2009.
[16] M. G. Crandall and P.-L. Lions, Two approximations of solutions of hamilton-jacobi equations, Mathematics of
Computation, vol. 43, no. 167, pp. 119, July 1984.
[17] G. Barles and P. E. Souganidis, Convergence of approximation schemes for fully nonlinear second order equations,
Asymptotic Anal., vol. 4, no. 3, pp. 271283, 1991.
[18] A. M. Oberman, Convergent difference schemes for degenerate elliptic and parabolic equations: Hamilton-Jacobi
equations and free boundary problems, SIAM J. Numer. Anal., vol. 44, no. 2, pp. 879895 (electronic), 2006. [Online].
Available: http://dx.doi.org/10.1137/S0036142903435235
[19] J. N. Tsitsiklis, Efficient algorithms for globally optimal trajectories, IEEE Transactions on Automatic Control, vol. 40,
no. 9, pp. 15281538, 1995.
[20] J. A. Sethian, A fast marching level set method for monotonically advancing fronts, in Proc. Nat. Acad. Sci, 1995, pp.
15911595.
[21] E. Rouy and A. Tourin, A viscosity solutions approach to shape-from-shading, SIAM J. Numer. Anal., vol. 29, no. 3, pp.
867884, 1992.
[22] Y.-H. R. Tsai, L.-T. Cheng, S. Osher, and H.-K. Zhao, Fast sweeping algorithms for a class of HamiltonJacobi equations, SIAM J. Numer. Anal., vol. 41, no. 2, pp. 673694 (electronic), 2003. [Online]. Available:
http://dx.doi.org/10.1137/S0036142901396533
[23] H. Zhao, A fast sweeping methog for eikonal equations, Mathematics of Computation, vol. 74, no. 250, pp. 603627,
May 2004.
[24] C.-Y. Kao, S. Osher, and Y.-H. Tsai, Fast sweeping methods for static hamiltonjacobi equations, SIAM J. Numer. Anal.,
vol. 42, no. 6, pp. 26122632, 2004.
[25] C. Y. Kao, S. Osher, and J. Qian, Lax-Friedrichs sweeping scheme for static Hamilton-Jacobi equations, J. Comput.
Phys., vol. 196, no. 1, pp. 367391, 2004. [Online]. Available: http://dx.doi.org/10.1016/j.jcp.2003.11.007
[26] M. Falcone and R. Ferretti, Semi-lagrangian schemes for hamilton-jacobi equations, discrete representation formulae and
godunov methods, J. Comput. Phys., vol. 175, no. 2, pp. 559575, 2002.
[27] C. Hu and C.-W. Shu, A discontinuous galerkin finite element method for hamilton-jacobi equations, SIAM J. Sci.
Comput, vol. 21, pp. 666690.
[28] Y. Cheng and C.-W. Shu, A discontinuous galerkin finite element method for directly solving the hamilton-jacobi

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

30

equations, Journal of Computational Physics, vol. 223, no. 1, pp. 398 415, 2007. [Online]. Available:
http://www.sciencedirect.com/science/article/B6WHY-4M81CC6-2/2/afed0f26e2ebc67e62b569af353fdf65
[29] R. Takei, R. Tsai, H. Shen, and Y. Landa, A practical path-planning algorithm for a vehicle with a constrained turning
radius: a Hamilton-Jacobi approach, in American Control Conference, 2010. ACC 10., 7 2010.
[30] R. Bellman, Dynamic programming, ser. Princeton Landmarks in Mathematics. Princeton, NJ: Princeton University Press,
2010, reprint of the 1957 edition, With a new introduction by Stuart Dreyfus.
[31] R. Takei and R. Tsai, A class of Hamilton-Jacobi equations for computing curvature constrained motion, UCLA CAM
reports 10-67, Tech. Rep., 2010.
[32] H. J. Sussmann and G. Tang, Shortest paths for the reeds-shepp car: A worked out example of the use of geometric
techniques in nonlinear optimal control. SYCON Report 91-10, Tech. Rep., 1991.
[33] Y. Giga and M.-H. Sato, A level set approach to semicontinuous viscosity solutions for Cauchy problems, Comm. Partial
Differential Equations, vol. 26, no. 5-6, pp. 813839, 2001. [Online]. Available: http://dx.doi.org/10.1081/PDE-100002379
[34] Y.-H. R. Tsai, Y. Giga, and S. Osher, A level set approach for computing discontinuous solutions of
Hamilton-Jacobi equations, Math. Comp., vol. 72, no. 241, pp. 159181 (electronic), 2003. [Online]. Available:
http://dx.doi.org/10.1090/S0025-5718-02-01438-2
[35] G.-Q. Chen and B. Su, On global discontinuous solutions of hamilton-jacobi equations, Comptes Rendus Mathematique,
vol. 334, no. 2, pp. 113 118, 2002. [Online]. Available: http://www.sciencedirect.com/science/article/B6X1B-45088J46/2/d37b431fca960977a54e4dff1a0efad2
[36] H. Ishii, Perrons method for Hamilton-Jacobi equations, Duke Math. J., vol. 55, no. 2, pp. 369384, 1987. [Online].
Available: http://dx.doi.org/10.1215/S0012-7094-87-05521-9
[37] R. Gonzalez and E. Rofman, On deterministic control problems: An approximation procedure for the optimal cost i. the
stationary problem, SIAM Journal on Control and Optimization, vol. 23, no. 2, pp. 242266, 1985. [Online]. Available:
http://link.aip.org/link/?SJC/23/242/1
[38] M. Bardi, M. Falcone, and P. Soravia, Fully discrete schemes for the value function of pursuit-evasion games, in Advances
in dynamic games and applications (Geneva, 1992), ser. Ann. Internat. Soc. Dynam. Games.

Boston, MA: Birkhauser

Boston, 1994, vol. 1, pp. 89105.

[39] , Numerical methods for pursuit-evasion games via viscosity solutions, in Stochastic and differential games, ser.
Ann. Internat. Soc. Dynam. Games.

Boston, MA: Birkhauser Boston, 1999, vol. 4, pp. 105175.

[40] A. Kumar and A. Vladimirsky, An efficient method for multiobjective optimal control and optimal control subject to
integral constraints. Journal of Computational Mathematics, vol. 28, no. 4, pp. 517551, 2010.
[41] M. Falcone, Fast marching methods for front propagation, November 2007, lecture notes at Introduction to Numerical
Methods for Moving Boundaries.
[42] R. V. Cowlagi and P. Tsiotras, Existence and synthesis of curvature-bounded paths inside nonuniform rectangular channels,
in Proceedings of the 2010 American Control Conference, Baltimore, MD, USA, 2010.

December 2, 2010

DRAFT

JOURNAL OF ?, VOL. ?, NO. ? ? ?

31

Ryo Takei
PLACE
PHOTO
HERE

Richard Tsai
PLACE
PHOTO
HERE

December 2, 2010

DRAFT