一種兩步自適應抗差K alman濾波在GPS/INS組合導航中的應用

吳富梅,楊元喜

1.信息工程大學測繪學院,河南鄭州450052;2.西安測繪研究所,陜西西安710054

一種兩步自適應抗差K alman濾波在GPS/INS組合導航中的應用

吳富梅1,2,楊元喜2

1.信息工程大學測繪學院,河南鄭州450052;2.西安測繪研究所,陜西西安710054

針對偽距、偽距率緊組合導航精度低、姿態角誤差修正誤差較大的缺點,從參數可觀測性角度提出一種兩步自適應Kalman濾波算法。首先簡單介紹緊組合Kalman濾波的過程,然后給出兩步自適應抗差濾波的公式和具體步驟,并且進行分析和比較。最后用實測算例對提出的算法進行驗證。結果表明,相比于偽距、偽距率緊組合Kalman濾波,兩步自適應抗差濾波的導航精度受組合周期的長短、INS慣性元件誤差的大小影響較小,精度略有提高,更重要的是能夠控制動態擾動異常和觀測異常的影響,在慣性元件誤差較大的情形下也能夠很好地估計元件誤差,避免姿態角錯誤修正。

分步自適應Kalman濾波;GPS/INS組合導航;緊組合導航;自適應因子

1 引 言

全球定位系統(GPS)和慣性導航系統(INS)具有很強的功能互補性,將兩者組合起來可以有效地利用各自的優點,進行系統間的取長補短以減小系統誤差影響,提高導航系統的性能,目前已在陸海空導航領域受到廣泛的應用。GPS/INS組合導航系統不僅能夠提供載體的位置、速度和姿態信息,而且能夠實現長時間導航定位的高精度,高數據采樣率以及抗差性[1-2]。

在GPS/INS組合導航中,緊組合方式因為觀測量不相關,而且在衛星少于4顆的情況下,也能在較短的時間里實現 GPS/INS數據融合,因而備受關注[1]。緊組合中對速度進行校正一般采用多普勒觀測值,對位置進行校正可以采用偽距或采用載波相位作為觀測量。采用偽距可避免求解模糊度和處理周跳等問題,但是由于偽距的觀測噪聲較大、殘留誤差大,實時導航的精度較低,不能精確校正INS的誤差;采用載波相位作為觀測量,導航精度高,但是需要解算模糊度,實時導航時過程復雜、模糊度解算可靠性較低,很難滿足連續、可靠、高精度的導航要求[3]。另外,用載波相位進行實時導航需要利用參考站進行差分解算,從而增加了成本,而且導航精度還受到用戶與基準站距離的限制[4]。

將GPS/INS數據通過一定的算法融合形成最優解是組合導航的關鍵。目前 Kalman濾波是最普遍采用的一種方法[5]。為了克服 Kalman濾波次優甚至發散的缺點,在載體動態導航中,一般利用自適應濾波來控制動態擾動異常、動態模型誤差以及隨機誤差的影響[6-7]。在GPS/INS組合導航Kalman濾波解算模型中觀測往往個數小于狀態參數個數,只能利用預報殘差來構造自適應因子,這就需要觀測量可靠、觀測精度較高[8]。而且,在利用Kalman濾波對INS誤差進行估計時不僅需要可靠的隨機模型還需要高精度的觀測量[9],顯然,偽距觀測量一般不能滿足要求。另外,偽距易受到外界影響,觀測中難免存在粗差。

基于此,本文提出一種兩步自適應抗差 Kalman濾波算法。首先考慮利用高精度的多普勒觀測值進行一步自適應 Kalman濾波對組合導航系統誤差進行校正,然后用校正后的動態系統模型信息與低精度的偽距觀測量進行二步抗差Kalman濾波。

2 GPS/INS緊組合 K alman濾波

在GPS/INS緊組合導航中,系統的狀態由兩部分組成:一是INS的誤差狀態;二是 GPS的誤差狀態,觀測量由INS導航結果推算的偽距、偽距率與 GPS觀測得到的偽距、偽距率作差得到[10-11]。

系統的狀態方程為

系統的觀測方程為

