一種魯棒GPS/INS組合導(dǎo)航濾波算法設(shè)計(jì)

王偉， 叢寧， 鄔佳

(哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，黑龍江 哈爾濱 150001)

GPS/INS組合導(dǎo)航系統(tǒng)能夠充分利用2種導(dǎo)航系統(tǒng)的優(yōu)點(diǎn)進(jìn)行互補(bǔ)，具有高精度、高可靠性的特點(diǎn)，在動(dòng)態(tài)定位領(lǐng)域獲得了極為廣泛的應(yīng)用。GPS/INS中常用的濾波方式主要是卡爾曼算法[1-3]，但該算法會(huì)出現(xiàn)過(guò)于依賴噪聲特性準(zhǔn)確性的情況，魯棒性較差，有諸多學(xué)者對(duì)其進(jìn)行了研究和改進(jìn)。文獻(xiàn)[4]提出了基于協(xié)方差匹配技術(shù)的自適應(yīng)UKF算法，解決了慣性器件和GPS的噪聲分布不準(zhǔn)確時(shí)UKF的精度下降的問(wèn)題。文獻(xiàn)[5]提出的自適應(yīng)魯棒CKF，應(yīng)用馬氏距離減弱了異常量測(cè)值對(duì)估計(jì)的影響，一定程度上增強(qiáng)了系統(tǒng)的魯棒性。文獻(xiàn)[6]提出一種改進(jìn)的自適應(yīng)擴(kuò)展卡爾曼濾波算法，該算法可以基于殘差實(shí)時(shí)估計(jì)系統(tǒng)噪聲。但當(dāng)系統(tǒng)噪聲或量測(cè)噪聲不確定，再或初始條件不具備時(shí)，上述Kalman衍生算法均不能實(shí)現(xiàn)精確估計(jì)。

為解決上述問(wèn)題，Shmaliy等[7]提出了一種在濾波過(guò)程中能夠無(wú)視噪聲統(tǒng)計(jì)特性的無(wú)偏有限沖擊響應(yīng)濾波算法(unbiased finite impulse response filter, UFIR)。該算法在不明確系統(tǒng)噪聲、量測(cè)噪聲的統(tǒng)計(jì)特性且初始條件未知的情況下，僅利用最佳窗口長(zhǎng)度1個(gè)參數(shù)就能對(duì)系統(tǒng)做出精確估計(jì)。最佳濾波窗長(zhǎng)的大小會(huì)影響濾波精度和運(yùn)算時(shí)間。為了提高確認(rèn)最佳濾波窗長(zhǎng)的效率，文獻(xiàn)[8]提出一種在線估計(jì)方法，但該方法沒(méi)有實(shí)現(xiàn)真正的在線估計(jì)，影響系統(tǒng)的實(shí)時(shí)性。且由于UFIR自身的濾波原理所限，其導(dǎo)航精度要略差于卡爾曼濾波及其衍生濾波。

針對(duì)上述問(wèn)題，本文將UFIR應(yīng)用于GPS/INS組合導(dǎo)航系統(tǒng)中，提出一種級(jí)聯(lián)式導(dǎo)航濾波器，設(shè)計(jì)一種在線估計(jì)最佳窗口長(zhǎng)度算法的同時(shí)，引入了級(jí)聯(lián)自適應(yīng)卡爾曼濾波器以提高濾波器的導(dǎo)航估計(jì)精度。

1 UFIR及其改進(jìn)算法

1.1 UFIR算法

設(shè)系統(tǒng)狀態(tài)方程和量測(cè)方程為：

(1)

式中：X(t)為系統(tǒng)狀態(tài)矢量；F(t)為狀態(tài)轉(zhuǎn)移矩陣；W(t)為系統(tǒng)噪聲矢量；Z(t)為量測(cè)矢量；H(t)為量測(cè)矩陣；V(t)為量測(cè)噪聲矢量。

離散后為：

(2)

式中：Xk表示第k時(shí)刻的系統(tǒng)狀態(tài)矢量，即X(t)，同理，Zk為Z(t)，Wk為W(t)，Vk為V(t)，F(xiàn)k為F(t)。當(dāng)已知第k時(shí)刻的量測(cè)時(shí)，利用從m=k-N+1時(shí)刻到k時(shí)刻的N組量測(cè)可以實(shí)現(xiàn)對(duì)k時(shí)刻的系統(tǒng)狀態(tài)估計(jì)。

1.1.1 濾波過(guò)程

UFIR不需要設(shè)置噪聲協(xié)方差矩陣和初始誤差，它以最佳濾波窗長(zhǎng)代替了上述初始條件。在最佳濾波窗長(zhǎng)下獲取的估計(jì)值具有最小的均方誤差(the mean-square error, MSE)。UFIR算法從k-N+1時(shí)刻開(kāi)始通過(guò)濾波獲得估計(jì)值，且通過(guò)并行計(jì)算降低了窗口濾波的運(yùn)算量，不需要為獲得某一時(shí)刻的估計(jì)值而重復(fù)運(yùn)行N遍。對(duì)上文中建立的狀態(tài)空間模型進(jìn)行擴(kuò)展[9]

