融合激光雷達(dá)與雙層地圖模型的智能車定位*

鄧澤武，胡釗政，周 哲，劉裕林，彭 超

（1. 武漢理工大學(xué)信息工程學(xué)院，武漢 430070；2. 武漢理工大學(xué)智能交通系統(tǒng)研究中心，武漢 430063；3. 武漢理工大學(xué)重慶研究院，重慶 401120）

前言

在智能車推廣過(guò)程中，高精度定位技術(shù)是車輛進(jìn)行路徑規(guī)劃、精確導(dǎo)航和障礙物避撞的關(guān)鍵基礎(chǔ)技術(shù)。目前，車輛定位中最常用的方式是GPS（global positioning system）定位，但GPS 定位精度較低，誤差通常在10 m左右，且存在多路徑和非視距問(wèn)題，難以滿足人們對(duì)智能車定位的要求。為將GPS 的定位精度提升到厘米級(jí)，通常須建立地面差分基站，然而該方案的使用和維護(hù)成本較高，不利于大規(guī)模推廣。此外，研究人員將GPS 與慣性導(dǎo)航技術(shù)相融合雖可獲取連續(xù)的位置信息，但在“城市峽谷”等條件下累積誤差無(wú)法及時(shí)消除，且慣性導(dǎo)航傳感器存在價(jià)格昂貴等缺點(diǎn)，無(wú)法在智能車上大規(guī)模使用。

近年來(lái)，隨著人們對(duì)智能汽車定位研究的不斷加深，基于視覺(jué)和激光的定位方法為智能車定位提供了新的思路。文獻(xiàn)［4］中提出了以相機(jī)為傳感器的ORB-SLAM 視覺(jué)定位方法，通過(guò)提取與匹配圖像特征點(diǎn)來(lái)估計(jì)車輛運(yùn)動(dòng)，但視覺(jué)傳感器對(duì)光照較敏感，在光照條件惡劣的環(huán)境中該定位方法的魯棒性有待提高。激光傳感器具有測(cè)量精度高、對(duì)光照不敏感的特點(diǎn)，受到研究人員的關(guān)注。最初研究人員使用經(jīng)典的迭代最近點(diǎn)（iterative closest point，ICP）算法來(lái)求解車輛的相對(duì)運(yùn)動(dòng)關(guān)系從而實(shí)現(xiàn)定位，但該方法存在魯棒性差和定位精度低等問(wèn)題。文獻(xiàn)［6］中提出了以激光雷達(dá)為核心的LOAM（LiDAR odometry and mapping）定位方案，該方法利用提取的平面、邊緣等特征點(diǎn)進(jìn)行匹配，優(yōu)化點(diǎn)到直線與平面的距離從而估計(jì)車輛的運(yùn)動(dòng)狀態(tài)。在LOAM 框架基礎(chǔ)上，Shan 等又提出了改進(jìn)算法，它更改了特征點(diǎn)的提取形式，添加了后端回環(huán)優(yōu)化，因此，定位精度得到提升。文獻(xiàn)［8］中引入了車輛運(yùn)動(dòng)學(xué)預(yù)測(cè)模塊，在充分利用LiDAR 和IMU（inertial measurement unit）測(cè)量特性的基礎(chǔ)上以提高定位精度。然而，這些基于激光雷達(dá)定位方法具有馬爾科夫性質(zhì)，下一時(shí)刻的位姿由歷史時(shí)刻的位姿推算而來(lái)，隨著時(shí)間的推移，多次位置推算后，由于缺少回環(huán)條件和先驗(yàn)信息的輔助，其定位效果將變差，不能滿足車輛定位需求。為此，研究人員開(kāi)始探索基于先驗(yàn)高精地圖的方法。

