本发明涉及的是一种auv协同导航方法，属于一种非线性滤波及协同导航方法。
背景技术
自主水下航行器(autonomousunderwatervehicles,auv)是人类开发海洋环境的重要工具，随着探索活动的深入，应用单体航行器执行任务已经不能满足实际要求。多水下航行器协同作业依靠其强大的优势而受到越来越多的重视。受限于复杂多变的水下环境，多水下航行器高精度导航问题是航行器协同作业系统亟待解决的首要问题。集中式协同导航方法将各个auv原始的量测信息输送至融合中心处理，在融合中心进行数据融合过程，结构的灵活性差，无法对导航数据进行实时操作，实用性不强，一旦融合中心故障，整个系统都会瘫痪。
为了解决集中式协同导航的问题，研究人员提出了分布式协同导航结构。分布式协同导航方法将各个auv的量测信息在各自平台上进行实时处理，并充分利用了auv之间的距离量测信息，对于实时导航来说是更好的选择。
目前，人们在分布式协同导航技术领域的研究处于探索阶段，所用到的滤波方法大多建立在系统量测噪声精确并且为高斯分布的前提下。但是，在auv在水下作业时，异常杂波会对量测信息产生很大影响，并且在很多时候噪声呈现厚尾分布特性，势必造成状态估计精度下降，严重的情况下可能会造成协同导航算法定位误差发散。
技术实现要素：
本发明的目的在于提供一种能够避免导航信息发散，实现协同导航的高精度实时定位的基于鲁棒信息滤波的auv协同导航方法。
本发明的目的是这样实现的：
步骤一：建立描述auv导航系统的状态方程和量测方程；
步骤二：对主auv进行基于鲁棒信息滤波的状态估计，并在有数据包传递给从auv的时刻将当前时刻信息保存到状态向量中；在数据包传递结束后对状态向量进行信息边缘化处理；
步骤三：对从auv进行基于鲁棒信息滤波的状态估计，并在有主auv传递的数据包到达的时刻进行数据接收和处理，进行距离量测更新；在进行量测更新时，首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程；在接收到的数据包被处理后对状态向量进行信息边缘化处理；
步骤四：对主、从auv的信息滤波状态恢复，得到auv的导航信息。
本发明还可以包括：
1、所述建立描述auv导航系统的状态方程和量测方程具体包括：非线性的系统模型为：
其中，状态方程为xk＝f(xk-1)+nk-1，观测方程为zk＝h(xk)+vk，xk为第k时刻的n维状态向量，表征了auv的位置信息和速度信息，zk为第k时刻的m维量测向量，表征了对auv的方位观测信息，f(·)和h(·)为已知的非线性函数，nk-1为第k-1时刻n维系统噪声，vk为第k时刻m维观测噪声，设随机系统噪声nk-1～n(0,qk-1)、q～n(μ,σ)表示随机向量q服从均值为μ方差为σ的高斯分布，随机量测噪声vk～n(0,rk)，而且nk-1与vk不相关。
2、步骤二具体包括：
对主auv系统进行基于非信息滤波的状态估计:
(1)一步预测更新
当前时刻主auv不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，具体方法如下：
设当前状态为：
其中，
信息滤波将状态重新定义为：
其中，
其中
一步预测结果表示如下：
其中，
其中，
其中，
当前时刻主auv进行数据包传递时，一步预测进行状态扩充，加入当前的状态，具体包括：
首先进行数据包传递，将k时刻数据包信息传递给从auv，
k时刻传递的数据包表示为：
其中，λt表示上一数据包传递时刻主auv的信息矩阵，ηt表示上一数据包传递时刻主auv的信息向量；
信息传递结束后，对λt、ηt及时更新：
将k时刻状态信息扩充到状态向量中，结果如下：
则其对应的信息矩阵和信息向量如下：
(2)量测更新
其中，
其中，
(3)边缘化处理
在量测更新完成后，对状态向量进行信息边缘化处理。
3、步骤三具体包括：
对从auv进行基于信息滤波的状态估计，
(1)一步预测更新
当从auv进行一步预测时，具体算法如下：
其中
一步预测结果表示如下：
其中，
(2)量测更新
当前时刻从auv未接收到主auv传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新，具体算法如下：
其中，
当前时刻从auv接收到从主auv传递的数据包时，一步预测后进行数据包处理，具体算法如下:
距离量测更新：
首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程，
其中
将量测更新方程转变为线性回归问题，具体方程如下：
定义如下变量：
则建立线性回归模型如下所示：
zk＝mkxk+ξk定义m估计代价函数如下所示：
式中，ek+1,i＝(zk-mkxk)i，
最小化代价函数的解满足如下条件：
式中，φ(ek+1,i)＝ρ'(ek+1,i)，
定义函数如下所示：
ψ(ek+1,i)＝φ(ek+1,i)/ek+1,i
定义矩阵如下：
ψ＝diag[ψ(ek+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协同导航系统的状态方程和量测方程。具体为，建立非线性的系统模型如下：
xk＝f(xk-1)+nk-1(1)
zk＝h(xk)+vk(2)
其中，状态方程为xk＝f(xk-1)+nk-1，观测方程为zk＝h(xk)+vk，xk为第k时刻的n维状态向量，表征了auv的位置信息和速度信息，zk为第k时刻的m维量测向量，表征了对auv的方位观测信息，f(●)和h(●)为已知的非线性函数，nk-1为第k-1时刻n维系统噪声，vk为第k时刻m维观测噪声，假设随机系统噪声nk-1～n(0,qk-1)(q～n(μ,σ)表示随机向量q服从均值为μ方差为σ的高斯分布)，随机量测噪声vk～n(0,rk)，而且nk-1与vk不相关。
步骤二：对主auv系统进行基于鲁棒信息滤波的状态估计。
(1)一步预测更新
当前时刻主auv不进行数据包传递时，一步预测不进行状态扩充即不加入当前时刻的状态，具体算法如下：
假设当前状态如下：
其中，
信息滤波将状态重新定义如下：
其中，
其中
一步预测结果表示如下：
其中，
其中，
其中，
当前时刻主auv进行数据包传递时，一步预测进行状态扩充，加入当前的状态，具体算法如下：
此时要进行数据包传递，把k时刻数据包信息传递给从auv。数据包传递时，无需传递k时刻的全部信息，只需传递从上一数据包传递时刻到k时刻的增量信息，如此既保证了传递信息完整，又对带宽要求较低。
k时刻传递的数据包可以表示为：
其中，λt表示上一数据包传递时刻主auv的信息矩阵，ηt表示上一数据包传递时刻主auv的信息向量。
信息传递结束后，需要对λt、ηt及时更新：
将k时刻状态信息扩充到状态向量中，结果如下：
则其对应的信息矩阵和信息向量如下：
(2)量测更新
其中，
其中，
(3)边缘化处理
步骤三：对从auv进行基于信息滤波的状态估计。
(1)一步预测更新
当从auv进行一步预测时，不需要向主auv传递数据包，所以不需要进行信息扩充，具体算法如下：
其中
一步预测结果表示如下：
其中，
(2)量测更新
当前时刻从auv未接收到主auv传递的数据包时，一步预测后不进行数据包处理，直接进行本地更新，具体算法如下：
其中，
当前时刻从auv接收到从主auv传递的数据包时，一步预测后进行数据包处理，具体算法如下:
注意此时
距离量测更新：
首先进行鲁棒滤波过程对距离量测噪声进行修正，接着再进行量测更新过程。
其中
将量测更新方程转变为线性回归问题，具体方程如下：
为了采用m估计方法进行求解，定义如下变量：
则建立线性回归模型如下所示：
zk＝mkxk+ξk(39)
定义m估计代价函数如下所示：
式中，ek+1,i＝(zk-mkxk)i，函数ρ满足式(5-13)。
最小化代价函数的解满足如下条件：
式中，φ(ek+1,i)＝ρ'(ek+1,i)。
定义函数如下所示：
ψ(ek+1,i)＝φ(ek+1,i)/ek+1,i(42)
定义矩阵如下：
ψ＝diag[ψ(ek+1,i)](44)
则矩阵ψ修正量测噪声如下结果：
本地量测信息更新
(3)边缘化处理
为了保证从auv状态向量的维数不至于过高，造成计算困难，在量测更新完成后，对状态向量进行信息边缘化处理，具体算法同主auv信息边缘化过程。
步骤四：对主、从auv状态向量进行信息恢复，得到auv的导航信息。
实施例：在auv协同导航定位时，水声通信条件是必须考虑的限制因素。由于水下环境复杂，水声通信受限，所以结合实际情况，分布式auv协同导航方法更满足实际要求。但是，现有的分布式方法也面临着诸多问题。本发明提供的方法旨在解决水下噪声异常问题，为auv提供高精度导航信息。下面以具体实施例子来说明本发明的优越性。具体如下：
本例中我们以两个auv协同导航系统为例进行说明，其中一个主auv，一个从auv，主auv可以把自身信息和距离量测数据传递给从auv，从auv只有数据包接收能力，不传递信息。
在水下导航系统中，auv的姿态和深度可以分别用相应的传感器测得，并且得到的均为误差有界的导航信息。我们在建模时仅考虑auv的位置和速度信息，减小状态向量维数，这样更容易满足水下带宽限制要求，状态向量选取如下：
x＝[xyvxvy]t(43)
则建立状态模型和距离量测模型如下：
xk+1＝fkxk+nk(44)
其中，
其中，zk表示第k时刻的距离量测信息；
初始真实状态值和初始协方差阵设置如下：
其中xs和xc分别表示主auv和从auv的初始状态，
则根据初始状态和协方差设置，可分别计算得到主、从auv的初始信息矩阵和信息向量，具体结果如下：
实施过程：仿真过程中采用如下定义的均方误差性能指标，比较滤波方法的误差：
其中n为montecarlo次数。对auv导航信息估计的误差值越小则表征定位精度越高，效果越好。
仿真时间为1000秒，进行500次montecarlo仿真，验证本发明提供高精度定位信息。
实验1：在实验进行到500s后，分别在500s、1000s、1500s加入持续100s的20m异常量测噪声误差，其他时间段的量测为正常值。为了验证鲁棒算法的估计效果，将该算法与第四章中提出的基于uif的协同导航算法进行仿真对比，验证加入异常噪声后uif算法和huif算法对于不准确量测的抑制程度。
实验2：在实际的水下环境中，量测噪声还会呈现出非高斯特性，主要表现为厚尾分布。为了验证huif滤波对于厚尾分布的量测噪声的有效性，本节用两个标准高斯噪声混合来模拟厚尾分布，加入距离量测噪声中。具体形式如下：
其中，σ1、σ2分别为两个标准高斯噪声标准差。在理想条件下，量测噪声呈现为式(53)中前一部分高斯分布，σ1为其标准差，与上一章仿真实验条件相同，本节仿真实验中设为σ1＝1；在实际水下环境中，量测噪声呈现厚尾特性，在前一部分标准高斯分布中，加入标准差为σ2的异常量测，仿真中设为σ2＝5σ1。另外，(53)式中的参数∈用来表征厚尾噪声的程度。