彭志穎, 夏海寶，盧 航，郝順義，黃國榮

(空軍工程大學(xué)航空工程學(xué)院, 西安 710038)

0 引言

SINS的實際工作環(huán)境通常比較復(fù)雜且具有較大的不確定性，SINS粗對準(zhǔn)的結(jié)果為大失準(zhǔn)角，此時基于小失準(zhǔn)角假設(shè)的線性誤差模型和卡爾曼濾波方法已不再適用，文獻(xiàn)[1]采用由歐拉平臺誤差角(EPEA)描述的非線性誤差模型和非線性濾波算法來解決這個問題，但因為系統(tǒng)會受到模型誤差、噪聲以及外界擾動等影響，非線性濾波存在魯棒性不佳和跟蹤能力差的問題[2-3]。文獻(xiàn)[4]提出了強跟蹤濾波理論，強跟蹤濾波在面對模型不確定性、噪聲、外界干擾較大的情況下可以保持較強的模型失配魯棒性和跟蹤能力[4-6]。文獻(xiàn)[3]將強跟蹤UKF應(yīng)用于初始對準(zhǔn)，但系統(tǒng)中每個狀態(tài)變量的不確定性互不相同，采用單一漸消因子無法保證對每個狀態(tài)變量都進(jìn)行較好的跟蹤。文獻(xiàn)[7]提出了多重漸消UKF濾波(MSTUKF)算法無需計算EKF中繁瑣的雅克比矩陣?，F(xiàn)有理論已經(jīng)證明[8-10]，UKF精度達(dá)到二階，CKF精度可以達(dá)到三階，并且CKF無需多的采樣點和調(diào)整的參數(shù)，具有更短的濾波時間和更可靠的數(shù)值穩(wěn)定性。

文中根據(jù)三階球面-徑向容積準(zhǔn)則中容積點的分布特征和量測方程為線性的模型特點，推導(dǎo)了簡化CKF(RCKF)，并將STF理論和RCKF算法框架相結(jié)合，引入兩個多重漸消因子陣，提出一種適用于初始對準(zhǔn)的簡化多重漸消因子強跟蹤CKF算法(RMSTCKF)，整個濾波算法只需計算一次容積變換，無需計算雅各布矩陣，同時多重漸消因子可以根據(jù)不同狀態(tài)的不確定性程度大小相應(yīng)提高各個狀態(tài)的跟蹤能力。文中采用文獻(xiàn)[1]中由EPEA描述的SINS非線性誤差方程并推導(dǎo)了非線性濾波方程，通過系統(tǒng)噪聲不匹配和基座受擾動情況下的仿真實驗，結(jié)果驗證了RMSTCKF的有效性。

1 多重漸消因子強跟蹤濾波器

STF原理是通過在預(yù)測協(xié)方差矩陣Pk|k-1中引入一個多重漸消因子陣λk，達(dá)到在線調(diào)整增益矩陣Kk的目的，濾波器需要滿足2個條件[7]：

(1)

式(1)使得濾波器滿足最小方差估計的性能指標(biāo)。式(2)強迫濾波器的殘差序列時刻保持正交，克服了濾波器在遇到模型不確定或者狀態(tài)突變等情況時，因為殘差序列不正交，導(dǎo)致濾波器性能下降的問題[4-5]，使得強跟蹤濾波器對模型不確定性有較強的魯棒性，對狀態(tài)突變有較強的跟蹤能力。

1.1 RMSTCKF算法流程

RMSTCKF算法流程如1)～10)所示，其中1)～4)為時間更新，5)～10)為量測更新。

1)計算容積點Xi,k-1|k-1,i=1,2,…,2n。

(2)

(3)

3)計算當(dāng)前時刻的狀態(tài)估計值。

(4)

(5)

(6)

對于量測方程為線性的CKF來說，可以在其量測更新中進(jìn)行簡化，下面給出簡化CKF量測更新的推導(dǎo)過程。

(7)

(8)

(9)

(10)

(11)

10)當(dāng)前時刻k的濾波狀態(tài)值。

(12)

(13)

在Pk/k-1中引入兩個多重漸消因子陣代替?zhèn)鹘y(tǒng)的單漸消因子，形式如下：

