付心如，徐愛功，孫 偉

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

?

抗差自適應(yīng)UKF的INS/GNSS組合導(dǎo)航算法

付心如，徐愛功，孫 偉

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

為了進(jìn)一步提高濾波精度和穩(wěn)定性，針對INS/GNSS組合導(dǎo)航系統(tǒng)的特點(diǎn)建立系統(tǒng)非線性誤差模型，并提出一種抗差自適應(yīng)UKF算法：基于UKF濾波中新息序列動態(tài)調(diào)整觀測噪聲數(shù)學(xué)統(tǒng)計特性，并結(jié)合殘差序列構(gòu)造自適應(yīng)因子，改善初始協(xié)方差誤差對標(biāo)準(zhǔn)UKF的影響，提高標(biāo)準(zhǔn)UKF自適應(yīng)能力，減少存在觀測粗差情況下濾波精度下降問題。仿真結(jié)果表明，該算法能夠較好抑制觀測粗差對定位精度及穩(wěn)定性的影響。

組合導(dǎo)航；抗差；自適應(yīng)濾波；UKF

0 引言

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

論文擬將抗差自適應(yīng)引入標(biāo)準(zhǔn)UKF，設(shè)計基于速度/位置組合導(dǎo)航抗差自適應(yīng)UKF濾波器，以期有效抵制觀測粗差對濾波精度及穩(wěn)定性的影響。

1 抗差自適應(yīng)UKF算法

1.1 UKF算法

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

UT變換的重點(diǎn)在于確定Sigma點(diǎn)集個數(shù)、位置，及對應(yīng)的權(quán)值[11]。為避免粒子點(diǎn)退化問題，本文采用2N+1個Sigma點(diǎn)對稱確定性采樣。

INS/GNSS組合導(dǎo)航系統(tǒng)的非線性狀態(tài)方程和以速度與位置為觀測量的線性觀測方程分別為：

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

(1)

yk=Hkxk+vk。

(2)

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

基于狀態(tài)協(xié)方差計算Sigma點(diǎn)集為

(3)

Sigma點(diǎn)集對應(yīng)權(quán)值為

(4)

(5)

(6)

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

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

(7)

(8)

(9)

UKF測量更新。標(biāo)準(zhǔn)UKF中不僅時間更新需要多次運(yùn)行非線性函數(shù)以保證狀態(tài)空間的準(zhǔn)確性，而且測量更新中也同樣需要?；谒俣?位置的組合導(dǎo)航系統(tǒng)是線性觀測方程，在系統(tǒng)噪聲和觀測噪聲為加性的前提下，對標(biāo)準(zhǔn)UKF的觀測方程進(jìn)行簡化，減少由于多次運(yùn)行Sigma采樣點(diǎn)引起的巨大計算量對濾波實(shí)時性的影響。簡化的測量更新過程為：

(10)

(11)

(12)

(13)

(14)

(15)

1.2 抗差自適應(yīng)UKF

標(biāo)準(zhǔn)UKF模型框架中噪聲協(xié)方差是確切的、帶有一定數(shù)學(xué)統(tǒng)計特性的數(shù)值，但在實(shí)際應(yīng)用中通常是未知或隨時間變化的。錯誤先驗(yàn)統(tǒng)計信息的使用很可能導(dǎo)致濾波較大的定位誤差，甚至發(fā)散。

(16)

(17)

根據(jù)零均值高斯白噪聲數(shù)學(xué)特征及狀態(tài)協(xié)方差公式，可得新息序列協(xié)方差為

(18)

實(shí)際觀測中樣本協(xié)方差為

(19)

故

(20)

即將

(21)

換為

(22)

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

(23)

基于判別統(tǒng)計量ΔVk的自適應(yīng)因子為

(24)

式中：αk為自適應(yīng)因子；k為經(jīng)驗(yàn)數(shù)值，一般取1

(25)

換為

(26)

即當(dāng)UKF初始協(xié)方差選取有偏差或載體處于異常擾動情況下，αk小于1，自適應(yīng)重新分配狀態(tài)預(yù)報值與當(dāng)前觀測信息的權(quán)重，提高最終濾波定位解的精度與穩(wěn)定性。當(dāng)動力學(xué)模型穩(wěn)定且無異常擾動時，αk值為1，濾波為標(biāo)準(zhǔn)UKF濾波。

2 INS/GNSS組合導(dǎo)航數(shù)學(xué)模型

