許廷金，李 杰，胡陳君，高 寧，張佳宇，姜海洋

(1.中北大學(xué) 電子測(cè)試技術(shù)重點(diǎn)實(shí)驗(yàn)室， 太原 030051；2.蘇州中盛納米科技有限公司， 江蘇 蘇州 215123)

INS/GNSS 組合系統(tǒng)兼?zhèn)淞丝垢蓴_性好、自主能力強(qiáng)、定位精度高等優(yōu)點(diǎn)，已逐漸成為業(yè)界應(yīng)用最為廣泛的導(dǎo)航系統(tǒng)之一[1]。但在高樓林立及山林等復(fù)雜環(huán)境下，GPS信號(hào)極不穩(wěn)定、VDOP和HDOP值較大、甚至出現(xiàn)長(zhǎng)時(shí)間定位無(wú)效的情況，此時(shí)，基于MEMS慣性器件的導(dǎo)航方法占據(jù)主導(dǎo)地位，導(dǎo)航精度迅速下降，因此，提高車(chē)載系統(tǒng)在無(wú)GPS信號(hào)情況下的慣性導(dǎo)航精度勢(shì)在必行。

目前針對(duì)GPS失鎖狀況下的慣性導(dǎo)航技術(shù)有很多，文獻(xiàn)[2]提出了一種利用MEMS-IMU的三軸加速度計(jì)信號(hào)并交聯(lián)磁航向信號(hào)來(lái)精確測(cè)量載體航向和姿態(tài)信息,以輔助修正組合導(dǎo)航系統(tǒng)的方法。文獻(xiàn)[3]利用里程計(jì)的輸出作為GPS失效時(shí)的觀測(cè)信息來(lái)增加導(dǎo)航精度。但是這兩種方法都是以增加新的傳感器為代價(jià)，增加了應(yīng)用成本。文獻(xiàn)[4]引入車(chē)輛的天向和側(cè)向速度作為觀測(cè)量，并用非線性UKF濾波算法進(jìn)行估計(jì)，改善導(dǎo)航性能，UKF雖然能有效減小非線性誤差對(duì)導(dǎo)航精度的影響，但是其運(yùn)算量大、耗時(shí)。

模型輔助導(dǎo)航是一種廉價(jià)有效的MEMS-SINS誤差修正方法[5]，主要有動(dòng)力學(xué)輔助和運(yùn)動(dòng)學(xué)約束兩種模式。本文首先提出了基于運(yùn)動(dòng)學(xué)輔助的動(dòng)態(tài)零速修正技術(shù)，即利用航行時(shí)車(chē)體橫向及天向軸速度誤差作為觀測(cè)量，推導(dǎo)了卡爾曼濾波模型，有效抑制了導(dǎo)航誤差隨時(shí)間發(fā)散的問(wèn)題。然而KF也有一個(gè)致命缺點(diǎn)，它只能在線性環(huán)境下工作，若導(dǎo)航系統(tǒng)產(chǎn)生非線性，KF 就會(huì)產(chǎn)生非線性估計(jì)誤差，導(dǎo)致導(dǎo)航誤差增大。而在實(shí)際導(dǎo)航過(guò)程中，很難保證系統(tǒng)模型線性化[6-8]。為了降低非線性誤差對(duì)導(dǎo)航精度的影響，本文在提出的卡爾曼濾波模型基礎(chǔ)上建立了快速正交搜索模型，可以有效降低非線性誤差，提高導(dǎo)航解算精度。

1 慣導(dǎo)系統(tǒng)誤差方程

(1)

(2)

(3)

2 運(yùn)動(dòng)學(xué)約束和快速正交搜索原理

2.1 運(yùn)動(dòng)學(xué)約束的卡爾曼濾波方法

2.1.1 運(yùn)動(dòng)學(xué)約束條件

當(dāng)車(chē)體處于平穩(wěn)行駛過(guò)程且不發(fā)生側(cè)滑等現(xiàn)象時(shí)，車(chē)體的橫向和天向軸速度理論值為0，即Vy=0，Vz=0，車(chē)體坐標(biāo)系如圖1所示。

圖1 車(chē)體坐標(biāo)系

但在實(shí)際情況下，由于系統(tǒng)中存在某些確定性及隨機(jī)誤差，往往會(huì)使其橫向和天向軸速度不為零[10]，以速度誤差Vy和Vz作為觀測(cè)量，通過(guò)建立卡爾曼濾波模型對(duì)各種誤差進(jìn)行估計(jì)，并在解算時(shí)對(duì)相應(yīng)誤差進(jìn)行剔除，有效抑制線性誤差。

