VISION-BASED MAP BUILDING AND TRAJECTORY PLANNING TO ENABLEAUTONOMOUS FLIGHT THROUGH URBAN ENVIRONMENTS

By

ADAM S. WATKINS

A DISSERTATION PRESENTED TO THE GRADUATE SCHOOLOF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT

OF THE REQUIREMENTS FOR THE DEGREE OFDOCTOR OF PHILOSOPHY

UNIVERSITY OF FLORIDA

2007

1

c© 2007 Adam S. Watkins

2

To my parents for their support, encouragement, and, most of all, lovefor a son who never calls nearly enough.

To my brother for always setting the bar so high that I have to stretchand for always reminding me that, someday, I need to graduate.

To Jessica for making the last long years in Gainesville fun, exciting,and sometimes even more stressful than they should have been.

To Allie for making sure that I get up every morningand put one foot in front of the other,

if only to go for a short walk.

3

ACKNOWLEDGMENTS

I would like to thank my advisor Dr. Rick Lind for his guidance, criticisms, and

motivation throughout my graduate school experience. I would like to express gratitude

to my committee members for their patience and insight during my doctoral studies. I

would also like to extend thanks to my lab fellow researchers in the Flight Controls Lab

including Mujahid Abdulrahim, Eric Branch, Ryan Causey, Daniel Grant, and Joe Kehoe.

Thanks to Mujahid, Eric, and Ryan for dealing with my nearly constant disruptions of the

workplace and generally sarcastic, downbeat attitude. Thanks go to Daniel “Tex” Grant

for handling my verbal abuse so well and teaching me how to shoot a gun. Thanks to Joe

Kehoe for his thousands of research related ideas, including the three that actually worked.

4

TABLE OF CONTENTS

page

ACKNOWLEDGMENTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

CHAPTER

1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.3 Historical Perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.4 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2 SCENE RECONSTRUCTION FROM MONOCULAR VISION . . . . . . . . . 24

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 242.2 Structure from Motion Overview . . . . . . . . . . . . . . . . . . . . . . . 24

2.2.1 Pinhole Camera Model . . . . . . . . . . . . . . . . . . . . . . . . . 252.2.2 Feature-Point Tracking . . . . . . . . . . . . . . . . . . . . . . . . . 262.2.3 Epipolar Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . 282.2.4 Euclidean Reconstruction . . . . . . . . . . . . . . . . . . . . . . . . 30

2.3 Calculating Structure from Motion . . . . . . . . . . . . . . . . . . . . . . 312.3.1 Eight-Point Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 312.3.2 Recursive Structure from Motion . . . . . . . . . . . . . . . . . . . . 32

2.4 Optical Flow State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . 342.5 Image Processing for Environment Mapping . . . . . . . . . . . . . . . . . 35

3 ENVIRONMENT REPRESENTATION . . . . . . . . . . . . . . . . . . . . . . 37

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373.2 Environment Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

3.2.1 Topological Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . 393.2.2 Occupancy Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . 393.2.3 Geometric Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

3.3 Dimensionality Reduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 403.3.1 Principal Components Analysis . . . . . . . . . . . . . . . . . . . . 413.3.2 Data Clustering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433.3.3 Plane Fitting Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 45

5

4 FILTERING FOR STATE ESTIMATION . . . . . . . . . . . . . . . . . . . . . 49

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494.2 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

4.2.1 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . 524.2.2 The Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . 534.2.3 The Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . 544.2.4 The Square-Root Unscented Kalman Filter . . . . . . . . . . . . . . 56

4.3 The Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 584.4 The Rao-Blackwellized Particle Filter . . . . . . . . . . . . . . . . . . . . . 60

5 SIMULTANEOUS LOCALIZATION AND MAP BUILDING . . . . . . . . . . . 62

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.2 Airborne SLAM Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

5.2.1 Vehicle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 665.2.2 Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

5.3 Filtering for SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 685.3.1 Rao-Blackwellized Particle Filter . . . . . . . . . . . . . . . . . . . . 685.3.2 Unscented Rao-Blackwellized Particle Filter . . . . . . . . . . . . . 715.3.3 Square-Root Unscented Rao-Blackweillized Particle Filter . . . . . . 73

5.4 Data Association . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 755.5 Map Management . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

6 TRAJECTORY PLANNING . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 786.2 Direct Trajectory Planning Methods . . . . . . . . . . . . . . . . . . . . . 80

6.2.1 Mixed-Integer Linear Programming . . . . . . . . . . . . . . . . . . 806.2.2 Maneuver Automaton . . . . . . . . . . . . . . . . . . . . . . . . . . 82

6.3 Decoupled Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . 846.4 Sampling-Based Path Planning . . . . . . . . . . . . . . . . . . . . . . . . 85

6.4.1 Expansive-Spaces Tree Planner . . . . . . . . . . . . . . . . . . . . . 876.4.2 Rapidly-Exploring Random Tree Planner . . . . . . . . . . . . . . . 88

6.5 The Dubins Paths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 906.5.1 Extension to Three Dimensions . . . . . . . . . . . . . . . . . . . . 936.5.2 Collision Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

6.5.2.1 Global path collision detection . . . . . . . . . . . . . . . . 946.5.2.2 Local path collision detection . . . . . . . . . . . . . . . . 96

6.6 Environment Coverage Criteria . . . . . . . . . . . . . . . . . . . . . . . . 976.7 Optimal Path Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1016.8 Trajectory Planning Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 102

7 DEMONSTRATIONS AND RESULTS . . . . . . . . . . . . . . . . . . . . . . . 105

7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

6

7.2 Plane Fitting Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1057.2.1 Effects of Feature Point Noise . . . . . . . . . . . . . . . . . . . . . 1067.2.2 Nonplanar Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . 108

7.3 SLAM Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1087.3.1 Simulation Environment . . . . . . . . . . . . . . . . . . . . . . . . 1097.3.2 Localization Result . . . . . . . . . . . . . . . . . . . . . . . . . . . 1117.3.3 State Estimation Error . . . . . . . . . . . . . . . . . . . . . . . . . 1137.3.4 Filter Consistency . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1137.3.5 Filter Efficiency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1167.3.6 Example SLAM Result . . . . . . . . . . . . . . . . . . . . . . . . . 117

7.4 Trajectory Planning Results . . . . . . . . . . . . . . . . . . . . . . . . . . 1177.4.1 Environment Coverage . . . . . . . . . . . . . . . . . . . . . . . . . 1177.4.2 Optimal Control Results . . . . . . . . . . . . . . . . . . . . . . . . 121

8 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122

8.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1228.2 Future Directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123

REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124

BIOGRAPHICAL SKETCH . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

7

LIST OF TABLES

Table page

7-1 Efficiency comparison of the SLAM filters. . . . . . . . . . . . . . . . . . . . . . 117

8

LIST OF FIGURES

Figure page

1-1 Map construction by an autonomous UAV . . . . . . . . . . . . . . . . . . . . . 13

1-2 Examples of unmanned vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

1-3 Examples of unmanned air vehicles . . . . . . . . . . . . . . . . . . . . . . . . . 15

1-4 Unmanned air vehicle mission profiles . . . . . . . . . . . . . . . . . . . . . . . . 16

2-1 Pinhole camera model and perspective projection of a feature point onto theimage plane. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2-2 Feature-point tracking result from an image sequence . . . . . . . . . . . . . . . 26

2-3 Epipolar geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3-1 Environment representations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

3-2 2D Prinicipal Components Analysis for a dataset with a linear trend. . . . . . . 41

3-3 3D Principal Components Analysis for a planar dataset. . . . . . . . . . . . . . 43

3-4 Centroid splitting for iterative k-means . . . . . . . . . . . . . . . . . . . . . . . 46

3-5 Dimensionality reduction of feature point data. . . . . . . . . . . . . . . . . . . 48

4-1 Recursive state estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

4-2 The Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5-1 Map building in the presence of noise . . . . . . . . . . . . . . . . . . . . . . . . 63

5-2 The SLAM estimation strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

5-3 Fixed-wing aircraft model definitions . . . . . . . . . . . . . . . . . . . . . . . . 66

6-1 An overview of the trajectory planning process . . . . . . . . . . . . . . . . . . . 79

6-2 An example Maneuver Automaton . . . . . . . . . . . . . . . . . . . . . . . . . 83

6-3 Probabilistic Roadmap planning in a two-dimensional environment . . . . . . . 86

6-4 Expansive-Spaces Tree planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

6-5 Rapidly-Exploring Tree planning . . . . . . . . . . . . . . . . . . . . . . . . . . 89

6-6 The coordinate system for calculating the paths in the Dubins set D. . . . . . . 91

6-7 The Dubins set of paths for two configurations. . . . . . . . . . . . . . . . . . . 93

6-8 Examples of 3D Dubins paths . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

9

6-9 Collision detection for the global path. . . . . . . . . . . . . . . . . . . . . . . . 95

6-10 Interior to boundary test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

6-11 Collision detection for the local path . . . . . . . . . . . . . . . . . . . . . . . . 98

6-12 Motion planning for coverage strategies . . . . . . . . . . . . . . . . . . . . . . . 99

6-13 Environmental coverage metric . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

7-1 Result of the plane fitting algorithm without image noise. . . . . . . . . . . . . . 105

7-2 Angle of incidence constraint. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106

7-3 Results of the plane fitting algorithm in the presence of image noise. . . . . . . . 107

7-4 Result of the plane fitting algorithm when nonplanar obstacles are included. . . 109

7-5 The vision-based flight simulation environment for testing the SLAM algorithm. 110

7-6 Example localization result for the airborne SLAM simulation . . . . . . . . . . 112

7-7 Average estimation error for the three SLAM filters and the nominal trajectoryfor 50 Monte Carlo runs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114

7-8 Filter consistency results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116

7-9 An example result for the complete airborne SLAM solution using the URBPF . 118

7-10 Example paths for environment coverage . . . . . . . . . . . . . . . . . . . . . . 119

7-11 Comparison of environment coverage results. . . . . . . . . . . . . . . . . . . . . 120

7-12 Resultant trajectories from the path tracking optimization. . . . . . . . . . . . . 121

10

Abstract of Dissertation Presented to the Graduate Schoolof the University of Florida in Partial Fulfillment of theRequirements for the Degree of Doctor of Philosophy

VISION-BASED MAP BUILDING AND TRAJECTORY PLANNING TO ENABLEAUTONOMOUS FLIGHT THROUGH URBAN ENVIRONMENTS

By

Adam S. Watkins

August 2007

Chair: Richard C. Lind, Jr.Major: Mechanical Engineering

The desire to use Unmanned Air Vehicles (UAVs) in a variety of complex missions has

motivated the need to increase the autonomous capabilities of these vehicles. This research

presents autonomous vision-based mapping and trajectory planning strategies for a UAV

navigating in an unknown urban environment.

It is assumed that the vehicle’s inertial position is unknown because GPS in

unavailable due to environmental occlusions or jamming by hostile military assets.

Therefore, the environment map is constructed from noisy sensor measurements taken

at uncertain vehicle locations. Under these restrictions, map construction becomes a

state estimation task known as the Simultaneous Localization and Mapping (SLAM)

problem. Solutions to the SLAM problem endeavor to estimate the state of a vehicle

relative to concurrently estimated environmental landmark locations. The presented work

focuses specifically on SLAM for aircraft, denoted as airborne SLAM, where the vehicle

is capable of six degree of freedom motion characterized by highly nonlinear equations

of motion. The airborne SLAM problem is solved with a variety of filters based on the

Rao-Blackwellized particle filter. Additionally, the environment is represented as a set

of geometric primitives that are fit to the three-dimensional points reconstructed from

gathered onboard imagery.

11

The second half of this research builds on the mapping solution by addressing the

problem of trajectory planning for optimal map construction. Optimality is defined in

terms of maximizing environment coverage in minimum time. The planning process

is decomposed into two phases of global navigation and local navigation. The global

navigation strategy plans a coarse, collision-free path through the environment to a

goal location that will take the vehicle to previously unexplored or incompletely viewed

territory. The local navigation strategy plans detailed, collision-free paths within the

currently sensed environment that maximize local coverage. The local path is converted

to a trajectory by incorporating vehicle dynamics with an optimal control scheme which

minimizes deviation from the path and final time.

Simulation results are presented for the mapping and trajectory planning solutions.

The SLAM solutions are investigated in terms of estimation performance, filter

consistency, and computational efficiency. The trajectory planning method is shown to

produce computationally efficient solutions that maximize environment coverage.

12

CHAPTER 1INTRODUCTION

1.1 Problem Statement

The problem addressed in this research is the mapping of unknown, urban

environments by an Unmanned Air Vehicle (UAV) equipped with a vision sensor. Map

building is a vital task for autonomous vehicle navigation in that it provides information

enabling vehicle localization, obstacle avoidance, and high-level mission planning. The

presented approach uses information gathered by an onboard camera to construct a

three-dimensional environment map while accounting for sensor noise and uncertainty in

vehicle state estimates. Additionally, motion planning strategies are developed to generate

trajectories which maximize environment coverage and avoid collisions with obstacles. The

combined map building and motion planning algorithms yield a solution for autonomous

flight through urban environments. The overall concept of this research is depicted in

Figure 1-1 where a UAV navigating at low altitude uses sensor information to construct a

three-dimensional map and, subsequently, plan a trajectory.

A B

Figure 1-1. Map construction by an autonomous UAV. A) Missions envisioned for UAVsinvolve safe navigation in cluttered urban environments. B) Navigation incluttered environments will require vehicles capable of autonomously buildinga map from sensor data and, subsequently, planning safe trajectories.

13

1.2 Motivation

In recent years, autonomous vehicles have become increasingly ubiquitous. Unmanned

vehicles are employed in a variety of situations where direct human involvement is

infeasible. Such situations include tasks occurring in environments that are hazardous to

humans, missions that humans are incapable of performing due to physical limitations,

or assignments considered too mundane or tedious to be reliably performed by a human.

Examples of autonomous vehicles currently in use, shown in Figure 1-2, include the

Packbot EOD which assists in Explosive Ordnance Disposal (EOD) [1], the Mars

Exploration Rovers (MER) Spirit and Opportunity used for planetary exploration [2],

and the Roomba vacuum robot [3].

Figure 1-2. Examples of unmanned vehicles. A) The JPL Mars Exploration Rover Spirit.B) The iRobot Packbot EOD. C) The iRobot Roomba.

The unmanned vehicles depicted in Figure 1-2 exemplify the current level of

autonomy reliably achieved by unmanned systems. The Packbot EOD requires constant

guidance from a human operator and does not perform any truly autonomous tasks.

The MER autonomously monitors its health in order to maintain safe operation and

tracks vehicle attitude to aid in safe navigation over uneven terrain. Preloaded and

communicated instructions, however, are executed by the MER after significant human

analysis and interaction. The Roomba displays a higher level of autonomy by sensing

14

its environment and updating the vehicle path accordingly. The floor cleaning task,

however, is simplistic when compared to envisioned unmanned vehicle missions such as

autonomous navigation of a car through traffic. The successful completion of complex

missions requires technological advances that increase the level of autonomy currently

displayed by unmanned vehicles.

A specific area of unmanned vehicles that have gained significant attention is

UAVs. UAVs are currently used in a variety of reconnaissance, surveillance, and combat

applications. Examples of UAVs, depicted in Figure 1-3, include the MQ-1 Predator that

has been used for reconnaissance missions and is capable of firing Hellfire missiles, the

RQ-5 Hunter designed for reconnaissance missions, and the Dragon Eye designated for

reconnaissance, surveillance,and target acquisition [4]. The current UAV technology has a

low level of autonomy requiring considerable human interaction to perform basic missions.

Highly trained operators are necessary to remotely operate each individual unmanned

aircraft.

A B C

Figure 1-3. Examples of unmanned air vehicles. A) The General Atomics AeronauticalSystems Inc. MQ-1 Predator. B) The Northrop Grumman RQ-5 Hunter. C)The AeroVironment Dragon Eye.

Future missions envisioned for unmanned aircraft require a greater level of autonomy

than is currently available in existing systems. Common autonomous behaviors exhibited

by UAVs include waypoint navigation [5] and simple obstacle avoidance [6]. Such

behaviors are well-suited for relatively benign missions that occur at high altitudes and

require little reaction to the surrounding environment as shown in Figure 1-4A. Advances

15

in microprocessors, battery power, and sensor miniaturization, however, have produced

smaller UAVs capable of flight through more complex environments. The ability of smaller

UAVs to aggressively maneuver through cluttered environments indicates that they are

ideal platforms for completing low-altitude missions, as depicted in Figure 1-4B, that

occur outside of an operator’s line-of-sight. As vehicle missions require autonomous flight

through increasingly elaborate, cluttered environments, a higher degree of environmental

awareness is required. Therefore, two critical requirements for autonomous unmanned

vehicles are the ability to construct a spatial representation of the sensed environment

and, subsequently, plan safe trajectories through the vehicle’s surroundings.

A B

Figure 1-4. Unmanned air vehicle mission profiles. A) Current missions occur at highaltitude, therefore, not requiring rapid path planning and environmentalawareness. B) Future missions will be low altitude assignments necessitatingquick reaction to environmental obstacles.

This dissertation concentrates on vision-based environmental sensing. Computer

vision has been identified as an attractive environmental sensor for small UAVs due to the

large amount of information that can be gathered from a single light-weight, low-power

camera. The sensor selection is a result of the decrease in vehicle size which severely

constrains the available payload both in terms of size and weight along with power

consumption. Therefore, autonomous behaviours for UAVs must be designed within the

framework of vision-based environmental sensing. In particular, two critical capabilities

16

are investigated for creating a truly autonomous UAV: vision-based map building and

computationally-efficient trajectory planning.

The first capability is a vision-based map building task in which the vehicle gathers

information concerning its surroundings with an onboard camera and constructs a spatial

representation of the environment. For the past two decades, robotic mapping has been

an actively researched topic because it is regarded as the first step toward absolute

autonomy [7]. Robotic mapping allows the vehicle to localize itself with respect to the

constructed map for navigating in GPS-denied environments. This capability is critical for

military operations where GPS can be jammed or cluttered environments preclude the use

of GPS due to occlusion.

Additionally, robotic mapping is vitally important to safe, time-efficient navigation

through unknown environments. Finding the shortest path to a goal location using purely

reactive control methods, where the robot selects control actions based only on the current

sensor measurements, is a trial-and-error process in unknown environments with no

provably optimal solution. By generating an obstacle map, a robot can retain knowledge

of its surroundings and discriminate between obstructed and unobstructed paths. As

knowledge of the environment grows, the task of traversing to a goal location can be

accomplished in a time-efficient manner with the planning of complete, obstacle-free paths.

The second capability developed in this dissertation is a computationally efficient

trajectory planning routine which maximizes environment coverage to minimize the

time to construct a complete environment map. In implementation, the method must

be computationally efficient since the planning process occurs incrementally as new

information is gathered. The rapidity of the planning process allows for path updates as

knowledge of the environment increases which is critical for safe navigation in unknown

environments. Additionally, vehicle dynamics are incorporated into the planning process

in order to generate trajectories. This task is vital for aircraft which may not be capable

of tracking every kinematically feasible path. Most ground vehicles are able to stop their

17

forward motion and reverse direction which enables the tracking of any feasible path;

however, aircraft motion is significantly more complex and dynamics must be considered

to ensure safe navigation.

This dissertation is motivated by the need to increase the level of autonomy present

in UAV technology. To obtain this goal, a map building architecture is developed which

constructs an environment representation from noisy sensor measurements and plans safe

trajectories through the environment that ensure map completeness.

1.3 Historical Perspective

The two main topics discussed in this dissertation, map building and trajectory

planning for autonomous vehicles, are rich fields of study for which much research has

been conducted. The presented work is an extension of previous research efforts which

focuses on six degree of freedom vehicle motion, computational efficiency, and vision-based

sensing.

The robotic map building task described in this dissertation is an extension of

solutions to the well-known Simultaneous Localization and Mapping (SLAM) problem.

The SLAM problem was first introduced in the seminal papers by Smith, Self, and

Cheeseman [8, 9] which described a methodology for generating stochastic obstacle maps

from a series of uncertain spatial relationships. In regards to unmanned vehicles, this work

describes the SLAM problem: the construction of an environment map from noisy sensor

measurements taken from uncertain vehicle positions while concurrently localizing the

vehicle with respect to the learned map. The solution to the SLAM problem presented by

Smith, et al. has since been extended to produce SLAM algorithms for numerous sensors,

vehicles, and environment types. This dissertation focuses on SLAM for vision-equipped

aircraft navigating in highly-structured, urban environments.

Vision-based SLAM refers to instances when vision is the only sensor available for

measuring the environment. Historically, solutions to the SLAM problem have focused

on vehicles equipped with sensors such as laser range finders and sonar [10, 11]. The

18

dense depth information available from such sensors is not equivalent to the sparse depth

information afforded a vehicle with a vision sensor. The use of vision in SLAM typically

involves the identification and differentiation of landmarks, but relies on other sensors

to measure the structure of the scene [12, 13]. In an example of purely vision-based

SLAM, Davison uses vision to detect locally planar features which are incorporated as

landmarks into the SLAM framework [14–16]. The implementation emphasizes accurately

estimating camera ego-motion and is not concerned with generating an environment

model. Consequently, the constructed map is a sparse approximation of the environment.

The approach in this dissertation shifts the focus from accurate ego-motion estimation

to building a complete map of the environment for the purpose of efficiently planning

collision-free vehicle motions.

Though the SLAM problem has been a highly researched topic in the ground vehicle

community for the past two decades, very little research has been performed for aircraft.

A few ground vehicle implementations have considered undulating terrain which requires

the assumption of nonplanar motion [17]. Kim and Sukkarieh implemented SLAM on

a UAV equipped with a vision sensor that locates artificial landmarks of known size

[18, 19]. A bearings-only SLAM approach for a Micro Air Vehicle (MAV) equipped with

a camera and an Inertial Measurement Unit (IMU) operating in a forest environment was

introduced by Langelaan [20–22]. Current approaches to airborne SLAM focus primarily

on augmenting inertial navigation systems (INS) to reduce drift inherent in the calculated

state estimates. These approaches do not address the difficulties associated with building a

complete environment map as is addressed in this dissertation.

Solutions to the SLAM problem require repeated observations of environmental

landmarks in order to build a stochastic obstacle map. Therefore, information regarding

each landmark must be stored and observations of a landmark must be correlated with

past observations. The correlation task is known as data association and incorrect

associations often lead to catastrophic failures of the SLAM estimation process. For these

19

reasons, map representation is a vital aspect of any SLAM algorithm. Traditionally, SLAM

algorithms define landmarks as three-dimensional points of interest in the environment

[10]. The practice of using point landmarks often yields environment models containing

millions of features [23]. Such environment models of large size are infeasible due to

the data storage requirements and computational power required to incorporate large

numbers of landmarks into the SLAM calculation. Additionally, the data association

problem becomes intractable because the relative measurements to landmarks cannot

be correlated as a result of the large number of possible associations combined with

