一種基于實(shí)時(shí)再處理技術(shù)的SINS初始對(duì)準(zhǔn)算法

鄧?yán)^權(quán)，馬小艷，張吉先，薛 凱

（1.北京自動(dòng)化控制設(shè)備研究所，北京 100074;2.北京空天技術(shù)研究所，北京 100074）

0 引言

初始對(duì)準(zhǔn)是慣導(dǎo)系統(tǒng)進(jìn)入導(dǎo)航的前提，也是慣導(dǎo)系統(tǒng)的關(guān)鍵技術(shù)之一。初始對(duì)準(zhǔn)的兩個(gè)重要指標(biāo)是精確性和快速性，但是在自對(duì)準(zhǔn)中這兩者之間往往是相互矛盾的［1］。傳統(tǒng)初始對(duì)準(zhǔn)方法如果要在其他條件不變的情況下提高對(duì)準(zhǔn)精度，只能通過延長(zhǎng)初始對(duì)準(zhǔn)時(shí)間來實(shí)現(xiàn)。而通常情況下，慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)時(shí)間都有嚴(yán)格限制，不可能無限延長(zhǎng)，因此，如何在特定的初始對(duì)準(zhǔn)時(shí)間下提高系統(tǒng)初始對(duì)準(zhǔn)精度是個(gè)很大的難題。

可以將SINS中陀螺儀和加速度計(jì)的采樣數(shù)據(jù)當(dāng)作一組時(shí)間序列看待，通常意義下的導(dǎo)航解算是對(duì)該序列按時(shí)間先后順序進(jìn)行實(shí)時(shí)處理，而不必進(jìn)行數(shù)據(jù)存儲(chǔ)，就能夠獲得實(shí)時(shí)導(dǎo)航結(jié)果。如果導(dǎo)航計(jì)算機(jī)存儲(chǔ)容量足夠大，能夠把IMU以及外部參考信息采樣數(shù)據(jù)存儲(chǔ)下來，那么既然可以按時(shí)間順序正向處理，很容易聯(lián)想到，也可以對(duì)它作逆向分析和處理。對(duì)存儲(chǔ)的數(shù)據(jù)作正向和逆向的反復(fù)分析，有可能提高精度，或者在某項(xiàng)任務(wù)中能夠減小實(shí)際用于分析的數(shù)據(jù)長(zhǎng)度，即縮短任務(wù)時(shí)間［2-3］。文獻(xiàn)［5］利用對(duì)準(zhǔn)過程存儲(chǔ)的數(shù)據(jù)進(jìn)行多次Kalman濾波迭代計(jì)算以提高初始對(duì)準(zhǔn)精度。本文提出一種優(yōu)化的基于實(shí)時(shí)再處理技術(shù)的初始對(duì)準(zhǔn)方法，在初始對(duì)準(zhǔn)的不同階段均利用相同的數(shù)據(jù)，進(jìn)行多次正向、逆向處理，有利于實(shí)現(xiàn)慣導(dǎo)系統(tǒng)短時(shí)間高精度初始對(duì)準(zhǔn)。

1 基于實(shí)時(shí)再處理技術(shù)的初始對(duì)準(zhǔn)算法

1.1 初始對(duì)準(zhǔn)算法分析

