組合導(dǎo)航數(shù)據(jù)融合自適應(yīng)UKF算法研究

吳 濤，戴 卿，匡秀梅，王炎川

(1.中測新圖(北京)遙感技術(shù)有限責(zé)任公司 航空遙感技術(shù)自然資源部重點實驗室，北京 100039；2.邢臺市勘察測繪院 測繪部，河北 邢臺 054000)

0 引言

微慣性導(dǎo)航系統(tǒng)(MEMS-INS)與全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)近年來在導(dǎo)航與位置服務(wù)領(lǐng)域發(fā)展迅速，將兩者結(jié)合的MEMS-INS/GNSS組合導(dǎo)航技術(shù)，優(yōu)勢互補、性價比高，并發(fā)展為航空、交通、農(nóng)業(yè)、測繪等領(lǐng)域的應(yīng)用熱點[1]。由于導(dǎo)航系統(tǒng)方程的非線性，且在高動態(tài)環(huán)境下工作時，導(dǎo)航結(jié)果會受到噪聲統(tǒng)計特征時變性的影響，因此，需要對時變噪聲情況下的非線性導(dǎo)航系統(tǒng)在信息融合算法上提出更高的要求，這是現(xiàn)代測繪導(dǎo)航數(shù)據(jù)處理研究的熱門話題[2-3]。

擴展卡爾曼濾波(Extended Kalman Filter, EKF)是測繪導(dǎo)航數(shù)據(jù)處理領(lǐng)域中被廣泛采用的一種非線性濾波算法，但由于算法中使用函數(shù)模型近似來逼近非線性系統(tǒng)，會產(chǎn)生泰勒高階項截斷誤差，并最終引起濾波精度下降甚至發(fā)散[4]。為克服EKF局部線性化和Jacobin矩陣計算繁瑣的問題，無跡卡爾曼濾波(Unscented Kalman Filter , UKF)中借鑒sigma點對概率密度函數(shù)的逼近易于非線性函數(shù)本身的思路，構(gòu)造了一種基于確定性采樣的濾波算法，以二階泰勒精度進行逼近，但當(dāng)系統(tǒng)噪聲統(tǒng)計特征不確定時，濾波精度則會受到顯著影響[5]。抗差自適應(yīng)UKF算法的出現(xiàn)，有效提升了噪聲影響下的濾波魯棒性，但自適應(yīng)因子的選取受到經(jīng)驗性制約[6]。于此同時，為改善UKF算法數(shù)值穩(wěn)定性方面的問題，平方根無跡卡爾曼濾波(Square Root Unscented Kalman Filter，SRUKF)的提出有效避免了舍入誤差影響和矩陣求逆計算[7]。

為進一步改善SRUKF算法的穩(wěn)定性和計算效率，本文在現(xiàn)有研究基礎(chǔ)上，提出了一種基于最大后驗概率估計(Maximum A Posteriori, MAP)的快捷UKF算法。新算法將奇異值分解(Singular Value Decomposition, SVD)引入傳統(tǒng)平方根狀態(tài)協(xié)方差計算，然后采用基于MAP的時變噪聲估計器改善算法在時間更新和量測更新階段的自適應(yīng)能力，提高算法的魯棒性，最后通過MEMS-INS/GNSS組合導(dǎo)航系統(tǒng)數(shù)據(jù)測試，結(jié)果表明，新算法在確保導(dǎo)航輸出結(jié)果的同時，能有效降低計算成本，提高導(dǎo)航的實時性。

1 MEMS-INS/GNSS松散耦合導(dǎo)航模型

松散耦合導(dǎo)航是一種應(yīng)用較廣的低成本組合導(dǎo)航方式。松散耦合導(dǎo)航工作流程如圖1所示，MEMS-INS模塊和GNSS衛(wèi)星導(dǎo)航模塊相互獨立工作，輸出位置、速度和姿態(tài)信息后進行數(shù)據(jù)融合計算，并對MEMS-INS閉環(huán)修正，是一種GNSS輔助慣導(dǎo)的工作模式[8]。

圖1 松散耦合導(dǎo)航工作流程

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

MEMS-INS/GNSS組合導(dǎo)航系統(tǒng)中，建模狀態(tài)方程為

1.2 導(dǎo)航系統(tǒng)量測方程

松散耦合導(dǎo)航系統(tǒng)中，慣性裝置和GNSS衛(wèi)星模塊輸出的位置和速度之差一并構(gòu)成系統(tǒng)量測值，因此組合導(dǎo)航的量測方程表示如下

其中，

2 基于MAP噪聲估計器的UKF算法

2.1 基本算法框架

設(shè)時間離散系統(tǒng)的非線性模型為

xk=Mk,k-1(xk-1)+uk(5)

yk=Hk(xk)+vk(6)

其中，Mk,k-1表示狀態(tài)轉(zhuǎn)移矩陣，Hk表示觀測矩陣，均表現(xiàn)為一定的非線性特征，uk和vk分別表示狀態(tài)方程和量測方程的噪聲項，xk為k時刻狀態(tài)量；yk為量測向量。

