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

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

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

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

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

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

(1)

(2)

(3)

2 運動學(xué)約束和快速正交搜索原理

2.1 運動學(xué)約束的卡爾曼濾波方法

2.1.1 運動學(xué)約束條件

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

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

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

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

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

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

δα=αs-α

δβ=βs-β

δγ=γs-γ

(4)

(5)

2.1.3 卡爾曼濾波誤差模型

(6)

(7)

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

(8)

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

(9)

(10)

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

(11)

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

2.2 快速正交搜索原理

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

通常，一個非線性系統(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)對應(yīng)的權(quán)值；e(n)為該系統(tǒng)模型誤差。

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

(13)

(14)

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

(15)

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

3 KF/FOS算法誤差估計原理

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

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

ΔV=VINS-δVKF-V靜

(16)

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

(17)

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

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

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

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

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

4 試驗驗證

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

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

圖4 車載試驗平臺

類別定位精度姿態(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)與單獨KF算法、UKF算法相比，融合算法解算結(jié)果更接近高精度定位定姿系統(tǒng)解算結(jié)果。由圖8可知，利用融合算法解算誤差明顯降低。表2為三種方案下的位置誤差。

圖8 位置誤差曲線

5 結(jié)論

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