基于 MEMS?INS/GNSS 組合導(dǎo)航抗差自適應(yīng)Kalman濾波算法

周 彬，趙萬良，榮義杰，胡小毛，馬吉雨

（1.上海航天控制技術(shù)研究所，上海201109；2.上海慣性工程技術(shù)研究中心，上海201109）

0 引言

標(biāo)準(zhǔn)的Kalman濾波器在 MEMS-INS/GNSS組合導(dǎo)航領(lǐng)域已得到廣泛的應(yīng)用，標(biāo)準(zhǔn)Kalman濾波解為最小方差濾波解，但它需要有一定的條件：適合的估計(jì)方法、可靠的觀測信息以及準(zhǔn)確的動態(tài)模型。然而，在實(shí)際應(yīng)用中，系統(tǒng)的動態(tài)模型是復(fù)雜多變的，其中包括超高速運(yùn)動、振動環(huán)境、變加速運(yùn)動、超機(jī)動運(yùn)動等運(yùn)動狀態(tài)。針對這些復(fù)雜運(yùn)動狀態(tài)，構(gòu)建精確的系統(tǒng)動態(tài)模型是非常困難的，這需要對載體的運(yùn)動特性有充分準(zhǔn)確的掌握。另外，GNSS接收機(jī)在復(fù)雜運(yùn)動環(huán)境下易受干擾，導(dǎo)致衛(wèi)星信號的觀測信息誤差瞬時(shí)增大。如果不對這些異常的信息做特別的處理，那么當(dāng)前系統(tǒng)的動態(tài)模型參數(shù)的估計(jì)值和觀測信息是不準(zhǔn)確的，經(jīng)過Kalman濾波之后則會導(dǎo)致濾波結(jié)果發(fā)散。因此，如何利用當(dāng)前觀測信息和狀態(tài)估值更新先驗(yàn)信息和補(bǔ)償運(yùn)動方程誤差，已成為當(dāng)前組合導(dǎo)航領(lǐng)域尤其是動態(tài)導(dǎo)航技術(shù)研究領(lǐng)域的熱點(diǎn)及難點(diǎn)之一。

近年來，為了進(jìn)一步減弱動態(tài)模型和觀測信息帶來的誤差的影響，先后發(fā)展了自適應(yīng)濾波、抗差濾波、Sage自適應(yīng)濾波等。本文在Kalman濾波的基礎(chǔ)上，設(shè)計(jì)了抗差自適應(yīng)Kalman濾波對組合導(dǎo)航系統(tǒng)進(jìn)行研究。根據(jù)實(shí)驗(yàn)結(jié)果可以得到：利用抗差自適應(yīng)kalman濾波能夠有效地抑制誤差發(fā)散，提供更加精確的導(dǎo)航結(jié)果。而且相比于標(biāo)準(zhǔn)Kalman濾波，基于抗差自適應(yīng)濾波的組合導(dǎo)航系統(tǒng)的誤差波動幅度降低了很多，能夠更好地控制觀測信息誤差以及動力學(xué)模型異常所帶來的影響，在組合導(dǎo)航領(lǐng)域上具有很好的研究意義。

1 抗差自適應(yīng)Kalman濾波器的設(shè)計(jì)

首先對標(biāo)準(zhǔn)的 Kalman濾波進(jìn)行分析，設(shè)Kalman濾波的狀態(tài)方程為：

式（1）中，Xk為tk時(shí)刻狀態(tài)向量，Xk－1為tk－1時(shí)刻的m維狀態(tài)向量，Wk為Gauss白噪聲過程誤差向量，Φk，k－1為tk－1時(shí)刻m×m階狀態(tài)轉(zhuǎn)移矩陣。

Kalman濾波的觀測方程為：

式（2）中，Vk為n×m階的觀測噪聲向量，Hk為n×m階的觀測矩陣。

1）預(yù)測的狀態(tài)向量：

2）預(yù)測的狀態(tài)向量協(xié)方差矩陣：

3）增益矩陣：

4）新一時(shí)刻的狀態(tài)估計(jì)值：

5）新一時(shí)刻的狀態(tài)估計(jì)值的協(xié)方差矩陣：