產(chǎn)生2lk-1+1個sigma點

進行時間更新：

(13)

進行量測更新：

(18)

其中，Γk表示閾值，并限制lk范圍區(qū)間為ll≤lk≤lu。

時變噪聲估計統(tǒng)計器是一種次優(yōu)無偏MAP估計器，如文獻[4-5]所述:

由上述推導(dǎo)發(fā)現(xiàn)，基于MAP噪聲估計器的UKF算法通過在線實時估計狀態(tài)方程和量測方程的噪聲項進一步精化了噪聲統(tǒng)計模型，可改善算法的估計性能，基于MAP噪聲估計器的UKF算法流程如圖2所示。

圖2 基于MAP噪聲估計器的UKF算法流程圖

2.2 算法性能分析

3 導(dǎo)航數(shù)據(jù)測試分析

利用MATLAB編譯MEMS-INS/GNSS組合導(dǎo)航系統(tǒng)仿真平臺，仿真數(shù)據(jù)采用動態(tài)仿真加組合導(dǎo)航誤差融合的方式獲得，其中量測誤差由MEMS-INS慣導(dǎo)單元和GNSS衛(wèi)星接收機模塊在實測數(shù)據(jù)中獲得，具有統(tǒng)計不確定性和時變性。模擬GNSS測速誤差 0.5 m/s，定位誤差5 m，MEMS陀螺噪聲均方根 0.5° /s，加速度計噪聲均方根10-3g。GNSS數(shù)據(jù)輸出1 Hz衛(wèi)星可見顯示數(shù)如圖3所示。慣導(dǎo)裝置數(shù)據(jù)輸出100 Hz，橫滾角量測輸出、俯仰角量測輸出、航偏角量測輸出分別如圖4、圖5、圖6所示。導(dǎo)航系統(tǒng)采用1 s周期的閉環(huán)反饋校正方式，工作時間共1800 s。

圖3 衛(wèi)星可見顯示數(shù)

圖4 橫滾角量測輸出

圖5 俯仰角量測輸出

圖6 航偏角量測輸出

粒子濾波算法(Particle Filter, PF)是在貝葉斯框架準則下推導(dǎo)的一種高性能濾波，雖然算法計算量較大易出現(xiàn)維度災(zāi)難，但在非線性非高斯系統(tǒng)中，仍是一種輸出精度較高的濾波算法。因此為驗證本文提出的新算法估計精度，將PF計算結(jié)果設(shè)為實驗參考值。

方案1：標準PF算法

方案2：基于MAP的快捷UKF算法

方案3：SRUKF算法

由實驗結(jié)果可以看出：

(1)平面位置輸出誤差如圖7、圖8所示，三種濾波方案均能基本滿足組合導(dǎo)航定位計算，而方案3的精度略低于方案1和方案2，因為SRUKF相比PF和基于MAP的快捷UKF算法缺少對量測項的降噪處理。

圖7 平面位置輸出誤差(E)

圖8 平面位置輸出誤差(N)

(2)速度輸出誤差如圖9、圖10所示，姿態(tài)輸出誤差如圖11、圖12所示。方案1和方案2輸出精度明顯優(yōu)于方案3。說明基于標準PF算法和MAP的快捷UKF算法對噪聲統(tǒng)計特性不確定的動態(tài)導(dǎo)航系統(tǒng)具有一定的魯棒性。

圖9 速度輸出誤差(E)

圖10 速度輸出誤差(N)

圖11 姿態(tài)輸出誤差(X)

圖12 姿態(tài)輸出誤差(Y)

(3)不同濾波方案的RMSE結(jié)果如表1所示。分析表1數(shù)據(jù)可知，方案2所用新算法穩(wěn)定性較好，輸出位置、速度與姿態(tài)誤差較小，相比傳統(tǒng)UKF算法在濾波精度上有所提升。新算法雖然相比方案1所用PF在精度上有所損失，但計算效率較高，計算時間上大大減小，方案3相比于方案2計算效率提升了16%，在組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合實時性上有很大的提升。

表1 不同濾波方案的RMSE結(jié)果

4 結(jié)語

為消除UKF算法在組合導(dǎo)航數(shù)據(jù)融合中輸出精度和計算效率之間的矛盾，本文提出了一種帶時變噪聲估計的快捷UKF算法。通過MAP準則對噪聲時變統(tǒng)計特性進行實時跟蹤估計，可有效提高濾波魯棒性，同時引入SVD分解技術(shù)較好的緩解算法中sigma點的計算負擔(dān)，有利于組合導(dǎo)航UKF算法的實時自適應(yīng)計算。將新算法應(yīng)用于MEMS-INS/GNSS測繪導(dǎo)航數(shù)據(jù)實驗中，結(jié)果表明，該算法在受時變噪聲影響的高機動導(dǎo)航環(huán)境下，能有效改善濾波輸出精度，提高算法的計算效率，對低成本MEMS-INS/GNSS的工程應(yīng)用具有一定參考價值。