馬曉杰 林雪原 孫巧妍 陳祥光,2

1 煙臺(tái)南山學(xué)院工學(xué)院，山東省龍口市大學(xué)路12號(hào)，265713 2 北京理工大學(xué)化工與環(huán)境學(xué)院，北京市中關(guān)村南大街5號(hào)，100081

相對(duì)于其他非線性濾波方法，UKF算法因其計(jì)算量小、精度高的特點(diǎn)，被廣泛應(yīng)用于導(dǎo)彈制導(dǎo)、空間探索等各個(gè)領(lǐng)域。但隨著系統(tǒng)維數(shù)和非線性度的增加，其效果會(huì)變差[1]。且UKF算法存在對(duì)濾波器初始值選取比較敏感的問(wèn)題，同時(shí)系統(tǒng)狀態(tài)模型的異常擾動(dòng)也會(huì)對(duì)濾波精度帶來(lái)一定影響[2]。

為此，本文將自適應(yīng)原理與UKF算法相結(jié)合，得到一種改進(jìn)的UKF算法。該算法能夠自行判斷系統(tǒng)是否異常，并利用自適應(yīng)因子自發(fā)調(diào)節(jié)導(dǎo)航解的結(jié)構(gòu)。將改進(jìn)后的UKF算法應(yīng)用于BDS/SINS組合導(dǎo)航，驗(yàn)證其克服常規(guī)UKF濾波算法缺陷的能力。

1 UKF算法

1.1 UKF算法概述

UKF本質(zhì)上也是線性的最小方差估計(jì)，是建立在模型基礎(chǔ)上的算法。標(biāo)準(zhǔn)卡爾曼濾波需保證系統(tǒng)的狀態(tài)方程與量測(cè)方程均為線性，而UKF算法在計(jì)算最佳增益陣的過(guò)程中，對(duì)量測(cè)方程以及狀態(tài)方程的性質(zhì)沒有任何具體的約束，所以其既可以在線性條件下使用，也可以在非線性條件下使用[2]。

設(shè)系統(tǒng)狀態(tài)方程和量測(cè)方程為：

(1)

式中，k-1、k為離散時(shí)刻；Xk-1為上一時(shí)刻的狀態(tài)估計(jì)，其方差陣為Pk-1；uk-1為系統(tǒng)確定性控制項(xiàng)；Wk-1為系統(tǒng)噪聲，其方差陣為Qk-1；Zk為量測(cè)值；Vk為量測(cè)噪聲，其方差陣為Rk。系統(tǒng)和量測(cè)噪聲均服從高斯分布。UKF算法的任務(wù)是在已知狀態(tài)初始值X0、系統(tǒng)確定性控制序列u0,u1,…,uk-1以及量測(cè)值序列Z1,Z2,…,Zk的情況下，估計(jì)系統(tǒng)的狀態(tài)變量Xk。

UKF算法采用遞推計(jì)算方式，包括2個(gè)過(guò)程：狀態(tài)預(yù)測(cè)和修正計(jì)算。已知狀態(tài)變量的維數(shù)為n，UKF算法用UT變換的方式獲取2n+1個(gè)采樣點(diǎn)(sigma點(diǎn))實(shí)現(xiàn)狀態(tài)及其相關(guān)統(tǒng)計(jì)量的遞推計(jì)算[3]。UKF算法具體計(jì)算步驟如下：

1)初始化。狀態(tài)向量初始化為X0，初始化方差陣Pa，初始化UKF算法比例因子α、β、κ。

(3)

(4)

4)量測(cè)更新。在量測(cè)時(shí)刻，根據(jù)系統(tǒng)量測(cè)方程求取量測(cè)一步預(yù)測(cè)的傳遞值：

(5)

計(jì)算量測(cè)均值和方差：

(6)

(7)

5)計(jì)算UKF增益，更新狀態(tài)向量和方差。獲得測(cè)量值及預(yù)測(cè)值后，進(jìn)行新息計(jì)算，通過(guò)濾波增益矩陣，得到每個(gè)狀態(tài)對(duì)應(yīng)的誤差修正量。UKF中新息及濾波增益的計(jì)算都涉及到狀態(tài)預(yù)測(cè)點(diǎn)集、測(cè)量預(yù)測(cè)點(diǎn)集均值和方差的計(jì)算。

計(jì)算狀態(tài)預(yù)測(cè)與量測(cè)估計(jì)的協(xié)方差：

