R-T-S平滑算法與抗差Kalman在數據后處理中的應用

，，，

(北京理工大學 自動化學院，北京 100081)

0 引言

GPS/INS組合導航系統(tǒng)的發(fā)展隨著GPS技術的普及，在軍用和民用各領域產生了深遠的影響。GPS雖然具有長航時精度高、不受天氣影響的優(yōu)點，但其缺點也顯而易見，那就是衛(wèi)星信號在有些地方受遮擋造成丟星從而影響定位，以及抗干擾性差等因素。INS則是一種完全自主式的導航系統(tǒng)，導航過程中不向外輻射電磁信號，和外界不發(fā)生任何光、電的聯系，因此具有隱蔽性好、抗干擾能力強,工作不受氣象條件限制等優(yōu)點[1]。但是，純慣性導航系統(tǒng)也有一個致命的缺點，就是其導航誤差隨時間增長。組合導航常以INS 作為主導航系統(tǒng)，而將其他導航定位誤差不隨時間積累的導航系統(tǒng)，如多普勒導航、地形匹配導航等系統(tǒng)作為輔助導航系統(tǒng)，來修正INS在導航過程中的偏差。同時，在算法上采用Kalman濾波技術，對組合系統(tǒng)的狀態(tài)變量進行最優(yōu)估計，獲得修正信息，從而提高導航精度[2]。這樣，既保持了INS系統(tǒng)的自主性，又防止了導航定位誤差隨時間積累。由于GPS的低動態(tài)、窄帶寬、高精度與SINS的高動態(tài)、寬頻帶、誤差慢漂移特性形成強烈的互補性，所以 GPS/INS 組合在航空、航天、航海、陸地戰(zhàn)車等導航領域已得到越來越廣泛的應用，具有巨大的發(fā)展?jié)摿Α?/p>

近年來，由于微慣性器件的迅速發(fā)展，使得 MINS/GPS 組合導航系統(tǒng)己經在某些領域投入應用，它克服了低精度慣導系統(tǒng)單獨工作時誤差隨時間積累的缺點和GPS系統(tǒng)固有的局限性，并且由于體積小、質量小，大大拓寬了其應用領域。GPS能提供高精度的導航信息，如位置和速度數據，并連續(xù)地對準和校準MINS系統(tǒng)[3]。MINS可提供短期的高精度的速度數據，并能輔助接收機在丟星時縮短捕獲和重新捕獲衛(wèi)星信號的時間，故在丟星期間可直接采用其進行導航，并提供加速度和姿態(tài)數據。因此，MINS/GPS組合導航精度在一定程度上高于2個系統(tǒng)單獨工作的導航精度，同時也提高了系統(tǒng)的魯棒性[4]。

本文提出一種基于R-T-S(Rauch-Tung-Striebel)平滑算法和抗差Kalman濾波的數據后處理算法，應用于測繪領域。其基本原理是利用抗差估計原理，構造等價協方差矩陣，降低含有粗差觀測值的利用程度[5]，從而提高Kalman濾波的抗差能力，并在此前向濾波的基礎上，進行后向R-T-S最優(yōu)固定區(qū)間平滑處理，并對該算法進行了實物仿真。結果表明，與傳統(tǒng)Kalman濾波相比，R-T-S平滑算法不僅可以提高位置、姿態(tài)精度，而且在衛(wèi)星信號失鎖的情況下精度也得到顯著改善，并且在不丟星的時刻，抗差Kalman濾波可以有效處理GPS信號中的異常觀測值，遏制濾波發(fā)散，是一種有效的數據后處理方法。

1 組合導航數據后處理算法流程

在數據后處理過程中，以GPS設備所得數據為基準，用來估計慣導設備的偏差，在衛(wèi)星信號失鎖時，慣導設備所提供的信息可以用來估計載體的運動狀態(tài)，從而減少衛(wèi)星信號重新捕獲所需的時間[6]。

圖1 數據后處理算法流程Fig.1 The flow diagram of data post-processing algorithm