高精地圖是智能車定位的關(guān)鍵基礎(chǔ)設(shè)施，在智能車定位中具有重要作用，它可以協(xié)助其他設(shè)備提升車輛定位精度。文獻(xiàn)［10］中在GPS 信號(hào)缺失情況下，利用激光雷達(dá)與高精地圖結(jié)合快速實(shí)現(xiàn)了車輛的重定位，保障了定位的穩(wěn)定性。文獻(xiàn)［11］中利用路側(cè)單元實(shí)現(xiàn)車輛定位，文獻(xiàn)［12］和文獻(xiàn)［13］中提出了基于地圖的定位方法，采用配準(zhǔn)方法將道路標(biāo)記的邊緣與投影地圖對(duì)齊，并將其表述為非線性優(yōu)化問(wèn)題實(shí)現(xiàn)定位。Caselitz 等將圖像特征重建為一組稀疏的三維點(diǎn)，然后與地圖三維點(diǎn)云匹配對(duì)齊來(lái)間接定位車輛。但這些方法受環(huán)境影響較大，算法魯棒性有待提高。文獻(xiàn)［15］和文獻(xiàn)［16］中利用在線數(shù)據(jù)與離線地圖進(jìn)行匹配來(lái)實(shí)現(xiàn)定位，利用地圖來(lái)校正車輛軌跡，并減小定位誤差。但此類方法須保存大量的地圖三維點(diǎn)云信息且匹配耗時(shí)長(zhǎng)，實(shí)時(shí)性較差。為降低地圖存儲(chǔ)空間，保證定位的實(shí)時(shí)性，Li 等提出了基于桿狀目標(biāo)地圖的車輛定位方法，利用提取的目標(biāo)特征進(jìn)行匹配，該方法高度依賴于一種特定類型特征，不利于很好表征道路環(huán)境信息，限制了方法的推廣。文獻(xiàn)［18］中提出了城市環(huán)境下基于多層激光雷達(dá)的車輛定位方法，將特征檢測(cè)方法與蒙特卡羅定位方法相結(jié)合來(lái)估計(jì)車輛位置，但該方法在長(zhǎng)直道路上表現(xiàn)較差。

綜上所述，針對(duì)目前定位方法中存在的問(wèn)題，本文中提出了融合激光雷達(dá)與雙層地圖模型的智能車定位方法，其核心思想是利用車道地圖與稀疏特征地圖雙層結(jié)構(gòu)信息對(duì)車輛軌跡進(jìn)行約束，并通過(guò)構(gòu)建線性卡爾曼濾波融合算法提高定位的穩(wěn)定性和魯棒性。其主要工作為：（1）面向智能車定位，提出了一種雙層結(jié)構(gòu)高精地圖模型，該模型在車道圖層基礎(chǔ)上構(gòu)建了稀疏特征圖層，為智能車定位提供高精度的位置參考，能有效降低里程計(jì)的累積誤差；（2）在雙層地圖模型基礎(chǔ)上，提出融合基于車道線的橫向距離約束和基于稀疏特征的絕對(duì)位置約束，并通過(guò)構(gòu)建短時(shí)間的勻速運(yùn)動(dòng)模型，將車輛的運(yùn)動(dòng)信息與觀測(cè)信息相融合，以提高定位的穩(wěn)定性。

1 本文算法

本文中提出的融合激光雷達(dá)與雙層地圖模型的智能車定位方法，其整體流程如圖1 所示。雙層地圖采用離線方式構(gòu)建，定位階段主要分為點(diǎn)云處理與基于卡爾曼濾波的融合定位兩個(gè)部分。點(diǎn)云處理須提取車道線和特征點(diǎn)云，其中，特征點(diǎn)云用于稀疏特征地圖匹配提供絕對(duì)位姿參考，車道線與車道地圖相結(jié)合提供橫向距離約束。基于卡爾曼濾波的融合定位主要計(jì)算狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣，輸入是車輛的位置信息和檢測(cè)得到的橫向距離，輸出為融合后的車輛位姿。

圖1 系統(tǒng)流程圖

1.1 雙層高精地圖模型構(gòu)建

本文中所建立的地圖元素信息較為簡(jiǎn)單，相對(duì)于目前高精度地圖中須構(gòu)建各元素之間的連接關(guān)系而言，本文只對(duì)車道線和稀疏特征點(diǎn)進(jìn)行組織，形成車道地圖與稀疏特征地圖雙層結(jié)構(gòu)，如圖2 所示。其中車道圖層為智能車定位提供線性的橫向位置約束，稀疏特征圖層為智能車定位提供高精度的位置參考，有效降低累計(jì)定位誤差。車道級(jí)地圖采用廣泛應(yīng)用的OpenDrive的構(gòu)圖方式制作，針對(duì)彎道部分采用折線化方式處理。稀疏特征地圖選取強(qiáng)度特征明顯的路段作為稀疏地圖采集點(diǎn)，采用離線的方式制作。稀疏特征地圖由全局位姿、二維特征和三維點(diǎn)云特征3 部分組成。其中，全局位姿通過(guò)搭載的高精度組合慣性導(dǎo)航系統(tǒng)和LiDAR 里程計(jì)融合獲取，并采用圖優(yōu)化的方式對(duì)車輛位姿進(jìn)行優(yōu)化，其定位精度可達(dá)到厘米級(jí)，全局位姿為后續(xù)的粗定位與度量級(jí)定位提供精確的參考位姿。二維特征則是對(duì)激光雷達(dá)點(diǎn)云強(qiáng)度投影到可以展開(kāi)的圓柱形表面，形成強(qiáng)度二維特征圖，投影方法如下所述。

