摘 要： 建立了INS（Inertial Navigation System）/GPS緊耦合組合導(dǎo)航系統(tǒng)，針對測量粗差對系統(tǒng)定位結(jié)果的影響，將抗差估計理論應(yīng)用于非線性濾波算法，提出了基于等價權(quán)原理的抗差UKF定位算法。加入模擬粗差進(jìn)行實驗，結(jié)果表明，觀測量異常時，抗差UKF算法能成功探測和剔除觀測量粗差，實現(xiàn)定位結(jié)果的有效估計。
關(guān)鍵詞： 慣性導(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）是一種以牛頓力學(xué)原理為基礎(chǔ)的自主式導(dǎo)航系統(tǒng)。而由INS和GPS組合構(gòu)成的INS/GPS組合導(dǎo)航系統(tǒng)可以充分發(fā)揮子系統(tǒng)的優(yōu)勢，為進(jìn)一步克服松耦合系統(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)的變化，基于慣性坐標(biāo)系得到INS/GPS緊耦合系統(tǒng)的狀態(tài)向量為[2]：
其中，δR為位置誤差，δV為速度誤差，δ?漬為平臺失準(zhǔn)角，δε為陀螺儀零偏，δa為加速度計零偏，假設(shè)δε、δa為常值偏差，則基于上述狀態(tài)變量的狀態(tài)誤差方程為：
方程中Fe是比力向量的反對稱矩陣，Ne是引力系數(shù)矩陣，是地球系相對于慣性系的角速度在地球系的投影，C代表載體系到慣性系的方向余弦矩陣，M和N為一階馬爾可夫過程的常量表示[2]。
將式（2）整理并離散化可寫成：
式中，w為系統(tǒng)噪聲矩陣。
1.2 緊耦合導(dǎo)航系統(tǒng)量測方程
偽距與多普勒頻移測量殘余為GPS接收模塊得到的偽距、多普勒頻移實際測量值與依靠慣性器件計算的偽距、多普勒頻移預(yù)測值相減后得到的結(jié)果[1]：
為消除本地時鐘與衛(wèi)星時鐘的時差，將相對于不同衛(wèi)星得到的偽距與偽距率測量殘余相減，可以得到偽距與偽距率星間差[10]：
式中A為量測矩陣，v為測量噪聲。
2 基于等價權(quán)原理的抗差算法
2.1 等價權(quán)原理
設(shè)觀測向量為L，則系統(tǒng)的誤差方程為[5，11]：
式（9）中，V為n維殘差向量，A為量測矩陣，則狀態(tài)量的最小二乘估計解為：
其中，B為先驗權(quán)矩陣，由式（9）可知，最小二乘估計是由觀測序列{Li}對參數(shù){xi}進(jìn)行估計，所以觀測值會直接影響狀態(tài)估計結(jié)果。為了抵制異常值對{xi}的影響，應(yīng)用抗差M估計，可建立如下準(zhǔn)則函數(shù)：
對式（11）求極值解，則得到參數(shù)向量的抗差M估值為：
X=（ATBA）-1ATBL（12）
可見，式（12）只是將式（10）最小二乘估值中的權(quán)矩陣換成了等價權(quán)矩陣B，其迭代形式為：
Xk+1=（ATBkA）-1ATBkL（13）
2.2 IGG方案權(quán)函數(shù)
IGG方案對應(yīng)的權(quán)函數(shù)采用三段法，即保值域直接采用量測值作為解算變量，降值域降低觀測值的權(quán)從而部分采用量測數(shù)據(jù)，拒絕域通過給等價權(quán)Bi賦零從而完全抑制粗差的影響，通過對數(shù)據(jù)質(zhì)量進(jìn)行針對性處理實現(xiàn)最優(yōu)估計[12]，IGG方案等價權(quán)函數(shù)為：
式中，vi為基于觀測殘差的標(biāo)準(zhǔn)化殘差統(tǒng)計量。k0，k1為常量，一般取k0=1.0，k1=2.5:8.0，|vi|≤k0為保值域，k0< |vi|<k1為降值域，|vi|≥k1 為拒絕域，函數(shù)圖形如圖1所示。
3 抗差UKF濾波算法設(shè)計
根據(jù)UKF濾波采用UT變換對稱采樣可知，這種方法采樣得到的Sigma點完全取決于前一時刻系統(tǒng)的狀態(tài)估計值，如果組合導(dǎo)航系統(tǒng)在前一時刻的狀態(tài)估計誤差較大，則會導(dǎo)致結(jié)果嚴(yán)重失真[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ù)分析，抗差UKF算法的濾波方程只需要在標(biāo)準(zhǔn)UKF算法的基礎(chǔ)上，對噪聲協(xié)方差陣Rk進(jìn)行其等價協(xié)方差替換，通過等價權(quán)替換調(diào)節(jié)卡爾曼濾波增益Kk，從而使濾波方程剔除觀測粗差實現(xiàn)估計的有效性。
4 緊耦合系統(tǒng)的抗差UKF算法實驗
本文建立緊耦合系統(tǒng)狀態(tài)方程及量測方程，利用四元數(shù)法求解慣導(dǎo)系統(tǒng)微分方程，分別采用UKF和抗差UKF進(jìn)行數(shù)據(jù)融合。本次實驗直接采用地理坐標(biāo)系作為參考坐標(biāo)系，實驗的初始條件及常量設(shè)定值：地球自轉(zhuǎn)角速度：e=7.292 115 146 7×10-5 rad/s；基準(zhǔn)橢球體的長半徑：a=6.378 137×10-6 m；橢球偏心率為：0.081 8；基準(zhǔn)重力加速度常量：g0=9.8 m/s2。
本次實驗設(shè)定載體處于恒加速運動狀態(tài)，載體加速度an=[3 4 0]T，采樣間隔取Ts=0.01 s，仿真時間為tall=400 s，每100個采樣點進(jìn)行一次數(shù)據(jù)融合輸出，載體初始位置在WGS-84地心地固直角坐標(biāo)系下設(shè)定為：x0= -1 710 547 m，y0=4 933 560 m，z0=3 569 065 m。
初始方向余弦矩陣C=1 0 00 1 00 0 1
狀態(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)實粗差的干擾，然后分別利用標(biāo)準(zhǔn)UKF和抗差UKF進(jìn)行濾波輸出，結(jié)果分別如圖2、3所示。
4.1 姿態(tài)誤差估計結(jié)果對比圖
圖2姿態(tài)估計結(jié)果顯示，標(biāo)準(zhǔn)UKF在粗差歷元處姿態(tài)估計出現(xiàn)明顯跳變，隨著載體的運行，航向角yaw仍會收斂到36.7°，但估計異常卻十分明顯。而抗差UKF算法在實現(xiàn)準(zhǔn)確姿態(tài)估計的同時能有效地抑制粗差干擾。為量化分析姿態(tài)結(jié)果的改善情況，對仿真結(jié)果從50 s以后進(jìn)行誤差統(tǒng)計如表1所示。
4.2 狀態(tài)誤差估計結(jié)果對比圖
考察圖3位置估計結(jié)果，2.5倍的觀測量異常使得UKF位置結(jié)果抖動明顯。同樣以東向位置Pe為例計算誤差均方根，標(biāo)準(zhǔn)UKF算法為99.65 m，而抗差UKF算法為53.97 m，可見抗差UKF算法抑制粗差效果明顯。
4.3 大地坐標(biāo)系下的輸出結(jié)果
圖4為大地坐標(biāo)系定位結(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定位算法。實驗表明，在觀測量異常時，標(biāo)準(zhǔn)UKF算法不能抑制粗差，而抗差UKF算法能成功實現(xiàn)對觀測量粗差的探測和剔除，定位輸出的經(jīng)度、緯度、高度結(jié)果表明，抗差UKF算法是一種性能穩(wěn)定的數(shù)據(jù)整合定位算法。
參考文獻(xiàn)
[1] 謝鋼.GPS原理與接收機設(shè)計[M].北京：電子工業(yè)出版社，2009.
[2] 董緒榮，張守信，華仲春.GPS/INS組合導(dǎo)航定位及其應(yīng)用[M].長沙：國防科學(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].測繪學(xué)報，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è)大學(xué)出版社，2006.
[9] 唐康華.GPS/MIMU嵌入式組合導(dǎo)航關(guān)鍵技術(shù)研究[D].長沙：國防科學(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].中國測繪學(xué)會第九次全國會員代表大會暨學(xué)會成立50周年紀(jì)念大會論文集，北京，2009：37-47.