煤礦場(chǎng)景下基于RGBD 的視覺(jué)導(dǎo)航技術(shù)

王 鵬，趙紅菊

（1.國(guó)家能源集團(tuán)神東煤炭集團(tuán)有限公司，陜西 榆林 719135；2.中煤科工集團(tuán)沈陽(yáng)研究院有限公司，遼寧 撫順 113122；3.煤礦安全技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 撫順 113122；4.中煤科工機(jī)器人科技有限公司，廣東 深圳 518054）

當(dāng)前，對(duì)于煤礦場(chǎng)景應(yīng)用機(jī)器人而言，具備一定環(huán)境信息采集功能的巡檢類(lèi)機(jī)器人大致分2 種：軌道式巡檢機(jī)器人和輪式巡檢機(jī)器人[1-4]。相較于軌道式巡檢機(jī)器人的軌道鋪設(shè)困難、巡檢空間受限等問(wèn)題，輪式巡檢機(jī)器人則具有應(yīng)用范圍廣、地面適應(yīng)性好等特點(diǎn)而成為煤礦巡檢類(lèi)機(jī)器人的首選。

自主導(dǎo)航功能是輪式巡檢機(jī)器人智能化的重要特征之一。由于煤礦井下場(chǎng)景具有低光照、大粉塵等復(fù)雜的半結(jié)構(gòu)化環(huán)境特征，進(jìn)而現(xiàn)有普通工業(yè)機(jī)器人的自主導(dǎo)航算法難以在實(shí)際應(yīng)用中取得良好的展示，因此，隨著近年來(lái)煤礦機(jī)器人的興起，越來(lái)越多的科研院所開(kāi)始逐步探索煤礦機(jī)器人的自主導(dǎo)航研究[5-6]。目前，已經(jīng)在煤礦機(jī)器人自行走功能取得一定成績(jī)的導(dǎo)航技術(shù)有磁釘+慣性導(dǎo)航[7-8]、二維碼視覺(jué)導(dǎo)航[9]和激光雷達(dá)導(dǎo)航[10]。磁釘+慣性導(dǎo)航受限于煤礦場(chǎng)所的施工改造、短距離定位精準(zhǔn)長(zhǎng)距離累積誤差大；同樣的二維碼視覺(jué)導(dǎo)航也是需要現(xiàn)場(chǎng)場(chǎng)地改造，增加施工及維修陳本；激光雷達(dá)導(dǎo)航則在巷道場(chǎng)景特征點(diǎn)少時(shí)，低線束雷達(dá)建圖稀疏、高線束雷達(dá)成本昂貴；與前面導(dǎo)航技術(shù)方案相比，視覺(jué)導(dǎo)航因具有特征信息完備、地圖描述精準(zhǔn)、定位算法魯棒性強(qiáng)的特點(diǎn)，并且視覺(jué)所需的傳感器即深度相機(jī)相比激光雷達(dá)更加廉價(jià)穩(wěn)定，同時(shí)增加輔助高光照明系統(tǒng)解決低光照問(wèn)題，可以較好地控制機(jī)器人完成在井下復(fù)雜環(huán)境中的巡檢任務(wù)。

因此，結(jié)合現(xiàn)有礦用輪式巡檢機(jī)器人，研究了一種煤礦場(chǎng)景下基于RGBD 的視覺(jué)導(dǎo)航方法，建立一套完整的視覺(jué)導(dǎo)航設(shè)計(jì)流程，用于滿足機(jī)器人在煤礦井下環(huán)境的三維建圖、機(jī)器人自身的重定位以及到達(dá)目標(biāo)點(diǎn)的自主路徑規(guī)劃與導(dǎo)航等功能。

1 總體架構(gòu)

