基于卡方檢驗(yàn)的自適應(yīng)魯棒CKF組合導(dǎo)航算法

熊 鑫，黃國(guó)勇，王曉東

(1.昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院，云南 昆明 650500；2.云南省礦物管道輸送工程技術(shù)研究中心，云南 昆明 650500)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)和全球衛(wèi)星導(dǎo)航系統(tǒng)(GPS)廣泛應(yīng)用于飛機(jī)和車輛載體的定位與姿態(tài)估計(jì)[1]。擴(kuò)展卡爾曼濾波(EKF)算法和無(wú)跡卡爾曼濾波(UKF)算法是目前導(dǎo)航中兩種常用的算法[2-3]，但卻存在一定的局限性。容積卡爾曼濾波(CKF)算法[4-5]，是近年來(lái)提出的一種新型非線性濾波算法，具有更好的實(shí)時(shí)性和濾波精度。但將CKF應(yīng)用于SINS/GPS組合導(dǎo)航系統(tǒng)，最大的困難是難以獲得系統(tǒng)模型和觀測(cè)模型完整的先驗(yàn)統(tǒng)計(jì)信息。由于難以準(zhǔn)確描述慣性系統(tǒng)的運(yùn)動(dòng)規(guī)律，建立的系統(tǒng)模型通常具有近似性，作為系統(tǒng)模型主要先驗(yàn)信息的過(guò)程噪聲方差對(duì)濾波精度有著重要的影響[6-7]，以及觀測(cè)衛(wèi)星失鎖，觀測(cè)模型和動(dòng)力學(xué)模型間存在的誤差，也會(huì)使系統(tǒng)可能發(fā)散[8-9]。為同時(shí)降低過(guò)程噪聲不確定性和觀測(cè)值異常對(duì)系統(tǒng)的影響，文獻(xiàn)[10]通過(guò)對(duì)誤差開窗擬合的方式來(lái)解決觀測(cè)異常和噪聲不確定的影響，但該方式會(huì)占用大量存儲(chǔ)資源不利于實(shí)時(shí)的處理。文獻(xiàn)[11]提出通過(guò)兩次卡方檢驗(yàn)對(duì)系統(tǒng)的判斷，在標(biāo)準(zhǔn)CKF，抗差濾波，和自適應(yīng)濾波進(jìn)行選擇，由于它至多只對(duì)一個(gè)參數(shù)進(jìn)行了調(diào)節(jié)，并不能處理觀測(cè)異常和噪聲不確定同時(shí)存在的情況。文獻(xiàn)[12]通過(guò)利用Doyle-Stein條件構(gòu)造的卡爾曼濾波對(duì)動(dòng)力學(xué)模型誤差不敏感這一現(xiàn)象，來(lái)分離觀測(cè)異常與模型誤差，但是該方法只用于受控系統(tǒng)。針對(duì)組合導(dǎo)航系統(tǒng)在過(guò)程噪聲統(tǒng)計(jì)特性不確定和觀測(cè)值異常時(shí)魯棒性差的問題，本方法將魯棒性估計(jì)與自適應(yīng)算法結(jié)合，提出了基于卡方檢驗(yàn)的自適應(yīng)魯棒CKF組合導(dǎo)航算法(CTAR-CKF)。

1 CKF算法

對(duì)于INS/GPS組合導(dǎo)航非線性離散系統(tǒng)建立如下：

(1)

對(duì)于非線性系統(tǒng)(1)，CKF的計(jì)算如下：

步驟1 初始化

(2)

步驟2 時(shí)間更新

對(duì)誤差協(xié)方差Pk-1/k-1進(jìn)行Cholesky分解：

(3)

計(jì)算容積點(diǎn)：

(4)

式(4)中，i為容積點(diǎn)序號(hào)。

通過(guò)系統(tǒng)函數(shù)傳播容積點(diǎn)：

(5)

計(jì)算狀態(tài)一步預(yù)測(cè)和對(duì)應(yīng)的協(xié)方差：

(6)

(7)

步驟3 量測(cè)更新

Cholesky因式分解：

(8)

計(jì)算新的容積點(diǎn)：

(9)

通過(guò)量測(cè)函數(shù)傳播容積點(diǎn)：

Zi,k/k-1=h(Xi,k/k-1)

(10)

K時(shí)刻的觀測(cè)預(yù)測(cè)值：

(11)

殘差ηk和自相關(guān)協(xié)方差Pzz,k/k-1：

(12)

(13)

互相關(guān)協(xié)方差Pxz,k/k-1：

(14)

步驟4 狀態(tài)更新

K時(shí)刻濾波增益：

(15)

K時(shí)刻狀態(tài)估計(jì)值：