圖2 雙層地圖模型示意

首先將激光傳感器掃描范圍劃分為水平和豎直兩個(gè)方向，形成一個(gè)二維投影空間，再設(shè)定兩個(gè)方向的投影角度分辨率，最終得到一個(gè)N× N的矩陣，矩陣的數(shù)值為該段區(qū)域點(diǎn)的最大強(qiáng)度值：

式中：()為激光點(diǎn)云中點(diǎn)的強(qiáng)度值；(P)為二維矩陣第行與第列的值，該值大小為歸屬于該空間所有三維點(diǎn)的最大強(qiáng)度值。獲取每個(gè)矩陣元素的投影值，將其組合形成一個(gè)二維矩陣，用于二維特征相似度計(jì)算，完成節(jié)點(diǎn)級(jí)定位：

稀疏特征地圖中的三維特征由一些標(biāo)志物組成，包括路標(biāo)、路牌、桿狀物和彎道的車道線等，這些特征能很好地表征環(huán)境，具有唯一性。三維特征提取方法可參見(jiàn)文獻(xiàn)［17］和文獻(xiàn)［19］，保留了特征點(diǎn)的三維坐標(biāo)信息，將所有特征點(diǎn)進(jìn)行組合構(gòu)成最終的三維特征。在定位過(guò)程中，利用當(dāng)前獲取的三維特征與地圖中的三維特征進(jìn)行配準(zhǔn)得到相對(duì)于地圖中的三維特征的相對(duì)關(guān)系，最后結(jié)合特征地圖的全局位姿實(shí)現(xiàn)定位。由于獲取的車道線信息為平面坐標(biāo)系下的距離信息，為計(jì)算和描述方便，本文中將GPS 全局坐標(biāo)轉(zhuǎn)化為UTM（universal transverse mercator）坐標(biāo)系下，實(shí)現(xiàn)坐標(biāo)的統(tǒng)一，其轉(zhuǎn)換方式見(jiàn)文獻(xiàn)［20］。

1.2 基于雙層地圖模型的定位方法

基于雙層地圖模型的定位包含基于稀疏特征圖層的多尺度定位與基于車道圖層的橫向距離約束兩部分，不同圖層實(shí)現(xiàn)不同定位效果。當(dāng)車輛行駛至特征地圖附近時(shí)，可利用特征地圖包含的二維特征與三維特征來(lái)提供一個(gè)絕對(duì)位姿參考，降低累計(jì)定位誤差。車道圖層可利用檢測(cè)的車道線提供高精度的、線性的橫向位置約束，結(jié)合里程計(jì)信息提升定位精度。

1.2.1 基于稀疏特征圖層的多尺度定位

為保證定位的準(zhǔn)確性，基于稀疏特征圖層的多尺度定位主要分為3 個(gè)部分：粗定位、基于二維特征的節(jié)點(diǎn)定位和基于點(diǎn)云三維特征的度量級(jí)定位。粗定位是指：當(dāng)車輛行駛至路段的某個(gè)位置時(shí)，根據(jù)當(dāng)前定位結(jié)果預(yù)測(cè)下一時(shí)刻位置，將預(yù)測(cè)位置與地圖中的位置信息進(jìn)行匹配，選取距離最近的地圖數(shù)據(jù)節(jié)點(diǎn)作為定位節(jié)點(diǎn)，但考慮到當(dāng)前定位存在一定誤差，因此本文選取距離當(dāng)前定位結(jié)果一定范圍內(nèi)的地圖數(shù)據(jù)節(jié)點(diǎn)作為候選節(jié)點(diǎn)，以降低定位誤差所產(chǎn)生的影響，完成粗定位。

