基于ROS機(jī)器人的激光與視覺融合建圖方法*

姬 鵬，高帥軒

(河北工程大學(xué)機(jī)械與裝備工程學(xué)院，邯鄲 056038)

0 引言

同時定位和地圖構(gòu)建(SLAM)技術(shù)是指移動平臺在沒有環(huán)境先驗信息的情況下，在運(yùn)動過程中建立環(huán)境的模型，同時估計自己的運(yùn)動。SLAM技術(shù)是載有傳感器的移動平臺在未知環(huán)境中進(jìn)行偵查、導(dǎo)航的前提。在實際應(yīng)用中，單一傳感器會存在易受環(huán)境干擾、環(huán)境信息缺失、精度低等問題。而傳感器融合則可以綜合各個傳感器的優(yōu)勢，為機(jī)器人環(huán)境感知提供冗余信息，通過多傳感器融合技術(shù)可以提升移動機(jī)器人在復(fù)雜環(huán)境中的定位和建圖精度。

多傳感器信息融合作為一種可有效增強(qiáng)系統(tǒng)數(shù)據(jù)準(zhǔn)確性、提高數(shù)據(jù)利用率的信息處理技術(shù)，在機(jī)器人、醫(yī)學(xué)、目標(biāo)跟蹤、導(dǎo)航、圖像分析等方面獲得廣泛關(guān)注，因此國內(nèi)外專家學(xué)者已提出許多傳感器信息融合方法，MONTEMERLO、DOUCET等[1-2]使用基于粒子濾波器的SLAM方法，該算法近似得到機(jī)器人定位和建圖的概率密度，但方法會造成粒子種類減少，造成計算資源浪費(fèi)等問題，CASTELLANOS、莊嚴(yán)等[3-4]對激光雷達(dá)和單目相機(jī)融合SLAM進(jìn)行了研究，將機(jī)器人所處的未知環(huán)境中一些相關(guān)特征關(guān)聯(lián)作為地標(biāo)，在一定程度上增加了數(shù)據(jù)的準(zhǔn)確性，但計算量較大實時性較差。

本文針對單一傳感器構(gòu)建地圖精度低、易受環(huán)境干擾等問題，提出將深度圖像信息與單線激光雷達(dá)信息進(jìn)行數(shù)據(jù)融合，并基于貝葉斯理論構(gòu)建概率模型更新地圖，保證環(huán)境地圖信息的完整性和自主導(dǎo)航的實時性。

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

ROS機(jī)器人[5]系統(tǒng)由兩個相對獨立部分構(gòu)成，如圖1所示，這兩個部分采用串口通信的方式進(jìn)行雙向數(shù)據(jù)傳輸，STM32作為驅(qū)動部分與ROS主控進(jìn)行信息交互完成對運(yùn)動底盤目標(biāo)速度的控制，上層ROS主控部分為英偉達(dá)JESTON NANO微型計算機(jī)，可以通過wifi模塊接收本地計算機(jī)發(fā)出的控制命令，通過微型計算機(jī)Ubuntu系統(tǒng)下的ROS系統(tǒng)完成機(jī)器人運(yùn)動、建圖、導(dǎo)航等功能。

圖1 系統(tǒng)結(jié)構(gòu)圖

奧比中光Astra pro RGB-D相機(jī)如圖2所示，它的觀測深度范圍為0.6～8 m，水平探測范圍為58°，垂直探測范圍為45°，可獲得物體顏色信息和深度信息。思嵐單線激光雷達(dá)RPLIDAR A1如圖3所示，掃描范圍為360°、探測半徑為12 m，掃描頻率最高可達(dá)10 Hz。兩種傳感器的探測范圍如圖4所示，視覺傳感器可采集到環(huán)境中三維信息，相比較單線激光雷達(dá)而言空間信息較為豐富，但視角有限且易受光照影響，單線激光雷達(dá)掃描范圍較廣，但只能掃描自身所在平面環(huán)境信息，本文的研究意義在于融合兩種低成本傳感器的優(yōu)勢，使機(jī)器人可提供更加完整、準(zhǔn)確的環(huán)境信息。

