Mapping of Complex Marine Environments using
an Unmanned Surface Craft
by
Jacques Chadwick Leedekerken
Submitted to the Department of Electrical Engineering and Computer
Science
in partial fulfillment of the requirements for the degree of
Doctor of Philosophy in Computer Science and Engineering
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
September 2011
@
Massachusetts Institute of Technology 2011. All rights res
ARCHIVES
MASSACHUSETTS INSTITUTSEP
27
011
erved.
/)
Author ... ... ...Department of Electrical Engineering and Computer Science
August 9, 2011
,1 ICertified by...
Accepted by...
. . . .. . . . . . . . .. . . . .. J ohn LeonardProfessor of Mechanical and Ocean Engineering
Thesis Supervisor
.. . . . . .
(/ C
Leslie A. Kolodziejski
Chairman, Department Committee on Graduate Theses
Mapping of Complex Marine Environments using an
Unmanned Surface Craft
by
Jacques Chadwick Leedekerken
Submitted to the Department of Electrical Engineering and Computer Science on August 9, 2011, in partial fulfillment of the
requirements for the degree of
Doctor of Philosophy in Computer Science and Engineering
Abstract
Recent technology has combined accurate GPS localization with mapping to build 3D maps in a diverse range of terrestrial environments, but the mapping of marine envi-ronments lags behind. This is particularly true in shallow water and coastal areas with man-made structures such as bridges, piers, and marinas, which can pose formidable challenges to autonomous underwater vehicle (AUV) operations. In this thesis, we propose a new approach for mapping shallow water marine environments, combining data from both above and below the water in a robust probabilistic state estimation framework. The ability to rapidly acquire detailed maps of these environments would have many applications, including surveillance, environmental monitoring, forensic search, and disaster recovery. Whereas most recent AUV mapping research has been limited to open waters, far from man-made surface structures, in our work we focus on complex shallow water environments, such as rivers and harbors, where man-made structures block GPS signals and pose hazards to navigation. Our goal is to enable an autonomous surface craft to combine data from the heterogeneous environments above and below the water surface - as if the water were drained, and we had a complete integrated model of the marine environment, with full visibility.
To tackle this problem, we propose a new framework for 3D SLAM in marine environments that combines data obtained concurrently from above and below the water in a robust probabilistic state estimation framework. Our work makes systems, algorithmic, and experimental contributions in perceptual robotics for the marine en-vironment. We have created a novel Autonomous Surface Vehicle (ASV), equipped with substantial onboard computation and an extensive sensor suite that includes three SICK lidars, a Blueview MB2250 imaging sonar, a Doppler Velocity Log, and an integrated global positioning system/inertial measurement unit (GPS/IMU) device. The data from these sensors is processed in a hybrid metric/topological SLAM state estimation framework. A key challenge to mapping is extracting effective constraints
from 3D lidar data despite GPS loss and reacquisition. This was achieved by devel-oping a GPS trust engine that uses a semi-supervised learning classifier to ascertain the validity of GPS information for different segments of the vehicle trajectory. This eliminates the troublesome effects of multipath on the vehicle trajectory estimate, and provides cues for submap decomposition. Localization from lidar point clouds is performed using octrees combined with Iterative Closest Point (ICP) matching, which provides constraints between submaps both within and across different map-ping sessions. Submap positions are optimized via least squares optimization of the graph of constraints, to achieve global alignment. The global vehicle trajectory is used for subsea sonar bathymetric map generation and for mesh reconstruction from lidar data for 3D visualization of above-water structures.
We present experimental results in the vicinity of several structures spanning or along the Charles River between Boston and Cambridge, MA. The Harvard and Longfellow Bridges, three sailing pavilions and a yacht club provide structures of interest, having both extensive superstructure and subsurface foundations. To quan-titatively assess the mapping error, we compare against a georeferenced model of the Harvard Bridge using blueprints from the Library of Congress. Our results demon-strate the potential of this new approach to achieve robust and efficient model capture for complex shallow-water marine environments. Future work aims to incorporate au-tonomy for path planning of a region of interest while performing collision avoidance to enable fully autonomous surveys that achieve full sensor coverage of a complete marine environment.
Thesis Supervisor: John Leonard
5
Acknowledgments
This work was partially supported by the Office of Naval Research under Grants N00014-06-10043, N00014-05-10244 and N00014-07-11102, by the MIT Sea Grant College Program under grant 2007-R/RCM-20, and by Singapore's NRF via funding through the SMART
/
CENSAM Center (http://censam.mit.edu).Contents
1 Introduction 1.1 Research Objectives... . . . . 1.2 Technical Approach.... . . . . ... 1.3 Related Research . . . . 1.4 Contributions . . . .... 1.5 Sum m ary . . . .2 The 3D Mapping Problem
2.1 Probabilistic State Estimation for Mapping 2.2 Related Mapping Research . . . . 2.2.1 Terrestrial 3D Mapping . . . . 2.2.2 Marine Mapping . . . .
2.3 GPS-aided Mapping . . . ....
2.4 Open Issues . . . .
2.5 Sum m ary . . . .
3 Perception-driven GPS Trust Engine 3.1 GPS Aided Navigation ... 3.2 Discriminative Classifier ... 3.3 Classifier Performance . . . ... 7 39 39 47 49 52 54 55 56 . ... ... .. . . . . ... ... ... .. .
8
CONTENTS
4 A Hierarchical Approach to 3D Marine Mapping 81
4.1 System Overview ... ... 81
4.2 Localization ... ... 90
4.3 Mapping ... 94
4.3.1 Grid-based Metric Maps ... 94
4.3.2 Metric Map Matching ... ... 97
4.3.3 Global Map Alignment . . . . 100
4.4 Sonar M apping . . . . 106
4.4.1 Sonar Image Processing... . . . . . . .. 106
4.4.2 Bathymetric Mapping.... . . . . . . .. 106 4.5 Mesh Generation... . . . . . . . . . .. 110 5 Experimental Results 113 5.1 Experimental Setup... . . . . . . . .. 113 5.2 Localization ... . . . . . . . . . . .. 117 5.3 Subm aps . . . . 123
5.4 Bridge Model Validation . . . . 130
5.5 Bathymetry... . . . . . . . . 138
5.6 Discussion... . . . . . .. 141
6 Conclusion 147 6.1 Summary of Contributions . . . . 147
6.2 Future Research . . . . 148
6.2.1 Localization and Mapping... . . . . . . . . 149
6.2.2 A utonom y . . . . 151
A Harvard Bridge Model 155
List of Figures
1-1 Digital Nautical Chart . . . .
1-2 Example target environments
1-3 1-4 1-5 1-6 1-7 1-8
Platform and sensors . . . . Google Street View platforms . Small AUVs and SCOUT . Platform application example . Bridge mapping highlight . . .
Range data under a bridge with and
2-1 Bayes net example . . . . . 2-2 Factor graph example. .
3-1 GPS hazards example .
3-2 Lidar features mask ... 3-3 Model classifier features
3-4 IMU and GPS features .
3-5 Lidar derived features . 3-6 Velocity uncertainty .
3-7 Mode classifier output .
3-8 Trajectory by mode... 3-9 Vehicle model improvement 3-10 Feature training comparison
without classifier
. . . .. . . .. . .
-.. . . -.. -.. . . -.. . .
47
. . . . 62. . .. . . .. . . .
68
. . .
. . . .. . . .
..
. . .
69
. . . . .. . . .. . . .
70
. . . .. . . .. . . . .
71
. . . .
. .
..
. . . .. . . . .
72
. .. . . .. .. . . .. . .
76
. .. . . ...
.
. . .. . . . .
. . . . ..
77
. .. . .
. . . .. .. . . ..
78
--- - -
. . . . .. .. . . .
7 9
10
LIST OF FIGURES
4-1 System components ... 82
4-2 Mapping architecture... . . . . . . . . . 83
4-3 Range sensor FOV . . . . 84
4-4 Marine environment perception... . . . . . . .. 85
4-5 Localization overview... . . . . . . . . 91
4-6 Map generation... . . . . . . . . . 94
4-7 Map registration example... . . . . . . .. 104
4-8 Pose transformation... . . . . . . . . . 105
4-9 Submap factor graph example . . . . 105
4-10 Sonar range extraction... . . . . . . . .. 107
4-11 Sonar im ages . . . . 108
5-1 Area of operations... . . . . . . . .. 114
5-2 Launch and recovery ... .. ... ... .116
5-3 Localization May 13. ... . . . . . . .. 119
5-4 Localization May 13, zoom... . . . . . . . .. 120
5-5 Localization May 11... . . . . . . . . . . . .. 121
5-6 Localization May 11,zoom... . . . . . . . . 122
5-7 Range projection amplifies localization error... . . . . . .. 123
5-8 Submaps and registrations... . . . . . .. 124
5-9 Submap adjacency matrix . . . . 125
5-10 Global m ap results . . . . 126
5-11 Harvard Bridge results . . . . 127
5-12 Map results near Harvard Bridge . . . . 128
5-13 Map results near Longfellow Bridge . . . . 128
5-14 Map results near sailing pavilion... . . . . . . 129
5-15 Prior bridge model . . . . 130
5-16 Dense sampling of bridge model . . . . 131 5-17 Map data for sequential pier distance comparison . . . .1 132
5-18 Sequential pier distance error. . . . . . . .. 5-19 Model validation mean squared error histogram 5-20 Model registration pairs . . ...
5-21 Model registration transform distribution 5-22 Kayak bathymetry with satellite... 5-23 5-24 5-25 5-26 5-27 5-28 Kayak bathymetry. . . . . . . .. REMUS vehicle for river bathymetry
Fused kayak and AUV bathymetry Minor map defects . . . . Dynamic regions . . . .
Low dynamic regions . . . .
6-1 M arine camera distortions . . . . Bridge blueprint, general . . .
Pier specifications . . . . Harvard Bridge remodeling .
Unmodeled bridge pier . .
Prior bridge model ...
Vehicle deployment... Hull pontoon . . . . Vehicle computer enclosure . Computer modules...
Vehicle support module . . .
SICK scanner support module
Sonar support module . . .
Magnetic compass hazard .
DVL mount . . . . . . . . 1 8 1
LIST OF FIGURES
...
.. .. .
...
.
A-1 A-2 A-3 A-4 A-5 B-i B-2 B-3 B-4 B-5 B-6 B-7 B-8 B-9 133 134 135 137 138 139 140 141 142 144 145 154 156 159 160 161 161 164 166 168 169 171 174 175 178 17912
LIST OF FIGURES
B-11 Actuated lasers .. .. .. .. .. . .. . . . . 182
List of Tables
B .1 Sensors . . . . 165
14
LIST OF TABLES
List of Algorithms
1 Generation of hypotheses for map registration.... . . . . . . . 101
2 Construction of the graph by verifying possible pair-wise constraints 101 3 Sonar map voxel updates . . . . 109
4 Bathymetry map filtering: consistency check... . . . . . . ... 110
5 Generate random samples of triangular mesh model . . . . . . 158
16
LIST OF ALGORITHMS
Chapter 1
Introduction
Recent technology has combined accurate GPS localization with mapping to build
3D maps in a diverse range of terrestrial environments, but the mapping of marine
environments lags behind. This is particularly true in shallow water and coastal areas with man-made structures such as bridges, piers, and marinas, which can pose formidable challenges to autonomous underwater vehicle (AUV) operations. In this thesis, we propose a new approach for mapping shallow water marine environments, combining data from both above and below the water in a robust probabilistic state estimation framework. We describe an implemented system that performs large-scale Simultaneous Localization and Mapping (SLAM) using data streams from an autonomous surface vehicle equipped with GPS, sonar, and lidar sensing. Our results demonstrate the potential of this new approach to achieve robust and efficient model capture for complex shallow-water marine environments.
CHAPTER 1. INTRODUCTION
1.1
Research Objectives
Our goal is to enable an autonomous surface craft to create accurate 3D maps of complex marine environments, both above and below the water. Environments of particular interest are bridges, harbors, and other shallow marine environments with significant human use or construction. Typically such environments have no accurate digital map representation, nor are regular sonar surveys conducted. Underwater depth maps are often called bathymetry maps, and are analogous to terrain maps for terrestrial environments. Most bathymetric data are for major lakes, high-traffic coastal regions, and in the deep ocean beyond 100 meters depth. An example of a typical digital nautical chart (for Boston Harbor) is shown in Figure 1-1. While bathymetric data covers much of the world, the majority of undersea map data is decades old, and as shown in the figure, coverage is typically poor in coastal areas.
The environments that we consider in this thesis possess the following traits. The environment above the water should contain structures observable with the ranging sensors. The environment below the surface contains structures or bathymetry within the perceptual field of the platform sensors. For example, a river or harbor region with maximum depth of five meters and possessing surface structures, geological formations, or natural vegetation within or along the shore would be suitable for our platform (described below), which has a sonar with a maximum range of ten meters and surface laser ranging sensors with maximum range of thirty meters. Figure 1-2 illustrates examples of target environments within the domain of this research.
The ability to rapidly acquire detailed maps of these environments would have many applications, including surveillance, forensic search, and environmental mon-itoring. Unmanned vehicles may be used for environmental sampling [28], disaster response [106], forensic recovery [125], and surveillance [128]. Natural disasters mo-tivate the need for marine robotic perception systems. Robotic systems may provide
1.1. RESEARCH OBJECTIVES
a means to locate human survivors and casualties, inspect structures for damage, and survey environments for recovery construction efforts without endangering hu-mans. For example, Murphy et al. documented inspection of a bridge for damage with a robotic surface craft in the wake of a hurricane [106]. In such situations, small surface craft offer advantages of safety, maneuverability, and perception unavailable from other marine vehicles or aerial survey methods. While aerial surveys can provide shallow water bathymetry [1631, the sampling is sparse and subject to occlusions such as bridges.
Our primary thesis claim is that bathymetric mapping may be improved relative to AUV bathymetry mapping using a surface craft with GPS sensing for accurate localization. Our work incorporates constraints from surface maps to provide greater qualitative consistency of surface maps, which demonstrate distortions from GPS errors when uncorrected. More generally, our method for improving localization of underwater maps applies to underwater sampling when the observations are inade-quate for self-alignment. This could result from using a sonar with a limited field of view or for making point measurements of parameters such as temperature, salinity, or chemical concentrations.
20
CHAPTER 1. INTRODUCTION
aSILTNK 0 LAKARE BERTHS U DRYDOC SOUNDG U a BUISGL kCOALNE4% e 1.FOGSIG SgG DEPARE c *41 BUAARE MNSYS aDAMCON * DYKCONSaved mpmag to.png .e P o
(a) Boston Inner Harbor
(b) Boston Inner Harbor to Charles River
Figure 1-1: Example views of multiple data layers of Digital Nautical Chart (DNC) data. Depth soundings are labeled in meters. (a) Part of Boston's inner harbor, including the North End, Charlestown, and East Boston; (b) Close-up of view of the Charles River near MIT and the inner harbor adjacent to the North End and Charlestown. The Longfellow Bridge is the linear structure in the lower left of the figure. Note the absence of depth data in the Charles River.
1.1. RESEARCH OBJECTIVES
Figure 1-2: This work aims to map shallow marine environments such as those shown here: bridges spanning rivers and natural or man-made features along shores.
CHAPTER 1. INTRODUCTION
1.2
Technical Approach
Our technical approach for developing this unique capability - concurrent mapping above and below the water surface - has the following key elements: (1) design of a novel autonomous marine platform, equipped with GPS, inertial, sonar, and lidar sensors; (2) development of a machine learning approach to classifying GPS errors that are encountered in close proximity to marine structures; and (3) the realization of a robust and efficient 3D simultaneous localization and mapping (SLAM) algorithm to process the streams of heterogeneous data generated by the vehicle's sensors.
Our platform is shown in Figure 1-3. For positioning and localization, the vehicle has an OXTS integrated IMU and GPS and an RDI Doppler Velocity Log (DVL). The terrestrial sensors include three SICK LIDARs, a low-cost web cam, and two Hokuyo LIDARs mounted on a shaft rotating at approximately 1Hz. Two of the
SICK LIDARs are mounted to scan vertically to permit 3D reconstruction as the
vehicle passes along features. For underwater sensing we mount a Blueview MB2250 imaging sonar. The computational core of the vehicle contains two motherboards carrying a quad-core 2.83 GHz CPU and a low-power dual-core 1.6 GHz CPU.
The platform evolved through an iterative design process, and sensors were added and removed through the life of the project. The camera and a rotating laser scanner shown in Figure 1-3 were logged but not used in the results reported in the thesis. Fur-ther details on the vehicle design and a discussion of the various sensor configurations that were evaluated during the course of the research is provided in Appendix B.
The data from these sensors is processed in a hybrid metric/topological SLAM state estimation framework. From an algorithmic perspective, tackling this problem forces us to address a number of difficult issues not encountered in previous SLAM research. Operation at the water surface presents unique challenges for robotics. Wind and current forces induce unpredictable platform motion, posing modeling and
1.2. TECHNICAL APPROACH
Figure 1-3: The marine mapping platform developed for the thesis research project, shown with close-up images of the primary sensors. These include the Oxford Iner-tial+ IMU and GPS (inset 1, 4, and 7), an RDI Doppler Velocity Log (6), three SICK
LMS291 laser scanners (2), a Blueview MB2250 imaging sonar (5). Also shown are
two Hokuyo UTM30LX laser scanners on a continuously rotating shaft (3), for which data was collected, but not processed in the thesis.
CHAPTER 1. INTRODUCTION
control challenges. Unstructured terrain, sparse landmarks, and GPS distortions near man-made structures make navigation difficult and increase uncertainty in control and coverage capabilities.
A key challenge to mapping is extracting effective constraints from 3D lidar data
despite GPS loss and reacquisition. We wish to make effective use of GPS infor-mation, when appropriate, but to not be fooled by the GPS errors that inevitably occur in close proximity to marine structures. This was achieved by developing a
GPS trust engine that uses a semi-supervised learning classifier to ascertain the
va-lidity of GPS information for different segments of the vehicle trajectory. This elim-inates the troublesome effects of multipath on the vehicle trajectory estimate, and provides cues for submap decomposition. Localization from lidar point clouds is per-formed using octrees combined with Iterative Closest Point (ICP) matching, which provides constraints between submaps both within and across different mapping ses-sions. Submap positions are optimized via least squares optimization of the graph of constraints, to achieve global alignment. The global vehicle trajectory is used for subsea sonar bathymetric map generation and for mesh reconstruction from lidar data for 3D visualization of above water structures.
The primary target in the environment for our work is the Harvard Bridge (also known as the Mass Ave. Bridge), which spans the Charles River at MIT, connecting Boston and Cambridge. A particular challenge problem for this work was to obtain a complete model of the supporting structures of the Harvard Bridge, which would entail frequent transits in and out of GPS availability, posing a great challenge to sensor processing. The Harvard Bridge occupies a special place in MIT folklore, as described by a plaque placed on the bridge in 2008 by the MIT class of 1962. The plaque reads:
1.3. RELATED RESEARCH
Oliver Reed Smoot MIT '62 and found to be 364.4 smoots
+/-
1 ear. This provided a very useful metric for generations of bridge travelers since, and the Smoot has joined the angstrom, meter, and light-year as enduring standards of length."Dedicated to our Classmate Oliver Reed Smoot '62 on October 4, 2008
by the MIT Class of 1962.
1.3
Related Research
The idea of using computer vision and GPS measurements to capture large-scale urban environments has been the subject of considerable research in the past. A notable early system was developed by Teller and colleagues at MIT, in the City Scanning project [4,151]. More recently, one of the most renowned applications of
3D mapping by a commercial enterprise is the Google Street View project [67]. The
Google Street View project launched in 2007 using automobiles for environmental capture along navigable streets. With integration to their mapping technology, the project permits users a virtual street-level perspective of geographic locations. While many of the details of the Street View project are proprietary, the project clearly shows the tremendous utility to users of obtaining detailed map data for the world.
In an effort to capture more diverse environments beyond easily navigable streets, Google has constructed a variety of alternative platforms, such as bicycles and snow-mobiles, shown in Figure 1-4. The configuration of laser range sensors on Google's platforms are quite similar to our vehicle, shown in Figure 1-6 and described in detail in Appendix B. The approach that we present in this thesis could provide a similar capability to Street View or Microsoft's Bing Maps, for shallow-water coastal marine environments. For example, significant portions of cities such as Venice and
Amster-CHAPTER 1. INTRODUCTION
dam, with their substantial waterway systems, could be mapped in great detail using the 3D mapping framework that we describe in this work.
Figure 1-4: The Google Street View project employs a variety of platforms for cap-turing environments in 3D [67].
The work presented in this thesis touches upon a variety of research topics in robotics: perception, sensor fusion or SLAM optimization, marine applications, submap-ping, point registration, and surface reconstruction. There has been substantial re-search on these topics, and for terrestrial domains there exist deployed commercial systems that can efficiently capture large environments using airborne and land vehi-cles for data capture. However, the use of these technologies in the marine environ-ment lags behind terrestrial applications, due to a number of difficult issues. These
1.3. RELATED RESEARCH
include platform motion, limited sensor range, massive data streams, limited onboard computation, perceptual aliasing due to repeated environmental structure, erroneous
GPS readings, ambiguous data association, and outlier measurements. The elusive
goal is robustness. When erroneous constraints are applied within conventional ap-proaches, the result can easily lead to non-convergence or incomprehensible results.
Unlike terrestrial robots, for a marine robot the assumption of static support is invalidated, so vehicle motion is possible in the absence of control inputs. The robot moves within a fluid and is subject to current, wind, and wave disturbances. This makes the issue of modeling platform motion much more difficult than for wheeled land vehicles. Similar issues can be encountered with unmanned air vehicles [7]. Un-der these conditions a robot is rarely stationary even when not using any actuators. In fact station keeping is a challenge for many marine vehicles, especially when un-deractuated, i. e. unable to directly control motion of all degrees of freedom. What is more relevant for estimation when static support is invalidated is the predictive modelling of vehicle dynamics when a vehicle is subject to motion induced by forces unrelated to either control inputs or a priori parameters, such as gravity or predeter-mined buoyancy. The environmental forces weaken the accuracy of simpler predictive control models, and consequently reduce mapping accuracy. Ameliorating such error may be accomplished with increasingly complex models of environmental forces or a combination of more accurate motion sensors and robust estimation algorithms.
Underwater perception is difficult compared to terrestrial sensing. Part of this is due to technology limits, since sensor technology for vision and laser or radar ranging are in much higher demand. Underwater sensing is primarily limited to acoustics, al-though some underwater vision is possible at close range in calm, clear water. Acoustic sensing is a substantially different mechanism when compared to visual- or laser-based sensing [12,61]. While underwater mapping may benefit from improvements in sonar
CHAPTER 1. INTRODUCTION
sensors or advanced processing techniques [100,110,130], we propose an alternative mechanism to aid underwater mapping using constraints derived from simultaneous surface sensing.
GPS can provide good position accuracy even with low-cost single antenna devices.
Applications of GPS technology have recently increased, and GPS sensors may be found in consumer portable audio players, mobile phones, electronic readers, wrist watches, etc. The majority of applications do not require very high accuracy. For example, anti-theft GPS tracking devices in automobiles need only coarse positioning. Professional surveys and military applications do require higher levels of accuracy, and the systems and technology required to achieve greater accuracy rapidly increase in cost. Even with expensive equipment, the underlying mechanisms of GPS are sus-ceptible to many sources of error which can be difficult to model or detect. Structural conditions, such as canopies or canyons whether natural or man-made, corrupt GPS ranging through multipath or signal obstruction.
This thesis does not attempt to model complex GPS parameters, or to perform signal-level error detection. Instead, we present a framework to detect when common
GPS hazards are most likely to occur, such as canopy or canyon situations, using
other sensor data available to the robot. While there clearly has been a large body of work in SLAM in the past decade [9,47,153], the marine mapping task posed in this thesis presents several challenges for SLAM algorithm development not addressed in previous research. Integrating GPS with SLAM can be a surprisingly difficult issue [30], especially in our target environment, where GPS multipath errors are extremely common.
A common tradeoff in navigation and mapping system design is local versus global
consistency [23]. Locally consistent maps provide accurate relative relations between the true features in the world, although a local region may be inaccurately positioned
1.3. RELATED RESEARCH
in the global frame. Local consistency is usually due to limited sensing range, the use of relative motion sensors such as inertial measurement units or wheel encoders,
and predictive (relative) motion models [103]. One may observe gradual loss of global consistency as drifts and biases corrupt the estimated map; the resulting error is commonly referred to as odometry drift or dead-reckoning error. Efforts to achieve greater global consistency often disrupt local consistencies. Global corrections may manifest as loop closures, sparse GPS fixes, or sparse features with known data asso-ciation. In incremental recursive estimators, global corrections can produce "kinks" in the trajectory, breaking consistency of the map before and after the correction event. Recent work in full trajectory and map optimization and smoothing attempts to distribute the error [43,119].
Related platforms for this research include a wide range of autonomous surface vehicles (ASVs) and autonomous underwater vehicles (AUVs). Historically, ASV re-search has attracted less interest from academic rere-searchers, in comparison to AUV development. However, in recent years, there has been a surge in interest in robotic surface vehicles. Bertram [21] and Manley [98] provide recent surveys of notable
ASV platforms. Many military surface vehicles are manned platforms that have been
modified for autonomous operation. The US Navy Unmanned Surface Vehicle Mas-ter Plan, published in 2007, lists a wide range of potential surface vehicle missions, including: mine countermeasures, UUV delivery, maritime security, surface warfare, special operations forces support, electronic warfare, and maritime interdiction oper-ations support [109). To our knowledge, the capability that we have set out to achieve
- integrated mapping above and below the water with an ASV - has not previously
been attempted in any of this literature.
Our platform is most similar to the SCOUT (Surface Craft for Oceanographic and Underwater Testing) vehicles [40]. The SCOUT vehicles offer relatively low-cost
alter-CHAPTER 1. INTRODUCTION
natives to traditional truly submersible robots. The low cost and use of commercial-off-the-shelf (COTS) components permitted rapid deployment of multiple vehicles, as demonstrated in [15]. The primary research applications of the SCOUT have been in testing marine autonomy [16,17] and navigation assistance for autonomous underwa-ter vehicles [8, 41,157]. Our platform inherits the hull design of the SCOUT vehicle, and our work extends the perceptual and computational capabilities used in prior work. While the SCOUT hull design is a COTS recreational kayak, other researchers
have noted catamaran type hulls offer stability benefits [95,97,149].
In the most recent work with autonomous surface vehicles similar to our work, Huntsberger et al. present a robotic boat for navigation and target following [76]. Their system integrates GPS, IMU, stereo vision, radar sensors to generate maps of objects above the waterline. The objective in their work is to generate a two-dimensional hazard map for obstacle avoidance at relatively high speeds; precise map estimation for environmental reconstruction is not their stated objective. Underwater perception such as sonar mapping is beyond the scope of their application. They implement their perception and navigation system on several platform hulls with dimensions approximately 13 to 14 meters in length. The physical size is much larger than our platform (approximately 2 meters) and permits operation in rougher sea states and at higher speeds. Interestingly, the authors note laser ranging sensors to be ineffective for their application, and they state wave motion effects, limited range, and limited angular resolution at longer ranges as factors for not incorporating lasers into their sensor suite.
In an earlier vision-based navigation application, Subramanian et al. present re-sults demonstrating shoreline tracking [149]. Their platform included typical naviga-tional sensors such as a GPS receiver, an IMU, and Doppler Velocity Logger (DVL) in addition to a camera sensor for the shoreline tracking.
1.3. RELATED RESEARCH
Recent research with significant similarity to ours in terms of application envi-ronment and the use of unmanned surface craft with inspection abilities is shown in [106,107]. Their platform employed a twin-hull catamaran design. As the authors note, the catamaran design is more stable than the SCOUT hull in high currents, and our platform employed a pair of stabilizing pontoons. The primary objectives in Murphy's work were inspection and mapping of a bridge structure and debris field in the aftermath of a hurricane in a shallow coastal region. A unique aspect of Murphy's work is the experimental comparison of unmanned underwater vehicles (an ROV and AUV) with a surface vehicle in a cluttered environment. The authors report a collision rate of 40% with the underwater vehicles, and achieved their pri-mary objective primarily with the surface craft. The tether on the ROV presented an additional hazard encountered experimentally. In their post experimental anal-ysis on disaster response feasibility and human-robot interaction, the authors note the control challenge presented by GPS loss near the bridge structure and manual teleoperation required near the structure. Murphy provides further analysis on the human-robot interaction requirements in their application. Multiple displays and a team of experts required coordination to successfully deploy and monitor the status of the robot, interpret the sensor data for the inspection role, and navigate successfully near obstacles. Summarily, Murphy's research presented a real scenario motivating the use of robotic vehicles in cluttered marine environments, the challenges associ-ated with the objective, and provided operational comparison of robotic surface and subsurface vehicles within the target.
Underwater and surface vehicle technology has evolved over the last two decades enough for autonomous surveys to be possible over a reasonably sized region and with reduced cost [3]. With the development of the SCOUT [40], costs can be further reduced using comparatively cheaper surface craft having greater flexibility in sensor
CHAPTER 1. INTRODUCTION
attachment. Underwater vehicles face tighter engineering constraints limiting their flexibility to accommodate new sensor technology. Figure 1-5(a) shows two AUVs and a SCOUT vehicle en route to deployment for a mine hunting mission. The larger vehicle on the left is a REMUS model AUV, a vehicle developed for shallow water operations [2] with applications to coastal mine countermeasures [3,147]. The smaller
AUV on the right is an iRobot Ranger, which is a less expensive vehicle developed for
mine acquisition [36,64]. Due to the complex engineering of such vehicles, integrating new sensors such as the sonars on the AUVs shown in Figure 1-5(a) is a non-trivial task requiring trained experts. In contrast, surface craft provide much easier integration of sensors, such as the simple rig shown in Figure 1-5(b). Figure 1-5(b), shows a
SCOUT vehicle equipped with two imaging sonars after a previous deployment with
a sidescan sonar towfish.
Figure 1-5: At left, two small autonomous underwater vehicles (AUVs) and a SCOUT autonomous surface craft (ASC) prior to deployment. A REMUS vehicle is shown to the left of the smaller lower cost iRobot Ranger. At right, simple mounting rigs enable the SCOUT autonomous surface craft (ASC) to rapidly deploy new sensors, such as a multiple imaging sonars (top) or a sidescan sonar (bottom).
1.4. CONTRIBUTIONS
Figure 1-6: Integrating a rapidly deployable low-cost platform with accurate sensors and high performance computing motivates new opportunities in robotic mapping of marine environments. The platform at left permits the rapid survey of a sailing pavilion on the right.
1.4
Contributions
The primary contributions of this thesis are threefold. First, we have developed a novel approach to improving the quality of subsea mapping using surface sensor data, via creation of an autonomous platform with sensors both above and below the waterline. Second, we have developed a new approach for GPS trust assessment using perceptual data fed into a semi-supervised learning classifier. Third, we have implemented a large-scale hierarchical 3D mapping system that combines multiple mapping sessions into a network of submaps whose global positions are optimized with an efficient pose graph optimization algorithm. We have validated our experimental results by comparing with a historical model for the Harvard Bridge, providing a quantitative analysis of mapping error. The overall 3D mapping system that we have implemented compares well with other recent results from the 3D SLAM literature, and provides insights for a number of interesting future research areas, including the integration of perception with real-time motion control for coverage and obstacle avoidance.
Highlights of our results are shown in Figures 1-7 and 1-8, which demonstrate
CHAPTER 1. INTRODUCTION
the Harvard Bridge. Critical to map consistency is the use of a GPS trust engine, described in Chapter 3, to adapt localization models as GPS hazards are encountered. Figure 1-8 illustrates a typical GPS hazard condition under a bridge and the utility of the classifier modes in trajectory optimization for addressing the hazard. The images show range data projected from the estimated trajectory with and without the aid of the classifier modes. The data projected with the aid of the classifier is more consistent with the true structure.
An additional use for the techniques developed in this thesis may be to provide quantitative assessment of subsea mapping algorithms. By mapping an environment both above and below the water surface, this thesis presents a novel approach to improving the accuracy of bathymetric maps when no ground truth is available due to the simultaneous capture of surface and subsurface maps. Accuracy of surface maps can be assessed quantitatively if ground truth is available. For some applications, qualitative assessment may be sufficient, as people are sensitive to even minor errors in map estimates of objects they can see themselves. An observer may inspect both surface and subsurface maps and readily infer the relative distribution of error. For example, a surface map of high accuracy but having small distortions in a small region would imply the bathymetry in that region is less accurate than other regions.
1.4. CONTRIBUTIONS
lO0 M
Figure 1-7: The camera images at top show the Harvard Bridge with views similar to the mapping results shown here. The center image shows mapping results with half of the bridge in view adjacent to the Cambridge stone retaining wall on the right side. The final image shows the map detail provided underneath one segment of the bridge.
CHAPTER 1. INTRODUCTION
40 m
(a) Range projections without classifier modes
40
m
(b) Range projections with classifier modes
Figure 1-8: The images compare projections of laser and sonar range data while the vehicle traversed under a bridge. The vehicle starts at the bottom of the image and travels toward the top along the track in the tan colored sonar points. Laser points are false colored by height. In the top image, poor GPS conditions distort the trajectory and the projected range data. In the lower image, the classifier-aided trajectory and range data are more consistent with the true bridge structure.
1.5. SUMMARY
1.5
Summary
This chapter has motivated the autonomous marine mapping problem that is the focus of the thesis, presented our technical approach and the new robotic platform developed for this investigation, and has described some of the challenges of tackling this problem. The structure of the remainder of the thesis is as follows: Chapter 2
defines the 3D mapping problem, reviews previous research in 3D SLAM, and high-lights open issues faced in the complex marine environments considered in this thesis. Chapter 3 describes a new technique for robustly incorporating GPS sensor measure-ments while operating in cluttered environmeasure-ments, using a semi-supervised learning classifier. Chapter 4 describes the major new algorithmic contributions of the thesis, presenting a hierarchical submap/pose-graph mapping algorithm that achieves ro-bustness to degraded GPS and poor environmental observability with the aid of the
GPS classification engine described in Chapter 3. Chapter 5 presents experimental
results for multi-session mapping of part of the Charles River basin. Validation is performed against historical models of the Harvard Bridge piers. Chapter 6 summa-rizes our contributions and makes suggestions for future research. Appendices A and B provide information on the ground truth comparison for the Harvard Bridge and details of the construction of our marine sensing platform.
CHAPTER 1. INTRODUCTION
Chapter 2
The 3D Mapping Problem
This chapter defines the 3D marine mapping problem formally and reviews the rel-evant literature from the SLAM community. While the previous research in SLAM considered broadly is vast, the 3D mapping task domain presents great difficulties for current state-of-the-art algorithms. We give the probabilistic framework defining the
3D map estimation problem, and we discuss the solution of the estimation problem
with emphasis on state-of-the-art least squares algorithms.
2.1
Probabilistic State Estimation for Mapping
At the core of many robotic estimation systems is a sensor fusion or Simultaneous Localization and Mapping (SLAM) framework. A standard approach is the use of a Kalman Filter [80] or Extended Kalman Filter (EKF). Numerous examples of EKF approaches to the sensor fusion and SLAM problems exist [31, 46, 94]. Julier and Uhlmann in [78] presented the Unscented Kalman Filter (UKF), which uses nonlinear transformations of selected sample points to avoid some issues with accumulated linearization error. Later work by Thrun [154] and Walter [158] emphasized the
CHAPTER 2. THE 3D MAPPING PROBLEM
sparsity of the dual form of the EKF, the Extended Information Filter. Sparsity refers to the information matrix having mostly zero entries off the diagonal and is most apparent when robot poses are not marginalized. Although the state space grows over time leading to O(n2) growth of the covariance or information matrix, the sparse problem structure permits use of efficient representations and sparse optimization methods. Compared to marginalization in traditional recursive implementations such as the EKF, maintaining the entire trajectory permits recalculation of linearization points for nonlinear models. More recent research has focused on optimization of the sparse problem and providing more emphasis on graphical models of problem. Examples include Graphical SLAM [62], square root Smoothing and Mapping (SAM) [42], pose graph optimization [119], and gradient descent methods such as in [44,69].
Other mapping and localization research has focused on particle filtering, which is a stochastic approach to the problem. A key insight by Montemerlo in [101] was that given a robot pose, landmarks are conditionally independent. This allows land-marks to be represented by independent filters without maintaining covariances or co-information between landmarks. The efficiency tradeoff for particle filters is main-taining sufficient particle diversity and not maintain highly improbable particles. Re-sampling procedures prune particle sets at the risk of depletion, where the set shares a common ancestry. Depletion prevents new observations, especially loop closures, from affecting feature positions prior the common ancestry point. While particle filters re-duce the dimensions for the state representation, the state must be replicated over the n particles. For small environments with landmarks of small dimension, one may use a very large number of particles for a system robust to non-Gaussian distributions. Map representation may be a critical factor in the ability to apply particle filtering to a problem. In evidence grid approaches the memory requirements to maintain parti-cle set size for a non-trivial environment quickly become large. Intelligent approaches
2.1. PROBABILISTIC STATE ESTIMATION FOR MAPPING
to maintaining an effective particle set were shown in [68]. Improving memory usage with clever use of data structures allowing sharing was shown in [51,52]. Overall par-ticle filters provide a robust estimation framework using randomized sampling and work well on modest environment scales and low dimensional representations. As environments grow larger and map representation dimensionality increases, the cost of maintaining a sufficient set of particles without rapid depletion becomes compu-tationally intractable. If trajectories contain many large or nested cycles, the ability to apply shared data structure optimizations fades. Shared data optimizations work best for particle sets having common ancestry, and particle filters are more robust when they exhibit particle diversity.
Probabilistic Formulation
As mentioned in Chapter 1, we have designed a vehicle equipped with sensors pro-viding observability of vehicle position and partial views of the environment relative to the vehicle. Real sensors are imperfect, and measurements are corrupted by noise. To deal with noisy measurements, we introduce an estimation framework to approach the mapping problem from a probabilistic perspective.
The general probabilistic formulation for map estimation is as follows. The objec-tive is to determine the values for estimated states maximizing the joint probability of all states. For the purposes of this thesis, we consider only Gaussian error distribu-tions in the vehicle prediction and sensor models, which is a widely adopted assump-tion underlying most soluassump-tion frameworks. Consider the set of estimated states to be
X, where X typically includes vehicle poses, such as a 3D pose x = [.x, y, z, < 0 ,]T.
Implementations often include a representation of the map with landmarks and other estimated parameters such as bias states such that X may be a set containing
Con-CHAPTER 2. THE 3D MAPPING PROBLEM
sider the observable inputs to the system to be the set C, where C includes sensor measurements and control inputs each having an uncertainty from additive Gaussian noise. The set C may be considered constraints upon the unknown states X through the respective sensor models and predictive motion models. The models are shown in Equations 2.1 and 2.2 with corrupting additive Gaussian noise represented by the variables v and w. The probability density functions are shown in Equations 2.3 and 2.4, where
Q
and R represent covariance matrices. The predictive motion model in Equation 2.1 provides a constraint between two sequential pose states Xk_1 and Xkgiven a control input Uk and covariance
Q.
The measurement model in Equation 2.2provides a constraint between a pose state Xk and a landmark state mj through the observation Zi, and covariance R. The set C would contain the control inputs Uk and measurements Zj,k as the necessary parameters to generate constraints on states in X. The actual constraints consist of the differences between model predictions current state given the input parameters: Xk - Xk and Zik, - Z,k. Constraints also include prior estimates (often called priors). One example of a prior is an initial condition on the initial vehicle pose or landmark locations from a prior map.
Xk = f(xk-1, Uk) + v (2.1)
Zj,k
=
h(Xk, mj) + W(2.2)
(k) ~ NV(f(Xk_1, n), )
(2.3)
P(^,k) ~ N(h(zmj), R) (2.4)
The joint probability distribution for estimated states is given in Equation 2.5. Since the set C consists of known inputs rather than unknowns requiring estimation, the joint distribution is equal to the conditional distribution. Using the Markov
2.1. PROBABILISTIC STATE ESTIMATION FOR MAPPING
assumption in the predictive vehicle model and the independence of measurements, the distribution may be written as a product of the individual probability density functions of the measurements and predicted vehicle motions. Let the number of poses be
N,
where each pose is represented as zk fork = 0... N.
Let the number of observations be M. Each observation is represented as Zj,k with subscripts denotingthe association of the measurement to an estimated map landmark
j
and observed from vehicle pose k.P(X, C) = P(XjC)
N M
=
P(xo) H7 P(xkXk_1, UO)fJ
P(zj,klXk, my)(2.5)
k=1 j=1
We wish to compute the estimated states that maximize the joint probability distribution given in Equation 2.5. In other words, we wish to find the maximum a posteriori (MAP) estimate of the states X (trajectory, landmarks, etc) given the uncertain constraints in C as shown in Equation 2.6. Given the Gaussian nature of the distributions comprising the terms within Equation 2.5, one may apply a monotonic log-likelihood transformation as shown in Equation 2.8. The transformation reduces the product of exponentials to a summation of inner products, and constant factors with no effect to optimization are removed. The result is a weighted non-linear least squares objective function shown in Equation 2.9, which is a well-known form for optimization with many solution methods available [43]. Here we use E to represent the value of the objective function (error) we wish to minimize, and clarify the source of error for the two summation terms.
CHAPTER 2. THE 3D MAPPING PROBLEM
X* = argmaxP(X,C)
x= argmaxP(XIC)
(2.6)
x N-1 MX*
=argmax P(xo)
j P(xklXk_1,Uk)H
P(zJ,klxk, mj) (2.7)X k=1 j=1
X* = arg min - log P(X, C) x
N-1 M
arg min -log P(xo) -
>
log P(XzlXk_1,Uk) -ElogP(zy,klxkm
3 )X k=1 k=1
arg min |xo - |
+...
(2.8)k=1 Xk - f(xklxk-1, Uk)Q + y 1j-
h(xk,
mj)predictive error measurement error
v'riur
_ _ N-1 M
E
lxo
-0112
+ 1: Ilxk
-f(XklXkUk) 12 +
(||zE,k
-
h(xk,
m)
1||2 (2.9)
k=1 j=1
In the preceding equations we have introduced the subscript notation for covariance weighted inner products, also known as Mahalanobis distances.
||yllI
yTA-iy
(2.10)A variety of techniques exist for computing the solution to the probabilistic
formulation given in Equation 2.9. Early work formulated the problem graphi-cally as Markov Random Fields [33,82], and inference methods were developed to solve MRF problems. Later work in the SLAM research provided insights into the
2.1. PROBABILISTIC STATE ESTIMATION FOR MAPPING
problem structure. Specifically, using the naturally sparse structure for information matrix [158, 159] for the full-SLAM optimization [43] and the well known matrix-graph duality, solutions using matrix-graphical methods emerged in the last decade. Ex-amples include SEIF [153], Graphical SLAM [63], GraphSLAM [153], square root Smoothing and Mapping (SAM) [43] and variants [79,114], Pose Graph Optimization
(PGO) [119], Sparse Bundle Adjustment (SBA) [85], etc. The smoothing approaches,
such as GraphSLAM, SBA, PGO, and SAM, have the advantage of exactness, when using non-linear models, over the earlier filtering approaches, such as SEIF. Smooth-ing methods iteratively update linearizations of the non-linear models, and filterSmooth-ing approaches linearize once. The more recent graphical smoothing methods incorpo-rate optimizations for speed, efficiency, and scalability. Square root smoothing and mapping (SAM) incorporates fast matrix column reordering to quickly solve the lin-ear equations [43]. Kaess et al. extends SAM to allow incremental incorporation of new constraints in iSAM using Givens rotations to maintain near optimal column ordering [79]. Other approaches use gradient descent methods rather than matrix
reordering for efficient optimization of the linear equations [44,85,119].
For clarity, consider a small estimation problem for a two-dimensional vehicle where there are three vehicle states and a single map landmark. In this example, our estimated state is X
=
{xo, x1, x2, rm}, the constraints consist of a prior on the initialpose and two control inputs for state propagation and three landmark observations to yield C = {po , uo, , zO1, z2}. Graphically, the Bayes Net for this example is
shown in Figure 2-1. An alternative graphical form of this example, called a factor graph, is provided in Figure 2-2. Factor graphs are undirected bipartite graphs, where links connect the variable nodes with factor nodes [38]. In Figure 2-2, the square nodes represent the factors and round nodes represent the estimated state variables. The factor graph representation has a clear connection to the mathematical problem
THE 3D MAPPING PROBLEM
formulation, where each factor node has an associated probability distribution factor in the total probability distribution. Mathematically, the factor nodes correspond to the terms in Equation 2.7.
X0 1 X2
Po U0 U1
Figure 2-1: A graphical model of a small localization and mapping problem. There are three vehicle states, each of which make an observation of a map. The initial robot pose has a prior to establish the coordinate origin.
2.2. RELATED MAPPING RESEARCH
Figure 2-2: A factor graph representation of a mapping and localization problem also shown in Figure 2-1. Circles represent state nodes, and small squares represent factors. Relative measurements, such as vehicle prediction or landmark observation, result in binary factors connecting two nodes. The node colors are consistent with Figure 2-1.
2.2
Related Mapping Research
Given the generic 3D mapping problem formulation given in the previous section, this section describes research with robotic mapping with similarities to the work in this thesis. While the general formulation defines the problem, research results vary significantly in implementation. The research objective, platform, sensing capabilities, and environment present significant variability in research results.
First we discuss a selection of topological mapping methods to highlight the trade-offs involved in applications placing greater importance on navigation than on global map consistency. Sibley et al. [140] convey several important considerations for de-signing and evaluating a SLAM system. Their work involved navigation outdoors in large-scale environments using primarily visual sensing. Aside from the technical achievements, the authors make several statements related to their design choices worth discussion. First the authors make the distinction between SLAM systems for
CHAPTER 2. THE 3D MAPPING PROBLEM
surveying tasks and navigation tasks. A primary goal of surveying is generating a consistent map in a global coordinate frame. Navigation tasks do not necessarily require global consistency, as local consistency is sufficient for local navigation, and topological links may convey relations between local areas without requiring global consistency. The authors emphasize that their maps are constructed on Riemannian manifolds rather than a global Euclidean coordinate frame (note that Riemannian manifolds may be considered locally Euclidean). The authors' statement has rele-vance in other SLAM implementations. Namely, global map consistency is not al-ways a necessity to the application. Conversely, surveying applications place high importance on global map consistency.
The ATLAS framework [27] employed a submap-based SLAM system with forced independence between submaps. A topological graph of submaps maintains edges between the submap nodes. Each local map could be considered locally Euclidean, and one must choose a root submap node to embed the graph in a global Euclidean frame. This is similar to the locally consistent mapping in [140], and the features and tradeoffs of ATLAS provide further support to the navigation versus surveying ques-tion. The local submaps of ATLAS indeed provide locally consistent maps, which is crucial to support one of its key features: traversal with multiple competing hypothe-ses. Each independent submap has an origin, whose uncertainty is captured in the uncertain edges of the topological graph. A robot may be well localized within a map while being highly uncertain of global position, so local navigation capabilities do not suffer from global uncertainties. Failed loop closures in ATLAS result in unnecessary new map generation, which has the advantage of robust real-time operation. Instead of experiencing navigation failure, the robot starts a new map and may continue operating.
