鄧志紅，孫亮，付夢(mèng)印,2，王博

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

基于慣導(dǎo)信息的地圖匹配算法

鄧志紅1，孫亮1，付夢(mèng)印1,2，王博1

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

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

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

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

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

1 慣導(dǎo)/地圖匹配算法

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

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

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

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

圖1 平移向量對(duì)INS位置的補(bǔ)償Fig.1 Compensation of location based on translation vector

1.2 基于動(dòng)態(tài)權(quán)值的道路選擇方法

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

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

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

(1)

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

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

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

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

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

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

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

因素3：道路網(wǎng)絡(luò)的拓?fù)浣Y(jié)構(gòu)。

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

如圖4所示，其中有7段道路，分別為1～7。點(diǎn)Rt-1和Rt分別為t-1時(shí)刻和t時(shí)刻對(duì)應(yīng)的載體位置點(diǎn)。假設(shè)t-1時(shí)刻載體在Rt-1位置，即在道路1上，那么t時(shí)刻，載體行駛的可能道路為1～3。當(dāng)t時(shí)刻載體在Rt位置，即在道路3上時(shí)，那么t+1時(shí)刻載體行駛的可能道路為1～4，而不可能為道路5～7。為了加快選擇速度，減少計(jì)算時(shí)間，在計(jì)算加權(quán)值之前，可利用道路的拓?fù)湫耘懦环系暮蜻x道路，進(jìn)而減少后續(xù)的計(jì)算量。

圖4 道路網(wǎng)絡(luò)的拓?fù)浣Y(jié)構(gòu)Fig.4 Topological structure of road network

最后，加權(quán)公式為

(2)

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

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

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

與B重合。當(dāng)0≤γ≤1時(shí)，設(shè)Q2點(diǎn)的坐標(biāo)為(x,y)，A的坐標(biāo)為(x1,y1)，B的坐標(biāo)為(x2,y2)，投影點(diǎn)C的坐標(biāo)為(xi,yi)，則

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

(3)

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

算法流程圖如圖5所示。

2 跑車試驗(yàn)驗(yàn)證

2.1 試驗(yàn)條件

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

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

2.2 試驗(yàn)結(jié)果

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

圖7 跑車試驗(yàn)結(jié)果Fig.7 Experimental result of driving car

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

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

3 結(jié)論

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

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

[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] 龔柏春,羅建軍,李歲勞,等.基于移動(dòng)相關(guān)的最小二乘法地圖匹配新算法[J].中國(guó)慣性技術(shù)學(xué)報(bào), 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.

本文引用格式：

鄧志紅，孫亮，付夢(mèng)印，等. 基于慣導(dǎo)信息的地圖匹配算法[J]. 哈爾濱工程大學(xué)學(xué)報(bào)， 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.

國(guó)家自然科學(xué)基金項(xiàng)目(61422102).

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

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

10.11990/jheu.201610055

U666.1

A

1006-7043(2017)08-1268-05

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