(16)

K時(shí)刻狀態(tài)誤差協(xié)方差估計(jì)值：

(17)

2 基于卡方檢驗(yàn)的自適應(yīng)魯棒CKF

2.1 卡爾曼濾波的正交性原理

在卡爾曼濾波理論中，當(dāng)系統(tǒng)的模型確定且不存在模型誤差時(shí)，卡爾曼濾波輸出的殘差序列為零均值的高斯白噪聲，故這些殘差處處正交，這就是卡爾曼濾波的正交性[13]。

根據(jù)卡爾曼濾波的正交性原理，殘差馬氏距離平方服從卡方分布，造如下卡方檢驗(yàn)統(tǒng)計(jì)量：

(18)

因此可以通過(guò)計(jì)算殘差序列的馬氏距離的平方，進(jìn)而利用假設(shè)檢驗(yàn)原理對(duì)系統(tǒng)進(jìn)行評(píng)估，如果系統(tǒng)狀態(tài)突變或者存在觀測(cè)異常時(shí)，會(huì)使得殘差的馬氏距離的平方不再是自由度為n的卡方分布。

2.2 過(guò)程噪聲統(tǒng)計(jì)特性自適應(yīng)估計(jì)

引入卡方檢驗(yàn)，對(duì)系統(tǒng)進(jìn)行評(píng)估并通過(guò)預(yù)設(shè)的模糊邏輯函數(shù)計(jì)算相應(yīng)的過(guò)程噪聲加權(quán)值，利用加權(quán)值求取過(guò)程噪聲的估計(jì)值，從而達(dá)到自適應(yīng)調(diào)整過(guò)程噪聲統(tǒng)計(jì)特性的目的。

當(dāng)系統(tǒng)出現(xiàn)狀態(tài)突變時(shí)，過(guò)程噪聲的統(tǒng)計(jì)特性的不確定性增加，會(huì)導(dǎo)致濾波器的狀態(tài)估計(jì)值偏離系統(tǒng)的狀態(tài)值。此時(shí)可通過(guò)對(duì)過(guò)程噪聲統(tǒng)計(jì)協(xié)方差矩陣Qest的調(diào)整，調(diào)節(jié)濾波增益，減少過(guò)程噪聲統(tǒng)計(jì)特性不確定性對(duì)濾波結(jié)果的影響。當(dāng)過(guò)程噪聲統(tǒng)計(jì)特性不確定性增加時(shí)，Mk1會(huì)增加，反之，Mk1則會(huì)減小。則可通過(guò)設(shè)定檢測(cè)閾值的上界mmax和下界mmin來(lái)表示系統(tǒng)不確定性的程度，mmax和mmin分別取置信概率為99%和90%時(shí)所對(duì)應(yīng)的卡方檢驗(yàn)值，查表可知道m(xù)max=11.35，mmin=6.25。定義如下模糊邏輯函數(shù)，用于計(jì)算Qest：

(19)

Mk1為k時(shí)刻第一次卡方檢驗(yàn)的檢驗(yàn)值，初始時(shí)刻Q0，Qmin取正常情況下過(guò)程噪聲統(tǒng)計(jì)特性的協(xié)方差矩陣，Qmax一般取Qmin的整數(shù)倍。設(shè)定協(xié)方差矩陣的上界Qmax和下界Qmin是為了在特大機(jī)動(dòng)的情況下不會(huì)因?yàn)檫^(guò)大的卡方檢驗(yàn)值而得到過(guò)大的過(guò)程噪聲協(xié)方差矩陣，導(dǎo)致濾波發(fā)散[15]。

2.3 量測(cè)噪聲增強(qiáng)因子的自適應(yīng)估計(jì)

當(dāng)系統(tǒng)出現(xiàn)觀測(cè)異常時(shí)，卡爾曼濾波的殘差序列幅值增大，此時(shí)量測(cè)的權(quán)值需要降低，可以在式(13)中引入增強(qiáng)因子φk[16]：

(20)

并使下式成立：

(21)

利用公式，利用牛頓迭代求取φk，即：

(22)

(23)

(24)

這就保證了在測(cè)量粗差出現(xiàn)時(shí)增大了Rk。

2.4 算法實(shí)現(xiàn)

考慮非線性離散系統(tǒng)(1)，CTAR-CKF的主要計(jì)算過(guò)程如下：

步驟1 根據(jù)式(2)初始化狀態(tài)估計(jì)值和協(xié)方差。

步驟2 時(shí)間更新。根據(jù)式(3)—式(7)計(jì)算狀態(tài)預(yù)測(cè)值及其協(xié)方差陣。

