低成本GPS/DR容錯組合導航系統設計

張 強，孫 堯，萬 磊，夏全喜

（哈爾濱工程大學 自動化學院 水下機器人技術國防科技重點實驗室，哈爾濱 150001）

目前大多數室外陸地導航任務均采用GPS作為導航設備。但是在高樓林立或林蔭覆蓋的道路上GPS信號很容易被遮擋而影響定位精度。通常的辦法是利用輔助導航設備與 GPS一起構成組合導航系統，不但能夠提高導航系統的精度而且當GPS信號嚴重衰減的時候，仍可以繼續提供精確的定位信息。傳統的GPS/DR組合導航系統通常采用里程計作為距離傳感器。但是利用里程計構成的GPS/DR組合導航系統存在諸如安裝不便、測量誤差大，成本較高等缺陷[1]。文獻[2-4]利用由多個MEMS陀螺儀和MEMS加速度計配置成的慣性測量單元與GPS構成INS/GPS組合導航系統，能夠提供較為精確的定位和姿態信息，但城市路面導航一般只需要精確的位置信息而不太需要車輛的姿態信息，因此這種組合導航系統成本相對較高且都采用諸如神經網絡[2-3]、在線估計系統噪聲矩陣[4-5]等較為復雜的算法來克服元件誤差，因此系統模型復雜，運算量較大，容錯能力不強，系統魯棒性不好。文獻[6-7]提出一種試探消減 MEMS陀螺儀隨機漂移的方法——零偏試探消減法（Heuristic Drift Reduction，HDR）。該方法無需建立誤差模型就可以提高MEMS慣性元件的精度，具有魯棒性好算法簡單的優點。針對由一個MEMS陀螺儀和一個MEMS加速度計與GPS構成的低成本GPS/DR組合導航系統設計的容錯UKF/KF聯邦Kalman濾波器，由于無需考慮MEMS慣性元件的誤差模型，系統模型較為簡單，魯棒性好。仿真結果表明，這種低成本GPS/DR組合導航系統的定位精度能夠達到0.5 m，并且具有較強的容錯能力，特別適用于普通車輛的城市導航定位。

1 GPS/DR組合導航系統的結構及模型

1.1 GPS/DR組合導航系統的組成

如圖1所示，DR導航系統傳感器由一個MEMS陀螺儀和一個MEMS加速度計構成。它們的輸出信號經試探性零偏消減后，同GPS的位置信號和速度信號一起被送入容錯UKF/KF聯邦Kalman濾波器。同一般的聯邦Kalman濾波器不同的是，容錯UKF/KF聯邦Kalman濾波器只采用兩個局部Kalman濾波器KF和UKF，分別處理GPS和DR傳感器的測量數據。GPS信號檢測模塊用于檢測車輛行駛過程中GPS信號由于樹木或樓房的遮蔽而產生的突然變化使系統具有容錯能力。同時主濾波器無信息分配，為抑制DR導航系統發散過快，將全部信息分配給DR導航系統，主濾波器僅完成對兩個子濾波器信息的最優融合估計，這樣系統計算量小，濾波速度快。

圖1 GPS/DR組合導航系統結構圖Fig.1 Structure of GPS/DR integrated navigation system

1.2 容錯UKF/KF聯邦Kalman濾波模型

1.2.1 狀態方程的建立

對于在二維平面內運動的GPS/DR組合導航系統的Kalman濾波的狀態變量可取為：

其中，xe、xn分別為車輛東向和北向位置分量，ve、vn分別為車輛東向和北向速度分量，ae、an分別為車輛東向和北向加速度分量。

采用“當前”統計模型[8]描述系統狀態變化過程，并通過典型的離散化處理方法得到離散狀態方程為：程噪聲協方差陣Q(k)的表達式及計算參見文獻[9]。

1.2.2 觀測方程的建立

將GPS接收機輸出的車輛東向和北向位置信息eobs、nobs（單位均化為m），以及車輛東向和北向速度信息ve、vn（單位均化為m/s）作為觀測矢量，得到GPS子系統的觀測方程如下：