基于二維特征的節(jié)點(diǎn)定位。首先須將點(diǎn)云數(shù)據(jù)進(jìn)行投影轉(zhuǎn)化為二維強(qiáng)度圖特征，為加強(qiáng)其表征的唯一性，消除環(huán)境中車牌等影響，本文中只將一定高度范圍內(nèi)的點(diǎn)云進(jìn)行投影。具體做法是將水平方向的點(diǎn)按1°的間隔進(jìn)行投影；將豎直方向的點(diǎn)按0.5°間隔進(jìn)行投影，最終形成一個(gè)50×80 大小的二維矩陣，利用這個(gè)矩陣來(lái)表征環(huán)境信息，圖3（b）為投影形成的二維特征。在粗定位基礎(chǔ)上，選取候選節(jié)點(diǎn)的二維矩陣信息進(jìn)行匹配來(lái)定位最相似的地圖節(jié)點(diǎn)。其相似度的計(jì)算公式為

式中：M和c表示當(dāng)前投影矩陣和該矩陣的第列；M和c表示待匹配地圖節(jié)點(diǎn)存儲(chǔ)的二維矩陣和該矩陣的第列；為矩陣列數(shù)，本文中= 80；(M，M)為相似度大小，其值越大說(shuō)明兩者越相似。將候選地圖節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)進(jìn)行相似度計(jì)算，選擇其中相似度最大的地圖節(jié)點(diǎn)作為定位節(jié)點(diǎn)，完成節(jié)點(diǎn)定位。

基于點(diǎn)云三維特征的度量級(jí)定位。點(diǎn)云三維特征提取方法與制圖過(guò)程一樣，提取目標(biāo)主要包含路標(biāo)、路牌和路桿等，三維特征提取結(jié)果如圖3（c）所示。定位過(guò)程中，在獲取當(dāng)前三維點(diǎn)云特征后，利用優(yōu)化算法來(lái)優(yōu)化求解待定位節(jié)點(diǎn)與地圖數(shù)據(jù)庫(kù)中最接近節(jié)點(diǎn)之間的旋轉(zhuǎn)向量與平移矩陣，其誤差模型為

圖3 特征信息提取

式中：(，)為誤差；=，，...，x為當(dāng)前數(shù)據(jù)；=，，...，y為地圖數(shù)據(jù)，再結(jié)合地圖中的全局位姿信息完成車輛定位。在Kalman 濾波融合框架中，觀測(cè)信息的誤差水平對(duì)于最后的融合結(jié)果有較大影響。為確定基于稀疏特征地圖的車輛定位誤差，事先選取一段測(cè)試路段，獲取基于稀疏特征的車輛定位誤差。

1.2.2 基于車道圖層的橫向距離約束

對(duì)于橫向距離約束，利用激光雷達(dá)獲取路緣石或建筑等信息來(lái)確定當(dāng)前車輛所處的車道，進(jìn)而精確求解與車道線之間的橫向距離。對(duì)于車道線提取，為消除其他物體的影響，在提取地面的基礎(chǔ)上進(jìn)行。對(duì)每幀點(diǎn)云，根據(jù)激光雷達(dá)放置的高度選擇地面點(diǎn)存在的范圍，濾除非地面點(diǎn)并降低車道點(diǎn)提取的復(fù)雜度。將經(jīng)上述處理的點(diǎn)云再用RANSAC算法進(jìn)行平面擬合得到地面點(diǎn)，利用強(qiáng)度特征把地面點(diǎn)云中的車道線點(diǎn)云分離出來(lái)。為降低噪聲點(diǎn)對(duì)測(cè)試的影響，對(duì)分離出來(lái)的點(diǎn)還須進(jìn)行離群點(diǎn)剔除和歐式聚類，再利用主成分分析算法選擇具有直線特征的類別。經(jīng)過(guò)數(shù)據(jù)預(yù)處理后，對(duì)每個(gè)類別進(jìn)行直線擬合，求解每個(gè)類別的直線方程。由于地圖構(gòu)建得到的方程是將點(diǎn)云投影到平面中，因此定位中同樣須將點(diǎn)云進(jìn)行投影，以求解直線方程。直線方程的形式為

在單幀點(diǎn)云中，由于激光雷達(dá)測(cè)距具有很高的精度，所以可提供一個(gè)很強(qiáng)的約束條件。本文在長(zhǎng)度為200 m的道路上選取100個(gè)測(cè)點(diǎn)進(jìn)行橫向誤差分析，測(cè)試表明橫向距離計(jì)算的平均誤差為0.05 m。