式中,FIk,k-1為 INS連續系統的狀態轉移矩陣; GIk為INS系統的動態噪聲矩陣;WIk為INS系統的過程白噪聲矢量;FGk,k-1為 GPS連續系統的狀態轉移矩陣;GGk為 GPS系統的動態噪聲矩陣; WGk為 GPS系統的過程白噪聲矢量;XIk為15維未知參數向量,表達式為 XIk=[δx δy δz δ˙x δ˙y δ˙z φxφyφzεxεyεzaxayaz]T;XGk為二維未知參數向量,表達式為 XGk= [CδtuCδtru]T;C為光速;Lρk和˙L˙ρk為觀測向量; Aρ1、Aρ2、A˙ρ1和A˙ρ2為觀測矩陣中的分塊矩陣;n為可測衛星的個數,表達式具體意義和定義見文獻[10]。

在這里,為了便于下面的分析,將狀態參數的順序重新調整分為直接可測參數 X1和間接可測參數 X2兩個部分。在偽距、偽距率緊組合Kalman濾波中,X1和 X2分別為

相應的狀態矩陣、觀測矩陣以及噪聲矩陣也按照估計參數的順序作相應調整,則連續系統狀態方程為

觀測方程為

式中,

對式(5)離散化,聯列狀態方程和觀測方程進行 Kalman濾波,則導航解為[5,12]

式中,Kk稱為Kalman濾波增益矩陣

式中,Ak為觀測矩陣;Φk,k-1為狀態轉移矩陣。

令

則式(7)可寫成[9]

由式(8)可得

若觀測協方差陣和狀態協方差陣可靠,間接可測參數X2的估計依賴于直接可測參數 X1的估計,若 X1的估計精度較低,就會影響 X2的估計精度。在 GPS/INS偽距、偽距率緊組合系統中,因為偽距的觀測噪聲較大,造成可測參數 X1中的位置誤差參數的估計精度較低,由此得到的間接可測參數 X2的精度也會降低,用此估計值對INS誤差進行校正,必然會影響INS誤差的校正結果。

3 兩步自適應 K alman濾波算法

在 GPS/INS偽距、偽距率緊組合系統中, Doppler觀測值具有較高的觀測精度,計算的速度具有厘米級精度,可以用來對濾波間接可測參數進行估計。設計的方案是采用兩步 Kalman濾波。在第一步 Kalman濾波中,利用高精度的速度參數對INS誤差估計并進行修正;為避免誤差累積,第二步Kalman濾波中,利用偽距觀測量對INS一步修正后的位置誤差再進行修正。

3.1 基于Doppler觀測值的一步自適應 K alman濾波

在第一步 Kalman濾波中,觀測量僅用Dopplor觀測值,因此直接可測參數 X1和間接可測參數X2為

狀態方程參照式(1)按照參數的順序作相應調整,這里狀態方程中的元素并未變化,只是位置發生變化。

觀測方程為

為控制擾動異常的影響,引入自適應濾波[5-7]。自適應濾波解為

式中,αk為自適應因子。

因為觀測個數小于狀態參數的個數,基于預報殘差ˉVk構造統計量為[8]

自適應因子αk為

式中,c為常量,可以取c=0.85～1.0。

因為直接可測參數 X1的估計精度較高,可以用間接可測參數的濾波估計值對INS系統誤差進行校正。

一般地,在偽距、偽距率緊組合中是利用上面的Kalman濾波得到的誤差改正量直接對INS推算得到的位置和速度進行修正,即

但是,在基于Doppler觀測值的一步自適應Kalman濾波中,由觀測方程式(13)可知,只有速度是直接可測參數,其他都是間接可測參數。用這樣的結果對位置進行修正,必然還會存在一定的偏差。考慮在短時間內速度變化較小,可以用經過正確修正的速度,重新對位置進行推算,即

式中,dt為修正的時間間隔。

3.2 基于偽距觀測值的二步抗差 K alman濾波

在第二步 Kalman濾波中,為避免推算的誤差累積,再采用偽距觀測值進行 Kalman濾波,因此直接可測參數X1和間接可測參數X2為

與式(12)中的狀態方程類似,其元素按照參數的順序作相應調整。

觀測方程為