為了能夠存儲(chǔ)整個(gè)初始對(duì)準(zhǔn)以及導(dǎo)航過程中的IMU數(shù)據(jù)并且實(shí)現(xiàn)實(shí)時(shí)再處理技術(shù)，整套慣導(dǎo)系統(tǒng)采用了兩個(gè)獨(dú)立的導(dǎo)航計(jì)算機(jī)以及大容量的存儲(chǔ)器。其中導(dǎo)航計(jì)算機(jī)主板 （CPU1）用于實(shí)時(shí)的正向?qū)Ш浇馑悖陬A(yù)定的對(duì)準(zhǔn)結(jié)束時(shí)刻立即轉(zhuǎn)入實(shí)時(shí)導(dǎo)航。導(dǎo)航計(jì)算機(jī)副板 （CPU2）實(shí)時(shí)存儲(chǔ)對(duì)準(zhǔn)以及導(dǎo)航過程中的IMU數(shù)據(jù)，并且在CPU1初始對(duì)準(zhǔn)結(jié)束時(shí)刻利用其導(dǎo)航結(jié)果以及存儲(chǔ)的數(shù)據(jù)進(jìn)行雙向?qū)Ш健㈦p向?yàn)V波實(shí)時(shí)再處理計(jì)算，由于計(jì)算機(jī)的運(yùn)算速率遠(yuǎn)大于傳感器的采樣率，所以，經(jīng)過一段時(shí)間延遲（T）之后，CPU2計(jì)算得到的導(dǎo)航結(jié)果也就是實(shí)時(shí)導(dǎo)航的結(jié)果［4-5］。其基本結(jié)構(gòu)如圖1所示，具體步驟如下:

1）系統(tǒng)通電，CPU1先進(jìn)入粗對(duì)準(zhǔn)階段，粗對(duì)準(zhǔn)結(jié)束后，進(jìn)入Kalman濾波精對(duì)準(zhǔn)階段。CPU2將整個(gè)對(duì)準(zhǔn)時(shí)間內(nèi) （設(shè)從t0時(shí)刻開始到te時(shí)刻結(jié)束）的陀螺儀、加速度計(jì)以及外部參考信息的采樣數(shù)據(jù)存儲(chǔ)下來。

2）CPU1卡爾曼濾波精對(duì)準(zhǔn)結(jié)束后，對(duì)姿態(tài)角、陀螺漂移以及加速度計(jì)零偏等相關(guān)量進(jìn)行修正。修正結(jié)束后，轉(zhuǎn)入實(shí)時(shí)導(dǎo)航階段。CPU2繼續(xù)實(shí)時(shí)存儲(chǔ)陀螺儀、加速度計(jì)以及外部參考信息的采樣數(shù)據(jù)，同時(shí)以te時(shí)刻CPU1的速度、位置、姿態(tài)以及陀螺漂移、加速度計(jì)零偏等信息作為初值，進(jìn)行基于逆向?qū)Ш剿惴ǖ哪嫦騅alman濾波估計(jì)，其中逆向陀螺漂移須將CPU1卡爾曼濾波精對(duì)準(zhǔn)估計(jì)得到的值取反。

圖1 基于實(shí)時(shí)再處理技術(shù)的初始對(duì)準(zhǔn)原理圖Fig.1 The initial alignment algorithm based on realtime-reprocessing

3）CPU2逆向Kalman濾波至t0時(shí)刻后，逆向Kalman濾波結(jié)束，繼續(xù)以t0時(shí)刻數(shù)據(jù)作為初始值進(jìn)行正向Kalman濾波估計(jì)。逆向?yàn)V波估計(jì)結(jié)束后，并不進(jìn)行任何修正，直接將逆向?yàn)V波結(jié)束時(shí)刻的狀態(tài)估計(jì)值conv_x^k以及估計(jì)均方誤差陣conv_Pk作為正向?yàn)V波估計(jì)的初始狀態(tài)估計(jì)值和初始均方誤差陣，狀態(tài)估計(jì)值中速度以及陀螺漂移對(duì)應(yīng)項(xiàng)同樣取反。初始速度、位置、姿態(tài)信息取逆向估計(jì)結(jié)束時(shí)刻的值。