uncertainty in vehicle location. Several SLAM formulations have constructed geometric

environment maps for highly structured environment maps by fitting geometric primitives

to the gathered pointwise data [24, 25]. The benefits of geometric primitives include the

reduction of data required to store an environment map and landmarks that are more

easily distinguished because they contain structural information. This dissertation employs

a similar dimensionality reduction step in order to construct a complete environment map

consisting of planar features.

In order to autonomously and rapidly construct an environment map, this dissertation

presents a trajectory planning algorithm. Ideally, the trajectory planning problem

would be posed as an optimal control problem in order to find a globally optimal

solution. An optimal solution, however, is computationally intractable because the

problem is non-convex and occurs in a high-dimensional space. For this reason, several

approximations to the full optimal control solution have been proposed which included

Mixed Integer Linear Programming (MILP) and Maneuver Automatons (MAs).

The MILP solution poses the full nonlinear optimization as a linear optimization

[90, 91]. Therefore, the solution is only capable of handling a linearized dynamic aircraft

model. The linearization restriction severely limits the efficacy of the MILP solution due

to the reduction of available maneuvers. A linear model cannot properly characterize

the aggressive maneuvers necessary to navigate in a cluttered environment. Additionally,

20

the MILP solution cannot prove optimality because the problem is non-convex and

the optimization must be implemented in a Receding Horizon (RH) format to be

computationally efficient.

The MA is a method which employs a discrete representation of the vehicle dynamics

in order to plan trajectories [35–38]. The dynamics of the vehicle are discretized into a set

of trim trajectories, or steady-state motions, and maneuvers that connect the trim states.

The MA planning process is decomposed into a combinatorial problem which searches for

the best maneuver sequence and an optimization that finds the optimal time durations

for the trim trajectories. Unlike, MILP the discretized dynamics are based on a nonlinear

model and can incorporate aggressive maneuvers. The MA solution, however, can become

computationally intractable when solving for large trajectories requiring a long maneuver

sequence. This issue is exacerbated if the discretization is not restricted to a small set

of vehicle motions or if obstacles are considered. Therefore, the MA is combined with

sampling-based planning methods to produce a solution in which optimality cannot be

guaranteed.

This dissertation employs a decoupled trajectory-planning strategy [40, 41] in which

a feasible collision-free path is planned and optimal control is employed to track the path

in minimal time. The path is subject to kinematic constraints and incorporates metrics

for selecting paths that provide maximum coverage with respect to other paths generated

during the planning process. The decoupled approach allows for the use of the complete

nonlinear aircraft model; however, optimality in terms of coverage is not guaranteed since

the coverage criteria is only included in the path planning task. The path to be tracked is

generated with a sampling-based planning strategy [26].

Sampling-based algorithms were introduced by a methodology called Probabilistic

Roadmaps (PRMs) [27]. PRMs construct a graph that captures the connectivity of the

configuration space of a robot by randomly sampling configurations and connecting

them to the existing map with kinematically feasible paths. Once a PRM is constructed

21

for a given environment, it can be repeatedly used to find paths from a given initial

configuration to a desired goal configuration. Navigation in unknown environments,

however, does not require full exploration of the configuration space since new sensor

information must be incrementally incorporated into the planning process. Therefore,

extensions of PRMs such as the Expansive-Spaces Trees (ESTs) [28–30] and the

Rapidly-Exploring Random Trees (RRTs) [31, 32] planners are employed. The EST and

RRT planning strategies are more efficient since they are designed to find feasible paths

between two specific configurations, rather than provide a general planning solution like

the PRM. This dissertation employs a variant of the RRT to efficiently plan collision-free

paths through the environment.

An abundance of research has been performed in the areas of SLAM and trajectory

planning. This dissertation extends previous research efforts in both fields to provide a

methodology for autonomous map building in highly-structured environments.

1.4 Contributions

The main contribution of this work is a complete solution for autonomous map

building by a UAV in an unknown, urban environment. The solution incrementally

constructs a map from noisy, relative sensor measurements without knowledge of the

vehicle’s location. The map is used as feedback to generate collision-free trajectories that

maximize environment coverage.

In the process of developing the main contribution of this work, the following

contributions were made:

• A plane fitting algorithm is developed to reduce the dimensionality of gatheredthree-dimensional, pointwise sensor data.

• A method for incorporating planar features into the SLAM algorithm is developed.

• A new SLAM filter based on the Rao-Blackwellized particle filter and the Square-Root Unscented Kalman filter is developed.

• Three SLAM filters are investigated in relation to the airborne SLAM problem.

22

• A method for for rapid path planning in an unknown environment based on local andglobal navigation tasks is developed.

• An environment coverage criteria is developed which does not require discretization ofthe environment.

• An optimization problem is posed for tracking the planned path in minimal time.

23

CHAPTER 2SCENE RECONSTRUCTION FROM MONOCULAR VISION

2.1 Introduction

Computer vision is an attractive sensor option for autonomous vehicle navigation

tasks due to the wealth of information that can be inferred from digital imagery [42]. In

addition, the low-cost, small payload requirements, and passive nature of vision has made

it the primary sensor for many small Unmanned Air Vehicles (UAVs) [4]. This study

assumes that computer vision is the only available onboard sensor capable of measuring

the surrounding environment.

Mapping an unknown environment with a moving, monocular camera requires

that three-dimensional information be inferred from a sequence of two-dimensional

images. This process of three-dimensional reconstruction from imagery is an actively

researched area in the image processing community commonly referred to as Structure

from Motion (SFM) [43, 44]. This chapter presents an overview of SFM in Section 2.2.

Specific solutions to the SFM problem are then discussed in Sections 2.3.1 and 2.3.2.

Section 2.4 describes a vehicle state estimation task similar to SFM which calculates

vehicle velocity states. Subsequently, Chapters 3 and 5 describe how the vision-based state

estimates and environmental structure information is employed in the robotic mapping

mission.

2.2 Structure from Motion Overview

The class of algorithms known as SFM endeavor to reconstruct the pose of a moving

camera as well as the three-dimensional structure of its environment. Calculating the

motion of the camera typically begins with an operation known as feature-point tracking,

as described in Section 2.2.2, which calculates correspondences between points of interest

in successive images. Section 2.2.3 demonstrates that by exploiting epipolar geometry

between correspondence pairs the relative rotation and translation between the two

camera views can be estimated. The calculated motion of the camera can then be used

24

to triangulate the three-dimensional position of the tracked feature points as shown in

Section 2.2.4.

2.2.1 Pinhole Camera Model

This chapter derives image processing algorithms which assume a pinhole camera

model. The pinhole camera is an idealized model of camera function in which all light

converges on an infinitesimally small hole that represents a camera lens with zero aperture.

In the frontal pinhole imaging model, as shown in Figure 2-1, the two-dimensional

projection f = [µ, ν]T of a three-dimensional point ρ = [η1, η2, η3]T is the intersection

of the ray emanating from the camera optical center ending at point ρ and the image

plane. The image plane is located at a distance f , called the focal length. The camera

coordinate frame C is located at the camera optical center with coordinates c1 and c2

oriented parallel to the image plane in the negative vertical and horizontal directions,

respectively. The coordinate c3 is oriented perpendicular to the image plane and denotes

the camera axis.

Figure 2-1. Pinhole camera model and perspective projection of a feature point onto theimage plane.

For the pinhole camera model, perspective projection of a three-dimensional point can

be written in terms of the camera focal length

µ

ν

=

f

η3

η1

η2

(2–1)

25

Equation 2–1 shows that only the depth of the three-dimensional feature point is

required to resolve the location of the point from the image projection.

2.2.2 Feature-Point Tracking

Feature-point tracking is a well-established image processing operation and is

commonly a prerequisite for solving the SFM problem. Essentially, a feature-point

tracking algorithm tracks neighborhoods of pixels, referred to as feature points, through

a sequence of images. Feature points are defined as any distinguishable image region that

can be reliably located within an image which are commonly corners and edges of objects.

An example of tracked feature points are shown in Figure 2-2.

A B

Figure 2-2. Feature-point tracking result from an image sequence. A) Frame 1. B) Frame10.

The Lucas-Kanade feature-point tracker [45] is a standard algorithm for small baseline

camera motion. The algorithm solves for the displacement of all feature points in an image

where the displacement of a feature point is measured in the horizontal and vertical image

coordinates denoted by µ and ν, respectively. For a series of two images, the displacement

of a feature point, d = [dµ, dν ]T , can be described as the translation of a group of image

pixels as follows

I1(f)|W = I2(f + d)|W (2–2)

26

where I1 and I2 are the image intensity values for two images taken from infinitesimally

close vantage points, W is a window that defines the neighborhood of pixels that comprise

the feature point, and f = [µ, ν]T is the center of the feature window.

The solution to Equation 2–2 can be formulated as a least-squares minimization of

the change in image intensity values between feature point windows in two consecutive

images as given by

d = arg mind

∑W

[I1(f)− I2(f + d)

]2

(2–3)

The first-order Taylor series expansion of I2(f + d) about f is

I2(f + d) = I2(f) +

[δI2δµ

δI2δν

]

dµ

dν

(2–4)

The first-order approximation of the Taylor series can be substituted into Equation 2–3 to

produce

d = arg mind

∑W

I1(f)− I2(f)−

[δI2δµ

δI2δν

]

dµ

dν

2

(2–5)

By differentiating the sum in Equation 2–5 with respect to feature point displacement the

solution to the linear least-squares estimate of feature point displacement can be written

as follows

∑W

[δI2δµ

δI2δν

]T

I1(f)− I2(f)−

[δI2δµ

δI2δν

]

dµ

dν

= 0 (2–6)

which can be rewritten in the form

d = −G−1b (2–7)

where G is a matrix encoding the two-dimensional gradient information of the second

image in the feature point window

G =

∑W

(δI2δµ

)2 ∑W

δI2δµ

δI2δν

∑W

δI2δµ

δI2δν

∑W

(δI2δν

)2

(2–8)

27

and b is a matrix representing the difference between the two images

b =

∑W

δI2δµ

(I1(f)− I2(f))

∑W

δI2δν

(I1(f)− I2(f))

(2–9)

This form of the Lucas-Kanade feature tracker suffers from two weakness. The first

is its inability to track features for large camera motions. This is a critical problem for

image sequences taken from vehicles with fast dynamics in comparison to the camera

frame rate. The second weakness is that each calculated feature location resides at discrete

pixel values. The discretization of the feature locations makes the tracking operation very

susceptible to noise.

Two alterations to the above algorithm can be implemented to overcome the

aforementioned issues. First, a multiresolution tracking step is added to aid in the

tracking of larger feature point displacements. Second, an iterative step is added to

calculate the location of the feature points to sub-pixel accuracy. These modifications

significantly increase the performance of the algorithm for images streams indicative of

vehicle navigation [46].

2.2.3 Epipolar Geometry

The most basic form of SFM calculates camera motion between two sequential image

frames and concatenates the solution with previous motion estimates. This calculation

is commonly accomplished by exploiting the epipolar constraint, depicted in Figure 2-3,

which is a geometric constraint relating camera motion to the image projections of a

tracked feature point.

Epipolar geometry dictates that there is a plane which is defined by a three

dimensional feature point ρ and two camera coordinate frames C1 and C2 from which

the point is visible. The epipolar plane intersects both image planes at the epipolar lines

l1 and l2 which contain the image projections f1 and f2 of the feature point ρ and the

epipoles e1 and e2.

28

Figure 2-3. Epipolar geometry. The epipolar constraint describes the geometricrelationship between camera motion and the projection of a feature point ontothe image plane.

The epipolar constraint relates the image projections, f1 and f2, of the

three-dimensional point, ρ, to the relative position, T, and orientation, R, of the two

camera locations. Since these three vectors are coplanar their scalar triple product is zero

as follows

fT2 (T×Rf1) = 0 (2–10)

The relative position and orientation of the camera are typically encoded as a single

entity known as the essential matrix, E ∈ R3×3.

E.= T×R (2–11)

The core calculation of the SFM algorithm is estimation of the essential matrix using

a set of tracked feature points and the epipolar constraint given by Equation 2–10. A

method for esimating the essential matrix is described in Section 2.3.1. Once the essential

matrix is estimated, it can be decomposed into the relative rotation and translation of the

camera. The relative translation, however, can only be estimated to a scale-factor since

Equation 2–10 is homogeneous with respect to the essential matrix (Equation 2–11) and is

29

not modified by multiplying the essential matrix by a nonzero constant. Estimation to a

scale-factor is an inherent ambiguity in the SFM problem stemming from the inability to

distinguish between the motion through one environment and twice the motion through a

scene twice as large, but two times further away.

2.2.4 Euclidean Reconstruction

Once camera motion is known, the final step in the SFM algorithm involves

estimating the three-dimensional location of the tracked two-dimensional feature

points. The calculation of pointwise scene structure is accomplished by triangulating

the correlated feature points. Recall that the three-dimensional location of a feature point

is denoted ρ = [η1, η2, η3]T . Therefore, the triangulation step can be formulated by relating

the unknown depths of the ith feature point, ηi3,m and ηi

3,n, measured with respect to two

different camera poses, Cm and Cn, yielding

ηi3,nf

in = ηi

3,mRf im + γT (2–12)

where γ is the unknown translational scale-factor and f im denotes the ith feature point

projected onto the image plane of the mth camera pose.

Since ηi3,m and ηi

3,n represent the same depth measured with respect to different

camera frames Equation 2–12 can be rewritten in terms of a single unknown depth as

follows

ηi3,mf i

n ×Rf im + γf i

n ×T = 0 (2–13)

Equation 2–13 can be solved for j correspondence pairs simultaneously by expressing

the problem in the form

Mη = 0 (2–14)

where η is a vector of the unknown feature point depths and unknown translational scale

factor

η = [η13,m, η2

3,m, . . . , ηj3,m, γ]T (2–15)

30

and M is the matrix

M.=

f1n ×Rf1

m 0 0 0 f1n ×T

0 f2n ×Rf2

m 0 0 f2n ×T

0 0. . . 0

...

0 0 0 f jn ×Rf j

m f jn ×T

(2–16)

The solution of Equation 2–14 provides the depth of the tracked feature points.

Therefore, this section has outlined the solution to the SFM problem from two-dimensional

image data to estimated camera motion and three-dimensional Euclidean scene structure.

2.3 Calculating Structure from Motion

This section presents two methods for calculating SFM. The first method is the

eight-point algorithm which requires only two camera images and a minimum of eight

tracked feature points. The second method is a recursive algorithm which employs a

Kalman filter in order to incorporate vehicle dynamics into the motion estimation process.

2.3.1 Eight-Point Algorithm

The eight-point algorithm introduced by Longuet-Higgins [47] calculates the rotation

and translation between two images given a minimum set of eight tracked feature points.

Larger numbers of tracked feature points can be used in the calculation by solving a linear

least-squares minimization problem. The inclusion of additional tracked points has been

shown to improve the results of the algorithm.

The eight-point algorithm exploits the epipolar constraint by solving a linear system

of equations in the form

Ae = 0 (2–17)

where e ∈ R9×1 is a stacked version of the essential matrix E and A ∈ Rj×9 is constructed

from the two-dimensional correspondence pairs. Each row of A is constructed by

calculating the Kronecker product of the ith feature point correspondence. The ith

feature point correspondence is two feature points correlated between two images denoted

31

by f im = [µi

m, νim, 1]T and f i

n = [µin, νi

n, 1]T . The Kronecker product of the correspondence

pair is equal to the ith row of A as follows

A(i, :) = [µimµi

n, µimνi

n, µim, νi

mµin, νi

mνin, νi

m, µin, νi

n, 1] (2–18)

The essential matrix can be recovered from A by calculating its singular value

decomposition (SVD)

A = UAΣAVTA (2–19)

The ninth column of VA can be unstacked to recover the essential matrix. Once the

essential matrix is found, camera rotation and translation can be extracted as seen by

Equation 2–11. It should be noted that this presentation of the eight-point algorithm is

highly sensitive to noise and requires a normalizing step as presented by Hartley [48].

2.3.2 Recursive Structure from Motion

The estimates of camera egomotion produced by two-frame SFM algorithms, such as

the eight-point algorithm, are subject to systematic, correlated errors since the current

calculation is appended to past reconstructions. Several batch processing algorithms have

been presented to eliminate these additive errors [49]. These algorithms, however, are not

suited for trajectory generation because they incur a delay from the data gathering process

and cannot be implemented in real-time. For causal, real-time SFM a Kalman filter can be

employed to recursively calculate structure and motion estimates [50].

The standard recursive SFM algorithms gradually refine SFM state estimates by

incorporating new data [51]. Alternatively, batch recursive algorithms create a series

of intermediate reconstructions which are fused into a final reconstruction [52]. Both

recursive techniques show improved results over nonrecursive techniques, however, no

single algorithm works well in all situations [44].

The problems specific to SFM reconstruction for a UAV are identified by Prazenica

et al. [46]. These issues include the loss of feature tracking due to aggressive vehicle

motion, unreliable reconstructions due to small baselines, and the well-known scale-factor

32

ambiguity associated with SFM. The SFM approach proposed by Prazenica mitigates

errors inherent in SFM by incorporating vehicle dynamics into the Kalman filter. This

approach adds robustness to the SFM calculation by producing state estimates even

when the SFM calculation fails. The scale-factor can be resolved by a gradual refinement

of estimates made by incorporating information from the physical model of the vehicle

[53, 54].

The Kalman filtering approach defines both a state transition model g(·) and a

measurement model h(·) written as

xk = f(xk−1,uk−1, κ) + qk−1, qk−1 ∼ N (0,Σq)

yk = h(xk, κ) + rk, rk ∼ N (0,Σr)(2–20)

where x denotes the complete state vector including vehicle states and three-dimensional

feature point locations. The vector y denotes the projection of the feature points onto

the image plane, u represents the control inputs to the vehicle equations of motion, κ

is the camera pose relative to the body-fixed coordinate system, and the additive noise

parameters q and r are zero-mean Gaussian random vectors with covariances Σq and

Σr, respectively. The notation a ∼ p(b) denotes that random variable a is drawn from a

probability distribution defined by p(b).

Therefore, f(·) is a nonlinear function that updates the vehicle states and estimated

feature point locations according to the previous system state and vehicle control input.

The nonlinear function h(·) predicts the projection of the three-dimensional feature points

onto the image plane given the estimated inertial feature point locations and the camera

pose. Consequently, the measurement model is the epipolar constraint (Section 2.2.3).

The filtering approach solves the SFM problem by estimating the state of the model

given by Equation 2–20. The estimation process is computed with an Extended Kalman

Filter (EKF). The EKF first predicts the state and covariance of the system according to

xk|k−1 = f(xk−1|k−1,uk−1, κ) (2–21)

33

Pk|k−1 = FkPk−1|k−1FTk + Σq (2–22)

where xk|k−1 denotes a state estimate at time k given measurements up to time k − 1,

Pk|k−1 is the covariance matrix of the state without a measurement update, and Fk is the

linearized form of the state transition function g(·).Next the optimal Kalman gain K is calculated according to

Sk = HkPk|k−1HTk + Σr (2–23)

Kk = Pk|k−1HTk S−1

k (2–24)

where Hk is the linearized form of the measurement model h(·).The final step in the filtering process is to update the state and covariance of the

system by incorporating the sensor measurement as follows

xk|k = xk|k−1 + Kk(yk − h(xk|k−1, κ)) (2–25)

Γk = I−KkHk (2–26)

Pk|k = ΓkPk|k−1ΓTk + KkΣrK

Tk (2–27)

The updated state estimate xk|k gives both structure and motion for the current time

step.

2.4 Optical Flow State Estimation

Several research efforts have pursed optical flow state estimation for UAVs [55–57].

Optical flow is similar to feature tracking with the distinction that displacement is

calculated at a fixed location as opposed to tracking a specific image feature through an

image stream. Additionally, optical flow refers to feature point velocity as opposed to

displacement, but for a given fixed frame rate the difference in calculation is trivial.

For optical flow state estimation, the focal plane velocity of the feature point,

approximated by the optical flow, can be obtained by differentiating Equation 2–1.

The result is an equation for the focal plane velocity of a feature point in terms of its

34

three-dimensional location and relative velocity with respect to the camera as follows

µ

ν

=

f

η23

η3 0 −η1

0 η3 −η2

η1

η2

η3

(2–28)

The optical flow of the feature points is a known quantity determined in the feature

tracking algorithm that is an integral part of the SFM calculation. The equations of

motion of an aircraft can be coupled with Equation 2–28 in order to express optical flow in

terms of aircraft states, as given by

µ

ν

= f

µη3

0 −1η3

ν (1 + µ2) µν

νη3

1η3

0 −µ µν (1 + ν2)

u

v

w

p

q

r

(2–29)

where {u, v, w} are aircraft linear velocities, {p, q, r} are aircraft angular velocities, and η3

is the unknown depth of the feature point.

Various solutions to Equation 2–29 have been proposed using optimization routines.

The resulting solutions have been shown to produce accurate estimates of aircraft linear

and angular velocities though the total velocity of the aircraft must be known to account

for the scale ambiguity that also affects SFM.

2.5 Image Processing for Environment Mapping

This chapter has presented several key image processing results suitable for

aiding UAV navigation. The described SFM algorithms are suitable algorithms for

providing information regarding the three-dimensional structure of the environment.

This information will be used in Chapter 3 to generate a geometric representation of

the environment. Furthermore, both SFM and the optical flow approach provide vehicle

35

state information that is subject to drift over time. In Chapter 5 this information will

be fused with onboard state estimates and the external environment measurements in a

Simultaneous Localization and Mapping (SLAM) algorithm to reduce drift in vehicle state

estimates and concurrently decrease map errors.

36

CHAPTER 3ENVIRONMENT REPRESENTATION

3.1 Introduction

The proposed goal of autonomously generating an environment map requires a

vehicle to sense its surroundings and infer meaningful information concerning scene

structure from sensor measurements. Conventional sensors commonly used for measuring

a vehicle’s surroundings include laser range finders, sonar, and cameras. The three

dimensional environmental information provided by such sensors is pointwise, relative

range information. An obstacle map defined by a cloud of three-dimensional feature points

is not amenable to motion planning algorithms [26] and is a poor map in terms of data

storage, landmark definition, and enabling high-level mission planning.

Motion planning algorithms typically used for robotic navigation presuppose a known

obstacle map [58]. The obstacle map enables the identification of safe routes through a

vehicle’s surroundings by limiting the set of admissible positions a vehicle can achieve.

A pointwise representation of obstacle geometry is ambiguous in terms of defining safe

regions within the environment. A more concise representation of the environment, such as

geometric objects, in which complete obstacles can be accurately described is better suited

to the process of motion planning.

Mapping large-scale environments requires that many sensor measurements be taken

to accurately define scene structure. Storing individual sensor measurements during

a mapping operation can quickly lead to a computationally intractable problem [59].

Lower-dimensional representations of sensor data can greatly reduce the number of

parameters needed to define an obstacle map. For example, a single plane can reasonably

