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

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.