以Kinect v2 相機(jī)作為數(shù)據(jù)采集平臺(tái)，基于極創(chuàng)Warthog-01-M 輪式機(jī)器人移動(dòng)底盤(pán)進(jìn)行視覺(jué)導(dǎo)航架構(gòu)設(shè)計(jì)，從而實(shí)現(xiàn)輪式機(jī)器人在煤礦場(chǎng)景環(huán)境中的自動(dòng)建圖、自主定位以及面向目標(biāo)點(diǎn)的路徑規(guī)劃與導(dǎo)航等功能。依據(jù)現(xiàn)有煤礦輪式巡檢機(jī)器人的應(yīng)用環(huán)境與巡檢需求，采用模塊化設(shè)計(jì)思路對(duì)導(dǎo)航系統(tǒng)進(jìn)行功能定義和拆分，在明確各部分輸入輸出的基礎(chǔ)上，將獨(dú)立的功能模塊進(jìn)行模塊化開(kāi)發(fā)，然后通過(guò)模塊之間的配合完成全部的目標(biāo)任務(wù)，加速系統(tǒng)的并行開(kāi)發(fā)與測(cè)試時(shí)間，提升導(dǎo)航系統(tǒng)的開(kāi)發(fā)效率，同時(shí)方便后續(xù)的階段化調(diào)試以及功能的迭代更新。

視覺(jué)導(dǎo)航系統(tǒng)架構(gòu)的主要功能模塊以及模塊間運(yùn)行的邏輯關(guān)系如圖1。

圖1 總體架構(gòu)原理框圖Fig.1 Principle block diagram of overall architecture

視覺(jué)導(dǎo)航系統(tǒng)架構(gòu)共包含3 個(gè)核心功能模塊，即建圖模塊、定位模塊以及路徑規(guī)劃與導(dǎo)航模塊，通過(guò)這3 個(gè)功能模塊的銜接，實(shí)現(xiàn)從深度相機(jī)信息采集到輪式巡檢機(jī)器人路徑規(guī)劃與導(dǎo)航的全過(guò)程。

1）建圖模塊?；贠RB_SLAM2 算法，將Kinect v2 相機(jī)采集到的RGB 圖片信息以及對(duì)應(yīng)的深度信息轉(zhuǎn)化為三維點(diǎn)云地圖，構(gòu)建出平面柵格地圖用于實(shí)現(xiàn)后續(xù)的重定位。

2）定位模塊。采用蒙特卡羅方法，對(duì)當(dāng)前幀在已構(gòu)建地圖中的局部位置已經(jīng)估計(jì)，從而實(shí)時(shí)產(chǎn)生當(dāng)前位置。

3）路徑規(guī)劃與導(dǎo)航。在明確環(huán)境地圖和機(jī)器人自身位置的情況下，對(duì)輸入的目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃，通過(guò)全局路徑規(guī)劃產(chǎn)生優(yōu)化路徑、局部路徑規(guī)劃實(shí)現(xiàn)動(dòng)態(tài)避障，最終引導(dǎo)機(jī)器人完成自主導(dǎo)航任務(wù)。

2 關(guān)鍵技術(shù)

煤礦井下環(huán)境復(fù)雜，機(jī)器人視覺(jué)導(dǎo)航研究需重點(diǎn)解決其應(yīng)用環(huán)境如低照度、開(kāi)放環(huán)境場(chǎng)景特征點(diǎn)稀疏等帶來(lái)的導(dǎo)航可靠性、穩(wěn)定性問(wèn)題。在三維建圖設(shè)計(jì)中，地圖創(chuàng)建的精度直接影響機(jī)器人自主導(dǎo)航的定位精度，構(gòu)建細(xì)節(jié)信息完備的稠密地圖，采用幀間匹配算法來(lái)逐幀的進(jìn)行地圖拼接，同時(shí)設(shè)計(jì)回環(huán)算法來(lái)消除幀間匹配所產(chǎn)生的累計(jì)誤差，進(jìn)而實(shí)現(xiàn)精準(zhǔn)的三維地圖構(gòu)建。對(duì)于機(jī)器人重定位需求而言，以明確機(jī)器人所處的局部區(qū)域?yàn)橹行?，縮小參與匹配的范圍，并基于特征匹配算法實(shí)現(xiàn)機(jī)器人的精準(zhǔn)定位，完成魯棒性良好的視覺(jué)里程計(jì)構(gòu)建。路徑規(guī)劃與自主導(dǎo)航功能的研究則需要實(shí)時(shí)跟蹤機(jī)器人當(dāng)前位置并規(guī)劃路徑，在實(shí)時(shí)獲取視覺(jué)里程計(jì)信息的同時(shí)，基于二維柵格地圖進(jìn)行平面路徑規(guī)劃，完成準(zhǔn)確的導(dǎo)航任務(wù)，最終實(shí)現(xiàn)機(jī)器人的自主行走功能。

