煤礦井下探測搜救機器人地形感知系統(tǒng)及路徑規(guī)劃方法研究*

盧萬杰,付 華,趙洪瑞

(1.遼寧工程技術(shù)大學(xué)機械工程學(xué)院,遼寧 阜新 123000;2.遼寧工程技術(shù)大學(xué)電氣工程學(xué)院,遼寧 葫蘆島 125000; 3.煤科集團沈陽研究院有限公司,沈陽 110000)

煤礦搜救機器人作為煤礦救災(zāi)的器械而被發(fā)明出來,需要解決的核心問題是它的導(dǎo)航技術(shù)。由于,礦災(zāi)發(fā)生后會造成巷頂冒落、機械設(shè)備傾倒和巷道漏水等各種復(fù)雜工況,對就業(yè)機器人的作業(yè)環(huán)境的包容性提出更大的挑戰(zhàn)。在復(fù)雜的地面作業(yè)環(huán)境的條件限制下,救災(zāi)機器人需要具有較強的復(fù)雜地形適應(yīng)能力、較強的越障性能、較好系統(tǒng)可靠性、更持久的工作電源、較高的智能識別系統(tǒng)精度和智能路徑規(guī)劃性能。故救災(zāi)機器人的導(dǎo)航技術(shù)的研究是極其重要的,它包含對運動結(jié)構(gòu)的控制、環(huán)境識別的精度及智能規(guī)劃路徑等眾多研究問題。

煤礦搜救機器人自適應(yīng)智能導(dǎo)航技術(shù)是眾多搜救機器人技術(shù)中的最關(guān)鍵一項,該項技術(shù)的成功研究是煤礦搜救機器人推廣應(yīng)用的前提條件。

文獻[1-4]中都運用Doucet 等學(xué)者研究的SLAM 方法,該種方法是以ao-Black wellized 粒子濾波器RBPF為基礎(chǔ),他可以高精度的建立移動機器人的位姿與環(huán)境間的聯(lián)系。文獻[5-6]中都以激光掃描儀與單目視覺相融合的SLAM算法為研究對象,對周圍工作環(huán)境特征進行識別,形成新的環(huán)境坐標,減少了數(shù)據(jù)的不準確性,但由于數(shù)據(jù)計算量較大,在時間上會有一定延誤。文獻[7]首次提出RBPF 算法,它是以退火參數(shù)優(yōu)化理論為基礎(chǔ),此種方法具有減少所需粒子數(shù)但不減少粒子的種類的優(yōu)勢。文獻[8]對計算提議分布過程進行研究,對過程中的里程計信息與激光采集的距離信息進行融合,通過驗證得知,它可以在很大程度上減少粒子數(shù)但同時能保證預(yù)測階段機器人位姿的預(yù)測精度。

以上文獻所述的機器人導(dǎo)航系統(tǒng)基本是采用激光掃描儀或視覺傳感器實現(xiàn)未知環(huán)境內(nèi)的地形識別,所觀察的維度單一,如果觀測目標是近距離障礙物,則無法對遠距離環(huán)境內(nèi)的障礙物進行觀測,容易使機器人走入“死胡同”,如果觀測目標是遠距離環(huán)境,則對近距離障礙物的觀測盲區(qū)有可能會使機器人駛?cè)肷顪系葻o法逾越的障礙物中。針對上述問題,本文對煤礦井下探測搜救機器人地形感知系統(tǒng)進行研究,使用遠近感知系統(tǒng)數(shù)據(jù)融合,提高機器人避障能力。并研究基于地形感知信息的機器人路徑規(guī)劃方法。

圖1 機器人地形感知系統(tǒng)硬件框架

1 機器人地形感知系統(tǒng)組成

煤礦井下探測搜救機器人的地形感知系統(tǒng)硬件框架如圖1所示。系統(tǒng)硬件主要由中央計算機和各種傳感器組成[9-10]。其中傳感器主要包括用于機器定位的編碼器、用于機器人姿態(tài)檢測的傾角傳感器、用于遠距離地形感知的激光掃描儀以及用于近距離地形感知的Kinect視覺相機等組成。