2.1.2 系統(tǒng)安裝誤差補(bǔ)償

慣導(dǎo)系統(tǒng)安裝時(shí)，IMU坐標(biāo)系理論上應(yīng)與車(chē)體坐標(biāo)系一致，但由于多種因素，安裝誤差不可避免，為了能使IMU真實(shí)反應(yīng)車(chē)體的運(yùn)動(dòng)信息，必須對(duì)安裝誤差角進(jìn)行補(bǔ)償[11-12]。

本實(shí)驗(yàn)車(chē)上裝有高精度動(dòng)態(tài)定位定姿系統(tǒng)，在前期安裝時(shí)已進(jìn)行了標(biāo)校，即可把高精度動(dòng)態(tài)定位定姿系統(tǒng)輸出的偏航角(αs)、俯仰角(βs)、滾轉(zhuǎn)角(γs)作為車(chē)體的初始姿態(tài)角，慣導(dǎo)系統(tǒng)的偏航角、俯仰角、滾轉(zhuǎn)角則分別由α、β、γ表示，則安裝誤差角為：

δα=αs-α

δβ=βs-β

δγ=γs-γ

(4)

(5)

2.1.3 卡爾曼濾波誤差模型

(6)

(7)

式(7)中，Vb、Vn分別為車(chē)體坐標(biāo)系和導(dǎo)航坐標(biāo)系的速度向量。將式(7)展開(kāi)得：

(8)

分別提取式(8)的第二行、第三行得到：

(9)

(10)

由Z=HX+V可得出式(11)：

(11)

至此，推導(dǎo)出了基于運(yùn)動(dòng)學(xué)約束的卡爾曼濾波模型，利用此模型編寫(xiě)DSP解算程序?qū)崟r(shí)對(duì)系統(tǒng)速度和姿態(tài)角進(jìn)行更新補(bǔ)償，降低線性誤差對(duì)系統(tǒng)導(dǎo)航精度的影響。但是單純的卡爾曼濾波模型只是對(duì)線性誤差起作用，對(duì)于高階的非線性誤差無(wú)能為力，因此下文推導(dǎo)了能對(duì)非線性誤差進(jìn)行作用的快速正交搜索模型(FOS)。

2.2 快速正交搜索原理

快速正交搜索(Fast Orthogonal Search，F(xiàn)OS)算法能夠最大限度地減小估算量相對(duì)于目標(biāo)函數(shù)的均方誤差[13-14]。快速正交算法與神經(jīng)網(wǎng)絡(luò)相比，不需要長(zhǎng)時(shí)間的迭代就能找到合適的候選函數(shù)，并計(jì)算出相應(yīng)的系數(shù)。其詳細(xì)搜索過(guò)程是在所有的候選函數(shù)中選擇對(duì)均方誤差影響最大的函數(shù)作為模型項(xiàng)，并從候選函數(shù)中剔除，在剩余的候選函數(shù)中重復(fù)上述過(guò)程，直到剩余的非線性誤差足夠小或者滿足預(yù)設(shè)條件。

通常，一個(gè)非線性系統(tǒng)可以表示為:

Y(n)=F[Y(n-1)，…，Y(n-k)，X(n-1),…,

(12)

式(12)中，L、K為輸入、輸出階次；Pm(n)為任意階次的候選函數(shù)，由系統(tǒng)的輸入輸出或其向量積組成；Am為Pm(n)對(duì)應(yīng)的權(quán)值；e(n)為該系統(tǒng)模型誤差。

系統(tǒng)的均方誤差表示為：

(13)

(14)

由式(14)可知，每增加一個(gè)模型Wm(n)，均方誤差相應(yīng)減少量為：

(15)

即msem=msem-1-DEVm，這樣就可以將對(duì)誤差影響較大的候選函數(shù)依次作為模型項(xiàng)，并從候選函數(shù)中剔除，直到搜索完成。由于正交函數(shù)的計(jì)算非常耗時(shí)，本文在正交化過(guò)程中每次只計(jì)算出正交系數(shù)和權(quán)值系數(shù)，從而得到模型項(xiàng)的系數(shù)，完成快速正交搜索模型的建立。

3 KF/FOS算法誤差估計(jì)原理

3.1 快速正交模型訓(xùn)練

取車(chē)體靜止?fàn)顟B(tài)作為FOS模型訓(xùn)練階段，此時(shí)，系統(tǒng)速度和姿態(tài)角誤差關(guān)系分別表示為：

ΔV=VINS-δVKF-V靜

(16)

Δφ=φINS-δφKF-φ靜