(14)

1.2 RMSTCKF中多重漸消因子的次優(yōu)求解算法

多重漸消EKF的多重漸消因子陣次優(yōu)解法如下[2]：

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

此時Mk、Nk可以表示為：

(23)

(24)

其中Hk即為量測矩陣。將式(23)、式(24)分別代入式(17)、式(18)，即可得到多重漸消因子陣次優(yōu)解法。

2 大方位失準(zhǔn)角誤差模型

(25)

2.1 SINS姿態(tài)誤差方程

(26)

(27)

(28)

2.2 SINS速度誤差方程

理想條件下，捷聯(lián)慣導(dǎo)的速度微分方程如下

(29)

然而在實際工作時，捷聯(lián)慣導(dǎo)系統(tǒng)所計算的速度微分方程：

(30)

(31)

2.3 SINS非線性濾波模型

(32)

(33)

(34)

將式(28)、式(31)、式(34)代入式(33)，整理并忽略二階及二階以上小量即可得到大方位失準(zhǔn)角濾波模型,這里不再詳細(xì)寫出具體表達(dá)式。

3 數(shù)值仿真與分析

將RCKF、RSTCKF、RMSTCKF濾波算法用于大方位失準(zhǔn)角濾波模型進(jìn)行初始對準(zhǔn)仿真。SINS位置取北緯45°，東經(jīng)108°，高度100 m處，系統(tǒng)的初始狀態(tài)X0=0，慣性測量組件參數(shù)如表1所示。

表1 慣性測量組件主要性能參數(shù)

初始方差陣P(0)、系統(tǒng)噪聲陣Q、量測噪聲陣R，設(shè)置為：

P(0)=diag[(2°)2(2°)2(10°)2(0.1 m/s)2(0.1 m/s)2(100 μg)2(100 μg)2(0.1°)2(0.1°)2(0.1°)2]

Q=diag[(0.05°/h)2(0.05°/h)2(0.05°/h)2·

(50 μg)2(50 μg)2(50 μg)205×1]

R=diag[(0.1 m/s)2(0.1 m/s)2]

3.1 靜基座初始對準(zhǔn)仿真

設(shè)置SINS東向、北向、天向的初始失準(zhǔn)角為φ=[1° 1° 30°]T，應(yīng)用上述3種濾波算法進(jìn)行600 s的初始對準(zhǔn)仿真，天向失準(zhǔn)角誤差曲線(經(jīng)過開環(huán)濾波修正的航向角減去真實航向角)如圖1所示。在收斂精度方面，RCKF、RSTCKF、RMSTCKF三種濾波算法精度差別不大，通過100次Monte-Carlo仿真，初始對準(zhǔn)精度用最后100 s方位失準(zhǔn)角誤差的算術(shù)平均值來衡量，3種算法的具體結(jié)果如表2所示。在收斂速度方面，RMSTCKF在120 s左右收斂至極限精度附近，稍快于RSTCKF和RCKF。在理想環(huán)境下，系統(tǒng)沒有受到模型誤差和外界擾動的影響，殘差序列時刻保持正交，3種濾波器始終可以保持較高的濾波性能。

圖1 3種濾波算法所得失準(zhǔn)角誤差

表2 3種濾波算法對準(zhǔn)結(jié)果比較

3.2 系統(tǒng)噪聲方差陣不匹配初始對準(zhǔn)仿真

為了檢驗提出的RMSTCKF算法的適應(yīng)性和魯棒性，將系統(tǒng)真實的噪聲協(xié)方差陣擴(kuò)大9倍，濾波模型中Q的設(shè)定值不變，天向失準(zhǔn)角誤差曲線如圖2所示。當(dāng)系統(tǒng)噪聲不匹配時3種算法均收斂，600 s的濾波值分別為192.36′、39.12′、38.64′。RCKF波動最為明顯，400 s后波動才逐漸消失，收斂速度和精度均較差；RSTCKF和RMSTCKF表現(xiàn)明顯優(yōu)于RCKF，但RSTCKF也存在較小的擾動，兩者均可以很快收斂至極限精度附近。

圖2 系統(tǒng)噪聲不匹配3種濾波算法所得失準(zhǔn)角誤差

