MULTI-VEHICLE COOPERATIVE CONTROL FOR...
Transcript of MULTI-VEHICLE COOPERATIVE CONTROL FOR...
MULTI-VEHICLE COOPERATIVE CONTROL FOR VISION-BASED ENVIRONMENTMAPPING
By
ERIC ALAN BRANCH
A THESIS PRESENTED TO THE GRADUATE SCHOOLOF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OFMASTER OF SCIENCE
UNIVERSITY OF FLORIDA
2007
1
c© 2007 Eric Alan Branch
2
To my family and friends,
whose patience and support made this possible
3
ACKNOWLEDGMENTS
This work was supported jointly by the Air Force Research Laboratory and the Air Force
Office of Scientific Research under F49620-03-1-0381 and F49620-03-1-0170.
I want to acknowledge and thank AFRL-MN and Eglin Air Force for the funding of this
project. I would also like to sincerely thank my advisor, Dr.Rick Lind, for his guidance and
support throughout my time at the University of Florida. Special thanks also to my supervisory
committee members, Dr. Warren Dixon and Dr. Carl Crane, for their time and consideration.
This work would not be possible without the members of the Flight Controls Lab; Adam
Watkins, Joe Kehoe, Ryan Causey, Daniel Grant and Mujahid Abdulrahim, who have always
been willing to share their knowledge and expertise. My parents, Mark and Peggy Branch, who
always encourage and support me in all my pursuits.
4
TABLE OF CONTENTS
page
ACKNOWLEDGMENTS. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
LIST OF FIGURES. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
CHAPTER
1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.1 Problem Statement. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.2 Motivation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.3 Multi-robot Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121.4 System Architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2 COMPUTER VISION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.2 Feature Point Detection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.3 Feature Point Tracking. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172.4 Two-view Image Geometry. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.4.1 Epipolar Constraint. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.4.2 Structure from Motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3 DIMENSIONALITY REDUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2 Environment Representation. . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.2.1 Spatial Decomposition/Occupancy Grid. . . . . . . . . . . . . . . . . . 243.2.2 Topological Representation. . . . . . . . . . . . . . . . . . . . . . . . . 253.2.3 Geometric Maps. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.3 Data Clustering. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3.1 K-means. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3.2 PCA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.3.3 Plane Fitting. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.4 Incremental Map Building . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4 MAPPING CRITERIA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.2 Coverage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.2.1 Random Movement Method. . . . . . . . . . . . . . . . . . . . . . . . . 304.2.2 Frontier-Based Exploration. . . . . . . . . . . . . . . . . . . . . . . . . 314.2.3 Seek-Open Space Algorithm. . . . . . . . . . . . . . . . . . . . . . . . 314.2.4 Mapping Criteria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
5
5 TRAJECTORY PLANNING . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345.2 Rapidly Exploring Random Tree. . . . . . . . . . . . . . . . . . . . . . . . . . 345.3 Sampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.4 Collision Avoidance. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
6 SIMULATION RESULTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
7 CONCLUSION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
7.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 437.2 Future Direction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
BIOGRAPHICAL SKETCH . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
6
LIST OF FIGURES
Figure page
1-1 Micro Air Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1-2 Urban Environment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1-3 Vehicle Architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1-4 Team Architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2-1 Mapping from Environment to Image Plane. . . . . . . . . . . . . . . . . . . . . . . 15
2-2 The Geometry of the Epipolar Constraint. . . . . . . . . . . . . . . . . . . . . . . . . 19
3-1 Environment Representations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3-2 Principle Component Analysis. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3-3 Incremental Plane Merging. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
5-1 Rapidly-Exploring Random Tree Expansion. . . . . . . . . . . . . . . . . . . . . . . 36
5-2 Rapidly-Exploring Random Tree w/ Kinematic Constraints . . . . . . . . . . . . . . . 37
5-3 Collision Check. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
6-1 Simulated Urban Environment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
6-2 Simulation at T=0s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
6-3 Simulation at T=80s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
6-4 Simulation at T=150s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
6-5 Simulation at T=150s Top View. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
7
Abstract of Thesis Presented to the Graduate Schoolof the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Master of Science
MULTI-VEHICLE COOPERATIVE CONTROL FOR VISION-BASED ENVIRONMENTMAPPING
By
Eric Alan Branch
August 2007
Chair: Richard C. Lind, Jr.Major: Aerospace Engineering
Flight within urban environments is currently achievable using small and agile aircraft;
however, the path planning for such vehicles is difficult dueto the unknown location of obstacles.
This thesis introduces a multi-aircraft formulation to themapping approach. The problem
addressed is the trajectory generation for the mapping of anunknown, urban environment by a
distributed team of vision-based Micro Air Vehicles (MAVs). Most mission scenarios require
a vehicle to know its position relative its surroundings in order to assure safe navigation and
successful mission planning. A vision sensor can be used to infer three-dimensional information
from a two-dimensional image for use in generating an environment map. Structure from Motion
(SFM) is employed to solve the problem of inferring three-dimensional information from two-
dimensional image data. No global information is availablein the unmapped regions so the map
must be constructed incrementally and safe trajectories must be planned on-line. A feature point
data set can be reduced to a set of geometric primitives, if the environment can be well estimated
by a geometric model. In the case of an urban environment the scene can be well represented by
planar primitives. The trajectories need to be planned to gather information required to construct
an optimal map. An efficient mapping can be accomplished by maximizing environment
coverage and vehicle distribution. The distribution and coverage criteria are governed by an
augmented cost function which takes into account the distance between mapped areas and all
other aircraft. Maximizing this cost function automatically distributes aircraft throughout a
region and disperses them around regions that remain to be mapped. Also, the mapping approach
8
uses maneuvering capabilities to determine the paths for each aircraft. This determination results
by utilizing branches from tree structures that represent achievable maneuvers. Such an approach
inherently alters the path between aircraft that may have different turn radius or climb rates.
9
CHAPTER 1INTRODUCTION
1.1 Problem Statement
This thesis introduces a multi-aircraft formulation to themapping approach. The problem
addressed is the trajectory generation for the mapping of anunknown, urban environment by a
distributed team of vision-based Micro Air Vehicles (MAVs). Most mission scenarios require
a vehicle to know its position relative its surroundings in order to assure safe navigation and
successful mission planning. A vision sensor can be used to infer three-dimensional information
from a two-dimensional image for use in generating an environment map. No global information
is available in the unmapped regions so the map must be constructed incrementally and safe
trajectories must be planned on line. The trajectories needto be planned to gather information
required to construct an optimal map. An efficient mapping can be accomplished by maximizing
environment coverage and vehicle distribution. The distribution and coverage criteria are
governed by an augmented cost function which takes into account the distance between mapped
areas and all other aircraft. Maximizing this cost functionautomatically distributes aircraft
throughout a region and disperses them around regions that remain to be mapped. Also, the
mapping approach uses maneuvering capabilities to determine the paths for each aircraft.
This determination results by utilizing branches from treestructures that represent achievable
maneuvers. Such an approach inherently alters the path between aircraft that may have different
turn radius or climb rates.
1.2 Motivation
The MAV has been developed to be a low-cost platform capable of completing complex,
low-altitude missions for both military and civilian applications. The long list of missions
envisioned for MAVs include military reconnaissance, surveillance and tracking of a specified
target, mapping an area affected by the dispersal of a chemical/biological agent, and visual
monitoring of traffic. A common aspect to these missions is that the vehicle must fly at low
altitudes and even inside rooms; and function in regions that are either beyond an operators
line-of-sight or deemed too hazardous for direct human involvement. Therefore, the ability
10
to exhibit a high level of autonomy, and situational awareness becomes a critical attribute for
ensuring the usefulness of MAVs [33]. An accurate mapping ofunknown environments is a
critical task for ensuring accurate situational awareness. The number of hazards at these flight
conditions is disproportionately large in comparison to flight at high altitudes. Information must
be gathered that indicates the location of objects in these areas along with their size and shape in
relation to the vehicles. It is reasonable to assume pre-flight knowledge of major objects within
cities or rooms; however, it is doubtful that every tree and sign or angle of every door is known
in advance. As such, in-flight mapping is a realistic expectation to obtain information about
the environment and permit flight operations. Figure1-1 shows several examples of current
generation MAVs.
Figure 1-1. Micro Air Vehicles
An approach was developed to optimize mapping using a singleaircraft. This approach
developed a metric that noted the distance between the aircraft and features that had already
been mapped. Path planning thus resulted by maximizing thatdistance. The motivation for this
thesis is to develop technologies that enable a distributedteam of MAVs to autonomously map
an unknown environment. The capability of cooperative map generation allows the vehicles
to accurately and efficient map the environment and share locally generated incremental maps
with the vehicle team which increases vehicle safety and enables efficient trajectory generation
beyond any individual vehicles line-of-sight. An accurateenvironment map is also a critical
component for future high level mission planning.
11
The mapping system proposed in this paper iteratively performs the three following
activities:
1. locally building a partial map that represents the portion of the environment current sensed
2. updating the global map according to the newly acquired partial map
3. determining and planning toward the next observation position, according to theexploration strategy
1.3 Multi-robot Systems
Multiple autonomous robot teams are suggested to have many advantages over single
vehicle systems. Cooperating robots have the potential to accomplish tasks more efficiently and
with greater speed than a single vehicle. Beyond gains in efficiency multiple vehicle systems
have the potential to demonstrate faster and more accurate localizations if they exchange
information about their position and sensed environment ina cooperative manner. Distributed
team systems also have the added advantage of increasing redundancy and sensor variety. Using
several low cost vehicles introduces redundancy and a higher level of risk reduction than having
only one, possibly more expensive, vehicle. Multi-robot systems have the following properties:
• greater efficiency• improved system performance• fault tolerance• lower economic cost• distributed sensing and action• a larger range of task domains
In the field of mobile robotics, research on multiple robot systems is gaining popularity
because the multiple robot approach provides distinct advantages over a single robot such as
scalability, reliability and speed. On the other hand it increases the complexity of the system over
that of a single vehicle system.
Unlike most cooperative mult-vehicle behavior exploration using multiple vehicles is
characterized by techniques that avoid tightly coordinated behavior. Most cooperative multiple
vehicle behavior relates to the close coordination of vehicles in a formation or a “follow the
leader” strategy. However exploration using multiple vehicles is characterized by a team
12
distribution strategy which efficiently covers an environment and avoids techniques which rely on
tightly coordinated behavior.
Figure 1-2. Urban Environment
1.4 System Architecture
This thesis is concerned with trajectory generation for vision-based mapping of an urban
environment by a distributed MAV team. The overall system architecture is made up of four sep-
arate problems: three-dimensional reconstruction from two-dimensional image data, environment
representation and incremental map building, explorationand mapping criteria, and trajectory
generation.
Chapter 2 describes the camera model and the approach for mapping a three-dimensional
object from a two-dimensional image, feature point detection , and feature point tracking. As
well as describe the implicit relationship between the camera and environment using two view
geometry and the epipolar constraint.
Chapter 3 describes methods for environment representation and dimensionality reduction.
A sensor based data set, in this case feature points, can be reduced to a set of geometric prim-
itives, if the environment can be well estimated by a geometric model. In the case of an urban
environment the scene can be well represented by planar primitives.
Chapter 4 describes the mapping criteria and team distribution strategy used to ensure
coverage of the environment. The search and exploration strategy uses a multivariate approach to
13
determine goal locations which have a higher probability ofcontaining environment features of
interest.
Chapter 5 describes the trajectory planning technique using a sample based motion plan-
ning algorithm, which is able to be executed as an on-line algorithm to produce a conditional
trajectory, which can replan as environment information increases.
Chapter 6 and 7 presents simulation results and conclusionson the algorithms presented.
Also describes future direction for research and development.
An overview of how the separate components are interconnected is graphically depicted
below in Figure1-3 and 1-4.
Figure 1-3. Vehicle Architecture
Figure 1-4. Team Architecture
14
CHAPTER 2COMPUTER VISION
2.1 Camera
A camera provides a mapping between the three-dimensional world and a two- dimensional
image. For this discussion we will consider a basic pinhole camera model, consisting of the
central projection of points in space onto an image plane. Inthis case the center of projection will
be the origin of a Euclidean coordinate system, with the image plane defined as a plane normal to
the camera’s central axis and located a focal length,f , away from the center of projection. The
pinhole camera geometry is described in Fig.2-1.
Camera Axis
Image Plane
ηx
P
ηy
ηz
Projection ofPi1
i3
i2
~η
f i3
I
µν
Figure 2-1. Mapping from Environment to Image Plane
Under the pinhole camera model a point in space with coordinatesη = [ηx,ηy,ηz]T
is mapped to the point on the image plane where a line joining the pointη to the center of
projection meets the image plane. By similar triangles the point (ηx,ηy,ηz)T is mapped to
the point(µ,ν)T . Equation2–1describes the central projection mapping from world to image
coordinates [4].
(ηx,ηy,ηz)T −→
(
fηx
ηz, f
ηy
ηz
)T
(2–1)
For a pinhole camera with the camera origin placed at the lensthe relative positions are
given by the following model.
15
µ= fηx
ηz(2–2)
ν = fηy
ηz(2–3)
The central projection can further be expressed in homogeneous coordinates as shown below
ηz
µ
ν
1
=
f 0 0 0
0 f 0 0
0 0 1 0
ηx
ηy
ηz
1
(2–4)
2.2 Feature Point Detection
The first step in the scene reconstruction process is the detection of features within the
viewable environment. The detection of feature points allows image pixels, features, to be
defined as geometric entities, such as points and lines. The feature point selection is based on
changes in image gradient from pixel to pixel over the entireimage, therefore selected feature
points are usually areas with unique characteristics. Features such as corners, edges, and lines are
characterized by large changes in color intensity and therefore allow for good feature selection,
capable of being easily tracked image to image [32]. For scene reconstruction the assumption
is that the selected feature points correspond to obstaclesin the environment which must be
detected for collision avoidance and mapping tasks. Objects of interest such as buildings,
bridges, and city signs have many sharp edges and abrupt color changes allowing for good
feature detection.
A commonly used feature point detection method is the gradient-based corner detection
algorithm. The algorithm requires that the image gradient∇I be calculated over the image frame.
Therefore, given an image gradientI(µ,ν) a corner feature can be detected at a given pixel(µ,ν)
by computing the summation of the outer product of the gradients,G(µ,ν), over a search window
of the sizeW(µ,ν).
16
G(x) = ∑x∈W(x)
∇I(x)∇IT(x) =
∑ I2µ ∑ IµIν
∑ IµIν ∑ I2ν
(2–5)
at each pixel in the windowW the matrixG is computed. If the smallest singular value of
G is found to be greater than a defined threshold,τ, then the pixel(µ,ν) is classified as a corner
feature [3] [12] [10].
2.3 Feature Point Tracking
In order to match individual feature points from image to image a feature tracking process
must be performed. The feature correspondence process relies on image intensity matching over
a defined pixel region. The Lucas-Kanade feature point tracker is a standard algorithm for small
baseline camera motion. The algorithm tracks feature points through the correspondence of
pixel groupings, a neighborhood of pixels is tracked between two images. Therefore, a feature
point can be associated with a feature point on a subsequent image by a translation in pixel
coordinates where the displacement is defined ash = [dx,dy]T . The solution forh is found
through the minimization of intensity error obtained by matching pixel windows of similar
intensities [21] [32].
Therefore, if one assumes a simple translational
h = argminh
∑x∈W(x)
‖I1(x− I2(x+δx)‖2 (2–6)
Applying the Taylor series expansion to this expression about the point of interest,x, while
retaining only the first term in the series results in Equation 2–7.
∂I∂µ
dµdt
+∂I∂ν
dνdt
+∂I∂t
= 0 (2–7)
Rewriting Equation2–7in matrix form results in Equation2–8.
∆ITu+ It = 0 (2–8)
17
whereu = [dµdt ,
dνdt ].
We can solve for the velocities if additional constraints are enforced on the problem,
which entails restraining regions to a local window that move at constant velocity. Upon these
assumption one can minimize the error function given in Equation 2–9.
E1(u) = ∑W(x)
[∇IT(x, t)u(x)+ It(x, t)]2 (2–9)
The minimum of this function is obtained by setting∇E1 = 0 and therefore, resulting in
Equation2–10.
∑ I2µ ∑ IµIν
∑ IµIν ∑ I2ν
u+
∑ IµIt
∑ IνTt
= 0 (2–10)
or, rewritten in matrix form results in the following
u = −G−1b (2–11)
where
G =
∑ I2µ ∑ IµIν
∑ IµIν ∑ I2ν
(2–12)
b =
∑ IµIt
∑ IνTt
(2–13)
2.4 Two-view Image Geometry
The two-view image geometry relates the measured image coordinates to the 3D scene.
The camera configuration could be either two images taken over time of the same scene, as in
the monocular case, or two cameras simultaneously capturing two images of the same scene,
as in the stereo vision case. The geometry of the two-view configuration will generate epipolar
constraint from which the 3D scene reconstruction will be formulated [22].
18
2.4.1 Epipolar Constraint
The relationship between camera and environment is formulated as the epipolar constraint
or coplanarity constraint. This constraint requires position vectors, which describe a feature point
relative to the camera at two instants in time, to be coplanarwith the translation vector and the
origins of the camera frames. This geometric relationship is illustrated in Figure2-2 whereη1
andη2 denote the position vectors of the feature point,P, in the camera reference frames. Also,
the values ofx1 andx2 represent the position vectors projected onto the focal plane whileT
indicates the translation vector of the origin of the cameraframes.
(R,T)
P
p1
e2
I1
l1
e1
p2
I2
l2
Figure 2-2. The Geometry of the Epipolar Constraint
A geometric relationship between the vectors in Figure2-2 is expressed by introducingR
as a rotation matrix. This rotation matrix is the transform of the camera position between image
frames. Assuming a pin-hole camera which is collinear with its projection into the focal plane,
the geometric relationship is described in Equation2–14.
19
x2 · (T ×Rx1) = 0 (2–14)
The expression in Equation2–14reflect that the scalar triple product of three coplanar
vectors is zero, which forms a plane in space. These relationships can be expanded using linear
algebra to generate a standard form of the epipolar geometryas in Equation2–15. This new
form indicates a relationship between the rotation and translation, written as the essential matrix
denoted asE, to the intrinsic parameters of the camera and associated feature points.
[x2]E [x1] = 0 (2–15)
2.4.2 Structure from Motion
Structure from motion (SFM) is a technique to estimate the location of environmental
features in 3D space [26]. This technique utilizes the epipolar geometry in Figure2-2 and
assumes that the rotation,R, and translation,T, between frames is known. Given that, the
coordinates ofη1 andη2 can be computed, by the fundamental relationship given in Equa-
tion 2–16[21] [20] [27].
η2 = Rη1+T (2–16)
The location of environmental features is obtained by first noting the relationships be-
tween feature points and image coordinates given in Equation 2–2and Equation2–3. These
relationships allow some components ofηx andηy to be written in terms ofµ andν which are
known from the images. Thus, the only unknowns are the depth components,η1,z andη2,z, for
each image. The resulting system can be cast as Equation2–17and solved using a least-squares
approach.
µ2f −(R11
µ1f +R12
ν1f +R13)
ν2f −(R21
µ1f +R22
ν1f +R23)
1 −(R31µ1f +R32
ν1f +R33)
η2,z
η1,z
=
Tx
Ty
Tz
(2–17)
20
This equation can be written in a compact form as shown in Equation 2–18usingz =
[η2,z,η1,z] as the desired vector of depths.
Az = T (2–18)
The least-squares solution to Equation2–18obtains the depth estimates of a feature point
relative to both camera frames. This information along withthe image plane coordinates can be
used to compute(η1,x,η1,y) and(η2,x,η2,y) by substituting these values back into Equations2–2
and2–3. The resulting components ofη1 can then be converted to the coordinate frame of the
second image and it should exactly matchη2. These values will never match perfectly due
to noise and unknown camera parameters so, in practice, an average process is often used to
estimate the feature coordinates [1] [13] [34].
21
CHAPTER 3DIMENSIONALITY REDUCTION
3.1 Introduction
For autonomous MAVs the ability to create maps of the environment autonomously is the
first requirement for the planning and completion of many tasks. It cannot be assumed a priori
environment information will be available for navigation and mission planning, in fact it is rarely
the case. Not only do city blueprints or related types of mapsfail to be consistently reliable but,
numerous aspects of an environment are not likely to appear on a map such as signs, billboards
and semi-stationary objects (i.e., stationary vehicles).Maps made for people often depend upon
the interpretive skills of the person using the map and on hisor her ability to infer functional
information. Maps usually represent structural elements with abstract symbols and semantic
labels. A MAV must be able to relate its current location directly to its own sensory information
regarding its environment. This suggests that an appropriate map for an autonomous robot should
relate to the types of sensor data the robot is likely to observe. In general, these factors imply that
the ability to perform some degree of autonomous map construction, update, and validation is the
primary requirement for successful mission planning [29] [5].
Maps can take many forms and fall between two specific representational extremes, which
are particularly relevant to autonomous vehicles. The firstare metric maps, which are based on
an absolute reference frame and location estimates of whereobjects are in space. The second
are topological maps, or relational maps, that only explicitly represent connectivity information
between important objects or landmarks. Each of these representation extremes allow for various
specific descriptions of an environment [23] [30].
The amount of information contained in a representation andthe assumptions made about
the environment leads to layering of representations of mapdata with increasingly strong
assumptions and a narrowing of suitable mission tasks. Below is a list of map structures in order
implied model assumptions.
22
1. Sensorial. Raw sensor data (i.e., raw feature point data)
2. Spatial Decomposition/Occupancy Grid. Discretely sampled environment, definingprobability of occupancy
3. Geometric. 2- or 3-Dimensional objects inferred from sensor data
4. Topological. The large-scale rational links that connect objects and locations across theenvironment as a whole
5. Semantic. Function labels associated with the landmarksof the map
Geometric maps are generally the most intuitive at first glance and give a more general
representation of the environment. Because the obtained map is more general in nature it is well
suited for a wide variety of mission tasks. While a topological mapping is more closely related
to an individual mission requirement, and thus less easily adapted to diverse mission tasks.
Sensorial mappings represent minimally altered sensor data, however the large storage size and
difficulty inferring information makes them undesirable for some high level mission planning.
Not only are existing maps of most environments inaccurate,but almost every environment
occupied by humans undergoes change. As a result robot systems must be able to accommodate
change in their environment. If they are to maintain maps, they must be able to update them. The
general approach of map building is to incrementally integrate new data into the map. When each
new frame is obtained, it is aligned to a cumulative global map.
The exploration of an unknown environment, the construction of a geometric map from
that exploration, and other related problems have been studied extensively in the field of
computational geometry. Exploration relates to a variety of related capabilities. These include
searching for a specific objective or goal position, searching for a route with specific properties,
covering free space, and learning about the occupancy space(i.e., mapping). A broad class of
problems deal with search in unknown or partially known environments. Such problems are
interesting not only in their own right but also because theyare closely related to environmental
exploration in general where the goal being sought is additional geometric knowledge.
23
3.2 Environment Representation
3.2.1 Spatial Decomposition/Occupancy Grid
One of the most straightforward and widely used environmentrepresentations for robotic
mapping are grid structures. Grid structures or occupancy grids are spatial decompositions of
the environment, where the two- or higher-dimensional environment is discretely sampled. The
simplest method used to sample the environment is to sample the space using a uniform grid,
with each sample taken at points in the grid expressing the probability of occupancy at that
sample point. At each sampled point the space is therefore defined as empty, full, or partially
full. Because this method of environment representation only describes the space sampled and
not the individual object contained within that space, no information is available to identify or
discriminate between objects. Other disadvantages are that the grid resolution is limited by how
course or fine the environment is discretized and storage requirement is the same whether the
environment is empty or occupied. The main advantage of a uniform grid representation is its
extreme generality, requiring no assumptions be made regarding the type of objects contained
within the space. The 2-dimensional occupancy map can be extended to represent 3-dimensional
spaces through use of a volumetric model. The volumetric model defines the environment by
defining a 3-dimensional grid which divides the workspace into voxels (cubes) with edges and
volume [24]. Furthermore, more efficient sampling can be obtained by using an adaptive cell size
to more efficiently model empty and occupied space, by using areas of high resolution for areas
of cluttered features and low resolution in open space [8] [15] [28].
A B C
Figure 3-1. Environment Representations A) Occupancy GridB) Topological Map C) GeometricMap
24
3.2.2 Topological Representation
A topological map is a graph like representation, which usesnodes and edges to describe
distinct features of an environment. Nodes represent the key-places in the map and edges repre-
sent the transitions to navigate between key-places [29]. In contrast to metric maps, topological
maps naturally capture qualitative information while deemphasizing what may be irrelevant
or confusing details. The key to a topological relationshipis some explicit representation of
connectivity between regions or objects. The vehicle has noreal understanding of the geometric
relationship between locations in the environment; locations are only linked by their topological
representation. As a result, topological maps have a very explicit connection to mission tasks and
problem statement. A typical subway map, or the typical navigation instructions used by humans
over substantial regions of space are examples of topological representations [6].
3.2.3 Geometric Maps
Geometric maps are those made up of discrete geometric primitives; for example lines,
polygons and/or polyhedral. Such maps have the advantage ofbeing highly space-efficient
because an arbitrarily large region of space can be represented by a model with only a few
parameters. In addition, geometric maps can store occupancy data with almost arbitrarily high
resolution without becoming liable to the storage limitations incurred by techniques based on
spatial sampling.
The primary shortcoming of geometric model-based representations relates to the fact that
they can be difficult to infer reliably from sensor data. Three fundamental modeling problems
are regularly encountered: the representation may change drastically depending on the modeling
assumptions; many different environments may map to the same representation, building will
have no discerning characteristics; and it may be difficult to represent prominent features of the
environment within the modeling system.
Geometric maps however can be an efficient description of theenvironment, if one assumes
that the sensor provides suitable data, and that the environment to be described is well suited
25
to the geometric modeling primitives to be used. In this casethe environment, an urban city, is
characterized by large planar features which lends itself to a planar modeling primitive.
The world,W, will be defined as a 3-dimensional space in whichW ∈ R3. The described
world will consist of two objects; obstacles and vehicles. Obstacles are considered the portions
of the world that are permanently occupied, for example, as in walls of a building. Vehicles are
bodies that are modeled geometrically and controllable viaa motion planning step. In this case
vehicles will be thought off as a point in space with no geometric size. This assumption is do
to the fact that the characteristic size of a MAV is very smallin relation to the defined world.
Therefore collision avoidance between vehicles and obstacles is assured solely by checking the
vehicle path for conflict.
The following sections present a method for systematicallyconstructing representations for
obstacles and vehicles using a collection of primitives [2][22]. Obstacle regions will be denoted
asO, a set of all points inW that lie in one or more obstacles;O ∈ W. Three-dimensional
obstacles will be represented using a boundary representation consisting of convex polygons
initialized in 3-dimensional space. Ann-sided polygon can be described using two kinds of
features, vertices and edges. Every vertex corresponds to acorner of the polygon. and every
edge corresponds to a line segment between a pair of vertices. The polygon can be specified by a
sequence,(x1,y1),(x2,y2), ...,(xn,yn) of n points in the plane defined byP.
3.3 Data Clustering
3.3.1 K-means
K-means is an unsupervised learning algorithm which solvesthe clustering problem. The
K-means algorithm allows for a way to split a given data set ofN points intoK subsets so as to
minimize the sum-of-squares criteria [11].
wherexn is a vector representing thenth data point andµ is the geometric centroid of the
data subset.
J =K
∑j=1
N
∑i=1
∥
∥
∥x( j)
i −µj
∥
∥
∥
2(3–1)
26
The algorithm works by initializingK centroids into the space represented by the data set
being clustered. Once the centroids are initially set each data point is assigned to the subgroup
with the closest centroid. Once all data points are assignedthe centroids are recalculated for the
currently defined data sets. The above procedure is repeateduntil the centroids no long move or
the change in centroid position falls below some user definedthreshold.
The K-means algorithm suffers from several sort comings. One is the algorithm is sensitive
to the initial randomly selected centroid locations. Also the number of centroids must be selected
a priori. The algorithm does not guarantee a globally optimal solution ofJ over the centroid
assignments.
3.3.2 PCA
Principal Component Analysis (PCA) is a way to identify patterns in a data set and express
the data set in such a way as to highlight the similarities anddifferences in the data. PCA can be
used to simplify a dataset by reducing the dimensionality while retaining those characteristics of
the dataset that contribute most to the variation. PCA is an orthogonal linear transformation that
transforms the data to a new coordinate system such that the first principle component accounts
for as much of the variability in the data as possible, with each succeeding component accounting
for a majority of the remaining variation. The primary objectives of PCA are to discover or to
reduce the dimensionality of a dataset and to identify new meaningful underlying variables. In
PCA such directions are merely the eigenvectors of the covariance matrix having the largest
eigenvalue; this projection optimizes a sum-squared-error criterion [7] [31] [35].
First, thed-dimensional mean vectorµ anddxd covariance matrixΣ are computed for
the full data set. Next, the eigenvectors and eigenvalues are computed, and sorted according to
decreasing eigenvalue. Choosing thek eigenvectors having the largest eigenvalues. Often there
will be just a few large eigenvalues, and this implies thatk is the inherent dimensionality of the
subspace governing the data while the remaining dimensionsgenerally contain noise. Next we
form adxk matrixA whose columns consist of thek eigenvectors. The representation of the
27
Figure 3-2. Principle Component Analysis
data by principal components consists of projecting the data onto thek-dimensional subspace
according to
3.3.3 Plane Fitting
The plane fitting procedure works by clustering the data set using the k-means algorithm
and checking the planarity of the newly defined clusters using the PCA algorithm in an iterative
approach. First the number of clusters,k, is found by selectively splitting and removing centroids
based on the planarity of the data set they represent. Once a data subset can be accurately
modeled by a plane, then that cluster is removed from the dataset. If no clusters are found to fit
the planar model then the centroid is split and the k-means clustering algorithm is iterated over
the newly defined centroids. The planarity check is based on the fact that for a data set where
all data points lie with in a plane the principal components computed by PCA will give two
eigenvectors which lie in the plane and a third that is the normal vector of the plane. Therefore,
the decision to remove a data set associated with a centroid is determined by projecting the data
set onto the calculated eigenvectors (principal components). The data which after projection fall
within a threshold distance from the origin along the normalof the defined plane are determined
to approximately lie on the plane.
28
3.4 Incremental Map Building
The general approach of map building is to incrementally integrate new data into the map.
When each new frame is obtained, it is aligned to a cumulativeglobal map.
In the case of multiple vehicles, each vehicle’s local incremental map must be integrated
into the global map for use in future path planning by not onlyitself but all other vehicles in the
distributed team. Each vehicle continuously performs the scene reconstruction and plane fitting
process over the locally sensed feature point data. By completing the dimensionality reduction
process on each local data set the incremental global map building process is reduced to a plane
merging task. By performing the dimensionality reduction on the incremental local data set the
storage requirement for each vehicle is reduced and the dataset transferred for inclusion in the
global map is reduced. The plane fitting procedure is therefore completed on a smaller data set
allowing for more computationally efficient data clustering. The global map building procedure
therefore becomes a merging process, where the individual local low dimensional maps are
incorporated into the central global map.
Although communication restrictions are not explicitly considered in the problem statement,
by sharing a reduced order data set with the centralized global map the required communication
bandwidth would be reduced to a more realistic level from therequirement necessary to share the
unaltered sensor data.
0
2000
0
20000
1000
x (ft)
y (ft)
altit
ude
(ft)
A
0
2000
0
20000
1000
x (ft)
y (ft)
altit
ude
(ft)
B
Figure 3-3. Incremental Plane Merging A) Local Planes B) Merged Planes
29
CHAPTER 4MAPPING CRITERIA
4.1 Introduction
The primary problem in any exploration task is that given what you know about the
environment where should you move to gain as much new information as possible or ensure a
high probability of learning new information in unexploredregions. The choice of movement
algorithms or mapping criteria will affect the success and efficiency of a vehicle team attempting
to disperse themselves in an unknown environment. This section will develop a mapping criteria
which simultaneously distributes a MAV team and ensure exploration of unexplored regions of
the environment. Many methods are available to formulate movement algorithms and define goal
locations for autonomous exploration. Random explorationstrategies are easily implemented,
however they do not make use of current map knowledge for planning. Another method used
often in indoor cyclic environments is Frontier-based exploration. Frontier-based exploration
works on the assumption that new information can be gained byexploring the boundary between
open space and unexplored territory. Another method is the ’Seek-Open Space’ method which
attempts to simultaneously distribute and cover an environment with mobile robots. The ’Seek-
Open Space’ method is formulated for mobile robots in a 2-dimensional space, however the basic
idea can be used and expanded for 3-dimensional environmentwith a MAV.
The search and exploration strategy proposed here is based on selecting goal locations
which offer the highest probability of finding new obstacles. The strategy is based on multivariate
exploration and decision methods, which classifies a goal location which is both statistically
distant from known obstacles and other team MAVs.
The coverage problem has been defined as the maximization of the total obstacle surface
area covered by the MAV vision sensor.
4.2 Coverage
4.2.1 Random Movement Method
Random movement algorithms are the most basic exploration algorithms. The random
movement algorithm works by commanding the search vehicle to move forward with small
30
random control inputs at random time intervals. The overallvehicle path exhibits a random
’wondering’ movement with a continuously curved path. To ensure forward movement and avoid
repeated circling, the turn command and the time interval are constrained to promote exploration.
Another random movement strategy works by planning straight line paths between the
vehicle position and a randomly generated location, contained inside the search area. This
method leads to complete coverage, however the search effort leads to excessive coverage of the
central area at the expense of the boundary area.
Random exploration strategies are easily implemented and computationally inexpensive.
However they do not make use of current map knowledge for planning and do not inherently
provide a method for coordinated team distribution.
4.2.2 Frontier-Based Exploration
Frontier-based exploration works on the assumption that new information can be gained by
exploring the boundary between open space and unexplored territory. Frontier-based exploration
has been shown to work well for 2-dimensional mobile robot mapping, where the primary
goal is to map the boundaries of an environment, for example the outer walls of a room. The
frontier-based approach incrementally constructs a global occupancy map of the environment.
The map is analyzed to locate the ’frontiers’ between the free and unknown space. Exploration
proceeds in the direction of the closest ’frontier’. The frontier method is proven to work well for
indoor cyclic environment where a boundary wall can be continuously explored, providing new
environment information. The frontier approach does not explicitly provide for team distribution.
Frontier based exploration also is formulated around an occupancy grid representation which will
have the same limitations detailed in Chapter3.
4.2.3 Seek-Open Space Algorithm
The seek-open space movement algorithm causes a robot vehicle to move toward open areas
in the map which are distant from known obstacles. The algorithm is executed by first calculating
the average obstacle vector for all obstacles in sensor range. The average obstacle vector is then
computed for all obstacles sensed. The obstacle vector is defined such that the magnitude of the
31
vector must be large for objects close to the vehicle and small for objects far away. After the
average obstacle vector is computed, the goal of the vehiclebecomes to move in the opposite
direction of the average obstacle vector. The vehicle is given a control input which turns the
vehicle toward the direction of the negative obstacle vector. The rate of turn is determined by the
magnitude of the average obstacle vector. The Seek-Open Space algorithm has the advantage
that team distribution can be explicitly accounted for by modeling each vehicle as an obstacle.
Therefore, the vehicle team will explore the environment while avoiding regions with both team
vehicles and obstacles present.
4.2.4 Mapping Criteria
As with the Seek-Open Space strategy the search and exploration strategy proposed here
is based on selecting a goal location which offers highest probability of finding new obstacles,
which are assumed to be in areas which are distant from known obstacles. The strategy is based
on multivariate exploration and decision methods [25].
The presented approach enables coverage of the environmentby selecting individual vehicle
goal locations that are statistically distant from measured landmarks and other vehicle locations.
The distance measure used to determine the goal location is the Mahalanobis distanceDM [7].
The Mahalanobis distance is a distance measurement that is scaled by the statistical variation of
a known data set; therefore, the Mahalanobis distance can beused to determine the similarity
of an unknown sample to a known distribution. For a multivariate Gaussian with meanµ and
covariance matrixΣ, the Mahalanobis distance of a multivariate vectorx is given by
DM(x) =√
(x−µ)TΣ−1(x−µ) (4–1)
The goal locationxG of the vehicle that promotes coverage of the environment andvehicle
separation is chosen to be the point that maximizes the Mahalanobis distance forN known
landmarks and team vehicles as given by
32
xG = maxx
N
∑i‖DMi(x)‖ (4–2)
The Mahalanobis distance of the each vehicle positionDMviare also included in order to
drive the goal locations away from the current vehicle positions.
The spatial characteristic of the expected data set are modeled by the covariance matrix.
This approach allows for a flexible coverage criteria which ensures exploration and team
distribution regardless of whether or not prior information is known about the environment.
The more information known about the environment the higherthe probability a goal location,
determined by the exploration criteria, will contain new obstacle information. If no prior
knowledge of the environment is utilized the Mahalanobis distance collapses to a standard
Euclidean distance measurement.
The goal locations are calculated by each individual vehicle, with the assumption that each
vehicle location is available for inclusion in the calculation of xG. Goal locations for each vehicle
are calculated independently of each other. Therefore eachvehicle’s goal location is based on
the current global map information and the current vehicle locations. By using this method for
goal calculation a vehicle search pattern can be intentionally fixed or calculated using a different
performance criteria, but still influence the goal locations of the remaining vehicles.
33
CHAPTER 5TRAJECTORY PLANNING
5.1 Introduction
One major disadvantage with many path planners is the assumption that the environment
is known in advance. Methods that explicitly deal with the need to replan and can reevaluate
the path as it is being executed are known as on-line algorithms, and the trajectory they produce
is sometimes referred to as a conditional plan. A true on-line algorithm should be able to
generate a preliminary trajectory even in the complete absence of any map. These algorithms
may not guarantee an optimal trajectory however they do guarantee a planned trajectory and
connectivity [19].
Sample-based motion planning is performed as a search over the defined configuration space
generating samples to learn about the problem space being search. Sample-based algorithms
relies on random sampling of states to explore the configuration space, but can also be based on
a deterministic sampling scheme. Samples representing theconfiguration space are stored in a
data structure which can be used to approximate the space. The data structure is composed of
nodes and edges; nodes represent sampled states in the configuration space and edges represent a
valid path connecting two states. The data structure is stored as a expanding tree. Sample-based
approaches have appealing properties such as guaranteed connectivity, are computationally
inexpensive, and are easily made to suit the dynamics constraints of the system. Sample-based
motion planning methods also are probabilistically complete; meaning that if a solution exists,
the probability to find it converges to the one when the computation time approaches infinity. If
no path is found through the configuration space it could meanthat no valid path exists or the
sampling process was insufficient to adequately explore thespace [18].
Rapidly Exploring Random Tree (RRT) planners are a class of random motion planning
algorithm that can be used for systems involving kinodynamic constraints [14] [16] [17].
5.2 Rapidly Exploring Random Tree
The Rapidly-Exploring Random Tree (RRT) algorithm provides a randomized method for
the construction of roadmaps. The goal is to rapidly and efficiently explore the state-space or
34
configuration space. The RRT algorithm was developed by LaValle and Kuffner specifically to
handle problems that involve dynamics and differential constraints. An advantage of an RRT is
that they can be directly applied to kinodynamic planning; which stems from the fact that RRTs
do not require any connections to be made between pairs of configurations as in probabilistic
roadmaps. Through rapid exploration of the space, the RRT can achieve efficient roadmap-style
solutions between two points, and as with all sample-based planners has been shown to be
probabilistically complete.
When RRTs are used for MAV path planning, the tree nodes are potential MAV waypoints
and the branches are paths to the waypoints. A tree is initially composed of the UAV’s position
as a single node. A random MAV state is generated and the tree is extended toward the random
point creating a new branch and node as outlined in Algorithm1. When a path is found or a
maximum number of iterations is reached, the RRT terminates.
Algorithm 1 Rapidly-Exploring Random Trees
1. Initialize a tree containing one node the starting location of the UAV2. while a path has not been found do3. Xrand = a random UAV state4. Xnear = The state in the tree that is closest toXrand5. if the tree can be connected fromXnear to Xrand without collision then6. extend the tree fromXnear to Xrand7. end if8. end while
Path Planning is viewed as the search in spaceX, for a continuous path from an initial state
xinit to a goal statexgoal. It is assumed that the spaceX is a bounded region which contains a
fixed obstacle region,Xobs, and free space,Xf ree. The RRT will be constructed so that all of its
vertices are withinXf ree and each edge of the RRT will correspond to a path that lies entirely in
Xf ree. Collision detection will be performed by an incremental method, and will be described in
future section.
For a given initial state location,xinit , an RRT,T, with K vertices is constructed as follows.
The first node ofT is initialized to be equal toxinit , for a single forward planning RRT. Then
35
A B
Figure 5-1. Rapidly-Exploring Random Tree Expansion A) Sample Random Node B) ExtendTree
for each iteration of the RRT generating algorithm a new random state,xrand is selected from
the configuration spaceX. The node nearestxrand in terms of a distance metricρ is found and
used as the point for expansion. An incremental control input u is selected which minimizes the
distance fromxnear to xrand. The new node and edge must be located in free space and checked
for obstacle collision. The tree,T, is updated with the new node and control input.
5.3 Sampling
Traditionally randomized motion planning algorithms use auniform sample distribution
to generatexrand which tends to grow the tree in such a way that the entire spaceis covered. In
robot motion planning applications, the presence of obstacles often restricts the use of “greedy”
solutions which involve the robot proceeding directly to the goal. The use of the uniform
distribution increases the algorithms robustness when solving problem in which the space is not
convex, meaning the space contains local minimums which mayresult in the planning algorithm
not being able to find a valid path. However, when the space is convex and the vehicle dynamics
are fast enough to ensure adequate maneuverability, using auniform distribution is unnecessary
and computationally expensive, because the “obvious” solution is not explored immediately.
Another option would be to use a distribution which was biased in such a way to generate
with some probability samples which fall close to the goal location. The possible drawback of a
biased search scheme or any “greedy” search strategy is thatit can get stuck in local minima and
fail to find a valid path.
36
0 200 400 600 800 1000 1200 1400 1600 1800 20000
200
400
600
800
1000
1200
1400
1600
1800
2000
X
Y
A
500
1000
1500
2000
500
1000
1500
2000
250
500
750
X
Y
Z
B
Figure 5-2. Rapidly-Exploring Random Tree w/ Kinematic Constraints A) Two-DimensionalRRT B) Three-Dimensional RRT
5.4 Collision Avoidance
Once it has been decided where the samples will be placed, thenext problem is to determine
whether the configuration is in collision. Thus, collision detection is a critical component of
sampling-based planning. In most motion planning applications, the majority of computation
time is spent on collision checking. The choice of the collision detector is extremely important,
as most of the time spent by planners is devoted to collision checking, both for validating
samples and edges connecting samples. The problem of obstacle avoidance for autonomous
aircraft is complicated by the dynamic constraints of the vehicle. A large body of work exists
that has focused on the problem of obstacle avoidance for mobile ground robots. In contrast to
ground vehicles and rotorcraft which can always stop, aircraft must maintain a minimum airspeed
in order to generate sufficient lift to remain aloft. In comparison to helicopters which can turn in
place, the lateral dynamics of aircraft limit the turning capabilities of the system.
The obstacle avoidance algorithm developed in this work assumes the UAV is stabilized by
a low level autopilot and that the standard kinematic model describes the navigation behavior of
the vehicle.
The three dimensional obstacles are composed of two dimensional convex polygons, as
defined in Chapter3. Each convex polygon, as shown in Figure5-3, represents an obstacle face.
A polygon is in collision with a potential incremental path segment, defined between nodesxi
andx j , if there exists a pointr which lies on the line connecting the two nodes and contained
37
A B
Figure 5-3. Collision Check A) Line-Plane Intersection B) Point Inside Polygon
inside the polygon. The pointr is contained inside the polygon if the sum of the interior angles,
αi , is equal to 2π.
38
CHAPTER 6SIMULATION RESULTS
A simulation is used to demonstrate the team distribution and mapping strategy. The
simulation consists of a two MAV team governed by individualkinematic models. Kinematic
models were used instead of a nonlinear dynamic model to reduce complexity and computational
cost. The purpose of the proposed simulation is to test the coverage and team distribution
algorithms, therefore the assumption is made that the kinematic model sufficiently describes the
flight characteristics which would be expected from a more accurate dynamic model combined
with a suitable control system.
There is a camera mounted on the nose of each MAV, aligned parallel with the longitudinal
axis. The camera is updated at a sensor rate of .01Hz. This simulation was run using a feature
point mesh over the faces of the buildings at an even interval. At each sensor time step the
feature points in the camera frustrum are located using scene reconstruction and processed
with the previously described dimensionality reduction, plane fitting described in Chapter3.
Optimal implementations of the scene reconstruction analysis is unnecessary. Therefore, the
actual computation time will be ignored and the sensor rate and scene reconstruction process
will be updated at rates found in published literature. For this example, the scene reconstruction
algorithm will be running at 0.01Hz.
For this simulation, it is assumed that perfect feature point extraction and tracking is
available. There is also assumed to be perfect state estimation, perfect terrain mapping, and
perfect path planning. Clearly these assumptions are unrealistic but will suffice to demonstrate
the effectiveness of the mapping criteria.
The MAV team will fly through an environment designed to demonstrate the performance of
the map criteria in an urban environment. The environment isshown in Figure6-1. The mission
objective is to ensure the vehicles are distributed in the environment and efficiently map the
unknown environment.
Path planning is accomplished using the RRT algorithm, as described in Chapter5. The
RRT provides a kinodynamically feasible, collision free trajectory for the currently known
39
Figure 6-1. Simulated Urban Environment
environment map. The planned path connects the vehicle fromits current pose to the currently
defined goal location, determined from the mapping criteria. The planned path serves as a
’conditional trajectory’, and only ensures a feasible pathfor the current environment map. The
conditional trajectory does not guarantee obstacle avoidance for the unknown portion of the
environment, nor an optimal trajectory to the goal location. Because the vehicle is operating in a
changing state of environment knowledge, the RRT algorithmis used as an ’on-line’ path planner
which replans the trajectory at a given rate. The RRT algorithm will provide a new trajectory at a
rate of .25 Hz.
0500
10001500
20002500
3000
0
500
1000
1500
2000
2500
3000
0
500
1000
x (ft)
y (ft)
altit
ude
(ft)
Figure 6-2. Simulation at T=0s
The simulation is initialized with a completely unknown environment, with the search area
bounded by[3000f t.,3000f t.,1000f t.] environment volume. The two vehicle team is initialized
40
slightly laterally seperated at equal altitudes with identical initial attitudes. The initial goal
location as determined from the mapping criteria is based completely on vehicle location, since
there are no initially known obstacles. Figure6-2shows the initial environment, vehicle locations
and conditional trajectories. The initial goal locations quickly result in diverging trajectories,
distributing the vehicles to opposing environment boundaries.
When a team vehicle approaches within a defined proximity of the exploration goal
location the map criteria recalculates a new exploration goal based on the current map and
team knowledge. The new goal location moves the vehicle awayfrom its current position, team
vehicle position and known map knowledge.
0500
10001500
20002500
3000
0
500
1000
1500
2000
2500
3000
0
500
1000
x (ft)
y (ft)
altit
ude
(ft)
Figure 6-3. Simulation at T=80s
Figure6-3 shows the replanned goal location and new conditional trajectory.
After continuing the simulation for 150 seconds 65% obstacle coverage was achieved.
Obstacle coverage is defined as the percent of mapped obstacle surface area.
The simulation was also run with randomly selected goal locations. Therefore current map
knowledge and team distribution were not included in the mapcriteria. The simulation was again
run for 150 seconds, with 60% of the obstacles mapped. Using the map criteria the team vehicles
remained more widely distributed. The two simulations, random and proposed method, are
only intended to compare the overall representative behavior of each method as they effect team
distribution and coverage. No guarantees are made that the percentage of coverage would remain
41
0500
10001500
20002500
3000
0
500
1000
1500
2000
2500
3000
0
500
1000
x (ft)
y (ft)al
titud
e (f
t)
Figure 6-4. Simulation at T=150s
0 500 1000 1500 2000 2500 3000
0
500
1000
1500
2000
2500
3000
y (ft)
x (ft)
altit
ude
(ft)
Figure 6-5. Simulation at T=150s Top View
the same for subsequent simulation runs. This is due to the random nature of the trajectory
generation and therefore the path defined toward each new goal location.
42
CHAPTER 7CONCLUSION
7.1 Summary
Micro Air Vehicles (MAVs) are a reasonable platform for vision-based autonomous
environment mapping. Cameras are relatively small and lightweight, making them especially
well suited for vehicles with small payloads. They provide arich stream of data describing their
environment, suitable for map building. Multiple MAV teamshave the potential to exhibit many
advantages over single vehicle systems, however team distribution and task assignments must be
efficiently governed to realize the added benefits.
Use of geometric maps to represent the sensed environment allows for sufficient detail to
be mapped. The geometric representation provides a suitable map for a wide range of mission
tasks, provided an adequate geometric model is used. The model must be able to accurately
capture environment features with enough detail to satisfythe mission requirements. In this case
an urban environment can be well modeled by polygon plane segments providing a suitable map
for mission planning, collision avoidance, and obstacle recognition.
The mapping criteria developed in this thesis proved to be a valid exploration method,
allowing for the intelligent selection of goal locations togovern vehicle movement and team
distribution. The mapping criteria exhibited advantages over other uncoordinated, random
vehicle exploration strategies, and was found to be easily scaled for any number of team vehicles
without increasing system complexity.
The use of RRT path planners for trajectory generation provided a flexible method for
computing conditional vehicle trajectories which are ableto be quickly replanned when new map
knowledge becomes available. RRT algorithms also allowed kinodynamic constraints for each
vehicle to be tailored for heterogeneous vehicle teams adding another layer of flexibility.
The simulations are limited by the assumptions of perfect feature point detection, feature
tracking, and scene reconstruction but suffice to demonstrate the feasibility and performance of
the mapping criteria proposed.
43
7.2 Future Direction
Recently many research efforts have considered the problemof simultaneous localization
and mapping (SLAM). The SLAM process uses a single mobile robot to simultaneously generate
a global environment maps and localize the vehicle within the environment. A somewhat
different problem, is that of cooperative localization andmapping (CLAM). Basically, CLAM
involves two or more robots cooperating to build a map of the environment. The Cooperative
strategy is not only aimed at simply increasing the speed with which the map is constructed , but
also it is aimed at increasing the accuracy of the resultant maps and vehicle location estimates.
44
REFERENCES
[1] Broida, T. and Chellappa, R., “Estimation of Object Motion Parameters from NoisyImages,”IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 8, No. 1,pp. 90-99, Jan. 1986.
[2] Brunskill, E., and Roy, N., “SLAM using Incremental Probabilistic PCA and Dimen-sionality Reduction,”IEEE International Conference on Robotics and Automation, April2005.
[3] Canny, J.F., “A Computational Approach to Edge Detection,” IEEE Transactions onPattern Analysis and Machine Intelligence, Vol. 8, No. 6, November 1986, pp. 679-698.
[4] Castro, G.J., Nieto, J., Gallego, L.M., Pastor, L., Cabello, E., “An Effective CameraCalibration Method,”IEEE 0-7803-4484-7/98, 1998, pp. 171-174.
[5] Choset, H., Lynch, K., Hutchinson, S., Kantor, G., Burgard, W., Kavarki, L., and Thrun,S.,“Principles of Robot Motion: Theory, Algorithms, and Implementations ”,MIT Press,Cambridge, MA, 2005.
[6] Choset, H. and Nagatani, K., “Topological SimultaneousLocalization and Mapping(SLAM): Toward Exact Localization Without Explicit Localization,” IEEE Transactionson Robotics and Automation, Vol. 17, No. 2, April 2001.
[7] Duda, R. O., Hart, P. E., and Strok, D. G., “Pattern Classification,” John Wiley and Sons,Inc., 2001.
[8] Elfes, A. “Sonar-Based Real-World Mapping and Navigation,” IEEE Journal of Roboticsand Automation, Vol. RA-3, No. 3, June 1987.
[9] Elfes, A., “Using Occupancy Grids for Mobile Robot Perception and Navigation,”Computer, Vol. 22, Issue 6, pp. 46-57, 1989.
[10] Forsyth, D.A. and Ponce, J.,Computer Vision : A Modern Approach, Prentice-HallPublishers, Upper Saddle River, NJ, 2003.
[11] Hammerly, G., and Elkan, C., “Learning the k in k-means”, Neural Information Process-ing Systems, 15, 2004.
[12] Harris, C. and Stephens, M., “A Combined Corner and EdgeDetector,”Proceedings of theAlvey Vision Conference, 1988, pp. 147-151.
[13] Hartley, R., “In Defense of the Eight-Point Algorithm,” IEEE Transactions on PatternAnalysis and Machine Intelligence, Vol. 19, No. 6, June 1997.
[14] Kuffner, J., and LaValle, S., “RRT Connect: An efficientApproach to Single-QueryPath Planning,”Proceedings of the IEEE International Conference on Robotics andAutomation, 2000, pp. 995-1001.
45
[15] Kuipers, B. and Byun, Y., “A Robot Exploration and Mapping Strategy Based on SemanticHierarchy of Spacial Representations”,Journal of Robotics and Autonomous Systems,8:47-63, 1991.
[16] LaValle, S.M., and Kuffner, J.J., “Randomized Kinodynamic Planning,”InternationalJournal of Robotics Research, Vol. 20, No. 5, May 2001, 378-400.
[17] LaValle, S.M.,Planning Algorithms, Cambridge University Press, Also availableat:http://msl.cs.uiuc.edu/planning/, 2006.
[18] Latombe, J.C.,Robot Motion Planning, Boston, MA:Kluwer Academic, 1991.
[19] Laumond, J.P.,Robot Motion Planning and Control, Online book: http://www.laas.fr/jpl/book.html.
[20] Longuet-Higgins, H.C., “A Computer Algorithm for Reconstructing a Scene from TwoProjections,”Nature, Vol. 293, pp. 133-135, Sept 1981.
[21] Lucas, B. and Kanade, T., “An Iterative Image Registration Technique with an Applicationto Stereo Vision,”Proceedings of the DARPA Image Understanding Workshop, 1981, pp.121-130.
[22] Ma, Y., Soatto, S., Kosekca, J., and Sastry S., “An Invitation to 3-D Vision: From Imagesto Geometric Models,”Springer-Verlag New York, Inc. 2004.
[23] Mahon, I., and Williams, S., “Three-Dimensional Robotic Mapping”,Proceedings of the2003 Australasian Conference on Robotics and Automation, Brisbane, Queensland, 2003.
[24] Martin, C., and Thrun S., “Online Acquisition of Compact Volumetric Maps with MobileRobots”,In IEEE International Conference on Robotics and Automation, Washington, DC,2002.
[25] MacQueen, J.B., “Some Methods for Classification and Analysis of Multivariate Ob-servations”,Proceedings of the 5th Berkeley Symposium on Mathematical Statistics andProbability, pp. 281-297, 1967.
[26] Oliensis, J., “A Critique of Structure-from-Motion Algorithms”,Computer Vision andImage Understanding, 80:172-214,2000.
[27] Prazenica, R., Watkins, A., Kurdila, A., Ke, Q., and Kanade T., “Vision-Based KalmanFiltering for Aircraft State Estimation and Structure fromMotion”, AIAA Guidance,Navigation, and Control Conference and Exhibit, San Francisco, California, August 2005.
[28] Surmann, H., Nuchter, A., and Hertzberg, J., “An Autonomous Mobile Robot with a 3DLaser Range Finder for 3D Exploration and Digitalization ofIndoor Environments”,Robotics and Autonomous Systems, Vol. 45, pp. 181-198, 2003.
46
[29] Thrun, S., Gutmann, J.-S., Fox, D., Burgard, W., and Kuipers, B., “Integrating Topologicaland Metric Maps for Mobile Robot Navigation: A Stastical Approach”,In Proceedings ofthe National Conference on Artificial Intelligence (AAAI), 1998.
[30] Thrun, S., Burgard, W., and Fox D., “A Real-Time Algorithm for Mobile Robot Mapping-With Applications to Multi-Robot and 3D Mapping”,In IEEE International Conference onRobotics and Automation, 2000
[31] Tipping,M., and Bishop, C., “Probabilistic PrincipalComponent Analysis”,Journal of theRoyal Statistical Society, Series B, 61, Part 3, pp. 611-622.
[32] Tomasi, C. and Kanade, T., “Shape and Motion from Image Streams Under Orthography”,International Journal of Computer Vision, Vol. 9, No. 2, pp. 137-154.
[33] Office of the Secretary of Defense, United States, “Unmanned Aircraft Systems Roadmap2005-2030”, August 2005.
[34] Thomas, J. and Oliensis J., “Dealing with Noise in Multiframe Structure from Motion”,Computer Vision and Image Understanding,Vol. 76, No. 2, November, pp. 109-124, 1999.
[35] Weng, J., Zhang, Y., and Hwang W., “Candid Covariance-Free Incremental PrincipleComponent Analysis”,IEEE Transactions on Pattern Analysis and Machine Intelligence,2003.
47
BIOGRAPHICAL SKETCH
Eric Branch was born and raised in Hollywood, Florida. He attended the University of
West Florida in Pensacola, Florida. During his time in Pensacola, he worked as a flightline
attendant at Pensacola Region Airport, where he started hisflight training and eventually
earned a Commercial Pilots License. In 2001, he transferredto the University of Florida, where
he received a Bachelor of Science degree in aerospace engineering in August 2003. After
graduation, Eric went to work for Naval Air Systems Command (NAVAIR) as a Flight Test
Engineer. Eric later returned to the University of Florida to earn a Master of Science degree in
aerospace engineering. His graduate research involved thedevelopment of vision-based flight
control methodologies.
48