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

（1.中國船舶重工集團(tuán)公司 第705研究所，陜西 西安，710075；2.水下信息與控制重點實驗室，陜西 西安，710075）

多模型自適應(yīng)聯(lián)邦卡爾曼濾波及其在魚雷導(dǎo)航定位中的應(yīng)用

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

（1.中國船舶重工集團(tuán)公司 第705研究所，陜西 西安，710075；2.水下信息與控制重點實驗室，陜西 西安，710075）

針對復(fù)雜環(huán)境所引起的魚雷組合導(dǎo)航系統(tǒng)模型參數(shù)變化導(dǎo)致傳統(tǒng)單一固定參數(shù)濾波器精度降低的問題，設(shè)計了一種適用于復(fù)雜情況的多模型自適應(yīng)聯(lián)邦卡爾曼濾波器。該濾波器采用多模自適應(yīng)聯(lián)邦濾波算法，利用多模型濾波器參數(shù)來逼近系統(tǒng)的動態(tài)性能，該濾波器同時采用了在信息融合時自適應(yīng)調(diào)整信息分配系數(shù)的方法。實驗室導(dǎo)航試驗結(jié)果表明，文中設(shè)計的多模自適應(yīng)聯(lián)邦卡爾曼濾波器可以大大提高導(dǎo)航定位系統(tǒng)的估計精度、跟蹤速度以及穩(wěn)定性。

魚雷；組合導(dǎo)航；多模自適應(yīng)聯(lián)邦卡爾曼濾波器；信息融合

0　引言

魚雷具有隱蔽性好、破壞威力大、命中概率高、抗干擾性強(qiáng)等特點，是海戰(zhàn)中不可缺少的重要武器。目前，大型航船編隊的內(nèi)層反潛防區(qū)范圍擴(kuò)大，且航船編隊的航速在不斷提高、反潛探測能力不斷增強(qiáng)，若要實現(xiàn)反潛防區(qū)外的精確打擊，需要大幅提高魚雷的航速和航程。

隨著重型魚雷航程的不斷提高，在滿足雷體航姿測量的同時，對魚雷定位功能的需求也不斷加強(qiáng)。魚雷航速提高，噪聲將隨之增強(qiáng)，必然大大縮減自導(dǎo)系統(tǒng)作用距離?？梢姡瑢崿F(xiàn)高精度導(dǎo)航定位是目前首要解決的關(guān)鍵問題之一。魚雷具備遠(yuǎn)程精確導(dǎo)航定位能力，可以顯著提高其作戰(zhàn)效能，確保在魚雷高速強(qiáng)噪聲干擾和復(fù)雜水聲對抗環(huán)境下實現(xiàn)對敵有效打擊，從某種意義上講比增加裝藥量更具威力和高效。同時，精確的導(dǎo)航定位也是實現(xiàn)魚雷遠(yuǎn)程巡航及集群攻擊的基礎(chǔ)，是魚雷進(jìn)入網(wǎng)絡(luò)化作戰(zhàn)系統(tǒng)的必要條件。

目前魚雷的定位方式仍然是以慣性導(dǎo)航為主，可使用的雷載多傳感器信息主要包括發(fā)動機(jī)轉(zhuǎn)速推算雷速（雷載測速）、深度信息推算的雷速以及磁航向等。結(jié)合魚雷的航行彈道，充分挖掘這些雷載多傳感器信息的隱含資源，輔助慣性導(dǎo)航系統(tǒng)，實現(xiàn)多傳感器信息間的最優(yōu)融合。然而，通常假設(shè)魚雷在航行過程中，系統(tǒng)的模型參數(shù)是時不變的，但是由于魚雷在全航程中會有彈道機(jī)動、洋流變化及子系統(tǒng)故障等多種環(huán)境條件的變化，系統(tǒng)模型參數(shù)會隨之變化，單一固定的濾波器參數(shù)必將導(dǎo)致估計精度的降低。