這里因為觀測量精度較低,而由速度推算的位置短時間內精度較高,以推算速度作為參考,為抵制偽距觀測粗差的影響,利用抗差等價權來控制觀測異常的影響[15]。等價權函數為[16]

式中,Δ?vk,i為第 i個偽距觀測量的標準化預測殘差。

需要指出的是,上面是為了方便分析將狀態參數調整順序,在實際計算過程中,參數的順序無需調整。

通過對緊組合Kalman濾波與兩步自適應抗差Kalman濾波比較,可知:

1.在緊組合 Kalman濾波中采用的是 INS推算的位置和速度與 GPS觀測量進行濾波。在兩步自適應抗差 Kalman濾波的一步濾波中,采用的是INS推算的速度與 GPS Doppler值進行濾波,而在二步濾波中,采用的是經過一步濾波修正后的位置與 GPS偽距進行濾波。這對于精度較高或者組合周期較短的組合導航系統其差別不明顯,但是對于精度較低、組合周期較長的組合導航系統其改善效果明顯,因為INS的精度越低、單獨導航時間越長累積的誤差就越大。

2.在緊組合 Kalman濾波中,直接可測參數是位置和速度,由于位置的估計精度較低,將直接影響間接可測參數姿態角誤差、陀螺儀和加速度計誤差的估計;在分步自適應 Kalman濾波中,直接可測參數是精度較高的速度參數,因此提高了間接可測參數姿態角誤差、陀螺儀和加速度計誤差的估計精度。

3.在分步自適應 Kalman濾波中,通過一步自適應因子的調節可以控制狀態模型誤差的影響,通過二步等價權函數抵制觀測粗差的影響。

4 計算與分析

取一組 GPS/INS車載導航數據進行試驗。INS配備戰術級慣性元件,測量精度較高,采樣頻率為100 Hz,GPS數據采樣頻率10 Hz。算例中,下列參數由經驗確定:陀螺儀和加速度計誤差相關時間分別為600 s,600 s;陀螺儀和加速度計初始均方差分別取10.0 deg/h和100μg;初始位置誤差為5 m,5 m,7 m;初始速度誤差為0.1 m/s;初始平臺失準角誤差分別為100.0 s,100.0 s和500.0 s;采用GPS偽距和多普勒觀測值進行緊組合導航解算時,初始方差取25 m2和0.01 m2/s2。圖1給出了行車軌跡。

圖1 行車軌跡Fig.1 Tracking figure

采用兩種方案進行組合導航:

方案1:偽距、偽距率緊組合Kalman濾波;

方案2:分步自適應Kalman濾波。

為了驗證分步自適應 Kalman濾波的性能,對上述數據進行處理,分為四種情形:

1.原始數據,組合周期0.1 s;

2.為驗證自適應濾波的效果,在軌跡圖中標注處6個歷元(每個標注兩個比較接近的歷元), GPS 2,10號衛星偽距觀測值中加入30 m,20 m粗差,在載體速度預測向量中隨機加入10 m,5 m擾動,組合周期0.1 s;

3.為驗證組合周期對兩種濾波算法的影響,采用原始數據導航,組合周期1.0 s;

4.為驗證INS觀測精度對兩種算法的影響,在INS原始數據中加入白噪聲誤差,陀螺儀誤差為(0,0.1°/s),加速度計誤差為(0,0.000 01 m/s2),組合周期為1.0 s。

兩種算法在四種情形下導航誤差見圖2～圖5,由于 X,Y,Z方向的誤差相似,在此只給出X方向的誤差圖。圖6和圖7給出了情形4下兩種方案航向角與參考值的比較。表1給出了兩種算法在四種情形下導航誤差的RMS比較。

圖2 情形1X方向誤差Fig.2 Xerrors of case 1

圖3 情形2X方向誤差Fig.3 Xerrors of case 2

圖4 情形3X方向誤差Fig.4 Xerrors of case 3

圖5 情形4X方向誤差Fig.5 Xerrors of case 4

圖6 情形4方案1航向角與參考值比較Fig.6 Comparison of yaw angle with reference by Scheme 1 of case 4

圖7 情形4方案2航向角與參考值比較Fig.7 Comparison of yaw angle with reference by scheme 2 of case 4