(3)

將狀態(tài)轉(zhuǎn)移矩陣作如下擴(kuò)展：

(4)

本文中觀測(cè)矩陣Hk為時(shí)不變矩陣，可簡(jiǎn)化表示為H,故H的狀態(tài)擴(kuò)展可表示為：

(5)

與卡爾曼濾波原理相似，UFIR濾波也利用類似的思想，令UFIR增益為[10]：

(6)

(7)

則：

(8)

1.1.2 批處理過(guò)程

UFIR濾波初始條件由批處理濾波獲得：

(9)

(10)

式中s=m+M-1,M為批處理窗長(zhǎng)。

由上述推導(dǎo)可看出，濾波窗長(zhǎng)N的大小決定了矩陣的維數(shù)，同時(shí)也決定了UFIR算法的運(yùn)算速度，而找出算法的最佳濾波窗長(zhǎng)是濾波的前提。

1.2 在線估計(jì)設(shè)計(jì)與實(shí)現(xiàn)

UFIR濾波器是一種窗口濾波器。雖然在使用中不需要提供初始狀態(tài)向量、初始協(xié)方差矩陣、系統(tǒng)噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性，但需要對(duì)其濾波窗口大小N進(jìn)行估計(jì)。N越大，估計(jì)效果可能越好，但同時(shí)運(yùn)算量急劇增大；N過(guò)小，達(dá)不到要求的濾波精度。所以最佳濾波窗長(zhǎng)的在線估計(jì)是UFIR濾波算法的重點(diǎn)。

本文對(duì)文獻(xiàn)[8]中提出的在線估計(jì)方法進(jìn)行了改進(jìn)，提出了一種適用于時(shí)變系統(tǒng)的在線估計(jì)最佳濾波窗長(zhǎng)算法。為保證濾波性能只與濾波窗長(zhǎng)N有關(guān)，將批處理窗長(zhǎng)M設(shè)為定值(批處理窗長(zhǎng)M?N)。

1.2.1 確定Nmin和Nmax

為了能夠更快的確定最佳濾波窗長(zhǎng)，首先需要確定窗長(zhǎng)的變化范圍。現(xiàn)有方法中窗長(zhǎng)的范圍一般為經(jīng)驗(yàn)值，不能適應(yīng)不同條件下的需求，且若給定的范圍過(guò)大，則影響運(yùn)算速度，范圍小則不能包含最佳濾波窗長(zhǎng)。

針對(duì)上述情況，本文設(shè)計(jì)了詳細(xì)的運(yùn)算步驟來(lái)計(jì)算范圍Nmin和Nmax，主要包括下列2個(gè)階段：

(11)

(12)

(13)

Gk=tr(MN)

(14)

(15)

取Nmin和Nmax之間的整數(shù)對(duì)剩下的數(shù)據(jù)進(jìn)行處理。范圍內(nèi)的每個(gè)N都會(huì)對(duì)剩余n-L+1組(n>L)數(shù)據(jù)進(jìn)行1次UFIR濾波處理，每σ組數(shù)據(jù)為一批次(σ為濾波窗口大小N與批處理窗口大小M的差值)。

(16)

再利用lN|m可得：

(17)

(18)

(19)

(20)

2 級(jí)聯(lián)濾波算法設(shè)計(jì)

與KF算法不同，UFIR不是最優(yōu)估計(jì)，由于自身的濾波原理所限，在系統(tǒng)噪聲特性變化不大時(shí)，其濾波精度略低于卡爾曼算法。為了兼顧系統(tǒng)的魯棒性和精度，本文提出一種級(jí)聯(lián)式GPS/INS導(dǎo)航濾波算法。圖1為INS/GPS組合導(dǎo)航系統(tǒng)的原理框圖。GPS為系統(tǒng)提供速度、位置和航向信息：速度位置與慣導(dǎo)解算出的速度位置之差作為主濾波器改進(jìn)UFIR算法的量測(cè)，繼而通過(guò)主濾波器對(duì)速度和位置信息進(jìn)行校正；將通過(guò)加速度計(jì)和GPS航向信息獲得的姿態(tài)與慣導(dǎo)解算出的姿態(tài)之差作為從濾波器的量測(cè)，再利用主濾波器的新息、UFIR增益等信息獲得自適應(yīng)因子，通過(guò)從濾波器完成對(duì)姿態(tài)信息的校正。

圖1 INS/GPS組合系統(tǒng)原理Fig.1 INS/GPS integrated system block diagram

2.1 主濾波器設(shè)計(jì)

采用間接法建立狀態(tài)方程，INS選取東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系。

選取系統(tǒng)的狀態(tài)量：

(21)

取速度位置差值作為觀測(cè)量：

(22)

相關(guān)參數(shù)變量見(jiàn)文獻(xiàn)[13]，這里不再贅述。

