KF/UPF在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用

王 健,張 濤,童金武,顏亞雄

(1.東南大學(xué)儀器科學(xué)與工程學(xué)院,江蘇 南京 210096;2.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 210096)

0 引 言

對(duì)準(zhǔn)精度和實(shí)時(shí)性是慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)的兩項(xiàng)重要技術(shù)指標(biāo)。初始對(duì)準(zhǔn)精度影響捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system,SINS)的性能,初始對(duì)準(zhǔn)實(shí)時(shí)性標(biāo)志著系統(tǒng)的快速反應(yīng)能力[1]。針對(duì)捷聯(lián)慣導(dǎo)大失準(zhǔn)角初始對(duì)準(zhǔn)非線性、非高斯噪聲的特點(diǎn),需采用高斯近似的非線性濾波器進(jìn)行處理。非線性濾波是基于貝葉斯理論推導(dǎo)次優(yōu)濾波器,采用高斯假設(shè)的方法解決解析求解中出現(xiàn)的多維加權(quán)積分的問題,例如,擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)、無跡卡爾曼濾波(unscented Kalman filter,UKF)、容積卡爾曼濾波(cubature Kalman filter,CKF)等等。粒子濾波 (particle filter,PF)則是基于隨機(jī)采樣的方法進(jìn)行解析求解,其不受模型線性及高斯假設(shè)的限制,相比EKF、UKF和CKF,PF是非高斯非線性系統(tǒng)狀態(tài)估計(jì)的最優(yōu)濾波器[2-3]。

針對(duì)粒子濾波存在粒子退化、粒子易貧化、計(jì)算量大等缺陷,已有很多學(xué)者將無跡粒子濾波(unscented particle filter,UPF)方法成功地應(yīng)用到大失準(zhǔn)角初始對(duì)準(zhǔn)中[4-7],其實(shí)質(zhì)就是基于最新觀測(cè)值,通過UKF計(jì)算后驗(yàn)分布的均值和方差來實(shí)現(xiàn)對(duì)狀態(tài)向量后驗(yàn)概率密度近似,結(jié)合重采樣方法可以有效地緩解粒子的退化問題,提高初始對(duì)準(zhǔn)的精度和收斂速度。但是,在大方位失準(zhǔn)角下,SINS初始對(duì)準(zhǔn)非線性模型的狀態(tài)維數(shù)比較高,采用UPF算法需要較多的粒子數(shù)才能更好地模擬后驗(yàn)概率密度,隨著粒子數(shù)增多,計(jì)算量急劇增加使其實(shí)時(shí)性嚴(yán)重降低。為減小粒子濾波的計(jì)算量,目前研究主要集中在兩個(gè)方面:一是對(duì)構(gòu)建動(dòng)態(tài)粒子的研究,構(gòu)建動(dòng)態(tài)粒子數(shù)的方法是在線實(shí)時(shí)調(diào)整濾波器所需的粒子數(shù),在不損失濾波精度的前提下,盡可能地減少粒子數(shù)。文獻(xiàn)[8]以后驗(yàn)估計(jì)精度為依據(jù),提出了在重采樣過程實(shí)現(xiàn)KL散度(Kullback-Leibler divergence,KLD)自適應(yīng)采樣方法,比在采樣過程更為有效和方便。文獻(xiàn)[9]基于特設(shè)閾值,將粒子數(shù)控制在一定硬性范圍內(nèi)。結(jié)合文獻(xiàn)[8]和文獻(xiàn)[9]的想法,文獻(xiàn)[10]通過設(shè)置閾值和檢測(cè)PF后驗(yàn)分布的收斂性,對(duì)濾波器所需的粒子數(shù)進(jìn)行在線實(shí)時(shí)調(diào)整,但增加了算法的復(fù)雜性。二是對(duì)降低狀態(tài)空間維度的研究,一種行之有效的解決思路即是對(duì)狀態(tài)空間進(jìn)行邊緣化[11]。

本文從降低狀態(tài)空間維度角度出發(fā),基于簡化理論Rao-Blackwellization定理[12],將非線性初始對(duì)準(zhǔn)模型分解為線性與非線性兩部分,分別采用KF和UPF對(duì)系統(tǒng)線性和非線性部分的狀態(tài)進(jìn)行估計(jì)。需要進(jìn)行UPF濾波的狀態(tài)維數(shù)由原來的10 維降為3 維,在保證對(duì)準(zhǔn)精度的同時(shí)增強(qiáng)了濾波算法的實(shí)時(shí)性。

1 大方位失準(zhǔn)角捷聯(lián)慣導(dǎo)系統(tǒng)非線性誤差模型

1.1 SINS大方位失準(zhǔn)角靜基座非線性誤差模型

在初始對(duì)準(zhǔn)中,速度誤差和失準(zhǔn)角誤差的狀態(tài)方程分別為[13-14]

(1)

(2)

(4)

