基于D-GPS/IMU的組合導(dǎo)航方法研究與分析

張長遠(yuǎn)

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

【關(guān)鍵詞】 組合導(dǎo)航 可觀測性 擴展卡爾曼濾波 自適應(yīng)

一、D-GPS/IMU動力模型的構(gòu)造

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

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

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

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

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

二、D-GPS/IMU組合導(dǎo)航方法的觀測性分析

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

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

D-GPS/IMU觀測性

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

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

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

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

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

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

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

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

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

三、仿真分析

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

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

四、結(jié)束語

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

參 考 文 獻(xiàn)

[1]張濤，徐曉蘇，李瑤，宮淑萍.基于慣導(dǎo)及水下聲學(xué)輔助系統(tǒng)的AUV容錯導(dǎo)航技術(shù)[J].中國慣性技術(shù)學(xué)報，2013，04：512-516.

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