CN108827305A - 一种基于鲁棒信息滤波的auv协同导航方法 - Google Patents

Abstract

本发明提供的是一种基于鲁棒信息滤波的AUV协同导航方法。建立描述AUV导航系统的状态方程和量测方程；对主AUV进行状态估计，并在有数据包传递给从AUV的时刻将当前时刻信息保存到状态向量中；在数据包传递结束后对状态向量进行信息边缘化处理；对从AUV进行基于鲁棒信息滤波的状态估计，并进行数据接收和处理，进行距离量测更新；首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程；对状态向量进行信息边缘化处理；对主、从AUV的信息滤波状态恢复，得到AUV的导航信息。本发明解决了水声通信中量测噪声异常造成的AUV定位精度低的问题，避免了导航信息发散，实现了协同导航的高精度实时定位的目标。

Description

一种基于鲁棒信息滤波的AUV协同导航方法

技术领域

本发明涉及的是一种AUV协同导航方法，属于一种非线性滤波及协同导航方法。

背景技术

自主水下航行器(Autonomous underwater vehicles,AUV)是人类开发海洋环境的重要工具，随着探索活动的深入，应用单体航行器执行任务已经不能满足实际要求。多水下航行器协同作业依靠其强大的优势而受到越来越多的重视。受限于复杂多变的水下环境，多水下航行器高精度导航问题是航行器协同作业系统亟待解决的首要问题。集中式协同导航方法将各个AUV原始的量测信息输送至融合中心处理，在融合中心进行数据融合过程，结构的灵活性差，无法对导航数据进行实时操作，实用性不强，一旦融合中心故障，整个系统都会瘫痪。

为了解决集中式协同导航的问题，研究人员提出了分布式协同导航结构。分布式协同导航方法将各个AUV的量测信息在各自平台上进行实时处理，并充分利用了AUV之间的距离量测信息，对于实时导航来说是更好的选择。

目前，人们在分布式协同导航技术领域的研究处于探索阶段，所用到的滤波方法大多建立在系统量测噪声精确并且为高斯分布的前提下。但是，在AUV在水下作业时，异常杂波会对量测信息产生很大影响，并且在很多时候噪声呈现厚尾分布特性，势必造成状态估计精度下降，严重的情况下可能会造成协同导航算法定位误差发散。

发明内容

本发明的目的在于提供一种能够避免导航信息发散，实现协同导航的高精度实时定位的基于鲁棒信息滤波的AUV协同导航方法。

本发明的目的是这样实现的：

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

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

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

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

本发明还可以包括：

1、所述建立描述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不相关。

2、步骤二具体包括：

对主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)边缘化处理

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

3、步骤三具体包括：

对从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)边缘化处理

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

本发明针对水下通信时异常噪声干扰情况，提出了基于鲁棒无迹信息滤波的AUV协同导航方法。以无迹信息滤波为研究基础，充分考虑在水下作业环境中，量测噪声异常对信息滤波状态估计方法的严重影响，本发明应用M估计方法对异常噪声问题进行处理，给出了基于鲁棒无迹信息滤波的协同导航方法的关键步骤。

本发明的优点在于：

(1)建立了AUV协同导航系统模型，利用AUV之间的距离量测信息，充分考虑了异常噪声问题的存在，给出了一种高精度的基于鲁棒信息滤波的协同导航方法。

(2)充分考虑了异常噪声的呈现特点，以异常杂波和厚尾噪声两种形式为研究对象，与基于信息滤波的算法进行对比仿真，充分验证了基于鲁棒信息滤波方法的优越性。

附图说明

图1是本发明的方法流程图；

图2是异常杂波情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对x坐标轴方向位置估计的误差曲线对比图；

图3是异常杂波情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对y坐标轴方向位置估计的误差曲线对比图；

图4是厚尾噪声情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对x坐标轴方向位置估计的误差曲线对比图(∈＝0.1)；

图5是厚尾噪声情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对y坐标轴方向位置估计的误差曲线对比图(∈＝0.1)；

图6是厚尾噪声情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对x坐标轴方向位置估计的误差曲线对比图(∈＝0.3)；

图7是厚尾噪声情况下本发明提供的基于鲁棒信息滤波方法与基于信息滤波方法的从AUV导航系统对y坐标轴方向位置估计的误差曲线对比图(∈＝0.3)。

具体实施方式

下面举例对本发明做更详细的描述。

本发明的一种基于鲁棒信息滤波的AUV协同导航方法，包括以下几个步骤：

步骤一：建立描述AUV协同导航系统的状态方程和量测方程。具体为，建立非线性的系统模型如下：

x_k＝f(x_k-1)+n_k-1 (1)

z_k＝h(x_k)+v_k (2)

其中，状态方程为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不相关。

步骤二：对主AUV系统进行基于鲁棒信息滤波的状态估计。

(1)一步预测更新

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

假设当前状态如下：

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

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

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

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

一步预测结果表示如下：

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

其中，表示和互协方差，在UIF算法中可以用sigma采样点表示如下：

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

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

此时要进行数据包传递，把k时刻数据包信息传递给从AUV。数据包传递时，无需传递k时刻的全部信息，只需传递从上一数据包传递时刻到k时刻的增量信息，如此既保证了传递信息完整，又对带宽要求较低。

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

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

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

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

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

