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

陳文杰，李浩，沈勇

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

0 引言

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

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

1 創建適用嵌入式定位系統的電子地圖

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

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

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

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

圖1 嵌入式定位系統電子地圖繪制流程

1.1 道路區域劃分

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

圖2 道路區域劃分

1.2 帶權約束三次B樣條壓縮算法

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

1.2.1 算法實現過程

首先確定采集的道路信息數據所對應的參數值以及節點矢量：

(1)

其次根據曲率來確定各個數據點的約束權值，采用的方式如下：

(2)

式中：KC為各個數據點的約束權值；Cr為數據點的曲率大小；ρCr為數據點曲率變化率。

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

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

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

(3)

分別對A和P求導，并令其等于零，得到：

(4)

聯立方程組可以先求得A,然后通過A求得P:

(5)

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

(6)

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

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

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

1.2.2 壓縮率的選取

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

根據這個壓縮原則，如圖5、圖6所示，采用帶權約束三次B樣條壓縮算法對區域1以不同的壓縮率(0～20%)，分別計算極差和離差。可以看出：當壓縮率為4%時，極差和離差的大小已經滿足了數據壓縮精度的要求，并且其減小的趨勢趨于平穩狀態，再加大壓縮率對于其減小的效果不明顯。

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

圖5 區域1壓縮率α和極差δM的關系

圖6 區域1壓縮率α和離差σP的關系

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

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

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

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

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

圖7 地圖匹配算法流程圖

2.1 匹配區域確定

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

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

候選區域Ri的總權值WRi定義為：

WRi=WDi+WBi+WHi

(7)

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

距離權值定義為：

(8)

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

磁場權值定義為：

(9)

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

方向權值定義為：

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

(10)

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

2.2 地圖匹配過程

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

2.2.1 參數值的求解

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

圖8 參數值求解示意圖

利用數量積定義函數：

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

(11)

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

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

(12)

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

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

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

(13)

2.2.2 非特征路段地圖匹配

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

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

然后用該預測軌跡點確定點間距離以及磁場強度加權參數值：

u=WDuD+WMuM

(14)

式中：u為點間距離以及磁場強度加權參數值；WD為點間距離匹配權值；uD為預測軌跡點的點間距離參數值；WM為磁場強度匹配權值；uM為軌跡點磁場強度與區域磁場序列的磁場強度參數值。

WD、WM定義為：

(15)

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

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

(16)

2.2.3 特征路段地圖匹配

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

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

u=WDuD+WMuM+WHuH+WCuC+WSuS

(17)

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

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

(18)

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

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

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

(19)

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

3 實驗結果及分析

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

圖10 同濟大學嘉定校區實車匹配局部放大結果

參考文獻:

[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].西安：西安電子科技大學軟件工程,2015.

[11]楊建宇,楊崇俊,明冬萍,等.WebGIS系統中矢量數據的壓縮與化簡方法綜述[J].計算機工程與應用,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]劉文強,楊海忠.一種應用于電子地圖道路特征點提取的新方法[J].科學技術與工程,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.