基于VB-AUKF的GNSS/SINS組合導(dǎo)航信息融合算法

孫玉梅，潘新龍，王美春

(1 濰坊科技學(xué)院計(jì)算機(jī)學(xué)院，山東 濰坊 262700；2 海軍航空大學(xué)，山東 煙臺(tái) 264001)

0 引言

經(jīng)典線性卡爾曼濾波采用線性高斯?fàn)顟B(tài)空間模型,是GNSS/SINS組合導(dǎo)航系統(tǒng)經(jīng)常采用的信息融合方式[1]。當(dāng)系統(tǒng)的非線性比較嚴(yán)重時(shí),擴(kuò)展卡爾曼濾波(EKF)的估計(jì)不準(zhǔn)確,而無跡卡爾曼濾波(UKF)可以降低EKF線性化帶來的誤差,可以將組合導(dǎo)航系統(tǒng)擴(kuò)展到非線性濾波模型[2]。然而,UKF方法的一個(gè)嚴(yán)重缺陷是假設(shè)準(zhǔn)確的系統(tǒng)模型及已知的先驗(yàn)噪聲統(tǒng)計(jì)特性,而這個(gè)假設(shè)在實(shí)際環(huán)境下不成立[3]。

自適應(yīng)濾波是解決上述問題的有效方法,在估計(jì)系統(tǒng)狀態(tài)參數(shù)的同時(shí)也可以估計(jì)噪聲統(tǒng)計(jì)特性。而貝葉斯方法是目前最普遍應(yīng)用的方法,包括協(xié)方差匹配法、極大似然法均為貝葉斯方法的近似方法[4]。而變分貝葉斯方法具有較低的計(jì)算成本,可以近似求解貝葉斯方法中高度耦合的高維隱變量的后驗(yàn)推理,近年來在諸多領(lǐng)域得到應(yīng)用,如情報(bào)監(jiān)測[5]、目標(biāo)跟蹤[6]、雷達(dá)系統(tǒng)[7]等。

近年來,將變分貝葉斯方法與線性卡爾曼濾波技術(shù)相結(jié)合而形成的VB-KF算法在組合導(dǎo)航系統(tǒng)也得到較深入的研究[8]。如:文獻(xiàn)[8-9]將VB-KF技術(shù)應(yīng)用于GNSS/SINS組合導(dǎo)航系統(tǒng);文獻(xiàn)[10]、文獻(xiàn)[11]分別將VB-KF技術(shù)應(yīng)用于同步與異步多傳感器組合導(dǎo)航系統(tǒng);針對VB-KF技術(shù)對噪聲跟蹤性能差的問題,文獻(xiàn)[12-13]提出了基于T分布的變分高斯濾波算法,但是該方法將量測噪聲建模為T分布、導(dǎo)致不能單獨(dú)調(diào)節(jié)噪聲方差內(nèi)獨(dú)立變化的噪聲。同時(shí),上述文獻(xiàn)的共同特點(diǎn)是采用線性卡爾曼濾波方法,無法有效解決模型誤差導(dǎo)致的濾波精度下降問題。

為此,文中將變分貝葉斯方法與UKF算法相結(jié)合,在對GNSS/SINS組合導(dǎo)航系統(tǒng)進(jìn)行變分貝葉斯自適應(yīng)UKF非線性濾波建模的基礎(chǔ)上,提出了量測噪聲方差自適應(yīng)估計(jì)的迭代模型。

1 變分貝葉斯估計(jì)模型

對于狀態(tài)方程非線性而量測方程線性的離散時(shí)間非線性系統(tǒng)[10]:

(1)

對于式(1)模型,如果Rk是時(shí)變的或未知的,則貝葉斯最優(yōu)濾波的目標(biāo)是計(jì)算系統(tǒng)狀態(tài)和量測噪聲方差矩陣的聯(lián)合后驗(yàn)分布p(Xk,Rk|Z1∶k),此時(shí)該濾波過程包含以下步驟[14-15]:

1)初始化:計(jì)算系統(tǒng)狀態(tài)和量測噪聲方差矩陣的先驗(yàn)分布p(X0,R0)。

2)預(yù)測:系統(tǒng)狀態(tài)向量Xk與測量噪聲方差矩陣Rk的聯(lián)合預(yù)測分布由Chapman-Kolmogorov方程給出[16]:

