基于多模型聯(lián)合濾波的MIMU/GPS組合導(dǎo)航方法

沈 強(qiáng)，劉潔瑜，李 燦，崔亞龍，李新三

（1. 火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，西安 710025；2. 中國人民解放軍96851 部隊，盤錦 124214）

慣性導(dǎo)航系統(tǒng)具有隱蔽性、自主性、寬頻性以及信息全面等優(yōu)勢，在軍用和民用領(lǐng)域均得到了廣泛應(yīng)用。其缺點為導(dǎo)航誤差隨時間累積，特別是對于MIMU 這種精度相對較低的慣性系統(tǒng)而言，有必要通過組合導(dǎo)航的方式對導(dǎo)航誤差進(jìn)行校正[1]。

組合導(dǎo)航需要高性能導(dǎo)航濾波技術(shù)的支撐。目前，導(dǎo)航濾波通常采用概率化濾波方法來解決[2-4]。這類方法解決了很多應(yīng)用中的問題，但是，這些概率化方法對噪聲的分布都有嚴(yán)格的要求，并要求其統(tǒng)計特性已知，導(dǎo)致這類方法存在一定的缺陷。為解決該問題，可采用自適應(yīng)的卡爾曼濾波算法，將系統(tǒng)的動態(tài)變化看作隨機(jī)干擾歸到模型和噪聲中去，對模型參數(shù)和噪聲統(tǒng)計特性進(jìn)行估計和修正使之適應(yīng)系統(tǒng)動態(tài)變化[5-7]，組合導(dǎo)航中使用的自適應(yīng)濾波的方法有多種，包括模糊自適應(yīng)濾波、衰減記憶濾波、Sage-Husa自適應(yīng)濾波等，研究表明，在組合導(dǎo)航中自適應(yīng)濾波可以修正一定的噪聲統(tǒng)計特性。近年來，另一類濾波方法，集員估計方法[8-10]正逐漸受到重視。該類方法只要求噪聲未知但有界（Unknown But Bounded, UBB），由于在實際系統(tǒng)中噪聲的邊界信息較之概率化假設(shè)來說更容易獲取，可克服概率化濾波方法在應(yīng)用中存在的局限性。

但是，對于MIMU/GPS 組合導(dǎo)航系統(tǒng)，兩類噪聲是同時存在的，GPS 接收機(jī)跟蹤環(huán)路中的熱噪聲是典型的高斯白噪聲，而動態(tài)應(yīng)力和振動引起的時鐘相位抖動則屬于非高斯噪聲[11]，MEMS 陀螺的隨機(jī)漂移是白噪聲與多種非高斯噪聲的疊加。對于這些包含了未知不確定干擾的非高斯噪聲，獲取邊界往往比獲取其隨機(jī)統(tǒng)計特性更加可行。在這種情況下，采用隨機(jī)噪聲的濾波方法會導(dǎo)致估計結(jié)果過于樂觀甚至失去收斂性，而采用集員估計方法又會造成邊界過于保守，所以，對于含有兩種噪聲的系統(tǒng)，人為地忽略其中一種不確定性，單獨使用隨機(jī)噪聲類方法或集員估計方法對估計結(jié)果都是不利的。為克服單一方法的局限，充分利用兩種方法的優(yōu)勢，保證可靠的估計結(jié)果，有學(xué)者提出了結(jié)合兩種噪聲模型的聯(lián)合濾波（Combined Filter）方法。文獻(xiàn)[12]用概率密度函數(shù)集合取代單一的概率密度函數(shù)來描述噪聲造成的不確定性，提出了集合概率的概念。Liu[13]等針對純方位機(jī)動目標(biāo)的跟蹤問題，在橢球集員濾波的更新過程中考慮了隨機(jī)噪聲，仿真結(jié)果顯示其跟蹤性能優(yōu)于EKF。文獻(xiàn)[14]則是采用可加性隨機(jī)噪聲和有界噪聲的設(shè)定，并假設(shè)系統(tǒng)中的兩種噪聲獨立存在，隨機(jī)噪聲采用卡爾曼濾波處理，有界噪聲則采用橢球定界方法處理，然后結(jié)合在一起構(gòu)成聯(lián)合濾波算法。仿真結(jié)果顯示，這類算法可以避免非高斯噪聲導(dǎo)致的卡爾曼濾波性能下降，同時得到魯棒性較強(qiáng)的邊界保證估計。但是，對于導(dǎo)航類應(yīng)用，當(dāng)運動狀態(tài)多變時，單模型方法會因為無法跟蹤載體的機(jī)動狀態(tài)而導(dǎo)致精度損失，而交互多模型（Interacting Multiple Model, IMM）方法因其優(yōu)異的性能得到了廣泛的應(yīng)用[15,16]。

