張復(fù)建，單 斌，楊 波，薛 亮，騰紅磊

(火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院，西安 710025)

0 引言

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

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

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

1 初始對(duì)準(zhǔn)原理

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

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

記地心慣性坐標(biāo)系i系;地球坐標(biāo)系e系;以“東－北－天”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系n系;以“右－前－上”為載體坐標(biāo)系b系;計(jì)算導(dǎo)航坐標(biāo)系記為n'系;失準(zhǔn)角矢量［?x，?y，?z］T定義為理想導(dǎo)航系 n系依次按 z－x－y順序分別繞?x，?y，?z旋轉(zhuǎn)后得到計(jì)算導(dǎo)航系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］表示如下:

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

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

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

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

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

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

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

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

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

單位化后得到:

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

1.2 航向角濾波

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

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

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

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

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

至此，在k時(shí)刻時(shí)，可得到航向角濾波器的觀測(cè)值z(mì)2，k計(jì)算如下:

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

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

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

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

1.3 陀螺儀誤差濾波

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

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

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

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

1.4 速度濾波

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

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

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

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

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

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

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

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

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

1.6 卡爾曼濾波算法

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

其遞推算法過(guò)程［10］如下:

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

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

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

2 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

圖1 俯仰角估計(jì)

圖2 橫滾角估計(jì)

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

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

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

圖3 陀螺儀誤差估計(jì)

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

3 結(jié)束語(yǔ)

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

圖4 航向角估計(jì)