抗差自適應UKF的INS/GNSS組合導航算法

付心如，徐愛功，孫 偉

(遼寧工程技術大學 測繪與地理科學學院，遼寧 阜新 123000)

?

抗差自適應UKF的INS/GNSS組合導航算法

付心如，徐愛功，孫 偉

(遼寧工程技術大學 測繪與地理科學學院，遼寧 阜新 123000)

為了進一步提高濾波精度和穩定性，針對INS/GNSS組合導航系統的特點建立系統非線性誤差模型，并提出一種抗差自適應UKF算法：基于UKF濾波中新息序列動態調整觀測噪聲數學統計特性，并結合殘差序列構造自適應因子，改善初始協方差誤差對標準UKF的影響，提高標準UKF自適應能力，減少存在觀測粗差情況下濾波精度下降問題。仿真結果表明，該算法能夠較好抑制觀測粗差對定位精度及穩定性的影響。

組合導航；抗差；自適應濾波；UKF

0 引言

解決非線性問題的最優算法是基于條件后驗概率完整描述的數學模型，而精確的數學模型需要無盡參數描述，難以應用于實際系統；所以對于非線性系統來說，核心在于近似方法[1-2]。為此人們提出對非線性數學模型進行線性化、采樣方法近似等多種次優濾波器算法。常用擴展Kalman濾波(extended Kalman filter, EKF)屬于前者，采用對模型高階項逼近或忽略的局部線性最優技術；但其算法精度取決于導航系統的非線性強度以及展開點的選擇[3-6]，且難以模塊化應用。無跡Kalman濾波(unscented Kalman filter，UKF)利用對系統狀態非線性函數概率密度分布的近似，基于先驗知識選擇一組加權Sigma點集，隨后對每個Sigma點進行非線性轉換，捕捉到較高精度的后驗均值及協方差[7-9]。由文獻[10-11]可知：在計算量上UKF與EKF相當，但性能優于EKF；但UKF對初值的選取較為敏感，且噪聲數學統計特性的不確定性或觀測異常[12-14]等都會使UKF濾波精度下降。

論文擬將抗差自適應引入標準UKF，設計基于速度/位置組合導航抗差自適應UKF濾波器，以期有效抵制觀測粗差對濾波精度及穩定性的影響。

1 抗差自適應UKF算法

1.1 UKF算法

UKF核心是通過Unscented變換(簡稱UT變換)傳播和更新非線性數學模型中狀態均值與誤差協方差陣[15-16]。UT變換是利用后驗概率密度進行近似得到次優解，即利用一組確定性選擇加權Sigma點，使這些點的某些特性與先驗分布相匹配，隨后對每個Sigma點施加UT變換，并對其結果點(均值、協方差)進行加權處理，得到最終狀態估值及其協方差陣，其原理如圖1。將UT變換應用在Kalman濾波框架中得到UKF濾波算法。

UT變換的重點在于確定Sigma點集個數、位置，及對應的權值[11]。為避免粒子點退化問題，本文采用2N+1個Sigma點對稱確定性采樣。

INS/GNSS組合導航系統的非線性狀態方程和以速度與位置為觀測量的線性觀測方程分別為：

xk=f(xk-1,ωk)；

(1)

yk=Hkxk+vk。

(2)

式中：xk為狀態量；yk為觀測量；k為濾波次數；ωk、vk分別為系統過程噪聲序列和觀測噪聲序列；f表示非線性狀態方程；Hk為觀測矩陣。

基于狀態協方差計算Sigma點集為

(3)

Sigma點集對應權值為

(4)

(5)

(6)

UKF時間更新。對輸入變量Sigma點集χ中的每個點進行非線性轉換，并對其進行加權處理，具體計算過程如下：

χk|k-1=f(χk-1,k-1)；

(7)

(8)

(9)

UKF測量更新。標準UKF中不僅時間更新需要多次運行非線性函數以保證狀態空間的準確性，而且測量更新中也同樣需要。基于速度/位置的組合導航系統是線性觀測方程，在系統噪聲和觀測噪聲為加性的前提下，對標準UKF的觀測方程進行簡化，減少由于多次運行Sigma采樣點引起的巨大計算量對濾波實時性的影響。簡化的測量更新過程為：

(10)

(11)

(12)

(13)

(14)

(15)

1.2 抗差自適應UKF

標準UKF模型框架中噪聲協方差是確切的、帶有一定數學統計特性的數值，但在實際應用中通常是未知或隨時間變化的。錯誤先驗統計信息的使用很可能導致濾波較大的定位誤差，甚至發散。

(16)

(17)

