一種基于GPS/INS全組合的航向自適應(yīng)濾波算法

田 源，隋立芬，田翌君，肖國銳，戴 卿

(信息工程大學(xué)地理空間信息學(xué)院，鄭州 450001)

一種基于GPS/INS全組合的航向自適應(yīng)濾波算法

田 源，隋立芬，田翌君，肖國銳，戴 卿

(信息工程大學(xué)地理空間信息學(xué)院，鄭州 450001)

首先基于陸地導(dǎo)航討論了GPS/INS全組合系統(tǒng)，分別在載體動態(tài)和靜態(tài)兩種情況下對比分析了全組合和緊組合的姿態(tài)矯正效果。實驗結(jié)果表明全組合模式的航向角精度較緊組合有明顯提升，且當(dāng)載體由動態(tài)轉(zhuǎn)為靜態(tài)時，全組合模式能夠克服緊組合航向角緩慢發(fā)散的現(xiàn)象。其次針對載體大角度轉(zhuǎn)向時組合系統(tǒng)動力學(xué)模型異常增大的問題，提出了一種對航向角進(jìn)行自適應(yīng)修正的全組合濾波算法，仿真實驗結(jié)果表明：該方法能夠一定程度上削弱載體大角度轉(zhuǎn)向時動力學(xué)模型異常的影響，航向精度有一定提高。

GPS/INS組合；全組合系統(tǒng)；航向角；自適應(yīng)濾波；分類因子

0 引 言

慣性導(dǎo)航系統(tǒng)(Inertial navigation system, INS)作為獨(dú)立自主高動態(tài)的導(dǎo)航系統(tǒng)，與GNSS有著良好的互補(bǔ)性，二者組合系統(tǒng)在車輛導(dǎo)航、移動測圖系統(tǒng)、航空攝影測量等高精度定位領(lǐng)域得到廣泛應(yīng)用[1]。隨著GNSS姿態(tài)測量技術(shù)的發(fā)展，GNSS多天線測姿已具備實時姿態(tài)獲取的能力[2-3]，因此可以將姿態(tài)信息加入GNSS/INS組合濾波器與位置速度一起進(jìn)行數(shù)據(jù)融合。該方法能克服傳統(tǒng)組合方式中姿態(tài)誤差及INS元件誤差需要根據(jù)位置和速度等誤差之間的關(guān)系來估計的缺陷，提高組合系統(tǒng)的導(dǎo)航精度[4]。

本文將加入姿態(tài)信息之后的組合系統(tǒng)分為不完全組合和完全組合兩種形式。不完全組合即僅利用三個姿態(tài)角的其中之一進(jìn)行組合。由于在陸地導(dǎo)航中俯仰角和橫滾角一般數(shù)值較小影響較弱，單獨(dú)與INS組合效果不大，因此該方面的研究通常只采用航向角組合。其中吳富梅、柴艷菊等多位學(xué)者提出利用GPS單天線確定的航向角與INS航向角組合[5-7]，提高了組合系統(tǒng)姿態(tài)誤差和元件誤差的估計精度且系統(tǒng)成本較低。但GPS單天線得到的姿態(tài)角由速度或位置增量導(dǎo)出，被稱為偽姿態(tài)，這種計算方法也決定了GPS單天線測姿在載體低速或靜止?fàn)顟B(tài)時精度很差的特點(diǎn)[8]。完全組合即為全組合系統(tǒng)，多采用GPS多天線姿態(tài)測量值進(jìn)行組合。該方式中姿態(tài)參數(shù)由間接估計轉(zhuǎn)變?yōu)橹苯庸烙?，在GPS姿態(tài)測量值精度能夠保證的情況下，組合系統(tǒng)姿態(tài)參數(shù)估值的準(zhǔn)確性能夠得到一定提高[9-11]。多天線姿態(tài)較單天線得到的偽姿態(tài)更加穩(wěn)定，因此在載體低速或靜止情況下全組合系統(tǒng)理論上能夠同樣保持較高精度的姿態(tài)輸出，但目前鮮有研究對該情況做出詳細(xì)分析。