基于此，文中設(shè)計了一種適用于復(fù)雜情況的多模型自適應(yīng)卡爾曼濾波器。該濾波器利用多模型卡爾曼濾波器參數(shù)來逼近系統(tǒng)的動態(tài)性能，并通過帶有不同參數(shù)值的濾波器估計的加權(quán)值對系統(tǒng)狀態(tài)進(jìn)行估計，得到對復(fù)雜系統(tǒng)較好的估計精度、跟蹤速度以及穩(wěn)定性。

1　多模自適應(yīng)卡爾曼濾波

多模型濾波器是由N個并行的卡爾曼濾波器和狀態(tài)加權(quán)算法組成。首先測量值通過N個獨立的卡爾曼濾波器可以得到N個狀態(tài)估計值，然后用這N個狀態(tài)估計值形成對觀測量的N個預(yù)測值；將實際量測矢量與這N個預(yù)測值相減得到的殘差作為各濾波器與實際系統(tǒng)模型相似度的衡量標(biāo)準(zhǔn)，殘差越小，說明濾波器的模型與實際模型越相似，反之則相似度越差，然后由殘差來計算系統(tǒng)不同工作模型的概率值；最后對各狀態(tài)估計取概率加權(quán)平均值，從而形成對實際系統(tǒng)的混合狀態(tài)估計值［1］。

圖1為多模型卡爾曼濾波器系統(tǒng)框圖。

圖1　多模型濾波器系統(tǒng)框圖Fig.1　System diagram of a multi-model filter

如何應(yīng)用殘差計算系統(tǒng)中每個獨立的卡爾曼濾波器在融合時的概率值是整個多模自適應(yīng)濾波算法的關(guān)鍵，文中引入條件概率來解決此問題。

定義對于第i個卡爾曼濾波器，其被分配的概率為［2-4］

式中，fi（k）表示在以往的量測和參數(shù)向量的條件下，第i個卡爾曼濾波器模型在k時刻的量測值為Zk的條件概率密度函數(shù)。且

式中：ei（k）為濾波器的量測新息，且ei（k）= Zi（k）-Hi（k ）X?i（k，k-1）；m為量測向量的維數(shù)；l為濾波器的個數(shù)。

因此，對于每個獨立的卡爾曼濾波器模型都有一個相應(yīng)的權(quán)重αi與其對應(yīng)，在進(jìn)行概率加權(quán)融合后即可得到實際混合狀態(tài)的估計值和誤差協(xié)方差陣。

混合狀態(tài)的估計值和誤差協(xié)方差陣

2　魚雷組合導(dǎo)航系統(tǒng)模型

2.1系統(tǒng)的狀態(tài)方程

魚雷多信息融合組合導(dǎo)航定位系統(tǒng)選取慣性導(dǎo)航參數(shù)誤差量及傳感器誤差量作為狀態(tài)變量，即選取雷速誤差、姿態(tài)角誤差、陀螺漂移、加速度計零偏以及磁航向計誤差作為狀態(tài)向量

其中：δVN，δVE為沿北、東方向的速度誤差；φN，φU，φE為沿北、天、東方向的姿態(tài)角誤差；εN，εU，εE為沿北、天、東方向的陀螺漂移；?N，?U，?E為沿北、天、東方向的加速度計零偏；δφMG為磁航向計誤差。

系統(tǒng)的狀態(tài)方程為

有關(guān)F（t）的具體表達(dá)式見文獻(xiàn)［5］和文獻(xiàn)［6］。

2.2系統(tǒng)的量測方程

1）子濾波器1的量測方程

子濾波器1引入捷聯(lián)式慣性導(dǎo)航系統(tǒng)（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）。

以上即為魚雷組合導(dǎo)航系統(tǒng)的量測方程［7］。

3　多模型自適應(yīng)聯(lián)邦卡爾曼濾波器設(shè)計

在魚雷組合導(dǎo)航定位系統(tǒng)中，引入多模型卡爾曼濾波器設(shè)計一方面能夠提高濾波器的適應(yīng)性和估計精度，另一方面也會帶來較大的計算量。因此在設(shè)計中，針對捷聯(lián)慣性導(dǎo)航系統(tǒng)與雷載測速系統(tǒng)，建立多模型自適應(yīng)卡爾曼濾波器。系統(tǒng)模型見圖2。