(2)

3)更新:給定下一個(gè)測量值Zk,通過貝葉斯規(guī)則將上述預(yù)測分布更新為后驗(yàn)分布:

p(Xk,Rk|Z1∶k)∝p(Zk|Xk,Rk)p(Xk,Rk|Z1∶k-1)

(3)

式(2)、式(3)所涉及的積分通常是無法解析處理的,可利用后驗(yàn)變分近似表示為[17]:

p(Xk,Rk|Z1∶k)=QX(Xk)QR(Rk)

(4)

其中:

(5)

式(5)中兩個(gè)方程相互耦合,為此,對兩個(gè)方程的期望進(jìn)行評估及參數(shù)匹配,得到密度:

(6)

則式(2)、式(3)變?yōu)閇16]:

(7)

(8)

(9)

(10)

為了使觀測噪聲的變化規(guī)律具有自適應(yīng)調(diào)整特性,引入因子ρi∈(0,1]。對于i=1,2,…,d,則有[16]:

(11)

2 基于變分貝葉斯的組合導(dǎo)航UKF算法

2.1 變分貝葉斯UKF算法

根據(jù)系統(tǒng)模型,將標(biāo)準(zhǔn)UKF算法與變分貝葉斯估計(jì)模型相結(jié)合,得到組合導(dǎo)航系統(tǒng)的基于變分貝葉斯的自適應(yīng)UKF算法(VB-AUKF):

對于式(1)所示的非線性系統(tǒng),將系統(tǒng)狀態(tài)和系統(tǒng)噪聲進(jìn)行合并,得到增廣狀態(tài)向量:

(12)

式中:χa,χX和χW分為Xa,Xk和Wk的采樣點(diǎn)向量,假設(shè)其維數(shù)分別為N,Lx,Lw,則N=Lx+Lw。

步驟1:初始化

(13)

步驟2:樣點(diǎn)計(jì)算

(14)

(15)

其中:λ=α2(N+κ)-N;α,β和κ為比例因子。

步驟3:時(shí)間更新

(16)

(17)

(18)

其中:ρ=[ρ1ρ2…ρd]T;α=[α1α2…αd]T;β=[β1β2…βd]T。

步驟4:量測更新

(19)

(20)

(21)

(22)

(23)

(24)

式(21)～式(24)中,1≤j≤J。

2.2 GNSS/SINS組合導(dǎo)航系統(tǒng)模型

GNSS/SINS組合導(dǎo)航系統(tǒng)非線性濾波的狀態(tài)方程采用SINS的力學(xué)方程,組合導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)直接選取為被估計(jì)的狀態(tài)。選取“東北天”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系,則連續(xù)時(shí)間系統(tǒng)的狀態(tài)方程為:

(25)

選取GNSS接收機(jī)輸出的三維速度和三維位置直接作為觀測向量,則量測方程可以表示為:

(26)

對式(25)、式(26)進(jìn)行離散化處理可以得到離散化的狀態(tài)方程及量測方程。

3 仿真結(jié)果與分析

在Matlab環(huán)境下進(jìn)行仿真,首先評估參數(shù)ρ,α,β的取值對量測噪聲未知均方差估計(jì)的影響,然后評估GNSS/SINS組合導(dǎo)航系統(tǒng)VB-AUKF算法的濾波性能。

3.1 仿真條件

以飛行器作為仿真對象,其飛行過程包含各種機(jī)動(dòng)運(yùn)動(dòng)。初始位置為(118°E,29°E,50 m),仿真時(shí)間為3 600 s;濾波周期為1 s。初始參數(shù)設(shè)置如表1所示。

表1 導(dǎo)航傳感器誤差參數(shù)設(shè)置Table 1 Navigation sensor error parameter setting

根據(jù)表1所示的GNSS誤差設(shè)置,則正常情況下GNSS量測噪聲的均方差為:

rroot=sqrtm(R)=diag([0.2 0.2 0.2 8 8 8])

(27)

其中函數(shù)sqrtm(·)的功能是對矩陣中每個(gè)元素取開平方。

為了驗(yàn)證系統(tǒng)算法性能,在飛行器飛行期間設(shè)定GNSS量測噪聲均方差變化為:

(28)

3.2 仿真校驗(yàn)

以下的仿真實(shí)驗(yàn)均是以相同的導(dǎo)航仿真原始數(shù)據(jù)為基礎(chǔ),對式(21)～式(24)的迭代步數(shù)取j=2。

3.2.1 參數(shù)ρ對未知方差估計(jì)的影響

為了評估ρ對量測噪聲未知方差估計(jì)精度的影響,首先使得α,β的取值不變并設(shè)定為:

(29)

對參數(shù)ρ的取值,定義方案A:

方案A1:ρ=[0.85 0.85 0.85 0.85 0.85 0.85]

方案A2:ρ=[0.90 0.90 0.90 0.90 0.90 0.90]

方案A3:ρ=[0.95 0.95 0.95 0.95 0.95 0.95]

方案A4:ρ=[1 1 1 1 1 1]

方案A5:ρ=[0.99 0.99 0.99 0.99 0.99 0.99]

基于上述5種方案,圖1～圖2為利用文中算法估計(jì)到的位置誤差及速度誤差均方差曲線。

圖1 GNSS位置噪聲均方差估計(jì)曲線Fig.1 GNSS position noise RMSE estimation curve

圖2 GNSS速度噪聲均方差估計(jì)曲線Fig.2 GNSS velocity noise RMS estimation curve

由圖1～圖2可見,當(dāng)ρ中的元素ρi=1(i=1,2,…,d;d=6)時(shí),變分貝葉斯算法適合于估計(jì)平穩(wěn)噪聲的均方差(方案A4);ρi<1并接近于1時(shí),文中算法估計(jì)的量測噪聲均方差在實(shí)際均方差變化結(jié)束時(shí)存在較嚴(yán)重的“拖尾”現(xiàn)象(方案A5);當(dāng)ρi<0.9時(shí),估計(jì)到的均方差能較精確地反映實(shí)際均方差的變化規(guī)律,但是估計(jì)曲線波動(dòng)幅度隨著ρi的減小而增大(方案A1、方案A2)。

實(shí)驗(yàn)證明,當(dāng)0.95≤ρi≤0.97時(shí),文中算法估計(jì)到的均方差效果較好,不僅能反映實(shí)際均方差的變化規(guī)律,而且估計(jì)曲線波動(dòng)幅度較小(方案A3)。

3.2.2 參數(shù)α對未知方差估計(jì)的影響

為了評估α對量測噪聲未知方差估計(jì)精度的影響,首先使得ρ,β的取值不變并設(shè)定為:

(30)

方案B1:α=[1 1 1 1 1 1]

方案B2:α=0.2×[1 1 1 1 1 1]

方案B3:α=5×[1 1 1 1 1 1]

基于上述3種方案,圖3～圖4為利用文中算法估計(jì)的位置誤差及速度誤差均方差曲線。

圖3 GNSS位置噪聲均方差估計(jì)曲線Fig.3 GNSS position noise RMSE estimation curve

圖4 GNSS速度噪聲均方差估計(jì)曲線Fig.4 GNSS velocity noise RMSE estimation curve

由圖3～圖4可以看出,當(dāng)ρ,β不變時(shí),α的選擇僅僅改變均方差的初始估計(jì)值,經(jīng)過短時(shí)間后α的選擇對均方差的估計(jì)值無影響。

3.2.3 參數(shù)β對未知方差估計(jì)的影響

為了評估β對量測噪聲未知方差估計(jì)精度的影響,首先使得α,ρ的取值不變并設(shè)定為:

(31)

方案C1:β=[0.3 0.3 0.3 6 6 6]

方案C2:β=3×[0.3 0.3 0.3 6 6 6]

方案C3:β=[0.02 0.02 0.02 32 32 32]

基于上述3種方案,圖5～圖6為利用文中算法估計(jì)的位置誤差及速度誤差均方差曲線。

圖5 GNSS位置噪聲均方差估計(jì)曲線Fig.5 GNSS position noise RMSE estimation curve

圖6 GNSS速度噪聲均方差估計(jì)曲線Fig.6 GNSS velocity noise RMSE estimation curve

由圖5～圖6可以看出,當(dāng)α,ρ不變時(shí),β的選擇也僅僅改變均方差的初始估計(jì)值,經(jīng)過短時(shí)間后β的選擇對均方差的估計(jì)值無影響。