在低成本陸地導(dǎo)航中，全組合系統(tǒng)雖能夠提高姿態(tài)精度，但依然無法克服GPS/INS組合在載體大角度轉(zhuǎn)向時狀態(tài)異常增大導(dǎo)致航向精度變差的問題。原因是低成本陸地導(dǎo)航采用低成本設(shè)備，且系統(tǒng)模型具有不確定性，包括模型狀態(tài)簡化，噪聲及初始狀態(tài)統(tǒng)計特性建模不準(zhǔn)確等[12]。因此載體在轉(zhuǎn)向時，隨著航向角變化幅度的增大動力學(xué)模型誤差增大，標(biāo)準(zhǔn)Kalman濾波不能控制動力學(xué)模型異常擾動的影響從而導(dǎo)致濾波精度較低[13]。本文在GPS/INS緊組合的基礎(chǔ)上擴(kuò)充姿態(tài)觀測方程，以“偽距偽距率緊組合+姿態(tài)松組合”的策略構(gòu)成全組合系統(tǒng)，在載體動態(tài)和靜態(tài)兩種情況下分析其姿態(tài)校正效果。并引入自適應(yīng)濾波理論[14]，以期削弱載體大角度轉(zhuǎn)向時動力學(xué)模型異常擾動的影響，最后通過實驗對采用自適應(yīng)濾波前后的航向角結(jié)果進(jìn)行了對比分析。

1 GPS/INS全組合濾波模型

1.1狀態(tài)方程

GPS/INS全組合系統(tǒng)是一個連續(xù)的過程，與緊組合系統(tǒng)的區(qū)別在于量測方程的擴(kuò)充，狀態(tài)方程與緊組合一致，可用連續(xù)微分方程的形式描述：

(1)

其中，X(t)為系統(tǒng)狀態(tài)，F(xiàn)(t)和G(t)為系數(shù)陣，u(t)為驅(qū)動白噪聲過程。

將(1)式離散化，得離散的狀態(tài)方程：

Xk=Φk,k-1Xk-1+GkWk

(2)

其中，Φk,k-1為離散后的狀態(tài)轉(zhuǎn)移矩陣；Wk為動力學(xué)模型噪聲；Gk為模型噪聲分配陣；Xk-1，Xk分別為k-1和k歷元的狀態(tài)參數(shù)向量，取如下所示的20維狀態(tài)參數(shù)：

X=[δPδVφεbεr▽δtδtr]T

(3)

從前到后分別為位置、速度誤差，平臺失準(zhǔn)角(計算得到的姿態(tài)矩陣和真實姿態(tài)矩陣之間的旋轉(zhuǎn)角度誤差)，陀螺儀常值零偏、相關(guān)漂移，以及加速度計偏置和接收機(jī)鐘差、鐘速。

1.2加入姿態(tài)的觀測方程擴(kuò)充

(4)

(5)

其中，C和S為余弦和正弦算子。該姿態(tài)矩陣與真實姿態(tài)矩陣R之間的存在如下關(guān)系：

(6)

其中，旋轉(zhuǎn)矩陣R′可表示為[15-16]：

(7)

(8)

對式(7)進(jìn)行近似處理，由于等效旋轉(zhuǎn)角度φ為小量，故sinφ/φ≈1，并忽略式中的二階小量，則式(6)變?yōu)椋?/p>

(9)

將式(4)代入式(9)，可推出平臺失準(zhǔn)角和姿態(tài)角誤差之間的關(guān)系：

(10)

aINS-aGPS=Aa,3×3·φ+εA

(11)

(12)

其中,φ表示失準(zhǔn)角；εA表示角度量測噪聲。將式(11)擴(kuò)充到GPS/INS緊組合觀測方程中，即得到全組合系統(tǒng)觀測方程：

(13)

(14)

2 自適應(yīng)卡爾曼濾波

GPS/INS組合系統(tǒng)是一個非線性連續(xù)系統(tǒng)，常采用擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF) 作為數(shù)據(jù)融合工具。聯(lián)立式(2)和式(13)，利用擴(kuò)展卡爾曼濾波就能得到每一歷元的狀態(tài)參數(shù)濾波解，現(xiàn)為了削弱大角度轉(zhuǎn)向時動力學(xué)模型異常擾動的影響，減小航向角誤差，引入自適應(yīng)濾波理論并直接給出自適應(yīng)濾波解：

