張復建，單 斌，楊 波，薛 亮，騰紅磊

(火箭軍工程大學導彈工程學院，西安 710025)

0 引言

隨著微機電系統(tǒng) (Micro－Electro Mechanical System，MEMS)技術(shù)的不斷發(fā)展，在民用和軍事方面，都應(yīng)用的越來越廣泛。傳統(tǒng)的高精度慣性器件在捷聯(lián)慣性導航系統(tǒng)(Strap－down Inertial Navigation Systems，SINS) 應(yīng)用中，最突出的是成本和重量問題，為緩解這個問題，基于微機電技術(shù)的微型陀螺儀和加速度因具有低成本、功耗低、體積小等優(yōu)勢，在許多應(yīng)用中，逐漸取代傳統(tǒng)的陀螺儀和加速度計［1］。

捷聯(lián)慣性導航系統(tǒng)進行初始對準的目的是提供系統(tǒng)的初始姿態(tài)矩陣，便于在導航坐標系中進行導航解算。因此，許多學者對此進行大量研究，推導了捷聯(lián)慣性導航系統(tǒng)的對準模型，許多成果在實際中取得廣泛運用［2-3］，然而，大部分導航系統(tǒng)的慣性器件都采用了高精度的陀螺儀和加速度計，對于低成本的微型慣性測量單元［4］(Miniature Inertial Measurement Units，MIMU，簡稱:微慣組)，由于受微機電技術(shù)工藝等所限，微慣組的量測噪聲、零偏以及漂移等誤差較大，其測量精度相對傳統(tǒng)的慣性器件要低很多，以至于不能采取相同的方法實現(xiàn)微慣組的初始對準。針對此問題，許多學者也作出了相應(yīng)研究成果，其中，李士心［5］針對低成本的IMU偏差和噪聲大的問題，采用GPS提供的速度信息輔助對準，對IMU參數(shù)誤差和航向角誤差取得較好的估計，在仿真和實際應(yīng)用都得到較好的效果;龐春雷［6］針對低精度的捷聯(lián)慣性導航無法實現(xiàn)航向角的自對準和GPS雙天線側(cè)向需要較長時間初始化的問題，研究了二者相互輔助的算法，建立互相輔助的算法模型，通過實驗驗證了算法的簡單實用性。

本文基于微慣組的捷聯(lián)慣性導航系統(tǒng)無法實現(xiàn)自對準的問題，依靠外部測量信息輔助對準，根據(jù)全球衛(wèi)星定位系統(tǒng) (the global positioning system，GPS)與捷聯(lián)慣性導航系統(tǒng)優(yōu)勢互補的特點，當載體靜止時，可以采用GPS系統(tǒng)提供的速度信息及磁強計輸出的航向角信息，輔助基于微慣組的捷聯(lián)慣性導航系統(tǒng)完成初始對準。

1 初始對準原理

當載體處于靜止狀態(tài)時，針對微慣導不能實現(xiàn)自主對準，本文采用四個級聯(lián)式卡爾曼濾波器的濾波方法解決微慣導對準問題，四個濾波器主要分為水平姿態(tài)角濾波器、航向角濾波器、陀螺儀誤差濾波器和速度濾波器，其中，水平姿態(tài)角濾波器主要是根據(jù)加速度計輸出作為觀測量，估計出狀態(tài)值，并反解出水平姿態(tài)角 (包括俯仰角和橫滾角)，并將水平姿態(tài)角送入航向角濾波器計算濾波觀測向量數(shù)據(jù);在航向角濾波器中，航向角觀測信息由磁強計輸出并融合水平姿態(tài)角信息提供，通過濾波得到狀態(tài)值，并反解得到載體航向角;在陀螺儀誤差濾波器中，主要考慮陀螺儀誤差為漂移誤差和隨機誤差，通過濾波實時估計出當前陀螺儀誤差值，用于修正水平姿態(tài)濾波和航向角濾波中的狀態(tài)空間模型，改善濾波估計精度;在速度濾波中，根據(jù)GSP對靜止載體的速度觀測值，可作為速度誤差觀測量，通過濾波估計出加速度計零偏誤差和載體失準角，加速度零偏可用于水平姿態(tài)角濾波中的觀測量修正，減小觀測誤差，估計的失準角用于對姿態(tài)矩陣作進一步修正，提高姿態(tài)矩陣的穩(wěn)定性。