同時(shí),綜合對比圖1～圖6,可以看出,當(dāng)ρ的值不變時(shí),α和β值的選擇僅僅改變了均方差的初始估計(jì)值,經(jīng)過短時(shí)間后α和β值的選擇對均方差的估計(jì)值無影響;對均方差估計(jì)結(jié)果影響的決定參數(shù)是ρ。所以,只要ρ的取值處于合理區(qū)域,文中算法估計(jì)到的量測噪聲均方差具有較好的穩(wěn)定性。

3.2.4 基于VB-AUKF的組合導(dǎo)航實(shí)驗(yàn)

圖7～圖9給出了基于標(biāo)準(zhǔn)UKF算法及文中算法方案15的組合導(dǎo)航系統(tǒng)位置誤差、速度誤差及姿態(tài)誤差曲線對比圖。由圖7～圖9可以看出,由于標(biāo)準(zhǔn)UKF算法對量測噪聲均方差無自適應(yīng)調(diào)節(jié)能力,所以當(dāng)量測噪聲均方差發(fā)生變化時(shí)標(biāo)準(zhǔn)UKF算法得到的導(dǎo)航參數(shù)誤差較大;而文中算法由于對量測噪聲均方差的自適應(yīng)調(diào)節(jié)能力較強(qiáng),故可以保證系統(tǒng)的濾波精度。

圖7 位置誤差曲線對比圖Fig.7 Position error curve comparison

圖8 速度誤差曲線對比圖Fig.8 Velocity error curve comparison

圖9 姿態(tài)誤差曲線對比圖Fig.9 Attitude error curve comparison

當(dāng)前述的迭代步數(shù)j≥3時(shí),量測噪聲均方差的估計(jì)精度變化可忽略不計(jì),且也不會(huì)對組合導(dǎo)航系統(tǒng)的濾波精度形成影響。

為了定量分析圖7～圖9所示的兩種濾波算法的濾波精度,在整個(gè)導(dǎo)航時(shí)段內(nèi),表2給出了基于VB-AUKF算法及標(biāo)準(zhǔn)UKF算法的各導(dǎo)航參數(shù)誤差均方差統(tǒng)計(jì)分析。

表2 基于VB-AUKF及標(biāo)準(zhǔn)UKF的導(dǎo)航參數(shù)誤差統(tǒng)計(jì)分析Table 2 Statistical analysis of navigation parameter error based on VB-AUKF and Standard UKF

由表2可以看出,相對于標(biāo)準(zhǔn)UKF算法,VB-AUKF算法可提高位置精度及速度精度分別約12%及51%;提高橫滾角及俯仰角精度約75%,同時(shí)可以改變航向角在量測噪聲均方差變化時(shí)存在的發(fā)散現(xiàn)象。為此,基于VB-AUKF算法可以克服標(biāo)準(zhǔn)UKF算法不能自適應(yīng)調(diào)節(jié)量測噪聲均方差的缺陷,進(jìn)而提高了濾波器的魯棒性。

4 結(jié)論

基于GNSS/SINS組合導(dǎo)航系統(tǒng)的非線性濾波模型的特點(diǎn),為了聯(lián)合估計(jì)系統(tǒng)狀態(tài)參數(shù)及量測噪聲統(tǒng)計(jì)特性,提出了組合導(dǎo)航系統(tǒng)的VB-AUKF算法,并建立了量測噪聲方差的迭代估計(jì)模型;針對決定量測噪聲均方差估計(jì)精度的3個(gè)參數(shù)、組合導(dǎo)航系統(tǒng)的VB-AUKF算法性能,論文進(jìn)行了仿真實(shí)驗(yàn)。

仿真結(jié)果表明:量測噪聲均方差估計(jì)模型的調(diào)節(jié)因子是決定均方差估計(jì)精度的關(guān)鍵因素,而其他2個(gè)因子的選擇僅僅影響均方差估計(jì)的初始值;相對于標(biāo)準(zhǔn)UKF,當(dāng)量測噪聲均方差發(fā)生變化時(shí),VB-AUKF算法可明顯提高組合導(dǎo)航系統(tǒng)的濾波精度。