Kinect作為一種典型的多傳感器融合系統(tǒng),它主要由三個攝像頭構(gòu)成。其中中間部分的攝像頭采用的是RGB攝像頭,它的功用主要體現(xiàn)在彩色圖像的獲得。兩側(cè)的鏡頭為紅外線發(fā)射器和紅外線攝像頭,兩者功用相輔相成,共同組成3D結(jié)構(gòu)光深度感應(yīng)器,它的主要功用為采集深度信息[11]。

系統(tǒng)采集各類傳感器數(shù)據(jù),由機器定位的編碼器、用于機器人姿態(tài)檢測的傾角傳感器采集的數(shù)據(jù)確定機器人的位置和姿態(tài),由遠距離地形感知的激光掃描儀采集的二維點云數(shù)據(jù)建立未知環(huán)境下的地圖構(gòu)建。由近距離地形感知的Kinect相機采集的地形深度信息建立柵格地圖。機器人路徑規(guī)劃決策系統(tǒng)根據(jù)軟件構(gòu)建的地圖信息和柵格地圖信息以及機器人機身運動學(xué)模型等進行路徑規(guī)劃。本文主要針對地形感知系統(tǒng)進行研究,對于路徑規(guī)劃決策系統(tǒng)將在今后進一步進行研究。

2 地形感知系統(tǒng)數(shù)學(xué)模型

2.1 機器人運動學(xué)模型

建立機器人坐標系如圖2所示。其中{ow}為世界坐標系;{ob}為機器人機體坐標系;{os}為機器人遠距離測距激光掃描儀坐標系;{oc}為機器人近距離地形識別Kinect視覺相機坐標系。

圖2 機器人坐標系

式中:xg為機器人相對于世界坐標系x軸的位移變化;yg為機器人相對于世界坐標系y軸的位移變化;zg為機器人相對于世界坐標系z軸的位移變化;α是機器人航向角度;β為機器人的翻滾角度;γ為機器人的俯仰角度[12]。

(3)

綜上可得機器人在世界坐標系中的地形深度信息表示方法:

2.2 近距離感知系統(tǒng)模型

Kinect 在實質(zhì)上是由兩部分構(gòu)成:RGB攝像頭和紅外攝像頭,兩者間相互配合共同構(gòu)成3D結(jié)構(gòu)光深度傳感器,可實現(xiàn)彩色圖像與深度圖像的同步 收集。

通過RGB攝像頭得到的圖像中每一個像素點都可在深度圖像中尋找到像素點與之對應(yīng),從而可獲得與之相對應(yīng)的深度值。借助于攝像機投影模型和相對應(yīng)的物理參數(shù),可以把二維圖像像素坐標點進行轉(zhuǎn)換,變?yōu)槿S坐標點,可直觀獲取觀測信息,直接構(gòu)建三維點云圖像。坐標轉(zhuǎn)換公式如下:

X=(u-u0)Z/fxY=(v-v0)Z/fyZ=d/s

(5)

上式中以信息(u,v)與深度信息d為自變量,通過構(gòu)建數(shù)學(xué)模型推導(dǎo)出因變量空間坐標(X,Y,Z)。s為尺度縮放因子,參數(shù)已知。u0,v0,fx,fy為通過實驗確定的參數(shù)數(shù)值。Z為距離參數(shù),和Kinect視覺系統(tǒng)中的的距離信息意義相同[13]。

圖3 Kinect相機圖像采集流程圖

以PCLPCL(Point Cloud Library)為基礎(chǔ),應(yīng)用像素遍方法,并依據(jù)上述模型中的深度信息和位置顏色信息,實現(xiàn)觀測信息的采集與云圖像的構(gòu)建[14]。具體流程如圖3所示。

由于Kinect中得到的深度數(shù)據(jù)誤差和深度距離具有成正比的特性,為保證其檢測精度,故對觀測距離設(shè)置距離上限閾值Dmax,對于超過上限值的數(shù)據(jù)進行剔除。其中上限閾值可依據(jù)具體問題進行綜合選取。

2.3 地形構(gòu)建

