基于Sage-Husa自適應濾波的快速系統級標定方法

趙桂玲，譚茂林，姜子昊

(遼寧工程技術大學 測繪與地理科學學院， 遼寧 阜新 123000)

1 引言

捷聯慣導系統的核心是由3個加速度計和3個陀螺儀組成的慣性測量單元(inertial measurement unit，IMU)，其主要誤差為零偏誤差、標度因數誤差、安裝誤差、隨機噪聲[1-2]。IMU的誤差對導航精度會產生很大影響，并且隨時間累積，因此必須對其進行標定和補償,以提高實際應用中系統的導航精度[3-5]。

在系統級標定濾波法中，IMU誤差參數作為狀態量，通過構建Kalman濾波器將其估計出[6]。文獻[7-10]構造不同的誤差模型、設計標定路徑，通過Kalman濾波來估計IMU誤差參數。文獻[11]使用強跟蹤濾波(strong tracking filter，STF)來估計IMU零偏誤差和標度因數誤差。文獻[12]提出一種甲殼蟲群須搜索(beetle swarm antenna search，BSAS)算法，通過設計24個測量位置標定出三軸加速度計零偏誤差、標度因數誤差、安裝誤差和加速度計二次項誤差共計15項誤差參數。文獻[13]結合模型預測濾波和Sage-Husa自適應濾波，提出一種自適應模型預測組合濾波算法用于在線標定，標定出IMU標度因數誤差和零偏誤差共10項誤差參數。文獻[14]設計一種基于新息自適應濾波方法在線標定方案，克服常規Kalman濾波需預先設置噪聲的不足，標定出24項IMU誤差參數。在系統級標定中，是否可以引入新的濾波算法？以提升濾波收斂精度或收斂速度。

本文提出一種基于Sage-Husa自適應濾波的系統級標定方法，對系統中量測噪聲方差陣進行自適應處理。對噪聲進行自適應處理的效果與系統的可觀測性強度有一定關系[15-16]，將標定模型降維，設計中間參數對誤差參數項進行合并，得到12維的可觀測性高的濾波模型。采用6個靜態位置進行標定實驗，利用得到的每個靜態標定位置下中間參數濾波收斂結果進行解耦計算，最終成功標定出24項IMU誤差參數。仿真實驗表明，相較于常規Kalman濾波系統級標定，本文中提出的方法在濾波收斂速度更快，可節省標定所需時間。

2 Sage-Husa自適應濾波算法

Kalman濾波是一種最優估計技術，根據系統量測量估計系統的狀態量[17]。系統狀態空間模型由狀態方程和觀測方程組成，設離散化后的狀態方程和量測方程為：

(1)

式中:k為離散時間；Xk為k時刻的狀態向量；Φk|k-1為k時刻到k-1時刻的狀態轉移矩陣；Γk-1為系統噪聲矩陣；Wk-1為系統噪聲；Zk為量測向量；Hk為量測矩陣；Vk為量測噪聲。

應用Kalman濾波時，一般要求Wk和Vk互不相關，且均為零均值白噪聲。離散系統標準Kalman濾波由5個基本公式構成，如下：

1) 時間更新方程

(2)

(3)

2) 量測更新方程

(4)

(5)

Pk=(I+KkHk)Pk|k-1

(6)

常規Kalman濾波只有在隨機動態系統結構參數和噪聲統計特性參數都準確已知的條件下,才能獲得狀態的最優估計[18]。在使用常規Kalman濾波時，對噪聲協方差陣進行經驗設定，即系統噪聲方差陣Qk和量測噪聲方差陣Rk被設定為固定不變的經驗值，濾波的狀態估計結果將受到影響[19]。自適應濾波對噪聲進行實時估計和更新，能夠減小濾波器對先驗信息的依賴，有助于提升濾波狀態估計效果[20]。文獻[21]提出一種基于Sage-Husa濾波漸消因子選擇的陀螺在線標定方法，在外部環境變化情況下，對比常規Kalman濾波，該方法在標定時間和標定精度上更能有效地估計出陀螺零偏誤差和標度因數誤差。文獻[22]利用Sage-Husa自適應濾波對捷聯慣導系統進行初始對準，實驗結果表明，相對常規Kalman濾波，自適應濾波在收斂時間上優勢明顯。

自適應濾波要解決的主要問題是估計Qk和Rk。Qk與系統特性有關，不易發生改變。量測噪聲由外部因素引起，容易發生較大變化，設定固定不變的Rk可能對收斂時間和收斂精度造成影響[15,23]。因此為準確掌握量測噪聲方差陣Rk的特性，以提升捷聯慣導系統級標定的效果，引入Sage-Husa自適應Kalman濾波，對系統中Rk進行估計和更新。Sage-Husa自適應Kalman濾波的Wk和Vk滿足下式：

