鄧志紅，孫亮，付夢印,2，王博

(1.北京理工大學 自動化學院，北京 100081； 2.南京理工大學 自動控制系，南京 210094)

基于慣導信息的地圖匹配算法

鄧志紅1，孫亮1，付夢印1,2，王博1

(1.北京理工大學 自動化學院，北京 100081； 2.南京理工大學 自動控制系，南京 210094)

為抑制慣性導航系統(tǒng)(inertial navigation system, INS)導航誤差發(fā)散，提出了一種基于INS信息的地圖匹配方法。該方法根據(jù)INS輸出位置信息的特點，引入了“平移向量”的概念，并利用一種改進的投影匹配法對平移向量進行修正。跑車實驗結果表明，與傳統(tǒng)地圖匹配算法相比，本文所提出的慣導/地圖匹配算法充分利用了INS的信息，具有匹配效率高、匹配精度高等優(yōu)點，為基于地圖匹配信息進行INS的誤差校正奠定了基礎。

慣性導航系統(tǒng)； 地圖匹配算法； 平移向量； 縱向位置誤差； 橫向位置誤差； 誤差修正

慣性導航系統(tǒng)以其自主性、實時性和全導航參量的優(yōu)點在各類車輛中應用廣泛，但INS的定位誤差隨時間而積累，必須引入其他的輔助手段來對INS進行校正，常用方法有全球定位系統(tǒng)(global positioning system, GPS)、地圖匹配及航位推算等?，F(xiàn)階段，數(shù)字地圖的精度已經(jīng)可以達到米級，甚至更高，因此用地圖匹配方法來輔助INS進行導航已經(jīng)成為熱點。地圖匹配方法有多種，如權值法[1]、概率統(tǒng)計法[2]、模糊邏輯法[3]、拓撲結構法[4]、D-S推理法[5]、Kalman濾波法[6]及貝葉斯法[7]等，但上述方法大部分都是針對GPS數(shù)據(jù)的，而針對INS數(shù)據(jù)的極少。ZHAO等提出了基于曲率半徑的地圖匹配算法[8]；龔柏春等提出了基于移動相關的最小二乘地圖匹配算法[9]；LUO H等在基于權重地圖匹配算法的基礎上，針對道路搜索提出了二次網(wǎng)格劃分的搜索方法[10]。文獻[8]是通過曲率計算INS軌跡與道路的匹配度，進而選擇最優(yōu)匹配道路。文獻[9]是利用最小二乘法計算INS軌跡與道路的相關性，再選擇最優(yōu)匹配道路。雖然文獻[8-9]可以消除INS沿著道路的縱向位置誤差，但是文獻[8-9]提出的地圖匹配算法都是在車輛的航向角發(fā)生較大變化時才有效，并且計算量較大。而文獻[10]提出了加快道路搜索的方法，但不能夠消除INS沿道路的縱向位置誤差。

為了能夠消除INS與道路垂直的橫向位置誤差和沿著道路的縱向位置誤差，同時有更高的匹配效率和匹配精度，本文提出了一種新的基于INS信息的地圖匹配算法。與文獻[8]相比，本文采用基于動態(tài)權值的道路選擇方法，具有更高的效率。在此基礎上，引入平移向量，利用平移向量對當前時刻INS數(shù)據(jù)的偏移進行補償，進而實現(xiàn)對與道路垂直的橫向位置誤差和沿著道路的縱向位置誤差的修正。

1 慣導/地圖匹配算法

1.1 基于平移向量的道路搜索方法

INS的誤差主要包括三種形式的周期振蕩：舒拉周期振蕩、地球周期振蕩和付科周期振蕩。對于裝載純INS的車載導航系統(tǒng)，雖然這種周期振蕩會導致導航位置出現(xiàn)偏差，但它的位置依然是連續(xù)變化的。因此，對于INS來說，可以引入一個平移向量，在每次進行匹配候選道路搜索之前，都對當前INS輸出的位置進行補償，使補償之后的位置更加接近真實的位置，進而縮小搜索候選道路的范圍，節(jié)省時間，提高效率。另外，候選道路的選擇范圍和INS返回的速率相關，速率大，選擇范圍變大；速率小，選擇范圍變小。