分析上面的結果,可以得出:

1.在組合周期較短、INS慣性元件誤差較小的情形下,偽距、偽距率緊組合 Kalman濾波與兩步自適應抗差 Kalman濾波的導航精度相當。

2.從情形2的算例可以看出,當狀態向量存在擾動時,兩步自適應抗差 Kalman濾波通過一步自適應因子控制了擾動異常的影響;當偽距觀測中存在觀測粗差時,通過二步濾波中的等價權函數抵制了觀測異常的影響,相比于緊組合Kalman濾波,提高了系統抵制狀態異常和粗差的能力。

表1 四種情形下兩種方案的Mean Error比較Tab.1 Mean Error by two schemes of four cases

3.在組合周期較長或者慣性元件誤差較大的情形下,兩步自適應抗差 Kalman濾波的導航精度要優于緊組合 Kalman濾波,這是因為分步自適應濾波采用的是經過一步濾波修正后的位置與GPS偽距進行濾波的結果;對比情形3和情形4還可以看出,慣性元件誤差對緊組合Kalman濾波的影響更大,組合周期的影響稍小。

4.當慣性元件誤差較大時,在緊組合Kalman濾波中,由于偽距觀測誤差較大,造成間接可測參數估計誤差較大,慣性元件誤差被錯誤修正,因此航向角誤差較大;在兩步自適應抗差Kalman濾波中,由于采用高精度的Doppler觀測值作為觀測量,提高了間接可測參數估計精度,誤差得到很好的修正,航向角保持較高的精度。俯仰角和橫滾角也具有相同的情形。需要說明的是,間接可測參數得到很好估計不僅需要較高精度的直接可測參數,還需要可靠的誤差隨機模型,兩者缺一不可。

5 結 論

綜上所述,在采用偽距、偽距率緊組合 Kalman濾波時,由于直接采用INS推算的位置和速度與GPS觀測組合,導航精度隨著組合周期的增長、慣性元件誤差的增大而降低;并且由于 GPS位置觀測精度較低,造成INS慣性元件誤差估計精度較低,INS誤差錯誤修正,姿態角誤差偏大;在兩步自適應抗差 Kalman濾波中,由于一步濾波中利用了高精度的速度信息,導航精度受組合周期的長短、慣性元件誤差的大小影響較小;而且在一步濾波中利用高精度的速度參數作為直接可測參數,INS誤差得到很好的修正。另外,由于在兩步自適應抗差Kalman濾波中采用自適應因子和等價權函數,既能控制擾動異常的影響也能抵制觀測異常的影響。因此當GPS觀測可用時,與偽距、偽距率緊組合Kalman濾波相比,采用兩步自適應抗差Kalman濾波具有一定的優勢。

[1] YANG Y.Tightly Coupled MEMS INS/GPS Integration with INS Aided Receiver Tracking Loops[D].Calgary: University of Calgary,2008.

[2] ALBAN S,AKOS D M,ROCK S M.Performance Analysis and Architectures for INS-aided GPS Tracking Loops [C]∥ION NTM.Anaheim:[s.n.],2003:611-622.

[3] WANG J H.Intelligent MEMS INS/GPS Integration for Land Vehicle Navigation[D].Calgary:University of Calgary,2006.

[4] SUN Zhengming,GAO Jingxiang,WANG Jian.Discussion on Weighting Factors of GPS Dual Frequency Phasesmoothed Pseudo-range[J]. Hydrographic Survey and Charting,2007,27(4):13-16.(孫正明,高井祥,王堅. GPS雙頻相位平滑偽距權重因子的探討[J].海洋測繪, 2007,27(4):13-16.)

[5] YANG Yuanxi.Properties of the Adaptive Filtering for Kinematic Positioning[J].Acta Geodaetica et Cartographica Sinica,2003,32(3),189-192.(楊元喜.動態定位自適應濾波解的性質[J].測繪學報,2003,32(3):189-192.)

[6] YANG Yuanxi,HE Haibo,XU Tianhe.On Adaptively Kinematic Filtering[J].Acta Geodaetica et Cartographica Sinica,2001,30(4):293-298.(楊元喜,何海波,徐天河.論動態自適應濾波[J].測繪學報,2001,30(4): 293-298.)

