多模型自適應聯邦卡爾曼濾波及其在魚雷導航定位中的應用

曹　萌，李建辰，國琳娜，黃　海，洪劍英

（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-），女，在讀碩士，主要研究方向為武器系統與運用工程制導技術.