一种水下无源组合导航系统衰减自适应信息融合方法与流程

本发明涉及水下导航领域，尤其涉及一种水下无源组合导航系统衰减自适应信息融合方法。

背景技术

海洋是人类生存和发展的重要领域，蕴含着丰富的资源。近年来，对海洋资源的开发和利用已经成为各个国家的重要战略目标。由于我国人口众多，人均资源相对匮乏，对海洋进行全面深入的认识以及开发保护具有十分重要的意义。自主式水下航行器属于水下机器人的一种，可以凭借自身的动力系统和导航系统顺利的完成工作任务。水下航行器具有隐蔽性好，机动性强，工作范围广等优点，在海洋资源开发的过程中起到了非常重要的作用。

对于水下航行器而言，导航精度是其重要的技术指标。高精度的导航系统可以持续提供准确的位置信息，使得水下航行器能够长时间的进行水下作业。其中，惯性导航系统作为一种常用的导航设备可以提供载体的位置、速度和姿态等导航信息。惯性导航系统在没有外界信息修正的情况下，定位误差会随着时间累积，难以满足远航程航行要求。提出了采用多普勒计程仪、声呐定位系统、重礼仪等与惯性导航系统相结合的组合导航系统，利用各种导航设备提供的辅助信息对惯导的定位误差进行修正，进而提高了系统的定位精度。随着水下导航技术的发展，将多种传感器的导航信息进行有效融合，进而提高水下航行器的定位精度，也是信息融合技术领域的一个重要的研究方向。

技术实现要素：

本发明目的在于克服上述现有技术的不足，提供了一种水下无源组合导航系统衰减自适应信息融合方法，具体由以下技术方案实现：

所述水下无源组合导航系统衰减自适应信息融合方法，具体包括如下步骤：

步骤1)初始时主滤波器向子滤波器分配公共状态数据，然后各子滤波器单独工作；

步骤2)各子滤波器按照各自的系统方程和量测方程，独立进行滤波，即进行时间更新和量测更新，各子滤波器将同一时刻的滤波结果送入主滤波器；

步骤3)主滤波器接收各子滤波器的滤波结果，将公共状态和相应的估计误差协方差直接进行融合，主滤波器不进行滤波运算，保留融合后的值，直到下一融合时刻；主滤波器的信息不反馈给子滤波器，子滤波器之间不产生信息交互。

所述水下无源组合导航系统衰减自适应信息融合方法的进一步设计在于，步骤1)中按照信息守恒原理在各个子滤波器直接进行分配，如下式：

其中，β1、β2和β3为信息分配系数，满足β1+β2+β3＝1，q为公共状态的干扰噪声方差强度阵，下标c表示公共状态，下标g为全局估计。

所述水下无源组合导航系统衰减自适应信息融合方法的进一步设计在于，子滤波器采用普通卡尔曼滤波器，步骤2)中的滤波算法包括如下步骤：

2-1)根据计算状态预测值，其中，是一步预测值、k为时刻、φk,k-1是状态一步转移矩阵；

2-2)根据计算新息；其中，εk为新息、zk子系统测量值、hk量测矩阵、是残差；

2-3)计算状态估计其中表示k时刻估计值；

2-4)计算衰减因子矩阵，

取λk＝diag(s1,s2,s3,1,…1)，λk为衰减因子矩阵，衰减因子s1,s2,s3按下式进行自适应估计

si＝max(1,si)(i＝1,2,3)，式中bii(k)、eii(k)、εi为新息，vi(k)噪声协方差值，zk为k时刻量测值，γk,k-1噪声转移矩阵，qk-1噪声矩阵，状态方差值，bk、ck为自定义公式，pk-1为状态方差矩阵、ek、cij、pk-1为k-1时刻状态协方差；

2-5)根据计算预测误差协方差阵pk,k-1，

其中，λk表示衰减因子矩阵；

2-6)根据计算滤波增益阵kk；

2-7)根据pk＝[i-kkhk]pk,k-1估计误差方差阵pk；

2-8)测量方差自学习估计：

