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

摘要

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

说明书

技术领域

本发明涉及一种通信延迟下的多AUV协同导航滤波方法，属于多AUV协同导航滤波技术领域。

背景技术

海洋覆盖了地球大约2/3的地表面积，蕴藏有大量的资源，是人类社会可持续发展的宝库。海洋与陆地不同，其自然条件十分苛刻，这个人类开发海洋带来了极大的困难。随着微电子、计算机、人工智能、小型导航设备、高精度控制系统、高性能软件等技术的发展，自主水下航行器(Autonomous Underwater Vehicles,AUV)已成为发达国家海洋技术研究的前沿领域。它具有航程远、智能化、隐蔽性、机动性、使命重构性及经济性等特点，无论在民用还是军事方面都有广泛的应用。民用方面如海洋资源勘察与开发、海洋环境时空变化的监测、海底地形地貌调查与勘测、水下设施检查、海洋救险和深海打捞等。军事方面如战区勘察、探测及扫除水雷、海上预警、攻击敌舰船或潜艇、破坏敌能源设施及通信网络等。AUV是海洋工程技术发展的新领域，具有重大的经济和军事价值。

目前，AUV导航方式主要包括：(1)航位推算，设备简单，可独立完成导航任务，但导航误差精度低且随时间累积；(2)惯性导航，具有完全自主的特性，定位精度高，误差随时间积累，但高精度的惯导价格昂贵；(3)水声定位(长、短、超短基线)，这种方法定位精度高，但都需要外部的换能器或换能器阵才能实现导航，且存在基线布置成本大以及基线作用距离有限等缺点；(4)地球物理 导航方法(重力匹配、地磁匹配、测绘图导航等)需要事先提供匹配数据，不适合未知领域；(5)多AUV协同导航是基于网络的导航形式，利用各AUV携带的导航传感器，结合水声通信技术共享传感器的量测信息，通过对AUV间的相对位置关系进行融合来提高导航和定位精度的技术。相对于前几种导航方法，协同导航成本较低，导航区域不受限制。

协同导航滤波算法是多AUV协同导航系统的关键技术之一，其基本原理是利用已有的量测数据和状态估计根据某种数据处理方法对下一时刻系统的状态进行更新。多AUV协同导航导航系统是非线性系统，故系统的滤波通常采用非线性滤波，其中以扩展卡尔曼滤波(EKF)最为常见。但多AUV协同定位系统很大程度上依赖于彼此间的通信、探测和信息共享来提高定位精度。由于水下环境的特殊性，水声通信是目前水下唯一有效的通信方式。各AUV信息处理的能力不同且水声的传播需要一定的时间，所以协同定位所需的量测量普遍存在时间延迟。因此需要考虑水声通信延迟对多AUV协同定位所造成的影响。为了解决此类问题，刘明雍等提出来基于时延的扩展卡尔曼滤波算法(DEKF)，此方法有效的抑制了通信时延对AUV协同导航的精度的影响；姚尧等提出了通信延迟下基于航迹预测的实时更新算法，这种算法由从UUV获得的主UUV通信包中的状态信息利用随机变量总体分布理论得到主UUV的航迹预测，从UUV基于主UUV的航迹预测来实现通信延迟下的量测更新；HermosoCarazo等提出了适用于带一步或两步随机延迟量测的改进扩展卡尔曼滤波器和改进的UKF。

发明内容

本发明的目的是为了克服已有技术存在的不足，提出一种通信延迟下的多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协同导航滤波。

进一步地，本发明计算扩维后t_k时刻从AUV状态Μ_k的一步预测及一步预测的误差方差为：

${\hat{M}}_{k|k-1}=\left(\begin{array}{c}{\hat{\Delta }}_{k|k-1}\\ {\hat{X}}_{k|k-1}\end{array}\right)---\left(2\right)$