步驟3 量測(cè)更新。根據(jù)式(8)—式(12)，式(20)，式(14)計(jì)算自相關(guān)協(xié)方差和互相關(guān)協(xié)方差。

步驟4 狀態(tài)更新。根據(jù)式(16)，式(17)計(jì)算狀態(tài)估計(jì)及其協(xié)方差陣。

步驟5 利用卡方檢驗(yàn)對(duì)系統(tǒng)模型進(jìn)行評(píng)估，利用式(19)更新過(guò)程噪聲方差估計(jì)值，若檢驗(yàn)值小于6.25，則直接跳到步驟7。

步驟6 再次利用卡方檢驗(yàn)，判斷是否出現(xiàn)量測(cè)異常，并利用式(24)更新噪聲增強(qiáng)因子。

步驟7 令xk-1=xk/k，Pk-1=Pk/k，k=k+1,返回步驟2，進(jìn)行下一次循環(huán)迭代。

3 仿真分析

3.1 仿真設(shè)置

SINS/GPS組合導(dǎo)航系統(tǒng)采用SINS系統(tǒng)為主系統(tǒng)解算得到姿態(tài)，速度，位置信息，并通過(guò)GPS系統(tǒng)的信息定期對(duì)SINS進(jìn)行反饋修正。建立的狀態(tài)方程為：

xk=f(xk-1)+ωk-1

(25)

式(25)中，f(·)為非線性函數(shù)，由SINS的力學(xué)編排和誤差方程得到。ωk-1為系統(tǒng)過(guò)程噪聲向量，xk為15維的狀態(tài)向量：

xk=[δrx,δry,δrz,δvx,δvy,δvz,δψx,δψy,δψz,εx,εy,εz,Δx,Δy,Δz]

(26)

式(26)中，δrx,δry,δrz為三軸位置誤差，δvx,δvy,δvz為三軸速度誤差，δψx,δψy,δψz為三軸姿態(tài)角誤差，εx,εy,εz為三軸陀螺儀零漂，Δx,Δy,Δz三軸加速度計(jì)零偏。加速度計(jì)和陀螺儀的誤差建模為一階馬爾科夫過(guò)程。量測(cè)方程建立如下：

(27)

ZSINS為SINS解算得到的三軸速度和三軸位，ZGPS為GPS輸出的三軸速度和三軸位置。仿真中GPS采樣周期為1 s，SINS的采樣周期為0.02 s，濾波周期為1 s,仿真其他參數(shù)設(shè)置如表1所示。

載體在0～77 s進(jìn)行了滑跑、加速拉起、爬高、改平飛行，75～200 s為平直飛行，200～220 s進(jìn)行了一次大機(jī)動(dòng)轉(zhuǎn)彎，221～440 s為平直飛行，441～600 s進(jìn)行了連續(xù)的機(jī)動(dòng)轉(zhuǎn)彎。飛行軌跡如圖1所示。為了驗(yàn)證算法的魯棒性和自適應(yīng)性，在100～105 s和220～225 s加入了20 m的量測(cè)粗差。這樣就存在如下三種噪聲情形，條件1,在100～105 s只存在量測(cè)異常；條件2,210～215 s系統(tǒng)存在較大機(jī)動(dòng)，系統(tǒng)過(guò)程噪聲統(tǒng)計(jì)特性的不確定性增加且存在量測(cè)異常；條件3,441～600 s系統(tǒng)只存在較大機(jī)動(dòng)。

表1 仿真參數(shù)

圖1 飛機(jī)飛行軌跡Fig.1 Trace of the airplane

3.2 仿真結(jié)果及分析

圖2為本算法的兩次卡方檢驗(yàn)結(jié)果。第一次檢驗(yàn)時(shí)由于未引入自適應(yīng)濾波和魯棒濾波相當(dāng)于標(biāo)準(zhǔn)CKF下的檢驗(yàn)值，對(duì)比兩圖可知，圖1在系統(tǒng)存在大機(jī)動(dòng)和量測(cè)粗差時(shí)，具有較大的卡方檢驗(yàn)值，因此CKF的卡方檢驗(yàn)值能檢測(cè)到系統(tǒng)的不準(zhǔn)確程度；而圖2中將本文算法引入過(guò)程噪聲自適應(yīng)修正后，由大機(jī)動(dòng)引起的系統(tǒng)模型不準(zhǔn)確程度得到了相應(yīng)的改善，對(duì)應(yīng)處的卡方檢驗(yàn)值均在置信概率為99%時(shí)所對(duì)應(yīng)的卡方檢驗(yàn)值之下，只有在存在量測(cè)粗差的時(shí)刻系統(tǒng)模型不準(zhǔn)確度仍較大。因此將置信概率為99%時(shí)所對(duì)應(yīng)的卡方檢驗(yàn)值作為第二次檢驗(yàn)的閾值，對(duì)于大于此閾值的時(shí)刻應(yīng)繼續(xù)進(jìn)行魯棒濾波以消除粗差的影響。

