基于激光雷達的輕量化定位與建圖方法

龔柯陽，侯寶龍，史志飛，王孝宇

（長安大學 汽車學院，陜西 西安 710064）

隨著自動駕駛技術的發展，作為其關鍵技術之一的定位與建圖技術越發重要。現有的定位方案主要為全球定位系統（Global Positioning Ststem, GPS）+慣性測量單元（Inertial Measurement Unit, IMU）的方案，然而在高樓林立、隧道等環境中GPS存在信號缺失問題，在這些場景中上述的定位方案無法達到良好的效果。激光雷達是穩定性強的且能感知周圍環境的傳感器，在GPS信號不好的環境中，通過激光雷達的輔助定位可以提高定位系統的魯棒性，且通過其感知的周圍環境可以構建全局地圖。

在現有的高等級自動駕駛解決方案中，高精度地圖是必不可少的，而激光雷達正是目前自動駕駛汽車構建高精度地圖必不可少的傳感器之一。激光雷達在進行定位與建圖時，隨著時間的推移，其定位誤差會逐漸累積，通過回環檢測的方法可以將軌跡拉回真實情況，提高定位精度從而提高建圖精度。與此同時，自動駕駛模塊眾多，在有限算力的情況下，定位與建圖模塊的運算輕量化尤為重要。

1 整體框架

本文提出的基于激光雷達的輕量化定位與建圖方法主要由前端配準、回環檢測、后端優化、建圖等四個部分組成。其流程如圖1所示。

圖1 整體框架流程圖

其主要實現流程簡述如下：雷達通過掃描接收到點云數據后通過前端配準經過點云分割、特征提取得到粗略的里程計位姿，同時將點云發送給回環檢測模塊，如檢測到回環則將雷達里程計和回環約束同時加入到后端進行優化，得到較為精確的位姿。最后通過無人車的位姿以及雷達的點云構建全局的點云地圖。Velodyne64線激光雷達如圖2所示。

圖2 Velodyne64線激光雷達

2 前端配準

2.1 點云分割

P={,,...,P}是在時刻接收到的雷達點云，其P={x,y,z}。將P映射到距離圖像中，該距離圖像的行是360度除以雷達的水平角分辨率、列是雷達的線束。每個P在距離圖像中對應一個像素，每個像素的值r為該點距離雷達發射中心的距離。通過距離圖像的同列相鄰行的坐標值的比較可以得出當前點的地面屬性，當小于某一閾值時將其標記為地面點。

式中，為當前點的行標；為當前點的列標。

根據距離圖像的距離信息，將非地面點進行聚類處理，當屬于同一類的點的數量小于一定閾值時將其標記為異常點云。實際應用中，異常點云通常是較小的物體、飄散的樹葉等，通過剔除這些不穩定的特征有助于提高前端里程計的精度。

經過上述過程的處理，原始的激光雷達點云被劃分為幾個部分：（1）地面點云；（2）較大聚類數量的非地面點云；（3）帶有行列索引的距離圖像。

2.2 特征提取

特征提取部分與基于雷達的里程計與建圖（Lidar Odometry and Mapping in Real-time, LOAM）中所用的方法類似。由于點云數量過多，為了在保證匹配精度的基礎上減少計算，分別從已得到的地面點和非地面點中提取出更具特征性的平面點和角點。根據當前點和其周圍點的距離信息，可以得到當前點的曲率信息，根據曲率信息進行平面點和角點的提取。

式中，是當前點附近的點云的總數；為當前點的索引；為附近點的索引。

為了避免提取的特征點聚集影響匹配精度，將點云的距離圖像均勻劃分為多個子圖像，分別在子圖像中提取平面點和角點。分別設定平面點閾值和角點閾值。每個子圖像中，選取曲率小于的pS個地面點為主平面點，曲率大于的個地面點為次平面點，選取曲率大于的個非地面點為主角點，曲率小于的個非地面點為次角點。主平面點和次平面點合稱平面點，其集合為，主角點和次角點合稱角點，其集合為，主平面點的集合為，主s角點的集合為。特征提取前后點云的情況如圖3所示，其中為未處理的點云，為提取的平面點，為提取的角點。

圖3 特征提取前后點云的鳥瞰圖

2.3 雷達里程計

為求解該位姿估計問題，我們采用兩步L-M優化算法，流程圖如圖4所示。待優化的位姿為，其中優化的過程為（1）由于其平面特征的匹配對于高程、橫滾角、俯仰角具有較好的約束，因此第一步先通過平面特征的匹配優化得到 [t,,]。（2）將第一步優化得到的[t,,]作為第二步優化的約束，通過對角點特征的匹配進一步優化得到[t,t,]。

圖4 兩步L-M優化流程圖

3 回環檢測

3.1 SC描述子

回環檢測部分，我們采用掃描上下文SC描述子來進行戶外場景的回環識別。根據方位角和半徑，我們將雷達一幀點云劃分為多個子塊，并將其展開為行列的形式，分別記行數為，列數為。取每個子塊的屬性定義如下

式中，()·為返回點云的軸坐標。

因此，一幀點云的SC描述子可以用一個×的矩陣表示為

點云子塊示意如圖5所示。

圖5 點云子塊示意圖

3.2 相似度分數

當得到一對掃描上下文SC描述子時，我們需要一個尺度去衡量兩個位置的相似度。這里令當前幀點云的描述子為，候選幀點云的描述子為。將兩個描述子按列進行比較并計算其距離尺度，其計算公式為

3.3 旋轉不變性

當智能汽車在行駛的過程中回到同一地點時，其朝向與之前到達時未必一致，通過上述得到的描述子的距離來判斷回環則無法檢測到上述存在轉向的情況。

圖6、圖7以距離圖像的差別來表示每幀點云的旋轉差異，這兩幅圖表示汽車處于同一位置只是轉向發生了變化。考慮到這一問題，需要對描述子按列進行移動，從而找到最小的距離尺度，其公式為

圖6 當前幀的距離圖像

圖7 候選幀的距離圖像

式中，用于與距離閾值進行比較從而判斷回環是否存在。

4 后端優化與建圖

后端優化采用圖優化的方法，將雷達里程計和回環檢測聯合優化得到全局的位姿。

圖8 局部地圖與全局地圖示意圖

5 實驗及驗證

本文采用自動駕駛KITTI數據集00序列進行驗證，其采用Velodyne 64線激光雷達進行數據采集。分別用帶回環檢測和不帶回環檢測的定位與建圖方法跑數據集，得到的點云地圖如圖9、圖10所示。

圖9 帶回環檢測的點云地圖

圖10 不帶回環檢測的點云地圖

從上圖可以得出，本文所提出的基于激光雷達的定位與建圖方法在較大的場景下具有良好的建圖效果，且加入回環檢測能顯著提高定位與建圖的精度。

6 結論

基于激光雷達的輕量化定位與建圖方法克服了采集到的點云數量過多而占用過多的計算資源的問題，能夠進行輕量化定位與建圖，且保證其定位與建圖精度。與此同時，利用SC描述子構建的回環檢測的引入極大地提高了整個方法的性能。通過該方法建立的點云地圖可作為自動駕駛高精度地圖的底層模板，從而加快高精度地圖的開發應用。

僅通過激光雷達進行定位不可避免地會引起高程誤差從而導致垂直于地面方向的地圖存在偏移，本文提出的方法并沒有很好地解決這一方面的問題。考慮多傳感器融合是未來研究的方向。