基于因子图的主从式AUV协同定位算法

王苏; 黄鸿殿; 赵健文; 周红进; 李倩

doi:10.13700/j.bh.1001-5965.2024.0378

基于因子图的主从式AUV协同定位算法

doi: 10.13700/j.bh.1001-5965.2024.0378

1.
海军大连舰艇学院航海系，大连 116018
2.
哈尔滨工程大学智能科学与工程学院，哈尔滨 150001

基金项目:

国家自然科学基金(52371368)；黑龙江省自然科学基金(YQ2021E011)

详细信息

通讯作者:
E-mail：zhou_hongjin@126.com

中图分类号: U666.1
计量
- 文章访问数: 343
- HTML全文浏览量: 74
- PDF下载量: 23
- 被引次数: 0
出版历程
- 收稿日期: 2024-06-04
- 录用日期: 2024-08-17
- 网络出版日期: 2024-09-24
- 整期出版日期: 2026-02-28

Master-slave AUV cooperative localization algorithm based on factor graph

1.
Department of Navigation，Dalian Naval Academy，Dalian 116018，China
2.
College of Intelligent Systems Science and Engineering，Harbin Engineering University，Harbin 150001，China

Funds:

National Natural Science Foundation of China (52371368); Heilongjiang Provincial Natural Science Foundation of China (YQ2021E011)

More Information

Corresponding author: E-mail：zhou_hongjin@126.com

摘要

摘要:
针对无人自主水下航行器(AUV)集群高精度导航定位需求，提出一种基于因子图(FG)的主从式AUV协同定位算法。针对主从式AUV协同定位系统，构建系统状态方程和量测方程，并在此基础上构建相应因子图模型；根据和积算法(SPA)推导因子图中各节点间消息传递，通过因子图协同定位算法获得从艇位置变量节点概率密度函数(PDF)。利用陆上小车、GPS、惯性设备及数据链设备构建一主一从式协同定位试验平台并开展实际试验验证，结果表明：所提因子图协同定位算法相对于常规扩展卡尔曼滤波(EKF)协同定位算法，定位精度提高18.60%。同时，试验结果也表明测距误差对协同定位精度有较大影响。
- 无人自主水下航行器 /
- 协同定位 /
- 因子图 /
- 扩展卡尔曼滤波 /
- 数据链
Abstract:
Using factor graph (FG), a master-slave cooperative localization technique is suggested to meet the high-precision positioning needs of autonomous underwater vehicle (AUV) clusters. First, the state equation and measurement equation for a master-slave AUV cooperative localization system are formulated, and a corresponding FG model is constructed. Second, message passing between nodes within the FG model is derived using the sum-product algorithm (SPA), leading to the acquisition of the probability density function (PDF) for the slave AUV’s position. In order to carry out useful experimental verification, a one-master-one-slave cooperative localization test platform is subsequently set up utilizing ground vehicles, GPS, inertial equipment, and data link equipment. The experimental results demonstrate that the proposed cooperative localization algorithm can enhance positioning accuracy by 18.60% compared to the conventional extended Kalman filter (EKF)-based cooperative localization algorithm. Additionally, the results indicate that ranging errors significantly impact the accuracy of cooperative localization
- autonomous underwater vehicle /
- cooperative localization /
- factor graph /
- extended Kalman filter /
- data link

HTML全文

图 1 因子图中各节点消息传递

Figure 1. Message passing between nodes in FG

下载: 全尺寸图片幻灯片

图 2 一主一从式AUV协同定位系统因子图模型

Figure 2. FG model for a cooperative localization system with a master AUV and a slave AUV

下载: 全尺寸图片幻灯片

图 3 试验设备安装示意图

Figure 3. Installation of field test equipment

下载: 全尺寸图片幻灯片

图 4 试验车与试验场地

Figure 4. Field test vehicle and test site

下载: 全尺寸图片幻灯片

图 5 试验车轨迹

Figure 5. Trajectory of field test vehicle

下载: 全尺寸图片幻灯片

图 6 主从车之间真实距离与数据链测距

Figure 6. True distance and measured distance by data link between master vehicle and slave vehicle

下载: 全尺寸图片幻灯片

图 7 数据链测距误差

Figure 7. Distance error measured by data link

下载: 全尺寸图片幻灯片

图 8 补偿后的数据链测距误差

Figure 8. Distance error measured by data link after compensation

下载: 全尺寸图片幻灯片

图 9 从车真实轨迹与估计轨迹

Figure 9. True trajectory and estimated trajectory of slave vehicle

下载: 全尺寸图片幻灯片

图 10 从车定位误差

Figure 10. Localization error of slave vehicle

