荊 蕾 孫煒瑋 喬玉新 劉成銘

1 煙臺(tái)南山學(xué)院工學(xué)院，山東省龍口市大學(xué)路12號(hào)，265713 2 海軍航空大學(xué)，山東省煙臺(tái)市，264001 3 山東東海熱電有限公司，山東省龍口市東海工業(yè)園，265713

無(wú)跡卡爾曼濾波UKF在GNSS/SINS組合導(dǎo)航系統(tǒng)中具有高精度特性[1]。但UKF是在量測(cè)噪聲方差恒定的基礎(chǔ)上建立的，當(dāng)量測(cè)噪聲方差發(fā)生變化時(shí)，其濾波精度會(huì)降低。目前主要采用基于新息的自適應(yīng)濾波方法解決該問(wèn)題。田甜[2]將自適應(yīng)因子引入U(xiǎn)KF算法中以消除粗差對(duì)系統(tǒng)的影響；陳光武等[3]利用UKF對(duì)組合導(dǎo)航進(jìn)行處理，當(dāng)GPS信號(hào)丟失時(shí)利用訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)來(lái)增強(qiáng)UKF的濾波效果，但該算法嚴(yán)重依賴訓(xùn)練樣本的質(zhì)量與數(shù)量；吳濤等[4]引入時(shí)變?cè)肼暪烙?jì)器來(lái)估計(jì)時(shí)變?cè)肼曇蕴岣遀KF濾波的自適應(yīng)能力，但其狀態(tài)方程依然采取線性化處理方式，限制了濾波精度的提高；馬曉杰等[5]引入自適應(yīng)因子以減小濾波器初值偏差和系統(tǒng)模型異常擾動(dòng)對(duì)UKF的影響，但該算法在量測(cè)噪聲異常擾動(dòng)的情況下性能不佳。

基于Sage-Husa的自適應(yīng)濾波在組合導(dǎo)航系統(tǒng)中應(yīng)用廣泛，但該方法存在對(duì)濾波初值敏感、直接使用時(shí)濾波易發(fā)散以及噪聲統(tǒng)計(jì)特性估計(jì)有偏差等缺陷[6]。而基于變分貝葉斯原理的自適應(yīng)濾波方法可以克服上述缺點(diǎn)，并能實(shí)現(xiàn)系統(tǒng)狀態(tài)與量測(cè)噪聲方差的同步估計(jì)[7]?；谏鲜鲇懻?，本文重點(diǎn)針對(duì)量測(cè)噪聲異常擾動(dòng)情況，引入變分貝葉斯原理，提出一種自適應(yīng)UKF算法以適應(yīng)量測(cè)噪聲方差的變化。

1 組合導(dǎo)航非線性濾波模型

以ENU地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，基于UKF的GNSS/SINS組合導(dǎo)航系統(tǒng)狀態(tài)方程為[7]：

(1)

假設(shè)導(dǎo)航系統(tǒng)的先驗(yàn)狀態(tài)、過(guò)程噪聲與未知量測(cè)噪聲間相互獨(dú)立。選取GNSS輸出速度和位置作為觀測(cè)量，即

(2)

式中，Z(t)和V(t)分別為量測(cè)向量和量測(cè)噪聲向量，且滿足V(t)∈N(0,R(t))。

2 組合導(dǎo)航自適應(yīng)UKF算法

2.1 變分貝葉斯原理

變分貝葉斯算法近似使用一種分解的自由形式遞歸逼近誤差狀態(tài)和量測(cè)噪聲方差的聯(lián)合后驗(yàn)分布，然后通過(guò)遞歸算法中的定點(diǎn)迭代估計(jì)狀態(tài)和量測(cè)噪聲的方差[8]。在系統(tǒng)噪聲方差已知、量測(cè)噪聲方差未知的情況下，變分貝葉斯算法可分為預(yù)測(cè)階段和更新階段[9]。預(yù)測(cè)階段可利用C-K(Chapman-Kolmogorov)方程擬合出誤差狀態(tài)和量測(cè)噪聲方差的條件分布。

