STREAM FUNCTION PATH PLANNING AND CONTROL
FOR UNMANNED GROUND VEHICLES
Except where reference is made to the work of others, the work described in this
dissertation is my own or was done in collaboration with my advisory committee. This
dissertation does not include proprietary or classified information.
_______________________________
Robert L. Daily
Certificate of Approval:
______________________________
Nels Madsen
Professor
Mechanical Engineering
______________________________
David M. Bevly, Chair
Associate Professor
Mechanical Engineering
______________________________
John Y. Hung
Professor
Electrical and Computer Engineering
______________________________
George T. Flowers
Interim Dean
Graduate School
STREAM FUNCTION PATH PLANNING AND CONTROL
FOR UNMANNED GROUND VEHICLES
Robert L. Daily
A Dissertation
Submitted to
the Graduate Faculty of
Auburn University
in Partial Fulfillment of the
Requirements for the
Degree of
Doctor of Philosophy
Auburn, Alabama
August 9, 2008
iii
STREAM FUNCTION PATH PLANNING AND CONTROL
FOR UNMANNED GROUND VEHICLES
Robert L. Daily
Permission is granted to Auburn University to make copies of this dissertation at its
discretion, upon request of individuals or institutions and at their expense. The author
reserves all publication rights.
__________________________________
Signature of Author
__________________________________
Date of Graduation
iv
DISSERTATION ABSTRACT
STREAM FUNCTION PATH PLANNING AND CONTROL
FOR UNMANNED GROUND VEHICLES
Robert L. Daily
Doctor of Philosophy, August 9, 2008
(M.S., Auburn University, 2005)
(B.S., Auburn University, 1999)
188 Typed Pages
Directed by David M. Bevly
This research develops a harmonic potential field based path planner and controller.
The first component of the dissertation compares several analytical and numerical
potential field generation methods. To overcome limitations present in the existing
methods, two new methods are developed to meet the specific requirements of this
research (generic shape obstacles and an explicit stream function). In particular, an
analytic method to combine circles and create generic shaped obstacles is presented.
Additionally, a numeric technique to directly calculate a potential field stream function is
developed.
v
The second part of this research extends the traditional potential field controller to
track a desired streamline as well as the potential field gradient. In addition to tracking a
streamline, the reference path is also modified to maximize safety as the vehicle is
driving. In particular, a separate harmonic potential field is created for the desired speed.
This speed is low near obstacles and high in the open field. The lateral acceleration of
the vehicle is also limited by reducing the steer angle and desired speed whenever an
acceleration threshold is crossed. Finally, to create a buffer between the vehicle and
obstacles, whenever the vehicle is close to an obstacle the desired streamline is shifted
away from the obstacles. Together these three real-time modifications to the controller
keep the vehicle safely away from obstacles and within its handling limitations.
vi
ACKNOWLEDGEMENTS
The author would like to thank his entire family for their support throughout his
school career. Without their encouragement, this dream would have died long ago. He
also wishes to express his gratitude to each of the members of his advisory committee,
both the members present at the start of the process and the ones present for its
conclusion. Their support leading up to and during the creation of this dissertation has
been priceless. In particular, the committee chair, Dr. David Bevly, has provided advice,
encouragement, and perhaps most importantly the opportunity to work on incredible
research projects. A great research lab has been built during the author?s time working
for Dr. Bevly. Although their signatures are not on this dissertation for unforeseeable
reasons, Dr. A. S. Hodel, and Dr. Subhash Sinha provided valuable insight into the
subjects presented in this dissertation both in and out of class. In addition, the author
would like to thank the many other professors he has worked for or studied under during
his career at Auburn University. This dissertation is the culmination not only of the
presented research, but also of many years of education. He would also like to thank Ms.
Susan Wooden for her encouragement during this process and for introducing him to this
field. Finally, the author would like to thank the past and present members of the GPS
and Vehicle Dynamics Lab at Auburn University. Their help has been invaluable in
tackling the wide range of challenges that developed throughout his career in the lab.
vii
Style manual or journal used: Journal of Dynamic Systems, Measurement, and
Control.
Computer software used: Microsoft Word 2007.
viii
TABLE OF CONTENTS
LIST OF TABLES .................................................................................................................. xi
LIST OF FIGURES ................................................................................................................ xii
NOMENCLATURE ............................................................................................................. xviii
1 INTRODUCTION .............................................................................................................. 1
1.1 Prior Work ............................................................................................................2
1.2 Contributions.........................................................................................................6
2 BACKGROUND ................................................................................................................ 8
2.1 Potential Flow .......................................................................................................8
2.1.1 Analytical Solutions ...................................................................................11
2.1.2 Numerical Solutions...................................................................................20
2.2 Vehicle Model .....................................................................................................26
3 OBSTACLE REPRESENTATION ....................................................................................... 32
3.1 Analytical Solutions ............................................................................................33
3.1.1 Small Sources in Flow ...............................................................................35
3.1.2 Circle Theorem Method .............................................................................39
3.1.3 Flow Velocity Average Method.................................................................45
ix
3.1.4 Flow Potential Average..............................................................................52
3.2 Numerical Solutions............................................................................................68
3.2.1 Dirichlet Boundary Condition....................................................................68
3.2.2 Neumann Boundary Condition ..................................................................77
3.2.3 Stream Function Boundary Condition .......................................................83
4 CONTROLLER DEVELOPMENT ...................................................................................... 92
4.1 Bicycle Model Vehicle Controller ......................................................................92
4.1.1 Uncontrollable Speed .................................................................................94
4.1.2 Constant Closed-Loop Pole Controller ....................................................100
4.1.3 LQR Design .............................................................................................106
4.2 Lateral Control to Reference Circle ..................................................................110
4.2.1 Reference Circle Calculation ...................................................................117
4.2.2 Controller Response .................................................................................121
5 PATH MODIFICATION ................................................................................................. 131
5.1 Reference Speed................................................................................................131
5.2 Reference Streamline Shifting ..........................................................................142
6 CONCLUSIONS ............................................................................................................ 154
6.1 Overall Results ..................................................................................................154
6.2 Future Work ......................................................................................................158
x
6.2.1 Potential Field ..........................................................................................158
6.2.2 Vehicle Controller ....................................................................................161
BIBLIOGRAPHY ................................................................................................................ 165
xi
LIST OF TABLES
Table 2.1 Lateral vehicle model parameters ..................................................................... 28
Table 2.2 Closed-loop speed model parameters ............................................................... 30
Table 3.1 Potential field method calculation times ........................................................... 90
xii
LIST OF FIGURES
Figure 2.1 Uniform flow complex potential ..................................................................... 12
Figure 2.2 Source flow complex potential ........................................................................ 13
Figure 2.3 Vortex flow complex potential ........................................................................ 15
Figure 2.4 Circular obstacle in uniform flow.................................................................... 16
Figure 2.5 Circle Theorem coordinate transformation ..................................................... 18
Figure 2.6 Circular obstacle in source/sink flow .............................................................. 19
Figure 2.7 Four wheel vehicle model schematic .............................................................. 27
Figure 3.1 Source and sink potential................................................................................. 34
Figure 3.2 Source in uniform flow .................................................................................... 36
Figure 3.3 Source in source/sink flow .............................................................................. 38
Figure 3.4 Two vertical, separated circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 40
Figure 3.5 Two vertical, touching circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 41
Figure 3.6 Five vertical, touching circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 42
Figure 3.7 Two horizontal, separated circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 43
xiii
Figure 3.8 Two horizontal, touching circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 44
Figure 3.9 Five horizontal, touching circles in uniform flow calculated using the Circle
Theorem method ............................................................................................................... 45
Figure 3.10 Two vertical, separated circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 48
Figure 3.11 Two vertical, touching circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 49
Figure 3.12 Five vertical, touching circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 50
Figure 3.13 Two horizontal, separated circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 51
Figure 3.14 Two horizontal, touching circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 51
Figure 3.15 Five horizontal, touching circles in uniform flow calculated using the Flow
Velocity Average method ................................................................................................. 52
Figure 3.16 Two vertical, separated circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 54
Figure 3.17 Two vertical, touching circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 56
Figure 3.18 Five vertical, touching circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 57
xiv
Figure 3.19 Two horizontal, separated circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 58
Figure 3.20 Two horizontal, touching circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 58
Figure 3.21 Five horizontal, touching circles in uniform flow calculated using the Flow
Potential Average method ................................................................................................. 59
Figure 3.22 Flow Potential Average method, vertical/horizontal obstacle transition....... 61
Figure 3.23 Square obstacle formed of four touching circles calculated using the Flow
Potential Average method ................................................................................................. 63
Figure 3.24 Square obstacle formed of eight touching circles calculated using the Flow
Potential Average method ................................................................................................. 64
Figure 3.25 Square obstacle formed of four separated circles calculated using the Flow
Potential Average method ................................................................................................. 65
Figure 3.26 Averaging stream function, sample map ....................................................... 67
Figure 3.27 Start and goal potential obstacle potential field, using Dirichlet boundary
conditions .......................................................................................................................... 70
Figure 3.28 Start, goal, and zero potential obstacle potential obstacle potential field, using
Dirichlet boundary conditions ........................................................................................... 72
Figure 3.29 Start, goal, and positive potential obstacle potential field, using Dirichlet
boundary conditions .......................................................................................................... 73
Figure 3.30 Start, goal, and three obstacles potential field, using Dirichlet boundary
conditions .......................................................................................................................... 74
xv
Figure 3.31 Goal and three obstacles potential field, using Dirichlet boundary conditions
........................................................................................................................................... 76
Figure 3.32 Start and goal potential field, using Neumann boundary conditions............. 78
Figure 3.33 Start, goal, and obstacle potential field, using Neumann boundary conditions
........................................................................................................................................... 79
Figure 3.34 Start, goal, and three obstacles potential field, using Neumann boundary
conditions .......................................................................................................................... 80
Figure 3.35 Start and goal boundary values for the stream function boundary condition 85
Figure 3.36 Start and goal potential field, using the stream function boundary condition 87
Figure 3.37 Start, goal, and obstacle potential field, using the stream function boundary
condition ........................................................................................................................... 88
Figure 3.38 Start, goal, and three obstacles potential field, using the stream function
boundary condition ........................................................................................................... 89
Figure 4.1 Bicycle model pole characteristics .................................................................. 94
Figure 4.2 Bicycle model real poles and zeros ................................................................. 95
Figure 4.3 Bicycle model real/complex transition speed .................................................. 98
Figure 4.4 Norm of inverse of bicycle model controllability gramian (Vcrit = 5.8 m/s) ... 99
Figure 4.5 Bicycle model control gains: constant closed-loop poles (Vcrit = 5.8 m/s) .... 101
Figure 4.6 Bicycle model response: constant closed-loop poles, varying speed ............ 103
Figure 4.7 Bicycle model response: constant closed-loop poles, constant speed ........... 104
Figure 4.8 Bicycle model response: constant closed-loop poles, with noise .................. 105
Figure 4.9 Bicycle model control gains using LQR design (Vcrit = 5.8 m/s) .................. 107
Figure 4.10 Bicycle model and LQR closed-loop real poles and zeros .......................... 108
xvi
Figure 4.11 Bicycle model response: LQR, varying speed ............................................ 109
Figure 4.12 Potential field gradient control .................................................................... 112
Figure 4.13 Reference circle additional control states .................................................... 114
Figure 4.14 Radius calculation validation ...................................................................... 120
Figure 4.15 Trajectory of the circle controller validation, with a linear vehicle model . 121
Figure 4.16 States of the circle controller validation, with a linear vehicle model ........ 123
Figure 4.17 Trajectory of the circle controller validation, with a nonlinear vehicle model
......................................................................................................................................... 124
Figure 4.18 States of the circle controller validation, with a nonlinear vehicle model .. 125
Figure 4.19 Trajectory of the streamline controller response ......................................... 126
Figure 4.20 States of the streamline controller response ................................................ 128
Figure 4.21 Tire curves from the streamline controller response ................................... 129
Figure 5.1 Reference speed potential using Dirichlet boundary conditions ................... 133
Figure 5.2 Trajectory of the streamline controller response, adjusting speed ................ 134
Figure 5.3 States of the streamline controller response, adjusting speed ....................... 136
Figure 5.4 Tire curves from the streamline controller response, adjusting speed .......... 138
Figure 5.5 Trajectory of the streamline controller response, limiting lateral acceleration
......................................................................................................................................... 140
Figure 5.6 Tire curves from the streamline controller response, limiting lateral
acceleration ..................................................................................................................... 140
Figure 5.7 States of the streamline controller response, limiting lateral acceleration .... 141
Figure 5.8 Reference speed gradient ............................................................................... 143
Figure 5.9 Reference streamline shift near an obstacle .................................................. 145
xvii
Figure 5.10 Trajectory of the streamline controller response, shifting the reference
streamline ........................................................................................................................ 147
Figure 5.11 States of the streamline controller response, shifting the reference streamline
......................................................................................................................................... 149
Figure 5.12 Streamline controller response with a large streamline shift gain ............... 151
Figure 5.13 Streamline controller response for a dense obstacle field ........................... 152
xviii
NOMENCLATURE
? ......................................................................................................... vehicle side slip angle
? ................................................................................................................ vehicle steer angle
? ..................................................................................................................... vehicle course
? .................................................................................................................... stream function
? ................................................................................................................ velocity potential
?V ................................................................................................... reference speed potential
? ................................................................................................................... vehicle heading
C??T .................................................................................................... tire cornering stiffness
C?? .................................................................................................... axle cornering stiffness
E ........................................................................................................................ East position
Fymax? ........................................................................................... maximum lateral tire force
Iz ........................................................................................... vehicle yaw moment of inertia
N ..................................................................................................................... North position
R .......................................................................................................................... Turn radius
T .............................................................................................................. vehicle track width
xix
V ....................................................................................................................... vehicle speed
VE ............................................................................................... Potential field east velocity
VN ............................................................................................ Potential field north velocity
a................................................................................ distance from front axle to vehicle CG
ay ................................................................................................ lateral vehicle acceleration
b................................................................................. distance from rear axle to vehicle CG
m ........................................................................................................................ vehicle mass
r ................................................................................................................... vehicle yaw rate
yerr .......................................................................................................... vehicle lateral error
1
1 INTRODUCTION
Potential fields are a method of path planning and control for directing a robot
through an obstacle field to a goal location. To accomplish this, an artificial potential
field is created over the range of the world map. Based on this potential field, a
controller is developed for the robot to reach the goal, typically by following the gradient
of the potential field. Potential fields offer an alternative to the traditionally independent
operations of path planning and vehicle control. Typical path planners calculate a
collision free route from a vehicle?s current location to a goal. This route is represented
(either directly or after a translation step) as a reference trajectory, e.g. a series of
waypoints, lines and arcs, or clothoids. The reference trajectory is passed to the vehicle
controller, which drives the vehicle onto the path. Usually, information other than the
reference trajectory is not passed to the controller. An example of this method of path
planning and control was represented in Auburn University?s entries in the DARPA
Grand Challenge series [Daily 2006]. Potential field techniques differ from traditional
path planning and control methods in that a potential field over the entire world map is
created and available to the controller. The potential field is directly used to compute the
control effort. Because the potential field is global, the controller has more information
about the vehicle?s environment than simply a reference trajectory.
2
Harmonic potential fields improve on the traditional potential field concept. In
particular, early potential fields suffered from local minima into which the controlled
robot could become stuck. Harmonic potential fields are also used by fluid dynamicists
to model ideal fluid flow. Several methods have been developed to study flow around
obstacle fields. These same methods can be adapted into paths for a vehicle to follow.
This research utilizes the stream function component of the potential field to control the
vehicle. Contours of the stream function are streamlines, which are the paths the fluid or
vehicle should follow. Tracking the streamlines is the basis for the vehicle navigation
controller developed in this work.
This dissertation is divided into six chapters. The Introduction presents prior and
ongoing research in the potential field control area as well as the specific contributions of
this research. Chapter 2 discusses background information about potential field theory,
specifically with respect to the harmonic potential fields used in this research as well as
the vehicle model used in the simulation studies. Chapter 3 examines different methods
of computing the potential fields and develops new potential field generation techniques.
Chapter 4 develops the streamline controller and discusses controllability issues related to
the vehicle model. Chapter 5 modifies the streamline controller to improve safety by
incorporating vehicle dynamic limitations. Finally, the Conclusion (Chapter 6)
summarizes the work and provides avenues for continuing this research.
1.1 Prior Work
Khatib first proposed the concept of potential field control [Khatib 1985]. His
research was directed toward robotic manipulators as well as mobile robots. The idea
3
behind the potential field was, ?The manipulator moves in a field of forces. The position
to be reached is an attractive pole for the end effector and obstacles are repulsive surfaces
for the manipulator parts.? [Khatib 1985] The attractive and repulsive potentials were
simple functions based on the distance to either the goal or an obstacle. The total
potential was the sum of the individual goal and obstacle potentials. The control effort
applied to the robot was the gradient of the potential field (magnitude and direction),
along with dissipative terms to ensure asymptotic stability. This potential field controller
provided a simple method to reach a goal while avoiding obstacles. However, it suffered
from the fact that local minima were present in the potential field, which could cause the
robot to be attracted to the local minima instead of the goal location. Several attempts
were made to correct the local minima problem. For example, Kholsa and Volpe
modified the repulsive potential such that the contours tended to circles as distance from
the obstacles increased [Kholsa 1988].
Another approach to eliminate the local minima was to generate the potential field by
solving the Laplace equation, i.e. harmonic functions. One method to generate harmonic
potential fields is to calculate analytic solutions to the Laplace equation. Analytic
harmonic functions are typically modeled to represent fluid flow, i.e. hydrodynamic
potentials. Akishita, Kawamura, and Hayashi were the first to propose analytic solutions
to the Laplace equation for potential field control [Akishita 1990]. Obstacles were
modeled as doublets (defined in Section 2.1.1) inserted into the flow. Kim and Khosla
studied using distributed sources as opposed to the traditional point source to
approximate polygon shaped obstacles [Kim 1992]. More recently Waydo and Murray
inserted circular obstacles into analytic potential fields using the Circle Theorem [Waydo
4
2003]. Their research also examined the effects of multiple obstacles in the potential
field.
Sato and Connolly, Burns, and Weiss independently pioneered the use of numerical
solutions to the Laplace equation, specifically using Dirichlet boundary conditions
[Connolly 1990, Sato 1993]. Dirichlet boundary conditions approximate Khatib?s
potential, i.e. the robot is repelled from an obstacle. However, as Section 3.2.1 describes,
they may not be the most effective boundary condition. Keymeulen and Decuyper
studied different types of boundary conditions, in particular Dirichlet and Neumann
boundary conditions [Keymeulen 1994]. Specifically, they studied how the number of
singular points in the potential field varies with the boundary conditions. Connolly and
Grupen extended the boundary conditions to combining both Dirichlet and Neumann
boundary conditions in a single potential field [Connolly 1993].
In addition to the fluid dynamics analogy, other physical phenomena have also been
studied as foundations for the potential field. For example Masoud, Masoud, and
Bayoumi employed a stress analogy utilizing biharmonic functions for the potential fields
[Masoud 1994]. Biharmonic potential fields do not have the large streamline curvature
that may occur in harmonic potential fields. A heat flow analogy has also been proposed
[Wang 2000]. In the heat flow potential, the robot follows the heat flux. Finally,
Guldner and Utkin utilized an electrostatic potential field [Guldner 1995]. Electrostatic
potentials are essentially hydrodynamic potentials consisting of only sources and sinks
(described in Sections 2.1.1 and 3.1.1).
The research into potential field control has not only concentrated on the potential
field development. Efforts have also been placed on incorporating potential field
5
techniques into a wider range of control schemes. One area of interest is incorporating
moving obstacles into the potential fields [Ge 2002, Newman 1987]. Analytic harmonic
functions can also be modified to allow moving obstacles, [Akishita 1990, Waydo 2003].
Another area of interest is adapting potential field methods for cooperative and multiple
robot control [Song 2002, Sullivan 2003]. The potential field method has also been
modified to deflect a virtual beam as opposed to controlling the robot directly [Hesse
2007, Quinlan 1993]. The robot then follows the beam as a path. Potential field methods
have also been applied to underwater vehicles [Eichhorn 2004].
Potential field techniques have also been adapted to drive full size vehicles as
opposed to small mobile robots. In particular, Kyriakopoulos, Kakambouras, and,
Krikelis considered control of nonholonomic vehicles [Kyriakopoulos 1995]. This is
significant because they began to appreciate the fact that most vehicles cannot react
instantaneously to follow the gradient of the potential field. Accounting for vehicle
dynamics is a critical aspect of the research presented in this dissertation, although the
approach uses a streamline tracking controller instead of the cascaded
holonomic/nonholonomic controllers employed by Kyriakopoulos. Rossetter and Gerdes
applied potential field control to automobile lane keeping [Rossetter 2003]. Their
research did not use traditional goal/obstacle potential fields. Instead, a parabolic
potential centered on the lane was created. Finally, off road navigation of full size
vehicles has also been studied [Shimoda 2005]. This research also did not use traditional
potential fields. The potential field was defined over a trajectory space consisting of steer
angle and speed axes. It was created by summing individual potential fields
corresponding to obstacles, vehicle constraints, and speed. The vehicle control
6
calculation was also non-traditional for potential fields. The steer angle and speed
applied to the vehicle were the location in the trajectory space where the potential field
was minimized. This results in the potential field effectively serving as a cost function
for the steer angle and speed commands.
1.2 Contributions
This research advances the body of knowledge in two broad areas: potential field
generation and controlling a vehicle to a streamline. First, different analytical and
numerical methods of generating potential fields are compared. The analytic
comparisons concentrate on various methods to represent generic shaped obstacles.
Specifically the flow resulting from using a combination of circles to approximate an
obstacle is studied. The numeric comparisons investigate how different boundary
conditions along the obstacle and world edge affect the flow. While these potential field
generation techniques are not novel, an in depth study comparing analytic and numeric
solutions has not been previously presented. Additionally, to overcome the shortcomings
of the existing techniques, two new potential field generation techniques, one analytical
and one numerical, are proposed and demonstrated.
Second, the traditional potential field controller is modified to account for the
dynamics of high speed front wheel steered (as opposed to skid steered) vehicles.
Developing this controller involves several new techniques. Specifically, for the vehicle
controller, an issue with the model becoming uncontrollable at a critical speed is exposed
and a method to counter the problem is proposed. The potential field tracking system is
also augmented to follow a reference streamline in addition to the typical potential field
7
gradient. This prohibits tracking errors (due to system dynamics) from compounding
when the traditional potential field gradient controller is used. The streamline tracking
controller is then augmented to account for the specific dynamics of ground vehicles. In
particular, the reference speed and lateral acceleration are limited and the reference
streamline is shifted as the vehicle travels. To verify the potential field and control
techniques developed in this dissertation, simulation tests are performed using a widely
accepted vehicle model.
8
2 BACKGROUND
Potential flow theory forms the building block for the path planning algorithms in this
research. Before the path planning techniques are described, some foundational
knowledge is presented. The underlying theory behind potential flow, specifically in
relation to fluid dynamics, is explained along with the analytical and numerical
techniques used to calculate the flow. Additionally, the vehicle model used in the
simulation study is presented.
2.1 Potential Flow
Potential function techniques have long been used to model certain types of fluid flow
[Currie 1993, Milne-Thomson 1996]. In particular, the fluid flow must be in steady state
(no change with time), incompressible (no change in density), inviscid (viscous terms are
negligible), and irrotational (zero vorticity) to be modeled with potential functions. For a
fluid meeting these assumptions with position and velocity vectors defined as
??EN??x I J (2.1)
and
??ENVV??V I J (2.2)
respectively, the continuity equation (conservation of mass) becomes
0NE VVEN??? ? ? ???V? (2.3)
9
i.e. the divergence of the velocity vector field is identically zero. The irrotational
assumption results in
0N EV VEN? ?? ? ? ? ???V (2.4)
This is equivalent to stating that the velocity vector field is conservative, i.e. a vector
field is conservative if and only if its curl is identically zero (assuming the vector field is
defined over a simply connected region) [Barr 1997]. For the velocity vector field to be
conservative, a scalar velocity potential, ?, must exist [Barr 1997] such that
??EN??? ??? ? ? ?V I J (2.5)
i.e.
,
ENVVEN??????
(2.6)
Contours of the velocity potential are known as equipotential lines. Inserting Equation
(2.6) into (2.3) yields the Laplace equation,
222 0EN??? ??? ? ? ? (2.7)
2? is known as the Laplace operator. This is the equation that must be satisfied to
generate potential flow.
The stream function, ?, is defined as
??NE??????V I J (2.8)
i.e.
,
ENVVNE????? ? ?
(2.9)
10
Note that by inserting Equation (2.9) into (2.4) the stream function also satisfies the
Laplace equation,
222 0
xy??? ??? ? ? ?
(2.10)
Contours of the stream function are known as streamlines. Streamlines are tangent to the
fluid flow; the fluid flow follows the streamlines. If a velocity vector field is derived
from a potential field, its corresponding streamlines and equipotential lines will always
be perpendicular, i.e. Figure 2.1 below.
The velocity potential and stream function are typically combined into a complex
potential,
Fi???? (2.11)
along with a complex position,
z E iN?? (2.12)
where E and N are the east and north positions respectively.
To determine both the velocity potential and stream function, the two partial
differential equations, Equations (2.7) and (2.10) must be solved. Both of these equations
are the Laplace equation. Solutions to the Laplace equation are known as harmonic
functions. A critical property of harmonic functions is that they have no local minima or
maxima interior to the flow [Needham 1997]. This property makes harmonic functions
quite popular in the area of potential field control as was mentioned earlier. Two general
methods to solve the Laplace equation exist: analytical and numerical.
11
2.1.1 Analytical Solutions
Analytic solutions to the Laplace equation have been developed by fluid dynamicists
to model common fluid flows. In particular, for this research uniform, source/sink,
vortex, and doublet flows are of interest.
Uniform flow is flow in a straight line. Given a velocity magnitude, U, and a
direction, ? (from the E-axis), the complex potential for uniform flow is
iF Ue z??? (2.13)
and the velocity is
??c o s s inUU????V I J (2.14)
The velocity potential, stream function, and contour lines for uniform flow are shown in
Figure 2.1. Recall that the fluid flow follows the streamlines and thus models straight
line flow. This flow is not directly useful in vehicle path planning, but it is a simple flow
for testing techniques and is used as the basis for more complex flows.
12
Figure 2.1 Uniform flow complex potential
Sources or sinks model flow from a faucet or to a drain, except the flow is two-
dimensional. This flow allows fluid to be added to or removed from the flow, violating
the conservation of mass. In particular, a singularity exists at the location of the source or
sink. Given a strength C, the potential for source/sink flow is
? ?ln iF C ze ?????? ?? (2.15)
The sign of C determines whether the potential is a source or sink; a positive C is a
source (fluid is introduced into the flow). Note that at the location of the source/sink the
potential and velocity are both infinite. Also, note that this form is slightly modified
from most references by the introduction of a rotation angle, ?. The rotation angle does
not change the flow created; it simply rotates the stream function discontinuity. This is
??
13
important when the gradient is calculated numerically as the discontinuity produces an
incorrect gradient. The rotation allows the discontinuity to be out of the region of
interest. The potential for source flow is shown in Figure 2.2. The only difference for
sink flow is that the images are mirrored about the E-N plane. Note that the flow is from
low potential to high. This is slightly counterintuitive physically, but mathematically
correct. For path planning, the vehicle will drive up the slope of the potential instead of
following it down.
Figure 2.2 Source flow complex potential
Sources and sinks are used to model start and goal locations in path planning.
Sources can also be used as a simple obstacle model. The flow is directly away from the
source thus inhibiting vehicle paths from approaching the source. This direct repulsion
represents electrostatic potentials more than fluid flow. Source and sink flow also forms
Discontinuity
??
14
the basis of doublet flow that is used for adding circular obstacles to a potential field as is
discussed below.
Vortex flow simply swaps the real and imaginary parts, i.e. the stream function and
velocity potential, of source/sink flow. This flow represents rotation in the fluid as
happens near drains. The equation for vortex flow is
lnF iC z? (2.16)
Note that a rotation angle could be added as with the source/sink flow, but because vortex
flow is not directly used in path planning, it is not included. The potential for vortex flow
is shown in Figure 2.3. The flow circles the vortex with streamlines of constant radius.
This creates a singular point at the vortex that has infinite vorticity, just as the source/sink
has infinite flow rate. Vortex flow is not used in path planning, but it is the potential field
version of tracking a circle, which is the basis of the vehicle controller, described in
Chapter 4 and so is used in controller testing.
15
Figure 2.3 Vortex flow complex potential
The final basis flow that this research relies upon is doublet flow. A doublet is
created when a source and sink lie infinitely close. In particular, a doublet can be
inserted into uniform flow to produce a circular obstacle of radius a:
2aF U z
z????????
(2.17)
A circular obstacle in uniform flow is shown in Figure 2.4. Note that the streamlines
form a circle. Inside this circle, the doublet can be seen. However, since fluid cannot
pass through a streamline the flow inside the circle is unimportant. In practice, only flow
outside of the circle is calculated. Doublets form the basis for analytically representing
obstacles in potential fields. However, the potential must be generalized to insert a circle
into any existing flow.
16
Figure 2.4 Circular obstacle in uniform flow
Milne-Thomson introduced the Circle Theorem which allows a circular obstacle to be
inserted into a generic flow [Milne-Thomson 1996]. The resulting potential is
? ? 2
uuaF F z F z???? ????
(2.18)
where Fu is the undisturbed potential. F is the conjugate analytic function defined as
? ? ? ?F z F z? (2.19)
Note that inserting a circular obstacle into uniform flow satisfies Equation (2.18). Also
note that on the circle edge, |z| = a, the stream function is identically zero; thus the circle
is a streamline.
17
? ? ? ? ? ? ? ?2 20
u u u u u uza
z zzF F z F F z F F z F z
zz ? ??
?? ??? ? ? ? ? ? ? ? ? ? ???
?? ????
(2.20)
The procedure for programmatically computing the complex potential is described
below and represented in Figure 2.5.
1. The undisturbed potential is calculated over a grid of points (?).
2. The original grid points are transformed using the conjugate analytic function
argument, 2
trans az z?
. For this transformation, only points that satisfy
220.9E N a?? are considered. As is previously stated, the potential inside
the circle is not important. The slight overlap (0.9a) aids interpolation in the
next step. The transformed points (?) are now inside the circle.
3. The conjugate of the undisturbed potential is interpolated over the transformed
coordinates and becomes the conjugate analytic potential. This interpolation
is made more difficult due to the transformed coordinates being irregularly
spaced. In Matlab, the command ?griddata? must be used in place of
?interp2?.
4. The undisturbed potential is added to the conjugate analytic potential to
produce the total potential.
18
Figure 2.5 Circle Theorem coordinate transformation
As an example of the Circle Theorem, consider a circle inserted between a source and
sink. To keep the method general, the potential is not analytically calculated but
determined using the algorithm above. This potential is shown in Figure 2.6. Notice that
the circle is a streamline.
19
Figure 2.6 Circular obstacle in source/sink flow
The different flows presented above provide the basis for the analytic methods to
calculate potential field based vehicle paths. Because the Laplace equation is linear, a
linear combination of harmonic functions is also harmonic. This concept has already
been used when the circular obstacle in source/sink flow is generated by superimposing a
source and sink (explained in more detail is Section 3.1) and then applying the Circle
Theorem.
The simple basis potentials described above can be combined into quite complicated
world maps and corresponding potential fields. A downside of this approach is that a
streamline in a basis potential is not guaranteed to be a streamline in the overall potential.
This causes problems when an obstacle edge is intended to be a streamline, e.g. the Circle
20
Theorem. Section 3.1 of this dissertation investigates many methods of combining the
basis potentials to generate streamlines that closely match the true obstacle shape.
2.1.2 Numerical Solutions
The first step in developing numerical solutions to the Laplace equation is to
discretize the equation. To do this, a grid of solution points is created over the solution
area. There are many methods to generate the solution grid [Ferziger 2002]. However,
this research utilizes a simple square grid. A center difference is used to approximate the
second derivatives. The Laplace equation can be generalized by including a forcing
function, G, to become the Poisson equation,
222 GEN??? ??? ? ? ? (2.21)
Note that if the forcing function is zero, the Poisson equation simplifies to the Laplace
equation. The forcing function is used in the numeric solution algorithm. The discrete
Poisson equation is
1 , , 1 , , 1 , , 12
,,2222j k j k j k j k j k j kj k j kGEN? ? ? ? ? ?? ? ? ? ?? ? ? ?? ? ? ???
(2.22)
where j and k are the indices of the east and north coordinates respectively and ?E and
?N are the east and north grid point spacing. Note that the discrete solution is in general
real. The velocity potential is calculated, but not the stream function.
The discrete Poisson equation can be solved directly by rearranging the M equations
(where M is the total number of solution points) into matrix form and inverting an M?M
matrix to solve for the potential at each point in the solution grid. However, the solution
grid is typically large enough that the matrix inversion becomes impractical to perform
21
quickly. Instead, an approximate solution is computed recursively. This research uses a
multi-grid Gauss-Seidel method with successive over-relaxation to solve the Poisson
equation.
The Gauss-Seidel method is an iterative approach to solving discrete partial
differential equations [Ferziger 1998]. For the discrete Poisson equation, the algorithm
involves rearranging Equation (2.22) to solve for the potential in terms of its surrounding
potential and the forcing function:
? ? ? ?? ?2 2 2 2, 1 , 1 1 , 1 , ,
, 222j k j k j k j k j kjk
E N G E N
EN
? ? ? ?? ? ? ? ?? ? ? ? ? ? ? ??
? ? ?
(2.23)
This solution is then repeated for each of the points in the solution grid. Once the entire
grid is solved, the residual error, R, is calculated,
1 , , 1 , , 1 , , 1
,, 2222j k j k j k j k j k j kj k j kRG EN? ? ? ? ? ?? ? ? ?? ? ? ?? ? ???
(2.24)
If the norm of the residual is small enough, the algorithm is complete; otherwise,
Equation (2.23) is used over the solution grid again. The residual norm used to test for
convergence is scaled by the mean of ?E2 and ?N2 to remove the dependence on the grid
spacing.
Successive over-relaxation modifies Equation (2.23) to speed up convergence
[Ferziger 2002]:
? ? 1, , ,1mmj k j k j k? ? ? ???? ? ? (2.25)
where m is the iteration index and ,jk? is the output of Equation (2.23). The value of the
relaxation factor, ?, can vary between 0 and 2. The optimal relaxation factor (for fastest
22
convergence) depends on the particular problem; for this work, ? ? 1.4 is used. Note that
when ? ? 1 the method simplifies to the Gauss-Seidel method.
Multi-grid methods also speed up convergence. However, they work on the entire
solution grid instead of at the individual solution point level. Even with successive over-
relaxation, a solution point in the Gauss-Seidel method is only affected by its immediate
neighbors. Therefore, it can take an excessively long time for a solution to propagate
from known points to the entire solution grid. Multi-grid methods attempt to address this
problem by changing the number of points in the solution grid [Briggs 1987]. When
there are fewer points in the solution grid, information propagates through the grid faster.
Ultimately, the coarse grid information must be passed back to the original, fine grid. In
particular, once a candidate solution, ?*, for the Poisson equation is calculated, its
residual can be determined using Equation (2.24). Although the exact solution is not
known, a solution error can be defined,
e*? ? ??? (2.26)
The Poisson operator is linear, allowing the solution error to be related to the residual, i.e.
the solution error satisfies the Poisson equation with the residual as a forcing function:
? ?2 e 2 2 * G G R R? ? ?? ? ? ? ? ? ? ? ? (2.27)
Note that for compactness Equation (2.27) is written in the continuous operator notation,
however the concept applies to the discrete Poisson equation as well. Equation (2.27)
allows the residual to be used as a forcing function to calculate the solution error. The
residual forcing function is the reason the multi grid algorithm must solve the Poisson
equation instead of the Laplace equation.
23
To speed convergence, the residual and solution grid are down-sampled before
solving the solution error/residual problem. The calculated solution error is then up-
sampled back to the original grid and added to the original candidate solution to produce
a better solution. Down-sampling is simply taking every other point in the grid; while
up-sampling is a linear interpolation. Additionally, before the down-sampling and after
the up-sampling a set number of Gauss-Seidel iterations are run to smooth the solution.
The technique to solve the down-sampled Poisson?s equation is to repeat the smoothing
and down-sampling until the grid size is small enough to be solved without further down-
sampling.
Overall, the steps in the multi-grid algorithm are
Initialize the Poisson equation solution (typically to zero), and set the forcing
function to zero.
Compute the residual of the Poisson equation solution, Equation (2.24).
While the residual is not below a tolerance (0.0001):
While the grid size is not at the minimum size (5?5):
Run a set number of smoothing iterations (10) of the Gauss-Seidel with
successive over-relaxation algorithm to update the Poisson equation
solution, Equations (2.25) and (2.23).
Save the Poisson equation solution with the corresponding grid size.
Note: the solution at a lower grid size is a correction to this solution.
Compute the residual of the Poisson equation solution, Equation (2.24).
Down-sample the residual and solution grid to reduce the grid size.
Set the forcing function to the down-sampled residual and initialize the
down-sampled solution (to zero).
When the grid size is at the minimum, run the Gauss-Seidel with successive
over-relaxation algorithm until the residual is below a tolerance (0.0001) to
24
produce a solution to the Poisson equation, Equations (2.25) and (2.23).
While the grid size is not at the original size:
Up-sample the solution to obtain an error correction, which increases the
grid size.
Add the up-sampled error correction to the Poisson equation solution at
the corresponding grid size (saved above) to generate an updated Poisson
equation solution.
Run a set number of smoothing iterations (10) of the Gauss-Seidel with
successive over-relaxation algorithm to update the Poisson equation
solution, Equations (2.25) and (2.23).
When the grid size is back to the original, compute the residual of the
Poisson equation solution, Equation (2.24).
When the residual is below the tolerance threshold, the multi-grid algorithm is
complete.
Boundary conditions are what allow numerical solutions of the Laplace equation to
model different flows. The area immediately surrounding the solution grid as well as any
features inside the solution grid (starts, goals, and obstacles) must be represented by
boundary conditions. The need for boundary conditions is also a major difference
between analytical and numeric solutions. Analytic solutions are global in scope, while
numeric solutions only exist on a finite space. In particular, two boundary conditions are
common in fluid dynamics: Dirichlet and Neumann [Ferziger 2002].
Dirichlet boundary conditions set the edge of the potential function to a specified
value. This boundary condition is used to model a start or goal, although unlike an
analytical source or sink, the potential of the numerical equivalent is finite. Dirichlet
boundary conditions are also often used on the boundary of the solution grid due to their
25
simplicity and fast solution time. Dirichlet boundary conditions can also be used to
model simple obstacles similar to using analytical sources as obstacles.
Neumann boundary conditions set the derivative of the potential field, i.e. the fluid
velocity, at the edge of the solution grid. This is the more common boundary condition
fluid dynamicists use to model obstacles and the solution boundary because it has more
physical meaning to fluids. Specifically, the fluid velocity vector along an obstacle must
be parallel with the obstacle boundary such that no fluid can flow into or out of the
obstacle. Numerically, this is accomplished by setting the potential equal to a point along
a line perpendicular to the obstacle edge a certain distance into the flow. Therefore, the
gradient of the potential must be in the direction of the obstacle edge. Along with
modeling an obstacle, this boundary condition can be used to model the solution edge so
that fluid does not leave the solution area. The down side of this boundary condition is
that it significantly increases the solution time for two reasons. First, the boundary
potential changes each iteration, so it effectively must be calculated along with the
solution potential. Second, because the boundary potential is equal to its interior
neighbor, the boundary does not provide a starting point for the solution to propagate to
the remainder of the solution grid. The solution must propagate from only the start and
goal location instead of from the entire solution boundary.
As with analytical solutions being built from many basis potentials, numerical
solutions typically use a combination of boundary conditions. For instance, the type of
boundary condition used on obstacle edges greatly influences the character of the
resulting potential [Keymeulen 1994]. Section 3.2 of this dissertation describes the
26
different boundary conditions and grades the calculated potentials using the specific
requirements of the vehicle control algorithm given in Chapter 4.
2.2 Vehicle Model
To test the path planning algorithms, a nonlinear vehicle model is utilized [Daily
2004]. This model has been extensively used and verified to model lateral vehicle
dynamics [Rossetter 2003, Smith 1995, Wenzel 2005]. A schematic of the vehicle model
is shown in Figure 2.7. The lateral dynamics of this vehicle model are described by
? ? ? ?
? ? ? ? ? ?
c o s
c o s
c o s s in
2
y I F y O F y I R y O R
y I F y O F y I R y O R y I F y O F
z
F F F F
r
mV
Ta F F b F F F F
r
I
?
?
?
??
? ? ?
??
? ? ? ? ?
?
?
?
(2.28)
where r is the vehicle?s yaw rate (rotation about a vertical axis), ? is the side slip
(difference in heading and velocity vector direction), V is the magnitude of the velocity
vector, and ? is the steer angle input. The lateral forces, Fy?, are described below and the
vehicle parameters, m, a, b, etc., are given in Table 2.1 along with the tire parameters.
27
Figure 2.7 Four wheel vehicle model schematic
The lateral force at each tire depends on the slip angle at the tire, ?, and the vertical
load on the tire as well as tire and surface properties. Many models of the tire forces
exist. For this research the Dugoff model is used [Dugoff 1970]:
N
E
V
x
V
y
V
r
?
?
?
a
b
T
?
L F
F
y L F
?
V
L F
?
R F
?
F
y R F
V
R F
?
R R
V
R R
F
y R R
?
L R
V
L R
F
y L R
28
? ?
m a x
2 t a n( )
2 i f 1
()
1 i f 1
( ) t a n( )
y
T
yT
F
C
f
F f C
?
?
?
?
? ? ?
?
?
??
?
?
?
? ? ?
? ?
??
??
(2.29)
Note that Fymax?, the peak lateral force, is a property of the tire/surface interaction and
vertical load on the tire, and C??T is the tire cornering stiffness, a property of the tire.
Although each tire has its individual peak force and cornering stiffness, typically the two
tires on the same axle are identical. Due to vehicle geometry, the front and rear tires
often have different properties. Table 2.1 lists the parameters used to simulate the lateral
vehicle dynamics. These parameters represent a 1997 Corvette [Rossetter 2003]. Further
research into estimating vehicle properties (specifically Dugoff tire properties) is
presented in [Daily 2007].
Table 2.1 Lateral vehicle model parameters
Variable Parameter Value Units
m Mass 1860 kg
Iz Vertical moment of inertia 3100 kg?m2
a Distance from center of gravity to front axle 1.37 m
b Distance from center of gravity to rear axle 1.43 m
T Track width 1.5 m
C?FT Front tire cornering stiffness 72500 N/rad
C?RT Rear tire cornering stiffness 72500 N/rad
FymaxF Front tire peak lateral force 3960 N
FymaxR Rear tire peak lateral force 3794 N
29
Note that this vehicle model makes several assumptions; the speed is assumed
constant and thus longitudinal forces are negligible and all non-planar motion such as
roll, pitch, and vertical acceleration is ignored. While models for the neglected dynamics
exist, as long as the vehicle maneuver is not extreme, the neglected dynamics do not
significantly alter the vehicle response. The path planning algorithm described below has
safe guards to prevent drastic maneuvers.
Although Equation (2.28) assumes a constant vehicle speed, an important aspect of
the path planning algorithm presented below is the ability to adjust the vehicle?s speed
when near obstacles. For this reason, a model for the vehicle?s speed is required. Most
vehicles now have a cruise control module and algorithms for controlling vehicle speed
are widely used. The goal of this research is lateral control, so instead of redeveloping a
vehicle speed model and controller, an existing closed-loop model with a desired speed
input and actual speed output developed by Rajamani is used [Rajamani 2006]. This
model assumes a first order lag between the desired longitudinal acceleration, throttle
input, and actual longitudinal acceleration, and an integrator from longitudinal
acceleration to speed. The throttle input comes from a PI-controller. Overall, the closed-
loop dynamics from reference speed, Vref, to actual speed, V, are described by the transfer
function
, s p e e d , s p e e d
spe 32e d , s p e e d , s p e e d
PI
r e f P Iss
k s kVV k s k? ?? ?? ? (2.30)
When simulated, this transfer function is discretized at the sample rate of 100 Hz. Table
2.2 lists the parameters used when simulating the vehicle, which are taken directly from
[Rajamani 2006].
30
Table 2.2 Closed-loop speed model parameters
Variable Parameter Value Units
kP,speed Throttle proportional control gain 0.75 1/s
kI,speed Throttle integral control gain 0.1875 1/s2
?speed Longitudinal acceleration time constant 0.5 s
One critical note about the combined lateral/longitudinal vehicle model is that it is
either nonlinear (if speed is counted as a state in the combined model) or more typically
time varying (speed is first calculated independent of lateral dynamics then becomes a
parameter in the lateral model). In the second case, typical linear stability criteria are not
guaranteed to yield correct results. This aspect of the method?s stability is not considered
in this research. In general, the speed varies slowly enough that fixed parameter stability
analysis should be valid. However, several methods exist to further examine the time-
varying stability [Packard 1993, Tan 2000], and this is a topic that should be studied
further in all vehicle control applications. In particular, instability has been observed in
unrelated autonomous ground vehicle testing when the vehicle was both turning and
accelerating heavily. The exact cause was not determined, but both the lateral and
longitudinal systems were independently stable, and for most operation, the combined
system was stable. The most likely explanation is the coupling when both modes were
simultaneously excited.
In addition to the vehicle model, kinematic relationships are needed to determine the
vehicle?s global east and north location, E and N respectively, and its heading, ?:
sin
cos
EV
NV
r
?
?
?
?
?
??
?
?
(2.31)
31
Note that ?, the vehicle?s course, is defined as
? ? ??? (2.32)
Therefore, its derivative is
r?????? (2.33)
with ?? given in Equation (2.28).
This vehicle model, from steer angle and desired speed to global position and
heading, is used in all of the simulation tests presented in this research. The model will
also be the starting point for developing the lateral controller presented in Chapter 4.
32
3 OBSTACLE REPRESENTATION
The potential flow techniques described in Section 2.1 can be used to develop flow
fields that have a start and goal position and which avoid any obstacles in the area to be
traversed. This research imposes specific requirements on the potential field being
created. Typical potential field control relies only on the gradient of the potential field
for the control input [Ge 2002, Khatib 1985, Krogh 1986, Kyriakopoulos 1995]. The
algorithm developed in Chapter 4 requires an explicit stream function in addition to the
gradient field. Therefore, either a technique must be developed which explicitly
calculates the stream function, or a method to calculate the stream function from the
gradient field must be employed.
This chapter examines two broad techniques for generating potential functions,
analytical and numerical. In Section 3.1 analytical methods are considered. In particular,
the viable methods under consideration are analyzed based on a standard set of tests
involving multiple obstacles in uniform flow. Section 3.1.1 looks at existing methods to
represent obstacles as low strength sources. Section 3.1.2 describes the technique Milne-
Thomson developed to add circular obstacles to a flow. Section 3.1.3 analyzes Waydo
and Murray?s extension to the Milne-Thomson Theorem for averaging the velocity vector
fields from individual circles and extends their approach to the case with touching circles.
In Section 3.1.4 a new method is developed for averaging the stream functions of
33
individual obstacles. Section 3.2 investigates numerical solutions. As before, the
methods are compared using a common set of tests. Sections 3.2.1 and 3.2.2 analyze
Dirichlet and Neumann boundary conditions respectively. Finally, Section 3.2.3 presents
a new method of calculating the stream function directly and the boundary conditions that
are used in the method.
3.1 Analytical Solutions
Analytical solutions for the potential field offer many benefits. They explicitly
calculate a stream function and the calculation is often much quicker than numeric
solutions. The first step with an analytic solution is to generate a field with a start and
goal, but no obstacles. Two such fields generated using a source and sink are shown in
Figure 3.1. The potential for these fields is
? ? ? ?
s ta r t s ta r t go a l go a ll n l nF C z z C z z? ? ? ?
(3.1)
where zstart and zgoal are the start and goal locations respectively. Notice that due to the
linear nature of the Laplace equation, the start and goal locations can be shifted arbitrarily
and any number of sources and sinks (or other basis potentials) can be summed and still
yield a harmonic function. The difference in the two plots of Figure 3.1 is the relative
strength of the source and sink, Cstart and Cgoal respectively.
34
Figure 3.1 Source and sink potential
One important feature of this field is that all streamlines should intersect the start, and
more importantly, the goal. Therefore, no matter what streamline a vehicle is following,
it will eventually reach the goal. The left field exhibits this feature while the right does
not. The difference is that in the left field the source and sink are equal strength;
however, the source is stronger than the sink in the right field. Notice that in the right
field some streamlines go to infinity instead of approaching the sink. Therefore, a vehicle
may not reach the goal location. Alternatively, potentials where the sink is stronger than
the source look like the right plot except with the start and goal reversed. This condition
is not as bad since all streamlines do lead to the goal. However, if the vehicle starts near
35
the source, then a portion of the usable space will be disregarded since some streamlines
do not intersect the source.
Note that the overall strength of the start and goal does not matter for this research.
The strength determines the amount of fluid being added to or removed from the flow
and thus the magnitude of the flow velocity. For traditional potential field controllers this
also determines the desired vehicle speed. However, for this research the desired speed
comes from a separate process so only the flow direction matters. Thus, as long as the
source strength matches the sink strength, the overall strength is not critical.
3.1.1 Small Sources in Flow
The first obstacle model to investigate is a small source. As Section 2.1.1 described,
this obstacle model more closely matches an electrostatic potential instead of a
hydrodynamics potential. The potential generated by placing a source in uniform flow is
shown in Figure 3.2. This potential is given by
lnF C z Uz?? (3.2)
A stagnation point is defined as a point in the flow where the velocity is zero. The
stagnation point defines the leading edge of the obstacle. It is also a point where the
streamline splits. Outside of fluid dynamics, this point would be known as a saddle point
and a separatrix in the flow. On one side of the streamline passing through the stagnation
point, fluid comes from the uniform flow; on the other side, the fluid comes from the
source. For this potential, the stagnation point is located at
stag Cz U??
(3.3)
36
Notice that the stagnation point (and thus the obstacle boundary) depends on flow
parameters, namely the uniform flow speed. This causes problems when dealing with
more complex flows because the flow parameters are not generally known, or easily
quantifiable. The circle labeled as ?obstacle? is simply there to aid comparisons to other
methods. The true obstacle in this case is the entire region defined by the separatrix,
which extends infinitely downstream of the source. This is typically not a desirable
feature since an obstacle would often remove an area much larger than itself from the
flow.
Figure 3.2 Source in uniform flow
With an analytic circular obstacle, Figures 2.4 and 2.6, stagnation points are also
present at the leading and trailing edge of the circle. However, because there are two
stagnation points, the area defined by the separatrix is finite. In particular, the obstacle
37
boundary is exactly equal to the circle itself. In terms of the flow, the finite obstacle is
due to the doublet that creates the circle. The doublet is formed from a source and a sink,
so no net fluid is added to the flow. Also note that the obstacle does not require flow
parameters to define as the source stagnation point does.
For an obstacle (defined by strength and location Cobs and zobs respectively) inserted
into start/goal flow, the potential, shown in Figure 3.3, is given by
? ? ? ? ? ?? ? ? ?s ta r t s ta r t o b s o b s
s ta r t o b s g o a l
l n l n
ln
F z C z z C z z
C C z z
? ? ? ?
? ? ?
(3.4)
In general, the obstacle strength is much less than the start; the goal strength must be
equal to the sum of the sources as described above. The stagnation point defining the
obstacle is given by
? ?? ?s t a r t o b s o b s s t a r t g o a l s t a r t s t a r t o b s o b s s t a r t o b s
s t a g s t a r t o b s g o a l s t a r t s t a r t o b s o b s
C z C z z C z z C z zz C C z C z C z? ? ?? ? ? ? (3.5)
Calculating the leading edge of the obstacle requires intimate knowledge of the
underlying basis flow. The design problem would involve determining an obstacle
source location and strength to match an identified obstacle knowing the underlying flow,
which is a much more difficult problem. Notice that the area contained by the separatrix
is finite in this flow, but it extends from the stagnation point to the goal location. This
creates a large wake downstream of the obstacle that has been rendered unusable.
Effectively, the disturbance to the potential created by the obstacle does not vanish with
distance from the obstacle.
38
Figure 3.3 Source in source/sink flow
Modifications to the source obstacle method have been used for obstacle avoidance,
[Guldner 1995, Kim 1992]. However, the deficiencies in the flow created by source
obstacles, e.g. the disturbance not vanishing with distance and the difficulty in
determining the obstacle shape and location, lead it to be discarded as a practical,
analytical solution in this dissertation. This obstacle type is revisited in the numeric
solution when the obstacle shape can, to a degree, be more strictly controlled.
The most common method of representing obstacles in potential fields is using the
Circle Theorem described in Section 2.1.1. However, several hurdles need to be
overcome in order to use this method. First, if an additional obstacle is added to flow
already containing obstacles, then the streamlines defining the previous obstacles will be
distorted to some extent. Second, not all obstacles are circular. The solution to both
39
these problems may lie together. If a method to place circular obstacles next to each
other without distorting the defining streamline can be developed, then a generic shape
obstacle can be generated using many circles.
3.1.2 Circle Theorem Method
To study the interaction between multiple obstacles in a potential field, a test scenario
of multiple circular obstacles inserted into uniform flow is used. The obstacles are tested
with vertical and horizontal orientations, both spaced and touching. Additionally, a test
of five circles creating a rough wall is tested. The objective of these tests is to determine
a method of generating generic obstacle shapes using known methods for circular
obstacles.
The first method tested is the Circle Theorem (CT) method. This method is a direct
application of the Circle Theorem [Milne-Thomson 1996] using Equation (2.18). It is
important to note that using this method, the order of adding circles affects the resulting
flow. The theorem requires, and modifies, the existing flow. Therefore, only the last
circle added is guaranteed to exactly match a streamline. The streamline distortion can
be observed in the test scenario of two vertically separated circular obstacles inserted into
horizontal uniform flow from left to right, shown in Figure 3.4. Only the streamlines are
shown since these are what the vehicle would follow. The lower circle is added to the
flow first. Because of this, the addition of the upper circle causes the streamlines that
define the lower circle, the separatrices, to become distorted from the desired circle.
However, at a spacing of only one radius between circle edges, the distortion is not large.
The missing information in the center of the circles is due to the interpolation required to
perform this technique. However, recall that the stream function inside the separatrices is
40
not used in the path planning and so is only shown (and calculated) to fully illustrate the
effects of multiple circles.
Figure 3.4 Two vertical, separated circles in uniform flow calculated using the Circle
Theorem method
The distortion caused by the second circle becomes more apparent when the circles
touch, as shown in Figure 3.5. More importantly, fluid still flows between the two circles
as is represented by the streamlines that pass between them. The fact that streamlines
pass between the circles means this method cannot be used directly to represent non-
circular obstacles. From a purely mathematical standpoint, notice the second, smaller
undefined region in the upper circle. Recall from Equation (2.18) that the potentials of
41
points exterior to the circle are transformed to be interior to the circle for the conjugate
analytic function. Thus, the undefined area around the doublet in the first obstacle is
reflected in the second obstacle.
Figure 3.5 Two vertical, touching circles in uniform flow calculated using the Circle
Theorem method
To further explore the effects of many circular obstacles together, five circles are
placed vertically to form a wall in Figure 3.6. As expected, the last, uppermost, circle
placed exactly matches the streamlines. All the other circles are distorted from the
desired circle allowing streamlines to pass through the wall. The only streamlines that
completely avoid the wall are outside the extreme upper and lower separatrices. It is
42
interesting that the distortion does not vary much with distance from the last obstacle
added. Because the disturbance created from adding a circular obstacle decays with
distance from the circle, effectively adding a circle only affects its immediate neighbors.
Therefore, in the scenario of adding circles from the bottom up, a circle distorts its lower
neighbor and is distorted by its upper neighbor.
Figure 3.6 Five vertical, touching circles in uniform flow calculated using the Circle
Theorem method
When inserting a series of circles into a generic existing flow, there is no guarantee
that the series will be perpendicular to the flow direction. Therefore, the same tests are
performed with both the flow and the circle locations lying horizontal. As before, the
43
first scenario is with the circles spaced apart (Figure 3.7). The right circle is added last
and so exactly matches a streamline as expected. However, unlike the perpendicular
flow, both circles are completely contained within the separatrices; thus, both circles
would be avoided.
Figure 3.7 Two horizontal, separated circles in uniform flow calculated using the Circle
Theorem method
As before, the distortion becomes more apparent when the obstacles touch (Figure
3.8). However, unlike the vertical orientation (Figure 3.5), the distorted streamlines do
not penetrate the first circle; instead, the separatrices merge. Note that the closest two
stagnation points that originally lay on the centerline of the flow have shifted onto the
second circle. Overall, the streamlines produce a path such that the first circle is still
completely avoided.
44
Figure 3.8 Two horizontal, touching circles in uniform flow calculated using the Circle
Theorem method
When a series of five circles are joined to form a wall (Figure 3.9), the same merging
phenomenon is observed. The stagnation points shift to the last added circle or form on
the interior joints of the circles. This in effect creates a super obstacle that is defined by
the streamlines intersecting the leading, leftmost, stagnation point and continues until
they intersect the trailing circle where they then follow that circle. All of the inserted
circles are contained within this super obstacle. The CT method therefore does form an
effective wall obstacle in line with the flow.
45
Figure 3.9 Five horizontal, touching circles in uniform flow calculated using the Circle
Theorem method
The fact that the behavior is drastically different when the circle formation is parallel
or perpendicular to the existing flow presents a problem. When used for path planning,
the overall obstacle shape must represent a physical object, and the potential field flow
will not necessarily be in any particular orientation with respect to that physical object
due to the goal location and the starting position of the vehicle. Therefore, a method is
needed that works regardless of the orientation of the existing flow.
3.1.3 Flow Velocity Average Method
Waydo and Murray proposed a method to add multiple circular obstacles to a
potential field without distorting the separatrix streamlines of any of the obstacles
[Waydo 2003], the Flow Velocity Average (FVA) method. This method calculates the
potential field created by inserting the individual circles into the unmodified flow using
? ? 2
obs,obs,jj u u jj
aF F z F zzz??? ? ????
??
(3.6)
46
This is effectively Equation (2.18) with two additions. The modified potential, Fj, is
saved for each individual circle, j. Additionally, the circle is centered at a generic
location in the field, zobs,j. From this potential, the velocity vector field for each obstacle
potential, (VE,j, VN,j), is calculated using Equation (2.6) Once a disturbed potential is
calculated for each of the circles, a weighting factor, ?, is generated based on the distance
to each circle edge, d:
k
j kj jkddd? ?? ??
(3.7)
For a given circle, this weighting is one at the circle edge and zero at all other circle
edges. The total velocity vector field is the sum of the weighted individual fields
,,,E j E j N j N jjjV V V V??????
(3.8)
Note that the individual potential fields, weighting factors, and velocity vector fields are
calculated over the entire solution area. Additionally, since each individual circle is
added to the unmodified flow, the order of addition does not matter.
The intent of this method is to accurately represent multiple separate circular
obstacles, rather than to allow circles to touch and form larger obstacles. However, the
weighting factor can be modified to allow for touching obstacles by setting the weight to
the inverse of the number of touching circles,
? ?
1 ; 0 & 0
c ount 0
; othe r w is e
jk
k
j
k
kj jk
d k j dd
d
dd
?
?
? ? ? ? ?
? ??
? ?
?
? ?? ?
(3.9)
47
To evaluate this method the same series of tests from Section 3.1.2 are performed.
The first test, shown in Figure 3.10, is two vertically separated circles. The most obvious
difference with the CT method is that no streamlines are visible inside the circles.
Because the output of this method is a velocity vector field, an explicit stream function
does not exist. Instead, individual streamlines are calculated by simulating the fluid flow
from distinct starting locations. For traditional potential field control, an explicit stream
function is not required as the control effort is calculated exclusively from the potential
field gradient, i.e. the velocity vector field. However, for the vehicle navigation
controller presented in Chapter 4, a stream function is necessary. To use this controller a
technique of generating a stream function from a gradient field would need to be
employed. Section 3.2.2 points to some possible techniques to calculate the stream
function. The main reason for the FVA method is to generate multiple circular obstacles
with exact streamline edges. If the edges are streamlines both circles are completely
avoided. Analytically, this is due to the weighting function, which prevents the
disturbance potentials from other obstacles from affecting the potential at a circle edge.
48
Figure 3.10 Two vertical, separated circles in uniform flow calculated using the Flow
Velocity Average method
As seen in Figure 3.11, a problem still exists when the circles touch. In particular, all
the streamlines between lines intersecting the separatrix stagnation points pass between
the circles.
49
Figure 3.11 Two vertical, touching circles in uniform flow calculated using the Flow
Velocity Average method
As would be expected, when five obstacles are lined together (Figure 3.12)
streamlines pass between each of the circles. Therefore, the FVA method does not
produce an effective wall obstacle.
50
Figure 3.12 Five vertical, touching circles in uniform flow calculated using the Flow
Velocity Average method
For the series of tests with flow parallel to the obstacle orientation (Figures 3.13-
3.15), the results are similar to the CT method. The main difference is that now all circle
edges are streamlines. As with the CT method, the FVA method generates streamlines
that allow a vehicle to avoid all the obstacles. However, when multiple circles are
touching the separatrices defining the super obstacle exactly match the intended circular
shape.
51
Figure 3.13 Two horizontal, separated circles in uniform flow calculated using the Flow
Velocity Average method
Figure 3.14 Two horizontal, touching circles in uniform flow calculated using the Flow
Velocity Average method
52
Figure 3.15 Five horizontal, touching circles in uniform flow calculated using the Flow
Velocity Average method
The FVA method does perform exactly as it is designed, i.e. the edges of each of the
circular obstacles are streamlines. However, it is not intended for touching circles, in
which case the FVA method breaks down. In particular, streamlines still pass between
the circles, although unlike the CT method, the circle boundaries are not distorted.
Additionally, the output of the method is a velocity vector field, not a complex potential
function. To utilize this method a stream function would need to be computed from the
velocity field. Overall, the FVA method is not effective for creating generic shaped
obstacles for vehicle path control.
3.1.4 Flow Potential Average
To address the issue of an explicit stream function not being calculated, a new Flow
Potential Average (FPA) method is developed that directly averages the complex
potential instead of the velocity vector [Daily 2008]. Using the same individual obstacles
potentials given in Equation (3.6), the overall complex potential is a weighted average of
the individual potentials,
53
jjjFF???
(3.10)
The same weighting function given in Equation (3.9), is used. The test case of two
vertically separated circles is shown in Figure 3.16. Several features stand out in the FPA
method that are not present in the previous methods. First, like the FVA method, the
circle edge is a streamline. However, unlike the FVA method, even if the original
circular obstacles are separated, a larger super obstacle is formed. This is exhibited by
the streamlines intersecting the two stagnation points on the circular obstacles touching at
two additional stagnation points. Both circles would therefore be avoided by the path
planner and even more, no streamlines pass between the obstacles. As Equation (2.20)
states, the stream function on a circle edge is zero. Therefore, the super obstacle
separatrices are formed where the weighted average of the two (or more) stream
functions cancel out. This is promising for creating generic shaped obstacles.
54
Figure 3.16 Two vertical, separated circles in uniform flow calculated using the Flow
Potential Average method
Another feature of the FPA method is that both the streamlines and equipotential lines
exist without being numerically integrated. As is shown in Figure 3.16, these contours
are not always perpendicular. This implies the averaged complex potential is not a
harmonic function. Other than demonstrating that the flow does not satisfy the Laplace
equation, the equipotential lines are not used in this method. As with the other methods,
the vehicle follows the streamlines. However, the velocity vector field must be
calculated from the stream function using Equation (2.9), instead of from the velocity
potential.
55
Because the weighting function varies over the solution grid, the averaged complex
potential is not a superpositioning of individual potentials and thus is not guaranteed to
satisfy the Laplace equation. An important extension of this fact is that the potential is
not necessarily minima free. In particular, as seen in Figure 3.16, two vortices exist
inside the super obstacle, the closed contour streamlines. These represent a local minima
and maxima in the stream function. For this scenario, the extrema are contained within
the super obstacle and therefore would not affect the vehicle. Additionally, the
equilibrium points at the extrema are conservative as they are centers rather than focuses.
Therefore, all the streamlines around the extrema form closed contours until a separatrix
is encountered, i.e. a vehicle following a streamline far from the extrema should not
encounter the extrema without shifting streamlines. However, neither of these features of
the extrema (contained in the super obstacle and conservative) is necessarily true for all
scenarios. Therefore, in certain situations problems may arise where the vehicle is
attracted to a local extremum.
The next test is the vertically oriented, touching circles shown in Figure 3.17. As
expected, both circles are entirely avoided and a super obstacle is formed that is slightly
larger than the original circles.
56
Figure 3.17 Two vertical, touching circles in uniform flow calculated using the Flow
Potential Average method
The result becomes more interesting when five circles are added (Figure 3.18). While
all circles are avoided, the streamlines near the circles are not as smooth as before. The
super obstacle formed where the stream function is zero is not a smooth joining of the
individual circles. In particular, protrusions are formed off the second and fourth circles.
Additionally, stagnation points (and the corresponding separatrices and extrema) appear
outside of the super obstacle that is formed. Therefore, streamlines tightly following the
super obstacle edge have a very convoluted path. However, only streamlines that
approach near the center of the super obstacle wall follow its contour that closely. Most
57
streamlines do smoothly avoid the obstacle. The phenomenon of the streamlines
becoming convoluted is present in most scenarios using this method when the obstacle is
created from a large number of circles.
Figure 3.18 Five vertical, touching circles in uniform flow calculated using the Flow
Potential Average method
For the two obstacle horizontal tests shown in Figures 3.19 and 3.20, the potential is
much like the average velocity method. The potential field has streamlines that avoid the
individual circular obstacles, but no larger super obstacle is formed.
58
Figure 3.19 Two horizontal, separated circles in uniform flow calculated using the Flow
Potential Average method
Figure 3.20 Two horizontal, touching circles in uniform flow calculated using the Flow
Potential Average method
59
However, the scenario consisting of five touching circles in line with the flow (Figure
3.21) exhibits slightly different behavior. A super obstacle consisting only of the five
circles is formed, but away from the super obstacle, the potential field does not smoothly
revert to the underlying uniform flow. In particular, the streamlines initially approach the
obstacle, and then a hump exists near the center of the super obstacle. The flow being
disturbed far from the obstacle is another undesirable feature of the average potential
method when large numbers of circles are used to form the obstacle. In this test scenario,
the disturbance would cause a vehicle following the streamlines to initially approach the
obstacle before needlessly veering away from it.
Figure 3.21 Five horizontal, touching circles in uniform flow calculated using the Flow
Potential Average method
As with the previous methods, the flow characteristics are different if the circles are
oriented perpendicular to or in line with the underlying flow. Specifically for the FPA
method, when the circles are perpendicular to the flow, shown previously in Figure 3.16,
60
two streamlines join the two circles and local extrema exist in the region formed by the
two streamlines. However, if the circles are oriented in line with the flow only one
streamline joins them and no local extrema exist (other than the doublets which are
guaranteed to be inside the circular obstacle and so are acceptable). The transition
between the inline and perpendicular behaviors is critical because the local extrema are
dangerous to a vehicle path planner unless they are contained within a super obstacle.
The bifurcation the streamlines experience as the obstacle orientation shifts from
perpendicular (0 deg) to in line (90 deg) with the underlying flow is shown in Figure
3.22. The most precise method of analyzing this flow is as a phase portrait in north, east
space. For each of the orientations, two saddle points exist on each of the circular
obstacles. At 0 deg two additional saddle points exist between the circles. A single
separatrix joins all the saddle points and two centers exist inside the region defined by the
separatrix. As soon as the orientation leaves perpendicular to the flow, shown at 10 deg,
the single separatrix breaks into three distinct streamlines: one joining the two circles and
two others intersecting the off-circle saddle points which still exist. The centers also still
exist in regions defined by the two off-circle separatrices. As the angle continues to
increase, each center/off-circle saddle point pair begins to merge until the singular points
cease to exist at 15 deg. Increasing the angle further at 45 deg, smoothes the flow until
the obstacles are in line with the underlying flow at 90 deg.
61
Figure 3.22 Flow Potential Average method, vertical/horizontal obstacle transition
62
The inline/perpendicular transitional behavior can be problematical when using this
method for path planning. Because the underlying flow is not uniform, it is not easy to
define a flow direction much less guarantee its orientation relative to a real world
obstacle. Therefore, it is possible that scenarios arise such as the 10 deg example where
the path taken by the streamlines (and thus the vehicle) are very convoluted. This is quite
undesirable behavior for vehicle path planning. On the positive side, the flow in this test
is conservative. The singular points are either saddles or centers as opposed to nodes or
focuses. Therefore, no local extrema are encountered outside of closed regions formed
by separatrices.
Although the FPA method can produce some undesirable behavior particularly when
a large number of circles are used to form an obstacle, it does meet the basic criteria for
obstacle avoidance. An obstacle can be approximated by a set of circles enclosing it and
that set of circles will be avoided. To expand on this concept, a series of circles are used
to determine the best representation of a square obstacle. First, four touching circles form
the square (Figure 3.23). In this case, the potential field is well behaved. The only area
in the super obstacle but outside the original circles is at the leading and trailing edges.
This actually helps the path planner since it would prevent abrupt course changes in the
vehicle if the streamlines were to approach the leading edge intersection of the two
circles. A downside of this representation is that at the top and bottom edges of the
obstacle, the streamlines do not form a straight line; they bend toward the center of the
obstacle. This means that to avoid a square, the circles need to extend well beyond the
edge of the square.
63
Figure 3.23 Square obstacle formed of four touching circles calculated using the Flow
Potential Average method
A possible solution to the circles extending beyond the desired square is to use more
circles to approximate the square. However, as is demonstrated in Figure 3.24, this
representation has significant problems. There are many extraneous features outside the
designed circles such as the two additional saddle points on both the leading and trailing
edge. However, the major problem is the shape of the streamlines above and below the
obstacle. A large portion of the flow approaching the obstacle initially is drawn towards
it unnecessarily. Although this representation does generate paths that avoid the obstacle,
they are not elegant paths.
64
Figure 3.24 Square obstacle formed of eight touching circles calculated using the Flow
Potential Average method
Recall that with the FPA method the circles are always joined by a streamline.
Because of this, the circles do not need to be touching to form the obstacle. A square
obstacle created using four separated circles is shown in Figure 3.25. Because only four
circles are used the flow is smooth, but the shape of the connecting streamlines cannot be
easily predicted so there is no guarantee that a path is generated such that a vehicle would
avoid the desired obstacle.
65
Figure 3.25 Square obstacle formed of four separated circles calculated using the Flow
Potential Average method
Overall, with the new FPA method it is better to use as few circles as possible to
represent obstacles. Additionally, to guarantee the shape of the obstacle, the circles
usually should touch. One useful obstacle that is created with separated circles is a wall.
A small circle is placed on each edge of the wall and the connecting streamline forms the
wall. Within these limitations, it is possible to generate vehicle paths using the FPA
method. The scheme is as follows.
66
Create a potential field containing only start and goal locations as sources and
sinks respectively using Equation (3.1). The discontinuities should be rotated to
point away from the line segment between the start and goal. This potential
forms the new basis potential.
For each obstacle present in the world map:
If the obstacle is approximated as a circle:
Use the Circle Theorem to add the circular obstacle to the basis potential
using Equation (2.18).
If the obstacle is approximated as a wall:
Use the FPA method to add a small circle to the basis potential at each end
of the wall with Equations (3.6) and (3.10).
If the obstacle is approximated by a combination of circles:
Use the FPA method to add each of the circles to the basis potential,
Equations (3.6) and (3.10).
The potential resulting after the obstacle is added becomes the new basis
potential.
Once all obstacles are added to the potential field, the overall potential field is
complete.
The new FPA method has the same limitation of the original Circle Theorem, i.e.
adding an additional obstacle modifies the edges of any previously added obstacles.
However, as long as the obstacles are significantly separated relative to their size the
distortion is minimal. A sample map generated using this method is shown in Figure
3.26. It contains a wall, a circular obstacle, and a square obstacle approximated by four
circles. The streamlines from start to goal smoothly avoid each of the obstacles.
67
Figure 3.26 Averaging stream function, sample map
While this map is successful, it is difficult to predict when some of the less attractive
aspects of this method will appear in the generated potential field. The method meets the
basic requirements, but has significant secondary drawbacks that may be worth
investigating in future work.
Many other techniques for representing obstacles also exist. For instance, Needham
expands the concept of the Circle Theorem with the ?method of images? for inserting a
generic shape obstacle into a flow [Needham 1997]. The idea centers around making the
obstacle edge a streamline. By reflecting the undisturbed flow about the obstacle edge,
that edge becomes a streamline. For a circular obstacle, this reflection is the conjugate
analytic function with the argument given in Equation (2.18). Additionally, conformal
mapping techniques allow some obstacle shapes to be mapped to a circle along with the
68
resulting fluid flow [Currie 1993, Milne-Thomson 1996, Rimon 1992]. This is a
common way to study basic flow around wings. These techniques should be studied in
the future to determine their applicability to path planning.
3.2 Numerical Solutions
As Section 2.1.2 described, numerical solutions are an alternative method for
calculating a potential field. The main down side of the numerical method is that
typically only the velocity potential is calculated. The stream function must therefore be
determined from the velocity potential. For fluid dynamicists this is acceptable since the
objective is the velocity vector field and corresponding streamlines, thus the stream
function is not necessary. For this work, however, an explicit stream function is required
to control the vehicle to the goal. The benefit of numeric solutions is that any shape
obstacle can be represented. This feature makes numerical solutions attractive since
generic shaped obstacle representation is the main drawback of analytical solutions. The
way any feature (such as a start, goal, or obstacle) is modeled in numerical potential
fields is through boundary conditions. The outer edge of the solution area is treated as an
obstacle creating a finite area the potential is defined within, or that the vehicle can drive
in. Section 2.1.2 mentioned two common types of numerical boundary conditions for
potential functions, Dirichlet and Neumann, which are examined here.
3.2.1 Dirichlet Boundary Condition
Dirichlet boundary conditions involve setting the velocity potential to a specific
value. This is the technique to model start and goal locations. The start is set to a low
potential and the goal high. This boundary condition can also be used to model obstacles
69
by making the obstacle potential higher than its surroundings. This concept is appealing
because it closely models Khatib?s original potential field idea [Khatib 1985]. However,
Dirichlet obstacles have several drawbacks described below.
The first test of the Dirichlet boundary condition (DBC) method is a potential field
created by a low potential start in the southeast corner and a high potential goal in the
northwest as shown in Figure 3.27. The start and goal potentials are negative and
positive one respectively (although as with the analytical solutions the magnitude is not
critical since the gradient direction is the important aspect). The world border (not shown
on the plot) uses Dirichlet boundary conditions with zero potential and no obstacles are
present. In order to avoid boundary condition conflicts when a start or goal lies on the
border an extra row or column of points are added to each edge of the solution area. This
extra set of points becomes the world border for the numerical solver. This technique of
increasing the grid size by one in each direction is used on each of the numerical
boundary conditions methods described in this dissertation.
70
Figure 3.27 Start and goal potential obstacle potential field, using Dirichlet boundary
conditions
As is shown in Figure 3.27, the velocity potential is very flat meaning that the effects
of the start and goal do not noticeably extend very far into the field. The equipotential
contour lines are shown logarithmically spaced to show the slight curvature near the
center of the field. Recall that the stream function is not calculated. Instead, the
streamlines are generated by numerically integrating the velocity vector field. Therefore,
71
the flat potential may cause numerical problems when calculating the velocity vector.
Because the gradient is so small, slight errors in the gradient calculation can cause large
changes in the direction of the velocity vector.
Beyond the numerical problems, several structural problems exist with the DBC
method. As with the analytical techniques, the flow is characterized by its saddle points
and corresponding separatrices. Although they are not obvious, there are two saddle
points at the northeast and southwest corners of the solution area. These streamlines do
not converge to a single point on the plot because of the extra row of grid points.
Streamlines contained within the separatrices intersecting these saddle points reach both
the start and goal, resulting in a vehicle that would drive where desired. However,
streamlines outside this area go outside the solution grid.
Also, note that the streamlines shown all converge to virtually a single line
approaching the start and goal. Many streamlines converging to a nearly single line is a
common feature when dealing with Dirichlet boundary conditions. In this case, it means
that even when a vehicle is following one of the streamlines in the area that intersects the
goal, near the start it can only drive in one direction. For this scenario, the problem could
likely be reduced by linearly varying the border potential near the start and goal to reduce
the gradient between the start/goal and the border. It is also possible to add more than
one extra row of solution points before the border boundary condition is applied. Either
solution should widen the possible departure angles near the start. However, solutions to
keep streamlines from merging are not as obvious for other scenarios.
When adding an obstacle, the potential value of the obstacle is an important design
parameter. Figure 3.28 represents an obstacle with zero potential, i.e. equal to the border.
72
The overall flow around the obstacle is similar to the no obstacle case shown previously
in Figure 3.27. This is not surprising since the obstacle potential is so close to the
surrounding potential. Streamlines outside the area defined by the separatrices
intersecting the border saddle points ( ) still do not intersect both the start and goal. The
critical difference between the flows is defined by the separatrices intersecting the saddle
points in the northeast and southwest quadrants of the obstacle. All streamlines inside the
area defined by these streamlines ( ) intersect the obstacle. This leaves a limited area in
which the vehicle can avoid the obstacle but still reach the goal. Effectively a super
obstacle is created reaching from the start to the goal and containing the desired obstacle.
However, a much larger section of the solution area is also contained completely dividing
the solution area.
Figure 3.28 Start, goal, and zero potential obstacle potential obstacle potential field, using
Dirichlet boundary conditions
73
Increasing the value of the potential function at the obstacle does not help the
situation. A potential field with an obstacle potential of 0.0002 is shown in Figure 3.29.
Notice that the obstacle saddle points move along the obstacle edge toward the start while
the border saddle points move toward the goal. However, the same three regions exist:
intersecting the border and either the start or goal ( ), intersecting the obstacle and either
the start or goal ( ), and intersecting both the start and goal ( ). If the obstacle potential
is increased much further, at least one streamline will intersect both the obstacle and the
border. This means that no streamlines intersect both the start and goal. Therefore, the
allowable range of obstacle potentials is quite small.
Figure 3.29 Start, goal, and positive potential obstacle potential field, using Dirichlet
boundary conditions
74
Expanding the concept to multiple obstacles (Figure 3.30) the problems become even
more apparent. As with each of the other flows, the separatrices intersecting these
singular points along with singular points on the border define the regions that will
intersect an obstacle, areas that intersect the border, and areas that intersect the start and
goal. However, more sub-regions exist for the flow: avoiding one obstacle but
intersecting another, etc. Note that where these sub-regions meet one often collapses to
virtually a single line, leading to very small allowable approach and departure angles
to/from the goal and start. Because these are caused by the interaction of the obstacles
and border, there is no easy solution to this problem.
Figure 3.30 Start, goal, and three obstacles potential field, using Dirichlet boundary
conditions
75
In the above example, each obstacle has its potential set to zero. Note how the saddle
points are in different locations on each of the obstacles: on the northern obstacle they are
drifting toward the leading edge of the obstacle, on the western obstacle they have
effectively combined into a single singular point, and on the eastern obstacle the saddle
points have combined and separated from the obstacle. Ideally, these saddle points
would be placed to more closely match the obstacle shape by varying the obstacle
potential. However, there is no simple method to determine the ideal potential for each
individual obstacle.
One possibility for simultaneously removing the need to choose individual obstacle
potentials and having some streamlines avoid one obstacle only to intersect another is to
remove the start location from the potential field as shown in Figure 3.31. In this case,
the obstacles and border are the lowest potential at zero, while the goal is still one. All
streamlines begin at either an obstacle or the border and all intersect the goal. Effectively
the border and obstacles are the source. Because the obstacles are at the lowest potential,
no flow approaches them. The downside of this method as compared to the other
Dirichlet methods is that only one streamline connects a given point in the field and the
goal. For example, if the southeast corner is the start location of the vehicle, only one
streamline passes through that point. With the previous methods presented in this
dissertation all streamlines pass through the start point, so many possible paths exist.
76
Figure 3.31 Goal and three obstacles potential field, using Dirichlet boundary conditions
Using Dirichlet boundary conditions to represent obstacles can satisfy the basic goal
of approaching a goal location and avoiding all obstacles. However, as seen, the
resulting potential fields often have undesirable characteristics. The super obstacles
generated are often much larger than the true obstacle and like using sources in analytical
flows (Section 3.1.1), they often extend to the start, goal, or both. The allowable driving
area also often collapses to a single line through the obstacles. While these problems
may be reduced (by modifying the border potential, individually optimizing the obstacle
potentials, or removing the start location) a larger structural problem still exists. The
potential created is virtually flat. Because the border is a uniform potential, most of the
potential field is also near that value. This makes numerically calculating the gradient to
produce streamline paths problematical.
77
3.2.2 Neumann Boundary Condition
The other boundary condition useful for path planning is the Neumann boundary
condition. With this boundary condition, the derivative of the potential function is
constrained instead of the potential value itself. In particular, to model fluid flow along
surfaces the normal velocity is set to zero. The main downside of this method is the time
to compute a solution due to the boundary and obstacle potential depending on the
surrounding potential.
An example of the Neumann boundary condition (NBC) method for a map with a
start and goal location but no obstacles is shown in Figure 3.32. The start and goal
locations are represented with Dirichlet conditions at negative and positive one
respectively, while the border uses a Neumann condition. Compared with the DBC
method (Figure 3.27), several features are immediately apparent. The potential is much
smoother in varying from the start to the goal, i.e. it does not flatten away from the start
and goal. This makes calculating the gradient more robust to numerical error. Like the
DBC method, the streamlines are numerically calculated by integrating the velocity
vector field. However, in this case all the streamlines stay in the solution area,
intersecting both the start and goal. This potential is much more like the corresponding
analytical solution shown in Figure 3.1, except for the fact that the solution is over a
limited area instead of a global space.
78
Figure 3.32 Start and goal potential field, using Neumann boundary conditions
Other benefits of the NBC method become apparent when an obstacle is added to the
flow as shown in Figure 3.33. The area of the solution that the streamlines avoid exactly
matches the obstacle shape. In addition, all streamlines still intersect both the start and
goal. Unlike the DBC method, no streamlines may leave the solution area.
79
Figure 3.33 Start, goal, and obstacle potential field, using Neumann boundary conditions
The main advantages compared to analytical methods become apparent when
multiple obstacles are in the solution area (Figure 3.34). Each of the obstacle edges is a
streamline. Because the entire field is computed at once, obstacles are not distorted by
the addition of later obstacles. Note that the obstacles can be any shape, although they
are circles in this example for simplicity. As with each of the other methods, the flow is
characterized by separatrices intersecting saddle points in the potential field. However,
unlike the DBC method the saddle points are always on the obstacle edge. Also unlike
the DBC method, where the separatrices determine what portion of the solution area
reaches the goal, with the NBC method the separatrices simply determine which side of
an obstacle the streamlines pass on.
80
Figure 3.34 Start, goal, and three obstacles potential field, using Neumann boundary
conditions
The NBC method produces the best potential field for vehicle path planning since any
obstacle shape can be represented exactly (limited only by the discretization of the
solution grid) without needing any other parameters from the flow and all streamlines
intersect both the start and the goal allowing a design parameter of the best streamline to
follow. The limitations of the NBC method are that it is computationally expensive and
the fact that streamlines are numerically calculated instead of contours of a stream
function.
There are several possible methods for calculating a stream function given a velocity
potential. The first method relies on the fact that the stream function is an exact
81
differential. Recall that streamlines are parallel to the velocity vector field. This implies
that
cnst.
N
E
V dNV dE
???
(3.11)
Along with the conservation of mass requirement given previously in Equation (2.3), this
means that the stream function is an exact differential [Rainville 1969] and can be
calculated by
,
ENVVNE????? ? ?
(3.12)
Notice that this is in fact the definition of the stream function, Equation (2.9). Examining
the north velocity equation,
? ?
NV dE f N? ? ? ??
(3.13)
where ? ?fN is the unknown integration constant function. For each index point, ? ?,jk ,
in the solution grid, the north velocity integral can be numerically approximated,
m in
,
j
k
E
j k NE
NN
g V dE
?
?? ? (3.14)
To calculate the integration constant function, the east velocity is used,
E g d fV N N d N???? ? ?
(3.15)
The partial derivative of the north velocity integral, g, can be numerically approximated
leaving the integration constant function to be approximated with numeric integration,
m in
,
k
j
N
j k E
N EE
gf V d NN
?
????????
???
(3.16)
82
Finally, the stream function is given by
gf??? (3.17)
This method works well in theory; however, there are two main issues that must be
resolved. The first is that extensive numeric integration and differentiation is being
employed on a potential field that already has some numerical errors due to the iterative
solver. The second is how to handle obstacles. The potential field interior to an obstacle
is not defined. Technically the obstacle interior is outside the solution grid. The numeric
integration must therefore reset across the obstacle gap. What value the integration
should reset to is not known. If the obstacles are represented by Neumann boundary
conditions then the underlying principle is that the stream function is constant over the
obstacle edge. For Dirichlet boundary conditions, the solution is not known. When these
issues can be disregarded, i.e. when no obstacles are present in the solution area, this
method does produce a workable stream function. However, this would rarely be the
case in path planning.
Several other methods exist for determining the stream function using properties of
analytic harmonic functions [Sarang 2007]. For instance, in early versions of Theoretical
Hydrodynamics Milne-Thomson describes one such method [Laitone 1977]. However,
this technique is absent in later editions of the book [Milne-Thomson 1996] and thus was
not able to be implemented. Many of these methods, however, require analytic solutions
for the velocity potential. In the potential field generation techniques used in this
research, if an analytic solution exists then the stream function is already known. Still
more techniques for calculating the stream function include stream surface methods
83
[Kenwright 1992] and combining distinct streamlines into a complete stream function
[van Wijk 1993]. For this research, however, the velocity potential is not necessary to
produce a path planning solution. Therefore, instead of using any of the methods
mentioned above to calculate the velocity potential and determine the stream function
from the velocity potential, it should be possible to develop a new method to calculate the
stream function directly.
3.2.3 Stream Function Boundary Condition
As Chapter 2 mentioned, the partial differential (or difference in the discrete case)
equation that underlies both the velocity potential and the stream function is the Laplace
equation. The difference between the two functions therefore lies in the boundary
conditions. In particular, start and goal locations, obstacles, and the world border need to
be represented for the stream function. To this end, a new stream function boundary
condition (SFBC) method is developed.
To model a start or goal location, characteristics of the analytical source or sink are
matched. In particular, for an analytic source streamlines all lead radially away from a
source as in Figure 2.2. Mathematically, the stream function at a point around a source is
effectively the angle of that point in polar coordinates centered at the source or sink. The
analogous concept for the numerical boundary condition is to linearly vary the stream
function of the points surrounding the start or goal location based on their polar angle.
This implies that the stream function will have a discontinuity. As with the analytical
source, the discontinuity is designed to be oriented facing directly away from the line
segment connecting the start and goal. The angle of each point surrounding the start or
goal is measured from the line segment connecting the start and goal, i.e. the line segment
84
is defined as 0 deg. Note that because of this definition, points have a different angle
when referenced from the start location as opposed to the goal. However, the different
reference frames are accounted for in the boundary condition definition. The
discontinuity is always at ?180 deg.
It is critical that the orientation of increasing stream function match the analytical
model so that the velocity vector field (and thus desired heading) will not be rotated by
180 deg. In particular, the stream function should increase counterclockwise at the start
and clockwise at the goal. Since the overall magnitude of the stream function is not
critical, ?180 deg is chosen to correspond with a stream function of ?1 and 180 deg
corresponds to a stream function of ?1. This implies that the polar angle increases
counterclockwise at the start and clockwise at the goal. Note that the streamline leaving
the start or approaching the goal along the line connecting the two points will have a
stream function of zero.
Because of the superior flow characteristics of the Neumann velocity potential
boundary conditions, the stream function boundary conditions at the world border and
obstacles are chosen so that the resulting flow matches the Neumann boundary condition
flow. Recall that for the Neumann boundary condition the normal velocity is zero. This
means that the stream function will be constant, i.e. the border is a streamline. As
described above, the stream function for points with negative polar angles measured from
either the start or the goal have a negative stream function value. The minimum stream
function is ?1 and should occur at the world border. Therefore, border points with a
negative polar angle are assigned a stream function of negative one. Similarly, border
85
points with a positive polar angle are assigned a stream function of ?1. This is
effectively a Dirichlet boundary condition on the stream function at the world border.
The concept of the stream function boundary condition for start and goal locations as
well as the world border is illustrated in Figure 3.35. The upper plot shows the boundary
points with assigned stream function values vs. the interior points, which are solved to
satisfy the discrete Laplace equation from Equation (2.22) (with the forcing function, G,
equal to zero). The lower two plots show the assigned stream function values for the
points immediately surrounding the start and goal as well as along the world border.
Figure 3.35 Start and goal boundary values for the stream function boundary condition
86
There are two important considerations when using this method to calculate the
stream function. The first is that for this research the start and goal location always lie on
the world edge (before the extra set of solution points are added as discussed in Section
3.2.1). This condition is not strictly necessary. Just as the stream function discontinuity
exists along an infinite line segment for the analytic source, the numeric boundary
condition could be applied to the sequence of points immediately next to the
discontinuity (positive or negative one depending on the side of the discontinuity).
However, for path planning it is rare that the desired behavior is to drive away from the
goal before turning towards it. Therefore, in most situations only including points
between the start and goal is a valid constraint for the solution grid. The second
restriction is that only one start and one goal exist. There is no obvious method for
placing the discontinuities for multiple starts or goals. Analytically the discontinuity
location does not affect the flow, but when determining correct border boundary
conditions it becomes more important. Having multiple starts or goals is not common in
vehicle path planning. However, one possible scenario would involve multiple robots
starting at the same location and going to different goals; this scenario is beyond the
scope of this research.
The complete potential field from the newly developed SFBC method is shown in
Figure 3.36 for the test scenario used above: start at the southeast corner, goal at the
northwest, and no obstacles. This figure is very similar to the Neumann boundary
condition (Figure 3.32). The difference is that in this case the stream function is
calculated instead of the velocity potential. This means that the equipotential lines are
87
numerically integrated from the velocity vector filed instead of the streamlines.
However, for path planning the velocity potential is not needed.
Figure 3.36 Start and goal potential field, using the stream function boundary condition
When an obstacle is added to the field (Figure 3.37), the obstacle edge is a streamline
just as with the world border. The difference between the border and an obstacle edge is
that the value of the stream function at the obstacle is not known a priori. More
specifically, one streamline will intersect the obstacle and the obstacle edge will take the
value of that stream function contour. To accomplish this, at each iteration of the solver
the stream function at the obstacle edge is set to the average of the stream function values
at the points immediately surrounding the obstacle. When the solution converges the
behavior of the stream function is as desired.
88
Figure 3.37 Start, goal, and obstacle potential field, using the stream function boundary
condition
When multiple obstacles are added to the field (Figure 3.38), the procedure is the
same as the single obstacle. During each iteration of the solver, the stream function at
each individual obstacle edge is set to the average of the points immediately surrounding
that obstacle. It is critical that the obstacles are handled individually. Individual
obstacles will very rarely have the same streamline intersect them; therefore setting the
stream function at the obstacle edge to the average of the points surrounding all of the
obstacles is incorrect. Note that as expected the behavior of the streamlines matches that
of the same scenario with Neumann boundary conditions shown previously in Figure
3.34.
89
Figure 3.38 Start, goal, and three obstacles potential field, using the stream function
boundary condition
The SFBC method satisfies both of the requirements for vehicle path planning: any
shape obstacles can be modeled and an explicit stream function is calculated. An
additional benefit of this new method is that its calculation time is much shorter than the
Neumann boundary condition that it mimics in behavior. Table 3.1 shows the average
calculation time for four solution methods: analytical Circle theorem, numerical Dirichlet
boundary condition, numerical Neumann boundary condition, and the new numerical
stream function boundary condition. For each test, Matlab is used to calculate the
potential field 30 times over a 101?101 grid and the calculation time is averaged. The
test is performed for both the no obstacle and three obstacle scenarios presented above.
90
Table 3.1 Potential field method calculation times
Method No obstacle computation time (s) Three obstacle computation time (s)
Circle theorem 0.0161 1.09
Dirichlet BC 0.667 0.642
Neumann BC 13.5 33.7
Stream Function BC 2.02 4.57
With no obstacles present, the CT method is obviously the shortest calculation time;
the calculation is a simple analytic function. However, when obstacles are added, the CT
method time increases dramatically, becoming longer than the DBC method time. This is
most likely due to the interpolations that must be performed to add obstacles to an
existing field. As expected the DBC method is the fastest of the numerical techniques
due to the boundary condition being fixed across iterations. The calculation time is
slightly shorter with obstacles simply because there are fewer interior points to solve in
the solution grid. The NBC method takes the longest to calculate because the world
border and obstacle edge potential vary with each iteration of the solver. The calculation
time is so long that real time implementation may be problematical with reasonable
computing power. The SFBC method developed in this dissertation takes longer to
compute than the DBC method due to the stream function on obstacle edges varying with
solver iteration. However, the computation time is still significantly shorter than the
NBC method and should be feasible to implement for real time path planning
applications.
Overall, several analytical and numerical techniques have been examined. A new
analytical method has been developed which meets the requirements of avoiding all
obstacles by combining circles to approximate generic shaped obstacles. However, this
91
method suffers when many circles are required to match the obstacle shape. A new
numerical solution developed using the stream function boundary condition is the best
technique to generate potential fields for vehicle path planning. Unlike analytical
methods, exact obstacle shapes can be represented. In addition, unlike the numerical
techniques that calculate the velocity potential, an explicit stream function is calculated.
This stream function is integral to the vehicle steering controller presented in Chapter 4.
Finally, the computation time is shown to be reasonable enough to allow real time
implementation on a vehicle.
92
4 CONTROLLER DEVELOPMENT
After the potential field method has been developed, the next critical algorithm is the
vehicle controller, which utilizes the potential field to move from the start to the goal. To
design the controller a linear vehicle model which represents the lateral error to a desired
streamline for given steer angles is utilized. The controller relies on both vehicle
response measurements as well as reference states determined from the potential field.
Section 4.1 presents the simplified model used in the steering controller. This section
also exposes a new issue with a vehicle becoming uncontrollable at a critical speed along
with methods to mitigate the problem. Section 4.2 expands the vehicle model to include
global position states and tracking a reference circle and demonstrates the method using a
test potential field.
4.1 Bicycle Model Vehicle Controller
Section 2.2 presented the vehicle model used for simulation purposes. This model is
simplified to the well known bicycle model [Dixon 1991] to use in controller design. In
addition to the assumptions made in the full vehicle model (constant speed and planar
motion), the bicycle model has two additional assumptions. First, the inner and outer
tires and slip angles are assumed to be identical and second, the equations of motion
shown in Equations (2.28) and (2.29) are linearized for small angles and a linear tire
model. Under these assumptions the system is described by the following model
93
2
22
1F R F R F
FF R F R
zzz
C C a C b C C
m V m V mV
aCa C b C a C b C rr
II I V
? ? ? ? ?
?? ? ? ?
?? ?
???? ??? ? ?
?? ???? ??
?????? ????
?????? ??
??????
?
? (4.1)
This model fits the common linear state space form,
??x Ax Bu? (4.2)
with the state vector
r????????x
(4.3)
and the input (scalar for this system)
u ?? (4.4)
The parameters for this model are given in Table 2.1. Note that for equation simplicity
the cornering stiffnesses in the bicycle model are per axle whereas Table 2.1 lists them
per tire. Neglecting weight transfer, the axle cornering stiffness is twice the tire
cornering stiffness (assuming two tires per axle).
The bicycle model is second order and, as with the nonlinear model, the
characteristics change with vehicle speed. The magnitude and damping ratio of the
bicycle model poles are shown in Figure 4.1. A transition speed exists where the models
shifts from over-damped to under-damped. This speed is defined as
? ? ? ? ? ?
? ? ? ?? ?
22 2 2 2
22
2
12 12
2
2
tr a n z F R F R F R
z F R z F R
V m I a b C C a C b C m a C b C
I C C m I a C b C
? ? ? ? ? ?
? ? ? ?
? ??? ? ? ? ?? ???
??? ?????
?
?
(4.5)
Note that the transition speed may be complex implying the model is over-damped for all
speeds. For the parameters used in this research, the transition speed is 8.5 m/s (19 mph).
94
In Figure 4.1, this is the speed at which the magnitudes of the two poles converge and the
damping ratio becomes less than one.
Figure 4.1 Bicycle model pole characteristics
4.1.1 Uncontrollable Speed
The magnitudes of the poles and zeros for the range of speed where the poles are real
are shown in Figure 4.2. Because the magnitude of the poles and zeros are all close
together, the lower plot shows the magnitude relative to the slower pole. The significant
aspect of this plot is that a critical speed (Vcrit) occurs at 5.83 m/s (13 mph) where one of
the poles cancels the zeros leaving a first order model. This is similar to the pole/zero
cancellation that occurs in neutral steer vehicles [Dixon 1991]. However, in this case the
cancellation is a function of speed as well as the vehicle parameters. Note that this
95
critical speed is not the same as the speed at which the bicycle model may become
unstable (also known as the critical speed) [Gillespie 1992].
Figure 4.2 Bicycle model real poles and zeros
The reason this pole/zero cancellation is important is that at the critical speed the
model becomes uncontrollable causing problems in designing the control gains. If a
system is controllable then control efforts exist to transition the system between any two
states in finite time [Bay 1999, Friedland 2005]. In terms of system performance, if a
system is controllable it is possible to design a state feedback controller such that the
96
closed-loop system eigenvalues can be chosen freely. A system is controllable if the
controllability matrix is full rank. The controllability matrix for the bicycle model is
? ?
? ? ? ?
? ? ? ?
2
2 2 2
22
2
F R FF R FF
z
F R FF R FF
z z z
aC bC m V aCC C CC
m V m m I V
a C b C aCaC bC CaC
I m I V V
V
I
? ? ?? ? ??
? ? ?? ? ??
????
?
??
??
??
??
?
?
?
?
CB B AB (4.6)
The critical speed is calculated by determining the speed at which the determinate of the
controllability matrix is zero:
? ?? ?
0
c r itVV
c r it
RzC a b m a b I
maV
?
?
?
?
?
? ?
CB
(4.7)
There are two interesting features about this equation. First, the critical speed will not
exist if the vertical moment of inertia is large enough compared to the mass and center of
gravity location. Specifically, if
zI mab? (4.8)
then the critical speed is imaginary, i.e. does not exist. This relationship is interesting
because for passenger vehicles the yaw moment of inertia is often approximated as
[Garrott 1988]
zI mab? (4.9)
This means that the critical speed is often zero or at least very slow. However, if this
relationship does not hold, as with the Corvette, then the critical speed should be taken
into consideration when designing a vehicle controller.
97
Another interesting point is that the critical speed depends on all vehicle parameters
except the front axle cornering stiffness. This by itself is not significant, but recall that
the bicycle model also has a transition speed from over-damped to under-damped which
is given in Equation (4.5). For the pole /zero cancellation to occur, the model must be
over-damped because it is not possible to cancel half of a complex pair. Therefore, the
critical speed must be below the transition speed.
For the same critical speed, it is possible to have multiple transition speeds by varying
the front axle cornering stiffness. The relationship between the transition and critical
speeds is shown in Figure 4.3. Notice that the minimum transition speed is the critical
speed. Initially there does not appear to be much in common between the critical and
transition speeds, Equations (4.9) and (4.5) respectively. However, when the derivative
of the transition speed with respect to the front cornering stiffness is set to zero, the
analytical minimum transition speed is the critical speed:
? ? ? ?
m in
0tran
F
Rz
tran
V
C
C a b m ab I
V
ma
?
?
?
??
?
?
?
?
(4.10)
As described above, it is necessary for the minimum transition speed to be at least the
critical speed. However, it is interesting that the minimum transition speed is the same as
the critical speed, particularly because the transition speed depends only on the state
matrix, A, while the critical speed depends on the input matrix, B, as well as the state
matrix. Although the relationship between the transition speed and critical speed is not
necessary to account for the uncontrollability, it is remarkable that mathematically the
98
two seemingly unrelated speeds are linked such that the pole/zero cancellation is
possible.
Figure 4.3 Bicycle model real/complex transition speed
An alternative method to study the controllability of the bicycle model is to study the
controllability gramian [Antoulas 2004, Bay 1999, Friedland 2005]. The infinite
controllability gramian is defined as
TT
0
e e d????? ? AAG BB (4.11)
It also satisfies the Lyapunov equation:
TT 0? ? ?AG GA BB (4.12)
In order for the system to be controllable, the controllability gramian must be non-
singular, which corresponds to the controllability matrix not being full rank. However,
the controllability gramian provides more insight than the controllability matrix because
along with a binary controllable/uncontrollable result, the controllability gramian can be
99
used to calculate the minimum energy necessary to transition between two states, i.e. for
the vehicle to move from one yaw rate and side slip to another. The minimum energy
required to transition from the null state to a given state, xd, is defined as
T1min ddE ?? x G x (4.13)
Notice that the control input to transition between the states is not needed to compute this
energy. The norm of the inverse of the controllability gramian for the bicycle model is
shown in Figure 4.4. As the speed nears the critical speed, the norm increases
dramatically, implying the energy necessary to transition to a desired state also increases
correspondingly. The uncontrollable problem is not isolated to simply the critical speed.
Even in the region around the critical speed, the energy required to control the vehicle is
not physically feasible.
Figure 4.4 Norm of inverse of bicycle model controllability gramian (Vcrit = 5.8 m/s)
100
4.1.2 Constant Closed-Loop Pole Controller
State feedback is used to demonstrate the effects of the controllability on the control
design. State feedback controllers have the form
??u Kx (4.14)
For this system the control gain matrix, K, is made up of the sideslip and yaw rate gains:
rKK???? ??K
(4.15)
For this example, these gains are chosen so that the closed-loop system
? ???x A BK x? (4.16)
has constant closed-loop poles at a frequency, ?n,des, and damping ratio, ?des, of 5 Hz and
0.707 respectively, i.e.
222 2 , ,2 d e s n d e s n d e ss s s? ? ?? ? ? ? ? ?I A B K (4.17)
The resulting control gains are shown in Figure 4.5. Note that at the critical velocity the
gains are undefined. More importantly, the gains grow very large in the region around
the critical velocity. The large gains correspond to the increased energy necessary to
transition between states.
101
Figure 4.5 Bicycle model control gains: constant closed-loop poles (Vcrit = 5.8 m/s)
To demonstrate the response when these control gains are used, the system is
simulated with a linearly varying speed. In this simulation a reference yaw rate, rref, of
10 deg/s is used. From the reference yaw rate, a reference sideslip, ?ref, and steer angle,
?ref, are calculated using the DC gains of the system:
1 re fre f
r
r
re f re f
rDC
DCDC
DC
?
?
?
??
? ??? ? ? ???
?? ?AB
(4.18)
These correspond to the steady state values necessary to maintain the desired yaw rate.
The controlled steer angle then becomes
r e f
r e f r r e f
rrKK
??? ??
??????? ???? ?
??
(4.19)
102
The motivation behind the feed forward formulation for the controller is explained in
more detail in Section 4.2. The vehicle plant for this simulation is the bicycle model
instead of the nonlinear model in order to eliminate any errors not due to the control
gains. The sample rate of the simulation is 100 Hz.
As the response shows in Figure 4.6, when the vehicle nears the critical speed (5.83
m/s) it begins to become unstable. The only reason the response returns to the desired
behavior is that the speed increases past the critical velocity. One interesting feature of
this test is that because the simulation is calculated at discrete time steps, the speed (and
thus gain calculation) does not occur at exactly the critical velocity. Because of this, the
closed-loop response should still match the desired response. Specifically, the difference
in the desired and actual closed-loop poles is never larger than 10?5.
103
Figure 4.6 Bicycle model response: constant closed-loop poles, varying speed
The reason for the discrepancy between the actual and desired response is that the
system parameters vary with time. Recall that time varying systems are not guaranteed to
be stable even if the poles have negative real parts at every time step [Slotine 1991]. This
fact is true for every vehicle controller designed from the bicycle model. However, it has
never been adequately studied for vehicle systems. For this system when the control
gains are reasonable, the varying speed does not have a major effect. However, when the
gains become large (and the variation between one time step and the next is
correspondingly large) the varying speed causes the system to behave erratically. To
104
justify this conclusion, a similar simulation is shown in Figure 4.7. The difference is that
in this simulation the speed is held constant at 5.82 m/s, very close to the critical velocity.
In this simulation, the response is as expected even though the control gains are very
large. However, the constant speed assumption is not invalidated as it is in the prior
simulation.
Figure 4.7 Bicycle model response: constant closed-loop poles, constant speed
Even when holding the speed constant there are other problems with the large control
gains near the critical velocity. For example, the same simulation with the addition of
zero mean Gaussian noise to the feedback states is shown in Figure 4.8. The standard
deviation of the yaw rate noise is 0.05 deg/s, typical for automotive grade gyroscopes.
105
The standard deviation of the sideslip noise is 0.36 deg. Although there is no typical
sideslip sensor, this is the noise when using GPS and a gyroscope to estimate sideslip
[Daily 2004]. When the speed is not near the critical velocity (left plots), the response is
acceptable. However, near the critical velocity the response is unacceptable (right plots).
The large control gains aggravate the uncorrelated noise on the two states and corrupt the
system response. The clean (no noise) response shown previously does not have this
undesirable behavior because the states are correlated causing the large gains to
effectively cancel out and produce a reasonable steer angle.
Figure 4.8 Bicycle model response: constant closed-loop poles, with noise
106
In general, a fixed closed-loop pole control is not acceptable even if the speed is not
precisely at the critical velocity because the large control gains near the critical velocity
drive the system unstable both by varying the model parameters and by exacerbating
measurement noise.
4.1.3 LQR Design
In order to reduce the effects of uncontrollable speed, a better method to calculate the
control gains is using LQR [Stengel 1994]. LQR designs the control gains to minimize
the cost function
? ?TT
0
J d t???? x Q x u R u (4.20)
where Q and R are the state and input weight matrices respectively. The control gain
matrix is calculated as
1T??K R B P (4.21)
Where P is the solution to the algebraic Riccati equation:
T 1 T?? ? ? ?A P P A P B R B P Q 0 (4.22)
For this example, the weights are chosen to be of one on sideslip, ten on yaw rate, and
one on steer angle, i.e.
10 1
0 10 R????????Q
(4.23)
Typically, it is more important to match the reference yaw rate than the sideslip;
therefore, the yaw rate weight is higher. Section 4.2 describes the actual weights used in
the streamline controller. The control gains resulting from these weights are shown in
107
Figure 4.9. Note that the control gains are acceptable for all speeds, even in the vicinity
of the critical velocity.
Figure 4.9 Bicycle model control gains using LQR design (Vcrit = 5.8 m/s)
Typically for an LQR design to be valid the system must be controllable. However,
in this case the design is able to compensate for the uncontrollability. As is shown in
Figure 4.10, one of the closed-loop eigenvalues intersects the pole/zero cancellation at the
critical velocity. From a systems point of view this makes sense. Recall that for a root-
locus the closed-loop poles always move from the open-loop poles to the open-loop zeros
(or infinity) as the loop gain increases. If there is a pole /zero cancellation, the closed-
loop pole must therefore be at that pole/zero location. In this scenario, LQR designs the
closed-loop poles to meet this requirement as is shown in Figure 4.10. Because an
attempt is not made to place the closed-loop poles in locations that are not feasible, the
108
control gains remain reasonable over the entire speed range. Note that in the lower plot
of Figure 4.10 the large closed-loop pole is not shown to concentrate on the poles and
zeros of interest in the cancellation.
Figure 4.10 Bicycle model and LQR closed-loop real poles and zeros
As before, these control gains are demonstrated in a simulation with increasing speed
(Figure 4.11). The same yaw rate reference (10 deg/s) is used and the reference sideslip
and steer angle are calculated using Equation (4.18), resulting in the controlled steer
angle including a feed forward term given in Equation (4.19). The only difference in this
simulation is that the LQR control gains are now used. Using the LQR gains the system
109
responds to the changing speed much better and the critical velocity does not cause the
erratic behavior observed with the fixed closed-loop pole controller.
Figure 4.11 Bicycle model response: LQR, varying speed
Alternative methods to LQR may also avoid the controllability problem. In particular
classical transfer function control does not need to consider controllability; however, care
would need to be taken. As can be seen in Figures 4.1 and 4.2, as the vehicle speed
increases the open-loop poles transition from real to complex and the relative magnitude
of the poles and zeros shifts, i.e. the zero becomes larger than one of the poles. The
movement in the open-loop pole and zero locations makes classical transfer function
110
control design techniques more difficult. Equivalent to transfer function control is output
feedback as opposed to full state feedback. Yaw rate may be considered as the only
measurement (similar to a transfer function from steer angle to yaw rate). However,
because only one closed-loop pole is being placed the other is floating, i.e. choosing the
control gain also chooses the second closed-loop pole. At some speeds, this second pole
may have a positive real part making the system unstable. For these reasons, LQR is
preferred over the complexities of classical control and output feedback.
4.2 Lateral Control to Reference Circle
The bicycle model controller alone is not enough to drive the vehicle. Higher level
states must be incorporated into the system to direct the vehicle to the goal position. For
typical potential field controllers, the direction of the gradient of the potential field, i.e.
the velocity vector field, is used as a desired vehicle course, ?ref. Note that the stream
function can be used in place of the velocity potential to calculate the velocity vector
field, i.e. Equation (2.9) instead of (2.6). Using only the potential field gradient as the
reference state has two main drawbacks. First, the path the vehicle will follow is
completely determined by its initial course, since the gradient is calculated at the
vehicle?s current location. At any point (other than the start or goal) a unique desired
course exists. Therefore, only one path will be taken to the goal. At the start or goal, the
desired course does not exist and thus a controller bypass must be employed. The
simplest workaround is to drive straight until a desired course exists. Note that although
the precise path cannot be chosen by the end user, all available paths do reach the goal.
A second and more troubling problem comes from the dynamic response of the vehicle.
111
If the velocity vector field is tracked exactly, then a streamline is followed. However,
this requires the ability to instantly follow the desired course. Typical steered ground
vehicles are not able to do this. As the bicycle (or nonlinear) model implies, there is a lag
between the steer angle input and the yaw rate response. This lag means that the vehicle
will not exactly track a streamline. Instead, there is a course error, which causes the
vehicle to drift from its initial streamline. The desired course would then be recomputed
at the new position as opposed to along the initial streamline, effectively meaning a new
streamline is being followed. Additionally, the change in desired course may be larger
than the vehicle can achieve, i.e. the vehicle cannot turn tightly enough.
For many scenarios, these issues will not cause trouble with the vehicle reaching the
goal. However, as is shown in Figure 4.12, these problems can combine to cause the
vehicle to intersect an obstacle. The ultimate cause of the collision is the vehicle not
being able to turn fast enough to follow the velocity vector field at the obstacle edge. The
velocity vector field is parallel to the obstacle along the obstacle edge as is dictated by the
boundary conditions. The reason the abrupt change is needed is that the vehicle
trajectory drifts from its initial streamline due to the lag in the vehicle response. This
streamline is initially followed because it matches the vehicle?s initial heading (pointing
directly at the goal location). While this scenario might seem like an unrealistic
alignment of factors that leads to the collision, as the number of obstacles increases, the
likelihood of a collision also increases.
112
Figure 4.12 Potential field gradient control
The solution to pure gradient following is to also track a desired streamline, ?des. In
terms of the control model, this requires an additional state, lateral position error, yerr. As
shown in Figure 4.13, the lateral error is defined as perpendicular to the vehicle?s
velocity. For calculating the change in lateral error, the desired path is assumed to be a
circle. This path allows a reference yaw rate to be used along with the corresponding
steer angle and sideslip as described in Section 4.1. Under this assumption, the equation
for the lateral error can be calculated by finding the intersection of the desired circle
(centered at Ec, Nc and with radius R) and the line perpendicular to the vehicle velocity
vector:
113
? ? ? ?
? ? ? ? 22
c os sin
sin c os
e r r c c
cc
y E E N N
R E E N N
??
??
? ? ? ?
? ? ? ? ?????
(4.24)
After much algebraic and trigonometric simplification the derivative of the lateral error
becomes
? ? ta ne rr e rr e rry V y ???? ?? (4.25)
Note that unlike the lateral error equation, the lateral error derivative does not depend on
the vehicle?s position or global course, only the lateral position and course errors. This is
important for having the system model vary with as few parameters as possible. As with
the bicycle model, vehicle speed is the only time varying parameter included in the
model. The lateral error derivative can also be linearized for small lateral position and
course errors for use in the linear control model:
err erryV??? (4.26)
114
Figure 4.13 Reference circle additional control states
As with the bicycle model controller, the circle tracking controller must be
formulated to drive a state to zero (as opposed to a reference state). For this reason, an
error state is defined as the difference in the reference and actual state:
re f
re f
e rr re f
re f
e rr
rr
y
??
??
???
???
? ? ? ???
??
x x x (4.27)
The reference states also match those used in the bicycle model controller, Equation
(4.18) with the addition of a reference course, ?ref. However, when tracking a reference
circle, the reference yaw rate is determined by the circle radius and vehicle speed:
ref Vr R?
(4.28)
V
y
e r r
?
e r r
R
E
c
, N
c
E , N
?
E
r e f
, N
r e f
115
As with traditional potential field controllers, the reference course comes from the
direction of the velocity vector field of the potential field at the vehicle?s location:
1tan E
ref NVV? ? ??? ????
(4.29)
Note that the east and north velocities in this equation are for the potential field, not the
vehicle. Unlike the other states in Equation (4.27), there is no reference lateral position;
the state is an error state directly.
The derivative of the error state is similar to the original state given (in linear form) in
Equations (2.33) and (4.1). Because the reference sideslip and yaw rate are computed as
the steady state values for a reference steer angle, their derivatives are zero. Note that
this assumption is exact when tracking a reference circle at a constant speed, i.e. along a
circle at a constant speed the yaw rate and thus steer angle and sideslip are constant. The
reference course, on the other hand, obviously varies when moving along the reference
circle. In particular,
ref refr? ?? (4.30)
Recall that the reference sideslip derivative is zero, which is why this Equation (4.30) is
different than the actual course derivative given previously in Equation (2.33). Also, note
that when the reference course is taken to be tangent to the reference circle, the change in
reference course is the reference yaw rate. In practice, the reference course comes from
the gradient of the potential field at the vehicle?s location, not the intersection point on
the reference circle. However, when the vehicle is on the reference circle this distinction
is meaningless as the potential field gradient is tangent to the circle.
116
Just as the states must be transformed to error states, the input must also be
transformed:
err ref? ? ??? (4.31)
As with the reference sideslip and yaw rate, the reference steer angle given in Equation
(4.18) is the steer angle required to keep the vehicle on the reference circle. The
linearized equations of motion for tracking a reference circle then become
2
22
2
1 0 0
00
00
00 0 0
F R F R F
FF R F R
ze r r e r r e r rzz
FF R F R
C C aC bC C
m V m V mV
aCaC bC a C b C
II I V
CC C aC bC
mVm V m V
V
? ? ? ? ?
?? ? ? ?
?? ? ? ?
?
???? ??
? ? ??? ??
??
?? ??
?? ??
??
?? ??
?? ??
?? ??
??
?? ????
xx?
(4.32)
As with the bicycle model controller shown in Equation (4.19), full state feedback
with feed forward is used to control the reference circle tracking system. However, in
this case the gain matrix is expanded to account for the additional states:
re f
re f
re f r y
re f
e rr
rr
K K K K
y
??
????
??
???
???
???? ?? ???
??
(4.33)
The uncontrollable speed of the bicycle model presented in the previous section is also
present in this system since it contains the bicycle model. For this reason, the gains for
the controller are designed with LQR principles, i.e. Equations (4.21) and (4.22). The
state and input weights were tuned by hand to yield acceptable system performance
without demanding inputs that are not physically realizable, resulting in
117
0 . 0 1 0 0 0
0 0 . 2 0 0 2
0 0 0 . 0 5 0
0 0 0 0 . 5
R
??
??
??
??
Q (4.34)
The other information the controller needs, of course, is the error state to be fed back.
The yaw rate, sideslip, and course can be measured directly from the vehicle. Lateral
error is calculated by numerically searching the stream function along the line
perpendicular to the vehicle velocity vector until the desired streamline (stream function
contour) is found. This is the reason an explicit stream function is required with this
control method. If only the desired streamline were known as opposed to the entire
stream function then this search becomes more difficult since it is harder to determine on
which side of the streamline the search point lies. Note that the stream function is only
defined at distinct points on a solution grid. The search points along the perpendicular
line do not fall on these grid points in general. Therefore, a 2D interpolation must be
performed for each search point. This can result in many interpolations for each steer
angle calculation. This interpolation is one of the computational bottlenecks when
implementing this controller in real time. The reference states and input are calculated
from the potential field using Equations (4.18), (4.28), and (4.29).
4.2.1 Reference Circle Calculation
For the reference yaw rate a turn radius is required. The circle that most closely
matches a curve at a given point is known as the osculating circle. The radius of the
osculating circle of the stream function contour is defined as [Weisstein 2008]
118
3222
222 2 2
222
EN
R
E N E N E N N E
??
? ? ? ? ? ? ?
????? ? ? ??
??? ? ? ???? ? ? ?
???
? ? ? ? ? ? ?? ? ? ???
? ? ? ?? ? ? ? ? ? ?? ? ? ?
(4.35)
The center of the circle lies along the line perpendicular to the curve:
22
22
c r e f
c r e f
R
EEE
EN
R
NNN
EN
?
??
?
??
?
???
??? ? ? ?
?? ? ? ?
??? ? ? ?
?
???
??? ? ? ?
?? ? ? ?
??? ? ? ?
(4.36)
where Eref and Nref are the coordinates of the intersection of the reference streamline and
the line perpendicular to the vehicle velocity vector given by
? ?? ?c os
sinre f e rrre f e rr
E E y
N N y
?
?
??
??
(4.37)
Note that each of the partial derivatives in Equation (4.35) must be computed over the
entire solution grid producing a radius and center for each point. At each time step, the
osculating circle parameters are interpolated at the point the desired stream and lateral
error line segment intersect. This interpolation is not as troubling as the lateral error
search since it only occurs once per steer angle calculation. Note that the radius could be
calculated at the vehicle location in the same manner as the reference course. As with the
desired course, the points converge when the vehicle is on the desired streamline. Also,
note that the radius is not necessarily positive. Equation (4.35) is actually the inverse of
the curvature of the osculating circle. The sign of the curvature determines the direction
119
of the center of the osculating circle, which in turn determines the sign of the reference
yaw rate:
? ? 1sign sign ta n c
re f cEEr NN?
?????? ?????
????
(4.38)
Calculating the streamline radius involves many numerical approximations. To
verify the accuracy of the approximation, the vortex potential given in Equation (2.16), is
used. The streamlines of vortex flow are circles centered at the origin. Therefore, the
radius is known exactly. The results of the true and calculated radius from the vortex
potential are shown in Figure 4.14. Over the majority of the solution grid, the difference
in the true and calculated radius is very small. The error grows in two locations: along
the solution edge and near the origin. At the solution edge, the error is due the numerical
nature of the partial derivatives. Ideally, the numerical first and second derivative
approximations used in Equation (4.35) rely on points on both sides of the calculation
grid point. On the solution edge however, points only exist interior of the calculation
point. This reduces the accuracy of the numerical derivative. However, the vehicle
should not be following a streamline near the edge of the solution grid. If the desired
path nears the solution boundary, then the grid should be expanded to allow room around
the desired path. The effects of the edge inaccuracy do not extend beyond the grid points
immediately on the edge of the solution grid. This error is the reason the radius error is
not shown on the solution edge.
The remaining large radius error is centered near the origin. This error is due to the
grid spacing. As the radius approaches the magnitude of the grid spacing, the numerical
approximations begin to break down. Notice that the largest errors (both positive and
120
negative) occur near the origin. The errors in the missing region around the origin grow
quite large, and so are not shown. This error only manifests itself when the radius
becomes small, e.g. at the singularity in the center of the vortex. While these errors may
be significant near streamline singularities such as the stagnation points on obstacles,
over most of the potential field the errors in radius are small enough to be ignored. The
stagnation points should not present a problem because the desired streamline should not
go that close to an obstacle regardless of this radius calculation.
Figure 4.14 Radius calculation validation
121
4.2.2 Controller Response
The vortex potential is also used to demonstrate the response of the complete
streamline tracking controller. For this simulation, the vehicle is initialized with ?0.01
stream function error, which relates to nearly 1 m of lateral error. The vehicle is due
north of the reference streamline facing east. The speed is held constant at 10 m/s and the
sample rate is 100 Hz. For this test, the bicycle model is used in place of the nonlinear
model to remove errors created by the vehicle model. However, the global position is
still computed using Equation (2.31), which is then used to calculate the reference states
and lateral error. The trajectory resulting from this simulation is shown in Figure 4.15.
Note that after ten meters (one second) the vehicle is essentially on the reference
streamline with zero tracking error.
Figure 4.15 Trajectory of the circle controller validation, with a linear vehicle model
The states and steer angle from the simulation are shown in Figure 4.16. Because
vortex flow matches the controller assumptions (i.e. tracking a reference circle) it is
122
possible to compare the actual response to that of the expected closed-loop system. Note
that the two lie on top of each other even though the actual system does not directly
simulate the lateral error state as the closed-loop system does.
123
Figure 4.16 States of the circle controller validation, with a linear vehicle model
124
The previous simulation is repeated using the nonlinear vehicle model with the results
shown in Figures 4.17 and 4.18. Note that although the response does not match the
closed-loop model, the settle time is approximately the same. The difference in the
responses (which is explored in more detail below) is due to the vehicle nonlinearities, in
particular tire saturation. However, the controller is still able to compensate for these
unmodeled dynamics.
Figure 4.17 Trajectory of the circle controller validation, with a nonlinear vehicle model
125
Figure 4.18 States of the circle controller validation, with a nonlinear vehicle model
126
To test the controller in a more realistic scenario, the map from the gradient
controller, Figure 4.12, is used. In particular, the map consists of a rectangular and two
circular obstacles between a start location to the southeast and goal location to the
northwest. The stream function is calculated using the method developed in Section
3.2.3. The vehicle starts directly facing the goal. The desired streamline is chosen to
smoothly avoid all obstacles. The simulation is run at a constant speed of 17.9 m/s (40
mph). As with the previous simulations, the sample rate is 100 Hz. Finally for the
simulation, the nonlinear vehicle model is used given in Equations (2.28), (2.29), (2.31),
and (2.32) is used. The trajectory of the vehicle is shown in Figure 4.19. Overall, the
trajectory looks reasonable. The vehicle deviates from the desired streamline in two
areas, near the start location and after passing the circular obstacle.
Figure 4.19 Trajectory of the streamline controller response
127
The system response is shown in Figure 4.20. This view of the controller is more
troubling than the trajectory. As expected, the reference state is not matched for two
periods. The first deviation is due to the initial vehicle state not matching the desired
streamline. Recall that this was one of the major down sides of the gradient tracking: the
trajectory depends entirely on the initial condition. However, with the streamline
controller it is possible to choose the desired trajectory. Also, note that during this period
the steer angle saturates at 30 deg. If the initial heading is not close to the tangent of the
desired streamline problems may occur. The second deviation is more troubling. This
deviation occurs after the vehicle passes the circular obstacle. Some tracking error is
expected due to the changing reference circle, i.e. the instantaneous streamline radius is
not constant. This changing reference can be seen in the reference yaw rate. Because the
speed is constant, the reference yaw rate is inversely proportional to the reference radius
as seen in Equation (4.28). In this case, however, the deviation from the reference states
is extreme. In particular, the vehicle sideslip reaches 30 deg and yaw rate peaks at
?65 deg/s. The vehicle is out of control even if it does eventually return to the desired
streamline. Notice that even the reference yaw rate reaches 30 deg/s and this is when the
vehicle behavior becomes erratic.
128
Figure 4.20 States of the streamline controller response
129
Ultimately, tracking the desired streamline in this scenario requires the vehicle to
perform outside its capabilities. The tire responses for the simulation are shown in Figure
4.21. Note that the tires become saturated during the simulation. For normal driving and
to not invalidate the controller assumptions, vehicle maneuvers should be restricted to
ensure that the tire forces remain in the linear region of the tire. An initial concept to
correct this problem might be to raise the LQR weights on the states so that the states
more closely match the reference and thus are in the linear region. However because the
reference state is outside the linear region, this would not correct the problem. The
solution must lie in modifying the reference states so that unreasonable behavior is not
expected and the vehicle limitations are respected.
Figure 4.21 Tire curves from the streamline controller response
In summary, a vehicle controller that tracks a desired streamline given a potential
field stream function has been developed. This controller accounts for a critical speed at
which the vehicle becomes uncontrollable by using LQR techniques to design the control
130
gains. The controller is designed around a system based on tracking a constant reference
circle. Methods for calculating the reference circle and other reference states have been
presented. This controller, and more precisely the reference trajectory, has the problem
of exceeding the physical limitations of the vehicle. Chapter 5 describes several methods
to modify the reference trajectory to accommodate the constraints imposed by the
vehicle.
131
5 PATH MODIFICATION
To account for the constraints imposed by the vehicle, three modifications are made
to the controller presented in Chapter 4: the vehicle speed is adjusted, lateral acceleration
is limited, and the reference streamline is shifted. Specifically, Section 5.1 presents a
method to approximate the distance to an obstacle using potential fields. This potential
field is then used to determine a reference speed for the vehicle. Additionally, this
section discusses a method to limit steer angle and speed when the vehicle exceeds a
lateral acceleration limit. Section 5.2 further improves the control algorithm by shifting
the reference streamline away from obstacles if the vehicle becomes too close. This
serves two purposes: it allows the vehicle to travel faster since the reference speed is slow
near an obstacle, and it increases safety since there is more space between the vehicle and
obstacles.
5.1 Reference Speed
Typically, potential field controllers use the magnitude of the potential field gradient
as the vehicle speed reference. In terms of the fluid dynamics scheme for the potential
field, this means the fluid speed is also the vehicle speed. This poses problems for
vehicle control. When two obstacles are close together, e.g. Figure 3.4, the streamlines
become compressed between the obstacles; this indicates an increased fluid speed. In this
scenario a vehicle should slow down. Far from the obstacles, the streamlines are spaced
132
out indicating a slow fluid speed, but the vehicle can safely speed up in these regions.
Based on these two examples, a solution may be to use the inverse of the gradient
magnitude as the vehicle speed. However, recall the stagnation points that exist on the
obstacle edges. At these points the fluid velocity is zero implying its inverse, the
proposed vehicle speed, is infinite. The obstacle edge is not the location for an infinite
speed singularity. Therefore, a new scheme is developed to determine the vehicle?s
reference speed.
Intuitively, a vehicle?s speed should decrease near an obstacle and increase with
distance from obstacles. Distance to a generic shape, however, is not always an easy
number to calculate. Alternatively, potential field techniques can be used to implement
the same intuitive concept. In particular, using Dirichlet boundary conditions the
reference speed can be calculated as a potential field, ?V. The potential is low (or zero)
on an obstacle edge and a max allowable speed on the world boundary. Presumably the
world boundary is far enough away from any obstacles that a high speed is safe. If this
assumption is not true, the solution area should be increased in size.
An example of the reference speed potential field is shown in Figure 5.1. Notice that
the reference speed is zero on the obstacle edges and increases with distance from the
obstacles to a maximum of 17.9 m/s (40 mph) at the world boundary. Note that this
potential is not proportional to the distance to the obstacle. Recall that Dirichlet
boundary conditions correspond to sources or sinks in analytical flow. From Figure 2.2 it
is obvious that the velocity potential of a source does not vary linearly with distance from
the source location. The same is true with Dirichlet boundary conditions. Even though
133
this potential is not proportional to distance to an obstacle, it serves the same purpose of
creating a low reference speed near obstacles and a high speed far from obstacles.
Figure 5.1 Reference speed potential using Dirichlet boundary conditions
Incorporating the reference speed into the vehicle controller means that two potential
fields must be calculated, the reference streamline and the reference speed. However as
is demonstrated in Table 3.1, calculating a potential field with Dirichlet boundary
conditions does not add appreciably to the overall computation time. Additionally, the
vehicle?s response to the reference speed must be included in the simulation. To this end,
Equation (2.30) is used to calculate the actual vehicle speed given a reference speed
input, Vref. The reference speed input is interpolated from the reference speed potential,
?V, to the vehicle?s location.
134
To test the reference speed addition to the controller, a vehicle is simulated in the
same test map used previously, but with the reference speed potential shown in Figure
5.1. The trajectory resulting from this simulation is shown in Figure 5.2. Notice that
unlike the previous simulation (Figure 4.19), the vehicle is able to follow the desired
streamline after passing the circular obstacle. However, the initial error is still present.
Because the vehicle is not initially near any obstacles, its reference speed is not
drastically modified from the initial 17.9 m/s (40 mph) and the response is effectively the
same. In other words, modifying the reference speed is designed to mitigate errors near
obstacles, not the transient errors due to initial conditions.
Figure 5.2 Trajectory of the streamline controller response, adjusting speed
The vehicle response to this simulation is shown in Figure 5.3. Note that the vehicle
speed significantly drops as the obstacles are passed. Also, note that unlike the constant
135
speed simulation, the reference states are tracked (after the initial transients die out).
Reducing the speed near obstacles serves two purposes. First, it increases safety by
slowing down near obstacles as a human driver would. Second, it produces lower speeds
in the areas most likely to require tighter maneuvering. The most drastic changes in the
direction of the potential field gradient occur near singular points or sharp obstacle
corners. Recall that the stream function boundary condition only produces singular
(stagnation) points on the obstacles edges. Therefore, the majority of sharp turns in the
reference trajectory occur near obstacles. The vehicle is better able to negotiate the sharp
turns if it is moving slowly. The world boundary corners also produce sharp turns
because the boundary is treated like an obstacle. However, as has been mentioned, the
vehicle should not be following a streamline near the world boundary. The increased
maneuverability at low speeds is the reason the vehicle is able to track the desired
streamline when the reference speed is modified (which it was not able to accomplish
when driving at a high constant speed).
136
Figure 5.3 States of the streamline controller response, adjusting speed
137
Another interesting feature of this system response is the slight oscillation in the steer
angle between 30 and 45 seconds. This is due to the low vehicle speed during this time.
Recall from Figure 4.1 that the natural frequency of the vehicle model increases as speed
decreases. Because of this, the controller is too aggressive at low speeds, i.e. the gains
are too high. The solution to the problem would be to lower the LQR weights on the
states. However, in order to concentrate specifically on the interaction of the path
planner and controller, in this research a single set of LQR weights is used. This results
in the oscillation observed in the response.
The last feature of this response is the transient dynamics. During the transient phase
of the response, the steering angle still saturates at 30 deg and the lateral acceleration is in
excess of 1 g at times. The maximum lateral acceleration possible for a Chevrolet
Corvette (such as the vehicle simulated in this work) is 0.93 g, so the simulation results
reinforce the nonlinear vehicle model (although the model parameters are slightly
incorrect resulting in the higher lateral acceleration). However, this much lateral
acceleration is not safe vehicle behavior. As the tire curve from the simulation in Figure
5.4 shows, to create the 1 g of lateral acceleration the tires saturate, just as with the
constant speed simulation. Even though the trajectory is followed, the extreme vehicle
response is not acceptable.
138
Figure 5.4 Tire curves from the streamline controller response, adjusting speed
To keep the tire behavior in the acceptable range as well as to prevent the vehicle
from rolling, a limit is imposed on the lateral acceleration. To guarantee the vehicle does
not exceed the lateral acceleration limit, both the steer angle and the reference speed are
limited. In particular, lateral acceleration can be approximated in steady state as
yra Vr V D C ??? ? (5.1)
where the DC gain comes from Equation (4.18). If the lateral acceleration from Equation
(5.1) exceeds a safe limit, the steer angle is reduced to maintain the lateral acceleration at
the safe limit:
? ?m a x
m a x
lim
if
si gn
ry
y
r
V DC a
a
V DC
?
??
?
?
?
?
(5.2)
Additionally, when the lateral acceleration limit is exceeded, the reference speed is
limited:
139
m a x
, li m
i f &r y re f
re f
V DC a V VVV? ???? (5.3)
The combination of these two limitations together allows the vehicle to turn as much as
possible without becoming unsafe. Beyond the potential field path planner and
controller, this limit or one similar to it should be included in any lateral vehicle
controller to prevent extreme behavior.
The simulated response including the lateral acceleration limit is shown in Figures
5.5-5.7. For this simulation, the lateral acceleration limit is set to 0.5 g. For the most part
the response is identical to the previous case without the lateral acceleration limit. The
only difference is in the initial transients. The vehicle converges to the desired streamline
in four seconds as opposed to three seconds when the lateral acceleration is not limited
and the maximum lateral error increases from 2.5 m to 3 m. Notice that during the
transients the lateral acceleration is held at the limit as the algorithm is designed to do.
Because the lateral acceleration is limited, the lateral tire forces do not saturate. In
general, the lateral acceleration limit can be adjusted based on the desired aggressiveness
of the vehicle controller and the terrain conditions. In particular, methods exist to
estimate the peak lateral tire force as the vehicle drives [Daily 2007]. The lateral
acceleration limit can then be adjusted in real time based on the peak force so that the tire
forces do not saturate regardless of the terrain.
140
Figure 5.5 Trajectory of the streamline controller response, limiting lateral acceleration
Figure 5.6 Tire curves from the streamline controller response, limiting lateral
acceleration
141
Figure 5.7 States of the streamline controller response, limiting lateral acceleration
142
5.2 Reference Streamline Shifting
With the reference speed potential and the lateral acceleration limit, the vehicle
controller safely navigates the map of obstacles while keeping the vehicle within
acceptable behavior limits. The last modification to the controller involves shifting the
desired streamline away from obstacles as the vehicle is driving. There are two main
reasons for this modification. The first is to increase the vehicle speed through the
obstacle field. Recall from the previous section that reference speed increases with
distance from obstacles. Therefore, when the reference streamline is shifted away from
the obstacle the vehicle speed increases. The second reason to shift the reference
streamline is to keep the vehicle a safe distance from obstacles. Although all streamlines
reach the goal without intersecting any obstacles, they may pass quite close to an
obstacle. By shifting the desired streamline away from the obstacle, a factor of safety is
added to the controller to prevent collisions with obstacles due to tracking error. Ideally
the initial reference streamline is chosen with this factor of safety. However,
occasionally a single streamline may not be able to avoid all obstacles with a safety
buffer. Additionally, if new obstacles are detected and the potential field is modified,
then there is no guarantee on how close the reference streamline comes to obstacles.
Ultimately, allowing the reference streamline to shift in real time reduces the dependence
on the initial reference streamline choice.
To accomplish the reference streamline shifting, the reference speed potential
developed in Section 5.1 is used. This potential varies with distance from the obstacles
and its minimum is at the obstacle edges. The gradient of the reference speed potential
points away from the obstacles as shown in Figure 5.8. The reference streamline is
143
shifted in the direction of this gradient and thus away from the obstacles. The purpose of
shifting the reference streamline is to keep a buffer between the vehicle and any
obstacles. Therefore, when the vehicle is already far from obstacles, the reference
streamline is held constant. The reference streamline only shifts when the vehicle is near
an obstacle. This aids controller performance since it is easier to track a constant
reference rather than one that is continuously being updated. This threshold is
implemented by only shifting the reference streamline when the reference speed, which is
correlated with distance to an obstacle, is below a limit. The threshold in Figure 5.8 (and
the remaining simulations) is 4.47 m/s (10 mph).
Figure 5.8 Reference speed gradient
144
An example of the reference streamline shifting is shown in Figure 5.9. The control
point on the reference streamline (Eref, Nref) is calculated using Equation (4.37). The
direction of the reference speed gradient is calculated at the control point:
1tan V
V
EN?? ?? ????? ?????? (5.4)
The distance the streamline is shifted is
22VV
pos K EN? ??? ??? ? ? ?? ? ?? ? ? ?? ? ? ?
(5.5)
where K? is a tuning parameter to determine how much the reference streamline shifts
each time step. Note that as the magnitude of the gradient increases so does the amount
the streamline shifts. From this information, the point on the new reference streamline is
calculated:
? ?
? ?
,
,
c os
sin
V
ne w re f pos re f
V
ne w re f pos re f
E E E K E
N N N K N
??
???
???
?? ? ? ? ?
?
?? ? ? ? ?
?
(5.6)
The new reference streamline, ?ref,new, is the contour of the stream function that passes
through this point, i.e. the value of the stream function at this point.
145
Figure 5.9 Reference streamline shift near an obstacle
Note that the streamline shifting bears some resemblance to Khatib?s or other
Dirichlet type potentials, [Khatib 1985, Kholsa 1988, Sato 1993]. In particular, the
reference streamline is moved directly away from obstacles. However, the fact that the
reference streamline is being repelled from the obstacle instead of the vehicle itself is an
important distinction. The vehicle control still comes from the stream function potential.
In particular, the reference course is calculated at the vehicle?s position and therefore is
not affected by the reference streamline shift. The reference yaw rate, steer angle, and
sideslip all come from the osculating circle at the control point on the reference
streamline. These values change when the reference streamline shifts, but if the shift is
small then so are the corresponding changes in reference states. The term that is most
directly affected is the lateral error. Near an obstacle, the streamlines are tangent to the
? ?
p o s
?
E , N
E
r e f
, N
r e f
?
r e f
?
r e f , n e w
E
? , n e w
, N
? , n e w
V
y
e r r
O b s t a c l e
146
obstacle edge. If the controller is working well, the vehicle course will also be tangent to
the obstacle edge. This means that the lateral error is nearly normal to the obstacle edge.
Due to the nature of the Dirichlet boundary condition used in the reference speed
potential, its gradient is also nearly normal to the obstacle edge. Therefore, these two
terms effectively add together to produce the new lateral error. Because of the other
reference states remaining nearly constant however, the vehicle continues to drive along
the reference streamline while also being pulled away from the obstacle. In pure
Dirichlet potential field control, the control effort drives the vehicle directly away from
the obstacle.
The simulated response including the reference streamline shifting is shown in Figure
5.10. For this simulation the streamline shift gain, K?, is 0.08 m?s. Recall that the
reference speed decreases near obstacles. Therefore, when the reference speed is over
4.47 m/s (10 mph), i.e. the vehicle is away from obstacles, the reference streamline is
constant. Inside the speed threshold the reference streamline changes. Around the
rectangular obstacle, the shift is minor since the vehicle is far enough away from the
obstacle that the gradient is small. However, as the vehicle begins to approach the
circular obstacle the streamline shift is much greater.
147
Figure 5.10 Trajectory of the streamline controller response, shifting the reference
streamline
The vehicle response for this simulation is shown in Figure 5.11. This response is
similar to the constant reference streamline response (Figure 5.7) with the obvious
exception of when the reference speed is below 4.47 m/s (10 mph). In this region, the
reference streamline, i.e. the stream function value, shifts. In particular, the value
initially decreases then increases. Recall from Section 3.2.3 that a stream function of ?1
corresponds to the world border clockwise from the start, e.g. northeast corner, whereas
?1 corresponds to counterclockwise from the start, e.g. southwest corner. Therefore, the
reference streamline initially moves to the right (from the vehicle?s perspective) then to
the left. This corresponds to the trajectory shown in Figure 5.10. Another interesting
feature of this response is that when the reference streamline is shifting there is tracking
148
error. This is not unexpected since changing references often result in tracking error
(depending on system type, etc.). For this controller however, the tracking error does not
cause any problems. The controller is innately knowledgeable of the fact that no
obstacles are nearby due to the potential fields. Therefore, the tracking error is
acceptable. Once the reference speed increases past the threshold, the reference
streamline remains constant and the vehicle quickly converges to it.
149
Figure 5.11 States of the streamline controller response, shifting the reference streamline
150
Changing the streamline shift gain can alter the behavior of the controller. For
instance, the same test scenario with a shift gain of 1 m?s is shown in Figure 5.12. In this
simulation, the shift gain is large enough to overcome the controller reference states and
force the vehicle to go to the outside of the obstacles. In particular, the shift gain is high
enough to force the reference streamline to follow the reference speed shift threshold.
The streamline shift gain effectively acts like an additional control gain. This gain
determines the importance of keeping a high reference speed vs. the other control states.
For instance, with the high shift gain the vehicle took 45.18 seconds to reach the goal
with an average speed of 6.82 m/s (15.3 mph) whereas the low shift gain required 50.25
seconds with an average speed of 5.87 m/s (13.1 mph). Notice that both are faster than
when the reference streamline is constant; this simulation had a time of 54.06 seconds
and an average speed of 5.34 m/s (12 mph). If minimizing distance travelled is critical
then a lower shift gain is better so that passing between obstacles is allowed. On the
other hand, if minimizing travel time is important, a higher shift gain may be better to
keep the vehicle speed higher. Note that a higher shift gain does not guarantee a shorter
travel time. In particular, the increased travel distance to go outside the obstacles may
outweigh the increased speed resulting in a longer travel time.
151
Figure 5.12 Streamline controller response with a large streamline shift gain
A dense obstacle grid simulation is shown in Figure 5.13. In this scenario, twelve
20?40 meter rectangles are spaced 20 meters apart. These could represent buildings
separated by roads, although as the trajectory shows the vehicle does not obey traffic
laws. This simulation demonstrates the controller?s ability to handle dense obstacle
fields. There are two interesting points about this simulation. First, the boundary
condition on the reference speed potential at the obstacle edges is set to 2.24 m/s (5 mph)
as opposed to zero in the previous simulations. This is so that the vehicle does not stop
when passing between obstacles. Recall that potential fields with a Dirichlet boundary
condition become flat when a single boundary value dominates the potential field (as
shown in Figure 3.27). This is the case for the reference speed potential between the
obstacles. If the reference speed is zero at the obstacle edges, then between obstacles the
152
reference speed potential is effectively zero and the vehicle stops. The other feature of
this simulation is how close the vehicle comes to the obstacles. The cause of this is also
related to the flat reference speed potential. The streamline shift gain is set to 0.08 m?s as
in Figure 5.10. However, because the potential is essentially flat in this scenario, the
magnitude of the gradient is small and the reference streamline is not appreciably shifted.
Figure 5.13 Streamline controller response for a dense obstacle field
In this chapter, the streamline controller has been modified, making it more effective
and safer. In particular, a lateral acceleration limit is imposed on the vehicle by limiting
steer angle and longitudinal acceleration. The reference speed sent to the vehicle is also
modified to vary as the vehicle progresses through the course. The reference speed
increases with distance from the obstacles. This allows the vehicle to maneuver tighter
near obstacles and increases safety by slowing down in the proximity of obstacles.
153
Finally, a mechanism to shift the reference streamline that the vehicle is following away
from obstacles has been developed. This modification also increases safety by creating a
buffer between the vehicle and obstacles. It may also decrease the time to reach the goal
by keeping the reference speed higher. At this point, a successful path planner and
controller have been developed. Additionally, several test scenarios have been shown
where the vehicle negotiates an obstacle field to reach a goal.
154
6 CONCLUSIONS
This work has developed a harmonic potential field based path planner and controller
for navigating a vehicle through an obstacle field to a goal location. The potential fields
provide a method to combine the path planning and control routines into a single
algorithm. The control effort to the vehicle comes directly from the potential field
instead of through a reference path. This allows the vehicle controller to be more aware
of the vehicle?s surroundings and therefore make better decisions. To this end, a new
method of calculating a potential field was developed. Based on this potential field, a
controller was designed to drive the vehicle to the goal location. This base controller was
then modified to account for vehicle dynamic limitations and increase safety.
6.1 Overall Results
The potential field used in this research was derived from fluid dynamics concepts.
In particular, it satisfies the Laplace partial differential equation. Solutions of the
Laplace equation (harmonic functions) are free of extrema thus the vehicle is guaranteed
to reach the goal. Varieties of design methods to generate a potential field that satisfies
the Laplace equation and represents the physical world the vehicle is maneuvering in
were presented. These methods fall into two categories, analytical and numerical. Each
of the potential field design methods was judged based on three requirements of the
vehicle controller: the ability to accurately represent generic shaped obstacles, the
155
presence of an explicit stream function for use in the vehicle controller, and the
calculation time necessary to compute the potential field.
Two new methods of generating the potential field were developed. The first method
is an analytic solution. This method uses a weighted average of individual obstacle
potential fields to combine circular obstacles and roughly approximate the shape of a
physical obstacle. This method satisfies the three judging criteria. However, its behavior
outside these criteria often becomes unacceptable. In particular, the reference paths the
vehicle follows are convoluted near obstacles and often needlessly approach obstacles
before veering away. The second method involves a numeric solution of the stream
function (as opposed to the velocity potential most numeric solutions calculate). This
method exactly (to the resolution of the solution grid) represents any shape obstacles and
satisfies the other judging criteria. Because of the ability to exactly represent any shape
obstacle and the smoother streamlines, the numeric stream function potential field was
chosen as a more effective path planning method.
Typical potential field controllers rely solely on the gradient of the potential field to
direct the vehicle to the goal location. This works well for robots that can
instantaneously track the changing course defined by the gradient. However, for steered
vehicles the dynamic response and vehicle limitations, e.g. turning radius, make exact
gradient tracking problematical. To correct this problem, a vehicle controller that tracks
a reference streamline in addition to the potential field gradient was designed. This
controller is implemented as a full state feedback controller designed around the bicycle
vehicle model with two additional states of course and lateral error based on tracking a
reference circle. It was shown that the bicycle model becomes uncontrollable at a critical
156
speed. The cause of the uncontrollability is a pole/zero cancellation in the system model.
To counter the uncontrollability, LQR techniques are used to calculate the control gains.
LQR inherently places one of the closed-loop poles at the pole/zero cancellation.
Therefore, the control gains remain reasonable even at the critical speed.
The vehicle controller was developed with a feed forward term so that there was no
tracking error to the reference circle (assuming no model uncertainty or disturbances).
The reference states used to calculate the feed forward steer angle and state error are
calculated from the potential field. In particular, the reference course is the direction of
the gradient of the potential field, the reference yaw rate is calculated from the osculating
circle of the reference streamline, and the reference sideslip and steer angle are the steady
state values required to hold the reference yaw rate. Although the numerical stream
function potential field is used in this research, the controller will work with any potential
field that contains an explicit stream function. In simulation, this controller was shown to
work for test scenarios that matched the model assumptions, i.e. the streamlines were
circles, as well as on a test obstacle field. While this controller drove the vehicle to the
goal, it did so in a method that was not safe for the vehicle. This extreme behavior
motivated changes to the base controller.
Therefore, three additions were made to the base vehicle controller to remove the
vehicle?s severe response: the reference speed was modified, the lateral acceleration was
limited, and the reference streamline was shifted. The purpose of modifying the
reference speed is to force the vehicle to slow down as it approached obstacles. This
obviously increases safety by reducing speed when the chance of a collision is increased,
but it also increases the vehicle?s maneuverability near obstacles where tighter turns are
157
often required. To implement the reference speed, a second potential field was created.
This potential is low near obstacles and high at the world boundary. Using this method,
the solution grid must be chosen such that the world boundary is far enough away from
the obstacles to make higher speed driving safe. The reference speed sent to the vehicle
speed model (derived from a simplified cruise controller) is the value of the reference
speed potential at the vehicle?s location. This modification to the controller reduces
unwanted vehicle behavior near obstacles. However, it does not affect the behavior far
from obstacles (usually due to initial conditions). To guarantee that the vehicle remained
within its safety limits (i.e.to prevent roll over or sliding) a lateral acceleration limit was
also imposed. This limit is imposed by restricting the steer angle the controller can
command. Additionally, when the vehicle is at this limit the reference speed is not
allowed to increase. This effectively keeps the vehicle in the linear tire region and
removes extreme handling behaviors.
The last modification made to the controller was a method to shift the reference
streamline when the vehicle is near an obstacle. This shifting is based on the reference
speed potential since that potential is correlated to the distance to an obstacle. If the
reference speed potential is below a threshold, i.e. the vehicle is near an obstacle, then the
reference streamline is shifted in the direction of the gradient of the reference speed
potential, i.e. away from the obstacle. This modification both increases safety by creating
a buffer between the vehicle and obstacles and decreases time to the goal location by
keeping the reference speed high since the reference speed is reduced near obstacles.
Throughout the dissertation, the various controllers were shown navigating a standard test
course. In addition, the complete controller was demonstrated on a test course densely
158
populated with obstacles. Once the modifications were made to reduce the
aggressiveness of the controller, it performed well in both tests.
6.2 Future Work
Although a successful path planner and controller are presented in this dissertation,
there remain several areas where both the potential field and the controller can be
researched further. For the potential field, as with the work presented in this dissertation,
areas of investigation are present in both the analytic and numeric solutions. There are
also areas that could be researched that apply to both methods. Possible improvements to
the controller exist for both the structure of the controller and for its implementation.
6.2.1 Potential Field
Several broad areas could be further researched with regard to the potential field. The
first is investigating other differential equations to generate the potential field. In
particular, other research has initially investigated biharmonic functions (fourth
derivatives instead of second as with the Laplace equation) [Masoud 1994]. However,
these techniques have not been studied in the more advanced controller presented in this
research. Biharmonic potential fields may produce smoother streamlines that are easier
for a vehicle to follow. Another area of major interest is moving obstacles. These have
been studied for analytic circular obstacles, but not for the broader case of general shaped
obstacles, or numeric solutions. It has been shown that simply updating the obstacle
location every time step and assuming it is constant is not sufficient [Waydo 2003]. A
technique that is becoming popular for obstacle avoidance is probabilistic obstacles.
These techniques acknowledge that the obstacle and vehicle position are not known
159
exactly. Incorporating these techniques into the potential field methods would be a major
improvement. Finally, a method to represent not only obstacles but also terrain types in
the potential fields would be beneficial. For example, a steep hill may be possible to
navigate, but only slowly. Incorporating a scale of drivability into the potential field
would allow for more advanced navigation than simply binary (i.e. yes/no) obstacles.
For the analytic potential fields, the improvements center on alternative methods to
define obstacles since none of the methods studied in this work were entirely successful.
In particular, this research concentrated mainly on combining circles to create generic
shaped obstacles. However, fluid dynamicists often use conformal mapping techniques
to transform the flow around circles into flow around other shapes. This is a common
method used to analyze airfoils. These same techniques may allow circles to be
transformed into common obstacle shapes. However, problems would still exist with
obstacles corrupting the edge streamline of previously added obstacles. Another
possibility to form an analytic potential field is to place a series of doublets directly in the
flow. Recall that doublets are formed inside the circular obstacles. For that reason, it
should be possible to insert a series of doublets into the flow instead of a series of circles.
When creating a doublet, two parameters must be defined: strength and direction. If the
doublet method is to be pursued, a technique must be developed to calculate these
parameters to match a physical obstacle. Another interesting method to represent an
obstacle is using a distributed source to model flow around a polygon. However, as
described in Section 3.1.1, sources create unwanted wakes downstream of the obstacle.
A possible fix for the wake may be to add an equivalent sink inside the polygon obstacle.
160
For the numeric techniques, the stream function calculation method appears to
successfully navigate around obstacles. Improvements to the method are either to relax
the assumptions this work makes or to improve the solution technique. The stream
function potential makes two assumptions about the start and goal location: they are
along the world boundary and there is only one of each. Moving the start and goal away
from the world boundary is theoretically not difficult. A discontinuity is created along a
line segment, preferably facing away from the other start or goal location. Although not
theoretically difficult, implementing this may be more complex. Determining which side
of the line segment a point is on is not difficult. However, the value on the boundary is
more problematical. Either a single line of points must create the boundary condition in
which case the boundary condition is different on opposite sides of the point (i.e. the
point actually has two boundary conditions), or a region of points around the line
segment must be used to set the boundary condition. Additionally, the discontinuity
would need to be accounted for when computing the gradient (i.e. the velocity vector
field).
Adding multiple starts or goals is also problematical. It is not immediately obvious
what the boundary condition should be, in particular, the location of the discontinuity.
For an analytic source, the discontinuity location does not matter. The same should hold
true of the numeric approximation. This implies that the world edge boundary condition
switches signs when each of the discontinuities is crossed along the edge, which does not
seem intuitive or trivial to account for.
One major area that deserves study in all potential field methods is the grid generation
technique. With fluid dynamics PDE solvers, creating the solution grid is often a large
161
portion of the computation time. Using a non-uniform grid both speeds computation time
and increases accuracy. The computation time is improved by reducing the grid size in
areas that are open and do not need many grid points to define. However, around
obstacles, in particular, many grid points would be used both to precisely define the
obstacle shape and to capture the larger changes in the potential field that occur around
obstacles.
The last major area that should be studied is adding obstacles to the potential field in
real time. In practical applications, the complete world map will not be known until the
vehicle begins to navigate through the course and sensors detect the obstacles.
Intuitively, numeric potential fields are well suited to handle additional obstacles. The
potential field before an obstacle is detected would serve as the initial guess for the
updated field. If the changes are minor, the solution should converge quickly. However,
for the convergence to occur in real time, better PDE solvers may need to be employed.
This research used custom developed software to calculate the potential field. PDE
solvers are another aspect of computational fluid dynamics that have seen a large amount
of improvement. Applying more complicated (and faster) solvers to the potential fields
used in path planning may extend their use into dynamic real world environments.
6.2.2 Vehicle Controller
There are several possible improvements that could be made to the controller
presented in this dissertation. The main improvement would be to replace the lateral state
with a stream function error. These two errors serve the same purpose: to determine the
error relative to the reference streamline. The difference is that lateral error is obviously
a true distance and so its dynamics are simple kinematic relationships with the other
162
states. The change in stream function as the vehicle moves is not as easy to predict. In
particular, the change depends on the gradient of the stream function. Therefore, the state
matrix would depend on the potential function gradient as well as the vehicle speed.
However, the benefit of this approach is that the computationally expensive lateral error
search would not be necessary. The stream function error is simply the difference in the
desired streamline and the stream function at the vehicle?s location. This would also be a
more elegant solution since it further reduces the similarity to tracking a reference path
and fits directly into the potential field control framework.
Another improvement would be developing a method to optimally determine the
initial reference streamline. This operation however, could be computationally
expensive. A cost function for the optimal streamline could be either distance travelled
or time to goal. In either case, computing the cost function for a candidate streamline
would involve integrating a particle travelling along the streamline. This would need to
be done for every streamline under consideration. This integration should include the
reference speed potential as the particle?s speed. For complete accuracy, it could also
include the streamline shifting. However, including these terms increases the
computational burden on the algorithm. Additionally the cost function will typically
have local minima in addition to the global minimum. For instance, the streamlines on
either side of an obstacle will have faster travel times than those near the obstacle due to
the slower reference speed near obstacles. However, only one side of the obstacle can be
the true minimum. These local minima would make searching for the true optimum
streamline difficult.
163
The reference speed potential could also be improved. The dense grid simulation
demonstrated a problem with this potential becoming very flat in an area surrounded by
obstacles. Therefore, a potential field proportional to the true distance to the obstacle
may make a better reference speed. This potential would have a better defined maximum
through the obstacles. This implies a larger gradient, which would improve the
streamline shifting algorithm. The downside of using the actual distance to an obstacle is
the cost of computing that distance. One other improvement that could be developed for
the reference speed is to make it directional. Once the vehicle is past an obstacle, its
speed can increase even if it is still close to the obstacle. This improvement would most
likely not occur in the reference speed potential creation algorithm since this potential is
independent of the vehicle orientation (although direction to the goal may substitute,
albeit less safely). Instead, the conversion from reference speed potential to the reference
speed for the vehicle could take into account the vehicle orientation, e.g. through a dot
product.
The controller may also need some modification to allow for a wider application. In
particular, sideslip is typically an expensive measurement to acquire. Removing the need
for this measurement would make the controller easier to implement. The best way to
remove the sideslip measurement is to use an estimator. Typically, model based sideslip
estimators have not been successful due to the requirement for an accurate vehicle model
and the relatively small magnitude of the signal. However, the controller in this research
already assumes an accurate vehicle model exists. Additionally, the state could be
deweighted in the control gains so that errors in the measurement/estimate do not have a
large impact on the response. An alternative method would be to use output feedback
164
instead of state feedback. However, in this case the location of one of the closed-loop
poles could not be arbitrarily chosen. For certain speeds this floating pole may be in the
right half plane, causing the vehicle to become unstable. Related to the difficult sideslip
measurement, a study into the effects of sensor uncertainties would be beneficial. In
particular, how the measurement errors propagate through the potential field interpolation
to the reference states is not obvious. Techniques exist for minimizing the effects of
sensor error. However, these techniques assume the error is directly applied to an input
or measurement, not propagated into the reference state and input.
The last obvious improvement to this research would be to implement the controller
on an actual vehicle. However, this requires a working autonomous vehicle with known
model parameters and the sensor suite necessary to measure the required information. At
the time of this writing, such a system was not available for use with this research.
165
BIBLIOGRAPHY
[1] Akishita, S., Kawamura, S., and Hayashi, K., 1990, "New Navigation Function
Utilizing Hydrodynamic Potential for Mobile Robot," IEEE International
Workshop on Intelligent Motion Control, 2, pp. 413-417.
[2] Antoulas, T. and Slavinsky, J., 2004, "Controllability and Observability
Grammians," , Connexions, March 20, 2008.
[3] Barr, T. H., 1997, Vector Calculus, Prentice Hall, Upper Saddle River, NJ.
[4] Bay, J. S., 1999, Fundamentals of Linear State Space Systems, WCB/McGraw-
Hill, Boston.
[5] Briggs, W. L., 1987, A Multigrid Tutorial, Society for Industrial and Applied
Mathematics, Philadelphia PA.
[6] Connolly, C. I., Burns, J. B., and Weiss, R., 1990, "Path Planning Using Laplace's
Equation," IEEE International Conference on Robotics and Automation, 3, pp.
2102-2106.
[7] Connolly, C. I. and Grupen, R. A., 1993, "On the Applications of Harmonic
Functions to Robotics," Journal of Robotic Systems, 10(7), pp. 931-946.
[8] Currie, I. G., 1993, Fundamental Mechanics of Fluids 2nd ed., McGraw-Hill,
New York NY.
[9] Daily, R. and Bevly, D. M., 2004, "The Use of Gps for Vehicle Stability Control
Systems," IEEE Transactions on Industrial Electronics, 51(2), pp. 270-277.
[10] Daily, R., Travis, W., Bevly, D. M., et al., 2006, "Sciautonics-Auburn
Engineering?s Low-Cost, High-Speed Atv for the 2005 Darpa Grand Challenge,"
Journal of Field Robotics, 23(8), pp. 579-597.
[11] Daily, R., Travis, W., and Bevly, D. M., 2007, "Cascaded Observers to Improve
Lateral Vehicle State and Tyre Parameter Estimates," International Journal of
Vehicle Autonomous Systems, 5(3), pp. 230-255.
[12] Daily, R. and Bevly, D. M., 2008, "Harmonic Potential Field Path Planning for
High Speed Vehicles," American Control Conference.
166
[13] Dixon, J. C., 1991, Tyres, Suspension, and Handling, Cambridge University
Press, Cambridge England.
[14] Dugoff, H., Fancher, P. S., and Segel, L., 1970, "An Analysis of Tire Traction
Properties and Their Influence on Vehicle Dynamic Performance," SAE Paper,
No. 700377.
[15] Eichhorn, M., 2004, "An Obstacle Avoidance System for an Autonomous
Underwater Vehicle," International Symposium on Underwater Technology, pp.
75-82.
[16] Ferziger, J. H., 1998, Numerical Methods for Engineering Application 2nd ed.,
Wiley, New York NY.
[17] Ferziger, J. H. and Peric, M., 2002, Computational Methods for Fluid Dynamics
3rd ed., Springer, New York NY.
[18] Friedland, B., 2005, Control System Design : An Introduction to State-Space
Methods Dover ed., Dover Publications, Mineola, NY.
[19] Garrott, W. R., Monk, M. W., and Chrstos, J. P., 1988, "Vehicle Inertial
Parameters--Measured Values and Approximations," SAE Paper No. 881767.
[20] Ge, S. S. and Cui, Y. J., 2002, "Dynamic Motion Planning for Mobile Robots
Using Potential Field Method," Autonomous Robots, 13(3), pp. 207-222.
[21] Gillespie, T. D., 1992, Fundamentals of Vehicle Dynamics, Society of
Automotive Engineers, Warrendale PA.
[22] Guldner, J. and Utkin, V. I., 1995, "Sliding Mode Control for Gradient Tracking
and Robot Navigation Using Artificial Potential Fields," IEEE Transactions on
Robotics and Automation, 11(2), pp. 247-254.
[23] Hesse, T. and Sattel, T., 2007, "Path-Planning with Virtual Beams," American
Control Conference, pp. 3904-3905.
[24] Kenwright, D. N. and Mallinson, G. D., 1992, "A 3-D Streamline Tracking
Algorithm Using Dual Stream Functions," IEEE Conference on Visualization, pp.
62-68.
[25] Keymeulen, D. and Decuyper, J., 1994, "The Fluid Dynamics Applied to Mobile
Robot Motion: The Stream Field Method," IEEE International Conference on
Robotics and Automation, 1, pp. 378-385.
167
[26] Khatib, O., 1985, "Real-Time Obstacle Avoidance for Manipulators and Mobile
Robots," IEEE International Conference on Robotics and Automation, 2, pp. 500-
505.
[27] Kholsa, P. and Volpe, R., 1988, " Superquadric Artificial Potentials for Obstacle
Avoidance and Approach " IEEE International Conference on Robotics and
Automation, pp. 1778-1784.
[28] Kim, J. O. and Khosla, P. K., 1992, "Real-Time Obstacle Avoidance Using
Harmonic Potential Functions," IEEE Transactions on Robotics and Automation,
8(3), pp. 338-349.
[29] Krogh, B. and Thorpe, C., 1986, "Integrated Path Planning and Dynamic Steering
Control for Autonomous Vehicles," IEEE International Conference on Robotics
and Automation, 3, pp. 1664-1669.
[30] Kyriakopoulos, K. J., Kakambouras, P., and Krikelis, N. J., 1995, "Potential
Fields for Nonholonomic Vehicles," IEEE International Symposium on Intelligent
Control, pp. 461-465.
[31] Laitone, E. V., 1977, "Relation of the Conjugate Harmonic Functions to F(Z),"
The American Mathematical Monthly, 84(4), pp. 281-283.
[32] Masoud, A. A., Masoud, S. A., and Bayoumi, M. M., 1994, "Robot Navigation
Using a Pressure Generated Mechanical Stress Field: "The Biharmonic Potential
Approach"," IEEE International Conference on Robotics and Automation, 1, pp.
124-129.
[33] Milne-Thomson, L. M., 1996, Theoretical Hydrodynamics 5th ed., Dover
Publications, New York NY.
[34] Needham, T., 1997, Visual Complex Analysis, Oxford University Press, New
York NY.
[35] Newman, W. and Hogan, N., 1987, "High Speed Robot Control and Obstacle
Avoidance Using Dynamic Potential Functions," IEEE International Conference
on Robotics and Automation, 4, pp. 14-24.
[36] Packard, A. and Wu, F., 1993, "Control of Linear Fractional Transformations,"
IEEE Conference on Decision and Control, pp. 1036-1041.
168
[37] Quinlan, S. and Khatib, O., 1993, "Elastic Bands: Connecting Path Planning and
Control," IEEE International Conference on Robotics and Automation, 2, pp.
802-807.
[38] Rainville, E. D. and Bedient, P. E., 1969, Elementary Differential Equations 4th
ed., Macmillan, New York NY.
[39] Rajamani, R., 2006, Vehicle Dynamics and Control, Springer Science, New York
NY.
[40] Rimon, E. and Koditschek, D. E., 1992, "Exact Robot Navigation Using Artificial
Potential Functions," IEEE Transactions on Robotics and Automation, 8(5), pp.
501-518.
[41] Rossetter, E. J., 2003, "A Potential Field Framework for Active Vehicle
Lanekeeping Assistance," Ph.D. dissertation, Stanford University, Stanford, CA.
[42] Sarang, S., 2007, "Complex Variables," , Chaitime Education, Feburary 23, 2008.
[43] Sato, K., 1993, " Deadlock-Free Motion Planning Using the Laplace Potential
Field " Advanced Robotics, 7(5), pp. 449-461.
[44] Shimoda, S., Kuroda, Y., and Iagnemma, K., 2005, "Potential Field Navigation of
High Speed Unmanned Ground Vehicles on Uneven Terrain," IEEE International
Conference on Robotics and Automation, pp. 2828-2833.
[45] Slotine, J. J. E. and Li, W., 1991, Applied Nonlinear Control, Prentice Hall,
Englewood Cliffs NJ.
[46] Smith, D. E. and Starkey, J. M., 1995, "Effects of Model Complexity on the
Performance of Automated Vehicle Steering Controllers: Model Development,
Validation and Comparison," Journal of Vehicle System Dynamics, 24(2), pp.
163-181.
[47] Song, P. and Kumar, V., 2002, "A Potential Field Based Approach to Multi-Robot
Manipulation," IEEE International Conference on Robotics and Automation, 2,
pp. 1217-1222.
[48] Stengel, R. F., 1994, Optimal Control and Estimation, Dover Publications, New
York NY.
[49] Sullivan, J., Waydo, S., and Campbell, M., 2003, "Using Stream Functions for
Complex Behavior and Path Generation," AIAA Guidance, Navigation, and
Control Conference and Exhibit.
169
[50] Tan, W., Packard, A. K., and Balas, G. J., 2000, "Quasi-Lpv Modeling and Lpv
Control of a Generic Missile," American Control Conference, 5, pp. 3692-3696.
[51] Van Wijk, J. J., 1993, "Implicit Stream Surfaces," IEEE Conference on
Visualization, pp. 245-252.
[52] Wang, Y. and Chirikjian, G. S., 2000, "A New Potential Field Method for Robot
Path Planning," IEEE International Conference on Robotics and Automation, 2,
pp. 977-982.
[53] Waydo, S. and Murray, R. M., 2003, "Vehicle Motion Planning Using Stream
Functions," IEEE International Conference on Robotics and Automation, 2, pp.
2484-2491.
[54] Weisstein, E. W., 2008, "Curvature," , MathWorld--A Wolfram Web Resource,
March 30, 2008.
[55] Wenzel, T. A., Burnham, K. J., Williams, R. A., and Blundell, M. V., 2005,
"Closed-Loop Driver/Vehicle Model for Automotive Control," International
Conference on Systems Engineering, pp. 46-51.