基于深度相機與激光雷達數據融合SLAM方法研究*

吳耀威，吳自然,2，趙宇博，繆宏圣

（1.溫州大學電氣與電子工程學院，浙江 溫州 325035；2.溫州大學樂清工業研究院，浙江 溫州 325035）

0 引言

目前的同步定位和地圖構建（SLAM）方案大致分為使用激光雷達傳感器的激光SLAM 與使用視覺傳感器的視覺SLAM。激光SLAM 與視覺SLAM 經過了近30年的研究，目前已經有成熟并且可以落地的方案出現。其中基于激光雷達的SLAM 方案有GMapping[1]、Cartographer[2]、Hector[3]等。基于相機的SLAM 方案有基于單目相機的ORB-SLAM 系列[4-6]、使用深度相機的RGB-D SLAM[7]。雖然上述方案基本解決了SLAM 問題，但使用單一傳感器的SLAM 方案仍然存在著許多不足。如使用單目相機的SLAM 方案，通過提取關鍵幀圖像上的特征點[8]來估計機器人的位姿增量，然而單目相機所構建的關鍵幀圖像質量嚴重依賴環境中的光照條件。通常在強光或者黑暗的條件下由于RGB 圖像缺乏環境中的紋理信息，將導致SLAM 系統無法提取圖像中的特征點來構建特征地圖，因此基于單目的SLAM 在室外強光或者黑夜環境中極易失效；使用深度相機的RGB-D SLAM，利用RGB-D 相機的成像特點，通過深度圖獲得每一幀圖像中的像素深度，彌補了單目SLAM 需要消耗大量計算性能用于估計像素深度的缺點，但是RGB-D 相機往往精度與量程有限，因此只能用于室內建圖。相較于受環境限制的視覺SLAM，激光SLAM 方案的應用場景要更加廣泛，因此被常用于移動機器人的建圖與導航任務[9]。雖然激光雷達具有精度高、抗干擾能力強等特點，但是仍有其局限性。例如二維激光雷達只能掃描單一平面上的2D信息，無法檢測到掃描平面之外的障礙物，使用多線激光雷達[10]或者添加多組處在不同平面的二維激光雷達[11]可以解決上述問題，但是這樣的方案造價高昂無法普及。由于使用單一傳感器的SLAM 技術存在上述缺點，目前研究者開始探索融合激光雷達與深度相機等多傳感器信息融合的SLAM方案。Ali Yeon[12]分析了融合深度相機與2D激光雷達信息的可行性，但是并未進行實驗驗證。Federico Nardi[13]提出了一種利用深度圖模擬2D 激光雷達的方法，利用深度相機發布的深度圖像提取出偽激光雷達數據，進而用于構建占用柵格地圖，但是所提出的方法精度與效率較低。陳鵬[14]提出了一種利用貝葉斯估計理論融合視覺局部地圖與激光局部地圖得到全局地圖的方案。鐘敏[15]利用深度相機進行運動估計矯正激光SLAM 過程中的位姿累積誤差，鄭麗麗[16]通過擴展卡爾曼濾波框架融合IMU、輪式里程計與視覺里程計，從而提高機器人位姿估計精度，并沒有考慮地圖構建任務。

針對上述問題本文提出了一種深度相機與激光雷達在數據層面上的融合方法，融合后的傳感器數據使用GMapping算法進行建圖，最后利用Gazebo仿真軟件搭建移動機器人仿真環境，驗證本文所提出的傳感器融合方法的可行性。

1 傳感器模型

本文提出的傳感器數據融合方法依賴于單線激光雷達與深度相機這兩種傳感器，單線激光雷達作為移動機器人環境感知的核心功能傳感器，具有測距精準、感知范圍廣、抗干擾、運行穩定可靠等特點，在傳感器融合過程中主要用于環境中的平面障礙物信息測量。深度相機是一種能夠獲取場景中物體與攝像頭之間物理距離的相機，主流的深度相機通常由多種鏡頭、光學傳感器組成，通常使用深度相機獲取三維空間中的障礙物信息。本文主要研究的側重點在于融合兩種傳感器的數據。

