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

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

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)

CLC Number:

V249.3

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.

Figures/Tables 7

Fig.1

Fig.2

Fig.3

Fig.4

Fig.5

Table 1

Fig.6

References 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]	Haobo FENG, Qiao HU, Zhenyi ZHAO. AUV swarm path planning based on elite family genetic algorithm [J]. Systems Engineering and Electronics, 2022, 44(7): 2251-2262.
[2]	Yongqi GAO, Weiqiang MA, Linsen ZHANG, Peng WANG, Miao ZHAO. Distributed multi-AUVs cooperative search method [J]. Systems Engineering and Electronics, 2022, 44(5): 1670-1676.
[3]	Ling ZHUANG, Siyang LIU. Improved scheme design of sparse code multiple access transceiver [J]. Systems Engineering and Electronics, 2022, 44(1): 320-326.
[4]	Bing HUA, Yingying LIANG, Rui NI. Spacecraft integrated attitude determination method based on improved factor graph model [J]. Systems Engineering and Electronics, 2021, 43(8): 2273-2281.
[5]	Yanan LI, Haibin HUANG, Liangming CHEN, Yufei ZHUANG, Xiaoli WANG. Energy-optimal three-dimensional path planning for AUV under changing ocean current environment [J]. Systems Engineering and Electronics, 2021, 43(12): 3667-3674.
[6]	LI Juan, ZHANG Bingjian, YANG Lijuan, WANG Mengdi. Multi-AUV target search algorithm with cognitive based adaptive optimization in unknown environment#br# [J]. Systems Engineering and Electronics, 2018, 40(8): 1839-1845.
[7]	GAO Junqiang, TANG Xiaqing, ZHANG Huan, WU Meng. Vehicle INS/GNSS/OD integrated navigation algorithm based on factor graph [J]. Systems Engineering and Electronics, 2018, 40(11): 2547-.
[8]	YAN Wei-sheng, QI Bei-bei, GAO Jian, LI Yong. Homing guidance algorithm for an autonomous underwater vehicle based on dipolar field [J]. Systems Engineering and Electronics, 2016, 38(4): 902-908.
[9]	WANG Lei, CHENG Xiang-hong, RAN Chang-yan, CHEN Hong-mei, HU Jie. Improved multiple model algorithm based on Bayesian network for AUV integrated navigation [J]. Systems Engineering and Electronics, 2015, 37(4): 901-906.
[10]	JIANG Lei, WANG Tong. Array error estimation based on Frobenius norm fitting of clutter data [J]. Systems Engineering and Electronics, 2015, 37(12): 2713-2718.
[11]	LIU Ming-yong, DONG Ting-ting, ZHANG Li-chuan. Underwater SLAM navigation algorithm based on random beacons [J]. Systems Engineering and Electronics, 2015, 37(12): 2830-2834.
[12]	YAN Wei-sheng, ZUO Lei, CUI Rong-xin. Model based adaptive coverage control for multiple autonomous underwater vehicles [J]. Systems Engineering and Electronics, 2015, 37(11): 2574-2578.
[13]	JIANG Lei，WANG Tong. Array error estimation using subspace-based approach [J]. Systems Engineering and Electronics, 2014, 36(4): 656-660.
[14]	XIA Wei-xing， YANG Xiao-dong. High accuracy estimation algorithm for IMU error of under water INS [J]. Systems Engineering and Electronics, 2013, 35(10): 2176-2185.
[15]	YANG Yong-peng, ZHAO Yu-xin, HAO Yan-ling, DU Hang-yuan. Decoupling control system for AUV hovering nearsurface [J]. Journal of Systems Engineering and Electronics, 2012, 34(3): 572-577.

Cooperative positioning and error estimation algorithm based on factor graph

RichHTML

PDF (PC)

Knowledge

Abstract

Cite this article

share this article

Figures/Tables 7

References 19

Related Articles 15

Recommended Articles

Metrics

Comments