基于EKF-FastSLAM与高斯过程回归的 水下建图优化

崔鹏; 李欣达; 张飞虎; 杜鹏

doi:10.11993/j.issn.2096-3920.2025-0122

基于EKF-FastSLAM与高斯过程回归的水下建图优化

doi: 10.11993/j.issn.2096-3920.2025-0122

崔鹏^1,,
李欣达^1,,
张飞虎^2, ,,
杜鹏^2,

1.
海军大连舰艇学院, 辽宁大连, 116018
2.
西北工业大学, 陕西西安, 710077

详细信息

作者简介:
崔鹏：崔　鹏(1978-), 男, 博士, 副研究员, 主要研究方向为水下小目标检测

通讯作者:
张飞虎(1986-), 男, 博士, 教授, 主要研究方向为水下智能感知.

中图分类号: TP18; P715.5
计量
- 文章访问数: 15
- HTML全文浏览量: 12
- PDF下载量: 11
- 被引次数: 0
出版历程
- 收稿日期: 2025-09-09
- 修回日期: 2025-11-27
- 录用日期: 2025-11-28
- 网络出版日期: 2026-03-16

Optimization of Underwater Mapping Using EKF-FastSLAM and GPR

CUI Peng^1
,,
LI Xinda^1
,,
ZHANG Feihu^{2
, ,},
DU Peng^2
,

1.
Dalian Naval Academy, Dalian 116018, China
2.
Northwesten Polytechnical University, Xi′an 710077, China

摘要

摘要: 随着水下探测技术的发展, 多波束测深声呐(MBES)以其高效测量和高分辨率成为水下地形扫描的关键工具。然而, 在复杂动态水域中, 如何利用声呐数据构建高精度地图仍是一大挑战。针对传统FastSLAM算法在动态环境中易发生粒子退化的问题, 提出了一种基于扩展卡尔曼滤波(EKF)的FastSLAM优化方法, 通过在粒子滤波过程中引入EKF作为建议分布, 有效融合最新观测信息, 减轻粒子退化, 提高滤波器的稳定性和精度。同时, 针对水下测量数据稀疏和重叠不足的情况, 引入高斯过程回归(GPR)进行非线性建模与地图外推, 弥补多波束声呐建图的稀疏性。仿真结果表明, EKF-FastSLAM相较于标准FastSLAM显著减少了轨迹误差, 结合GPR的优化算法进一步提升了地图精度, 湖试试验中实现了米级建图精度。
- 多波束测深声呐 /
- 水下地图构建 /
- 扩展卡尔曼滤波 /
- FastSLAM /
- 高斯过程回归
Abstract: With the advancement of underwater exploration technologies, multibeam echo sounders(MBES) have become a key tool for underwater terrain scanning due to their efficient measurement capabilities and high spatial resolution. However, constructing high-precision maps from sonar data in complex and dynamic aquatic environments remains a significant challenge. To address the issue of particle degradation commonly encountered in traditional FastSLAM algorithms under such conditions, this paper proposes an optimized FastSLAM method based on the Extended Kalman Filter(EKF). By incorporating EKF as a proposal distribution within the particle filtering process, the method effectively integrates the latest observation data, mitigates particle degeneration, and enhances the stability and accuracy of the filter. Furthermore, considering the sparsity and lack of overlap in underwater sonar measurements, Gaussian Process Regression(GPR) is introduced to perform nonlinear modeling and map extrapolation, thereby compensating for the discontinuities in MBES-based mapping. Experimental results demonstrate that the proposed EKF-FastSLAM significantly reduces trajectory errors and improves mapping accuracy compared to standard FastSLAM. The integration of GPR further enhances the overall mapping performance. Field experiments conducted in a lake environment confirm that the proposed method achieves meter-level mapping precision.
- multibeam echo sounder /
- underwater mapping /
- /
- FastSLAM /
- Gaussian process regression

HTML全文

图 1 多波束声呐观测示意图

Figure 1. Schematic Diagram of Multibeam Sonar Observation

下载: 全尺寸图片幻灯片

图 2 粒子轨迹图

Figure 2. Particle trajectory plot

下载: 全尺寸图片幻灯片

图 3 不同方法的有效粒子数

Figure 3. Effective particle count for different methods

下载: 全尺寸图片幻灯片

图 4 2种方法路径结果对比

Figure 4. Comparison of Trajectory Results Between Two Methods

下载: 全尺寸图片幻灯片

图 5 某一粒子的GPR预测结果