2.1 地圖創(chuàng)建

地圖創(chuàng)建是實(shí)現(xiàn)煤礦場(chǎng)景下視覺(jué)導(dǎo)航的重要研究?jī)?nèi)容之一，其主要是對(duì)Kinect v2 相機(jī)傳感器輸入的RGBD 信息進(jìn)行處理，生成包含環(huán)境完備信息的RGBD 三維點(diǎn)云地圖，并在此基礎(chǔ)上進(jìn)一步獲得可供定位和導(dǎo)航的平面柵格地圖。地圖創(chuàng)建過(guò)程采用ORB_SLAM2 算法來(lái)實(shí)現(xiàn)，算法組成包括ORB 和SLAM 2 部分，其中ORB 算法是一種快速特征點(diǎn)提取和描述的算法，包括圖像梯度特征點(diǎn)的提取以及描述子的設(shè)計(jì)；SLAM 算法則是基于ORB 特征進(jìn)行連續(xù)幀之間的地圖配準(zhǔn)以及后續(xù)導(dǎo)航過(guò)程中的重定位。由于ORB 特征穩(wěn)定且可以高速獲取，因此，ORB_SLAM2 算法具有良好的實(shí)時(shí)性，并通過(guò)BA 實(shí)現(xiàn)回環(huán)檢測(cè)來(lái)保證其建圖的可靠性，使得此算法在室內(nèi)室外環(huán)境中都有較強(qiáng)的適應(yīng)性和魯棒性。

經(jīng)過(guò)模擬井下巷道和現(xiàn)場(chǎng)實(shí)際測(cè)試，采用該算法完成的三維地圖創(chuàng)建可以滿足下一步的定位需求。系統(tǒng)環(huán)境建圖流程如圖2。

圖2 系統(tǒng)環(huán)境建圖流程Fig.2 System environment mapping process

地圖創(chuàng)建依據(jù)ORB_SLAM2 算法原理，采用3個(gè)并行線程：跟蹤、局部建圖和閉環(huán)檢測(cè)。每個(gè)線程實(shí)現(xiàn)的功能描述如下：

1）跟蹤。提取圖像幀的ORB 特征，然后查找其與局部地圖匹配的位置，并通過(guò)BA 實(shí)現(xiàn)最小化投影誤差，從而跟蹤每一幀對(duì)相機(jī)進(jìn)行定位。

2）局部建圖。將篩選出的關(guān)鍵幀插入地圖，構(gòu)建新的地圖點(diǎn)，同時(shí)通過(guò)局部BA 去除冗余的局部關(guān)鍵幀。

3）閉環(huán)檢測(cè)。在機(jī)器人運(yùn)行途中，將當(dāng)前幀與關(guān)鍵幀進(jìn)行對(duì)比，若發(fā)現(xiàn)機(jī)器人經(jīng)過(guò)了重復(fù)的環(huán)境，則通過(guò)前后位姿構(gòu)建起關(guān)鍵幀和當(dāng)前幀之間的閉環(huán)，通過(guò)圖優(yōu)化的方式添加約束并去除累計(jì)誤差。