為此，本文將高斯噪聲融入集員框架下，構(gòu)造雙重不確定模型集，同時推導(dǎo)了多個橢球集的加權(quán)Minkowski 和的運算過程并給出了參數(shù)優(yōu)化方法，從而實現(xiàn)集合運算與IMM 交互過程相結(jié)合，將其中的UBB 噪聲應(yīng)用集員估計的思想進(jìn)行處理，提出了考慮兩種不確定干擾的交互多模型聯(lián)合濾波（IMM-based Combined filter, IMMCF ）方法，并將用于MIMU/GPS 組合導(dǎo)航中。

1 雙重不確定性組合導(dǎo)航系統(tǒng)模型

在松組合[1]模式下，GPS 和MIMU 各自獨自工作，然后通過適當(dāng)?shù)臑V波算法對兩種系統(tǒng)的數(shù)據(jù)進(jìn)行融合，得到最終的導(dǎo)航結(jié)果，系統(tǒng)狀態(tài)方程中的狀態(tài)向量共15 維，包括緯度、經(jīng)度、高度誤差 δL、δ λ 、δ h ，東向、北向、天向速度誤差 δ VE、δVN、 δVU，數(shù)學(xué)平臺姿態(tài)角誤差δφE、δφN、 δφU以及x、y、 z 軸上的陀螺漂移誤差 εbx、εby、εbz和加速度計漂移誤差Bbx、Bby、Bbz。

狀態(tài)方程為:

式中，系統(tǒng)噪聲為w=[ wgx,wgy,wgz,wax,way,waz]T，其中的元素通常假設(shè)為x、 y 、 z 軸上的陀螺白噪聲和加速度計白噪聲，為了提高導(dǎo)航的精度和魯棒性，本文采用高斯/有界雙重噪聲來描述其誤差特性。系統(tǒng)噪聲分配陣為

量測方程為：

式中，L、λ 、h 、 VE、VN、 VU分別為系統(tǒng)緯度、經(jīng)度、高度和東向、北向、天向速度，而下標(biāo)I 代表MIMU 解算值，G 代表GPS 測量值。 RM、 RN分別為載體位置對應(yīng)的子午圈和卯酉圈的曲率半徑。量測陣H 為6×15 維矩陣，其非零元素為H1,1=RM，H2,2=RN， H3,3= 1， H4,4= 1， H5,5= 1， H6,6= 1。觀測噪聲為 v=其中的元素為GPS 沿地理坐標(biāo)系各軸向的距離、速度測量誤差，本文采用高斯/有界雙重噪聲來描述其誤差特性。

離散化后的組合導(dǎo)航系統(tǒng)方程如下所示：

過程噪聲和量測噪聲分別描述為：

其中， Dk和 Ek為已知的正定矩陣。由此可以得到過程噪聲和量測噪聲的分布分別為

初始狀態(tài) x0符合如下分布：

2 交互多模型聯(lián)合濾波算法

2.1 多橢球加權(quán)Minkowski 和運算

在標(biāo)準(zhǔn)IMM 算法中，模型間的交互是以加權(quán)求和的方式實現(xiàn)的。當(dāng)系統(tǒng)中高斯噪聲和有界噪聲均存在時，模型的交互涉及到集合運算，最終會表現(xiàn)為多個橢球的加權(quán)Minkowski 和，并利用橢球近似外包實現(xiàn)狀態(tài)的重初始化和更新。為求解多橢球加權(quán)Minkowski 和的外定界橢球，首先給出如下引理[17]：

給定橢球集：

則對于

? α ∈Rr滿足αk＞ 0且包含其中：

對于橢球集 E（ak,Mk），給定 μk∈R，容易得到結(jié)合引理1，可以得到如下結(jié)論：給定 μk∈R 和橢球集 E（ak,Mk）則對于? α ∈Rr滿足αk＞ 0且橢球 E（a ,M）包含其中：

依照上述結(jié)論得到一個與參數(shù)α 有關(guān)的橢球簇，該橢球簇包含多個橢球的加權(quán)Minkowski 和，需要依據(jù)一定準(zhǔn)則尋求其中的最優(yōu)解。考慮到導(dǎo)航的實時性要求，本文采用最小跡準(zhǔn)則。待優(yōu)化的目標(biāo)函數(shù)可以寫為

利用拉格朗日乘子法，可以得到最小跡準(zhǔn)則下的最優(yōu)參數(shù)取值為

2.2 算法推導(dǎo)

