沈圓倪福生魏長(zhǎng)赟謝子陽(yáng)

基于粒子濾波方法的GPS/DR組合導(dǎo)航仿真研究?

沈圓倪福生魏長(zhǎng)赟謝子陽(yáng)

（1.河海大學(xué)機(jī)電工程學(xué)院常州213022）（2.疏浚技術(shù)教育部工程研究中心常州213022）

傳統(tǒng)車載導(dǎo)航系統(tǒng)大多基于全球定位系統(tǒng)（Global Positioning System，GPS）實(shí)現(xiàn)，GPS導(dǎo)航系統(tǒng)能為車輛提供位置信息，但是在都市復(fù)雜環(huán)境下所表現(xiàn)出的導(dǎo)航質(zhì)量難以滿足人們對(duì)導(dǎo)航性能更高的要求。針對(duì)目前車載導(dǎo)航系統(tǒng)在導(dǎo)航精度和可靠性方面所存在的問(wèn)題，應(yīng)用GPS與航位推算（Dead Reckoning，DR）相結(jié)合的組合導(dǎo)航算法，以及粒子濾波算法進(jìn)行后處理，以提高導(dǎo)航系統(tǒng)的精度、連續(xù)性。實(shí)驗(yàn)結(jié)果表明GPS/DR組合導(dǎo)航系統(tǒng)在導(dǎo)航時(shí)可以達(dá)到預(yù)定的精度，當(dāng)GPS信號(hào)中斷時(shí)，車輛依然可以實(shí)現(xiàn)導(dǎo)航定位，但隨著GPS信號(hào)中斷時(shí)間的延長(zhǎng)，誤差隨之增大。

導(dǎo)航；GPS/DR；粒子濾波；航位推算

Class NumberTN966

1引言

隨著工業(yè)化的不斷加速推進(jìn)，汽車在產(chǎn)業(yè)規(guī)?；?、市場(chǎng)化下越來(lái)越普及。越來(lái)越多的車輛給人們生活帶來(lái)巨大便利的同時(shí)，也引發(fā)了一系列問(wèn)題如城市擁堵、事故頻發(fā)、大氣污染等。這些問(wèn)題導(dǎo)致了很大的經(jīng)濟(jì)損失，還嚴(yán)重威脅人們的出行安全。對(duì)車輛、道路、行人進(jìn)行更有效地綜合管理，刻不容緩。

GPS導(dǎo)航系統(tǒng)能為車輛提供低成本、高精度的三維位置、速度和精確定時(shí)等導(dǎo)航信息，極大方便了車輛的使用，但是GPS信號(hào)容易被干擾和遮擋，尤其在市區(qū)道路中，“峽谷效應(yīng)”導(dǎo)致可見(jiàn)衛(wèi)星個(gè)數(shù)明顯變少，以至于不能完成導(dǎo)航定位［1］，DR的結(jié)構(gòu)簡(jiǎn)單，成本低，定位精度沒(méi)那么高，DR在短時(shí)間有較好效果，這與GPS相反。本文建立GPS/DR組合導(dǎo)航系統(tǒng)［2］，模擬車輛實(shí)際行駛過(guò)程中接收到間歇性的GPS信號(hào)，利用Matlab軟件以及粒子濾波算法仿真車輛導(dǎo)航過(guò)程。

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

DR（Dead Reckoning）［3］用車輛行駛的方向和距離信息來(lái)推算車輛的瞬時(shí)位置，通常車輛的運(yùn)動(dòng)可以近似看作是在地表平面上的二維運(yùn)動(dòng)，如果車輛的起始位置和所有時(shí)刻的位移已知，則通過(guò)在初始位置上累加位移矢量的方法就可計(jì)算出車輛的位置，若采用東北坐標(biāo)系，則車輛的位置就可以由東向、北向位置坐標(biāo)(x，y)來(lái)描述。假設(shè)t0時(shí)刻，車輛的初始位置為(x0，y0)，則在tn(n≥1)時(shí)刻，車輛的位置(xn，yn)可按如下公式計(jì)算：

