SINS/GPS組合導航的直接狀態估計方法?

代曉巍 趙書圓 張 夢

（1.中國人民解放軍91550部隊 大連 116023）（2.海軍大連艦艇學院 大連 116018）

1 引言

組合導航系統將具有長時穩定性的GPS導航系統輸出與具有短時精確性的捷聯式慣性測量單元（IMU）輸出通過濾波估計算法進行融合，能夠得到相比原系統更為準確和穩定的結果。目前已經成為同時估計運動載體位置速度與姿態（INS狀態）的標準方法。從狀態估計的形式上看，GPS/SINS組合導航可以分為直接估計與間接估計兩種形式［1～4］，即濾波器是通過估計誤差間接地估計位速姿態狀態，還是直接估計出所需要的信息。融合GPS數據與IMU數據最常用的方法即是基于間接估計的卡爾曼濾波方法，將姿態估計誤差，速度估計誤差和位置估計誤差作為濾波模型的狀態矢量，根據慣導力學編排，忽略二階及以上小量，得到具有線性動態的誤差狀態方程，通過估計出的誤差來修正當前的INS變量。得益于其融合模型為線性，具有低復雜性，低運算量和易于實際系統應用實現的優勢。間接估計方法已經在導航領域有長時間的成熟應用。然而隨著搭載平臺計算技術的發展，研究更為精確和高效的方法是必要的［5～8］。

直接估計方法是基于標準的慣導力學編排，將姿態參數、速度矢量與位置矢量以及IMU偏差等變量直接作為濾波模型的狀態矢量。因此，直接估計模型更能準確地描述載體的動態特性，對于大誤差、惡劣環境和載體大機動的條件下具有優勢，且相比間接估計的開環修正方式，直接估計的閉環修正結構也能夠更好地避免和抑制誤差累積傳播。限制直接估計方式應用的主要原因是由于狀態方程的非線性特征導致的計算復雜性、運算量與算法數值問題［9～12］。

本文提出一種基于直接估計方式和擴展卡爾曼濾波的松組合導航方法，首先通過慣導力學編排建立了直接估計狀態模型與松組合的觀測模型，然后在求解雅克比矩陣的基礎上，采用擴展卡爾曼濾波求解。數值仿真結果表明，在對準誤差較大等惡劣的條件下，直接估計方法相比間接估計方法具有更好的性能，但是同時其也存在計算形式復雜、數值處理精度要求高和運算量巨大的缺點。在精確對準和良好條件下，直接估計方式與間接估計方式的估計誤差是相當的，因此前者的適用性會受到其大運算量的限制。

2 INS/GPS組合導航的濾波模型

2.1 慣性導航力學方程

對于直接估計方式，SINS的標準慣性導航力學方程即作為濾波狀態模型。SINS體坐標系為FRD（前-右-下）坐標系，記為b系；當地導航坐標系為NED（北-東-地）坐標系，記為n系，ECEF系記為e系，ECI系記為i系，以四元數表示姿態的標準慣導力學方程為

2.2 間接估計的誤差狀態方程

假設位置、速度以及姿態誤差都是小量，其二階可忽略，那么根據式（1）～（3）可以得到姿態角、速度位置以及誤差狀態方程為

2.3 直接估計的狀態空間模型

根據慣導力學方程導出直接估計的狀態方程，狀態方程主要包括姿態、速度、位置以及IMU測量偏差的演進方程。對于姿態演進方程，式（1）中，并考慮到測量偏差的影響，則可以將式（1）重寫為如下的方程：

NED系平臺相對地球的角速度為

地球轉動角速度在NED系下表示為

陀螺儀測量值可以用如下模型來描述。

其中bg為建模為一階隨機游走的陀螺儀測量偏差，vg為陀螺儀測量白噪聲，ug為隨機游走過程的驅動噪聲。

類似地，加速度計測量值也采用上式給出的模型來描述，即

其中ba為建模為一階隨機游走的加速度計偏差漂移，va為陀螺儀測量白噪聲，ua是隨機游走過程的驅動噪聲。

綜上可以歸納出直接模式狀態量為

由式（2）～（3），式（5）～（6），式（9）～（10）構成函數矢量，這些函數構成的連續時間方程進行離散化，進一步可得到直接估計的狀態方程為

其中f(·)是離散狀態函數，wk-1為過程噪聲矢量。

對于本文選擇的慣導與GPS的松組合方式，直接以GPS給出的位置與速度量測作為觀測值，則可以得到線性的觀測模型為

其中εGPS為GPS的位置和速度測量誤差，觀測矩陣

需要指出的是，實際GPS的位置測量誤差是在NED系下以米為單位給出的，而GPS測量位置的平面坐標是以曲線坐標系下以角度給出的（緯度和經度），因此要通過式（5）將NED系下的測量誤差轉換到緯度和經度單位。

3 數值結果與分析

為了直觀地對比直接估計方法與經典方法對于姿態、速度以及位置等感興趣的INS狀態量的估計性能，本文基于文獻中的GNSS模擬器生成的載體飛行數據進行仿真。飛行數據包括緯、經度及橢球高，北向、東向及地向速度，橫滾角，俯仰角，偏航角。其中姿態角是載體相對于NED系的歐拉角。

為了直觀描述上述載體運動情況，圖1給出了飛行軌跡和姿態角隨時間的變化情況。其中飛行速度為 200m/s，經歷兩個45°轉彎后爬升至500m。加速度計和陀螺儀的讀數首先利用上述運動數據經導航方程變換得到理想的慣性測量讀數，然后在此之上再疊加典型慣性測量單元（IMU）的刻度、正交性、噪聲、偏差以及漂移誤差對理想讀數進行變換得到仿真測量讀數，具體加速度和陀螺儀傳感器誤差設置為加速度計零偏[900，-1300，800]μg ；測量噪聲；刻度誤差與軸耦合誤差矩陣為

圖1 載體飛行過程

加速度相關偏差：

通過上述針對根據模擬載體運動測量數據的處理結果與分析，如圖2～4，在存在較大初始誤差的情況下，經典的間接誤差估計方法在位置估計上出現了大幅的累積誤差。直接狀態估計的濾波方法對于INS信息的估計結果要較大幅度的優于基于誤差傳播模型的卡爾曼濾波的經典方法，且能夠在缺少準確初始條件的情況下收斂到真實值，因此相比間接模式估計具有較好的動態估計性能。

圖2 跟蹤跑動中的人的實例

圖2 直接估計方法與經典方法的位置估計誤差對比

圖3 兩種方法的速度估計誤差對比

圖4 兩種方法的姿態估計誤差對比

4 結語

通過慣性導航力學編排建立的直接形式狀態估計模型與松組合觀測模型。提出了一種基于擴展卡爾曼濾波求解的GPS/SINS組合導航方法，通過基于模擬飛行機動運動數據的數值仿真對提出模型和算法進行了驗證，并與現有方法進行了對比。結果表明，直接估計方法相在惡劣初始誤差條件下的具有較大的優勢。在運算條件充分滿足的條件下，可作為一種在初始對準階段快速提供準確精確初值的濾波方法。下一步研究的重點是其在實際條件下的誤差特性與高效計算的方法。