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

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

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

1 GPS/DR組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)及模型

1.1 GPS/DR組合導(dǎo)航系統(tǒng)的組成

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

圖1 GPS/DR組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure of GPS/DR integrated navigation system

1.2 容錯UKF/KF聯(lián)邦Kalman濾波模型

1.2.1 狀態(tài)方程的建立

對于在二維平面內(nèi)運動的GPS/DR組合導(dǎo)航系統(tǒng)的Kalman濾波的狀態(tài)變量可取為：

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

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

1.2.2 觀測方程的建立

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

2 容錯UKF/KF聯(lián)邦Kalman濾波設(shè)計

2.1 GPS信號檢測方法

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

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

GPS信號被屏蔽：

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

2.2 容錯UKF/KF聯(lián)邦Kalman濾波方程

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

1） 時間傳播方程

3 零偏試探消減法

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

3.1 低通濾波和滯后消除

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

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

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

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

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

3.2 零偏試探消減算法

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

自適應(yīng)積分因子Aa、Aω可由下式計算：

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

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

4 算法性能檢驗

仿真采用的慣性器件的性能如下：MEMS陀螺儀的隨機零漂為1 (°)/s，角度隨機游走系數(shù)為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段做勻減速運動。首先假設(shè)GPS信號被屏蔽的條件下，由于CD段情況與AB段相同，因此分別在AB段和在BC段對比DR導(dǎo)航系統(tǒng)傳感器經(jīng)零偏試探消減和未經(jīng)零偏消減時的東向和北向的定位誤差，其中點劃線為前者的定位誤差，實線為后者的定位誤差。如圖4所示，首先，在AB段，車輛作勻加速直線運動，由于加速度的存在，零偏試探消減法無法消除車輛橫滾軸向上的 MEMS加速度計的隨機零漂，但由于此時車輛航向不發(fā)生變化，因此 MEMS陀螺儀的隨機零漂得到了零偏試探消減法（HRD）的有效補償，提高了航向計算精度，進而提高了DR導(dǎo)航系統(tǒng)的精度。

圖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子系統(tǒng)位置誤差對比Fig.5 Position error comparison between GPS/DR and GPS subsystem

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

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

5 結(jié) 論

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

（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組合位置自適應(yīng)濾波算法[J]. 中國慣性技術(shù)學(xué)報，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] 巴宏欣，趙宗貴，楊飛，等. 機動目標的模糊自適應(yīng)跟蹤算法[J]. 系統(tǒng)仿真學(xué)報，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組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算法[J]. 北京航空航天大學(xué)學(xué)報，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.