4）CPU2正向?yàn)V波估計(jì)至te時(shí)刻后，CPU2正向Kalman濾波精對(duì)準(zhǔn)計(jì)算結(jié)束，對(duì)姿態(tài)角、陀螺漂移以及加速度計(jì)零偏等相關(guān)量進(jìn)行修正，修正結(jié)束后轉(zhuǎn)入導(dǎo)航階段。CPU2導(dǎo)航初始一段時(shí)間內(nèi)所用的數(shù)據(jù)仍然是存儲(chǔ)的數(shù)據(jù)，由于計(jì)算機(jī)運(yùn)算速率遠(yuǎn)大于傳感器的采樣頻率，經(jīng)歷一段時(shí)間延遲T之后，CPU2導(dǎo)航運(yùn)算所用的數(shù)據(jù)將是實(shí)時(shí)采樣的數(shù)據(jù)。因此，CPU2導(dǎo)航解算得到的結(jié)果也將是實(shí)時(shí)導(dǎo)航的結(jié)果，并且精度要高于CPU1導(dǎo)航解算得到的結(jié)果。用精度更高的CPU2導(dǎo)航結(jié)果對(duì)CPU1導(dǎo)航結(jié)果進(jìn)行修正，提高了系統(tǒng)實(shí)時(shí)導(dǎo)航的定位精度。CPU2實(shí)時(shí)再處理對(duì)準(zhǔn)運(yùn)算結(jié)束，CPU1繼續(xù)進(jìn)行實(shí)時(shí)定位導(dǎo)航。

由上述分析可以看出，CPU2的導(dǎo)航運(yùn)算并沒有粗對(duì)準(zhǔn)計(jì)算，CPU2通過實(shí)時(shí)再處理技術(shù)對(duì)整個(gè)初始對(duì)準(zhǔn)過程中的數(shù)據(jù)都進(jìn)行了再次處理，提高了數(shù)據(jù)的利用率。此外，CPU2逆向Kalman濾波結(jié)束后并不做任何修正，使得CPU2一次實(shí)時(shí)再處理Kalman濾波精對(duì)準(zhǔn)計(jì)算的時(shí)間要長(zhǎng)于CPU1初始對(duì)準(zhǔn)時(shí)間，可以有效提高CPU2實(shí)時(shí)再處理Kalman濾波估計(jì)的穩(wěn)定性。其中，延遲時(shí)間T主要取決于導(dǎo)航計(jì)算機(jī)副板 （CPU2）的運(yùn)算能力。

1.2 卡爾曼濾波模型的建立

1.2.1 正向Kalman濾波計(jì)算

文中選取“北－天－東”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系 （OnXnYnZn），原點(diǎn)與載體系原點(diǎn)重合，Xn軸指北，Yn軸指天，Zn軸指東。XnYnZn構(gòu)成右手坐標(biāo)系。載體坐標(biāo)系（ObXbYbZb）原點(diǎn)位于載體重心，Xb軸沿載體縱軸向前，Yb軸垂直載體縱軸向上，Zb軸按右手坐標(biāo)系。

（1）狀態(tài)方程

由于捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)時(shí)間較短，因此陀螺漂移和加速度計(jì)零偏均可看成隨機(jī)常數(shù)過程。另外，由于捷聯(lián)慣性導(dǎo)航系統(tǒng)的高度通道是不穩(wěn)定的，所以忽略垂直方向的速度以及高度，最終選取系統(tǒng)的狀態(tài)變量XF=［δVNδVEφNφUφE δφ δλ εxεyεz?x?y?z］T（由于后文將涉及正向及逆向兩個(gè)狀態(tài)的濾波，這里的上標(biāo)F代表正向Kalman濾波，以作區(qū)別。），δVN、δVE分別為系統(tǒng)在導(dǎo)航坐標(biāo)系下北向、東向速度誤差;φN、φU、φE分別為導(dǎo)航系統(tǒng)的北向、天向、東向失準(zhǔn)角;δφ、δλ分別為系統(tǒng)的緯度、經(jīng)度誤差;εbx、εby、εbz和?b x、 ?by、?bz分別為載體系各軸向的陀螺漂移和加速度計(jì)零偏。

根據(jù)Kalman濾波狀態(tài)方程

式中，Cij（i=1，2，3;j=1，2，3）為姿態(tài)轉(zhuǎn)換矩陣Cbn中相應(yīng)的元素。