如圖1所示，其中AB為道路，點Pt-1、Qt-1、Ct-1和Rt-1分別為t-1時刻INS輸出位置點、經(jīng)過平移向量補償后的INS位置點、地圖匹配后的位置點和車輛真實位置點。定義平移向量：v=Pt-1Ct-1，在t時刻，相應的點為Pt、Qt和Rt。在選取候選道路之前，用平移向量v對INS位置點Pt進行補償，得到補償之后的位置點Qt，公式為Qt=v+Pt。

容易看出，點Qt比點Pt更接近于真實位置Rt。即，如果平移向量選取適當，只要選取很小范圍內(nèi)的路段作為候選道路即可。

圖1 平移向量對INS位置的補償Fig.1 Compensation of location based on translation vector

1.2 基于動態(tài)權值的道路選擇方法

下面具體分析影響候選道路選擇的因素：

因素1：INS位置與候選道路的距離d。

如圖2所示，其中AB為道路，點Q1、Q2和Q3代表經(jīng)過平移向量補償后INS可能輸出的三種不同情況位置點。為了區(qū)分三種情況，引入輔助變量γ，設INS補償后的位置點Q(Q1或Q2或Q3)的坐標為(x,y)，道路上點A為(x1,y1)，點B為(x2,y2)。則輔助變量γ的計算公式為

(1)

圖2 距離d與γ的定義Fig.2 The definition of distance d and γ

當INS位置點在區(qū)域I中，即γ<0時，定義距離d=|Q1A|；當INS位置點在區(qū)域Ⅱ中(包括邊界)，即0≤γ≤1時，定義距離d=|Q2C|；當INS位置點在區(qū)域Ⅲ中，即γ>1時，定義距離d=|Q3B|。

因素2：INS航向與道路方向的夾角。

如圖3所示，其中AB為道路，P為INS輸出位置點，以P為起點的向量為載體的航向角向量，平移航向角向量，使點P與點A重合，β即為載體航向與道路矢量的夾角。定義β余弦值的絕對值為θ，即θ=|cosβ|。

圖3 INS航向與道路的夾角βFig.3 Angle β between INS and road

由于載體的航向角在道路轉彎處會發(fā)生連續(xù)的變化，如果采用變化中的航向角作為加權因素之一，可能產(chǎn)生錯誤的結果。因此，道路轉彎處可加入一個延遲環(huán)節(jié)，即當載體的航向角穩(wěn)定后，再進行計算。

判斷載體是否進入轉彎階段方法：根據(jù)載體航向角的變化率和經(jīng)過地圖匹配算法后的載體位置點與交叉路口點的距離可以判斷載體是否進入轉彎階段，即當載體航向角的變化率越大，地圖匹配后的位置點與交叉路口點的距離越短時，載體進入轉彎的可能性越大。

因素3：道路網(wǎng)絡的拓撲結構。

即車輛在道路上行駛，當前時刻t車輛所在的道路一定與前一時刻t-1車輛所在的道路相同或者相鄰(假設兩個時刻間隔足夠短)。

如圖4所示，其中有7段道路，分別為1～7。點Rt-1和Rt分別為t-1時刻和t時刻對應的載體位置點。假設t-1時刻載體在Rt-1位置，即在道路1上，那么t時刻，載體行駛的可能道路為1～3。當t時刻載體在Rt位置，即在道路3上時，那么t+1時刻載體行駛的可能道路為1～4，而不可能為道路5～7。為了加快選擇速度，減少計算時間，在計算加權值之前，可利用道路的拓撲性排除不符合的候選道路，進而減少后續(xù)的計算量。

圖4 道路網(wǎng)絡的拓撲結構Fig.4 Topological structure of road network

最后，加權公式為

(2)

