一种通信延迟下的多AUV协同导航滤波方法

摘要

本发明提供一种通信延迟下的多AUV协同导航滤波方法，包括如下步骤：步骤一、对t_k时刻从AUV状态X_k扩维为Μ_k；步骤二、计算扩维后t_k时刻从AUV状态Μ_k的一步预测及一步预测的误差方差；步骤三、计算扩维后t_k时刻从AUV距离量测的一步预测、一步预测的协方差及一步预测互协方差；步骤四、将步骤二和三所计算的参量带入卡尔曼滤波理论，获取t_k时刻待估计的状态和状态估计误差方差实现多AUV协同导航滤波。该方法根据AUV间的通信时延，对从AUV的状态进行扩维，利用扩维后的状态估计来对延迟的量测进行预测，以解决传统滤波算法(EKF)由于通信延迟所造成的定位失效问题。

主权项

一种通信延迟下的多AUV协同导航滤波方法，其特征在于，包括如下步骤：步骤一、对t_k时刻从AUV状态X_k扩维为Μ_k；$\left\{\begin{array}{c}{\Delta }_k={[{X_{k-d+1}}^T,{X_{k-d+2}}^T...{X_{k-1}}^T]}^T\\ M_k={[{{\Delta }_k}^T,{X_k}^T]}^T\end{array}\right.$其中，d为从AUV从主AUV接收声波信号与接收数据包信号的时间延时；步骤二、计算扩维后t_k时刻从AUV状态Μ_k的一步预测及一步预测的误差方差；步骤三、计算扩维后t_k时刻从AUV距离量测的一步预测、一步预测的协方差及一步预测互协方差；步骤四、将步骤二和三所计算的参量带入卡尔曼滤波理论，获取t_k时刻待估计的状态和状态估计误差方差实现多AUV协同导航滤波。