費恒敏，施琴，田俊杰

（1.解放軍理工大學理學院，江蘇南京211101；2.解放軍96512部隊，陜西漢中723000）

INS/GPS緊耦合組合導(dǎo)航系統(tǒng)抗差定位算法*

費恒敏1，2，施琴1，田俊杰1

（1.解放軍理工大學理學院，江蘇南京211101；2.解放軍96512部隊，陜西漢中723000）

建立了INS（Inertial Navigation System）/GPS緊耦合組合導(dǎo)航系統(tǒng)，針對測量粗差對系統(tǒng)定位結(jié)果的影響，將抗差估計理論應(yīng)用于非線性濾波算法，提出了基于等價權(quán)原理的抗差UKF定位算法。加入模擬粗差進行實驗，結(jié)果表明，觀測量異常時，抗差UKF算法能成功探測和剔除觀測量粗差，實現(xiàn)定位結(jié)果的有效估計。

慣性導(dǎo)航；全球定位系統(tǒng)；抗差估計；卡爾曼濾波

0 引言

為克服單一導(dǎo)航系統(tǒng)的缺點，實現(xiàn)性能互補，以衛(wèi)星導(dǎo)航和自主式導(dǎo)航結(jié)合形成的組合導(dǎo)航技術(shù)便成為導(dǎo)航領(lǐng)域的主要研究和發(fā)展方向[1-2]。慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）是一種以牛頓力學原理為基礎(chǔ)的自主式導(dǎo)航系統(tǒng)。而由INS和GPS組合構(gòu)成的INS/GPS組合導(dǎo)航系統(tǒng)可以充分發(fā)揮子系統(tǒng)的優(yōu)勢，為進一步克服松耦合系統(tǒng)高度依賴衛(wèi)星信號質(zhì)量的缺點，設(shè)計基于偽距和偽距率測量值的緊耦合組合導(dǎo)航系統(tǒng)成為研究的重點[3-4]。

INS/GPS緊耦合系統(tǒng)定位算法在實現(xiàn)有效定位的同時，如何克服測量粗差對狀態(tài)估計結(jié)果的影響，建立組合導(dǎo)航系統(tǒng)的抗差定位算法成為導(dǎo)航領(lǐng)域研究的重要內(nèi)容[5]。為排除有害信息的干擾，防止濾波發(fā)散，通過綜合考慮非線性濾波技術(shù)及抗差估計理論，將傳統(tǒng)無跡Kalman濾波（Unscented Kalman Filtering，UKF）算法與抗差估計理論相結(jié)合[6-7]，提出基于等價權(quán)原理的抗差UKF定位算法，以期保障定位結(jié)果有效性的同時，提高系統(tǒng)的健壯性。

1 緊耦合導(dǎo)航系統(tǒng)的系統(tǒng)方程

緊耦合系統(tǒng)是從INS模塊解算出位置、速度信息，并由該位置、速度信息計算出INS偽距和偽距率信息，然后將慣導(dǎo)器件預(yù)測的偽距和偽距率信息與通過GPS得到的偽距和偽距率信息相減后再作星間差作為觀測量[8-9]。

1.1 緊耦合導(dǎo)航系統(tǒng)狀態(tài)方程

INS/GPS緊耦合系統(tǒng)需要考慮載體運動的位置、速度和加速度等信息，為全面反映載體運動狀態(tài)的變化，基于慣性坐標系得到INS/GPS緊耦合系統(tǒng)的狀態(tài)向量為[2]：

其中，δR為位置誤差，δV為速度誤差，δφ為平臺失準角，δε為陀螺儀零偏，δa為加速度計零偏，假設(shè)δε、δa為常值偏差，則基于上述狀態(tài)變量的狀態(tài)誤差方程為：

方程中Fe是比力向量的反對稱矩陣，Ne是引力系數(shù)矩陣，是地球系相對于慣性系的角速度在地球系的投影，代表載體系到慣性系的方向余弦矩陣，M和N為一階馬爾可夫過程的常量表示[2]。

將式（2）整理并離散化可寫成：

式中，Φ為系統(tǒng)轉(zhuǎn)移矩陣，w為系統(tǒng)噪聲矩陣。

1.2 緊耦合導(dǎo)航系統(tǒng)量測方程

偽距與多普勒頻移測量殘余為GPS接收模塊得到的偽距、多普勒頻移實際測量值與依靠慣性器件計算的偽距、多普勒頻移預(yù)測值相減后得到的結(jié)果[1]：

為消除本地時鐘與衛(wèi)星時鐘的時差，將相對于不同衛(wèi)星得到的偽距與偽距率測量殘余相減，可以得到偽距與偽距率星間差[10]：

由式（6）、式（7）可得量測方程：

式中A為量測矩陣，v為測量噪聲。

2 基于等價權(quán)原理的抗差算法

2.1 等價權(quán)原理

設(shè)觀測向量為L，未知參數(shù)估值為X?，則系統(tǒng)的誤差方程為[5，11]：

