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

王偉， 叢寧， 鄔佳

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

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

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

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

1 UFIR及其改進算法

1.1 UFIR算法

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

(1)

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

離散后為：

(2)

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

1.1.1 濾波過程

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

(3)

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

(4)

本文中觀測矩陣Hk為時不變矩陣，可簡化表示為H,故H的狀態(tài)擴展可表示為：

(5)

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

(6)

(7)

則：

(8)

1.1.2 批處理過程

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

(9)

(10)

式中s=m+M-1,M為批處理窗長。

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

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

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

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

1.2.1 確定Nmin和Nmax

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

針對上述情況，本文設(shè)計了詳細的運算步驟來計算范圍Nmin和Nmax，主要包括下列2個階段：

(11)

(12)

(13)

Gk=tr(MN)

(14)

(15)

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

(16)

再利用lN|m可得：

(17)

(18)

(19)

(20)

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

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

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

2.1 主濾波器設(shè)計

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

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

(21)

取速度位置差值作為觀測量：

(22)

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

2.2 從濾波器設(shè)計

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

2.2.1 濾波算法模型

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

(23)

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

(24)

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

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

(25)

(26)

(27)

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

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

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

(28)

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

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

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

(29)

3 仿真評估與分析

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

3.1 仿真計算

圖2 仿真軌跡Fig.2 Simulation trajectory

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

圖3 最佳濾波窗長曲線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 實驗驗證

通過跑車實驗驗證CUFIR的實用性，跑車軌跡見圖6。

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

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

圖7 實測速度誤差曲線Fig.7 Measured velocity error curve

圖8 實測位置誤差曲線Fig.8 Measured position error curve

4 結(jié)論

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

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