李小將， 王志恒， 王建華， 張東來

（1.裝備學院 航天裝備系，北京101416； 2.裝備學院 研究生管理大隊，北京101416）

INS／GPS組合導航通常需要濾波器來實現(xiàn)數(shù)據(jù)融合并減小定位誤差，交互式多模型（interacting multiple model，IMM）濾波算法是一種常用的INS／GPS組 合 導 航 濾 波 算 法［1－3］。采 用IMM算法進行組合導航狀態(tài)估計時，如果模型集中的某個模型與系統(tǒng)實際模態(tài)匹配，則可以得到較好的濾波效果；如果實際模態(tài)不在模型集覆蓋的范圍內(nèi)，IMM算法濾波精度降低甚至發(fā)散［4］。因此在組合導航系統(tǒng)中應用IMM算法時，通常需要一個較大的模型集，但是由于IMM算法使用多個并行的濾波器，實現(xiàn)對每一個模型進行計算，這會帶來巨大的計算量，難以滿足導航系統(tǒng)實時性要求。

IMM算法中，一方面要保證模型集有足夠的模型來覆蓋系統(tǒng)的實際模態(tài)；另一方面要求減小模型數(shù)量以保證計算速度，這是相互矛盾的。針對IMM算法的上述問題，LI X R在文獻［5］中提出了變 結(jié)構(gòu)多模型（variable structure multiple model，VSMM）算法的思想，并在文獻［6］中給出了算法的具體實現(xiàn)。VSMM算法被廣泛應用在機動目標跟蹤領(lǐng)域［7－9］，但是VSMM算法在組合導航中的應用和研究的文獻尚未見到，鑒于此，本文研究了應用VSMM算法對INS／GPS組合導航進行狀態(tài)估計的問題，并進行了仿真計算。仿真結(jié)果表明，基于有向圖切換的變結(jié)構(gòu)多模型（DS－VSMM）算法的INS／GPS組合導航系統(tǒng)能夠得到比使用IMM算法更高的導航精度，并且濾波計算量明顯降低。

1 INS／GPS組合導航數(shù)學模型

以輸出三維位置、速度和姿態(tài)信息的INS和輸出三維位置和速度信息的GPS為組合對象進行研究。采用INS的誤差方程作為系統(tǒng)的狀態(tài)方程，導航坐標系選用東北天地理坐標系，通過對INS的性能及誤差源的分析，可以獲得濾波器的誤差方程為

式中：系統(tǒng)的狀態(tài)變量定義為15階，即

式中：φe，φn，φu為數(shù)學平臺誤差角；Δve，Δvn，Δvu為速度誤差；Δλ，ΔL，Δh為經(jīng)度、緯度和高度誤差；εe，εn，εu，e，n，u分別為陀螺和加速度計零偏；FI（t）為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，GI（t）為系統(tǒng)噪聲輸入矩陣，WI（t）為零均值高斯白噪聲，式（1）中各矩陣具體形式可見文獻［10］。

組合導航系統(tǒng)的量測量為INS給出的位置、速度與GPS給出的位置、速度的差值，建立量測方程如下：

式中：量測矩陣H＝［03×6，I6×6，06×6］，V（t）為量測噪聲，假設(shè)為零均值的高斯白噪聲，其協(xié)方差R（t）通常隨著載體的運動狀態(tài)變化和外界擾動等因素的影響而發(fā)生變化［11－12］。

2 有向圖切換變結(jié)構(gòu)多模型算法

通常一個VSMM方法具有2層結(jié)構(gòu)：高層和低層。高層負責模型集合自適應（model－set adeptation，MSA），低層負責在確定的模型集合內(nèi)運用IMM算法進行計算。不同于IMM算法，所有時刻都使用同一個模型集合進行計算，而VSMM算法，每一時刻所使用的模型集合可能只是總體模型集合中的一部分，因此能夠有效降低計算量。截至目前，根據(jù)模型集合自適應思想的不同，可以將VSMM算法分為有向圖切換方法、激活有向圖方法和自適應網(wǎng)格方法3類［13］，基于這些思想產(chǎn)生了：模型群切換（model group switching，MGS）算法、可能模型集（likely－model set，LMS）算法、期望模型擴張（expected－model augmentation，EMA）算法等具體的VSMM算法。VSMM算法的提出最初是為了解決IMM算法的模型數(shù)量與計算量的矛盾問題，它的成功主要依賴于高效的MSA方法。所謂MSA方法，就是確定一個可以在線決定哪些模型需要刪除，哪些模型需要添加的規(guī)則。建立更加智能、高效的MSA方法是VSMM估計的理論研究的核心。有向圖切換是一種比較簡單通用的MSA方法，針對上述INS／GPS組合導航變噪聲濾波問題，建立了DS－VSMM模型集有向圖，并設(shè)計了有向圖切換規(guī)則。

2.1 INS／GPS組合導航模型集合的建立

LI X R將圖論的思想引入多模型估計問題中，并利用圖論的相關(guān)理論，解決了模型集合自適應的問題。模型中的每一個模型可以看作圖的一個頂點；模型之間的相互轉(zhuǎn)化，可用一條帶有權(quán)值的有向邊表示，則VSMM算法所使用的模型集和模型轉(zhuǎn)換法則一起，便構(gòu)成了一個隨機有向圖［14］。

