多模型自适应联邦卡尔曼滤波及其在鱼雷导航定位中的应用

曹　萌，李建辰，国琳娜，黄　海，洪剑英

（1.中国船舶重工集团公司 第705研究所，陕西 西安，710075；2.水下信息与控制重点实验室，陕西 西安，710075）

多模型自适应联邦卡尔曼滤波及其在鱼雷导航定位中的应用

曹萌1，2，李建辰1，2，国琳娜1，2，黄海1，洪剑英1

（1.中国船舶重工集团公司 第705研究所，陕西 西安，710075；2.水下信息与控制重点实验室，陕西 西安，710075）

针对复杂环境所引起的鱼雷组合导航系统模型参数变化导致传统单一固定参数滤波器精度降低的问题，设计了一种适用于复杂情况的多模型自适应联邦卡尔曼滤波器。该滤波器采用多模自适应联邦滤波算法，利用多模型滤波器参数来逼近系统的动态性能，该滤波器同时采用了在信息融合时自适应调整信息分配系数的方法。实验室导航试验结果表明，文中设计的多模自适应联邦卡尔曼滤波器可以大大提高导航定位系统的估计精度、跟踪速度以及稳定性。

鱼雷；组合导航；多模自适应联邦卡尔曼滤波器；信息融合

0　引言

鱼雷具有隐蔽性好、破坏威力大、命中概率高、抗干扰性强等特点，是海战中不可缺少的重要武器。目前，大型航船编队的内层反潜防区范围扩大，且航船编队的航速在不断提高、反潜探测能力不断增强，若要实现反潜防区外的精确打击，需要大幅提高鱼雷的航速和航程。

随着重型鱼雷航程的不断提高，在满足雷体航姿测量的同时，对鱼雷定位功能的需求也不断加强。鱼雷航速提高，噪声将随之增强，必然大大缩减自导系统作用距离。可见，实现高精度导航定位是目前首要解决的关键问题之一。鱼雷具备远程精确导航定位能力，可以显著提高其作战效能，确保在鱼雷高速强噪声干扰和复杂水声对抗环境下实现对敌有效打击，从某种意义上讲比增加装药量更具威力和高效。同时，精确的导航定位也是实现鱼雷远程巡航及集群攻击的基础，是鱼雷进入网络化作战系统的必要条件。

目前鱼雷的定位方式仍然是以惯性导航为主，可使用的雷载多传感器信息主要包括发动机转速推算雷速（雷载测速）、深度信息推算的雷速以及磁航向等。结合鱼雷的航行弹道，充分挖掘这些雷载多传感器信息的隐含资源，辅助惯性导航系统，实现多传感器信息间的最优融合。然而，通常假设鱼雷在航行过程中，系统的模型参数是时不变的，但是由于鱼雷在全航程中会有弹道机动、洋流变化及子系统故障等多种环境条件的变化，系统模型参数会随之变化，单一固定的滤波器参数必将导致估计精度的降低。

基于此，文中设计了一种适用于复杂情况的多模型自适应卡尔曼滤波器。该滤波器利用多模型卡尔曼滤波器参数来逼近系统的动态性能，并通过带有不同参数值的滤波器估计的加权值对系统状态进行估计，得到对复杂系统较好的估计精度、跟踪速度以及稳定性。

1　多模自适应卡尔曼滤波

多模型滤波器是由N个并行的卡尔曼滤波器和状态加权算法组成。首先测量值通过N个独立的卡尔曼滤波器可以得到N个状态估计值，然后用这N个状态估计值形成对观测量的N个预测值；将实际量测矢量与这N个预测值相减得到的残差作为各滤波器与实际系统模型相似度的衡量标准，残差越小，说明滤波器的模型与实际模型越相似，反之则相似度越差，然后由残差来计算系统不同工作模型的概率值；最后对各状态估计取概率加权平均值，从而形成对实际系统的混合状态估计值［1］。

图1为多模型卡尔曼滤波器系统框图。

图1　多模型滤波器系统框图Fig.1　System diagram of a multi-model filter

如何应用残差计算系统中每个独立的卡尔曼滤波器在融合时的概率值是整个多模自适应滤波算法的关键，文中引入条件概率来解决此问题。

定义对于第i个卡尔曼滤波器，其被分配的概率为［2-4］

式中，fi（k）表示在以往的量测和参数向量的条件下，第i个卡尔曼滤波器模型在k时刻的量测值为Zk的条件概率密度函数。且

式中：ei（k）为滤波器的量测新息，且ei（k）= Zi（k）-Hi（k ）Xˆi（k，k-1）；m为量测向量的维数；l为滤波器的个数。

因此，对于每个独立的卡尔曼滤波器模型都有一个相应的权重αi与其对应，在进行概率加权融合后即可得到实际混合状态的估计值和误差协方差阵。

混合状态的估计值和误差协方差阵

2　鱼雷组合导航系统模型

2.1系统的状态方程

鱼雷多信息融合组合导航定位系统选取惯性导航参数误差量及传感器误差量作为状态变量，即选取雷速误差、姿态角误差、陀螺漂移、加速度计零偏以及磁航向计误差作为状态向量

