基于三次B樣條多信息融合實時地圖匹配方法

陳文杰，李浩，沈勇

(同濟大學汽車學院，上海 201804)

0 引言

車輛智能駕駛、車道級導(dǎo)航、智能交通等，均要求運動載體有極高的定位精度以及可靠性，載體的高精度定位是實現(xiàn)智能化交通的核心問題之一[1-2]。目前車輛獲取高精度位置一般通過GPS、實時動態(tài)定位技術(shù)(Real Time Kinematic, RTK)、慣性定位(Inertial Navigation System, INS)、航位推算(Dead Reckoning, DR)等之間相互組合獲得[3-5]。這些組合定位技術(shù)或多或少均存在一定的定位誤差，因此利用電子地圖提供的絕對精度數(shù)據(jù)，通過地圖匹配算法，能夠減小定位誤差，達到精確定位的目的。在地圖匹配定位技術(shù)領(lǐng)域，有效的地圖匹配算法可以顯著地提高定位精度。常見的地圖匹配算法有拓撲關(guān)系算法、模糊邏輯算法、概率統(tǒng)計算法、神經(jīng)網(wǎng)絡(luò)算法等[6-9]，這些算法要么匹配的精度不高，要么原理復(fù)雜，實現(xiàn)難度大，對硬件要求較高，不適用于實時地圖匹配。實時地圖匹配算法要求實時性、魯棒性、精確性[10]。匹配算法的實時性主要體現(xiàn)在匹配候選區(qū)域的時間以及匹配規(guī)則的復(fù)雜度，魯棒性體現(xiàn)在算法對于各種異常情況做出正確的判斷和處理，精確性體現(xiàn)在導(dǎo)航定位誤差、電子地圖精度誤差、算法計算誤差。

文中根據(jù)實時地圖匹配算法的要求，提出一種基于三次B樣條的多信息融合實時地圖匹配方法。首先對采集的高精度定位數(shù)據(jù)進行區(qū)域劃分，接著采用帶權(quán)約束三次B樣條壓縮算法對道路信息進行壓縮，獲得適用于基于三次B樣條多信息融合匹配方法的控制點序列和節(jié)點矢量，創(chuàng)建適用于嵌入式定位系統(tǒng)的電子地圖；然后求解三次B樣條，以點間距離、航向、磁場、坡度、曲率因素為權(quán)重的參數(shù)值，進行地圖匹配，并對地圖匹配修正量進行更新和校正，最終實現(xiàn)車輛在道路上的實時高精度匹配定位。

1 創(chuàng)建適用嵌入式定位系統(tǒng)的電子地圖

電子地圖是指用數(shù)字的形式來描述地圖要素，文中創(chuàng)建的電子地圖的地圖要素包含：定位位置、地磁基準及特征、坡度特征。創(chuàng)建適用于嵌入式定位系統(tǒng)的電子地圖流程如圖1所示，創(chuàng)建的過程分為3個部分：

(1)采集道路信息。采用高精度定位模塊，利用GPS/RTK采集道路位置信息、磁強計采集道路磁場特征信息、加速度計采集道路坡度特征信息。

(2)道路區(qū)域劃分。道路區(qū)域劃分用于將道路信息的特征進行分組，以便于后續(xù)的地圖匹配。

(3)道路信息壓縮。采集的原始道路信息的數(shù)據(jù)量非常龐大，不僅存在不必要的信息冗余，而且對硬件的存儲要求較高，同時不利于對電子地圖的快速搜索，因此需要對采集的原始道路信息進行壓縮。文中采用帶權(quán)約束的三次B樣條壓縮算法，在保證一定精度范圍的前提下，壓縮率盡可能小。

圖1 嵌入式定位系統(tǒng)電子地圖繪制流程

1.1 道路區(qū)域劃分

為了便于道路特征的分組，并且保證航向、曲率特征的唯一性，對道路進行區(qū)域劃分，如圖2所示，對同濟大學嘉定校區(qū)部分道路進行區(qū)域劃分，同時為了使軌跡點能夠更快速且準確地搜索到所處道路區(qū)域，取每個區(qū)域的中心點作為該區(qū)域的位置標記。通過計算軌跡點到位置標記的距離，可以判定軌跡點處在哪個匹配區(qū)域。

圖2 道路區(qū)域劃分

1.2 帶權(quán)約束三次B樣條壓縮算法