(17)

式(16)、(17)中，ΔV和Δφ分別為剩余非線性速度和姿態(tài)角誤差；VINS和φINS分別為實(shí)時(shí)導(dǎo)航解算得到的速度和姿態(tài)角；δVKF和δφKF為卡爾曼濾波得到的速度和姿態(tài)角誤差；V靜和φ靜分別為高精度定位定姿系統(tǒng)輸出的速度和姿態(tài)角。詳細(xì)建模過(guò)程如圖2所示。首先車(chē)體處于靜止?fàn)顟B(tài)，利用高精度定位定姿系統(tǒng)輸出的姿態(tài)角、速度信息作為靜止?fàn)顟B(tài)標(biāo)準(zhǔn)值，將式(16)、(17)解算得到的高階非線性誤差進(jìn)行快速正交搜索，找到對(duì)非線性誤差影響較大的候選函數(shù)，計(jì)算出該候選函數(shù)對(duì)應(yīng)的系數(shù)，即完成ΔV和Δφ的非線性系統(tǒng)模型的建立。

圖2 FOS模型訓(xùn)練圖

3.2 KF/FOS算法融合導(dǎo)航階段

車(chē)體處于航行狀態(tài)時(shí)，利用車(chē)體靜止時(shí)的快速正交模型實(shí)時(shí)對(duì)系統(tǒng)的非線性誤差進(jìn)行估計(jì)，并對(duì)系統(tǒng)的姿態(tài)角、速度和位置進(jìn)行補(bǔ)償，提高導(dǎo)航精度，具體實(shí)現(xiàn)如圖3所示。

圖3 KF/FOS導(dǎo)航示意圖

4 試驗(yàn)驗(yàn)證

為了驗(yàn)證運(yùn)動(dòng)學(xué)約束模型和快速正交搜索模型的準(zhǔn)確性以及融合算法的有效性，將實(shí)驗(yàn)室自制的組合導(dǎo)航系統(tǒng)搭載在裝有高精度動(dòng)態(tài)定位定姿系統(tǒng)的小車(chē)上，進(jìn)行車(chē)載試驗(yàn)，本次試驗(yàn)屏蔽GPS數(shù)據(jù)，充分模擬純慣性條件下本融合算法的可靠性，試驗(yàn)現(xiàn)場(chǎng)如圖4所示，高精度動(dòng)態(tài)定位定姿系統(tǒng)性能指標(biāo)如表1所示。

根據(jù)3.1節(jié)中的快速正交模型訓(xùn)練方法得到候選函數(shù)后，建立FOS模型。FOS模型建立后，進(jìn)行跑車(chē)試驗(yàn)。試驗(yàn)結(jié)果如下：圖5、圖6、圖7分別為單獨(dú)KF算法、UKF算法、KF與FOS融合算法解算結(jié)果與高精度動(dòng)態(tài)定位定姿系統(tǒng)的姿態(tài)角、位置及水平面軌跡對(duì)比圖，圖8為單獨(dú)KF算法、UKF算法、KF與FOS融合算法解算結(jié)果與高精度動(dòng)態(tài)定位定姿系統(tǒng)的位置誤差對(duì)比圖。

圖4 車(chē)載試驗(yàn)平臺(tái)

類(lèi)別定位精度姿態(tài)角方位角(RMS)0.023°俯仰角(RMS)0.008°滾轉(zhuǎn)角(RMS)0.008°速度(RMS)0.03 m/s位置(RMS)0.1 m

圖5 姿態(tài)角解算曲線

圖6 位置解算曲線

圖7 水平面軌跡曲線

由圖5～圖7可知，利用KF&FOS融合算法(KF/FOS)與單獨(dú)KF算法、UKF算法相比，融合算法解算結(jié)果更接近高精度定位定姿系統(tǒng)解算結(jié)果。由圖8可知，利用融合算法解算誤差明顯降低。表2為三種方案下的位置誤差。

圖8 位置誤差曲線

5 結(jié)論

本文通過(guò)分析慣導(dǎo)系統(tǒng)誤差方程，針對(duì)卡爾曼濾波對(duì)非線性誤差不能有效抑制的問(wèn)題，提出了一種快速正交搜索方法，建立了KF/FOS融合算法模型，通過(guò)跑車(chē)試驗(yàn)證明了KF/FOS融合算法的可行性。在將近60 s的跑車(chē)試驗(yàn)中，融合算法解算位置誤差不超過(guò)0.95 m，實(shí)現(xiàn)了低成本高精度自主導(dǎo)航，具有工程應(yīng)用價(jià)值。