基于多模型聯合濾波的MIMU/GPS組合導航方法

沈 強，劉潔瑜，李 燦，崔亞龍，李新三

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

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

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

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

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

1 雙重不確定性組合導航系統模型

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

狀態方程為:

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

量測方程為：

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

離散化后的組合導航系統方程如下所示：

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

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

初始狀態 x0符合如下分布：

2 交互多模型聯合濾波算法

2.1 多橢球加權Minkowski 和運算

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

給定橢球集：

則對于

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

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

依照上述結論得到一個與參數α 有關的橢球簇，該橢球簇包含多個橢球的加權Minkowski 和，需要依據一定準則尋求其中的最優解。考慮到導航的實時性要求，本文采用最小跡準則。待優化的目標函數可以寫為

利用拉格朗日乘子法，可以得到最小跡準則下的最優參數取值為

2.2 算法推導

IMMCF 與標準IMM 算法的主要區別在于加入了集合的運算，也就是其估計結果包括協方差陣 Ck和橢球集該集合本質上是狀態估計均值的集合。當考慮UBB 噪聲時，各濾波器的重初始化過程和總體估計計算過程實際上是多個橢球集的加權Minkowski 和，需要根據 2.1 節內容進行加權Minkowski 和外包橢球的求解和參數的優化，而各子濾波器的更新則需要同時進行橢球集和協方差的更新。

IMMCF 算法的結構如圖1 所示：

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

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

其中，上標（i）表示模型m（i），R為噪聲基準，di、ei為模型系數。模型構造后，即可按照如下步驟進行導航信息的更新：

Step 2：模型條件重初始化。根據標準IMM 算法過程計算混合概率協方差陣根據2.1 節的結論計算重初始化的混合估計橢球

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

Step 4：模型概率更新。根據標準IMM 算法過程計算模型概率；

Step 5：估計融合。根據2.1 節的結論計算總體估計橢球以及協方差陣則為當前時刻的導航誤差信息估計；

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

3 試驗驗證

系統采用的慣導為HX-IMU-M02 慣性組件，由MIMU 和外部采集板兩部分構成。MIMU 集成了3 軸MEMS 陀螺、加速度計、磁強計，試驗中應用MEMS陀螺和加速度計進行慣性導航解算。GPS接收機為北斗星通的C200 系列接收機，搭配高精度板卡BDM682。試驗設備主要技術指標如表1 所示。

表1 主要技術指標Tab.1 Main technical specifications

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

圖2 導組合航試驗裝置Fig.2 Test device of integrated navigation

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

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

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

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

通過計算，得到組合導航的位置、速度、姿態信息，如圖4-9 所示。

圖4 試驗1 姿態角Fig.4 Attitude angle in test 1

作為對比，同時也使用標準IMM 濾波器以及使用單個模型（模型2）的聯合濾波方法進行了組合導航解算。另外，表3 和表4 給出了使用不同算法得到的導航誤差的統計特性。

表3 試驗1 導航誤差統計特性Tab.3 Statistical characteristics of navigation error in Test 1

表4 試驗2 導航誤差統計特性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 姿態角Fig.7 Attitude angle in test 2

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

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

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

另外，根據IMMCF 估計結果中的邊界橢球，得到了各導航信息的估計邊界，如圖10-13 所示。可以看出，多系統輸出結果包含在IMMCF 算法估計得到的邊界之內，這對于提高導航系統的魯棒性和載體后續的制導、控制和路徑規劃都具有重要的意義，而這種邊界估計是標準IMM 算法無法實現的。

圖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 結 論

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