一種混合視覺的車載激光雷達即時定位與地圖構(gòu)建方法＊

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

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

主題詞：激光雷達 混合視覺 即時定位與地圖構(gòu)建 點云 匹配

1 前言

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

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

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

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

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

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

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

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

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

圖2 具有顏色信息的點云

圖3 相機圖片

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

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

3.1 算法基本思路

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

圖4 算法基本流程

3.2 點云預(yù)處理

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

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

借鑒LeGO-LOAM 方法，將點云進行40 行1 800 列的結(jié)構(gòu)化排序并分割為有效聚類點、離群點和地面點。基本處理步驟為：

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

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

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

3.3 特征點的選取

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

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

3.4 特征點的匹配

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

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

圖5 平面點匹配

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

圖6 邊緣點匹配

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

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

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

求其對變換矩陣的雅可比矩陣：

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

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

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

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

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

4 算法驗證

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

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

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

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

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

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

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

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

4.2 實車驗證

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

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

圖11 KITTI數(shù)據(jù)集00號數(shù)據(jù)其他參數(shù)對比

圖12 試驗車輛

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

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

圖13 試驗路線

表3 實車試驗結(jié)果對比

圖14 路線4絕對位置誤差對比

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

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

5 結(jié)束語

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