預(yù)測(cè)階段：

p(Xk-1,Rk-1|Z1:k-1)·dXk-1·dRk-1

(3)

更新階段：

p(Xk,Rk|Z1:k)∝

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

(4)

雖然式(3)和式(4)在理論上最優(yōu)，但由于引入了噪聲方差矩陣及多維數(shù)值積分，因此在實(shí)際情況中很難求解。而變分貝葉斯原理可以采用近似的方法，利用多個(gè)已知分布近似求解后驗(yàn)分布。根據(jù)變分貝葉斯原理，式(3)、(4)中的量測(cè)方差與聯(lián)合預(yù)測(cè)和后驗(yàn)分布可轉(zhuǎn)換為如下的逆Gamma分布和高斯分布：

(5)

(6)

引入預(yù)測(cè)加權(quán)系數(shù)ρi∈(0,1]，對(duì)式(3)中的量測(cè)噪聲方差的預(yù)測(cè)分布作一階近似：

(7)

式中，αk-1|k-1,i和βk-1|k-1,i為逆Gamma分布的2個(gè)參數(shù)，詳細(xì)推導(dǎo)過(guò)程見文獻(xiàn)[10]。

2.2 自適應(yīng)UKF算法

將變分貝葉斯原理與UKF算法結(jié)合，得到適用于GNSS/SINS組合導(dǎo)航系統(tǒng)的自適應(yīng)UKF算法。系統(tǒng)增廣狀態(tài)向量如下：

(8)

式中，χa的維數(shù)定義為n。具體步驟如下：

1)初始化。

(9)

2)樣點(diǎn)計(jì)算。

(10)

(11)

式中，λ=α2(n+κ)-n；α、β和κ為比例因子。

3)時(shí)間更新。

(12)

(13)

(14)

4)量測(cè)更新。

(15)

(16)

(17)

5)濾波更新。

(18)

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

3.1 仿真條件

以小型飛行器作為載體構(gòu)建仿真系統(tǒng)，載體飛行航跡包含加速、爬升、轉(zhuǎn)彎等各種運(yùn)動(dòng)。載體起飛初始經(jīng)度、緯度、高度分別為29°E、118°N、50 m，初始航向角為90°，飛行時(shí)間為3 600 s，捷聯(lián)解算周期為0.02 s，濾波周期為1 s。濾波初始參數(shù)設(shè)置如下：三維姿態(tài)角誤差均為0.5°，三維速度誤差均為0.1 m/s，三維位置誤差均為5 m。設(shè)定陀螺隨機(jī)游走驅(qū)動(dòng)噪聲及陀螺白噪聲均為0.2°/h，加速度計(jì)隨機(jī)游走驅(qū)動(dòng)噪聲及陀螺白噪聲均為0.5×10-5g。GNSS位置誤差和測(cè)速誤差分別為8 m和0.2 m/s，采樣周期為1 s。在仿真實(shí)驗(yàn)中，式(12)、式(17)中ρ、α、β的初始值分別取為：

根據(jù)GNSS測(cè)量精度設(shè)定及式(2)，正常情況下，GNSS量測(cè)噪聲的均方差可以表示為：

Rroot=sqrtm(R)=

(20)

式中，sqrtm(·)為對(duì)矩陣中每個(gè)元素取開平方的函數(shù)；diag(·)為用于構(gòu)造對(duì)角矩陣的函數(shù)。

在仿真過(guò)程的不同時(shí)段內(nèi)設(shè)定GNSS量測(cè)噪聲的突變與緩變2種情況的均方差：

(21)

3.2 仿真結(jié)果及分析

根據(jù)上述設(shè)定，分別進(jìn)行基于常規(guī)UKF算法及自適應(yīng)UKF算法的GNSS/SINS組合導(dǎo)航系統(tǒng)性能仿真驗(yàn)證。圖1～3分別為位置誤差、速度誤差及姿態(tài)誤差對(duì)比曲線。