根據零均值高斯白噪聲數學特征及狀態協方差公式，可得新息序列協方差為

(18)

實際觀測中樣本協方差為

(19)

故

(20)

即將

(21)

換為

(22)

式中：n為實際觀測樣本數目，也叫窗口大小。利用新息序列匹配算法能保持與實際觀測噪聲數學特性一致。即當觀測值出現粗差時，也能通過實時調整狀態協方差矩陣改善增益矩陣Kk值。為更好地實時調整濾波器中的增益矩陣，提高濾波跟蹤性能，必須使濾波因子自適應當前時刻實際噪聲協方差，利用殘差序列構造誤差判別統計量

(23)

基于判別統計量ΔVk的自適應因子為

(24)

式中：αk為自適應因子；k為經驗數值，一般取1

(25)

換為

(26)

即當UKF初始協方差選取有偏差或載體處于異常擾動情況下，αk小于1，自適應重新分配狀態預報值與當前觀測信息的權重，提高最終濾波定位解的精度與穩定性。當動力學模型穩定且無異常擾動時，αk值為1，濾波為標準UKF濾波。

2 INS/GNSS組合導航數學模型

設計以速度/位置為觀測量的15維INS/GNSS組合導航抗差自適應UKF濾波器，其中觀測參數為

x=[φEφNφUδvEδvNδvUδλδL

非線性姿態誤差與速度誤差方程分別為：

(27)

(28)

位置誤差方程為：

(29)

(30)

(31)

(32)

3 實驗與結果分析

設載體的初始位置為42°N、121°E，高度150m。陀螺常值漂移為0.03(°)/h，加速度計常值偏置為5ug，慣性測量單元采樣周期0.01s，衛星接收機采樣周期1s。其他Kalman濾波參數設置如表1所示。載體運動軌跡包括：爬高、轉彎、俯沖、加速、減速、勻速等過程，總共歷時1 600s，如圖2所示。軌跡模擬器給出的各時間點速度、位置信息作為計算導航誤差參考值。

表1 Kalman濾波器參數

根據抗差自適應UKF濾波模型及上述參數，在沒有外界觀測粗差干擾情況下，對標準UKF、抗差自適應UKF(RAUKF)進行仿真模擬，得到的濾波結果分別如圖3、圖4所示。

對比無粗差情況下UKF與RAUKF定位誤差，抗差自適應濾波在一定程度上使誤差值趨于平穩。具體精度比較如表2所示。

表2 無粗差時精度比較 m

表2中數學統計值也證明了上述結論，即在無觀測粗差情況下，抗差自適應UKF濾波精度較標準UKF濾波定位精度較高。

為研究本文算法在外界觀測粗差擾動情況下的定位精度，分別在600 s、700 s、800 s、900 s時刻分別加入持續5 s時間30 m的觀測粗差。得到的定位結果分別如圖5、圖6所示。

由圖5中導航定位結果可以看出，加入的粗差對標準UKF導航解有明顯影響，在600 s、700 s、800 s、900 s處濾波解出現了定位誤差突變現象，而且由于Kalman濾波算法的遞推性，突變誤差對該時間點以后的定位結果有一定的影響。而對于RAUKF，由于誤差協方差陣估計能夠實時適應誤差數學特性及其濾波因子自適應調整，從而可以調節濾波循環周期中的增益矩陣Kk，合理調整狀態預測值與觀測值對組合導航解的貢獻，達到抵制觀測粗差的作用，如圖6所示。

表3給出了加入觀測粗差后各時間段內UKF與RAUKF濾波結果的定位誤差。

表3 存在觀測粗差時定位結果比較

通過統計數據可以看出，采用RAUKF算法能較好地提升導航定位性能，有效控制觀測粗差對定位精度的影響。

4 結束語

在分析標準UKF算法的基礎上，針對UKF受初值誤差及外界擾動影響的問題，利用新息序列與殘差序列進行實時觀測噪聲統計特性匹配并自適應調整Kk，提高UKF抗差與自適應能力。文中對抗差自適應UKF在INS/GNSS組合導航系統中的應用進行了研究，仿真實驗結果表明，無論粗差是否存在，RAUKF定位精度均高于標準UKF濾波，而且在一定程度上能夠抵制外界擾動對導航定位精度的影響。

[1] 毛克誠, 孫付平, 李海豐.UKF在GPS/INS組合導航系統中的應用[J]. 測繪科學技術學報, 2007, 24(3): 200-202.

[2]RIGATOSGG.NonlinearKalmanfiltersandparticlefiltersforintegratednavigationofunmannedaerialvehicles[J].Robotics&AutonomousSystems, 2012, 60(7): 978-995.

[3]JULIERS,UHLMANNJ.AnewextensionoftheKalmanfiltertononlinearsystems[J].SPIEProceedingsSeries, 1997, 3068: 182-193.

[4]RHUDYM,GUY,GROSSJ,etal.SensitivityandrobustnessanalysisofEKFandUKFdesignparametersforGPS/INSsensorfusion[J].AIAAJournalofAerospaceInformationSystems, 2013, 10(3): 131-143.

[5] 高為廣, 楊元喜, 張婷. 一種提高神經網絡泛化能力的自適應UKF濾波算法[J]. 武漢大學學報(信息科學版), 2008, 33(5): 500-503.

[6] 高社生, 王建超, 焦雅林. 自適應SVD-UKF算法及在組合導航的應用[J].中國慣性技術學報, 2010, 18(6): 737-741.

[7]MENEGAZHMT,ISHIHARAJY,BORGESGA,etal.AsystematizationoftheunscentedKalmanfiltertheory[J].IEEETransactionsonAutomaticControl, 2015, 60(10): 2583-2598.

[8]CHANGLB,QINFJ,ZHAF.Pseudoopen-loopunscentedquaternionestimatorforattitudeestimation[J].IEEESensorsJournal, 2016, 16(11): 4460-4469.

[9] 胡高歌, 劉逸涵, 高社生,等. 改進的強跟蹤UKF算法及其在INS/GPS組合導航中的應用[J].中國慣性技術學報, 2014, 22(5): 634-639.

[10]GROSSJ,GUY,RHUDYM,etal.FlighttestevaluationofGPS/INSsensorfusionalgorithmsforattitudeestimation[J].IEEETransonAerospaceElectronicSystems, 2012, 48(3): 2128-2139.

[11]潘泉, 楊峰, 葉亮, 等.一類非線性濾波器—UKF綜述[J]. 控制與決策, 2005(5): 481-489.

[12]何正斌, 吳富梅, 聶建亮.先驗協方差陣誤差對動態Kalman濾波解的影響[J].武漢大學學報(信息科學版), 2011, 36(6): 34-38.

[13]楊元喜,何海波, 徐天河.論動態自適應濾波[J].測繪學報, 2001, 30(4): 293-298.

[14]楊元喜, 徐天河. 基于移動開窗法協方差估計和方差分量估計的自適應濾波[J]. 武漢大學學報(信息科學版), 2003, 28(6): 714-718.

[15]SORENONHW.Kalmanfiltering:theoryandapplication[M].NewYork:IEEEPress, 1985.

[16]JWODJ,YANGCF,CHUANGCH,etal.Performanceenhancementforultra-tightGPS/INSintegrationusingafuzzyadaptivestrongtrackingunscentedKalmanfilter[J].NonlinearDynamics, 2013, 73(1): 377-395.

An INS/GNSS integrated navigation algorithm based on robust adaptive UKF

FUXinru,XUAigong,SUNWei

(School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China)

In order to further improve the filtering accuracy and stability, the paper established the systematic nonlinear error model according to the characteristics of INS/GNSS integrated navigation system, and proposed a robust adaptive unscented Kalman filter algorithm: the statistical characteristics of the observation noise were dynamically estimated and adjusted based on innovation vectors of UKF, and the adaptive factors based on residual sequences were construct, the influence of the values of initial parameters on standard UKF was reduced, the adaptive capability of the standard UKF was enhanced, and the decreased filter accuracy when gross errors existed in the measurement values was improved.Simulation results showed that the method could effectively restrain the influence of observed gross errors on the positioning accuracy and stability of the integrated navigation.

integrated navigation; robust; adaptive filter; UKF

2016-09-06

國家重點研發計劃項目(2016YFC0803102)；國家自然科學基金項目(41304032)；遼寧工程技術大學測繪與地理科學學院優秀研究生論文培養計劃(YS201502)。

付心如(1991—)，女，河北邢臺人，碩士研究生，研究方向為組合導航。

徐愛功(1963—)，男，山東日照人，博士，教授，博生生導師，研究方向為衛星定位與地理信息系統相關理論與應用。

付心如，徐愛功，孫偉.抗差自適應UKF的INS/GNSS組合導航算法[J].導航定位學報,2017,5(2):111-116.(FU Xinru, XU Aigong, SUN Wei.An INS/GNSS integrated navigation algorithm based on robust adaptive UKF[J].Journal of Navigation and Positioning,2017,5(2):111-116.)

10.16547/j.cnki.10-1096.20170220.

P228

A

2095-4999(2017)02-0111-06