2 容錯UKF/KF聯邦Kalman濾波設計

2.1 GPS信號檢測方法

采用殘差2χ檢驗法判斷GPS信號的有效性。由Kalman濾波方程可知GPS局部濾波器的新息為：

可以證明，當GPS信號沒有被屏蔽時，新息r1(k)為零均值高斯白噪聲；當GPS信號被屏蔽時，新息r1(k)均值為非零值。因此通過對r1(k)均值的檢驗即可判斷GPS信號的有效性。對r1(k)做二元假設：

GPS信號被屏蔽：

T是設定的門限值。設誤報警率為5‰，又GPS觀測矢量Z1(k)的維數為 4，查表可得T=14.86。用上述方法即可對GPS信號的有效性進行檢測。

2.2 容錯UKF/KF聯邦Kalman濾波方程

從式（3）知，DR導航子系統的觀測方程是非線性的，一般采用擴展Kalman濾波進行線性化，但是考慮到MEMS慣性元件的精度較低，且觀測方程的非線性程度較高，因此使用UKF作為DR子系統的濾波器，并采用Sigma點對稱采樣策略確保任意分布的近似精度達到泰勒展開式二階截斷，而且計算量與EKF同階。根據UKF和KF濾波遞推方程以及所建立的GPS/DR組合定位系統的狀態方程式（1）和觀測方程式（2）（3），可以建立GPS子系統和DR子系統的Kalman濾波方程，具體狀態的最優綜合和信息分配過程如下：

1） 時間傳播方程

3 零偏試探消減法

如圖2所示，MEMS慣性元件的輸出信號，經低通濾波、零偏試探消減（Heuristic Drift Reduction，HDR）和滯后消除后輸入DR導航系統。

3.1 低通濾波和滯后消除

DR導航推算前一般不需要進行低通濾波，因為積分運算本身就是一個低通濾波器[6-7]，但是由于對 MEMS慣性元件的零偏試探消減是在 DR導航推算之前進行的，因此采用如式（16）所示的數字低通濾波器對MEMS陀螺儀和加速度計的輸出信號進行濾波，可以消除高頻噪聲對零偏試探消減算法的影響。

圖2 零偏試探消減法程序流程圖Fig.2 Program flowchart of heuristic drift reduction

其中，T為采樣時間，ok′-1為低通濾波信號，τ為低通濾波器時間常數。

雖然數字低通濾波器平滑了MEMS慣性元件的輸出信號，但是數字低通濾波器同樣會引起MEMS慣性元件輸出信號的延遲。式（17）所示的滯后消除算法可解決這個問題。

其中od,k為經零偏試探消減校正，并消除延遲后的MEMS慣性元件的最終輸出信號。

3.2 零偏試探消減算法

試探消減MEMS慣性元件的零偏是基于如下假設：即在城市路況下，路面車輛基本保持勻速巡航狀態，車輛變速行駛或轉彎情況較少。由于車輛在巡航狀態下慣性器件的輸入值為零，慣性器件的輸出是由白噪聲、常值零偏和隨機零漂構成的誤差信號。因此可以通過零偏試探消減法（HDR）消減MEMS慣性元件誤差信號中的隨機零漂，提高MEMS慣性元件的精度。零偏試探消減法（HDR）由自適應積分因子計算和自適應零偏積分運算兩部分組成。自適應積分因子根據車輛機動情況自動調節自身大小：當車輛做變速運動或轉彎運動時，MEMS慣性元件的輸出值不再為誤差信號，此時自適應積分因子會根據輸出信號幅值的大小以一定速率趨向于零，可以抑制零偏試探消減法對實際信號的影響；當車輛處于巡航狀態時，自適應積分因子會接近于1。

自適應積分因子Aa、Aω可由下式計算：

