一種混合視覺(jué)的車(chē)載激光雷達(dá)即時(shí)定位與地圖構(gòu)建方法＊

張劍鋒 彭育輝 鄭瑋鴻 周增城 林晨浩

（福州大學(xué)，福州 350118）

主題詞：激光雷達(dá) 混合視覺(jué) 即時(shí)定位與地圖構(gòu)建 點(diǎn)云 匹配

1 前言

即時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）是指在沒(méi)有環(huán)境先驗(yàn)信息的前提下，搭載傳感器的主體在運(yùn)動(dòng)中建立環(huán)境地圖模型，同時(shí)估計(jì)自身在環(huán)境地圖中的位置和運(yùn)動(dòng)[1]。隨著無(wú)人駕駛技術(shù)的研究和應(yīng)用日益受到政府和行業(yè)關(guān)注，SLAM算法逐漸成為國(guó)內(nèi)外研究的熱點(diǎn)。根據(jù)傳感器類(lèi)型不同，SLAM 算法可分為視覺(jué)SLAM（Visual SLAM）和基于雷達(dá)的SLAM（LiDAR SLAM）；根據(jù)數(shù)理模型的不同，SLAM算法可分為基于估計(jì)理論的SLAM算法和基于圖優(yōu)化的SLAM算法。

視覺(jué)SLAM 通過(guò)特征點(diǎn)法或直接法實(shí)現(xiàn)幀間圖像配準(zhǔn)完成位姿估計(jì)，ORB（Oriented FAST and Rotated BRIEF）-SLAM 方法為特征點(diǎn)法，能夠合理兼顧效率和精度問(wèn)題[2]。LSD（Large Scale Direct monocular）-SLAM方法則無(wú)需計(jì)算特征點(diǎn)來(lái)構(gòu)建半稠密地圖[3-4]；SVO（Semi-direct Visual Odometry）將特征點(diǎn)法和直接法融合使用，去除了描述子的計(jì)算，避免了半稠密法和稠密法需要處理大量信息的劣勢(shì)[5]。但是，由于相機(jī)傳感器光學(xué)特性的限制，視覺(jué)SLAM只能在光照充足且穩(wěn)定的環(huán)境下工作。激光雷達(dá)因其不受外界光照影響，成為無(wú)人駕駛感知方案的主流。早期對(duì)3D激光SLAM的研究大都停留在對(duì)靜態(tài)物體的三維重建技術(shù)上，常用迭代臨近（Iterative Closest Point，ICP）法[6]。LOAM（LiDAR Odemetry And Mapping）方法是2014 年開(kāi)源的3D 激光SLAM方法，以點(diǎn)云中的平面點(diǎn)和邊緣點(diǎn)為特征點(diǎn)進(jìn)行幀間匹配，并通過(guò)幀與地圖匹配矯正位姿[7]。LVIO（Laser Visual Inertial Odometry）方法是一種多傳感器融合的自身運(yùn)動(dòng)估計(jì)和建圖數(shù)據(jù)處理方案，順序多層地通過(guò)激光雷達(dá)、相機(jī)和慣性測(cè)量單元（Inertial Measurement Unit，IMU）獲得自身運(yùn)動(dòng)估計(jì)[8]。LeGO（Lightweight and Ground-Optimized）-LOAM方法是改進(jìn)的LOAM 方法，利用地面約束減小計(jì)算量，加入了回環(huán)檢測(cè)模塊，減少長(zhǎng)時(shí)間工作導(dǎo)致的累計(jì)誤差[9]。谷歌公司的Cartographer 利用幀間匹配，在最佳位置估計(jì)處插入子圖，采取分枝定界和預(yù)先計(jì)算，在所有子圖都插入后進(jìn)行全局回環(huán)檢測(cè)[10]。

針對(duì)搭載非均勻垂直角分辨率激光雷達(dá)的無(wú)人車(chē)的多傳感器融合即時(shí)定位與地圖構(gòu)建問(wèn)題，本文提出一種融合圖像與點(diǎn)云密度的激光雷達(dá)SLAM方法，在特征點(diǎn)匹配中加入顏色信息，在優(yōu)化位姿中加入密度權(quán)重，并以KITTI數(shù)據(jù)集和實(shí)車(chē)試驗(yàn)驗(yàn)證本文方法的有效性。