(5)

1.2 SINS大方位失準(zhǔn)角靜基座初始對(duì)準(zhǔn)的線性/非線性濾波模型[15]

1.2.1 線性部分的濾波模型

將式(5)展開,可得線性部分的狀態(tài)方程為

(6)

(7)

1.2.2 非線性部分的濾波模型

由式(5)可得非線性部分的狀態(tài)方程為

(8)

為了求取非線性部分的觀測(cè)方程及其觀測(cè)量,將式(6)東向、北向速度誤差的微分方程離散化，即

(9)

(10)

(11)

(12)

結(jié)合式(8)和式(11),非線性部分的狀態(tài)方程和量測(cè)方程為

(13)

2 KF/UPF組合濾波算法設(shè)計(jì)

KF/UPF組合濾波具體的步驟如下:

步驟1濾波初始化

(14)

(15)

下面的步驟按照k=1,2,…,N執(zhí)行。

圖1 KF/UPF組合濾波的流程圖Fig.1 KF/UPF integrated filter flow chart

步驟2采用KF算法估計(jì)

(1) 卡爾曼濾波的時(shí)間更新為

(16)

(2) 卡爾曼濾波的量測(cè)更新為

(17)

步驟3采用UPF算法估計(jì)

步驟3.1UKF算法

(18)

步驟3.2選取重要推薦概率密度函數(shù)為

(19)

步驟3.3計(jì)算粒子的權(quán)重系數(shù)為

(20)

步驟3.4權(quán)值歸一化為

(21)

步驟3.5計(jì)算有效粒子的尺寸為

(22)

當(dāng)Neff小于閾值Nth時(shí),為解決粒子存在的退化問題,按照順序執(zhí)行步驟3.6,否則直接跳轉(zhuǎn)到步驟3.7進(jìn)行初始對(duì)準(zhǔn)狀態(tài)估計(jì)。

步驟3.6重采樣

步驟3.7初始對(duì)準(zhǔn)狀態(tài)估計(jì)，即

第k時(shí)刻初始對(duì)準(zhǔn)狀態(tài)估計(jì)值和估計(jì)誤差方差陣分別為

(23)

步驟4再次采用KF算法估計(jì)

步驟5令k=k+1,返回到步驟3,循環(huán)運(yùn)算,得到SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)過程中各個(gè)時(shí)刻失準(zhǔn)角的估計(jì)值。

3 試驗(yàn)驗(yàn)證與討論

圖2～圖4為UPF算法和KF/UPF組合濾波算法失準(zhǔn)角的誤差曲線。

圖2 俯仰角誤差曲線Fig.2 Error curves of pitch angle

圖3 橫滾角誤差曲線Fig.3 Error curves of roll angle

圖4 航向角誤差曲線Fig.4 Error curves of azimuth angle

由圖2～圖4可知,兩種濾波算法下姿態(tài)角誤差曲線的收斂性幾乎一致,水平失準(zhǔn)角誤差曲線很快收斂到穩(wěn)定值。由圖4還可知,本文設(shè)計(jì)的KF/UPF組合濾波算法下航向角誤差曲線較UPF算法提前收斂。為了定量地對(duì)對(duì)準(zhǔn)結(jié)果進(jìn)行分析,分別對(duì)第400～600 s各失準(zhǔn)角誤差的均值進(jìn)行統(tǒng)計(jì)。由于仿真時(shí)隨機(jī)粒子和白噪聲特性不同等原因,每次的穩(wěn)態(tài)精度略有差異,通過對(duì)系統(tǒng)進(jìn)行20次蒙特卡羅仿真取均值,其結(jié)果如表1 所示。

表1 失準(zhǔn)角誤差的比較Table 1 Comparison of misalignment angle errors

由表1可知,這兩種算法狀態(tài)估計(jì)的精度相當(dāng),均接近由慣性器件性能所決定的理論極限精度。但是,本文設(shè)計(jì)的KF/UPF組合濾波算法需要進(jìn)行UPF濾波的狀態(tài)維數(shù)由原來的10 維降為3 維,與UPF算法相比,可以顯著地縮短算法的運(yùn)算時(shí)間。粒子數(shù)同時(shí)取1 000時(shí),UPF算法平均運(yùn)算的時(shí)間為947.18 s,而KF/UPF組合濾波算法運(yùn)算的時(shí)間僅為499.05 s,時(shí)間縮短至原來的52.69%。因此,KF/UPF組合濾波和UPF濾波算法精度相當(dāng),但在一定程度上解決了UPF算法計(jì)算負(fù)荷太大導(dǎo)致對(duì)準(zhǔn)時(shí)間較長的問題。