圖2　魚雷多模型自適應(yīng)聯(lián)邦卡爾曼濾波器原理框圖Fig.2　Principle diagram of multi-model adaptive federated Kalman filter for a torpedo

首先確定信息分配系數(shù)。定義3個子濾波器的信息分配系數(shù)分別為β1，β2和β3，主濾波器的信息分配系數(shù)為βm，滿足β1+β2+β3+βm=1。文中，使主濾波器只對子濾波器的估計輸出進(jìn)行綜合而不進(jìn)行濾波運算，即令主濾波器的信息分配系數(shù)βm=0。其他子濾波器的信息分配系數(shù)按照其估計的均方誤差陣進(jìn)行如下分配［8］

式中，i=1，2，3。

子濾波器1是多模自適應(yīng)卡爾曼濾波器，其算法如下。

1）卡爾曼濾波

時間更新

量測更新

2）模型概率計算

對于第i個模型，其被分配的概率為

3）輸出綜合

子濾波器2和子濾波器3是標(biāo)準(zhǔn)的卡爾曼濾波器，分別采用標(biāo)準(zhǔn)的卡爾曼濾波算法得到狀態(tài)估計）以及狀態(tài)估計的均方誤差陣P2（k）和P3（k），文中不再贅述。

最后，在得到各子濾波器的估計值后按式（17）和式（18）進(jìn)行最優(yōu)合成

4　試驗驗證

實驗室導(dǎo)航試驗驗證是費效比最佳的驗證方式。對文中濾波器所采用的魚雷多模型自適應(yīng)聯(lián)邦卡爾曼濾波算法進(jìn)行實驗室導(dǎo)航試驗驗證。

試驗設(shè)備以雷體特性模擬模塊為中心，通過雷體方程解算模擬雷體運動特性，并將角運動信息送至轉(zhuǎn)臺控制設(shè)備，驅(qū)動轉(zhuǎn)臺按照雷體運動特性進(jìn)行轉(zhuǎn)動，而線運動特性則與產(chǎn)品直接構(gòu)成解算回路。試驗中，角運動由導(dǎo)航產(chǎn)品直接敏感測量，線運動由數(shù)學(xué)模擬產(chǎn)生，根據(jù)需要加入轉(zhuǎn)速測速、磁航向等誤差后，完成導(dǎo)航定位系統(tǒng)的實驗室導(dǎo)航試驗。魚雷實驗室導(dǎo)航試驗原理見圖3。

圖3　魚雷實驗室導(dǎo)航試驗框圖Fig.3　Diagram of torpedo navigation test in laboratory

實驗室導(dǎo)航試驗數(shù)據(jù)見圖4～圖5。導(dǎo)航周期為5 ms，陀螺漂移為1°/h，加計零偏為3×10-3g。

由捷聯(lián)慣導(dǎo)/雷載測速/深度信息推算的雷速/磁航向進(jìn)行組合濾波。其中，雷載測速的量測噪聲均值為0.75 m/s，深度信息推算的雷速量測噪聲均值為0.5 m/s，磁航向計量測噪聲為0.5°；考慮到慣性導(dǎo)航系統(tǒng)在工作初期航向誤差較小，在航行1200 s后加入磁航向信息。分別使用傳統(tǒng)單一模型卡爾曼濾波算法、多模自適應(yīng)聯(lián)邦卡爾曼濾波算法進(jìn)行濾波計算。導(dǎo)航定位誤差結(jié)果見圖6～圖13。

圖4　實驗室導(dǎo)航試驗下航姿曲線Fig.4　Curves of attitude angle versus time from navigation test in laboratory

圖5　實驗室導(dǎo)航試驗下速度曲線Fig.5　Curves of speed versus time from navigation test in laboratory

圖6　傳統(tǒng)單一模型卡爾曼濾波算法下航姿誤差曲線Fig.6　Curves of attitude angle error versus time with traditional single model Kalman filter algorithm

