摘要： 對二維激光雷達與RGB-D相機聯(lián)合標定，采用改進的ORB-SLAM2算法實現(xiàn)稠密的點云地圖、八叉樹地圖、柵格地圖的構(gòu)建。提出了一種將Cartographer算法與改進的ORB-SLAM2算法融合建圖的改進算法。實驗結(jié)果表明，相比傳統(tǒng)的ORB-SLAM2算法，改進的融合算法在建圖過程中障礙物的識別率達到了96.8%，絕對位姿誤差減小了53.2%，提高了建圖的精確性和魯棒性。

關鍵詞： 激光雷達；RGB-D相機；ORB-SLAM2算法；同步定位與建圖；多傳感器融合

中圖分類號： TP242.6

文獻標志碼： A"""""文章編號： 2097-3853（2024）01-0058-07

SLAM mapping based on fusion of LiDAR and RGB-D camera

FU Penghui， YAN Xiaolei， YU Jie， YU Tinghai， YE Sheng

（Fujian Provincial Key Laboratory of Automotive Electronics and Electric Drive Technology， Fuzhou 350118， China）

Abstract： For the joint calibration of 2D LiDAR and RGB-D camera， the improved ORB-SLAM2 algorithm was used to construct dense point cloud map， octree map and raster map. An improved mapping algorithm combining Cartographer algorithm with improved ORB-SLAM2 algorithm is proposed. The experimental results show that compared with the traditional ORB-SLAM2 algorithm， the new fusion algorithm can achieve 96.8% obstacle recognition rate and reduce the absolute pose error by 53.2%， which improves the accuracy and robustness of map construction.

Keywords： LiDAR; RGB-D camera；ORB-SLAM2 algorithm; SLAM; multi-sensor fusion

在采用單一的二維激光雷達實現(xiàn)同步定位與建圖（simultaneous localization and mapping，SLAM）研究方面，Queralta等［1］提出了一種基于FPGA架構(gòu)多個旋轉(zhuǎn)的二維激光雷達來模擬低成本的三維激光雷達的設計，為低成本的移動機器人三維建圖提供了可能性，但并未解決二維激光雷達描述環(huán)境信息不充分的問題。Xia等［2］通過二維激光雷達獲取點云數(shù)據(jù)，并利用開源的Cartographer算法來實現(xiàn)移動機器人的地圖構(gòu)建，雖然該方法能夠反映二維平面的環(huán)境信息，但對小型障礙物的描述仍然不完整。

采用低成本的單一相機實現(xiàn)SLAM研究方面，Klein等［3］提出了適用于單目相機的PTMA算法，實現(xiàn)了跟蹤與建圖過程的并行化，并采用了非線性優(yōu)化來提高建圖的精度。Mur-Artal等［4］提出了應用于單目相機的ORB-SLAM算法，隨后又提出一種適用于單目、雙目、RGB-D相機的ORB-SLAM2算法［5］。由于相機易受光照強度等環(huán)境因素影響，對透明物體掃描不清，導致構(gòu)建的地圖存在誤差，無法直接用于移動機器人的導航。

為了解決上述問題，本文對傳統(tǒng)的ORB-SLAM2算法進行改進，實現(xiàn)了稠密點云地圖、八叉樹地圖、柵格地圖的構(gòu)建，再結(jié)合貝葉斯融合法則將激光雷達與RGB-D相機的點云數(shù)據(jù)融合，得到了融合后的柵格地圖。

1"改進SLAM算法的方案設計

改進的SLAM算法的流程如圖1所示。

由圖1可知，通過對2D激光雷達與RGB-D相機的點云數(shù)據(jù)處理，結(jié)合貝葉斯估計，可以得到融合的全局柵格地圖。其中R為旋轉(zhuǎn)矩陣，t為平移矩陣。

2"激光雷達與RGB-D相機的聯(lián)合標定

激光雷達與RGB-D相機的點云數(shù)據(jù)都在各自的坐標系下，無法保證二者數(shù)據(jù)在空間上同步，因此需要通過聯(lián)合標定來實現(xiàn)點云數(shù)據(jù)的同步。通過選取不同時刻的激光點云數(shù)據(jù)與對應的RGB-D相機點云數(shù)據(jù)進行匹配，得到RGB-D相機與激光雷達之間的旋轉(zhuǎn)矩陣與平移矩陣等參數(shù)。

設空間某一點P同時被激光雷達和RGB-D相機觀測到，如圖2所示。該點在激光雷達坐標系下，其坐標為Pl（xl，yl，zl），在RGB-D相機坐標系下，其坐標為Pc（xc，yc，zc）。根據(jù)上述坐標系之間的幾何關系，可推導出被測物體在兩個坐標系中的轉(zhuǎn)化關系為：

xcyczc=Rt01xlylzl（1）

在RGB-D相機中，深度信息與坐標系之間的關系為：

zcuv1=fx0qx0fyqy001xcyczc（2）

式（2）中，fx、fy、qx、qy為相機的內(nèi)置參數(shù)，像素；其中，fx、fy為在X、Y軸上的焦距；qx、qy為相機光心在像素坐標系中的坐標。

根據(jù)式（1）和式（2），將RGB-D相機采集到的檢測數(shù)據(jù)，轉(zhuǎn)換至激光雷達坐標系中，兩者之間的轉(zhuǎn)化關系為：

zcuv1=fx0qx0fyqy001Rt01xlylzl（3）

最終得到的關系式為：

Pc=XcYcZc=RXlYlZl+t=RPl+t（4）

采用Autoware聯(lián)合標定，聯(lián)合標定流程圖如圖3所示。根據(jù)標定原理，激光雷達和RGB-D相機同時觀測標定板，并在此過程中不斷更換標定板的位置，以獲取不同時刻的點云位置，得到RGB-D相機的標定參數(shù)結(jié)果如表1所示。經(jīng)過多次不同時刻的位姿變換，分別計算出兩傳感器的標定結(jié)果。

標定完成后可得到RGB-D相機的內(nèi)參矩陣、徑向畸變和切向畸變等參數(shù)值，如表1所示。

3"改進的ORB-SLAM2算法與融合建圖

ORB-SLAM2是一種基于特征點的視覺SLAM算法［6］，主要由跟蹤、局部地圖、回環(huán)檢測線程等3部分組成。ORB-SLAM2通過對RGB-D相機輸入的圖像幀來提取ORB特征，并結(jié)合上一幀圖像估計RGB-D相機的位姿信息，選擇一些具有代表性的圖像作為關鍵幀。將關鍵幀插入地圖以后，創(chuàng)建局部地圖，同時對局部BA（光束法平差）優(yōu)化。通過優(yōu)化后的關鍵幀檢測閉環(huán)線程，檢測到閉環(huán)以后對位姿更新，最終通過全局BA優(yōu)化線程，更新全局地圖。

3.1"改進的ORB-SLAM2算法

圖4為改進的ORB-SLAM2算法框架圖，其中虛線內(nèi)為改進部分。在原有的跟蹤線程、局部地圖線程、回環(huán)檢測線程的基礎上，新增了稠密點云地圖線程，實現(xiàn)了稠密點云地圖、八叉樹地圖、柵格地圖的建圖功能。

（1）稠密點云地圖

設三維空間中任意一個點為X，其坐標為X=［xw，yw，zw］，在相機坐標系下對應的像素坐標為：x=［u，v，1］，根據(jù)相機針孔成像原理得到點云的空間位置信息。

zuv1=K（R|t）xwywzw1（5）

式中，K為相機內(nèi)參，z為深度值和實際空間距離的比例因子。

世界坐標系原點與相機坐標系原點重合后，沒有平移和旋轉(zhuǎn)，此時R為單位矩陣；t為零矩陣，通過式（5）變換得到點云的三維空間位置坐標。

xw=z（u-qx）fxyw=z（u-qy）fyzw=z（6）

通過式（5）（6），計算出圖像的深度信息，實現(xiàn)三維稠密點云地圖的構(gòu)建。

基于原ORB-SLMA2算法構(gòu)建的稀疏點云地圖，新增一種稠密點云回環(huán)的方法。當系統(tǒng)對詞袋模型回環(huán)檢測優(yōu)化時，會記錄每一個關鍵幀的位姿數(shù)據(jù)。通過全局BA優(yōu)化并更新關鍵幀的位姿信息與地圖的坐標，將優(yōu)化后的關鍵幀位姿進行點云拼接和濾波優(yōu)化，更新優(yōu)化局部的點云地圖，可以減小誤差并提高系統(tǒng)的魯棒性。

（2）八叉樹地圖

在稠密點云地圖的基礎上構(gòu)建八叉樹地圖［7］，原理如圖5所示。八叉樹地圖可表示為一個被均勻分為八塊的大立方體，它的根節(jié)點下方有8個子節(jié)點，白色表示未被占據(jù)，黑色表示已經(jīng)被占據(jù)且不可被分割，每一個節(jié)點類比成體素。其中未被占據(jù)的節(jié)點繼續(xù)展開形成八個子節(jié)點，被占據(jù)的節(jié)點停止展開。當?shù)貓D中添加信息時，由于空白的地方會連在一起，大多數(shù)八叉樹節(jié)點無需展開到葉子層面，因此八叉樹地圖比點云地圖節(jié)省了大量的存儲空間。

設某節(jié)點為n，觀測數(shù)據(jù)為M，從開始時刻到t時刻某節(jié)點的概率值為L（n|M1∶t），則在（t+1）時刻的值為：

L（n|M1∶t+1）=L（n|M1∶t-1）+L（n|M1∶t）（7）

通過式（7）計算出該節(jié)點在某一時刻的概率值。εoccupy為預先設定的參數(shù)，用于計算某一時刻該節(jié)點的概率值。

L（n|M1∶t+1）=εoccupy0（8）

通過式（8）得到概率值，再結(jié)合RGB-D相機的數(shù)據(jù)，即可得到八叉樹地圖。

（3）柵格地圖

當八叉樹地圖構(gòu)建完成后，需對其進行處理。八叉樹是基于體素濾波來描述三維空間的環(huán)境信息，而移動機器人在導航的過程中需要的是二維平面的地圖信息，故需要將其轉(zhuǎn)化為二維的柵格地圖來用于導航。利用ROS（機器人操作系統(tǒng)）的Octomap_serve和Map_server這兩個功能包來實現(xiàn)八叉樹地圖的轉(zhuǎn)化。其原理是利用投影來將三維的空間信息投影到二維平面，最終可以得到可用于導航的柵格地圖。

3.2"激光雷達與RGB-D相機融合

Cartographer是一種基于圖優(yōu)化的算法［8］，相比于粒子濾波的SLAM算法，增加了回環(huán)檢測機制來優(yōu)化移動機器人的位姿，因此在處理數(shù)據(jù)時更加高效、穩(wěn)定。但該算法只能接受一種類型的激光數(shù)據(jù)作為輸入方式，不能直接處理RGB-D相機采集的數(shù)據(jù)。故需要將RGB-D相機的圖像數(shù)據(jù)訂閱為激光雷達的點云數(shù)據(jù)，并將深度相機的圖像數(shù)據(jù)轉(zhuǎn)化為激光數(shù)據(jù)來實現(xiàn)二者的融合。

根據(jù)激光與視覺數(shù)據(jù)融合原理，本文采用貝葉斯法則［9-10］來融合激光雷達和RGB-D相機的點云數(shù)據(jù)，并將融合后的點云數(shù)據(jù)用于構(gòu)建柵格地圖。貝葉斯法則是用后驗概率統(tǒng)計的方式進行數(shù)據(jù)融合，融合規(guī)則如表3所示。

式中，P（Zk|xk）表示測量模型的似然函數(shù)；P（xk|Zk-1）表示（k-1）時刻的先驗分布函數(shù)；P（Zk|Zk-1）表示（k-1）時刻估計在k時刻的概率。

通過將傳感器的觀測數(shù)據(jù)與貝葉斯公式結(jié)合，可以得到傳感器的柵格單元占據(jù)概率值如式（10）所示。

P0=PsPmPsPm+（1-Ps）（1-Pm）（10）

式中，P0表示數(shù)據(jù)融合后的更新概率值；Ps表示點云返回后占用柵格單元的條件概率；Pm表示柵格被占用的先驗概率；（1-Ps）表示激光雷達柵格為空的概率；（1-Pm）表示RGB-D相機柵格為空的概率。

4"實驗研究

4.1"實驗平臺架構(gòu)

采用如圖6所示的智能小車，驗證激光雷達與RGB-D相機融合建圖的可行性。小車使用的主要傳感器為Rplidir A1激光雷達和奧比中光的RGB-D相機。Rplidir A1激光雷達主要參數(shù)：測量半徑：16 m；采樣頻率：8 Hz；掃描頻率：15 Hz；角度分辨率：0.9°；供電電壓：5 V；測距分辨率：≤實際距離的1%（測距≤12 m）；測距精度：實際距離的1%（≤3 m）。奧比中光RGB-D相機的主要參數(shù)：深度分辨率：1 280×1 024@7FPS；彩色分辨率：1 280×720@30FPS；測量范圍：0.6～8 m；深度FOV：H58.4°～V45.5°等。

4.2"實驗驗證

4.2.1"RGB-D相機建圖

實驗平臺在Ubuntu18.04版本中對傳統(tǒng)ORB-SLAM2算法建圖，傳統(tǒng)算法在提取完特征點后構(gòu)建的地圖比較稀疏，只能用于定位，無法直接用于移動機器人的導航。實驗結(jié)果如圖7所示。

利用改進的ORB-SLAM2算法構(gòu)造出稠密點云地圖，與改進前的稀疏點云地圖對比，然后利用Octomap的點云數(shù)據(jù)轉(zhuǎn)換，輸出八叉樹地圖，最后通過Octomap_server投影后將八叉樹轉(zhuǎn)化為柵格地圖。在柵格地圖中，黑色部分表示有障礙物且被占用，白色部分表示未被占用，實驗結(jié)果如圖8所示。

4.2.2"激光雷達及融合建圖

實驗的環(huán)境為中型辦公室，如圖9所示，使用Cartographer算法進行二維柵格地圖的構(gòu)建。當移動機器人開啟雷達后，開始掃描周圍環(huán)境，并不斷更新二維空間的點云和位姿信息，繼續(xù)控制小車移動，最終得到如圖10的二維柵格地圖。利用改進的融合算法，經(jīng)過多次實驗對比，并對一些累計誤差、相對位姿進行處理，最終得到圖11的融合柵格地圖。

由圖10可知，單一的激光雷達建圖時輪廓描述不清晰，對小型障礙物的掃描信息也不全。而使用RGB-D相機與激光雷達融合構(gòu)建的地圖則彌補了這一缺陷，融合后的算法對障礙物的表征更加清晰，對一些較小的障礙物的掃描更加完整。單一傳感器的建圖時間與障礙物的檢測率對比如表4所示。對于定位的精度，采用絕對位姿誤差指標來進行比較，絕對誤差越小，建圖的定位精度越高。由表5可知，無論是平均誤差還是誤差中值，融合后的效果都要優(yōu)于融合前，可見融合建圖的絕對位姿誤差更小，全局一致性也高。

綜上可知，融合后的算法在建圖過程中障礙物的識別率達到96.8%，絕對位姿誤差降低了53.2%，表明該算法真實位姿與估計位姿的軌跡誤差較小，定位的精度和障礙物的識別率更高。

5"結(jié)論

1）改進的ORB-SLAM2算法實現(xiàn)了稠密點云地圖、八叉樹地圖、局部柵格地圖的構(gòu)建，為后期的融合提供了理論支持。

2）實現(xiàn)了激光雷達與RGB-D相機的融合，激光雷達對二維平面進行360°掃描，RGB-D相機提供高精度的深度信息，二者優(yōu)勢互補后提高了建圖的精度與準確性。

3）融合后建圖更加完整清晰，對細小的障礙物識別更加清晰，且障礙物的識別率達到了96.8%，相對位姿誤差減小了53.2%，提高了建圖的精度和障礙物的識別率，為后期移動機器人的路徑規(guī)劃和導航提供了研究方向。

參考文獻：

［1］ QUERALTA J P，YUHONG F，SALOMA A L. FPGA-based architecture for a low-cost 3D lidar design and implementation from multiple rotating 2D lidars with ROS［C］∥2019 IEEE SENSORS. Montreal，QC，Canada： IEEE，2019：1-4.

［2］ XIA T，SHEN X. Research on parameter adjustment method of cartographer algorithm［C］∥2022 IEEE 6th Advanced Information Technology，Electronic and Automation Control Conference （IAEAC）. Beijing，China： IEEE，2022：1292-1297.

［3］ ZHANG S S，ZHENG L Y，TAO W B. Survey and evaluation of RGB-D SLAM［J］. IEEE Access，2021，9：21367-21387.

［4］ MUR-ARTAL R，TARDOS J D. ORB-SLAM2：an open-source SLAM system for monocular，stereo，and RGB-D cameras［J］. IEEE Transactions on Robotics，2017，33（5）：1255-1262.

［5］ LIU C，ZHANG G J，RONG Y M，et al. Hybrid metric-feature mapping based on camera and Lidar sensor fusion［J］. Measurement，2023，207：112411.

［6］ HOU X，SHI H，QU Y H，et al. A fusion method for 2D LiDAR and RGB-D camera depth image without calibration［C］∥SUN F，LI J，LIU H，et al. International Conference on Cognitive Computation and Systems. Singapore：Springer，2023：89-101.

［7］ PARK K，KIM S，SOHN K. High-precision depth estimation using uncalibrated LiDAR and stereo fusion［J］. IEEE Transactions on Intelligent Transportation Systems，2020，21（1）：321-335.

［8］ GRISETTI G，STACHNISS C，BURGARD W. Improved techniques for grid mapping with Rao-blackwellized particle filters［J］. IEEE Transactions on Robotics，2022，23（1）：34-46.

［9］ 韓彥峰，唐超超，肖科. 基于改進ORB-SLAM2算法的RGB-D稠密地圖構(gòu)建［J］. 湖南大學學報（自然科學版），2023，50（2）：52-62.

［10］ 羅亮，談莉斌，余曉流，等. 一種融合二維激光雷達和RGB-D相機的移動機器人建圖方法研究［J］. 制造業(yè)自動化，2023，45（4）：137-140，190.

（責任編輯： 陳雯）

收稿日期：2023-10-29

基金項目：福建工程學院科研啟動基金項目（GY-Z21025）

第一作者簡介：付鵬輝（1997—），男，河南信陽人，碩士研究生，研究方向：多傳感器融合SLAM。

通信作者：閆曉磊（1981—），男，河南許昌人，教授，博士，研究方向：優(yōu)化算法與結(jié)構(gòu)輕量化設計。