式中：fi是道路i的加權值，di是INS補償后的位置點Q與道路i的距離，θi是INS航向角與道路i夾角余弦值的絕對值。當fi越大時，表明道路i的可信度越大，反之越小。ω1和ω2可以根據(jù)位置點所在的道路網(wǎng)絡結構進行動態(tài)調(diào)整。調(diào)整規(guī)則如下：當候選道路較少，路網(wǎng)比較簡單時，取ω1=0.6，ω2=0.4；當候選道路較多，路網(wǎng)比較復雜時，取ω1=0.4，ω2=0.6；當載體離道路的交叉點比較近，即可能發(fā)生轉彎時，取ω1=0.35，ω2=0.65。統(tǒng)計fi大于閾值H的個數(shù)，如果候選道路個數(shù)為0，則等待下一個INS信息重新開始。如果候選道路個數(shù)連續(xù)為0的個數(shù)大于H3，則進行初始對準，重新開始。如果候選道路個數(shù)為1，則選擇該道路為匹配道路；如果候選道路個數(shù)大于1，則判斷最大加權值fmax和次大加權值fsmax的差值是否大于閾值H1，即fmax-fsmax>H1，并且判斷fmax>H2是否成立，如果是則選擇最大加權值fmax對應的道路作為匹配道路，否則等待下一個INS信息重新開始。其中H、H1、H2和H3取值分別為0.9、0.03、0.95和8，可根據(jù)道路實際情況進行動態(tài)調(diào)整。

1.3 基于平移向量的INS位置修正方法

在候選道路中選出匹配道路后，采用平移向量補償后的位置點Q進行投影匹配。參考圖2，使用輔助變量γ，當γ<0時，即在區(qū)域I中，位置點Q1投影到A點，投影點C與A重合；當0≤γ≤1，即在區(qū)域Ⅱ中時(包括邊界)，位置點Q2投影到點C；當γ>1，即在區(qū)域Ⅲ中時，點Q3投影到點B，投影點C

與B重合。當0≤γ≤1時，設Q2點的坐標為(x,y)，A的坐標為(x1,y1)，B的坐標為(x2,y2)，投影點C的坐標為(xi,yi)，則

圖5 算法流程圖Fig.5 Flow chart of algorithm

(3)

得到投影點C之后，修正平移向量，即v=PC。由慣性導航系統(tǒng)輸出位置信息的連續(xù)性可以推出，相鄰的兩個平移向量應滿足|vt-vt-1|

算法流程圖如圖5所示。

2 跑車試驗驗證

2.1 試驗條件

為了驗證算法的有效性，用實地跑車數(shù)據(jù)進行了檢驗。實地跑車采用某型號SINS，陀螺的漂移為0.02(°)/h，加速度計的零偏為50 μg。使用INS和GPS組合導航輸出的位置作為車輛的真實參考位置，精度優(yōu)于0.1 m。地圖選用MapInfo格式的北京市電子地圖，精度為1 m。實際跑車起點為北京市門頭溝區(qū)軍莊鎮(zhèn)西楊坨村北，終點為北京市豐臺區(qū)長辛店鎮(zhèn)大灰廠村東，全程約20 km，耗時約30 min，平均速度約為40 km/h，整段路程路況復雜，包括直行路段、丁字路口、多叉路口、平行路段等。路線如圖6中粗線所示。

圖6 跑車線路圖Fig.6 Route of driving car

2.2 試驗結果

圖6為跑車結果及終點附近局部放大圖。其中1線為INS返回的載體位置點，2線為地圖匹配的位置點,3線為GPS和INS組合導航返回的載體精確位置點(載體參考位置點)。從圖7中可以看出，INS位置點經(jīng)過地圖匹配算法后，可以得到更加準確的載體位置。(地圖匹配位置點和參考位置點基本重合)

圖7 跑車試驗結果Fig.7 Experimental result of driving car

整段路程INS位置點相對于參考位置點的距離和地圖匹配后INS位置點相對于參考位置點的距離隨時間的變化如圖8(a)所示。作為對比，去掉平移向量后，INS位置點相對于參考位置點的距離和地圖匹配后INS位置點相對于參考位置點的距離隨時間的變化如圖8(b)所示。

從圖8中可以看出，車輛在行駛一段時間后，INS位置點相對于參考位置點的距離出現(xiàn)了明顯的誤差，最大距離約為220 m，這個誤差包括與道路垂直的橫向位置誤差和沿著道路的縱向位置誤差。而經(jīng)過有平移向量的地圖匹配算法后，INS位置點與參考位置點之間的距離明顯縮小，約為1.5 m。而去掉平移向量之后，經(jīng)過地圖匹配算法的INS位置點相對于參考位置點之間的最大距離由220 m減小到約100 m，雖然誤差得到了修正，但效果并不明顯。

3 結論

1)多動態(tài)權值的道路選擇方法、改進的投影修正方法、平移向量和轉彎延遲的引入，使地圖匹配算法不僅可以消除INS與道路垂直的橫向位置誤差和沿著道路的縱向位置誤差，且在車輛航向角變化較小的情況下，仍然具有很高的匹配精度，這為基于地圖匹配信息的INS的誤差校正奠定了基礎。

