GNSS/ SINS組合導航系統(tǒng)的自適應UKF算法

荊 蕾 孫煒瑋 喬玉新 劉成銘

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

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

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

1 組合導航非線性濾波模型

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

(1)

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

(2)

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

2 組合導航自適應UKF算法

2.1 變分貝葉斯原理

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

預測階段：

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ù)值積分，因此在實際情況中很難求解。而變分貝葉斯原理可以采用近似的方法，利用多個已知分布近似求解后驗分布。根據(jù)變分貝葉斯原理，式(3)、(4)中的量測方差與聯(lián)合預測和后驗分布可轉(zhuǎn)換為如下的逆Gamma分布和高斯分布：

(5)

(6)

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

(7)

式中，αk-1|k-1,i和βk-1|k-1,i為逆Gamma分布的2個參數(shù)，詳細推導過程見文獻[10]。

2.2 自適應UKF算法

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

(8)

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

1)初始化。

(9)

2)樣點計算。

(10)

(11)

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

3)時間更新。

(12)

(13)

(14)

4)量測更新。

(15)

(16)

(17)

5)濾波更新。

(18)

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

3.1 仿真條件

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

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

Rroot=sqrtm(R)=

(20)

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

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

(21)

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

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

圖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量測噪聲的實際均方差與正常情況完全一致，此時常規(guī)UKF算法與本文算法得到的各導航參數(shù)誤差完全相同；而在GNSS量測噪聲的均方差發(fā)生突變與緩變的2個階段內(nèi)，本文算法得到的各導航參數(shù)誤差明顯小于常規(guī)UKF算法。

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

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

為了詳細說明GNSS量測噪聲均方差變化終止后各導航參數(shù)的誤差變化情況，列出不同時段內(nèi)部分導航參數(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跟蹤GNSS量測噪聲均方差的變化，當GNSS量測噪聲均方差變化終止并回歸正常狀態(tài)后，本文算法得到的各導航參數(shù)也能夠快速收斂至正常狀態(tài)，并最終達到與常規(guī)UKF算法相同的濾波精度。由圖5的姿態(tài)誤差曲線可見，當GNSS量測噪聲均方差突變終止后，本文算法收斂速度較快。由圖6的速度誤差曲線可見，當GNSS量測噪聲均方差緩變終止后，常規(guī)UKF算法收斂速度較快。綜上所述，整體而言，本文算法的自適應性較強且性能高于常規(guī)UKF算法。

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

4 結(jié) 語

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

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