一種基于實時再處理技術的SINS初始對準算法

鄧繼權，馬小艷，張吉先，薛 凱

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

0 引言

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

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

1 基于實時再處理技術的初始對準算法

1.1 初始對準算法分析

為了能夠存儲整個初始對準以及導航過程中的IMU數據并且實現實時再處理技術，整套慣導系統采用了兩個獨立的導航計算機以及大容量的存儲器。其中導航計算機主板 （CPU1）用于實時的正向導航解算，在預定的對準結束時刻立即轉入實時導航。導航計算機副板 （CPU2）實時存儲對準以及導航過程中的IMU數據，并且在CPU1初始對準結束時刻利用其導航結果以及存儲的數據進行雙向導航、雙向濾波實時再處理計算，由于計算機的運算速率遠大于傳感器的采樣率，所以，經過一段時間延遲（T）之后，CPU2計算得到的導航結果也就是實時導航的結果［4-5］。其基本結構如圖1所示，具體步驟如下:

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

2）CPU1卡爾曼濾波精對準結束后，對姿態角、陀螺漂移以及加速度計零偏等相關量進行修正。修正結束后，轉入實時導航階段。CPU2繼續實時存儲陀螺儀、加速度計以及外部參考信息的采樣數據，同時以te時刻CPU1的速度、位置、姿態以及陀螺漂移、加速度計零偏等信息作為初值，進行基于逆向導航算法的逆向Kalman濾波估計，其中逆向陀螺漂移須將CPU1卡爾曼濾波精對準估計得到的值取反。

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

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

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

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

1.2 卡爾曼濾波模型的建立

1.2.1 正向Kalman濾波計算

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

（1）狀態方程

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

根據Kalman濾波狀態方程

式中，Cij（i=1，2，3;j=1，2，3）為姿態轉換矩陣Cbn中相應的元素。

（2）量測方程

慣導系統在Kalman濾波精對準過程中采用零速匹配方式，設tk時刻的量測值為Zk，則量測方程可表示為式 （3）。

（3）濾波計算

Kalman濾波是由 R．E．Kalman于 1960年首次提出。目前，Kalman濾波理論作為一種最重要的最優估計理論被廣泛應用于各種領域，組合導航系統的設計是其成功應用中的一個最主要方面。文獻［8］對Kalman濾波作了詳細直觀的推導，這里直接給出離散型正向Kalman濾波的基本方程:

狀態一步預測

在開始濾波之前需選擇合適的狀態初始值XF0（一般選擇零向量作為狀態初始值）、估計均方誤差初始值PF0以及系統噪聲的初始方差陣Q0，并根據實際系統和應用環境確定適當的量測噪聲方差陣Rk。參數設置完成之后，即可按照上述濾波方程遞推計算系統的狀態估計值。

1.2.2 逆向Kalman濾波計算

對正向捷聯慣性導航算法稍作變化可以得到逆向捷聯慣性導航算法［6］，由分析可以得到，只要將正向算法中的陀螺采樣和地球自轉角速率符號取反，并將正向結束時刻的位置、速度以及姿態信息作為逆向導航算法的初始信息，對采樣數據作逆向處理，即可實現從終點至初始點的逆向導航解算。其中，逆向初始時刻速度須將正向結束時刻的速度值取反。因此，將正向卡爾曼濾波模型中的地球自轉角速率和速度項取反即可得到逆向卡爾曼濾波模型。

記上標“B”表示逆向組合導航過程，則逆向組合導航的系統狀態方程為

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

狀態一步預測

2 數學仿真與分析

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

具體仿真條件設置如下:

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

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

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

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

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

3）設初始對準時間為6min，前10s為系統準備時間，粗對準時間設為50s，精對準階段采用雙位置卡爾曼濾波精對準，在200s時通過航向角轉動90°引入第二位置，360s時對準結束，轉位時間設為10s。

在圖2～圖4中，曲線 （1）表示CPU1正向Kalman濾波精對準估計結果，相當于傳統Kalman濾波對準方法。曲線 （2）表示CPU2逆向Kalman濾波估計結果，曲線 （3）表示CPU2正向Kalman濾波估計結果。

為了更加直觀地體現出CPU2實時再處理技術對系統初始對準精度的改善，表1給出了CPU1對準結束時刻與CPU2實時再處理對準結束時刻的姿態角誤差以及陀螺漂移、加速度計零位估計結果比較。

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

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

3 實驗驗證

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

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

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

圖6 定位誤差Fig.6 Position error

4 結論

本文提出了一種優化的基于實時再處理技術的捷聯慣導系統初始對準方法，該方法基于雙CPU捷聯慣導系統，將雙向導航、雙向濾波相結合，實現了慣導系統的短時間高精度對準。與傳統對準方法相比，該方法可以在較短的時間內取得同等的精度，或者在相同的對準時間內取得更高的對準精度。最后，通過車載試驗進行了驗證，具有一定的工程實用性。

［1］萬德鈞，房建成．慣性導航初始對準［M］．南京:東南大學出版社，1998．

［2］秦永元．慣性導航［M］．北京:科學出版社，2006．

［3］嚴恭敏．捷聯慣導系統動基座初始對準及其他相關問題研究［D］．西北工業大學博士后研究工作報告，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濾波理論及其在導航系統中的應用（第二版）［M］．北京:科學出版社，2010．

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

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