胡曉梅 潘新龍 劉成銘

1 煙臺南山學院智能科學與工程學院,山東省煙臺市大學路12號,265713 2 海軍航空大學,山東省煙臺市,264001 3 山東南山鋁業(yè)股份有限公司,山東省煙臺市南山南路6號,265706

SINS/GNSS組合導航時,若濾波過程中出現(xiàn)量測噪聲異常,則會導致濾波精度下降甚至發(fā)散[1]?？梢允褂脦в辛繙y噪聲估計器的自適應濾波解決此問題[2-5]。如鄧傳遠[2]、曾慶化等[3]對基于標準Sage-Husa的組合導航自適應卡爾曼濾波進行研究,提高了組合導航的精度。但將Sage-Husa自適應卡爾曼濾波應用于組合導航系統(tǒng)中存在2個問題[4]:1)由于組合導航系統(tǒng)階次比較高,往往存在濾波發(fā)散的情況;2)在狀態(tài)及觀測誤差不穩(wěn)定時,組合導航系統(tǒng)也會存在濾波發(fā)散現(xiàn)象。為此,本文提出一種改進的Sage-Husa自適應濾波算法。

1 組合導航系統(tǒng)模型

導航坐標系取東北天地理坐標系,GNSS/SINS組合導航系統(tǒng)的狀態(tài)方程表示為[6]:

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(1)

式中,Xk=[φEφNφUδvEδvNδvUδλδLδhεxεyεz?x?y?z]T為k時刻的狀態(tài)向量,其中,φE、φN、φU為數(shù)字平臺角誤差,δvE、δvN、δvU為速度誤差,δλ、δL、δh為經(jīng)度誤差、緯度誤差和高度誤差,εx、εy、εz為三軸陀螺誤差的一階Markov過程,?x、?y、?z為三軸加速度計誤差的一階Markov過程;Φk,k-1為狀態(tài)一步轉移矩陣;Γk-1為系統(tǒng)噪聲矩陣;Wk-1為系統(tǒng)噪聲。

GNSS/SINS組合導航系統(tǒng)的量測方程為:

Zk=HkXk+Vk

(2)

式中,觀測向量Zk取GNSS和SINS三維位置、三維速度的差值;觀測噪聲Vk近似為白噪聲。

基于標準Sage-Husa自適應濾波的GNSS/SINS組合導航的常規(guī)卡爾曼濾波算法為[4]:

(3)

(4)

(5)

Pk|k=[I-KkHk]Pk|k-1

(6)

(7)

式(3)為狀態(tài)向量的預測值,則新息向量為:

(8)

基于標準Sage-Husa的估計方法為[5]:

(9)

(10)

式中,b為遺忘因子(0

2 改進的Sage-Husa自適應濾波算法

在對GNSS/SINS組合導航系統(tǒng)運用式(3)～(7)的最優(yōu)線性卡爾曼濾波時,目前常采用補償反饋校正法,即濾波器工作時間內(nèi)任何時刻均需對元件施加校正量。采用補償反饋校正法時,考慮到工程實現(xiàn),式(3)變?yōu)閇1]:

(11)

此時,式(8)變?yōu)?

εk=Zk=HkXk+Vk

(12)

則有:

(13)

(14)

根據(jù)式(14)構建加權系數(shù):

(15)

對Rk進行估計:

(16)

(17)

式中,diag(·)是以某個向量為主對角線元素產(chǎn)生對角矩陣的函數(shù),或以向量的形式返回一個矩陣上對角線元素的函數(shù)。

3 仿真結果及分析

3.1 仿真條件

根據(jù)上述設定,在正常情況下,GNSS量測噪聲的RMS矩陣可以表示為:

rroot=sqrtm(R)=

diag([8880.20.20.2])

式中,sqrtm(R)為對R中每個元素取開平方。

設置包括變速、爬升、轉彎、平飛等機動過程的飛行航跡,飛行時間為3 600 s。飛行過程中,在不同時間段內(nèi)設定GNSS量測噪聲RMS穩(wěn)定、發(fā)生突變與緩變3種情況,具體變化為:

(18)

3.2 仿真結果及分析

為了綜合評估本文算法估計量測噪聲RMS的性能,使用4種方案進行解算:1)SHKF1,本文改進的Sage-Husa算法,b=0.95;2)SHKF2,本文改進的Sage-Husa算法,b=0.9;3)SHKF3,本文改進的Sage-Husa算法,b=0.99;4)VBKF,變分貝葉斯估計算法。

圖1為不同方案對量測位置噪聲RMS的估計,對速度噪聲RMS的估計效果與之類似?？梢钥闯?1)b接近于1時,估計曲線較為平滑,但是估計結果存在較為嚴重的拖尾效應,當b減小時,估計結果曲線存在較為嚴重的波動現(xiàn)象;2)SHKF1和VBKF的估計精度基本一致,即選擇適當?shù)腷值時,本文算法對量測噪聲RMS的估計精度與變分貝葉斯濾波相當;3)通過對濾波初始值的觀測,本文算法對量測噪聲RMS估計的初始值與b值無關,進而增強了算法的自適應性;4)本文算法不僅能夠準確估計量測噪聲RMS的突變,也能準確估計量測噪聲RMS的緩變,自適應性較強;5)b取0.95左右時,估計精度最佳。

基于上述仿真數(shù)據(jù),圖2～3分別給出基于標準KF及本文算法(SHKF,b=0.95)的位置誤差、速度誤差對比圖?？梢钥闯?當量測噪聲RMS發(fā)生變化時,相對于標準KF,本文算法能夠提供更加精確的導航信息。

圖2 位置誤差對比Fig.2 Comparison of position error

圖3 速度誤差對比Fig.3 Comparison of velocity error

為綜合對比本文SHKF算法及標準KF的性能,將GNSS輸出分為噪聲RMS突變、緩變及恒定3個時段,分別對各導航參數(shù)誤差的RMS進行統(tǒng)計(表1)。可以看出,相對于標準KF,當GNSS噪聲RMS突變時,SHKF算法可提高約18.8%的位置精度、約19%的速度精度;當GNSS噪聲RMS緩變時,SHKF算法可提高約21%的位置精度、約23%的速度精度;當GNSS噪聲RMS不變時,由SHKF算法得到的導航參數(shù)精度略低于標準KF,主要原因是由SHKF算法估計到的GNSS噪聲RMS具有滯后性。同時,本文算法在執(zhí)行過程中未發(fā)生濾波發(fā)散現(xiàn)象,克服了標準Sage-Husa自適應濾波算法在組合導航系統(tǒng)中存在的發(fā)散問題。

表1 基于SHKF及標準KF的組合導航系統(tǒng)誤差對比Tab.1 Error comparison of integrated navigation system based on SHKF and standard KF

4 結 語

在量測噪聲統(tǒng)計特性未知的情況下,為提高GNSS/SINS組合導航系統(tǒng)的濾波精度,提出一種改進的Sage-Husa自適應濾波方法。該方法能夠?qū)崟r、準確地估計量測噪聲的未知RMS,且具有初始估計結果與遺忘因子無關、估計精度與變分貝葉斯方法相當?shù)奶攸c,同時克服了標準Sage-Husa自適應濾波方法存在的濾波發(fā)散問題。仿真實驗表明,當量測噪聲RMS發(fā)生變化時,本文方法明顯優(yōu)于常規(guī)卡爾曼濾波算法,有效提高了組合導航系統(tǒng)的濾波精度。