式中：θi為ti時(shí)刻車輛的航向角；di為ti到ti+1時(shí)刻車輛駛過(guò)的航向角。

DR系統(tǒng)能連續(xù)地提供車輛的位置信息，是一種自主式的定位系統(tǒng)，不易受外部環(huán)境的影響，但是由于定位傳感器存在測(cè)量誤差，使得DR的定位誤差隨時(shí)間增加而積累，因此經(jīng)過(guò)較長(zhǎng)的時(shí)間后，DR的定位誤差要超出系統(tǒng)所允許的誤差范圍。

GPS利用衛(wèi)星進(jìn)行定位，能夠迅速、準(zhǔn)確、全天候提供三維位置、速度和時(shí)間信息，具有定位誤差不隨時(shí)間積累、定位精度高以及成本低等優(yōu)點(diǎn)。但是GPS接收機(jī)的定位精度會(huì)受到外部環(huán)境因素的影響，當(dāng)車輛行駛在兩旁高樓林立的市區(qū)街道上或林蔭道上、隧道內(nèi)、立交橋下時(shí)，會(huì)因GPS衛(wèi)星信號(hào)被遮擋和丟失，而使GPS接收機(jī)定位精度很差或無(wú)法定位。

針對(duì)GPS和DR各自的特點(diǎn)，利用GPS/DR［4］組合定位方案，采用信息融合技術(shù)來(lái)組合GPS和DR系統(tǒng)的信息，使得GPS/DR組合后系統(tǒng)的性能優(yōu)于各個(gè)子系統(tǒng)的性能。

3粒子濾波算法

粒子濾波（Particle Filter）是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法，首先依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布在狀態(tài)空間產(chǎn)生一組隨機(jī)樣本的集合，稱這些樣本為粒子，然后根據(jù)量測(cè)不斷調(diào)整粒子的權(quán)重和位置，通過(guò)調(diào)整后的粒子信息修正最初的經(jīng)驗(yàn)條件分布，并且根據(jù)算法遞推更新離散隨機(jī)測(cè)度。當(dāng)樣本容量很大時(shí)，這種蒙特卡羅描述就近似于狀態(tài)變量真實(shí)的后驗(yàn)概率密度函數(shù)。這種技術(shù)適用于任何能用狀態(tài)空間模型表示的非高斯背景的非線性隨機(jī)系統(tǒng)，精度可以逼近最優(yōu)估計(jì)，是一種很有效的非線性濾波技術(shù)。

序列重要性采樣（SIS）［5］算法是一種通過(guò)Mon?te Carlo［6～9］方法模擬實(shí)現(xiàn)遞推貝葉斯濾波器的技術(shù)，利用一組代表驗(yàn)后概率密度函數(shù)p(x(0i:)k|y1:k)且?guī)в袡?quán)重系數(shù)的粒子{(w(ki)，x(ki)):i=1，2，…，N}來(lái)逼近分布p(x0:k|y1:k)：

系數(shù)w(ki)是相應(yīng)的隨機(jī)粒子x(ki)歸一化權(quán)值，即，由于p(x0:k|y1:k)很難采樣，所以可用重要性函數(shù)q(x0:k|y1:k)進(jìn)行采樣。假設(shè)樣本從重要性密度函數(shù)q(x0:k|y1:k)上抽取，那么權(quán)值可以定義為

SIS粒子濾波算法的一個(gè)最大問(wèn)題是退化現(xiàn)象［10］，即隨著時(shí)間的遞增，重要性權(quán)值有可能集中在少數(shù)粒子上。所以針對(duì)此問(wèn)題，引入重采樣算法，通過(guò)對(duì)粒子和相應(yīng)權(quán)值表示的概率密度函數(shù)重新采樣，增加權(quán)值較大的粒子數(shù)，減少權(quán)值較少的粒子數(shù)。

