水下地形匹配定位抗差粒子滤波算法

doi:10.11947/j.AGCS.2025.20240469

测绘学报 ›› 2025, Vol. 54 ›› Issue (10): 1841-1851.doi: 10.11947/j.AGCS.2025.20240469

水下地形匹配定位抗差粒子滤波算法

李根¹(), 柴洪洲¹(), 靳凯迪², 展昭¹

^1.信息工程大学地理空间信息学院，河南　郑州　450001
^2.军事科学院战争研究院，北京　100091

收稿日期:2024-11-19 修回日期:2025-09-05 出版日期:2025-11-14 发布日期:2025-11-14
通讯作者: 柴洪洲 E-mail:a1145216790@163.com;chaihz1969@163.com
作者简介:李根（2002—），男，博士生，研究方向为水下UUV自主定位。E-mail：a1145216790@163.com
基金资助:
国家自然科学基金(42404010);智能空间信息国家级重点实验室基金(SYS-ZX01-2024-01)

Underwater terrain matching method based on robust particle filter

Gen LI¹(), Hongzhou CHAI¹(), Kaidi JIN², Zhao ZHAN¹

^1.Institute of Surveying and Mapping, Information Engineering University, Zhengzhou 450001, China
^2.Institute of War Studies, Academy of Military Science, Beijing 100091, China

Received:2024-11-19 Revised:2025-09-05 Online:2025-11-14 Published:2025-11-14
Contact: Hongzhou CHAI E-mail:a1145216790@163.com;chaihz1969@163.com
About author:LI Gen (2002—), male, PhD candidate, majors in UUV autonomous navigation. E-mail: a1145216790@163.com
Supported by:
The National Natural Science Foundation of China(42404010);The National Key Laboratory of Intelligent Spatial Information(SYS-ZX01-2024-01)

摘要/Abstract

摘要：

地形匹配辅助导航（TAN）系统可以修正水下无人航行器（UUV）的位置误差，实现UUV水下绝对定位。针对单波束测深值易受粗差影响，导致基于粒子滤波（PF）的TAN系统易发散的问题，本文提出了一种基于抗差粒子滤波的水下地形匹配定位方法。通过分析测深粗差影响UUV水下地形匹配定位的作用机理，在PF地形匹配中引入抗差估计方法，使用IGG Ⅲ函数设置抗差因子，动态调节粗差观测值对后验状态参数的贡献。蒙特卡洛仿真试验表明，导航结束时，相较于标准PF算法，本文算法的精度与稳定性分别提高了12.20%与58.81%。引入抗差因子后，本文算法面对不同类型粗差时，均表现出了良好的精度与稳定性。

关键词: 水下无人航行器, 捷联惯性导航系统, 地形匹配定位, 粒子滤波, 抗差估计

Abstract:

Terrain-aided navigation (TAN) systems are capable of correcting the position errors of unmanned underwater vehicle (UUV), enabling absolute positioning underwater. This paper addresses the issue that single-beam sounding values are susceptible to gross errors, which can degrade the positioning accuracy of TAN systems based on particle filters (PF). To address this, a robust particle filter-based underwater terrain-matching localization method is proposed. By analyzing the mechanism by which gross errors in sounding affect UUV underwater terrain matching, a robust estimation method is introduced into the PF terrain matching, utilizing the IGG Ⅲ function to set robust factors that dynamically adjust the contribution of gross error observations to the posterior state parameters. Monte Carlo simulation experiments show that at the end of navigation, compared to the standard PF algorithm, the accuracy and stability of the proposed algorithm are improved by 12.20% and 58.81%, respectively. After introducing the robust factor, the proposed algorithm demonstrated better accuracy and robustness when facing different types of gross errors.

Key words: unmanned underwater vehicle, strapdown inertial navigation system, terrain-aided navigation, particle filter, robust estimation

中图分类号:

P229

李根, 柴洪洲, 靳凯迪, 展昭. 水下地形匹配定位抗差粒子滤波算法[J]. 测绘学报, 2025, 54(10): 1841-1851.

Gen LI, Hongzhou CHAI, Kaidi JIN, Zhao ZHAN. Underwater terrain matching method based on robust particle filter[J]. Acta Geodaetica et Cartographica Sinica, 2025, 54(10): 1841-1851.

图/表 17

图1

图2

图3

图4

图5

图6

表1

表2

图7

图8

表3

图9

图10

图11

图12

表4

图13

参考文献 29

[1]	杨元喜, 徐天河, 薛树强. 我国海洋大地测量基准与海洋导航技术研究进展与展望[J]. 测绘学报, 2017, 46(1): 1-8. DOI: . doi: 10.11947/j.AGCS.2017.20160519
	YANG Yuanxi, XU Tianhe, XUE Shuqiang. Progresses and prospects in developing marine geodetic datum and marine navigation of China[J]. Acta Geodaetica et Cartographica Sinica, 2017, 46(1): 1-8. DOI: . doi: 10.11947/j.AGCS.2017.20160519