本文根據(jù)INS／GPS導航系統(tǒng)的量測噪聲方差矩陣的變化范圍，建立一個包含5個模型的模型集合D，如圖1所示。這里假設(shè)M1為R（k）變化的最小值；M5為R（k）變化的最大值；M3為R（k）變化的中間值，并且R（k）大部分時間在M3附近波動。將模型集合按照R（k）的變化規(guī)律劃分為3個 子 模 型 集 合：D1、D2、D3。D1＝｛M2，M3，M4｝，D2＝｛M1，M2，M3｝，D3＝｛M3，M4，M5｝，3個子模型集合分別以M2、M3、M4為中心模型，3個子模型也可稱作有向子圖，中心模型的后驗概率與有向圖切換密切相關(guān)。

圖1 INS／GPS組合導航模型集合

2.2 有向圖切換規(guī)則的建立

根據(jù)圖1的模型集合，本文根據(jù)中心模型的概率進行有向子圖之間的切換。有向圖切換規(guī)則具體為

式中：D（k）表示k時刻匹配的有向圖；μM1、μM2、μM3分別為M1、M2、M33個模型k－1時刻的后驗概率；μ為一個大于0.5的、根據(jù)經(jīng)驗設(shè)定的門限值。某個中心模型概率一旦大于門限值，即切換到以該模型為中心的有向子圖進行IMM運算；若沒有中心模型后驗概率大于門限值，則不進行有向圖切換。需要注意的是，由于模型切換的馬爾可夫性，不允許從D2直接切換到D3，而必須通過D1作為過渡，反之亦然；每個模型也只允許在相鄰的模型間切換，這些將在轉(zhuǎn)移概率矩陣中體現(xiàn)。

2.3 DS－VSMM算法的具體步驟

DS－VSMM一個計算周期包括2個步驟：根據(jù)中心模型概率和有向圖切換規(guī)則進行有向圖切換；在匹配有向圖內(nèi)進行IMM濾波。DS－VSMM算法流程［15］如圖2所示。

圖2 DS－VSMM算法流程

2.3.1 有向圖切換

根據(jù)上一周期的有向子圖以及有向圖切換規(guī)則選擇當前周期與系統(tǒng)實際模態(tài)匹配的有向子圖。

2.3.2 IMM濾波

以當前所位于的有向子圖為IMM算法的模型集進行濾波估計。IMM算法主要包含4個步驟。

1）輸入交互：新激活模型的初始概率由那些允許切換到這些新模型的上一時刻模型（狀態(tài)轉(zhuǎn)移概率矩陣中的對應值不為零）概率值得到，新激活模型的初始估計，也由那些允許切換到這些新模型的上一時刻模型的估計值確定。

2）模型濾波：選擇適當?shù)臑V波器對當前有向圖中的每個模型進行濾波計算，本文選擇標準的卡爾曼濾波器。

3）模型概率更新：計算各模型的后驗概率，用于輸出融合和決定下一時刻的MSA。

4）輸出融合：將不同模型的估計值，按照概率加權(quán)組合得到最終濾波估計。

IMM算法的計算過程可參見文獻［16］。

3 仿真分析

為了檢驗算法的性能，將DS－VSMM算法應用到INS／GPS組合導航系統(tǒng)中。選取INS為主導航系統(tǒng)，以姿態(tài)、速度及位置誤差為狀態(tài)向量，建立濾波器狀態(tài)方程和量測方程，如第1節(jié)中式（1）和式（3）所示，GPS作為輔助導航系統(tǒng)提供量測信息。

分別采用IMMKF，DS－VSMM濾波器對載體的姿態(tài)、速度及位置等誤差信息進行估計，算法的性能好壞可通過INS／GPS組合導航系統(tǒng)的定位精度和濾波計算時間來衡量。假設(shè)載體初始位置為東經(jīng)116°，北緯40°，高度50m，以20m／s的速度勻速向東運動，持續(xù)1 400s。INS的陀螺常值漂移為0.01°／h，隨機漂移為0.001°／h；加速度計零偏為10－4g，隨機漂移為10－5g；GPS水平位置誤差為10m，高度誤差為20m，水平速度誤差為0.1m／s，垂直速度誤差0.4m／s。地球半徑r取6 378 137 m。INS采樣周期為0.1s，GPS采樣周期為1s。

量測噪聲協(xié)方差矩陣的初始值為R0＝diag｛（10m）2；（10 m）2；（20 m）2；（0.1 m／s）2；（0.1m／s）2；（0.4m／s）2｝，在載體運動過程中，假設(shè)GPS受外界干擾使量測噪聲R（k）出現(xiàn)如下變化：

DS－VSMM濾波器中模型集合如圖1所示，M1～M5中量測噪聲方差矩陣分別為0.25R0、0.5R0、R0、2R0、4R0，有向圖劃分與2.1節(jié)中D1、D2、D3相同，轉(zhuǎn)移概率矩陣為

