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

（長安大學(xué) 汽車學(xué)院，陜西 西安 710064）

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

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

1 整體框架

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

圖1 整體框架流程圖

其主要實現(xiàn)流程簡述如下：雷達(dá)通過掃描接收到點(diǎn)云數(shù)據(jù)后通過前端配準(zhǔn)經(jīng)過點(diǎn)云分割、特征提取得到粗略的里程計位姿，同時將點(diǎn)云發(fā)送給回環(huán)檢測模塊，如檢測到回環(huán)則將雷達(dá)里程計和回環(huán)約束同時加入到后端進(jìn)行優(yōu)化，得到較為精確的位姿。最后通過無人車的位姿以及雷達(dá)的點(diǎn)云構(gòu)建全局的點(diǎn)云地圖。Velodyne64線激光雷達(dá)如圖2所示。

圖2 Velodyne64線激光雷達(dá)

2 前端配準(zhǔn)

2.1 點(diǎn)云分割

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

式中，為當(dāng)前點(diǎn)的行標(biāo)；為當(dāng)前點(diǎn)的列標(biāo)。

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

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

2.2 特征提取

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

式中，是當(dāng)前點(diǎn)附近的點(diǎn)云的總數(shù)；為當(dāng)前點(diǎn)的索引；為附近點(diǎn)的索引。

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

圖3 特征提取前后點(diǎn)云的鳥瞰圖

2.3 雷達(dá)里程計

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

圖4 兩步L-M優(yōu)化流程圖

3 回環(huán)檢測

3.1 SC描述子

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

式中，()·為返回點(diǎn)云的軸坐標(biāo)。

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

點(diǎn)云子塊示意如圖5所示。

圖5 點(diǎn)云子塊示意圖

3.2 相似度分?jǐn)?shù)

當(dāng)?shù)玫揭粚呙枭舷挛腟C描述子時，我們需要一個尺度去衡量兩個位置的相似度。這里令當(dāng)前幀點(diǎn)云的描述子為，候選幀點(diǎn)云的描述子為。將兩個描述子按列進(jìn)行比較并計算其距離尺度，其計算公式為

3.3 旋轉(zhuǎn)不變性

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

圖6、圖7以距離圖像的差別來表示每幀點(diǎn)云的旋轉(zhuǎn)差異，這兩幅圖表示汽車處于同一位置只是轉(zhuǎn)向發(fā)生了變化?？紤]到這一問題，需要對描述子按列進(jìn)行移動，從而找到最小的距離尺度，其公式為

圖6 當(dāng)前幀的距離圖像

圖7 候選幀的距離圖像

式中，用于與距離閾值進(jìn)行比較從而判斷回環(huán)是否存在。

4 后端優(yōu)化與建圖

后端優(yōu)化采用圖優(yōu)化的方法，將雷達(dá)里程計和回環(huán)檢測聯(lián)合優(yōu)化得到全局的位姿。

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

5 實驗及驗證

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

圖9 帶回環(huán)檢測的點(diǎn)云地圖

圖10 不帶回環(huán)檢測的點(diǎn)云地圖

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

6 結(jié)論

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

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