This paper presents InsSo3D, an accurate and efficient method for large-scale 3D Simultaneous Localisation and Mapping (SLAM) using a 3D Sonar and an Inertial Navigation System (INS). Unlike traditional sonar, which produces 2D images containing range and azimuth information but lacks elevation information, 3D Sonar produces a 3D point cloud, which therefore does not suffer from elevation ambiguity. We introduce a robust and modern SLAM framework adapted to the 3D Sonar data using INS as prior, detecting loop closure and performing pose graph optimisation. We evaluated InsSo3D performance inside a test tank with access to ground truth data and in an outdoor flooded quarry. Comparisons to reference trajectories and maps obtained from an underwater motion tracking system and visual Structure From Motion (SFM) demonstrate that InsSo3D efficiently corrects odometry drift. The average trajectory error is below 21cm during a 50-minute-long mission, producing a map of 10m by 20m with a 9cm average reconstruction error, enabling safe inspection of natural or artificial underwater structures even in murky water conditions.

翻译：本文提出InsSo3D，一种利用三维声呐与惯性导航系统实现大规模三维同步定位与建图的精确高效方法。传统声呐生成包含距离与方位信息但缺乏高程信息的二维图像，而三维声呐可生成三维点云，从而避免高程模糊问题。我们引入一个鲁棒的现代化SLAM框架，该框架适配三维声呐数据，并以INS数据作为先验，实现回环检测与姿态图优化。我们在具备真值数据的测试水池及室外淹没采石场中对InsSo3D性能进行评估。与水下运动跟踪系统及视觉运动恢复结构方法获取的参考轨迹和地图对比表明，InsSo3D能有效校正里程计漂移。在长达50分钟的任务中，平均轨迹误差低于21厘米，生成10米×20米的地图平均重建误差为9厘米，即使在浑浊水域条件下也能实现对自然或人工水下结构的安全检测。