融合激光雷達和RGB-D相機建圖

李少偉,鐘勇,楊華山,張樹,范周慧

(福建省汽車電子與電驅動技術重點實驗室,福建 福州 350118)

同時定位和地圖構建(simultaneous localization and mapping, SLAM)[1]技術是指在沒有環境先驗信息的情況下,移動平臺在運動過程中建立環境模型并估計自身的運動。SLAM技術是搭載傳感器的移動平臺在未知環境中進行偵查和導航的前提。

經過多年的研究,激光SLAM和視覺SLAM接近成熟[2-3]。國內外學者開源了多種SLAM方法,包括基于激光雷達的Gmapping[4]和Cartographer[5],以及基于視覺相機的RGB-D-SLAM[6]和ORB-SLAM[7]等。然而,在實際應用中,單一傳感器容易受到環境干擾,導致環境信息缺失和精度下降等。為了克服這些缺點,學者們開始探索激光雷達和深度相機的融合。通過多傳感器融合技術,提高移動機器人在復雜環境中的定位和建圖精度。陳文佑等[8]提出了一種激光與相機局部地圖融合構建全局地圖的策略,近似地獲取機器人定位和建圖的概率密度,但卻導致粒子數量減少,從而浪費計算資源。晏小彬等[9]用單目相機作為輔助進行激光SLAM。Xu等[11]提出了一種基于擴展卡爾曼濾波器原理的RGB-D相機與激光雷達融合的SLAM方法。該方法在相機匹配失敗時,通過將激光雷達數據補充到相機數據中并生成地圖。白崇岳等[12]使用擴展卡爾曼濾波算法將激光雷達、慣性測量單元(IMU)和光電編碼器融合定位。盧俊鑫等[13]提出一種新的RGB-D視覺里程計方法,結合了點線的優點。

本文采用激光雷達和RGB-D相機在數據層次上融合的方法,基于RTABMAP算法提供里程計信息,進行回環檢測和全局優化,解決了單一傳感器構建地圖精度低、易受環境干擾等問題,保證環境地圖信息的完整性。

1 系統整體結構與傳感器模型

如圖1所示,智能小車的主要控制器是樹莓派。在電腦上,同樣安裝了與樹莓派相同版本的機器人操作系統,并通過WIFI連接到樹莓派的熱點。通過SSH建立遠程登錄,實現遠程發布指令。STM32主板負責接收里程計、IMU和電池電壓等信息,并將其傳輸給樹莓派ROS主控。樹莓派ROS主控將運動底盤的目標速度等指令發送給STM32主板。此外,樹莓派還通過USB與激光雷達和RGB-D相機連接。把接收到的數據信息經過處理、整合,通過相關算法完成智能小車的運動、建圖等功能。

圖1 系統結構圖Fig.1 System structure diagram

采用奧比中光RGB-D相機獲取深度圖像,參數如表1所示。采用思嵐二維激光雷達RPLIDAR A1,參數如表2所示。

表1 奧比中光RGB-D相機參數Tab.1 Parameters of ORBBEC RGB-D camera

表2 RPLIDAR A1激光雷達參數Tab.2 RPLIDAR A1 LiDAR parameters

2 傳感器數據融合方法

2.1 RGB-D相機標定

使用張正友標定法,通過相機標定實驗,獲取上述相機的內、外部參數。如圖2所示,準備一張棋盤格,將其貼在表面光滑的板子上作為標定板。使用RGB-D相機和紅外相機分別對棋盤格標定板拍照。兩種相機的標定步驟相同。標定實驗需要拍攝20組照片,以獲得更準確的結果。

圖2 棋盤格標定板Fig.2 Checker board calibration board

標定后的總體誤差像素在正常范圍內,因此標定結果較為準確。通過RGB-D相機標定,獲得RGB-D相機和紅外相機的內參及畸變系數,如表3所示。

表3 奧比中光相機標定結果Tab.3 ORBBEC camera calibration results

2.2 相機與激光雷達聯合標定

2.2.1 相機建模

常用的是針孔相機模型。假設光線從物體通過針孔進入相機成像,o-x-y-z為相機坐標系。點P在世界坐標系中的坐標為(XW,YW,ZW),點P在像素坐標系下投影的坐標為(XC,YC,ZC), 經過世界坐標系到相機坐標系下的變換得:

(1)

式中,M為外參矩陣,R為旋轉矩陣,t為平移向量。

根據相似三角形關系可得:

(2)

式中,負號表示倒立的像,f為相機的焦距,X′和Y′分別為成像點的坐標。

將成像平面置于三維空間點同一邊,可將式(2)簡化為:

(3)

即:

(4)

將式(4)寫成矩陣形式表達式為:

(5)

根據針孔相機模型,假如圖像物理坐標系的原點O′在像素坐標系中為坐標(u0,v0),令dx、dy分別為像素在X′和Y′方向上的實際物理尺寸大小,則:

(6)

將式(6)用矩陣形式表示:

(7)

聯立式(5)和(7),XC、YC用X′和Y′表示可得:

(8)

聯立式(1)和(8),XW、YW表示XC、YC,且令fx=f/dx,fy=f/dy,則可得:

(9)

式中,fx、fy、u0和v0為相機內參。

若激光雷達掃描空間中的點P距離激光雷達為d,激光雷達旋轉角度θ時,則點P在激光雷達坐標系中的坐標為:

(10)

該點在世界坐標系下可以表示為:

(11)

點P在相機和激光雷達坐標系中的坐標都表示該點的位置,則由世界坐標系中的式(1)和式(7)的關系可得:

(12)

聯立公式(9)可得,點P在激光雷達坐標系和相機坐標系下的對應關系為:

(13)

2.2.2 內參數據

通過對相機的標定實驗得到內參數據。使用得到的相機內參啟動相機激光雷達聯合標定程序,接著啟動激光雷達節點,使用打開的rviz可視化圖形程序觀察激光雷達掃描的數據,選定相機和激光雷達匯總的點進行標定,直到滿足要求的點標定完畢。將標定結果存儲在yaml文件里。聯合標定結果如式(14 )矩陣所示。

(14)

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

只使用激光雷達傳感器構建的地圖并不能準確反映環境特征,因為激光雷達主要提供距離信息而缺乏顏色和紋理等視覺特征。相比之下,RGB-D相機傳感器可以直接感受到深度信息,并且能夠提供更豐富的點云地圖,從而更準確地反映環境的特征。然而,相機的視野較小,測量范圍有限,并且容易受到光照條件的影響。因此,通過將多個傳感器融合可以顯著提高整個傳感系統的精度和魯棒性。本文使用RTABMAP算法實現融合視覺圖像和激光SLAM建圖。

2.3.1 RTABMAP系統框架

RTABMAP系統框架的設計如圖3所示。將視覺傳感器、激光傳感器和機器人底盤等通過tf關系輸入系統。經過同步模塊進行時間戳對齊,以確保數據的一致性。RTABMAP系統采用圖結構來組織地圖,圖結構由節點和節點之間的連接邊組成。經過同步后的傳感器數據將被存儲到短期內存(short-term memory,STM)模塊中。對于每一幀傳感器數據,系統會創建一個節點,并將該節點中包含的信息存儲其中。這些信息包括該幀對應的里程計位姿、視覺和激光的觀測數據,以及從該幀中提取出的視覺單詞和局部地圖等有用信息。

圖3 RTABMAP系統框架Fig.3 RTABMAP system framework

2.3.2 內存管理機制

RTABMAP采用圖結構組織全圖,每采集一幀傳感器數據創建一個節點存儲相關數據,當所建的圖規模很大時,創建的節點數量也很大,如果只在全部節點上閉環檢測和全局優化就會影響到實時性,RTABMAP引入內存管理機制,以分級管理這些節點。內存管理機制將地圖中的節點分成3個級別: STM用于存儲局部地圖的節點;工作內存(working memory,WM)用于存儲全局地圖的節點;長期內存(long-term memory,LTM)用于存儲暫時與全局地圖無關或不重要的節點。

內存管理機制的具體過程如圖4所示。傳感器數據經過傳感器內存(sensor memory,SM)模塊進行預處理,包括數據維度的精簡和特征提取,計算出當前的位姿信息,處理后的數據被用來創建新的節點加入STM中。如果新節點與前一個剛加入STM的節點非常相似,那么可以利用權重更新方法將這兩個節點融合在一起。

圖4 內存管理機制Fig.4 Memory management mechanism

2.3.3 閉環檢測與圖優化

當STM創建一個新的節點時,可利用深度圖像、激光掃描數據、點云數據等來生成對應的局部地圖。如果選擇生成三維地圖,可以直接用三維點云創建三維局部地圖,也可將三維點云經過三維光束模型處理創建三維局部地圖。

通過一個獨立的ROS節點提供RTABMAP所需的里程計信息。由于局部建圖所依賴的里程計存在累積誤差,因此需要進行回環檢測和全局優化。在RTABMAP中,回環檢測包括視覺閉環檢測和激光相似檢測,而全局優化則采用位姿圖優化方法,視覺閉環檢測基于視覺詞袋模型和貝葉斯濾波器。視覺詞袋模型能夠快速計算當前位姿節點與候選回環節點之間的相似度。它通過提取圖像特征并構建詞袋表示,實現高效的圖像匹配和相似度計算。貝葉斯濾波器用于維護候選回環節點相似度的概率分布,通過更新概率分布來選擇最可能的回環節點。視覺閉環檢測能有效地檢測到不同時刻和位置下的相似場景,從而解決里程計累積誤差的問題。

2.3.4 全局地圖集成