2 傳感器數據融合

2.1 傳感器數據融合框架

傳統的SLAM 研究通常集中在單一傳感器上，然而不管是基于視覺傳感器的SLAM 還是基于激光雷達的SLAM 都無法準確的描述出復雜的環境，因而需要用到傳感器融合技術，使用多個傳感器能夠提供豐富的環境信息，減少測量誤差，提高移動機器人的運行魯棒性。本文使用深度相機與單線激光雷達傳感器，通過數據融合的形式實現傳感器之間的優勢互補。下面將從傳感器外參聯合標定、深度相機點云處理、傳感器數據融合3個方面進行研究，圖1所示為傳感器數據融合總體框圖。

圖1 傳感器數據融合總體框圖

2.2 傳感器外參標定

移動機器人搭載多種傳感器，不同傳感器通常位于移動機器人載體的不同位置，因此傳感器之間是存在外參變換的。然而傳感器融合技術需要將不同傳感器之間的數據轉換到同一坐標系下進行處理。為了求出傳感器之間的外參矩陣，需要進行傳感器外參聯合標定實驗，該步驟是傳感器融合技術的必要步驟。

本文需要融合深度相機與激光雷達的數據，采用文獻[17]的方法進行深度相機與激光雷達的聯合標定。圖2所示為深度相機與激光雷達聯合標定實驗示意圖，由圖可知深度相機通過場景中的棋盤格標定內參，激光雷達對場景中的棋盤格進行測量形成線狀特征，通過構建三維激光點與棋盤格三維坐標之間的約束關系，進而求解深度相機與激光雷達的外參。其中傳感器外參求解的本質就是估計激光雷達坐標系與相機坐標系之間的坐標變換。

圖2 聯合標定實驗示意圖

為了描述外參標定的過程，引入如圖3所示4個坐標系：激光雷達坐標系Ol-XlYlZl，基準坐標系Ow-XwYwZw，相機坐標系Oc-XcYcZc以及圖像坐標系O-uv，其中基準坐標系為世界坐標系，為空間中的每一個點賦予坐標信息。激光雷達與相機坐標系為傳感器中的每一個觀測點賦予坐標信息。雷達外參的求解可以由式（1）實現。

圖3 各坐標軸關系

同理對于相機坐標系與基準坐標系之間的外參標定，由式（2）描述：

根據針孔相機模型，如圖4 所示。對于在相機坐標系下的三維空間點P，有齊次坐標P=[xc yc zc1]T，點P在成像平面上的坐標為P′=[u v1]T，點P與P’存在如下轉換關系：

圖4 針孔相機模型

式中：[xw yw zw]T為世界坐標系中的坐標；fx和fy分別為相機坐標為相機在x方向與y方向上的等效焦距；Cx和Cy分別為像素中心點在成像平面上坐標。

在聯合標定過程中，激光雷達會在棋盤格標定板上生成掃描點，每一個掃描點都會在相機的圖像空間中存在唯一的一個像素點與之對應。因此二維激光雷達與相機之間的外參標定問題即為激光雷達數據點與二維圖像像素點之間的映射矩陣求解問題，聯立式（1）、（2）、（3）可得外參標定問題方程組為：

式中：[xl yl1]T為激光雷達數據點的齊次坐標；[u v1]T為激光點對應的像素齊次坐標；Rt和tt分別為激光雷達坐標到相機坐標的旋轉矩陣與平移向量。

2.3 深度相機點云預處理

根據第1 節描述的傳感器模型，深度相機能夠發布環境點云，但是發布的環境點云中包含大量的冗余點云。如圖5 所示為深度相機發布的點云，由圖可知，過高的墻體與地面點云不屬于機器人建圖時的障礙物信息，因此需要將這些點云進行剔除。本文采用VoxelGrid 體素化網格濾波器進行點云降采樣，之后在使用隨機抽樣一致性算法RANSAC 提取地面點云并剔除，對于過高的墻體點云可以通過設置點云高度閾值進行剔除，具體步驟如下。