本文主要是結(jié)合GPS/DR的優(yōu)點(diǎn)，模擬車輛行駛過(guò)程中的實(shí)際情況，利用粒子濾波算法進(jìn)行數(shù)據(jù)處理［11］，進(jìn)行車輛導(dǎo)航定位的仿真研究，如圖1所示為系統(tǒng)的總體框架。陀螺儀和里程計(jì)可以對(duì)車輛的位置進(jìn)行航位推算，間歇性的GPS信號(hào)可以糾正航位推算所引起的誤差，利用粒子濾波算法對(duì)數(shù)據(jù)進(jìn)行處理，從而實(shí)現(xiàn)導(dǎo)航定位。

圖1 系統(tǒng)的基本框架

4 GPS/DR組合系統(tǒng)的建立

4.1 GPS/DR組合系統(tǒng)狀態(tài)方程的建立

組合定位系統(tǒng)的狀態(tài)變量為

式中：xe、xn為車輛東向與北向的位置分量；ve、vn為車輛東向與北向的速度分量；ae、an為車輛東向與北向的加速度分量。

組合定位系統(tǒng)連續(xù)的狀態(tài)方程為

設(shè)采樣周期為T，將系統(tǒng)連續(xù)的狀態(tài)方程離散化，得到系統(tǒng)離散的狀態(tài)方程為

式中：

4.2 GPS/DR組合系統(tǒng)觀測(cè)方程的建立

將GPS輸出的東向位置信息ex、北向位置信息ny、角速率陀螺的輸出ω以及里程計(jì)在一個(gè)采樣周期內(nèi)輸出的距離s作為觀測(cè)量，里程計(jì)的刻度系數(shù)取為K=1。

于是系統(tǒng)連續(xù)的觀測(cè)方程為

式中：v1、v2為東向與北向位置觀測(cè)噪聲；εω為陀螺的漂移；εs為里程計(jì)觀測(cè)噪聲。

將觀測(cè)方程離散化，得到系統(tǒng)離散的觀測(cè)方程為

式中：

5仿真實(shí)驗(yàn)及結(jié)果

5.1軌跡規(guī)劃

圖2 模擬軌跡

5.2結(jié)果與分析

車輛在實(shí)際行駛過(guò)程中，GPS接收到的信號(hào)，會(huì)出現(xiàn)實(shí)時(shí)中斷的情況，在Matlab仿真實(shí)驗(yàn)中，就不同的情況進(jìn)行模擬，分別為實(shí)時(shí)GPS信號(hào)，以及每隔20s、100s給出GPS信號(hào)的仿真結(jié)果如下。

圖3 實(shí)時(shí)給定GPS信號(hào)的定位誤差

圖4 每20s給定GPS信號(hào)的定位誤差

圖5 每100s給定GPS信號(hào)的定位誤差

從圖中可以看出，隨著接收到GPS信號(hào)間隔的時(shí)間變長(zhǎng)，車輛的定位誤差逐漸增大。

6結(jié)語(yǔ)

本文采取GPS/DR組合導(dǎo)航系統(tǒng)提供導(dǎo)航服務(wù)，設(shè)計(jì)了基于粒子濾波的GPS/DR組合導(dǎo)航算法，通過(guò)Matlab仿真實(shí)驗(yàn)驗(yàn)證了算法的有效性。實(shí)驗(yàn)結(jié)果顯示，在組合導(dǎo)航系統(tǒng)下，可以實(shí)現(xiàn)車輛的導(dǎo)航定位，達(dá)到一定的精度，當(dāng)GPS信號(hào)中斷時(shí)，也可以起到導(dǎo)航的作用，只是隨著GPS信號(hào)中斷時(shí)間的延長(zhǎng)，誤差隨著時(shí)間逐漸增大，GPS可以糾正航位推算的誤差。

［1］黃攀.基于GPS/DR緊組合車載導(dǎo)航系統(tǒng)研究及實(shí)現(xiàn)［D］.哈爾濱：哈爾濱工程大學(xué)，2013.

HUANG Pan.Study and Implementation of Vehichle Navi? gation System Based on Tightly-Coupled GPS/DR［D］. Haerbin：Harbin Engineering University，2013.