傳統(tǒng)壓縮算法有Douglas-Peucker壓縮算法、分段多項式曲線壓縮算法、垂距限值法、光欄法等[11]，然而這些壓縮算法無法在較低壓縮率的情況下，保證一定的精度。文中采用帶權(quán)約束三次B樣條壓縮算法，該算法可以在較低壓縮率的情況下，保證一定的精度，獲得用于基于三次B樣條多信息融合匹配方法的控制點序列和節(jié)點矢量。

1.2.1 算法實現(xiàn)過程

首先確定采集的道路信息數(shù)據(jù)所對應(yīng)的參數(shù)值以及節(jié)點矢量：

(1)

其次根據(jù)曲率來確定各個數(shù)據(jù)點的約束權(quán)值，采用的方式如下：

(2)

式中：KC為各個數(shù)據(jù)點的約束權(quán)值；Cr為數(shù)據(jù)點的曲率大??；ρCr為數(shù)據(jù)點曲率變化率。

最后引入目標函數(shù)，求解壓縮后的控制點序列。這里令S為非約束數(shù)據(jù)(數(shù)據(jù)點約束權(quán)值為1)集合，T為帶權(quán)約束數(shù)據(jù)集合，W為各個數(shù)據(jù)權(quán)值的集合，N為非約束數(shù)據(jù)對應(yīng)的基函數(shù)集合，M為帶權(quán)約束數(shù)據(jù)的基函數(shù)集合，P為所求控制點集合。

帶權(quán)約束三次B樣條壓縮算法的思想在于在滿足約束條件MP=T的前提條件下，使得非約束方程組NP=S的殘差S-NP的加權(quán)平方和最小。引入拉格朗日乘子A=[λi]，根據(jù)拉格朗日乘子法，得出如下目標函數(shù)：

f(P,A)=(ST-PTNT)W(S-NP)+AT(MP-T)

(3)

分別對A和P求導(dǎo)，并令其等于零，得到：

(4)

聯(lián)立方程組可以先求得A,然后通過A求得P:

(5)

最終道路信息數(shù)據(jù)壓縮為控制點序列P及節(jié)點矢量U，由P和U即可解壓為一條逼近非約束數(shù)據(jù)項，并插值約束數(shù)據(jù)項的一條曲線：

(6)

為了驗證帶權(quán)約束三次B樣條壓縮算法的優(yōu)點，將其與Douglas-Peucker壓縮算法進行對比，如圖3、圖4所示，在壓縮率均取5%的情況下，對區(qū)域1進行壓縮，可以看出帶權(quán)約束三次B樣條壓縮算法在距離誤差和方向誤差上較小，其距離誤差的最大值為4.81 cm，方向誤差的最大值為2.13°，并且其誤差變化穩(wěn)定，不存在突變。

圖3 兩種壓縮算法距離誤差比較

圖4 兩種壓縮算法方向誤差比較

1.2.2 壓縮率的選取

壓縮率選取得恰當與否直接影響到壓縮的質(zhì)量，過低的壓縮率，無法保證一定的精度，而過高的壓縮率又會導(dǎo)致數(shù)據(jù)的冗余。矢量數(shù)據(jù)壓縮的原則為：在采用盡可能小壓縮率的同時，保證較小的極差δM和離差σP[12]。

根據(jù)這個壓縮原則，如圖5、圖6所示，采用帶權(quán)約束三次B樣條壓縮算法對區(qū)域1以不同的壓縮率(0～20%)，分別計算極差和離差?？梢钥闯觯寒攭嚎s率為4%時，極差和離差的大小已經(jīng)滿足了數(shù)據(jù)壓縮精度的要求，并且其減小的趨勢趨于平穩(wěn)狀態(tài)，再加大壓縮率對于其減小的效果不明顯。

對其他幾個區(qū)域采用相同的方式得出極差、離差和壓縮率的關(guān)系，可以得到相同的結(jié)果，故文中對道路信息數(shù)據(jù)采用4%壓縮率帶權(quán)約束三次B樣條壓縮算法，在保證較小極差、離差的前提下，以最小的壓縮率壓縮數(shù)據(jù)。

圖5 區(qū)域1壓縮率α和極差δM的關(guān)系

圖6 區(qū)域1壓縮率α和離差σP的關(guān)系

2 基于三次B樣條多信息融合匹配方法

在車輛運行過程中，綜合考慮定位系統(tǒng)輸出的實時位置、航向、加速度、磁場以及由歷史軌跡推出的軌跡曲率，作者設(shè)計了一種基于三次B樣條的多信息融合實時匹配方法。算法的流程如圖7所示，該算法分為3個階段：

