專利名稱：基于反饋機(jī)制的車輛導(dǎo)航方法及系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及智能交通技術(shù)領(lǐng)域，特別涉及一種基于反饋機(jī)制的車輛導(dǎo)航方法及系 統(tǒng)。
背景技術(shù)：
車輛導(dǎo)航系統(tǒng)在車輛追蹤，車輛調(diào)度等現(xiàn)實(shí)問題中有著廣泛的應(yīng)用。一個(gè)實(shí)用的 車輛導(dǎo)航系統(tǒng)需要獲取連續(xù)而精確的車輛定位信息。人們通常選用GPS (Global Positioning System，全球定位系統(tǒng))作為車輛的主要 定位信息來源。在正常條件下，GPS能夠提供連續(xù)而準(zhǔn)確的定位信息。但是在都市條件下， 由于建筑物(高樓，隧道)的遮擋，GPS信號(hào)經(jīng)常會(huì)中斷，并且即使GPS信號(hào)能夠被接收到， 在多路徑效應(yīng)下，GPS提供的定位信息也會(huì)出現(xiàn)極大的偏差。所以GPS并不適合單獨(dú)提供車 輛導(dǎo)航系統(tǒng)的定位信息。INSdnertial Navigation System，慣性導(dǎo)航系統(tǒng))是另一種常見 的定位信息來源。這種系統(tǒng)位于車輛內(nèi)部，可以提供連續(xù)的航位信息。然而在實(shí)際應(yīng)用中， 由于車載的慣性儀器一般成本較低，精度不高，累計(jì)誤差會(huì)隨著時(shí)間急劇增長，所以慣性導(dǎo) 航系統(tǒng)也不能單獨(dú)提供定位信息。解決上述問題一種通常的方式是將GPS與INS的數(shù)據(jù)進(jìn)行融合，共同提高定位信 息，從而能夠一定程度上克服二者的固有缺點(diǎn)，從而能夠達(dá)到更好的定位效果。Zhao (參考 文獻(xiàn)[2])等人使用擴(kuò)展Kalman濾波器(EKF)作為主要手段進(jìn)行數(shù)據(jù)的融合。為了在此基 礎(chǔ)上進(jìn)一步提高定位精度，然而Kalman濾波器為主的定位校正手段，在都市環(huán)境中的表現(xiàn) 都不理想，一個(gè)很重要的原因在于，多路徑效應(yīng)造成的GPS偏差遠(yuǎn)遠(yuǎn)大于GPS本身的誤差， 所以GPS的測(cè)量誤差遠(yuǎn)不是高斯分布，從而不符合Kalman濾波器對(duì)于測(cè)量誤差的基本假 設(shè)。另一方面，現(xiàn)有的地圖匹配系統(tǒng)可以將車輛位置限定在某個(gè)路段附近，由于電 子地圖的誤差遠(yuǎn)小于傳感器的誤差，從而可以利用電子地圖的信息作為反饋，對(duì)上述的 INS累積誤差和GPS多路徑偏差進(jìn)行修正，可以有效地減小定位的誤差。對(duì)于地圖匹配， Quddus.(參考文獻(xiàn)[3]) ,Philippe (參考文獻(xiàn)[1])等人有過很多研究。然而，地圖匹配算 法的一個(gè)重要問題在于，車輛的定位誤差容易在復(fù)雜的路況下，造成相似路段之間的誤匹 配。一旦誤匹配發(fā)生，傳統(tǒng)的算法很難進(jìn)行錯(cuò)誤的恢復(fù)。多假設(shè)(Multihypothesis)算法 是一種新提出的地圖匹配算法，這種算法中，所有當(dāng)前可能的路段以及路段的祖先信息都 會(huì)被保留，當(dāng)定位誤差導(dǎo)致的誤匹配發(fā)生時(shí)，正確的路徑仍然存在于假設(shè)空間中，當(dāng)定位誤 差減小時(shí)，該正確路徑自然會(huì)被正確匹配，此時(shí)通過回溯正確路徑可以糾正以往的誤匹配。 實(shí)踐表明，這種算法具有較好的魯棒性。使用地圖匹配進(jìn)行反饋的主要問題在于，當(dāng)前時(shí)刻 用于反饋的匹配結(jié)果并不一定是正確的，如果使用了錯(cuò)誤的匹配路徑進(jìn)行反饋，有可能進(jìn) 一步加大定位的誤差。因而必須使用某種策略來衡量能否使用匹配結(jié)果進(jìn)行反饋，以保證 反饋的正確性。上面提到的參考文獻(xiàn)如下
[l]Philippe Bonnifait, Maged Jabbour, and Gerald Dherbomez. Real-time implementation of a gis—based localization system for intelligent vehicles. EURASIP Journal on Embedded System,2007.[2]L. Zhao, W. Y. Ochieng, M. A. Quddus, and R. B. Noland. An extended kalman filter algorithm for integrating gps and low cost dead reckoning system data for vehicle performance and emissions monitoring. Journal of Navigation, 53 257—275,2003.[3] Quddus Μ. A. , Ochieng W. Y. , Zhao. L. , and Noland R. B. General map matching algorithm for transport telematics applications. GPS Solutions,14(3) 157—167,2003.