下载: 全尺寸图片幻灯片

表 1 数据链设备性能指标

Table 1. Performance metrics of data link device

参数数值

工作频段/MHz 1420～1520

测距误差/m 不大于100

测距间隔/s ≤1

有效作用距离/km ≥5

下载: 导出CSV

表 2 不同算法定位误差

Table 2. Localization error of different algorithms

算法定位误差RMS/m

EKFD 35.2208

FGD 28.6680

PFD 26.8719

EKFG 18.3407

FGG 15.1902

PFG 12.1966

下载: 导出CSV

表 3 不同测距噪声下算法定位误差

Table 3. Localization error with different distance varying ranging noise conditions

测距噪声方差/m² 定位误差RMS/m

EKF 因子图

25 19.1722 16.7160

100 24.4727 21.4452

225 30.0917 27.4371

下载: 导出CSV

参考文献(22)

[1]	杜雪, 孙兆栋, 徐成龙, 等. 输入时延与通信时延下AUV集群的牵制控制[J]. 北京航空航天大学学报, 2024, 50(5): 1474-1480. DU X, SUN Z D, XU C L, et al. Pinning control of AUV cluster under input delay and communication delay[J]. Journal of Beijing University of Aeronautics and Astronautics, 2024, 50(5): 1474-1480(in Chinese).
[2]	姚绪梁, 王晓伟. 基于MPC导引律的AUV路径跟踪和避障控制[J]. 北京航空航天大学学报, 2020, 46(6): 1053-1062. YAO X L, WANG X W. Path following and obstacle avoidance control of AUV based on MPC guidance law[J]. Journal of Beijing University of Aeronautics and Astronautics, 2020, 46(6): 1053-1062(in Chinese).
[3]	李鑫滨, 郭力争, 韩松. 一种分布式异构多AUV任务分配鲁棒拍卖算法[J]. 北京航空航天大学学报, 2022, 48(5): 736-746. LI X B, GUO L Z, HAN S. A robust auction algorithm for distributed heterogeneous multi-AUV task assignment[J]. Journal of Beijing University of Aeronautics and Astronautics, 2022, 48(5): 736-746(in Chinese).
[4]	徐博, 白金磊, 郝燕玲, 等. 多AUV协同导航问题的研究现状与进展[J]. 自动化学报, 2015, 41(3): 445-461. XU B, BAI J L, HAO Y L, et al. The research status and progress of cooperative navigation for multiple AUVs[J]. Acta Automatica Sinica, 2015, 41(3): 445-461(in Chinese).
[5]	刘峰, 王宇雄, 陈惠芳, 等. 集成逆超短基线的自主式水下航行器集群协同定位方法[J]. 声学学报, 2023, 48(4): 687-698. LIU F, WANG Y X, CHEN H F, et al. Cooperative localization methods of autonomous underwater vehicle swarm integrated with inverted ultrashort baseline[J]. Acta Acustica, 2023, 48(4): 687-698(in Chinese).
[6]	刘俊成, 张京娟, 冯培德. 基于相互测距信息的机群组网协同定位技术[J]. 北京航空航天大学学报, 2012, 38(4): 541-545. LIU J C, ZHANG J J, FENG P D. Swarming aircraft collaborative localization based on mutual rangings[J]. Journal of Beijing University of Aeronautics and Astronautics, 2012, 38(4): 541-545(in Chinese).
[7]	张然, 宋来亮, 冉龙俊. 一种基于非视距误差补偿的协同定位算法[J]. 北京航空航天大学学报, 2017, 43(7): 1426-1432. ZHANG R, SONG L L, RAN L J. A collaborative localization algorithm based on non-line-of-sight error compensation[J]. Journal of Beijing University of Aeronautics and Astronautics, 2017, 43(7): 1426-1432(in Chinese).
[8]	PI R, CIEŚLAK P, RIDAO P, et al. TWINBOT: autonomous underwater cooperative transportation[J]. IEEE Access, 2021, 9: 37668-37684. doi: 10.1109/ACCESS.2021.3063669
[9]	赵婧旭, 赵晨, 周锋. 基于主从式水下自主航行器移动组网的合作目标定位方法[J]. 电子与信息学报, 2022, 44(6): 1919-1926. ZHAO J X, ZHAO C, ZHOU F. Cooperative target location method based on master-slave autonomous underwater vehicles mobile network[J]. Journal of Electronics & Information Technology, 2022, 44(6): 1919-1926(in Chinese).
[10]	曹正阳, 张冰, 白屹轩, 等. GNSS/INS/VNS组合定位信息融合的多无人机协同导航方法[J]. 兵工学报, 2023, 44(增刊2): 157-166. CAO Z Y, ZHANG B, BAI Y X, et al. Multi-UAV cooperative navigation method based on GNSS/INS/VNS combined positioning information fusion[J]. Acta Armamentarii, 2023, 44(Sup 2): 157-166(in Chinese).
[11]	李闻白, 刘明雍, 张立川, 等. 单领航者相对位移测量的多自主水下航行器协同导航[J]. 兵工学报, 2011, 32(8): 1002-1007. LI W B, LIU M Y, ZHANG L C, et al. Cooperative navigation for multiple autonamous underwater vehicles based on relative displacement measurement with a single leader[J]. Acta Armamentarii, 2011, 32(8): 1002-1007(in Chinese).
[12]	张淏酥, 王盛, 王涛, 等. 一种基于极坐标模型的多AUV协同导航与定位算法[J]. 无人系统技术, 2022, 5(1): 69-79. ZHANG H S, WANG S, WANG T, et al. A cooperative navigation and positioning algorithm of multi-AUVs based on the polar-coordinate model[J]. Unmanned Systems Technology, 2022, 5(1): 69-79(in Chinese).
[13]	FAN S W, ZHANG Y, YU C Y, et al. An advanced cooperative positioning algorithm based on improved factor graph and sum-product theory for multiple AUVs[J]. IEEE Access, 2019, 7: 67006-67017. doi: 10.1109/ACCESS.2019.2918586
[14]	FAN S W, ZHANG Y, HAO Q, et al. Cooperative positioning for multi-AUVs based on factor graph and maximum correntropy[J]. IEEE Access, 2019, 7: 153327-153337. doi: 10.1109/ACCESS.2019.2948864
[15]	LI S X, LI G Y, WANG L, et al. SLAM integrated mobile mapping system in complex urban environments[J]. ISPRS Journal of Photogrammetry and Remote Sensing, 2020, 166: 316-332. doi: 10.1016/j.isprsjprs.2020.05.012
[16]	胡悦, 李旭, 徐启敏, 等. 卫星拒止环境下基于因子图的智能车可靠定位方法[J]. 仪器仪表学报, 2021, 42(11): 79-86. HU Y, LI X, XU Q M, et al. Reliable positioning method of intelligent vehicles based on factor graph in GNSS-denied environment[J]. Chinese Journal of Scientific Instrument, 2021, 42(11): 79-86(in Chinese).
[17]	李倩, 蒋正华, 孙炎, 等. 基于因子图的INS/UWB室内行人紧组合定位技术[J]. 仪器仪表学报, 2022, 43(5): 32-45. LI Q, JIANG Z H, SUN Y, et al. INS/UWB tight integrated localization technology for pedestrian indoor based on factor graph[J]. Chinese Journal of Scientific Instrument, 2022, 43(5): 32-45(in Chinese).
[18]	张玉鹏, 王子璇, 刘剑威, 等. 因子图框架下无人艇主从式协同导航算法[J]. 导航定位学报, 2023, 11(2): 131-138. ZHANG Y P, WANG Z X, LIU J W, et al. Leader-follower cooperative navigation algorithm for unmanned surface vessels based on factor graph optimization[J]. Journal of Navigation and Positioning, 2023, 11(2): 131-138(in Chinese).
[19]	卢健, 陈旭, 罗毛欣, 等. 考虑通信延迟的多自治水下航行器协同定位算法[J]. 控制理论与应用, 2020, 37(9): 2061-2072. LU J, CHEN X, LUO M X, et al. Cooperative localization algorithm considering of communication delay for autonomous underwater vehicles[J]. Control Theory & Applications, 2020, 37(9): 2061-2072(in Chinese).
[20]	张立川, 屈俊琪, 潘光, 等. 基于几何解释的集群AUV协同定位误差及编队构型分析[J]. 西北工业大学学报, 2020, 38(4): 755-765. doi: 10.1051/jnwpu/20203840755 ZHANG L C, QU J Q, PAN G, et al. Analyzing of cooperative locating error and formation configuration of AUV based on geometric interpretation[J]. Journal of Northwestern Polytechnical University, 2020, 38(4): 755-765(in Chinese). doi: 10.1051/jnwpu/20203840755
[21]	LOELIGER H A. An introduction to factor graphs[J]. IEEE Signal Processing Magazine, 2004, 21(1): 28-41. doi: 10.1109/MSP.2004.1267047
[22]	MOVAGHATI S, ARDAKANI M. Particle-based message passing algorithm for inference problems in wireless sensor networks[J]. IEEE Sensors Journal, 2011, 11(3): 745-754. doi: 10.1109/JSEN.2010.2067209