[2]	XUE Shuqiang, XU Tianhe, LIU Yanxiong, et al. Recent advances in marine geodesy of China[J]. Journal of Geodesy and Geoinformation Science, 2023, 6(3): 58-66.
[3]	宋保维, 潘光, 张立川, 等. 自主水下航行器发展趋势及关键技术[J]. 中国舰船研究, 2022, 17(5): 27-44.
	SONG Baowei, PAN Guang, ZHANG Lichuan, et al. Development trend and key technologies of autonomous underwater vehicles[J]. Chinese Journal of Ship Research, 2022, 17(5): 27-44.
[4]	JALAL F, NASIR F. Underwater navigation, localization and path planning for autonomous vehicles: a review[C]//Proceedings of 2021 International Bhurban Conference on Applied Sciences and Technologies. Islamabad: IEEE, 2021: 817-828.
[5]	张伟, 王乃新, 魏世琳, 等. 水下无人潜航器集群发展现状及关键技术综述[J]. 哈尔滨工程大学学报, 2020, 41(2): 289-297.
	ZHANG Wei, WANG Naixin, WEI Shilin, et al. Overview of unmanned underwater vehicle swarm development status and key technologies[J]. Journal of Harbin Engineering University, 2020, 41(2): 289-297.
[6]	杜祯强, 柴洪洲, 向民志, 等. 顾及水声误差的UUVs协同定位构型设计及分析[J]. 测绘学报, 2023, 52(8): 1268-1277. DOI: . doi: 10.11947/j.AGCS.2023.20210597
	DU Zhenqiang, CHAI Hongzhou, XIANG Minzhi, et al. Configuration design and analysis of UUVs cooperative localization with underwater acoustic error[J]. Acta Geodaetica et Cartographica Sinica, 2023, 52(8): 1268-1277. DOI: . doi: 10.11947/j.AGCS.2023.20210597
[7]	SALAVASIDIS G, MUNAFÒ A, HARRIS C A, et al. Terrain-aided navigation for long-endurance and deep-rated autonomous underwater vehicles[J]. Journal of Field Robotics, 2019, 36(2): 447-474.
[8]	张立华, 刘现鹏, 贾帅东, 等. 一种线面组合的水下地形匹配算法[J]. 测绘学报, 2018, 47(10): 1406-1414. DOI: . doi: 10.11947/j.AGCS.2018.20170673
	ZHANG Lihua, LIU Xianpeng, JIA Shuaidong, et al. A line-surface integrated algorithm for underwater terrain matching[J]. Acta Geodaetica et Cartographica Sinica, 2018, 47(10): 1406-1414. DOI: . doi: 10.11947/j.AGCS.2018.20170673
[9]	ALEXANDRIS C, PAPAGEORGAS P, PIROMALIS D. Positioning systems for unmanned underwater vehicles: a comprehensive review[J]. Applied Sciences, 2024, 14(21): 9671.
[10]	靳凯迪, 柴洪洲, 宿楚涵, 等. DVL/SINS组合导航技术发展现状及趋势[J]. 导航定位学报, 2022, 10(2): 1-12.
	JIN Kaidi, CHAI Hongzhou, SU Chuhan, et al. Research status and trends of DVL/SINS integrated navigation technology[J]. Journal of Navigation and Positioning, 2022, 10(2): 1-12.
[11]	TURAN B, ÜNSAL H O. Flight test results of terrain referenced aircraft navigation with laser altimeter[C]//Proceedings of 2023 IEEE/ION Position, Location and Navigation Symposium. Monterey: IEEE, 2023: 835-840.
[12]	KIM T, KIM J, BYUN S W. A comparison of nonlinear filter algorithms for terrain-referenced underwater navigation[J]. International Journal of Control, Automation and Systems, 2018, 16(6): 2977-2989.
[13]	GUSTAFSSON F, GUNNARSSON F, BERGMAN N, et al. Particle filters for positioning, navigation, and tracking[J]. IEEE Transactions on Signal Processing, 2002, 50(2): 425-437.
[14]	MA Teng, LI Ye, ZHAO Yuxin, et al. An AUV localization and path planning algorithm for terrain-aided navigation[J]. ISA Transactions, 2020, 103: 215-227.
[15]	刘东东. 基于粒子滤波的海底地形辅助导航技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2022.
	LIU Dongdong. Research on submarine terrain aided navigation technology based on particle filter[D]. Harbin: Harbin Engineering University, 2022.