IMMCF 與標(biāo)準(zhǔn)IMM 算法的主要區(qū)別在于加入了集合的運算，也就是其估計結(jié)果包括協(xié)方差陣 Ck和橢球集該集合本質(zhì)上是狀態(tài)估計均值的集合。當(dāng)考慮UBB 噪聲時，各濾波器的重初始化過程和總體估計計算過程實際上是多個橢球集的加權(quán)Minkowski 和，需要根據(jù) 2.1 節(jié)內(nèi)容進(jìn)行加權(quán)Minkowski 和外包橢球的求解和參數(shù)的優(yōu)化，而各子濾波器的更新則需要同時進(jìn)行橢球集和協(xié)方差的更新。

IMMCF 算法的結(jié)構(gòu)如圖1 所示：

圖1 IMMCF 算法結(jié)構(gòu) (三個模型)Fig.1 Structure of IMMCF algorithm (with three models)

在第1 節(jié)的雙重不確定性組合導(dǎo)航系統(tǒng)模型基礎(chǔ)上，本文對于組合導(dǎo)航算法模型集的構(gòu)造思路與文獻(xiàn)[16]相似，同樣是采用不同大小的量測噪聲來構(gòu)造多個模型，通過模型的交互使噪聲大小可以覆蓋一定的范圍，最終實現(xiàn)高精度的導(dǎo)航信息估計。同時將量測噪聲和過程噪聲作為雙重不確定性噪聲進(jìn)行處理。各模型狀態(tài)空間方程與式(5)(6)一致，只是量測噪聲不同，不同模型量測噪聲信息描述如下：

其中，上標(biāo)（i）表示模型m（i），R為噪聲基準(zhǔn)，di、ei為模型系數(shù)。模型構(gòu)造后，即可按照如下步驟進(jìn)行導(dǎo)航信息的更新：

Step 2：模型條件重初始化。根據(jù)標(biāo)準(zhǔn)IMM 算法過程計算混合概率協(xié)方差陣根據(jù)2.1 節(jié)的結(jié)論計算重初始化的混合估計橢球

Step 3：模型條件濾波。計算各匹配模型濾波器的估計橢球和協(xié)方差陣，單模型具體更新過程參考文獻(xiàn)[14]；

Step 4：模型概率更新。根據(jù)標(biāo)準(zhǔn)IMM 算法過程計算模型概率；

Step 5：估計融合。根據(jù)2.1 節(jié)的結(jié)論計算總體估計橢球以及協(xié)方差陣則為當(dāng)前時刻的導(dǎo)航誤差信息估計；

Step 6：令k←k+ 1并返回Step 2，直到程序終止。

3 試驗驗證

系統(tǒng)采用的慣導(dǎo)為HX-IMU-M02 慣性組件，由MIMU 和外部采集板兩部分構(gòu)成。MIMU 集成了3 軸MEMS 陀螺、加速度計、磁強(qiáng)計，試驗中應(yīng)用MEMS陀螺和加速度計進(jìn)行慣性導(dǎo)航解算。GPS接收機(jī)為北斗星通的C200 系列接收機(jī)，搭配高精度板卡BDM682。試驗設(shè)備主要技術(shù)指標(biāo)如表1 所示。

表1 主要技術(shù)指標(biāo)Tab.1 Main technical specifications

為檢驗算法性能，利用上述設(shè)備進(jìn)行了推車試驗，試驗中將MIMU 和GPS 接收機(jī)一同放置在試驗平臺上，GPS 天線放置在推車上部，試驗裝置如圖2所示。

圖2 導(dǎo)組合航試驗裝置Fig.2 Test device of integrated navigation

試驗進(jìn)行兩次，第1 次試驗推車沿矩形軌跡行進(jìn)，時間為1100 s，第2 次試驗推車沿8 字軌跡行進(jìn)，時間為1500 s，兩次試驗中推車的運動軌跡如圖3 所示。

圖3 試驗裝置運動軌跡Fig.3 Trajectory of test device

試驗中系統(tǒng)的初始位置、速度誤差分別設(shè)置為2 m、0.1 m/s，GPS 數(shù)據(jù)更新率為1 Hz，慣導(dǎo)系統(tǒng)數(shù)據(jù)更新率為200 Hz。該系統(tǒng)可以使用信號接收器接收到的多GNSS 系統(tǒng)對載體位置和速度進(jìn)行解算，所以試驗中將多系統(tǒng)聯(lián)合解算的結(jié)果作為試驗參考值。上位機(jī)通過USB 接口接收GPS 和MIMU 數(shù)據(jù)并利用IMMCF 算法進(jìn)行處理，采用三模型結(jié)構(gòu)，噪聲基準(zhǔn)： R=diag （0.5m20.5m20.5m23(m/s)23(m/s)25(m/s)2） 模型系數(shù)如表2 所示。

