一种基于最大互相关熵无迹粒子滤波的AUV协同导航方法与流程

本发明涉及一种基于最大互相关熵无迹粒子滤波(maximumcorrentropyunscentedparticlefilter,mcupf)的auv协同导航方法，属于非线性滤波及协同导航技术领域。

背景技术：

自主水下航行器(autonomousunderwatervehicles,auv)是一种综合了人工智能、计算机软件、传感器等先进技术的现代科技产物。在海洋资源勘探与开发、水下设备检查、战区侦查等方面都有广泛的应用。由于人类对海洋的探索和开发活动日益深入，仅使用单体auv已经不能满足要求，于是科学家们把目光转移到了多个auv组成的协作系统上。对auv的位置信息进行估计是auv协同导航中一个十分重要的问题，常用的方法之一就是基于状态空间模型的的滤波方法，它能够实现统计意义上的最优估计，获得精度较高的估计信息。基于此，人们提出了许多针对auv协同导航的非线性滤波，例如扩展卡尔曼滤波(extendedkalmanfilter,ekf)、无迹卡尔曼滤波(unscentedkalmanfilter,ukf)、粒子滤波(particlefilter,pf)等。在实际的协同导航中，量测噪声经常出现野值的现象，比如当多普勒计程仪(dopplervelocitylog，dvl)发生水锁现象时会引起厚尾非高斯量测噪声；此外，声源和接收器之间的多声学传播路径导致声波的反射而引起声速随着深度的改变以及对水面和海床的反射也会引起对位置的量测出现野值。

为了解决野值问题，人们提出了许多野值鲁棒滤波。例如，基于huber方法的非线性滤波(hubernonlinearkalmanfilter,hnkf)和基于student’st方法的迭代变分贝叶斯容积卡尔曼滤波(iterationvariationalbayesiancubaturekalmanfilter,ivbckf)，但由于hnkf中影响函数在影响参数γ超过1.345之后不会回降，导致估计性能下降，而ivbckf的性能依赖于student’st分布中尺度矩阵和自由度参数的选取，在实际的协同导航中很难确定真实的尺度矩阵和自由度参数，这会使得ivbckf的精度下降。随后，研究人员提出一类新的用于解决量测噪声野值问题的方法，即基于最大互相关熵的滤波方法，例如最大互相关熵卡尔曼滤波(maximumcorrentropykalmanfilter,mckf)和最大互相关熵无迹卡尔曼滤波(maximumcorrentropyunscentedkalmanfilter,mcukf)。不同于传统的鲁棒滤波方法，互相关熵不仅可以捕获通常的二阶统计信息，还可以获得更高阶的统计信息，因此能获得更好的估计结果。然而，目前人们在auv协同导航领域内应用的最大互相关熵滤波mcukf只适用于高斯噪声情况下，在非高斯噪声情况下，这种方法不再适用。

技术实现要素：

本发明提供了一种基于最大互相关熵无迹粒子滤波的auv协同导航方法，目的在于解决在auv协同导航领域内应用的最大互相关熵滤波mcukf只适用于高斯噪声情况下，在非高斯噪声情况下，这种方法不再适用的问题。

为实现上述目的，本发明提供的方法针对水下通信时异常噪声干扰情况，提出了基于最大互相关熵无迹粒子滤波的auv协同导航方法。以无迹粒子滤波为研究基础，充分考虑在水下作业环境中，量测噪声异常对粒子滤波状态估计方法的严重影响，应用最大互相关熵方法对异常噪声问题进行处理，给出了基于最大互相关熵无迹粒子滤波的协同导航方法的关键步骤。

本发明提供的方法包括以下几个步骤：

步骤一：建立描述auv协同导航系统的状态方程和量测方程。

步骤二：初始化，设置所需要的参数。

步骤三：将auv协同导航的系统量测方程和状态方程重构成非线性递归方程的形式进行处理，然后进行重要性采样。根据步骤二得到的初始粒子，应用mcufk获得下一时刻的后验状态估计和对应的后验状态估计方差，基于此生成一个高斯分布，从此高斯分布中随机抽取n个新的粒子，根据新抽取的粒子计算每个粒子对应的权值并将其归一化。