設(shè)計以速度/位置為觀測量的15維INS/GNSS組合導(dǎo)航抗差自適應(yīng)UKF濾波器，其中觀測參數(shù)為

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

非線性姿態(tài)誤差與速度誤差方程分別為：

(27)

(28)

位置誤差方程為：

(29)

(30)

(31)

(32)

3 實(shí)驗(yàn)與結(jié)果分析

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

表1 Kalman濾波器參數(shù)

根據(jù)抗差自適應(yīng)UKF濾波模型及上述參數(shù)，在沒有外界觀測粗差干擾情況下，對標(biāo)準(zhǔn)UKF、抗差自適應(yīng)UKF(RAUKF)進(jìn)行仿真模擬，得到的濾波結(jié)果分別如圖3、圖4所示。

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

表2 無粗差時精度比較 m

表2中數(shù)學(xué)統(tǒng)計值也證明了上述結(jié)論，即在無觀測粗差情況下，抗差自適應(yīng)UKF濾波精度較標(biāo)準(zhǔn)UKF濾波定位精度較高。

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

由圖5中導(dǎo)航定位結(jié)果可以看出，加入的粗差對標(biāo)準(zhǔn)UKF導(dǎo)航解有明顯影響，在600 s、700 s、800 s、900 s處濾波解出現(xiàn)了定位誤差突變現(xiàn)象，而且由于Kalman濾波算法的遞推性，突變誤差對該時間點(diǎn)以后的定位結(jié)果有一定的影響。而對于RAUKF，由于誤差協(xié)方差陣估計能夠?qū)崟r適應(yīng)誤差數(shù)學(xué)特性及其濾波因子自適應(yīng)調(diào)整，從而可以調(diào)節(jié)濾波循環(huán)周期中的增益矩陣Kk，合理調(diào)整狀態(tài)預(yù)測值與觀測值對組合導(dǎo)航解的貢獻(xiàn)，達(dá)到抵制觀測粗差的作用，如圖6所示。

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

表3 存在觀測粗差時定位結(jié)果比較

通過統(tǒng)計數(shù)據(jù)可以看出，采用RAUKF算法能較好地提升導(dǎo)航定位性能，有效控制觀測粗差對定位精度的影響。

4 結(jié)束語

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

[1] 毛克誠, 孫付平, 李海豐.UKF在GPS/INS組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 測繪科學(xué)技術(shù)學(xué)報, 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] 高為廣, 楊元喜, 張婷. 一種提高神經(jīng)網(wǎng)絡(luò)泛化能力的自適應(yīng)UKF濾波算法[J]. 武漢大學(xué)學(xué)報(信息科學(xué)版), 2008, 33(5): 500-503.

[6] 高社生, 王建超, 焦雅林. 自適應(yīng)SVD-UKF算法及在組合導(dǎo)航的應(yīng)用[J].中國慣性技術(shù)學(xué)報, 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] 胡高歌, 劉逸涵, 高社生,等. 改進(jìn)的強(qiáng)跟蹤UKF算法及其在INS/GPS組合導(dǎo)航中的應(yīng)用[J].中國慣性技術(shù)學(xué)報, 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]何正斌, 吳富梅, 聶建亮.先驗(yàn)協(xié)方差陣誤差對動態(tài)Kalman濾波解的影響[J].武漢大學(xué)學(xué)報(信息科學(xué)版), 2011, 36(6): 34-38.

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

[14]楊元喜, 徐天河. 基于移動開窗法協(xié)方差估計和方差分量估計的自適應(yīng)濾波[J]. 武漢大學(xué)學(xué)報(信息科學(xué)版), 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

國家重點(diǎn)研發(fā)計劃項(xiàng)目(2016YFC0803102)；國家自然科學(xué)基金項(xiàng)目(41304032)；遼寧工程技術(shù)大學(xué)測繪與地理科學(xué)學(xué)院優(yōu)秀研究生論文培養(yǎng)計劃(YS201502)。

付心如(1991—)，女，河北邢臺人，碩士研究生，研究方向?yàn)榻M合導(dǎo)航。

徐愛功(1963—)，男，山東日照人，博士，教授，博生生導(dǎo)師，研究方向?yàn)樾l(wèi)星定位與地理信息系統(tǒng)相關(guān)理論與應(yīng)用。

付心如，徐愛功，孫偉.抗差自適應(yīng)UKF的INS/GNSS組合導(dǎo)航算法[J].導(dǎo)航定位學(xué)報,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