represent thousands of three-dimensional points using four parameters including three

to define a normal direction and one to define the plane’s distance from the origin along

that normal direction. For highly structured environments, using geometric primitives

such as planes to model the sensor data can greatly reduce the amount of data required

37

to generate a spatial model. In addition, dimensionality reduction can filter the effects of

sensor noise by creating a smooth representation of noisy data.

Robotic navigation can be divided into different methodologies depending on the

level of knowledge afforded the vehicle concerning its surroundings and relative position

within the environment. The most general case requires that the vehicle estimate both

a map of the environment and its location within that map simultaneously [10, 60, 61].

This case is known as the Simultaneous Localization and Mapping (SLAM) problem as

discussed in detail in Chapter 5. The SLAM framework requires that distinguishable and

re-observable landmarks be present in the environment. Landmarks allow the vehicle

to ascertain its relative location to the environment through sensing operations. The

only discernible characteristic of a 3D feature point is its location in the environment

which makes 3D points a poor choice for landmarks. A geometric landmark, however,

can provide additional information in terms of orientation and size. Additionally, feature

points that are visible from a given vantage point are not necessarily visible from another

location due to occlusion and lighting variations. Geometric maps define landmarks that

are robustly observable with respect to camera pose.

Many missions envisioned for autonomous vehicles require human interaction in

terms of high-level mission planning. Surveilling targets in an environment such as a

specific building or roadway depends upon the ability to properly designate the known

objective. Similarly, reconnaissance of an area of interest in the environment requires that

the region be clearly specified. Such high-level decisions will be made by a human operator

who must designate the targets or regions of interest in some manner. In this capacity, a

spatial model is advantageous as it provides an intuitive interface for target selections as

opposed to a pointwise model which is an ambiguous representation that requires further

interpretation of the data.

The need to enable motion planning, construct compact map representations, define

distinguishable landmarks, and assist high-level mission planning motivates the generation

38

of reduced-dimensionality geometric models of the environment. The following chapter

describes the chosen map representation and the method for dimensionality reduction.

3.2 Environment Models

Mapping operations are typically categorized by how the environment is modeled.

Maps commonly used for robotic navigation can be classified as either topological,

occupancy grid, or geometric representations as depicted in Figure 3-1.

A B C

Figure 3-1. Environment representations. A) Topographic maps depict spatial connectivityby storing locations of interest and connecting paths. B) Occupancy mapsrecord the probability that a discrete location contains an obstacle. C)Geometric maps define obstacles as geometric primitives.

3.2.1 Topological Maps

Topological maps model the environment as a series of nodes and connecting arcs

[62–64] as shown in Figure 3-1A. Typically, the nodes consist of places of interest such as

the intersection of corridors and arcs provide connectivity information about the nodes,

to suggest navigation paths between nodes. Topological models are not necessarily spatial

models as the nodes can represent various states of the vehicle. Such maps, however, rely

on spatial models for their construction since the arcs need to define maneuvers that avoid

obstacles. This fact blurs the distinction between topological and geometric maps since

topological maps rely on geometric information.

3.2.2 Occupancy Maps

Occupancy grid representations discretize the environment and each generated cell

is assigned a value that corresponds to the likelihood that the cell is occupied [65, 66].

39

Though occupancy grid approaches are easy to construct they suffer from the amount

of space required to handle an environment that is discretized finely enough to capture

the necessary amount of detail. For large, three-dimensional environments this lack of

compactness causes this representation to be intractable in terms of the data storage

requirements for the UAV mapping problem.

3.2.3 Geometric Maps

Geometric representations attempt to fit geometric primitives to sensor data. In two

dimensions this fit is accomplished by replacing point data with line segments [25, 67].

Three-dimensional maps generated from dense depth information typically use triangle

meshes to represent the environment [60, 68]. Examples exist for representing structured,

three-dimensional environments by a less general set of geometric primitives such as

distinct planar surfaces [24, 69, 70].

This dissertation employs a geometric environment map because the properties are

beneficial for both the SLAM (Chapter 5) and trajectory planning (Chapter 6) tasks.

The geometric map provides a compact representation that reduces the computational

burden for both processes. Additionally, a geometric description of the landmarks makes

them more observable than point landmarks or visually defined landmarks. Lastly, the

geometric landmarks are amenable to collision checking algorithms required for planning

safe trajectories and are an intuitive representation for aiding in higher-level mission

planning concerns.

The geometric map is constructed from the pointwise observations of the environment

provided by SFM. This task is accomplished via a dimensionality reduction process.

3.3 Dimensionality Reduction

Dimensionality reduction refers to the process of representing a dataset with a smaller

set of parameters while preserving the information conveyed by the dataset. For example,

a set of m two-dimensional points that are approximately linear can be represented by

four parameters for the endpoints of the linear fit. For large values of m, the dimensionally

40

reduced representation vastly decreases the storage requirements for a given dataset.

The dimensionality reduction step in this dissertation is achieved by fitting planes to the

three-dimensional points generated by the SFM algorithm.

3.3.1 Principal Components Analysis

The plane fitting task is accomplished using Principal Components Analysis (PCA)

[71], a statistical linear transformation used for reducing the dimensionality of a dataset.

PCA finds the directions in a dataset with the most variation, as shown in Figure 3-2 in

order to capture the variability of the data with fewer parameters. These directions are

also the eigenvectors that correspond to the largest eigenvalues of the data’s covariance

matrix. Therefore, PCA can be employed to transform a dataset to a new basis where the

first coordinate, called the first principal component, defines the direction of maximum

variance. All subsequent directions defined by the basis are orthogonal to all previous

directions and define the remaining direction of greatest variance.

Figure 3-2. 2D Prinicipal Components Analysis for a dataset with a linear trend. Theprincipal components are labeled pn for the nth principal component and the2σ covariance of the dataset is shown.

41

The principal components can be calculated by finding the eigenvectors of the data’s

covariance matrix or, more succinctly, by performing a Singular Value Decomposition

(SVD). The calculation begins by centering the dataset X by subtracting its mean µ.

Because the goal of the calculation is to find directions of greatest variance, a non-zero

mean will produce erroneous results. The SVD of the mean-subtracted dataset X is given

by

XSVD= UΣVT (3–1)

where U and V define matrices of orthonormal vectors and Σ defines a diagonal matrix

of singular values assumed to be ordered from largest to smallest. The columns of V

define the principal components of X with the column corresponding to largest singular

value designating the first principal component. The result is typically ordered so that the

first principal component is the first entry of Σ with subsequent components listed in a

descending order. The dataset can be projected on to the basis formed by the principal

components according to

Y = XV (3–2)

where Y is the transformed dataset.

The SVD computation is related to the eigen decomposition method because the SVD

of the rectangular matrix X calculates the eigenvectors of XT X and XXT which are then

the matrices U and V in Equation 3–1, respectively. Additionally, the singular values

Σ are the square roots of the eigenvalues of XT X and XXT . Since the dataset has zero

mean, the covariance matrix of X is XT X and the eigen decomposition of the covariance

gives eigenvectors identical to V and produces eigenvalues equal to the square of the

singular values. Therefore, SVD and eigen decomposition can be used interchangeably for

PCA.

For three-dimensional information being mapped to a planar representation, shown

in Figure 3-3, the first two principal components define two orthogonal in-plane directions

while the third component defines the plane’s normal direction. Therefore, three principal

42

components and a centroid will define each planar surface in the environment regardless of

how many feature points comprise the surface.

Figure 3-3. 3D Principal Components Analysis for a planar dataset. The principalcomponents are labeled pn for the nth principal component and the 2σcovariance of the dataset is shown.

3.3.2 Data Clustering

The aforementioned PCA algorithm cannot be naively implemented to generate

planar landmarks for a given sensing operation. The three-dimensional data points

returned by the computer vision algorithm must be clustered into candidate planar regions

before PCA can return the plane parameters. A difficulty arises because no knowledge

regarding the number of visible planes is available. Therefore, the number of planar

clusters must be estimated for each sensing operation.

Planar clustering is accomplished with an iterative algorithm based on k-means.

K-means is an unsupervised learning algorithm that defines a set of clusters for a dataset

by defining cluster centers, or centroids, that minimize the distance between all data

points and their associated cluster centroid [72]. Effectively, the k-means algorithm

43

minimizes the objective function

J =k∑

j=1

∑xi∈Sj

‖xi − cj‖2 (3–3)

where xi is a data point contained in the data vector X, Sj is the set of all data points

associated with the jth centroid, and cj is the centroid location of the jth cluster.

The minimization of the objective function in Equation 3–3 is carried out according to

Algorithm 1 which iterates until all centroid locations converge to locally optimal solution.

The algorithm begins by guessing the number of centroids k and randomly initializing the

centroid locations {c1, c2, . . . , ck}. For each iteration, the data points are associated to the

nearest centroid according to Euclidean distance. The centroid locations are then updated

to the mean of the newly associated data points. Convergence occurs when the maximum

centroid displacement is less than a given tolerance tolk.

Algorithm 1 The k-means algorithm

1: select the number of clusters k2: randomly initialize cluster centroids {c1, c2, . . . , ck}3: repeat4: for each data point xi ∈ X do5: for j = 1 to k do6: compute the squared 2-norm dj = ‖xi − cj‖2

7: end for8: classify xi ∈ Sj where j = arg min

j(dj)

9: end for10: for j = 1 to k do11: compute new centroid location cj = mean(xi ∈ Sj)12: calculate centroid movement Dj = ‖cj − cj‖2

13: update centroid location cj = cj

14: end for15: until max(Dj) < tolk

The k-means algorithm, however, requires that the number of clusters k is known

a priori an the number of planar clusters in unknown for each sensing operation. An

iterative extension of k-means, G-means, locates an unspecified number of clusters based

on a statistical test that determines if a cluster fits a Gaussian distribution [73]. Similarly,

44

the employed clustering algorithm iteratively searches for clusters that satisfy a planarity

test.

3.3.3 Plane Fitting Algorithm

The clustering algorithm, presented in Algorithm 2, iteratively searches for planar

clusters by starting with a single cluster (k = 1) and increasing the number of clusters

to best fit the dataset. Each iteration begins with the k-means algorithm which produces

a set of clustered feature points. Each cluster is checked for planarity with PCA by

inspecting the singular value associated with the third principle component p3.

The singular values produced by PCA denote the amount of the data’s variance

captured by the associated principal component. Therefore, the magnitude of the third

singular value, σ3, denotes how planar is the data cluster with zero magnitude describing

a perfectly planar data set. Planarity is determined by imposing a threshold σt on σ3. A

threshold value of 0.05 ≤ σt ≤ 0.1 produces good results for realistically noisy datasets.

Clusters found to be planar are removed from further consideration in the clustering

procedure. All feature points, including points currently attributed to different clusters,

are projected onto the planar basis according to Equation 3–2. The feature points are

tested for inclusion into the current plane definition according to connectivity and distance

along the normal direction tests. The connectivity test searches for all points within a

specified distance from any currently included point beginning with the point closest

to the cluster’s centroid. The normal direction test removes points that are a specified

distance from an in-plane position. The remaining points are classified as a single planar

object and removed from the dataset. The convex hull of the points H, the plane’s

normal direction, and the perpendicular distance from the sensor position to the plane are

saved as an observation z. All observations are used in the SLAM algorithm described in

Chapter 5.

The centroids of non-planar clusters are split, as shown in Figure 3-4, in order to

choose new centroids that properly capture the variability of the dataset. The centroids

45

A

B

C

Figure 3-4. Centroid splitting for iterative k-means. A) The initial result of k-means whichdoes not fit the dataset. B) Centroid splitting results in two new centroidsthat more aptly characterize the dataset. C) The final k-means result thatproperly clusters the data.

46

are split in the direction of the first principal component p1 as described by vector r

whose magnitude is a function of the components singular value σ1 as given by

r = ±p1

√2σ1

π(3–4)

The expression in Equation 3–4 is equal to the splitting distance employed in the

G-means algorithm [73]. After the centroids have been split or removed k-means is rerun

on the remaining data set. This process is performed repeatedly until the number of

remaining data points falls below a threshold tolf . This tolerance is typically set as 10% of

the total number of feature points.

Algorithm 2 Plane fitting algorithm

1: initialize a single cluster centroid (k = 1)2: initialize observation counter j = 03: repeat4: perform k-means on the dataset returning centroid locations {c1, c2, . . . , ck}5: for i = 1 to k clusters do6: perform PCA on cluster ci

7: extract third singular value σ3

8: if σ3 < σt then9: increment observation counter j = j + 1

10: project all feature points onto PCA basis Y = XV11: find all feature points that fit the planar definition12: compute convex hull H of associated feature points13: store H and PCA result as sensor observation zj

14: remove centroid ci and associated feature points from data set15: else16: split centroid ci along first principal component p1

17: end if18: end for19: until number of fpts < tolf

The result of the plane fitting algorithm is shown in Figure 3-5. The algorithm

transforms the pointwise data provided by SFM into a set of geometric primitives.

Concatenating these observations will create an incorrect geometric map due to drift in

vehicle state estimates. Chapter 5 will present an algorithm that fuses the vehicle state

estimates and environment observations in order to create a spatially consistent map.

47

A

B

Figure 3-5. Dimensionality reduction of feature point data. A) Pointwise data from theSFM algorithm. B) The dimensionally-reduced dataset consists of geometricprimitives in the form of planar features.

48

CHAPTER 4FILTERING FOR STATE ESTIMATION

4.1 Introduction

Chapter 3 described a method for extracting planar features from a highly structured

environment. These features will be used in Chapter 5 to incrementally construct an

environment map with a filtering technique known as Simultaneous Localization and

Mapping (SLAM). The SLAM task is a state estimation approach in which the state

of the system is a combination of the vehicle state, defined in terms of position and

orientation, and the state of the map, designated by the position and orientation of

environmental landmarks.

State estimation is commonly addressed as a filtering problem in which estimates

are calculated recursively by incrementally incorporating sensor information [79]. This

chapter describes several filters that follow a similar predictor-corrector structure depicted

in Figure 4-1. The filtering process begins with the prediction of a noisy estimate of the

system state according a state transition model. Measurements of the system are predicted

as if the true system state is equal to the state prediction. The measurement prediction

is compared to an actual, noisy system measurement and the state estimate is updated

according to a weighted measurement residual. The weight of the measurement residual

represents the confidence of the measurement with respect to the model-based state

estimate.

Two filters that form the foundation of the majority of SLAM solutions are the

Kalman filter [80] and particle filter [82]. This chapter describes the general form of these

filters and several variants that will be used in the SLAM filtering approaches described in

Chapter 5.

4.2 The Kalman Filter

The Kalman filter is an estimator for linear systems that yields minimum mean

square error (MMSE) estimates. Kalman filtering estimates the state x ∈ Rn of a dynamic

49

Figure 4-1. Recursive state estimation. The recursive process involves three steps:prediction, observation, and update.

system described by the linear stochastic difference equation

xk = Axk−1 + Buk−1 + qk−1 (4–1)

where A ∈ Rn×n is the state matrix, B ∈ Rn×l is the input matrix, u ∈ Rl is the input

vector, and qk−1 ∈ Rn is a vector of random variables representing additive process noise.

The noise variable qk−1 is a zero-mean Gaussian variable with covariance Σq.

The state estimation process incorporates measurements z ∈ Rm of the system that

are modeled as

zk = Hxk + rk (4–2)

where H ∈ Rm×m is an output matrix mapping states to observations and r ∈ Rm is a

zero-mean Gaussian random variable vector with covariance Σr representing measurement

noise.

For a given state estimate x, the error e and error covariance P are defined as

e = x− x (4–3)

P = E[eeT

](4–4)

50

where E[·] denotes the expectation or mean value of a random variable.

The Kalman filtering procedure begins with a prediction step that updates the state

estimate and error covariance as follows

xk|k−1 = Axk−1|k−1 + Buk−1 (4–5)

Pk|k−1 = APk−1|k−1AT + Σq (4–6)

where the subscript k|k − 1 denotes a value at time k given measurement information, or

evidence, up to time k − 1 also known as an a priori estimate. The state and covariance

at k|k − 1 define the prior probability distribution of the state estimate. The notation

k|k is referred to as an a posteriori estimate, with the mean and covariance denoting the

posterior probability distribution, since such estimates include measurements taken up to

the current time step.

The optimal Kalman gain, which weights the measurement residual, for MMSE

estimation is defined as

Kk = Pk|k−1HT

(HPk|k−1H

T + Σr

)−1(4–7)

The final step of the Kalman filter is to update the predictions given in Equations 4–5

and 4–6 after a measurement zk is taken. The measurement update equations are

xk|k = xk|k−1 + Kk

(zk −Hxk|k−1

)(4–8)

Pk|k = (I−KkH)Pk|k−1 (4–9)

The resulting estimates xk|k and Pk|k are used as previous time estimates in

Equations 4–5 and 4–6 for the next prediction as the recursive process continues.

The Kalman filter is designed for linear systems with linear measurement models. The

SLAM problem, however, deals with nonlinear systems in the form of vehicle dynamic

models and sensors with nonlinear measurement models. Therefore, the nonlinear

extensions of the Kalman filter are employed for the estimation task. The nonlinear

51

Kalman filters include the Extended Kalman Filter (EKF) [81], the Unscented Kalman

Filter (UKF) [83], and the Square-Root Unscented Kalman Filter (SQUKF) [84].

4.2.1 The Extended Kalman Filter

The EKF allows for a nonlinear process model f(·) and a nonlinear measurement

model h(·)xk = f (xk−1,uk−1,qk−1) (4–10)

zk = h (xk, rk) (4–11)

It should be noted that the process noise and measurement noise are no longer

restricted to be additive uncertainties. The EKF follows the Kalman filter approach by

linearizing the models with respect to the current state estimate of the system as follows

Ak =∂f

∂x

∣∣∣xk−1|k−1,uk−1(4–12)

Hk =∂h

∂x

∣∣∣xk|k−1, (4–13)

Additionally, the models are linearized with respect to the noise values

Qk =∂f

∂q

∣∣∣xk−1|k−1,uk−1(4–14)

Rk =∂h

∂r

∣∣∣xk|k−1, (4–15)

which means the EKF filter equations, analogous to Equations 4–5 through 4–9, can be

written as

xk|k−1 = f(xk−1|k−1,uk−1, 0

)

Pk|k−1 = AkPk−1|k−1ATk + QkΣqQ

Tk

Kk = Pk|k−1HTk

(HkPk|k−1H

Tk + RkΣrR

Tk

)−1

xk|k = xk|k−1 + Kk

(zk − h

(xk|k−1, 0

))

Pk|k = (I−KkHk)Pk|k−1

(4–16)

which defines the complete EKF. It should be noted that the state a measurement

predictions use the full nonlinear equations while the linearized models are required to

52

estimate the distribution’s covariance. This linearization is the primary source of error in

the EKF state estimation process.

4.2.2 The Unscented Transform

The EKF does not retain the optimality of the Kalman filter because the linearization

step does not properly handle the statistics of the problem. This inaccuracy is because

the random variables no longer have normal distributions after undergoing a nonlinear

transformation. The UKF employs the Unscented Transform (UT) to more accurately

estimate the statistics of a random variable that is passed through a nonlinear

transformation.

The UT calculates the mean and covariance of a variable undergoing a nonlinear

transformation by propagating a set of points (called sigma points) through the nonlinear

function and estimating the new statistics of the distribution as shown in Figure 4-2. The

sigma points are chosen according to a deterministic algorithm so that they accurately

approximate the true mean and covariance of the untransformed distribution.

Figure 4-2. The Unscented Transform. The UT propagates a set of sigma points through anonlinear function to calculate the posterior probability distribution.

53

In general, the UT defines a set of 2L + 1 sigma points X and associated weights for

an L-dimensional random variable with mean µ and covariance P as follows

X 0 = µk−1 W s0 = λ

L+λ, W c

0 = λL+λ

+ (1− α2 + β)

X i = µk−1 +(√

(L + λ)Pk−1|k−1

)i

W si = W c

i = 12(L+λ)

X i+L= µk−1 −(√

(L + λ)Pk−1|k−1

)i

W si = W c

i = 12(L+λ)

λ = α2(L + κ)− L

(4–17)

where the subscript of the square root denotes the ith column of the matrix square root

and α, β, and κ are tuning parameters for controlling the placement and weighting of the

sigma points. The sigma points are transformed by the nonlinear function

Y i = f(X i), ∀ i = 1, . . . , 2L + 1 (4–18)

and the new mean and covariance are the weighted average and weighted outer product of

the transformed points

µk =2L∑i=0

W si Y i (4–19)

P =2L∑i=0

W ci [Y i − µk][Y i − µk]

T (4–20)

4.2.3 The Unscented Kalman Filter

The unscented transform is applied to the EKF to replace the prediction and update

equations of Equation 4–16 used to create the UKF. This derivation is accomplished with

an augmented state vector x′ ∈ Rn+q, where q is the dimension of the noise vector q,

defined as

x′ =

x

q

(4–21)

The process model, Equation 4–10, is rewritten to accept the augmented state vector

as input,

xk = f ′(x′k−1,uk−1,qk−1

)(4–22)

54

so that the sigma points in Equation 4–17 are drawn from a probability distribution with

a mean value

x′k−1|k−1 =

xk−1|k−1

0q×1

(4–23)

and covariance

P′k−1|k−1 =

Pk−1|k−1 Pxv,k−1|k−1

Pxv,k−1|k−1 Σq

(4–24)

where Pxv is the cross correlation matrix between the state errors and the process noise.

The measurement model, Equation 4–11, is rewritten so that measurement noise is

restricted to be additive as was assumed in the linear Kalman filter

zk = h′ (xk−1) + rk−1 (4–25)

The resulting UKF filter equations begin with the prediction step where sigma points

X ′, as given in Equation 4–17, are drawn from the distribution defined by Equations 4–23

and 4–24. These sigma points are transformed by the nonlinear process model with zero

noise

X i,k|k−1 = f ′(X ′

i,k−1|k−1,uk−1, 0), ∀ i = 1, . . . , 2L + 1 (4–26)

The mean and covariance of the sigma points are calculated from Equations 4–19 and

4–20 as

xk|k−1 =2L∑i=0

W si X ′

i,k|k−1 (4–27)

Pk|k−1 =2L∑i=0

W ci [X i,k|k−1 − xk|k−1][X i,k|k−1 − xk|k−1]

T (4–28)

Next, observations are predicted by transforming the updated sigma points with the

measurement model

Z i,k = h′(X i,k|k−1,uk−1, 0

), ∀ i = 1, . . . , 2L + 1 (4–29)

55

where the mean of the probability distribution of the current observation is

zk =2L∑i=0

W si Z ′

i,k (4–30)

with corresponding covariance

Pzz,k =2L∑i=0

W ci [Z i,k − zk][Z i,k − zk]

T (4–31)

The Kalman gain for the UKF is calculated according to

Pxz,k|k−1 =∑2L

i=0 W ci [X i,k|k−1 − xk|k−1][Z i,k − zk]

