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

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

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

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

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

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

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

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

(1)

(2)

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

(3)

(4)

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

(5)

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

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

2.1 變分貝葉斯原理

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

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

(6)

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

(7)

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

(8)

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

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

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

(9)

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

(10)

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

2.2.1 時間更新

1)產(chǎn)生Sigma點

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

(11)

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

(12)

(13)

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

(14)

(15)

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

(16)

(17)

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

計算概率密度分布參數(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。

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

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

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

2.2.3 姿態(tài)更新

(30)

3 仿真與艦載測試

3.1 仿真實驗

為驗證算法有效性，利用軌跡發(fā)生器模擬飛行器大機(jī)動運動，其軌跡如圖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，總時長為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)估計誤差對比Fig.4 Comparison on attitude estimate errors

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

3.2 艦載實驗

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

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

圖5 艦船運動軌跡Fig.5 Trajectory of the ship

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

(31)

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

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

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

(32)

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

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

圖7 位置估計誤差對比Fig.7 Comparison on position estimate errors

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

表2 航向均方根誤差和算法運行時間

4 結(jié)論

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