[16]	TICHAVSKY P, STRAKA O, DUNIK J. Grid-based Bayesian filters with functional decomposition of transient density[J]. IEEE Transactions on Signal Processing, 2023, 71: 92-104.
[17]	MATOUŠEK J, DUNÍK J, BRANDNER M. Design of efficient point-mass filter with terrain aided navigation illustration[C]//Proceedings of 2023 International Conference on Information Fusion. Charleston: IEEE, 2023: 1-8.
[18]	MA Teng, DING Shuoshuo, LI Ye, et al. A review of terrain aided navigation for underwater vehicles[J]. Ocean Engineering, 2023, 281: 114779.
[19]	MEDUNA D K, ROCK S M, MCEWEN R. Low-cost terrain relative navigation for long-range AUVs[C]//Proceedings of 2008 OCEANS. Quebec city: IEEE, 2008: 1-7.
[20]	OSTERMAN N, RHEN C. Exploring the sensor requirements for particle filter-based terrain-aided navigation in AUVs[C]//Proceedings of 2020 IEEE/OES Autonomous Underwater Vehicles Symposium. St Johns: IEEE, 2020: 1-6.
[21]	李明叁, 张杰, 严怀志, 等. 序统计滤波估计检测海洋测深异常数据[J]. 海洋测绘, 2007, 27(1): 7-11.
	LI Mingsan, ZHANG Jie, YAN Huaizhi, et al. Order statistics filtering for detecting outliers in depth data along a sounding line[J]. Hydrographic Surveying and Charting, 2007, 27(1): 7-11.
[22]	黄辰虎, 陆秀平, 欧阳永忠, 等. 多波束水深测量误差源分析与成果质量评定[J]. 海洋测绘, 2014, 34(2): 1-6.
	HUANG Chenhu, LU Xiuping, OUYANG Yongzhong, et al. Analysis of error source and quality assessment about multibeam sounding product[J]. Hydrographic Surveying and Charting, 2014, 34(2): 1-6.
[23]	黄谟涛, 翟国君, 王瑞, 等. 海洋测量中异常数据的定位研究[J]. 海洋测绘, 1999, 19(2): 10-19.
	HUANG Motao, ZHAI Guojun, WANG Rui, et al. Study on the location of abnormal data in marine survey[J]. Hydrographic Surveying and Charting, 1999, 19(2): 10-19.
[24]	杨元喜. 自适应动态导航定位[M]. 北京: 测绘出版社, 2006.
	YANG Yuanxi. Adaptive dynamic navigation and positioning[M]. Beijing: Surveying and Mapping Press, 2006.
[25]	杨元喜. 抗差估计理论及其应用[M]. 北京: 八一出版社, 1993.
	YANG Yuanxi. Theory of robust estimation and its application[M]. Beijing: Bayi Press, 1993.
[26]	杨元喜, 何海波, 徐天河. 论动态自适应滤波[J]. 测绘学报, 2001, 30(4): 293-298.
	YANG Yuanxi, HE Haibo, XU Tianhe. Adaptive robust filtering for kinematic GPS positioning[J]. Acta Geodaetica et Cartographic Sinica, 2001, 30(4): 293-298.
[27]	TURAN B, KUTAY A T. Particle filter studies on terrain referenced navigation[C]//Proceedings of 2016 IEEE/ION Position, Location and Navigation Symposium. Savannah: IEEE, 2016: 949-954.
[28]	SHOM. MNT topo-bathymetrique cotier à 20m du détroit du Pas-de-Calais (Projet TANDEM)[EB/OL]. [2024-05-18]. http://dx.doi.org/10.17183/MNT_COTIER_DETROIT_PDC_TANDEM_20m_WGS84.
[29]	黄谟涛, 邓凯亮, 欧阳永忠, 等. 卫星测高重力模型在海空重力测量误差检测中的应用[J]. 华中科技大学学报(自然科学版), 2022, 50(9): 126-133.
	HUANG Motao, DENG Kailiang, OUYANG Yongzhong, et al. Application of satellite altimeter-derived gravity model in the error detection of shipborne and airborne gravimetry[J]. Journal of Huazhong University of Science and Technology (Natural Science Edition), 2022, 50(9): 126-133.

匹配区参数	数值
经度范围	1.188°E—1.263°E
纬度范围	50.718°N—50.767°N
分辨率/m	20
最大水深/m	37.900
最小水深/m	18.770
平均水深/m	25.418
水深标准差/m	2.514

传感器	参数类型	数值
SINS	采样频率/Hz	100
	陀螺零偏/（°/h）	0.2
	角度随机游走/（°/h）	0.1
	加速度计零偏/μg	500
	速度随机游走/（μg/）	200