（2）量測(cè)方程

慣導(dǎo)系統(tǒng)在Kalman濾波精對(duì)準(zhǔn)過程中采用零速匹配方式，設(shè)tk時(shí)刻的量測(cè)值為Zk，則量測(cè)方程可表示為式 （3）。

（3）濾波計(jì)算

Kalman濾波是由 R．E．Kalman于 1960年首次提出。目前，Kalman濾波理論作為一種最重要的最優(yōu)估計(jì)理論被廣泛應(yīng)用于各種領(lǐng)域，組合導(dǎo)航系統(tǒng)的設(shè)計(jì)是其成功應(yīng)用中的一個(gè)最主要方面。文獻(xiàn)［8］對(duì)Kalman濾波作了詳細(xì)直觀的推導(dǎo)，這里直接給出離散型正向Kalman濾波的基本方程:

狀態(tài)一步預(yù)測(cè)

在開始濾波之前需選擇合適的狀態(tài)初始值XF0（一般選擇零向量作為狀態(tài)初始值）、估計(jì)均方誤差初始值PF0以及系統(tǒng)噪聲的初始方差陣Q0，并根據(jù)實(shí)際系統(tǒng)和應(yīng)用環(huán)境確定適當(dāng)?shù)牧繙y(cè)噪聲方差陣Rk。參數(shù)設(shè)置完成之后，即可按照上述濾波方程遞推計(jì)算系統(tǒng)的狀態(tài)估計(jì)值。

1.2.2 逆向Kalman濾波計(jì)算

對(duì)正向捷聯(lián)慣性導(dǎo)航算法稍作變化可以得到逆向捷聯(lián)慣性導(dǎo)航算法［6］，由分析可以得到，只要將正向算法中的陀螺采樣和地球自轉(zhuǎn)角速率符號(hào)取反，并將正向結(jié)束時(shí)刻的位置、速度以及姿態(tài)信息作為逆向?qū)Ш剿惴ǖ某跏夹畔ⅲ瑢?duì)采樣數(shù)據(jù)作逆向處理，即可實(shí)現(xiàn)從終點(diǎn)至初始點(diǎn)的逆向?qū)Ш浇馑恪Ｆ渲校嫦虺跏紩r(shí)刻速度須將正向結(jié)束時(shí)刻的速度值取反。因此，將正向卡爾曼濾波模型中的地球自轉(zhuǎn)角速率和速度項(xiàng)取反即可得到逆向卡爾曼濾波模型。

記上標(biāo)“B”表示逆向組合導(dǎo)航過程，則逆向組合導(dǎo)航的系統(tǒng)狀態(tài)方程為

參照正向Kalman濾波計(jì)算，選擇合適的反向狀態(tài)初始值XB0、反向估計(jì)均方誤差初始值PB0、系統(tǒng)噪聲的初始方差陣Q0及測(cè)量噪聲方差陣R0。直接可得逆向Kalman濾波計(jì)算過程如下:

狀態(tài)一步預(yù)測(cè)

2 數(shù)學(xué)仿真與分析

為了能夠有效的將陀螺漂移以及加速度計(jì)零偏估計(jì)出來，在仿真過程中，采用雙位置Kalman濾波精對(duì)準(zhǔn)方法。為了驗(yàn)證該技術(shù)的可行性，不妨假設(shè)導(dǎo)航計(jì)算機(jī)副板 （CPU2）的運(yùn)算能力無限大，即延遲時(shí)間T等于零。具體仿真結(jié)果如圖2～圖4所示。對(duì)于實(shí)際系統(tǒng)可根據(jù)需要進(jìn)行多次正、逆向迭代計(jì)算，迭代次數(shù)越多，延遲時(shí)間T越長(zhǎng)。

具體仿真條件設(shè)置如下:

1）運(yùn)載體初始緯度 φ=39.8°， 經(jīng)度 λ=116.2°，高度h=80m，初始航向角ψ=－90°，俯仰角θ=0°，滾動(dòng)角γ=0°。