步骤四：kld重采样，对上一个步骤得到的n个粒子进行筛选，得到最终的粒子集。

步骤五：状态滤波更新。应用步骤四获得的粒子，完成状态的滤波更新，得到协同定位参数的估计状态和估计误差协方差矩阵完成协同导航的任务，其中代表第k时刻重采样后的第i个粒子，代表第k时刻重采样后第i个粒子相对应的权值，代表第k时刻的状态估计值。

本发明的优势在于：

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

(2)为了避免线性化误差，通过将系统状态方程和量测方程重新构建成非线性递归方程的形式进行处理；

(3)通过理论分析与实际运行经验，得到在四维情况下，当量测噪声出现野值时，取σ＝15时，本发明提出的基于mcupf的auv协同导航方法可以获得比现有的基于粒子滤波(particlefilter,pf)、mcukf、hrupf及ivbckf的auv协同导航方法更优的性能，提高auv协同导航的精度和鲁棒性。

附图说明

图1是本发明所述的一种基于mcupf方法的流程图；

图2是本发明提供的方法与基于pf、mcukf、hrupf及ivbckf的auv协同导航方法中对目标位置估计的平均定位误差曲线；

图3是本发明中实验里的速度观测的曲线图；

图4是本发明中实验里的范围量测的曲线图；

图5是本发明中实验里过程噪声和量测噪声的概率密度曲线；

图6是本发明提供的方法与基于pf、mcukf、hrupf及ivbckf的auv协同导航方法的平均定位误差。

具体实施方式

下面结合附图对本发明作进一步的详细说明：

本发明为一种基于最大互相关熵无迹粒子滤波的auv协同导航方法，包括以下几个步骤：

步骤一：建立描述auv协同导航系统的状态方程和量测方程。在此考虑的是主从式auv协同定位模型，此处领航者是通信导航设备(communicationandnavigationaid，cna)。因为can和auv的深度能够被压力传感器精确地测量出来，所以3维的协同定位问题能够被简化为2维的协同定位问题。基于协同定位系统声学测距的状态空间模型具体为：

其中，式(1)为状态方程，式(2)为观测方程为，xk和yk分别为auv第k时刻的东向和北向位置，δt是采样时间，和分别为第k时刻由dvl提供的右舷速度和前向速度，是由罗盘测量出的航向角，xk＝[xk,yk]^t表示auv的位置向量，wk＝[wx,k,wy,k]^t表示过程噪声向量，zk是can和auv之间的相对方位，并且它是使用水声调制解调器并通过到达时间的方法来测量的，是can在第k时刻的位置通过水声调制解调器阶段性地提供，δk代表量测噪声。基于式(1)和式(2)，协同定位系统广义的离散时间状态空间模型(状态方程和量测方程)能被建立如下：

这里状态转移矩阵为f＝i2，其中i2表示2维单位矩阵，控制输入表达式为

量测函数表达式具体为

步骤二：通过理论分析与实际运行经验选择合适的mcukf的核宽度σ并从已知的先验分布中随机抽取n个初始粒子，并设置它们的权值为1/n。

具体为，从已知的先验分布中抽取n个粒子并满足

其中，是n个初始时刻粒子的均值；e是是对变量求均值；p0是n个初始时刻粒子的协方差矩阵。

步骤三：将系统状态方程和量测方程重新构建成非线性递归方程的形式进行处理并完成重要性采样。根据上一步骤抽取的初始粒子，应用mcukf获得下一时刻的后验状态估计和对应的后验状态估计方差，以此状态估计值和方差生成一个高斯分布，从此高斯分布中随机抽取n个新的粒子，根据新抽取的粒子计算每个粒子对应的权值并将其归一化。

具体为，将系统状态方程和量测方程重新构建成非线性递归方程的形式：

进行如下定义：

此处bk是通过cholesky分解相应的矩阵得到的，是第k时刻的一步预测状态值。在等式(4)左右两边同时乘上得

ck＝gk(xk)+εk(8)

此处

此处i是单位矩阵。

基于信息最大熵准则，定义如下的代价函数

这里n+m是ck的维数,ci,k和gi,k(xk)分别是ck和gk(xk)的第i个元素；xk的最优解可以通过如下的步骤得到：

求解上式：

上式等于：