dk＝1-b^k,(0＜b≤1)

其中，m、zi、dk-1、hi、b^k分别表示什么含义？

按照下列公式计算得到最优数据融合；

其中，pg是全局状态协方差，n为子滤波器数，为第i个子滤波器状态协方差。

本发明的优点如下：

本发明的水下无源组合导航系统衰减自适应信息融合方法具有容易实现，能够为水下无源组合导航可靠性和准确性提供保证。具有容易实现的优点。

附图说明

图1为auv组合导航系统联邦滤波器结构图。

具体实施方式

以下结合附图，对本发明的技术方案进行详细说明。

本实施例采用auv组合导航系统联邦滤波结构方案，如图1所示，由一个主滤波器和三个子滤波器组成。sins与tan(输出位置)组成子滤波器lf1，进行位置融合，sins与dvl(输出速度)组成子滤波器lf2，进行速度融合，sins与mcp组成子滤波器lf3，进行姿态融合，sins为公共参考系统。

lf1、lf2和lf3均为普通卡尔曼滤波器，它们滤波的数据送入主滤波器(mf)进行最终融合。mf的信息不反馈给lf，因而不同的lf之间没有交联影响，某个lf的故障不会对其他lf产生污染。故容错性能很好，适合auv导航系统高可靠性的要求。

采用直接法滤波由于惯导系统是时变的，状态变化快，为了得到准确的估计，滤波计算周期必须很短，不利于实时计算，且直接法的系统方程是非线性方程，必须采用广义滤波不利于工程实现。间接法可以保持原导航系统工作独立的基础上充分发挥惯导系统输出频率高的优点，反馈校正工程实现比较复杂，滤波器故障会直接污染惯导系统输出，降低系统可靠性，故本文采用间接法滤波，以导航参数的误差作为估计对象，估计的是导航参数的误差，进行输出校正方法。

进一步的，建立联邦滤波器的状态方程和量测方程。sins/tan位置子滤波器进行位置融合，将二者位置量测量之差作为量测值。

对于状态方程：以惯导误差状态变量和tan误差状态变量合并而成sins/tan位置子滤波器的状态矢量，捷联式惯性导航系统状态为：

地形匹配模块状态为：

xt＝[δlδλ]，xst＝[xsxt]^t

该子滤波器的状态方程为：

即

对于量测方程：

惯导位置信息为：

tan模块给出的位置信息(高度方向位置信息由测深测潜仪提供)为：

定义位置测量向量为

其中为量测白噪声。

本实施例中，sins/dvl子滤波器进行速度组合滤波。

本实施例的状态方程以惯导误差状态变量和dvl误差状态变量合并成sins/dvl速度子滤波器的状态矢量，其中xs同上，xd＝[δvdxδvdyδkxδky]^t，xsd＝[xsxd]^t则该子滤波器的状态方程为

即

本实施例的量测方程，sins/dvl子滤波器以sins与dvl的所测量速度差值作为观测量，

具体为：

首先定义速度量测矢量为：

式中，vid＝[ηeηn]^t量测白噪声，

为

量测矩阵。

对于sins/mcp姿态子滤波器：sins/mcp姿态子滤波器的状态方程中mcp测姿相关性较弱，可以认为mcp测姿误差为白噪声。所以sins/mcp姿态组合子滤波器状态方程为惯导误差状态方程。

sins/mcp姿态子滤波器的测量方程惯导姿态信息为：

式中，ψe、ψn、ψu为纵摇角、横摇角、航向角真实值。

mcp给出的姿态信息为：

式中，mψe、mψn、mψu是mcp测姿白噪声。

定义姿态角测量向量为：

式中，为量测矩阵，为mcp测姿白噪声。

式(11)即为sins与mcp进行姿态组合时的量测方程。

auv组合导航系统信息融合算法如下：

(1)初始时主滤波器将公共状态向局部滤波器分配一次，然后各自滤波器单独工作；

(2)各个子滤波器按照各自的系统方程和量测方程，独立进行卡尔曼滤波，即进行时间更新和量测更新。本系统捷联姿态更新周期为10ms，量测更新为1s。各子滤波器将同一时刻的滤波结果送入主滤波器；

