基于立體視覺的2D 占據(jù)網(wǎng)格地圖構(gòu)建

王永祥，常 青，黃良紅，孫永明

（1.太原理工大學(xué)信息與計(jì)算機(jī)學(xué)院，山西晉中 030600；2.民航山西空管分局，山西太原 030031；3.山西省林業(yè)科學(xué)研究院，山西太原 030000）

科學(xué)技術(shù)的快速發(fā)展，對機(jī)器人的要求也越來越高，機(jī)器人不僅可以代替重復(fù)的工作，還可以通過傳感器采集數(shù)據(jù)，判斷決策。當(dāng)在未知環(huán)境中工作時(shí)，就必須對自身進(jìn)行定位，并且對當(dāng)前所處環(huán)境建圖。機(jī)器人同時(shí)定位與建圖(Simultaneous Localization and Mapping，SLAM)技術(shù)可以使機(jī)器人得到未知環(huán)境的信息。機(jī)器人在未知環(huán)境中通過傳感器測量得到自身在環(huán)境中所處的位置和周圍的環(huán)境信息[1-3]。當(dāng)前，根據(jù)傳感器的不同，SLAM 算法大致可分為視覺SLAM[4]與激光SLAM[5]，采用激光SLAM 算法構(gòu)建2D 占據(jù)網(wǎng)格地圖相對成熟，但是激光測距儀價(jià)格昂貴，很難適用于低成本設(shè)備。相機(jī)成本低，而且采集的視頻數(shù)據(jù)描述的信息豐富。視覺SLAM 算法只能輸出稀疏點(diǎn)云地圖，比較典型的方案有DSO[6]、ORBSLAM2[7-8]、VINS-mono[9]、LSD-SLAM[10]等，但這些開源算法并不適用于機(jī)器人進(jìn)行路徑規(guī)劃、導(dǎo)航任務(wù)。所以要建立可用于機(jī)器人導(dǎo)航避障的2D 占據(jù)網(wǎng)格地圖[11-13]。

文獻(xiàn)[14]中將RGB-D 相機(jī)的深度圖像轉(zhuǎn)換為laser 數(shù)據(jù)，來代替昂貴的激光傳感器構(gòu)建網(wǎng)格地圖，但是RGB-D 相機(jī)在室外使用時(shí)易受環(huán)境光影響[14]。文獻(xiàn)[15]中提出了一種方法來建立單目相機(jī)的逆?zhèn)鞲衅?Inverse Sensor Model，ISM)概率模型。評估了傳感器精度和網(wǎng)格分辨率變化對逆?zhèn)鞲衅髂Ｐ偷挠绊懀]有通過占據(jù)概率構(gòu)建可用于導(dǎo)航的網(wǎng)格地圖[15]。文獻(xiàn)[16]中提出了一種基于LSD-SLAM 的實(shí)時(shí)占據(jù)網(wǎng)格建圖，采用單目相機(jī)采集圖像，利用點(diǎn)云地圖投影的方式生成網(wǎng)格地圖[16]。

1 基于立體視覺的逆?zhèn)鞲衅髂Ｐ?/h2>

1.1 雙目立體視覺幾何模型

雙目相機(jī)類似人的眼睛，由左右兩個(gè)相機(jī)組成，兩個(gè)相機(jī)的光圈中心都位于x軸上，兩個(gè)光圈中心的連線為雙目相機(jī)的基線，它在相機(jī)出廠后就是固定的，真實(shí)尺度可根據(jù)這個(gè)基線計(jì)算，是雙目相機(jī)的重要參數(shù)，立體視覺幾何模型如圖1 所示[17-19]。

圖1 立體視覺幾何模型

其中，f為焦距，b為基線，uL為左像素點(diǎn)的橫坐標(biāo)，uR為右像素點(diǎn)橫坐標(biāo)，d為左右像素點(diǎn)的橫坐標(biāo)之差，稱為視差，z即為通過視差值計(jì)算出的測量距離。

1.2 雙目立體視覺的逆?zhèn)鞲衅髂Ｐ?/h3>