为了简便，可以将其写成矩阵形式：

然后定义

fx,k＝diag(gσ(e1,k),...,gσ(en,k))(18)

fy,k＝diag(gσ(en+1,k),...,gσ(en+m,k))(19)

所以式(17)能够被写成

利用fk对误差协方差重新加权

把(7)和(20)带入(22)得

假设估计状态是无偏的，于是有

所以

fx,k＝im(25)

即

通过上面的推导可知，状态误差协方差不变，而量测误差协方差能够被修正为

将修正得到的量测误差协方差应用于无迹卡尔曼滤波(unscentedkalmanfilter,ukf)的量测更新过程中就能得到mcukf，基于初始粒子，应用mcukf即可获得下一时刻的后验状态估计和对应的后验状态估计方差。

步骤四：kdl重采样，根据每个新粒子的权值对这些新粒子进行重采样，得到重采样后的粒子，并重新设置粒子权值为1/n。

是上一个步骤得到的第k时刻的n个粒子，是它们对应的权值，resample代表重采样，是重采样之后的第k时刻的n个粒子，它们的权值在重采样之后都变为

步骤五：状态滤波更新，应用前面步骤得到的状态和量测一步预测参数，完成状态的滤波更新，得到auv协同导航参数的线性最小方差估计和估计误差协方差矩阵完成auv协同导航的任务。

具体为

此时就可以完成对目标位置参数和速度参数的估计，实现对目标的跟踪。式(29)给出的目标位置参数和式(30)给出的状态估计误差协方差矩阵将用于下一时刻目标位置参数的估计。

下面通过实施例对本发明提供的方法进行说明：由于在实际的协同导航中，对速度和位置进行量测时经常出现野值，这会导致传统的非线性滤波算法精度下降甚至发散，因此需要使用针对野值的非线性鲁棒滤波，通常应用基于mcukf、pf、hrupf及ivbckf的非线性鲁棒滤波方法来获得目标的运动状态。但在量测噪声有野值出现的情况下，上述滤波方法不能满足要求。本发明提供的基于mcupf的auv协同导航方法比现有的方法具备更高的鲁棒性，在量测噪声有野值的情况下能够提高协同导航的精度。下面以具体实施例子来说明本发明的优越性。具体如下：

根据实际进行的湖试来验证算法的有效性。使用3艘勘探船，其中2艘作为湖面领航者，即cnas，剩下一个作为auv。两个领航者和auv都装备有声学调制解调器atm-885，并且通过水下声学调制解调器来广播信息。在每一个时刻，auv只和两个领航者之一交流。因此，在每一时刻只有一个单独的领航者作为can。

图3和图4是本实验中的观测速度和范围量测，从图3和图4可知，速度和范围都有野值存在。为了证明量测野值在过程和量测噪声中的影响，噪声值能够被计算如下：

此处和分别代表近似的东向位置和北向位置噪声值，是近似量测噪声值，是第k时刻由gps提供的位置信息，本次试验中采样时间δt＝1s。基于式(31)能够得到一系列近似过程噪声和量测噪声。图5显示了过程噪声和量测噪声的概率密度曲线。从图5可知，高斯分布不能很好地拟合过程噪声和量测噪声,因为过程噪声和量测噪声都有厚尾分布。表1是本发明提供的方法与基于pf、mcukf、hrupf及ivbckf的auv协同导航方法的平均定位误差。

基于以上的讨论，湖试能够模拟一个auv协同定位的情况，并且能够用于验证算法的有效性和优越性。

初始状态由gps提供，初始的状态估计误差协方差矩阵被设置为p0|0＝i2。标称过程噪声和量测噪声协方差矩阵分别被设置为和rk＝10m²。

实施过程：仿真过程中采用如下定义的性能指标ale来比较各种滤波方法：

其中是auv在第k时刻由gps提供的参考位置，是第k时刻的估计位置，实验时间t＝1760s。对目标位置估计的ale越小则表征估计精度越高，效果越好。

在相同的仿真条件下采用基于mcukf、pf、hrupf、ivbckf的auv协同导航算法和本发明提出的基于mcupf的auv协同导航算法，其中，mcupf和mcukf中的核宽度σ都取15,粒子数n＝1000。