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

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

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

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

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

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

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

在松組合[1]模式下，GPS 和MIMU 各自獨(dú)自工作，然后通過(guò)適當(dāng)?shù)臑V波算法對(duì)兩種系統(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)姿態(tài)角誤差δφE、δφN、 δφU以及x、y、 z 軸上的陀螺漂移誤差 εbx、εby、εbz和加速度計(jì)漂移誤差Bbx、Bby、Bbz。

狀態(tài)方程為:

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

量測(cè)方程為：

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

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

過(guò)程噪聲和量測(cè)噪聲分別描述為：

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

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

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

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

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

給定橢球集：

則對(duì)于

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

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

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

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

2.2 算法推導(dǎo)

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

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

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

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

其中，上標(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 算法過(guò)程計(jì)算混合概率協(xié)方差陣根據(jù)2.1 節(jié)的結(jié)論計(jì)算重初始化的混合估計(jì)橢球

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

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

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

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

3 試驗(yàn)驗(yàn)證

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

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

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

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

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

圖3 試驗(yàn)裝置運(yùn)動(dòng)軌跡Fig.3 Trajectory of test device

試驗(yàn)中系統(tǒng)的初始位置、速度誤差分別設(shè)置為2 m、0.1 m/s，GPS 數(shù)據(jù)更新率為1 Hz，慣導(dǎo)系統(tǒng)數(shù)據(jù)更新率為200 Hz。該系統(tǒng)可以使用信號(hào)接收器接收到的多GNSS 系統(tǒng)對(duì)載體位置和速度進(jìn)行解算，所以試驗(yàn)中將多系統(tǒng)聯(lián)合解算的結(jié)果作為試驗(yàn)參考值。上位機(jī)通過(guò)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

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

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

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

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

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

圖5 試驗(yàn)1 位置信息Fig.5 Location in test 1

圖6 試驗(yàn)1 速度信息Fig.6 Velocity in test 1

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

圖8 試驗(yàn)2 位置信息Fig.8 Location in test 2

圖9 試驗(yàn)2 速度信息Fig.9 Velocity in Test 2

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

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

圖10 試驗(yàn)1 位置估計(jì)邊界Fig.10 Location boundary in Test 1

圖11 試驗(yàn)1 速度估計(jì)邊界Fig.11 Velocity boundary in Test 1

圖12 試驗(yàn)2 位置估計(jì)邊界Fig.12 Location boundary in Test 2

圖13 試驗(yàn)2 速度估計(jì)邊界Fig.13 Velocity boundary in Test 2

3 結(jié) 論

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