2)INS可以輸出車輛的高度信息和姿態(tài)信息，如何利用該信息輔助地圖匹配算法，提高匹配精度和效率，可以作為本文進一步研究的重點。

[1] LI L, QUDDUS M, LIN Z. High accuracy tightly-coupled integrity monitoring algorithm for map-matching [J]. Transportation research part C: emerging technologies, 2013, 36: 13-26.

[2] BIERLAIRE M, CHEN J, NEWMAN J. A probabilistic map matching method for smartphone GPS data [J]. Transportation research part C: emerging technologies, 2013, 26: 78-98.

[3] REN M, KARIMI H A. Movement pattern recognition assisted map matching for pedestrian/wheelchair navigation [J]. The journal of navigation, 2012, 65(4): 617-633.

[4] BLAZQUEZ C, MIRANDA P, PONCE A. Performance of a new enhanced topological decision-rule map-matching algorithm for transportation application [J]. Journal of applied research and technology, 2012, 10(6): 929-940.

[5] NASSREDDINE G, ABDALLAH F, DENOEUX T. Map matching algorithm using analysis and Dempster-Shafer theory [C]//2009 IEEE Intelligent Vehicles Symposium, 2009: 494-499.

[6] CHU H J, TSAI G J, CHIANG K W, et al. GPS/MEMS INS data fusion and map matching in urban areas [J]. Sensors, 2013, 13(9): 11280-11288.

[7] SMAILI C, EI NAJJAR M E B, CHARPILLET F. A hybrid Bayesian framework for map matching formulation using switching kalman filter [J]. Journal of intelligent & robotic systems, 2014, 74(3): 725-743.

[8] ZHAO C Z, LI S L, LENG Y X. A new map matching algorithm for In-vehicle Inertial Navigation Systems [C]// 2010 Second IITA International Conference on Geoscience and Remote Sensing. Shandong, China, 2010: 590-592.

[9] 龔柏春,羅建軍,李歲勞,等.基于移動相關的最小二乘法地圖匹配新算法[J].中國慣性技術學報, 2012, 20(4): 435-439. GONG Baichun, LUO Jianjun, LI Suilao, et al. Novelmap-matching algorithm based on moving-related least squares [J]. Journal of Chinese inertial technology, 2012, 20(4): 435-439.

[10] LUO H, DENG Z H, FU M Y, et al. A map matching algorithm for Inertial Navigation Systems based on the adaptive projection method[C]//2014 Seventh International Symposium on Computational Intelligence and Design. Hangzhou,China, 2014: 304-308.

本文引用格式：

鄧志紅，孫亮，付夢印，等. 基于慣導信息的地圖匹配算法[J]. 哈爾濱工程大學學報， 2017， 38(8): 1268-1272.

DENG Zhihong, SUN Liang, FU Mengyin, et al. New map-matching algorithm based on inertial navigation system[J]. Journal of Harbin Engineering University， 2017， 38(8): 1268-1272.

New map-matching algorithm based on inertial navigation system

DENG Zhihong1, SUN Liang1, FU Mengyin1,2, WANG Bo1

(1.School of Automation, Beijing Institute of Technology, Beijing 100081, China; 2.Department of Automatic control, Nanjing University of Science and Technology, Nanjing 210094, China)

To suppress the divergence of navigation error, a new map-matching algorithm was presented based on the position information of an inertial navigation system (INS). The algorithm proposed the concept of translation vector according to the characteristics of the information of INS. An improved projection method was presented to correct the translation vector. Experimental results of a car moving in real time show that the map-matching algorithm based on INS maximizes the information of INS and is more efficient and precise than the traditional map-matching algorithm. Thus, our findings serve as a foundation for error correction of INS based on map-matching information.

inertial navigation system (INS); map matching algorithm; translation vector; error of longitudinal position; error of lateral position; error correction

2016-10-17.

日期：2017-04-28.

國家自然科學基金項目(61422102).

鄧志紅(1974-), 女, 教授，博士生導師.

鄧志紅，E-mail：dzh_deng@bit.edu.cn.

10.11990/jheu.201610055

U666.1

A

1006-7043(2017)08-1268-05

網(wǎng)絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20170428.1338.026.html