趙彥明，秦永元

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

0 引言

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

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

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

1 非線(xiàn)性濾波對(duì)準(zhǔn)模型

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

(1)

(2)

(3)

(4)

式中i=x,y,z。

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

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

(5)

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

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

(6)

(7)

(8)

(9)

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

(10)

2 非線(xiàn)性濾波模型分解

Glin(t)wlin(t)

(11)

(12)

(13)

(14)

(15)

(16)

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

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

(17)

可以分離出非線(xiàn)性部分的濾波模型：

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

3 KF/EKF2混合濾波算法

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

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

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

(26)

(27)

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

4 仿真實(shí)驗(yàn)驗(yàn)證

4.1 仿真條件

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

仿真條件：

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仿真，每次仿真時(shí)間為300 s。

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

4.2 實(shí)驗(yàn)結(jié)果

圖1～3分別為KF/EKF2混合濾波對(duì)準(zhǔn)俯仰角誤差(δφx)、橫滾角誤差(δφy)和方位角誤差(δφz)。從圖1～3可看出，KF/EKF2濾波對(duì)準(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精對(duì)準(zhǔn)的極限精度。

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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