2 相機(jī)和激光雷達(dá)的混合標(biāo)定

在三維世界中，相機(jī)能夠?qū)⑷S坐標(biāo)點(diǎn)通過(guò)投影映射在二維圖像平面內(nèi)，該過(guò)程可以用針孔相機(jī)模型來(lái)簡(jiǎn)單描述。三維激光雷達(dá)能以三維坐標(biāo)的形式直接獲取周?chē)h(huán)境信息。通過(guò)二者間的坐標(biāo)變換可以賦予三維點(diǎn)云以顏色信息，二者標(biāo)定原理如圖1所示。相機(jī)坐標(biāo)系為OcXcYcZc，激光雷達(dá)坐標(biāo)系為OlXlYlZl。

圖1 相機(jī)與雷達(dá)標(biāo)定原理

由相機(jī)內(nèi)參數(shù)標(biāo)定獲得內(nèi)參矩陣，通過(guò)相機(jī)與激光雷達(dá)的外參數(shù)標(biāo)定獲得相機(jī)的外參矩陣，得到三維激光雷達(dá)坐標(biāo)系到像素坐標(biāo)系的變換關(guān)系：

式中，P為目標(biāo)點(diǎn)在激光雷達(dá)坐標(biāo)系下的齊次坐標(biāo)；K為內(nèi)參矩陣；T為外參矩陣；u、v為目標(biāo)點(diǎn)在像素坐標(biāo)系中的坐標(biāo)。

通過(guò)雷達(dá)坐標(biāo)系到像素坐標(biāo)系的變換關(guān)系，可以為相機(jī)視域內(nèi)的三維點(diǎn)云賦予顏色信息，如圖2、圖3所示。

圖2 具有顏色信息的點(diǎn)云

圖3 相機(jī)圖片

采用HSV 顏色模型對(duì)點(diǎn)云進(jìn)行顏色分割，通過(guò)色調(diào)（Hue）、飽和度（Saturation）、明度（Value）來(lái)表現(xiàn)物體顏色，其中明度在不同光照條件下變化最大，而色調(diào)基本不受光照強(qiáng)度影響，即通過(guò)色調(diào)判斷物體的顏色。

3 幀間預(yù)匹配算法

3.1 算法基本思路

首先，根據(jù)激光雷達(dá)技術(shù)參數(shù)對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行結(jié)構(gòu)化排序，并剔除匹配數(shù)據(jù)中的不穩(wěn)定點(diǎn)。然后，選取相鄰幀點(diǎn)云之間的特征點(diǎn)，利用先前獲取的激光雷達(dá)與相機(jī)之間的變換矩陣賦予顏色信息，并根據(jù)特征點(diǎn)的性質(zhì)制定匹配策略。最后，對(duì)匹配點(diǎn)采取未解耦的位姿求解算法，獲取兩幀間的變換矩陣作為L(zhǎng)eGO-LOAM方法中幀間匹配模塊的匹配初值，流程如圖4所示。

圖4 算法基本流程

3.2 點(diǎn)云預(yù)處理

激光雷達(dá)的技術(shù)參數(shù)如表1所示，通過(guò)掃描獲取的三維點(diǎn)云包含斷點(diǎn)、遮擋點(diǎn)等無(wú)效點(diǎn)。為有效減少點(diǎn)云數(shù)量并降低錯(cuò)誤匹配出現(xiàn)的概率，必須對(duì)點(diǎn)云進(jìn)行預(yù)處理。

表1 Pandar40P激光雷達(dá)技術(shù)參數(shù)

借鑒LeGO-LOAM 方法，將點(diǎn)云進(jìn)行40 行1 800 列的結(jié)構(gòu)化排序并分割為有效聚類(lèi)點(diǎn)、離群點(diǎn)和地面點(diǎn)?；咎幚聿襟E為：

a.進(jìn)行地面點(diǎn)分割，通過(guò)觀察點(diǎn)云圖獲取地面點(diǎn)所占的行數(shù)來(lái)確定預(yù)估的地面點(diǎn)云，在此類(lèi)點(diǎn)云中計(jì)算列方向上相鄰兩點(diǎn)的斜率來(lái)確定地面點(diǎn)。

