一種改進(jìn)的自適應(yīng)USQUE組合導(dǎo)航姿態(tài)估計(jì)方法

呂 旭， 胡柏青， 徐大偉， 李開龍， 趙 濤

(1.中國人民解放軍海軍工程大學(xué)電氣工程學(xué)院， 武漢 430033； 2.錦州航星集團(tuán)錦州航星船舶科技有限公司， 錦州 121000; 3.海裝沈陽局駐錦州地區(qū)軍代表室， 錦州 121000)

捷聯(lián)慣性導(dǎo)航系統(tǒng)是推算導(dǎo)航系統(tǒng)的范例，該系統(tǒng)既可以單獨(dú)工作，也可以作為組合導(dǎo)航的一部分進(jìn)行使用。捷聯(lián)慣性導(dǎo)航系統(tǒng)/全球衛(wèi)星導(dǎo)航系統(tǒng)(strapdown inertial navigation system / global navigation satellite system，SINS/GNSS)的組合導(dǎo)航方式具有輸出連續(xù)、高可靠性，定位精確等優(yōu)點(diǎn)，是一組較理想且被廣泛應(yīng)用的組合導(dǎo)航模式[1-2]。組合導(dǎo)航系統(tǒng)進(jìn)行信息融合過程中，濾波方法的優(yōu)劣是導(dǎo)航性能的關(guān)鍵技術(shù)體現(xiàn)[3-4]。卡爾曼濾波(Kalman filter，KF)算法既適用于平穩(wěn)隨機(jī)過程，也是適應(yīng)于非平穩(wěn)過程，一經(jīng)提出便得到廣泛應(yīng)用[5-7]。但是經(jīng)典的KF只能解決線性狀態(tài)估計(jì)問題。無味卡爾曼濾波(unscented Kalman filter，UKF)[8]在非線性系統(tǒng)中具有較高的估計(jì)精度和穩(wěn)定性，成為研究熱點(diǎn)。捷聯(lián)慣性基組合導(dǎo)航在實(shí)際應(yīng)用中，估計(jì)精度和實(shí)時(shí)性都是需要考慮的問題。UKF的計(jì)算復(fù)雜度和較高的精度使其廣泛應(yīng)用于導(dǎo)航的非線性濾波系統(tǒng)中，這是其成為研究熱點(diǎn)的一個(gè)重要原因。Crassidis[9]首次提出了一種基于四元數(shù)的航天器姿態(tài)估計(jì)UKF，稱為四元數(shù)無味估計(jì)器(unscented quaternion estimator，USQUE)。在文獻(xiàn)[10-11]中，對(duì)四元數(shù)表示姿態(tài)的USQUE算法進(jìn)行了改進(jìn)，降低了USQUE算法的計(jì)算復(fù)雜度。將該方法應(yīng)用于初始對(duì)準(zhǔn)和SINS/GNSS組合導(dǎo)航等。

在應(yīng)用 USQUE 進(jìn)行組合導(dǎo)航姿態(tài)估計(jì)時(shí)，雖然USQUE實(shí)現(xiàn)簡單，操作方便，但無法克服量測噪聲統(tǒng)計(jì)量不準(zhǔn)確帶來的濾波精度下降問題。文獻(xiàn)[12]中提及一種將新息滑動(dòng)窗的(innovation-based adaptive estimation，IAE)擴(kuò)展到USQUE姿態(tài)估計(jì)器，IAE通過對(duì)滑動(dòng)窗內(nèi)新息的統(tǒng)計(jì)實(shí)現(xiàn)自調(diào)節(jié)，但是在方法滑動(dòng)窗中新息量越大，算法估計(jì)精度越高，同時(shí)也增加了復(fù)雜度，且對(duì)于量測噪聲方差陣內(nèi)獨(dú)立變化的噪聲不能單獨(dú)調(diào)節(jié)，該算法計(jì)算量大，實(shí)現(xiàn)起來較為困難。在USQUE 算法的基礎(chǔ)上，采用變分貝葉斯(variational Bayesian，VB)近似估計(jì)器，對(duì) USQUE 算法進(jìn)行改進(jìn)，采用高斯迭代過程近似估計(jì)當(dāng)前時(shí)刻量測噪聲協(xié)方差矩陣，設(shè)計(jì)了基于變分貝葉斯的自適應(yīng)四元數(shù)無味卡爾曼濾波器(variational Bayesian adaptive USQUE，VB-AUSQUE)的模型。