Figure 5. GPR prediction result of a single particle

下载: 全尺寸图片幻灯片

图 6 地图“补丁”对齐

Figure 6. Alignment of map “Patch”

下载: 全尺寸图片幻灯片

图 7 数据采集平台

Figure 7. Data acquisition platform

下载: 全尺寸图片幻灯片

图 8 系统组成示意图

Figure 8. Diagram of system architecture

下载: 全尺寸图片幻灯片

图 9 粒子数目变化

Figure 9. Variation in Particle Number

下载: 全尺寸图片幻灯片

图 10 Cholesky因子最大值

Figure 10. Maximum value of Cholesky factor

下载: 全尺寸图片幻灯片

图 11 实时地图结果

Figure 11. Real-time mapping result

下载: 全尺寸图片幻灯片

图 12 位置误差

Figure 12. Position Error

下载: 全尺寸图片幻灯片

表 1 粒子相异性

Table 1. Particle Diversity

次数 FastSLAM EKF-FastSLAM

1 0.39 1.03
2 0.25 0.96
3 0.31 1.00
4 0.37 0.91
5 0.21 0.96
6 0.28 1.02
7 0.35 0.93
8 0.48 1.05
9 0.36 0.95
10 0.32 0.96

下载: 导出CSV

表 2 路径误差

Table 2. Trajectory Error

$ \mathrm{d}{x}^{} $ $ \mathrm{d}{y}^{} $ $ \sqrt{\mathrm{d}{x}^{2}+\mathrm{d}{y}^{2}} $

FastSLAM 6.25 8.61 10.64
EKF-FastSLAM 3.87 5.25 6.52

下载: 导出CSV

表 3 SLAM试验中使用的参数

Table 3. Parameters used in the SLAM experiment

参数值

粒子寿命/s 4
重采样步数 6
最小模型粒子年龄/s 180
粒子最大子代数 2
最大粒子数 16
最小粒子数 0

下载: 导出CSV

参考文献(31)