(3)对各子滤波器送来的滤波结果，主滤波器只把公共状态和相应的估计误差协方差直接进行融合，主滤波器不进行滤波运算，保留融合后的值，直到下一融合时刻。主滤波器的信息不反馈给子滤波器，每个子滤波器的信息与其它子滤波器没有交联影响。

在没有测量信息时，各子滤波器根据各自的状态方程进行时间更新，不进行量测更新：

在时间更新的基础上，当有新的测量值时，各子滤波器根据测量值进行修正，更新公式如下：

递推一步可得：

然后子滤波器重复上述时间更新和量测更新步骤。

初始分配一次信息，由于惯导系统同时参与了三个子滤波器的滤波，惯导状态为公共状态，其信息按照信息守恒原理在各个子滤波器直接进行分配：

其中，β1、β2和β3为信息分配系数，满足β1+β2+β3＝1，其值可以根据实际情况选择。q为公共状态的干扰噪声(过程噪声)方差强度阵，下标“c”表示公共状态，此处为sins状态。g为全局估计。

以下介绍信息分配系数的取值原则即公共参考信息的分配原则。

设在联邦滤波器中，参与组合的导航子系统除惯导外，还有n个非相似导航子系统。由于惯导是公共参考系统，它参与了由该n个子系统和惯导构成的n个子滤波器的滤波，所以惯导的信息应在这n个子滤波器之间进行分配。根据信息守恒原理，分配系数应满足式中，βi为第i各子滤波器获得的信息分配系数。

设x的估计误差的均方误差阵为p，p描述了对x的估计质量，而p^-1为信息矩阵。p越大，x的估计质量就越差，信息矩阵p^-1就越小；反之，p越小，x的估计质量就越好，信息矩阵就越大。

由式可以看出，对惯导系统的信息作分配，实质上就是将参与第i个子滤波器滤波的惯导的估计均方误差阵pc扩大1/βi倍。从中可以看出，βi越小，则pc扩大的倍数就越大。由于卡尔曼滤波器能自动根据信息质量的优劣作权重不同的利用，所以βi越小，对惯导信息的利用权重就越低，该子滤波器的滤波精度主要取决于子系统i的信息质量，而惯导系统的输出信息所起的作用相对降低；反之亦然。

综上所述，惯导信息在子滤波器分配的一般原则：子系统精度越差，惯导信息的分配系数应该越大；子系统精度越高，惯导信息的分配系数应适当取得小些。因为子系统精度越高，相应子滤波器滤波精度受βi的影响就越小，此时，βi取得小些可使总量有限的惯导信息在较低精度子系统所在的子滤波器中能充分发挥作用。

主滤波器数据融合周期与子滤波器滤波周期相同。

把三个子滤波器的公共状态进行融合。将三个子滤波器每步滤波的结果中，取出公共状态pci，i＝1,2,3，传递给全局滤波器。全局滤波器按照下边的公式完成信息的最优综合，形成全局系统的综合信息pcg：

以上是sins/tan/dvl/mcp联邦滤波器数据融合算法。

本实施例的算法为对5auv自适应联邦kalman滤波算法及相应改进，经典的卡尔曼滤波应用中的一个重要的先决条件是必须准确知道噪声的统计特性，但由于auv使用条件和海洋工作环境的变化，传感器噪声统计特性的不确定性可能导致滤波器性能的下降。另外，为了保证估计结果的无偏性和滤波的稳定性，必须选取合适的初值及其方差矩阵。由于事先无法准确知道系统噪声，滤波进行一段时间后，计算的协方差逐渐趋于零，状态估计太依赖于过去的数据，从而导致滤波发散。针对以上问题，相继出现了一些自适应滤波方法，但这些方法计算复杂，对于实时导航系统，需要采用降低计算量的措施。这里介绍一种衰减记忆自适应kalman滤波算法并对其进行了改进，将之应用于联邦组合滤波中，进行了相应的仿真分析，取得了较好的估计效果。

