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

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

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

前言

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

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

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

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

1 本文算法

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

圖1 系統流程圖

1.1 雙層高精地圖模型構建

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

圖2 雙層地圖模型示意

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

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

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

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

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

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

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

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

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

基于點云三維特征的度量級定位。點云三維特征提取方法與制圖過程一樣，提取目標主要包含路標、路牌和路桿等，三維特征提取結果如圖3（c）所示。定位過程中，在獲取當前三維點云特征后，利用優化算法來優化求解待定位節點與地圖數據庫中最接近節點之間的旋轉向量與平移矩陣，其誤差模型為

圖3 特征信息提取

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

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

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

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

對全局坐標系下的距離，可根據當前定位提供的在全局坐標系下的坐標來查詢離線構建地圖中的直線方程參數進行計算。在構建地圖時，對其進行了分段處理，每段中有一個坐標信息，定位中只須查詢距離當前位置最近的分段點并獲取其直線方程參數。假設查詢到最近點的直線方程為l：

式中l表示地圖數庫中第個查詢點的直線方程信息，坐標系為全局坐標。

結合上述獲得的橫向距離，便可對車輛所在位置坐標建立約束，其推導過程為

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

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

在上述利用激光點云數據得到全局位置坐標和橫向距離的基礎上，采用卡爾曼濾波器將其進行融合，利用檢測獲取的可靠的橫向約束，結合地圖信息，實現車輛更高精度的定位。

利用2 階馬爾可夫模型對短時間車輛運動進行勻速運動建模，用車輛的歷史狀態對當前狀態進行預測。卡爾曼濾波器的狀態變量X由當前位置及其個歷史時刻位置信息來表示，可以寫成式（10）的形式：

其中位置x為一個二維向量：

為降低計算復雜度，本文中狀態變量只選取當前位置和上一時刻位置，即取值為1。在較短時間內，車輛可以近似認為在勻速運動，根據勻速運動模型最終可以得到

由式（10）～式（13）進行推導，最終可以得到卡爾曼濾波器中的狀態轉移矩陣，并假設系統過程噪聲服從0 均值的高斯分布，通過選取的測試點確定預測過程的定位結果與真實位置之間的誤差，確定過程協方差矩陣：

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

基于稀疏地圖多尺度定位產生的觀測值為

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

結合里程計觀測信息、多尺度定位產生的觀測信息和橫向約束信息，最終的觀測矩陣為

考慮到在實際道路場景中，激光雷達掃描視野中可能包含多條車道線，根據其檢測個數可以建立多個約束，觀測矩陣可進行相應修改。利用獲取的狀態轉移矩陣和觀測矩陣來構建卡爾曼濾波器，利用激光實時檢測車道線提供的厘米級橫向距離來約束車輛軌跡，實現車輛更精確的定位。

2 測試與結果分析

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

圖4 測試平臺和場景路線圖

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

本測試在制圖過程和定位過程中，傳感器的安裝位置無變動，對于使用該地圖的其他車輛須事先對制圖車輛的傳感器的外部參數進行標定，消除由于傳感器安裝位置導致的定位誤差。此外，對于目前的量產車來說，傳感器的安裝位置一般相對固定，為本算法的實施提供便利，故制圖和定位時傳感器的安裝位置差別不大時，所提出的雙層地圖模型均具有較好的適應性。

2.1 校園場景測試結果

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

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

圖5 定位誤差水平比較圖

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

表1 在校園場景中融合地圖測試結果

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

表2 不同算法在校園場景的測試結果

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

圖6 校園場景測試定位誤差概率對比

2.2 城市道路場景測試結果

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

圖7 城市道路場景誤差分布對比

表4 不同算法在城市道路場景測試的對比

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

表3 在城市道路場景中融合地圖測試結果

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

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

3 結論

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