綜上所述，針對(duì)組合導(dǎo)航系統(tǒng)受外界環(huán)境干擾，造成量測噪聲未知及時(shí)變的問題，將嚴(yán)重影響USQUE方法的姿態(tài)估計(jì)性能。現(xiàn)利用VB對(duì) USQUE 中未知或時(shí)變的量測噪聲進(jìn)行近似，通過高斯迭代進(jìn)行校準(zhǔn)，將近似結(jié)果重新輸入到濾波器模型中，改善姿態(tài)估計(jì)精度。通過捷聯(lián)式慣性導(dǎo)航系統(tǒng)/全球定位系統(tǒng)(SINS / GNSS)組合導(dǎo)航仿真與艦載實(shí)驗(yàn)，并驗(yàn)證所提算法的有效性。

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

采用直接式松組合SINS/GNSS系統(tǒng)。使用GNSS位置和速度作為組合算法的量測輸入，與SINS校正類型或GNSS輔助無關(guān)。建立系統(tǒng)方程和量測方程，其中姿態(tài)部分為四元數(shù)表示。

(1)

(2)

對(duì)于中低精的慣導(dǎo)設(shè)備，陀螺儀漂移和加速度計(jì)零也是系統(tǒng)狀態(tài)估計(jì)中的主要元素。其誤差方程分別表示為

(3)

(4)

量測模型采用速度和位置松組合模式，得到線性離散量測方程，即

(5)

式(5)中：ηvk和ηpk分別是零均值速度和位置高斯白噪聲。

2 變分貝葉斯四元數(shù)無味估計(jì)器

2.1 變分貝葉斯原理

變分貝葉斯(variational Bayesian，VB)估計(jì)算法是一種以迭代的方式對(duì)后驗(yàn)分布進(jìn)行優(yōu)化估計(jì)的方法，算法的核心思想是將復(fù)雜的聯(lián)合條件概率密度分解為幾個(gè)可以求解的獨(dú)立參數(shù)的概率密度乘積的形式[13]。文獻(xiàn)[14]中，開發(fā)了一種基于VB近似的自適應(yīng)擴(kuò)展卡爾曼濾波器(variational Bayesian adaptive extended Kalman filter, VBAEKF)，用于測量噪聲的時(shí)變自適應(yīng)估計(jì)。對(duì)于處理量測噪聲協(xié)方差矩陣Rk統(tǒng)計(jì)特性不確定的問題，本質(zhì)就是在量測量為y1:k-1的條件概率密度P(xk,Rk|y1:k)的求解問題。在量測噪聲協(xié)方差矩陣Rk統(tǒng)計(jì)特性未知或者時(shí)變的情況下，無法直接對(duì)P(xk,Rk|y1:k)求解。VB估計(jì)算法可以將這種復(fù)雜的概率密度函數(shù)轉(zhuǎn)換為對(duì)濾波狀態(tài)量的概率密度分布與量測噪聲概率密度分布乘積的形式，即

P(xk,Rk|y1:k)≈?x(xk)?R(Rk)

(6)

式(6)中:?x(xk)為濾波狀態(tài)量的概率密度分布， ?R(Rk)為量測噪聲的概率密度分布。兩者滿足關(guān)系

(7)

依據(jù)最小化后驗(yàn)概率密度分布與真實(shí)概率密度分布之間的Kullback-Leibler(KL)散度距離的原則，可以將式(6)近似分解為一個(gè)高斯概率密度分布和一個(gè)Inverse-Wishart(IW)概率密度分布的乘積的形式，即

(8)

式(8)中：tr(·)表示矩陣的跡；mk和Mk是IW概率密度分布的兩個(gè)參數(shù)，由此可見IW中包含了量測噪聲協(xié)方差Rk，因此在Rk不確定或者時(shí)變的情況下，使用VB估計(jì)可以自適應(yīng)的估計(jì)Rk陣。

2.2 改進(jìn)的USQUE姿態(tài)估計(jì)器

