低成本GPS/DR容錯(cuò)組合導(dǎo)航系統(tǒng)設(shè)計(jì)

張 強(qiáng)，孫 堯，萬 磊，夏全喜

（哈爾濱工程大學(xué) 自動(dòng)化學(xué)院 水下機(jī)器人技術(shù)國(guó)防科技重點(diǎn)實(shí)驗(yàn)室，哈爾濱 150001）

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

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

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

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

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

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

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

1.2.2 觀測(cè)方程的建立

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

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

2.1 GPS信號(hào)檢測(cè)方法

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

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

GPS信號(hào)被屏蔽：

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

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

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

1） 時(shí)間傳播方程

3 零偏試探消減法

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

3.1 低通濾波和滯后消除

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

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

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

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

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

3.2 零偏試探消減算法

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

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

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

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

4 算法性能檢驗(yàn)

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

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

圖3 仿真實(shí)驗(yàn)路徑Fig.3 Simulation experiment route

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

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

圖5 GPS/DR與GPS子系統(tǒng)位置誤差對(duì)比Fig.5 Position error comparison between GPS/DR and GPS subsystem

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

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

5 結(jié) 論

GPS接收機(jī)作為車輛普遍采用的導(dǎo)航設(shè)備，雖然能夠滿足城市車輛定位的大部分需求，但是其定位精度往往因信號(hào)被高樓、樹木遮擋而受到影響，并且不具備容錯(cuò)能力。文中設(shè)計(jì)的低成本GPS/DR容錯(cuò)組合導(dǎo)航系統(tǒng)，在基于容錯(cuò)UKF/KF聯(lián)邦Kalman濾波算法將GPS輸出的位置、速度信息與MEMS陀螺儀、加速度計(jì)輸出的經(jīng)零偏試探消減后的航向角速率和車輛加速度信息融合后，得到更為光滑、收斂性更好的定位結(jié)果；并且當(dāng) GPS信號(hào)因遮擋而突然失效時(shí)，該導(dǎo)航系統(tǒng)依然能夠提供較為準(zhǔn)確的定位信息，具有一定的容錯(cuò)能力。該低成本GPS/DR容錯(cuò)組合導(dǎo)航系統(tǒng)具有以下特點(diǎn)：其一，系統(tǒng)在原有GPS接收機(jī)基礎(chǔ)上僅增加了一個(gè)MEMS陀螺儀和一個(gè)MEMS加速度計(jì)，硬件設(shè)計(jì)簡(jiǎn)單，成本較低；其二，應(yīng)用零偏試探消減算法提高M(jìn)EMS慣性元件輸出信號(hào)精度，簡(jiǎn)化了導(dǎo)航系統(tǒng)濾波模型，降低了Kalman濾波器的維數(shù)，算法實(shí)現(xiàn)簡(jiǎn)單；其三，在仿真條件下，系統(tǒng)達(dá)到了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] 周艷麗，張海，高婷婷，等. 一種改進(jìn)的GPS/DR組合位置自適應(yīng)濾波算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，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ī)動(dòng)目標(biāo)的模糊自適應(yīng)跟蹤算法[J]. 系統(tǒng)仿真學(xué)報(bào)，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é)報(bào)，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.