大失準(zhǔn)角下SINS的KF/EKF2混合濾波對準(zhǔn)

趙彥明，秦永元

(西北工業(yè)大學(xué) 自動化學(xué)院,陜西 西安 710129)

0 引言

捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)導(dǎo)航精度在很大程度上依賴初始對準(zhǔn)精度[1]。傳統(tǒng)的初始對準(zhǔn)方法是利用靜基座下重力加速度和地球自轉(zhuǎn)角速率信息先完成粗對準(zhǔn)[2]，再進(jìn)行精對準(zhǔn)。對于低精度慣導(dǎo)系統(tǒng)或系泊狀態(tài)下艦載慣導(dǎo)系統(tǒng)粗對準(zhǔn)誤差較大，精對準(zhǔn)是在大失準(zhǔn)角條件下進(jìn)行的。此時，SINS誤差模型呈現(xiàn)強(qiáng)非線性，標(biāo)準(zhǔn)卡爾曼濾波(KF)不再適用，需要采用非線性模型和非線性濾波算法[2]。

針對大失準(zhǔn)角條件下SINS對準(zhǔn)問題，文獻(xiàn)[2]提出了一種基于二階非線性量測的二階擴(kuò)展卡爾曼濾波(EKF2)算法，可以在任意姿態(tài)條件下進(jìn)行初始對準(zhǔn)。文獻(xiàn)[3]將容積卡爾曼濾波法(CKF)應(yīng)用于SINS非線性初始對準(zhǔn)中，提高初始對準(zhǔn)精度。文獻(xiàn)[4]提出了一種基于快速正交搜索(FOS)和KF的非線性參數(shù)估計方法。文獻(xiàn)[5-6]給出了大失準(zhǔn)角條件下的非線性對準(zhǔn)模型，通過無跡卡爾曼濾波(UKF)完成精對準(zhǔn)。文獻(xiàn)[7]提出了一種改進(jìn)的采用類高斯和重采樣的粒子濾波算法(PF)，可以在大失準(zhǔn)角的非線性、非高斯?fàn)顟B(tài)下完成精對準(zhǔn)。文獻(xiàn)[8]采用變換無跡求積卡爾曼濾波(TUKF)進(jìn)行了噪聲不確定和大失準(zhǔn)角條件下SINS對準(zhǔn)實驗，結(jié)果顯示濾波算法具有高精度、快速性和魯棒性。目前的大失準(zhǔn)角對準(zhǔn)所采用的全狀態(tài)非線性濾波具有濾波維數(shù)大、算法復(fù)雜、計算量大等缺點[9]。

本文提出了一種基于模型分解的KF/EKF2混合濾波方法，將基于歐拉平臺誤差角的非線性濾波模型分解為非線性部分和線性部分，分別采用EKF2濾波和KF濾波進(jìn)行處理。實驗結(jié)果表明，該方法可以有效減小計算量，改進(jìn)實時性和穩(wěn)定性。

1 非線性濾波對準(zhǔn)模型

1.1 大失準(zhǔn)角下SINS誤差模型

(1)

(2)

(3)

(4)

式中i=x,y,z。

1.2 準(zhǔn)靜基座大失準(zhǔn)角初始對準(zhǔn)濾波模型

對于IMU器件誤差，將陀螺常值漂移和加速度計常值零偏擴(kuò)充到系統(tǒng)狀態(tài)中，狀態(tài)向量選為

(5)

式中δvn為地理坐標(biāo)系下速度誤差向量。

結(jié)合式(1)、(2)可得用于非線性濾波的系統(tǒng)方程：

(6)

(7)

(8)

(9)

以SINS的速度輸出作為觀測量，則量測方程為

(10)

2 非線性濾波模型分解

Glin(t)wlin(t)

(11)

(12)

(13)

(14)

(15)

(16)

式中g(shù)n為應(yīng)力加速度矢量。

2) 非線性部分濾波模型。從式(6)中分離出狀態(tài)非線性部分，整理可得非線性部分狀態(tài)方程；而非線性部分量測向量為

(17)

可以分離出非線性部分的濾波模型：

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

3 KF/EKF2混合濾波算法

基于模型分解的KF/EKF2混合濾波過程如下：

KF濾波遞推方程可參照文獻(xiàn)[11]，重點將介紹非線性部分的EKF2濾波。

1) EKF2時間更新。非線性部分狀態(tài)維數(shù)l=3，量測維數(shù)m=3。則非線性部分的EKF2時間更新公式如下：

(26)

(27)