圖2 Astra pro RGB-D相機(jī) 圖3 RPLIDAR A1激光雷達(dá)

圖4 激光雷達(dá)和視覺傳感器掃描范圍

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

2.1 聯(lián)合標(biāo)定

在進(jìn)行激光雷達(dá)和視覺傳感器信息融合前，要對二者進(jìn)行標(biāo)定，圖5中P點在激光雷達(dá)坐標(biāo)系中坐標(biāo)為PL，在相機(jī)坐標(biāo)系中為Pk，本文通過文獻(xiàn)[6]中所述標(biāo)定方法求得不同坐標(biāo)系間相互變換的旋轉(zhuǎn)矩陣R和平移矩陣T，則圖中P點在兩個坐標(biāo)系下的關(guān)系為：

Pk=RPL+T

(1)

圖5 激光雷達(dá)和相機(jī)坐標(biāo)系轉(zhuǎn)換

2.2 深度圖像轉(zhuǎn)激光數(shù)據(jù)

RGB-D相機(jī)采集的深度圖像中任一點都相對應(yīng)一個像素值，圖6中H表示物體A到視覺傳感器所在平面的垂直距離，R表示障礙物A到視覺傳感器的直線距離，α位障礙物與視覺傳感器水平夾角，可得：

H=depth[x][y]

(2)

(3)

式中，depth[x][y]表示處于深度圖像中第x行與第y列位置處像素點的信息值，為了融合二維激光雷達(dá)信息，需要將視覺傳感器采集的數(shù)據(jù)轉(zhuǎn)換為光線信息在二維平面投影使不同傳感器信息處于同一維度，步驟如下：

步驟1：將深度圖像中的p點映射到RGB-D相機(jī)坐標(biāo)系下相對應(yīng)的點P(x,y,z)。

步驟2：計算夾角OAB。

θ=arctan(X/Z)

(4)

步驟3：將圖中包含的所有深度信息關(guān)聯(lián)到激光雷達(dá)數(shù)據(jù)中，激光雷達(dá)傳感器探測范圍為0～360°，將雷達(dá)光束分為M份，則激光雷達(dá)信息可表示為laser[M]，點P信息索引號為m，可得m的值：

m=θ/(360/M)=θM/360

(5)

laser[m]的值為點P投影到x軸的點B到A的距離d，即：

(6)

圖6 深度圖幾何模型 圖7 深度圖轉(zhuǎn)激光數(shù)據(jù)原理圖

由于激光雷達(dá)信息類型是一個行向量，而視覺傳感器通過上述步驟得到的數(shù)據(jù)類型為二維矩陣，因此考慮用深度圖中每一列數(shù)據(jù)中的最小值替換此列較大的值，最小值即為障礙物到視覺傳感器的最近距離，從而將深度數(shù)據(jù)轉(zhuǎn)為激光數(shù)據(jù)。

2.3 貝葉斯法則

對激光雷達(dá)掃描到的場景信息和RGB-D攝像機(jī)三維轉(zhuǎn)化為二維的場景信息，應(yīng)用基于概率模型的貝葉斯推理方法進(jìn)行融合，該方法可根據(jù)已有向量Z，預(yù)測n維向量X，假設(shè)觀測Zk={zi,i=1,2,…,k}和此時概率xk，則先驗分布如下：

(7)

式中，p(zk|xk)為基于給定傳感器測量模型的似然函數(shù)；p(xk|Zk-1)為轉(zhuǎn)換系統(tǒng)模型的先驗分布；p(Zk|Zk-1)為歸一化概率密度函數(shù)。

(8)

(9)

2.4 改進(jìn)多傳感器融合地圖更新方法

(10)

圖8 激光雷達(dá)與視覺傳感器融合流程圖

根據(jù)上述方法，利用簡化之后的柵格單元占據(jù)概率取代原始柵格單元占據(jù)概率，對于兩個傳感器構(gòu)建的柵格地圖中同一處柵格點按表1融合規(guī)則進(jìn)行融合。

(11)

3 實驗論證