式中，Ka、Kω為衰減因子，決定自適應積分因子的衰減速度；為加速度幅值。自適應零偏積分運算會根據自適應積分因子的大小實時計算MEMS慣性元件的隨機零漂，具體計算過程如式（20）和式（21）所示：

這里iac、icω為與加速度計和陀螺儀隨機零漂相對應的固定增量值，可根據實際情況進行調節；sign()為符號函數；Iak、分別為MEMS加速度計和MEMS陀螺儀隨機零漂的估計值。則MEMS慣性傳感器的輸出可以由下式補償：

4 算法性能檢驗

仿真采用的慣性器件的性能如下：MEMS陀螺儀的隨機零漂為1 (°)/s，角度隨機游走系數為0.05 (°)/(s·Hz1/2)。MEMS加速度計的隨機零漂為0.02 m/s2，白噪聲方差為0.002 m/s2，GPS接收機輸出的東向位置和北向位置的觀測噪聲標準差分別為16 m和15 m，東向和北向速度分量的觀測噪聲標準差為0.2 m/s和0.3 m/s，采樣周期為1 s。

仿真實驗路徑如圖3所示。以A點作為起點，D點作為終點，其中AB段做勻加速運動，BC段做勻速運動，CD段做勻減速運動。首先假設GPS信號被屏蔽的條件下，由于CD段情況與AB段相同，因此分別在AB段和在BC段對比DR導航系統傳感器經零偏試探消減和未經零偏消減時的東向和北向的定位誤差，其中點劃線為前者的定位誤差，實線為后者的定位誤差。如圖4所示，首先，在AB段，車輛作勻加速直線運動，由于加速度的存在，零偏試探消減法無法消除車輛橫滾軸向上的 MEMS加速度計的隨機零漂，但由于此時車輛航向不發生變化，因此 MEMS陀螺儀的隨機零漂得到了零偏試探消減法（HRD）的有效補償，提高了航向計算精度，進而提高了DR導航系統的精度。

圖3 仿真實驗路徑Fig.3 Simulation experiment route

圖4 HDR DR與DR AB段和BC段的位置誤差對比Fig.4 Position error comparison between HDR DR and DR of AB and BC segments

其次，在BC段，車輛作勻速運動。BC段由兩種路段組成，即三個勻速彎道和兩個勻速直線路段。在三個勻速彎道處由于角速度的存在，零偏試探消減法（HDR）無法消除 MEMS陀螺儀的隨機零漂，但可有效消除車輛橫滾軸向上的 MEMS加速度計的隨機零漂，且由于通過時間較短，因此HDR-DR可以保持一定的定位精度。在兩個勻速直線路段，零偏試探消減法（HDR）有效消除了 MEMS陀螺儀和加速度計的隨機零漂，可見零偏試探消減法（HDR）在車輛巡航的條件下同樣可以起到提高MEMS慣性器件精度的效果。

圖5 GPS/DR與GPS子系統位置誤差對比Fig.5 Position error comparison between GPS/DR and GPS subsystem

在能夠接收GPS信號的條件下，對比GPS/DR組合的容錯UKF/KF聯邦Kalman濾波與GPS接收機單獨構成的導航子系統1在前25 s的定位精度，因為導航系統在10 s后便趨于穩定，二者穩態時定位誤差的標準差沒有差異。結果如圖5所示，前者定位誤差為點劃線，后者定位誤差為實線。由圖5可見，兩者穩態時定位誤差均為0.5 m(1σ)，這是由于MEMS慣性元件的精度較低的緣故，其對定位精度并沒有顯著的改善。但與GPS接收機單獨構成的導航子系統1相比，GPS/DR組合的容錯UKF/KF聯邦Kalman濾波的定位軌跡更為平滑，定位時間也更短。當GPS數據突然失效時，比較23 s至45 s之間兩者的定位誤差（如圖6所示）可見，雖然二者都具有一定的魯棒性，但是GPS/DR組合的容錯UKF/KF聯邦Kalman濾波表現出了更好的容錯能力，在GPS信號被屏蔽的條件下，短時間內定位精度相對較高。

