基于D-GPS/IMU的組合導航方法研究與分析

張長遠

【摘 要】 研究了一種新的雙GPS單元與IMU慣性測量單元組合結構（D-GPS/IMU）的導航應用問題；基于D-GPS/IMU的動力模型結構，分析了其狀態空間的可觀測性能，提出并證明了系統完全可觀測的條件；最后，通過仿真實驗，結果表明，該方法收斂穩定且快、組合誤差小，具有一定的實用性。

【關鍵詞】 組合導航 可觀測性 擴展卡爾曼濾波 自適應

一、D-GPS/IMU動力模型的構造

D-GPS/IMU的結構組成如圖1所示。其中的{I}為慣性坐標系，它的坐標原點在GPS基準天線處；{b}為機體坐標系，它的坐標原點位于IMU的幾何中心并與IMU的坐標軸平行。P1、P2、r分別表示在{I}中GPS1、GPS2和IMU的向量；e1、e2、△e分別表示在{b}中GPS1、GPS2和兩衛星間連線的向量。{b}系相對于{i}的方向用四元素q=[qTvqTo]T來表示，其中的下標v和o分別表示四元素的矢量部分和標量部分。則由四元素表示的載體旋轉矩陣A（q）為：

A（q）=（2q2v-1）I3+2qo[qvX]+2qvqTv （1）

其中：[·X]表示向量的叉乘矩陣形式（反對稱矩陣），In表示nXn的單位矩陣。

由圖1知，GPS天線相對于坐標系{i}的位置矢量計算公式為：

定義：D-GPS/IMU模型的狀態向量為x=[qTv、rT、rT、bT]T，其中b為陀螺漂移，非線性系統的觀測方程為： z=h（x）+v

二、D-GPS/IMU組合導航方法的觀測性分析

組合導航系統的觀測性能對系統量測結果的輸出會產生很大的影響。不完全可觀測的系統會引起閉環回路的量測結果估計失效，同時不能保證輸出結果收斂到期望真值。最大效率的利用KF算法要求系統是完全可觀測的。

對于線性時變系統，當且僅當它的觀測矩陣滿秩的時候系統是全局可觀測的。如果組合導航系統全局可觀測，則輸出的估計誤差僅僅為系統噪聲，而狀態初始值對系統產生的誤差影響將逐步消失，這樣就不會出現原時變系統模型的不穩定的行為特征，增強了系統的穩定性，下面對D-GPS/IMU結構模型的觀測性進行分析。

D-GPS/IMU觀測性

如果矩陣A可逆，則可以通過對V的第四行左乘[e1X] A-1加到第三行，變換為三角矩陣，因為該三角矩陣的所有對角線子矩陣都是可逆的所以觀測矩陣V滿秩。

推論：如果連接兩個GPS天線間的基線向量△e和加速度向量q不平行，則D-GPS/IMU模型（11）為完全可觀測系統。

證明：如果△ea，則D-GPS/IMU模型為完全可觀測系統（A為滿秩矩陣）。如果A不滿秩，則一定存在非零向量ξ≠0，使Aξ=0成立，變換為：

[ξx]△e-λ△e=0 （7a）

其中：λ=ξT（△e·a） （7b）

式（7a）為反對稱矩陣[ξX]的特征方程表達式。根據反對稱矩陣的實特征值僅為零的性質知，λ=0，ξ=△e代入式（7b），得出：△e·（△e·a）=0 （7c）

僅僅當非零向量△e和加速度向量a平行時，式（7c）才會成立，這與前提條件矛盾，因此△ξ=0不可能成立，即D-GPS/IMU模型為完全可觀測系統（A為滿秩矩陣）。

同時，△e和a之間的夾角可以通過下式兩個向量之間點乘公式來求出：

可以實時地通過檢測θ的角度變化來判斷D-GPS/IMU系統模型的可觀測性能。

三、仿真分析

按照相同的飛行軌跡，采用以下兩種不同的結構模型和處理算法進行對比分析驗證：仿真飛行路線和參數設置為：初始位置（34.2 108.9 0），初始速度為0m/s，初始姿態誤差角為2'，陀螺和加計的噪聲方差分別為σg=2·10-5rad/ s，σa=0.25m/s2， 位置和速度初始誤差分別為5m和0.1m/s，IMU和GPS的采樣速率為20Hz和1Hz，算法濾波周期為1s，滑動長度為L=5且θ≠0。經過爬升、轉彎、勻速平飛、加速爬升等飛行階段，仿真時間為600s，其飛行軌跡如圖2所示。

在兩種結構模型中系統狀態解算后的速度和位置誤差是直接可觀測的，而姿態誤差有著不同的可觀測性。所以，可將姿態誤差估計作為參考狀態量。D-GPS/IMU結構模型的姿態誤差估計控制在±0.1°以內，航向誤差估計控制在±0.2°以內。在仿真的200～300 s和500～550s中，仿真飛行軌跡的航向變化劇烈（即兩次大轉彎），使D-GPS/IMU解算的姿態誤差角估計在仿真飛行中都保持穩定收斂性和較小的估計誤差。

四、結束語

（1）建立了D-gps/IMU的非線性化的動力學模型，并應用線性化處理方法建立了相應的線性化的動力學模型；（2）分別對D-gps/IMU和S-gps-IMU兩種結構模型的狀態空間的現測性能進行了分析，確定了D-gps/IMU完全可現測的條件；（3）EKF算法中的增益值根據定義的GPS量測殘差值進行了調整，殘差協方差應用區間反向平滑策略，來提高算法收斂速度和穩定性；通過仿真實驗來說明本文的結構模型和算法的有效性。D-gps/IMU模型在滿足一定條件時為可觀測系統，但增加了硬件成本，同時算法的復雜度也增加了，下一步將在提升算法效率和減小硬件成本上進行優化處理。

參 考 文 獻

[1]張濤，徐曉蘇，李瑤，宮淑萍.基于慣導及水下聲學輔助系統的AUV容錯導航技術[J].中國慣性技術學報，2013，04：512-516.

[2]任沁源.基于視覺信息的微小型無人直升機地標識別與位姿估計研究[D].浙江大學，2008.