(15)

(16)

其中,ΔMk表示狀態(tài)異常判別量；c為經(jīng)驗常數(shù)，通常取c=1.0～1.5。

本文提出一種基于閉環(huán)校正的自適應(yīng)濾波策略，僅對航向角進(jìn)行自適應(yīng)修正。新息向量計算式如下：

(17)

(18)

由式(18)可知,對于閉環(huán)校正新息與觀測等價。此時在GPS姿態(tài)測量值可靠的情況下，組合系統(tǒng)的航向觀測值(INS航向角與GPS航向角之差)能夠部分反映出系統(tǒng)的狀態(tài)異常。因此可借鑒分類因子自適應(yīng)濾波[18-19]的思想，直接利用組合系統(tǒng)的航向觀測值單獨(dú)構(gòu)造自適應(yīng)因子對航向角進(jìn)行自適應(yīng)修正。航向觀測值即以GPS的航向角為真值時的INS航向角誤差Δψ，狀態(tài)異常判別量可按下式構(gòu)造：

(19)

(20)

其中，αyaw為航向角自適應(yīng)因子；I為單位陣。

3 實驗分析

設(shè)計了兩組仿真實驗。實驗一分析了全組合系統(tǒng)在載體動態(tài)和靜態(tài)兩種狀態(tài)下的解算效果。實驗二分析了航向自適應(yīng)修正算法的效果。在兩組實測的車載組合導(dǎo)航數(shù)據(jù)的基礎(chǔ)上仿真GPS姿態(tài)測量值進(jìn)行實驗。

數(shù)據(jù)一：動態(tài)數(shù)據(jù)，INS陀螺零偏為1.0°/h；加速度計偏置為10-4g；采樣率為100 Hz；GPS采樣頻率為10 Hz；組合周期為1 s；偽距觀測中誤差取4 m；偽距率觀測中誤差取0.06 m/s。將陀螺儀與高精度的GPS RTK組合得到的姿態(tài)角作為參考值。在姿態(tài)參考值上加入標(biāo)準(zhǔn)差為0.2°的偽隨機(jī)噪聲作為GPS姿態(tài)測量值。

數(shù)據(jù)二：包含一段靜止?fàn)顟B(tài)，INS陀螺零偏1.0°/h；采樣頻率100 Hz；GPS采樣率1 Hz；組合周期1 s；偽距觀測中誤差取3 m；偽距率觀測中誤差取0.06 m/s。以高精度導(dǎo)航級慣導(dǎo)構(gòu)成的組合定位定姿系統(tǒng)POS LV610輸出的姿態(tài)作為參考真值。在姿態(tài)參考值上加入標(biāo)準(zhǔn)差為0.2°的偽隨機(jī)噪聲作為GPS姿態(tài)測量值。

3.1實驗一

分別按照兩種方案對上述兩組數(shù)據(jù)進(jìn)行組合導(dǎo)航解算：

方案一：緊組合模式(Tightly integrated, TI)；

方案二：全組合模式(Completely integrated, CI)。

首先給出數(shù)據(jù)一兩種方案的解算結(jié)果。圖1表示航向角誤差；圖2表示方案二的航向角結(jié)果；表1給出了數(shù)據(jù)一的姿態(tài)誤差統(tǒng)計。

表1 數(shù)據(jù)一的姿態(tài)角RMSTable 1 Attitude RMS of data 1

從以上結(jié)果可以看出：

1)圖1的航向角誤差對比清楚的表明了全組合的優(yōu)勢。相比于緊組合而言，全組合的誤差曲線更加集中，航向角精度提升非常明顯。由表1可知,提升幅度大約為60%。本文對航向角的定義為：(-180°,180°)，因此圖2中在500歷元左右線條出現(xiàn)的跳變是正?，F(xiàn)象。將圖1和圖2進(jìn)行對比分析可以看出，在航向角數(shù)值出現(xiàn)較大變化的歷元，兩種方案的航向誤差曲線因為狀態(tài)異常的變大而存在一些相對較大波動，緊組合受影響程度更大。圖1中1000歷元之后包含一小段靜止?fàn)顟B(tài)，可看出緊組合航向誤差呈緩慢發(fā)散趨勢。

