[发明专利]一种基于鲁棒信息滤波的AUV协同导航方法

权利要求书

1.一种基于鲁棒信息滤波的AUV协同导航方法，其特征是：

步骤一：建立描述AUV导航系统的状态方程和量测方程；

步骤二：对主AUV进行基于鲁棒信息滤波的状态估计，并在有数据包传递给从AUV的时刻将当前时刻信息保存到状态向量中；在数据包传递结束后对状态向量进行信息边缘化处理；

步骤三：对从AUV进行基于鲁棒信息滤波的状态估计，并在有主AUV传递的数据包到达的时刻进行数据接收和处理，进行距离量测更新；在进行量测更新时，首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程；在接收到的数据包被处理后对状态向量进行信息边缘化处理；

步骤四：对主、从AUV的信息滤波状态恢复，得到AUV的导航信息。

2.根据权利要求1所述的基于鲁棒信息滤波的AUV协同导航方法，其特征是所述建立描述AUV导航系统的状态方程和量测方程具体包括：非线性的系统模型为，

其中，状态方程为x_k＝f(x_k-1)+n_k-1，观测方程为z_k＝h(x_k)+v_k，x_k为第k时刻的n维状态向量，表征了AUV的位置信息和速度信息，z_k为第k时刻的m维量测向量，表征了对AUV的方位观测信息，f(·)和h(·)为已知的非线性函数，n_k-1为第k-1时刻n维系统噪声，v_k为第k时刻m维观测噪声，设随机系统噪声n_k-1～N(0,Q_k-1)、q～N(μ,Σ)表示随机向量q服从均值为μ方差为Σ的高斯分布，随机量测噪声v_k～N(0,R_k)，而且n_k-1与v_k不相关。

3.根据权利要求2所述的基于鲁棒信息滤波的AUV协同导航方法，其特征是步骤二具体包括：

对主AUV系统进行基于非信息滤波的状态估计:

(1)一步预测更新

当前时刻主AUV不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，具体方法如下：

设当前状态为：

其中，表示k时刻联合状态向量，它有两部分组成，为k时刻状态，为历史时刻状态；

信息滤波将状态重新定义为：

其中，表示k时刻估计误差协方差，为k时刻信息矩阵、用表示，为k时刻信息向量、用表示；则对于k时刻联合状态矩阵和状态向量有如下表示：

其中表示k时刻联合信息矩阵，分别表示k时刻和历史时刻的信息矩阵，表示k时刻与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量；

一步预测结果表示如下：

其中，表示随机系统噪声的协方差，表示非线性函数f(·)伪系统矩阵，定义如下：

其中，表示和的互协方差，在无迹卡尔曼滤波算法中用sigma采样点表示如下：

其中，均为采样点，2n为总采样个数；

当前时刻主AUV进行数据包传递时，一步预测进行状态扩充，加入当前的状态，具体包括：

首先进行数据包传递，将k时刻数据包信息传递给从AUV，

k时刻传递的数据包表示为：

其中，Λ_T表示上一数据包传递时刻主AUV的信息矩阵，η_T表示上一数据包传递时刻主AUV的信息向量；

信息传递结束后，对Λ_T、η_T及时更新：

将k时刻状态信息扩充到状态向量中，结果如下：

则其对应的信息矩阵和信息向量如下：

(2)量测更新

其中，表示量测噪声v_k的方差，表示k+1时刻的量测向量，代表非线性函数h(·)伪量测矩阵，表示如下：

其中，表示一步预测估计和量测预测的互协方差，表示一步预测误差协方差，均用无迹卡尔曼滤波基本方程求得；

(3)边缘化处理

在量测更新完成后，对状态向量进行信息边缘化处理。

4.根据权利要求3所述的基于鲁棒信息滤波的AUV协同导航方法，其特征是步骤三具体包括：

对从AUV进行基于信息滤波的状态估计，

(1)一步预测更新

当从AUV进行一步预测时，具体算法如下：

其中表示k时刻联合信息矩阵，分别表示k时刻的信息矩阵和历史时刻的信息矩阵，均表示k时刻关联信息矩阵与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量；

一步预测结果表示如下：

其中，表示一步预测信息矩阵，表示一步预测信息向量，表示非线性函数f(·)伪系统矩阵，表示随机系统噪声的方差；

(2)量测更新

当前时刻从AUV未接收到主AUV传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新，具体算法如下：

其中，代表非线性函数h(·)伪量测矩阵，表示量测噪声方差，表示k+1时刻的量测；

当前时刻从AUV接收到从主AUV传递的数据包时，一步预测后进行数据包处理，具体算法如下:

距离量测更新：

首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程，

其中求解如下：

将量测更新方程转变为线性回归问题，具体方程如下：

定义如下变量：

则建立线性回归模型如下所示：

z_k＝M_kx_k+ξ_k

定义M估计代价函数如下所示：

式中，e_k+1,i＝(z_k-M_kx_k)_i，

最小化代价函数的解满足如下条件：

式中，φ(e_k+1,i)＝ρ'(e_k+1,i)，

定义函数如下所示：

ψ(e_k+1,i)＝φ(e_k+1,i)/e_k+1,i

定义矩阵如下：

Ψ＝diag[ψ(e_k+1,i)]

则矩阵Ψ修正量测噪声如下结果：

本地量测信息更新

(3)边缘化处理

在量测更新完成后，对状态向量进行信息边缘化处理。