MIMU/GPS系統(tǒng)在衛(wèi)星不足時(shí)的導(dǎo)航算法研究*

張復(fù)建，單斌，楊波，王躍鋼，騰紅磊

(火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，陜西 西安 710025)

0 引言

低成本的微型慣性測(cè)量單元(miniature inertial measurement units,MIMU)(簡(jiǎn)稱(chēng)：微慣組)具有成本低、功耗低、體積小優(yōu)勢(shì)，在許多應(yīng)用中，逐漸取代傳統(tǒng)的陀螺儀和加速度計(jì)[1]。然而，由于受微機(jī)電技術(shù)工藝等所限，微慣組的量測(cè)噪聲、零偏以及漂移等誤差較大，其測(cè)量精度相對(duì)傳統(tǒng)的慣性器件要低很多。因此，基于MIMU純慣性導(dǎo)航系統(tǒng)的導(dǎo)航誤差隨時(shí)間不斷發(fā)散，為解決這個(gè)問(wèn)題，許多學(xué)者采用衛(wèi)星與慣導(dǎo)進(jìn)行組合導(dǎo)航[2-3]，抑制誤差發(fā)散，工程上最常用的是慣導(dǎo)/衛(wèi)星松組合導(dǎo)航系統(tǒng)，由于其結(jié)構(gòu)簡(jiǎn)單，慣導(dǎo)系統(tǒng)與GPS衛(wèi)星系統(tǒng)分別獨(dú)立進(jìn)行工作，二者在組合模式上進(jìn)行優(yōu)勢(shì)互補(bǔ)，使得組合導(dǎo)航系統(tǒng)可以長(zhǎng)時(shí)間保持高精度的導(dǎo)航輸出。然而，當(dāng)衛(wèi)星數(shù)量少于4顆時(shí)，由于GPS接收機(jī)的時(shí)鐘誤差無(wú)法消除，使得定位方程的方程數(shù)量小于待求解未知變量的數(shù)量，系統(tǒng)不能正常工作，沒(méi)有GPS系統(tǒng)穩(wěn)定的輸出對(duì)慣導(dǎo)系統(tǒng)輸出進(jìn)行修正，使得MIMU/GPS松組合導(dǎo)航系統(tǒng)變成純慣導(dǎo)解算，長(zhǎng)時(shí)間的導(dǎo)航精度難以得到滿足實(shí)際需要[4-5]。針對(duì)衛(wèi)星數(shù)量小于4顆時(shí)，可考慮采用衛(wèi)星與慣導(dǎo)進(jìn)行緊組合，以GPS接收機(jī)接收的原始偽距信息為觀測(cè)信息內(nèi)容，實(shí)現(xiàn)MIMU/GPS緊組合導(dǎo)航解算。當(dāng)可觀測(cè)衛(wèi)星數(shù)量為2顆或1顆時(shí)，MIMU/GPS緊組合系統(tǒng)輸出的導(dǎo)航精度相比4顆衛(wèi)星時(shí)大幅度下降，這是由于系統(tǒng)的可觀性降低[6-8]，組合導(dǎo)航系統(tǒng)性能下降；當(dāng)可觀測(cè)衛(wèi)星數(shù)量為3顆時(shí)，雖然能實(shí)現(xiàn)組合導(dǎo)航的功能，但精度相比于4顆衛(wèi)星會(huì)有所下降，尤其在載體處于機(jī)動(dòng)轉(zhuǎn)彎或改變運(yùn)動(dòng)方向狀態(tài)期間，導(dǎo)航精度下降最為明顯[9]。對(duì)此，當(dāng)只有3顆可觀測(cè)衛(wèi)星時(shí)，本文針對(duì)MIMU/GPS緊組合導(dǎo)航在機(jī)動(dòng)改變運(yùn)動(dòng)狀態(tài)時(shí)精度下降的問(wèn)題，設(shè)計(jì)姿態(tài)誤差觀測(cè)信息，對(duì)此展開(kāi)研究。

1 MIMU/GPS緊組合導(dǎo)航系統(tǒng)

記地心慣性坐標(biāo)系i系；地球坐標(biāo)系e系；以“東-北-天”地理坐標(biāo)系為導(dǎo)航坐標(biāo)系n系；以“右-前-上”為載體坐標(biāo)系b系；計(jì)算導(dǎo)航坐標(biāo)系記為n′系。

通常，MIMU慣性導(dǎo)航解算方案是對(duì)導(dǎo)航參數(shù)的微分方程采取積分方法實(shí)現(xiàn)更新，MIMU導(dǎo)航參數(shù)微分方程具體如下[10-11]：

(1)

(2)

(3)

(4)

(5)

(6)

1.1 常用坐標(biāo)系及轉(zhuǎn)換關(guān)系

(7)

