基于因子图的AUV集群速度估计和协同定位

Velocity estimation and cooperative localization for AUV swarm based on factor graph

随着海洋科技发展,自主水下航行器(AUVs)协作执行任务技术被广泛应用,AUVs的准确定位是实现AUV集群协同作业的基础技术要求.然而,在协同定位系统中,AUVs间的异步时钟会影响测距精度,并且惯性测量系统推算的速度有较大误差.本文针对AUV集群系统中的协同定位问题,提出了利用距离测量和多普勒频移测量进行误差修正的方法.该方法首先针对时钟异步问题对距离测量的影响,利用泰勒算法对时钟参数进行估计,解决了异步时钟问题;然后,建立以位置和速度为变量节点和以距离测量和多普勒频移测量为函数节点的因子图模型,利用因子图的消息传递计算变量的边缘分布,得到位置和速度的估计;最后,针对线性化过程带来的误差,提出根据变量的协方差矩阵构造自适应因子调整置信度,从而改变对变量节点的估计.仿真结果表明,所提时钟参数能够得到良好估计,所提算法能够有效抑制惯性导航系统的累积误差.

With the development of marine technology,autonomous underwater vehicles(AUVs)are widely used for executing collaborative missions.Accurate positioning of AUVs is fundamental for cooperative operation of a AUV swarm.However,in the cooperative positioning system,the asynchronous clock between AUVs affects the ranging accuracy,and the velocities deduced by the inertial measurement system have large errors.In this paper,we propose an error correction method by using distance measurement and Doppler shift measurement for the cooperative positioning problem in AUV cluster system.Firstly,the Taylor algorithm is used to estimate clock parameters,which solves the asynchrony of onboard clocks and in turn addresses the impact of clock asynchrony when obtaining distance measurements.Subsequently,a factor graph model is established with position and velocity information as variable nodes and distance measurement and Doppler frequency shift measurement as function nodes.The marginal distribution of the variables is calculated,and thus acquiring the estimated position and velocity information.Finally,in response to the errors caused by the linearization programs,a self-adapting factor based on the covariance matrix of variables in the message passing process of the factor graph is proposed to change the estimation of the variable nodes.Simulation results demonstrate that the proposed clock parameter can be accurately estimated.Additionally,the proposed algorithm is shown to effectively mitigate the cumulative errors of the inertial navigation system.