從式（3）～式（7）可以得到，Kalman 濾波是采用遞推的算法對狀態(tài)空間進(jìn)行一系列的動態(tài)估計(jì)，即根據(jù)前一時(shí)刻參數(shù)的估計(jì)值和新一時(shí)刻的觀測數(shù)據(jù)進(jìn)行狀態(tài)參數(shù)的更新。當(dāng)系統(tǒng)的動態(tài)模型參數(shù)的估計(jì)值和觀測信息是不準(zhǔn)確的，濾波器得到的估計(jì)誤差的方差可能趨于無窮大，估計(jì)值具有有偏性，即發(fā)生濾波發(fā)散的現(xiàn)象。

Kalman濾波的極值條件為：

式（8）中，Pk為Lk的權(quán)矩陣，為的權(quán)矩陣，Lk和互為不相關(guān)，為系統(tǒng)的狀態(tài)預(yù)測向量。根據(jù)式（8）可以推導(dǎo)出標(biāo)準(zhǔn)Kalman濾波的解為：

當(dāng)系統(tǒng)的觀測數(shù)據(jù)不準(zhǔn)確時(shí)，則將Kalman濾波的極值條件變換成抗差濾波的極值條件。

式（10）對觀測向量采用了抗差估計(jì)的原則，而對狀態(tài)參數(shù)的先驗(yàn)值采用最小二乘的原則，故可稱為M-LS濾波原則，可以得到抗差估計(jì)值為：

可以看出，式（11）的Kalman濾波解與式（10）的抗差估計(jì)值的形式是基本一致的，只是用觀測向量Zk的抗差協(xié)方差矩陣替換掉原先的協(xié)方差矩陣Σk。 當(dāng)預(yù)測的動態(tài)模型與實(shí)際運(yùn)動狀態(tài)有偏差或者是動態(tài)模型發(fā)生異常時(shí)，則采用自適應(yīng)濾波方法，該濾波原則為：

式（12）中，αk為自適應(yīng)因子（0≤αk≤1）。αk的取值是根據(jù)動力學(xué)模型誤差大小決定的，當(dāng)動力學(xué)模型的誤差小于某個(gè)設(shè)定的閾值時(shí)，則αk＝1；當(dāng)動力學(xué)模型的誤差大于某個(gè)設(shè)定的閾值時(shí)，則αk＜1。當(dāng)αk的值確定之后，得到自適應(yīng)濾波的解為：

抗差估計(jì)主要是對觀測信息中的隨機(jī)變量進(jìn)行判斷，當(dāng)系統(tǒng)的觀測信息存在較大的偏差或者發(fā)生異常現(xiàn)象時(shí)，通過降低異常觀測信息的權(quán)重來減小異常觀測信息對濾波結(jié)果的影響。而自適應(yīng)濾波則是根據(jù)觀測量信息是否正確來判斷系統(tǒng)的動力學(xué)模型信息是否發(fā)生異常，若觀測量信息發(fā)生異常情況，則判定動力學(xué)模型信息有誤，因此觀測量信息的可靠程度直接影響了整個(gè)系統(tǒng)濾波的效果。當(dāng)動力學(xué)模型和觀測量信息同時(shí)發(fā)生異常時(shí)，為了降低兩種異常情況對狀態(tài)參數(shù)估計(jì)值的影響，根據(jù)抗差估計(jì)原理以及自適應(yīng)估計(jì)原理可實(shí)現(xiàn)抗差自適應(yīng)濾波，該濾波構(gòu)造極值原則如下：

式（14）對動態(tài)系統(tǒng)的狀態(tài)預(yù)測信息采用了自適應(yīng)估計(jì)原則，而對觀測向量信息采用了抗差估計(jì)原則，對狀態(tài)參量估值求極值后，可得：

根據(jù)式（15），可以得到觀測殘差和狀態(tài)預(yù)測殘差為：

將式（16）和式（17）代入式（15），可以計(jì)算出抗差自適應(yīng)濾波解為：