圖2 航向角誤差Fig.2 Heading error

圖3 水平陀螺漂移估計(jì)Fig.3 Kalman filter results of gyro-drift

2）設(shè)陀螺常值漂移為0.02（°）/h，加速度計(jì)常值零偏為100μg。

圖4 水平加速度計(jì)零位估計(jì)Fig.4 Kalman filter results of accelerator-bias

3）設(shè)初始對(duì)準(zhǔn)時(shí)間為6min，前10s為系統(tǒng)準(zhǔn)備時(shí)間，粗對(duì)準(zhǔn)時(shí)間設(shè)為50s，精對(duì)準(zhǔn)階段采用雙位置卡爾曼濾波精對(duì)準(zhǔn)，在200s時(shí)通過航向角轉(zhuǎn)動(dòng)90°引入第二位置，360s時(shí)對(duì)準(zhǔn)結(jié)束，轉(zhuǎn)位時(shí)間設(shè)為10s。

在圖2～圖4中，曲線 （1）表示CPU1正向Kalman濾波精對(duì)準(zhǔn)估計(jì)結(jié)果，相當(dāng)于傳統(tǒng)Kalman濾波對(duì)準(zhǔn)方法。曲線 （2）表示CPU2逆向Kalman濾波估計(jì)結(jié)果，曲線 （3）表示CPU2正向Kalman濾波估計(jì)結(jié)果。

為了更加直觀地體現(xiàn)出CPU2實(shí)時(shí)再處理技術(shù)對(duì)系統(tǒng)初始對(duì)準(zhǔn)精度的改善，表1給出了CPU1對(duì)準(zhǔn)結(jié)束時(shí)刻與CPU2實(shí)時(shí)再處理對(duì)準(zhǔn)結(jié)束時(shí)刻的姿態(tài)角誤差以及陀螺漂移、加速度計(jì)零位估計(jì)結(jié)果比較。

表1 CPU1初始對(duì)準(zhǔn)結(jié)果與CPU2實(shí)時(shí)再處理對(duì)準(zhǔn)結(jié)果比較Tab.1 The comparison of CPU2 realtime-reprocessing result and CPU1 normal initial alignment result

從仿真結(jié)果可以看出，粗對(duì)準(zhǔn)結(jié)束時(shí)刻，航向角誤差約為5.67'，經(jīng)過CPU1 Kalman精對(duì)準(zhǔn)濾波之后，航向角誤差收斂到2.21'左右。而CPU2通過實(shí)時(shí)再處理技術(shù)使航向角誤差收斂到0.1'左右，使航向角更加接近于真實(shí)值。從滾動(dòng)角和俯仰角也可以得到類似的結(jié)論，但是由于Kalman濾波水平失準(zhǔn)角收斂速度較快，CPU2實(shí)時(shí)再處理后對(duì)滾動(dòng)角和俯仰角精度的改善非常有限 （0.01'以內(nèi)）。此外，由于CPU1 Kalman濾波時(shí)間較短，導(dǎo)致陀螺漂移、加速度計(jì)零位估計(jì)不夠準(zhǔn)確，而CPU2通過實(shí)時(shí)再處理技術(shù)在CPU1濾波的基礎(chǔ)上，通過雙向?qū)Ш健㈦p向?yàn)V波對(duì)陀螺漂移以及加速度計(jì)零位進(jìn)行再次估計(jì)，提高了估計(jì)精度。

3 實(shí)驗(yàn)驗(yàn)證