T

Kk = Pxz,k|k−1 (Pzz,k)−1

(4–32)

Finally, the state estimate and covariance matrix can be updated to generate the

current state estimate

xk|k = xk|k−1 + Kk (zk − zk)

Pk|k = Pk|k−1 −KkPzz,kKTk

(4–33)

Therefore, the state estimate of the system has been computed without requiring

linearization of the process and measurement models.

4.2.4 The Square-Root Unscented Kalman Filter

The UKF approach to state estimation has been shown to produce superior results to

the EKF for nonlinear systems [83]. The UKF, however, is a computationally expensive

algorithm in comparison to the EKF. This expense is because the sigma point calculation,

Equation 4–17, requires 2L + 1 evaluations of the nonlinear process model and matrix

square roots of the state covariance. The Square-Root Unscented Kalman Filter

(SR-UKF) is an efficient variant of the UKF in which the matrix square root required to

generate the sigma points is eliminated by propagating the square root of the covariance

matrix in lieu of the full covariance [84]. Therefore, the sigma point calculation, replacing

Equation 4–17, is

X ik−1|k−1 =

[xi

k−1,xik−1 ±

(√(L + λ)Si

k−1|k−1

)](4–34)

56

where S is the matrix square root of the covariance matrix initialized by a Cholesky

factorization of P0. The Cholesky factor S is updated in the prediction step using a QR

decomposition to to incorporate the weighted sigma points excluding the centroid as

follows

Six,k|k−1 = qr

([√W c

1 (X i1:2L,k|k−1 − xk)

i√

Σq

])(4–35)

where qr(A) represents a QR decomposition A = QR where Q is a square unitary

matrix and R is an upper triangular matrix returned by the qr operator. Since the weight

corresponding to the centroidal sigma point may be negative it is incorporated separately

according to

Six,k|k−1 = cholupdate

(Si

x,k|k−1,X i0,k|k−1 − xk)

i,W c0

)(4–36)

where cholupdate(A,X, s) represents a Cholesky update returning the Cholesky factor of

A ± XX∗ with s dictating the sign on the update. Equations 4–35 and 4–36 replace the

UKF state covariance update Equation 4–28.

The measurement covariance update equations, replacing the analogous UKF

Equation 4–31, follow the same formulation as the state prediction methodology as

shown in the following equations

Siz,k = qr

([√W c

1 (Z i1:2L,k − zi

k)√

Σr

])(4–37)

Siz,k = cholupdate

(Si

z,k,Z i0,k − zi

k),Wc0

)(4–38)

Uik = Ki

kSiz,k Si

x,k|k = cholupdate(Si

x,k|k−1,Uik,−1

)(4–39)

where the Kalman gain is calculated as follows

Kik = (Pi

xkzk/(Si

z,k)T )/Si

z,k (4–40)

Therefore, the SR-UKF calculates state estimates without linearizing the system

models as in the EKF and without the computationally expensive matrix square root in

the UKF approach.

57

4.3 The Particle Filter

The Kalman filter and its nonlinear variants (EKF,UKF,SR-UKF) operate under

the assumption that the probability distribution of the state estimate is Gaussian, or

able to be parameterized by a mean and covariance. The particle filter is a Monte Carlo

(MC) method for state estimation which represents the probability distribution as a set of

weighted random samples [82]. This formulation allows the estimation process to handle

non-Gaussian and multi-modal probability density functions.

Like the Kalman filtering approaches, the particle filter operates in a recursive

fashion. The state is predicted according to a process model and, subsequently, updated

by comparing sensor measurements to measurement predictions calculated with a

measurement model. However, while the Kalman filter propagates a normal distribution

through the nonlinear models, the particle filter propagates a set of weighted samples

called particles. Particle filters employ a set of N particles defined as

S ={xi

k,wik

} ∀i = 1, . . . , N (4–41)

where xik are random samples of the state’s probability density function p(xk|xk−1,uk)

and wik are associated importance weights calculated according to environmental

observations. The particles differ from the sigma points employed by the UKF in that

they are randomly selected and not deterministically calculated. Additionally, the particles

are weighted in order to maintain a general probability distribution while the sigma points

are used to reconstitute a Gaussian distribution.

The particle filtering algorithm, outlined in Algorithm 3, begins by drawing state

samples from a probability density defined as

xk ∼ p(xk|xk−1,uk) = f(xk−1,uk,qk) (4–42)

which is the same nonlinear process model definition used in the EKF (Equation 4–10).

The notation a ∼ b denotes that the random variable a has the probability distribution

58

b and the notation p(c|d) represents a probability distribution for variable c given the

evidence d.

Each particle drawn from the process model distribution is assigned a weight

wik ∝ wi

k−1p(zk|xik) (4–43)

where p(zk|xik) is defined as the nonlinear measurement model

zk,j ∼ p(zk|xk) = h(xk, rk) (4–44)

and the proportionality is eliminated by normalizing the weights at each step. Therefore,

each particle is assigned a weight which is a function of its previous weight and the

probability that the sample would produce the current sensor measurement. The weights

can be seen as the importance of the particles with respect to maintaining an accurate

representation of the probability distribution.

The final step of the particle filter is the resampling of the particles with replacement

according to their weights. The resampling allows for particles with a low weight, that

no longer represent the actual probability distribution, to be removed from further

consideration. Since the particle set is sampled with replacement, particles with a large

weight are often selected multiple times. The sampling step, however, can lead to particle

deficiency if sampling occurs too frequently. Particle deficiency refers to a lack of particle

diversity in which the distribution is approximated by N particles, but few are unique.

Many sampling heuristics have been developed to determine when best to perform the

resampling step. The method used in this dissertation is a resampling procedure based on

the number of effective particles Neff [85]. The number of effective particles

Neff,k =1∑N

i=1(wik)

2(4–45)

estimates how effectively the current particle set approximates the true posterior

distribution. Resampling occurs when Neff falls below a threshold NT .

59

Algorithm 3 Particle filter

1: loop2: for i = 1 to N samples do3: draw sample xi

k ∼ p(xk|xk−1,uk)4: assign a weight to the particle wi

k = wik−1p(zk|xi

k)5: end for6: calculate the total weight wk =

∑Ni wi

k

7: for i = 1 to N samples do

8: normalize the importance weights wik =

wik

wk

9: end for10: calculate Neff

11: if Neff < NT then12: resample with replacement13: end if14: end loop

4.4 The Rao-Blackwellized Particle Filter

Particle filtering can be extremely inefficient in high-dimensional spaces [86]. This

inefficiency is vitally important with respect to the SLAM problem where the state space

includes the vehicle states and the map landmark states. Therefore, particle filtering has

been applied to the SLAM problem with the Rao-Blackwellized Particle Filter (RBPF).

The Rao-Blackwellization is a process in which the number of states that need to

be sampled is reduced by partitioning the joint state. Therefore, for a joint distribution

p(x1,x2) the state is partitioned using the product rule

p(x1,x2) = p(x2|x1)p(x1) (4–46)

If the joint state is partitioned in such a way that the conditional probability p(x2|x1)

can be determined analytically then only the probability distribution of the partial state

vector p(x1) needs to be sampled as xi1 ∼ p(x1). Therefore, the new particle set becomes

S ={xi

1, p(x2|x1)i,wi

k

} ∀i = 1, . . . , N (4–47)

which means that each particle contains the partial state estimate xi1 and a probability

density p(x2|x1) that is conditioned on the particle’s specific estimate of xi1.

60

The sampling of a reduced state space increases the computational efficiency of the

RBPF by decreasing the dimension of the problem. This approach is applied to SLAM by

partitioning the state space so that the vehicle pose is sampled and the map’s landmark

states are determine analytically. This application is described further in Chapter 5 where

the RBPF is combined with the aforementioned EKF, UKF, and SR-UKF to produce

three filters for solving the SLAM problem.

61

CHAPTER 5SIMULTANEOUS LOCALIZATION AND MAP BUILDING

5.1 Introduction

Trajectory planning for a vehicle navigating an unknown environment motivates the

need to incrementally construct a spatial map of the vehicle’s surroundings. Motion

planning strategies that react only to current environmental sensor measurements

produce inefficient results since no knowledge outside the current sensor field of view

is incorporated into the planning process [74]. Time-dependent missions, such as target

acquisition and tracking, require a map to produce time efficient solutions. Additionally,

the majority of motion planning strategies presuppose a known environment map [26].

Therefore, map construction becomes a vitally important task for efficient motion

planning.

This research is concerned with navigation in GPS denied environments meaning

knowledge of the vehicle’s inertial location is not known. Therefore, navigation must rely

on relative vehicle position estimates which are subject to drift due to model uncertainties

and external disturbances. Additionally, sensor measurements of the external environment

are susceptible to noise. The combination of errors incurred from uncertain vehicle

location and noisy senor measurements produces spatially inconsistent maps as shown in

Figure 5-1.

The issues incurred by placing a vehicle at an unknown location in an unknown

environment and requiring it to incrementally build a spatially consistent map while

concurrently computing its location relative to the map are known as the Simultaneous

Localization and Mapping (SLAM) problem [75]. Research into the SLAM problem

has produced a variety of solutions based on probabilistic recursive algorithms with

implementations on a variety of platforms including ground vehicles, aircraft, and

underwater vehicles [19, 76, 77]. All successful SLAM solutions follow a similar overall

estimation strategy as depicted in Figure 5-2.

62

Figure 5-1. Map building in the presence of noise. Noise in the map building processproduces a spatially inconsistent map.

The SLAM estimation strategy begins with a prediction step (Figure 5-2A) in

which the vehicle position and orientation are estimated according to a motion model.

Additionally, sensor measurements are predicted using a measurement model that

estimates sensor readings based on the uncertain state of the vehicle and the current

map of the environment. Essentially, the prediction step generates a guess of what the

vehicle should observe given the current state estimates. Next, actual sensor measurements

of the environment are taken by the vehicle’s onboard sensor suite as depicted in Figure

5-2B. Finally, the actual observations are compared with the predicted observations and

the map and vehicle states are updated accordingly (Figure 5-2C).

There are several open problems in the SLAM community including the reduction

of computation complexity to enable real-time implementation, correct association of

observed landmarks with mapped landmarks, and environment representation [78].

This dissertation addresses these problems by employing a dimensionally reduced map

representation as described in Chapter 3. The map representation is a set of planar

landmarks that fit three-dimensional feature points gathered by an onboard camera. The

geometric map alleviates the computational burden of the SLAM solution by reducing the

63

A

B

C

Figure 5-2. The SLAM estimation strategy. A) In the prediction step the pose of thevehicle and sensor measurements are predicted according to motion andmeasurement models, respectively. The error ellipses denote uncertainty invehicle state and map estimates. B) The observation step involves acquiringrelative measurements from onboard sensors to environmental landmarks. C)In the update step the vehicle state and map estimates are updated bycomparing the predicted sensor measurements with the actual sensormeasurements.

64

number of landmarks that must be stored and incorporated into the estimation process.

Additionally, the geometric landmarks are easily distinguishable which aids the association

of sensor observations with map features.

This chapter builds on the overview of recursive filters that are important to the

SLAM estimation process presented in Chapter 4. Section 5.2 introduces the vehicle and

measurement models developed for the presented SLAM formulation. Section 5.3 describes

three specific filters that were investigated in relation to the airborne SLAM problem.

Lastly, Sections 5.4 and 5.5 describe the data association process and map management

procedures employed to incrementally incorporate sensed geometric primitives into a

global map.

5.2 Airborne SLAM Models

The goal of the airborne SLAM algorithm is to concurrently estimate the state of an

aircraft, x ∈ R6, defined by position and attitude, and a relative map consisting of j static

environmental landmarks m ∈ R6×j. The landmarks in this formulation are distinct planar

features defined by a centroidal location and a unit normal direction m = [µj, ~nj]T .

As described in Chapter 4, state estimation requires the definition of process and

measurement models that describe the system of interest. The general process and

measurement model definitions for the SLAM problem are probabilistically defined as the

following distributions

xk ∼ p(xk|xk−1,uk) = f (xk−1,uk−1,qk−1) (5–1)

zk,j ∼ p(zk,j|xk,mj) = h (xk,mj, rk) (5–2)

where f(·) is a nonlinear state transition model that calculates the current state of

the system xk based on the previous state xk−1, control input uk−1, and process noise

qk−1 with covariance Σq. The nonlinear measurement model h(·) generates a sensor

measurement zk,j for each landmark mj based on the vehicle state xk and measurement

noise rk with covariance Σr.

65

5.2.1 Vehicle Model

The addressed airborne SLAM problem assumes the vehicle is a fixed-wing aircraft

as depicted in Figure 5-3. Additionally, it is assumed that vehicle linear velocity [u, v, w]

and angular velocity [p, q, r] information is provided via an IMU or vision-based state

estimation algorithm (Sections 2.3.2 and 2.4). Therefore, the input to the vehicle model is

selected to be u = [u, v, w, p, q, r]T

Figure 5-3. Fixed-wing aircraft model definitions. The aircraft linear and angular rates aredefined with respect to a body-fixed basis B. The inertial position andorientation of the vehicle is measured with respect to a North East Down(NED) inertial frame E.

The vehicle model f(·) accepts noisy estimates of vehicle velocities as inputs.

Position and orientation state information is generated according to the aircraft kinematic

equations

x = u cos θ cos ψ + v(− cos φ sin ψ + sin φ sin θ cos ψ)

+w(sin φ sin ψ + cos φ sin θ cos ψ)

y = u cos θ sin ψ + v(cos φ cos ψ + sin φ sin θ sin ψ)

+w(− sin φ cos ψ + cos φ sin θ sin ψ)

z = u sin θ − v sin φ cos θ − w cos φ cos θ

φ = p + tan θ(q sin φ + r cos φ)

θ = q cos φ− r sin φ

ψ = (q sin φ + r cos φ)/ cos θ

(5–3)

66

where [x, y, z]T denotes the inertial position measure in a NED frame and [φ, θ, ψ]T are the

vehicle Euler angles defining roll, pitch, and yaw. The errors in linear and angular velocity

qk are assumed to be Gaussian with a covariance

Σq =

σ2u 0 0 0 0 0

0 σ2v 0 0 0 0

0 0 σ2w 0 0 0

0 0 0 σ2p 0 0

0 0 0 0 σ2q 0

0 0 0 0 0 σ2r

(5–4)

5.2.2 Measurement Model

The measurement model h(·) maps planes defined in the inertial coordinate system

to a body-fixed definition so that predicted measurements z can be compared with actual

sensor readings z. The definition of a landmark mj consists of an inertially defined

centroid and normal direction mj = [µj, ~nj]T . The sensor observations consist of planes

defined by a normal direction n and a perpendicular distance d from the plane to the

sensor. Therefore, the observation model h(·) is defined as

zk,j =

nk,j

dk,j

=

Rkeb~nj

nk,j ·Rkeb(µj − pk)

(5–5)

where Rkeb is the rotation matrix from the inertial frame to the body-fixed frame at time k

and pk is the current vehicle position. The sensor induced errors for the perceived normal

direction n = [n1, n2, n3]T and perpendicular distance are characterized by

Σr =

σ2n1

0 0 0

0 σ2n2

0 0

0 0 σ2n3

0

0 0 0 σ2d

(5–6)

67

5.3 Filtering for SLAM

The airborne SLAM problem addressed in this dissertation is subject to difficulties

exceeding ground vehicle SLAM with respect to computational complexity and high

nonlinearity in both vehicle dynamics and environmental measurements [19]. The airborne

SLAM problem has been investigated with the Extended Kalman Fitler (EKF) [18]

and the Unscented Kalman Filter (UKF) [20]. The EKF approach has been shown to

be somewhat successful in implementation for a very limited number of landmarks.

Additionally, UKF approach has proven to be superior to the EKF in terms of estimation

accuracy, but suffers from increased computational complexity. This dissertation

investigates three additional SLAM filters based on the Rao-Blackwellized Particle Filter

(RBPF). The goal is to find a solution to airborne SLAM that is more computationally

efficient than both the EKF and UKF approaches and handles the nonlinearities as well as

the UKF. The complete formulation of these filters, including the equations necessary for

implementation, is described in this section.

The SLAM estimation task is framed as a probabilistic process with the goal of

calculating the joint posterior distribution of the vehicle and landmark states

p(xk,m|Z0:k,U0:k,x0) (5–7)

where Z0:k is the history of all landmark observations, U0:k is the history of all control

inputs, and x0 is the known initial vehicle state. The following three filters estimate this

distribution solving the airborne SLAM problem.

5.3.1 Rao-Blackwellized Particle Filter

The RBPF was introduced as an alternative to EKF SLAM in an approach referred

to in literature as FastSLAM [23, 87]. The RBPF is a sampling-based approach which

factors the joint SLAM posterior distribution according to the observation that landmark

positions can be estimated independently if the vehicle path is known. The resulting

Rao-Blackwellized form contains a sampled term representing the probability of vehicle

68

paths and M terms that estimate the landmark locations

p(X0:k,m|U0:k,Z0:k,x0) = p(Xk:0|Z0:k,U0:k,x0)M∏

j=1

p(mj|Xk:0,Z0:k) (5–8)

where X0:k denotes a vehicle state history. According to this formulation the posterior

distribution is represented as a set of N particles {wik,X

i0:k, p(m|Xi

0:k,Z0:k)}Ni where wi

k is

a weight assigned to each particle and p(m|Xi0:k,Z0:k) is an individual map associated with

each particle. Therefore, one benefit of the RBPF approach is that each particle contains

a separate map which allows for multiple data association hypothesis to be assigned at

each observation update. This allowance means that the RBPF approach is more robust,

in terms of filter convergence, than EKF SLAM with respect data to association.

The FastSLAM approach, which is based on the RBPF with EKF-style updates,

proceeds as follows:

1. Generate N vehicle pose samples xik and associated observations zi

k,j:

xik|k−1 ∼ p(xi

k|xik−1,uk) = f(xi

k−1,uk,qk−1) (5–9)

Pixx,k|k−1 = ∇fxP

ixx,k−1|k−1∇fx

T +∇fuΣq∇fuT (5–10)

zik,j ∼ p(zi

k,j|xik,mj) = h(xi

k|k−1,mj,k−1, rk) (5–11)

where Pxx is the covariance of the vehicle pose, ∇fx is the Jacobian of the vehicle

model with respect to the vehicle state, and ∇fu is the Jacobian of the vehicle model

with respect to the vehicle control inputs.

2. Calculate importance weights wik and resample according to weights to promote

particle diversity:

wik = wi

k−1Ci (5–12)

where the constant C is the likelihood of all observations zk (the subscript j has

been dropped to show that the following is performed for all landmarks in a batch

fashion) given the estimated position of the vehicle. The likelihood is calculated as

69

follows

C i =√

(2π)m|Piwk| e− 1

2(zk−zi

k)T (Piwk

)−1(zk−zik) (5–13)

where

Piwk

= ∇hxΣq∇hTx +∇hmPi

m,k−1∇hTm + Σr (5–14)

and ∇hx is the Jacobian of the observation model with respect to vehicle pose

evaluated at xk|k−1 and mk−1, ∇hm is the Jacobian of the observation model with

respect to landmark location evaluated at xk|k−1 and mk−1, and Pm is the covariance

of the map states.

3. Calculate the proposal distribution and draw N samples xk defining the updated

vehicle state where the distribution is Gaussian with parameters:

xik|k = xi

k|k−1 + Pixx,k|kh

Tx (Bi

k)−1(zk − zi

k) (5–15)

Pix,k|k = (hT

x (Bik)−1hx + (Σq)

−1)−1 (5–16)

where Bk is defined as

Bik = Σr +∇hmPi

m,k−1∇hTm (5–17)

4. Update definitions of observed landmarks according to the EKF measurement

update:

Kim,k = Pi

m,k−1∇hTm(Bi

k)−1 (5–18)

mik = mi

k−1 + Kim,k(zk − zi

k) (5–19)

Pim,k = (I−Ki

m,k∇hm)Pim,k−1 (5–20)

If a new landmark is observed it’s covariance is initialized to

Pim,k = ∇hmΣr∇hT

m (5–21)

From the above FastSLAM algorithm it can be seen that another advantage of

the RBPF is that only observed landmarks are updated. Therefore, the RBPF is more

70

efficient than a naive implementation of an EKF where all landmark and vehicle states

are concatenated into a single matrix and updated simultaneously. Current research

approaches use submaps to increase in the efficiency of KF-based solutions.

5.3.2 Unscented Rao-Blackwellized Particle Filter

The Unscented Rao-Blackwellized Particle Filter (URBPF) is formulated by applying

the Unscented Transform (UT) (Section 4.2.2) to the RBPF SLAM formulation [88]. The

URBPF is an improvement over the RBPF in terms of estimation performance similar

to the comparison between the EKF and UKF discussed in Section 4.2.2. Additionally,

the URBPF is easier to implement because it does not require linearization of the state

transition and measurement models.

1. Generate N vehicle pose samples xik and associated observations zi

k,j according to

the UKF prediction rule:

X ik−1|k−1 =

[xi

k−1,xik−1 ±

(√(L + λ)Pi

xx,k−1||k−1

)](5–22)

X ik|k−1,t = f(X i

k−1|k−1,t,uk) xik|k−1 =

∑2Lt=0 W s

t X ik|k−1,t

(5–23)

Pixx,k|k−1 =

2L∑t=0

W ct

[X ik|k−1,t − xi

k|k−1

] [X ik|k−1,t − xi

k|k−1

]T(5–24)

Z ik,t,j = h(X i

k|k−1,t,mj,k−1) zik,j =

∑2Lt=0 W s

t Z ik,t,j

(5–25)

2. Calculate importance weights wik and resample according to weights to promote

particle diversity:

wik = wi

k−1Ci (5–26)

where the constant C is the likelihood of all observations zk (the subscript j has

been dropped to show that the following is performed for all landmarks in a batch

fashion) given the estimated position of the vehicle. The likelihood is calculated as

follows

C i =√

(2π)m|Piwk| e− 1

2(zk−zi

k)T (Piwk

)−1(zk−zik) (5–27)

71

where

Piwk

= (Pixkzk

)TΣqPixz,k + Pi

zz,k + Σr (5–28)

where

Pizz,k =

2L∑t=0

W ct

[Z ik|,t − zi

k

] [Z ik,t − zi

k

]T(5–29)

Pixx,k =

2L∑t=0

W ct

[X ik|k−1,t − xi

k|k−1

] [Z ik,t − zi

k

]T(5–30)

3. Calculate the proposal distribution and draw N samples to define the new vehicle

state xk where the distribution is Gaussian with parameters:

xik|k = xi

k|k−1 + Kik(zk − zi

k) (5–31)

Pixx,k|k = Pi

xx,k|k−1 −KikP

izz,k(K

ik)

T (5–32)

and the Kalman gain is

Kik = Pi

xz,k(Pizz,k)

−1 (5–33)

4. Calculate sigma points for the observed landmarks and transform according to the

observation model:

Mik =