为防止滤波发散，fagin和sorenson提出了衰减记忆滤波算法^[100-107]，用衰减因子限制卡尔曼滤波器的记忆长度，使得滤波过程更加重视信息的利用而降低了对验前信息的依赖性。下面给出衰减记忆自适应卡尔曼滤波方程。

为了构成衰减记忆估计方法，修改原状态模型和量测模型方程为：

式中为零均值白噪声向量序列，且

初始状态的统计特性取为

并且互不相关。于是的滤波方程为

记

则

式(31)～(34)所列滤波方程与卡尔曼滤波方程基本方程相比，不同的地方仅在式(14)多了一个标量因子s。由于s＞1，总比pkk-1大，所以总有这意味着采用衰减记忆滤波算法时，对新量测值的利用权重比采用基本方程时的权重大。同时

意味着的权重相对地降低，即降低了陈旧量测值对估计值的影响。

通常情况下，衰减记忆滤波在应用中衰减因子取为一个大于1的固定实数。由于衰减因子的选取具有很大的随意性，且在整个滤波估计过程中值为恒定，在系统有不可测的扰动时，估计效果不够理想。文献[105,106]给出了衰减因子的自适应估计方法，具有一定的理论指导意义。然而，这些方法要求计算矩阵的特征值，计算量较大，在工程上并不实用。本文提出一种自适应估计衰减因子的方法，可在实际应用中快速计算出衰减因子的估计值，具有一定的应用价值。

在以往的算法中，自适应卡尔曼滤波器的协方差阵只乘以一个衰减因子s，然而在实际导航算法中，每一个误差状态估计程度是不一样的。用一个衰减因子不能保证滤波器绝对最佳。为了使滤波器具有更高程度的最佳性，在这里考虑用衰减因子矩阵代替衰减因子，给各个通道以不同渐消速率。有带衰减因子矩阵的自适应卡尔曼滤波算法：

pk＝[i-kkhk]pk,k-1(39)

式中，λk＝diag(s1,s2,…,sn)为衰减因子矩阵。

当滤波正常时，残差序列有如下性质：

式中，

记

则有

式中，cij(k)(i＜n,j＜n)为ck的元素。

以速度匹配组合导航为例，取状态变量为

x＝[δveδvnδvuφeφnφu▽x▽y▽zεxεyεx]^t

速度为外观测量，则观测矩阵hk可写为

有

设

则ak＝δkek。

记gk＝ak+bk，当滤波正常时，有

分别检验vk的每一个分量，有：

式中，vi(k)和gii(k)分别是vk的第i个元素和gk对角线上第i个元素，

gii＝aii+bii＝δiieii+bii＝sieii+bii。

相应的检验规则如下：

式中门限值εi由预定的虚警概率α借助χ²分布表来选定。

若要使滤波正常，将式(46)代入式(45)中，得

得衰减因子阵对角元素的正常值范围为：

取δii＝max(1,δii)，(i＝1,2,3)(49)

从以上推导过程可以看出，衰减因子阵λk＝diag(s1,s2,…,s12)中能进行自适应估计的对角元素为s1,s2,s3，其余元素无法进行自适应估计，可取为1。则衰减因子阵可写为

λk＝diag(s1,s2,s3,1,…1)

(50)

对应相应的物理概念可知，在捷联惯导系统中，速度误差状态是直接可观测的，而其它误差状态是不可直接观测的。因此，只有协方差p阵对应于速度误差状态的对角元素才有可能对其状态估计进行控制，其它元素因无法得到量测值，它们的估计程度是无法修正的。所以，在衰减因子阵λk＝diag(s1,s2,…,s12)中，只有s1,s2,s3可进行自适应估计，其余元素则取为1。

(2)测量方差估计值的改进

对自适应卡尔曼滤波过程的研究发现，它是运用一种预测+修正的思想，是以k-1时刻的最佳估计为准，依据状态方程预测k时刻的状态向量同时，又对状态进行观测得到测量向量zk，再在测量与预测之间进行折中，折中的关键在对于测量新息的加权，主要是针对滤波增益合理选择的问题。为了更清楚了解滤波增益的实质，将量测方程中的测量矩阵hk设为1，则自适应卡尔曼滤波中的增益方程变为