(1)初始階段。由于在初始時刻，車輛處于由靜止狀態(tài)轉(zhuǎn)為運動狀態(tài)的過程。在靜止狀態(tài)，此時所得到的汽車航向信息并不準確，匹配區(qū)域的確定容易出現(xiàn)誤判定，同時存儲的歷史軌跡序列在這一階段為無效數(shù)據(jù)，由于后續(xù)軌跡點的匹配將用到初始匹配信息，為了保證后續(xù)軌跡點的匹配準確性，在車輛靜止狀態(tài)直接將軌跡點作為匹配點輸出。在運動狀態(tài)將進行匹配區(qū)域的確定以及歷史數(shù)據(jù)的存儲。

(2)匹配階段。綜合利用歷史軌跡序列，得到航向變化率、曲率大小及變化率、坡度，對滿足特征條件的軌跡點，確定以航向、曲率、坡度、點間距離、磁場強度為加權(quán)的參數(shù)值進行特征路段匹配；對于不滿足特征條件的匹配點，首先對軌跡點進行預(yù)估校正，然后確定以點間距離和磁場強度加權(quán)的參數(shù)值進行非特征路段匹配。

(3)更新校正階段。對地圖匹配修正量e(k)的更新和校正用于減小軌跡點與車輛真實位置之間的偏差，從而提高匹配精度。當軌跡點處于特征路段匹配階段時，由于此時的匹配精度較高，將對e(k)進行更新；而當軌跡點處于非特征路段匹配時，只對e(k)進行校正而不進行更新。

圖7 地圖匹配算法流程圖

2.1 匹配區(qū)域確定

匹配初始階段時，主要是確定車輛行駛的匹配區(qū)域。初始階段下，為了減少誤匹配，車輛靜止狀態(tài)下的軌跡點不進行匹配，直接將軌跡點作為匹配點輸出。為了保證初始匹配區(qū)域的準確性，在車輛運動狀態(tài)下，如果6個歷史軌跡點的匹配區(qū)域均為一個區(qū)域，則確定該匹配區(qū)域為初始匹配區(qū)域。

匹配區(qū)域的確定。首先分別計算軌跡點到各個區(qū)域的距離權(quán)值、磁場權(quán)值以及方向權(quán)值，加權(quán)得出各匹配區(qū)域的總權(quán)值，取權(quán)值最大的區(qū)域作為匹配區(qū)域(這里約定當某項權(quán)值為負數(shù)時，該項權(quán)值為零)。

候選區(qū)域Ri的總權(quán)值WRi定義為：

WRi=WDi+WBi+WHi

(7)

式中：距離權(quán)值WDi表示軌跡點與各匹配區(qū)域的接近程度，接近程度越大則距離權(quán)值也越大；磁場權(quán)值WBi表示軌跡點的磁場與各區(qū)域的磁場基準的相近程度，越相近則磁場權(quán)值越大；方向權(quán)值WHi表示軌跡點的方向與各區(qū)域的投影點方向的一致程度。

距離權(quán)值定義為：

(8)

式中：Dmax是軌跡點到各匹配區(qū)域投影距離的最大值；Di是軌跡點到各匹配區(qū)域的投影距離；Dk是距離權(quán)值系數(shù)(文中Dk取0.4)。

磁場權(quán)值定義為：

(9)

式中：Bmax是軌跡點的磁場強度與各區(qū)域的磁場基準差值的最大值；Bi是軌跡點的磁場強度與各區(qū)域的磁場基準的差值；Bk是磁場權(quán)值系數(shù)(文中Bk取0.2)。

方向權(quán)值定義為：

WHi=(v-3)·|cos(Ac-Ai)|·εH·Hk

(10)

式中：v是車輛行駛速度；Ac是車輛軌跡的航跡角；Ai是軌跡點在各區(qū)域投影點的方向或反方向；εH是車輛航向角的變化率；Hk是方向權(quán)值系數(shù)(文中Hk取0.4)。在一定范圍內(nèi)，車速越快且車頭朝向軌跡點對區(qū)域投影點的方向越接近，其方向權(quán)值越大。

2.2 地圖匹配過程