對(duì)全局坐標(biāo)系下的距離，可根據(jù)當(dāng)前定位提供的在全局坐標(biāo)系下的坐標(biāo)來(lái)查詢離線構(gòu)建地圖中的直線方程參數(shù)進(jìn)行計(jì)算。在構(gòu)建地圖時(shí)，對(duì)其進(jìn)行了分段處理，每段中有一個(gè)坐標(biāo)信息，定位中只須查詢距離當(dāng)前位置最近的分段點(diǎn)并獲取其直線方程參數(shù)。假設(shè)查詢到最近點(diǎn)的直線方程為l：

式中l表示地圖數(shù)庫(kù)中第個(gè)查詢點(diǎn)的直線方程信息，坐標(biāo)系為全局坐標(biāo)。

結(jié)合上述獲得的橫向距離，便可對(duì)車輛所在位置坐標(biāo)建立約束，其推導(dǎo)過(guò)程為

式中(x，y)表示當(dāng)前車輛在全局坐標(biāo)系下的位置。

1.3 融合雙層地圖模型的智能車定位

在上述利用激光點(diǎn)云數(shù)據(jù)得到全局位置坐標(biāo)和橫向距離的基礎(chǔ)上，采用卡爾曼濾波器將其進(jìn)行融合，利用檢測(cè)獲取的可靠的橫向約束，結(jié)合地圖信息，實(shí)現(xiàn)車輛更高精度的定位。

利用2 階馬爾可夫模型對(duì)短時(shí)間車輛運(yùn)動(dòng)進(jìn)行勻速運(yùn)動(dòng)建模，用車輛的歷史狀態(tài)對(duì)當(dāng)前狀態(tài)進(jìn)行預(yù)測(cè)。卡爾曼濾波器的狀態(tài)變量X由當(dāng)前位置及其個(gè)歷史時(shí)刻位置信息來(lái)表示，可以寫成式（10）的形式：

其中位置x為一個(gè)二維向量：

為降低計(jì)算復(fù)雜度，本文中狀態(tài)變量只選取當(dāng)前位置和上一時(shí)刻位置，即取值為1。在較短時(shí)間內(nèi)，車輛可以近似認(rèn)為在勻速運(yùn)動(dòng)，根據(jù)勻速運(yùn)動(dòng)模型最終可以得到

由式（10）～式（13）進(jìn)行推導(dǎo)，最終可以得到卡爾曼濾波器中的狀態(tài)轉(zhuǎn)移矩陣，并假設(shè)系統(tǒng)過(guò)程噪聲服從0 均值的高斯分布，通過(guò)選取的測(cè)試點(diǎn)確定預(yù)測(cè)過(guò)程的定位結(jié)果與真實(shí)位置之間的誤差，確定過(guò)程協(xié)方差矩陣：

觀測(cè)信息來(lái)源于多個(gè)方面，一部分來(lái)自激光里程計(jì)與基于稀疏地圖多尺度定位產(chǎn)生的觀測(cè)值，一部分來(lái)自于車道線檢測(cè)提供的橫向距離約束。對(duì)于里程計(jì)部分，其觀測(cè)矩陣可以表示為

基于稀疏地圖多尺度定位產(chǎn)生的觀測(cè)值為

對(duì)于橫向約束提供的觀測(cè)矩陣，可由式（9）進(jìn)行求解。在計(jì)算出橫向距離d和尋找到距離當(dāng)前位置最近的車道線方程后，利用式（9）將激光相對(duì)距離信息轉(zhuǎn)換為對(duì)車輛位置坐標(biāo)的約束，將其進(jìn)行轉(zhuǎn)化后得到觀測(cè)矩陣。

結(jié)合里程計(jì)觀測(cè)信息、多尺度定位產(chǎn)生的觀測(cè)信息和橫向約束信息，最終的觀測(cè)矩陣為

考慮到在實(shí)際道路場(chǎng)景中，激光雷達(dá)掃描視野中可能包含多條車道線，根據(jù)其檢測(cè)個(gè)數(shù)可以建立多個(gè)約束，觀測(cè)矩陣可進(jìn)行相應(yīng)修改。利用獲取的狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣來(lái)構(gòu)建卡爾曼濾波器，利用激光實(shí)時(shí)檢測(cè)車道線提供的厘米級(jí)橫向距離來(lái)約束車輛軌跡，實(shí)現(xiàn)車輛更精確的定位。

