Resumen de Sonar scan matching for simultaneous localization and mapping in confined underwater environments
Angelos Mallios
This thesis presents the development of a localization and mapping algorithm for an autonomous underwater vehicle (AUV). It is based on probabilistic scan matching of raw sonar scans within a pose-based SLAM framework. To address the motion-induced distortions affecting the generation of full sector scans, an Extended Kalman Filter (EKF) is used to estimate the robot motion during that scan. The filter uses a constant velocity model with acceleration noise for motion prediction. Velocities from doppler velocity log (DVL) and heading measurements from attitude and heading reference system (AHRS) are fed asynchronously and update the state. The scan is undistorted by compounding the relative robot position in the scan, with the range and bearing measurements of the beams gathered by the sonar. Assuming Gaussian noise, the algorithm is able to estimate the uncertainty of the sonar measurements with respect to a frame located at the center of the scan. For estimating the global trajectory of the vehicle, a second filter, an augmented-state EKF (ASEKF), stores the pose of the vehicle where each full scan was completed. Each new full scan is cross registered with all the previous scans that are in a certain range and a modified pIC algorithm is applied. This technique has a twofold effect: first, we obtain a better estimate of the vehicle's displacement that is then used to update the ASEKF, and second, loop-closing events are updated automatically and simultaneously. In addition, we present a closed form method for estimating the uncertainty of the scan matching result. The proposed method is well suited for confined environments including but not limited to: geological folds, boulder areas, caves, man-made walls and structures, where a horizontal scanning sonar can constantly detect and distinguish its surroundings over most of the vehicle's trajectory. The method was tested with three real-world datasets: one obtained in an abandoned marina during an engineering test mission, and two additional ones in the natural environment of underwater cavern systems. In the marina dataset, the results show the quality of our algorithm by comparing it to the ground truth from a GPS receiver and to other previously published algorithms.For the cavern datasets, the results are compared against fixed ground truth points that the vehicle visits twice along the trajectory it travels. In all the experiments the trajectory correction is notable and the unoptimized algorithm execution time is much faster than the experiment time, indicating the potential of the algorithm for real-time on-board AUV operation.