逆?zhèn)鞲衅髂Ｐ兔枋隽藗鞲衅鲾?shù)據(jù)到地圖的轉(zhuǎn)換，傳感器到障礙物之間所有的網(wǎng)格單元都需要填充一個(gè)概率值，通過這個(gè)概率得知這些單元網(wǎng)格是占據(jù)還是空閑狀態(tài)的可能性，而沒有被傳感器探測到的單元網(wǎng)格則被描述為未知狀態(tài)。單元網(wǎng)格的概率狀態(tài)分布有助于在地圖中引入噪聲和不確定度。傳感器測量的噪聲與不確定度通常會(huì)影響測量結(jié)果，所以，在設(shè)計(jì)逆?zhèn)鞲衅髂Ｐ偷臅r(shí)候，需要將噪聲與不確定度考慮進(jìn)來。

深度圖像的每個(gè)像素到物體的距離都被認(rèn)為是逆?zhèn)鞲衅髂Ｐ偷臏y量距離。假設(shè)空間中的一點(diǎn)P在相機(jī)坐標(biāo)系下的坐標(biāo)為Pc=(xc,yc,zc)T，測量距離lP定義如下：

理想狀態(tài)下的傳感器模型為：

Pmin是單元網(wǎng)格中的初始概率，為了減少不確定度的影響，在此引入了一個(gè)二次指數(shù)函數(shù)來估計(jì)測量距離。

那么實(shí)際環(huán)境中雙目視覺系統(tǒng)單次測量單元網(wǎng)格地圖的占據(jù)概率為：

顯著性因子k表明了單次測量的權(quán)重，它確保遠(yuǎn)處的測量結(jié)果影響小，近處的測量結(jié)果影響大，ΔlP為深度相關(guān)不確定度，lP為測量距離，r表示自變量。

1.3 逆?zhèn)鞲衅髂Ｐ蛥?shù)討論

1.3.1 顯著性因子k對ISM的影響

在討論k值影響的時(shí)候，采用控制變量的方法，設(shè)定測量距離z=3 m，深度相關(guān)不確定度ΔlP=0.1。通過改變k值來分析ISM 的變化，如圖2 所示。

圖2 顯著性因子k對ISM的影響

由圖2 可知，顯著性因子k值越大，測量結(jié)果的概率就越大，k值越小，測量結(jié)果的概率就越小。它描述了單次測量的重要性，所以選擇一個(gè)合適的k值對于傳感器模型的好壞至關(guān)重要。經(jīng)過實(shí)驗(yàn)分析，當(dāng)k=0.1 時(shí)，逆?zhèn)鞲衅髂Ｐ偷男Ч^好。

1.3.2 深度相關(guān)不確定度ΔlP對ISM的影響

在討論深度相關(guān)不確定度對ISM 模型的影響時(shí)，同樣采取控制變量的方法，顯著性因子k=0.1，測量距離z=3 m，通過選取不同的ΔlP分析ISM 的性能，如圖3 所示。

圖3 深度相關(guān)不確定度ΔlP對ISM的影響

由圖3 可知深度相關(guān)不確定度ΔlP越高，波峰越低，同時(shí)波峰也越寬，而不確定度ΔlP越低，波峰越高，同時(shí)波峰也越窄。

1.3.3 測量距離z對ISM的影響

由于測量的距離不同，就會(huì)導(dǎo)致測量不確定度不同，那么測量結(jié)果也就不同，由于z的變化導(dǎo)致了ΔlP的變化，選取k=0.1，通過z與ΔlP之間的變換關(guān)系計(jì)算出ΔlP，如圖4 所示。

圖4 測量距離z對ISM的影響

由圖4 可知，測量值越小被占用的可能性就越高，并且每次測量的概率都會(huì)隨著距離的增大而減小，直到到達(dá)最大的距離為止，此時(shí)單元格變得未知。

2 2D網(wǎng)格地圖模型

占據(jù)網(wǎng)格地圖的目的是將環(huán)境分成尺寸大小均勻的單元，這些單元存儲(chǔ)著環(huán)境的相關(guān)信息，通過這些信息來判斷當(dāng)前環(huán)境狀況，環(huán)境有3 種狀態(tài)：占據(jù)、空閑、未知。

通過傳感器的測量值z以及從開始到t時(shí)刻的姿態(tài)x，就可以計(jì)算地圖m的后驗(yàn)概率，可以表示為p(m|z1:t,x1:t)。