3.1 環(huán)境建圖

為驗證本文融合方法的有效性，將本文融合算法應(yīng)用于ROS機(jī)器人中進(jìn)行驗證，機(jī)器人平臺參數(shù)如表2所示，與機(jī)器人通訊的筆記本電腦在Ubuntu18.04下的ROS系統(tǒng)發(fā)布命令[7-10]，本文實驗數(shù)據(jù)均為室內(nèi)實驗數(shù)據(jù)，該實驗基于Gmapping建圖算法利用單一傳感器和多傳感器融合分別建圖，Gmapping算法在構(gòu)建室內(nèi)小場景地圖時計算量相對較小且準(zhǔn)確度較高、魯棒性高，該算法充分的采集里程計信息，為自身提供了先驗位姿。

表2 移動機(jī)器人硬件表

實驗場景如圖9所示，共包含5個形狀各異的障礙物，其中障礙物D高度為6 cm，低于單線激光雷達(dá)檢測平面，ROS機(jī)器人與障礙物D高度如圖10所示，在柵格地圖中黑色區(qū)域代表柵格存在障礙物，白色區(qū)域代表柵格不存在障礙物為可通行區(qū)域，圖11為單線激光雷達(dá)建圖結(jié)果，可以看出，激光雷達(dá)所掃描范圍較寬闊，環(huán)境建圖更為準(zhǔn)確，但由于障礙物D高度值低于單線激光雷達(dá)掃描平面，因此并未被檢測到。

圖9 實驗場景 圖10 雷達(dá)與障礙物高度

圖11 激光雷達(dá)環(huán)境建圖

RGB-D相機(jī)檢測環(huán)境三維建圖如圖12所示，三維點云信息較為稀疏[11]，視覺傳感器易受光照影響且視野范圍有限，但障礙物的重要特征信息并未丟失，這也是將視覺傳感器與激光雷達(dá)融合的依據(jù)。

本文通過融合激光雷達(dá)和相機(jī)兩個傳感器的環(huán)境信息建圖如圖13所示，相比于單傳感器所建地圖獲得更為完整的環(huán)境信息，更接近于真實環(huán)境。

圖12 視覺傳感器三維建圖 圖13 數(shù)據(jù)融合二維建圖

3.2 自主導(dǎo)航

以激光雷達(dá)所建地圖為先驗地圖進(jìn)行導(dǎo)航，設(shè)置導(dǎo)航起點為圖14中左下角1處，終點為右上角2處，圖14a中由于障礙物D不在攝像頭視角范圍內(nèi)，因此ROS機(jī)器人規(guī)劃的路線經(jīng)過障礙物D；圖14b圖中，隨著機(jī)器人的行進(jìn)，攝像機(jī)感知到障礙物D并對原始路徑重新進(jìn)行規(guī)劃；圖14c圖中，機(jī)器人移動到障礙物附近并成功繞開；圖14d圖中，所有障礙物信息都被檢測到，地圖得到更新，機(jī)器人成功到達(dá)預(yù)設(shè)的終點。

(a) 初始路徑 (b) 重新規(guī)劃后路徑

(c) 繞開障礙物 (d) 到達(dá)終點

4 結(jié)束語

本文針對單一傳感器在SLAM中存在建圖精度低，易受干擾等問題，提出了一種多傳感器數(shù)據(jù)融合方法，該方法將視覺傳感器采集到的3D點云信息轉(zhuǎn)化為2D激光雷達(dá)信息與激光雷達(dá)采集到的信息按一定規(guī)則融合，以彌補(bǔ)單線激光雷達(dá)丟失的較矮障礙物信息。實驗結(jié)果表明，利用多傳感器感知的冗余信息進(jìn)行融合，構(gòu)建了魯棒性更高、精度更高的2D柵格地圖，反映出了真實的環(huán)境地圖，有效的提高了機(jī)器人導(dǎo)航的準(zhǔn)確性，多傳感器融合建圖對機(jī)器人準(zhǔn)確感知復(fù)雜環(huán)境、導(dǎo)航、避障具有重要意義。