圖5 深度相機發布的原始點云

（1）點云降采樣處理：將深度相機發布的點云轉換為能被PCL 庫處理的點云格式，利用PCL 中的體素濾波器進行濾波處理，濾波后的點云如圖6所示。通過與圖5所示的原始點云進行對比可以看出，濾波后的點云數量明顯減少，但是點云的形狀特征并沒有丟失。

圖6 濾波后的點云

（2）去除過高點云：由于本文采用的機器人為地面移動機器人，因此過高的點云將不被視為障礙物。本文所用的機器人總高度為0.15 m，以0.30 m 為閾值，遍歷所有點云的z軸數值，當點云的z軸數值小于0.30 時，即可保留該點云，圖7所示為剔除過高點云之后的點云圖。從圖中可以看到使用該方法有效地去除了過高的點云，并且保留了地面上的障礙物點云。

圖7 剔除過高點云

（3）去除地面點云：本文使用隨機抽樣一致性算法RANSAC去除地面點云，首先通過計算地面點云法向量，利用地面法向量計算地面擬合方程式如下：

通過RANSAC 算法隨機選擇數據點并計算該點所在平面方程π(A,B,C,D)，計算點云數據中每一個點到該平面的距離，利用距離閾值dmax統計出該閾值內的點數Nin，重復上述過程，最終選擇點數最多的平面方程，該平面方程所構成的點集為{P1,…,PN}，利用該集合最小化距離函數式（6），求得最優地面擬合方程如下：

其中Dk表示集合{P1,…,PN}中的第k個點到地面平面的距離。滿足最優地面擬合方程的點云即為地面點云，去除地面點云后的深度相機點云數據如圖8所示。

圖8 剔除過高點云與地面點云后

（4）點云轉換偽激光雷達數據：去除地面與過高點云之后的數據仍然具有冗余的信息，本文采取點云投影的方法，將第三步所得點云轉換為偽激光雷達數據，從而實現深度相機點云對激光雷達數據的模擬，轉換原理如圖9所示。

設障礙物點云為Mi(xi,yi,zi)，地面投影點云為mi(θi,di)，兩者之間的轉換關系由式（7）～（8）描述：

對于給定的深度相機，已知相機水平FOV 范圍為(βmin,βmax)，一幀激光束共有N個點，假設點mi(θi,di)為OC射線上距離光心O最近的點，則可以計算點mi(θi,di)在模擬激光數據中的索引值為：

重復上述步驟，即可得到模擬激光數據，如圖10 所示為模擬激光數據。

2.4 傳感器數據融合

本文采用點云融合的方式將深度相機模擬激光數據與激光雷達數據進行融合，先將激光雷達數據轉換成點云格式，之后利用傳感器外參矩陣將模擬激光點云通過坐標變換，轉移到激光雷達坐標系下，最后在同一坐標系下將兩種傳感器點云進行融合，融合后的點云數據通過程序轉換為激光雷達數據，為后文的SLAM 算法提供數據來源。圖11所示為融合后的激光雷達數據，可以看出融合后的數據保留了深度相機點云的障礙物特征。

圖11 融合后的激光雷達數據

3 GMapping算法研究

3.1 算法概述

GMapping 是一種基于Rao-Blackwellised 粒子濾波器（RBPF）的SLAM 算法，GMapping 在RBPF 的框架改進了提議分布，使用里程計與激光雷達數據作為提議分布。其次GMapping算法改進了重采樣策略，使用自適應重采樣來緩解粒子匱乏問題。因此本文第二章提出的傳感器數據融合方法生成的激光掃描數據能夠直接輸入GMapping算法用于地圖構建。

3.2 提議分布優化

傳統RBPF框架中，通常使用移動機器人里程計數據作為控制量計算提議分布。此時粒子的權重由式（10）計算：

3.3 重采樣策略