局部地圖在機器人自身坐標系下構建,而全局地圖在世界坐標系下構建。這兩者之間通過機器人坐標系到世界坐標系的變換關系進行轉換。算法主節點RTAB-Map利用閉環檢測和全局優化來維護機器人的全局位姿估計,并通過發布機器人坐標系和世界坐標系之間的變換關系來實現轉換使用。

使用RGB-D深度傳感器和激光雷達對信息采集和建圖是RTABMAP建圖的關鍵步驟。在RTABMAP中,當每個新的特征節點被添加到地圖中時,新生成的局部地圖將與已構建的全局地圖進行融合。這個過程基于各個節點的里程計位姿,將各個局部地圖拼接起來形成一張全局地圖。

3 實驗驗證

3.1 環境建圖

搭建一臺室內智能小車,該智能小車具有長達8 h的單次續航時間,系統整體架構組成:阿克曼轉向系統、驅動電機、樹莓派4B、STM32主板、激光雷達和慣性測量單元(IMU)。選擇的場景長約10 m,寬約8 m,具有明顯的空曠區域、障礙區域和線條清晰的邊界,同時具備閉環的特性,能直觀地對比建圖效果。

激光雷達建圖結果如圖5所示,啟動小車環繞周圍環境一圈后形成了柵格地圖,白色部分代表智能避障車已經掃描到的無障礙區域,即可以到達的區域;黑色部分表示存在障礙物的區域,可以觀察到激光雷達所掃描的范圍較廣,建立的環境地圖接近真實環境,且地圖的完整性和精確度較高。

圖5 激光雷達環境建圖Fig.5 Lidar environment mapping

RGB-D相機檢測環境三維建圖如圖6所示,啟動小車和建圖程序,初始建圖效果如圖6(a)所示,控制小車繞周圍場景一圈,最終的建圖效果如圖6(b)所示。整體的建圖效果較為準確,雖然視覺傳感器易受光照影響且視野范圍有限,但障礙物的重要特征信息并未丟失,這也是將視覺傳感器與激光雷達融合的依據。

圖6 視覺傳感器三維建圖Fig.6 3D construction of visual sensors

通過融合激光雷達和相機兩個傳感器進行環境建圖,實驗結果如圖7(a)和(b)所示。結果表明,通過準確投影三維空間點到二維平面并處理深度相機數據后,地圖更新能夠與激光雷達構建的地圖保持一致,同時能夠較為完整和清晰地反映實驗場景。這說明融合視覺和激光雷達的建圖方法具有良好的效果。相比于單一傳感器建立的地圖,融合建圖方法能夠獲取更為完整的環境信息,更接近真實環境。

圖7 融合激光雷達和視覺建圖Fig.7 Fusion of LiDAR and visual mapping

用激光雷達與RGB-D相機融合計算建圖時間、障礙物檢測率和地圖的尺寸精度及角度精度,計算結果如下。

(1)建圖時間和障礙物檢測率

建圖時間為10次建圖所用的平均時間。假設障礙物檢測率為p,則

p=C1/C2

(15)

式(15)中,C1為融合傳感器建圖所檢測到的障礙物總邊長,m;C2為實際圖中障礙物總邊長,m。

建圖時間和障礙物檢測率如表4所示。

表4 障礙物檢測率和建圖時間Tab.4 Obstacle detection rate and mapping time

(2)地圖尺寸精度及角度精度

選取圖中3條線段和兩個角度進行測量和誤差分析,結果如表5所示。

表5 測量結果及誤差分析Tab.5 Measurement results and error analysis

由表4、表5可見,激光雷達與RGB-D相機融合建圖方法的障礙物檢測率為94.31%,比僅使用激光建圖高出30.75%,比僅使用RGB-D相機建圖高出18.63%。此外,在地圖尺寸精度方面,平均絕對誤差比僅使用激光建圖小了0.013 m,比僅使用RGB-D相機建圖小了0.150 m。在角度精度方面,平均絕對誤差比僅使用激光建圖小了1°,比僅使用RGB-D相機建圖小了3°。綜上,激光與RGB-D相機融合建圖方法在多個方面都表現出更高的性能和更好的精度,為機器人在復雜環境中的導航和感知任務提供了更可靠的支持。

4 結論

1)提出一種RTABMAP算法,將視覺傳感器與激光雷達采集到的信息按照一定的規則融合,以彌補單一傳感器不能準確構圖的問題。

2)通過利用多傳感器感知的冗余信息進行融合,構建了一張魯棒性更高、精度更高的3D柵格地圖。

3)將激光雷達與RGB-D相機融合建圖,障礙物的檢測率比單獨使用激光雷達建圖和RGB-D相機建圖分別提高了30.75%和18.63%。地圖尺寸平均絕對誤差相比單獨使用激光雷達建圖和RGB-D相機建圖,分別減少了0.013 m和0.150 m。地圖的角度平均絕對誤差比激光雷達建圖的小1°,比RGB-D相機建圖小3°。