一種兩步自適應(yīng)抗差K alman濾波在GPS/INS組合導(dǎo)航中的應(yīng)用

吳富梅,楊元喜

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

一種兩步自適應(yīng)抗差K alman濾波在GPS/INS組合導(dǎo)航中的應(yīng)用

吳富梅1,2,楊元喜2

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

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

分步自適應(yīng)Kalman濾波;GPS/INS組合導(dǎo)航;緊組合導(dǎo)航;自適應(yīng)因子

1 引 言

全球定位系統(tǒng)(GPS)和慣性導(dǎo)航系統(tǒng)(INS)具有很強的功能互補性,將兩者組合起來可以有效地利用各自的優(yōu)點,進行系統(tǒng)間的取長補短以減小系統(tǒng)誤差影響,提高導(dǎo)航系統(tǒng)的性能,目前已在陸?？諏?dǎo)航領(lǐng)域受到廣泛的應(yīng)用。GPS/INS組合導(dǎo)航系統(tǒng)不僅能夠提供載體的位置、速度和姿態(tài)信息,而且能夠?qū)崿F(xiàn)長時間導(dǎo)航定位的高精度,高數(shù)據(jù)采樣率以及抗差性[1-2]。

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

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

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

2 GPS/INS緊組合 K alman濾波

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

系統(tǒng)的狀態(tài)方程為

系統(tǒng)的觀測方程為

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

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

相應(yīng)的狀態(tài)矩陣、觀測矩陣以及噪聲矩陣也按照估計參數(shù)的順序作相應(yīng)調(diào)整,則連續(xù)系統(tǒng)狀態(tài)方程為

觀測方程為

式中,

對式(5)離散化,聯(lián)列狀態(tài)方程和觀測方程進行 Kalman濾波,則導(dǎo)航解為[5,12]

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

式中,Ak為觀測矩陣;Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣。

令

則式(7)可寫成[9]

由式(8)可得

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

3 兩步自適應(yīng) K alman濾波算法

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

3.1 基于Doppler觀測值的一步自適應(yīng) K alman濾波

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

狀態(tài)方程參照式(1)按照參數(shù)的順序作相應(yīng)調(diào)整,這里狀態(tài)方程中的元素并未變化,只是位置發(fā)生變化。

觀測方程為

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

式中,αk為自適應(yīng)因子。

因為觀測個數(shù)小于狀態(tài)參數(shù)的個數(shù),基于預(yù)報殘差ˉVk構(gòu)造統(tǒng)計量為[8]

自適應(yīng)因子αk為

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

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

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

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

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

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

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

與式(12)中的狀態(tài)方程類似,其元素按照參數(shù)的順序作相應(yīng)調(diào)整。

觀測方程為

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

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

需要指出的是,上面是為了方便分析將狀態(tài)參數(shù)調(diào)整順序,在實際計算過程中,參數(shù)的順序無需調(diào)整。

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

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

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

3.在分步自適應(yīng) Kalman濾波中,通過一步自適應(yīng)因子的調(diào)節(jié)可以控制狀態(tài)模型誤差的影響,通過二步等價權(quán)函數(shù)抵制觀測粗差的影響。

4 計算與分析

取一組 GPS/INS車載導(dǎo)航數(shù)據(jù)進行試驗。INS配備戰(zhàn)術(shù)級慣性元件,測量精度較高,采樣頻率為100 Hz,GPS數(shù)據(jù)采樣頻率10 Hz。算例中,下列參數(shù)由經(jīng)驗確定:陀螺儀和加速度計誤差相關(guān)時間分別為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偽距和多普勒觀測值進行緊組合導(dǎo)航解算時,初始方差取25 m2和0.01 m2/s2。圖1給出了行車軌跡。

圖1 行車軌跡Fig.1 Tracking figure

采用兩種方案進行組合導(dǎo)航:

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

方案2:分步自適應(yīng)Kalman濾波。

為了驗證分步自適應(yīng) Kalman濾波的性能,對上述數(shù)據(jù)進行處理,分為四種情形:

1.原始數(shù)據(jù),組合周期0.1 s;

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

3.為驗證組合周期對兩種濾波算法的影響,采用原始數(shù)據(jù)導(dǎo)航,組合周期1.0 s;

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

兩種算法在四種情形下導(dǎo)航誤差見圖2～圖5,由于 X,Y,Z方向的誤差相似,在此只給出X方向的誤差圖。圖6和圖7給出了情形4下兩種方案航向角與參考值的比較。表1給出了兩種算法在四種情形下導(dǎo)航誤差的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

分析上面的結(jié)果,可以得出:

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

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

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

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

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

5 結(jié) 論

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

[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雙頻相位平滑偽距權(quán)重因子的探討[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.(楊元喜.動態(tài)定位自適應(yīng)濾波解的性質(zhì)[J].測繪學(xué)報,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.(楊元喜,何海波,徐天河.論動態(tài)自適應(yīng)濾波[J].測繪學(xué)報,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.(高為廣,楊元喜,張雙成.基于當前加速度模型的抗差自適應(yīng)Kalman濾波[J].測繪學(xué)報,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ǎo)航精度的理論與方法研究[D].武漢:中國科學(xué)院測量與地球物理研究所,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組合導(dǎo)航定位及其應(yīng)用[M].長沙:國防科技大學(xué)出版社,1998.)

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

[12] DENG Zili.Optimal Filtering Theory and Its Application—The Time Series Analysis[M].Harbin:Harbin Industry University Press,2000.(鄧自立.最優(yōu)濾波理論及其應(yīng)用——現(xiàn)代時間序列分析方法[M].哈爾濱:哈爾濱工業(yè)大學(xué)出版社,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.(楊元喜.抗差估計理論及其應(yīng)用[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

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

2009-07-20

2009-08-31

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