為了進(jìn)一步驗(yàn)證上述算法的有效性，應(yīng)用此對(duì)準(zhǔn)方法對(duì)該光纖陀螺慣導(dǎo)系統(tǒng)進(jìn)行了車載實(shí)驗(yàn)驗(yàn)證。該慣導(dǎo)系統(tǒng)采用6min雙位置初始對(duì)準(zhǔn)，CPU2進(jìn)行三次逆向、正向?yàn)V波實(shí)時(shí)再處理計(jì)算。在車載試驗(yàn)中接入GPS信息作為慣導(dǎo)系統(tǒng)純慣性導(dǎo)航的基準(zhǔn)。為了直觀地體現(xiàn)基于實(shí)時(shí)再處理技術(shù)初始對(duì)準(zhǔn)算法的有效性，將試驗(yàn)采集到的數(shù)據(jù)用常規(guī)對(duì)準(zhǔn)方法進(jìn)行仿真，比較兩種情況下系統(tǒng)純慣性導(dǎo)航1h的定位誤差。

從車載試驗(yàn)結(jié)果可以得到，采用基于實(shí)時(shí)再處理技術(shù)的初始對(duì)準(zhǔn)方法后，系統(tǒng)純慣性導(dǎo)航1h的最大定位誤差由3.93nmile減少到1.79nmile，有效地提高了系統(tǒng)純慣性導(dǎo)航定位精度。車載試驗(yàn)結(jié)果進(jìn)一步驗(yàn)證了文中提出的初始對(duì)準(zhǔn)算法的有效性及可行性。

圖5 水平速度誤差Fig.5 Velocity error

圖6 定位誤差Fig.6 Position error

4 結(jié)論

本文提出了一種優(yōu)化的基于實(shí)時(shí)再處理技術(shù)的捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)方法，該方法基于雙CPU捷聯(lián)慣導(dǎo)系統(tǒng)，將雙向?qū)Ш健㈦p向?yàn)V波相結(jié)合，實(shí)現(xiàn)了慣導(dǎo)系統(tǒng)的短時(shí)間高精度對(duì)準(zhǔn)。與傳統(tǒng)對(duì)準(zhǔn)方法相比，該方法可以在較短的時(shí)間內(nèi)取得同等的精度，或者在相同的對(duì)準(zhǔn)時(shí)間內(nèi)取得更高的對(duì)準(zhǔn)精度。最后，通過車載試驗(yàn)進(jìn)行了驗(yàn)證，具有一定的工程實(shí)用性。

［1］萬德鈞，房建成．慣性導(dǎo)航初始對(duì)準(zhǔn)［M］．南京:東南大學(xué)出版社，1998．

［2］秦永元．慣性導(dǎo)航［M］．北京:科學(xué)出版社，2006．

［3］嚴(yán)恭敏．捷聯(lián)慣導(dǎo)系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)及其他相關(guān)問題研究［D］．西北工業(yè)大學(xué)博士后研究工作報(bào)告，2008．

［4］Zhang Chuanbin，Tian Weifeng，Jin Zhihua．A novel method improving the alignment accuracy of a strapdown inertial navigation system on a stationary base［J］．Measurement Science And Technology，2004，15:756-769．

［5］I Clay Thompson，Jr，Kenneth S Morgan．Rapid self-alignment of a strapdown inertial system through real-time reprocessing［P］．US，US7739045，2006-5-31．

［6］Kenneth S Morgan，I Clay Thompson，Jr．High speed gyrocompass alignment via multiple Kalman filter based hypothesis testing［P］．US，US7512493，2006-5-31．

［7］Goshen-Meskin D，Bar-Itzhack I Y．Observability analysis of piece-wise constantsystem-partI:Theory［J］． IEEE Transactions on Aerospace and Electronics Systems，1992，28（4）:1056-1067．

［8］付夢(mèng)印，鄧志紅，閆莉萍．Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用（第二版）［M］．北京:科學(xué)出版社，2010．

［9］David H Tittertion，John L Weston．Strapdown inertial navigation technology（2ndEdition） ［M］．United Kingdom:The Institution of Electrical Engineers，2004．

［10］秦永元，張洪鉞，汪叔華．卡爾曼濾波與組合導(dǎo)航原理［M］．西安:西北工業(yè)大學(xué)出版社，1998．