發(fā)明內(nèi)容
(一)要解決的技術(shù)問題本發(fā)明要解決的技術(shù)問題是如何提高地圖匹配的準(zhǔn)確程度，降低測(cè)量誤差；同 時(shí)提高車輛定位的精度；降低誤反饋的可能性，提高系統(tǒng)的魯棒性。(二)技術(shù)方案為解決上述技術(shù)問題，本發(fā)明提供了一種基于反饋機(jī)制的車輛導(dǎo)航方法，包括以 下步驟Si、根據(jù)全球定位系統(tǒng)GPS數(shù)據(jù)和慣性數(shù)據(jù)生成車輛定位信息；S2、根據(jù)所述車輛定位信息和電子地圖的數(shù)據(jù)，采用多假設(shè)的地圖匹配算法計(jì)算 車輛最可能所處路段作為匹配結(jié)果；下面所用到的關(guān)于路段的信息，比如匹配的路段信息， 都來自電子地圖的數(shù)據(jù)。S3、判斷所述匹配結(jié)果是否可以用于反饋，若可以，則利用該匹配結(jié)果計(jì)算GPS數(shù) 據(jù)中的位置信息在垂直于路段方向上的偏差、車輛位置信息以及車輛方向信息這三者作為 反饋數(shù)據(jù)，利用該反饋數(shù)據(jù)修正所述車輛定位信息。其中，所述GPS數(shù)據(jù)包括位置信息和有效標(biāo)志位數(shù)據(jù)，所述位置信息包括經(jīng)度、緯 度和方向，所述慣性數(shù)據(jù)包括角速度、加速度和速度數(shù)據(jù)，且步驟Si具體包括S11，對(duì)于測(cè)量得到的所述角速度Wi，如果Coi小于5°，則將ω i置為0，對(duì)于所述 位置信息，用二維坐標(biāo)Xi，Ii分別表示東向和北向的坐標(biāo)；如果存在步驟S3所提供的反饋 數(shù)據(jù)，則利用所述反饋數(shù)據(jù)修正所述GPS數(shù)據(jù)；S12，假設(shè)上一時(shí)刻車輛方向?yàn)镠p1，當(dāng)前時(shí)刻測(cè)得角速度為Coi,當(dāng)前時(shí)刻與前面4 個(gè)時(shí)刻GPS數(shù)據(jù)中的方向分別為屯，d^,屯_2，屯_3，屯_4，由步驟S3所提供的反饋數(shù)據(jù)中，車 輛方向信息為f”則對(duì)當(dāng)前時(shí)刻車輛方向Hi的校正方法為若ω i 大于 0，則 Hi = Η"+ ω 士 ；否則，如果對(duì)于Cli，C^1,屯_2，屯_3，(1卜4中任意兩個(gè)Clj, dk，有dj-dj <5°，則Hi = Average (di; d^,屯_2，屯_3，屯_4)，Average表示取平均；如果對(duì)于(Ii, (Ii^1,屯_2，屯_3，屯_4中任 意兩個(gè)…，dk，有Idj-Clk I彡5°，且如果存在步驟S3反饋的所述方向信息fi;則Hi = &，如 果不存在所述方向信息A，則Hi = Hh ；S13，根據(jù)所述GPS數(shù)據(jù)是否有效，采用卡爾曼濾波器或者航位推算法計(jì)算所述車輛定位信息。其中，所述步驟S13具體為若所述GPS數(shù)據(jù)無效，則采用航位推算法計(jì)算所述車輛定位信息設(shè)上一時(shí)刻的 車輛位置為χ"，y"，當(dāng)前時(shí)刻的速度為Si,車輛方向?yàn)镠i,則當(dāng)前時(shí)刻的車輛位置Xi，Yi 為Xi = Xi^siCos(Hi)，Yi = Yi-^SiSin(Hi)；若GPS數(shù)據(jù)有效，則采用卡爾曼濾波器計(jì)算所述車輛定位信息。其中，所述卡爾曼濾波器為無跡卡爾曼濾波器，在計(jì)算所述車輛定位信息時(shí)，所述 濾波器設(shè)置如下狀態(tài)矩陣為義=^·^, ,^ ，^^7·，其中11，\， 分別為北向位置、速度和加速 度，^^，\分別為東向位置、速度和加速度，ε為獲得所述GPS數(shù)據(jù)時(shí)所采用的陀螺儀的 隨機(jī)漂移系數(shù)，爐為獲得所述GPS數(shù)據(jù)時(shí)所采用的里程計(jì)的比例系數(shù)，T表示矩陣的轉(zhuǎn)置； 狀態(tài)轉(zhuǎn)移矩陣為M =
權(quán)利要求
1.一種基于反饋機(jī)制的車輛導(dǎo)航方法，其特征在于，包括以下步驟51、根據(jù)全球定位系統(tǒng)GPS數(shù)據(jù)和慣性數(shù)據(jù)生成車輛定位信息；52、根據(jù)所述車輛定位信息和電子地圖的數(shù)據(jù)，采用多假設(shè)的地圖匹配算法計(jì)算車輛 最可能所處路段作為地圖匹配結(jié)果；53、判斷所述地圖匹配結(jié)果是否可以用于反饋，若可以，則利用該地圖匹配結(jié)果計(jì)算 GPS數(shù)據(jù)中的位置信息在垂直于路段方向上的偏差、車輛位置信息以及車輛方向信息這三 者作為反饋數(shù)據(jù)，利用該反饋數(shù)據(jù)修正所述車輛定位信息。
2.如權(quán)利要求1所述的方法，其特征在于，所述GPS數(shù)據(jù)包括位置信息和有效標(biāo)志位數(shù) 據(jù)，所述位置信息包括經(jīng)度、緯度和方向，所述慣性數(shù)據(jù)包括角速度、加速度和速度數(shù)據(jù)，且 步驟Sl具體包括S11，對(duì)于測(cè)量得到的所述角速度Coi，如果Coi小于5°，則將Coi置為0，對(duì)于所述位置 信息，用二維坐標(biāo)Xi，Yi分別表示東向和北向的坐標(biāo)；如果存在步驟S3所提供的反饋數(shù)據(jù)， 則利用所述反饋數(shù)據(jù)修正所述GPS數(shù)據(jù)；S12，假設(shè)上一時(shí)刻車輛方向?yàn)镠p1，當(dāng)前時(shí)刻測(cè)得角速度為Oi，當(dāng)前時(shí)刻與前面4個(gè)時(shí) 刻GPS數(shù)據(jù)中的方向分別為屯，d^,屯_2，屯_3，屯_4，由步驟S3所提供的反饋數(shù)據(jù)中，車輛方 向信息為f”則對(duì)當(dāng)前時(shí)刻車輛方向Hi的校正方法為若 Coi 大于 0，則 Hi = Hi^1+ω ,；否則，如果對(duì)于屯，‘，屯_2，屯_3，cU中任意兩個(gè)…，dk，有|drdk| <5°，則Hi = Average (di; d^,屯_2，屯_3，屯_4)，Average表示取平均；如果對(duì)于(Ii, (Ii^1,屯_2，屯_3，屯_4中任 意兩個(gè)…，dk，有Idj-Clk I彡5°，且如果存在步驟S3反饋的所述方向信息fi;則Hi = &，如 果不存在所述方向信息A，則Hi = Hh ；S13，根據(jù)所述GPS數(shù)據(jù)是否有效，采用卡爾曼濾波器或者航位推算法計(jì)算所述車輛定 位信息。
3.如權(quán)利要求2所述的方法，其特征在于，所述步驟S13具體為若所述GPS數(shù)據(jù)無效，則采用航位推算法計(jì)算所述車輛定位信息設(shè)上一時(shí)刻的車輛 位置為Xh，Yi-!,當(dāng)前時(shí)刻的速度為Si,車輛方向?yàn)镠i,則當(dāng)前時(shí)刻的車輛位置Xi，Yi為Xi = Xi-JsiCos(Hi), Yi = Yi-^SiSin(Hi)；若所述GPS數(shù)據(jù)有效，則采用卡爾曼濾波器計(jì)算所述車輛定位信息。
4.如權(quán)利要求3所述的方法，其特征在于，所述卡爾曼濾波器為無跡卡爾曼濾波器，在 計(jì)算所述車輛定位信息時(shí)，所述濾波器設(shè)置如下狀態(tài)矩陣為X = [ ，Vi^nAHApf，其中n，vn,知分別為北向位置、速度和加速度， 別為東向位置、速度和加速度，ε為獲得所述GPS數(shù)據(jù)時(shí)所采用的陀螺儀的隨機(jī) 漂移系數(shù)，爐為獲得所述GPS數(shù)據(jù)時(shí)所采用的里程計(jì)的比例系數(shù)，T表示矩陣的轉(zhuǎn)置；狀態(tài)轉(zhuǎn)移矩陣為
5.如權(quán)利要求1所述的方法，其特征在于，在步驟S2中，采用多假設(shè)的地圖匹配算法計(jì) 算車輛最可能所處路段時(shí)，首先使用假設(shè)節(jié)點(diǎn)表示車輛的狀態(tài)和與該狀態(tài)匹配的路段，所 述假設(shè)節(jié)點(diǎn)包含車輛的位置信息、當(dāng)前匹配的路段信息、車輛在路段上的行駛距離、假設(shè)節(jié) 點(diǎn)的父假設(shè)和假設(shè)節(jié)點(diǎn)的權(quán)值，所述假設(shè)節(jié)點(diǎn)的權(quán)值越大表示假設(shè)節(jié)點(diǎn)的可能性越高，所 有假設(shè)節(jié)點(diǎn)構(gòu)成一棵假設(shè)樹，權(quán)值最大的假設(shè)節(jié)點(diǎn)就是當(dāng)前車輛最可能所處路段，即所述 匹配結(jié)果；從一個(gè)假設(shè)節(jié)點(diǎn)開始，回溯該假設(shè)節(jié)點(diǎn)的父假設(shè)，得到車輛從開始運(yùn)行到當(dāng)前時(shí) 刻的最可能行駛路線。
6.如權(quán)利要求5所述的方法，其特征在于，所述多假設(shè)的地圖匹配算法在每一次新的 車輛狀態(tài)信息產(chǎn)生時(shí)被調(diào)用，被調(diào)用時(shí)執(zhí)行以下幾個(gè)步驟首先根據(jù)新的車輛狀態(tài)信息S更新各個(gè)假設(shè)節(jié)點(diǎn)，對(duì)于每個(gè)假設(shè)節(jié)點(diǎn)更新各個(gè)假設(shè) 節(jié)點(diǎn)的方法分以下幾種情況如果車輛的位置遠(yuǎn)離路段沿車輛行駛方向的端點(diǎn)一定的距離，則直接更新該假設(shè)節(jié) 點(diǎn)，包括更新車輛的位置信息，根據(jù)車輛的速度增大車輛在路段上的行駛距離，以及計(jì)算新 的假設(shè)節(jié)點(diǎn)權(quán)值wQli)，其中Khi) = wOO+P(s，hi)，其中ρ(s，hi)表示車輛狀態(tài)信息s和 假設(shè)節(jié)點(diǎn)h的不相符程度，P(S，10越小，說明車輛越不可能處在Iii所表示的路段上；如果車輛的位置接近路段沿車輛行駛方向的端點(diǎn)一定的距離，則產(chǎn)生新的假設(shè)節(jié)點(diǎn)， 具體方式是對(duì)于每條與上述端點(diǎn)相鄰的路段，假設(shè)節(jié)點(diǎn)h分別產(chǎn)生新的假設(shè)節(jié)點(diǎn)~，包括 設(shè)定當(dāng)前車輛的位置信息，設(shè)定新的匹配路段信息，初始化車輛在路段上的行駛距離，并將 新的假設(shè)節(jié)點(diǎn)h的父假設(shè)設(shè)置為原來的假設(shè)節(jié)點(diǎn)設(shè)置新的假設(shè)節(jié)點(diǎn)h的權(quán)值w(hj，其 中w(hj) = Khi)+ρ (s, hp，權(quán)值p(s，hp表示子假設(shè)會(huì)繼承父假設(shè)的可能性；當(dāng)所有節(jié)點(diǎn)更新完畢之后，從所述假設(shè)樹中刪除權(quán)值從大到小排名靠后的假設(shè)；其中， 所刪除的排名靠后的節(jié)點(diǎn)個(gè)數(shù)為預(yù)設(shè)值。
7.如權(quán)利要求6所述的方法，其特征在于，步驟S3中判斷所述匹配結(jié)果是否可以用于 反饋的方法為首先計(jì)算假設(shè)樹中刪除權(quán)值從大到小排名最前的假設(shè)節(jié)點(diǎn)的評(píng)價(jià)值jl，如果jl小于 閾值t，則所述匹配結(jié)果不可以用于反饋；否則計(jì)算排名第二的假設(shè)節(jié)點(diǎn)的評(píng)價(jià)值j2，如果jl > 2*j2，則所述匹配結(jié)果能夠用于反饋。
8.如權(quán)利要求7所述的方法，其特征在于，根據(jù)車輛方向與路段方向之差δΗ以及車輛 的位置距路段的距離S D來計(jì)算所述評(píng)價(jià)值j = (1-δ H/60)/2+(1-( δ D/90)2)/2o
9.如權(quán)利要求7所述的方法，其特征在于，在步驟S3中，計(jì)算車輛位置(X，y)在匹配 路段的垂直投影(X'，1’ )，則(X-X' ,1-1’ )分別是車輛位置在東向和北向的偏差，得 到所述位置信息在垂直于路段方向上的偏差；在車輛的角速度為0時(shí)，匹配路段的方向就 是反饋的車輛方向信息；利用權(quán)值最大的假設(shè)節(jié)點(diǎn)中記錄的車輛在匹配路徑上行駛的距離 計(jì)算車輛在路徑上的位置，這個(gè)位置就是反饋的車輛位置信息。
10.一種基于反饋機(jī)制的車輛導(dǎo)航系統(tǒng)，其特征在于，包括定位模塊，用于根據(jù)全球定位系統(tǒng)GPS數(shù)據(jù)和慣性數(shù)據(jù)生成車輛定位信息；匹配模塊，用于根據(jù)所述車輛定位信息和電子地圖的數(shù)據(jù)，采用多假設(shè)的地圖匹配算 法計(jì)算車輛最可能所處路段作為匹配結(jié)果；反饋模塊，用于判斷所述匹配結(jié)果是否可以用于反饋，若可以，則利用該地圖匹配結(jié)果 計(jì)算GPS數(shù)據(jù)中的位置信息在垂直于路段方向上的偏差、車輛位置信息以及車輛方向信息 這三者作為反饋數(shù)據(jù)，利用該反饋數(shù)據(jù)修正所述車輛定位信息。
全文摘要
本發(fā)明公開了一種基于反饋機(jī)制的車輛導(dǎo)航方法，包括以下步驟S1、根據(jù)全球定位系統(tǒng)GPS數(shù)據(jù)和慣性數(shù)據(jù)生成車輛定位信息；S2、根據(jù)所述車輛定位信息和電子地圖的數(shù)據(jù)，采用多假設(shè)的地圖匹配算法計(jì)算車輛最可能所處路段作為地圖匹配結(jié)果；S3、判斷所述地圖匹配結(jié)果是否可以用于反饋，若可以，則利用該地圖匹配結(jié)果計(jì)算GPS數(shù)據(jù)中的位置信息在垂直于路段方向上的偏差、車輛位置信息以及車輛方向信息這三者作為反饋數(shù)據(jù)，利用該反饋數(shù)據(jù)修正所述車輛定位信息。本發(fā)明采用了改進(jìn)的多假設(shè)的地圖匹配算法，提高了地圖匹配的準(zhǔn)確程度，降低了測(cè)量誤差；進(jìn)一步提高了車輛定位的精度。有效降低了誤反饋的可能性，提高了系統(tǒng)的魯棒性。
文檔編號(hào)G01C21/26GK102147258SQ20101062106
公開日2011年8月10日 申請(qǐng)日期2010年12月24日 優(yōu)先權(quán)日2010年12月24日
發(fā)明者劉松, 庫敏, 戴良光, 王道順, 董琳, 賀志宏, 賈星星, 高達(dá) 申請(qǐng)人:清華大學(xué)