Underwater terrain matching method based on robust particle filter

doi:10.11947/j.AGCS.2025.20240469

Abstract

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

CLC Number:

P229

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.

Figures/Tables 17

Fig. 1

Fig. 2

Fig. 3

Fig. 4

Fig. 5

Fig. 6

Tab. 1

Tab. 2

Fig. 7

Fig. 8

Tab. 3

Fig. 9

Fig. 10

Fig. 11

Fig. 12

Tab. 4

Fig. 13

References 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