$P_{k,k|k-1}^{MM}=E\left\{\left(\begin{array}{c}{\tilde{\Delta }}_{k|k-1}\\ {\tilde{X}}_{k|k-1}\end{array}\right)·\left(\begin{array}{cc}{{\tilde{\Delta }}_{k|k-1}}^T& {{\tilde{X}}_{k|k-1}}^T\end{array}\right)\right\}=\left(\begin{array}{cc}{P}_{k,k|k-1}^{\Delta \Delta }& P_{k,k|k-1}^{\Delta X}\\ {\left(P_{k,k|k-1}^{\Delta X}\right)}^T& P_{k,k|k-1}^{XX}\end{array}\right)---\left(3\right)$

${\hat{X}}_{k|k-1}={\Phi }_{k|k-1}{\hat{X}}_{k-1|k-1}$

其中，和为从AUV在t_k-1时刻对t_k时刻扩维状态的估计和估计误差，Φ_k|k-1为从AUV的一步状态转移矩阵，为t_k-1时刻从AUV对其在t_k-1的状态估计，为的估计误差。

进一步地，本发明计算扩维后t_k时刻从AUV距离量测的一步预测一步预测的误差方差及一步预测协方差为：

${\hat{Z}}_{k|k-1}=h\left({\hat{X}}_{k-d|k-1}\right)$

其中，表示t_k-1时刻从AUV对其在t_k-d时刻的状态估计，h()表示距离函数；

$P_{k,k|k-1}^{zz}=H_{k-d}{P}_{k-d,k-d|k-1}{H}_{k-d}^T+H_{mr}{P}_{mr}{H}_{mr}^T+R$

其中，P_k-d,k-d|k-1为从AUV在t_k-1时刻对t_k-d时刻自身状态估计的误差方差阵，H_k-d、H_mr为距离量测关于主、从AUV状态的雅克比矩阵，R为量测噪声的方差，P_mr为主AUV状态估计的误差方差；

$P_{k,k|k-1}^{MZ}=\left(\begin{array}{c}{P}_{k,k-d|k-1}^{\Delta X}{H}_{k-d}\\ P_{k,k-d|k-1}^{XX}{H}_{k-d}\end{array}\right)$

其中，表示t_k-1时刻对t_k时刻扩维状态估计与t_k-1时刻对t_k-d从AUV状态估计的协方差表示t_k-1时刻对t_k时刻从AUV状态估计与t_k-1时刻对t_k-d时刻从AUV状态估计的协方差。

有益效果：

本发明的滤波方法根据主、从AUV间的通信延迟对从AUV的状态进行扩维，并基于扩维后的卡尔曼滤波基本方程，获得待估计的状态和状态估计误差方差；该方法能够有效的抑制由于通信延迟所引起的定位误差的增长，定位精度相较于传统算法提高较大。

具体实施方式

下面结合具体实施例对本发明进行详细说明。

本发明提供一种通信延迟下的多AUV协同导航滤波方法，包括如下步骤：

步骤一、主AUV在预先约定的t_r时刻向外发射固定频率的声波信号，并在间隔一段时延后将包括自身的位置、方差信息的通信数据包以广播的形式发出； t_k-d时刻从AUV检测到主AUV通信的声波信号，并由该时刻与主AUV发送时刻的时间差解算出主、从AUV间的相对距离观测量Z_k-d；由于从从AUV接收主AUV的声波信号和数据包存在d步时延，从AUV于t_k时刻获得主AUV的通信数据包(包括主AUV的位置及方差数据)，并根据t_k-d时刻计算的相对距离观测量Z_k-d进行卡尔曼滤波估计，即t_k时刻从AUV进行滤波所利用的距离量测Z_k实则为t_k-d时刻主从AUV间的距离量测：

$Z_k=Z_{k-d}=\sqrt{{(x_{mr}-x_{k-d})}^2+{(y_{mr}-y_{k-d})}^2}+v_{k-d}=h\left(X_{k-d}\right)+v_{k-d}$

其中，x_mr、y_mr为t_k时刻从AUV获得的主AUV通信数据包中主AUV在t_r时刻的位置信息，x_k-d、y_k-d为从AUV在t_k-d时刻自身的位置信息，量测噪声v_k-d服从方差为R的高斯白噪声分布。将t_k时从AUV状态X_k扩维为Μ_k，定义如下扩展状态向量：