(7)

式中:qk、rk、Qk、Rk為噪聲參數;δkj為克羅尼克函數。

本文中提出的Sage-Husa自適應濾波方法，是在常規Kalman濾波的基礎上，對量測噪聲方差陣Rk進行估計，計算更新形式如下：

(8)

圖1 Sage-Husa自適應濾波計算流程

3 降維模型的建立

對IMU零偏誤差、標度因數誤差和安裝誤差這3種主要誤差進行研究，IMU誤差參數模型可寫為：

(9)

靜基座下IMU誤差與速度誤差和姿態誤差的關系為：

(10)

結合式(9)，式(10)進一步可以寫為：

(11)

根據系統誤差方程(11)建立濾波模型，模型維數為30維。由于自適應濾波算法的處理效果與系統可觀測強度存在關系，減少模型維數，有利于增強自適應濾波的有效性。將IMU誤差參數先進行合并，引入6個中間參數如下：

(12)

(13)

將式(12)和式(13)代入誤差方程(11)有：

(14)

于是，濾波器的維數由原來的30維降為12維，模型的可觀測強度有了很大的提升，濾波器狀態向量如下：

(15)

根據式(11)和式(14)，濾波器的狀態方程為：

(16)

濾波器的量測方程為：

Z=HX+V

(17)

設計6個靜態標定位置，標定路徑示意圖如圖2所示。

圖2 6位置標定路徑示意圖

通過濾波算法可以得到每個靜態標定位置下中間參數的收斂結果，即得到6個濾波收斂結果為e1-j～e6-j(j表示標定位置，j=1,2,…,6)。

將每個靜態標定位置代入系統速度誤差和姿態誤差方程中，可得到每個位置下中間參數和待標定誤差之間的關系。例如將位置1(東北天)代入公式(14)有：

(18)

式(18)為位置1下中間參數與待標定IMU誤差參數的數學關系。通過6個標定位置下中間參數與待標定參數的數學關系組，進一步進行解耦計算，最終解得待標定誤差參數數值，實現了IMU誤差參數的全標定。

4 仿真實驗

為驗證本文中提出的基于Sage-Husa自適應濾波的系統級標定方法的有效性，對比常規Kalman濾波分別開展仿真實驗，IMU誤差參數預設值見表1和表2。各位置下IMU標定仿真時間設置均為10 min，其中為使得與加速度計有關的3個中間參數在2種濾波下的比較更為明顯，與加速度計相關的濾波估計曲線截取標定時間內第0～1 min進行展示。6個靜態標定位置下的濾波估計曲線如圖3—圖8所示。

圖3 位置1下2種濾波估計曲線

圖4 位置2下2種濾波估計曲線

圖5 位置3下2種濾波估計曲線

圖6 位置4下2種濾波估計曲線

圖7 位置5下2種濾波估計曲線

圖8 位置6下2種濾波估計曲線

根據6個位置下濾波收斂結果，進一步完成解耦計算，得到IMU誤差參數的標定結果如表1和表2所示。

表1 加速度計誤差參數標定結果Table 1 Acceleration error parameter calibration results

表2 陀螺誤差參數標定結果Table 2 Gyro error parameter calibration results

從圖3—圖8的6個靜態標定位置下的濾波估計曲線可看出，中間參數在每個靜態標定位置下濾波均有較好的收斂結果。根據表1和表2的標定結果可知2種方法的相對誤差均小于0.6%，2種方法在標定精度上相當。相較于常規Kalman濾波(對應圖中KF)，本文提出的Sage-Husa自適應濾波算法(對應圖中SHAKF)的收斂速度更快。在與陀螺有關的中間參數濾波收斂上，Sage-Husa自適應濾波算法優勢十分明顯，常規Kalman濾波需要大約5 min才能滿足收斂條件，而Sage-Husa自適應濾波算法僅6 s即滿足了收斂條件。

5 結論

本文所提出的標定方法對降低捷聯慣導系統級標定時間具有一定的參考價值，仿真結果表明：系統級標定采用常規Kalman濾波算法，將量測噪聲方差陣設定為固定值，使得濾波收斂速度慢。采用Sage-Husa自適應濾波對量測噪聲方差陣進行實時估計和更新，能有效降低標定時間。與常規Kalman濾波相比，標定精度相當，均小于0.6%；濾波收斂速度有顯著提升，標定時間由5 min縮短至6 s，縮短了98%。