基于MIMU的慣性導(dǎo)航誤差方程，在許多文獻(xiàn)中廣泛采用[12-14]，在此不再贅述。與GPS信號(hào)有關(guān)的誤差有衛(wèi)星時(shí)鐘誤差、星歷誤差、電離層、對(duì)流層誤差和其他誤差、在考慮偽距量測(cè)ρ的模型時(shí)、大部分誤差都可以得到適當(dāng)修正、在本文僅考慮與接收機(jī)相關(guān)的時(shí)鐘誤差β和其他噪聲誤差εG，用r表示接收機(jī)與衛(wèi)星的真實(shí)幾何距離，則GPS偽距量測(cè)模型表示為

ρ=r+β+εG.

(8)

假設(shè)等效的時(shí)鐘誤差β為常值誤差，其模型為

(9)

則MIMU/GPS緊組合導(dǎo)航系統(tǒng)的離散化的狀態(tài)方程表示為

xk=φk-1xk-1+Gk-1wk-1,k≥1，

(10)

式中：φk-1為狀態(tài)轉(zhuǎn)移矩陣；Gk-1為系統(tǒng)噪聲分配矩陣；wk-1為系統(tǒng)噪聲，滿足wk～N(0,Θk)。

1.2 量測(cè)方程

假設(shè)第i顆衛(wèi)星在地球坐標(biāo)系中的位置是(xsi,ysi,zsi)，MIMU導(dǎo)航解算出機(jī)體位置為(xI,yI,zI)，機(jī)體的真實(shí)位置為(x,y,z)，MIMU解算的等效偽距為

(11)

假設(shè)機(jī)體在地球坐標(biāo)系中，(δx，δy,δz)T為MIMU導(dǎo)航解算的位置誤差，則

(12)

用泰勒公式將式(11)在點(diǎn)(x，y，z)處展開(kāi)，得到：

(13)

GPS接收機(jī)所測(cè)得的第i顆偽距為

ρGi=rGi+β+εGi

.

(14)

則MIMU/GPS緊組合導(dǎo)航系統(tǒng)的偽距觀測(cè)誤差表示為

δρi=ρIi-ρGi=eixδx+eiyδy+eizδz-β-εGi.

(15)

地球坐標(biāo)系(x,y,z)T與大地坐標(biāo)系(L，λ，h)T的轉(zhuǎn)換關(guān)系為

(16)

(17)

事實(shí)上，當(dāng)載體處于靜止或勻速運(yùn)動(dòng)時(shí)，重力矢量在載體系的投影即為加速度計(jì)輸出[15]，有如下關(guān)系：

(18)

單位化后得

(19)

將式(2)帶入式(19)并整理得

(20)

式中:下標(biāo)m表示對(duì)應(yīng)變量為量測(cè)計(jì)算值。當(dāng)載體處于非重力加速狀態(tài)時(shí)，加速度輸出包含重力加速度和外力引起的加速度2個(gè)信息，在計(jì)算式(20)時(shí)，會(huì)引起水平姿態(tài)角的較大誤差甚至錯(cuò)誤，因此需要將外力加速度剔除。當(dāng)載體處于非重力加速狀態(tài)時(shí)，用于計(jì)算式(20)時(shí)的加速度作出如下修正：

fb0=fb-Δfb,

(21)

式中：fb0表示載體處于靜止?fàn)顟B(tài)時(shí)的加速度計(jì)輸出;Δfb表示載體由于外力而引起的加速度;fb表示由于載體處于非重力加速時(shí)的加速度計(jì)輸出值。由于MIMU導(dǎo)航解算的速度更新頻率比GPS接收機(jī)的偽距接收頻率至少大1～2個(gè)數(shù)量級(jí)以上，因此外力引起的加速度項(xiàng)可由如下表示：

(22)

航向角ψm可通過(guò)GPS輸出得到，由于篇幅有限，在此不做介紹。

基于MIMU解算的姿態(tài)角表示為θI,γI,ψI，由此可得到姿態(tài)角觀測(cè)誤差向量為

(23)

式中:下標(biāo)I表示對(duì)應(yīng)變量為慣導(dǎo)解算的值。系統(tǒng)失準(zhǔn)角與姿態(tài)誤差角的關(guān)系如下：

(24)

式中：

至此，定義觀測(cè)向量為z=(δθ,δγ,δψ，δρ)T，i=1,2,3。根據(jù)式(15)，(24)得到MIMU/GPS緊組合系統(tǒng)的誤差觀測(cè)方程如下：

zk=Hkxk+Vk,

(25)

式中:Hk為觀測(cè)矩陣;Vk為觀測(cè)噪聲;滿足Vk～N(0,Rk)，且wk不相關(guān)。

1.3 濾波算法

本文采用經(jīng)典的卡爾曼濾波方法實(shí)現(xiàn)MIMU/GPS緊組合導(dǎo)航系統(tǒng)的濾波結(jié)算，通過(guò)量測(cè)值，以線性最小方差為估計(jì)準(zhǔn)則，按一定遞推算法完成狀態(tài)向量的估計(jì)，其算法過(guò)程如下：

(1) 初始化(k=0)：

(26)

(2) 時(shí)間更新(k=1,2，…,K)：計(jì)算系統(tǒng)狀態(tài)一步預(yù)測(cè)估計(jì)及一步誤差協(xié)方差矩陣：