圖2 卡方檢驗(yàn)值Fig.2 Value of Chi-square test

分別采用CKF，魯棒CKF和本文所提出CTAR-CKF對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行仿真，結(jié)果如圖3—圖5所示。三種算法在直線飛行時(shí)都具有較好的濾波精度，但CKF算法在載體存在較大機(jī)動(dòng)和觀測(cè)值異常時(shí)濾波精度會(huì)有明顯的下降，魯棒CKF雖能較好的處理觀測(cè)值異常的情況，卻在系統(tǒng)強(qiáng)非線性機(jī)動(dòng)時(shí)，過(guò)程噪聲統(tǒng)的計(jì)特性的變化會(huì)使魯棒CKF的濾波精度會(huì)較低，本文所提出的CTAR-CKF算法不僅可以分別處理系統(tǒng)存在量測(cè)粗差和強(qiáng)非線性運(yùn)動(dòng)時(shí)的情況，且在過(guò)程噪聲統(tǒng)計(jì)特性變化和存在觀測(cè)值異常時(shí)仍能保持較好的濾波精度。

圖3 姿態(tài)誤差Fig.3 Estimation error of attitude

圖4 速度誤差Fig.4 Estimation error of velocity

圖5 位置誤差Fig.5 Estimation error of position

對(duì)比三種算法的均方根誤差(RMSE)，如表2—表4所示。在三種噪聲條件下，本方法都能保持著較好的濾波精度，在條件1時(shí)，CKF算法的三維位置誤差達(dá)19.6 m，算法CTAR-CKF和魯棒CKF的位置精度仍能保持在5.8 m和6.4 m，CKF三維速度誤差精度達(dá)0.35 m/s，算法CTAR-CKF和魯棒CKF速度精度分別為0.215 m/s和0.206 m/s。在條件2時(shí)，CKF的位置精度為18.3 m，算法魯棒CKF的位置精度下降，達(dá)到了11.9 m但算法CTAR-CKF仍能保持在4.6 m。算法CKF的速度精度為0.66 m/s，算法CTAR-CKF和魯棒CKF速度精度分別為0.49 m/s和0.15 m/s。在條件3時(shí)，算法CKF和魯棒CKF的位置精度分別達(dá)到了21.2 m和15.3 m,速度誤差精度分別達(dá)到了0.48 m/s和0.44 m/s，算法CTAR-CKF的位置誤差和速度誤差精度仍能保持在4.5 m和0.21 m/s。

表2 條件1中不同算法的精度比較(RMSE)

表3 條件2中不同算法的精度比較(RMSE)

表4 條件3中不同算法的精度比較(RMSE)

表5 解算時(shí)間

由表5可知，本文算法運(yùn)行時(shí)間略長(zhǎng)于CKF算，相較與魯棒CKF在解算時(shí)間上并沒有明顯的增加，但當(dāng)系統(tǒng)存在較大機(jī)動(dòng)，系統(tǒng)不確定性增加時(shí)，本文提出的算法比魯棒CKF卻具有更好的濾波精度。

綜上所述，本文算法能有效抑制過(guò)程噪聲變化和觀測(cè)異常對(duì)系統(tǒng)的影響，在不同噪聲條件下能得到優(yōu)于CKF和魯棒CKF算法的濾波精度，同時(shí)計(jì)算量同魯棒CKF相似，略高于CKF，但相對(duì)于CKF提高的系統(tǒng)精度，增加的計(jì)算量是可以接受的。

4 結(jié)論

本文提出了基于卡方檢驗(yàn)的自適應(yīng)魯棒CKF組合導(dǎo)航算法。該算法首先第一次引入卡方檢驗(yàn)對(duì)系統(tǒng)進(jìn)行評(píng)估，并根據(jù)卡方檢驗(yàn)值和預(yù)設(shè)的模糊邏輯函數(shù)對(duì)過(guò)程噪聲統(tǒng)計(jì)特性進(jìn)行調(diào)節(jié)；然后，再次利用卡方檢驗(yàn)對(duì)觀測(cè)異常進(jìn)行判斷，并通過(guò)增強(qiáng)因子對(duì)量測(cè)噪聲的統(tǒng)計(jì)特性進(jìn)行調(diào)節(jié)。仿真結(jié)果表明，算法能有效抑制過(guò)程噪聲變化和觀測(cè)異常對(duì)系統(tǒng)的影響，且在噪聲正常和觀測(cè)值充足的情況下也同樣適用。