real-time path planning for autonomous vehicles · this thesis details the construction of a...

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

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.

“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

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

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

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

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

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

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].

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.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

[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.

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.

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

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.

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

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.

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.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.

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 .

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-

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

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

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.

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.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

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.

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

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

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

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,

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.

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.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

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.

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.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

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

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

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

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.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

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

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.

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

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.

real-time path planning for autonomous vehicles · this thesis details the construction of a...

Documents

using map decomposition to improve pathﬁnding€¦ ·...

autonomous vehicles, parking & the real estate sector

development and real-time dynamic positioning of an...

autonomous data warehousing in the real world: a case...

semi autonomous vehicle intelligence: real time target

benchmarks for pathﬁnding in 3d voxel spacebenchmarks for...

any-angle pathﬁnding for multiple agents based on sipp

real-time autonomous robot for object tracking using

real-time motion planning in autonomous vehicles

pathﬁnding algorithms in a unity 3d environment

real time road sign recognition system for autonomous car

primal: pathﬁnding via reinforcement and imitation multi...

real-world reinforcement learning for autonomous …...

camera-lidar sensor fusion in real time for autonomous

real-time motion planning for agile autonomous...

real time adaptive control of an autonomous underwater...

autonomous real-time surveillance system with ip cams

real-time camera-only 3d object detection for autonomous

real-time control for autonomous racing based on viability...

real-time indoor autonomous vehicle test environment .pdf