至此，就可以得到抗差自適應(yīng)Kalman濾波的解。抗差自適應(yīng)濾波解的流程如圖1所示，抗差自適應(yīng)濾波是通過改變自適應(yīng)因子以及等價(jià)權(quán)矩陣的值，來降低動觀測向量信息異常誤差與動力學(xué)模型異常誤差對濾波器的影響。當(dāng)運(yùn)動載體的實(shí)際運(yùn)動狀態(tài)與設(shè)定的動力學(xué)模型有較大的偏差時(shí)，則通過減小自適應(yīng)因子αk的值來抑制系統(tǒng)狀態(tài)模型預(yù)測信息異常對狀態(tài)參數(shù)估計(jì)值的影響。當(dāng)觀測向量信息存在較大的誤差時(shí)，即觀測向量發(fā)生異常，則減小對應(yīng)的觀測噪聲的等價(jià)權(quán)矩陣的元素，從而降低觀測向量信息異常對狀態(tài)參量估計(jì)值的影響。當(dāng)動力學(xué)模型與觀測向量信息都正常時(shí)，即αk＝1，，整個(gè)動態(tài)系統(tǒng)則是正常利用動力學(xué)模型以及觀測向量信息進(jìn)行最優(yōu)濾波估計(jì)，此時(shí)的抗差自適應(yīng)濾波就是標(biāo)準(zhǔn)Kalman濾波。

準(zhǔn)確地觀測噪聲等價(jià)權(quán)矩陣和自適應(yīng)因子能夠有效地減小因異常信息而產(chǎn)生的誤差，因此，如何構(gòu)造正確的等價(jià)權(quán)矩陣以及自適應(yīng)因子是整個(gè)抗差自適應(yīng)濾波理論的關(guān)鍵。對觀測噪聲等價(jià)權(quán)矩陣進(jìn)行求解，得到狀態(tài)向量的抗差估計(jì)值為：

式（19）中，為觀測矩陣。 由式（19）可以推導(dǎo)得到殘差向量為：

通過采用Huber的兩段函數(shù)法，可以求得基于殘差向量的觀測等價(jià)權(quán)矩陣元素：

式（21）中，σVki為殘差分量Vki的均方差，為標(biāo)準(zhǔn)化殘差，qi為觀測向量Zk的等價(jià)權(quán)矩陣的第i個(gè)對角線元素。

接下來是對自適應(yīng)因子進(jìn)行構(gòu)造，常用的構(gòu)造方法有兩種：兩段函數(shù)法和三段函數(shù)法。

（1）兩段函數(shù)法

兩段函數(shù)模型是非歸零函數(shù)的一種，若判別統(tǒng)計(jì)量增大，自適應(yīng)因子的值則會隨之減小，反之亦然，但自適應(yīng)因子的值永不為0。兩段函數(shù)模型所表示的自適應(yīng)因子為：

式（22）中，c為常量，一般在1.0～2.5之間取值，其最優(yōu)值為1。

式（23）中，自適應(yīng)因子的取值為0＜αk≤1，Δk為狀態(tài)不符值統(tǒng)計(jì)量，當(dāng) Δ越大，αk越小，動力學(xué)模型的權(quán)重也就越小。

（2）三段函數(shù)法

與二段函數(shù)法相同，三段函數(shù)表示的自適應(yīng)因子為：

式（24）中，c0和c1分別為兩個(gè)常量，它們的取值為c0＝1.0～1.5，c1＝3.0～4.5，自適應(yīng)因子的取值為0≤αk≤1。

由上述方法得到自適應(yīng)因子后，就可以計(jì)算出對應(yīng)的濾波解：遞推解和加權(quán)平均解。

遞推解滿足：

加權(quán)平均解為：

其中，遞推解中的自適應(yīng)因子不能為0，因此不適用于三段函數(shù)法。另外，狀態(tài)參量的預(yù)測值與當(dāng)前歷元觀測得到的狀態(tài)參量抗差估計(jì)值之差所組成的函數(shù)決定自適應(yīng)因子的取值，該函數(shù)的值能反映狀態(tài)參量預(yù)測值的偏離程度，該值越大，則表明系統(tǒng)的實(shí)際觀測值與狀態(tài)預(yù)估值偏差越大。

