Abstract
Due to the exceptional detection capabilities, the forward-looking sonar could be adopted in simultaneous localization and mapping (SLAM) for autonomous underwater vehicle (AUVs). This paper primarily investigates the application of the factor graph optimization SLAM algorithm based on feature maps in AUV. It achieves this by combining the smallest of constant false alarm rate (SO-CFAR) and adaptive threshold (ADT) to filter noise from the forward-looking sonar and extract feature point clouds. Furthermore, a weighted iterative closest point (WICP) algorithm is employed for feature point registration, which is extracted from the sonar image. The experimental result based on field data demonstrates that the proposed method, with an 8.52% improvement in root mean square error (RMSE) compared with dead reckoning (DR).