[mi

k−1,mik−1 ±

(√(L + λ)Pi

m,|k−1

)](5–34)

Y ik,t,j = h(xi

k|k,Mik,t) zi

k,j =∑2L

t=0 W st Y i

k,t,j(5–35)

Piyy,k =

2L∑t=0

W ct

[Y ik|,t − zi

k

] [Y ik,t − zi

k

]T(5–36)

5. Update definitions of observed landmarks according to the UKF update rule:

Kim,k = Pi

m,k|k−1Piyy,k((P

iyy,k)

TPim,k|k−1P

iyy,k + Σr)

−1 (5–37)

mik = mi

k−1 + Kim,k(zk − zi

k) (5–38)

Pim,k|k = (I−Ki

m,k(Piyy,k)

T )Pim,k|k−1 (5–39)

72

5.3.3 Square-Root Unscented Rao-Blackweillized Particle Filter

This dissertation introduces a new option for efficiently solving the SLAM problem

based on the Square-Root Unscented Kalman Filter (SR-UKF) (Section 4.2.4). The

SR-UKF is an efficient variant of the UKF in which the matrix square root required

to generate the sigma points is eliminated by propagating the square root of the

covariance matrix in lieu of the full covariance. Similarly, the Square-Root Unscented

Rao-Blackwellized Particle Filter (SR-URBPF) is formulated by replacing the UKF-style

calculations in the URBPF with the SR-UKF approach.

1. Generate N vehicle pose samples xik and associated observations zi

k,j according to

SR-UKF prediction rule:

X ik−1|k−1 =

[xi

k−1,xik−1 ±

(√(L + λ)Si

xx,k−1||k−1

)](5–40)

X ik|k−1,t = f(X i

k−1|k−1,t,uk) xik|k−1 =

∑2Lt=0 W s

t X ik|k−1,t

(5–41)

Sixx,k|k−1 = qr

([√W c

1 (X i1:2L,k|k−1 − xk|k−1)

i√

Σq

])(5–42)

Sixx,k|k−1 = cholupdate

(Si

xx,k|k−1,X i0,k|k−1 − xk|k−1)

i,W c0

)(5–43)

Z ik,t,j = h(X i

k|k−1,t,mj,k−1) zik,j =

∑2Lt=0 W s

t Z ik,t,j

(5–44)

Sizz,k = qr

([√W c

1 (Z i1:2L,k − zk)

i√

Σr

])(5–45)

Sizz,k = cholupdate

(Si

zz,k,Z i0,k − zk)

i,W c0

)(5–46)

2. Calculate importance weights wik and resample to promote particle diversity:

wik = wi

k−1Ci (5–47)

where the constant C is the likelihood of all observations zk (the subscript j has

been dropped to show that the following is performed for all landmarks in a batch

fashion) given the estimated position of the vehicle. The likelihood is calculated as

C i =√

(2π)m|Piwk| e− 1

2(zk−zi

k)T (Piwk

)−1(zk−zik) (5–48)

73

where

Piwk

= (Pixkzk

)TQkPixkzk

+ Pizkzk

+ Rk (5–49)

Pizkzk

=2L∑t=0

W ct

[Z ik,t − zi

k

] [Z ik,t − zi

k

]T(5–50)

Pixkzk

=2L∑t=0

W ct

[X ik|k−1,t − xi

k|k−1

] [Z ik,t − zi

k

]T(5–51)

3. Calculate the proposal distribution and draw N samples to define the new vehicle

state xk where the distribution is Gaussian with parameters:

xik|k = xi

k|k−1 + Kik(zk − zi

k) (5–52)

Uik = Ki

kSizz,k|k−1 Si

xx,k|k = cholupdate(Si

x,k|k−1,Uik,−1

)(5–53)

and the Kalman gain is

Kik = (Pi

xz,k/(Sizz,k|k−1)

T )/Sizz,k|k−1 (5–54)

4. Calculate sigma points for the observed landmarks and transform according to the

observation model:

Mik =

[mi

k−1,mik−1 ±

(√(L + λ)Si

m,|k−1

)](5–55)

Y ik,t,j = h(xi

k|k,Mik,t) zi

k,j =∑2L

t=0 W st Y i

k,t,j(5–56)

Sim,k|k−1 = qr

([√W c

1 (Y i1:2L,k − zk)

i√

Σr

])(5–57)

Sim,k|k−1 = cholupdate

(Si

m,k|k−1,Y i0,k − zk)

i, W c0

)(5–58)

Siyy,k = qr

([√W c

1 (Y i1:2L,k − zk)

i√

Σr

])(5–59)

Siyy,k = cholupdate

(Si

yy,k,Y i0,k − zk)

i,W c0

)(5–60)

5. Update definitions of observed landmarks according to the SR-UKF measurement

update rule:

Kim,k = (Pi

m,k|k−1/(Siyy,k|k−1)

T )/Siyy,k|k−1 (5–61)

74

mik = mi

k−1 + Kim,k(zk − zi

k) (5–62)

Uim,k = Ki

m,kSiyy,k Si

m,k|k = cholupdate(Si

m,k|k−1,Uik,−1

)(5–63)

The SR-URBPF procedure is more computationally efficient than the URBPF

approach. A comparison of all three filters is given in Chapter 7.

5.4 Data Association

Several important implementation issues were not addressed in the previous filter

development section. The first to be discussed is data association, the process of relating

existing map landmarks to the current environmental observations. Data association

is a critical step in the SLAM problem with incorrect associations often leading to

a catastrophic failure of the state estimation process. The RBPF mitigates the data

association problem by allowing for multiple association hypotheses because each particle

contains a separate estimate of the environment map. Additionally, defining landmarks

as geometric primitives further alleviates the problem by greatly reducing the number of

landmarks and making the landmarks more distinguishable by incorporating orientation

information.

The vast majority of SLAM implementations deal with a large number of landmarks

defined by inertial location. These situations lend themselves to batch gating which

exploits the geometric relationships between landmarks by considering multiple landmark

associations simultaneously [78]. The observed landmark information provided by the

dimensionality reduction step, however, is sparse and distinguishable. Furthermore,

texture variations on building facades can lead to multiple observations of the same

landmark during a single sensing operation. Therefore, a nearest neighbor approach

is used to allow for multiple associations to the same target. It should be noted that

individual gating techniques such as the nearest neighbor method fail in environments

that are not sparsely populated or structured. This failure is a result of the individual

gating method’s investigation of a single landmark and sensor measurement pair at a time.

75

Therefore, the overall structure of the environment is not considered and many incorrect

associations can be made when the measurements are noisy.

The nearest neighbor approach uses Mahalanobis distance as a statistical measure of

similarity for an observed landmark and a predicted landmark. The Mahalanobis distance

is a distance metric that is scaled by the statistical variation of the data points. For the

case of data association, the squared Mahalanobis distance is calculated as follows

Mij = (zi − zj)T (Pij)

−1(zi − zj) (5–64)

where Pij is the covariance of the ith observation zi with respect to the observation

estimate zj of the jth landmark. In the Gaussian case the Mahalanobis distance forms

a χ2 distribution dependent on the dimension of the innovation vector and the desired

percentage of accepted associations. For a 4-dimensional innovation vector and an

acceptance percentage of 95% the Mahalanobis gate value for which an association will be

accepted is 9.5. An additional step is added to the nearest neighbor search which checks

the the distance between the estimated and observed plane boundary and does not accept

associations with boundaries that are too distant from each other.

The data association step is used in the preceding SLAM filters whenever a

measurement residual is calculated. The residual is a function of the actual measurement

and the predicted measurement, but the mapping between which actual measurement

relates to which prediction is unknown. Therefore, the true value and predicted value are

associated as the minimum arguments to Equation 5–64 for all possible pairs of true and

predicted measurements. All associated pairs are used to update the SLAM filter.

5.5 Map Management

The observations in the described SLAM formulation consist of partially perceived

planes. New observations must be incrementally fused into the global map as the

environment is continuously surveyed. Incremental map building is accomplished by

mapping the boundary of an observation, described by a convex hull H, to the inertial

76

frame according to

Hk,e = RkbeHk,b + pk (5–65)

The points that comprise the boundary H are updated to match the current landmark

after the landmark update step of the SLAM filter by moving the points along the

updated normal direction as follows

Hk,e = (~n · µ− ~n · Hk,e)~n + Hk,e (5–66)

The combined observed and original three-dimensional boundary points Ht are

projected onto updated landmark basis V according to

yk = (Hk,t − µ)V (5–67)

and the plane boundary is updated by computing the convex hull of the projected set

y and transforming the points back to the inertial frame. The centroid of the updated

landmark is computed as the mean of the updated hull.

This chapter has presented three complete filters for solving the airborne SLAM

problem. Additionally, methods for handling planar landmarks within the SLAM

framework were presented. Simulation results and comparison between the filters are

presented in Chapter 7.

77

CHAPTER 6TRAJECTORY PLANNING

6.1 Introduction

Trajectory planning refers to any process which determines the history of control

inputs, u(t), for a system that yield a time parameterized curve, q(t), called a trajectory

[26]. The trajectory for system can be derived to avoid obstacles, regulate the system to a

desired state, optimize an objective function, or a combination of these tasks. Trajectory

planning is distinct from path planning because it accounts for system dynamics by

planning motions with physically realizable velocities and accelerations. This distinction

means that the curve q(t) is twice differentiable with respect to time. Path planning,

however, is limited to constructing a kinematically feasible curve, q(s), in the system’s

configuration space.

The goal of trajectory planning addressed in this research is to generate collision-free

trajectories for an aircraft navigating in an unknown environment. The trajectories are

designed to build a complete environment map in minimal time. The employed mapping

procedure which incrementally constructs a map of planar features was discussed in

Chapter 5. This map is used as feedback for obstacle avoidance in the trajectory planning

process.

Optimal trajectory planning for aircraft in the presence of obstacles is a difficult

problem because the optimization is non-convex [89]. This non-convexity leads to

multiple locally optimal solutions which makes most standard optimization routines

computationally intractable. Several approaches for directly solving the trajectory

optimization problem for aircraft have been proposed. Two prominent methods based on

Mixed-Integer Linear Programming (MILP) [90, 91] and the Maneuver Automaton [35–37]

are described in Section 6.2. These methods do not, in general, guarantee optimality, are

sensitive to initial conditions, and can become computationally intractable in realistic

situations.

78

This research employs a decoupled trajectory planning strategy which converts a path

planning result to a trajectory using optimal control theory. This approach is similar to

offering an accurate initial condition to the direct trajectory planning methods, but limits

the optimization to a minimum-time path following calculation. The path planning task

is segmented into a two phase problem as shown in Figure 6-1. First, a coarse path is

constructed to a selected goal location. The portion of the global path that occurs within

the current sensor field of view is refined to provide a detailed local path. As a final step,

the detailed local path is converted to a trajectory with optimal control.

Figure 6-1. An overview of the trajectory planning process. A coarse global path isplanned to a prescribed goal location defined to explore the environment. Adetailed local path is planned to provide local sensor coverage. Lastly, thelocal path is converted to a trajectory using optimal control theory.

The employed path planning methodology is based on well established sampling-based

strategies [27, 32] described in Section 6.4. Section 6.5 describes a curvature constrained

kinematic model used to generate kinematically feasible paths. The environment coverage

criteria which selects the best path for environment coverage are introduce in Section 6.6.

The optimal control formulation employed for converting the kinematically feasible path to

a dynamically feasible trajectory is described in Section 6.7. The final section, Section 6.8,

describes the full algorithm for trajectory planning for optimal map building.

79

6.2 Direct Trajectory Planning Methods

Direct trajectory planning methods generate motion plans directly in the state space

of the system. Several methods for direct trajectory planning include artificial potential

functions [92], optimal control [89], and sampling-based methods [93]. The latter two

methods have been directly applied to the aircraft motion planning problem and are

described below.

6.2.1 Mixed-Integer Linear Programming

MILP is the minimization of a linear function subject to linear constraints in which

some of the variables are constrained to have integer values. The integer valued variables

are key because they allow the incorporation of obstacle avoidance into the optimization.

MILP has been selected to solve the trajectory optimization problem because of its ability

to handle obstacle constraints and due to recent improvements in the computational

efficiency of commercially available software for solving MILP problems. The general form

of a mixed integer program for the variable vector x ∈ Rn with m constraints can be

written as

x∗ = arg minx

J(c,x) = (cTx)

s.t. : Cx ≤ b

l ≤ x ≤ u

(6–1)

where J is the objective function to be minimized, c ∈ Rn and b ∈ Rm are vectors of

known coefficients, C ∈ Rm×n is a constraint matrix, u is the upper bound of the solution

vector, and l is the corresponding lower bound.

The following formulation of the MILP problem for vehicle navigation is a simplified

version for a two-dimensional vehicle [91], but can be extended to three dimensions

[90]. The goal of the trajectory planning problem is to compute a control history

{uk+i ∈ R2 : i = 0, 1, . . . , L− 1} that defines a trajectory {xk+i ∈ R2 : i = 1, . . . , L}.The optimization seeks to find a minimum time trajectory that terminates at a specified

goal location xg. This minimum time, terminally-constrained problem can be formulated

80

as the following MILP

u∗ = arg minu

J(bg(u), t) =∑L

i=1 big(ui−1)ti

s.t. : −K(1− big) ≤ xk+i − xg ≤ K(1− bi

g)

∑Li=1 bi

g = 1

(6–2)

where bg ∈ BL is a set of binary decision variables, B = {0, 1}, that define the point where

the goal location is reached and K is large positive number. When the goal is reached at

point xk+i the value of big is set to 1, otherwise, a value of 0 makes the first constraint

inactive. The second constraint ensures that the goal location is reached only once. The

objective function J guarantees that the goal is reached in minimum time.

Vehicle dynamics, written in terms of position x and velocity x, are included as a

constraint on the optimization in the linearized form

xk+1

xk+1

= A

xk

xk

+Buk (6–3)

where A is the vehicle state transition matrix, B is the vehicle input matrix, and u is

the vehicle control input. The vehicle dynamic model is subject to limits on velocity and

control input

x ≤ xmax

uk ≤ umax

(6–4)

Lastly, collision avoidance is included in the optimization as a constraint stating that

trajectory points must not reside within an obstacle. The following form of the constraint

is written for rectangular obstacles

ohigh −K[bin,1, bin,2]T ≤ xk+i ≤ olow + K[bin,3, bin,4]

T

∑4j bin,j ≤ 3

(6–5)

where K is a large positive value, bin ∈ B4 is a binary vector that must have at least one

active constraint (bin,j = 0) to be outside a given obstacle, and the obstacle upper right

81

corner and lower left corner are given by ohigh and olow, respectively. The extension of this

constraint to three dimensions requires geometrically regular polygonal obstacles.

The above formulation for minimum time travel to a goal location is computationally

intractable for large-scale trajectories and high-dimensional systems with more complex

dynamic models and a greater number of obstacles. Therefore, the MILP trajectory

planning problem is implemented in a Receding Horizon (RH) fashion to force a

computationally efficient solution which, by construct, is not provably optimal. This is

true for all RH implementations because the problem is solved incrementally and does

not account for the full scope of the problem at once. The MILP solution, like most

optimization-based approaches, is sensitive to initial conditions and there is no guarantee

that the solution is globally optimal as opposed to locally optimal. Additionally, a vast

number of objective functions and constraints cannot be expressed linearly and thus fall

outside the umbrella of admissible MILP problems.

6.2.2 Maneuver Automaton

The MA motion planning strategy is a direct motion planning strategy which realizes

a computationally efficient solution by discretizing the system dynamics and controls

into a maneuver library. The maneuver library is a finite set of motion primitives that

can be concatenated to create a complete trajectory. The motion primitives consist of

a set of trim trajectories and maneuvers. The trim trajectories are time-parameterized

vehicle motions that are characterized by constant control input and body-axis velocities.

The maneuvers are finite time transitions that connect trim trajectories. The MA can be

depicted as a directed graph as shown in Figure 6-2.

The MA is a directed graph, as seen in Figure 6-2, consisting of a set of nodes and

connecting arcs. The nodes represent the trim trajectories Λ that define equilibrium states

for the vehicle. For example, the three trim trajectory model shown could represent an

aircraft operating at constant altitude with one trim trajectory representing steady level

flight and the two other trim trajectories representing left and right turns. The maneuvers

82

Figure 6-2. An example Maneuver Automaton. A Maneuver Automaton defined by amaneuver library of three trim trajectories and nine maneuvers. M0 definesthe initial state transition by which the aircraft enters the directed graph.

M define transitions between trim trajectory operating conditions with M0 defining the

initial state transition by which the aircraft enters the directed graph.

The MA defines a set of dynamically admissible motion primitives for a given

vehicle which can be concatenated to form a trajectory. To generate a motion plan, an

optimization is performed which searches for the optimal sequence of maneuvers and

the associated time duration for each connecting primitive. Therefore, the optimization

searches for a trajectory defined by the sequence

(w, τ ) = Λw1(τ1)Mw1Λw2(τ2)Mw2 . . . MwNΛwN+1

(τwN+1), τi ≥ 0, ∀i ∈ {1, N + 1} (6–6)

where w is a vector determining the maneuver sequence and τ is a vector of coasting

times that define how long the trim primitives are executed. The trim primitives are

uniquely defined by the maneuver sequence and the constraint on τ ensures that all

coasting times are positive. The trim trajectories and maneuvers can be multiplied in

this fashion because they represent transformations of the vehicle state written in a

homogeneous representation. Therefore, general rigid-body motion denoted by a rotation

R and translation T, such as a maneuver, is written in the form

g =

R T

0 1

(6–7)

83

Similarly, time-parameterized rigid body motions, such as trim primitives, can be written

in the form

g(t) =

R(t) T(t)

0 1

(6–8)

which can be written in the exponential form g(t) = exp(ηt) where η is called the twist.

Optimal trajectories are found by solving the Nonlinear Program (NLP)

(w∗, τ ∗) = arg min(w,τ )

J(w, τ ) =∑N

i=1

(ΓMwi

+ γΛwiτwi

)+ γΛwN+1

τwN+1

s.t. : gw

∏Nw+1i=1 exp(ηiτi) = g−1

0 gf

w ∈ L(MA)

τi ≥ 0, ∀i ∈ {1, N + 1}

(6–9)

where the cost function J is defined by a summation of all maneuver costs ΓMwiand

primitive costs γTwiwhich are scaled linearly by their duration of execution τwi

. The

first constraint ensures the system is driven to a goal state and second constraint assures

that the solution maneuver exists in the set of all admissible maneuver sequences L(MA)

defined by the MA.

The posed optimization becomes computationally intractable as the length of the

maneuver sequence and the number of available motion primitives increases. Therefore,

the problem is decomposed into a search for the optimal maneuver sequence w∗ and an

optimization to determine the optimal coasting times τ ∗. This formulation, however, does

not include constraints for obstacle avoidance. For obstacle avoidance, the MA motion

planning strategy is coupled with sampling-based methods [94] (Section 6.4).

6.3 Decoupled Trajectory Planning

The aforementioned direct trajectory planning methods employ strategies that plan

directly in the state space of the system. Decoupled, or indirect methods, plan paths in

the vehicle’s configuration space and convert the resulting collision-free path q(s) into a

trajectory. Therefore, this method can be used to find the time-optimal trajectory that

84

follows a specified path in the configuration space [40, 41]. This dissertation employs a

decoupled trajectory planning strategy in order to incorporate a more complicated cost

function into the planning process than can be employed efficiently into a direct method.

Additionally, the full vehicle dynamics are used. The main drawback of this method is

that the coverage cost is not included in the optimization. However, since the environment

is unknown the planning strategy must be implemented in a RH fashion so optimality can

never be guaranteed.

6.4 Sampling-Based Path Planning

Decoupled trajectory planning requires that a path be planned in the vehicle’s

configuration space. A popular methodology for efficiently solving the path planning

problem in high dimensional spaces is known as sampling-based planners. Sampling-based

planners randomly select collision-free configurations from the vehicle’s configuration space

and connect the samples with kinematically feasible paths to form a solution. It should

be noted that the MA can be combined with sample-based planning to directly produce

dynamically feasible trajectories.

The first sampling-based path planner to be developed was the Probabilistic

Roadmap (PRM) [27]. The PRM algorithm constructs a roadmap in the workspace

consisting of nodes and connecting arcs as shown in Figure 6-3. The nodes are vehicle

configurations sampled from the vehicle’s free configuration space Qfree. The connecting

arcs are collision-free paths generated by a local planner ∆ that connects two sampled

configurations and satisfies the kinematic constraints of the vehicle. Therefore, ∆ contains

a collision detection step alerting the planner if a collision-free path cannot be constructed.

The roadmap is intended to capture the connectivity of the vehicle’s configuration space

in order to efficiently plan feasible paths without an exhaustive search. The PRM process

involves a learning phase in which the roadmap is constructed and a query phase in which

multiple path planning problems are solved with the constructed roadmap. Therefore, the

roadmap only needs to be constructed once.

85

The roadmap is constructed incrementally in the learning phase by sampling a

configuration from Qfree and connecting it to its k closest neighbors according to ∆. The

distance metric is user defined and not necessarily Euclidean distance. This process is

repeated until a predetermined number of nodes has been added as shown in Figure 6-3A.

Once the roadmap has been constructed path planning queries, in the form of an initial

configuration qinit and goal configuration qgoal, are answered. As shown in Figure 6-3B,

qinit and qgoal are connected to the to their nearest neighbor in the roadmap q′ and q′′,

respectively. Lastly, a graph search is performed to find the shortest path connecting the

initial and goal locations.

A B

Figure 6-3. Probabilistic Roadmap planning in a two-dimensional environment. A) Aconstructed roadmap where the circles are nodes designating vehicleconfigurations and the straight lines are arcs connecting the nodes withkinematically feasible paths. B) The query phase where the shortest path isfound through the roadmap given an initial and goal configuration.

The PRM was designed as a computationally efficient strategy for capturing the

connectivity of Qfree in order to answer multiple path planning queries in regards to the

same environment. Many planning strategies, however, require the answer to a single

query and do not require exploration of the entire Qfree. Two examples of sampling based

single query planners are the Expansive-Spaces Tree (EST) planner and Rapidly-Exploring

Random Tree (RRT) planner. The difference between the two approaches is the manner in

which the roadmap, or tree, is constructed.

86

6.4.1 Expansive-Spaces Tree Planner

The EST planner was developed to efficiently cover the space between qinit and qgoal

without needlessly exploring the entire Qfree. Instead of building a complete roadmap, like

the PRM approach, the EST incrementally constructs a tree T that is guided toward the

goal location. The tree construction process is outlined in Algorithm 4.

Algorithm 4 Expansive-Spaces Tree Construction

1: initialize tree T as a set of nodes V = q0 and edges E = ∅2: for i = 1 to n number of tree expansions do3: randomly select q from T with probability πT (q)4: select a random collision-free configuration qrand near q5: if ∆ finds a collision-free connection between q and qrand then6: V = V ∪ qrand

