real-time path planning for autonomous vehicles · this thesis details the construction of a...
TRANSCRIPT
Real-Time Path Planning
for Autonomous Vehicles
Thomas L. P. Allen
A thesis submitted in fulfillment
of the requirements for the combined
degrees of Bachelor of Engineering
and Bachelor of Science
ARC Centre of Excellence in Autonomous Systems
Australian Centre for Field Robotics
School of Aerospace, Mechanical and Mechatronic Engineering
The University of Sydney
November, 2006
Declaration
I hereby declare that this submission is my own work and that, to the best of my knowl-
edge and belief, it contains no material previously published or written by another person
nor material which to a substantial extent has been accepted for the award of any other
degree or diploma of the University or other institute of higher learning, except where due
acknowledgement has been made in the text.
In support of this declaration, my contributions to this thesis are as follows:
• I designed and implemented a MatLab simulation of a logical path finding algorithm,
• I designed and implemented an abstract C++ framework for the A* algorithm in
conjunction with James Underwood,
• I designed and implemented an effective replanner system in C++, incorporating this
into the Argo project software library.
The above represents an accurate summary of the student’s contribution.
Thomas L.P. Allen Dr Steven J. Scheding
November, 2006
i
ii
Abstract
Thomas L.P. Allen Bachelor of Engineering & Science
The University of Sydney November, 2006
Real-Time Path Planningfor Autonomous Vehicles
This thesis describes the design and implementation of an adaptive and extensible
pathfinding system for autonomous vehicles. Different vehicles, missions, and environments
have varying requirements, which motivates the need for an adaptable planning system.
To support this, an abstract planning framework is presented, which offers the abil-
ity to plan over multiple layers of information, in any desired topology, using any discrete
pathfinding algorithm. For use by autonomous vehicles, this framework is specifically imple-
mented using the A* algorithm, and with optimisations and extensions, is shown to produce
suitable paths.
This framework is then extended into a replanner system, capable of planning through
unknown environments whilst simultaneously avoiding obstacles and optimising its paths.
This system is implemented for a particular vehicle, known as the Argo, and is shown to be
an effective means of autonomous vehicle navigation.
Acknowledgements
A large debt of gratitude is owed to the following people, who have all contributed to this
thesis, either through their words, their actions, or by simply being.
To my supervisor, Dr Steven Scheding, for providing the motivation, conversation, and
aggravation necessary to keep me working.
To James Underwood, whose patience, wisdom, and insight helped in almost every
facet of this thesis.
To Richard Grover, Ross Hennessey, Craig Rodgers, Andrew Hill, and Sisir Karu-
manchi, whose friendship and advice are both equally valuable.
To my parents, for their love and care throughout my university career, as well as the
money to pay for it!
Finally, to Natalie, whose unconditional love and support has been unfailing, and much
appreciated.
iii
“A journey of a thousand miles begins with one small step”
Contents
Declaration i
Abstract ii
Acknowledgements iii
Contents v
List of Figures ix
List of Notation xi
List of Acronyms xii
1 Introduction 1
1.1 Autonomous Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 The Argo Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Introduction to Pathfinding for Autonomous Vehicles . . . . . . . . . . . . . 4
1.4 Contributions of this Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.5 Thesis Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
v
CONTENTS vi
2 Literature Review 8
2.1 Pathfinding Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.1.1 Discrete Pathfinding . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1.2 Discrete Pathfinding Methods . . . . . . . . . . . . . . . . . . . . . . 11
2.1.3 Cost Assessing Methods . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.2 Pathfinding for Autonomous Vehicles . . . . . . . . . . . . . . . . . . . . . . 18
2.2.1 Sub-Optimal Replanning . . . . . . . . . . . . . . . . . . . . . . . . 18
2.2.2 Optimal Replanning . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.2.3 DARPA Grand Challenge Case Study . . . . . . . . . . . . . . . . . 20
2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3 A* Architecture 23
3.1 Operation of the A* Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.1.1 Key A* Components . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.1.2 A* Pseudo-Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.1.3 Discussion of Operation . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.2 Internal A* Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.2.1 Representing the Open and Closed Sets . . . . . . . . . . . . . . . . 27
3.2.2 Selection and Use of Heuristics . . . . . . . . . . . . . . . . . . . . . 28
3.2.3 Finding the Neighbouring States . . . . . . . . . . . . . . . . . . . . 30
3.2.4 Producing the Final Path . . . . . . . . . . . . . . . . . . . . . . . . 30
3.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4 Planning Framework and A* Implementation Results 32
4.1 Requirements for a Planning Framework . . . . . . . . . . . . . . . . . . . . 33
4.2 C++ Planning Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
CONTENTS vii
4.2.1 C++ Language Features . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.2.2 Abstract Planner Design . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.2.3 Multiple Cost Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.2.4 A* Planner Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.3 Grid-Based A* Implementation . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.3.1 Optimisations to the Standard A* Algorithm . . . . . . . . . . . . . 39
4.3.2 Optimised A* Pseudo-Code . . . . . . . . . . . . . . . . . . . . . . . 41
4.3.3 Representing the World . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.3.4 Map, Vector, and Deque Grid Implementation . . . . . . . . . . . . 46
4.3.5 Cost Functions, Data Sets, and their Combination . . . . . . . . . . 47
4.3.6 Post-Optimisation of A* Paths . . . . . . . . . . . . . . . . . . . . . 48
4.3.7 The Complete Implementation . . . . . . . . . . . . . . . . . . . . . 51
4.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5 Replanner Design 57
5.1 Obstacle Avoidance and the Sensor Horizon . . . . . . . . . . . . . . . . . . 57
5.2 A Logical Replanning Method . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.3 Extending this Method to Autonomous Vehicles . . . . . . . . . . . . . . . . 60
5.3.1 The Local Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.3.2 The Global Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
5.4 The Replanner Logic and Structure . . . . . . . . . . . . . . . . . . . . . . . 62
5.5 MatLab Replanner Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
CONTENTS viii
6 Replanner Implementation and Results 66
6.1 Threading and Data Protection . . . . . . . . . . . . . . . . . . . . . . . . . 67
6.2 The Subject/Observer Design Pattern . . . . . . . . . . . . . . . . . . . . . 68
6.3 Incorporating Sensory Information . . . . . . . . . . . . . . . . . . . . . . . 69
6.3.1 Generating Cost Information from Laser Data . . . . . . . . . . . . 70
6.3.2 Expanding the Obstacles . . . . . . . . . . . . . . . . . . . . . . . . 70
6.3.3 Informing the Replanner . . . . . . . . . . . . . . . . . . . . . . . . . 70
6.4 Rapid Path Optimisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
6.5 C++ Replanner Implementation . . . . . . . . . . . . . . . . . . . . . . . . 72
6.6 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
6.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
7 Conclusions and Future Work 79
7.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
7.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
A Source Code and Documentation 84
Bibliography 86
List of Figures
1.1 The Argo vehicle showing sensors and computer components. . . . . . . . . 3
1.2 Sample planning problem for an autonomous vehicle . . . . . . . . . . . . . 5
2.1 LaValle’s Generalised Planning Algorithm . . . . . . . . . . . . . . . . . . . 10
2.2 Calculation of the cost-to-come and cost-to-go functions . . . . . . . . . . . 13
2.3 State transition graph illustrating the operation of breadth first search . . . 16
2.4 State transition graph illustrating the operation of depth first search . . . . 17
2.5 State transition graph illustrating the operation of Dijkstra’s algorithm . . 17
3.1 A* Pseudo-Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.2 Euclidean, Manhattan, and eight-connected distance functions for grid based
path planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.1 UML diagram of a generic A* architecture . . . . . . . . . . . . . . . . . . . 38
4.2 Graphical demonstration of various optimisations on the performance of the
A* algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.3 Optimised A* Pseudo-Code . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.4 Class diagram for the Grid2D class . . . . . . . . . . . . . . . . . . . . . . . 46
4.5 A* algorithm speed for three different grid implementations . . . . . . . . . 48
4.6 Class diagram for the DataCostFunctionCostSet class . . . . . . . . . . . . 49
ix
LIST OF FIGURES x
4.7 Class diagram for the CostFunction class . . . . . . . . . . . . . . . . . . . . 49
4.8 Class diagram for the PathOptimisingAStar class . . . . . . . . . . . . . . . 50
4.9 Class diagram of the complete A* implementation . . . . . . . . . . . . . . 51
4.10 Mission plan through real-world cost information . . . . . . . . . . . . . . . 53
4.11 A* plan through real-world cost information . . . . . . . . . . . . . . . . . . 53
4.12 Montage of the Argo vehicle following an A* plan through real-world cost
information . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.1 Replanner Logic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.2 UML diagram showing the structure of the replanner system . . . . . . . . 63
5.3 The MatLab replanner simulation . . . . . . . . . . . . . . . . . . . . . . . . 65
6.1 Class diagram for the PathOptimiser framework within the replanner system 72
6.2 Class diagram for the Replanner class . . . . . . . . . . . . . . . . . . . . . 73
6.3 Complete replanner pseudo-code . . . . . . . . . . . . . . . . . . . . . . . . 74
6.4 C++ replanner simulation, parts one and two . . . . . . . . . . . . . . . . . 76
6.5 C++ replanner simulation, parts three and four . . . . . . . . . . . . . . . . 77
A.1 Directory structure of the attached source code and documentation CD. . . 84
List of Notation
A∗ The A* path planning algorithm . . . . . . . . . . . . . . . . . . . . . . 13
D∗ The D* path planning algorithm . . . . . . . . . . . . . . . . . . . . . . 19
F The function used to sort the open set of nodes, Q . . . . . . . . . . . 11
Q The open set for a discrete path planning algorithm . . . . . . . . . . . 10
S The state space of nodes in a graph . . . . . . . . . . . . . . . . . . . . 9
U(x) The action space of the state x . . . . . . . . . . . . . . . . . . . . . . . 9
XP A contiguous set of states in a graph, representing a path . . . . . . . . 9
XG A set of possible goal states for a path plan . . . . . . . . . . . . . . . . 9
f(x, u) The state transition function for state x and action u ∈ U(x) . . . . . . 9
g The cost-to-come function for a discrete path planning algorithm . . . 12
h The cost-to-go function for a discrete path planning algorithm . . . . . 12
n An integer quantity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
u A particular action for a state in a graph . . . . . . . . . . . . . . . . . 9
x A particular state in a graph . . . . . . . . . . . . . . . . . . . . . . . . 9
xG The goal state for a path plan . . . . . . . . . . . . . . . . . . . . . . . 9
xI The initial state for a path plan . . . . . . . . . . . . . . . . . . . . . . 9
xi
List of Acronyms
ACFR Australian Centre for Field Robotics . . . . . . . . . . . . . . . . . 2
AI Artificial Intelligence . . . . . . . . . . . . . . . . . . . . . . . . . . 9
AGV Autonomous Ground Vehicle . . . . . . . . . . . . . . . . . . . . . 1
ATV All Terrain Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
C−Space Configuration-Space . . . . . . . . . . . . . . . . . . . . . . . . . . 52
DARPA Defense Advanced Research Projects Agency . . . . . . . . . . . . 20
DGPS Differential Global Positioning System . . . . . . . . . . . . . . . . 2
E−Stop Emergency Stop . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
FIFO First In - First Out . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
FILO First In - Last Out . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
GPS Global Positioning System . . . . . . . . . . . . . . . . . . . . . . . 2
INS Inertial Navigation System . . . . . . . . . . . . . . . . . . . . . . 2
MUTEX MUTual EXclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
STL Standard Template Library . . . . . . . . . . . . . . . . . . . . . . 39
UML Unified Modelling Language . . . . . . . . . . . . . . . . . . . . . . 32
xii
Chapter 1
Introduction
This thesis details the construction of a real-time pathfinding system for an autonomous
ground vehicle (AGV) - that is, a computer program which can instruct an unmanned
vehicle how best to navigate from one location to another through a complicated real-world
environment, simultaneously avoiding obstacles and optimising its route, all without the
aid of human operators. This system comprises two main parts; a generic and reusable
framework for path planning, and a real-time replanning algorithm for an AGV which
makes use of this framework.
Section 1.1 describes the features of typical autonomous vehicles, which are further
described, as applied to a vehicle known as the Argo, in Section 1.2. Section 1.3 briefly
describes obstacle avoidance and pathfinding methods in the context of autonomous vehicles,
and provides a motivation for the approach taken in this thesis. The last two sections of
this chapter describe the primary contributions and the overall structure of this thesis.
1.1 Autonomous Vehicles
An autonomous vehicle such as the Argo shown in Figure 1.1 uses sensors to observe its envi-
ronment, and a computer control system to determine its actions. In so doing, autonomous
vehicles can (with appropriate control systems and sensors) operate without human inter-
vention. The goal of this thesis is to achieve a measure of persistent autonomy for such a
vehicle, whereby it can operate autonomously over a certain range of conditions, but more
1.2 The Argo Vehicle 2
importantly, can know under what conditions this autonomy is comprised, and a human
operator is required. Specifically, this thesis implements a real-time pathfinding system on
the Argo vehicle which allows it to traverse unknown terrain while locally avoiding obsta-
cles. The system has a specific failure mode such that when autonomous decisions cannot
be made, a human operator can be notified.
As described above, an autonomous vehicle makes use of sensors to acquire information.
These sensors include lasers, cameras, radars, and Global Positioning Systems (GPSs), as
well as vehicle specific devices such as potentiometers, voltmeters, and limit switches. The
first group of sensors typically provide environmental information such as terrain maps,
whereas the latter provide a measure of the vehicle’s operations within this environment,
such as its speed and heading. Both sets of sensors acquire information which is used by
the vehicle’s computer control system.
All AGVs require some form of computer control system, in order to make logical
decisions based on the information acquired by the sensors described above. The nature of
such a control system is defined only by the users of the vehicle itself, but for an effective
AGV, must allow the motions of the vehicle to be controlled. As this thesis is concerned
with the process of pathfinding, and is implemented on the Argo vehicle, a description of the
control systems available on this vehicle, and their use in a pathfinding system, is presented
in the following section.
1.2 The Argo Vehicle
The Argo 8x8 Conquest is an eight-wheeled amphibious all-terrain vehicle (ATV) produced
by Ontario Drive & Gear Ltd [1]. The Australian Centre for Field Robotics (ACFR) has
two such vehicles which have been retrofitted with a drive-by-wire system and computer
control. Hereafter in this thesis, all references to Argo refer to this robotic platform, one of
which is shown in Figure 1.1, with specific components identified.
The Argo system comprises the vehicle, on-board computer control, sensors, commu-
nications systems, and one or more remote ground stations. Currently the Argo carries a
differential GPS (DGPS) and an inertial navigation system, (INS), for position and head-
ing estimation, a forward mounted 180° laser with a range of roughly 10m (dependent on
mounting angle) and colour camera for environment sensing, as well as a radio modem for
1.2 The Argo Vehicle 3
Figure 1.1: The Argo vehicle showing sensors and computer components.
wireless communication, and Radio Frequency (RF) Emergency Stop system (E-Stop) for
vehicle safety.
The Argo control system comprises a hierarchy of controllers; a waypoint controller
produces the inputs to a rate controller, which produces the inputs required for a series
of actuator controllers. The waypoint controller takes a list of waypoints connected by
straight line paths, and produces desired velocities and turn rates. These rates are then the
inputs to actuator controllers such as the brake and throttle controllers, which determine
the vehicle’s motion.
With the control system as described above, a pathfinding system can determine a path
in an internal format, and then convert it to to a waypoint path, which can thus be followed
by the vehicle. Later sections of this thesis describe some of the problems that occur as a
result of this controller implementation, and yet the solutions presented allow for the Argo
vehicle to successfully follow the plans produced by the pathfinding system.
1.3 Introduction to Pathfinding for Autonomous Vehicles 4
1.3 Introduction to Pathfinding for Autonomous Vehicles
A path is defined as a sequence of directives which achieves a goal [2, 3], and the process of
pathfinding aims to produce such paths given some representation of the environment, and
a goal to aim for. For autonomous vehicles, these paths are a sequence of physical locations,
yet in the most abstract form, pathfinding can be used to solve any problem which can be
expressed in a particular structure, known as a cost-graph. Chapter 2 presents a number of
abstract pathfinding methods seen in the literature, but in order to motivate the approach
taken in this thesis, this section describes the pathfinding process as applied to AGVs and
obstacle avoidance.
For a typical AGV, the aim of the pathfinding process is to find the most cost-effective
path to a goal location, where this cost is defined by the users of the vehicle, and may
relate to the terrain shape, fuel consumption, time required, or any combination of such
factors. In many situations this cost reflects the traversability of the environment, and with
this representation, obstacles can be encoded as regions of maximum cost, which are then
avoided by a pathfinding algorithm. Such a representation is presented in Figure 1.2 below.
Obstacle avoidance is thus implicitly connected with pathfinding, as avoiding an ob-
stacle is best performed by finding a new path around it. Combining obstacle avoidance
with pathfinding, by encoding terrain traversability into the cost representation, achieves
the production of optimal cost paths, which also avoid obstacles in the environment. These
paths are thus immediately suitable for use on an AGV, and Chapters 3 and 4 describe the
design and implementation of such a pathfinding system.
One feature of pathfinding for AGVs not discussed above, is the need for replanning of
paths. This need arises because environmental information is only acquired incrementally
by the vehicle’s sensors, as it progresses through the environment. As a result, not only are
paths potentially compromised as the cost representation of the environment changes, the
goal location is often situated in unknown terrain. This result encapsulates the two distinct
requirements of pathfinding for AGVs; the need for both local obstacle avoidance, and also
unknown terrain traversal. The only solution which achieves both these requirements is
replanning the vehicle’s paths, to accommodate this changing information. Chapters 5 and
6 describe the design and construction of a replanner system, which achieves both these
requirements, and is implemented onboard the Argo vehicle.
1.4 Contributions of this Thesis 5
World Map
Start
Goal
Figure 1.2: Sample planning problem showing the map and goal for an autonomous vehi-cle. This map defines some form of information about the environment, in this case thetraversability of the terrain, encoded in shades of grey. More readily traversable areas arecoloured in white, and untraversable obstacles in black.
1.4 Contributions of this Thesis
The main contributions of this thesis are as follows:
• The design of an adaptable and extensible planning framework to support planning
over multiple topologies of information, and user-specific pathfinding implementations.
1.5 Thesis Structure 6
• The development of a portable A* pathfinding library.
• The design of a logical planning algorithm to fuse the two separate tasks of unknown
terrain traversal, and local obstacle avoidance.
• The implementation of this planning algorithm combined with the A* library into a
real-time replanner system onboard the Argo vehicle.
1.5 Thesis Structure
This chapter has presented an overview of the background which inspires this thesis, and
its general goals and contributions. An overview of the Argo system used in this thesis was
presented, and a brief discussion of the requirements for implementing a real-time obstacle
avoidance pathfinding system was given. The structure and content of subsequent chapters
of this thesis is now described below.
Chapter 2 presents a review of pathfinding methods in the literature, starting with
general graph search algorithms, and then focussing on mobile robotics pathfinding.
Chapter 3 describes the structure of the A* algorithm in detail, and illustrates its
internal operations. Pseudo-code is presented for the generic A* algorithm, which is then
extended in subsequent chapters.
Chapter 4 presents a generic planning framework, which is then efficiently imple-
mented using the A* algorithm. This abstract framework is designed to allow for planning
over multiple layers of information, and any user-defined topology. Results from a specific
implementation of this framework created for the Argo vehicles are presented, illustrating
the success and adaptability of the design.
Chapter 5 presents a replanner algorithm based on a logical brute-force approach.
Combining two separate but linked planners, this algorithm is capable of combining the
distinct requirements of unknown terrain traversal, and local obstacle avoidance. This
algorithm is shown to be effective in a semi-realistic simulation environment, and is extended
and implemented on the Argo vehicles in the next chapter.
Chapter 6 presents a real-time implementation of the algorithm described in the
preceding chapter. The assumptions necessary for the simulation environment are removed,
1.5 Thesis Structure 7
allowing for sensory information to be incorporated asynchronously with vehicle motion.
Extensions are required to the replanner algorithm in order to accommodate these changes,
but a second level of more realistic simulation illustrates the effectiveness of the algorithm.
Only minor changes are then needed in order to embed the system in the Argo vehicles,
and the success of this last simulation suggests that promising real-world results will be
attained.
Chapter 7 concludes this thesis, and describes a series of possible extensions to the
work. These extensions include further optimisations to the A* system, and additional spe-
cific implementations of the replanner system, to allow for testing against other algorithms
in the literature.
Chapter 2
Literature Review
This chapter presents a review of pathfinding work in the literature, briefly analysing general
pathfinding methods, but primarily focusing on pathfinding in the autonomous vehicles
field. Literature dealing with general pathfinding methods is described in Section 2.1, and
Section 2.2 analyses how these methods are used in state of the art systems for autonomous
vehicles. A case-study of the pathfinding systems used by competitors in the 2005 DARPA
Grand Challenge is then presented, with the relevant findings incorporated into the choice
of system employed in this thesis.
2.1 Pathfinding Systems
General pathfinding methods have been described in great detail by many authors, to such
extent that they have become mainstream knowledge in the fields of computer science,
artificial intelligence, and autonomous systems. As a result, many thorough reviews of
these ideas have been published, one of the best being LaValle’s Planning Algorithms, [2].
This recently published book covers virtually every general pathfinding method in common
usage, and devotes an entire section to the discrete methods that are employed and built
upon in this thesis. Such methods, and extensions to them, are commonly used in computer
games [4, 5], artificial intelligence research [3, 6, 7], and autonomous systems [8–14]. These
methods are also the simplest to describe and build upon, as more complicated pathfinding
methods involving differential constraints (where paths involve time-dependent factors) or
2.1 Pathfinding Systems 9
non-holonomic systems (where system behaviour is path-dependent) can be derived from
the simpler models [2].
2.1.1 Discrete Pathfinding
Discrete pathfinding methods are planning algorithms which operate over a finite state space,
S, where each possible choice or situation in this space is known as a state, x, where x ∈ S.
This state space is often denoted the world since it represents everything the planning
algorithm can operate upon. For a pathfinding application, this name is especially apt, as
physical paths for an autonomous vehicle are formed by finding state space paths through
the available states in this world. Traditionally, most pathfinding methods employed in
artificial intelligence (AI) and robotics research have been discrete search-based methods
[6].
Discrete methods employing this state space representation formulate a path, XP , as
a set of n linked states, {x1, . . ., xn}. Each state in this path, xk has a particular set of
actions, u in its action space, U(xk). These actions are the operation of a state transition
function, f(xk, u), which results in a new state, xk′ = f(xk, u) being reached. The action
space thus defines the neighbours of a given state, and a path is thus a consecutive sequence
of neighbouring states.
Under this notation, the operation of a discrete pathfinding system is to produce the
set, XP , from an initial state, xI , to a goal state, xG. LaValle defines the goal in a more
general manner as a set of possible goals, XG, but for the purposes of this thesis, paths are
considered to have only one goal state, which may or may not be attainable.
As mentioned in Chapter 1, pathfinding methods are often described as search methods
in that some form of search over the state space is the means of finding a path. A highly
general search method for a discrete pathfinding algorithm is described in LaValle (pp.
33-35) and involves recording which states have been visited by the algorithm. The states
are then divided into open, closed (or visited), and new sets, where these descriptions are
known as the state’s tag. All states (except xI) are initially new, meaning they have yet
to be visited. States which are due to be looked at are described as open; once a state has
been visited it is described as closed. Thus the set of open states can be thought of as the
frontier of the search, the set of closed states consists of all states inside this frontier [15].
2.1 Pathfinding Systems 10
LaValle’s general search algorithm operates by continuously selecting a particular state,
xk, from the list of open states. This list is described by a priority queue, Q, and states
are extracted using some function, h. If the selected state is the goal, the algorithm is
complete, otherwise the state is then expanded, meaning every possible action, u ∈ U(xk) is
explored. This exploration adjusts the tag of each neighbouring state, placing it into open
if it is currently new, and otherwise ignoring it. A state xk′ is considered the neighbour of
the state xk, if there exists an action, u, such that xk′ = f(xk, u). Once this expansion of
xk is complete, xk itself is placed into the closed list.
Whilst this general form ignores certain considerations of discrete pathfinding algo-
rithms, it provides a generalised form from which the operations of other algorithms can be
better illustrated. Figure 2.1 presents a schematic view of LaValle’s generalised planning
algorithm [2].
ForwardSearchinsert xI into Qinsert xI into visitedwhile Q �= ∅
x = first element in Qinsert x into visitedif x ∈ XG
return SUCCESSforall u ∈ U(x)
x′ = f(x, u)if x′ /∈ visited
insert x′ into Qinsert x′ into visited
elseresolve duplicate x′
return FAILURE
Figure 2.1: LaValle’s Generalised Planning Algorithm
2.1 Pathfinding Systems 11
2.1.2 Discrete Pathfinding Methods
For many discrete search based pathfinding methods, the only distinguishing characteristic
is the function, F , which selects the appropriate state on the open list to expand [2, 3].
Either Q is continually sorted according to F and the top state is selected, or else F is used
to select the appropriate state from an unsorted Q. (These descriptions are equivalent, but
certain situations may make the use of one preferable.) This section describes several of
these discrete search based pathfinding methods.
Breadth first search treats Q as a First-In First-Out (FIFO) buffer, selecting the state
which has been open the longest to expand. The name derives from the representation of
the world in a state transition graph form, wherein the tree of actions is expanded until all
leaves at the current depth have been visited; that is, breadth first. Figure 2.3 in Example
2.1.1 illustrates the operation of breadth first search on a state transition tree.
Depth first search treats Q as a First-In Last-Out (FILO) buffer, selecting the state
which has been most recently added to the open list to expand. In the state transition
graph representation, this method expands all sub-branches and leaf nodes on the current
branch before moving to other branches; that is, depth first. Figure 2.4 in Example 2.1.1
illustrates the operation of depth first search on a state transition tree.
Both of these methods are systematic for a finite state space S, meaning that they are
guaranteed to reach the goal within a finite number of iterations [2]. As a result, both these
methods are acceptable means of finding a path, yet depth first search has no guarantee of
optimality, and breadth first search takes the longest possible number of steps. As defined
in Chapter 1, a plan is optimal if it produces the best path to the goal. Breadth first
search operates by expanding the search tree for all nodes at a given depth, and is sub-
optimal because it finishes once the first successful path is found. This path may not be the
optimal path, even though it lies at the same depth as the optimal one. Depth first is also
sub-optimal because it has the potential to search directly down one branch of the search
tree, ignoring other, possibly better, branches. Example 2.1.1 at the end of this section,
illustrates the sub-optimality of both these methods.
The prime concern of this thesis is to produce paths suitable for an autonomous vehicle
to traverse an environment. In this situation, the paths found need to be optimal, or at the
very least sensible, in order that the robot reaches its goal within a practical time frame
2.1 Pathfinding Systems 12
[10, 16, 17]. This requirement invalidates the use of the preceding two methods, and suggests
that only cost assessing methods are viable.
2.1.3 Cost Assessing Methods
Methods involving cost assessment are not necessarily optimal, but at minimum provide
feasible plans. As is shown below, some of these methods can be shown to be optimal given
certain constraints.
One of the first discrete pathfinding methods to use cost assessment was Dijkstra’s
algorithm, described in 1959 by Edsger Dijkstra [18]. The algorithm sorts Q by a function,
g, known as the cost-to-come, where g(xk) is the cost of the path from xI to xk. This
g can be optimal, (and is then denoted g∗), if every possible path from xI to xk has
had its total cost calculated. Regardless of whether g is optimal, the cost-to-come can be
calculated iteratively as the algorithm progresses by adding the incremental cost of traveling
from xk−1 to xk, c(xk−1, xk), to the previously calculated cost-to-come, g(xk−1). Thus
g(xk) = g(xk−1) + c(xk−1, xk). The algorithm details are described in [18] and summarised
in [2], while a proof of the optimality of Dijkstra’s algorithm is given in [19]. Figure 2.5 in
Example 2.1.1 illustrates the operation of Dijkstra’s algorithm on a state transition tree.
The main problem with Dijkstra’s algorithm is its search time; finding an optimal
path requires searching over more states than simply finding a feasible path. For some
applications, such as real-time pathfinding, this increased search time limits the applicability
of Dijkstra’s algorithm [16].
Best first search is a variant of Dijkstra’s algorithm which sorts Q by the cost-to-
go function, h, rather than cost-to-come [2, 3, 7]. Sorting Q in this manner means the
algorithm can potentially be very fast, iterating rapidly towards the goal. The main problem
with this approach is that the cost-to-go function is typically imperfect; in most situations
there is no way of knowing ahead of time the cost of a given path. This fact means that
implementations of best first search and related algorithms typically use an estimate of this
function, commonly called the heuristic. The result of this heuristic based search is that
the algorithm is not guaranteed to be optimal, and algorithms such as this are classed as
greedy. Greedy algorithms such as best first search preference states which heuristically look
better, yet there is no assurance that a given state is actually a better choice.
2.1 Pathfinding Systems 13
It is difficult to represent the operation of algorithms such as best first search, as
their operation is highly dependent upon the specific heuristic used. Figure 2.2 below
illustrates the manner in which both the cost-to-come, and one typical heuristic cost-to-go
are calculated. Figure 3.2 in Chapter 3 presents a series of other heuristics for grid-based
path finding.
Figure 2.2: The cost-to-come is calculated by summing the cost of traveling to each indi-vidual node along the path taken to the current node, shown in blue. The cost-to-go isestimated as some form of distance measurement from the current node to the goal, suchas a straight line Euclidean distance, shown in red.
The preceding two discrete pathfinding methods, Dijstra’s algorithm and best first
search, have positive and negative attributes. Dijstra’s algorithm is optimal but computa-
tionally slow, best first search is fast but sub-optimal. Clearly it would be desirable to have
an algorithm which could trade-off these two attributes, being at once both optimal, and
efficient. In 1968, Hart, Nilsson, and Raphael described an algorithm which fulfilled this
criteria, naming it A* [20, 21]. The name derives from the algorithm being an optimal form
(hence the star, as in optimisation literature) of a common graph-search algorithm known
simply as A.
2.1 Pathfinding Systems 14
The A* algorithm combines aspects of both Dijstra’s algorithm and best first search, by
sorting the open list of states, Q, by the sum of the cost-to-come, g, and the estimated cost-
to-go, h. Thus, F (x) = g(x) + h(x), for any node x, and Q is then sorted by this function,
F . This says that the open list is sorted by estimates of the optimal cost of traversal from
xI to xG. It can be shown that if the heuristic cost-to-go, h(xk), is an under-estimate of
the true cost-to-go from xk to xG, for all xk ∈ X, then A* is guaranteed to produce optimal
paths, and furthermore will expand just as few states as any other algorithm using the same
heuristic [22].
Given that A* is optimal for an admissible (that is, never over-estimating) heuristic,
h, the algorithm’s chief benefit over Dijstra’s algorithm is its low computational time. By
sorting the open list using the sum of g and h, the number of states that are expanded is
reduced, meaning less computation is required to produce a plan.
The A* algorithm also possesses several other useful attributes arising from its com-
bined cost function. The closer h is to the true cost-to-go, the less states need to be
expanded, resulting in faster computation. As g reduces to zero for all x ∈ X, A* tends
towards Dijstra’s algorithm. If g is reduced to zero for all x ∈ X, A* tends towards best first
search. Thus by varying g and h, A* can be tuned to produce paths of differing types; either
sub-optimal but computationally fast, or optimal but computationally slower. If g and h
are calculated exactly, A* produces paths that are optimal in both cost and computation
time.
The discrete methods discussed here are used in a multitude of situations, including in
many cases, pathfinding for autonomous vehicles. The next section of this chapter examines
state-of-the-art systems which use these and similar methods.
2.1 Pathfinding Systems 15
Example 2.1.1
This example describes the operation of the breadth and depth first search, and Dijkstra’s
algorithm. Each algorithm is demonstrated with a diagram, in which the black numbers
represent the edge costs between connected nodes, the red text and arrows represent the
order in which different edges are assessed by the algorithm, and the green arrows represent
the final path chosen by the algorithm.
For breadth first search, the algorithm starts by expanding and closing node A. Two
branches, AB and AC, are found to nodes B and C respectively, and these nodes are now
closed too. Neither of these branches is preferenced, and so assuming AC is taken first,
node C is now expanded, revealing only branch CD, as B is already closed. Before this
branch from C are entered, all branches at the current depth are explored, meaning AB is
now taken. This reveals only branch BD, as node C is already closed. All branches at depth
A have now been explored, so the algorithm has explored the entire breadth, at this depth.
Either of the branches at the next depth, BD or CD, are now taken, without any preference.
Assuming CD is taken, node D is now expanded and closed, revealing only branch DE, as
nodes B and C are already closed. Furthermore, as node D is now closed, path BD is now
ignored, meaning all paths at the current depth have been taken. Path DE is now taken
expanding and closing node E, which is the goal, and thus the algorithm stops. The final
path chosen is thus A → C → D → E.
For depth first search, the algorithm starts by expanding and closing node A. Two
branches, AB and AC, are found to nodes B and C respectively, and these nodes are now
closed too. Neither of these branches is preferenced, and so assuming AC is taken first, node
C is now expanded and closed, revealing branch CD. Rather than returning and examining
branch AB, the depth first algorithm attempts to delve as deep along a branch as possible,
meaning it now chooses CD, expanding and closing node D. This reveals branch DE, which
deepens the search, and hence it is chosen, expanding and closing node E, which is the goal,
and thus the algorithm stops. The final path chosen is thus A → C → D → E.
For Dijkstra’s algorithm, the algorithm starts by expanding and closing node A. Two
branches, AB and AC, are found to nodes B and C respectively, and these nodes are
now closed too. The cost-to-come for both these branches is zero, so neither is preferenced.
Assuming branch AB is followed first, branch BD is now discovered (and node D is closed),
as all others from node B are closed. However, branches BA and BC are re-evaluated by
2.1 Pathfinding Systems 16
this new route’s cost-to-come. Path ABA to node A is clearly a higher cost (8 vs 0), and
is ignored. Path ABC to C is better than path AC (5 vs 7), and thus node C is updated
with this new cost-to-come, and re-opened. This reveals path BC, which closes node C
again. Of the available nodes from the current node B, node C has the lowest cost-to-come
(5 vs 12), and is thus taken. This again reveals only closed nodes, but the route to D is
now better (8 vs 12), hence it is updated and reopened. This reveals path CD, which closes
node D again. Of the available nodes from the current node C, node D is the only one
available, hence it is taken, revealing branch DE and closing node E. The only available
path is now DE which reaches the goal, and the algorithm stops. The final path chosen is
thus A → B → C → D → E, the optimal path for this cost graph.
Figure 2.3: State transition graph illustrating the operation of breadth first search.
2.1 Pathfinding Systems 17
Figure 2.4: State transition graph illustrating the operation of depth first search.
Figure 2.5: State transition graph illustrating the operation of Dijkstra’s algorithm.
2.2 Pathfinding for Autonomous Vehicles 18
2.2 Pathfinding for Autonomous Vehicles
Pathfinding in the mobile robotics world is often concerned with replanning, as the envi-
ronment in which the robot is traveling is not constant. Replanning refers to the situation
whereby a plan becomes invalidated for any reason, and thus a new plan is required. The
reasons for this invalidation of a path are as varied as the applications of pathfinding itself,
but primarily are due to changes in the environment. These dynamic environments arise
either because objects in the world are mobile, the world is physically changing, or more
commonly, because the robot’s representation of the world changes as its sensors acquire
more data.
2.2.1 Sub-Optimal Replanning
Until the 1990s, the vast majority of mobile robotics literature on path planning assumed
that the environment was completely known prior to planning a path [17]. In this situation,
replanning is not required because an optimal initial plan will suffice for the entire mission.
Early approaches to avoid this known environment requirement tended to be effective, but
sub-optimal.
One such approach is presented by Lumelsky, [23], whereby the environment is opti-
mistically initialised as empty. As the robot encounters obstacles, it continues around the
perimeter until it is able to make a beeline for the goal. Other authors have suggested
expanding the perimeter of these obstacles by some fixed amount in order to prevent col-
lisions due to imprecise sensor readings [2, 3]. Another option is to expand the obstacle
boundaries by an amount proportional to the uncertainty in the position of the robot and
obstacles [24]. No literature has yet suggested incorporating the tracking error (how far off
its intended path it is) of the vehicle, but this is another possible option.
Other approaches such as Pirzadeh [25] and Korf [26] use cost-to-come heuristics which
penalise subsequent navigation over previously traveled areas. By penalising backtracking,
paths are forced to navigate in a overall forward direction around an obstacle. These
algorithms tend to be greedy (essentially being variants of best-first search), and hence
sub-optimal.
2.2 Pathfinding for Autonomous Vehicles 19
2.2.2 Optimal Replanning
Zelinsky [27] and Stentz [17] describe a brute-force replanning approach, whereby a com-
plete plan to the goal is replanned every time the environment changes. As both authors
describe, this can become extremely inefficient, especially if the representation of the en-
vironment is large, and the goal lies a considerable distance away. Zelinsky attempts to
improve the efficiency of this brute-force replanning by using a quad-tree representation
of the environment. This representation continually divides any area of the environment
containing an obstacle into four equally sized squares, and recursively continues until no
square contains more than a threshold number of obstacles. Zelinsky shows that in many
situations, this can reduce the number of states searched through when planning, increasing
the efficiency of the replanning process. This quad-tree representation works best on binary
data, hence Stentz argues that for natural terrain, where traversability can be encoded
over a continuum, quad-trees are inappropriate or sub-optimal. Despite this, Yahja and
Stentz [28] use a variant of a quad-tree representation in a later paper for a mobile robotics
platform.
This brute-force replanning approach, whilst inefficient, is optimal in the sense that at
every instant, the current plan represents the optimal route to the goal given the current
knowledge of the world. Much work in the mobile robotics community aims to find more
efficient ways of performing equally optimal replanning in dynamic environments.
Stentz et al [8, 16, 17, 28, 29] developed an optimal and efficient system for replanning
in dynamic environments, known as D*. The name reflects the fact that the algorithm is
essentially a dynamic version of A*, allowing for the cost function to vary as new information
is acquired. The algorithm then provides an efficient way of replanning paths which avoids
searching over the entire state space again.
The D* algorithm essentially limits the amount of replanning required by propagating
information about states which have changed cost to all states in the open set, whilst it
simultaneously calculates a new path. D* also maintains two other sets of states, raise and
lower corresponding to states which have increased or decreased in cost respectively. Where
most algorithms expand a state to determine the cost-to-come or cost-to-go for this state,
D* also uses this expansion to pass on cost change information to all neighbouring states,
by adding the neighbours to these new sets as appropriate.
2.2 Pathfinding for Autonomous Vehicles 20
The main benefit of the D* approach is that replanning takes place only over the
vicinity of the obstacle detected, rather than the entire world. As soon as the algorithm
finds a state whose cost function is greater than or equal to the heuristic cost-to-go from
a given state, because the cost changes have already been propagated through the world,
this path is provably optimal. Stentz presents an informal proof of this property in [17].
D* gives far better results for larger worlds than brute-force replanning, intuitively because
where the brute-force approach replans over the entire world, D* replans only over the
affected region.
Koenig and Likhachev in [30] describe an extension to the D* algorithm, calling it D*
Lite, which has better performance than D*. The authors describe the lack of popularity
of D*, demonstrating its complexity and the difficulties in extending it to non-grid-based
topologies. D* Lite is easier to understand than Stentz’s formulation of D*, yet is more
efficient, meaning it is logically a better option than the standard D* algorithm.
Ferguson and Stentz in [31] describe the Field D* algorithm; an extension to D* and
D* Lite which uses interpolation to remove the limitations of grid-based discretisation of
continuous environments. This extension reduces the efficiency of the D* algorithm, but
allows the paths to take any direction by moving to the best point on the edge of a grid cell,
rather than the centre. This shortens the paths by up to 8%, and is an effective and useful
improvement. Furthermore, the interpolation procedure used in the Field D* algorithm is
equally applicable to the A* algorithm, and as such is described as a useful extension to
the work done in this thesis in Section 7.2.
2.2.3 DARPA Grand Challenge Case Study
The Defense Advanced Research Projects Agency (DARPA, [32]) is the primary research
organisation for the American Department of Defense. The Grand Challenge competition
run by DARPA has the principle aim of fostering increased research and development into
autonomous vehicles, primarily from a military perspective. The competition in 2004 and
2005 involved driving an unmanned vehicle over a 132 mile desert course with the course
route given to the competitors just two hours prior to starting. The differing pathfinding
techniques utilised by various competitors help to illustrate the current state of pathfinding
for mobile robotics systems.
2.3 Summary 21
Carnegie Mellon University’s “Red Team” [33] uses the A* algorithm over a binary ob-
stacle map to plan an initial path through the route corridor provided by DARPA. Collision
avoidance on the fly is performed by geometrically shifting the original path waypoints ac-
cording to pre-defined definitions of acceptable swerving motions at different speeds. Whilst
this technique proved to be acceptable, (Red Team finished second in the 2005 competi-
tion), this is not a particularly robust method. Additionally, the method assumes a binary
representation of obstacle traversability, and cannot, for example, tell the vehicle to slow
down for a hill, instead attempting to avoid the hill altogether. As a result of this, the
technique is clearly sub-optimal.
Stanford University’s “Stanford Racing Team”, [24], use a mixture of approaches in
their overall system, but do not provide specific information on the pathfinding implemen-
tations used. Their path planner uses the DARPA supplied data to formulate an initial plan
consisting of trajectories and velocity requirements for each waypoint. Their planner trades
off five objective definitions of effective paths, such as the proximity to the road centre,
clearance to obstacles, and vehicular motion requirements computed from a vehicle model.
The specific calculation of this trade off function is found from a learning model, taking
data from human controlled missions. The available documentation released by Stanford
does not specify the space over which planning is performed, but the implication is that
replanning is performed over some localised region, with reference to the original path cor-
ridor supplied by DARPA. An additional feature of the Stanford system is to adjust the
path generation depending on the level of uncertainty of the vehicle’s position. Thus the
vehicle can be slowed to allow for additional information to be acquired by the sensors in
order to better localise the vehicle. When the vehicle pose is well known, the use of the
vehicle model in the path generation system results in effective obstacle avoidance. This
use of a vehicle model to ascertain the applicability of paths generated provides for a robust
obstacle avoidance pathfinding system, demonstrated by the Stanford Racing Team’s win
in the 2005 Grand Challenge event.
2.3 Summary
The discussion presented in this chapter has illustrated a number of viable pathfinding
techniques and systems. The methods presented range from general discrete pathfinding
2.3 Summary 22
techniques to complete pathfinding and obstacle avoidance systems in mobile robotics ap-
plications.
The best methods described by this literature review benefit from a combination of
techniques. Local replanning combined with global plans initially and when local replanning
is ineffective, is shown to be a robust and efficient technique. The use of a vehicle model
and uncertainty information is seen to produce plans that are feasible and sensible from a
vehicle point of view. Grid representations of the world are seen to be common and effective
in outdoor environments, provided that the assumptions they make can be accommodated.
The Field D* method makes use of interpolation within the A* or D* algorithm in order
to lessen the effect of these assumptions. Alternatively, post-optimisation of the paths can
be used to again mitigate the effects of the discrete grid.
Of all the individual pathfinding systems presented, the A* system is the most ap-
plicable to this thesis. Whilst D* may prove a better system in many applications, it is
considerably more complicated than A* to implement. Project deadlines for the Argo group
require a working autonomous platform to be developed quickly, hence in this situation, the
simpler method is preferable. The generic planning architecture described in Chapter 3 is
easily modified to use the D* algorithm internally, meaning the choice of A* in this thesis
is non-restrictive in this regard.
Chapters 3 and 4 present an adaptive and extensible framework for path planning, using
the A* algorithm to plan over any combination of information topologies. In Chapters 5
and 6, this framework is incorporated into a replanning algorithm, which is capable of
controlling an autonomous vehicle through unknown terrain, locally avoiding obstacles.
Chapter 3
A* Architecture
Chapter 2 discussed a series of discrete pathfinding methods, and concluded that for the
situations described in Chapter 1, the A* algorithm was most suitable. This chapter thus
describes the A* algorithm in greater detail, starting with a thorough discussion and pseudo-
code presentation of its operation in Section 3.1. The algorithm’s internal structure, and
modifications to this which strongly influence its performance are presented in section 3.2.
These modifications are further described in Chapter 4, for the specific implementation
created in this thesis.
3.1 Operation of the A* Algorithm
The A* algorithm was presented in brief in Chapter 2, and was seen to follow LaValle’s
generalised pathfinding algorithm (Figure 2.1), save that the open list is sorted by the sum
of the cost-to-come, g, and an estimate of the cost-to-go, known as the heuristic, h. This
section discusses the operation of the A* algorithm, and presents the entire algorithm in
pseudo-code.
3.1.1 Key A* Components
The A* algorithm requires only a few key components to operate successfully; two sets of
states known as the open and closed sets (Q and R respectively), and a function, F , to find
the state in the open list with the lowest cost (or sort the open list by this function, making
3.1 Operation of the A* Algorithm 24
the lowest cost node the first element). These components define the abstract operation of
the algorithm, but in order to effectively implement it in a software architecture, a number
of other features are required, and are described later in this chapter.
Firstly, a function to determine the neighbours of a given node x, n(x), is needed. As
described in Chapter 2, a state x′ is considered the neighbour of the state x, if there exists
an action, u, such that x′ = f(x, u), where f is the state transition function (meaning there
is a way of traveling from x to x′).
Secondly, each state must have an associated backpointer to its parent state. That is,
when the function to find the neighbours of x, n(x) is called, all neighbours x′ adjust their
backpointers to point to x. The state x is thus the parent of all x′ because these states were
found through x.
Given these features, the operation of the A* algorithm is presented in Figure 3.1
below.
3.1.2 A* Pseudo-Code
In this pseudo-code representation, the following conventions apply:
• xI is the initial state
• xG is the goal state.
• xmin represents the lowest cost state at any stage in the FindPath() function.
• g(x) is the cost-to-come function for any state, x.
• c(x, x′) is the incremental cost of traveling from x to x′.
• n(x) returns the neighbours of x.
• parent(x) returns or sets the parent of x.
• Q and R are the open and closed sets respectively.
• F is the function used to sort the open set, Q.
• Q.top() returns the lowest cost state in Q, having been sorted by the function, F .
3.1 Operation of the A* Algorithm 25
Initialise()g(xI) = 0parent(xI) = xI
insert xI into Q
AdjustValues(x′,x)parent(x′) = xg(x′) = g(x) + c(x, x′)
FindPath( xI , xG )Initialise()sort Q using Fxmin = Q.top()while(Q �= ∅ )
forall x′ ∈ n(xmin)if x′ ∈ R
if g(xmin) + c(xmin, x′) <= g(x′)AdjustValues(x′,xmin)remove x′ from Rinsert x′ into Q
elsecontinue
if x′ ∈ Qif g(xmin) + c(xmin, x′) <= g(x′)
AdjustValues(x′,xmin)else
continueelse
AdjustValues(x′,xmin)insert x′ into Q
if( xmin = xG )while( parent(xmin)! = xmin )
add xmin to front of pathxmin = parent(xmin)
return path
Figure 3.1: A* Pseudo-Code
3.1.3 Discussion of Operation
As this pseudo-code illustrates, the operation of the A* algorithm comprises a main loop
which selects the optimum node and assesses and adjusts the state of its neighbours accord-
3.1 Operation of the A* Algorithm 26
ing to a series of logical comparisons. Each time a node is adjusted, its backpointer is set to
the node it was found from, and once the goal is found, these backpointers are followed to
produce the optimal path. The details of the logical comparisons and adjustment process
are further described below.
The algorithm starts by inserting the starting location, xI , into the open set, Q. At
each subsequent iteration, the best node, xmin, is removed from the open set and placed
into the closed set, R. The neighbours of this node are found and evaluated under the
following conditions:
If the neighbour is already in the open or closed sets, it is only adjusted if the path
to it is a lower cost than that found previously. If the neighbour was in the open set,
this adjustment merely changes the backpointer to xmin. If it was in the closed set, this
adjustment changes the backpointer and also removes the neighbour from the closed set,
returning it to the open. Finally, if the neighbour was in neither the open nor closed sets,
its backpointer is set to xmin and it is inserted into the open set.
The algorithm finishes either when xmin is equal to the goal state, or the open set is
empty. The former case is the only way in which the algorithm can exit successfully, and the
latter is the only means of failure. Chapter 1 briefly described the ideal of persistent auton-
omy, which desired a knowledge of when human operators are required by an autonomous
system, and as such, this failure mode is further described below.
Given that the A* algorithm only fails when the open list is completely empty, there
are only two means by which this can occur. The first is that the starting node is completely
surrounded by untraversable terrain, and thus the algorithm fails almost instantaneously;
this is essentially a trivial case. The second case requires that every reachable node in the
entire state space has been examined and placed into the closed set, leaving the open set
empty. In other words, if there is an impassible wall between the starting location and the
goal, not only does the algorithm need to detect the entire length of the wall, but it also
needs to then check every other cell on the reachable side of this wall before the open set
will be emptied. In a graph search this can make sense, as any node can potentially connect
to any other, so there could be a “teleporter” node connected to the other side of this wall,
making a search for it a sensible option. In a terrain based path planning however, once
such a wall is found, it would be desirable if the algorithm could report failure immediately
as there will never be such a teleporter; the A* algorithm does not support this.
3.2 Internal A* Structure 27
This discussion indicates that the failure mode of the A* algorithm is potentially very
slow. There are several ways around this problem, such as time-outs or detecting when a
linked set of untraversable nodes separates the goal from the current node, however only
the first of these is utilised in this thesis.
Once the A* algorithm has finished successfully, the path is extracted by following the
backpointers from the goal to the starting node. This works because each node has been
linked to the one it was found from, right back to the initial node (which points to itself,
providing an exit condition). In the failure case, no such path is returned, however if the
algorithm is stopped before completion, it is possible to return the path found thus far.
This path will always be the optimal route to its last node, but is not guaranteed to be
even a sensible starting route towards the goal.
This section has presented an overview of the operation of the A* algorithm, and
illustrated the process in pseudo-code. The remainder of this chapter explores the internal
structure of the algorithm in greater detail.
3.2 Internal A* Structure
While the overall operation of the A* algorithm is relatively simple, the internal structure
can be considerably more complicated, and presents numerous opportunities to optimise
performance, some of which are discussed in Chapter 4. This section discusses the four main
aspects of the algorithm; the representation of the open and closed sets, the selection and
use of the heuristic function, the means of finding neighbouring nodes, and the production
of the final path. This discussion does not focus on a specific software system, but raises
issues common across different implementations.
3.2.1 Representing the Open and Closed Sets
The open and closed sets are two of the most important aspects of the A* algorithm, and
as such should be implemented in an efficient manner specific to each, as the operations
most frequently performed upon them differ. For the open set, the most frequent function
is insertion, access is fairly frequent, and removal of the lowest cost element occurs slightly
less often. For the closed set, insertion is very frequent, access is fairly frequent, but removal
3.2 Internal A* Structure 28
is very rare. Additionally, both sets occasionally need to access an element and adjust its
information, but this operation is quite infrequent [15].
Representing sets in a software implementation is done using containers; particular
data structures which arrange a collection of data in a certain manner. Specific software
implementations of the most common container types have markedly differing structures,
and hence optimise different operations. For example, standard lists are very quick for
access at the beginning of the list, but considerably slower to access other elements, as one
must iterate through the list until the element is reached. Vectors provide quick access
to any element, but are much slower for insertion (if memory has not been pre-allocated).
Binary heaps, or maps, provide the the quickest insertion, removal and access, but only allow
access by value, not index. Sorted lists, or queues, optimise functions such as removing the
lowest cost element, but are somewhat slow to insert into, and do not provide any means
of accessing elements other than the top of the queue.
The complex series of actions required for the open and closed lists suggest a hybrid
implementation [15]. Such a representation would attempt to optimise all of these actions,
and one such implementation is presented in Chapter 4.
3.2.2 Selection and Use of Heuristics
Chapter 2 discussed the manner in which the A* algorithm relies on a heuristic function,
h(x), which produces an estimate of the cost of traveling from x to the goal node. It was
noted that A* is an optimal planner if and only if the heuristic is admissible; that is, it
never overestimates the cost. It was also noted that the closer this heuristic is to being
exact, the less nodes A* will search, and the faster it can operate. Appropriate selection of
this heuristic is thus of critical importance.
The A* algorithm was originally designed to operate over a cost graph structure which
can require very complicated heuristics in some scenarios. For terrain based path planning
however, the situation is usually considerably easier. Rather than estimating a cost of
traversing a complicated graph structure, the heuristic in this case is only estimating a
path distance between two points whose locations are known.
Several heuristic functions are common in the terrain based path planning literature.
Of these, the Euclidean and Manhattan distance functions are the most common. The
3.2 Internal A* Structure 29
former estimates the distance as that of a straight line between the node in question, and
the goal. The latter travels in a horizontal, then vertical direction (as if one was driving
in a Manhattan block, rather than traveling through the buildings) and is thus a larger
distance. Given that the Manhattan distance has the potential to be inadmissible, one
might well ask why it is used. The answer is that it does not require the slow square
root function of Euclid’s method, and in some cases, this speed-up can be more significant
than the slow-down from A* being sub-optimal [15]. In the cases considered in this thesis
however, optimal paths are required and the Euclidean method is thus the most suitable.
One final series of heuristic function can provide better results given a particular im-
plementation of the neighbouring node connection function, n(x). For example, a typical
connection function for a standard two-dimensional grid known as an eight-connected grid
allows for movement into any of the eight surrounding cells, much like a king moves in a
game of chess. Within this topology, a particular heuristic which accounts for the limita-
tions of this movement system will be closer to the exact path distance without ever being
inadmissible, and therefore will provide faster computation for the A* algorithm. These
three types of heuristics are demonstrated for an eight-connected grid in Figure 3.2.
y
x
Figure 3.2: Euclidean (red), Manhattan (blue), and eight-connected distance (green) func-tions for grid based path planning.
The Euclidean distance, shown by the straight red line, is calculated as d =√
x2 + y2.
The Manhattan distance, shown by the blue line, is calculated as d = x + y. The eight-
way connectivity function, shown by the green line, is calculated as d =√
2 × min(x, y) +
[max(x, y) − min(x, y)]. More detail on the problems and benefits of using eight-connected
grids to represent a continuous world is presented in Section 4.3.3.
3.2 Internal A* Structure 30
3.2.3 Finding the Neighbouring States
As discussed above, the A* algorithm also requires a function, n(x) to determine the neigh-
bours of a given node. For a typical cost graph, each node in the state space operates
like an item in a linked list, in that it has pointers to its neighbouring nodes. Finding the
neighbours of a node is then equivalent to following all these links. For terrain based path
planning a single function can be defined which calculates the neighbouring nodes of any
location, (as the neighbours are typically defined as points within a certain proximity of the
given node).
The A* algorithm can be completely generic, provided the user can provide a set
of cost information, and some function to determine the neighbours of any node. For
example, a linked cost graph, an abstract topology, or a discrete grid and an explicit function
to determine neighbouring cells are all possible methods of providing access to a node’s
neighbours. All of these representations can be rendered identical by the internal operation
of the A* algorithm, meaning it can be used to plan over any topology of cost information.
The A* planning implementation presented in this thesis makes explicit use of this feature,
leading to a very generic and reusable system.
3.2.4 Producing the Final Path
Once the A* algorithm has found the goal point, it determines the optimum path by follow-
ing the backpointers from the goal. All nodes given by these backpointers are guaranteed
to be in either the open or closed sets, as is shown below.
Every point in the state space, S, is either untraversable, not yet examined, or in the
open or closed sets. Points which are untraversable will never be in the final path, and
neither will unexamined points. As a result, the open and closed sets are the only storage
containers required; there is no need for an all encompassing collection of every node in S.
Following the backpointers thus produces a list of nodes, whose information is found
within the open and closed sets. This list of nodes comprises an A* path, but is not a list of
locations to travel to. Since each node contains its location in the topology supplied by the
user, this path can be converted to this format by interrogating each node. For example, for
a discrete grid topology, the path can be returned in grid co-ordinates; if this grid relates
3.3 Summary 31
to a real-world area, the user’s application must possess some co-ordinate transformation
to have created the grid originally, and can thus further convert the path to real-world co-
ordinates. The A* algorithm can thus produce a path in a user defined co-ordinate system,
without needing to know the specifics of the implementation.
3.3 Summary
This chapter has presented a detailed description of the A* algorithm and its operation
in Section 3.1, as well as presenting a pseudo-code formulation. Section 3.2 identified and
described the four main aspects of the algorithm; the representation of the open and closed
sets, the selection and use of the heuristic function, the means of finding neighbouring nodes,
and the production of the final path.
It was demonstrated that these internal aspects can be completely independent of
the application in which pathfinding is required; provided the user can define a means
of finding neighbouring nodes, and a heuristic suitable for the topology in question, then
the algorithm can plan effectively. More importantly, this generality is in fact true of any
pathfinding algorithm which operates over a cost graph structure.
Chapter 4 now presents a planning framework, which can make use of any discrete
pathfinding algorithm, but here is specifically implemented using the A* algorithm. The
A* algorithm used in this framework incorporates the hybrid data structure discussed in
Section 3.2.1, amongst other optimisations, and is shown to produce optimal and efficient
paths for the Argo vehicle.
Chapter 4
Planning Framework and A*
Implementation Results
This chapter discusses the design of a generic planning framework, and its specific imple-
mentation using the A* algorithm. The chapter is split into three main sections, followed
by a discussion of the results of this A* planner, as implemented on the Argo vehicle.
Section 4.1 details the requirements and features of a generic planning framework,
with specific reference to those requirements implied by the replanner system described in
Chapter 5. This planning framework allows for the use of any pathfinding system, and
any topology of planning information, but for the applications of this thesis is specifically
implemented using the A* algorithm.
Section 4.2 presents an abstract design for this planning framework, using the A* algo-
rithm as an initial implementation, because of its efficiency and simplicity. This framework
fulfills the requirements of Section 4.1, and can be used to plan over multiple sets of cost
information, in an arbitrary topology. This framework is presented as a Unified Markup
Language (UML) diagram, showing the structure and organisation of the classes that com-
prise it.
Section 4.3 discusses the specific implementation of the A* system used in this thesis;
a efficient grid-based path planner. Optimisations to the standard A* algorithm which are
incorporated into this design are discussed, and pseudo-code is presented for an optimised
and generic A* pathfinding system. This section then discusses the choice of topology, how
4.1 Requirements for a Planning Framework 33
the chosen discrete grid topology is implemented, and how this implementation is linked
into the planning framework described in Section 4.2. A brief discussion of how the paths
can be automatically optimised for use by the Argo autonomous vehicles, is also presented.
Lastly, the entire system is illustrated in a UML diagram, demonstrating both the abstract
planning framework, and the implementation surrounding it.
Section 4.4 presents the results of using this A* pathfinding implementation to produce
paths for the Argo vehicle. The implementation is incorporated into a mission planning
application, which takes a set of mission goals and plans paths between them. These paths
are planned through real-world cost information obtained from the system already on the
Argo vehicle, and a photo montage of the vehicle driving these paths is presented.
4.1 Requirements for a Planning Framework
Chapter 5 presents a detailed design for a replanning system capable of efficiently imple-
menting obstacle avoidance and path planning for an autonomous vehicle. This section
describes the requirements of planning framework for use in this and other systems, and
hence this replanner is briefly described here, in order to better determine the functionality
required by the planning framework.
The replanner system incorporates two planners; one is a global level planner which
operates over large-scale mission goals, the other is a local level planner which operates only
over the vehicle’s sensor range, but much more frequently than the global one. The cost in-
formation over which these planners operate, is constantly changing as the vehicle navigates
the world, and the local planner is used to replan over this information as it changes. Most
importantly, different mission objectives require planning over different topologies of infor-
mation, and multiple combinations of these. Currently, no available planner implementation
is easily configured for this type of planning [2, 3].
The requirements of this replanner system necessitate a number of features for this
planner framework. Firstly, it must be capable of planning over any topology of cost
information, provided a function to determine neighbouring nodes is also supplied. Secondly,
it must be capable of planning over a combination of cost information layers. Finally, a
specific implementation of it must be fast enough that a local planner using the framework,
4.2 C++ Planning Framework 34
can accommodate the high frequency changes in cost information propagated by the vehicle’s
perception modules.
These replanner requirements suggest a specific style of software design. Firstly, in
order to support different topologies, an abstract design supporting implementation specific
extensions is a logical choice; this implies the use of a language supporting inheritance
features. Secondly, in order to support different data types for the information content and
structure, a polymorphic language is required. Thirdly, the replanner system clearly needs
to perform multiple tasks simultaneously, requiring a language and operating system which
supports a threaded environment. This terminology is defined in the following section.
While these requirements do not stipulate either a language, or threading implemen-
tation, the Argo project makes use of the C++ language, and the Posix threads library,
allowing the software to be used on multiple operating systems. These choices have thus
been followed in this thesis, and the next section describes the design of a C++ framework,
and a specific A* implementation.
4.2 C++ Planning Framework
This section presents an abstract design for a planning framework written in C++, whose
structure is illustrated through the construction of an A* specific implementation. A thor-
ough description of the C++ language and its object-oriented structure is outside the scope
of this thesis, however several important features are used extensively in this planner frame-
work, and thus warrant a brief discussion. In all subsequent sections of this thesis, class
names are presented in bold font, and in the UML diagrams showing inheritance structures,
abstract classes are shown in light grey, and derived classes in dark grey.
4.2.1 C++ Language Features
C++ is an object-oriented language, meaning it specifically supports high level structured
design of classes and objects, where a class encapsulates a set of ideas related to a specific
concept, such as a car. A Car class then provides methods unique to cars, such as Go() and
Stop(), as well as holding specific data such as the current velocity or colour. Objects are
then instantiations of these classes; one particular Car for example. Any object instantiated
4.2 C++ Planning Framework 35
from a class can access any of the methods or data defined by the class, without needing to
redefine any of the functions.
This brief description of the object-oriented paradigm excludes two of the most impor-
tant features; inheritance, and polymorphism.
Inheritance allows one superclass to pass on features to subclasses. For example, a
Vehicle superclass might possess the Go() and Stop() methods. A Car class would then
inherit from the Vehicle class, giving it access to these methods without redefining them.
Moreover, this Car class can further define its own methods, such as GetPetrol() which
may not be features of all vehicles. A Bicycle class for example would still be able to
inherit from Vehicle in order to obtain the Go() and Stop() methods, but has no need for
a GetPetrol() method. In this manner, inheritance allows for simple and elegant software
designs to be implemented in a sensible manner.
One final definition which inheritance presents, is that of an abstract class. A class
is defined as abstract if it is a superclass which defines, but does not implement, certain
functions. For example, the Vehicle class could simply define that every vehicle requires
a Go() function, but not implement it itself. The Go() function is then described as pure
virtual, and must be implemented by the inheriting classes (Car and Bicycle in this exam-
ple). Alternatively, the function can be defined in a default manner, (making it virtual, but
not pure virtual), and inheriting classes can choose whether or not to extend or overwrite
this definition. Abstract classes are very important in this planner design, as they provide a
means by which the framework can plan over any topology, without needing to comprehend
the implementation-specific details.
Polymorphism is the ability of a function to operate on multiple types of data. For
example, a function to calculate the cosine of an angle may wish to operate over both
radians and degrees. By passing in the type of data as a template parameter, the same
function can be distinguished at compile time. This allows the function to be implemented
differently under the hood, but not be seen by the user. Thus, rather than needing to
distinguish between a CosineDegrees() function and a CosineRadians() function, the user
can simply use a templated Cosine() function which, based on the data type passed to it,
will automatically select the correct function to call. Polymorphism thus allows the internal
software structure to be hidden from the user, simplifying its use.
4.2 C++ Planning Framework 36
As described above, the C++ framework for this planner was designed to operate over
any topology of cost information. As such, the algorithm itself is designed as a set of abstract
classes, which are inherited from by the user’s code. A specific implementation of a planner,
such as an A* planner, then defines the type of information over which the plan is found,
and the type of connections between different nodes in this information topology. The A*
algorithm itself is templated on these data types and thus need not know how the user’s
code is structured, provided it conforms to the structure stipulated by the planner itself.
The following subsections describe firstly the abstract interface of the planner framework,
and then the specific A* implementations required for the replanner system described later
this thesis.
4.2.2 Abstract Planner Design
The highest level class in this generic planning framework is the abstract Planner class.
This class is abstract because it is implemented by linking it to any number of cost sets
and a related connectivity function, which are themselves abstract entities. A user of the
Planner class can then define implementation specific cost sets and connectivity functions
based on the rules stipulated by the CostSet and ConnectivityFunction classes.
The structure of the CostSet class is defined as an abstract entity which can be
interrogated through a series of functions in order to provide cost-to-come and cost-to-go
information, as well as whether or not the data being requested actually exists in the cost
set data store (see Section 4.3.3). The user is then free to define these methods in any way
whatsoever (presumably linking to a specifically defined topology of cost information).
Similarly, the structure of the ConnectivityFunction is also an abstract design. The
class merely stipulates that the implementation must provide a function which returns a
list of the neighbouring nodes when given a target node. However the user defines the cost
information topology, such a function should be fairly trivial to describe, and thus can be
linked into this class.
4.2.3 Multiple Cost Sets
One of the more important aspects of this planning framework is the ability to plan paths
over multiple cost sets. As described above, a cost set is an abstract entity, which when
4.2 C++ Planning Framework 37
combined with a connectivity function, is able to tie any form of cost information into the
pathfinding algorithm.
In many situations, multiple sources of cost information are available, and typical real-
world systems usually pre-combine all this information before planning. If the sources are
separable however, they should logically be maintained this way, and only combined when
needed. If the combination of sources is linear, (that is, the cost of traveling a path through
the combination is equal to the sum of the path costs through each source individually),
then the planner framework presented above can plan through the sources without them
needing to be pre-combined.
This is achieved by maintaining a list of cost sets, and linearly combining the costs from
each set whenever the heuristic function or the value of a node is invoked. Furthermore,
if weights are required for each source, such as if the most expensive (yet still traversable)
obstacle in one source is much more expensive to travel over than in another, then these
can be applied intuitively as well. Each source of cost information has the power of veto
over the the access to a given node. If any source reports that a node is untraversable, then
regardless of the cost reported by the other sources, the node is marked as untraversable
forever.
This combination of cost sets is a non-restrictive addition to the planning framework;
it does not limit its use in any way. If the cost sets are not able to be linearly combined,
they can still be pre-combined in a more complex fashion into one set, and planned over
in this form. In the A* implementation of this planning framework, the use of multiple
cost sets was seen to cause a very mild slow down to the algorithm, yet the benefit of not
needing to pre-combine cost sets is of greater value than the reduction in performance.
4.2.4 A* Planner Design
Figure 4.1 illustrates this structure for a specific Planner implementation, using the A*
algorithm. This A* implementation follows the same algorithm as presented in Chapter 3
and is encapsulated in the abstract class AStar.
The internal structure of the Planner requires a series of small classes to define data
types, and allow for the abstraction described above to operate. For example, inside the
AStar implementation of a Planner, nodes are referred to by an AStarIndex. This
4.2 C++ Planning Framework 38
Figure 4.1: This diagram illustrates the components of an abstract A* architecture. AnAStar class is an abstract entity derived from a Planner class (not shown), and combinesany number of CostSets, with a ConnectivityFunction which describes how neighbour-ing nodes are found. Both the cost sets and connectivity function are abstract entitiesthemselves, allowing the user to define their functionality.
class has no functionality, but is inherited by the user defined index types for a specific
implementation. The grid-based A* described in Section 4.3 defines a Grid2DAStarIndex
for example. This abstraction allows the algorithm to interrogate any arbitrary CostSet
by index, without needing to know anything about the index itself, or the cost set.
Nodes inside the open and closed sets are described by AStarSearchListElements,
which pair an AStarIndex to the specific details that the A* algorithm requires. Thus,
this list element stores which state the node is in, its cost-to-come, and cost-to-go. By
templating this class on the AStarIndex, this information can again be separate from the
user implementation.
Finally, as Section 3.2.4 described, the A* algorithm produces paths as lists of nodes.
More specifically, as lists of AStarIndexes. These paths are described by the AStarInd-
exPath class which allows the user to convert these lists to their relevant topology.
In this manner, the entire planning framework is separate to whatever type of cost
information it plans over. As Chapter 1 identified, the same pathfinding algorithms can
plan paths for an autonomous vehicle, a robotic arm, or some completely different puzzle
4.3 Grid-Based A* Implementation 39
solving problem. With an abstract framework, implemented with a specific algorithm such
as A*, all these types of problems can be completed by the same software, with the user
free to design the problem in whatever manner best suits it.
4.3 Grid-Based A* Implementation
The previous section described the abstract design of a planning framework, and the manner
in which the A* algorithm was incorporated into it. In order to utilise this structure, each
abstract class must have a specific implementation derive from it, and this section discusses
the manner in which this is performed, presenting the overall structure in Figure 4.9.
Subsections 4.3.1 to 4.3.2 describe optimisations to the A* algorithm, which are in-
corporated in this implementation. Subsections 4.3.3 to 4.3.4 describe different topologies
to describe the structure of an environment, and the implementation of three grid-based
methods. Subsection 4.3.5 describes the means by which these topologies are are incorpo-
rated into the planning framework, and subsection 4.3.6 describes a means of optimising
the paths produced by this A* implementation, to be better suited for the Argo vehicles.
Finally, subsection 4.3.7 concludes this section, presenting the entire structure of this A*
planner in a succinct UML diagram.
4.3.1 Optimisations to the Standard A* Algorithm
Section 3.2.1 proposed that a hybrid data structure would be most effective to represent the
open and closed sets inside the A* algorithm. Extensive testing of various data structures
has led to such a design for the open set, but not the closed, as their operations are markedly
different. The open set is here represented by a combination of two data structures from
the C++ Standard Template Library (STL); a map, and a priority queue. Both these
structures have advantages and problems, as does storing the data redundantly in both.
A map represents data as a binary heap of key-value pairs. In so doing, insertion,
removal, and access by key are very fast. Access by value however is very slow, as one
is forced to search through the entire map. A priority queue instead sorts data by value
and thus provides extremely fast access to the top of the queue, but does not provide any
access to other elements; it is thus equivalent to a sorted buffer. The combination of these
4.3 Grid-Based A* Implementation 40
two system allows the A* implementation to rapidly access elements by key(useful when
seeking neighbouring nodes), and the lowest cost element by value (which occurs once every
iteration).
The duplication of data required by this implementation is a waste of memory, yet this is
not as serious a problem as it would seem, for two main reasons. Firstly, computer memory
is far cheaper and easier to obtain than processing power. Since this A* optimisation
results in far faster processing than using a sort function, rather than a pre-sorted list, the
additional memory cost is easily justified. Secondly, the open list and the closed list can be
thought of as the frontier and interior of the search space [15], or the perimeter and area
of an enclosed shape. As a result, aside from the very early stages of the algorithm, the
open set is always considerably smaller than the closed, and even when stored redundantly,
is insignificant by comparison.
A second problem encountered by this implementation is that great care must be taken
to preserve the integrity of both versions of the open set. The worst example of this occurs
when a node is re-examined and is already in the open list. Should the new route to this
node be lower cost than the previously discovered one, the information in this node is
adjusted, and the node is left in the open set. For the map implementation of the open set,
this is a trivial task, as the value can by adjusted independently of the key. The priority
queue however has no such modification method. As a result, the entire queue must be
emptied, and then recreated from the contents of the map version of the open set, in order
to re-sort it effectively. This is the most costly operation that occurs from the use of this
combined operation, however it is also the rarest operation in the A* algorithm, and in many
commercially available A* implementations, is not even correctly accommodated [15]. As
a result of this rarity, and the low size of the open set, this problem is considered to be
insignificant, and the performance improvements arising from the combined representation
of the open set, are far more important than the problems it creates.
Another optimisation added to the implementation is an additional piece of information
attached to each node. To ascertain whether a node is new, in the open or closed sets, or
already seen and found to be untraversable, the traditional implementation searches each
set and uses a series of logical comparisons to make the decision. This searching is very
expensive from a computational standpoint, and can be avoided by expending slightly more
memory in storing which particular case applies to each node in the structure. By so doing,
4.3 Grid-Based A* Implementation 41
the node’s state can be ascertained in the same time frame as a standard access into the
data structure, and this process is considerably faster than a search by key, even into a map
structure.
4.3.2 Optimised A* Pseudo-Code
This section presents pseudo-code for the optimised A* algorithm in Figure 4.3, and in
addition to those defined in Section 3.1.2, the following notations apply:
• Qmap is the map representation of the open set.
• Qqueue is the priority queue representation of the open set.
• The function F used to sort the open set, Q, is not mentioned, as it is implicitly
defined in the creation of Qqueue and is never called specifically.
The results of applying these optimisations to the A* algorithm are shown graphically
in Figure 4.2, where the value being compared in this figure is the number of node expan-
sions per second the algorithm can perform. These results show that the sorted open set
optimisation makes by far the largest difference to the algorithm performance; the other
optimisations are useful, but have a much less significant effect. The completely optimised
solution is approximately twice as fast as the basic A* algorithm.
Figure 4.2: Graphical demonstration of various optimisations on the performance of the A*algorithm. In this diagram, the speeds of each implementation are shown in expansions persecond ; the number of nodes in the state space that can be examined in a second.
4.3 Grid-Based A* Implementation 42
AdjustValues(x′,x)parent(x′) = xg(x′) = g(x) + c(x, x′)
FindPath( xI , xG )g(xI) = 0parent(xI) = xI
insert xI into Qmap and Qqueue
while(size(Q) > 0 )xmin = first element in Qqueue
remove first element of Qqueue
remove xmin from Qmap
forall x′ ∈ n(xmin)if x′.state = UNTRAV ERSABLE
continueif x′.state = CLOSED
if g(xmin) + c(xmin, x′) <= g(x′)remove x′ from RAdjustValues(x′,xmin)insert x′ into Qmap and Qqueue
elsecontinue
if x′.state = OPENif g(xmin) + c(xmin, x′) <= g(x′)
remove x′ from Qmap
AdjustValues(x′,xmin)clear Qqueue
insert x′ into Qmap
rebuild Qqueue from Qmap
elsecontinue
elseAdjustValues(x′,xmin)insert x′ into Qmap and Qqueue
if( xmin = xG )while( parent(xmin)! = xmin )
add xmin to front of pathxmin = parent(xmin)
return path
Figure 4.3: Optimised A* Pseudo-Code
4.3 Grid-Based A* Implementation 43
4.3.3 Representing the World
As Chapter 2 described, the A* algorithm is a discrete path planner; it cannot plan across
continuous space, only a discrete representation. For autonomous vehicle navigation, and
other applications which utilise 2.5D environments, (a surface where only one z-value is
applicable to any x/y-value pair), grids, topological maps, and quad-trees provide especially
suitable data structures. This section describes the features and problems of all these
representations, concluding that as a first implementation, discrete grids are a sensible
choice. Subsection 4.3.4 then describes the functionality required to actually implement a
discrete grid in a C++ structure suitable for use with the planner framework.
The A* algorithm plans over a cost-graph; a list of nodes with associated edge costs
between connected nodes. The planner framework described above abstracts this notion to
operate over a series of cost sets, where neighbouring nodes are found with a connectivity
function. Both of these representations implicitly assume a discrete state space, and for
autonomous vehicle navigation, the key application for this thesis, this space is the physical
environment, and is clearly a continuum. To remedy this problem, some means of rendering
this environment discrete is required, and the vast majority of autonomous vehicle literature
makes use of the three methods described below.
Topological Maps
Topological maps represent an environment exactly like a cost-graph; a list of nodes, con-
nected by edges, hence this is easily linked to the A* algorithm for planning purposes. In
an outdoor environment, a topological map would most likely represent a discrete set of
target points, with pre-computed edge costs between them. This pre-computation is unfor-
tunately untenable for the autonomous system envisaged for this thesis. The final system
must be able to enter an unknown environment, and still make rational decisions; having
to pre-analyse the environment renders this moot.
For larger scale autonomous vehicle missions, such as traveling along a road network,
topological maps make far more sense. One can quickly and easily pre-calculate or obtain
the road segment distances, and constrain the path plan along these roads. This works well
because the road network is already a discrete state space of movement options. Outdoor
4.3 Grid-Based A* Implementation 44
environments do not restrict motion in any direction, making a data structure which assumes
a discrete set of choices redundant.
Grid Representations
Grid representations are a much better choice for outdoor environments, as they separate
the world in cells, where any information in real-world x/y co-ordinates is constrained to
lie within the closest cell. This works much like a chess-board with rice scattered on it;
regardless of the actual precise location of each grain, one can approximate the layout
simply by stating how many grains are in each square. The smaller the size of the cells, the
better this approximation is.
As briefly mentioned in Section 3.2.2, typical connectivity functions for grids are the
eight-way connection; which is similar to a king’s range of moves on a chess board, the four-
way connection; which is similar to a castle’s moves, or some other variation which would
allow motion in even more directions. This limited range of movement directions is one of
the biggest failings of grid based path planners; it is trivial to show that at worst, an eight-
way connection can result in paths up to 8% longer than the optimum directions of travel
[31], four-way connections can be 41% worse. While there are methods of avoiding these
limitations, either through post-processing of the paths, or interpolating a larger range of
directions through the discrete grid, these are not considered in this thesis. A more complete
analysis of these problems is presented in [31], and is also discussed in Section 7.2, later in
this thesis.
Using these connectivity functions only informs A* of the neighbours for each cell,
the algorithm also requires a means of extracting the path cost from the grid. There are
numerous ways of doing this for a standard grid, but the most common method is to define
the path cost of moving to an adjacent cell as the distance (which is dependent on the
connectivity function being used), multiplied (along with some scaling factor) by the value
of the grid cell being entered. Alternate methods use the cost of the cell being exited, or
some combination of the two, and these ideas are further discussed in Section 4.3.5 below.
When the grid itself, a definition of how costs are extracted from it, and a connectivity
function are all supplied, this is then sufficient information to provide a discrete state space
to the A* algorithm, and thus perform effective path planning.
4.3 Grid-Based A* Implementation 45
Sparse Grids and Quad-Trees
One problem this previous discussion has ignored, is the effect of sparse data. For most
autonomous vehicle navigation problems, the cost sets are built up from data acquired by
the vehicle’s sensors. Most sensors provide only a discrete set of data points; for example,
laser scanners may provide 0.5° angular resolution, meaning distance measurements are
only provided at these discrete angles. When this data is converted to a Cartesian or
grid representation, such as building an occupancy map from laser data, depending on the
resolution of the grid, there may be many cells with no data in them.
One solution to this is simply to increase the size of each cell, decreasing the resolution
of the grid. This has the clear downside however, that the approximation to the real world
is correspondingly worse. Alternatively, one can ignore the empty cells, or define them to
contain some default value, however both these solutions have another problem.
By representing all this empty space with an empty grid cell, more memory is required
to store the grid, even though many of the cells may be completely uninformative. A grid-
like data structure which avoids this problem is known as a quad-tree, which approximates
the world with square cells, but stipulates a maximum number of data points that each
cell can contain. Cells start off as large as possible, and subdivide into four equally sized
cells, with sides half the original length, recursively until no cell contains more than this
maximum number of data points (and this number can be one if so desired). By doing this,
the representation can store only the location, size, and value of each cell; sparse areas are
represented by larger cells, thus drastically reducing the memory requirements.
The downside to quad-trees is their additional complexity over grids; it is not as simple
to compute the neighbouring cells, or what the cost of traversing them should be. As a result,
this thesis has not implemented a quad-tree solution, however such an implementation would
be readily incorporated into the A* framework.
Of all these three methods, quad-trees are the most appropriate representation for
outdoor environments, yet are also the most complicated to implement. As a result, this
thesis elected to implement the far simpler discrete grid method as a first option, with
quad-trees left as a future option, as described in Section 7.2.
4.3 Grid-Based A* Implementation 46
4.3.4 Map, Vector, and Deque Grid Implementation
The previous subsection evaluated the three main methods of providing a discrete represen-
tation of real-world environments, and concluded that grid based methods were the most
suitable. Despite the high memory use for sparse data, a grid which sets a default value
for uninformative cells is the best option for this thesis. This subsection now describes the
C++ implementation of such grids, and the subsequent describes how these are specifically
integrated into the A* framework.
A grid, or more specifically, a 2D grid, is defined as an abstract class, so that different
implementations can internally represent it with different data structures. This Grid2D
class defines a grid as being templated on the data type used for its x/y indices, and the
data type of the values stored at each location. It defines the methods an implementation
must possess, such as accessing and altering particular cells, clearing regions, and operating
on collection of cells. The map, vector, and deque implementations referred to in this
subsection’s title are all specific inherited classes, which possess slightly different features,
and are based on different data containers from the STL. This class structure is illustrated
in Figure 4.4.
Figure 4.4: A class diagram illustrating the relationships between the abstract Grid2Dclass and the map, vector, and deque implementations of it.
A MapGrid2D stores each cell of information as a key/value pair from the STL. The
grid itself is then a map of maps; the columns of the grid are addressed by x-locations, and
are maps of y-location/value pairs. Thus to access a specific location requires accessing the
grid with an x-location to retrieve a one dimensional map. This one dimensional map is
then accessed using the y-location to retrieve the specific value.
The advantage of this representation is that empty cells do not require storage, allevi-
ating the problem alluded to in Section 4.3.3. The problem is that even though a map is
4.3 Grid-Based A* Implementation 47
implemented in the STL as a binary heap, for large grids performing a search to find a value
is an expensive operation. The following two classes alleviate this problem by providing
direct access to each element, at the expense of requiring additional memory.
The VectorGrid2D and DequeGrid2D classes store each cell of information as a
value only, with a linear mapping in place to allow direct access by x/y-location. The
grid itself is an STL vector of vectors, or an STL deque of deques (a deque being roughly
equivalent to a double ended vector; it can be accessed with the same speed at both ends).
Access is similar to the MapGrid2D in that the x-location allows access to the columns,
and the y-location retrieves a specific value from a column.
These two implementations are almost the exact opposite of the MapGrid2D in that
they provide very fast access to elements, but bring back the problem of excessive memory
use in sparse environments. Nonetheless, for the application of this thesis, namely path
finding over cost maps generated from real-world data, it was found that the DequeGrid2D
provided the fastest implementation. These results are shown in Figure 4.5 below.
The vector and deque implementations are clearly faster than the map version, with
the vector being slightly faster than the deque. This result is slightly invalid, because in
the testing scenario, due to the alignment of the image data, the vectors are only appended
at their ends and not their starts, meaning the inefficiencies of this are not seen. In more
realistic situations, the deque implementation is considerably faster than the vector.
4.3.5 Cost Functions, Data Sets, and their Combination
Section 4.3.3 briefly noted that there are many possible ways of defining a cost function for
grid representations; a function which determines the cost of travel between two neighbour-
ing nodes. Thus, a grid is in fact only a collection of data - a data set, and only becomes
a cost set when combined with such a cost function. Given that there are multiple ways
of describing a data set, of which a grid is but one, and there are also multiple ways of
describing cost functions, it is logical to define abstract representations of each of these
concepts.
This is precisely the structure designed in this thesis; a new abstract class, a Dat-
aCostFunctionCostSet inherits from a standard CostSet, and is defined to have one
CostFunction, and one DataSet. A Grid2D is then one possible implementation of
4.3 Grid-Based A* Implementation 48
Figure 4.5: A* algorithm speed for three different grid implementations. In this diagram,the speeds of each implementation are shown in expansions per second ; the number of nodesin the state space that can be examined in a second.
a DataSet, just as MapGrid2D, VectorGrid2D, and DequeGrid2D are all possible
implementations of Grid2D. Figure 4.6 illustrates the class diagram for the abstract Dat-
aCostFunctionCostSet implementation.
For the applications of this thesis, several such CostFunctions have been implemented.
These implementations are highly specific to the task at hand, as they typically are depen-
dent on the type of connectivity function being used. For the four-way and eight-way grid
connections, the distance cost of movement to any neighbour can be found using the Euclid-
ean distance. Combining this measure with the cost value of the cell being entered provides
a suitable cost function for path finding purposes. Figure 4.7 illustrates the class diagram
for the abstract CostFunction implementation.
4.3.6 Post-Optimisation of A* Paths
One final addition of this grid based implementation is a framework for the post-optimisation
of A* paths. This is not required functionality, but provides a means of accommodating
the limitations of the A* algorithm, the grid-representation, and the vehicle dynamics.
The A* algorithm, in any incarnation, produces paths as lists of nodes in the state space
4.3 Grid-Based A* Implementation 49
Figure 4.6: A class diagram illustrating the relationships between the abstract DataCost-FunctionCostSet class, the CostSet class it inherits from, and the DataSet and Cost-Function classes it holds.
Figure 4.7: A class diagram illustrating the relationships between the abstract CostFunc-tion class and the Euclidean, Manhattan, and 8-way grid implementations of it.
that should be traveled through in a specific order. For this grid based implementation,
these nodes will be in adjoining cells of the grid, simply because of how the connectivity
4.3 Grid-Based A* Implementation 50
function is defined. For the Argo vehicle used in this thesis, the vehicle control system is
currently applied such that the vehicle follows waypoint paths; lists of locations joined by
straight lines. While the A* paths can trivially be converted into these waypoint paths,
because of the proximity of the nodes in the A* path, the waypoints will be excessively
close. Depending on the tolerance with which the vehicle follows these waypoints, this can
result in problems.
To rectify this, a post-optimising framework was designed to perform a user defined op-
eration upon these paths, converting them to waypoint files in the process. This framework
defines an abstract PathOptimiser class and a PathOptimisingAStar which inherits
from the standard AStar class. The PathOptimisingAStar class stores a list of however
many optimisers are required, and runs all of them in sequence after the normal pathfinding.
These PathOptimisers are abstract, and user implemented, and thus several com-
monly used ones were implemented for this thesis. These include functions to reduce the
number of waypoints in the final paths using various distance measures, adjusting the de-
sired vehicle speed according to the curvature of the path segments, and adjusting the
vehicle speed according to the cost map information along the route. All these optimisa-
tions further improve the ability of the vehicle to follow the paths produced, by alleviating
the limitations of various approximations undertaken in the planning process. Figure 4.8
illustrates the class diagram for the abstract PathOptimisingAStar algorithm.
Figure 4.8: A class diagram illustrating the relationships between the abstract PathOpti-misingAStar class, the AStar class it inherits from, and the many possible implementa-tions of the PathOptimisers it holds.
4.3 Grid-Based A* Implementation 51
4.3.7 The Complete Implementation
This final section presents the entire grid-based A* implementation designed and con-
structed in this thesis, in the UML diagram of Figure 4.9. This diagram shows the abstrac-
tion of the A* implementation from the high-level Planner class, illustrating the manner
in which other pathfinding algorithms could be incorporated into the planning framework.
Figure 4.9: Class diagram of the complete A* implementation.In this diagram, the abstract classes are shown in light grey, and the specific imple-
mentations in dark grey.
4.4 Results 52
4.4 Results
To effectively test this planning framework, and more specifically, the grid-based A* imple-
mentation of it, additional software was developed. This software implements a pre-mission
planner, able to read a human supplied list of waypoint goals (the mission), and any number
of cost maps as images or data, and plan appropriate paths between each pair of consecu-
tive waypoint goals. The paths are then spliced together, and passed through the desired
optimisers, as well as being converted back to real-world co-ordinates. The final output is
then a list of waypoints which are sent to the vehicle, which follows this path autonomously.
This software is not Argo specific, and with very minor modifications to the final waypoint
conversion, could be used in virtually any autonomous ground vehicle.
The results of applying such a path to the Argo vehicle, using real-world cost infor-
mation, are shown below. This information was gathered by driving the vehicle through
a roughly circular area (approximately 20m radius), using the real-time cost information
software described in Section 6.3. This cost information was then converted to an image and
used as the input to the mission planner software. The mission path was an arbitrary series
of waypoints, some of which had costly terrain, or solid obstacles, near to the waypoints,
or along the direct path. This cost information, and the mission plan, are shown in Figure
4.10.
The resulting A* path plan through this terrain, following this mission plan is shown
in Figure 4.11. This waypoint path clearly avoids the white obstacles, and also travels
up the road, which is flat and appears as the most traversable type of terrain in this cost
information. This is the desired result of such a plan, and is in a form which can be sent
directly to the vehicle.
In this cost information, the white obstacles are all large circles, compared to the
medium cost information (the grey areas). This is because the solid obstacles (completely
untraversable) have had a configuration space (C-Space) transform applied. This transform
is further defined in Section 6.3.2, but is used because the A* planner treats the vehicle as
a point particle. In this case, the obstacles are expanded by half the vehicle’s maximum
dimension, meaning that even if the plan passes the absolute edge of a solid obstacle, the
vehicle should still avoid it.
4.4 Results 53
Figure 4.10: Mission plan through real-world cost information.In this diagram, the white regions represent solid obstacles, and the darker the colour,
the more readily traversable the terrain. The green line segments show the initial missionplan, which deliberately passes through obstacles.
Figure 4.11: A* plan through real-world cost information.In this diagram, in addition to the information from Figure 4.10, the red circles (which
have a numeric tag on their right) are the series of waypoints produced by the A* planner.
4.4 Results 54
Figure 4.12: Montage of the Argo vehicle following an A* plan through real-world costinformation.
4.4 Results 55
Figure 4.12 illustrates the progression of the vehicle along this waypoint path. In photo
1, the vehicle is at the start of its plan, preparing to drive up the road. Photo 2 shows
the vehicle having turned off the road and driven along the grassy area, yet avoiding the
fallen log. This route was chosen because as the cost information shows, the grooves in
the road surface are recorded as a higher cost than the smoother grass. Photo 3 shows the
vehicle approaching a barrel, the top-leftmost set of white obstacles in the cost information
of Figure 4.10. Photo 4 shows how narrowly (yet successfully) this obstacle is avoided.
The two primary reasons for this proximity are the optimistic C-Space transform, and the
difficulties of the vehicle in negotiating sharp corners, as required by this waypoint path.
These issues were discussed in Section 4.3.6 and are suggested as areas of future work in
Section 7.2. Photos 5 and 6 show the vehicle having negotiated this obstacle, and heading
towards the later sections of the path plan.
These results demonstrate the utility of the A* framework and grid-based implemen-
tation. Although the Argo vehicles struggle to follow the path at 2m/s, this is true of any
vehicle, and the development of better path-optimisers to adjust the desired speed accord-
ing to the curvature of the path are expected to reduce this problem. More conservative
expansion of obstacles would result in longer paths, but would allow for the vehicle to track
the paths badly and still avoid collisions.
The A* implementation runs at an acceptable speed, but likely has additional areas for
optimisation. Depending on the cost information, a 100 by 100 pixel image can be planned
across in approximately 0.2s, and the algorithm is O(n2), where n is the number of states in
the state space. This is slower than other implementations such as [31], primarily due to the
heavy templating of the framework code. Templated code requires more operations to reach
the actual implementation; a function call to access a specific node in the A* data store for
example, must access the CostSet, which may then need to access a DataCostFunction-
CostSet and subsequently the DataSet. This DataSet may then be a Grid2D, which
is specifically implemented as a MapGrid2D, which then requires searching through the
internal container class. This arrangement takes only slightly more time than direct access
to the data, but for an algorithm such as A*, which needs to access individual nodes as fast
as possible, the cumulative slow down is significant. The framework is designed however, to
favour generality over efficiency, and in this paradigm, the reusable framework more than
compensates for its lower speed.
4.5 Summary 56
The use of compiler optimisations offers the possibility of improved performance, by
focussing the compiled code for a specific processor and operating system. Additionally,
the use of inline functions, whereby the function’s code is inserted directly where it is
used, rather than calling into it, may improve the performance. Inline functions can thus
reduce the number of function calls required to reach the actual implementation, helping to
solve the problem described in the preceding paragraph. A thorough investigation of these
optimisations was not undertaken in this thesis due to time constraints, but is a likely area
of future work, as suggested in Section 7.2.
4.5 Summary
This chapter presented an abstract framework for path planning over any form and combi-
nation of cost information. This abstract framework was then implemented using the A*
algorithm, and several optimisations to the standard algorithm were presented, with their
benefits assessed in terms of computational speed versus memory usage. It was concluded
that for the applications anticipated by this thesis, the use of additional memory was of far
greater benefit than the cost it invokes.
A grid-based implementation of this A* planner was subsequently presented, and suc-
cessful results were obtained. This implementation incorporated further abstract concepts
of cost functions and data sets, and illustrated multiple ways in which these concepts could
be applied. Finally a post-optimisation stage was applied to the pathfinding system in order
to reduce some of the effects that the discrete grid has upon the paths, and the waypoint
controller has upon the vehicle’s ability to follow them.
This framework and implementation have been successfully incorporated into the Argo
project, and subsequent field testing has shown the implementation to be well suited for
its intended purposes. The real-time replanning system presented in the following chapters
improves and extends this framework and implementation, and is shown to be a novel and
effective means of autonomous vehicle control.
Chapter 5
Replanner Design
This chapter presents a novel replanning system, capable of performing optimal and efficient
plans through dynamic environments. This replanner combines two separate planners, one
to perform global level plans between mission objectives, and another to perform faster
local level plans through the vehicle’s sensor space. The system is thus able to combine the
distinct tasks of local obstacle avoidance, and unknown terrain traversal.
The motivation for such a system is described in Section 5.1, and the logical structure
of the proposed replanning method presented in Section 5.2. The method is subsequently
extended for use in autonomous vehicles in Section 5.3, incorporating the practicalities
of its implementation into a software architecture. This architecture is then described in
its separate modules, and the overall logic presented in Section 5.4. Finally, a MatLab
simulation of the system is presented and evaluated in Section 5.5.
5.1 Obstacle Avoidance and the Sensor Horizon
The use of obstacle avoidance presumes some level of knowledge about an environment;
without this knowledge, the obstacles are unknown, and there is simply no method for
avoiding them. Furthermore, all sensors used by autonomous vehicles (and other obsta-
cle avoiding systems) have a finite range, meaning the environment beyond this range is
similarly unknown. This sensor range, known as the sensor horizon, is the limit of envi-
ronmental knowledge; there is no obstacle information beyond this horizon, and hence no
5.2 A Logical Replanning Method 58
point attempting to plan obstacle avoiding paths in this area. For a typical autonomous
vehicle however, planning through unknown environments is a necessity. Given that there
is no information available beyond this sensor horizon, these plans are, at best, unreliable.
To accommodate this problem, the traditional answer has been to replan paths when-
ever the environmental information changes. As Chapter 2 describes, this brute-force re-
planning approach is decidedly sub-optimal when much of the environment is unknown.
The D* method described by Stentz et al in [16] was shown to be one optimal replanning
method, however it is a complicated, and A* specific (being a dynamic version of it) al-
gorithm. The replanner system presented in this thesis solves the same problem in a less
provably efficient, but arguably more logical manner.
5.2 A Logical Replanning Method
The previous section indicated an incongruity in the objectives of autonomous vehicle con-
trol; the vehicle needs to plan through unknown terrain, but simultaneously must avoid
obstacles in its path. If the assumption is made that within the vehicle’s sensor range,
environmental information is available, then plans can be made accurately in this region.
Planning through the remaining unknown areas requires another assumption; a heuris-
tic method like those described in Chapter 3. This section describes this method, which
logically separates the disparate tasks of local obstacle avoidance, and unknown terrain
traversal. The subsequent section then extends this method to accommodate the niceties
of autonomous vehicle control.
While the use of brute-force planning methods was described in Chapter 2 and found
to be lacking, many aspects of the method are logical, and are incorporated into the re-
planner system described below. Replanning whenever environmental information changes
is ineffective when some areas in the planning space remain unknown, or when the planning
space is large, yet it is nonetheless a logical procedure, as it produces optimal paths (with
respect to the known environment information) at the highest rate possible. Within the
vehicle’s sensor range, brute-force replanning will always produce the optimal path to a
goal at the sensor horizon, because the environmental information is already known. Thus,
if the vehicle has moved, or new information is received, it is logical to replan within this
region, to ensure the vehicle’s path remains optimal.
5.2 A Logical Replanning Method 59
Defining this planning within the vehicle’s sensor horizon as the job of a local planner,
brute-force methods can logically be applied to this planner. Though brute-force planning
fails in unknown environments, the causes of this failure are not present for a local planner,
suggesting that the brute-force approach is likely to be effective. Furthermore, planning
over just this local region is far faster than over the entire world, and is thus more feasible
for a brute-force method. The desire to have the vehicle constantly following an optimal
path further motivates the use of such a brute-force planner, in order that new optimal
plans are produced as fast as practical.
All path plans require a start and end point, and for the local planner described above,
while the local environment is known, the overall goal of the path may lie outside it, meaning
it cannot be reached by a local plan. An initial suggestion might be to locally plan from the
vehicle’s current position, to the point on the sensor horizon along the line connecting the
vehicle and the overall goal. While this is a logical approach, it is based upon the heuristic
that a straight line path to a goal is likely to be shortest. As discussed in previous chapters,
this is not necessarily so, and furthermore, to preserve generality, it would be best to allow
a user of the system to determine the appropriate heuristic to take.
To achieve this, a larger scale global planner is used, which finds a path from the
vehicle’s current position, to the overall goal. Using the framework described in previous
chapters, this planner can use any heuristic a user desires, and its output is used as the
input to the local planner to determine the local path goal. In the local planner then, the
furthest point on this global plan that still lies within the vehicle’s sensor horizon, is used
as the local goal. Every time the local planner replans, this goal point is re-evaluated, and
the local plan will thus be optimal, with respect to the original global plan. Assuming an
admissible heuristic was used by the global planner, the global plan is the best that can be
obtained through the unknown terrain, and thus the local planner’s goals are logically and
appropriately defined.
The procedure for a replanner system incorporating these two planners is described in
pseudo-code in Section 5.4, but is also described in brief here. The vehicle starts by making
a global plan between its current position, and the overall goal. Local plans are made as
rapidly as possible from the vehicle’s current position, to the furthest point on the current
global plan which still lies within the sensor horizon. If ever a local plan cannot be found,
or the local goal point is situated in a region of obstacles, the vehicle is stopped, and a new
5.3 Extending this Method to Autonomous Vehicles 60
global plan is performed. This process continues until either the global plan fails, or the
goal is reached.
The main feature of this method is the separation of planning requirements; the need
to plan through unknown terrain is fulfilled by the global plan, and the requirement of
obstacle avoidance is achieved by the local plans. The method works regardless of the prior
knowledge of the world; in the case where the entire environment is known, the local plans
exactly follow the optimum global plan, and the actual route followed is thus still optimal.
5.3 Extending this Method to Autonomous Vehicles
This method is easily extended to an autonomous vehicle system, with only a few caveats.
The method described above does not take account of the finite time required for planning;
for an autonomous vehicle, this path planning process must be performed iteratively in a
computer, and as the previous chapters describing the A* framework illustrated, this speed
is not particularly high. Another significant difference is that due to the finite planning
time, the vehicle position can change, and thus new information can be acquired whilst
planning is being performed. This section now describes the local and global planners,
incorporating these differences, for an autonomous vehicle replanning system.
5.3.1 The Local Planner
An autonomous vehicle should, ideally, not need to stop while locally replanning. Thus
while the local plan is being prepared, the vehicle is still moving, meaning the initial point
of the local plan cannot be the position the vehicle was at when the plan started, and must
instead be the position the vehicle will be at when the plan is complete. This requires
some means of predicting the future motion of the vehicle, and while this can be achieved
in many ways, only the most trivial of these has been considered for this thesis, although a
framework for implementing any method is described in Chapter 6.
The vehicle already possesses a path plan, which is about to be updated by the next
local plan. The simplest model assumes the vehicle to travel perfectly along this path, at
the velocities dictated by the waypoints themselves. If an estimate of the time required by
the local planner is available, it is trivial to predict the future position of the vehicle after
5.3 Extending this Method to Autonomous Vehicles 61
this time. Such an estimate can be determined by timing every local plan, and keeping a
rolling average of the results. To a first approximation, the A* algorithm takes a maximum
time directly proportional to the square of the Euclidean distance connecting the start and
goal points. In this local planner case, this distance is simply the sensor range, and by
timing each plan, an algorithm speed in distance per second can be determined. Future
plans can thus have their duration estimated using this algorithm speed multiplied by the
sensor range. Also, since this forward prediction is intended to ensure the starting point
of the new plan is at least as far ahead as the vehicle’s actual position will be, using the
maximum planning time is appropriate to provide this guarantee.
With a prediction of the vehicle’s position after the planning process, the starting point
of any local plan could thus be this point. Unfortunately, the practicalities of vehicle control
and estimation mean that the likelihood of the vehicle actually being in exactly this position
is fairly low. A better option is to predict this position in the same manner, but make the
starting point of the local plan the point on the global path just past this position. Using
this method, even if the vehicle position is slightly different to that predicted, the starting
point will still be an appropriate position upon the global path. The ending point for any
local plan is logically the furthest point along the global path that is still within the vehicle’s
sensor horizon, and this is considerably easier to determine.
Using these two starting and ending points, the local planner’s speed of operation is
completely determined by the vehicle’s sensor range. Should this speed be too great, (such
as if the time required means the vehicle completes its current path before the local planner
is finished, meaning it has to stop), then the target point can be moved closer to the starting
point. Such a plan is still optimal as all the information is still known, but will be faster to
produce by approximately the square of the reduction in path distance.
Defining the starting and ending points using a vehicle motion prediction and an average
measure of the local planning speed, provides for a local planner which can perform efficient
and optimal obstacle avoidance, without needing to pause the vehicle while planning is
performed.
5.4 The Replanner Logic and Structure 62
5.3.2 The Global Planner
The global planner is much simpler than the local planner, as its target position is always
the next mission goal, and its initial position can simply be the current vehicle position.
This initial position is acceptable because whenever the global planner is called, the vehicle
is stationary, (as discussed below), and thus will be in the same position after the plan
completes.
The only caveat with the global planner is that the vehicle must be stationary during
its operation. Were this not the case, it too would need to use this vehicle motion prediction
for a start, and could also lead to the vehicle colliding with an obstacle on the previous
local path. Thus, in the replanner logic, discussed in the following section, when the local
planner fails to find a path and must replan globally, the vehicle’s path must be emptied.
In the implementation of the waypoint controller onboard the Argo vehicle, this is sufficient
to halt the vehicle, but were it not, a command to do so would also be required.
5.4 The Replanner Logic and Structure
This section presents the replanner logic in a pseudo-code format, shown in Figure 5.1, for
a replanner where sensor information is acquired synchronously with vehicle motion. That
is, when the vehicle moves into a new location, the corresponding perception information is
immediately available. While this is not a realistic assumption, it is sufficient for the MatLab
simulation presented in Section 5.5, and is extended to a real system in the following chapter.
The structure of this Replanner class is shown in the UML diagram in Figure 5.2. This
diagram illustrates the manner in which the Replanner uses the A* framework described
in the preceding two chapters. Finally, although the LocalPlanner and GlobalPlanner
are distinct objects, they share the same AStar path planner owned by the Replanner,
as their operation is never synchronous.
5.4 The Replanner Logic and Structure 63
while( missionGoals �= ∅ )get currentV ehiclePositionif( currentV ehiclePosition is within tolerance of currentMissionGoal )
remove currentMissionGoal from missionGoalsif( globalPath �= ∅ )
find globalPath from currentV ehiclePosition to currentMissionGoalif( unsuccessful )
return FAILUREelse
vehiclePath = globalPathelse
get predictedV ehiclePosition after local planning timefind localPath from closest point in globalPath after predictedV ehiclePositionif( unsuccessful )
clear vehiclePath, stopping vehicleglobalPath = ∅
elsevehiclePath = localPath
Figure 5.1: Pseudo-code presentation of the logic for the replanner system. This systemassumes a synchronous acquisition of sensor information with vehicle movement.
Figure 5.2: UML diagram showing the structure of the replanner system.
5.5 MatLab Replanner Simulation 64
5.5 MatLab Replanner Simulation
The MatLab simulation presented herein implements the replanner system described above,
in conjunction with an efficient A* algorithm which incorporates the same optimisations as
presented in previous chapters. Sensor information is simulated by reading a radius around
the vehicle from a cost map image file. While this ties the sensor information synchronously
to the vehicle movement, it does illustrate the operation of the system over real-world data,
and as such, is a relatively realistic test of the replanner system.
The simulation is also designed to test several other features of the replanner sys-
tem. The cell size of the grid is adjustable to allow for testing of sparse data sources, and
uninitialised data can be set to have different values which determines whether the vehicle
actively explores unknown terrain or not. The heuristic functions of the A* implementation
can also be user determined, allowing for different ideas to be tested, or the A* algorithm to
be tuned for optimality or speed. As a demonstration of the utility of this simulation, it was
in fact implemented before the A* framework presented in Chapters 3 and 4, and several
of the optimisations presented therein were discovered through the use of the simulation.
While the results are difficult to demonstrate in static diagrams, the annotated images
in Figure 5.3 illustrate the operation of the replanner, and demonstrate its robust perfor-
mance over real-world cost map information. Read across and down the page, the images
show the world map and the start and goal points, the global path (red) in the absence of
any other information, the local path (blue) having seen an area of the world, the new global
path after local planning fails, the local path following this new global path effectively, and
the final path taken by the vehicle.
5.6 Summary
This chapter has presented a logical, efficient, and robust replanning system, suitable for
use in an autonomous vehicle. The system separates the two distinct requirements of
planning through unknown terrain and obstacle avoidance, through the use of separate
global and local planners. The system has a logical failure mode, and the MatLab simulation
demonstrates its utility and reliability over a range of real-world data.
5.6 Summary 65
Figure 5.3: Diagrams of the MatLab replanner simulation planning over a human markedrepresentation of real-world terrain.
Chapter 6
Replanner Implementation and
Results
This chapter presents a real-time implementation of the replanner system described in Chap-
ter 5. Unlike the main assumption of the simulated system, in this real-time system, updates
to the cost map information are completely asynchronous with vehicle motion. This requires
that the system be capable of performing several tasks simultaneously; a process known as
threading, and described in Section 6.1. Furthermore, the vehicle’s navigation data, and the
cost map information need an efficient way of informing the overall system when new in-
formation is available. One means of achieving this, known as the Subject/Observer design
pattern [34], is described in Section 6.2.
Sections 6.3 and 6.4 discuss a means by which this additional information is incorpo-
rated into the replanner system, and how the different thread tasks are linked. A method to
ensure vehicle safety by invalidating the remainder of a path after an obstacle is seen to lie
upon it is presented, and can be extended to use a similar process to the path-optimisation
step seen in Section 4.3.6.
Section 6.5 combines the requirements and ideas presented in the previous sections
and describes the complete replanning implementation. Finally, Section 6.6 describes a
useful simulation environment for the testing of this implementation, and presents results
illustrating its success.
6.1 Threading and Data Protection 67
6.1 Threading and Data Protection
Threads are the standard way of performing simultaneous actions on a single processor, and
while these actions are not strictly simultaneous, threads nonetheless provide an effective
implementation of parallel streams of execution.
The primary method by which this is achieved is known as time slicing. Each thread
in the system is given a slice of time in which to execute its actions, once these actions are
complete, or the time slice elapses, the processor is given to another thread. This context
switching saves the state of the exiting thread’s stack and program counter, and retrieves
this saved information for the entrant thread. As a result, when a thread starts a new time
slice, it is in exactly the same position as when it left, and can continue operations from
this point.
The main difficulty with the use of threads is that memory is shared between all of
them. Thus if one thread were to write over data which another thread was in the middle
of reading, this data is potentially corrupted, and the program may fail in unexpected
manners. To avoid these problems, a range of synchronisation objects are available. These
objects can be wrapped around data or functions and implement some means of preventing
access to other threads.
The most common and basic of these methods is the MUTual EXclusion (mutex). A
mutex is a lock around a piece (or multiple pieces) of data, and to which there is only one
key. When a thread wishes to access any this data it must lock this mutex, and release it
when complete. Any thread which requests this lock when another thread holds the mutex
key is said to be blocked, or in a blocking wait, and gives up the remainder of its time slice.
In this manner, threads can be made to share resources efficiently and safely.
Again, there are difficulties with this method of data protection. One must take great
care to unlock mutexes properly, or else deadlocks can occur where multiple threads are all
waiting for it to be released. Also, mutexes are best used around short operations, or else
other threads have to wait for long periods before they can access this data or code. Long
operations can be accommodated by either copying out the protected data and operating
on the copy (which is inefficient), or altering the priorities of different threads so that the
shorter operations get more opportunities to use the data before the longer operations can
lock it.
6.2 The Subject/Observer Design Pattern 68
An extension to these mutexes is the Multiple Read - Single Write (MRSW) pattern,
which allows multiple people to access the data for reading purposes, but only one access
for writing. If there is a thread writing to the data, no one can read, and this writing
thread cannot get writing access until all previous readers have released their rights. This
functionality is encapsulated in the Argo project software library as a ThreadSafeData
object, and this is the manner in which the data store object inside this replanner system
is protected.
In this replanner system, several threads are used in order to receive and operate on
vehicle and sensor data, as well as planning paths simultaneously. Data protection is needed
around several objects, most notably the main data store for the system, which is relied upon
by the planners, and constantly updated by new sensor information. The data protection
methods described in this section are thus extensively used by the replanner system.
6.2 The Subject/Observer Design Pattern
The Subject/Observer design pattern “define[s] a one-to-many dependency between objects
so that when one object changes state, all its dependents are notified and updated auto-
matically” [34]. The details of this pattern are described in [34], but are essentially as
follows:
A Subject is considered as a source of data, and is responsible for notifying its Observers
whenever this data is updated. Any number of observers can attach to a subject, and all
are notified independently. Each observer defines a notification function which is called by
the subject at the appropriate times.
The implementation of this pattern used in the Argo project provides a single process
system (that is, no inter-process communication, only intra-process) for these notifications
with one important implication. Whenever a subject notifies its observers, whatever code
is executed by the observer in its notification function is executed in the thread space of the
subject, not the observer. For example, the replanner system is a thread which observes
sensor data. When the sensor system notifies the replanner, this notification takes place
within the sensor system’s thread space, not the replanner’s. Thus, the replanner may still
be using the data store when it is notified, and the sensor system, not the replanner, will
block waiting for the data store to be released.
6.3 Incorporating Sensory Information 69
The ramifications of this present one of the most significant potential problems for
this replanner system. The replanner’s data store must be locked while planning is being
performed, or else the plans produced may be completely corrupted. While this data store
is locked, no sensor information notifications can be accepted, as they too need to lock
this data store in order to update it. Furthermore, as these notifications occur within the
sensor system’s thread space, depending on how it is implemented, it may be prevented
from performing any further useful work until the data store is unlocked again.
Avoiding this problem can be managed in several different ways. One option is to
extend the Subject/Observer pattern such that notifications spawn a separate thread in
whose space the the notification executes. This solution is potentially fraught with problems,
as the rapid creation and deletion of threads is handled quite differently between operating
systems.
Another better solution is to buffer the data to be sent in the subjects such that when
it is able to be sent, the entire buffer is notified upon (or if so desired, only the most recent
information). This solution is the most practical for this system, as in the real-world system,
the Subject/Observer pattern is replaced by some alternative means of communication
between computers, rather than threads in a single application on the one machine. In other
words, only the concept of the Subject/Observer remains, the implementation is performed
using some inter-computer communication method (such as TCP IP, or serial link), which is
explicitly provided with a threaded buffering mechanism. This method provides a solution
to the blocking problem implicit in a single machine Subject/Observer relationship, and is
thus the appropriate implementation for this replanner system.
6.3 Incorporating Sensory Information
Thus far in this thesis, grid-based cost map information has simply been assumed to be
available, and constantly updating as the vehicle navigates the world. The full details of
the process by which this occurs in the Argo project is beyond the scope of this thesis, and
furthermore with the planning framework designed herein, can take any form whatsoever.
The following subsections thus present only a brief description the method by which this
information is established in a grid and updates are sent to the vehicles in the Argo project,
as this has a significant effect on the operations of the replanner system.
6.3 Incorporating Sensory Information 70
6.3.1 Generating Cost Information from Laser Data
The Argo project’s real-time cost information is, (at present), generated solely from laser
data. This laser information is rendered into an occupancy grid, and heights, gradients, and
other information are extracted from the data. A complicated series of algorithms process
this data over a series of different grid cell sizes, and are able to pass this through a vehicle
model to develop a map of terrain traversability, specific to an Argo vehicle. As new laser
information is received, only the altered areas of this cost map are sent to the replanner
system, and despite the high frequency of this data, it is packed into a convenient format
that requires little bandwidth.
6.3.2 Expanding the Obstacles
One additional feature of the real-time cost information framework, is the ability to expand
the dimensions of obstacles in the environment. This was alluded to in Section 2.2.1 and
described as a C-Space transform. Such a transform allows the vehicle to be treated as
a point particle by the planning algorithm, which is an assumption made by virtually all
planners, including the A* algorithm.
This assumption allows the path plans to travel as close to the edge of an (expanded)
obstacle as possible, and still be safe, as the expansion is proportional to the vehicle dimen-
sions. The framework used in the Argo project allows for expansion by additional several
factors, including those discussed in Section 2.2.1.
6.3.3 Informing the Replanner
Having created this real-time cost information, it is sent to the replanner system using
the same concept as the Subject/Observer pattern. That is, every time this information
is updated, a notification (whether through a Subject/Observer framework, or some other
communication mechanism) is sent, along with the new data, to the replanner system.
As described above, this notification requires the replanner data store to be write-locked,
whereupon the new information can be incorporated into it. Depending on the structure
of the data store object, this incorporation can take many forms; for the grid-based imple-
mentation in this thesis, the old data is simply overwritten.
6.4 Rapid Path Optimisation 71
To avoid the problem whereby the data store is continually locked by either planner
(as one will always be running, save for short amounts of time inside the main replanner
loop), the local planner can perform a simple check to see whether a new plan is needed.
If neither the data store content, nor the vehicle position have changed since the last plan,
then the previously performed local plan was from and to the same positions, and through
the same data, as that about to be performed. This plan produced will be identical, and
is therefore unnecessary. By inserting this check, sufficient time is available for the sensor
information to be updated, and perform the path optimisation described below.
6.4 Rapid Path Optimisation
Path optimisation, such as that described in Section 4.3.6 can be used to alter the properties
of a path by whatever method desired. Since the replanner stores a copy of the vehicle’s
waypoint path, the data store costs of each waypoint in this path can be compared with the
new values in a sensor information update, should any of the grid locations in this update
correlate with the vehicle waypoint path locations. This comparison can then be used to
invalidate the path if an obstacle in the latest sensor data is seen to lie on this path. This
process thus allows the vehicle to react instantly to detected obstacles, as invalidating its
path will cause it to cease moving.
This process can also be extended in a manner similar to that presented in Section
4.3.6. Rather than merely searching for full obstacles and stopping the vehicle if they are
found, an optimisation step can be applied to alter waypoint path properties as soon as new
cost information is received. For example, this step could lower the desired vehicle speed set
by a waypoint according to the cost of the data in or around the waypoint or path segment.
A framework for this kind of path optimisation was already presented in Section 4.3.6,
but rather than being applied directly to the output of the A* algorithm, is here applied
when new sensor information arrives, in addition to whenever a new plan is created by
either the local or global planner.
This method has a key advantage over the first invalidation idea presented in this
section. As new sensor information is being received, the waypoint paths are constantly
being re-optimised, rather than simply nullified when obstacles are found upon them. Thus
6.5 C++ Replanner Implementation 72
rather than simply preventing collisions, this method also ensures the local path taken is
optimal both when the traversal cost rise or fall.
Figure 6.1 illustrates the same path optimiser concept as Section 4.3.6, but in the
context of the replanner system.
Figure 6.1: Class diagram for the PathOptimiser framework within the replanner system
6.5 C++ Replanner Implementation
Unlike the abstract A* framework, the replanner is a particular application for the Argo
vehicles used in this thesis. While certain sections of the implementation are left generic, to
allow for reuse in similar projects, the majority of the application is specific to this thesis.
For example, the replanner enforces the use of grid-based data and waypoint-based paths
(hence why implementations of these concepts were designed and presented in Chapter 4).
This section describes a C++ implementation of this replanner, and presents pseudo-code
illustrating its primary functionality.
Figure 6.2 presents the class diagram for the replanner implementation, illustrating the
notification methods by which cost and navigation information are propagated from other
6.5 C++ Replanner Implementation 73
systems, to the replanner. A pseudo-code representation of the complete replanner system
is presented in Figure 6.3, showing the main replanner loop, and the update methods for
cost and navigation information. All three of these functions run in separate threads, with
the update methods only being executed when notified.
Figure 6.2: Class diagram for the Replanner class.
6.5 C++ Replanner Implementation 74
Main Loopwhile( missionGoals �= ∅ )
get currentV ehiclePositionif( currentV ehiclePosition is within tolerance of currentMissionGoal )
remove currentMissionGoal from missionGoalsif( globalPath �= ∅ )
find globalPath from currentV ehiclePosition to currentMissionGoalif( unsuccessful )
return FAILUREelse
Optimise( globalPath )vehiclePath = globalPath
elseif( dataStore has changed, or currentV ehiclePosition has changed )
get predictedV ehiclePosition after local planning timefind localPath from closest point in globalPath after →
predictedV ehiclePositionif( unsuccessful )
clear vehiclePath, stopping vehicleglobalPath = ∅
elseOptimise( localPath )vehiclePath = localPath
Update(CostInformation)write-lock dataStoreOptimise( vehiclePath )update dataStore with CostInformationunlock dataStore
Update(NavigationInformation)set currentV ehiclePosition to NavigationInformation
Optimise(path)forall items in pathOptimiserList
apply pathOptimiser to path
Figure 6.3: Pseudo-code presentation of the of the complete replanner system. The mainand update functions run in separate threads, with the update methods only being executedwhen notified.
6.6 Results 75
6.6 Results
To demonstrate the applicability of this multi-threaded replanner implementation, a fur-
ther simulation environment was written. This simulator was specifically designed to exactly
mimic the type, frequency, and quantity of data from the vehicle navigation and cost infor-
mation systems. As such, these two systems are simulated as separate threads, hence the
manner in which they notify the replanner of new information is exactly equivalent to the
real system onboard a vehicle.
The sensor system is simulated much like the MatLab simulation of Section 5.5, in
that the cost information is read from an image. This image is mapped to the real-world
co-ordinates of where it was obtained, and can thus be linked to the navigation information
in order to notify upon a region around the vehicle.
The navigation system simulates a waypoint controller, and has the vehicle make bee-
lines for the next waypoint in its list. This controller is run as a synchronised thread,
meaning it executes new discrete motion steps at a specified frequency. This discrete model
has a multitude of limitations, yet is still sufficient to demonstrate the integrity of the
replanner system.
These two simulated systems are linked to the replanner system, and the frequencies
of their operation can be specified. Since the local planner makes no attempt to replan if
the cost information and vehicle position have not changed, these threads can run at their
maximum rate with no ill-effect upon the system.
To aid visualisation, the output paths and plans of the replanner system are written
to text files, and read by a continuously running MatLab script. This script then displays
the cost map, with the constantly updated vehicle path and plans upon it.
Figures 6.4 to 6.5 illustrate the operation of this replanner simulation over real-world
cost information obtained similarly to that in Figure 4.10. While these figures do not show
the speed of the system, the vehicle can be run at full speed, with the sensor range set to
be a 10m radius around the vehicle, and the sensor and navigation information being sent
far faster than in the real vehicle. Even at this high data rate, and thus high replanning
rate, the system performs as expected on a 3GHz Pentium IV. The real vehicle operates a
PC104 Pentium III chip, and yet is anticipated to perform equally well, especially as the
sensor and navigation systems are not run on the same computer.
6.6 Results 76
Figure 6.4: C++ replanner simulation, parts one and two.
Figure 6.4a (left) shows the initial global plan (red) in the absence of any cost infor-
mation. The local plan (blue) is also shown, extending as far along this global plan as the
sensor range allows (20m in this case), and taking more optimal paths through this local
region. The vehicle’s path thus far is shown in green.
Figure 6.4b (right) shows the global plan extending through a region of solid obstacles.
The local plan is seen to reach into this region, and is thus not tenable. At this point, the
vehicle is stopped, and a new global plan is requested, as seen in Figure 6.5a.
6.6 Results 77
Figure 6.5: C++ replanner simulation, parts three and four.
Figure 6.5a (left) shows the new global plan, which has now seen, and thus avoids, the
obstacle region that the global plan of Figure 6.4b went directly through.
Figure 6.5b (right) shows the vehicle having successfully reached its final goal, and the
overall path taken to reach it. This path avoids all the obstacles, and roughly follows the
original global path, save that on a local level it has optimised the route.
6.7 Summary 78
6.7 Summary
This chapter presented a C++ implementation of the replanner system first described in
Chapter 5. The system was extended to a multi-threaded architecture, and extended with
additional features to accommodate specific aspects of the Argo project and vehicles.
The results presented in Section 6.6 illustrate the suitability of such a system for au-
tonomous vehicle navigation. This simulation provides the replanner with real-world cost
data and navigation information in almost exactly the same manner as the real vehicle.
This fidelity ensures that the final transition to running the replanner as the highest level
vehicle controller should be a simple task.
Chapter 7 reiterates the cumulative results obtained in this thesis, and discusses the
main points of interest and difficulty. Aspects of the replanner system specific to running
it as the primary vehicle controller, such as that commands take a finite time to execute,
are described and their effect on the system evaluated.
Chapter 7
Conclusions and Future Work
This thesis has presented several important contributions to the field of autonomous vehicle
navigation, which are summarised in Section 7.1. Section 7.2 describes several improvements
to the work, which alleviate some of the assumptions and approximations made. Further
extensions and implementations are also described, which allow the work to be used in new
and different situations.
7.1 Conclusions
This thesis documented the design and implementation of an adaptive and extensible plan-
ning framework, and an efficient and logical replanner system.
The planning framework was shown to effectively abstract the internal pathfinding
algorithm from the user implementation, allowing for planning over any topology. Specifi-
cally implemented using the A* algorithm, the implementation was seen to be slower than
others in the literature, primarily because the design deliberately trades speed for general-
ity. Despite this, the ability to plan over multiple cost information layers in any topology,
more than compensates for this deficiency. Many opportunities for further optimisations
are available, some of which are discussed in Section 7.2, and these are likely to improve
the algorithm speed.
The replanner system was shown to be founded on a logical brute-force algorithm, and
simulation over real-world data proved effective. The replanner was found to be capable
7.1 Conclusions 80
of planning locally optimal paths successfully, even at sensory information rates far higher
than the actual vehicle produces. These results suggest that the final transition to vehicle
control will be relatively simple, and immediately effective.
The approach taken in this thesis has been to develop frameworks for algorithms, with
the implementations left up to the user. The specific implementations designed for this
thesis were thus chosen for simplicity and speed, and are not necessarily optimal. As a
result, one deficiency of this thesis is the lack of comparisons with other implementations
in the literature, particularly D*.
The D* algorithm is currently the most used replanner algorithm in mobile robotics,
and produces provably optimal plans. The replanner presented here also produces locally
optimal plans, but it is unknown which algorithm is faster. The replanner system presented
here is capable of using the D* algorithm internally, where its ability to efficiently replan a
path would be well used by both the local and global planners.
Another deficiency of the approach is the problem of moving obstacles. It is easy to
consider situations where the replanner system would sensibly avoid such obstacles, yet
pathological cases are also easily found. For example, a moving obstacle in front of the
vehicle, which remains at the same distance, would always be avoided in the same way.
This may or may not have a significant effect on the vehicle’s paths, but there are likely to
be other cases which do negatively affect the performance.
Several aspects of the use of this system for control of an autonomous vehicle were not
considered directly in this thesis. Of these, the fact that control actions take a finite time
to execute is likely to be the most significant. This means that despite producing plans
which are feasible and optimal, the vehicle’s momentum and other factors may contrive to
prevent the vehicle from accurately following the plans. Such an effect was described in
Section 4.4, whereby the vehicle could not follow the sharp corners in the path plans at its
maximum speed. Similar factors may thus affect the ability of the vehicle to change course
given a new plan, and have not yet been considered in this thesis.
As described above, the planning framework was shown to allow for user defined im-
plementations to plan over multiple layers of cost information, in any desired topology.
This ability is not provided directly by any available system, and is thus a novel and useful
contribution.
7.2 Future Work 81
The replanner system was shown to derive from a logical method, and a multi-threaded
C++ implementation of this was successfully demonstrated, planning through real-world
terrain while simultaneously optimising the path and avoiding obstacles. This implemen-
tation was built within the Argo project’s software framework, and is anticipated to be a
valuable contribution to the project.
The systems designed and implemented in this thesis are seen to be both effective and
efficient, and are thus important contributions to the field of autonomous vehicle navigation.
7.2 Future Work
The work undertaken in this thesis developed a practical set of software tools for pathfinding
and replanning on autonomous vehicles. These tools provide the means to perform a wide
range of tasks, but sacrifice efficiency for generality to achieve this. This section describes
firstly a series of possible optimisations and improvements to the planning framework, and
subsequently a number of additional areas of research using the replanning system.
Section 2.2.2 and [31] describe the Field D* algorithm, which incorporates an interpo-
lation step into the planning process as a means of alleviating the deficiencies of grid-based
worlds. This step alters the path plans from movements between the centres of neighbouring
cells, to movements between points on the edges of cells. The interpolation step determines
the appropriate point on each edge to move between, allowing for any direction of travel to
be accommodated. This step is equally applicable to the A* algorithm, and would remove
the problems due to the eight-way connectivity functions, reducing the curvature and length
of the path plans.
Section 4.3.3 described a quad-tree approach to storing data, which was seen to be
effective in [28] and [31]. This approach was not undertaken in this thesis due to time
constraints, but is a practical means of improving the system efficiency. [31] also extends
the Field D* algorithm’s interpolation step to apply to quad-tree representations, meaning
both this, and the previous improvement can be used concurrently.
Section 3.2.1 described the need for a hybrid data representation for the open and
closed sets of the A* algorithm. The use of these sets is also similar in algorithms such
as D*, meaning an optimised hybrid representation can be reusable throughout different
7.2 Future Work 82
pathfinding implementations. Future work in this area could thus implement a specific data
container which optimises all the operations required by the algorithms, rather than the
memory inefficient representation described in Section 4.3.1.
A final means of improving the A* implementation’s performance, is to consider the
use of compiler optimisations, such as those described in Section 4.4. Optimisations such
as the use of inline functions typically increase the amount of assembly code required, but
can result in faster implementations; again trading memory use for processing speed. Many
other compiler optimisations are also available; within the Microsoft Visual Studio compiler
for example, are options for favouring speed or code size, optimising float point use, and
favouring a specific processor type, amongst others. A brief test of these optimisations
found only a 2% speed increase, but a more thorough investigation may yield better results.
The replanner system incorporates the use of a very simplistic model to predict the
future position of the vehicle. Current work in the Argo project has developed a considerably
more accurate model, which could likely be used as a replacement. This would improve the
paths generated by placing the starting points of local plans closer to the actual vehicle
position.
The previous section described the need to test the system against the D* algorithm.
Further to this, the D* algorithm could also be written in the same style as the A* frame-
work, allowing it to be incorporated into the replanner system, rather than replacing it.
The D* algorithm is intended to run in a brute-force manner whenever cost information
changes, whereupon it only replans over the changed information, making this new plan very
efficient. D* is also capable of being interrupted with this new information, while already
planning. This feature would be well used in the replanner system to allow instantaneous
response to new information, rather than needing the invalidation step presented in Section
6.4.
The replanner system developed in this thesis was found to perform effectively at the
highest data rates possible, using a sensor range of 20m, and a grid resolution of 0.5m; ade-
quate for the Argo vehicles it is implemented on. Further research is necessary to determine
the limits of the system; at what point is the system incapable of effective replanning? Log-
ically, this question is dependent upon the speed of the specific planner implementation, the
type and fidelity of the environment topology, as well as the accuracy of the vehicle model
used in forward-predicting its motions. A possible extension to the system would allow for
7.2 Future Work 83
dynamic adaption of these factors, in order to guarantee successful planning.
Finally, Section 7.1 also described the likely problems the replanner system will suffer
when encountering moving obstacles. An investigation into these problems is a promising
area of research, as is attempting to track and avoid such obstacles. While the algorithm in
some situations may already avoid moving obstacles, it is likely to be insufficient for real-
world applications. By detecting and tracking such obstacles, their future motions can be
predicted using models of their behaviours. A more advanced planning system could likely
incorporate this information in a probabilistic manner, suggesting the most likely areas
occupied by the obstacles at a future time, and thus the best paths given this information.
This thesis has developed a practical framework for path planning, and an effective
system for use in real-time on autonomous vehicles. The opportunities for future work are
vast, and well supported by the framework and systems developed in this thesis.
Appendix A
Source Code and Documentation
The attached CD contains source code and documentation of the A* and replanner software
and applications created in this thesis. This software has a dependency on the proprietary
QLibs Library, which cannot be shown here for copyright reasons. As a result, this code will
not compile, but its structure is detailed in both this thesis and the included documentation,
to allow for use in other applications.
The directory structure of this CD is as follows:
Application Binaries - A folder containing Win32 executable files for the A*mission planner, and replanner applications.AStar Application - A folder containing a Win32 executable and sample cost
images for A* mission planning.Replanner Application - A folder containing a Win32 executable, MatLab
script, and sample cost images for the Replanner application.Application Source - A folder containing source code and documentation for the
A* mission planner, and the replanner application.AStar Source - A folder containing source code for the A* mission planner.Replanner Source - A folder containing source code for the replanner
application.LaTeX Document Source - The LaTeX source code of this document.Misc - Miscellaneous files, including a thesis poster and seminar presentation.
Figure A.1: Directory structure of the attached source code and documentation CD.
Source Code and Documentation 85
Source Code and Documentation CD
Bibliography
[1] Ontario Drive and Gear Ltd, “Argo vehicle website,” 2006, Available from http://www.argoatv.com/.
[2] Steven M. LaValle, Planning Algorithms, Cambridge University Press, 2006, Availablefrom http://planning.cs.uiuc.edu/.
[3] S. Russell and P. Norvig, Artifical Intelligence: A Modern Approach, Prentice Hall, 2edition, 2003.
[4] Dermot Madden, Declan Delaney, Seamus McLoone, and Tomas Ward, “Visibilitypath-finding in relation to hybrid strategy-based models in distributed interactive ap-plications,” in IEEE International Symposium on Distributed Simulation and Real-Time Applications, 2004.
[5] Yee Chia Hui, Edmond C. Prakash, and Narendra S. Chaudhari, “Game ai: Artificialintelligence for 3d path finding,” 2004.
[6] Jason M. O’Kane and Steven M. LaValle, “Sampling based methods for discrete plan-ning,” American Association for Artificial Intelligence, 2004.
[7] B. Bonet and H. Geffner, “Planning as heuristic search,” Artificial Intelligence, 2001.
[8] Anthony Stentz and Martial Hebert, “A complete navigation system for goal acquisitionin unknown environments,” in International Conference on Intelligent Robots andSystems, The Robotics Institute, Carnegie Mellon University, Pittsburgh, Pennsylvania15213, 1995.
[9] Alex Yahja, Sanjiv Singh, and Anthony Stentz, “Recent result in path planning for mo-bile robots operating in vast outdoor environments,” in Symposium on Image, Speech,Signal Processing and Robotics, The Robotics Institute, Carnegie Mellon University,Pittsburgh, Pennsylvania 15213, 1998.
[10] Neil C. Rowe, “Roads, rivers, and obstacles: Optimal two-dimensional path planningaround linear features for a mobile agent,” International Journal of Robotics Research,vol. 9, no. 6, pp. 67–74, 12 1990.
[11] Shahram Rezaei, Jose Guivant, and Eduardo M. Nebot, “Car-like robot path followingin large unstructured environments,” in International Conference on Intelligent Robotsand Systems, 2003.
BIBLIOGRAPHY 87
[12] Dai Feng, Sanjiv Singh, and Bruce H. Krogh, “Implementation of dynamic obstacleavoidance on the cmu navlab,” in IEEE International Conference on Systems Engi-neering, 8 1990.
[13] Jeff Witt, Vector Pursuit Path Tracking for Autonomous Ground Vehicles, Ph.D.thesis, University of Florida, 2000.
[14] Jeff Witt, Carl D. Crane, and David Armstrong, “Autonomous ground vehicle pathtracking,” Journal of Robotic Systems, vol. 21, no. 8, pp. 439–449, 7 2004.
[15] Amit J. Patel, “Amit’s a* pages,” 2006, Available from http://theory.stanford.edu/~amitp/GameProgramming/.
[16] Anthony Stentz, “Optimal and efficient path planning for unknown and dynamic envi-ronments,” Technical report CMU-RI-TR-93-20, Carnegie Mellon Robotics Institute,1993.
[17] Anthony Stentz, “Optimal and efficient path planning for partially-known environ-ments,” in IEEE International Conference on Robotics and Automation, The RoboticsInstitute, Carnegie Mellon University, Pittsburgh, Pennsylvania 15213, 1994, pp. 3310–3317.
[18] E. W. Dijkstra, “A note of two problems in connexion with graphs,” NumerischeMathematik, 1959.
[19] T. H. Cormen, C. E. Leiserson, R. L. Rivest, and C. Stein, Introduction to Algorithms,MIT Press, Cambridge, MA, 2 edition, 2001.
[20] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determina-tion of minimum cost paths,” IEEE Transactions on Systems Science and Cybernetics,vol. 2, pp. 100–107, 1968.
[21] P. E. Hart, N. J. Nilsson, and B. Raphael, “Correction to ”A Formal Basis for theHeuristic Determination of Minimum Cost Paths”,” SIGART Newsletter, vol. 37, pp.28–29, 1972.
[22] J. Pearl, Heuristics, Addison-Wesley, 1984.
[23] A. A. Stepanov V. J. Lumelsky, “Dynamic path planning for a mobile automaton withlimited information on the environment,” IEEE Transactions on Automatic Control,vol. 31, no. 11, 11 1986.
[24] Stanford University Artificial Intelligence Lab, “Stanford racing website,” 2006, Avail-able from http://www-cs.stanford.edu/group/roadrunner/.
[25] A. Pirzadeh and W. Snyder, “A unified solution to coverage and search in exploredand unexplored terrains using indirect control,” .
[26] R. E. Korf, “Real-time heuristic search: First results,” in Sixth National Conferenceon Artificial Intelligence, 7 1987.
BIBLIOGRAPHY 88
[27] Alex Zelinsky, “A mobile robot exploration algorithm,” IEEE Transactions on Roboticsand Automation, vol. 8, no. 6, 12 1992.
[28] Alex Yahja, Anthony Stentz, Sanjiv Singh, and Barry L. Brumitt, “Framed-quadtreepath planning for mobile robots operating in sparse environments,” in IEEE Confer-ence on Robotics and Automation, The Robotics Institute, Carnegie Mellon University,Pittsburgh, Pennsylvania 15213, 1998.
[29] Anthony Stentz, “The focussed d* algorithm for real-time replanning,” in InternationalJoint Conferences on Artificial Intelligence, The Robotics Institute, Carnegie MellonUniversity, Pittsburgh, Pennsylvania 15213, 1995.
[30] Sven Koenig and Maxim Likhachev, “Improved fast replanning for robot navigationin unknown terrain,” in International Conference on Robotics and Automation, 2002.
[31] Dave Ferguson and Anthony Stentz, “Using interpolation to improve path planning:The field d* algorithm,” Journal of Field Robotics, vol. 23, no. 2, pp. 79–101, 2 2006.
[32] Defense Advanced Research Projects Agency, “Grand challenge website,” 2006, Avail-able from http://www.darpa.mil/grandchallenge/index.asp.
[33] Carnegie Mellon University Robotics Institute, “Cmu red team website,” 2006, Avail-able from http://www.redteamracing.org/.
[34] Erich Gamma, Richard Helm, Ralph Johnson, and John Vlissides, Design Patterns,Addison Wesley Longman, Inc, 1998.