基于因子图的协同定位与误差估计算法

doi:10.12305/j.issn.1001-506X.2021.02.24

系统工程与电子技术 ›› 2021, Vol. 43 ›› Issue (2): 499-507.doi: 10.12305/j.issn.1001-506X.2021.02.24

基于因子图的协同定位与误差估计算法

范世伟(), 张亚(), 郝强(), 姜畔(), 于飞()

哈尔滨工业大学仪器科学与工程学院, 黑龙江哈尔滨 150001

收稿日期:2020-06-09 出版日期:2021-02-01 发布日期:2021-03-16
作者简介:范世伟(1993-),男,博士研究生,主要研究方向为组合导航、协同导航。E-mail:fanshiwei@outlook.com|张亚(1987-),女,讲师,博士,主要研究方向为惯性导航、组合导航、协同导航。E-mail:yazhang@hit.edu.cn|郝强(1992-),男,博士研究生,主要研究方向为室内定位、协同导航。E-mail:18745123763@163.com|姜畔(1993-),男,博士研究生,主要研究方向为组合导航、惯性导航。E-mail:jiangpan@hit.edu.cn|于飞(1974-),男,教授,博士,主要研究方向为惯性导航、组合导航、协同导航。E-mail:yufei@hit.edu.cn
基金资助:
国家自然科学基金(51709068);黑龙江省博士后基金(LBH-Z17094)

Cooperative positioning and error estimation algorithm based on factor graph

Shiwei FAN(), Ya ZHANG(), Qiang HAO(), Pan JIANG(), Fei YU()

School of Instrument Science and Engineering, Harbin Institute of Technology, Harbin 150001, China

Received:2020-06-09 Online:2021-02-01 Published:2021-03-16

摘要/Abstract

摘要：

针对多自主水下航行器协同定位系统中从艇的数据融合问题,首先建立了协同定位系统的数学模型,然后分析了速度误差及航向误差对从艇定位误差的影响,同时设计了协同定位及误差估计的因子图模型。接着,提出了基于高斯噪声的协同定位及误差估计算法,利用均值和方差在因子图各节点间传递完成对从艇位置、速度误差和航向误差的估计。为了验证算法的有效性,通过仿真实验和实船试验的离线数据对协同定位及误差估计算法进行验证。结果表明,所提算法可以有效降低从艇的定位误差,在从艇自主定位时尤其明显,大幅提高了从艇的导航定位能力。

关键词: 因子图, 协同定位, 误差估计, 自主水下航行器

Abstract:

Aiming at the data fusion problem of the follower autonomous underwater vehicle(AUV) in the cooperative positioning system of multiple AUVs, the mathematical model of the cooperative positioning system is established firstly. Secondly, the influence of velocity error and heading error on the positioning error of the follower AUV is analyzed, and the factor graph model of the cooperative positioning and error estimation is designed. Then, a cooperative positioning and error estimation algorithm based on Gaussian noise is proposed. The mean value and variance are transferred among the nodes of the factor graph to estimate the position and velocity error and heading error of the follower AUV. In order to verify the effectiveness of the algorithm, the simulation experiment and off-line data of real ship experiment are used to verify the cooperative positioning and error estimation algorithm. The results show that the proposed algorithm can reduce the positioning error of the follower AUV effectively, especially in the autonomous positioning of the follower AUV, improving the navigation and positioning ability of the follower AUV greatly.

Key words: factor graph, cooperative positioning, error estimation, autonomous underwater vehicle (AUV)

中图分类号:

V249.3

范世伟, 张亚, 郝强, 姜畔, 于飞. 基于因子图的协同定位与误差估计算法[J]. 系统工程与电子技术, 2021, 43(2): 499-507.

Shiwei FAN, Ya ZHANG, Qiang HAO, Pan JIANG, Fei YU. Cooperative positioning and error estimation algorithm based on factor graph[J]. Systems Engineering and Electronics, 2021, 43(2): 499-507.

图/表 7

图1

图2

图3

图4

图5

表1

图6

参考文献 19