2 測(cè)試與結(jié)果分析

本文的測(cè)試平臺(tái)為搭載激光雷達(dá)和組合慣導(dǎo)的智能車，如圖4（a）所示。其中，北斗星通生產(chǎn)的Npos220 型號(hào)組合慣導(dǎo)，結(jié)合載波相位差分技術(shù)，可提供高精度經(jīng)度、緯度和水平姿態(tài)等信息，其定位精度可達(dá)厘米級(jí)，因此本文用高精度組合慣導(dǎo)數(shù)據(jù)作為真值軌跡用于定位精度比較。采用大疆公司的Horizon 固態(tài)激光雷達(dá)，與傳統(tǒng)機(jī)械式激光雷達(dá)相比，它采用獨(dú)特的非重復(fù)掃描方式，每秒多達(dá)24 萬(wàn)個(gè)高精度點(diǎn)云，探測(cè)距離達(dá)到260 m，角度精度為0.05°，可以為車輛定位提供很好的環(huán)境檢測(cè)。

圖4 測(cè)試平臺(tái)和場(chǎng)景路線圖

測(cè)試選取校園和城市道路兩種不同場(chǎng)景。校園場(chǎng)景的路線圖如圖4（b）所示，該路線的總長(zhǎng)度約為1.1 km，在4 個(gè)特征明顯的路段設(shè)置了稀疏地圖節(jié)點(diǎn)，車道線均可穩(wěn)定檢測(cè)。城市道路場(chǎng)景的路線圖如圖4（c）所示，總長(zhǎng)度約為3.4 km，在16 個(gè)特征明顯的路段設(shè)置稀疏地圖節(jié)點(diǎn)。測(cè)試在相同的計(jì)算機(jī)硬件條件下進(jìn)行，其配置為Intel Core i7-10875H 2.8 GHz CPU，8 GB 內(nèi)存。對(duì)于地圖模型中車道圖層，本文中采用現(xiàn)有的OpenDrive 方式進(jìn)行，可參見(jiàn)文獻(xiàn)［21］。稀疏特征圖層通過(guò)搭載的高精度組合慣性導(dǎo)航系統(tǒng)與LiDAR 里程計(jì)融合獲得，并采用圖優(yōu)化的方式對(duì)車輛位姿進(jìn)行優(yōu)化，同時(shí)對(duì)激光雷達(dá)點(diǎn)云進(jìn)行特征提取，保存提取得到的特征點(diǎn)，并記錄此時(shí)刻車輛在全局坐標(biāo)系下的位姿形成稀疏特征圖層數(shù)據(jù)。

本測(cè)試在制圖過(guò)程和定位過(guò)程中，傳感器的安裝位置無(wú)變動(dòng)，對(duì)于使用該地圖的其他車輛須事先對(duì)制圖車輛的傳感器的外部參數(shù)進(jìn)行標(biāo)定，消除由于傳感器安裝位置導(dǎo)致的定位誤差。此外，對(duì)于目前的量產(chǎn)車來(lái)說(shuō)，傳感器的安裝位置一般相對(duì)固定，為本算法的實(shí)施提供便利，故制圖和定位時(shí)傳感器的安裝位置差別不大時(shí)，所提出的雙層地圖模型均具有較好的適應(yīng)性。

2.1 校園場(chǎng)景測(cè)試結(jié)果

首先利用高精度組合慣導(dǎo)和激光雷達(dá)來(lái)采集數(shù)據(jù)，采用離線方式構(gòu)建地圖信息。激光雷達(dá)和組合慣導(dǎo)數(shù)據(jù)采集頻率分別為10 和30 Hz，車輛最大行駛速度為15 km/h，為保證定位時(shí)能查詢到最近的地圖節(jié)點(diǎn)，每間隔5 m 設(shè)置一個(gè)查詢點(diǎn)，查詢點(diǎn)包含了該點(diǎn)的位置信息和該小段的直線方程參數(shù)。