傳統RBPF 濾波器中采用的重采樣策略為：在重采樣過程中使用權重大的粒子替代權重小的粒子，這種做法容易丟棄一些性質較好的低權重粒子，造成粒子匱乏使得最后的樣本近似的分布與目標分布相差甚遠。GMapping 采用自適應重采樣的策略，該策略使用式（12）計算指標Neff，通過設定的閾值來判斷是否需要進行重采樣操作。

式（12）中分母的意義在于衡量粒子樣本權值的差異，其核心思想：如果一組樣本是對目標分布的良好采樣，那么根據重要性權值的定義，各個樣本的權重應當基本一致。所以權重的差異越大，意味著樣本的分布與目標分布之間存在著較大的差異，需要進行重采樣來修正。理論上樣本的分布越靠近目標分布則各個粒子之間的權重應該越接近。

4 仿真實驗驗證和結果分析

4.1 仿真實驗設計

為驗證本文所提出的傳感器數據融合方法的有效性與可行性，利用Gazebo仿真軟件搭建3.5 m×3.5 m大小的實驗環境，在實驗環境中導入了2個邊長為10 cm的正立方體，3 個邊長7.5 cm 的正立方體，以及2 個邊長為2.5 cm 的正立方體，仿真環境如圖12（a）所示。實驗環境中導入開源機器人模型turtlebot3 模擬真實移動機器人，turtlebot3 最高處為15 cm，其中激光雷達處在13 cm 處，深度相機處在11 cm 處。圖13 所示為機器人仿真模型與障礙物對比。為了體現傳感器數據融合對建圖效果的影響，組別1僅使用激光雷達進行建圖；組別2僅使用深度相機建圖；組別3 使用深度相機與激光雷達數據融合進行建圖。3 組實驗皆為使用手動建圖模式，其行駛路徑節點順序為1-2-3-4-1-5，并且在每個節點處移動機器人都順時針旋轉360°，目的是為了讓傳感器更好地感知周圍的環境，行駛路徑如圖12（b）所示。

圖12 仿真環境及行駛路徑

圖13 機器人仿真模型與障礙物對比

4.2 實驗結果分析

實驗按照所使用的傳感器類型分為僅使用激光雷達、僅使用深度相機以及使用深度相機與激光雷達融合3 種方案，3種方案生成的傳感器數據如圖14所示。

圖14 3種方案傳感器數據

圖14（a）所示為僅使用深度相機模擬激光雷達時發布的偽激光雷達掃描點，由圖可知利用深度相機能夠感知三維空間中的物體信息的特點，機器人感知到前方的5個障礙物，但是受限于相機視場角FOV 的限制，并不能得到360°方位的障礙物信息，因此僅使用深度相機模擬激光雷達數據是不完備的。圖14（b）所示為僅使用激光雷達傳感器時發布的激光掃描點，由圖可知激光雷達很好地感知了周圍墻體的信息，但是由于激光雷達掃描平面高于障礙物所在平面，因此僅使用激光雷達數據也是不完備的。如圖14（c）所示為兩種傳感器融合后發布的激光掃描點數據，可以看出融合后的數據將深度相機與激光雷達的優點有效結合，因此本文提出的傳感器融合策略是有效的。

通過將上述傳感器數據輸入GMapping算法分別構建地圖，最終建圖結果如圖15所示。

圖15 建圖結果對比

從僅使用深度相機建圖結果圖15（a）來看，僅使用深度相機構建的地圖邊緣輪廓粗糙，這是由于在深度相機點云模擬激光雷達掃描的時候傳感器噪聲帶來的影響，雖然地圖輪廓粗糙，但是使用深度相機仍然觀測到了環境中的7 個障礙物。其次地圖發生了較大的位移，這是由于深度相機的FOV過小，引起GMapping算法中掃描匹配模塊失敗，具體原因：當前時刻機器人通過轉換深度相機點云至偽激光雷達數據，但是由于相機的FOV限制，該偽激光雷達數據過短，從而無法使用掃描匹配中的迭代最近點算法（ICP）估算出移動機器人此時的位姿增量，因為ICP 算法是通過迭代的方式求解最優位姿變換，所以采集到點越多意味著可迭代次數越多，求解出的位姿增量精度更高，同理數據越少ICP 效果越差。從圖15（b）來看，由于單線激光雷達的掃描平面高于障礙物平面，因此在僅使用激光雷達建圖時，地圖中并沒有構建出7個障礙物的位置。從圖15（c）來看使用傳感器融合的方法構建出的地圖成功顯示了7 個障礙物的位置，地圖輪廓清晰但地圖邊緣略微有點加粗，這是由于兩種傳感器數據中的噪聲所引起的。