假設(shè)當(dāng)前處于世界坐標(biāo)系下，同時(shí)傳感器測量值與機(jī)器人姿態(tài)x相關(guān)，那么將姿態(tài)合并到測量值中去，則地圖m的后驗(yàn)概率又可以表示為p(m|z1:t)。

如果地圖中的單元網(wǎng)格數(shù)量很多，則地圖就會(huì)有很多種可能，會(huì)導(dǎo)致地圖很復(fù)雜，所以就需要研究每一個(gè)單元網(wǎng)格的后驗(yàn)分布，地圖m被分成相同尺寸的單元網(wǎng)格，將第i個(gè)單元網(wǎng)格的占據(jù)變量記為mi，p(mi=1)表示第i個(gè)單元網(wǎng)格被占據(jù)的概率，p(mi=0)表示第i個(gè)單元網(wǎng)格空閑，第i個(gè)單元網(wǎng)格的后驗(yàn)分布表示為p(mi|z1:t)。

假設(shè)地圖單元網(wǎng)格之間是相互獨(dú)立的，就可以將地圖m表示為每一個(gè)單元網(wǎng)格后驗(yàn)分布的乘積：

當(dāng)傳感器有了新的測量結(jié)果時(shí)，地圖中的一部分單元網(wǎng)格的占據(jù)概率發(fā)生變化，就需要對地圖進(jìn)行更新。由于第i個(gè)單元網(wǎng)格不同時(shí)刻的傳感器測量值z條件獨(dú)立，故采用二進(jìn)制貝葉斯濾波器來解決二進(jìn)制狀態(tài)表示的靜態(tài)環(huán)境的占據(jù)概率，通過貝葉斯法可得：

同理可得第i個(gè)單元網(wǎng)格空閑時(shí)的概率為，與式（6）作商可得：

為了避免數(shù)值上的不穩(wěn)定，將占據(jù)概率的值穩(wěn)定地限制在0～1 之間，采用log-odd 率來表示占據(jù)概率，如下所示：

則式（7）可寫為:

分析式（9）可知，L(mi|z1:t)代表的是地圖更新后的第i個(gè)單元網(wǎng)格的占據(jù)概率，L(mi|zt)是通過傳感器測量值得到的t時(shí)刻的概率，該概率可以通過ISM 傳感器模型確定L(mi|z1:t-1)為前一時(shí)刻的占據(jù)概率，L(mi)是地圖的先驗(yàn)信息。

3 基于ORB-SLAM2算法構(gòu)建2D網(wǎng)格地圖

3.1 ORB-SLAM2算法

ORB-SLAM2 算法是一個(gè)基于特征點(diǎn)匹配的實(shí)時(shí)SLAM 系統(tǒng)，室內(nèi)外都可以使用，支持單目、雙目以及RGB-D 相機(jī)，該系統(tǒng)分為跟蹤、建圖、重定位、閉環(huán)檢測4 個(gè)部分。該系統(tǒng)能夠?qū)崟r(shí)計(jì)算出相機(jī)的軌跡，同時(shí)生成稀疏的點(diǎn)云地圖。

3.2 2D網(wǎng)格地圖的構(gòu)建

采用ORB-SLAM2 作為視覺里程計(jì)，但是ORBSLAM2 只能生成稀疏點(diǎn)云地圖，該地圖并不能滿足機(jī)器人對障礙物判斷的需求，所以為了達(dá)到機(jī)器人避障的目的，需要提供豐富的環(huán)境信息，就需要得到一個(gè)2D 網(wǎng)格地圖，可以利用ORB-SLAM2 作為一個(gè)視覺里程計(jì)，提供當(dāng)前位姿。此外還需要在此位姿狀態(tài)下的環(huán)境信息，利用雙目相機(jī)提供深度圖，來描述此位姿下的深度信息。而雙目相機(jī)提供的深度圖不能直接使用，需通過ROS 功能包DepthToScan 將雙目相機(jī)提供的深度圖轉(zhuǎn)化為ROS平臺(tái)下可使用的2D laser scan 格式的深度信息。每個(gè)關(guān)鍵幀對應(yīng)一個(gè)當(dāng)前位姿下深度圖轉(zhuǎn)化的2D laser scan 深度信息。