式（9）中，V為n維殘差向量，A為量測矩陣，則狀態(tài)量的最小二乘估計解為：

其中，B為先驗權(quán)矩陣，由式（9）可知，最小二乘估計是由觀測序列{Li}對參數(shù){xi}進行估計，所以觀測值會直接影響狀態(tài)估計結(jié)果。為了抵制異常值對{xi}的影響，應(yīng)用抗差M估計，可建立如下準則函數(shù)：

對式（11）求極值解，則得到參數(shù)向量的抗差M估值為：

可見，式（12）只是將式（10）最小二乘估值中的權(quán)矩陣換成了等價權(quán)矩陣B，其迭代形式為：

2.2 IGG方案權(quán)函數(shù)

IGG方案對應(yīng)的權(quán)函數(shù)采用三段法，即保值域直接采用量測值作為解算變量，降值域降低觀測值的權(quán)從而部分采用量測數(shù)據(jù)，拒絕域通過給等價權(quán)Bi賦零從而完全抑制粗差的影響，通過對數(shù)據(jù)質(zhì)量進行針對性處理實現(xiàn)最優(yōu)估計[12]，IGG方案等價權(quán)函數(shù)為：

式中，v?i為基于觀測殘差的標準化殘差統(tǒng)計量。k0，k1為常量，一般取k0=1.0，k1=2.5∶8.0，|v?i|≤k0為保值域，k0＜|v?i|＜k1為降值域，|v?i|≥k1為拒絕域，函數(shù)圖形如圖1所示。

3 抗差UKF濾波算法設(shè)計

根據(jù)UKF濾波采用UT變換對稱采樣可知，這種方法采樣得到的Sigma點完全取決于前一時刻系統(tǒng)的狀態(tài)估計值，如果組合導(dǎo)航系統(tǒng)在前一時刻的狀態(tài)估計誤差較大，則會導(dǎo)致結(jié)果嚴重失真[6]。根據(jù)等價權(quán)替換，構(gòu)造基于IGG方案的抗差UKF算法，用等價權(quán)替換迭代方程權(quán)值，則更新序列的協(xié)方差矩陣為：

其中：Rk為Rk的等價協(xié)方差矩陣，可以由等價權(quán)矩陣求逆獲得，即：Rk=（Bk）-1，于是與等價協(xié)方差陣有關(guān)的變量修正如下：

卡爾曼濾波增益：

經(jīng)測量殘余校正后的狀態(tài)變量更新值：

誤差協(xié)方差更新值：

根據(jù)分析，抗差UKF算法的濾波方程只需要在標準UKF算法的基礎(chǔ)上，對噪聲協(xié)方差陣Rk進行其等價協(xié)方差替換，通過等價權(quán)替換調(diào)節(jié)卡爾曼濾波增益Kk，從而使濾波方程剔除觀測粗差實現(xiàn)估計的有效性。

圖1 IGG等價權(quán)函數(shù)

4 緊耦合系統(tǒng)的抗差UKF算法實驗

本文建立緊耦合系統(tǒng)狀態(tài)方程及量測方程，利用四元數(shù)法求解慣導(dǎo)系統(tǒng)微分方程，分別采用UKF和抗差UKF進行數(shù)據(jù)融合。本次實驗直接采用地理坐標系作為參考坐標系，實驗的初始條件及常量設(shè)定值：地球自轉(zhuǎn)角速度：Ω˙e=7.292 115 146 7×10-5rad/s；基準橢球體的長半徑：a=6.378 137×10-6m；橢球偏心率為：0.081 8；基準重力加速度常量：g0=9.8 m/s2。

本次實驗設(shè)定載體處于恒加速運動狀態(tài)，載體加速度an=[3 4 0]T，采樣間隔取Ts=0.01 s，仿真時間為tall= 400 s，每100個采樣點進行一次數(shù)據(jù)融合輸出，載體初始位置在WGS-84地心地固直角坐標系下設(shè)定為：x0= -1 710 547 m，y0=4 933 560 m，z0=3 569 065 m。

狀態(tài)變量的初值設(shè)為：

x=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]T狀態(tài)變量的協(xié)方差陣初值為：

p0=diag[1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]

系統(tǒng)噪聲陣為：

Q=diag[10 10 10 0.1 0.1 0.1 0.01 0.01 0.01 1 1 1 1 1 1]

量測噪聲陣為：

R=diag[10 10 10 0.1 0.1 0.1]

GPS數(shù)據(jù)由星歷參數(shù)模型計算而來，對偽距和偽距率分別加上均方根為1 m和0.1 m/s的高斯白噪聲來模擬GPS誤差。慣導(dǎo)數(shù)據(jù)由芯片ADIS16365采集得到，運動偏移穩(wěn)定性為0.009°/s，陀螺儀零偏穩(wěn)定度為90°/h，加速度計精度為3.333 mg，陀螺儀精度為0.05°/s。實驗取等價權(quán)函數(shù)區(qū)間常量k0，k1分別為1.5、3。為了檢測抗差算法的估計效果，本次實驗在歷元m=10|20|30|40| 50|51|70|71|90|91|110|111|112|200|201|202|300|301|302時將觀測量調(diào)整為原值的2.5倍，用來模擬現(xiàn)實粗差的干擾，然后分別利用標準UKF和抗差UKF進行濾波輸出，結(jié)果分別如圖2、3所示。

