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

Cooperative positioning and error estimation algorithm based on factor graph

作　　者：范世伟[1] 张亚 郝强[1] 姜畔[1] 于飞[1] FAN Shiwei;ZHANG Ya;HAO Qiang;JIANG Pan;YU Fei(School of Instrument Science and Engineering,Harbin Institute of Technology,Harbin 150001,China)

机构地区：[1]哈尔滨工业大学仪器科学与工程学院,黑龙江哈尔滨150001

出　　处：《系统工程与电子技术》2021年第2期499-507,共9页Systems Engineering and Electronics

基　　金：国家自然科学基金(51709068);黑龙江省博士后基金(LBH-Z17094)资助课题。

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

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

分 类 号：V249.3[航空宇航科学与技术—飞行器设计]