基于簡化多重漸消因子強跟蹤CKF的SINS非線性初始對準算法

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

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

0 引言

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

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

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

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

(1)

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

1.1 RMSTCKF算法流程

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

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

(2)

(3)

3)計算當前時刻的狀態估計值。

(4)

(5)

(6)

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

(7)

(8)

(9)

(10)

(11)

10)當前時刻k的濾波狀態值。

(12)

(13)

在Pk/k-1中引入兩個多重漸消因子陣代替傳統的單漸消因子，形式如下：

(14)

1.2 RMSTCKF中多重漸消因子的次優求解算法

多重漸消EKF的多重漸消因子陣次優解法如下[2]：

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

此時Mk、Nk可以表示為：

(23)

(24)

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

2 大方位失準角誤差模型

(25)

2.1 SINS姿態誤差方程

(26)

(27)

(28)

2.2 SINS速度誤差方程

理想條件下，捷聯慣導的速度微分方程如下

(29)

然而在實際工作時，捷聯慣導系統所計算的速度微分方程：

(30)

(31)

2.3 SINS非線性濾波模型

(32)

(33)

(34)

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

3 數值仿真與分析

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

表1 慣性測量組件主要性能參數

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

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 靜基座初始對準仿真

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

圖1 3種濾波算法所得失準角誤差

表2 3種濾波算法對準結果比較

3.2 系統噪聲方差陣不匹配初始對準仿真

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

圖2 系統噪聲不匹配3種濾波算法所得失準角誤差

3.3 基座擾動時初始對準仿真

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

(35)

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

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

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

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

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

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

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

4 結論

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