其中：δVN，δVE为沿北、东方向的速度误差；φN，φU，φE为沿北、天、东方向的姿态角误差；εN，εU，εE为沿北、天、东方向的陀螺漂移；∇N，∇U，∇E为沿北、天、东方向的加速度计零偏；δφMG为磁航向计误差。

系统的状态方程为

有关F（t）的具体表达式见文献［5］和文献［6］。

2.2系统的量测方程

1）子滤波器1的量测方程

子滤波器1引入捷联式惯性导航系统（strapdown inertial navigation system，SINS）速度与雷载测速之差作为观测量，得到如下量测方程

式中：量测矩阵HV=［I2×202×10］；量测噪声VV～N（0，RV）。

2）子滤波器2的量测方程

子滤波器2采用SINS速度与鱼雷定角变深过程的深度信息推算的雷速之差作为观测量，其量测方程

式中：量测矩阵HU=［I2×202×10］；量测噪声VU～N（0，RU）。

3）子滤波器3的量测方程

子滤波器3采用SINS航向角与磁航向计输出的航向信息之差作为观测量，其量测方程

式中：量测矩阵HM=［01×3101×8］；量测噪声VM～N（0，RM）。

以上即为鱼雷组合导航系统的量测方程［7］。

3　多模型自适应联邦卡尔曼滤波器设计

在鱼雷组合导航定位系统中，引入多模型卡尔曼滤波器设计一方面能够提高滤波器的适应性和估计精度，另一方面也会带来较大的计算量。因此在设计中，针对捷联惯性导航系统与雷载测速系统，建立多模型自适应卡尔曼滤波器。系统模型见图2。

图2　鱼雷多模型自适应联邦卡尔曼滤波器原理框图Fig.2　Principle diagram of multi-model adaptive federated Kalman filter for a torpedo

首先确定信息分配系数。定义3个子滤波器的信息分配系数分别为β1，β2和β3，主滤波器的信息分配系数为βm，满足β1+β2+β3+βm=1。文中，使主滤波器只对子滤波器的估计输出进行综合而不进行滤波运算，即令主滤波器的信息分配系数βm=0。其他子滤波器的信息分配系数按照其估计的均方误差阵进行如下分配［8］

式中，i=1，2，3。

子滤波器1是多模自适应卡尔曼滤波器，其算法如下。

1）卡尔曼滤波

时间更新

量测更新

2）模型概率计算

对于第i个模型，其被分配的概率为

3）输出综合

子滤波器2和子滤波器3是标准的卡尔曼滤波器，分别采用标准的卡尔曼滤波算法得到状态估计）以及状态估计的均方误差阵P2（k）和P3（k），文中不再赘述。

最后，在得到各子滤波器的估计值后按式（17）和式（18）进行最优合成

4　试验验证

实验室导航试验验证是费效比最佳的验证方式。对文中滤波器所采用的鱼雷多模型自适应联邦卡尔曼滤波算法进行实验室导航试验验证。

试验设备以雷体特性模拟模块为中心，通过雷体方程解算模拟雷体运动特性，并将角运动信息送至转台控制设备，驱动转台按照雷体运动特性进行转动，而线运动特性则与产品直接构成解算回路。试验中，角运动由导航产品直接敏感测量，线运动由数学模拟产生，根据需要加入转速测速、磁航向等误差后，完成导航定位系统的实验室导航试验。鱼雷实验室导航试验原理见图3。

图3　鱼雷实验室导航试验框图Fig.3　Diagram of torpedo navigation test in laboratory

实验室导航试验数据见图4～图5。导航周期为5 ms，陀螺漂移为1°/h，加计零偏为3×10-3g。

由捷联惯导/雷载测速/深度信息推算的雷速/磁航向进行组合滤波。其中，雷载测速的量测噪声均值为0.75 m/s，深度信息推算的雷速量测噪声均值为0.5 m/s，磁航向计量测噪声为0.5°；考虑到惯性导航系统在工作初期航向误差较小，在航行1200 s后加入磁航向信息。分别使用传统单一模型卡尔曼滤波算法、多模自适应联邦卡尔曼滤波算法进行滤波计算。导航定位误差结果见图6～图13。

图4　实验室导航试验下航姿曲线Fig.4　Curves of attitude angle versus time from navigation test in laboratory

图5　实验室导航试验下速度曲线Fig.5　Curves of speed versus time from navigation test in laboratory

图6　传统单一模型卡尔曼滤波算法下航姿误差曲线Fig.6　Curves of attitude angle error versus time with traditional single model Kalman filter algorithm

由图4和图5可知，航行弹道包含变向和变速机动，采用多模型自适应卡尔曼滤波算法，使滤波器参数实时逼近系统的动态性能，较之传统单一模型的卡尔曼滤波算法，定位误差显著降低，在航行2 800 s后，定位误差为1 136 m，比传统单一模型的卡尔曼滤波算法定位误差降低约38.2％。

图7　多模自适应联邦卡尔曼滤波算法下航姿误差曲线Fig.7　Curves of attitude angle error versus time with multimodel adaptive federated Kalman filter algorithm