在車輛定位測(cè)試過(guò)程中，首先利用GPS 數(shù)據(jù)進(jìn)行位置初始化，確定車輛的一個(gè)起始狀態(tài)，然后利用激光雷達(dá)獲得一個(gè)里程計(jì)信息，并將該里程計(jì)信息投影至全局坐標(biāo)系下，同時(shí)就每時(shí)刻采集的點(diǎn)云數(shù)據(jù)，對(duì)其進(jìn)行車道線檢測(cè)和特征點(diǎn)提取，如果行駛至稀疏特征地圖節(jié)點(diǎn)一定范圍內(nèi)，則從地圖中選擇最接近當(dāng)前位置的地圖節(jié)點(diǎn)，然后用當(dāng)前點(diǎn)云與地圖特征點(diǎn)云進(jìn)行匹配，得到一個(gè)絕對(duì)位姿，該絕對(duì)位姿能有效減小累計(jì)誤差。對(duì)于檢測(cè)到的車道線，求解出局部坐標(biāo)系下的車道線方程，然后計(jì)算當(dāng)前時(shí)刻距離車道線的距離，根據(jù)當(dāng)前狀態(tài)來(lái)查詢距離當(dāng)前位置最近的車道地圖，利用地圖信息和當(dāng)前橫向距離來(lái)約束車輛軌跡，最后通過(guò)卡爾曼濾波實(shí)現(xiàn)融合定位。校園場(chǎng)景共進(jìn)行了3 次測(cè)試，每次測(cè)試，車輛從不同起點(diǎn)出發(fā)測(cè)試算法性能。圖5（a）是測(cè)試部分路段的定位誤差比較圖，圖5（b）為某次測(cè)試不同算法的定位軌跡比較圖。從圖5 可見(jiàn)，本文算法的定位誤差比文獻(xiàn)［6］中LOAM 算法的定位誤差小，驗(yàn)證了本文算法的有效性。

圖5 定位誤差水平比較圖

表1 為里程計(jì)定位與融合高精度地圖定位情況的誤差對(duì)比。展示地圖信息對(duì)定位的輔助作用，由表可見(jiàn)，融合地圖的定位算法比單一里程計(jì)的定位精度都高。在校園場(chǎng)景測(cè)試中，融合單一車道地圖與融合單一稀疏地圖的平均誤差為0.82 和0.68 m，相比于單一里程計(jì)的1.65 m，定位誤差降低了50.3%和58.8%，而同時(shí)融合兩者的平均誤差為0.31 m，比里程計(jì)定位誤差降低了81.2%。

表1 在校園場(chǎng)景中融合地圖測(cè)試結(jié)果

為展示本文算法的有效性，在兩個(gè)測(cè)試場(chǎng)景中將其與文獻(xiàn)［15］和文獻(xiàn)［22］的算法進(jìn)行對(duì)比，本文算法是同時(shí)融合車道圖層和稀疏圖層算法。文獻(xiàn)［15］組合里程計(jì)和位置識(shí)別來(lái)完成基于先驗(yàn)地圖的定位，利用先驗(yàn)離線點(diǎn)云地圖估計(jì)與當(dāng)前激光幀的相對(duì)位姿，最后通過(guò)全局位姿圖優(yōu)化修正里程計(jì)的漂移；文獻(xiàn)［22］中采用緊密耦合的迭代擴(kuò)展卡爾曼濾波器將激光雷達(dá)特征點(diǎn)與IMU 數(shù)據(jù)融合，使用前向和后向傳播來(lái)預(yù)測(cè)激光雷達(dá)掃描中的運(yùn)動(dòng)狀態(tài)和補(bǔ)償，并使用了新的卡爾曼增益公式加速計(jì)算。表2 為校園場(chǎng)景下不同算法的對(duì)比結(jié)果。從表2 可以看到，3 種算法的平均誤差分別為0.31、0.62 和0.85 m，最大誤差為1.25、1.81 和2.75 m。按平均誤差計(jì)，本文算法比文獻(xiàn)［15］和文獻(xiàn)［22］，其定位誤差分別降低了50%和63.5%。且本文算法平均耗時(shí)為33.1 ms，比兩個(gè)文獻(xiàn)分別縮短56%和17.3%，定位的實(shí)時(shí)性更強(qiáng)。

表2 不同算法在校園場(chǎng)景的測(cè)試結(jié)果

3 種算法的定位誤差概率分布如圖6 所示。由圖可見(jiàn)，本文算法誤差在0.5 m 內(nèi)的數(shù)據(jù)占75.4%，在1 m 內(nèi)的定位數(shù)據(jù)占96.4%，與另兩種算法相比，極大地提高了車輛的定位精度，保障了智能車的行駛。

