This work was developed under the need for an acoustic localization system to monitor marine protected areas (MPAs) with the help of autonomous underwater vehicles (AUVs). Although the use of acoustic signals for underwater localization has been previously studied, most of the solutions rely on filter-based optimization, which is prone to linearization problems in long-term applications. Instead, we implemented a Modular Acoustic Graph Simultaneous Localization and Mapping (SLAM) algorithm that, using a factor graph framework, tracks acoustic beacons with either ranges or bearings. In addition, we developed several novel methods, like a delayed-position update for ultra-short baseline (USBL) position factor integration process, an initialization algorithm for acoustic landmarks, and the creation of a new 3D bearing factor that combines two angles. After developing the algorithm, field experiments were carried out in different areas on the coast of Catalonia. Besides the localization, some monitoring tasks were also tested, such as visual mapping of localized landmarks or optical transmission of data with seafloor stations, which helped validate the accuracy of the acoustic localization system. The results of such experiments are presented and discussed.

Robot localization is a fundamental task in achieving true autonomy. Recently, many graph-based navigators have been proposed that combine an inertial measurement unit (IMU) with an exteroceptive sensor applying IMU preintegration to synchronize both sensors. IMUs are affected by biases that also have to be estimated. To increase the navigator robustness when faults appear on the perception system, IMU preintegration can be complemented with linear velocity measurements obtained from visual odometry, leg odometry, or a Doppler Velocity Log (DVL), depending on the robotic application. Moreover, higher grade IMUs are sensitive to the Earth rotation rate, which must be compensated in the preintegrated measurements. In this article, we propose a general purpose preintegration methodology formulated on a compact Lie group to set motion constraints on graph simultaneous localization and mapping problems considering the Earth rotation effect. We introduce the SE$_{N}(3)$group to jointly preintegrate IMU data and linear velocity measurements to preserve all the existing correlation within the preintegrated quantity. Field experiments using an autonomous underwater vehicle equipped with a DVL and a navigational grade IMU are provided and results are benchmarked against a commercial filter-based inertial navigation system to prove the effectiveness of our methodology.

Acoustic perception in underwater environments is challenging due to the low frequency of the acquisition system and multiple and huge sources of noise. Therefore, point clouds built by profiling sonars mounted on Autonomous Underwater Vehicles (AUV) are sparse and noisy. To solve the mapping task, AUVs need a registration algorithm to prevent maps from inconsistencies. Many scan matching algorithms are available, however, a few of them are specialized in acoustic data. In this paper, a probabilistic scan matching methodology based on Gaussian Mixtures Models (GMM) is presented and, for the first time, the Bayesian-GMM algorithm is applied in this context to model acoustic data. The scan matching problem is properly formulated using Lie groups to define pose. In addition, this methodology can return an uncertainty measure for the matching result, which is fundamental in Pose SLAM applications. This tool is implemented in a public C++library11The library repository can be found in https://bitbucket.org/gmmregistration/gmm_registration. that can process in real-time 2D and 3D scans acquired by a profiling sonar. Theoretical justification and results with real data are provided to benchmark our method against the state-of-the-art Normal Distributions Transforms (NDT) technique.