其中，X_k-d+1表示t_k-d+1时刻从AUV状态，并以此类推。

步骤二、计算扩维后t_k时刻从AUV状态Μ_k的一步预测及一步预测的误差方差；具体过程如下：

状态扩维后，从AUV在t_k时刻可获得的上一时刻的状态估计值和误差方差如下：

$\left(\begin{array}{c}{\hat{M}}_{k-1|k-1}=\left(\begin{array}{c}{\hat{X}}_{k-d|k-1}\\ {\hat{\Delta }}_{k|k-1}\end{array}\right)\\ P_{k-1,k-1|k-1}^{MM}=E\left[{\tilde{M}}_{k-1|k-1}{{\tilde{M}}_{k-1|k-1}}^T\right]=E\left[\left(\begin{array}{c}{\tilde{X}}_{k-d|k-1}\\ {\tilde{\Delta }}_{k|k-1}\end{array}\right)\left(\begin{array}{cc}{{\tilde{X}}_{k-d|k-1}}^T& {{\tilde{\Delta }}_{k|k-1}}^T\end{array}\right)\right]\\ =\left(\begin{array}{cc}E\left[{\tilde{X}}_{k-d|k-1}{{\tilde{X}}_{k-d|k-1}}^T\right]& E\left[{\tilde{X}}_{k-d|k-1}{{\tilde{\Delta }}_{k|k-1}}^T\right]\\ E\left[{\tilde{\Delta }}_{k|k-1}{{\tilde{X}}_{k-d|k-1}}^T\right]& E\left[{\tilde{\Delta }}_{k|k-1}{{\tilde{\Delta }}_{k|k-1}}^T\right]\end{array}\right)=\left(\begin{array}{cc}{P}_{k-d,k-d|k-1}^{XX}& P_{k-d,k|k-1}^{X\Delta }\\ {P_{k-d,k|k-1}^{X\Delta }}^T& P_{k,k|k-1}^{\Delta \Delta }\end{array}\right)\end{array}\right)---\left(1\right)$

其中，状态估计和即为从AUV在t_k-1时刻对t_k-d时刻自身状态的估计和估计误差，和为从AUV在t_k-1时刻对t_k时刻扩维状态的估计和估计误差，t_k时刻从AUV状态的一步预测以及一步预测的误差方差阵如下：

${\hat{M}}_{k|k-1}=\left(\begin{array}{c}{\hat{\Delta }}_{k|k-1}\\ {\hat{X}}_{k|k-1}\end{array}\right)---\left(2\right)$

$P_{k,k|k-1}^{MM}=E\left\{\left(\begin{array}{c}{\tilde{\Delta }}_{k|k-1}\\ {\tilde{X}}_{k|k-1}\end{array}\right)·\left(\begin{array}{cc}{{\tilde{\Delta }}_{k|k-1}}^T& {{\tilde{X}}_{k|k-1}}^T\end{array}\right)\right\}=\left(\begin{array}{cc}{P}_{k,k|k-1}^{\Delta \Delta }& P_{k,k|k-1}^{\Delta X}\\ {\left(P_{k,k|k-1}^{\Delta X}\right)}^T& P_{k,k|k-1}^{XX}\end{array}\right)---\left(3\right)$

其中，以及可以依据式(1)从上一时刻的滤波结果中获得。

${\hat{X}}_{k|k-1}={\Phi }_{k|k-1}{\hat{X}}_{k-1|k-1}$

$P_{k,k|k-1}^{\Delta X}=E\left\{\left(\begin{array}{c}{\tilde{X}}_{k-d+1|k-1}\\ .\\ .\\ .\\ {\tilde{X}}_{k-1|k-1}\end{array}\right){\tilde{X}}_{k|k-1}^T\right\}=\left(\begin{array}{c}{P}_{k-d+1,k|k-1}^{XX}\\ .\\ .\\ .\\ P_{k-i,k|k-1}^{XX}\\ .\\ .\\ .\\ P_{k-1,k|k-1}^{XX}\end{array}\right)---\left(4\right)$