2)由表1可知，全組合解算的橫滾角的RMS小于緊組合結(jié)果，說明全組合解算橫滾角精度更高，而俯仰角精度較緊組合有秒級別的微略降低。由于低成本車載導(dǎo)航中通常只考慮航向角，且俯仰角和橫滾角數(shù)值通常較小，所以俯仰角精度的略微損失這里可忽略不計。

以上為動態(tài)情況下的結(jié)果分析，下面給出數(shù)據(jù)二的解算結(jié)果。由于依然為車載數(shù)據(jù)，俯仰角和橫滾角結(jié)果與數(shù)據(jù)一結(jié)果類似，因此只給出航向角結(jié)果，如圖3和圖4所示。

表2 數(shù)據(jù)二航向角RMSTable 2 The yaw’s RMS of data 2

圖4中，120歷元左右載體有一個大角度的轉(zhuǎn)向，圖3相應(yīng)歷元的航向角誤差也出現(xiàn)了較大波動，較圖1更明顯地說明全組合在大角度轉(zhuǎn)向時航向精度會變差。在280歷元左右載體由動態(tài)變?yōu)殪o止?fàn)顟B(tài)，緊組合解算的航向角誤差存在逐漸發(fā)散的現(xiàn)象，原因是靜止?fàn)顟B(tài)下航向角誤差可觀測性不高，且間接估計姿態(tài)參數(shù)不夠準(zhǔn)確。全組合利用外部姿態(tài)觀測值直接估計失準(zhǔn)角，航向角誤差可觀測性好，在靜止?fàn)顟B(tài)下能夠較好的估計航向角誤差，因此航向角誤差不存在發(fā)散現(xiàn)象。由表2可知，全組合的航向角精度較緊組合有很大提升。

3.2實驗二

分別用數(shù)據(jù)一和數(shù)據(jù)二驗證自適應(yīng)算法，自適應(yīng)濾波閾值c取值1.5，用以下兩種方案解算：

方案一：普通的全組合(CI)；

方案二：航向自適應(yīng)修正的全組合(ACI)。

本實驗算法主要針對在載體大角度轉(zhuǎn)向時航向角精度差的問題，因此只給出航向角結(jié)果圖，如圖5和圖7所示，圖6和圖8展示了航向角自適應(yīng)判別量ΔMk的取值與判定閾值c的關(guān)系；兩組數(shù)據(jù)姿態(tài)解算結(jié)果的誤差統(tǒng)計見表3。

表3 實驗二的航向角RMSTable 3 The yaw’s RMS of test 2

由圖5和圖7可以看出，ACI方案誤差曲線較CI方案改善許多；由圖7可以明顯地看出航向精度跳變時ACI方案具備較好的修正效果；如表3所示，兩組數(shù)據(jù)ACI方案的航向RMS較CI方案明顯降低，數(shù)據(jù)一提升約13%，數(shù)據(jù)二提升約50%。說明在航向精度變差的時候ACI方案一定程度上能夠削弱狀態(tài)異常的影響，從而使得航向精度有所提高。

4 結(jié)束語

本文主要對全組合系統(tǒng)的姿態(tài)解算進(jìn)行了分析研究。全組合模式的姿態(tài)精度優(yōu)于緊組合模式，其中航向角精度提升非常明顯，俯仰角和橫滾角由于在車載實驗中數(shù)值較小的緣故，精度僅有略微變化。在載體由動態(tài)轉(zhuǎn)變?yōu)殪o止?fàn)顟B(tài)的情況下，由于緊組合模式航向誤差可觀測性差的緣故，航向角結(jié)果存在緩慢發(fā)散的現(xiàn)象，而加入了外部姿態(tài)觀測的全組合模式能夠很好的克服該問題。但全組合系統(tǒng)依然無法克服載體大角度機(jī)動時由于動力學(xué)模型異常變大而導(dǎo)致的航向精度變差這一現(xiàn)象。針對該問題，本文提出的閉環(huán)修正模式下單獨(dú)構(gòu)造航向角自適應(yīng)因子的自適應(yīng)濾波算法能夠一定程度上削弱動力學(xué)模型異常的影響，航向精度有一定提高。

