AUV multi-source information fusion localization method based on robust factor graph

doi:10.11947/j.AGCS.2023.20210735

Abstract

Abstract: Compared with the extended Kalman filtering (EKF) algorithm, factor graph optimization (FGO) shows better stability, flexibility and expansibility for the asynchronous and dynamic change of sensor information carried by AUV. This paper compares the performance of FGO and EKF algorithms applied to AUV multi-sensor information fusion and location firstly, and then proposes an AUV multi-source information fusion location method based on robust factor graph to solve the problem that abnormal observations of sensors affect the positioning accuracy of FGO algorithm in complex underwater environment. Dynamic covariance scaling (DCS) strategy was used to reduce the weight of the gross error factor, and the algorithm was verified by simulating the observed gross error based on the sea data. The ordinary FGO algorithm and DCS anti-difference FGO algorithm were used to calculate the data disturbed by gross error. The statistical results show that the proposed algorithm reduces the plane position error by 14.6% compared with that without anti-error processing, and has good robustness for abnormal observation.

Key words: factor graph, multi-source information fusion localization, autonomous underwater vehicle, robust factor graph

CLC Number:

P229.2

HUANG Ziru, CHAI Hongzhou, XIANG Minzhi, DU Zhenqiang. AUV multi-source information fusion localization method based on robust factor graph[J]. Acta Geodaetica et Cartographica Sinica, 2023, 52(8): 1278-1285.

References

[1] YANG Yuanxi, XU Tianhe, XUE Shuqiang. Progresses and prospects of marine geodetic datum and marine navigation in China[J]. Journal of Geodesy and Geoinformation Science, 2018(1): 16-24.
[2] 杨元喜, 徐天河, 薛树强. 我国海洋大地测量基准与海洋导航技术研究进展与展望[J]. 测绘学报, 2017, 46(1): 1-8. 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.
[3] XU Chenglong, XU Chunhui, WU Chengdong, et al. Accurate two-step filtering for AUV navigation in large deep-sea environment[J]. Applied Ocean Research, 2021, 115: 102821.
[4] KIM J. Cooperative localization and unknown currents estimation using multiple autonomous underwater vehicles[J]. IEEE Robotics and Automation Letters, 2020, 5(2): 2365-2371.
[5] 黄玉龙, 张勇刚, 赵玉新. 自主水下航行器导航方法综述[J]. 水下无人系统学报, 2019, 27(3): 232-253. HUANG Yulong, ZHANG Yonggang, ZHAO Yuxin. Review of autonomous undersea vehicle navigation methods[J]. Journal of Unmanned Undersea Systems, 2019, 27(3): 232-253.
[6] 白师宇, 赖际舟, 吕品, 等. 基于IMU/ODO预积分的多传感器即插即用因子图融合方法[J]. 中国惯性技术学报, 2020, 28(5): 624-628, 637. BAI Shiyu, LAI Jizhou, LV Pin, et al. Multisensory plug-and-play factor graph fusion method based on IMU/odometer preintegration[J]. Journal of Chinese Inertial Technology, 2020, 28(5): 624-628, 637.
[7] 马晓爽, 刘锡祥, 张同伟, 等. 基于因子图的AUV多传感器组合导航算法[J]. 中国惯性技术学报, 2019, 27(4): 454-459. MA Xiaoshuang, LIU Xixiang, ZHANG Tongwei, et al. AUV multi-sensor integrated navigation algorithm based on factor graph[J]. Journal of Chinese Inertial Technology, 2019, 27(4): 454-459.
[8] WEN W, PFEIFER T, BAI X, et al. Factor graph optimization for GNSS/INS integration: a comparison with the extended Kalman filter[J]. NAVIGATION, 2021, 68(2): 315-331.
[9] WATSON R M, GROSS J N, TAYLOR C N, et al. Enabling robust state estimation through measurement error covariance adaptation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2020, 56(3): 2026-2040.
[10] SVNDERHAUF N, PROTZEL P. Switchable constraints for robust pose graph SLAM[C]//Proceedings of 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura-Algarve: IEEE, 2012: 1879-1884.
[11] 戴海发, 卞鸿巍, 马恒, 等. 基于因子图的鲁棒性增量平滑算法的水面无人艇组合导航方法[J]. 中国惯性技术学报, 2018, 26(6): 778-786. DAI Haifa, BIAN Hongwei, MA Heng, et al. Application of robust incremental smoothing algorithm based on factor graph in integrated navigation of unmanned surface vehicle[J]. Journal of Chinese Inertial Technology, 2018, 26(6): 778-786.
[12] AGARWAL P, TIPALDI G D, SPINELLO L, et al. Robust map optimization using dynamic covariance scaling[C]//Proceedings of 2013 IEEE International Conference on Robotics and Automation. Karlsruhe: IEEE, 2013: 62-69.
[13] INDELMAN V, WILLIAMS S, KAESS M, et al. Information fusion in navigation systems via factor graph based incremental smoothing[J]. Robotics and Autonomous Systems, 2013, 61(8): 721-738.
[14] DELLAERT F, KAESS M. Factor graphs for robot perception[J]. Foundations and Trends in Robotics, 2017, 6(1/2): 1-139.
[15] KAESS M, JOHANNSSON H, ROBERTS R, et al. iSAM2: incremental smoothing and mapping with fluid relinearization and incremental variable reordering[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai: IEEE, 2011: 3281-3288.