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

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

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

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

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

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

1 系統(tǒng)整體結(jié)構(gòu)與傳感器模型

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

圖1 系統(tǒng)結(jié)構(gòu)圖Fig.1 System structure diagram

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

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

表2 RPLIDAR A1激光雷達參數(shù)Tab.2 RPLIDAR A1 LiDAR parameters

2 傳感器數(shù)據(jù)融合方法

2.1 RGB-D相機標定

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

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

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

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

2.2 相機與激光雷達聯(lián)合標定

2.2.1 相機建模

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

(1)

式中,M為外參矩陣,R為旋轉(zhuǎn)矩陣,t為平移向量。

根據(jù)相似三角形關(guān)系可得:

(2)

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

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

(3)

即:

(4)

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

(5)

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

(6)

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

(7)

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

(8)

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

(9)

式中,fx、fy、u0和v0為相機內(nèi)參。

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

(10)

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

(11)

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

(12)

聯(lián)立公式(9)可得,點P在激光雷達坐標系和相機坐標系下的對應(yīng)關(guān)系為:

(13)

2.2.2 內(nèi)參數(shù)據(jù)

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

(14)

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

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

2.3.1 RTABMAP系統(tǒng)框架

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

圖3 RTABMAP系統(tǒng)框架Fig.3 RTABMAP system framework

2.3.2 內(nèi)存管理機制

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

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

圖4 內(nèi)存管理機制Fig.4 Memory management mechanism

2.3.3 閉環(huán)檢測與圖優(yōu)化

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

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

2.3.4 全局地圖集成

局部地圖在機器人自身坐標系下構(gòu)建,而全局地圖在世界坐標系下構(gòu)建。這兩者之間通過機器人坐標系到世界坐標系的變換關(guān)系進行轉(zhuǎn)換。算法主節(jié)點RTAB-Map利用閉環(huán)檢測和全局優(yōu)化來維護機器人的全局位姿估計,并通過發(fā)布機器人坐標系和世界坐標系之間的變換關(guān)系來實現(xiàn)轉(zhuǎn)換使用。

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

3 實驗驗證

3.1 環(huán)境建圖

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

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

圖5 激光雷達環(huán)境建圖Fig.5 Lidar environment mapping

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

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

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

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

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

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

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

p=C1/C2

(15)

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

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

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

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

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

表5 測量結(jié)果及誤差分析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相機融合建圖方法在多個方面都表現(xiàn)出更高的性能和更好的精度,為機器人在復(fù)雜環(huán)境中的導(dǎo)航和感知任務(wù)提供了更可靠的支持。

4 結(jié)論

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

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

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