費(fèi)恒敏，施琴，田俊杰

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

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

費(fèi)恒敏1，2，施琴1，田俊杰1

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

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

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

0 引言

為克服單一導(dǎo)航系統(tǒng)的缺點(diǎn)，實(shí)現(xiàn)性能互補(bǔ)，以衛(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）是一種以牛頓力學(xué)原理為基礎(chǔ)的自主式導(dǎo)航系統(tǒng)。而由INS和GPS組合構(gòu)成的INS/GPS組合導(dǎo)航系統(tǒng)可以充分發(fā)揮子系統(tǒng)的優(yōu)勢(shì)，為進(jìn)一步克服松耦合系統(tǒng)高度依賴衛(wèi)星信號(hào)質(zhì)量的缺點(diǎn)，設(shè)計(jì)基于偽距和偽距率測(cè)量值的緊耦合組合導(dǎo)航系統(tǒng)成為研究的重點(diǎn)[3-4]。

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

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

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

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

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

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

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

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

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

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

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

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

由式（6）、式（7）可得量測(cè)方程：

式中A為量測(cè)矩陣，v為測(cè)量噪聲。

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

2.1 等價(jià)權(quán)原理

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

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

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

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

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

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

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

式中，v?i為基于觀測(cè)殘差的標(biāo)準(zhǔn)化殘差統(tǒng)計(jì)量。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ì)

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

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

卡爾曼濾波增益：

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

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

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

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

4 緊耦合系統(tǒng)的抗差UKF算法實(shí)驗(yàn)

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

本次實(shí)驗(yàn)設(shè)定載體處于恒加速運(yùn)動(dòng)狀態(tài)，載體加速度an=[3 4 0]T，采樣間隔取Ts=0.01 s，仿真時(shí)間為tall= 400 s，每100個(gè)采樣點(diǎn)進(jìn)行一次數(shù)據(jù)融合輸出，載體初始位置在WGS-84地心地固直角坐標(biāo)系下設(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]

量測(cè)噪聲陣為：

R=diag[10 10 10 0.1 0.1 0.1]

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

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

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

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

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

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

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

5 結(jié)論

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

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

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

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

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

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

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

[3]劉建業(yè)，曾慶化，趙偉.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，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].測(cè)繪學(xué)報(bào)，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)動(dòng)態(tài)導(dǎo)航定位[M].北京：測(cè)繪出版社，2006.

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

[9]唐康華.GPS/MIMU嵌入式組合導(dǎo)航關(guān)鍵技術(shù)研究[D].長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，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)用的主要進(jìn)展[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

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