［2］袁克非.組合導(dǎo)航系統(tǒng)多源信息融合關(guān)鍵技術(shù)研究［D］.哈爾濱：哈爾濱工程大學(xué)，2012.

HUANG Kefei.Key Technique of Multi-source Informa?tion Fusion in Integrated Navigation System［D］.Harbin：Harbin Engineering University，2012.

［3］付夢(mèng)印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京：科學(xué)出版社，2010：228.

FU Mengyin，DENG Zhihong，YAN Liping.The applica?tion of Kalman filtering theory in the navigation system［M］.Beijing：Science Press，2010：228.

［4］畢軍，關(guān)偉，申金升.車輛GPS/DR定位硬件系統(tǒng)的設(shè)計(jì)［J］.交通運(yùn)輸系統(tǒng)工程與信息，2005，5（3）：24-26.

BI Jun，GUAN Wei，SHEN Jinsheng.The Hardware De?sign of vehichle GPS/DR position system［J］.Journal of Transportation Systems Engineering and Information Tech?nology，2005，5（3）：24-26.

［5］朱志宇.粒子濾波算法及其應(yīng)用［M］.北京：科學(xué)出版社，2010：257.

ZHU Zhiyu.Algorithm and application of particle filter［M］.Beijing：Science Press，2010：257.

［6］Doucet A，GordonN J，Krishnamurthy V.Particle filters for state estimation of jump Markov linear systems［J］.IEEE Trans，Signal Processing，2001，49（49）：613-624.

［7］Spall J C.Estimation via markov chain monte carlo［J］. IEEE Control Systems Magazine，2003，23（2）：34-45.

［8］Li Hongru.The application ofa generalmathematicalmor?phologicalparticle as a novelindicator for the performance degradation assessment of a bearing［J］.Mechanical sys?tems and signalprocessing，2016，10（82）：490-502.

［9］魏國(guó)華.粒子濾波算法研究與實(shí)現(xiàn)［D］.成都：電子科技大學(xué)，2015.

WEI Guohua.Research and implementation of particle fil?ter algorithm［D］.Chengdu：Schoolof Electronic Engineer?ing，2015.

［10］程水英，張劍云.粒子濾波評(píng)述［J］.宇航學(xué)報(bào)，2008，29（4）：1100-1111.

CHENG Shuiying，ZHANG Jianyun.Remark of particle filter［J］.Journal of Astronauties，2008，29（4）：1100-1111.

［11］宋策.基于粒子濾波的目標(biāo)跟蹤技術(shù)研究［D］.長(zhǎng)春：中國(guó)科學(xué)院大學(xué)，2014.

SONG Ce.Research on target tracking based on particle filter［D］.Changchun：University of Chinese Academy of Sciences，2014.

Emulation Research on GPS/DR Integrated Navigation Algorithm Based on Particle Filter

SHEN Yuan NI Fusheng WEI Changyun XIE Ziyang
（1.College of Mechanicaland Engineering，Hohai University，Changzhou 213022）（2.Engineering Research Center of Dredging Technology of Ministry ofEducation，Changzhou 213022）

The traditional navigation mainly based on GPS which provides 3-d position for vehicles has tremendous difficulty in satisfying the stricter requirements for navigation performance in the complex environment of cities.To solve the problem of the navigation accuracy and the reliability of current vehicle navigation，The integrated navigation which combines GPS and DR has been applied to improve the accuracy and continuity of the navigation system.The post-processing method is particle filter.The re?sults show that GPS/DR integrated navigation system can achieve scheduled accuracy.When the GPS signalis broke off，navigation is stilleffected.However，with the extension ofthe interrupted time ofthe GPS signal，the error willalso increase.

navigation，GPS/DR，particle filter，dead reckoning

TN966

10.3969/j.issn.1672-9722.2017.08.017

2017年2月3日，

2017年3月18日

中央高校基本科研業(yè)務(wù)費(fèi)專項(xiàng)資金項(xiàng)目（編號(hào)：2015B30114）資助。

沈圓，男，碩士研究生，研究方向：疏浚技術(shù)與設(shè)備。