kk＝pk,k-1[pk,k-1+rk]^-1(51)

可见增益kk与pk,k-1和rk有关，而从自适应卡尔曼滤波算法可知，其实质是修正pk,k-1，进而优化增益kk。但对于影响增益的另一个因素测量方差rk，一般是根据传感器自身方差参数指定，而没有考虑到测量方差是由传感器自身的可信度与环境干扰程度共同造成的，从而不能充分利用每次测量带来新的信息，造成算法缺乏对测量过程中不确定性因素的适应性。另外，由于在自适应卡尔曼滤波算法中选择衰减因子时，忽略了测量方差的时变特性，将造成状态的过调节，最终，导致估计值与真实值的偏差较大，同时，也造成大量信息资源的浪费。sage、husa曾提出过一些时变噪声统计特性估计算法，本文在研究了这些算法的基础上，在进行测量方差估计时，将传感器的内部噪声与环境干扰综合考虑，提出一种随不确定因素而变化的测量方差自学习的估算方法。以单个传感器为研究对象，测量方差是传感器内部噪声和环境干扰的一种综合属性，这一属性始终存在于测量的全过程。因此，要将单个传感器的前一次采样的测量方差的估计值与当前测量方差(实时采样值)综合考虑，求得当前测量方差的实时估算，亦即在此提出了测量方差自学习的估算方法。

假设噪声均值不为零，即e{vk}＝rk，是对rk的递推估计，是对rk的平均估计，rk为系统观测噪声方差阵，是对rk的递推估计，是对rk的平均估计，则有测量方差自学习的估算方法如下：

其中dk,1-dk用于自学习加权，所谓自学习加权是指对待旧测量数据和新近测量数据给予不同权系数，在考虑新近数据在估计中发挥主要作用的同时，充分利用以往的经验数据，从而使测量方差逐步趋于最优过程。为此，取权系数序列dk＝1-b^k,(0＜b≤1),其中，b为学习因子，要根据试验确定。当外部干扰因素大时，则b应选得大些，以充分利用先验信息；当外部干扰因素小时，则b应选得小些，使测量方差的估算中能尽力捕捉和利用当前测量中新的信息。由测量方差估计的计算过程可以看出：每次新的测量数据都被充分利用，都对传感器的测量方差有调节作用，而且，这种调节作用将越来越显著，但实际上这种调节作用是随着采样时刻的增加由显著再到减弱的过程。这是因为将传感器与测量环境综合起来考虑，测量向量从统计意义上说，它的概率分布是确定的。在学习过程中，最初几个采样时刻是对测量向量分布特性从无到有的认识，这时需要充分发挥先验知识的作用，而且，此时的学习速度较快，体现在对测量方差的估计中是相邻采样点间传感器的测量方差估计值的变化较大，而随着采样的进行，这种学习过程将会趋于稳定，即被优化的测量方差的结果会趋于一个稳定的值。从算法中知，衰减因子的确定与测量方差存在一定的内在联系，对于衰减因子的修正，即对于下式中测量方差的修正

(3)改进的自适应kalman滤波算法具体实现步骤

综合上述两点，改进的自适应卡尔曼滤波器的具体算法可以描述为

①计算状态一步预测值

②计算新息

③计算状态估计

④计算衰减因子矩阵

取λk＝diag(s1,s2,s3,1,…1)，λk为衰减因子矩阵，衰减因子s1,s2,s3按下式进行自适应估计

取si＝max(1,si)(i＝1,2,3)

式中

⑤计算预测误差协方差阵

⑥计算滤波增益阵

⑦估计误差方差阵pk＝[i-kkhk]pk,k-1

⑧测量方差自学习估计

dk＝1-b^k,(0＜b≤1)

最后，将改进自适应滤波应用到联邦滤波器子滤波器中就构成了本文所采用的改进自适应联邦滤波算法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。