一種魯棒GPS/INS組合導航濾波算法設計

王偉， 叢寧， 鄔佳

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

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

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

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

1 UFIR及其改進算法

1.1 UFIR算法

設系統狀態方程和量測方程為：

(1)

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

離散后為：

(2)

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

1.1.1 濾波過程

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

(3)

將狀態轉移矩陣作如下擴展：

(4)

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

(5)

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

(6)

(7)

則：

(8)

1.1.2 批處理過程

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

(9)

(10)

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

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

1.2 在線估計設計與實現

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

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

1.2.1 確定Nmin和Nmax

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

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

(11)

(12)

(13)

Gk=tr(MN)

(14)

(15)

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

(16)

再利用lN|m可得：

(17)

(18)

(19)

(20)

2 級聯濾波算法設計

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

圖1 INS/GPS組合系統原理Fig.1 INS/GPS integrated system block diagram

2.1 主濾波器設計

采用間接法建立狀態方程，INS選取東北天坐標系作為導航坐標系。

選取系統的狀態量：

(21)

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

(22)

相關參數變量見文獻[13]，這里不再贅述。

2.2 從濾波器設計

從濾波器狀態方程為四元數誤差線性微分方程，將姿態角誤差作為量測量，量測方程利用四元數誤差與姿態角誤差之間的轉換關系得到。

2.2.1 濾波算法模型

通過加性四元數誤差微分方程整理出的狀態方程為：

(23)

觀測量設為加速度計與雙天線GPS航向構成的姿態角與慣導系統解算出的姿態角之差：

(24)

而上述姿態誤差角不能直接當成觀測量使用，需要利用變換矩陣將其轉換為數學平臺失準角才可用于導航濾波。

利用四元數與旋轉角的關系可知：

(25)

(26)

(27)

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

2.2.2 自適應性改進

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

(28)

式中β為調節因子。

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

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

(29)

3 仿真評估與分析

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

3.1 仿真計算

圖2 仿真軌跡Fig.2 Simulation trajectory

采用增大仿真軌跡的噪聲方差的方式來驗證級聯式GPS/INS松組合導航濾波器在系統噪聲和量測噪聲統計不準確的情況下的濾波性能。在500～1 000 s，將系統噪聲標準差增大10倍，在1 000～1 500 s，將系統噪聲標準差增大20倍，在1 500～2 000 s，將系統噪聲標準差增30倍，在2 000～2 500 s，將系統噪聲標準差增大50倍，在2 500 s之后的時間段，將系統噪聲的標準差增大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組合導航系統包括1套型號為ADIS16488的MEMS和1臺LEA-M8S GPS接收機，INS數據更新頻率為205 Hz，GPS更新頻率為5 Hz。跑車實驗的真值由RTK提供。由于車載導航設備受外界環境的影響較大，故其系統噪聲統計特性雖會發生變化，但變化幅度會小于仿真所設計的情況。

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

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

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

4 結論

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

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