圖1 位置誤差Fig.1 Position error

圖2 速度誤差Fig.2 Velocity error

圖3 姿態(tài)誤差Fig.3 Attitude error

由圖1～3可見，在0～600 s內(nèi)，GNSS量測(cè)噪聲的實(shí)際均方差與正常情況完全一致，此時(shí)常規(guī)UKF算法與本文算法得到的各導(dǎo)航參數(shù)誤差完全相同；而在GNSS量測(cè)噪聲的均方差發(fā)生突變與緩變的2個(gè)階段內(nèi)，本文算法得到的各導(dǎo)航參數(shù)誤差明顯小于常規(guī)UKF算法。

為了評(píng)估本文算法對(duì)GNSS量測(cè)噪聲均方差變化的跟蹤能力，繪制本文算法對(duì)GNSS位置量測(cè)噪聲均方差的跟蹤圖(圖4)。由圖可見，本文算法能實(shí)時(shí)跟蹤并反映出GNSS位置量測(cè)噪聲均方差的變化情況，但有一定的滯后性。對(duì)于速度量測(cè)噪聲均方差，本文算法得到的跟蹤圖與圖4相似。

圖4 GNSS位置均方差跟蹤圖Fig.4 Tracking figures of MSE of GNSS position

為了詳細(xì)說(shuō)明GNSS量測(cè)噪聲均方差變化終止后各導(dǎo)航參數(shù)的誤差變化情況，列出不同時(shí)段內(nèi)部分導(dǎo)航參數(shù)的誤差曲線(圖5、6)。

圖5 1 200～2 400 s期間姿態(tài)誤差曲線Fig.5 Attitude error curve from 1 200 s to 2 400 s

由圖5、6可見，本文算法能夠?qū)崟r(shí)跟蹤GNSS量測(cè)噪聲均方差的變化，當(dāng)GNSS量測(cè)噪聲均方差變化終止并回歸正常狀態(tài)后，本文算法得到的各導(dǎo)航參數(shù)也能夠快速收斂至正常狀態(tài)，并最終達(dá)到與常規(guī)UKF算法相同的濾波精度。由圖5的姿態(tài)誤差曲線可見，當(dāng)GNSS量測(cè)噪聲均方差突變終止后，本文算法收斂速度較快。由圖6的速度誤差曲線可見，當(dāng)GNSS量測(cè)噪聲均方差緩變終止后，常規(guī)UKF算法收斂速度較快。綜上所述，整體而言，本文算法的自適應(yīng)性較強(qiáng)且性能高于常規(guī)UKF算法。

圖6 3 000～3 600 s期間速度誤差曲線Fig.6 Velocity error curve from 3 000 s to 3 600 s

4 結(jié) 語(yǔ)

在GNSS/SINS組合導(dǎo)航系統(tǒng)常規(guī)UKF算法的時(shí)間更新及量測(cè)更新過(guò)程中引入變分貝葉斯估計(jì)理論，提出GNSS/SINS組合導(dǎo)航系統(tǒng)的自適應(yīng)UKF算法以提高系統(tǒng)在量測(cè)噪聲統(tǒng)計(jì)特性發(fā)生變化時(shí)的濾波精度。仿真結(jié)果表明，該算法能有效跟蹤量測(cè)噪聲均方差的變化，提高組合導(dǎo)航系統(tǒng)的濾波精度。當(dāng)量測(cè)噪聲均方差變化終止時(shí)，該算法也能快速收斂至正常狀態(tài)。相比于常規(guī)UKF算法，本文算法優(yōu)勢(shì)明顯。

由于本文自適應(yīng)濾波算法涉及到3個(gè)參數(shù)，因此3個(gè)參數(shù)對(duì)量測(cè)噪聲均方差估計(jì)精度的影響規(guī)律是下一步的研究重點(diǎn)。