7: E = E ∪ {q,qrand}8: end if9: end for

10: return T

The standard form of the EST constructs a tree Tinit rooted at qinit by incrementally

adding branches to the tree in the form of nodes and connecting arcs. The tree

construction process begins by randomly selecting a node q from the tree. A random

sample qrand is then selected from the neighborhood of q and the local planner ∆ attempts

to connect the two configurations. If the connection is successful then qrand and the

connecting arc is added to the tree. This process, depicted in Figure 6-4 continues until a

termination criteria such as the maximum number of nodes in the tree or nodal proximity

to the goal is satisfied.

The most important aspect of the EST is the ability to direct the tree growth

according to a prescribed metric. Guided sampling of Qfree is achieved by assigning a

probability density function πT to the nodes of the tree. The probability density function

biases the selection of tree nodes from which to extend the tree. This bias enables tree

growth toward specified regions such as areas that are sparsely populated by nodes or the

goal location.

87

Figure 6-4. Expansive-Spaces Tree planning. The EST involves constructing a tree T byextending the tree from randomly sampled configuration in the tree q. Arandom sample qrand local to q is connected to the tree by a local planner.Both a successful q′rand and unsuccessful connection q′′rand are shown.

The last step of the EST planning process is to connect the tree to the goal location

pgoal. This connecting is accomplished by checking for connections with the local planner

∆. If no connection is made, further expansion of the tree is required. Another variation

involves simultaneously growing a tree Tgoal rooted at pgoal and proceeding with a

connection process between q ∈ Tinit and q ∈ Tgoal to merge the trees.

6.4.2 Rapidly-Exploring Random Tree Planner

The RRT planner, like the EST, was developed to efficiently cover the space between

qinit and qgoal. The RRT differs from the EST with respect to the tree expansion process

as described in Algorithm 5.

Instead of extending the tree from randomly selected points in the tree T , as in the

EST approach, the RRT extends the tree towards randomly selected points qrand in the

free configuration space Qfree. Tree construction begins by selecting qrand and finding the

nearest configuration qnear in T to the sample. The tree is extend from qnear a distance δ

toward qrand. The extension is performed by a local operator ∆ terminating at qnew. If the

constructed path is collision-free, it is added to T . Tree construction, depicted in Figure

88

Algorithm 5 Rapidly-Exploring Tree Construction

1: initialize tree T as a set of nodes V = q0 and edges E = ∅2: for i = 1 to n number of tree expansions do3: randomly select qrand from Qfree with probability distribution πQ4: assign qnear to be the closest neighbor q in T to qrand

5: find path P between qnear and qrand with ∆6: extract sub-path p along P terminating at qnew a distance δ from qnear

7: if p is collision-free then8: V = V ∪ qnew

9: E = E ∪ {qnear,qnew}10: end if11: end for12: return T

6-5, continues until a termination criteria such as the maximum number of nodes in the

tree or nodal proximity to the goal is satisfied.

Figure 6-5. Rapidly-Exploring Tree planning. The RRT involves constructing a tree T byextending the tree toward a randomly sampled configuration qrand in Qfree.The tree is extended a distance δ toward qrand terminating at qnew.

Guided sampling can be incorporated into the RRT in a similar fashion to the EST.

To guide sampling, qrand is sampled according to a probability distribution πQ. Therefore,

the main difference between the EST and RRT planners is the mechanism for guiding the

sampling. The EST guides the sampling by selecting attractive nodes of the tree to extend

while the RRT expands by selecting regions in the environment to move towards. Though

89

the expansion methods are similar, this difference can produce very different results and

computational performance.

Like the EST, the RRT planning process culminates in an attempt to connect

the current tree to the goal location pgoal. If the local planner ∆ cannot construct a

collision-free path to the goal from any configuration in the tree further tree expansion is

required.

This section has describe two strategies for efficient path planning in cluttered

environments. The next section describes the local planning function ∆ which connects

the nodes in the trees of both the EST and RRT.

6.5 The Dubins Paths

The sampling-based planners described in Section 6.4 rely on a local planner ∆

to connect vehicle configurations. The local planners are defined by vehicle kinematic

constraints so that the constructed path is kinematically feasible. For nonholonomic

vehicle motion planning, a popular local planning strategy is the Dubins optimal path

[34, 95, 96].

The Dubins optimal path is a shortest path solution for connecting two vehicle

configurations subject to a minimum curvature constraint. The solution pertains to a

car-like vehicle operating in a three-dimensional configuration space defining location

and orientation qi = [xi, yi, ψi]T ∈ R3. The curvature constraint relates to the minimum

turning radius of the vehicle ρmin.

The seminal work by Dubins [33] proved that the optimal curvature constrained path

consists of three path segments of sequence CCC or CSC where C is a circular arc of

radius ρmin and S is a straight line segment. Therefore, by enumerating all possibilities

the Dubins set is defined by six paths D = {LSL,RSR, RSL, LSR, RLR, LRL} where

L denotes a left turn and R denotes a right turn. The optimal Dubins path q∗(s) is the

shortest path q(s) ∈ D. For a mobile robot, the optimal Dubins path is the time-optimal

path for non-accelerating motion where ρmin is unique to the given constant velocity.

90

The Dubins set can be calculated analytically for any pair of configurations qinit

and qgoal. These general configurations are first transformed to the coordinate system

pictured in Figure 6-6. The transformed configurations have the form qinit = [0, 0, ψi]T and

qgoal = [d, 0, ψg]T where qinit is located at origin and qgoal is located on the y-axis.

Figure 6-6. The coordinate system for calculating the paths in the Dubins set D. Theinitial configuration is qinit is translated to the origin and rotated so that qgoal

is located on y-axis.

The Dubins paths are the concatenation of three canonical transformations scaled so

that ρmin = 1 and the vehicle moves with unit speed. This formulation allows the general

solution to be scaled by the vehicle’s ρmin because the maximum heading rate is equal to

Vρmin

. The transformations are written in terms the distance of motion v along the path

segments for an arbitrary configuration q = [x, y, ψ]T as follows

Lv(x, y, ψ) = [x + sin(ψ + v)− sin ψ, y − cos(ψ + v) + cos ψ, ψ + v]T

Rv(x, y, ψ) = [x− sin(ψ − v) + sin ψ, y + cos(ψ − v)− cos ψ, ψ − v]T

Sv(x, y, ψ) = [x + v cos ψ, y + v sin ψ, ψ]T

(6–10)

The total length of a Dubins path is the sum of the individual segment lengths

v1, v2, v3 that transform the initial configuration [0, 0, ψi]T to the final configuration

[d, 0, ψg]T ,

L = v1 + v2 + v3 (6–11)

91

The Dubins paths for the entire Dubins set are:

1. Dubins path LSL

v1 = −ψi + tan−1(

cos ψg−cos ψi

d+sin ψi−sin ψg

)

v2 =√

2 + d2 − 2 cos(ψi − ψg) + 2d(sin ψi − sin ψg)

v3 = ψg − tan−1(

cos ψg−cos ψi

d+sin ψi−sin ψg

)(6–12)

2. Dubins path RSR

v1 = ψi − tan−1(

cos ψi−cos ψg

d−sin ψi+sin ψg

)

v2 =√

2 + d2 − 2 cos(ψi − ψg) + 2d(sin ψg − sin ψi)

v3 = −ψg + tan−1(

cos ψi−cos ψg

d−sin ψi+sin ψg

)(6–13)

3. Dubins path LSR

v1 = −ψi + tan−1(− cos ψi−cos ψg

d+sin ψi+sin ψg

)− tan−1

(−2v2

)

v2 =√−2 + d2 + 2 cos(ψi − ψg) + 2d(sin ψi + sin ψg)

v3 = −ψg + tan−1(− cos ψi−cos ψg

d+sin ψi+sin ψg

)− tan−1

(−2v2

)(6–14)

4. Dubins path RSL

v1 = ψi − tan−1(

cos ψi+cos ψg

d−sin ψi−sin ψg

)+ tan−1

(2v2

)

v2 =√−2 + d2 + 2 cos(ψi − ψg) + 2d(sin ψi + sin ψg)

v3 = ψg − tan−1(

cos ψi+cos ψg

d−sin ψi−sin ψg

)+ tan−1

(2v2

)(6–15)

5. Dubins path RLR

v1 = ψi − tan−1(

cos ψi−cos ψg

d−sin ψi+sin ψg

)+ v2

2

v2 = − cos−1(

18(6− d2 + 2 cos(ψi − ψg) + 2d(sin ψi − sin ψg))

)

v3 = ψi − ψg − v1 + v2

(6–16)

6. Dubins path LRL

v1 = −ψi + tan−1(

cos ψg−cos ψi

d+sin ψi−sin ψg

)+ v2

2

v2 = − cos−1(

18(6− d2 + 2 cos(ψi − ψg) + 2d(sin ψg − sin ψi))

)

v3 = −ψi + ψg − v1 + v2

(6–17)

92

It should be noted that all segment lengths in these Dubins set calculations that

represent angular distance are constrained to be on the interval [0, 2π). An example of the

Dubins set is shown in Figure 6-7.

Figure 6-7. The Dubins set of paths for two configurations. Notice that the LRL pathdoes not give a feasible solution for this set. The time-optimal is denoted asq∗(s).

6.5.1 Extension to Three Dimensions

The Dubins set determines the shortest curvature-constrained path between two

configurations in a planar environment. Aircraft are not constrained to in-plane motion

so altitude variations must be included to properly characterize the vehicle kinematics.

A recent application of the Dubins set to three dimensions assumes a linear variation

of altitude limited by the maximum climb rate of the vehicle [97]. A method similar to

the MA (Section 6.2.2) plans an initial path which places the vehicle at a position and

orientation that forms a plane with the final position and orientation [98]. A 2D Dubins

path is then planned between the two in-plane configurations.

Because the dynamics of the vehicle will be incorporated after the path planning

task, the 3D Dubins solution employed in this dissertation assumes that changes in flight

93

path angle γ are instantaneous. This solution follows the assumption for 2D Dubins paths

that assume changes in heading rate are instantaneous. The flight path angle for each

individual segment of the Dubins path can vary so that the maximum flight path angle

γmax for curved and straight segments can differ. A smoothness constraint limits the

maximum change of fight path angle δγ,max between segments. Therefore, the altitude

variation for the 3D Dubins path is piecewise linear. A few example 3D Dubins paths are

presented in Figure 6-8.

A B

Figure 6-8. Examples of 3D Dubins paths. The paths originate from for the same startconfiguration and terminate at randomly generated goal configurations. A)Perspective view. B) Top-down view.

6.5.2 Collision Detection

6.5.2.1 Global path collision detection

The 3D Dubins path defines the local planner ∆ to be used for connecting aircraft

configurations. Each path must be checked for collisions before it can be added to the

tree T according to the sampling-based planning strategies (Section 6.4). The result of ∆

is a path composed of piecewise linear segments. The sensed environment is represented

by a set of bounded planes. Therefore, the collision detection algorithm must search for

collisions along the entire path by detecting intersections between linear path segments

and planes as depicted in Figure 6-9.

94

Figure 6-9. Collision detection for the global path. The collision detection algorithmsearches for any intersections between the piecewise linear paths of a treeplanner and the planar environment obstacles.

The first step of the collision detection algorithm is to search for intersections between

each piecewise linear segment of the path and all infinite planes in the current map. The

intersection point pint is calculated by

a = pi·n+Dv·n

pint = pi − av(6–18)

where pi is the first point of the path segment, v is the vector of the path segment

originating at pi, n is the normal direction of the plane, and D is the intercept of the

plane from the plane equation Ax + By + Cz + D = 0. If the scalar value a is on the

interval [0, 1] then an intersection exists along the finite path segment. Alternatively, if

the denominator v · n is zero the line segment is parallel to the plane. Therefore, the

path segment is parallel to the plane and may be contained within the plane. These cases

can be differentiated by checking if one of the end points satisfies the plane equation

Ax + By + Cz + D = 0.

Once the intersections between the path segments and the infinite planar obstacles

have been calculated a search for collisions can be completed. A collision exists only

if the intersection point for each segment-plane pair is contained within the specified

95

Figure 6-10. Interior to boundary test. A point that intersects with the infinite plane iswithin the boundary defined by the convex hull H if the sum of the interiorangles is equal to 2π.

plane boundary. Chapter 3 defined each planar boundary by a convex hull H =

{h1,h1, . . . ,hn} ∈ R3×n of n boundary points. Therefore, the test for determining if a

collision has occurred becomes determining if the intersection point pint is interior to the

three-dimensional boundary H. This determination is accomplished by inspecting the two

dimensional projections onto the planar landmark of pint and H denoted by pint ∈ R2 and

H ∈ R2×n. As shown in Figure 6-10, pint can be connected with every point in H. If the

sum of the angles between the connecting vectors is equal to 2π then the point is interior

to the boundary. This constraint can be written for n boundary points as

n∑i=1

cos−1 (hi − pint) · (hi+1 − pint)

|hi − pint||(hi+1 − pint)|= 2π (6–19)

where the value hn+1 is equal to h1.

6.5.2.2 Local path collision detection

The above formulation for collision checking is well-suited for the global planning

task. It is a computationally efficient solution that can quickly search a large number of

lengthy paths for collision with a large environment map. The method assumes that the

vehicle is a point and that there are no uncertainties in vehicle and obstacle location. The

local planning strategy, however, requires a more detailed approach to account for the

various uncertainties in the problem. The current map and relative vehicle position are

96

subject to uncertainty due to sensor noise, modeling errors, and external disturbances.

Additionally, the constructed kinematically feasible path is, in general, not dynamically

feasible due to instantaneous changes in heading rate and climb rate. Therefore, the

trajectory will not exactly follow the planned path. To account for these uncertainties, the

local path collision checking algorithm ensures safe navigation by increasing the required

distance between the vehicle and obstacles.

The local collision checking algorithm represents the path as a cylinder of radius R.

The radius of the cylinder is a function of the various uncertainties in the problem. The

radius must be as large as the greatest variance in the vehicle’s and obstacles’ inertial

location. Additionally, the radius must account for deviations of the trajectory from the

prescribed path which can be determined from simulation of the optimal control problem

presented in Section 6.7.

Therefore, the local collision checking algorithm searches for intersections between

the piecewise linear path segments represented by finite length cylinders and obstacles

represented by bounded planes (see Figure 6.5.2.2). This search is accomplished by first

finding the intersection point pint between the central axis of the cylinder and the obstacle

using Equation 6–18. The intersection between the cylinder and plane is an ellipse with

center pint. As shown in Figure 6.5.2.2, the major axis of the ellipse will be Rcos ϕ

where ϕ is

the angle between the plane’s normal vector and the cylinder’s axis. The minor axis of the

ellipse has a value R. The orientation of the ellipse is determined by the major axis which

is oriented in the plane defined by the plane’s normal vector and the cylinder’s axis. The

discretized ellipse can be tested for being interior to the plane boundary with Equation

6–19.

6.6 Environment Coverage Criteria

Sections 6.4 and 6.5.2 described a method for constructing collision-free paths in a

cluttered environment. This section defines a method for selecting paths with the goal of

constructing a complete environment map in minimum time. For this situation, optimality

97

A B

Figure 6-11. Collision detection for the local path. The local collision detection algorithmincorporates a safety region which encompasses the path segments radially.A) The path segment represented by a cylinder with radius R intersects anobstacle described as a plane with normal vector n. B) A two-dimensionalview rotated orthogonal to the plane containing the major axis of theintersecting ellipse.

cannot be proven because initially the environment is entirely unknown. Therefore, an

antagonistic example can be constructed for any planner to provide a non-optimal result.

The robotic coverage problem refers to optimal coverage of an environment with

known obstacles [99]. Solutions to the robotic coverage problem generally involve

decomposing the free space of the environment into connected regions. Each region of

the environment is sensed with a back and forth lawnmower, or boustrophedon, pattern as

shown in Figure 6.6. The boustrophedon path is optimal for a two-dimensional holonomic

vehicle under the assumption that passing through a small region of the environment is

equivalent to sensing the region. The space decomposition process reduces the planning

task to finding the optimal route for which to connect the individual paths.

When the environment is unknown and the planning task must be directed by

gathered sensor information, the problem is called sensor-based coverage or sensor-based

exploration. Sensor-based coverage tasks typically operate under several strict assumptions

including a two-dimensional environment, an omnidirectional range sensor, and a

98

A B

Figure 6-12. Motion planning for coverage strategies. Traditional motion planningstrategies for environmental coverage are not well suited for vehicles withdynamic constraints and directional sensors. A) Robotic coverage tasksassume the environment is known and decompose the free space intomanageable search spaces. B) Sensor-based coverage strategies often assumeomnidirectional sensors.

robot capable of nonholonomic motion [100, 101] (see Figure 6.6). Therefore, current

sensor-based coverage algorithms do address the problems inherent in mapping a

three-dimensional environment with a dynamically constrained vehicle equipped with

a directional senor.

The coverage methodology used in this dissertation affects both the global and local

planning strategies. The strategy is based on tracking all locations to which the vehicle

has traveled and the attitudes from which individual locations have been sensed. The

goal of the planner is maximize the variance of these individual quantities. Therefore,

the discrete vehicle locations and the vectors defining location and orientation of sensed

regions are tracked over time as shown in Figure 6-13. The vectors defining sensed region

are constructed from the attitude of the vehicle during the sensing operation and the

center of the field of view of the sensor.

The total variance of the vehicle position is calculated as the trace of the covariance

of the vehicle positions qpos as given by

σ2pos = tr(Σpos) (6–20)

99

Figure 6-13. Environmental coverage metric. Environment coverage is quantified by thevariance of the position of the vehicle and sensing locations over the entirepath. Sensing locations are defined by the vector originating from the end ofthe sensor field of view oriented back towards the sensor.

where Σpos is the sample covariance of the all vehicle position vectors. The same equation

can compute the variance of the the sensor pose vectors σ2sen given the covariance of the

sensor pose vectors Σsen.

The variance of the vehicle position σ2pos is used to determine the global location

by selecting the point in the environment that, when added to the current vehicle

location history, maximizes σ2pos. This calculation is computed on a coarse grid of the

vehicle environment for the purpose of computational efficiency. The global path planner

constructs a path to this location with a greedy search and a coarse discretization of the

path.

The local path is determined by maximizing the variance of the sensor pose vectors

σ2sen over all candidate paths. As opposed to the quick, greedy search used for global path

planning, the local sampling-based strategy generates a tree that more completely explores

the environment. When a specified number of candidate paths are found the path that

maximizes σ2sen is selected.

100

6.7 Optimal Path Tracking

The presented path planning strategy computes short detailed paths within the

sensor field of view. The local collision detection algorithm (Section 6.5.2.2) ensures that

the planned paths traverse the environment inside of a safe corridor. The optimal path

tracking strategy seeks to find a time optimal trajectory that follows the planned path

within a distance r < R subject to the vehicle dynamics. The constraint r is less than the

radius of the safety corridor because additional uncertainties incurred in the map building

process must be accounted for.

The dynamic vehicle model used for optimization is the navigation-level aircraft

model

V = T cos(α)− CDV 2 − sin γ

γ =(

TV

sin(α) + CLV)cos φ− cos γ

V

ψ =(

TV

sin(α) + CLV)

sin φcos γ

h = V sin γ

x = V cos γ cos ψ

y = V cos γ sin ψ

(6–21)

where V is total vehicle velocity, α is angle of attack, γ is flight path angle, φ is

bank angle, ψ is heading, T is thrust, CD is the drag coefficient, and CL is the lift

coefficient. The thrust and lift and drag coefficients are approximated by polynomial

fits parameterized by V and α

T = A0 + A1V + A2V2

CD = B0 + B1α + B2α2

CL = C0 + C1α + C2(α− α1)2

(6–22)

where the constant values are given by Bryson [39]. The model was selected because

aircraft lift and drag coefficients are incorporated into the formulation which provides a

realistic aircraft response. Additionally, lift and drag curves from UAV wind tunnel test

could be included into the trajectory tracking optimization. The model was augmented so

101

that the control inputs, α and φ, have second order and first order dynamics, respectively,

as given by

α = ω2n,α(uα − α)− 2ζαωn,αα

φ =uφ−φ

τφ

(6–23)

where ωn,α is the the natural frequency for the α response, ζα is the damping ratio

corresponding to α, τφ is the time constant for the φ response, and the new control inputs

to the system are uα and uφ.

The optimization problem is formulated as the following nonlinear program (NLP)

(u∗) = arg minu

J(u) = t2f +∑N

k=1 Ψ(xk,q(s))

s.t. : xk+1 = f(xk,uk)dt

xN = qf

Ψk < r ∀k = 1, . . . , N

(6–24)

where Ψ is a function that calculates distance between the discrete trajectory points xk

and the path q(s). The objective function J minimizes final time tf and the sum of the

distances between the path and the trajectory by varying the control input u = [uα, uφ]T .

The first constraint is an Euler integration step between two successive trajectory points

ensuring the trajectory is dynamically feasible. The second constraint forces the endpoints

of the trajectory and the path to be equal. The final constraint ensures each calculated

values of the error function Ψ are less than the safety radius r.

The NLP was solved with the nonlinear optimization routine SNOPT as a part of the

Matlab TOMLAB toolbox [102].

6.8 Trajectory Planning Algorithm

An overview of the trajectory planning approach is presented in Algorithm 6. The

presented formulation is executed until the change in sensor variance σ2sen falls below a

threshold tolsen. Therefore, the planning process stops when additional vehicle motions do

not provide additional environment coverage. Supplementary metrics such as a maximum

final time can easily be employed in the same framework.

102

Algorithm 6 Trajectory Planner for Environment Coverage1: repeat2: calculate covariance Σpos of all vehicle locations in the trajectory3: select the global goal location qG that maximizes σ2

pos

4: plan greedy RRT to qG using Algorithm 55: select the shortest distance path qG(s) from the global RRT6: select local goal location qL on qG(s) inside sensor field of view7: repeat8: extend exploratory RRT towards qL using Algorithm 59: until N desired candidate paths q(s) have been generated

10: calculate sensor covariance Σsen along each q(s)11: select q(s) that maximizes σ2

sen

12: convert local path to trajectory using Equation 6–2413: execute path to gather sensor data and build environment map14: until |σ2

sent + 1− σ2sent| < tolsen

The planning process begins with the determination of the vehicle’s global goal

location qG. The global goal is selected so that it’s addition to the vehicle position

history maximizes the variance of vehicle position σ2pos. This process greedily drives the

vehicle to locations in the environment that have not been explored. For example, under

this strategy a vehicle starting in one corner of a rectangular environment will select a

goal location in the opposite corner. Therefore, the vehicle will cross the environment

diagonally performing a greedy search.

An RRT is planned from the initial vehicle configuration qint to the goal location

qG. The global RRT is extended in a greedy fashion in order to rapidly generate a path

solution to the goal location. The tree expansion is made greedy by sampling qG with a

high probability. The fact that other randomly generated configurations are sampled for