中心模型切換的門限值μ取為0.6。IMM濾波器模型集中同樣采用上述5個模型，轉(zhuǎn)移概率矩陣為

根據(jù)以上條件應用IMM算法和DS－VSMM算法對量測數(shù)據(jù)進行濾波計算，有量測信息時進行濾波，無量測信息時只根據(jù)各個模型進行預測，得到INS／GPS組合導航系統(tǒng)的東向位置誤差、東向速度誤差、航向角誤差分別如圖3～圖5所示。

圖3 2種濾波東向位置誤差對比

圖4 2種濾波東向速度估計誤差對比

圖5 2種濾波航向角誤差對比

從圖3～圖5可以看出，在采用相同模型集的情況下，分別采用IMMKF和DS－VSMM濾波時，INS／GPS組合導航系統(tǒng)的速度誤差、航向角誤差大小基本一致，但使用DS－VSMM濾波時，位置誤差要小于IMMKF濾波。表1給出了使用2種算法的計算時間。

表1 IMMKF和DS－VSMM濾波計算時間

IMMKF算法每一時刻都使用5個模型計算；DS－VSMM算法每一時刻使用3個模型計算，理論上，單從每個模型計算量來看，DS－VSMM濾波器計算量應該是IMM濾波器的60%。但是由于有向圖切換的計算增加了部分計算時間。因此，DS－VSMM算法濾波的計算時間是IMMEKF算法濾波的63.62%，這是可以接受的?？梢钥闯?，DS－VSMM濾波器相 對于IMMKF濾波器明顯降低了計算量。

4 結(jié) 論

本文針對INS／GPS組合導航的濾波過程中量測噪聲變化的問題，研究了將變結(jié)構(gòu)多模型算法應用到INS／GPS組合導航中的基本流程。根據(jù)有向圖切換的思想，建立了INS／GPS組合導航模型集合有向圖，并設(shè)計了一種模型集合自適應規(guī)則，對應用DS－VSMM算法的INS／GPS組合導航進行了仿真分析，仿真結(jié)果表明，DSVSMM濾波算法定位精度略優(yōu)于IMMKF算法，同時有效降低了濾波器的計算量。

（

）

［1］曹夢龍，崔平遠.多模型高精度組合導航算法研究［J］.系統(tǒng)工程與電子技術(shù)，2008，30（7）：1304－1307.

［2］王曉旭，趙琳.自適應融合濾波算法及其在INS／GPS組合導航中的應用［J］.宇航學報，2010，31（11）：2503－2511.

［3］QIAN Huaming，AN Di，XIA Quanxi.IMM－UKF based land－vehicle navigation with low－cost GPS／INS［C］／／IEEE.Proceedings of the 2010IEEE International Conference on Information and Automation.Harbin：IEEE，2010：2031－2035.

［4］徐田來，崔平遠，崔祜濤.組合導航自適應多模型算法研究［J］.系統(tǒng)工程與電子技術(shù)，2008，30（11）：2071－2074.

［5］LI X R.Multiple－model estimation with variable structure：some theoretical considerations［C］／／IEEE.33rdIEEE Conf.Decision and Control.Lake Buena Vista：IEEE，1994：1199－1204.

［6］LI X R，SHALOM Y B.Multiple－model estimation with variable structure［J］.IEEE Transactions on Automatic Control，1996，41（4）：478－493.

［7］陳曉峰，嵇成新，陳陽.一種新的機動目標跟蹤中的變結(jié)構(gòu)多模型算法［J］.火控雷達技術(shù)，2007，36：31－35.

［8］雷世文，吳慈伶，孫偉.一種基于VSMM的自適應高機動目標跟蹤方法［J］.現(xiàn)代雷達，2010，32（6）：54－58.

［9］郭云飛，張幸，林岳松.基于DS－VSMM的聲網(wǎng)絡(luò)低空機動目標跟蹤［J］.光電工程，2011，38（8）：1－6.

［10］卞鴻巍，李安，覃方君，等.現(xiàn)代信息融合技術(shù)在組合導航中的應用［M］.北京：國防工業(yè)出版社，2010：100－106.

［11］肖乾.偽距GPS／INS組合導航系統(tǒng)的多模型自適應卡爾曼濾波器設(shè)計［J］.船舶工程，2005，27（6）：59－62.

［12］劉睿，王常虹，李葆華.自適應交互多模型濾波在INS／CNS組合導航中的應用［J］.紅外與激光工程，2010，39（5）：843－847.

［13］劉揚，劉火平，吳勤章.多模型估計的新分支：變結(jié)構(gòu)多模型方法［J］.現(xiàn)代防御技術(shù)，2011，39（3）：163－169.

［14］許江湖，嵇成新，張永勝，等.基于當前統(tǒng)計模型的有向圖切換IMM算法［J］.火力與指揮控制，2003，28（2）：52－56.

［15］黃翔宇，彭冬亮.基于無味有向圖切換的機動目標跟蹤VSMM算法［J］.光電工程，2010，37（12）：30－34.

［16］林雪原.基于交互式多模型的多傳感器組合導航系統(tǒng)［J］.兵工自動化，2011，30（6）：27－30.