1.1 水平姿態(tài)角濾波

記地心慣性坐標系i系;地球坐標系e系;以“東－北－天”地理坐標系為導航坐標系n系;以“右－前－上”為載體坐標系b系;計算導航坐標系記為n'系;失準角矢量［?x，?y，?z］T定義為理想導航系 n系依次按 z－x－y順序分別繞?x，?y，?z旋轉(zhuǎn)后得到計算導航系n'系，記載體的俯仰角、橫滾角和航向角分別為θ，γ，ψ且n系繞“z－x－y”軸的順序依次旋轉(zhuǎn)θ，γ，ψ得到b系，則n系到b系的轉(zhuǎn)換矩陣為:

式中，c表示余弦函數(shù)，s表示正弦函數(shù) (下同)。按式(3)(4)(5)，由姿態(tài)矩陣轉(zhuǎn)換為姿態(tài)角:

姿態(tài)矩陣微分方程［7］表示如下:

通過式 (6)可提取出狀態(tài)微分方程:

對式 (9)進行離散化得到水平姿態(tài)角的狀態(tài)空間模型和量測模型如下:

采用式 (12) (13) 分別計算狀態(tài)轉(zhuǎn)移矩陣 ?1，k－1，過程噪聲 w1，k－1［8］:

式中，I3×3為三階的單位矩陣，H1，k為觀測矩陣，ηG為陀螺儀量測噪聲向量，可假設(shè)為零均值的高斯白噪聲，珘x1，k為向量x1，k構(gòu)造的反對稱矩陣，v1，k為加速度計的隨機量測噪聲向量，可假設(shè)為零均值的高斯白噪聲，T為卡爾曼濾波周期。

實際觀測向量z1，k為加速度計在b系中的投影且經(jīng)過誤差補償?shù)妮敵鲋?，?

系統(tǒng)過程噪聲方差矩陣 Q1，k－1和量測噪聲方差矩陣 R1，k計算如下［8］:

在k時刻時，由卡爾曼濾波估計后得到重力矢量在b系的狀態(tài)估計值:

由式 (3)(4)得到載體的俯仰角和橫滾角估計值，之后可給航向角濾波器的觀測值提供輸入。

事實上，當載體處于靜止時，重力矢量在載體系的投影即為加速度計輸出，有如下關(guān)系:

單位化后得到:

將式 (1)代入式 (22)并整理得到:

1.2 航向角濾波

在航向角濾波中，觀測向量來源于水平姿態(tài)濾波的輸出與磁強計輸出融合計算得到，類似于水平姿態(tài)濾波的方法，選取式 (1)中姿態(tài)矩陣的第一列元素作為狀態(tài)量，即:

通過式 (6)可提取出狀態(tài)微分方程:

對式 (26)進行離散化得到水平姿態(tài)角的狀態(tài)空間模型和量測模型如下:

類似于水平姿態(tài)濾波中的方法，得到如下關(guān)系式:

航向角可通過磁強計輸出得到，文中詳細闡述了高精度航向角可通過磁強計與加速度計融合輸出得到［9］［11］，在此不再贅述，由 (23)(24)得到航向角:

至此，在k時刻時，可得到航向角濾波器的觀測值z2，k計算如下:

在靜止狀態(tài)下，為有效利用水平姿態(tài)角濾波數(shù)據(jù)，降低航向角量測誤差，采取求平均的方式提高量測質(zhì)量，即一個航向角量測值為前50個量測值與k時刻量測值的平均值作為當前量測更新:

類似于水平姿態(tài)系統(tǒng)過程噪聲方差矩陣，航向角系統(tǒng)過程噪聲方差矩陣 Q2，k－1計算如下:

同樣使用卡爾曼濾波算法得到狀態(tài)最優(yōu)解:

航向角最優(yōu)估計值ψm如下:

1.3 陀螺儀誤差濾波

采用隨機游走過程表示陀螺儀的誤差(本文僅考慮陀螺儀漂移和隨機誤差)，其系統(tǒng)狀態(tài)空間模型表示如下［10］:

式中，x3，k表示由x，y，z軸三個陀螺儀誤差構(gòu)成的向量，?3，k－1=I3×3為狀態(tài)轉(zhuǎn)移矩陣，H3，k=I3×3為觀測矩陣，w3，k－1和v3，k為過程噪聲和量測噪聲。

在水平姿態(tài)卡爾曼濾波與航向角卡爾曼濾波中，由于陀螺儀誤差的存在，在利用反對稱矩陣計算狀態(tài)轉(zhuǎn)移矩陣 ?1，k－1和 ?2，k－1時，對狀態(tài)量的一步預測狀態(tài)和 x造成誤差傳遞。對此，用分別按式 (19) (20) (38)計算得到姿態(tài)角由狀態(tài)最優(yōu)估計值和采用同樣的方法計算得到的姿態(tài)角θm，γm，ψm，二者必然存在差異，其根本原因有陀螺儀的誤差引起，由此可計算在k時刻時，陀螺儀輸出角速度的誤差觀測量:

類似于航向角濾波器觀測量的方法，對陀螺儀誤差濾波器的觀測量也采取求平均的方法。

1.4 速度濾波

在速度卡爾曼濾波器中，考慮載體在靜止狀態(tài)進行對準，可忽略天向速度的影響，載體的失準角和加速度計誤差將在此得到最優(yōu)估計值，其中，失準角用來校正由式(19)(20) (38)計算的姿態(tài)矩陣，加速度計誤差用來校正水平姿態(tài)濾波器的觀測值。

選取載體速度誤差，失準角和加速度計零偏誤差為系統(tǒng)狀態(tài)量:

建立狀態(tài)空間模型如下:

式中，?4，k－1為狀態(tài)轉(zhuǎn)移矩陣，w4，k－1為過程噪聲，v4，k為量測噪聲，Bx，Bv，Bz為加速度計零偏誤差，H4，k為觀測矩陣，相關(guān)矩陣具體參數(shù)詳見文獻［5］。

由于GPS存在測量誤差，在載體靜止是，GPS輸出的水平速度值 δVE，GPS，δVN，GPS和 δVU，GPS認為是速度誤差觀測向量。

經(jīng)過卡爾曼濾波后得到k時刻的狀態(tài)最優(yōu)估計值:

利用此時刻估計的加速度計零偏誤差修正水平姿態(tài)濾波中式 (15)的加速度計輸出值，進入下一時刻的濾波更新。

1.5 姿態(tài)矩陣修正

以上敘述的四個級聯(lián)式的卡爾曼濾波完成一個濾波周期更新時，得到一組姿態(tài)角及失準角，為獲得更精確的姿態(tài)矩陣，充分利用最優(yōu)估計信息，可采用如下方式:由姿態(tài)濾波器輸出的姿態(tài)角θm，γm，ψm按式 (1)計算出待修正的姿態(tài)矩陣最后利用速度濾波器估計的失準角?x，?v，?z按式(52)對進行最終的修正，得到更精確和穩(wěn)定的估計值，完成整個初始對準任務(wù)，當達到導航預設(shè)條件時，可轉(zhuǎn)入導航狀態(tài)更新解算。

1.6 卡爾曼濾波算法