在完成上述3 個(gè)線程后，可以實(shí)現(xiàn)對(duì)該環(huán)境的建圖，地圖包括地圖點(diǎn)、關(guān)鍵幀、共視圖以及生成樹(shù)。但是此方法生成的地圖為稀疏的特征點(diǎn)地圖，而非細(xì)節(jié)全面的稠密地圖，因此利用ORB_SLAM2方法生成的連續(xù)幀間位姿變換關(guān)系，通過(guò)逐幀的RGBD 點(diǎn)云拼接，獲得了稠密的點(diǎn)云地圖以供后續(xù)的定位和導(dǎo)航。整個(gè)地圖在ROS 端的構(gòu)建過(guò)程描述如下：①將輪式巡檢機(jī)器人置于需要建圖的環(huán)境中，根據(jù)機(jī)器人的初始位置和朝向定義坐標(biāo)原點(diǎn)和坐標(biāo)軸；②在ROS 中啟動(dòng)Kinect v2 相機(jī)節(jié)點(diǎn)，對(duì)其RGB 圖像和Depth 圖像進(jìn)行消息發(fā)布；③在ROS 中啟動(dòng)ORB_SLAM2 建圖節(jié)點(diǎn)，訂閱Kinect v2 發(fā)布的圖像話題，根據(jù)當(dāng)前幀進(jìn)行地圖初始化；④開(kāi)啟Rviz節(jié)點(diǎn)，實(shí)時(shí)觀測(cè)地圖構(gòu)建情況；⑤啟動(dòng)輪式巡檢機(jī)器人底盤(pán)的鍵盤(pán)控制節(jié)點(diǎn)，給定機(jī)器人的移動(dòng)速度和旋轉(zhuǎn)速度，然后通過(guò)鍵盤(pán)控制機(jī)器人在場(chǎng)景中移動(dòng)，直至完成整個(gè)場(chǎng)景的建圖。

2.2 自主定位

自主定位功能主要利用已經(jīng)構(gòu)建的場(chǎng)景地圖以及當(dāng)前圖像幀的RGBD 點(diǎn)云信息，采用蒙特卡羅定位方法，對(duì)輪式巡檢機(jī)器人在當(dāng)前場(chǎng)景中所處的位置進(jìn)行估計(jì)，實(shí)現(xiàn)煤礦井下復(fù)雜結(jié)構(gòu)化環(huán)境的精準(zhǔn)定位。系統(tǒng)自主定位流程如圖3。

圖3 系統(tǒng)自主定位流程Fig.3 Independent positioning process of system

首先利用高斯分布作為巡檢機(jī)器人位置信息的噪聲，在此分布下通過(guò)大量粒子去表征機(jī)器人的可能位置；在此基礎(chǔ)上，通過(guò)觀測(cè)值去計(jì)算每個(gè)粒子運(yùn)動(dòng)后的位置可能性大小即評(píng)分；然后根據(jù)評(píng)分對(duì)粒子群進(jìn)行重采樣；重復(fù)進(jìn)行權(quán)重的計(jì)算和粒子群的重采樣，從而實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)過(guò)程中的連續(xù)定位。

2.3 路徑規(guī)劃

路徑規(guī)劃功能主要分為全局路徑規(guī)劃和局部路徑規(guī)劃，依據(jù)煤礦井下場(chǎng)景特征，在路徑規(guī)劃研究中，全局路徑規(guī)劃基于Dijkstra 算法，采用優(yōu)先廣度搜索和貪心策略路徑求解；局部路徑規(guī)劃采用DWA動(dòng)態(tài)窗口算法，在短時(shí)間內(nèi)對(duì)沒(méi)有碰撞區(qū)域進(jìn)行搜索，同時(shí)優(yōu)化算法使輪式巡檢機(jī)器人在短時(shí)間內(nèi)回避障礙物。系統(tǒng)全局路徑規(guī)劃流程如圖4。系統(tǒng)局部規(guī)劃流程如圖5。

圖4 系統(tǒng)全局路徑規(guī)劃流程Fig.4 Global path planning process of system

圖5 系統(tǒng)局部規(guī)劃流程Fig.5 Local planning process of system