$P_{k-i,k|k-1}^{XX}=E[{\tilde{X}}_{k-i|k-1}·{\tilde{X}}_{k|k-1}^T|Z_1^{k-1}]=E[{\Phi }_{k|k-i}^{-1}·({\tilde{X}}_{k|k-1}-{\Gamma }_k·W_k)·{\tilde{X}}_{k|k-1}^T\left|Z_1^{k-1}\right]={\Phi }_{k|k-i}^{-1}·P_{k,k|k-1}^{XX}---\left(5\right)$

其中，为t_k-1时刻从AUV状态的一步预测误差方差阵，Φ_k|k-1为从AUV的一步状态转移矩阵，为t_k-1时刻从AUV的状态估计可依据式(1)获得，为从AUV的状态逆推矩阵。

步骤三、计算扩维后t_k时刻从AUV距离量测的一步预测一步预测的误差方差及一步预测协方差

距离量测的一步预测表达式如下：

${\hat{Z}}_{k|k-1}={\hat{Z}}_{k-d|k-1}=h\left({\hat{X}}_{k-d|k-1}\right)$

从AUV的状态估计可根据式(1)从上一时刻的状态估计中获得，h()表示距离函数；

距离量测一步预测的误差方差：

$P_{k,k|k-1}^{zz}=E\left[{\tilde{Z}}_{k|k-1}{\tilde{Z}}_{k|k-1}^T\right]=H_{k-d}{P}_{k-d,k-d|k-1}{H}_{k-d}^T+H_{mr}{P}_{mr}{H}_{mr}^T+R---\left(6\right)$

P_k-d,k-d|k-1为从AUV在t_k-1时刻对t_k-d时刻自身状态估计的误差方差阵，可由上一时刻的中获得，H_k-d、H_mr为距离量测关于主、从AUV状态的雅克比矩阵，R为量测噪声的方差；P_mr主AUV的误差方差；

距离量测一步预测协方差矩阵：

$P_{k,k|k-1}^{MZ}=E\left[{\tilde{M}}_{k|k-1}{\tilde{Z}}_{k|k-1}^T\right]=E\left[\left(\begin{array}{c}{\tilde{\Delta }}_{k|k-1}\\ {\tilde{X}}_{k|k-1}\end{array}\right){(H_{k-d}{\tilde{X}}_{k-d|k-1}+G_{r,l}{V}_{r,l})}^T\right]=\left(\begin{array}{c}{P}_{k,k-d|k-1}^{\Delta X}{H}_{k-d}\\ P_{k,k-d|k-1}^{XX}{H}_{k-d}\end{array}\right)---\left(7\right)$

其中

$P_{k,k-d|k-1}^{\Delta X}=\left(\begin{array}{c}{P}_{k-d+1,k-d|k-1}^{XX}\\ .\\ .\\ .\\ P_{k-i,k-d|k-1}^{XX}\\ .\\ .\\ .\\ P_{k-1,k-d|k-1}^{XX}\end{array}\right)---\left(8\right)$

可从上一时刻的得到，可由式(3)得到。

步骤四、将步骤二和三所计算的参量带入卡尔曼滤波理论式(9)-式(11)，获取t_k时刻扩维后和状态估计误差方差实现多AUV协同导航滤波。

由卡尔曼滤波理论的推导可得：

$K_k^M=P_{k,k|k-1}^{MZ}{\left(P_{k,k|k-1}^{ZZ}\right)}^{-1}---\left(9\right)$

${\hat{M}}_{k|k}={\hat{M}}_{k|k-1}+K_k^M(Z_k-{\hat{Z}}_{k|k-1})---\left(10\right)$

$P_{k,k|k}^{MM}=P_{k,k|k-1}^{MM}-K_k^M{P}_{k,k|k-1}^{ZZ}{\left(K_k^M\right)}^T---\left(11\right)$

即为扩维后从AUV在t_k时刻的状态估计，和为扩维后卡尔曼滤波的状态增益和估计误差方差阵。式(2)(3)(4)(6)(7)(9)～(11)构成了AEKF的基本方程。待估计的状态和状态估计误差方差可从和中得到。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。