在上述提到的濾波算法中，均采用經(jīng)典卡爾曼濾波最優(yōu)估計算法。卡爾曼濾波以線性最小方差為估計準則，通過量測值按一定遞推算法實現(xiàn)狀態(tài)向量的估計。假設(shè)系統(tǒng)狀態(tài)空間模型如下:

其遞推算法過程［10］如下:

2)時間更新 (k=1，…，K):計算系統(tǒng)狀態(tài)一步預測估計及一步誤差協(xié)方差矩陣:

3)量測更新 (k=1，…，K):通過量測信息及反饋增益估計系統(tǒng)狀態(tài)及對應(yīng)的誤差協(xié)方差陣:

式中，xk為k時刻的狀態(tài)向量，?k－1為狀態(tài)轉(zhuǎn)移矩陣，wk－1為系統(tǒng)噪聲，vk為量測噪聲，Rk為量測噪聲方差陣，Qk為系統(tǒng)噪聲方差陣，δki為Kronecker符號，滿足:

2 實驗結(jié)果與分析

仿真參數(shù)進行如下設(shè)置:

1)載體靜止的參數(shù)為:緯度，經(jīng)度，和高度分別為34°，108°，380 m，俯仰角，橫滾角和航向角分別為60°，5°，40°，采樣頻率50 Hz，系統(tǒng)仿真時間為600 s;

3)GPS輸出的位置誤差均為10 m，測速誤差0.1 m/s，輸出頻率1 Hz。

4)GPS水平位置誤差10 m，高度誤差20 m，測速精度0．1 m/s，輸出頻率 1 Hz。

在水平姿態(tài)濾波器中，直接采用加速度計的輸出反解載體的俯仰角和橫滾角，稱為解算值，其結(jié)果波動較大(數(shù)據(jù)顯示頻率為1 Hz)，見圖1。然而，經(jīng)過水平姿態(tài)濾波估計后，降低了水平姿態(tài)角輸出的波動程度，有效提高了水平姿態(tài)角的穩(wěn)定性，統(tǒng)計信息見表1，對后續(xù)的航向角濾波器提供高質(zhì)量的觀測信息。

圖1 俯仰角估計

圖2 橫滾角估計

在陀螺儀誤差濾波估計中，陀螺儀誤差僅考慮漂移和隨機誤差，濾波估計結(jié)果見圖3。

_表1 水平姿態(tài)角統(tǒng)計數(shù)據(jù)

表2 穩(wěn)定后的航向角ψ統(tǒng)計數(shù)據(jù)

圖3 陀螺儀誤差估計

在航向角濾波中，從圖4明顯看出，航向角ψ在系統(tǒng)運行100 s后，開始向真實值收斂。然而，在100 s后的收斂過程中，經(jīng)過航向角濾波器估計的航向角 (稱為濾波值)波動范圍較大，且最大航向角誤差在0.03°之內(nèi)，表2統(tǒng)計了航向角ψ的濾波值和修正值在100～600 s之間的數(shù)據(jù)均值與方差;采用速度濾波估計的失準角向量，經(jīng)過式 (52)修正得到姿態(tài)矩陣，經(jīng)過式 (3)(4)(5)換算得到載體的新姿態(tài)角，稱其為修正值，其中俯仰角和橫滾角的變化差異不大，在此不再敘述，航向角修正值最大誤差在0.02°之內(nèi)，重點是修正值能有效提高航向角輸出的穩(wěn)定性，進而表明經(jīng)過修正的姿態(tài)矩陣更穩(wěn)定，為導航解算提供高精度的初始姿態(tài)矩陣。

3 結(jié)束語

本文從分析姿態(tài)矩陣的角度，進一步提出采用級聯(lián)式卡爾曼濾波的方法，對姿態(tài)角、MIMU誤差以及失準角進行估計，各個濾波器之間相互輔助，得到更加準確的狀態(tài)空間模型，仿真實驗表明該方法能有效解決微慣組的初始對準問題。

圖4 航向角估計