b.采取臨近點(diǎn)聚類(lèi)的方法分割剩余點(diǎn)云，如果某點(diǎn)滿(mǎn)足臨近點(diǎn)數(shù)量大于30個(gè)或豎直分布的臨近點(diǎn)行數(shù)大于3行，則認(rèn)為是有效聚類(lèi)點(diǎn)，否則判斷為離群點(diǎn)。

c.處理后的點(diǎn)云仍存在幀間匹配中不穩(wěn)定存在的易遮擋點(diǎn)和斷點(diǎn)，需要利用相鄰兩點(diǎn)與激光雷達(dá)的距離進(jìn)行深度估計(jì)來(lái)完成這兩類(lèi)點(diǎn)的去除。

3.3 特征點(diǎn)的選取

由于點(diǎn)云數(shù)據(jù)的稀疏性，激光SLAM中的特征點(diǎn)往往是在點(diǎn)云的行方向上通過(guò)計(jì)算某類(lèi)特征進(jìn)行選取，為此，定義一個(gè)類(lèi)粗糙度特征c來(lái)分割平面點(diǎn)和邊緣點(diǎn)：

地面點(diǎn)云中，c值較小的點(diǎn)標(biāo)記為平面點(diǎn)；有效聚類(lèi)點(diǎn)云中，c值較大的點(diǎn)標(biāo)記為邊緣點(diǎn)，并通過(guò)標(biāo)定得到的變換矩陣獲取著色特征點(diǎn)。

3.4 特征點(diǎn)的匹配

ICP算法是幀間匹配中最常用的算法，但是傳統(tǒng)的ICP算法效率低，對(duì)點(diǎn)云三維旋轉(zhuǎn)不敏感且易陷入局部極值。改進(jìn)的ICP 算法，如PL-ICP（Point-to-Line ICP）算法[11]和PP-ICP（Point-to-Plane ICP）算法[12]有效提升了傳統(tǒng)ICP 算法的性能。針對(duì)邊緣點(diǎn)和平面點(diǎn)特性不同，通常在邊緣特征上采用PL-ICP算法，在平面特征上采用PP-ICP算法。

考慮到實(shí)際應(yīng)用中，地面點(diǎn)的顏色往往是固定的，因此在平面點(diǎn)匹配中插入顏色要素能明顯提高匹配點(diǎn)的選擇精確度。本文提出的匹配策略如圖5所示，針對(duì)某幀t平面點(diǎn)云中的點(diǎn)a，在上一幀平面點(diǎn)云中尋找與a顏色相同的最近點(diǎn)b1，在b1同一線上尋找顏色相同的最近點(diǎn)b2，在b2上、下兩線中按照同樣方法搜索點(diǎn)b3，并利用非共線點(diǎn)b1、b2、b3來(lái)擬合平面，計(jì)算點(diǎn)到平面的距離dp：

圖5 平面點(diǎn)匹配

邊緣點(diǎn)從非地面點(diǎn)中選取，包括建筑物、樹(shù)木、?？寇?chē)輛等。由于實(shí)際環(huán)境中，這類(lèi)物體的顏色往往不是單一色調(diào)，所以采取的匹配策略與平面點(diǎn)有所區(qū)別。如圖6 所示，針對(duì)某幀邊緣點(diǎn)云中的點(diǎn)a，在上一幀的邊緣點(diǎn)云中尋找與a顏色相同的最近點(diǎn)b1，在b1的上、下兩線中尋找距離最近點(diǎn)b2，利用b1和b2構(gòu)造直線，計(jì)算點(diǎn)到線的距離de：

圖6 邊緣點(diǎn)匹配

在得到匹配的特征點(diǎn)后，采取非線性?xún)?yōu)化方法來(lái)求解ICP，將匹配點(diǎn)之間的距離范數(shù)和作為誤差函數(shù)f(Pk-1,Pk)，并通過(guò)最小二乘法求解：

式中，Pk為第k幀的點(diǎn)云；Pk,i為第k幀第i點(diǎn)的三維坐標(biāo)；τ()為將三維坐標(biāo)轉(zhuǎn)換為齊次坐標(biāo)的轉(zhuǎn)換函數(shù)；Tk為幀間位姿變換矩陣。