3.3 基座擾動時初始對準(zhǔn)仿真

SINS靜基座初始對準(zhǔn)仿真并沒有考慮實際慣導(dǎo)系統(tǒng)工作時常遇到的外界擾動等因素。為檢驗RMSTCKF在遇到外界擾動時的魯棒性與跟蹤性，當(dāng)對準(zhǔn)時間達(dá)到40 s時，在基座原點處加入沿俯仰角和橫滾角時長為10 s(40～50 s)的正弦晃動干擾：

(35)

其余的仿真條件不變，用上述3種濾波方法進(jìn)行600 s的初始對準(zhǔn)仿真實驗，天向失準(zhǔn)角誤差曲線如圖3所示，其中圖3(b)為圖3(a)在受擾處的放大圖。

從圖3(a)中可以看出，系統(tǒng)受到擾動后，RMSTCKF和RSTCKF相較于RCKF具有更強的調(diào)節(jié)能力和更短的收斂時間，其中RMSTCKF在150 s左右收斂到極限精度附近，RSTCKF在220 s左右收斂至極限精度附近，RCKF在受到擾動后調(diào)節(jié)能力較差，500 s左右才收斂至極限精度附近。

圖3 存在一次突然擾動時3種濾波算法的失準(zhǔn)角誤差

從圖3(b)中可以看出，在40 s到50 s受擾動期間，3種算法的濾波值都因為受擾而產(chǎn)生不同程度的波動。其中RMSTCKF波動明顯小于另外兩種算法，說明該算法在狀態(tài)突變時具有較強魯棒性和跟蹤性；RSTCKF受擾動后的波動要大于RMSTCKF，小于RCKF。RCKF的波動最大，濾波性能在受擾后明顯下降，無法滿足SINS的對準(zhǔn)要求。為了進(jìn)一步檢驗RMSTCKF的濾波性能，在濾波進(jìn)行到105 s時繼續(xù)在基座原點處加入沿俯仰角和橫滾角時長5 s的正弦晃動擾動以增加系統(tǒng)的復(fù)雜程度和不確定性。天向失準(zhǔn)角誤差曲線如圖4所示。

圖4 存在兩次突然擾動時3種濾波算法的失準(zhǔn)角誤差

由圖4可知，當(dāng)系統(tǒng)受到二次擾動后，3種濾波算法的性能受到了不同程度影響，RMSTCKF在受擾后100 s左右就可以迅速收斂至穩(wěn)態(tài)，具有較快的過渡過程；RSTCKF的波動略大于RMSTCKF，在受擾后120 s左右收斂至穩(wěn)態(tài)；RCKF對準(zhǔn)時間大于600 s，對準(zhǔn)精度大大下降。

結(jié)合圖3和圖4分析可知，當(dāng)系統(tǒng)受到擾動后，RMSTCKF具有比RSTCKF和RCKF更強的跟蹤能力和魯棒性。這是因為RMSTCKF的多重漸消因子在面對模型不匹配和外界干擾時可以用不同的漸消速率對不同的狀態(tài)進(jìn)行估計，更加準(zhǔn)確的調(diào)節(jié)增益矩陣，以保證每個狀態(tài)的濾波性能，具有更好的自適應(yīng)性和濾波精度。

4 結(jié)論

文中提出了一種適用于大方位失準(zhǔn)角初始對準(zhǔn)的RMSTCKF算法，給出了該算法的計算流程和多重漸消因子陣的次優(yōu)求解法，推導(dǎo)建立了用歐拉平臺誤差角表示的SINS非線性誤差方程，并在靜基座、系統(tǒng)噪聲方差陣不匹配、基座受擾動3種情況下利用RCKF、RSTCKF、RMSTCKF三種濾波算法進(jìn)行了大方位失準(zhǔn)角初始對準(zhǔn)仿真。結(jié)果表明，提出的RMSTCKF對于系統(tǒng)噪聲方差陣不匹配的情況具有較強的自適應(yīng)能力；當(dāng)系統(tǒng)受外界擾動時，RMSTCKF表現(xiàn)出較強的魯棒性和強跟蹤能力，與另兩種濾波算法相比，RMSTCKF具有更強的工程實用價值。