(8)

計(jì)算濾波增益：

Kk=Pxz(Pzz)-1

(9)

利用式(9)，獲得各個(gè)狀態(tài)的誤差修正量，狀態(tài)和方差更新為：

(10)

(11)

6)循環(huán)跳轉(zhuǎn)。k=k+1，返回步驟2)，進(jìn)行下一次循環(huán)，直至循環(huán)結(jié)束。

1.2 UKF算法存在的問(wèn)題

2 改進(jìn)的UKF濾波算法

引入自適應(yīng)因子ω，將UKF算法計(jì)算步驟中的式(4)、式(7)和式(8)改寫為：

(12)

(13)

(14)

自適應(yīng)因子ω的作用是通過(guò)判斷系統(tǒng)正常與否，在出現(xiàn)異常時(shí)能夠自發(fā)調(diào)節(jié)狀態(tài)預(yù)測(cè)信息和量測(cè)信息在濾波解中的權(quán)值比重，從而降低異常對(duì)濾波造成的影響。ω的取值如式(15)所示，式中的協(xié)方差矩陣Pzz按照改寫前的式(7)取值：

(15)

由改進(jìn)的UKF算法流程分析自適應(yīng)因子ω的作用如下：1)當(dāng)選取的初始值不存在偏差并且系統(tǒng)狀態(tài)模型正常時(shí)，自適應(yīng)因子ω將等于1，系統(tǒng)按照正常的UKF算法進(jìn)行濾波；2)當(dāng)選取的初始值存在偏差或者系統(tǒng)狀態(tài)模型存在異常擾動(dòng)時(shí)，自適應(yīng)因子ω將小于1，狀態(tài)預(yù)測(cè)對(duì)最終濾波解的貢獻(xiàn)變?。?)當(dāng)系統(tǒng)狀態(tài)模型擾動(dòng)明顯異常時(shí)，ω將接近0，系統(tǒng)狀態(tài)模型信息幾乎被完全棄用。

3 BDS/SINS組合導(dǎo)航模型

組合導(dǎo)航模型采用直接式濾波，以SINS機(jī)械編排方程為狀態(tài)方程，使用導(dǎo)航參數(shù)作為被估狀態(tài)，狀態(tài)方程及量測(cè)方程無(wú)需進(jìn)行線性化處理。

3.1 狀態(tài)方程

采用ENU地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，慣性導(dǎo)航系統(tǒng)采用指北方位機(jī)械編排，狀態(tài)方程為：

(16)

(17)

濾波過(guò)程中，噪聲進(jìn)入非線性系統(tǒng)，參與狀態(tài)轉(zhuǎn)移，將系統(tǒng)噪聲和量測(cè)噪聲與狀態(tài)向量一起組成增廣向量Xa，維數(shù)n為21，增廣的方差陣為Pa：

(18)

3.2 量測(cè)方程

以BDS輸出的速度和位置作為觀測(cè)向量，假設(shè)V為量測(cè)噪聲，則量測(cè)方程為：

(19)

4 仿真驗(yàn)證

以微小型飛行器作為載體構(gòu)建仿真驗(yàn)證系統(tǒng)，飛行軌跡過(guò)程包括加速、爬升、平直飛行、轉(zhuǎn)彎、下滑、盤旋等機(jī)動(dòng)過(guò)程，仿真時(shí)間為3 600 s；飛行器的初始姿態(tài)設(shè)定為：飛行器水平，航向?yàn)?0°，位置(118°，29°，50 m)。

根據(jù)目前在用的慣性器件性能，設(shè)定SINS參數(shù)和BDS測(cè)量精度，如表1所示。

濾波初始參數(shù)設(shè)置為：三維姿態(tài)角誤差均為0.5°/s，三維速度誤差均為0.1 m/s，三維位置誤差均為5 m。改進(jìn)的UKF算法的相關(guān)濾波初始參數(shù)設(shè)置為：初始比例因子α=1，β=2，κ=0。

為便于濾波性能分析，在相同仿真條件下對(duì)相同的仿真原始數(shù)據(jù)分別使用UKF算法和改進(jìn)的UKF算法，基于2種濾波方法的組合導(dǎo)航系統(tǒng)的各導(dǎo)航參數(shù)誤差曲線如圖1、2所示(由于篇幅限制，此處僅給出位置誤差曲線)。

表1 SINS參數(shù)和BDS測(cè)量精度