表2 不同模型的噪聲系數(shù)值Tab.2 Noise coefficient of different models

通過計算，得到組合導(dǎo)航的位置、速度、姿態(tài)信息，如圖4-9 所示。

圖4 試驗1 姿態(tài)角Fig.4 Attitude angle in test 1

作為對比，同時也使用標(biāo)準(zhǔn)IMM 濾波器以及使用單個模型（模型2）的聯(lián)合濾波方法進(jìn)行了組合導(dǎo)航解算。另外，表3 和表4 給出了使用不同算法得到的導(dǎo)航誤差的統(tǒng)計特性。

表3 試驗1 導(dǎo)航誤差統(tǒng)計特性Tab.3 Statistical characteristics of navigation error in Test 1

表4 試驗2 導(dǎo)航誤差統(tǒng)計特性Tab.4 Statistical characteristics of navigation error in Test 2

圖5 試驗1 位置信息Fig.5 Location in test 1

圖6 試驗1 速度信息Fig.6 Velocity in test 1

圖7 試驗2 姿態(tài)角Fig.7 Attitude angle in test 2

圖8 試驗2 位置信息Fig.8 Location in test 2

圖9 試驗2 速度信息Fig.9 Velocity in Test 2

由于推車試驗只是在小范圍的地面上運動，所以結(jié)果中忽略了天向速度和高度。同時，為方便比較，表中位置誤差的統(tǒng)計結(jié)果用單位米（m）描述。首先，無論是從圖中的導(dǎo)航位置速度信息還是表中的誤差統(tǒng)計特性來看，本文提出的方法在MIMU/GPS 組合導(dǎo)航系統(tǒng)中能正確導(dǎo)航解算，且兩次試驗中多模型方法和單模型方法相比都具有明顯優(yōu)勢。如試驗1 中采用單模型方法導(dǎo)航的最大位置誤差達(dá)到了10 m，且有逐漸增大的趨勢，利用多模型方法則將試驗1 的位置誤差限制在了1.5 m 以內(nèi)，這充分體現(xiàn)了多模型的優(yōu)勢。同是多模型方法，IMMCF 算法與標(biāo)準(zhǔn)IMM 算法相比具有一定的優(yōu)勢，特別是在速度估計方面精度提高明顯。

另外，根據(jù)IMMCF 估計結(jié)果中的邊界橢球，得到了各導(dǎo)航信息的估計邊界，如圖10-13 所示?？梢钥闯觯嘞到y(tǒng)輸出結(jié)果包含在IMMCF 算法估計得到的邊界之內(nèi)，這對于提高導(dǎo)航系統(tǒng)的魯棒性和載體后續(xù)的制導(dǎo)、控制和路徑規(guī)劃都具有重要的意義，而這種邊界估計是標(biāo)準(zhǔn)IMM 算法無法實現(xiàn)的。

圖10 試驗1 位置估計邊界Fig.10 Location boundary in Test 1

圖11 試驗1 速度估計邊界Fig.11 Velocity boundary in Test 1

圖12 試驗2 位置估計邊界Fig.12 Location boundary in Test 2

圖13 試驗2 速度估計邊界Fig.13 Velocity boundary in Test 2

3 結(jié) 論

本文根據(jù)組合導(dǎo)航系統(tǒng)中有界噪聲和隨機(jī)噪聲兩種不確定性噪聲共存的問題，提出了交互多模型聯(lián)合濾波算法。首先，將隨機(jī)噪聲融入集員框架中，用分布集合的概念來描述雙重不確定模型，并介紹了以此為基礎(chǔ)的雙重不確定系統(tǒng)聯(lián)合濾波方法；而后，推導(dǎo)了多橢球加權(quán)Minkowski 和的外定界橢球的運算過程，并提出參數(shù)優(yōu)化方法，將該過程融入IMM 交互過程中，給出了交互多模型聯(lián)合濾波算法的具體迭代過程；最后，將提出的方法用于MIMU/GPS 組合導(dǎo)航，試驗結(jié)果表明，IMMCF 算法估計精度優(yōu)于標(biāo)準(zhǔn)IMM算法，同時也優(yōu)于單模型濾波方法。由于充分利用了兩種不同性質(zhì)的噪聲，該方法既可以避免單一噪聲假設(shè)導(dǎo)致的導(dǎo)航性能下降，又可以得到保證邊界估計，具有較強(qiáng)的魯棒性。