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