IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 13, NO. 3, JUNE 1997 443
A Path Space Approach to Nonholonomic Motion
Planning in the Presence of Obstacles
Adam W. Divelbiss and John T. Wen
Abstract—This paper presents an algorithm for finding a kinematically
feasible path for a nonholonomic system in the presence of obstacles.
We first consider the path planning problem without obstacles by trans-
forming it into a nonlinear least squares problem in an augmented
space which is then iteratively solved. Obstacle avoidance is included as
inequality constraints. Exterior penalty functions are used to convert the
inequality constraints into equality constraints. Then the same nonlinear
least squares approach is applied. We demonstrate the efficacy of the
approach by solving some challenging problems, including a tractor-
trailer and a tractor with a steerable trailer backing in a loading dock.
These examples demonstrate the performance of the algorithm in the
presence of obstacles and steering and jackknife angle constraints.
Index Terms—Mobile robots, nonholonomic motion planning, obstacle
avoidance, path planning, tractor-trailer.
I. INTRODUCTION
The nonholonomic motion planning (NMP) problem involves
finding a path which links some initial configuration to some de-
sired final configuration for a system with nonholonomic velocity
constraints. Nonholonomic constraints occur in physical systems for
which constraints on the generalized velocity vector are nonintegrable
to equivalent configuration space constraints. As a result of this
condition, the instantaneous velocity of a system under nonholonomic
constraints is limited to certain directions. Constraints of this kind
occur for any wheeled vehicle under the no-slip condition, for any
two bodies in rolling contact, and also for systems in free fall
where the conservation of angular momentum applies. Examples
of these types of nonholonomic systems include mobile robots,
automobiles, tractor-trailer vehicles, finger contacts in robot hand
grasping, orbiting satellites, space-based robot manipulators and even
falling cats. Nonholonomic systems, such as these, possess some
peculiar characteristics which make motion planning for them chal-
lenging. For instance, nonholonomic systems are frequently globally
controllable and yet the linearized system about an equilibrium
point is not controllable. Further, there exists no continuous time-
invariant stabilizing feedback for driftless nonholonomic systems [1],
[2]. Due to these difficulties, standard motion planning techniques
for holonomic systems are not directly applicable to nonholonomic
motion planning.
In the rapidly accumulating literature on nonholonomic motion
planning, there are mainly two classes of approaches. The search
based methods, including those found in [3]–[9], generally involve
Manuscript received January 22, 1996. This work was supported in part
by the National Science Foundation under Grant IRI-9408874 and by the
New York State Center for Advanced Technology (CAT) in Automation,
Robotics and Manufacturing at Rensselaer Polytechnic Institute. The CAT
is partially funded by a block grant from the New York State Science and
Technology Foundation. This paper was recommended for publication by
Associate Editor A. De Luca and Editor S. Salcudean upon evaluation of
the reviewers’ comments.
A. W. Divelbiss is with the Reveo Corporation, Hawthorne, NY 10532
USA.
J. T. Wen is with the New York State Center for Advanced Technology
in Automation, Robotics and Manufacturing, Department of Electrical, Com-
puter, and Systems Engineering, Rensselaer Polytechnic Institute, Troy, NY
12180 USA.
Publisher Item Identifier S 1042-296X(97)03823-8.
some type of decomposition of the configuration space into cells.
A graph is then constructed whose nodes are configurations and
whose arcs are some type of path (shortest, optimal, etc.) connecting
two configurations. The graph is searched using an
A
3
or a similar
algorithm. One of the best characteristics of these approaches is that
they can plan paths in highly cluttered or constrained environments.
Many search based methods are also global in nature, meaning
that they search over the entire configuration space. However, these
methods can be computationally inefficient for complicated vehicles
such as a tractor with two or more trailers. Other methods involve
neural-networks and fuzzy controllers as in [10] but they require
extensive training and tuning. Control theoretic methods such as
[11]–[18], apply the tools in geometric control theory and provide
elegant insights into the structure of the solution. A popular approach
among these methods converts the system into some canonical form
and then uses cyclic motion in certain base space variables to cause a
net motion of the remaining variables [19]. Inequality constraints are
rarely considered, and physically impractical paths frequently result.
In this paper, we present a new approach to NMP. An initial path
is iteratively warped until all constraints are satisfied. Thus it is
a local path space method. The local nature comes from the fact
that convergence depends on the closeness of the initial path to a
feasible solution: one that satisfying all the constraints. The initial
path is itself not required to be feasible. Both equality and inequality
constraints can be included in this formulation. Task space constraints
involving nonlinear functions of the configuration variables are also
allowed. In contrast to other path space approaches, this approach
emphasizes feasibility over optimality. Once a feasible solution is
found, optimality can be incorporated as a secondary objective.
A common starting point for the control theoretic approaches is to
pose the NMP problem as a general nonlinear control problem. Our
formulation is based on converting the nonlinear control problem
into a nonlinear algebraic equation. The problem then becomes a
nonlinear root finding problem in which the dimension of the search
space is very high (infinite if
f
u
(
t
):
t
2
[0
;
1]
g
is taken from an
infinite dimensional functional space) compared to the number of
equality constraints (for the end point constraint). The nonlinear root
finding problem can be solved by many familiar numerical algorithms
including the Newton-Raphson method, the steepest descent method,
the conjugate gradient method, etc. A similar approach has also been
proposed independently in [20]–[22] for kinematic path planning with
only the end point constraint. In all of these algorithms, convergence
relies on a certain path gradient operator being full rank. This full
rank condition has been shown to be true generically in a certain
sense [22]. Coupled with a clever generic loop argument, convergence
of this algorithm class is virtually assured. Along this same line,
a conservative strong bracket generating condition is shown to be
sufficient for the wellposedness of the initial value problem arising
from the Newton-Raphson algorithm [20], [23].
There are typically many possible solutions to the root finding
problem resulting from the path planning problem. Since many of
these solutions will result in physically unrealizable paths, additional
constraints must be enforced on the path for the algorithm to be
useful in any practical situation. To enforce these constrains, we have
adopted an approach similar to the exterior penalty function method
[24], [25] which converts inequality constraints into a zero finding
problem by solving a sequence of unconstrained minimizations. In
contrast to the standard application of this method, there is no need to
successively increase the penalty weights since the minimum values
1042–296X/97$10.00 1997 IEEE
444 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 13, NO. 3, JUNE 1997
(i.e., zeros) of the penalty functions are by definition solutions to
the problem. Our method also differs from the familiar artificial
potential field method (popular in the holonomic path planning
literature [26]–[28]) which is an interior penalty function (or barrier
function) method in that the initial guess may be infeasible. The
exterior penalty functions associated with the inequality constraints
are combined with the equality end point constraint to form an
augmented zero finding problem. Nonconfiguration space constraints
including constraints on the corners of a vehicle, body of the robot,
etc., can also be incorporated in this formulation. For certain convex
polyhedral constraints, we show that the exterior penalty function
does not add extra path singularities. In general, however, there may
be local minima in the path space.
The remainder of this paper is organized as follows. In Section II,
some of the theory behind the basic algorithm for enforcing end
point equality constraints is presented. In Section III, the algorithm
is extended to incorporate inequality constraints on the path by
using the exterior penalty function method. In Section IV, simulations
involving tractor-trailers are presented.
II. NMP S
UBJECT TO EQUALITY CONSTRAINTS
Given a mechanical system with nonholonomic equality con-
straints, an initial configuration and a desired final configuration, we
are interested in finding a path which 1) satisfies the nonholonomic
equality constraint and 2) satisfies the inequality constraints associ-
ated with internal limits of the system, external limits imposed by
the environment, and limits imposed on the control. We assume that
the system is globally controllable, the system kinematic equation
is as smooth as required, and that obstacles and boundaries in the
environment are static. In this section, we first consider the path
planning problem with only the equality constraints.
We shall need the following definitions. A path connecting some
initial configuration, denoted by
x
0
, to some final configuration,
denoted by
x
f
, is defined as a sequence of consecutive system
configurations evenly spaced over a specified normalized time interval
and is denoted by
x
=
f
x
(
t
j
)
2
R
n
:
t
j
2
[0
;
1]
;j
=0
;
1
;
111
;N
g
where
n
is the state dimension,
(
t
j
0
t
j
0
1
)=
h
=1
=
(
N
0
1)
;
x
(
t
0
)=
x
0
, and
x
(
t
N
)=
x
f
. We denote a specific path point,
x
(
t
j
)
2
x
,by
x
j
. The
i
th element of
x
j
is denoted by
x
i
j
. The
continuous form of the path is denoted by
x
=
f
x
(
t
)
2
R
n
:
t
2
[0
;
1]
g
. The control space is defined as
U
L
m
2
[0
;
1]
. The
path space,
P
, is the collection of all paths generated by all possible
controls.
We shall consider the problem of imposing an an equality constraint
on the final point of the path (i.e.,
x
N
=
x
f
) while ensuring the
nonholonomic equality constraint is satisfied at each point along the
path. General nonholonomic velocity equality constraints are often
represented by
G
(
x;
_
x; t
)=0
(1)
where
x
is the generalized coordinate vector and
_
x
is the generalized
velocity vector. The constraints represented by (1) are said to be
nonholonomic if the equation is nonintegrable, i.e., there exists no
function of the form
F
(
x; t
)=0
such that
G
is the differential of
F
. There are also nonholonomic constraints involving accelerations
[29], called second order nonholonomic constraints, which we do not
consider here, but our method can in principle be applied to such
systems as well.
In most physical cases (e.g., the no-slip rolling constraint), (1) is
linear in
_
x
and time-invariant, and may be rewritten as
G
(
x
)_
x
=0
(2)
where
G
is a fat matrix (has more columns than rows).
A. The Basic Algorithm
The nonholonomic constraint (2) can be written in the form
_
x
=
f
(
x
)
u
;
x
(0) =
x
0
(3)
where
x
(
t
)
2
R
n
is the configuration variable,
u
(
t
)
2
R
m
is the
admissible velocity input, and the columns of
f
(
x
)
span the null space
of
G
(
x
)
, i.e.,
G
(
x
)
f
(
x
)=0
m
2
m
. Using this form, the path planning
problem can be posed as a general nonlinear control problem: find an
input,
u
, to drive the system from the initial configuration
x
0
to the
desired final configuration
x
f
. Even though the problem can be stated
in such a deceptively simple form, its solution is very challenging
due in part to the fact tha Furthermore, there exists no time-invariant
stabilizing feedback [1], [2].
Our approach is based on converting (3) to a nonlinear algebraic
equation
x
(1) =
^
F
(
x
0
;u
)
^
F
:
R
n
2
L
m
2
[0
;
1]
!
R
n
(4)
where
x
0
is the initial configuration,
u
f
u
(
t
):
t
2
[0
;
1]
g
, and
where the final time has been normalized to 1. The analytic form of
^
F
is in general difficult to find but will not be explicitly required. In this
perspective, (3) is globally controllable if and only if the nonlinear
mapping
^
F
(
x
0
;:
):
L
m
2
[0
;
1]
!
R
n
is onto for all
x
0
2
R
n
. The
system is locally controllable around
u
if and only if the Fr
´
echet
derivative of
^
F
with respect to
u;
r
u
^
F
(
u
):
L
m
2
[0
;
1]
!
R
n
,isa
linear onto map. Since the mapping,
r
u
^
F
(
x
0
;u
)
relates variations
in
x
(1)
to variations in
u
, it may be thought of as the operator
corresponding to the solution of the variational time-varying linear
system
_
x
=
A
(
t
)
x
+
B
(
t
)
u; x
(0) = 0
(5)
where
A
(
t
) [
@f
@x
(
x
(
t
))
u
(
t
)
111
@f
@x
(
x
(
t
))
u
(
t
)]
and
B
(
t
)
f
(
x
(
t
))
. Since
x
(0) = 0
, the solution to this equation is written as
x
(1) =
r
u
^
F
(
x
0
;u
)
u
1
0
8(1
;s
)
B
(
s
)
u
(
s
)
ds
(6)
where
8
is the state transition matrix of the linearized system and
x
0
is the given initial configuration. Controllability of the system in (5)
is equivalent to
r
u
^
F
(
x
0
;u
)
being onto, or,
rank(
r
u
^
F
(
x
0
;u
)) =
n
.
We now pose the path planning problem as a nonlinear root finding
problem as follows. Given an initial configuration
x
0
, find a control
u
2
L
m
2
[0
;
1]
such that
y
(
x
0
;u
)
^
F
(
x
0
;u
)
0
x
f
=0
:
(7)
Since the dimension of the search space is much higher than the
dimension of the constraint space, there are typically a large number
of solutions, many of which are physically unrealizable. Additional
constraints must be imposed to obtain implementable paths.
Many methods are available for solving the nonlinear root finding
problem for a fixed initial configuration
x
0
, such as Newton-Raphson,
gradient descent, conjugate-gradient, Levenberg-Marquardt, etc. We
have used the Newton-Raphson algorithm successfully. In our imple-
mentation, the control
u
is iteratively updated by
u
(
k
+1)
=
u
(
k
)
0
k
r
u
^
F
x
0
;u
(
k
)
+
y x
0
;u
(
k
)
(8)
where
k
is found through a line search to maximize the decrease
in
y
. A sufficient condition for the algorithm to converge is that the
gradient operator
r
u
^
F
(
x
0
;u
)
be full rank for each iteration. This
condition has been shown to be generic in some sense in [22] and an
explicit characterization of all the singular controls (i.e.,
u
for which
r
u
^
F
(
x
0
;u
)
loses rank) has been found for the
N
-trailer system [30].
IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 13, NO. 3, JUNE 1997 445
B. Fourier Basis Representation
For implementation, the control function
u
in (8) needs to be
approximated by a finite dimensional vector. We have used the
Fourier series expansion
u
(
t
)=
1
i
=1
i
(
t
)
i
8
t
2
[0
;
1]
(9)
where the
i
(
t
)
’s are the standard Fourier basis elements. Substituting
into (4), we get
x
(1) =
F
(
x
0
;
)=
^
F
(
x
0
;T
0
1
)
:
(10)
where we have written
u
=
T
0
1
. If the Newton-Raphson algorithm
is used, the update law for
is
(
k
+1)
=
(
k
)
0
k
r
F x
0
;
(
k
)
+
F x
0
;
(
k
)
(11)
where
r
F
(
x
0
;
)=
r
u
^
F
(
T
0
1
)
T
0
1
.
III. NMP S
UBJECT TO INEQUALITY CONSTRAINTS
Many of the possible solutions to (11) will result in physically
unrealizable motions. In order for the algorithm to be of any practical
use, it must be modified to enforce inequality constraints, such as joint
limits or task space constraints, on intermediate points along the path.
Suppose that a feasible region in the path space
P
is defined by
a set of
p
inequalities
c
(
x
)
0
(12)
where
is interpreted as a component-wise relationship. This con-
dition requires that each point along the path to lie in the feasible
region.
In the previous section, the mapping
F
was defined to relate the
Fourier coefficients of the control,
, to the final configuration. In
the same manner, functions relating each point in
x
to
are defined
as
x
(
t
j
)=
^
F
j
(
T
0
1
)=
F
j
(
)
, for each
j
=1
;
2
;
111
;N
, where
the initial configuration,
x
0
, is assumed given. Stacking each of
these functions into a single vector yields a new function
F
where
x
=
F
(
)
. Substituting into (12) defines a feasible region in the
control coefficient space
c
(
F
(
))
0
:
(13)
To enforce the
p
inequality constraints represented by (13) the in-
equality constraints are converted into equality constraints by defining
an exterior penalty function corresponding to the
i
th constraint as
z
i
(
)=
i
N
j
=1
g
(
c
i
(
F
j
(
)))
(14)
where
i
>
0
;c
i
is the
i
th constraint, and
g
is a continuous scalar
function with the property that
g
is positive and strictly monotonically
increasing for
c>
0
and zero for
c
0
. We have used
g
of the
following form:
g
(
c
)=
(1
0
e
0
rc
)
2
if
c>
0
0
if
c
0
r>
0
:
(15)
Each penalty function
z
i
forces
toward the feasible region when
the constraint is violated and has no effect when the constraint is
satisfied. The composite penalty is written as
z
=[
z
1
;
111
;z
p
]
T
. Note
that only the active constraints (i.e.,
z
i
>
0
) need to be included
z
.
The motion planning problem can now be restated as follows.
Given an initial configuration,
x
0
, find
such that
y
(
)=0
and
z
(
)=0
. The same Newton-Raphson approach can be applied
(
k
+1)
=
(
k
)
0
k
[
G
(
(
k
)
)]
+
(
(
k
)
)
(16)
where
(
)=
y
(
)
z
(
)
;G
(
)
r
F
(
)
r
z
(
)
=
r
(
)
:
(17)
Again, a sufficient condition for convergence is that
G
be full rank for
each iteration. To consider this issue further, we first decompose
G
as
G
(
)=
K
(
)
D
(
)
where
K
(
)
is the gradient of the cost vector
with respect to the path
x
(i.e.,
K
(
)
r
x
(
)
) and
D
(
)
is the
gradient of the path
x
=
F
(
)
with respect to the Fourier coefficient
vector
(i.e.,
D
(
)
r
F
(
)
). By taking the gradient of (14) and
putting it into matrix form,
K
and
D
can be found as
K
(
)
0
n;n
111
0
n;n
I
n
1
k
11
(
)
111
1
k
1
N
0
1
(
)
1
k
1
N
(
)
.
.
.
p
k
p
1
(
)
111
p
k
pN
0
1
(
)
p
k
pN
(
)
D
(
)
D
1
(
)
.
.
.
D
N
0
1
(
)
D
N
(
)
:
(18)
where
k
ij
(
)
g
0
(
c
i
(
F
j
(
)))[
r
x
c
i
(
F
j
(
))]
T
D
j
(
)
r
F
j
(
)
:
(19)
The augmented gradient
G
is full rank if 1)
D
is of full row rank
and 2)
K
T
(
)
(
)=0
if and only if
(
)=0
.
First consider
D
. A necessary condition for
D
to be of full row
rank is that it be fat. This condition means that there must be a
sufficiently number of Fourier basis elements
M
, i.e.,
M
N
1
n
.
We further note that the system is causal (future input does not
affect past state), therefore,
D
(
)
T
(where
T
maps
u
to
) is block
lower triangular. Hence,
D
(
)
T
is of full row rank if and only if
each of its diagonal blocks (corresponding to each constrained path
point) is of full row rank. This condition is equivalent to the time-
varying linearized system between any two consecutive path points
being controllable. The same genericity result and characterization
of singular control for the full path (as mentioned in the previous
section) can also be applied to these path segments.
Now consider
K
T
=0
. From (14) and (19), the
j
th equality,
1
j
N
0
1
in this equation can be written as
0=
p
i
=1
i
k
T
ij
z
i
=
p
i
=1
i
g
0
ij
r
T
x
c
ij
z
i
(20)
where
g
0
ij
and
c
ij
denote
g
0
(
c
i
(
x
j
))
and
c
i
(
x
j
)
, respectively, and
p
0
constraints are assumed to be active. This can be written in a more
compact form
(
r
x
c
(
x
j
))
T
w
(
x
j
)=0
(21)
where the
i
th element of
w
(
x
j
)
is
w
i
(
x
j
)=
i
g
0
ij
z
i
. If the worst
case simultaneous active constraints satisfy
N
(
r
x
c
T
)=
f
0
g
(
N
denotes the null space), then
w
(
x
j
)=0
;j
=1
;
111
;N
0
1
, which in
turn implies
z
i
=0
. A necessary condition for
N
(
r
x
c
)
T
=
f
0
g
is
that
n
p
0
. For example, suppose that each state is bounded above
and below. Even though there are
2
1
n
constraints
(
p
=2
1
n
)
, there
can be at most
n
simultaneous constraints.
The
N
th equality in
K
T
=0
can be written in a similar fashion
(
x
N
0
x
f
)+
p
i
=1
i
g
0
iN
r
T
x
c
iN
z
i
=0
:
(22)
If
z
i
=0
for
i
=1
;
111
;N
, then
x
N
=
x
f
, and
=0
as required.
The only remaining case is that only the path end point constraint is
446 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 13, NO. 3, JUNE 1997
Fig. 1. Triple tractor-trailer model.
active. In this case,
K
T
=0
reduces to (22). This condition may be
rare, but we can only conclusively eliminate it in certain polyhedral
constraint cases. Consider the case where
c
(
x
)=
Ax
+
b
0
:
(23)
Equation (22) then becomes
(
x
N
0
x
f
)+
A
T
w
(
x
N
)=0
(24)
where
A
only needs to include the worst case simultaneous con-
straints. Suppose
A
is of full row rank (implying that the maximum
number of simultaneous active constraints is less than the number of
states), then
Ax
f
+
b
=
Ax
N
+
b
+
AA
T
w
(
x
N
)
:
(25)
Suppose
x
N
violates only the
i
th constraint in (23) (this can be easily
generalized to
x
N
violating multiple constraints), i.e.,
e
T
i
(
Ax
N
+
b
)
>
0
,
w
i
(
x
N
)
>
0
, and
w
(
x
N
)=
e
i
w
i
(
x
N
)
where
e
i
is the
i
th unit
vector. Then
e
T
i
Ax
f
+
b
=
e
T
i
(
Ax
N
+
b
)+
e
T
i
AA
T
w
(
x
N
)
>
0+
e
T
i
AA
T
e
i
w
i
(
x
N
)
>
0
:
This condition implies that
x
f
violates the constraints which contra-
dicts the assumption. Therefore,
K
T
=0
implies that
=0
.
Inequality constraints may not always be directly expressed in
terms of the state variable. For example, collision avoidance may
be required for the boundary points of a vehicle. These constraints
can be converted to inequalities in the path space as in (12) through
the vehicle kinematics.
In many situations, analytic representation of the constraint may
be tedious or not possible. The penalty function can be directly
constructed based on a contour map from the task space obstacle
boundaries. The contour map assigns a cost and a gradient direction
for each point in a two-dimensional grid. Linear interpolation of the
four closest map grid points is used to compute the cost and gradient
for any point in the map, which are in turn used in the computation
of the exterior penalty function and its gradient.
IV. M
ODELS AND SIMULATIONS
In this section, we apply the path planning algorithm described in
the previous sections to several different types of articulated vehicles
to demonstrate its versatility and usefulness in planning paths under
a wide variety of constraints. To begin, the kinematic models for a
general tractor-trailer vehicle with
n
trailers and a general tractor-
trailer vehicle with
n
steerable trailers are presented. Derivations for
the models are found in the [31].
Fig. 2. Steerable tractor-trailer model.
A. Kinematic Models
There have been several multibody vehicle models proposed in
[32] and [33]. These models are derived with the trailer pivot points
located at the center of the rear axle of the preceding body. This
situation is almost never the case for real tractor-trailer systems. For
instance, a car with a boat trailer has the trailer attached to a point
behind the rear axle on the rear bumper. An example of a tractor-
trailer vehicle where the pivot point is actually in front of the rear
axle is a heavy pickup truck towing a gooseneck trailer. A gooseneck
trailer has its central beam bent up and over the tailgate of the truck
and attached to a ball joint in the middle of the bed. The pivot point
is placed slightly forward of the rear axle to help stabilize the vehicle
at highway speeds.
In our examples, we consider a kinematic model for multiple
tractor-trailer vehicles which have attachment points offset from the
rear axle of the preceding body; see Fig. 1 for an example of a tractor
with three trailers. The kinematic model for this vehicle has been
derived in [31]
_
x
1
=
c
3
c
4
u
1
_
x
2
=
c
3
s
4
u
1
_
x
3
=
u
2
_
x
4
=
1
l
1
s
3
u
1
_
x
5
=
f
5
(
x
)
u
1
=
1
l
2
c
3
s
4
0
d
12
l
1
s
3
c
4
c
5
0
c
3
c
4
+
d
12
l
1
s
3
s
4
s
5
u
1
.
.
.
_
x
n
=
f
n
(
x
)
u
1
n
=6
;
7
;
111
(26)
f
n
(
x
)
=
c
3
s
4
0
d
12
l
1
s
3
c
4
0
n
0
1
i
=5
(
l
i
0
3
+
d
i
0
3
;i
0
2
)
c
i
f
i
(
x
)
c
n
0
c
3
c
4
+
d
12
l
1
s
3
s
4
+
n
0
1
i
=5
(
l
i
0
3
+
d
i
0
3
;i
0
2
)
s
i
f
i
(
x
)
s
n
l
n
0
3
where
u
1
and
u
2
denote the driving and steering velocities,
l
i
0
3
+
d
i
0
3
;i
0
2
is the total length of the
(
i
0
4)
th trailer,
c
i
and
s
i
denote
cos(
x
i
)
and
sin(
x
i
)
, respectively. Notice that even though Fig. 1
is drawn with the pivot points behind the axles,
d
’s can be chosen
negative, causing the pivot to be in front of the axle as in the case
of the gooseneck trailer.
Consider now a chain of
n
steerable trailers pulled by the front
wheel drive tractor; see Fig. 2 for an example of a tractor pulling a
single steerable trailer. The kinematic equations for the front wheel
drive tractor are exactly the same as in the previous case. However
in the steerable trailer case, there is both an orientation and a steering
angle associated with each trailer. By letting
x
j
denote the orientation
IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 13, NO. 3, JUNE 1997 447
Fig. 3. Tractor-trailer initial guess path.
and
x
k
denote the steering angle, the kinematic model for the
i
th
steerable trailer can be written in the following form:
_
x
j
=
c
3
s
4
0
d
12
l
1
s
3
c
4
0
i
0
1
p
=1
(
l
p
+1
+
d
p
+1
;p
+2
)
c
j
f
j
(
x
)
2
c
jk
c
k
0
c
3
c
4
+
d
12
l
1
s
3
s
4
+
i
0
1
p
=1
(
l
p
+1
+
d
p
+1
;p
+2
)
s
j
f
j
(
x
)
s
jk
c
k
u
1
l
i
+1
_
x
k
=
u
i
+2
where
j
i
=2
i
+3
and
k
=2
i
+4
are the state indices for the
i
th trailer,
l
p
+1
is the length of the
p
th trailer,
d
p
+1
;p
+2
is the
distance between the
p
th trailer rear axle and the
(
p
+1)
th trailer
pivot point. Note that the model for steerable trailers has a singularity
at
x
k
=
6
=
2
. The physical significance of this singularity is that
when the steering wheels of a trailer are pointing perpendicular to the
central axis of the trailer it is not possible for the vehicle to move.
Because of this fact, care must be taken in motion planning to ensure
that the trailer steering wheels do not approach the singularity.
B. Simulation Results
To evaluate the usefulness of our path planning algorithm, we have
applied it to a wide variety of wheeled vehicles in various situations.
The examples range from the simplest nonholonomic system, a planar
unicycle, to complicated vehicles such as a tractor with two trailers
and a tractor with a steerable trailer. Examples presented in this
section are intended to demonstrate the algorithm’s versatility and
its efficiency in planning paths which are challenging for average
human drivers. The simulations presented in this section involve
enforcing both convex and nonconvex constraints on configuration
and nonconfiguration variables. The algorithm was implemented in
C
++
on an IBM compatible 80486 33 MHz computer. In each of the
following examples, the number of path points used (for constraint
checking) is 101 unless otherwise specified. The absolute path error is
calculated as the maximum excursion of the vehicle into the infeasible
region over the entire path. The stopping criterion is chosen to be
0.01.
Tractor-Trailer: We present three cases for a tractor-trailer back-
ing up into a loading dock. The first is totally unconstrained. The
second case enforces the steering and jackknife angle constraints.
The third case adds the no-collision constraint. The vehicle in this
example consists of a front wheel drive car with a trailer attached to
Fig. 4. Tractor-trailer unconstrained docking motion and steering angle.
the rear bumper. The car has a wheel base of 26.5 in, is 48 in long,
and 22 in wide. The trailer is 39 in long from pivot to rear axle, and
is 22 in wide. The goal is to perform a docking motion in which the
car must back the trailer into a 34 in wide loading bay. The kinematic
model used for each of the three simulations is
_
x
1
=
c
3
c
4
u
1
_
x
2
=
c
3
s
4
u
1
_
x
3
=
u
2
_
x
4
=0
:
03774
s
3
u
1
_
x
5
=0
:
0256[(
c
3
s
4
0
0
:
4622
s
3
c
4
)
c
5
0
(
c
3
c
4
+0
:
4622
s
3
s
4
)
s
5
]
u
1
(27)
where
x
5
denotes the absolute orientation of the trailer. In the
following discussions, the jackknife angle is defined as the difference
between the car orientation and the trailer orientation.
In the first case, the initial guess simply involves the vehicle drives
forward, backward, and then forward and is shown in Fig. 3. The
number of Fourier basis elements used is 41. In computing the path,
the algorithm took seven iterations with an average iteration time of
18.06 s. Fig. 4 shows the results. Notice that the steering angles go
through some very unrealistically large values. This fact demonstrates
the need to enforce inequality constraints in the algorithm.
In the second case, the initial guess of Fig. 3 is again used. The
steering angle is now limited to
6
30
from center and the jackknife
angle is limited to
6
60
from center. Under these conditions, the
algorithm took six iterations to converge to an absolute error tolerance
of 0.01 with the average iteration taking 17.71 s. Fig. 5 shows the
