This Is the Published Version of a Chapter

http://www.diva-portal.org This is the published version of a chapter published in Marine Robot Autonomy. Citation for the original published chapter: Fallon, M F., Johannsson, H., Kaess,, M., Folkesson, J., McClelland, H. et al. (2013) Simultaneous Localization and Mapping in Marine Environments. In: Marine Robot Autonomy (pp. 329-372). New York: Springer http://dx.doi.org/10.1007/978-1-4614-5659-9_8 N.B. When citing this work, cite the original published chapter. Permanent link to this version: http://urn.kb.se/resolve?urn=urn:nbn:se:kth:diva-160492 Chapter 8 Simultaneous Localization and Mapping in Marine Environments Maurice F. Fallon, Hordur Johannsson, Michael Kaess, John Folkesson, Hunter McClelland, Brendan J. Englot, Franz S. Hover, and John J. Leonard Abstract Accurate navigation is a fundamental requirement for robotic systems— marine and terrestrial. For an intelligent autonomous system to interact effectively and safely with its environment, it needs to accurately perceive its surroundings. While traditional dead-reckoning filtering can achieve extremely low drift rates, the localization accuracy decays monotonically with distance traveled. Other approaches (such as external beacons) can help; nonetheless, the typical prerogative is to remain at a safe distance and to avoid engaging with the environment. In this chapter we discuss alternative approaches which utilize onboard sensors so that the robot can estimate the location of sensed objects and use these observations to improve its own navigation as well as its perception of the environment. This approach allows for meaningful interaction and autonomy. Three motivating autonomous underwater vehicle (AUV) applications are outlined herein. The first fuses external range sensing with relative sonar measurements. The second application localizes M.F. Fallon () • H. Johannsson • M. Kaess • J.J. Leonard Computer Science and Artificial Intelligence Laboratory, Massachusetts Institute of Technology, Cambridge, MA, USA e-mail: [email protected]; [email protected]; [email protected]; [email protected] J. Folkesson Center for Autonomous Systems, Kungliga Tekniska Hogskolan¨ (KTH), Stockholm, Sweden e-mail: [email protected] H. McClelland Robotics and Mechanisms Laboratory, Virginia Polytechnic Institute of Technology and State University (Virginia Tech), Blacksburg, VA, USA e-mail: [email protected] B.J. Englot • F.S. Hover Department of Mechanical Engineering, Massachusetts Institute of Technology, Cambridge, MA, USA e-mail: [email protected]; [email protected] M.L. Seto (ed.), Marine Robot Autonomy, DOI 10.1007/978-1-4614-5659-9 8, 329 © Springer Science+Business Media New York 2013 330 M.F. Fallon et al. relative to a prior map so as to revisit a specific feature, while the third builds an accurate model of an underwater structure which is consistent and complete. In particular we demonstrate that each approach can be abstracted to a core problem of incremental estimation within a sparse graph of the AUV’s trajectory and the locations of features of interest which can be updated and optimized in real time on board the AUV. 8.1 Introduction In this chapter we consider the problem of simultaneous localization and mapping (SLAM) from a marine perspective. Through three motivating applications, we demonstrate that a large class of autonomous underwater vehicle (AUV) missions can be generalized to an underlying set of measurement constraints which can then be solved using a core pose graph SLAM optimization algorithm known as incremental smoothing and mapping (iSAM) [39]. Good positioning information is essential for the safe execution of an AUV mission and for effective interpretation of the data acquired by the AUV [25, 46]. Traditional methods for AUV navigation suffer several shortcomings. Dead reckoning and inertial navigation systems (INS) are subject to external disturbances and uncorrectable drift. Measurements from Doppler velocity loggers can be used to achieve higher precision, but position error still grows without bound. To achieve bounded errors, current AUV systems rely on networks of acoustic transponders or surfacing for GPS resets, which can be impractical or undesirable for many missions of interest. The goal of SLAM is to enable an AUV to build a map of an unknown environment and concurrently use that map for positioning. SLAM has the potential to enable long-term missions with bounded navigation errors without reliance on acoustic beacons, apriorimaps, or surfacing for GPS resets. Autonomous mapping and navigation is difficult in the marine environment because of the combination of sensor noise, data association ambiguity, navigation error, and modeling uncertainty. Considerable progress has been made in the past 10 years, with new insights into the structure of the problem and new approaches that have provided compelling experimental demonstrations. To perform many AUV missions of interest, such as mine neutralization and ship hull inspection, it is not sufficient to determine the vehicle’s trajectory in post- processing after the mission has been completed. Instead, mission requirements dictate that a solution is computed in real time to enable closed-loop position control of the vehicle. This requires solving an ever-growing optimization problem incre- mentally by only updating quantities that actually change instead of recomputing the full solution—a task for which iSAM is well suited. Each application presents a different aspect of smoothing-based SLAM: • Smoothing as an alternative to filtering: the use of nontraditional acoustic range measurements to improve AUV navigation [18] 8 Simultaneous Localization and Mapping in Marine Environments 331 • Relocalizing in an existing map: localizing and controlling an AUV using natural features using a forward looking sonar [23] • Loop closure used to bound error and uncertainty: combining AUV motion estimates with observations of features on a ship’s hull to produce accurate hull reconstructions [34] A common theme for all three applications is the use of pose graph representations and associated estimation algorithms that exploit the graphical model structure of the underlying problem. First we will overview the evolution of the SLAM problem in the following section. 8.2 Simultaneous Localization and Mapping The earliest work which envisaged robotic mapping within a probabilistic framework was the seminal paper by Smith et al. [68]. This work proposed using an extended Kalman filter (EKF) to estimate the first and second moments of the probability distribution of spatial relations derived from sensor measurements. Moutarlier and Chatila provided the first implementation of this type of algorithm with real data [54], using data from a scanning laser range finder mounted on a wheeled mobile robot operating indoors. The authors noted that the size of the state vector would need to grow linearly with the number of landmarks and that it was necessary to maintain the full correlation between all the variables being estimated; thus, the algorithm scales quadratically with the number of landmarks [11]. The scalability problem was addressed by a number of authors. The sparse extended information filter (SEIF) by Thrun et al. [73] uses the information form of the EKF in combination with a sparsification method. One of the downfalls of that approach was that it resulted in overconfident estimates. These issues were addressed in the exactly sparse delayed-state filters (ESDFs) by Eustice et al. [14,15] and later with the exactly sparse extended information filter (ESEIF) by Walter et al. [78]. Particle filters have also been used to address both the complexity and the data association problem. The estimates of the landmark locations become independent when conditioned on the vehicle trajectory. This fact was used by Montemerlo et al. [53] to implement FastSLAM. The main drawback of particle filters applied to the high-dimensional trajectory estimation is particle depletion. In particular, when a robot completes a large exploration loop and decides upon a loop closure, only a small number of particles with independent tracks will be retained after any subsequent resampling step. In purely localization tasks (with static prior maps) particle filters have been successful. Monte Carlo localization allowed the Minerva robotic museum guide to operate for 44 km over 2 weeks [71]. More recently it has been used by Nuske et al. 332 M.F. Fallon et al. to localize an AUV relative to a marine structure using a camera [59], exploiting GPU-accelerated image formation to facilitate large particle sets. Filtering approaches have some inherent disadvantages when applied to the SLAM problem: measurements are linearized only once based on the current state estimate—at the time the measurement is added. Further, it is difficult to apply delayed measurements or to revert a measurement once it has been applied to the filter. The Atlas framework by Bosse et al. [6] addresses these issues by combining local submaps and a nonlinear optimization to globally align the submaps. Each submap has its own local coordinate frame, so the linearization point cannot deviate as far from the true value as in the case of global parameterization. 8.2.1 Pose Graph Optimization Using Smoothing and Mapping As the field has evolved, full SLAM solutions [47, 72] have been explored to overcome the linearization errors that are the major source of suboptimality of filtering-based approaches. Full SLAM includes the complete trajectory into the estimation problem rather than just the most recent

Load more