圖1 基于UKF算法的位置誤差曲線Fig.1 Position error curves based on UKF algorithm

圖2 基于改進(jìn)的UKF算法的位置誤差曲線Fig.2 Position error curves based on improved UKF algorithm

將初始誤差擴(kuò)大為上述仿真條件下初始誤差的5倍，其他仿真參數(shù)不變，再次采用2種算法分別仿真，并進(jìn)行分析與對(duì)比，其目的有2個(gè)：1) 通過(guò)改進(jìn)的UKF算法的2次仿真可以較好地看出改進(jìn)的UKF算法克服濾波初始誤差的能力；2) 通過(guò)UKF算法的2次仿真可以看出濾波初始誤差對(duì)UKF算法濾波精度的影響程度。2種濾波方法在5倍初始誤差下的誤差曲線圖如圖3、4所示(同樣，由于篇幅限制，此處僅給出位置誤差曲線)。

圖3 基于UKF算法的位置誤差曲線Fig.3 Position error curves based on UKF algorithm

圖4 基于改進(jìn)的UKF算法的誤差曲線Fig.4 Position error curves based on improved UKF algorithm

為了更加清楚地表達(dá)上述仿真分析，表2列出了組合導(dǎo)航系統(tǒng)各導(dǎo)航參數(shù)誤差在上述幾種情況下的具體數(shù)值。

從曲線圖及表2可以看出：

1)對(duì)比單倍濾波器初始誤差條件下2種算法的整個(gè)濾波過(guò)程可見，在許多時(shí)間點(diǎn)(段)，改進(jìn)的UKF算法能夠不同程度地降低各導(dǎo)航參數(shù)的誤差波動(dòng)幅度，說(shuō)明改進(jìn)的UKF算法在抑制系統(tǒng)狀態(tài)模型異常擾動(dòng)方面也發(fā)揮了一定作用。

2)在單倍濾波器初始誤差條件下，相對(duì)于UKF算法，在濾波初始階段，改進(jìn)的UKF算法對(duì)應(yīng)的多個(gè)導(dǎo)航參數(shù)誤差波動(dòng)幅度明顯變??；而在5倍濾波器初始誤差條件下，相對(duì)于UKF算法，在濾波初始階段，改進(jìn)的UKF算法對(duì)應(yīng)的導(dǎo)航參數(shù)誤差波動(dòng)幅度均明顯變小，這說(shuō)明改進(jìn)的UKF算法能夠較好地抑制初始值誤差帶來(lái)的影響。

3)在單倍及5倍濾波器初始誤差條件下，基于改進(jìn)的UKF算法的組合導(dǎo)航參數(shù)誤差的均方值、最大值、最小值均分別小于基于UKF算法的組合導(dǎo)航參數(shù)誤差的均方值、最大值、最小值，進(jìn)而說(shuō)明相對(duì)于UKF算法，改進(jìn)的UKF算法性能得到明顯提升。

4)對(duì)比單倍和5倍濾波器初始誤差條件下改進(jìn)的UKF算法，姿態(tài)、速度、位置誤差在最值、均值、均方值等方面沒有明顯變化，僅在數(shù)值上有小幅波動(dòng)。說(shuō)明隨著初始誤差的增加，基于改進(jìn)的UKF算法的組合導(dǎo)航各參數(shù)的誤差變化較小，說(shuō)明改進(jìn)的UKF算法在抑制初始誤差擾動(dòng)方面性能較好。

以上分析表明，將自適應(yīng)估計(jì)原理引入U(xiǎn)KF算法中，能有效克服UKF算法存在的2個(gè)方面的問(wèn)題，可明顯提高導(dǎo)航精度，抑制濾波器初始誤差對(duì)系統(tǒng)性能的影響。

表2 各導(dǎo)航參數(shù)誤差比較

5 結(jié) 語(yǔ)

文本研究了UKF濾波算法，將自適應(yīng)估計(jì)原理與UKF算法相結(jié)合，得到一種改進(jìn)的UKF算法。構(gòu)建了BDS/SINS組合導(dǎo)航仿真驗(yàn)證系統(tǒng)，仿真結(jié)果驗(yàn)證了該改進(jìn)算法的有效性。將改進(jìn)的UKF濾波算法仿真曲線、數(shù)據(jù)與UKF算法的仿真曲線、數(shù)據(jù)進(jìn)行對(duì)比發(fā)現(xiàn)，改進(jìn)后的算法性能得到提升。