[7] GAO Weiguang,YANG Yuanxi,ZHANG Shuangcheng. Adaptive Robust Kalman Filtering Based on the Current Statistical Model[J].Acta Geodaetica et Cartographica Sinica,2005,35(1):15-18.(高為廣,楊元喜,張雙成.基于當前加速度模型的抗差自適應Kalman濾波[J].測繪學報,2005,35(1):15-18.)

[8] YANG Y,GAO W.A New Learning Statistic for Adaptive Filter Based on Predicted Residuals[J].Progress in Natural Science,2006,16(8):833-837.

[9] CHAI Yanju.Theory and Method for Improving the Navigation Accuracy of GPS/INS Integration by Digging the Hidden Information[D].Wuhan:Institute of Geodesy and Geophysics,Chinese Academy of Sciences,2008.(柴艷菊.挖掘信息提高GPS/INS導航精度的理論與方法研究[D].武漢:中國科學院測量與地球物理研究所,2008.)

[10] DONG Xurong,Zhang Shouxin,Hua Zhongchun.GPS/ INS Integrated Navigation and Its Application[M]. Changsha:NationalDefence Scienceand Technology University Press,1998.(董緒榮,張守信,華仲春. GPS/INS組合導航定位及其應用[M].長沙:國防科技大學出版社,1998.)

[11] WANG Huinan.GPS Navigation Principal and Its Application[M].Beijing:Science Press,2003:206-241.(王惠南.GPS導航原理與應用[M].北京:科學出版社,2003: 206-241.)

[12] DENG Zili.Optimal Filtering Theory and Its Application—The Time Series Analysis[M].Harbin:Harbin Industry University Press,2000.(鄧自立.最優濾波理論及其應用——現代時間序列分析方法[M].哈爾濱:哈爾濱工業大學出版社,2000.)

[13] GODHA S.Performance Evaluation of Low Cost MEMSBased IMU Integrated with GPS for Land Vehicle Navigation Application[D].Calgary:University of Calgary,2006.

[14] EUN-HWAN SHIN.Accuracy Improvement of Low Cost INS/GPS for Land Applications[D].Calgary:University of Calgary,2001.

[15] YANG Yuanxi.Theory and Application of Robust Estimation[M].Beijing:Bayi Press,1990.(楊元喜.抗差估計理論及其應用[M].北京:八一出版社,1990.)

[16] YANG Y.Estimators of Covariance Matrix at Robust Estimation[J]. ZeitschriftfuerVermessungswesen, 1997,122(4):166-174.

(責任編輯:叢樹平)

A New Two-Step Adaptive Robust Kalman Filtering in GPS/INS Integrated Navigation System

WU Fumei1,2,Y ANG Yuanxi2
1.Institute of Surveying and Mapping,Information Engineering University,Zhengzhou 450052,China;2.Xi’an Research Institute of Surveying and Mapping,Xi’an 710054,China

In tight integrated navigation based on pseudorange and doppler observation,the precision is poor and the modification of the attitude errors is not accurate for poor observation precision of pseudorange.So a two-step adaptive robust Kalman filtering based on the observability of the parameters is presented.First the process of the tight integration is given and then the formulas and the approaches of the new method are deduced and analyzed. Finally an actual calculation is given.It is shown that compared with tight integration,the two-step adaptive robust Kalman filtering can control the disturbances of the state and the outliers of the observation.And the navigation precision does not decrease while the integration period becomes longer and the INS errors become bigger.The INS errors can be rightly estimated and the precision of attitude angles is improved.

two-step adaptive robust Kalman filtering;GPS/INS integrated navigation;tight integrated navigation; adaptive factor

WU Fumei(1981—),female,PhD candidate,majors in kinematic geodetic data processing.

E-mail:wfm8431812@163.com

1001-1595(2010)05-0522-06

P228

A

國家自然科學基金(40774001,40841021,40604003);國家863計劃(2007AA12Z331);衛星導航與定位教育部重點實驗室(B類)開放基金

2009-07-20

2009-08-31

吳富梅(1981—),女,博士生,主要從事動態大地測量數據處理。