(2)量测更新

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

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

(3)边缘化处理

步骤三：对从AUV进行基于信息滤波的状态估计。

(1)一步预测更新

当从AUV进行一步预测时，不需要向主AUV传递数据包，所以不需要进行信息扩充，具体算法如下：

其中表示k时刻联合信息矩阵，分别表示k时刻的信息矩阵和历史时刻的信息矩阵，均表示k时刻关联信息矩阵与历史时刻的关联信息矩阵，表示k时刻联合信息向量，表示k时刻信息向量，表示历史时刻信息向量。注意：此时历史信息即为主AUV数据包传递的信息，由于距离量测信息的加入，使得主、从AUV信息具有相关性。此处，即为考虑信息相关性后的算法。

一步预测结果表示如下：

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

(2)量测更新

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

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

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

注意此时Λ_Δ不可直接相加，需要将矩阵补零再相加，因为传递数据包中不包含最新时刻从AUV的信息，同样，从AUV当前时刻的信息与数据包信息无关。

距离量测更新：

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

其中求解如下：

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

为了采用M估计方法进行求解，定义如下变量：

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

z_k＝M_kx_k+ξ_k (39)

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

式中，e_k+1,i＝(z_k-M_kx_k)_i，函数ρ满足式(5-13)。

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

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

定义函数如下所示：

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

定义矩阵如下：

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

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

本地量测信息更新

(3)边缘化处理

为了保证从AUV状态向量的维数不至于过高，造成计算困难，在量测更新完成后，对状态向量进行信息边缘化处理，具体算法同主AUV信息边缘化过程。

步骤四：对主、从AUV状态向量进行信息恢复，得到AUV的导航信息。

实施例：在AUV协同导航定位时，水声通信条件是必须考虑的限制因素。由于水下环境复杂，水声通信受限，所以结合实际情况，分布式AUV协同导航方法更满足实际要求。但是，现有的分布式方法也面临着诸多问题。本发明提供的方法旨在解决水下噪声异常问题，为AUV提供高精度导航信息。下面以具体实施例子来说明本发明的优越性。具体如下：

本例中我们以两个AUV协同导航系统为例进行说明，其中一个主AUV，一个从AUV，主AUV可以把自身信息和距离量测数据传递给从AUV，从AUV只有数据包接收能力，不传递信息。

在水下导航系统中，AUV的姿态和深度可以分别用相应的传感器测得，并且得到的均为误差有界的导航信息。我们在建模时仅考虑AUV的位置和速度信息，减小状态向量维数，这样更容易满足水下带宽限制要求，状态向量选取如下：

x＝[x y v_x v_y]^T (43)

则建立状态模型和距离量测模型如下：

x_k+1＝F_kx_k+n_k (44)

其中，ΔT为离散模型采样间隔。n_k为第k时刻的系统噪声，n_k～N(0,Q_k)，Q_k＝diag([10m 10m 0.02m/s 0.02m/s])，Q_k表征了系统模型的不确定性。

其中，z_k表示第k时刻的距离量测信息；和表示当前时刻从AUV的位置信息，和表示当前时刻从AUV接收到的主AUV的导航信息，v_k为第k时刻的量测噪声，v_k～N(0,R_k)，R_k＝9m，R_k表征了距离量测的不确定性。

初始真实状态值和初始协方差阵设置如下：

其中x_s和x_c分别表示主AUV和从AUV的初始状态，和分别表示主AUV和从AUV的初始误差协方差值，和表征了目标初始位置的不确定性。

则根据初始状态和协方差设置，可分别计算得到主、从AUV的初始信息矩阵和信息向量，具体结果如下：

实施过程：仿真过程中采用如下定义的均方误差性能指标，比较滤波方法的误差：

其中N为Monte Carlo次数。对AUV导航信息估计的误差值越小则表征定位精度越高，效果越好。

仿真时间为1000秒，进行500次Monte Carlo仿真，验证本发明提供高精度定位信息。

实验1：在实验进行到500s后，分别在500s、1000s、1500s加入持续100s的20m异常量测噪声误差，其他时间段的量测为正常值。为了验证鲁棒算法的估计效果，将该算法与第四章中提出的基于UIF的协同导航算法进行仿真对比，验证加入异常噪声后UIF算法和HUIF算法对于不准确量测的抑制程度。

实验2：在实际的水下环境中，量测噪声还会呈现出非高斯特性，主要表现为厚尾分布。为了验证HUIF滤波对于厚尾分布的量测噪声的有效性，本节用两个标准高斯噪声混合来模拟厚尾分布，加入距离量测噪声中。具体形式如下：

其中，σ₁、σ₂分别为两个标准高斯噪声标准差。在理想条件下，量测噪声呈现为式(53)中前一部分高斯分布，σ₁为其标准差，与上一章仿真实验条件相同，本节仿真实验中设为σ₁＝1；在实际水下环境中，量测噪声呈现厚尾特性，在前一部分标准高斯分布中，加入标准差为σ₂的异常量测，仿真中设为σ₂＝5σ₁。另外，(53)式中的参数∈用来表征厚尾噪声的程度。

Claims (4)

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)边缘化处理

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