connection to the tree differentiates the greedy RRT from an artificial potential function

planner. This differentiation is important because it prevents the planning strategy from

becoming mired in a local minimum. The solution path is denoted qG(s).

Next, the local goal configuration qL is selected. The configuration qL is found by

selecting the first point of intersection between the sensor field of view and qG(s). A

specified distance from the intersection point along qG(s) towards the interior of the field

103

of view is selected as qL. A local RRT is planned to this goal from the initial configuration

qint. Unlike the global RRT, the local RRT is performed in a less greedy fashion until N

number of candidate paths have been generated. The path which maximizes the senor

variance σ2sen is selected as the local path qL(s). The local RRT branching strategy is

biased towards regions that maximize variance. Therefore, the space within the sensor

field of view is efficiently searched for informative paths. The paths are restricted to the

field of view so that the detailed planning occurs within the region of lowest uncertainty.

Lastly, the local path path is converted to a trajectory by solving the NLP given in

Equation 6–24. The flexibility of the planning framework allows for the optimal control

formulation to be substituted with a MA implementation as described in 6.2.2. Both

methodologies would incorporate dynamics into the final solution. The final planned

trajectory is implemented and the planning process is repeated when the vehicle arrives at

qL.

This chapter has presented a framework that plans aircraft trajectories for complete

sensor coverage of an unknown, uncertain environment. Results of the planning strategy

are presented in Chapter 7.

104

CHAPTER 7DEMONSTRATIONS AND RESULTS

7.1 Introduction

This chapter presents results and analysis of the algorithms presented throughout this

research. Section 7.2 gives results of the plane fitting algorithm introduced in Chapter 3.

The plane fitting algorithm is employed in the SLAM algorithm (Chapter 5) to produce

the results in Section 7.3. Finally, results of the trajectory planning algorithm (Chapter 6)

are presented in Section 7.4.

7.2 Plane Fitting Results

The plane fitting algorithm introduced in Chapter 3 generates a low dimensional

environment representation of the pointwise sensor data provided by SFM (Chapter 2).

The quality of the fit is investigated with respect to feature point noise in Section 7.2.1

and the inclusion of nonplanar obstacles in Section 7.2.2. The simulation environment

employed for analyzing the plane fitting algorithm is shown in Figure 7-1.

Figure 7-1. Result of the plane fitting algorithm without image noise.

105

The simulation environment in Figure 7-1 consists of polyhedral obstacles viewed by

a camera at a given vantage point denoted by a vision frustum. The obstacles represent

an urban environment and the frustum indicates the field of view for a camera attached

to a UAV navigating the environment. Feature points are added to the obstacle facades

and checked for visibility according to a pinhole camera model. The visibility constraints

account for environmental occlusion, sensor field of view, and angle of incidence. The

angle of incidence constraint for a feature point is depicted in Figure 7-2 where feature

points with an angle of incidence, ϑ are deemed visible if 15◦ ≤ ϑ ≤ 75◦. Lastly, all visible

feature points are input into the plane fitting algorithm to generate the planar fit of the

pointwise data.

Figure 7-2. Angle of incidence constraint. The angle of incidence, ϑ, for feature point f isthe angle between the feature point’s position,~f , measured in the cameracoordinate frame and the normal direction, n, of the plane on which thefeature point is situated.

7.2.1 Effects of Feature Point Noise

Image noise is simulated by adding noise to the image plane positions of the tracked

feature points. This is accomplished by calculated the two-dimensional projections of

106

the feature points at two distinct vantage points. Noise is added to the two-dimensional

feature point locations in both images and noisy three dimensional points are calculated

by triangulation (Equation 2–14). Results of the plane fitting algorithm in the presence of

noise are shown in Figure 7-3.

A B

C D

Figure 7-3. Results of the plane fitting algorithm in the presence of image noise. A) Planefitting result with σ = 0.5 pixel image noise and a camera displacement of 25ft. B) Plane fitting result with σ = 1.0 pixel image noise and a cameradisplacement of 25 ft. C) Plane fitting result with σ = 0.5 pixel image noiseand a camera displacement of 15 ft. D) Plane fitting result with σ = 1.0 pixelimage noise and a camera displacement of 15 ft.

The plane fitting results, shown in Figure 7-3, were generated with Gaussian noise of

standard deviation σ = 0.5 and σ = 1.0 pixels applied to the images. Additionally, the

107

distance between the two vantage points was varied to values of 25 and 15 feet simulating

different values for camera translational motion.

The results show that noise does not affect the plane fitting algorithm for nearby

obstacles. This is a result of large apparent motion of the feature points associated

with nearby obstacles. The large displacement in the image plane of such feature points

produces more accurate results because the image noise is small in comparison to the

actual feature point displacement. The plane fitting algorithm, however, produces poorer

results for obstacles at a large distance, especially for shorter motions between image

vantage points. The effect of camera displacement can be seen by comparing Figures 7-3B

and 7-3D. These two results show that increasing the baseline between camera vantage

points produces more accurate triangulation results leading to a more complete planar

representation.

7.2.2 Nonplanar Obstacles

The plane fitting algorithm assumes that the environment is highly structured.

This is not entirely accurate for urban environments which often include non-structured

obstacles such as foliage. Figure 7-4 presents plane fitting results when non-structured

obstacles are included in the form of several tree-like objects. The additional feature

points generate spurious planar features. Such features are inaccurate representations

of the physical world and provide poor landmarks for the SLAM algorithm. Specifically,

the spurious planes are not necessarily observable from multiple vantage points. In order

to reduce the number of spurious planar obstacles in the map, the SLAM algorithm

only incorporates obstacles that have been re-observed a minimum number of times. A

minimum value of 3 re-observations was found to provide successful map results in the

presence of spurious planar features.

7.3 SLAM Results

Simulations of the SLAM formulation described in Chapter 5 are performed for

the three filters presented in this dissertation: the RBPF, URBPF, and SR-URBPF.

108

Figure 7-4. Result of the plane fitting algorithm when nonplanar obstacles are included.

The filters were compared by performing Monte Carlo (MC) simulations in which the

vehicle state measurement noise and sensor noise were randomly generated within the

confines of the covariance matrices Σq and Σr, respectively. This process tests the average

performance of the filters over many estimation processes.

7.3.1 Simulation Environment

The simulation environment, depicted in Figure 7-5A, consists of geometrically

regular obstacles indicative of an urban environment. The aircraft navigates along a

pre-computed trajectory generated by a high-fidelity nonlinear aircraft model. SFM,

as discussed in Chapter 2, is simulated by distributing three-dimensional feature points

on the building facades and ground. The feature points provide pointwise structure

information to the plane fitting algorithm. The plane fitting algorithm, introduced in

Chapter 3, generates observations as the vehicle navigates as shown in Figure 7-5B.

The vehicle state prediction occurs at 30 Hz. Though an IMU could provide a

faster state-estimation rate, it is assumed that the IMU is fused with image processing

109

A

B

Figure 7-5. The vision-based flight simulation environment for testing the SLAMalgorithm. A) The urban simulation environment is composed of geometricallyregular obstacles representing buildings. A precomputed trajectory generatedfrom a nonlinear aircraft model is shown circling the environment. B) As theaircraft navigates along the precomputed trajectory sensor information isgathered. The gathered sensor information is restricted to a senor cone definedby a maximum depth of sensing and horizontal and vertical fields of view.Three-dimensional feature points are distributed on the building faces andplanes are fit to the pointwise data.

110

information which is limited to 30 Hz. The observation updates in the SLAM process

occur at 3 Hz to allow for sufficient parallax for robust triangulation in creating

three-dimensional feature points. The three-dimensional feature points are generated

from the triangulation of two-dimensional image points, therefore, noise can be reduced

by allowing for a larger camera translation between calculations. If the translation

is too larger, however, two-dimensional feature points will leave the image plane and

no three-dimensional information will be calculated. This problem can be alleviated

by testing SFM algorithms on the actual flight data to determine the optimal rate

for triangulation or by instituting an adaptive step where triangulation occurs at the

maximum camera motion that retains the largest number of tracked feature points.

7.3.2 Localization Result

A localization result of the SLAM algorithm is shown in Figure 7-6. The result,

obtained with the URBPF approach, simulates a known environment by initializing the

landmarks to their actual locations. This is equivalent to using the SLAM framework

strictly for localizing the vehicle in the presence of noisy state measurements and noisy

sensor measurements. The noise values, defined in Equations 5–3 and 5–4, were given the

following standard deviations

σu = 5 ft/s

σv = 5 ft/s

σw = 5 ft/s

σp = 0.05 rad/s

σq = 0.05 rad/s

σr = 0.05 rad/s

σn1 = 0.1 ft

σn2 = 0.1 ft

σn3 = 0.1 ft

σd = 5 ft

(7–1)

The localization result(Figure 7-6) shows the filter is capable of producing very

good localization results in the presence of considerable noise. The estimated trajectory

given by the SLAM algorithm is defined by the particle with the largest weight and the

111

A B

C D

Figure 7-6. Example localization result for the airborne SLAM simulation. A) Thecomplete localization result with constructed map and estimated trajectory.B) The constructed map after a single loop through the environment. C) Thelocalization result without the constructed map. D) A top-down view of theactual vehicle trajectory, trajectory predicted from measured vehicle statesonly, and the trajectory estimated by the SLAM algorithm.

measured trajectory is calculated from the noisy state measurements without updates

from the vision sensor information. The estimated trajectory quickly converges to the

groundtruth trajectory as is considerably more accurate than the model based prediction

over the entire trajectory. This shows the versatility of the SLAM algorithm in that it can

be employed if the structure of the environment is known to provide accurate estimates of

vehicle location despite very noisy sensor measurements.

112

7.3.3 State Estimation Error

MC simulations were performed to analyze and compare the performance of the three

SLAM filters. Fifty MC runs were generated for each filter using the same environment

with randomly generated noise added to sensor and vehicle rate measurements. The noise

was generated according to the following standard deviations

σu = 0.5 ft/s

σv = 0.5 ft/s

σw = 0.5 ft/s

σp = 0.005 rad/s

σq = 0.005 rad/s

σr = 0.005 rad/s

σn1 = 0.1 ft

σn2 = 0.1 ft

σn3 = 0.1 ft

σd = 10 ft

(7–2)

Figure 7-7 shows the average position and orientation errors for the three SLAM

filters along with the average error of the measured trajectory. The measured trajectory

is the trajectory calculated purely from the aircraft kinematic model and velocity

measurements without updates from the environmental sensing. These graphs indicate

the accuracy of the position and attitude estimation. The results show that the SLAM

algorithms provide a significant decrease in estimation errors for aircraft position, roll

angle, and pitch angle. The heading angle estimates, however, begin to diverge halfway

through the simulation. The apparent sharp decreases in error for the measured trajectory

can be attributed the erratic behavior of the measured trajectory. The presented error

plots show the absolute value of the errors, therefore, when the error changes from positive

to negative it crosses the zero error axis and appears as a decrease in error. This sharp

decrease should not be mistaken for acceptable estimation performance.

7.3.4 Filter Consistency

Filter inconsistency refers to a the production of optimistic or conservative estimates

of estimation error. In other words, the actual variance of the estimates is significantly

larger or smaller than that predicted by the model. Filters that are inconsistent produce

113

0 10 20 30 40 50 60 70 80 900

2

4

6

8

10

12

time (s)

Nor

th e

rror

(ft)

RBPFURBPFSR−URBPFMeasured

A

0 10 20 30 40 50 60 70 80 900

1

2

3

4

5

6

7

time (s)

Eas

t err

or (

ft)

RBPFURBPFSR−URBPFMeasured

B

0 10 20 30 40 50 60 70 80 900

5

10

15

time (s)

Alti

tude

err

or (

ft)

RBPFURBPFSR−URBPFMeasured

C

0 10 20 30 40 50 60 70 80 900

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

time (s)

φ er

ror

(deg

)

RBPFURBPFSR−URBPFMeasured

D

0 10 20 30 40 50 60 70 80 900

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

time (s)

θ er

ror

(deg

)

RBPFURBPFSR−URBPFMeasured

E

0 10 20 30 40 50 60 70 80 900

0.2

0.4

0.6

0.8

1

1.2

1.4

time (s)

ψ e

rror

(de

g)

RBPFURBPFSR−URBPFMeasured

F

Figure 7-7. Average estimation error for the three SLAM filters and the nominaltrajectory for 50 Monte Carlo runs. A) Estimation error in the Northdirection. B) Estimation error in the East direction. C) Estimation error inaltitude. D) Estimation error in roll angle. E) Estimation error in pitch angle.F) Estimation error in heading angle.

114

poor state estimates without any internal knowledge that the estimates are erroneous.

A common method used for determining the consistency of a SLAM filter is called the

Normalized Estimation Error Squared (NEES)[79] calculated according to

ηk = (xk − xk|k)T (Pk|k)

−1(xk − xk|k) (7–3)

where xk is true vehicle state and xk|k is the vehicle state estimate. This metric relates the

actual variance of the estimation to the variance predicted by the filter and should have a

value equal to the dimension of the state vector. Therefore, the NEES is a measure of how

well the filter is predicting it own performance. If the filter has small covariance values

for the the states, but the actual errors are large, then the NEES value will be large and

the filter is optimistic. Conversely, if the state values are assigned a large covariance by

the filter, but the actual estimation errors are small, the NEES value will be small and the

filter is conservative. An optimistic filter is sensitive to noise and may diverge easily while

a conservative filter will produce poor estimates relative to the performance of a consistent

filter.

The NEES is calculated as an average over 50 MC runs in order to properly

characterized the SLAM filters. For a system with 50 Monte Carlo runs and a state

vector of dimension 6, the two-sided probability region for the NEES is [5.08, 7.00]. This

means that a conservative filter will have an NEES below 5.08 and an optimistic filter will

have NEES values above 7.00.

Ground vehicle simulations have shown that EKF SLAM and RBPF SLAM, though

shown to work in practice, are theoretically inconsistent filters [103, 104]. The consistency

of the filters, however, is an acceptable metric for assessing the robustness of the filter

because the length of time that a filter remains consistent relates to how well the filter

performs over long time periods. The results presented in Figure 7-8 show that the

URBPF provides the best results in terms of filter consistency. This is likely due to the

fact that the URBPF does not require linearization as does the RBPF. Also, it is believed

115

that the numerical approach to calculating the square-root of the covariances in the

SR-URBPF compromises the filter’s consistency. Additionally, is should be noted that

increasing the number of particles improves the consistency of the filters.

0 10 20 30 40 50 60 70 80 900

20

40

60

80

100

120

140

160

180

200

time (s)

NE

ES

avg

RBPFURBPFSR−URBPF

A

0 10 20 30 40 50 60 70 80 900

100

200

300

400

500

600

timeN

EE

Sav

g

100 particles1000 particles

B

Figure 7-8. Filter consistency results. A) Average NEES for all three SLAM filters over 50Monte Carlo runs. B) Average NEES for the URBPF filter for 50 Monte Carloruns with 100 and 1000 particles.

7.3.5 Filter Efficiency

The efficiency of the investigated filters is shown in Table 7-1. Since the filters

are implemented with non-optimized MATLAB code, the efficiency results have been

presented as computation time normalized to the RBPF result for N particles. The

comparison includes results for N , 2N , and 4N particles to compare the effects of

increased sampling on filter efficiency.

The efficiency results show that the RBPF filter is considerably faster than the other

two filters and that the filters scale linearly with the number of particles. The inefficiency

of the unscented filters is due to the sigma point propagation step in which 2L + 1 sigma

points must be transformed according to the system dynamics and sensor model. The

URBPF, however, is a more consistent filter meaning that it is more robust with respect

the length of the estimation time. Therefore, fewer particles could be used in the URBPF

to produce results comparable to the RBPF over the same time period. This decrease in

116

the number of particles would decrease the computational efficiency difference between the

URBPR and RBPF.

Table 7-1. Efficiency comparison of the SLAM filters.

Number of particles RBPF URBPF SR-URBPFN 1.00 2.06 1.602N 2.07 3.97 2.854N 3.98 7.63 5.64

7.3.6 Example SLAM Result

A sample result of the complete SLAM algorithm is shown in Figure 7-9 where the

estimated trajectory given by the SLAM algorithm is defined by the particle with the

largest weight at each time step. The error covariances for this solution are given in

Equation 7–2.

7.4 Trajectory Planning Results

This section presents results for the the trajectory planning algorithm introduced in

Chapter 6. The trajectories were planned within the simulation environment presented in

Section 7.3.1. The constructed maps used in the planning process are exact, representing a

SLAM solution with no error.

7.4.1 Environment Coverage

The trajectory planning process is decomposed into two phases denoted the global

and local planners. The global planner moves the vehicle to previously unexplored

regions of the environment and the local planner selects paths within the currently

visible environment that increase sensor coverage. In the complete planning process both

strategies are employed in tandem so that the global planner produces a global goal

location that performs a greedy search of the environment, and the local planner generates

detailed obstacle-paths that locally maximize sensor coverage.

The global planning process represents traditional planning strategies in that the

physics of the sensor are not considered. The global planner selects unexplored regions

of the environment in order to perform a greedy search under the assumption that once

117

A B

C D

Figure 7-9. An example result for the complete airborne SLAM solution using theURBPF. A) The complete SLAM result with constructed map and estimatedtrajectory. B) The constructed map after a single loop through theenvironment. C) The SLAM result without the constructed map. D) A topdown view of the actual vehicle trajectory, trajectory predicted from measuredvehicle states only, and the trajectory estimated by the SLAM algorithm.

a region has been reached it has been sensed. This, however, is not correct for a directed

sensor such as an onboard camera. In this case, direct paths through the environment will

not provide the needed information to build a complete map.

The local planning strategy incorporates the sensor physics by tracking vehicle

poses and selecting paths which maximize the variance in sensing vantages. Therefore,

paths are selected according to the amount of new information that will be provided by

traversing the path. The local planning process occurs within the vision cone defined by

118

the onboard camera in order to ensure that the planner incorporates the most recently

sensed environmental information. This ensures that the local path is collision free.

A B

C D

Figure 7-10. Example paths for environment coverage. A) Example result for the globalplanning strategy. B) Example result for the complete planning strategy. C)Top-down view of the global planning strategy result. D) Top-down view ofthe complete planning strategy result.

In order to quantify the benefit of the overall planning strategy, Figure 7-10 presents

trajectory results for cases when the planning process is restricted to the global planner

and when the complete planning strategy is employed. The first case, where just the

119

global planner is employed, represents traditional planning strategies that do not account

for a directed sensor. The second case is the complete planning strategy which accounts

for a directed sensor by tracking vehicle poses and selecting paths which maximize the

variance in sensing vantages.

For the example result, each case was run for an equal amount of time. Since the

global planner selects more direct paths through the map, its resultant path traverses a

larger portion of the environment. The path that results from including the local planning

strategy follows the same trend as the global path, but is more circuitous due to the goal

of maximizing the variance in sensor vantage. Both paths follow the same trend because

the first global goal location, the north-east corner of the environment, is identical in both

cases. The benefit of this methodology can be seen in Figure 7-11 which compares both

methods in terms of total obstacle surface area viewed. The result shows that adding

the local planning strategy increases environment coverage despite reducing the overall

distance traversed to the first global goal location.

0 5 10 15 20 250

1

2

3

4

5

6

7

8x 10

5

time(s)

sens

ed a

rea(

ft2 )

global strategycomplete strategy

Figure 7-11. Comparison of environment coverage results. The amount of obstacle surfacearea viewed during the transversal of paths given in Figure 7-10.

120

7.4.2 Optimal Control Results

The NLP presented in Chapter 6 was used to convert Dubins paths to aircraft

trajectories. The objective function of the NLP minimizes the distance between the

trajectory and the given paths. Therefore, the NLP defines a control strategy for optimal

path following. Results of the optimal control problem are presented in Figure 7-12.

0

50

100

150

200

−50

0

50

100

100120

North (ft)East (ft)

Alti

tude

(ft)

kinematic pathoptimal trajectory

A

0 50 100 150 200 250

−80

−60

−40

−20

0

20

40

60

80

100

North (ft)

E

ast (

ft)

kinematic pathoptimal trajectory

B

Figure 7-12. Resultant trajectories from the path tracking optimization. A) Results of theoptimal control strategy used to convert Dubins paths to aircraft trajectories.B) Top-down view of the optimal path following result.

The optimal control results are generated by supplying the Dubins path as the initial

condition to the optimization. The Dubins paths specify initial conditions for the states V ,

γ, ψ, x, y, h, and φ. The angle of attack, α, was calculated for each segment of the Dubins

path by calculating the trim angle of attack according to the vehicle’s equations of motion

(Equation 6–21).

The optimal control results show that the aircraft is capable of tracking the

Dubins paths in the lateral direction. The longitudinal directional tracking, however,

produces poorer results. It was found that the optimization did not always converge,

generating poor results for aggressive maneuvers. Additionally, the optimizations required

computation time on the order of 20 to 60 seconds. For these reasons, it is believed that

an MA strategy would provide a better solution to incorporating dynamics into the

planning strategy.

121

CHAPTER 8CONCLUSIONS

8.1 Conclusion

This research has presented a complete framework for solving the problem of

autonomous airborne mapping of unknown environments. The problem was decomposed

into two parts: map building in the presence of noisy measurements and uncertain vehicle

location and the planning of trajectories for minimum-time environment coverage.

The focus of the SLAM formulation is to generate consistent maps of the environment

in order to enable higher-level control decisions. Three filters were investigated in

terms of estimation performance, consistency, and efficiency. The results show that

the URBPF is superior to RBPF and SR-URBPF with respect to estimation performance

and consistency. The URBPF suffers due to its inefficiency in comparison to the other

filters. This problem can be alleviated by reducing the number of particles and reducing

the rate at which SLAM algorithm updates the aircraft’s INS estimates. Regardless, it

has been shown that it is possible to build an environment map from noisy sensor data

gathered by a vision-based UAV.

The trajectory planning portion of this dissertation showed that it is possible

to efficiently plan paths for sensor coverage of an unknown environment. The map

construction process was used as feedback for obstacle avoidance and the paths were

planned incrementally as knowledge of the environment increased. By definition, however,

optimality cannot be proven for this process since the structure of the environment is

unknown prior to the planning task. Presented results show that the trajectory planning

process planned local paths that increased the gained knowledge of the environment with

respect to a purely greedy exploration strategy. Additionally, it was found that an optimal

path following approach to incorporating dynamics is feasible, but can be computationally

inefficient. However, a MA approach could also be easily incorporated into the framework

to possibly produce more computationally efficient results.

122

8.2 Future Directions

The presented framework for map building and trajectory planning for environment

coverage shows that there are still many open problems to be solved before a UAV can

autonomously navigate within an urban environment. Suggested future directions include

investigations of the proposed architecture with respect to real aircraft state data and

gathered onboard imagery. Such investigations would involve gathering vision data and

groundtruth state information from a UAV navigating an urban environment. From a

real dataset the plane fitting and SLAM algorithms could be tested in a context more

