基于MEMS慣性器件的新型車載導航融合算法研究

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

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

INS/GNSS 組合系統兼備了抗干擾性好、自主能力強、定位精度高等優點，已逐漸成為業界應用最為廣泛的導航系統之一[1]。但在高樓林立及山林等復雜環境下，GPS信號極不穩定、VDOP和HDOP值較大、甚至出現長時間定位無效的情況，此時，基于MEMS慣性器件的導航方法占據主導地位，導航精度迅速下降，因此，提高車載系統在無GPS信號情況下的慣性導航精度勢在必行。

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

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

1 慣導系統誤差方程

(1)

(2)

(3)

2 運動學約束和快速正交搜索原理

2.1 運動學約束的卡爾曼濾波方法

2.1.1 運動學約束條件

當車體處于平穩行駛過程且不發生側滑等現象時，車體的橫向和天向軸速度理論值為0，即Vy=0，Vz=0，車體坐標系如圖1所示。

圖1 車體坐標系

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

2.1.2 系統安裝誤差補償

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

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

δα=αs-α

δβ=βs-β

δγ=γs-γ

(4)

(5)

2.1.3 卡爾曼濾波誤差模型

(6)

(7)

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

(8)

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

(9)

(10)

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

(11)

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

2.2 快速正交搜索原理

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

通常，一個非線性系統可以表示為:

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

(12)

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

系統的均方誤差表示為：

(13)

(14)

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

(15)

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

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

3.1 快速正交模型訓練

取車體靜止狀態作為FOS模型訓練階段，此時，系統速度和姿態角誤差關系分別表示為：

ΔV=VINS-δVKF-V靜

(16)

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

(17)

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

圖2 FOS模型訓練圖

3.2 KF/FOS算法融合導航階段

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

圖3 KF/FOS導航示意圖

4 試驗驗證

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

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

圖4 車載試驗平臺

類別定位精度姿態角方位角(RMS)0.023°俯仰角(RMS)0.008°滾轉角(RMS)0.008°速度(RMS)0.03 m/s位置(RMS)0.1 m

圖5 姿態角解算曲線

圖6 位置解算曲線

圖7 水平面軌跡曲線

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

圖8 位置誤差曲線

5 結論

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