[1] Grejner-Brzezinska D A, Da R, Toth C. GPS error modeling and OTF ambiguity resolution for high-accuracy GPS/INS integrated system [J]. Journal of Geodesy, 1998, 72(11): 626-638.

[2] 王冰. 基于GNSS的實時姿態(tài)確定算法研究[D]. 鄭州：解放軍信息工程大學(xué), 2013. [Wang Bing. Research on real-time attitude determination algorithms utilizing global navigation satellite system [D]. Zhengzhou:PLA Information Engineering University. 2013.]

[3] Zhao L, Li N, Li L, et al. Real-time GNSS-based attitude determination in the measurement domain [J]. Sensors, 2017, 17(2):296.

[4] 馬宏陽, 程鵬飛, 黃華東. GPS/INS位置、速度和姿態(tài)全組合導(dǎo)航系統(tǒng)研究[J]. 測繪通報, 2016(3):10-14. [Ma Hong-yang, Cheng Peng-fei, Huang Hua-dong. Research on the complete integrated GPS/INS navigation system of position, velocity and attitude [J]. Bulletin of Surveying and Mapping, 2016(3):10-14.]

[5] 吳富梅, 聶建亮, 何正斌. 低成本車載GPS/INS組合導(dǎo)航姿態(tài)角更新算法[J]. 中國慣性技術(shù)學(xué)報, 2010, 18(6):675-679. [Wu Fu-mei, Nie Jian-liang, He Zheng-bin. Low-cost GPS/INS integrated navigation algorithm in land vehicle system considering attitude update [J]. Journal of Chinese Inertial Technology, 2010, 18(6):675-679.]

[6] 柴艷菊, 歐吉坤, 袁運(yùn)斌,等. 附加方位約束的GPS/INS組合系統(tǒng)選權(quán)自適應(yīng)卡爾曼濾波[J]. 測繪學(xué)報, 2011, 40(4):421-428. [Chai Yan-ju, Ou Ji-kun, Yuan Yun-bin, et al. The adaptive Kalman filtering for single antenna GPS/INS integrated system with heading angle constraint by selecting the parameter weights [J]. Acta Geodaetica et Cartographica Sinica, 2011, 40(4):421-428.]

[7] Li Y, Efatmaneshnik M, Dempster A G. Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications [J]. GPS Solutions, 2012, 16(1):41-52.

[8] 甘雨, 隋立芬, 戚國賓,等. 陸地導(dǎo)航中GNSS/陀螺儀組合實時測姿方法[J]. 武漢大學(xué)學(xué)報信息科學(xué)版, 2015, 40(3):379-383. [Gan Yu, Sui Li-fen, Qi Guo-bin, et al. A real-time attitude determination approach for land navigation based on GPS/Gyroscope integration [J]. Geomatics and Information Science of Wuhan University, 2015, 40(3):379-383.]

[9] 趙偉, 袁信, 林雪原. 基于H ∞濾波技術(shù)的GPS/INS全組合導(dǎo)航系統(tǒng)研究[J]. 宇航學(xué)報, 2002, 23(3):39-43. [Zhao Wei, Yuan Xin, Lin Xue-yuan. Research on GPS/INS integration based on H∞ filter [J]. Journal of Astronautics, 2002, 23(3):39-43.]

[10] 楊艷娟, 卞鴻巍, 田蔚風(fēng),等. 一種新的INS/GPS組合導(dǎo)航技術(shù)[J]. 中國慣性技術(shù)學(xué)報, 2004, 12(2):23-26. [Yang Yan-juan, Bian Hong-wei, Tian Wei-feng, et al. A new INS/GPS integrated navigation technique [J]. Journal of Chinese Inertial Technology, 2004, 12(2):23-26.]

[11] 周姜濱, 袁建平, 羅建軍,等. 基于聯(lián)邦濾波的SINS/GPS全組合導(dǎo)航系統(tǒng)研究[J]. 系統(tǒng)仿真學(xué)報, 2009, 21(6):1562-1564. [Zhou Jiang-bin, Yuan Jian-ping, Luo Jian-jun, et al. Fully integrated SINS/GPS navigation system based on federated filters [J]. Journal of System Simulation, 2009, 21(6):1562-1564.]

