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

龔柯陽(yáng)，侯寶龍，史志飛，王孝宇

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

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

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

1 整體框架

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

圖1 整體框架流程圖

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

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

2 前端配準(zhǔn)

2.1 點(diǎn)云分割

P={,,...,P}是在時(shí)刻接收到的雷達(dá)點(diǎn)云，其P={x,y,z}。將P映射到距離圖像中，該距離圖像的行是360度除以雷達(dá)的水平角分辨率、列是雷達(dá)的線束。每個(gè)P在距離圖像中對(duì)應(yīng)一個(gè)像素，每個(gè)像素的值r為該點(diǎn)距離雷達(dá)發(fā)射中心的距離。通過(guò)距離圖像的同列相鄰行的坐標(biāo)值的比較可以得出當(dāng)前點(diǎn)的地面屬性，當(dāng)小于某一閾值時(shí)將其標(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ù)量小于一定閾值時(shí)將其標(biāo)記為異常點(diǎn)云。實(shí)際應(yīng)用中，異常點(diǎn)云通常是較小的物體、飄散的樹(shù)葉等，通過(guò)剔除這些不穩(wěn)定的特征有助于提高前端里程計(jì)的精度。

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

2.2 特征提取

特征提取部分與基于雷達(dá)的里程計(jì)與建圖（Lidar Odometry and Mapping in Real-time, LOAM）中所用的方法類似。由于點(diǎn)云數(shù)量過(guò)多，為了在保證匹配精度的基礎(chǔ)上減少計(jì)算，分別從已得到的地面點(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)云的距離圖像均勻劃分為多個(gè)子圖像，分別在子圖像中提取平面點(diǎn)和角點(diǎn)。分別設(shè)定平面點(diǎn)閾值和角點(diǎn)閾值。每個(gè)子圖像中，選取曲率小于的pS個(gè)地面點(diǎn)為主平面點(diǎn)，曲率大于的個(gè)地面點(diǎn)為次平面點(diǎn)，選取曲率大于的個(gè)非地面點(diǎn)為主角點(diǎn)，曲率小于的個(gè)非地面點(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)云的鳥(niǎo)瞰圖

2.3 雷達(dá)里程計(jì)

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

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

3 回環(huán)檢測(cè)

3.1 SC描述子

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

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

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

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

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

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

當(dāng)?shù)玫揭粚?duì)掃描上下文SC描述子時(shí)，我們需要一個(gè)尺度去衡量?jī)蓚€(gè)位置的相似度。這里令當(dāng)前幀點(diǎn)云的描述子為，候選幀點(diǎn)云的描述子為。將兩個(gè)描述子按列進(jìn)行比較并計(jì)算其距離尺度，其計(jì)算公式為

3.3 旋轉(zhuǎn)不變性

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

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

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

圖7 候選幀的距離圖像

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

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

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

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

5 實(shí)驗(yàn)及驗(yàn)證

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

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

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

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

6 結(jié)論

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

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