圖6 校園場(chǎng)景測(cè)試定位誤差概率對(duì)比

2.2 城市道路場(chǎng)景測(cè)試結(jié)果

城市道路場(chǎng)景下車輛最大行駛速度為40 km/h，相比于校園場(chǎng)景，車輛的行駛速度更高，數(shù)據(jù)的采集頻率也相應(yīng)提高，激光雷達(dá)和組合慣導(dǎo)數(shù)據(jù)采集頻率分別設(shè)為15 和80 Hz。城市道路場(chǎng)景也共進(jìn)行了3 次測(cè)試，每次的起點(diǎn)不同。為保證定位時(shí)能夠查詢到最近的地圖節(jié)點(diǎn)，每間隔10 m 設(shè)置一個(gè)查詢點(diǎn)，查詢點(diǎn)包含了該點(diǎn)的位置信息和該小段的直線方程參數(shù)。在該場(chǎng)景中，激光雷達(dá)掃描視野中檢測(cè)到多條車道線，可以建立多個(gè)約束關(guān)系。和校園場(chǎng)景一樣，先進(jìn)行高精度地圖構(gòu)建，保留其中稀疏特征地圖和車道地圖，在定位過(guò)程中再進(jìn)行地圖加載。其定位效果如表3、表4和圖7所示。

圖7 城市道路場(chǎng)景誤差分布對(duì)比

表4 不同算法在城市道路場(chǎng)景測(cè)試的對(duì)比

表3 示出在城市道路場(chǎng)景中融合高精度地圖的定位效果。由表3 可見(jiàn)，以融合單一車道地圖和單一稀疏地圖的平均誤差為0.92 和0.76 m，相比于里程計(jì)定位，融合地圖方法定位精度都得到了不同程度的提高，特別是融合車道地圖與稀疏地圖的方法，整個(gè)場(chǎng)景定位平均誤差可以控制在0.47 m，表明地圖起到了很好的輔助作用。

表3 在城市道路場(chǎng)景中融合地圖測(cè)試結(jié)果

從表4和圖7中可以看到，本文算法的平均誤差為0.47 m，最大誤差為1.53 m，誤差范圍在0.5 m 內(nèi)的數(shù)據(jù)占57.2%，在1 m 內(nèi)的定位數(shù)據(jù)占90.2%，而文獻(xiàn)［15］和文獻(xiàn)［22］算法的平均誤差為0.79 和1.03 m，誤差范圍在0.5 m 內(nèi)的數(shù)據(jù)占44.1%和32.3%，1 m 內(nèi)的定位數(shù)據(jù)占66.5%和52.6%。按平均誤差計(jì)，本文算法比兩個(gè)文獻(xiàn)算法其定位誤差分別降低了40.5%和54.4%。與校園場(chǎng)景相比，城市開(kāi)放道路場(chǎng)景環(huán)境較為復(fù)雜，定位精度也有所降低，但總體保持在一個(gè)較高水平，平均誤差都在0.5 m以內(nèi)，相對(duì)誤差在0.3%以內(nèi)，滿足車輛定位需求。

綜上兩種不同環(huán)境測(cè)試結(jié)果可以看出，本文提出的融合激光雷達(dá)和雙層地圖模型的智能車定位方法精度高，實(shí)時(shí)性好。

3 結(jié)論

針對(duì)智能車定位精度低和地圖數(shù)據(jù)復(fù)雜等問(wèn)題，本文中提出了融合激光雷達(dá)和雙層地圖模型的智能車定位方法，該方法利用不同地圖圖層實(shí)現(xiàn)不同約束，在定位過(guò)程中利用檢測(cè)到的車道線并結(jié)合車道地圖信息來(lái)提供一個(gè)穩(wěn)定和可靠的距離約束，利用稀疏特征圖層為智能車定位提供高精度的位置參考，以提升定位精度。測(cè)試結(jié)果表明，在保證車輛定位實(shí)時(shí)性的需求下，該方法可以提供一個(gè)很好的定位效果，與現(xiàn)有算法相比，定位誤差降低了40%～60%，擁有更好的定位精度。此外，本文的研究工作尚存在一些不足之處，在某些環(huán)境中，激光強(qiáng)度信息可能易受干擾，未來(lái)將增加點(diǎn)云強(qiáng)度校正，利用激光雷達(dá)回波寬度等信息提高對(duì)車道線的檢測(cè)精度。