后處理算法主要包含2個過程：首先利用IMU測量的加速度信息、角速度信息進行慣性導航解算，將獲得的位置信息、速度信息與GPS信息同步作差(為提高精度通常采用差分GPS)，作為前向Kalman濾波的觀測量。其次利用前向Kalman濾波過程中獲得的協方差矩陣和狀態(tài)轉移矩陣，對狀態(tài)估值進行反向R-T-S平滑處理，根據平滑結果對INS輸出的位置、速度和姿態(tài)進行校正，作為系統(tǒng)的最終輸出[7]。在本文中，前向Kalman濾波過程采用抗差Kalman濾波，通過濾波增益矩陣來抑制粗差的影響。

2 抗差Kalman技術

在MINS/GPS組合導航中，由于GPS容易受到外界環(huán)境的遮擋和干擾，從而產生的異常觀測值，稱為粗差。粗差作用與Kalman濾波，會引起系統(tǒng)測量模型與實際測量值之間的偏差，從而影響濾波算法的穩(wěn)定性，甚至導致濾波發(fā)散[8]。在GPS導航中，處理GPS觀測粗差的一種行之有效的方法是抗差估計，其目的是在檢測到觀測值存在粗差時，引入等價權，通過降低含有粗差的觀測值的權重來提高估計精度。

在本文中，采用抗差最小二乘估計。在緊組合中，假設某一時刻觀測有N顆衛(wèi)星(N≥4)，不需要抗差最小二乘的解算結果，只需要計算觀測向量的等價協方差矩陣即可。對GPS衛(wèi)星的觀測可以認為獨立、等權，故觀測值得相對權矩陣為單位矩陣。對緊組合觀測方程進行最小二乘估計，并用最小二乘殘差結果計算觀測值的權因子可得

αi=diag[α1α2…αn] 0<αi≤1，i=1,2…n

(1)

相應的抗差等價協方差矩陣為

(2)

故抗差Kalman濾波的基本方程可以表述為：

(3)

抗差Kalman濾波通過調節(jié)觀測噪聲協方差矩陣調整Kalman濾波的增益。當GPS觀測值中含有粗差時，利用抗差最小二乘算法構造等價權矩陣的方法構造觀測值的等價協方差矩陣，在Kalman濾波中，觀測噪聲方差的增大意味著相應觀測值增益的減小。因此，可以通過濾波增益矩陣抑制粗差對系統(tǒng)的影響[9]。當觀測值中不存在粗差時，觀測值的權因子αi=1(i=1,2…n)，此時抗差Kalman濾波轉變?yōu)槌Ｒ?guī)Kalman濾波。

3 平滑技術

平滑技術作為事后或準實時數據處理的一種方法，可以在一定程度上提高數據處理的精度，在測繪領域獲得了廣泛的應用。平滑技術總的來說分為三類：固定區(qū)間平滑(Fixed-Interval Smoothing)、固定點平滑(Fixed-Point Smoothing)和固定滯后平滑(Fixed-Lag Smoothing)。其中在數據后處理中應用最為廣泛的方法就是固定區(qū)間平滑，其原理圖如圖2所示。它是在前向Kalman濾波的基礎上進行的反向濾波處理，充分利用區(qū)間內所有時刻的測量值對某一時刻的狀態(tài)進行估計。該方法能提供比單向濾波更高的精度，同時在衛(wèi)星信號失鎖段也是一種很好的橋接算法[10]。

圖2 固定區(qū)間平滑算法原理圖Fig.2 Schematic diagram of fixed-interval smoothing algorithm

R-T-S算法是一種固定區(qū)間最優(yōu)平滑算法。許多文獻中將R-T-S固定區(qū)間最優(yōu)平滑算法應用到GPS/INS組合導航系統(tǒng)中，結果R-T-S固定區(qū)間平滑算法的精度比Kalman濾波的精度高，且該方法計算簡單，易于實現，是一種有效的事后處理方法[11]。由于該方法的優(yōu)勢主要在于GPS失鎖時刻，而在不丟星時并無顯著改進，故本文在該算法的基礎上引入了抗差Kalman濾波，使其在不丟星時的精度得到進一步改善，從而得到更好的數據后處理結果。

R-T-S平滑算法由前向濾波和后向濾波組成，前向濾波是經典的Kalman濾波器，用來估計每一時刻的狀態(tài)，后向濾波是在前向濾波的基礎上重復利用部分數據，以獲取更精確的狀態(tài)估計值[12]。