構(gòu)造環(huán)境模型是移動機器人能實現(xiàn)智能化適應(yīng)動作的前提條件,應(yīng)用普通方法對環(huán)境地圖進行構(gòu)建并使之服務(wù)于機器人是不現(xiàn)實的。傳統(tǒng)的室內(nèi)環(huán)境具有以下三種特點:垂直結(jié)構(gòu)多、高度結(jié)構(gòu)化以及包含多種線性構(gòu)造(線、面的結(jié)構(gòu)特征顯著)。正是由于這種特性,故可采用二維柵格地圖方法對室內(nèi)場景建模。但由于室外環(huán)境較復(fù)雜,隨機條件太多,采用單一的、少量的幾何元素對環(huán)境建模,極不現(xiàn)實,故簡單的二維柵格地圖使用推廣就受到限制。此時采用2.5維柵格方法可完全解決這一問題,通過該種方法可建立海拔信息地圖,與普通地圖相比,此種地圖信息更加豐富,但與傳統(tǒng)的三維地圖相比,建模難度相對簡單,故綜合參考,在不包含空間運動的機器人中,采用2.5維柵格地圖在存儲和計算量方面均是最適用的。

2.5維柵格地圖與傳統(tǒng)意義上的二維、三維地圖有所區(qū)別,它是以二維地圖為基礎(chǔ),在二維地圖上施加了單元信息,例如:高程差、坡度、紋理、顏色等評價準則信息。其中高程差、坡度信息的確定是由深度數(shù)據(jù)進行計算得到的,紋理、顏色信息主要通過特征提取來確定。這些評價準則的確定是幾種特征在經(jīng)過多次訓(xùn)練后獲得的。

地形信息的表示方法采用的是數(shù)字高程模型DEM,它是由規(guī)則的柵格組成,其中每個柵格中都存有與之相對應(yīng)柵格區(qū)域的平均高程[15]。其中,以DEM為基礎(chǔ)對柵格進行危險等級值D(D∈{[0,1],2})劃分。其中該數(shù)值的大小與地形的復(fù)雜程度息息相關(guān)。當D取0時,表示地勢平坦;當D取1時,表示地勢相對復(fù)雜,難通過;當D取0時,表示地勢復(fù)雜,不可通過,此時機器人需躲避;D的取值決定因素為地形坡度S(打滑約束)、起伏度H(步高及障礙邊緣約束)及粗糙度R(機體穩(wěn)定約束)三種,具體數(shù)值采用3×3 的DEM柵格進行計算獲得。采用文獻[16-17]中提出的地形信息計算方法:

H=max{ei-e0},i=0,1,2,…,7

(6)

式中:Swe為東西方向的坡度;Ssn為南北方向坡度;L為柵格邊長。

2.4 機器人感知系統(tǒng)實驗

本文使用搭建的煤礦井下探測搜救機器人平臺帶有思嵐RPLIDAR-A1型二維激光掃描儀,掃描距離為0.15 m～12 m,測量采樣頻率為≥4 000 Hz。搭載配備 Kinect V2相機,里程計以及支持串口連接的控制計算機等。計算機配置為英特爾I5-3317U,8G內(nèi)存,128 G固態(tài)硬盤,計算機的操作系統(tǒng)為Ubuntu14.04 LTS 版本,并且計算機系統(tǒng)中搭載了Indigo版本的ROS系統(tǒng)。機器人機載計算機通過54 M帶寬的802.11 g無線通訊協(xié)議與遠程的計算機通信,遠程計算機實驗同樣的Ubuntu14.04 LTS版本系統(tǒng)和Indigo版本的ROS系統(tǒng),使用ROS系統(tǒng)的RVIZ圖形化工具顯示出機器人建立的地圖等圖形信息。使用本文搭建的煤礦井下探測搜救機器人實驗平臺進行地形感知實驗。選取臺階和野外地形對本文研究的地形感知系統(tǒng)進行實驗研究。采集現(xiàn)場、處理后的灰度圖和深度圖以及由式(6)～式(8)得到的地形信息如圖4和圖5所示。

圖4 臺階地形感知實驗

圖5 野外地形感知實驗

圖4和圖5得到的實驗地形的高程圖和起伏度與實際地形信息相符,本文通過多個室內(nèi)及野外環(huán)境的測試,驗證了本文研究的地形感知系統(tǒng)得到的地形信息的可行性。實驗結(jié)果表明本文使用的計算方法可減小坡面對粗糙度的影響,可較好地反映地形粗糙度及高程信息?？衫帽疚难芯康牡匦胃兄到y(tǒng)得到的地形信息為機器人的路徑規(guī)劃提高較好的數(shù)據(jù)支持。