帶有顏色信息的特征點(diǎn)相對(duì)較少，所以在預(yù)匹配過(guò)程中采用未解耦的位姿求解算法以保證求解精度。用Sk表示和，d表示dp和de，用ΔT表示第k幀到第(k-1)幀的位姿變換，誤差函數(shù)表示為：

求其對(duì)變換矩陣的雅可比矩陣：

式中，J為雅可比矩陣；f為誤差函數(shù)；p為點(diǎn)云中的點(diǎn)。

此外，匹配距離d因素對(duì)ICP 算法也會(huì)造成影響，距離越大，置信度s越低。由于Pandar40P 激光雷達(dá)的線束分配不均勻，引入線束密度因子h，取中間線束密集區(qū)域的因子為1，往兩側(cè)越稀疏越小，最外側(cè)不參與匹配：

式中，dp2l為目標(biāo)點(diǎn)到激光雷達(dá)的距離。

最后得到高斯-牛頓（Gauss-Newton）方程組為：

使用高斯-牛頓方法迭代優(yōu)化求解，直至收斂或者迭代次數(shù)達(dá)到上限，得到兩幀間的位姿變換矩陣T。將初匹配后的變換矩陣傳至LeGO-LOAM 方法的幀間匹配模塊，完成著色點(diǎn)云幀間預(yù)匹配算法。

4 算法驗(yàn)證

4.1 KITTI數(shù)據(jù)集驗(yàn)證

KITTI 數(shù)據(jù)集是目前國(guó)際上最大的自動(dòng)駕駛場(chǎng)景下的計(jì)算機(jī)視覺(jué)算法評(píng)測(cè)數(shù)據(jù)集[13-14]。當(dāng)前SLAM算法往往只適用于低速環(huán)境，所以選用KITTI 數(shù)據(jù)集Odometry 數(shù)據(jù)中住宅區(qū)與城市場(chǎng)景進(jìn)行驗(yàn)證。選用LeGO-LOAM 方法作為原始方法，對(duì)比著色點(diǎn)云幀間初匹配方法的性能，并采用GitHub上的SLAM精度測(cè)量工具evo[15]進(jìn)行誤差分析，各組仿真結(jié)果如表2所示。

表2中，本文方法對(duì)00、07和10號(hào)數(shù)據(jù)性能提升較為明顯，均方根誤差上平均優(yōu)化了6.84%。00號(hào)數(shù)據(jù)的路程較長(zhǎng)，為3 772 m，需要通過(guò)回環(huán)檢測(cè)來(lái)保證長(zhǎng)時(shí)間工作時(shí)地圖的準(zhǔn)確性，從回環(huán)次數(shù)結(jié)果可知，本文方法相對(duì)原始方法增加了12次。圖7、圖8所示分別為00號(hào)數(shù)據(jù)在LeGO-LOAM 方法下和本文方法下的局部建圖結(jié)果。由圖7和圖8對(duì)比可知，原始方法在標(biāo)記處出現(xiàn)了無(wú)法回環(huán)的情況，這是由于汽車(chē)在經(jīng)過(guò)此場(chǎng)景前有一段長(zhǎng)時(shí)間無(wú)回環(huán)檢測(cè)的路程，原始方法的累計(jì)誤差超過(guò)回環(huán)檢測(cè)范圍，導(dǎo)致汽車(chē)無(wú)法與已建立的地圖匹配造成回環(huán)失敗。而在圖8中可以發(fā)現(xiàn)，標(biāo)記處成功實(shí)現(xiàn)回環(huán)檢測(cè)，說(shuō)明本文方法相比于原始方法累計(jì)誤差小，建圖效果如圖9所示。

表2 KITTI數(shù)據(jù)集結(jié)果對(duì)比

圖7 LeGO-LOAM方法建圖結(jié)果

圖8 本文方法建圖結(jié)果

圖9 本文方法00號(hào)數(shù)據(jù)建圖結(jié)果