4.1 姿態(tài)誤差估計結(jié)果對比圖

圖2姿態(tài)估計結(jié)果顯示，標準UKF在粗差歷元處姿態(tài)估計出現(xiàn)明顯跳變，隨著載體的運行，航向角yaw仍會收斂到36.7°，但估計異常卻十分明顯。而抗差UKF算法在實現(xiàn)準確姿態(tài)估計的同時能有效地抑制粗差干擾。為量化分析姿態(tài)結(jié)果的改善情況，對仿真結(jié)果從50 s以后進行誤差統(tǒng)計如表1所示。

4.2 狀態(tài)誤差估計結(jié)果對比圖

考察圖3位置估計結(jié)果，2.5倍的觀測量異常使得UKF位置結(jié)果抖動明顯。同樣以東向位置Pe為例計算誤差均方根，標準UKF算法為99.65 m，而抗差UKF算法為53.97 m，可見抗差UKF算法抑制粗差效果明顯。

4.3 大地坐標系下的輸出結(jié)果

圖4為大地坐標系定位結(jié)果顯示，抗差UKF算法成功地實現(xiàn)了結(jié)果輸出，定位結(jié)果為北緯34.243 8°、東經(jīng)108.908 9°、高度396 m，系統(tǒng)輸出結(jié)果與理論計算值相同。

5 結(jié)論

以等價權(quán)抗差估計算法結(jié)合非線性濾波理論，提出了基于偽距、多普勒頻移觀測量的抗差UKF定位算法。實驗表明，在觀測量異常時，標準UKF算法不能抑制粗差，而抗差UKF算法能成功實現(xiàn)對觀測量粗差的探測和剔除，定位輸出的經(jīng)度、緯度、高度結(jié)果表明，抗差UKF算法是一種性能穩(wěn)定的數(shù)據(jù)整合定位算法。

圖2 姿態(tài)估計結(jié)果

表1 抗差UKF姿態(tài)估計結(jié)果分析表

圖3 位置估計結(jié)果

圖4 大地坐標系定位結(jié)果

[1]謝鋼.GPS原理與接收機設(shè)計[M].北京：電子工業(yè)出版社，2009.

[2]董緒榮，張守信，華仲春.GPS/INS組合導(dǎo)航定位及其應(yīng)用[M].長沙：國防科學技術(shù)大學出版社，1998.

[3]劉建業(yè)，曾慶化，趙偉.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學出版社，2009.

[4]JWO D J，CHUNG F C.Fuzzy adaptive unscented Kalman filter for ultra-tight GPS/INS integration[C]. International Symposium on Computational Intelligence and Design，Taiwan，2010：229-235.

[5]楊元喜.衛(wèi)星導(dǎo)航的不確定性不確定度與精度若干注記[J].測繪學報，2012，41（5）：646-650.

[6]JULIER S J，UHLMANN J K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE Aerospace and Electronic Systems，2004，92（3）：401-422.

[7]楊元喜.自適應(yīng)動態(tài)導(dǎo)航定位[M].北京：測繪出版社，2006.

[8]鄧正隆.慣性技術(shù)[M].哈爾濱：哈爾濱工業(yè)大學出版社，2006.

[9]唐康華.GPS/MIMU嵌入式組合導(dǎo)航關(guān)鍵技術(shù)研究[D].長沙：國防科學技術(shù)大學，2008.

[10]JULIER S J.Estimating and exploiting the degree of independent information in distributed data fusion[C].12th International Conference on Information Fusion Seattle，WA，2009：772-779.

[11]BAE J，KIM Y.Nonlinear estimation for spacecraft attitude using decentralized unscented information filter[C]. International Conference on Control Automation and Systems，Gyeonggi-do，2010：1562-1566.

[12]楊元喜.自適應(yīng)抗差濾波理論及應(yīng)用的主要進展[C].

Algorithm of robust estimation for tightly coupled INS/GPS integration

Fei Hengmin1，2，Shi Qin1，Tian Junjie1
（1.College of Science，PLA University of Science and Technology，Nanjing 211101，China；2.Unit 96512 PLA，Hanzhong 723000，China）

Tightly coupled INS/GPS integration navigation system was constructed.A UKF based on equivalent weighting theory of robust estimation for nonlinear filtering was proposed to eliminate the effect of observed outliers.Simulation with outliers showed that the UKF of robust estimation can get a good experimental result by detecting and eliminating observed outliers.

inertial navigation；global positioning system；robust estimation；Kalman filter

TB561

：A

：1674-7720（2015）07-0020-04

解放軍理工大學理學院青年科研基金（LYQNJJ17）