趙彥明,秦永元
(西北工業(yè)大學 自動化學院,陜西 西安 710129)
捷聯(lián)慣導系統(tǒng)(SINS)導航精度在很大程度上依賴初始對準精度[1]。傳統(tǒng)的初始對準方法是利用靜基座下重力加速度和地球自轉角速率信息先完成粗對準[2],再進行精對準。對于低精度慣導系統(tǒng)或系泊狀態(tài)下艦載慣導系統(tǒng)粗對準誤差較大,精對準是在大失準角條件下進行的。此時,SINS誤差模型呈現(xiàn)強非線性,標準卡爾曼濾波(KF)不再適用,需要采用非線性模型和非線性濾波算法[2]。
針對大失準角條件下SINS對準問題,文獻[2]提出了一種基于二階非線性量測的二階擴展卡爾曼濾波(EKF2)算法,可以在任意姿態(tài)條件下進行初始對準。文獻[3]將容積卡爾曼濾波法(CKF)應用于SINS非線性初始對準中,提高初始對準精度。文獻[4]提出了一種基于快速正交搜索(FOS)和KF的非線性參數(shù)估計方法。文獻[5-6]給出了大失準角條件下的非線性對準模型,通過無跡卡爾曼濾波(UKF)完成精對準。文獻[7]提出了一種改進的采用類高斯和重采樣的粒子濾波算法(PF),可以在大失準角的非線性、非高斯狀態(tài)下完成精對準。文獻[8]采用變換無跡求積卡爾曼濾波(TUKF)進行了噪聲不確定和大失準角條件下SINS對準實驗,結果顯示濾波算法具有高精度、快速性和魯棒性。目前的大失準角對準所采用的全狀態(tài)非線性濾波具有濾波維數(shù)大、算法復雜、計算量大等缺點[9]。
本文提出了一種基于模型分解的KF/EKF2混合濾波方法,將基于歐拉平臺誤差角的非線性濾波模型分解為非線性部分和線性部分,分別采用EKF2濾波和KF濾波進行處理。實驗結果表明,該方法可以有效減小計算量,改進實時性和穩(wěn)定性。
(1)
(2)
(3)
(4)
式中i=x,y,z。
對于IMU器件誤差,將陀螺常值漂移和加速度計常值零偏擴充到系統(tǒng)狀態(tài)中,狀態(tài)向量選為
(5)
式中δvn為地理坐標系下速度誤差向量。
結合式(1)、(2)可得用于非線性濾波的系統(tǒng)方程:
(6)
(7)
(8)
(9)
以SINS的速度輸出作為觀測量,則量測方程為
(10)
Glin(t)wlin(t)
(11)
(12)
(13)
(14)
(15)
(16)
式中gn為應力加速度矢量。
2) 非線性部分濾波模型。從式(6)中分離出狀態(tài)非線性部分,整理可得非線性部分狀態(tài)方程;而非線性部分量測向量為
(17)
可以分離出非線性部分的濾波模型:
(18)
(19)
(20)
(21)
(22)
(23)
(24)
(25)
基于模型分解的KF/EKF2混合濾波過程如下:
KF濾波遞推方程可參照文獻[11],重點將介紹非線性部分的EKF2濾波。
1) EKF2時間更新。非線性部分狀態(tài)維數(shù)l=3,量測維數(shù)m=3。則非線性部分的EKF2時間更新公式如下:
(26)
(27)
2) EKF2量測更新。由于非線性部分濾波模型的量測方程是線性的,所以EKF2量測更新與KF量測更新相同,具體算法見文獻[11]。
為了分析濾波算法的性能,預設精對準初始誤差和器件誤差,生成航跡數(shù)據(jù),采用蒙特卡洛(Monte-Carlo)仿真方法對KF/EKF2混合濾波精對準算法進行仿真,且與EKF2、UKF算法進行對比。
仿真條件:
1) 載體的位置為經(jīng)度108.9 °,緯度34.2°,高度400 m;速度為0;三軸姿態(tài)設置為10°,0°,-180°。
3) 在Monte-Carlo仿真中,初始失準角設置:俯仰和橫滾角初始失準角服從[-20°,20°]區(qū)間上的均勻分布,方位角初始失準角服從[-40°,40°]上的均勻分布,共進行20次Monte-Carlo仿真,每次仿真時間為300 s。
4) 速度量測噪聲設置為0.05 m/s,濾波周期設置為1 s,并對慣導速度、姿態(tài)進行反饋校正。對準精度用反饋校正后的姿態(tài)與真實姿態(tài)之差進行評估。
圖1~3分別為KF/EKF2混合濾波對準俯仰角誤差(δφx)、橫滾角誤差(δφy)和方位角誤差(δφz)。從圖1~3可看出,KF/EKF2濾波對準的水平失準角收斂很快,方位誤差角在150 s內(nèi)收斂。達到濾波穩(wěn)態(tài)后,KF/EKF2混合濾波精度為[0.004 37°, -0.005 186°, 0.105°],接近靜基座小失準角KF精對準的極限精度。
圖1 KF/EKF2混合濾波俯仰角誤差
圖2 KF/EKF2混合濾波橫滾角誤差
圖3 KF/EKF2混合濾波方位角誤差
為了與傳統(tǒng)全誤差狀態(tài)非線性濾波對準進行比較,文章分別用KF/EKF2混合濾波,全狀態(tài)EKF2濾波、全狀態(tài)UKF濾波進行仿真測試,統(tǒng)計3種方法對準誤差的均方根值(RMS),如表1所示。3種方法穩(wěn)態(tài)精度接近,KF/EKF2和EKF2濾波的對準精度略優(yōu)于UKF,KF/EKF2和EKF2濾波的精度相當。
表1 對準誤差RMS統(tǒng)計對比
在CPU為Intel雙核i5@2.6 GHz、4 GB內(nèi)存的筆記本電腦上運行Matlab仿真程序,經(jīng)測算KF/EKF2混合濾波算法單次平均濾波時間為14.701 ms(即濾波程序塊的單次運行時間),EKF2算法單次平均濾波時間為25.062 ms,UKF算法單次平均濾波時間為23.059 ms??梢奒F/EKF2混合濾波的實時性更好。
綜上仿真結果表明,KF/EKF2混合濾波對準的綜合性能指標優(yōu)于全狀態(tài)EKF2、UKF等非線性濾波對準算法。
在慣性器件精度很差或強干擾條件下,SINS系統(tǒng)誤差模型可能呈現(xiàn)強非線性,標準KF不再適用,需要采用非線性誤差傳播方程和非線性濾波算法(其中EKF2和UKF濾波最常用)。但非線性濾波算法存在計算量隨著狀態(tài)維數(shù)增加而急劇增大的問題,影響濾波穩(wěn)定性和實時性。為了降低非線性濾波狀態(tài)維數(shù),減小計算量,本文提出了一種基于狀態(tài)空間模型分解的KF/EKF2混合濾波方法,并且對其進行了蒙特卡洛仿真驗證。實驗結果證明, KF/EKF2混合濾波算法在計算量、實時性和對準精度等方面綜合優(yōu)于EKF2濾波和UKF濾波。