accurately representing the actual operating conditions.

Further investigations into the trajectory planning algorithm should involve including

the MA framework into the planning task in order to directly incorporate dynamics into

the planning strategy. Additionally, the planning strategy should be applied to a variety

of three-dimensional environments in order to investigate the computational efficiency and

completeness of the solution, in terms of consistently providing collision-free paths.

123

REFERENCES

[1] Rudakevych, P. and Ciholas, M., “PackBot EOD Firing System,” Proceedings of theSPIE International Society for Optical Engineering, Vol. 5804, 2005, pp. 758-771.

[2] Cox, N., “The Mars Exploration Rovers: Hitting the Road on Mars,” 16th Inter-national Federation of Automatic Control (IFAC) World Congress, Prague, CzechRepublic, July 3-12, 2005.

[3] Forlizzi, J., and DiSalvo, C., “Service Robots in the Domestic Environment:A Study of the Roomba Vacuum in the Home,” Proceedings of the 1st ACMSIGCHI\SIGART Converence on Human-Robot Interaction, Salt Lake City, Utah,2006, pp. 258-265.

[4] Office of the Secretary of Defense, United States, Unmanned Aircraft SystemsRoadmap 2005-2030, August 2005.

[5] Kehoe, J., Causey, R., Abdulrahim, M., and Lind R., “Waypoint Navigation for aMicro Air Vehicle using Vision-Based Attitude Estimation,” Proceedings of the 2005AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, August2005.

[6] Frew, E., Xiao, X., Spry, S., McGee, T., Kim, Z., Tisdale, J., Sengupta, R., andHedrick, J., “Flight Demonstrations of Self-directed Collaborative Navigation ofSmall Unmanned Aircraft,” Invited paper, AIAA 3rd Unmanned Unlimited TechnicalConference, Workshop, & Exhibit, Chicago, IL, September 2004.

[7] Thrun, S., “Robotic Mapping: A Survey,” In G. Lakemeyer and B. Nebel editors,Exploring Artificial Intelligence in the New Millenium, Morgan Kaufmann, 2002.

[8] Smith, R., Self, M., and Cheeseman, P., “Estimating Uncertain Spatial Relationshipsin Robotics,” In I.J. Cox and G.T. Wilfong editors, Autonomous Robot Vehicles,pages 167-193, Springer-Verlag, 1990.

[9] Smith, R., and Cheeseman, P., “On the Representation and Estimation of SpatialUncertainty,” International Journal of Robotics Research, Vol.5, Issue 4, 1987, pp.56-68.

[10] Dissanayake, G., Newman, P., Clark, S., Durrant-Whyte, H., and Csorba, M., “ASolution to the Simultaneous Localization and Map Building (SLAM) Problem,”IEEE Transactions on Robotics and Automation, Vol. 17, No. 3, 2001.

[11] Tards, J. D., Neira, J., Newman, P. M., and Leonard, J. J., “Robust Mapping andLocalization in Indoor Environments using Sonar Data.,” The International Journalof Robotics Research, 2002.

[12] Neira, J., Ribeiro, I., and Tarods, J. D., “Mobile Robot Localization and MapBuilding using Monocular Vision,” International Symposium on Intelligent RoboticsSystems, Stockholm, Sweden, 1997.

124

[13] Hayet, J.B., Lerasle, F., and Devy, M., “A Visual Landmark Framework for IndoorMobile Robot Navigation,” IEEE International Conference on Robotics and Automa-tion, Washington, DC, May, 2002.

[14] Davison, A., “Real-Time Simultaneous Localization and Mapping with a SingleCamera,” IEEE International Conference on Computer Vision, Nice, France, 2003.

[15] Molton, N., Davison, A., and Reid I., “Locally Planar Patch Features for Real-TimeStructure from Motion,” British Machine Vision Conference, Kingston University,London, 2004.

[16] Davison, A., Cid, Y., and Kita, N., “Real-Time 3D SLAM with Wide-Angle Vision,”IFAC/EURON Symposium on Intelligent Autonomous Vehicles, Instituto SuperiorTecnico, Lisboa, Portugal, 2004.

[17] Davison, A., and Kita, N., “3D Simultaneous Localization and Map-Building UsingActive Vision for a Robot Moving on Undulating Terrain,” IEEE Computer SocietyConference on Computer Vision and Pattern Recognition, Kauai, Hawaii, 2001.

[18] Kim, J. H. and Sukkarieh, S., “Airborne Simultaneous Localisation and MapBuilding,” IEEE International Conference on Robotics and Automation, Taipei,Taiwan, 2003.

[19] Kim, J. and Sukkarieh, S., “Real-Time Implementation of Airborne Inertial-SLAM,”Robotics and Autonomous Systems, Issue 55, 62-71, 2007.

[20] Langelaan, J. and Rock, S., “Passive GPS-Fee Navigation for Small UAVs,” IEEEAerospace Conference, Big Sky, Montana, 2005.

[21] Langelaan, J. and Rock, S., “Navigation of Small UAVs Operating in Forests,”AIAA Guidance, Navigation, and Control Conference, Providence, RI, 2004.

[22] Langelaan, J. and Rock, S., “Towards Autonomous UAV Flight in Forests,” AIAAGuidance, Navigation, and Control Conference, San Francisco, CA, 2005.

[23] Montemerlo, M., Thrun, S., Koller, D., and Wegbrit, B., “FastSLAM: A FactoredSolution to the Simultaneous Localization and Mapping Problem,” Proceedings ofthe AAAI National Conference on Artificial Intelligence, 2002.

[24] Weingarten, J., and Sieqwart, R., “EKF-based 3D SLAM for StructuredEnvironment Reconstruction,” In Proceedings of IROS, Edmonton, Canada, August2-6, 2005.

[25] Brunskill, E., and Roy, N., “SLAM using Incremental Probabilistic PCA andDimensionality Reduction,” IEEE International Conference on Robotics andAutomation, April 2005.

125

[26] Choset, H., Lynch, K., Hutchinson, S., Kantor, G., Burgard, W., Kavarki, L., andThrun, S., “Principles of Robot Motion: Theory, Algorithms, and Implementations,”MIT Press, Cambridge, MA, 2005.

[27] Kavraki, L. E., Svestka, P., Latombe, J. C., and Overmars, M. H., “ProbabilisticRoadmaps for Path Planning in High-Dimensional Configuration Spaces,” IEEETransactions on Robotics and Automation, Vol. 12, No. 4, 1996, pp. 566-580.

[28] Hsu, D., “Randomized Single-Query Motion Planning in Expansive Spaces,” Ph.D.thesis, Department of Computer Science, Stanford University, 2000.

[29] Hsu, D., Latombe, J. C., and Motwani R., “Path Planning in ExpansiveConfiguration Spaces,” International Journal of Computational Geometry andApplications, Vol. 9, No. 4-5, 1998, pp. 495-512.

[30] Hsu, D., Kindel, R., Latombe, J. C., and Rock S., “Randomized KinodynamicMotion Planning with Moving Obstacles,” International Journal of Robotics Re-search, Vol. 21, No. 3, 2002, pp. 233-255.

[31] Kuffner, J. J. and LaValle, S. M., “RRT-Connect: An Efficient Approach toSingle-Query Path Planning,” In IEEE International Conference on Roboticsand Automation, 2000.

[32] LaValle, S. M. and Kuffner, J. J., “Randomized Kinodynamic Planning,” Interna-tional Journal of Robotics Research, Vol. 20, No. 5, 2001, pp. 378-400.

[33] Dubins, L. E., “On Curves of Minimal Length with a Constraint on AverageCurvature, and with Prescribed Initial and Terminal Positions and Tangents,”American Journal of Mathematics, Vol. 79, No. 3, July, 1957, pp. 497-516.

[34] Shkel, A. and Lumelsky, V., “Classification of the Dubins Set,” Robotics andAutonomous Systems, Vol. 34, 2001, pp. 179-202.

[35] Frazzoli, E., “Robust Hybrid Control of Autonomous Vehicle Motion Planning,”Ph.D. Thesis, MIT, June 2001.

[36] Frazzoli, E., Dahleh, M., and Feron, E., “Robust Hybrid Control for AutonomousVehicle Motion Planning,” Technical Report, LIDS-P-2468, 1999.

[37] Frazzoli, E., Dahleh, M., and Feron, E., “Maneuver-based Motion Planning forNonlinear Systems With Symmetries,” IEEE Transactions on Robotics, Vol. 21, No.6, December 2005.

[38] Schouwenaars, T., Mettler, B., Feron, E., and How, J., “Robust Motion PlanningUsing a Maneuver Automaton with Built-in Uncertainties,” Proceedings of the IEEEAmerican Control Conference, June 2003, pp. 2211-2216.

[39] Bryson, A. E., “Dynamic Optimzation,” Addison Wesley Longman, Inc., 1999.

126

[40] Pfeiffer, F. and Johanni, R., “A Concept for Manipulator Trajectory Planning,”IEEE Journal of Robotics and Automation, Vol. 3, No. 2, 1987, pp. 115-123.

[41] Slotine, J.-J. and Yang, S., “Improving the Efficiency of Time-OptimalPath-Following Algorithms,” IEEE Transactions on Robotics and Automation,Vol. 5, No. 1, 1989, pp. 118-124.

[42] DeSouza, G. and Kak, A., “Vision for Mobile Robot Navigation: A Survey,” IEEETransactions on Pattern Analysis and Machine Intelligence, Vol. 24, No. 2, February2002.

[43] Ma, Y., Soatto, S., Kosekca, J., and Sastry S., “An Invitation to 3-D Vision: FromImages to Geometric Models” Springer-Verlag New York, Inc. 2004.

[44] Oliensis, J., “A Critique of Structure-from-Motion Algorithms,” Computer Visionand Image Understanding, 80:172-214,2000.

[45] Tomasi, C. and Kanade, T., “Shape and Motion from Image Streams UnderOrthography,” International Journal of Computer Vision, Vol. 9, No. 2, 1992,pp. 137-154.

[46] Prazenica, R., Watkins, A., Kurdila, A., Ke, Q., and Kanade T., “Vision-BasedKalman Filtering for Aircraft State Estimation and Structure from Motion,”AIAA Guidance, Navigation, and Control Conference and Exhibit, San Francisco,California, August 2005.

[47] Longuet-Higgins, H.C., “A Computer Algorithm for Reconstructing a Scene fromTwo Projections,” Nature, Vol. 293, Sept. 1981, pp. 133-135.

[48] Hartley, R., “In Defense of the Eight-Point Algorithm,” IEEE Transactions onPattern Analysis and Machine Intelligence, Vol. 19, No. 6, June 1997.

[49] Broida, T. and Chellappa, R., “Estimation of Object Motion Parameters from NoisyImages,” IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 8,No. 1, Jan. 1986, pp. 90-99.

[50] Matthies, L., Szeliski, J., and Kanade, T., “Kalman Filter-based Algorithms forEstimating Depth from Image Sequences,” International Journal of ComputerVision, Vol. 3, 1989, pp. 209-236.

[51] Chuiso, A., Favaro, P., Jin, H., and Soatto S., “Structure from Motion CausallyIntegrated Over Time,” IEEE Transactions on Pattern Analysis and MachineIntelligence, Vol. 24, No. 4, April 2002.

[52] Thomas, J. and Oliensis J., “Dealing with Noise in Multiframe Structure fromMotion,” Computer Vision and Image Understanding, Vol. 76, No. 2, November1999, pp. 109-124.

127

[53] Webb, T., Prazenica, R., Kurdilla, A., and Lind, R., ”Vision-Based State Estimationfor Autonomous Micro Air Vehicles,” Proceedings of the AIAA Guidance, Naviga-tion, and Control Conference, AIAA-2004-5359, Providence, RI, August, 2004.

[54] Webb, T., Prazenica, R., Kurdilla, A., and Lind, R., ”Vision-Based State Estimationfor Autonomous Uninhabited Aerial Vehicles,” Proceedings of the AIAA Guidance,Navigation, and Control Conference, AIAA-2005-5869, San Francisco, CA, August,2005.

[55] Kehoe, J., Causey, R., Arvai, A., and Lind, R., “Partial Aircraft State Estimationfrom Optical Flow Using Non-Model-Based Optimization,” Proceedings of the 2006IEEE American Control Conference, Minneapolis, MN, June, 2006.

[56] Kehoe, J., Watkins, A., Causey, R., and Lind, R., “State Estimation using OpticalFlow from Parallax-Weighted Feature Tracking” Proceedings of the 2006 AIAAGuidance, Navigation, and Control Conference, Keystone, CO, August 2006.

[57] Iyer, R.V., He, Z., and Chandler, P.R., “On the Computation of the Ego-Motionand Distance to Obstacles for a Micro Air Vehicle,” Proceedings of the 2006 IEEEAmerican Control Conference, Minneapolis, MN, June, 2006.

[58] Latombe, J. C., “Robot Motion Planning,” Kluwer Academic Publishers, 1991.

[59] Mahon, I., and Williams, S., “Three-Dimensional Robotic Mapping,” Proceed-ings of the 2003 Australasian Conference on Robotics and Automation, Brisbane,Queensland, 2003.

[60] Thrun, S., Burgard, W., and Fox D., “A Real-Time Algorithm for Mobile RobotMapping With Applications to Multi-Robot and 3D Mapping,” In IEEE Interna-tional Conference on Robotics and Automation, 2000.

[61] Davison, A., and Murray, D., “Simultaneous Localization and Map-Building UsingActive Vision,” IEEE Transactions on Pattern Analysis and Machine Intelligence,Vol. 24, No. 7, 2002, pp. 865-880.

[62] Choset, H. and Nagatani, K., “Topological Simultaneous Localization and Mapping(SLAM): Toward Exact Localization Without Explicit Localization,” IEEE Transac-tions on Robotics and Automation, Vol. 17, No. 2, April 2001.

[63] Kuipers, B. and Byun, Y., “A Robot Exploration and Mapping Strategy Basedon Semantic Hierarchy of Spacial Representations,” Journal of Robotics and Au-tonomous Systems, 8:47-63, 1991.

[64] Thrun, S., Gutmann, J.-S., Fox, D., Burgard, W., and Kuipers, B., “IntegratingTopological and Metric Maps for Mobile Robot Navigation: A Stastical Approach,”In Proceedings of the National Conference on Artificial Intelligence (AAAI), 1998.

[65] Elfes, A., “Sonar-Based Real-World Mapping and Navigation,” IEEE Journal ofRobotics and Automation, Vol. RA-3, No. 3, June 1987.

128

[66] Elfes, A., “Using Occupancy Grids for Mobile Robot Perception and Navigation,”Computer, Vol. 22, Issue 6, 1989, pp. 46-57.

[67] Castelloanos, J., Tardos, J., and Schmidt, G., “Building a Global Map of theEnvironment of a Mobile Robot: The Importance of Correlations,” IEEE Interna-tional Conference on Robotics and Automation, Albuquerque, New Mexico, April,1997.

[68] Surmann, H., Nuchter, A., and Hertzberg, J., “An Autonomous Mobile Robotwith a 3D Laser Range Finder for 3D Exploration and Digitalization of IndoorEnvironments,” Robotics and Autonomous Systems, Vol. 45, 2003, pp. 181-198.

[69] Liu, Y., Emery, R., Chakrabarti, D., Burgard, W., and Thrun, S., “Using EMto Learn 3D models with mobile robots,” In Proceedings of the InternationalConference on Machine Learning, 2001.

[70] Martin, C., and Thrun S., “Online Acquisition of Compact Volumetric Maps withMobile Robots,” In IEEE International Conference on Robotics and Automation,Washington, DC, 2002.

[71] Jolliffe, I., “Principal Component Analysis,” Springer, ISBN 0387954422, 2002.

[72] MacQueen, J.B., “Some Methods for Classification and Analysis of MultivariateObservations,” Proceedings of the 5th Berkeley Symposium on Mathematical Statis-tics and Probability, 1967, pp. 281-297.

[73] Hammerly, G., and Elkan, C., “Learning the k in k-means,” Neural InformationProcessing Systems, 15, 2004.

[74] Stentz, A., “Map-Based Strategies for Robot Navigation in UnknownEnvironments,” Proceedings of AAAI Spring Symposium on Planning with In-complete Information for Robot Problems, 1996.

[75] Bailey, T. and Durrant-Whyte, H., “Simultaneous Localization and Mapping(SLAM): Part I - The Essential Algorithms,” Robotics and Automation Magazine,June, 2006.

[76] Guivant, J., and Mario, E., “Optimization of the Simultaneous Localization andMap-Building Algorithm for Real-Time Implementation,” IEEE Transactions onRobotics and Automation, Vol. 17, No. 3, June 2001.

[77] Williams, S., Newman, P., Dissanayake, G., and Durrant-Whyte H.F., “AutonomousUnderwater Simultaneous Localization and Map Building,” Proceedings of the IEEEInternational Conference on Robotics and Automation, San Francisco, CA, April,2000.

[78] Bailey, T. and Durrant-Whyte, H., “Simultaneous Localization and Mapping(SLAM): Part II - State of the Art,” Robotics and Automation Magazine,September,2006.

129

[79] Bar-Shalom, Y., Li, X.R., and Kirubarajan T., Estimation with Applications toTracking and Navigation, John Wiley and Sons, 2001.

[80] Kalman, R. E., “A New Approach to Linear Filtering and Prediction Problems,”Transactions of the ASME Journal of Basic Engineering, 1960, pp. 34-45.

[81] “Algorithms for Multiple Target Tracking,” American Scientist, Vol. 80, No. 2, 1992,pp. 128-141.

[82] Arulampalam, M. S., Maskell, S., Gordon, N., and Clapp, T., “A Tutorial onParticle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking,” Vol. 50, No.2, 2002, pp. 174-188.

[83] Julier, S. and Uhlmann, J., “A New Extension of the Kalman Filter to NonlinearSystems,” SPIE AeroSense Symposium, April 21-24, 1997.

[84] van der Merwe, R. and Wan., E., “The Square-Root Unscented Kalman Filter forState and Parameter Estimation,” International Conference on Acoustics, Speech,and Signal Processing, Salt Lake City, Utah, May, 2001.

[85] Liu, J and Chen, R., “Sequential Monte Carlo Methods for Dynamical Systems,”Journal of the American Statistical Association, Vol. 93, 1998, pp. 1032-1044.

[86] Murphy, K., “Bayesian Map Learning in Dynamic Environments,” In Advances inNeural Information Processing Systems, Vol. 12, 2000, pp. 1015-1021.

[87] Montemerlo, M., Thrun, S., Koller, D., and Wegbreit, B., “FastSLAM 2.0: AnImproved Particle Filtering Algorithm for Simultaneous Localization and Mappingthat Provably Converges,” International Joint Conference on Artificial Intelligence,Acapulco, Mexico, 2003.

[88] Li, M., Hong, B., Cai, Z., and Luo, R., “Novel Rao-Blackwellized Particle Filter forMobile Robot SLAM Using Monocular Vision,” Journal of Intelligent Technology,Vol. 1., No. 1., 2006.

[89] Richards, A. and How, J., “Aircraft Trajectory Planning With Collision AvoidanceUsing Mixed Integer Linear Programming,” Proceedings of the American ControlConference, Anchorage, AK, May 2002.

[90] Kuwata, Y. and How, J., “Three Dimensional Receding Horizon Control for UAVs,”AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence,Rhode Island, August 2004.

[91] Bellingham, J., Richards, A., and How, J., “Receding Horizon Control ofAutonomous Aerial Vehicles,” Proceedings of the American Control Conference,Anchorage, AK, May 2002.

130

[92] Henshaw, C., “A Unification of Artificial Potential Function Guidance and OptimalTrajectory Planning,” Advances in the Astronautical Sciences, Vol. 121, 2005, pp.219-233.

[93] Barraquand, B. and Latombe, J. C., “Nonholonomic Multibody Mobile Robots:Controllability and Motion Planning in the Presence of Obstacles,” Algorithmica,Vol. 10, 1993, pp. 121-155.

[94] Frazzoli, E., Dahleh, M., and Feron, E., “Real-Time Motion Planning for AgileAutonomous Vehicles,” AIAA Guidance, Navigation, and Control Conference andExhibit, Denver, CO, August 2000.

[95] Tang, Z. and Ozguner, U., “Motion Planning for Multitarget Surveillance withMobile Sensor Agents,” IEEE Transactions on Robotics, Vol. 21, No. 5, October2005.

[96] Laumond, J.-P., Jacobs, P., Taı,and Murray, M., “A Motion Planner forNonholonomic Mobile Robots,” IEEE Transactions on Robotics and Automation,Vol. 10, No. 5, October, 1944.

[97] Hwangbo, M., Kuffner, J., and Kanade, T., “Efficient Two-phase 3D MotionPlanning for Small Fixed-wing UAVs,” IEEE International Conference on Roboticsand Automation, Roma, Italy, April 2007.

[98] Shanmugavel, M., Tsourdos, A., Zbikowski, R., and White B.A., “3D Dubins SetsBased Coordinated Path Planning for Swarm UAVs,” AIAA Guidance, Navigation,and Control Conference and Exhibit, Keystone, Colorado, August 2006.

[99] Choset, H., “Coverage for Robotics - A Survey of Recent Results,” Annals ofMathematics and Artificial Intelligence, Vol. 31, No. 1-4, 2001, pp. 113-126.

[100] Choset, H., Walker, S., Eiamsa-Ard, K., and Burdick, J., “Sensor-Based Exploration:Incremental Construction of the Hierarchical Generalized Voronoi Graph,” TheInternational Journal of Robotics Research, Vol. 19, No. 2, February 2000, pp.126-148.

[101] Taylor, C. J. and Kriegman, D. J., “Vision-Based Motion Planning and ExplorationAlgorithms for Mobile Robots,” IEEE Transactions on Robotics and Automation,Vol. 14, No. 3, June 1998.

[102] Holmstrom, K., “TOMLAB - An Environment for Solving Optimization Problems inMATLAB,” Proceedings for the Nordic MATLAB Conference’97 Stockholm, Sweden,1997.

[103] Bailey, T., Neito J., and Nebot, E., “Consistency of the FastSLAM Algorithm,”IEEE International Conference on Robotics and Automation, 2006.

[104] Bailey, T., Neito J., and Nebot, E., “Consistency of the EKF-SLAM Algorithm,”IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006.

131

BIOGRAPHICAL SKETCH

Adam Scott Watkins was born in Orlando, Florida on November 11, 1980. After

graduating from Lake Mary High School, Adam attended the University of Florida

receiving a Bachelor of Science in Mechanical Engineering in 2003. Adam continued

his education at the University of Florida receiving a Master of Science in Mechanical

Engineering in 2005. Upon obtaining his MS, Adam pursued a doctoral degree under

the advisement of Dr. Richard Lind. Adam’s research involves the control of unmanned

systems and he is prepared to graduate with his Ph.D. in the summer of 2007. Upon

graduation Adam will begin work at the Johns Hopkins University Applied Physics Lab

continuing his research in the field of unmanned systems.

132