DVL	采样频率/Hz	1
DVL	测速噪声/（m/s）	0.05
PS	采样频率/Hz	1
PS	观测噪声/m	0.05
SBES	采样频率/Hz	1
SBES	测深噪声/m	0.2

不同时间/s	SINS/DVL		SPF		RPF
不同时间/s	均值/m	标准差/m	均值/m	标准差/m	均值/m	标准差/m
100	41.427 4	9.385 6	9.957 0	13.987 6	9.311 1	11.423 5
200	62.546 2	32.146 6	3.045 8	13.149 3	2.643 2	2.792 2
300	62.353 3	44.877 4	3.600 7	16.405 3	2.919 2	3.380 2
400	47.459 8	30.755 8	2.406 4	8.756 9	1.987 6	2.454 2
500	45.357 0	35.828 0	2.571 8	10.789 9	2.087 6	2.562 0
600	30.916 5	26.089 9	2.144 1	4.634 9	1.841 1	2.266 7

粗差类型	匹配算法	位置误差	标准差
无粗差	SPF	2.041 4	2.503 0
无粗差	RPF	1.993 3	2.453 9
小粗差	SPF	3.244 2	10.148 5
小粗差	RPF	2.472 4	3.008 1
大粗差	SPF	2.245 9	2.777 0
大粗差	RPF	2.224 7	2.724 8

水下地形匹配定位抗差粒子滤波算法

Underwater terrain matching method based on robust particle filter

RichHTML

PDF

可视化

摘要/Abstract

引用本文

使用本文

图/表 17

参考文献 29

相关文章 15

编辑推荐

Metrics

本文评价

[1]	姚朝龙, 游泓锐, 何玄晖, 陆钧雅, 谢依倩, 李琼, 朱双, 罗志才. 综合利用GNSS PWV/垂向形变与GRACE/GRACE-FO数据反演综合干旱指数[J]. 测绘学报, 2025, 54(10): 1757-1768.
[2]	邝英才, 吕志平, 李林阳, 王方超, 许国昌. GNSS-A水下定位的动态非线性Gauss-Helmert模型及其抗差总体卡尔曼滤波算法[J]. 测绘学报, 2023, 52(4): 559-570.
[3]	王泽明, 李浩军, 孙亚峰. GNSS-R潮位监测抗差估计[J]. 测绘学报, 2023, 52(2): 195-205.
[4]	孙悦, 薛树强, 韩保民, 肖圳. 邻近海底基准站坐标时序联合处理模型[J]. 测绘学报, 2023, 52(11): 1835-1843.
[5]	郝雨时, 孙剑伟, 隋心, 徐爱功, 施闯. 顾及ISB/IFB的多GNSS RTK/INS紧组合导航方法[J]. 测绘学报, 2022, 51(11): 2265-2272.
[6]	杨高朝, 王庆, 蔚保国, 刘鹏飞, 李爽. 基于抗差LM的视觉惯性里程计与伪卫星混合高精度室内定位[J]. 测绘学报, 2022, 51(1): 18-30.
[7]	周哲, 胡钊政, 李娜, 肖汉彪, 伍锦祥. 面向智能车的地下停车场环视特征地图构建与定位[J]. 测绘学报, 2021, 50(11): 1574-1584.
[8]	郭迎钢, 李宗春, 何华, 王志颖. 三维坐标转换公共点最优权值的单纯形搜索算法[J]. 测绘学报, 2020, 49(8): 1004-1013.
[9]	尹潇, 柴洪洲, 向民志, 杜祯强. 附加运动学约束的BDS抗差UKF导航算法[J]. 测绘学报, 2020, 49(11): 1399-1406.
[10]	焦博, 郝金明, 刘伟平, 张辉, 温旭峰, 师一帅. 单参考星下全星座单差FCB估计与应用[J]. 测绘学报, 2018, 47(10): 1326-1336.
[11]	李林阳, 崔阳, 王宇谱, 吕志平. 窄巷FCB估计方法改进及时变特性分析[J]. 测绘学报, 2017, 46(1): 34-43.
[12]	陈康慷, 徐天河, 杨玉国, 蔡洪亮, 陈国. iGMAS GNSS钟差产品综合与评估[J]. 测绘学报, 2016, 45(S2): 46-53.
[13]	王宇谱, 吕志平, 王宁, 李林阳, 宫晓春. 顾及卫星钟随机特性的抗差最小二乘配置钟差预报算法[J]. 测绘学报, 2016, 45(6): 646-655.
[14]	陶叶青, 高井祥, 姚一飞. 基于中位数法的抗差总体最小二乘估计[J]. 测绘学报, 2016, 45(3): 297-301.
[15]	王彬, 李建成, 高井祥, 刘超. 抗差加权整体最小二乘模型的牛顿-高斯算法[J]. 测绘学报, 2015, 44(6): 602-608.