由于R-T-S平滑算法主要體現在后向濾波處理上，故在執(zhí)行該算法前，應首先在時間區(qū)間 [0,N]內對系統(tǒng)進行前向抗差Kalman濾波，抗差Kalman濾波的遞推公式如下。

狀態(tài)向量和方差陣的一步預測：

(4)

PF(k|k-1)=Φk,k-1PF(k-1|k-1)×

(5)

式中，下標F表示Kalman濾波。

濾波增益為

(6)

狀態(tài)向量和方差陣更新：

(7)

抗差Kalman濾波過程結束后，利用前向濾波過程中存儲的數據進行R-T-S固定區(qū)間最優(yōu)平滑。在平滑前需要先對平滑器進行初始化，令k=N，則有：

PS(N|N)=PF(N|N)

(8)

式中，下標S表示最優(yōu)平滑。在時間區(qū)間[N-1,0]內R-T-S固定區(qū)間平滑算法的遞推公式如下。

平滑增益為

(9)

平滑的狀態(tài)向量和方差陣的更新為：

(10)

(11)

由式(10)、式(11)可見平滑遞推公式是一個由k=N-1到k=0的倒推過程，且平滑遞推過程中要用到的狀態(tài)向量與方差陣的估計值和預測值均為前向Kalman濾波過程中所得的值[14]。故R-T-S算法是前向濾波算法和后向濾波算法的融合，充分利用了區(qū)間[0,N] 內的每一組數據，因而精度高于一般Kalman濾波。

4 組合導航系統(tǒng)的數學模型

本文導航坐標系選取東北天地理坐標系，其狀態(tài)變量為

XI=[φEφNφUδvEδvNδvUδLδλδh

εxεyεzxyzδtuδtru]T

(12)

其中，φE、φN、φU為數學平臺失準角；δvE、δvN、δvU分別為載體的東向、北向和天向速度誤差；δL、δλ、δh分別為緯度誤差、經度誤差和高度誤差；εx、εy、εz、x、y、z分別為陀螺隨機常值漂移和加速度計隨機常值零偏；δtu、δtru分別是與時鐘誤差等效的距離誤差和與時鐘頻率誤差等效的距離率誤差。

系統(tǒng)采用緊組合的方式，其狀態(tài)方程為

(13)

基于偽距、偽距率的量測方程為

(14)

式中各系數矩陣均可通過導航關系解算出來，由于篇幅關系不再贅述。

5 跑車實驗介紹

為了驗證R-T-S算法和抗差Kalman濾波在數據后處理中的應用，進行了跑車試驗，車載3套MIMU，共進行了3組試驗，每組實驗的安裝方式如圖3所示。

圖3 實驗安裝方法Fig.3 The experimental installation method

按照第一種安裝方式，在基準點J01停止，采集MIMU的輸出，約10min。沿北清路行進、掉頭返回，繼續(xù)行進，采集該過程中MIMU的輸出數據。行進路徑如圖4所示。按照第二種、第三種安裝方式重復試驗。

圖4 實驗地段北清路Fig.4 The Beiqing Road in experiment

在Matlab環(huán)境下，先對GPS/MINS組合導航系統(tǒng)進行前向Kalman濾波，同時存儲Kalman濾波器在各個時刻的估計值。事后利用R-T-S最優(yōu)平滑算法對系統(tǒng)進行后向平滑處理，分別得到位置誤差、姿態(tài)誤差、組合位置的圖像如圖5～圖7所示。

圖5 位置誤差Fig.5 The position error

圖6 姿態(tài)誤差Fig.6 The attitude error

圖7 位置Fig.7 The position

從以上各圖中可以看出，與標準Kalman濾波相比，R-T-S最優(yōu)平滑濾波和抗差Kalman濾波的結合可以有效地減小系統(tǒng)的位置和姿態(tài)誤差，提高系統(tǒng)的精度。基于標準Kalman濾波的導航結果在過程中出現了較大跳變，其原因主要是受到偽距、偽距率粗差觀測值的影響。相比之下，抗差Kalman濾波降低了觀測噪聲方差較大項的濾波增益，可以有效減少噪聲的影響，圖像相比之下并沒有出現較大的跳變。并且當GPS不失鎖時，R-T-S算法對導航結果的平滑的優(yōu)勢主要體現在GPS測量更新的間隙，能夠有效減少SINS預測過程中誤差的積累，也能在一定程度上削弱隨機誤差的影響。但由于試驗所選慣導精度較高，故對隨機誤差的削弱主要還是由抗差Kalman完成。但當可見衛(wèi)星數小于4顆時，抗差Kalman濾波退化為普通Kalman濾波，此時導航解算精度的提高主要依賴于R-T-S算法，但由于本實驗所選地段較為開闊，整個過程中僅有3座橋梁會造成丟星的發(fā)生，不存在丟星時刻的長時間積累，故R-T-S算法的優(yōu)勢并不能完全發(fā)揮。但經過R-T-S算法和抗差Kalman濾波的結合，整體數據后處理的精度仍有大幅提高，位置精度能夠提高50%以上，姿態(tài)精度也有40%左右的提升。