USQUE算法將濾波分成內(nèi)外兩層，內(nèi)層采用誤差修正羅德里格斯參數(shù)(modified Rodrigues parameter, MRP)傳遞采樣點(diǎn)，外層利用四元數(shù)完成姿態(tài)更新，有效解決了歸一化約束問題[15]。其算法的狀態(tài)方程為

(9)

式(9)中:Xk∈Rn和Yk∈Rm；狀態(tài)方程為f(·)；Wk-1和Vk分別為高斯白噪聲，即

(10)

為簡化說明，下文僅對(duì)改進(jìn)自適應(yīng)USQUE算法的關(guān)鍵姿態(tài)估計(jì)流程進(jìn)行介紹。

2.2.1 時(shí)間更新

1)產(chǎn)生Sigma點(diǎn)

Pk-1/k-1),i=0,1,…,2n

(11)

2)外層姿態(tài)更新

(12)

(13)

3)內(nèi)層姿態(tài)遞推

(14)

(15)

狀態(tài)估計(jì)及其濾波方差表達(dá)式為

(16)

(17)

式中：ωi是Sigma點(diǎn)的權(quán)值。

計(jì)算概率密度分布參數(shù)mk和Mk：

mk/k-1=ρ(mk-1-n-1)+n+1

(18)

Mk/k-1=BMk-1BT

(19)

2.2.2 量測更新

量測更新使用迭代濾波框架。

初始設(shè)置為：

和mk=1+mk/k-1。

對(duì)于j=1:N，迭代以下內(nèi)容(N表示迭代次數(shù))。

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

當(dāng)j=N時(shí)，迭代結(jié)束，N設(shè)置為

2.2.3 姿態(tài)更新

(30)

3 仿真與艦載測試

3.1 仿真實(shí)驗(yàn)

為驗(yàn)證算法有效性，利用軌跡發(fā)生器模擬飛行器大機(jī)動(dòng)運(yùn)動(dòng)，其軌跡如圖1所示，包含勻速、加速、爬升、轉(zhuǎn)彎、滾轉(zhuǎn)等狀態(tài)，圖2和圖3分別給出載體姿態(tài)和速度變化。軌跡初始地理位置參數(shù)為東經(jīng)108.91°、北緯34.25°和高度1 000 m，初始姿態(tài)角為0°，初始速度為0，仿真步長為0.01 s，總時(shí)長為800 s。

圖1 軌跡仿真Fig.1 Simulation trajectory

圖2 姿態(tài)變化Fig.2 The change of attitude

圖3 載體速度變化Fig.3 The change of velocity

圖4 姿態(tài)估計(jì)誤差對(duì)比Fig.4 Comparison on attitude estimate errors

由圖4中可知，對(duì)于中低精度SINS/GNSS組合導(dǎo)航系統(tǒng)，在大初始姿態(tài)誤差角下，兩種算法的姿態(tài)濾波均收斂，其中，俯仰角和橫滾角均在50 s左右穩(wěn)定收斂。航向角估計(jì)效果相對(duì)水平姿態(tài)角要差，本文所提算法和傳統(tǒng)USQUE算法分別在150和200 s左右收斂至0.06°和0.08°以內(nèi)，VB-AUSQUE在200 s左右達(dá)到穩(wěn)定，而USQUE在400 s相對(duì)穩(wěn)定。航向角估計(jì)差的原因主要在于航向角和水平姿態(tài)角估計(jì)精度分別取決于陀螺儀和加速計(jì)的性能。仿真數(shù)據(jù)可知VB-AUSQUE濾波算法收斂速度及姿態(tài)估計(jì)性能更優(yōu)，能夠有效識(shí)別系統(tǒng)中未知和時(shí)變?cè)肼暎岣吡薝SQUE算法本身的自適應(yīng)性能。

3.2 艦載實(shí)驗(yàn)