由10 號(hào)數(shù)據(jù)可以看出，二者的回環(huán)次數(shù)均為0，說(shuō)明在沒(méi)有發(fā)生回環(huán)的情況下，加入著色點(diǎn)云幀間初匹配能有效提高建圖精度。本文方法相比于原始方法均方根誤差減小了12.5%，2 種方法的絕對(duì)位置誤差以及標(biāo)準(zhǔn)差、均方根誤差等參數(shù)對(duì)比如圖10、圖11 所示，表明本文方法的精度相比原始方法有所提升。

4.2 實(shí)車(chē)驗(yàn)證

試驗(yàn)車(chē)輛為自行研制的福州大學(xué)純電動(dòng)無(wú)人駕駛測(cè)試平臺(tái)，如圖12 所示。工控機(jī)采用宸曜科技Nuvo-7160GC，激光雷達(dá)采用禾賽Pandar40P，攝像頭采用小覓雙目深度版，并以ROS 機(jī)器人操作系統(tǒng)實(shí)現(xiàn)傳感器的數(shù)據(jù)通訊。

圖10 KITTI數(shù)據(jù)集00號(hào)數(shù)據(jù)絕對(duì)位置誤差對(duì)比

圖11 KITTI數(shù)據(jù)集00號(hào)數(shù)據(jù)其他參數(shù)對(duì)比

圖12 試驗(yàn)車(chē)輛

首先，獲取差分GPS 輸出的經(jīng)緯度信息，轉(zhuǎn)換為平面坐標(biāo)并作為行駛軌跡的真值。由于軌跡真值的姿態(tài)與試驗(yàn)車(chē)輛不一致，所以采用Umeyama 方法配準(zhǔn)真值與測(cè)量值。測(cè)試場(chǎng)地選取在福州大學(xué)旗山校區(qū)，如圖13 所示的4 條試驗(yàn)路線。其中，序號(hào)1、2、3 為3種不同的典型路線類(lèi)型，序號(hào)4 為福州大學(xué)北門(mén)區(qū)域路線圖。對(duì)比本文方法與LeGO-LOAM 方法，各組結(jié)果如表3 所示。

由表3 可以看出，本文提出的方法相比于LeGOLOAM 算法有一定的優(yōu)化，均方根誤差平均減小了4.53%。以?xún)?yōu)化最大的序號(hào)04為例，本文方法均方根誤差減小了11.7%。2種方法的絕對(duì)位置誤差對(duì)比如圖14所示，可以看出本文方法的絕對(duì)誤差總體上小于原始方法。其他對(duì)比參數(shù)如圖15 所示，本文方法的標(biāo)準(zhǔn)差為0.386 m，平均值為0.906 m，原始方法的標(biāo)準(zhǔn)差為0.558 m，平均值為0.965 m，分別減小了30%和6%，絕對(duì)誤差最大值為2.472 m，最小為0.017 m。最終獲得的福州大學(xué)北門(mén)區(qū)域地圖如圖16所示。

圖13 試驗(yàn)路線

表3 實(shí)車(chē)試驗(yàn)結(jié)果對(duì)比

圖14 路線4絕對(duì)位置誤差對(duì)比

圖15 路線4其他參數(shù)對(duì)比

圖16 福州大學(xué)北門(mén)區(qū)域地圖

5 結(jié)束語(yǔ)

傳統(tǒng)激光SLAM 方法通?；趩我患す饫走_(dá)傳感器，通過(guò)匹配點(diǎn)云獲取位姿變換關(guān)系。本文在純激光SLAM方法上增加相機(jī)顏色信息，提出了基于著色點(diǎn)云的預(yù)匹配算法。同時(shí)，針對(duì)豎直線束非均勻分布的雷達(dá)，提出基于點(diǎn)云線束密度的位姿求解優(yōu)化方法。通過(guò)KITTI數(shù)據(jù)集和實(shí)車(chē)試驗(yàn)對(duì)LeGO-LOAM方法和本文方法的精度進(jìn)行對(duì)比，結(jié)果表明本文方法在2個(gè)數(shù)據(jù)集上軌跡點(diǎn)均方根誤差分別優(yōu)化了6.84%和4.53%，相對(duì)于原算法精度有所提高，并且能夠滿(mǎn)足搭載垂直角分辨率非均勻雷達(dá)的無(wú)人車(chē)的多傳感器即時(shí)定位與地圖構(gòu)建要求。