式（27）中，為當(dāng)前歷元的狀態(tài)預(yù)測向量，，r為狀態(tài)參數(shù)抗差估計(jì)值。

當(dāng)觀測向量無偏時(shí)，即E（Zk）＝HkXk，濾波解，a的數(shù)學(xué)期望為：

若狀態(tài)預(yù)測向量也無偏，即E（），則：

根據(jù)式（29），當(dāng)狀態(tài)預(yù)測量X～k和觀測量Zk均無異常誤差時(shí)，那么自適應(yīng)濾波解也滿足無偏性。從理論研究可以得到，抗差自適應(yīng)濾波通過引入自適應(yīng)因子和等價(jià)權(quán)矩陣能夠有效地降低狀態(tài)信息異常和觀測向量信息異常帶來的影響。

2 跑車試驗(yàn)與性能分析

為了進(jìn)一步驗(yàn)證本文設(shè)計(jì)的相關(guān)算法以及分析組合導(dǎo)航系統(tǒng)的性能情況，本文進(jìn)行了MEMSINS/GNSS組合導(dǎo)航系統(tǒng)的跑車試驗(yàn)，跑車運(yùn)動的三維動態(tài)軌跡如圖2所示。在跑車試驗(yàn)中，組合導(dǎo)航系統(tǒng)安裝固定在汽車內(nèi)，MEMS-INS和GNSS同時(shí)采集雙方的數(shù)據(jù)，數(shù)據(jù)保存后并在Matlab中進(jìn)行時(shí)間對準(zhǔn)以及濾波分析。

整個(gè)跑車試驗(yàn)過程總共有8顆可見星（2、7、9、10、15、20、25、30號衛(wèi)星），其中10號衛(wèi)星的高度角最高作為基準(zhǔn)衛(wèi)星，本文選取某一段GNSS信號較為廣闊的一組數(shù)據(jù)進(jìn)行分析。圖3～圖5為MEMS-INS/GNSS組合導(dǎo)航相較于基準(zhǔn)導(dǎo)航系統(tǒng)的姿態(tài)角誤差圖。

對上述數(shù)據(jù)進(jìn)行相應(yīng)的統(tǒng)計(jì)分析，得到在不同濾波算法下的組合導(dǎo)航系統(tǒng)的姿態(tài)角誤差均值，如表1所示。

本文選取了一組時(shí)間較長的位置和速度信息數(shù)據(jù)，經(jīng)過解算得到組合導(dǎo)航系統(tǒng)的位置、速度誤差如圖6所示。

表1 不同濾波算法下的系統(tǒng)姿態(tài)角誤差Table 1 System attitude error under different filtering algorithms

經(jīng)過數(shù)理統(tǒng)計(jì)得到位置、速度誤差，如表2、表3所示。

從兩種濾波的誤差結(jié)果可以得到，利用抗差自適應(yīng)Kalman濾波能夠有效地抑制誤差發(fā)散，提供更加精確的導(dǎo)航結(jié)果，而且相比于標(biāo)準(zhǔn)Kalman濾波，基于抗差自適應(yīng)濾波的組合導(dǎo)航系統(tǒng)的誤差波動幅度降低了很多，能夠更好地控制觀測信息誤差以及動力學(xué)模型異常所帶來的影響。

表2 組合導(dǎo)航系統(tǒng)的位置誤差Table 2 Position error of integrated navigation system

表3 組合導(dǎo)航系統(tǒng)的速度誤差Table 3 Velocity error of integrated navigation system

3 結(jié)論

本文針對高動態(tài)環(huán)境下載體動態(tài)復(fù)雜多變特性帶來的動力學(xué)模型難以精確構(gòu)建問題，以及衛(wèi)星跟蹤環(huán)路信號易受擾動導(dǎo)致的觀測信息出現(xiàn)異常粗差問題，設(shè)計(jì)了一種基于MEMS-INS/GNSS組合導(dǎo)航系統(tǒng)抗差自適應(yīng)Kalman濾波算法，并研究了優(yōu)化自適應(yīng)因子的求解方法。最后，通過跑車實(shí)驗(yàn)驗(yàn)證了抗差自適應(yīng)Kalman濾波算法的抗異常粗差能力以及抗干擾能力。