圖6 巡檢機器人路徑規(guī)劃流程

3 機器人路徑規(guī)劃研究

3.1 機器人運動路徑規(guī)劃流程

移動機器人在執(zhí)行探測任務(wù)的時候,往往需要對探測區(qū)域進行數(shù)據(jù)采集、數(shù)據(jù)分析以及放置探測儀器等,為了使機器人能夠自主且準確的完成探測任務(wù)就需要考慮機器人所處的三維崎嶇環(huán)境對機器人的運動進行規(guī)劃。崎嶇地面上移動機器人運動規(guī)劃是機器人自主導(dǎo)航的基礎(chǔ),也是當前機器人領(lǐng)域研究的熱點之一。其關(guān)鍵技術(shù)包括崎嶇地面環(huán)境建模、運動規(guī)劃和評估,以及如何有效的實現(xiàn)已規(guī)劃路徑的運動控制。煤礦井下巡檢機器人需要通過崎嶇不平的復(fù)雜路面,過去人們研究機器人路徑規(guī)劃時往往將機器人看作一個質(zhì)點,但是機器人在崎嶇不平路面行走時,4個輪子著地點的不同會影響機器人的姿態(tài),因此需要考慮機器人運動學(xué)位姿模型對于路徑規(guī)劃的影響。本文研究的路徑規(guī)劃方法流程如圖6所示。

3.2 路徑規(guī)劃目標函數(shù)

在運動規(guī)劃中考慮路徑長度從而尋求機器人從初始點到目標節(jié)點的最短距離。設(shè)相鄰兩節(jié)點距離為:

式中:xi,yi,zi分別為節(jié)點i的X、Y、Z方向坐標值;xj,yj,zj分別為節(jié)點j的X、Y、Z方向坐標值。

設(shè)定機器人路徑規(guī)劃的長度因子為:

本文采用機器人投影域內(nèi)地面各點危險等級作為危險度評價指標,衡量機器人通過該區(qū)域的可能性,考慮機器人航向角度沿機器人規(guī)劃的路徑方向。機器人投影域定義為以機器人參考點坐標為中心,以參考點到機器人輪-地接觸點C的長度為半徑的圓。記R為該區(qū)域內(nèi)所有網(wǎng)格節(jié)點,則點(x,y)處的地面危險等級為R(x,y)為:

(11)

在目標函數(shù)中考慮地面危險等級的影響主要是因為崎嶇地形的該指標對機器人的靜態(tài)穩(wěn)定性以及地面通過性影響很大,防止機器人發(fā)生傾覆。另外,地面危險等級相對于輪子直徑的比值越大,崎嶇地面的通過性能往往越低,需要在這些區(qū)域標記上更加大的危險等級,以避開危險地形。因此采用了如下的相對危險等級:

(12)

式中:α為大于 1 的常數(shù),從而在保證對障礙物的表述連續(xù)性(相對于傳統(tǒng)二進制 0-1的地形描述)的前提下,有效提高了機器人在危險區(qū)域的通過代價。

定義目標函數(shù)為Λ,為地面不平度、路徑長度以及路徑評估因子的組合[18]:

Λ=ξR′+τL′

(13)

式中:ζ和τ分別為常數(shù),且0<(ζ+τ)1。用來調(diào)整目標函數(shù)與地面不平度和路徑長度的相關(guān)程度。

3.3 路徑規(guī)劃計算

按照Delaunay三角形剖分算法建立尺寸為200 cm×200 cm的仿真地圖,其中單位網(wǎng)格的尺寸為5 cm×5 cm,兩個運動規(guī)劃仿真地形實例如圖7所示。

圖7 運動規(guī)劃仿真地形

為了驗證本章中提出的機器人運動規(guī)劃算法的可行性,分別在該地形上對機器人最短路徑、最安全路徑以及最優(yōu)路徑這三種目標函數(shù)下的路徑進行了規(guī)劃和評估。實驗Dijkstra路徑規(guī)劃算法進行路徑規(guī)劃。得到仿真地形下按照不同的路徑規(guī)劃目標函數(shù)得到的路徑規(guī)劃結(jié)果如圖8所示。

