趙桂玲，譚茂林，姜子昊

(遼寧工程技術(shù)大學(xué) 測(cè)繪與地理科學(xué)學(xué)院， 遼寧 阜新 123000)

1 引言

捷聯(lián)慣導(dǎo)系統(tǒng)的核心是由3個(gè)加速度計(jì)和3個(gè)陀螺儀組成的慣性測(cè)量單元(inertial measurement unit，IMU)，其主要誤差為零偏誤差、標(biāo)度因數(shù)誤差、安裝誤差、隨機(jī)噪聲[1-2]。IMU的誤差對(duì)導(dǎo)航精度會(huì)產(chǎn)生很大影響，并且隨時(shí)間累積，因此必須對(duì)其進(jìn)行標(biāo)定和補(bǔ)償,以提高實(shí)際應(yīng)用中系統(tǒng)的導(dǎo)航精度[3-5]。

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

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

2 Sage-Husa自適應(yīng)濾波算法

Kalman濾波是一種最優(yōu)估計(jì)技術(shù)，根據(jù)系統(tǒng)量測(cè)量估計(jì)系統(tǒng)的狀態(tài)量[17]。系統(tǒng)狀態(tài)空間模型由狀態(tài)方程和觀測(cè)方程組成，設(shè)離散化后的狀態(tài)方程和量測(cè)方程為：

(1)

式中:k為離散時(shí)間；Xk為k時(shí)刻的狀態(tài)向量；Φk|k-1為k時(shí)刻到k-1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；Γk-1為系統(tǒng)噪聲矩陣；Wk-1為系統(tǒng)噪聲；Zk為量測(cè)向量；Hk為量測(cè)矩陣；Vk為量測(cè)噪聲。

應(yīng)用Kalman濾波時(shí)，一般要求Wk和Vk互不相關(guān)，且均為零均值白噪聲。離散系統(tǒng)標(biāo)準(zhǔn)Kalman濾波由5個(gè)基本公式構(gòu)成，如下：

1) 時(shí)間更新方程

(2)

(3)

2) 量測(cè)更新方程

(4)

(5)

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

(6)

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

自適應(yīng)濾波要解決的主要問(wèn)題是估計(jì)Qk和Rk。Qk與系統(tǒng)特性有關(guān)，不易發(fā)生改變。量測(cè)噪聲由外部因素引起，容易發(fā)生較大變化，設(shè)定固定不變的Rk可能對(duì)收斂時(shí)間和收斂精度造成影響[15,23]。因此為準(zhǔn)確掌握量測(cè)噪聲方差陣Rk的特性，以提升捷聯(lián)慣導(dǎo)系統(tǒng)級(jí)標(biāo)定的效果，引入Sage-Husa自適應(yīng)Kalman濾波，對(duì)系統(tǒng)中Rk進(jìn)行估計(jì)和更新。Sage-Husa自適應(yīng)Kalman濾波的Wk和Vk滿足下式：

(7)

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

本文中提出的Sage-Husa自適應(yīng)濾波方法，是在常規(guī)Kalman濾波的基礎(chǔ)上，對(duì)量測(cè)噪聲方差陣Rk進(jìn)行估計(jì)，計(jì)算更新形式如下：

(8)

圖1 Sage-Husa自適應(yīng)濾波計(jì)算流程

3 降維模型的建立

對(duì)IMU零偏誤差、標(biāo)度因數(shù)誤差和安裝誤差這3種主要誤差進(jìn)行研究，IMU誤差參數(shù)模型可寫為：

(9)

靜基座下IMU誤差與速度誤差和姿態(tài)誤差的關(guān)系為：

(10)

結(jié)合式(9)，式(10)進(jìn)一步可以寫為：

(11)

根據(jù)系統(tǒng)誤差方程(11)建立濾波模型，模型維數(shù)為30維。由于自適應(yīng)濾波算法的處理效果與系統(tǒng)可觀測(cè)強(qiáng)度存在關(guān)系，減少模型維數(shù)，有利于增強(qiáng)自適應(yīng)濾波的有效性。將IMU誤差參數(shù)先進(jìn)行合并，引入6個(gè)中間參數(shù)如下：

(12)

(13)

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

(14)

于是，濾波器的維數(shù)由原來(lái)的30維降為12維，模型的可觀測(cè)強(qiáng)度有了很大的提升，濾波器狀態(tài)向量如下：

(15)

根據(jù)式(11)和式(14)，濾波器的狀態(tài)方程為：

(16)

濾波器的量測(cè)方程為：

Z=HX+V

(17)

設(shè)計(jì)6個(gè)靜態(tài)標(biāo)定位置，標(biāo)定路徑示意圖如圖2所示。

圖2 6位置標(biāo)定路徑示意圖

通過(guò)濾波算法可以得到每個(gè)靜態(tài)標(biāo)定位置下中間參數(shù)的收斂結(jié)果，即得到6個(gè)濾波收斂結(jié)果為e1-j～e6-j(j表示標(biāo)定位置，j=1,2,…,6)。

將每個(gè)靜態(tài)標(biāo)定位置代入系統(tǒng)速度誤差和姿態(tài)誤差方程中，可得到每個(gè)位置下中間參數(shù)和待標(biāo)定誤差之間的關(guān)系。例如將位置1(東北天)代入公式(14)有：

(18)

式(18)為位置1下中間參數(shù)與待標(biāo)定IMU誤差參數(shù)的數(shù)學(xué)關(guān)系。通過(guò)6個(gè)標(biāo)定位置下中間參數(shù)與待標(biāo)定參數(shù)的數(shù)學(xué)關(guān)系組，進(jìn)一步進(jìn)行解耦計(jì)算，最終解得待標(biāo)定誤差參數(shù)數(shù)值，實(shí)現(xiàn)了IMU誤差參數(shù)的全標(biāo)定。

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

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

圖3 位置1下2種濾波估計(jì)曲線

圖4 位置2下2種濾波估計(jì)曲線

圖5 位置3下2種濾波估計(jì)曲線

圖6 位置4下2種濾波估計(jì)曲線

圖7 位置5下2種濾波估計(jì)曲線

圖8 位置6下2種濾波估計(jì)曲線

根據(jù)6個(gè)位置下濾波收斂結(jié)果，進(jìn)一步完成解耦計(jì)算，得到IMU誤差參數(shù)的標(biāo)定結(jié)果如表1和表2所示。

表1 加速度計(jì)誤差參數(shù)標(biāo)定結(jié)果Table 1 Acceleration error parameter calibration results

表2 陀螺誤差參數(shù)標(biāo)定結(jié)果Table 2 Gyro error parameter calibration results

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

5 結(jié)論

本文所提出的標(biāo)定方法對(duì)降低捷聯(lián)慣導(dǎo)系統(tǒng)級(jí)標(biāo)定時(shí)間具有一定的參考價(jià)值，仿真結(jié)果表明：系統(tǒng)級(jí)標(biāo)定采用常規(guī)Kalman濾波算法，將量測(cè)噪聲方差陣設(shè)定為固定值，使得濾波收斂速度慢。采用Sage-Husa自適應(yīng)濾波對(duì)量測(cè)噪聲方差陣進(jìn)行實(shí)時(shí)估計(jì)和更新，能有效降低標(biāo)定時(shí)間。與常規(guī)Kalman濾波相比，標(biāo)定精度相當(dāng)，均小于0.6%；濾波收斂速度有顯著提升，標(biāo)定時(shí)間由5 min縮短至6 s，縮短了98%。