2.2 從濾波器設(shè)計(jì)

從濾波器狀態(tài)方程為四元數(shù)誤差線性微分方程，將姿態(tài)角誤差作為量測(cè)量，量測(cè)方程利用四元數(shù)誤差與姿態(tài)角誤差之間的轉(zhuǎn)換關(guān)系得到。

2.2.1 濾波算法模型

通過(guò)加性四元數(shù)誤差微分方程整理出的狀態(tài)方程為：

(23)

觀測(cè)量設(shè)為加速度計(jì)與雙天線GPS航向構(gòu)成的姿態(tài)角與慣導(dǎo)系統(tǒng)解算出的姿態(tài)角之差：

(24)

而上述姿態(tài)誤差角不能直接當(dāng)成觀測(cè)量使用，需要利用變換矩陣將其轉(zhuǎn)換為數(shù)學(xué)平臺(tái)失準(zhǔn)角才可用于導(dǎo)航濾波。

利用四元數(shù)與旋轉(zhuǎn)角的關(guān)系可知：

(25)

(26)

(27)

式中va(t)為量測(cè)白噪聲。

2.2.2 自適應(yīng)性改進(jìn)

本文將UFIR(主濾波器)的新息引入自適應(yīng)漸消因子ck[16-17]中，對(duì)級(jí)聯(lián)卡爾曼濾波器的從濾波器進(jìn)行改進(jìn)：

(28)

式中β為調(diào)節(jié)因子。

利用式(28)對(duì)自適應(yīng)卡爾曼濾波增益方程進(jìn)行優(yōu)化(具體卡爾曼算法流程見(jiàn)文獻(xiàn)[13])：

Kk=ckPk|k-1HT[HPk|k-1HT+Rk]-1

(29)

3 仿真評(píng)估與分析

通過(guò)仿真計(jì)算和實(shí)驗(yàn)驗(yàn)證對(duì)提出的導(dǎo)航濾波算法性能進(jìn)行了評(píng)估，并與文獻(xiàn)[6]中提出的改進(jìn)型自適應(yīng)擴(kuò)展卡爾曼濾波算法和最佳濾波窗長(zhǎng)下的標(biāo)準(zhǔn)UFIR算法進(jìn)行了比較。

3.1 仿真計(jì)算

圖2 仿真軌跡Fig.2 Simulation trajectory

采用增大仿真軌跡的噪聲方差的方式來(lái)驗(yàn)證級(jí)聯(lián)式GPS/INS松組合導(dǎo)航濾波器在系統(tǒng)噪聲和量測(cè)噪聲統(tǒng)計(jì)不準(zhǔn)確的情況下的濾波性能。在500～1 000 s，將系統(tǒng)噪聲標(biāo)準(zhǔn)差增大10倍，在1 000～1 500 s，將系統(tǒng)噪聲標(biāo)準(zhǔn)差增大20倍，在1 500～2 000 s，將系統(tǒng)噪聲標(biāo)準(zhǔn)差增30倍，在2 000～2 500 s，將系統(tǒng)噪聲標(biāo)準(zhǔn)差增大50倍，在2 500 s之后的時(shí)間段，將系統(tǒng)噪聲的標(biāo)準(zhǔn)差增大100倍。

圖3 最佳濾波窗長(zhǎng)曲線Fig.3 The line of optimal filter window length

圖4 速度誤差仿真曲線Fig.4 Velocity error simulation curve

圖5 位置誤差仿真曲線Fig.5 Position error simulation curve

3.2 實(shí)驗(yàn)驗(yàn)證

通過(guò)跑車實(shí)驗(yàn)驗(yàn)證CUFIR的實(shí)用性，跑車軌跡見(jiàn)圖6。

車載INS/GPS組合導(dǎo)航系統(tǒng)包括1套型號(hào)為ADIS16488的MEMS和1臺(tái)LEA-M8S GPS接收機(jī)，INS數(shù)據(jù)更新頻率為205 Hz，GPS更新頻率為5 Hz。跑車實(shí)驗(yàn)的真值由RTK提供。由于車載導(dǎo)航設(shè)備受外界環(huán)境的影響較大，故其系統(tǒng)噪聲統(tǒng)計(jì)特性雖會(huì)發(fā)生變化，但變化幅度會(huì)小于仿真所設(shè)計(jì)的情況。

圖6 跑車軌跡Fig.6 The actual driving test route

圖7 實(shí)測(cè)速度誤差曲線Fig.7 Measured velocity error curve

圖8 實(shí)測(cè)位置誤差曲線Fig.8 Measured position error curve

4 結(jié)論

1)提出的估計(jì)最佳濾波窗長(zhǎng)的方法能有效地實(shí)現(xiàn)窗長(zhǎng)的在線估計(jì)。

2)通過(guò)仿真與實(shí)測(cè)，級(jí)聯(lián)濾波算法CUFIR能有效提高濾波精度，增強(qiáng)系統(tǒng)的魯棒性。