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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

1.2 卡爾曼濾波模型的建立

1.2.1 正向Kalman濾波計算

文中選取“北－天－東”地理坐標(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)的初始對準(zhǔn)時間較短，因此陀螺漂移和加速度計零偏均可看成隨機(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（由于后文將涉及正向及逆向兩個狀態(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ù)Kalman濾波狀態(tài)方程

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

（2）量測方程

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

（3）濾波計算

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

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

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

1.2.2 逆向Kalman濾波計算

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實驗驗證

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

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

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

圖6 定位誤差Fig.6 Position error

4 結(jié)論

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

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

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

［3］嚴(yán)恭敏．捷聯(lián)慣導(dǎo)系統(tǒng)動基座初始對準(zhǔn)及其他相關(guān)問題研究［D］．西北工業(yè)大學(xué)博士后研究工作報告，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］付夢印，鄧志紅，閆莉萍．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．