[1]	Rizzo A, De Giosa F, Donadio C, et al. Morpho-bathymetric acoustic surveys as a tool for mapping traces of anthropogenic activities on the seafloor: The case study of the Taranto Area, Southern Italy[J]. Marine Pollution Bulletin, 2022, 185: 114314. doi: 10.1016/j.marpolbul.2022.114314
[2]	Scardino G, De Giosa F, D’Onghia M, et al. The footprints of the wreckage of the Italian Royal Navy Battleship Leonardo Da Vinci on the Mar Piccolo Sea-Bottom (Taranto, Southern Italy)[J]. Oceans, 2020, 1(2): 77-93. doi: 10.3390/oceans1020007
[3]	Zhang H, Zhang S, Wang Y, et al. Subsea pipeline leak inspection by autonomous underwater vehicle[J]. Applied Ocean Research, 2021, 107: 102321. doi: 10.1016/j.apor.2020.102321
[4]	Weber T C. A CFAR detection approach for identifying gas bubble seeps with multibeam echo sounders[J]. IEEE Journal of Oceanic Engineering, 2021, 46(4): 1346-1355. doi: 10.1109/JOE.2021.3056910
[5]	张万远. 基于多波束测深声呐的泄漏气体检测与量化技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2023.
[6]	Lerda O, Mian A, Ginolhac G, et al. Robust detection for mills cross sonar[J]. IEEE Journal of Oceanic Engineering, 2024, 49(3): 1009-1024. doi: 10.1109/JOE.2024.3374958
[7]	Roman C, Singh H. Improved vehicle based multibeam bathymetry using sub-maps and SLAM[C]//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005: 3662-3669.
[8]	Fairfield N, Kantor G, Wettergreen D. Real-time SLAM with octree evidence grids for exploration in underwater tunnels[J]. Journal of Field Robotics, 2007, 24(1-2): 3-21. doi: 10.1002/rob.20165
[9]	Fairfield N, Wettergreen D. Active localization on the ocean floor with multibeam sonar[C]//OCEANS 2008, 2008: 1-10.
[10]	Barkby S, Williams S, Pizarro O, et al. Incorporating Prior maps with bathymetric distributed particle SLAM for improved AUV navigation and mapping[C]//OCEANS 2009, 2009: 1-7.
[11]	Barkby S, Williams S, Pizarro O, et al. An efficient approach to bathymetric SLAM[C]//2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009: 219-224.
[12]	Barkby S, Williams S B, Pizarro O, et al. Bathymetric SLAM with no map overlap using Gaussian processes[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2011: 1242-1248.
[13]	Barkby S, Williams S B, Pizarro O, et al. A featureless approach to efficient bathymetric SLAM using distributed particle mapping[J]. Journal of Field Robotics, 2011, 28(1): 19-39. doi: 10.1002/rob.20382
[14]	Kim J, Jung H S. An approach towards online bathymetric SLAM[C]//OCEANS’11 MTS/IEEE KONA, 2011: 1-6.
[15]	Stuckey R A. Navigational Error Reduction of Underwater Vehicles with Selective Bathymetric SLAM[J]. IFAC Proceedings Volumes, 2012, 45(5): 118-125. doi: 10.3182/20120410-3-PT-4028.00021
[16]	Palomer A, Ridao P, Ribas D. Multibeam 3D underwater SLAM with probabilistic registration[J]. Sensors, 2016, 16(4): 560. doi: 10.3390/s16040560
[17]	Norgren P, Skjetne R. A Multibeam-based SLAM algorithm for iceberg mapping using AUVs[J]. IEEE Access, 2018, 6: 26318-26337. doi: 10.1109/ACCESS.2018.2830819
[18]	Torroba I, Bore N, Folkesson J. A comparison of submap registration methods for multibeam bathymetric mapping[C]//2018 IEEE/OES Autonomous Underwater Vehicle Workshop, 2018: 1-6.
[19]	Krasnosky K, Roman C. A massively parallel implementation of Gaussian process regression for real time bathymetric modeling and simultaneous localization and mapping[J]. Field Robot., 2022, 2(1): 940-970. doi: 10.55417/fr.2022031
[20]	Krasnosky K, Roman C, Casagrande D. A bathymetric mapping and SLAM dataset with high-precision ground truth for marine robotics[J]. The International Journal of Robotics Research, 2022, 41(1): 12-19. doi: 10.1177/02783649211044749
[21]	Tan J, Torroba I, Xie Y, et al. Data-driven loop closure detection in bathymetric point clouds for underwater SLAM[C]//2023 IEEE International Conference on Robotics and Automation, 2023: 3131-3137.
[22]	Torroba I, Cella M, Terán A, et al. Online stochastic variational Gaussian process mapping for large-scale bathymetric SLAM in real time[J]. IEEE Robotics and Automation Letters, 2023, 8(6): 3150-3157. doi: 10.1109/LRA.2023.3264750
[23]	Ma T, Li Y, Wang R, et al. AUV robust bathymetric simultaneous localization and mapping[J]. Ocean Engineering, 2018, 166: 336-349. doi: 10.1016/j.oceaneng.2018.08.029
[24]	Ma T, Li Y, Zhao Y, et al. Robust bathymetric SLAM algorithm considering invalid loop closures[J]. Applied Ocean Research, 2020, 102: 102298. doi: 10.1016/j.apor.2020.102298
[25]	Ma T, Li Y, Zhao Y, et al. Efficient bathymetric SLAM with invalid loop closure identification[J]. IEEE/ASME Transactions on Mechatronics, 2020, 26(5): 2570-2580.
[26]	Zhang Q, Li Y, Ma T, et al. Bathymetric Particle Filter SLAM Based On Mean Trajectory Map Representation[J]. IEEE Access, 2021, 9: 71725-71736. doi: 10.1109/ACCESS.2021.3078854
[27]	Ling Y, Li Y, Ma T, et al. Active bathymetric SLAM for autonomous underwater exploration[J]. Applied Ocean Research, 2023, 130: 103439. doi: 10.1016/j.apor.2022.103439
[28]	Murphy K, Russell S. Rao-blackwellised particle filtering for dynamic Bayesian networks[M]//Sequential Monte Carlo methods in practice. Berlin: Springer, 2001: 499-515.
[29]	Lindsten F. Rao-blackwellised particle methods for inference and identification[D]. Sweden: Linkopings Universitet, 2011.
[30]	宋宇, 李庆玲, 康轶非, 等. 平方根容积Rao-Blackwillised粒子滤波SLAM算法[J]. 自动化学报, 2014, 40(2): 357-367. Song Y, Li Q L, Kang Y F, et al. SLAM with square-root cubature Rao-Blackwillised particle filter[J]. Acta Automatica Sinica, 2014, 40(2): 357-367.
[31]	Duplyakin D, Brown J, Ricci R. Active learning in performance analysis[C]//2016 IEEE International Conference on Cluster Computing, 2016: 182-191.