[12] 王小旭, 趙琳. 自適應(yīng)融合濾波算法及其在INS/GPS組合導(dǎo)航中的應(yīng)用[J]. 宇航學(xué)報, 2010, 31(11):2503-2511. [Wang Xiao-xu, Zhao Lin. Adaptive fusion filtering algorithm and its application for INS/GPS integrated navigation system [J]. Journal of Astronautics, 2010, 31(11):2503-2511.]

[13] 吳富梅, 聶建亮, 何正斌. 利用預(yù)測殘差和選權(quán)濾波構(gòu)造的分類因子在GPS/INS組合導(dǎo)航中的應(yīng)用[J]. 武漢大學(xué)學(xué)報(信息科學(xué)版), 2012, 37(3):261-264. [Wu Fu-mei, Nie Jian-liang, He Zheng-bin. Classified adaptive filtering to GPS/INS integrated navigation based on predicted residuals and selecting weight filtering [J]. Geomatics and Information Science of Wuhan University, 2012, 37(3):261-264.]

[14] 楊元喜, 何海波, 徐天河. 論動態(tài)自適應(yīng)濾波[J]. 測繪學(xué)報，2001, 30(4):293-298. [Yang Yuan-xi, He Hai-bo, Xu Tian-he. Adaptive robust filtering for kinematic GPS positioning [J]. Acta Geodaetica et Cartographica Sinica, 2001, 30(4):293-298 .]

[15] Markley F. Attitude error representations for Kalman filtering [J]. Journal of Guidance, Control and Dynamics，2003, 26(2):311-317.

[16] 高鐘毓. 慣性導(dǎo)航系統(tǒng)技術(shù)[M]. 北京：清華大學(xué)出版社，2012.

[17] 秦永元，張洪鉞，汪叔華. 卡爾曼濾波與組合導(dǎo)航原理(第二版)[M]. 西安：西北工業(yè)大學(xué)出版社，2012.

[18] 崔先強(qiáng), 楊元喜. 分類因子自適應(yīng)抗差濾波[J]. 自然科學(xué)進(jìn)展, 2006, 16(4):490-494. [Cui Xian-qiang, Yang Yuan-xi. Adaptively robust filtering with classified adaptive factors [J]. Progress in Natural Science, 2006, 16(4):490-494.]

[19] Yang Y X, Cui X Q. Adaptively robust filter with multi adaptive factors [J]. Survey Review, 2008, 40(309):260- 270.

AnAdaptiveFilteringAlgorithmforYawBasedonGPS/INSCompletelyIntegratedSystem

TIAN Yuan, SUI Li-fen, TIAN Yi-jun, XIAO Guo-rui, DAI Qing

(Institute of Geospatial and Information, Information Engineering University, Zhengzhou 450001, China)

Firstly, the GPS/INS completely integrated system is discussed based on land navigation. The attitude correction effects of the completely-integrated and tightly-integrated are respectively analyzed under the kinematic state and static state. The test result is concluded that the yaw precision of the completely-integrated is obviously better than that of the tightly-integrated. And the situation of the yaw’s slow divergence, appearing in the tightly-integrated when the carrier transforms from the kinematic state to static state, can be overcome by the completely-integrated. Secondly, in view of the situation that the anomaly of the kinetic model increases when the carrier is under large-angle turning, a completely-integrated filtering algorithm which only adaptively modifies the yaw is introduced. The simulated test result shows that the influence of the dynamic model abnormity enlarged during large-angle turning can be partly weakened and the yaw precision is partly improved.

GPS/INS integrated; completely integrated system; yaw; adaptive filtering; classified adaptive factor

P228.4

A

1000-1328(2017)11- 1212- 07

10.3873/j.issn.1000- 1328.2017.11.010

2017- 06- 16;

2017- 08- 29

國家自然科學(xué)基金(41674016,41274016)

田源(1993-)，男，碩士生，主要從事GNSS/INS組合定位測姿的研究。

通信地址：河南省鄭州市高新區(qū)科學(xué)大道62號(450001)

電話:18790272987

E-mail：tyuan1247@qq.com.

隋立芬(1961-)，女，教授,主要從事測量數(shù)據(jù)處理理論與方法的研究。本文通信作者。

通信地址：河南省鄭州市高新區(qū)科學(xué)大道62號(450001)

E-mail：suilifen@163.com