為了保證存儲歷史軌跡點的連續(xù)性，同時節(jié)約存儲空間，采用滑窗移動(窗口大小為30)的方式存儲滿足一定車速的(v≥3 m/s)歷史軌跡點，當存儲的歷史軌跡序列中的軌跡點數(shù)目達到30，則判斷該歷史軌跡序列有效。當歷史軌跡序列判斷為無效以及從有效歷史軌跡序列中得到的航向變化率、曲率大小及變化率、坡度不滿足特征條件時，確定以點間距離、磁場強度為加權(quán)的參數(shù)值對非特征路段進行匹配；當從有效歷史軌跡序列中得到的航向變化率、曲率大小及變化率、坡度滿足特征條件時，確定以航向、曲率、坡度、點間距離、磁場強度為加權(quán)的參數(shù)值對特征路段進行匹配。

2.2.1 參數(shù)值的求解

參數(shù)值的求解如圖8所示，這里對軌跡點采用三次B樣條投影算法，其基本思想在于利用Newton下山法來最小化軌跡點特征T(k)(位置、航向變化率、曲率及變化率、磁場強度、坡度)和C(u)的距離，當其距離小于一個事先指定的精度ε，則認為C(u)為投影點，其u即為軌跡點特征的參數(shù)值。

圖8 參數(shù)值求解示意圖

利用數(shù)量積定義函數(shù)：

f(u)=C′(u)·[C(u)-T(k)]

(11)

式中：C′(u)為曲線在參數(shù)值u的一階導(dǎo)矢；C(u)為投影點；T(k)為軌跡點特征。

當f(u)=0時，軌跡點特征T(k)到C(u)的距離達到最小，根據(jù)Newton下山法，可得迭代函數(shù)：

(12)

式中：ui表示第i次Newton下山法迭代所得到的參數(shù)值；λ為牛頓下山法迭代參數(shù)；C″(ui)是曲線在參數(shù)值ui處的二階導(dǎo)矢。

如果f(ui+1)小于一個事先指定的精度(ε=1×10-5)，則ui+1為軌跡點特征的參數(shù)值。

考慮到車輛運行過程中總有一些偶然誤差(例如信號的干擾等)，單獨采用式(12)可能會導(dǎo)致參數(shù)值產(chǎn)生比較大的擾動。為了平滑這種擾動，采用前3個歷史軌跡點特征所得到的參數(shù)值的平均值來修正：

(13)

2.2.2 非特征路段地圖匹配

對于非特征路段，確定以點間距離和磁場強度為加權(quán)的參考值進行匹配。

圖9 非特征路段匹配過程示意圖

然后用該預(yù)測軌跡點確定點間距離以及磁場強度加權(quán)參數(shù)值：

u=WDuD+WMuM

(14)

式中：u為點間距離以及磁場強度加權(quán)參數(shù)值；WD為點間距離匹配權(quán)值；uD為預(yù)測軌跡點的點間距離參數(shù)值；WM為磁場強度匹配權(quán)值；uM為軌跡點磁場強度與區(qū)域磁場序列的磁場強度參數(shù)值。

WD、WM定義為：

(15)

式中：Dmin為預(yù)測軌跡點到匹配區(qū)域的最小距離。這樣就保證了只有在點間距離相對較大的情況，才加大磁場序列匹配的權(quán)值。

最后獲得匹配點C(u)同時對匹配修正量e(k)進行實時矯正，同樣為了平滑這種擾動，采用前3個歷史數(shù)據(jù)的平均值來修正：

(16)

2.2.3 特征路段地圖匹配

由于位于特征路段的軌跡點特征非常明顯，故而進行特征路段的匹配時，不需要獲得該時刻的預(yù)測軌跡點。

對特征路段的軌跡點直接以點間距離、航向、曲率、坡度、磁場為加權(quán)的參數(shù)值進行匹配。

u=WDuD+WMuM+WHuH+WCuC+WSuS

(17)

式中：u為點間距離、航向、曲率、坡度、磁場為加權(quán)的參數(shù)值；WD、WM、WH、WC、WS分別為點間距離匹配權(quán)值、磁場強度匹配權(quán)值、航向匹配權(quán)值、曲率匹配權(quán)值、坡度匹配權(quán)值；uD、uM、uH、uC、uS分別為點間距離參數(shù)值、磁場強度參數(shù)值、航向參數(shù)值、曲率參數(shù)值、坡道參數(shù)值。

WD、WH的選取與非特征路段匹配一致，只是其占比僅為10%。WH、WC、WS定義為：

(18)

式中：ρH為航向變化率；Cr為曲率的大?。沪袰r為曲率的變化率；αS為坡度。對WD、WM、WH、WC、WS進行歸一化處理，得到帶權(quán)的參數(shù)值u，最后獲得特征匹配點CT(u)。