1	张立川, 徐德民, 刘明雍. 基于双水听器的多自主水下航行器协同导航方法[J]. 系统工程与电子技术, 2011, 33 (7): 1603- 1606. doi: 10.3969/j.issn.1001-506X.2011.07.32
	ZHANG L C , XU D M , LIU M Y . Cooperative navigation for multiple autonomous under-water vehicles based on two hydrophones[J]. Systems Engineering and Electronics, 2011, 33 (7): 1603- 1606. doi: 10.3969/j.issn.1001-506X.2011.07.32
2	CUI R X , YANG C G , LI Y , et al. Adaptive neural network control of AUVs with control input nonlinearities using reinforcement learning[J]. IEEE Trans.on Systems, Man, and Cyberne-tics: Systems, 2017, 47 (6): 1019- 1029. doi: 10.1109/TSMC.2016.2645699
3	KULKARNI I S , POMPILI D . Task allocation for networked autonomous underwater vehicles in critical missions[J]. IEEE Journal on Selected Areas in Communications, 2010, 28 (5): 716- 727. doi: 10.1109/JSAC.2010.100609
4	SHEN C , SHI Y . Distributed implementation of nonlinear model predictive control for AUV trajectory tracking[J]. Automatica, 2020, 115, 108863. doi: 10.1016/j.automatica.2020.108863
5	LIANG Q W , OU J L , XUE Z X , et al. Influences of temperature and salinity on holistic network performability of multi-AUV cooperative systems[J]. ISA Transactions, 2019, 93, 165- 171. doi: 10.1016/j.isatra.2019.03.014
6	刘亚龙.多水下航行器协同定位算法研究[D].哈尔滨:哈尔滨工程大学, 2015.
	LIU Y L. Research on cooperative localization algorithm for multiple autonomous underwater vehicles[D]. Harbin: Harbin Engineering University, 2015.
7	BETHENCOURT A , JAULIN L . Cooperative localization of underwater robots with unsynchronized clocks[J]. Paladyn Journal of Behavioral Robotics, 2013, 4 (4): 233- 244.
8	于飞, 范世伟, 李倩, 等. 主从式USV协同定位系统性能分析[J]. 哈尔滨工业大学学报, 2017, 49 (9): 129- 135.
	YU F , FAN S W , LI Q , et al. Performance analysis of leader-follower USVs cooperative localization system based cooperative coordinate[J]. Journal of Harbin Institute of Technology, 2017, 49 (9): 129- 135.
9	GRENON G , AN P E , SMITH S M , et al. Enhancement of the inertial navigation system for the Morpheus autonomous underwater vehicles[J]. IEEE Journal of Oceanic Engineering, 2001, 26 (4): 548- 560. doi: 10.1109/48.972091
10	ZHANG M Y , CAI W Y . Multi-AUV aided cooperative 3D-localization for underwater sensor networks[J]. Recent Advances in Electrical & Electronic Engineering, 2020, 13 (1): 80- 90.
11	杨建.滚动时域估计及其在多UUV协同定位中的应用[D].哈尔滨:哈尔滨工程大学, 2015.
	YANG J. Moving horizon estimation and application on the cooperation localization of multiple unmanned underwater vehicles[D]. Harbin: Harbin Engineering University, 2015.
12	LEE K H, YU C H, CHOI J W, et al. ToA based sensor localization in underwater wireless sensor networks[C]//Proc.of the SICE Annual Conference, 2008: 1357-1361.
13	KENDOUL F . Survey of advances in guidance, navigation, and control of unmanned rotorcraft systems[J]. Journal of Field Robotics, 2012, 29 (2): 315- 378. doi: 10.1002/rob.20414
14	BAHR A , LEONARD J J , FALLON M F . Cooperative localization for autonomous underwater vehicles[J]. Experimental Robotics, 2008, 39, 387- 395.
15	MCPHAIL S D , PEBODY M . Range-only positioning of a deep-diving autonomous underwater vehicle from a surface ship[J]. IEEE Journal of Oceanic Engineering, 2009, 34 (4): 669- 677. doi: 10.1109/JOE.2009.2030223
16	BAHR A. Cooperative localization for autonomous underwater vehicles[D]. Massachusetts: Massachusetts Institute of Technology, 2009.
17	WEBSTER S E. Decentralized single-beacon acoustic navigation: combined communication and navigation for underwater vehicles[D]. Baltimore: Johns Hopkins University, 2010.
18	张召友.非线性Bayesian滤波及其在SINS/GPS紧耦合导航中的应用研究[D].哈尔滨:哈尔滨工程大学, 2013.
	ZHANG Z Y. Research on nonlinear bayesian filtering and its application for SINS/GPS tightly coupled navigation[D]. Harbin: Harbin Engineering University, 2013.
