Underwater Loop-Closure Detection for Mechanical Scanning Imaging Sonar by Filtering the Similarity Matrix With Probability Hypothesis Density Filter

Robust and accurate estimation of position and attitude of a UUV (Unmanned Underwater Vehicle) from sonar scans is essential for simultaneous localization and mapping (SLAM). Both dead-reckoning based on the inertial navigation system and the motion parameter estimation based on the registration of the ultrasound scan sequence can contribute to the performance of the system. However, the rapidly-growing accumulated error tends to counteract the precise localization of the vehicle. In this paper, a method for loop-closure detection is proposed that adjusts the accumulated error for the underwater acoustic SLAM when the vehicle scans the underwater environment using an Mechanical Scanning Imaging Sonar (MSIS). Firstly, a similarity matrix for pairs of scans is constructed to represent the loop-closing tracks. In the registration step, two novel features, namely the intensity projection histograms and a polar gradient matrix, are extracted to calculate the translational and rotational parameters respectively. Secondly, the probability hypothesis density (PHD) filter is used to extract the possible loop-closure constraints from the similarity matrix, removing the random noise brought by accidental correlation and refining the concurrent loop-closing tracks resulted from long-range scanning. Lastly, the loop-closure constraints from the refined loop-closing tracks are fed into the GraphSLAM system to adjust the pose of each scan by constraint optimization. Experiments on the MSIS sonar images collected in structured and unstructured underwater environments validate the effectiveness of the proposed loop-closure detection method.