2) EKF2量測更新。由于非線性部分濾波模型的量測方程是線性的，所以EKF2量測更新與KF量測更新相同，具體算法見文獻(xiàn)[11]。

4 仿真實驗驗證

4.1 仿真條件

為了分析濾波算法的性能，預(yù)設(shè)精對準(zhǔn)初始誤差和器件誤差，生成航跡數(shù)據(jù)，采用蒙特卡洛(Monte-Carlo)仿真方法對KF/EKF2混合濾波精對準(zhǔn)算法進(jìn)行仿真，且與EKF2、UKF算法進(jìn)行對比。

仿真條件：

1) 載體的位置為經(jīng)度108.9 °，緯度34.2°，高度400 m；速度為0；三軸姿態(tài)設(shè)置為10°，0°，-180°。

3) 在Monte-Carlo仿真中，初始失準(zhǔn)角設(shè)置：俯仰和橫滾角初始失準(zhǔn)角服從[-20°,20°]區(qū)間上的均勻分布，方位角初始失準(zhǔn)角服從[-40°,40°]上的均勻分布，共進(jìn)行20次Monte-Carlo仿真，每次仿真時間為300 s。

4) 速度量測噪聲設(shè)置為0.05 m/s，濾波周期設(shè)置為1 s，并對慣導(dǎo)速度、姿態(tài)進(jìn)行反饋校正。對準(zhǔn)精度用反饋校正后的姿態(tài)與真實姿態(tài)之差進(jìn)行評估。

4.2 實驗結(jié)果

圖1～3分別為KF/EKF2混合濾波對準(zhǔn)俯仰角誤差(δφx)、橫滾角誤差(δφy)和方位角誤差(δφz)。從圖1～3可看出，KF/EKF2濾波對準(zhǔn)的水平失準(zhǔn)角收斂很快，方位誤差角在150 s內(nèi)收斂。達(dá)到濾波穩(wěn)態(tài)后，KF/EKF2混合濾波精度為[0.004 37°, -0.005 186°, 0.105°]，接近靜基座小失準(zhǔn)角KF精對準(zhǔn)的極限精度。

圖1 KF/EKF2混合濾波俯仰角誤差

圖2 KF/EKF2混合濾波橫滾角誤差

圖3 KF/EKF2混合濾波方位角誤差

為了與傳統(tǒng)全誤差狀態(tài)非線性濾波對準(zhǔn)進(jìn)行比較，文章分別用KF/EKF2混合濾波，全狀態(tài)EKF2濾波、全狀態(tài)UKF濾波進(jìn)行仿真測試，統(tǒng)計3種方法對準(zhǔn)誤差的均方根值(RMS)，如表1所示。3種方法穩(wěn)態(tài)精度接近，KF/EKF2和EKF2濾波的對準(zhǔn)精度略優(yōu)于UKF，KF/EKF2和EKF2濾波的精度相當(dāng)。

表1 對準(zhǔn)誤差RMS統(tǒng)計對比

在CPU為Intel雙核i5@2.6 GHz、4 GB內(nèi)存的筆記本電腦上運(yùn)行Matlab仿真程序，經(jīng)測算KF/EKF2混合濾波算法單次平均濾波時間為14.701 ms(即濾波程序塊的單次運(yùn)行時間)，EKF2算法單次平均濾波時間為25.062 ms，UKF算法單次平均濾波時間為23.059 ms?？梢奒F/EKF2混合濾波的實時性更好。

綜上仿真結(jié)果表明，KF/EKF2混合濾波對準(zhǔn)的綜合性能指標(biāo)優(yōu)于全狀態(tài)EKF2、UKF等非線性濾波對準(zhǔn)算法。

5 結(jié)束語

在慣性器件精度很差或強(qiáng)干擾條件下，SINS系統(tǒng)誤差模型可能呈現(xiàn)強(qiáng)非線性，標(biāo)準(zhǔn)KF不再適用，需要采用非線性誤差傳播方程和非線性濾波算法(其中EKF2和UKF濾波最常用)。但非線性濾波算法存在計算量隨著狀態(tài)維數(shù)增加而急劇增大的問題，影響濾波穩(wěn)定性和實時性。為了降低非線性濾波狀態(tài)維數(shù)，減小計算量，本文提出了一種基于狀態(tài)空間模型分解的KF/EKF2混合濾波方法，并且對其進(jìn)行了蒙特卡洛仿真驗證。實驗結(jié)果證明， KF/EKF2混合濾波算法在計算量、實時性和對準(zhǔn)精度等方面綜合優(yōu)于EKF2濾波和UKF濾波。