6 結論

為了提高數據處理的精度，獲得高精度的速度、位置和姿態(tài)信息，將R-T-S最優(yōu)平滑算法和抗差Kalman濾波應用于數據后處理中。其中當可觀測衛(wèi)星數大于等于4顆時，抗差Kalman濾波模型成立，可以降低觀測噪聲方差較大項的濾波增益，通過濾波增益矩陣來抑制粗差的影響；R-T-S算法此時對后處理精度提高的優(yōu)勢主要體現在GPS測量更新的間隙，能夠有效減少SINS預測過程中誤差的積累，也能在一定程度上削弱隨機誤差的影響。當可觀測衛(wèi)星數小于4顆時，抗差Kalman濾波退化為普通Kalman濾波，此時后處理精度的提高主要依賴于R-T-S算法，在前向Kalman濾波的基礎上進行的反向濾波，充分利用了區(qū)間內所有時刻的測量值對狀態(tài)進行估計，顯著提高數據后處理的精度。兩者的結合可以在全時段提高數據后處理的精度，是一種有效的數據后處理方法。

參考文獻

[1] Rochette P, Gattacceca J, Ivanov A V, et al. Mag-netic properties of lunar materials: Meteorites, Luna and Apollo returned samples[J]. Earth and Planetary Science Letters, 2010, 292(3-4): 383-391.

[2] 馬云峰. MSINS/GPS組合導航系統(tǒng)及其數據融合技術研究[D]. 南京：東南大學, 2006.

[3] 付夢印，鄧志紅，閆莉萍. Kalman濾波理論及其在導航系統(tǒng)中的應用[M]. 北京：科學出版社, 2010.

[4] Shortelle K J, Graham W R, Rabourn C. F-16 flight tests of a rapid transfer alignment procedure[C]// Position Location and Navigation Symposium. IEEE, 1998: 379-383.

[5] 秦永元. 慣性導航[M]. 北京：科學出版社, 2005.

[6] Richmond N C, Hood L L. A preliminary global map of the vector lunar crustal magnetic field based on Lunar Prospector magnetometer data[J]. Journal of Ge-ophysical Research: Planets,2008, 113(E2).

[7] Purucker M E. A global model of the internal magn-etic field of the Moon based on Lunar Prospector ma-gnetometer observations[J].Icarus, 2008, 197(1): 19-22.

[8] 嚴恭敏. 捷聯慣導算法及車載組合導航系統(tǒng)研究[D]. 西安：西北工業(yè)大學, 2004.

[9] 董明. 捷聯慣性導航誤差模型與初始對準技術研究[D]. 鄭州：解放軍信息工程大學, 2011.

[10] Nassar S. Different Algorithms for Bridging Kine-matic DGPS Outages Using SINS/DGPS Integration[C]// Institute of Navigation Satellite Division Technical Meeting. Portland, 2002: 1474-1480.

[11] Hood L L. Central magnetic anomalies of Nectarian-aged lunar impact basins: Probable evidence for an early core dynamo[J]. Icarus, 2011, 211(2):1109-1128.

[12] Xiao-Xu W, Quan P, LIANG Y, et al. Application of unscented transformation for nonlinear state smoo-thing[J].Acta Automatica Sinica, 2012, 38(7): 1107-1111.

[13] Liu Jianye, Yuan Xin. The application of smoothing filter to GPS/INS integrated navigation system[J]. Aerospace Control, 1995(4):36-44.

[14] 李靖松. 基于偽距、偽距率組合導航技術的研究[D]. 南京：南京理工大學, 2009.