，，，

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

0 引言

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

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

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

1 組合導(dǎo)航數(shù)據(jù)后處理算法流程

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

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

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

2 抗差Kalman技術(shù)

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

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

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

(1)

相應(yīng)的抗差等價協(xié)方差矩陣為

(2)

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

(3)

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

3 平滑技術(shù)

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

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

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

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

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

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

(4)

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

(5)

式中，下標(biāo)F表示Kalman濾波。

濾波增益為

(6)

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

(7)

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

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

(8)

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

平滑增益為

(9)

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

(10)

(11)

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

4 組合導(dǎo)航系統(tǒng)的數(shù)學(xué)模型

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

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

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

(12)

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

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

(13)

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

(14)

式中各系數(shù)矩陣均可通過導(dǎo)航關(guān)系解算出來，由于篇幅關(guān)系不再贅述。

5 跑車實驗介紹

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

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

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

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

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

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

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

圖7 位置Fig.7 The position

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

6 結(jié)論

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

參考文獻(xiàn)

[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組合導(dǎo)航系統(tǒng)及其數(shù)據(jù)融合技術(shù)研究[D]. 南京：東南大學(xué), 2006.

[3] 付夢印，鄧志紅，閆莉萍. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京：科學(xué)出版社, 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] 秦永元. 慣性導(dǎo)航[M]. 北京：科學(xué)出版社, 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] 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)算法及車載組合導(dǎo)航系統(tǒng)研究[D]. 西安：西北工業(yè)大學(xué), 2004.

[9] 董明. 捷聯(lián)慣性導(dǎo)航誤差模型與初始對準(zhǔn)技術(shù)研究[D]. 鄭州：解放軍信息工程大學(xué), 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ǎo)航技術(shù)的研究[D]. 南京：南京理工大學(xué), 2009.