圖8 仿真地形的仿真結(jié)果

由仿真地形下按照不同的路徑規(guī)劃目標函數(shù)得到的路徑規(guī)劃結(jié)果可知,最短路徑忽略了地面粗糙度對目標函數(shù)的影響,雖然路徑最短,但是跨過了地面中最崎嶇的區(qū)域;危險等級最低的路徑地面粗糙度明顯較小,繞過了地圖中地面高程較高的區(qū)域;最優(yōu)路徑則一方面保證了路徑長度相對較小,又保證了路徑的安全性。

對不同路徑規(guī)劃仿真結(jié)果進行分析。分別對路徑長度因子、地面危險等級以及路徑規(guī)劃時間進行統(tǒng)計,得到兩種仿真地形下的路徑規(guī)劃結(jié)果對比見表1所示。

表1 兩種仿真地形下的路徑規(guī)劃結(jié)果對比

由表1所示的兩種仿真地形下的路徑規(guī)劃結(jié)果可以看出,三種目標函數(shù)中最短路徑、危險等級最低的路徑以及最優(yōu)路徑中,最短路徑的規(guī)劃結(jié)果的路徑長度最短,但是這種路徑的地面危險等級最大,機器人沿著規(guī)劃的路徑運動時消耗的能量最多且發(fā)生傾覆的危險最大。最優(yōu)路徑為最短路徑和危險等級最低的折中路徑,路徑長度較最短路徑稍大,但是累計的危險等級較低,因而機器人能夠較安全的跟蹤這種目標函數(shù)所規(guī)劃的路徑,并且保證路徑長度因子較小。

3.4 路徑規(guī)劃結(jié)果仿真驗證

使用運動學(xué)仿真軟件ADAMS建立仿真地形圖,再按照路徑規(guī)劃結(jié)果設(shè)置小車的行走路徑。通過后處理顯示小車的質(zhì)心高度變化以及車身俯仰和翻滾偏角的變化情況。仿真地形中按照最短路徑得到的小車的質(zhì)心高度變化以及車身俯仰和翻滾偏角的變化情況如圖9所示。

仿真地形中按照最低危險等級得到的小車的質(zhì)心高度變化以及車身俯仰和翻滾偏角的變化情況如圖10所示。

圖10 仿真地形中最低危險等級得到的 小車的質(zhì)心高度及俯仰和翻滾偏角

仿真地形中按照最優(yōu)路徑得到的小車的質(zhì)心高度變化以及車身俯仰和翻滾偏角的變化情況如圖11所示。

圖11 仿真地形中最優(yōu)路徑得到的小車的 質(zhì)心高度及俯仰和翻滾偏角

由兩個仿真地形得到的最短路徑、危險等級最低路徑以及最優(yōu)路徑可以看出。最短路徑所使用的時間最短,路徑規(guī)劃算法將路徑長度作為目標函數(shù),選取了起始點和終點路徑長度最短作為機器人路徑。雖然機器人行走的路徑最短,機器人卻穿越了地圖上最為崎嶇的地面,機器人側(cè)傾角最大值超過了20°,有傾翻的危險。

危險等級最低的路徑是由規(guī)劃算法計算仿真地形中危險等級最低的區(qū)域而計算出來的路徑,所以相比較于最短路徑,該條路徑下機器人的俯仰角、側(cè)傾角的波動幅度小得多,均在±10°范圍內(nèi),機器人穩(wěn)定相比較最短路徑更加平穩(wěn)。

最優(yōu)路徑即為最短路徑和危險等級最低路徑的折中路線,一方面保證了機器人由起始點到達目標點的運動長度較短,另一方面地面該路徑的地面不平度也較最短路徑小。

4 結(jié)論

本文對煤礦井下探測搜救機器人地形感知系統(tǒng)進行研究,使用遠近感知系統(tǒng)數(shù)據(jù)融合,提高機器人避障能力,最后通過實驗驗證本文研究感知系統(tǒng)的可行性。本文使用Dijkstra算法進行了路徑規(guī)劃研究,建立了融合路徑長度和地面危險度等級的目標函數(shù)。通過仿真研究驗證了本文提出的最優(yōu)路徑減小機器人行走過程的俯仰角、側(cè)傾角的波動幅度。