采用的雙目相機(jī)為MYNTEYE s1030-IR，該相機(jī)通過一個(gè)ROS 節(jié)點(diǎn)發(fā)布左目、右目圖像與深度圖像等主題，ORB-SLAM2 算法將MYNTEYE 相機(jī)發(fā)布的主題作為ORB-SLAM2 系統(tǒng)的輸入，輸出當(dāng)前相機(jī)的位姿x。該方案直接利用MYNTEYE 相機(jī)發(fā)布的深度圖像，深度圖的計(jì)算是通過相機(jī)內(nèi)部計(jì)算的，該方案并沒有再次通過視差圖來計(jì)算深度圖，這樣大大減少了因計(jì)算深度圖而消耗的時(shí)間，得到的深度圖通過DepthToScan ROS 功能包轉(zhuǎn)化為Scan 格式的數(shù)據(jù)。ISM 逆?zhèn)鞲衅髂Ｐ屯ㄟ^相機(jī)位姿x與Scan深度數(shù)據(jù)，計(jì)算得到占據(jù)概率，并通過ROS 平臺(tái)下Occupancy Grid Map 構(gòu)建2D 網(wǎng)格地圖，算法流程如圖5 所示。

圖5 基于ORB-SLAM2算法構(gòu)建2D網(wǎng)格地圖算法流程圖

4 實(shí)驗(yàn)分析

4.1 室內(nèi)環(huán)境實(shí)驗(yàn)

室內(nèi)實(shí)驗(yàn)場景選擇在實(shí)驗(yàn)室，如圖6 所示。

圖6 室內(nèi)場景圖

通過ORB-SLAM2 算法構(gòu)建了實(shí)驗(yàn)室的稀疏點(diǎn)云地圖與相機(jī)的運(yùn)動(dòng)軌跡，如圖7 所示，可以看出，該地圖并不適應(yīng)于機(jī)器人導(dǎo)航。

圖7 ORB-SLAM2點(diǎn)云地圖

通過文中算法構(gòu)建該場景下的2D 網(wǎng)格地圖，可以看判斷出障礙物的位置，有利于機(jī)器人的導(dǎo)航，如圖8 所示。

圖8 室內(nèi)2D網(wǎng)格地圖

4.2 室外環(huán)境實(shí)驗(yàn)

室外實(shí)驗(yàn)場景選取兩邊有樹的公園小路，如圖9所示。

圖9 室外場景圖

該場景下的稀疏點(diǎn)云地圖以及運(yùn)動(dòng)軌跡如圖10所示。

圖10 ORB-SLAM2點(diǎn)云地圖

通過文中算法構(gòu)建該場景下的2D 網(wǎng)格地圖，并在ROS 中的RVIZ 可視化平臺(tái)下顯示，可以判斷出小路兩邊的障礙物被標(biāo)記了出來，適用于機(jī)器人判斷障礙物，如圖11 所示。

圖11 室外2D網(wǎng)格地圖

4.3 對比分析實(shí)驗(yàn)

將文中建圖算法與RTAB-map 建圖算法選擇室外環(huán)境2D 網(wǎng)格地圖與真實(shí)環(huán)境進(jìn)行對比，通過ROS平臺(tái)下的RVIZ 可視化界面中的測量工具對2D 地圖的3 處位置進(jìn)行測量，并與真實(shí)環(huán)境下同樣的位置進(jìn)行對比，如圖12 與圖13 所示。

圖12 室外2D網(wǎng)格地圖距離測量

圖13 RTAB-map網(wǎng)格地圖

以真實(shí)環(huán)境作為參考，將測量結(jié)果進(jìn)行對比，如表1 所示。

表1 2D網(wǎng)格地圖與真實(shí)環(huán)境對比

由表1 可知，文中算法相比于RTAB-map 算法，精度提高了3%左右，而且測量值都比實(shí)際值小，表明機(jī)器人不會(huì)觸碰到障礙物。所以此地圖可用于室外機(jī)器人導(dǎo)航。

5 結(jié)束語

該文在ORB-SLAM2 算法的基礎(chǔ)上采用MYNTEYE s1030-IR 相機(jī)構(gòu)建可用于室外機(jī)器人導(dǎo)航的2D 網(wǎng)格地圖，采用了適用于雙目相機(jī)的ISM 模型，詳細(xì)描述了地圖的構(gòu)建過程。通過與真實(shí)環(huán)境對比，所建地圖的精度滿足機(jī)器人的導(dǎo)航、避障。