(27)

(28)

量測(cè)更新(k=1,2，…,K)：通過(guò)量測(cè)信息及反饋增益估計(jì)系統(tǒng)狀態(tài)及對(duì)應(yīng)的誤差協(xié)方差陣：

(29)

(30)

(31)

2 仿真實(shí)驗(yàn)

仿真參數(shù)進(jìn)行如下設(shè)置：初始位置為：緯度34.2°，經(jīng)度108.9°，高度380 m，初始速度和姿態(tài)角均為0，3個(gè)方向的初始失準(zhǔn)角均為1°，陀螺儀常值漂移為30 (°)/h，隨機(jī)游走誤差為3 (°)/h-1，加速度計(jì)常值零偏誤差為7 mg/h，隨機(jī)游走誤差為0.7 mg/Hz-1，GPS接收機(jī)時(shí)鐘誤差的等效距離誤差為100 m，MIMU采樣頻率為50 Hz，GPS接收機(jī)更新頻率為1 Hz，載體運(yùn)動(dòng)狀態(tài)包括加速運(yùn)動(dòng)，左轉(zhuǎn)彎，右轉(zhuǎn)彎，直線運(yùn)動(dòng)等。

在可見(jiàn)衛(wèi)星數(shù)量為3顆的情況下，基于不同的觀測(cè)量信息，對(duì)MIMU/GPS緊組合導(dǎo)航系統(tǒng)進(jìn)行仿真：①僅偽距差為觀測(cè)量；②僅航向角誤差和偽距差作為觀測(cè)量；③全姿態(tài)誤差和偽距差作為觀測(cè)量。在可見(jiàn)衛(wèi)星為4顆或4顆以上情況下，MIMU/GPS緊組合導(dǎo)航系統(tǒng)能正常工作，導(dǎo)航精度較高，可視為參考對(duì)比。

由于慣性導(dǎo)航解算采用直接積分求解的方式，誤差隨時(shí)間而發(fā)散，當(dāng)載體作直線運(yùn)動(dòng)時(shí)，在組合導(dǎo)航的模式下，導(dǎo)航誤差能有效收斂，當(dāng)載體作機(jī)動(dòng)轉(zhuǎn)彎或變向運(yùn)動(dòng)時(shí)，在組合導(dǎo)航的模式下，導(dǎo)航誤差波動(dòng)較大。

在姿態(tài)誤差實(shí)驗(yàn)中，當(dāng)載體左轉(zhuǎn)彎和右轉(zhuǎn)彎時(shí)，僅偽距作觀測(cè)量時(shí)，姿態(tài)、速度和位置誤差均出現(xiàn)較大波動(dòng)，當(dāng)載體作直線運(yùn)動(dòng)時(shí)，大部分導(dǎo)航參數(shù)誤差逐漸穩(wěn)定收斂，但航向角誤差仍有緩慢逐漸發(fā)散的趨勢(shì)，針對(duì)此問(wèn)題，增加航向角觀測(cè)信息進(jìn)行實(shí)驗(yàn)，解決了航向角誤差穩(wěn)定收斂的問(wèn)題，但是對(duì)于載體處于轉(zhuǎn)彎時(shí)，俯仰角和橫滾角誤差波動(dòng)較大仍然不起作用，進(jìn)而引起速度和位置誤差出現(xiàn)大的波動(dòng)誤差；基于補(bǔ)償?shù)募铀俣扔?jì)輸出反解出水平姿態(tài)角，增加全姿態(tài)的觀測(cè)信息進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)表明所有導(dǎo)航參數(shù)誤差能有效穩(wěn)定保持收斂，不受載體的運(yùn)動(dòng)狀態(tài)改變的影響，但是由于3顆衛(wèi)星的可觀度不如4顆衛(wèi)星的，在組合導(dǎo)航時(shí)，系統(tǒng)的導(dǎo)航精度有所下降。相關(guān)實(shí)驗(yàn)結(jié)果見(jiàn)圖1～3。

圖1 緊組合的姿態(tài)誤差Fig.1 Attitude error of tightly coupled integration

圖2 緊組合的速度誤差Fig.2 Velocity error of tightly coupled integration

圖3 緊組合的位置誤差Fig.3 Position error of tightly coupled integration

3 結(jié)束語(yǔ)

本文對(duì)MIMU/GPS緊組合導(dǎo)航算法進(jìn)行研究，當(dāng)可見(jiàn)衛(wèi)星數(shù)低于4顆時(shí)，組合導(dǎo)航精度有所下降，但依然能實(shí)現(xiàn)導(dǎo)航功能，由于慣性導(dǎo)航解算采用積分方案實(shí)現(xiàn)，當(dāng)載體作變向運(yùn)動(dòng)時(shí)，導(dǎo)航參數(shù)誤差均出現(xiàn)較大波動(dòng)，增加姿態(tài)觀測(cè)信息，提高系統(tǒng)的可觀測(cè)性，能有效解決該問(wèn)題。