通過特征路段匹配得到的匹配點相對而言比較準確，故而在從特征路段過渡到非特征路段過程中，應(yīng)對地圖匹配修正量進行更新。其更新方式如式(19)所示：

enew(k)=CT(u)-g(k)

(19)

式中：enew(k)是更新后的地圖匹配修正量；CT(u)為特征匹配點；g(k)為軌跡點。

3 實驗結(jié)果及分析

為了驗證文中提出的實時地圖匹配算法的匹配精度，首先將同濟大學嘉定校區(qū)部分道路的電子地圖以及地圖匹配算法燒錄到DSP中，進行實車實驗。其中DSP芯片為F28035，GPS接收機為u-blox NEO-M8N，加速度計為MEMS三軸加速度計，陀螺儀為MEMS三軸磁力計。同時為了計算匹配精度，將在同一位置安裝一高精度RTK接收機。實驗結(jié)果如圖10所示，ublox軌跡均匹配到相應(yīng)的路網(wǎng)，其匹配軌跡和高精度RTK軌跡基本重合，絕大部分匹配點的誤差為±10 cm，可以看出文中的實時地圖匹配算法在確定匹配區(qū)域以及軌跡點的位置匹配時效果理想，能夠真實反映出車輛運行的軌跡，實現(xiàn)了實時高精度定位。

圖10 同濟大學嘉定校區(qū)實車匹配局部放大結(jié)果

參考文獻:

[1]AMINI A,VAGHEFI R M,de la GARZA J M,et al.Improving GPS-based Vehicle Positioning for Intelligent Transportation Systems[C]//Intelligent Vehicles Symposium.Michigan,2014:1023-1029.

[2]TOLEDO-MOREO R,BETAILLE D,PEYRET F,et al.Fusing GNSS,Dead-reckoning,and Enhanced Maps for Road Vehicle Lane-level Navigation[J].IEEE Journal of Selected Topics in Signal Processing,2009,3(5):798-809.

[3]JI C,DU X.GPS RTK Applied in Land Reclamation in Mining Area[C]//International Conference on Remote Sensing,Environment and Transportation Engineering.IEEE,2012:1-4.

[4]LEE I,LI H,HOANG N,et al.Navigation System Development of the Underwater Vehicles Using the GPS/INS Sensor Fusion[C]//International Conference on Control,Automation and System.Gyeonggi-do,2014:610-612.

[5]LU W,ZHANG Y.Research on Algorithm of GPS/DR Integrated

Vehicle Navigation System[C]//International Conference on Electric Information & Control Engineering.Wuhan，2011:6056-6060.

[6]SANTOS M M D,BALLESTER P,ZAFFARI G B,et al.A Topological Descriptor of Acoustic Images for Navigation and Mapping[C]//Latin American Robotics Symposium.Uberlandia,2016:1-6.

[7]YANG Y,YE H,FEI S.Integrated Map-matching Algorithm Based on Fuzzy Logic and Dead Reckoning[C]//International Conference on Control Automation & Systems.Gyeonggi-do，2010:1139-1142.

[8]ZHANG B，TAO B，GAO L，et al.Matching Probability of Visible Image Based on the Side Information of Buildings[C]//IEEI Chinese Guidance,Navigation & Control Conference.Nanjing,2017:878-880.

[9]WINTER M,TAYLOR G.Modular Neural Networks for Map-matched GPS Positioning[C]//International Conference on Web Information Systems Engineering Workshops.Roma，2003:106-111.

[10]李星軍.車輛定位導(dǎo)航系統(tǒng)中地圖匹配算法研究[D].西安：西安電子科技大學軟件工程,2015.

[11]楊建宇,楊崇俊,明冬萍,等.WebGIS系統(tǒng)中矢量數(shù)據(jù)的壓縮與化簡方法綜述[J].計算機工程與應(yīng)用,2004，40(32):36-38.

YANG J Y,YANG C J,MING D P,et al.Review on Vector Data Compression and Simplification of WebGIS[J].Computer Engineering and Applications,2004，40(32):36-38.

[12]劉文強,楊海忠.一種應(yīng)用于電子地圖道路特征點提取的新方法[J].科學技術(shù)與工程,2012,12(30):7911-7914.

LIU W Q,YANG H Z.A New Method of Road Feature Point Extraction Application in Electronic Map[J].Science Technology and Engineering,2012,12(30):7911-7914.