19	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

算法	EKF	IFGS	FGCPAEE
运行时间/s	0.110 1	0.051 4	0.062 2

[1]	庄陵, 刘思杨. 稀疏码分多址收发端改进方案的设计[J]. 系统工程与电子技术, 2022, 44(1): 320-326.
[2]	华冰, 梁莹莹, 倪瑞. 基于改进因子图模型的航天器组合姿态确定算法[J]. 系统工程与电子技术, 2021, 43(8): 2273-2281.
[3]	李娟, 张秉健, 杨莉娟, 王蒙迪. 未知环境下基于感知自适应的多AUV目标搜索算法[J]. 系统工程与电子技术, 2018, 40(8): 1839-1845.
[4]	高军强, 汤霞清, 张环, 武萌. 基于因子图的车载INS/GNSS/OD组合导航算法[J]. 系统工程与电子技术, 2018, 40(11): 2547-.
[5]	郭士荦, 许江宁, 吴苗. 基于平滑算法的惯导系统误差校正技术[J]. 系统工程与电子技术, 2017, 39(7): 1570-1575.
[6]	唐成凯, 张玲玲, 廉保旺. 基于因子图协同定位辅助的单星定位方法[J]. 系统工程与电子技术, 2017, 39(5): 1085-1090.
[7]	田威, 黄高明. 病态场景下多传感器系统误差的岭估计方法[J]. 系统工程与电子技术, 2017, 39(12): 2704-2708.
[8]	严卫生, 齐贝贝, 高剑, 李勇. 基于偶极势场的自主水下航行器回坞导引算法[J]. 系统工程与电子技术, 2016, 38(4): 902-908.
[9]	刘书君, 吴国庆, 张新征, 沈晓东, 李勇明. 基于线性最小均方误差估计的SAR图像降噪[J]. 系统工程与电子技术, 2016, 38(4): 785-791.
[10]	王广龙, 高伟伟, 张春熹, 陈建辉, 高凤岐. 基于光电瞄准/SINS组合的在线修正方法[J]. 系统工程与电子技术, 2015, 37(6): 1370-1376.
[11]	王磊, 程向红, 冉昌艳, 陈红梅, 胡杰. 贝叶斯网络增强型多模型AUV组合导航算法[J]. 系统工程与电子技术, 2015, 37(4): 901-906.
[12]	奔粤阳, 郭妍, 李敬春, 李倩, 霍亮. 基于联合分布状态信息滤波的机器人协同定位[J]. 系统工程与电子技术, 2015, 37(2): 385-393.
[13]	姜磊, 王彤. 基于杂波数据Frobenius范数拟合的阵元误差估计方法[J]. 系统工程与电子技术, 2015, 37(12): 2713-2718.
[14]	刘明雍, 董婷婷, 张立川. 基于随机信标的水下SLAM导航方法[J]. 系统工程与电子技术, 2015, 37(12): 2830-2834.
[15]	严卫生, 左磊, 崔荣鑫. 基于模型参考的多自主水下航行器自适应覆盖控制[J]. 系统工程与电子技术, 2015, 37(11): 2574-2578.

基于因子图的协同定位与误差估计算法

Cooperative positioning and error estimation algorithm based on factor graph

RichHTML

PDF (PC)

可视化

摘要/Abstract

引用本文

使用本文

图/表 7

参考文献 19

相关文章 15

编辑推荐

Metrics

本文评价