全局路徑規(guī)劃采用Dijkstra 算法，主要通過(guò)貪心和廣度搜索優(yōu)先路徑解算方法來(lái)實(shí)現(xiàn)，在巡檢機(jī)器人初始位置時(shí)，算法首先通過(guò)廣度優(yōu)先算法對(duì)未訪問(wèn)節(jié)點(diǎn)進(jìn)行遍歷，循環(huán)計(jì)算未遍歷節(jié)點(diǎn)和此時(shí)節(jié)點(diǎn)最短距離；當(dāng)計(jì)算出最短路徑節(jié)點(diǎn)時(shí)，將此節(jié)點(diǎn)視為下一個(gè)路徑點(diǎn)；循環(huán)迭代此過(guò)程，由此可以得到起始點(diǎn)到目標(biāo)點(diǎn)的1 條全局最優(yōu)路徑。

局部路徑規(guī)劃采用動(dòng)態(tài)窗口算法，主要目的在于實(shí)現(xiàn)避障功能，針對(duì)全局路徑的局部區(qū)域進(jìn)行二次路徑規(guī)劃，主要通過(guò)在短時(shí)間內(nèi)對(duì)巡檢機(jī)器人周?chē)鷧^(qū)域進(jìn)行碰撞搜索，檢測(cè)巡檢機(jī)器人是否發(fā)生碰撞，同時(shí)優(yōu)化算法使巡檢機(jī)器人在短時(shí)間內(nèi)回避障礙物。

3 試驗(yàn)驗(yàn)證

針對(duì)煤礦井下環(huán)境狀況，設(shè)計(jì)工廠環(huán)境下模擬光照良好和暗黑樓道模擬巷道低光照?qǐng)鼍?，?duì)提出的地圖構(gòu)建方法、路徑規(guī)劃進(jìn)行驗(yàn)證。巡檢機(jī)器人運(yùn)行速度設(shè)定為0.6 m/s，并采樣10 處目標(biāo)點(diǎn)進(jìn)行定位跟蹤測(cè)量。巡檢機(jī)器人導(dǎo)航功能相關(guān)指標(biāo)測(cè)試結(jié)果見(jiàn)表1。

表1 巡檢機(jī)器人導(dǎo)航功能相關(guān)指標(biāo)測(cè)試結(jié)果Table 1 Navigation function related index test results of inspection robot

根據(jù)測(cè)試結(jié)果可知：采用該導(dǎo)航技術(shù)得到的地圖構(gòu)建與實(shí)際環(huán)境一致，巡檢機(jī)器人活動(dòng)區(qū)域內(nèi)沒(méi)有存在未掃描到空間；定位導(dǎo)航精度良好，通過(guò)對(duì)多個(gè)目標(biāo)點(diǎn)進(jìn)行功能驗(yàn)證測(cè)試，重復(fù)定位精度最大6.8 cm，定位偏差小于14 cm，角度誤差小于12°，驗(yàn)證了該視覺(jué)導(dǎo)航架構(gòu)及算法流程具有較好的穩(wěn)定性和魯棒性，能夠適用于煤礦井下巷道開(kāi)放性場(chǎng)景環(huán)境下的機(jī)器人自行走功能。

4 結(jié) 語(yǔ)

針對(duì)井下巷道等具有特征點(diǎn)稀疏、低光照等環(huán)境的開(kāi)放性環(huán)境，設(shè)計(jì)了一種適用于煤礦機(jī)器人的基于RGBD 的視覺(jué)導(dǎo)航系統(tǒng)，建立視覺(jué)導(dǎo)航架構(gòu)流程，并根據(jù)流程進(jìn)行了輪式巡檢機(jī)器人的導(dǎo)航效果測(cè)試。實(shí)際測(cè)試結(jié)果表明：該導(dǎo)航流程可以準(zhǔn)確地完成視覺(jué)地圖的建立，并且可以實(shí)現(xiàn)很好的回環(huán)效果；路徑規(guī)劃后，機(jī)器人能夠有效地完成從起始點(diǎn)到規(guī)劃目標(biāo)點(diǎn)的自主運(yùn)動(dòng)。