為了驗(yàn)證本文算法的有效性，采用低精度SINS與GNSS組合艦載實(shí)驗(yàn)數(shù)據(jù)來比較USQUE和VB-AUSQUE濾波算法在組合導(dǎo)航中姿態(tài)估計(jì)效果。艦載實(shí)驗(yàn)中使用的傳感器主要技術(shù)指標(biāo)如表1所示，該試驗(yàn)場地在長江，實(shí)際船速9節(jié)(1節(jié)=1.852 km/h)左右。實(shí)驗(yàn)過程設(shè)計(jì)如下:打開實(shí)驗(yàn)系統(tǒng)后，試驗(yàn)船靠泊約 20 min，實(shí)驗(yàn)船出航后，運(yùn)動(dòng)約為3 h，選取800 s的數(shù)據(jù)進(jìn)行實(shí)驗(yàn)驗(yàn)證，整個(gè)運(yùn)動(dòng)軌跡如圖5所示。

表1 傳感器主要性能指標(biāo)

圖5 艦船運(yùn)動(dòng)軌跡Fig.5 Trajectory of the ship

通常導(dǎo)航系統(tǒng)受外部環(huán)境的復(fù)雜影響，未知和時(shí)變測量噪聲在船載試驗(yàn)中很常見，且在測量中有一些異常值。對(duì)于GPS數(shù)據(jù)，初始噪聲設(shè)置為100倍，并且采用時(shí)變?cè)肼暼缡?31)。初始條件:橫向誤差0.1°，偏航誤差1°,使用GPS初始化位置和速度。姿態(tài)誤差對(duì)比如圖6所示。

(31)

由圖6可以看出，在SINS/GNSS直接式組合導(dǎo)航艦載試驗(yàn)中，本文所提及的兩種方法在俯仰角和橫滾角誤差精度估計(jì)上相當(dāng)。由于航向角可觀測性弱，在載體運(yùn)動(dòng)直線段，航向角基本不變，濾波估計(jì)效果不好，估計(jì)誤差有增大趨勢，最大誤差約達(dá)到60′，當(dāng)艦船轉(zhuǎn)彎時(shí)可觀測性增強(qiáng)，本文所提的VB-USQUE算法明顯收斂速度較快較快，航向誤差估計(jì)較小。進(jìn)一步說明將VB近似估計(jì)方法應(yīng)用于USQUE姿態(tài)估計(jì)框架，能夠高斯迭代近似估計(jì)量測噪聲，有效抑制了噪聲對(duì)算法估計(jì)性能的干擾，提高了算法本身的自適應(yīng)能力。

圖6 艦載姿態(tài)估計(jì)誤差對(duì)比Fig.6 Comparison on attitude estimate errors of shipborne test

為了進(jìn)一步說明本文算法的有效性，對(duì)比位置誤差如圖7所示。該位置誤差為定義為

(32)

式(32)中：δλ為經(jīng)度誤差，m；δL為緯度誤差，m；δp為位置誤差。

由圖7知，該方法不但提高了本身姿態(tài)的估計(jì)性能，位置估計(jì)經(jīng)度也得到了提高。

圖7 位置估計(jì)誤差對(duì)比Fig.7 Comparison on position estimate errors

綜上，總結(jié)艦船在800 s實(shí)驗(yàn)數(shù)據(jù)的基礎(chǔ)上，對(duì)比本文所提及量中算法在航向方面的估計(jì)性能，其均方根誤差及計(jì)算時(shí)間對(duì)比如表2所示。可知，VB-AUSQUE和USQUE姿態(tài)估計(jì)方法在計(jì)算時(shí)間相當(dāng)?shù)那闆r下，本文算法姿態(tài)和位置估計(jì)誤差更小，性能更優(yōu)。

表2 航向均方根誤差和算法運(yùn)行時(shí)間

4 結(jié)論

USQUE濾波算法在組合導(dǎo)航姿態(tài)估計(jì)應(yīng)用中，量測噪聲的統(tǒng)計(jì)特性將直接影導(dǎo)航姿態(tài)的解算和濾波精度。在噪聲統(tǒng)計(jì)特性未知或時(shí)變的 SINS /GNSS 組合導(dǎo)航系統(tǒng)中，VB-AUSQUE算法可實(shí)時(shí)對(duì)量測噪聲近似逼近，調(diào)節(jié)時(shí)變方差陣。為此，通過仿真實(shí)驗(yàn)和實(shí)際艦載試驗(yàn)，相比標(biāo)準(zhǔn)的USQUE算法可明顯提高姿態(tài)估計(jì)精度，提高了算法的自適應(yīng)能力，具有較高的工程應(yīng)用價(jià)值。