图8　传统单一模型滤波算法下北向速度误差曲线Fig.8　Curves of north speed error versus time with traditional single model Kalman filter algorithm

图9　多模自适应联邦卡尔曼滤波算法下北向速度误差曲线Fig.9　Curves of north speed error versus time with multi-model adaptive federated Kalman filter algorithm

5　结论

针对鱼雷在全航程中会有弹道机动，洋流变化，子系统故障等多种环境条件的变化，单一固定的滤波器参数必将导致估计精度降低的问题，文中设计了一种多模自适应联邦卡尔曼滤波器，通过实验室导航试验验证，可以得到如下结论。

图10　传统单一模型滤波算法下东向速度误差曲线Fig.10　Curve of east speed error versus time with traditional single model Kalman filter algorithm

图11　多模自适应联邦卡尔曼滤波算法下东向速度误差曲线Fig.11　Curve of east speed error versus time with multimodel adaptive federated Kalman filter algorithm

图12　传统单一模型卡尔曼滤波算法下定位误差曲线Fig.12　Curve of positioning error versus time with traditional single model Kalman filter algorithm

1）在鱼雷导航定位系统中，利用多模型卡尔曼滤波器参数来逼近系统的动态性能，通过带有不同参数值的滤波器估计的加权值得到对系统状态的估计，在保持原有姿态精度的基础上，提高了定位精度。

2）在多模自适应联邦卡尔曼滤波算法中，由协方差和卡尔曼增益矩阵均是相互独立的。因此，多模型自适应联邦卡尔曼滤波器比较稳定。

图13　多模自适应联邦卡尔曼滤波算法下定位误差曲线Fig.13　Curve of positioning error versus time with multimodel adaptive federated Kalman filter algorithm

［1］李艳华，房建成.一种多模型自适应联邦滤波器及其INS/CNS/GPS组合导航系统中的应用［J］.航天控制，2003，21（2）：33-38.

Li Yan-hua，Fang Jian-cheng.A Multi-model Adaptive Federated Filter and It′s Application in INS/CNS/GPS Integrated Navigtion System［J］.Aerospace Control，2003，21（2）：33-38.

［2］Groves P D.GNSS与惯性及多传感器组合导航系统原理［M］.李涛，译.北京：国防工业出版社，2011.

［3］Charles K，Chui G R.卡尔曼滤波及其实时应用［M］.戴洪德，等译.北京：清华大学出版社，2013.

［4］Wang B，Wang J，Wu J P，et al.Study on GPS/INS Integration System［J］.IEEE Proceedings on Intelligent Transportation Systems，2003（2）：1016-1021.

［5］李建辰.鱼雷定位技术［M］.北京：国防工业出版社，2009.

［6］秦永元，张洪钺，汪淑华.卡尔曼滤波与组合导航原理［M］.西安：西北工业大学出版社，2012.

［7］曹萌，李建辰，国琳娜，等.自适应联邦滤波算法在鱼雷多参量导航定位中的应用［J］.鱼雷技术，2014，22（6）：420-424.

Cao Meng，Li Jian-chen，Guo Lin-na，et al.Application of Adaptive Federated Kalman Filter Algorithm to Multiparameter Estimation for Torpedo Navigation and Positioning［J］.Torpedo Technology，2014，22（6）：420-424.

［8］刘勇，徐世杰.基于联邦UKF算法的月球探测自主组合导航［J］.宇航学报，2006，27（3）：518-530.

Liu Yong，Xu Shi-jie.Autonomous Integrated Navigation for Lunar Probe Based on Federated UKF Algorithm［J］.Journal of Astronautics，2006，27（3）：518-530.

（责任编辑：杨力军）

Multi-model Adaptive Federated Kalman Filter and Its Application to
Torpedo Navigation and Positioning

CAO Meng1，2，LI Jian-chen1，2，GUO Lin-na1，2，HUANG Hai1，HONG Jian-ying1
（1.The 705 Research Institute，China Shipbuilding Industry Corporation，Xi′an 710075，China；2.Science and Technology on Underwater Information and Control Laboratory，Xi′an 710075，China）

Aiming at the problem that traditional fixed parameter filter lose its precision due to parameter variation of torpedo integrated navigation system model in complicated environment，a multi-model adaptive federated Kalman filter is designed for a torpedo integrated navigation system.The Kalman filter adopts multi-model adaptive federated filtering algorithm，which can approximate the dynamic performance of the system by making use of multi-model filter parameters.Furthermore，an adaptive information distribution strategy for information fusion is employed in the federated filter.The results of navigation test in laboratory show that the multi-model adaptive federated Kalman filter can improve estimation precision，tracking speed and stability of the navigation system greatly.

torpedo；integrated navigation；multi-model adaptive federated Kalman filter；information fusion

TJ765；TN967.2

A

1673-1948（2015）04-0305-06

2015-03-19；

2015-06-09.

海军科研资助项目（101100302.02）.

曹萌（1990-），女，在读硕士，主要研究方向为武器系统与运用工程制导技术.