為了定量分析本文所提出的方法，分別對上述3 種類別傳感器進行多次建圖實驗，通過計算建圖時間和障礙物檢測率體現所提出方法的優點。

（1）建圖時間。對3種類型的傳感器分別進行5次實驗，記錄每次實驗的建圖耗時，通過觀察實時建圖結果，當機器人所構建的地圖面積不再有明顯增加時停止建圖，通過計算每組實驗平均用時衡量3種建圖方法的效率。

實驗結果如表1所示。由表中數據可知，3種建圖實驗平均用時如下：深度相機建圖實驗平均用時為40.31 s，激光雷達建圖平均用時為16.91 s，傳感器融合建圖平均用時為30.72 s。3 種類別的使用中激光雷達建圖用時最短，因為激光雷達返回的每一幀數據占用內存小，輸入建圖算法后處理過程快，所以得到最短的建圖用時。使用深度相機建圖，需要先將點云轉換為激光雷達，這一過程需要將深度相機發布的大量點云數據進行提取并轉換成對應格式，因此深度相機用時最長。使用傳感器融合進行建圖平均用時比深度相機建圖平均用時減少31.22%，這是因為程序將深度相機發布的點云數據進行濾波和剪裁處理，使得融合時所用的數據量減小；又因為融合過程需要將點云格式進行轉換，因此平均用時會比激光雷達建圖增多44.95%。

表1 建圖時間數據s

（2）障礙物檢測率：本文使用障礙物檢測率來衡量建圖效果，統計由傳感器構建的二維柵格地圖中障礙物的總邊長S1，真實環境中的障礙物總邊長S2，通過式（13）計算障礙物檢測率：

式（13）的原理簡單表述：SLAM 構建的占用柵格地圖都保存為.pgm 格式的灰度圖，這種格式的圖片每一個像素都保存為灰度值。已知所有的障礙物灰度值都是黑色也就是255，因此只要統計圖像中所有黑色像素的個數，乘以每個像素的分辨率（算法規定的每一個像素柵格代表現實中多少米，本文使用的分辨率為0.05 m/柵格），即可得到障礙物總邊長，再通過與真實環境中計算的總障礙物邊長求比值，即可得到障礙物檢測率。

每次實驗的計算結果如表2 所示。由表2 數據可知，使用傳感器數據融合方法得到最好的障礙物檢測率為98.66%，比使用激光雷達的建圖方法提高了13.24%，比使用深度相機建圖方法提高了8.21%。

表2 障礙物檢測率數據%

綜上所述本文提出的傳感器融合方法具有一定的可行性與有效性。

5 結束語

本文提出了一種多傳感器數據融合的方法，該方法首先將深度相機點云數據進行濾波降采樣處理，然后去除點云中的無關部分，再將深度相機點云轉換成偽激光雷達數據，最后融合偽激光雷達與激光雷達數據，將融合后的激光雷達數據發布，利用GMapping算法構建二維占用柵格地圖。通過實驗對比分析建圖效果，實驗結果表明，本文所提出的傳感器融合建圖方法在建圖時間上比使用深度相機平均建圖用時減少31.22%。本文方法在障礙物檢測率方面比激光雷達建圖提高13.24%，比深度相機建圖提高8.21%。但本文所提出的方法也存在不足，一是由于深度相機發布的點云數據在轉換成偽激光雷達數據時計算量較大，影響融合程序性能。二是由于兩種傳感器的數據發布頻率不同，導致建圖時間增加。下一步的工作將會利用多線程方法改進融合程序，減少數據處理時間。