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

張長(zhǎng)遠(yuǎn)

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

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

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

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

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

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

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

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

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

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

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

D-GPS/IMU觀測(cè)性

如果矩陣A可逆，則可以通過對(duì)V的第四行左乘[e1X] A-1加到第三行，變換為三角矩陣，因?yàn)樵撊蔷仃嚨乃袑?duì)角線子矩陣都是可逆的所以觀測(cè)矩陣V滿秩。

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

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

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

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

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

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

同時(shí)，△e和a之間的夾角可以通過下式兩個(gè)向量之間點(diǎn)乘公式來求出：

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

三、仿真分析

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

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

四、結(jié)束語

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

參 考 文 獻(xiàn)

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

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