由圖4和圖5可知，航行彈道包含變向和變速機(jī)動，采用多模型自適應(yīng)卡爾曼濾波算法，使濾波器參數(shù)實時逼近系統(tǒng)的動態(tài)性能，較之傳統(tǒng)單一模型的卡爾曼濾波算法，定位誤差顯著降低，在航行2 800 s后，定位誤差為1 136 m，比傳統(tǒng)單一模型的卡爾曼濾波算法定位誤差降低約38.2％。

圖7　多模自適應(yīng)聯(lián)邦卡爾曼濾波算法下航姿誤差曲線Fig.7　Curves of attitude angle error versus time with multimodel adaptive federated Kalman filter algorithm

圖8　傳統(tǒng)單一模型濾波算法下北向速度誤差曲線Fig.8　Curves of north speed error versus time with traditional single model Kalman filter algorithm

圖9　多模自適應(yīng)聯(lián)邦卡爾曼濾波算法下北向速度誤差曲線Fig.9　Curves of north speed error versus time with multi-model adaptive federated Kalman filter algorithm

5　結(jié)論

針對魚雷在全航程中會有彈道機(jī)動，洋流變化，子系統(tǒng)故障等多種環(huán)境條件的變化，單一固定的濾波器參數(shù)必將導(dǎo)致估計精度降低的問題，文中設(shè)計了一種多模自適應(yīng)聯(lián)邦卡爾曼濾波器，通過實驗室導(dǎo)航試驗驗證，可以得到如下結(jié)論。

圖10　傳統(tǒng)單一模型濾波算法下東向速度誤差曲線Fig.10　Curve of east speed error versus time with traditional single model Kalman filter algorithm

圖11　多模自適應(yīng)聯(lián)邦卡爾曼濾波算法下東向速度誤差曲線Fig.11　Curve of east speed error versus time with multimodel adaptive federated Kalman filter algorithm

圖12　傳統(tǒng)單一模型卡爾曼濾波算法下定位誤差曲線Fig.12　Curve of positioning error versus time with traditional single model Kalman filter algorithm

1）在魚雷導(dǎo)航定位系統(tǒng)中，利用多模型卡爾曼濾波器參數(shù)來逼近系統(tǒng)的動態(tài)性能，通過帶有不同參數(shù)值的濾波器估計的加權(quán)值得到對系統(tǒng)狀態(tài)的估計，在保持原有姿態(tài)精度的基礎(chǔ)上，提高了定位精度。

2）在多模自適應(yīng)聯(lián)邦卡爾曼濾波算法中，由協(xié)方差和卡爾曼增益矩陣均是相互獨立的。因此，多模型自適應(yīng)聯(lián)邦卡爾曼濾波器比較穩(wěn)定。

圖13　多模自適應(yīng)聯(lián)邦卡爾曼濾波算法下定位誤差曲線Fig.13　Curve of positioning error versus time with multimodel adaptive federated Kalman filter algorithm

［1］李艷華，房建成.一種多模型自適應(yīng)聯(lián)邦濾波器及其INS/CNS/GPS組合導(dǎo)航系統(tǒng)中的應(yīng)用［J］.航天控制，2003，21（2）：33-38.

Li Yan-hua，F(xiàn)ang 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與慣性及多傳感器組合導(dǎo)航系統(tǒng)原理［M］.李濤，譯.北京：國防工業(yè)出版社，2011.

［3］Charles K，Chui G R.卡爾曼濾波及其實時應(yīng)用［M］.戴洪德，等譯.北京：清華大學(xué)出版社，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］李建辰.魚雷定位技術(shù)［M］.北京：國防工業(yè)出版社，2009.

［6］秦永元，張洪鉞，汪淑華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，2012.

［7］曹萌，李建辰，國琳娜，等.自適應(yīng)聯(lián)邦濾波算法在魚雷多參量導(dǎo)航定位中的應(yīng)用［J］.魚雷技術(shù)，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］劉勇，徐世杰.基于聯(lián)邦UKF算法的月球探測自主組合導(dǎo)航［J］.宇航學(xué)報，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.

（責(zé)任編輯：楊力軍）

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-），女，在讀碩士，主要研究方向為武器系統(tǒng)與運用工程制導(dǎo)技術(shù).