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

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

Cooperative positioning and error estimation algorithm based on factor graph

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.