為了體現(xiàn)KF/UPF在計(jì)算效率上的優(yōu)勢(shì),取KF/UPF組合濾波的粒子數(shù)為100,UPF算法仍然取粒子數(shù)為1 000,將兩者算法進(jìn)行比較。由于水平失準(zhǔn)角相對(duì)于方位失準(zhǔn)角很快收斂到穩(wěn)態(tài)值,兩種濾波算法下其收斂時(shí)間和濾波精度一致。因此,對(duì)計(jì)算結(jié)果只列出方位失準(zhǔn)角估計(jì)誤差曲線,如圖5所示。由圖5可知,KF/UPF組合濾波算法與UPF算法下航向角誤差均能收斂到穩(wěn)定值。同樣取第400 s到600 s的算術(shù)平均值進(jìn)行比較,其俯仰角的穩(wěn)態(tài)誤差為-0.176 8′,滾動(dòng)角誤差的穩(wěn)態(tài)精度為0.203 7′,航向角誤差的穩(wěn)態(tài)精度為-2.895 7′,接近由慣性器件性能所決定的理論極限精度。此時(shí),KF/UPF組合濾波算法下平均運(yùn)算的時(shí)間為56.83 s,與UPF算法(1 000個(gè)粒子數(shù))相比,時(shí)間縮短至原來的6.0%。

圖5 航向失準(zhǔn)角誤差曲線Fig.5 Curves of azimuth misalignment angle error

為了更直觀對(duì)初始對(duì)準(zhǔn)過程中的計(jì)算效率進(jìn)行比較,取粒子數(shù)為1 000時(shí)UPF算法與KF/UPF組合算法分別為模式1和模式2,取粒子數(shù)為100下KF/UPF組合算法為模式3,其運(yùn)算時(shí)間如圖6所示。

圖6 運(yùn)算時(shí)間的對(duì)比圖Fig.6 Comparison of the computation time

由圖6可知,對(duì)比UPF算法,KF/UPF組合算法顯著地提高了運(yùn)算速度。

為了驗(yàn)證算法的有效性,在三軸轉(zhuǎn)臺(tái)上進(jìn)行靜基座對(duì)準(zhǔn)的實(shí)驗(yàn),IMU采用的是航天三院三十三所FOSN,FOSN的技術(shù)手冊(cè)上主要的數(shù)據(jù)參數(shù)為:陀螺的常值漂移為0.006°/h,加速度計(jì)的常值偏置為5×10-5g,產(chǎn)品由于時(shí)間和環(huán)境因素,加上安裝誤差等的影響,實(shí)際的參數(shù)會(huì)有所偏差,需要進(jìn)一步標(biāo)定確定,而本文主要是用半物理仿真數(shù)據(jù)來驗(yàn)證算法有效性,仿真的結(jié)果如圖7～圖9所示。為了定量地對(duì)對(duì)準(zhǔn)結(jié)果進(jìn)行分析,分別對(duì)第400 s到600 s各失準(zhǔn)角誤差的均值進(jìn)行統(tǒng)計(jì),如表2所示。

圖7 東向失準(zhǔn)角誤差Fig.7 East misalignment angle error

圖8 北向失準(zhǔn)角誤差Fig.8 North misalignment angle error

圖9 方位失準(zhǔn)角誤差Fig.9 Azimuth misalignment angle error

表2 對(duì)準(zhǔn)結(jié)果比較Table 2 Comparison of the alignment results

由圖7～圖9失準(zhǔn)角的誤差曲線和表2統(tǒng)計(jì)結(jié)果可知,在半物理仿真下,KF/UPF組合濾波和UPF算法對(duì)準(zhǔn)精度相當(dāng),例如,對(duì)于重要的方位失準(zhǔn)角誤差,兩種濾波算法對(duì)準(zhǔn)精度相差僅0.07角分左右。因此,KF/UPF組合濾波雖然進(jìn)行了降維處理,卻綜合了卡爾曼濾波對(duì)線性系統(tǒng)和UPF對(duì)非線性系統(tǒng)最優(yōu)估計(jì)的優(yōu)勢(shì),是整體效果最優(yōu)的濾波,保證了對(duì)濾波模型的最優(yōu)估計(jì),同時(shí),解決了UPF算法計(jì)算負(fù)荷太大的問題。

4 結(jié) 論

為解決UPF應(yīng)用于捷聯(lián)慣導(dǎo)非線性初始對(duì)準(zhǔn)中計(jì)算量過大的問題,本文基于靜基座對(duì)準(zhǔn)誤差模型的特點(diǎn),推導(dǎo)了KF/UPF組合濾波算法。該方法綜合了KF對(duì)線性系統(tǒng)和UPF對(duì)非線性系統(tǒng)最優(yōu)估計(jì)的優(yōu)勢(shì),降低了需要進(jìn)行非線性濾波的維數(shù)。仿真結(jié)果表明,KF/UPF組合濾波算法在保證初始對(duì)準(zhǔn)估計(jì)精度和收斂速度的同時(shí),減少了計(jì)算量,提高了運(yùn)算速度,增強(qiáng)了濾波算法的實(shí)時(shí)性。