圖6 GPS/DR與GPS子系統容錯能力對比Fig.6 Fault tolerance ability comparison between GPS/DR and GPS subsystem

5 結 論

GPS接收機作為車輛普遍采用的導航設備，雖然能夠滿足城市車輛定位的大部分需求，但是其定位精度往往因信號被高樓、樹木遮擋而受到影響，并且不具備容錯能力。文中設計的低成本GPS/DR容錯組合導航系統，在基于容錯UKF/KF聯邦Kalman濾波算法將GPS輸出的位置、速度信息與MEMS陀螺儀、加速度計輸出的經零偏試探消減后的航向角速率和車輛加速度信息融合后，得到更為光滑、收斂性更好的定位結果；并且當 GPS信號因遮擋而突然失效時，該導航系統依然能夠提供較為準確的定位信息，具有一定的容錯能力。該低成本GPS/DR容錯組合導航系統具有以下特點：其一，系統在原有GPS接收機基礎上僅增加了一個MEMS陀螺儀和一個MEMS加速度計，硬件設計簡單，成本較低；其二，應用零偏試探消減算法提高MEMS慣性元件輸出信號精度，簡化了導航系統濾波模型，降低了Kalman濾波器的維數，算法實現簡單；其三，在仿真條件下，系統達到了0.5 m(1σ)的定位精度，且容許GPS數據的突然失效，表明該系統適用于車輛導航的城市應用。

（References）：

[1] Kaplan E D, Hegarty C. Understanding GPS: Principles and applications[M]. 2nd edition. Boston and London: Artech House, 2005: 491-522.

[2] El-Sheimy N, Abdel-Hamid W. An adaptive neuro-fuzzy model to bridge GPS outages in MEMS-INS/GPS land vehicle navigation[C]//ION GNSS 2004. Long Beach, California, September 21-24, 2004.

[3] Kai-Wei Chiang, Nassar S, El-Sheimy N. A constructive and autonomous integration scheme of low-cost GPS/MEMS IMU for land vehicular navigation application[C]// IEEE/ION PLANS 2006. San Diego, California, April 25-27, 2006: 235-243.

[4] Tang K H, Wu M P, Hu X P. Multiple model Kalman filtering for MEMS-IMU/GPS integrated navigation[C]//2007 Second IEEE Conference on Industrial Electronics and Applications. Harbin, China: May 23-25, 2007: 2062-2066.

[5] 周艷麗，張海，高婷婷，等. 一種改進的GPS/DR組合位置自適應濾波算法[J]. 中國慣性技術學報，2009，17(6)：728-733.ZHOU Yan-li, ZHANG Hai, GAO Ting-ting, et al. An improved GPS /DR integrated position adaptive filtering algorithm[J]. Journal of Chinese Inertial Technology, 2009, 17(6): 728-733.

[6] Borenstein J, Ojeda L, Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems[J]. The Journal of Navigation, 2009, 62: 41-58.

[7] Borenstein J, Ojeda L. Heuristic reduction of gyro drift in vehicle tracking application[J]. International Journal of Vehicle Autonomous Systems, 2009(2): 78-98.

[8] 巴宏欣，趙宗貴，楊飛，等. 機動目標的模糊自適應跟蹤算法[J]. 系統仿真學報，2004，16(6)：1181-1186.BA Hong-xin, ZHAO Zong-gui, YANG Fei, et al. Fuzzy adaptive tracking algorithm for maneuvering target[J]. Journal of System Simulation, 2004, 16(6): 1181-1186.

[9] 寇艷紅，張其善，李先亮. 車載GPS/DR組合導航系統的數據融合算法[J]. 北京航空航天大學學報，2003，29(3)：264-268.KOU Yan-hong, ZHANG Qi-shan, LI Xian-liang. Data fusion algorithm for GPS/DR integrated vehicle navigation system[J]. Journal of Beijing University of Aeronautics and Astronautics, 2003, 29(3): 264-268.