基于膜計(jì)算的煤礦井下機(jī)器人路徑規(guī)劃算法

黃友銳， 李靜， 韓濤， 徐善永

(安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001)

0 引言

煤礦井下空間狹小，環(huán)境惡劣，危險(xiǎn)性大，對煤炭自動(dòng)化采掘和運(yùn)輸提出了巨大挑戰(zhàn)[1]。使用機(jī)器人進(jìn)行煤礦井下生產(chǎn)和運(yùn)輸工作，可最大程度地減少傷亡事故。隨著研究不斷深入，機(jī)器人將被應(yīng)用于煤礦井下探測、開采、運(yùn)輸、救援等工作[2]，而要實(shí)現(xiàn)這些應(yīng)用，機(jī)器人需能夠在井下復(fù)雜環(huán)境中規(guī)劃出合理、高效的可行路徑。因此，路徑規(guī)劃成為煤礦機(jī)器人研究的重點(diǎn)之一。

路徑規(guī)劃的基本任務(wù)是尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑[3]，要求過程簡單、用時(shí)少、尋找到的路徑無碰撞且距離短等。路徑規(guī)劃算法一般分為基于柵格的路徑規(guī)劃算法和基于采樣的路徑規(guī)劃算法[4]?；跂鸥竦穆窂揭?guī)劃算法包括Dijkstra及其改進(jìn)算法等[5-7]，這類算法具有過程簡單、實(shí)時(shí)性好的優(yōu)點(diǎn)，但路徑規(guī)劃效果過度依賴于劃分地圖柵格時(shí)所采用的分辨率?；诓蓸拥穆窂揭?guī)劃算法不需要對環(huán)境進(jìn)行離散化或?qū)φ系K物進(jìn)行公式化處理，為路徑規(guī)劃提供了新思路。基于隨機(jī)采樣的快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree，RRT)近年來受到了極大的關(guān)注與重視[8-10]，RRT算法具有建?？?、搜索能力強(qiáng)的優(yōu)點(diǎn)。文獻(xiàn)[11]提出了具有漸進(jìn)最優(yōu)性的RRT*算法，解決了RRT算法無法收斂至最優(yōu)路徑的問題。針對RRT*算法采樣范圍過大、效率低的問題，文獻(xiàn)[12]提出了Informed RRT*算法，在路徑優(yōu)化過程中的采樣區(qū)域?yàn)闄E圓形，相比于RRT*算法對整個(gè)空間進(jìn)行搜索來講，縮小了采樣空間，加快了收斂速度。但基于采樣的路徑規(guī)劃算法采用隨機(jī)采樣、固定步長增長樹的方式生成路徑，規(guī)劃時(shí)間長、效率低。

為了加快路徑規(guī)劃速度，研究者們提出了多種算法融合的路徑規(guī)劃方法，如A*與RRT融合[13]、人工勢場與RRT融合[14]、貪婪策略與RRT*融合[15]、三角不等式與RRT*融合(PQ-RRT*)[16]、啟發(fā)式偏差與RRT*融合[17]等。這些融合算法優(yōu)化了全局最優(yōu)解的搜索過程，加快了搜索速度，但仍都采用固定步長和串行計(jì)算的規(guī)劃方式，在一些復(fù)雜環(huán)境中，尤其是煤礦井下狹小空間環(huán)境中，容易產(chǎn)生路徑規(guī)劃失敗、規(guī)劃速度慢、規(guī)劃路徑不合理等問題。

膜計(jì)算(Membrane Computing，MC)是一種通過編寫規(guī)則處理類細(xì)胞或類組織中多個(gè)對象的分布式并行計(jì)算仿生模型。多個(gè)基本膜同時(shí)獨(dú)立進(jìn)行規(guī)則更新，使得MC具有很強(qiáng)的分布式和并行性計(jì)算特點(diǎn)，在處理工程優(yōu)化問題上具有獨(dú)特優(yōu)勢。因此，本文利用MC改進(jìn)Informed RRT*算法，提出了一種基于MC的煤礦井下機(jī)器人路徑規(guī)劃算法，即基于MC的Informed RRT*(MC-IRRT*)算法。該算法充分利用了多核計(jì)算機(jī)的并發(fā)計(jì)算能力，使細(xì)胞型膜結(jié)構(gòu)中的多個(gè)基本膜能并行工作(不同步長、多采樣點(diǎn))，既可以在大空間區(qū)域進(jìn)行大步長快速搜索，也可以在小空間區(qū)域進(jìn)行小步長精細(xì)搜索，擴(kuò)大了采樣范圍，在控制搜索時(shí)間的前提下，增強(qiáng)了路徑優(yōu)化能力，特別適用于煤礦井下狹長空間的特殊環(huán)境。

1 MC和Informed RRT*算法

1.1 MC

圖1 細(xì)胞型膜系統(tǒng)結(jié)構(gòu)

MC的核心思想是在多個(gè)基本膜內(nèi)根據(jù)各自規(guī)則同時(shí)更新對象，并將更新結(jié)果輸送到表層膜，表層膜再根據(jù)自己的規(guī)則進(jìn)行計(jì)算和更新，并輸出最終結(jié)果。

1.2 Informed RRT*算法

Informed RRT*算法是一種基于采樣的路徑規(guī)劃算法，算法分為2個(gè)階段：第1階段，采用RRT算法在整個(gè)地圖中隨機(jī)采樣，并用固定步長增長樹的方式快速尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑；第2階段，根據(jù)已經(jīng)獲取的可行路徑，在1個(gè)特定的橢圓區(qū)域內(nèi)采樣，獲取新的可行路徑，不斷優(yōu)化找到的可行路徑，最終找到一條最短的可行路徑作為路徑規(guī)劃結(jié)果。

Informed RRT*算法路徑優(yōu)化過程中的采樣區(qū)域?yàn)闄E圓形，其搜索過程如圖2所示。

圖2 Informed RRT*算法搜索過程

與RRT*算法相比，Informed RRT*算法的采樣空間不斷縮小，其收斂速度也相應(yīng)加快，搜索效率提升。但I(xiàn)nformed RRT*算法的第1階段是以固定步長不斷獲取路徑點(diǎn)，生成可行路徑，若步長過大，則不能尋找到較窄路徑；若步長較小，則搜索速度變慢。第2階段則以串行方式搜索路徑并進(jìn)行比較后才能確定最優(yōu)路徑，效率較低。

2 MC-IRRT*算法

2.1 算法總體結(jié)構(gòu)

MC-IRRT*算法的總體結(jié)構(gòu)如圖3所示。

圖3 MC-IRRT*算法總體結(jié)構(gòu)

MC-IRRT*算法主要包括快速連通和路徑尋優(yōu)2個(gè)階段。在快速連通階段，構(gòu)建具有多個(gè)步長的細(xì)胞型膜結(jié)構(gòu)，多個(gè)基本膜分別以不同的步長進(jìn)行搜索，在表層膜中進(jìn)行篩選優(yōu)化，不斷迭代，最終找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑。在路徑尋優(yōu)階段，構(gòu)建具有多采樣點(diǎn)的細(xì)胞型膜結(jié)構(gòu)，多個(gè)基本膜分別生成多個(gè)不同采樣點(diǎn)，不斷搜索更優(yōu)路徑，最終找到最短可行路徑。

2.2 快速連通階段

MC-IRRT*算法快速連通階段構(gòu)建的多步長膜結(jié)構(gòu)如圖4所示。

圖4 多步長膜結(jié)構(gòu)

多步長并行膜結(jié)構(gòu)可描述為

Π=(V,T,u,w0,w1,…,wm,R0,R1,…,Rm)

(1)

(2)

u=[0[1]1,…,[j]j,…,[m]m]0

(3)

w0={Map,P0,PT,K,ν,i,Ltarget}

(4)

(5)

式中：V為系統(tǒng)中所有元素的集合；T為輸出對象的集合,T?V；u為膜系統(tǒng)，含有m個(gè)基本膜和1個(gè)表層膜(表層膜0)；wj表示膜系統(tǒng)u中基本膜j內(nèi)包含的字符對象；Rj為基本膜j的運(yùn)行規(guī)則；Map為區(qū)域地圖信息；P0，PT分別為起始點(diǎn)位置坐標(biāo)和目標(biāo)點(diǎn)位置坐標(biāo)；K為迭代次數(shù)；ν為路徑點(diǎn)集合；i為當(dāng)前迭代次數(shù)；Pnow為當(dāng)前路徑點(diǎn)集合；S為采用步長集合；Pth為隨機(jī)采樣概率閾值集合；Psmp為采樣點(diǎn)集合；Pnew為新路徑點(diǎn)集合；Prand為隨機(jī)采樣點(diǎn)集合；Ltarget為新路徑點(diǎn)到達(dá)目標(biāo)點(diǎn)的距離閾值；[j]表示基本膜j中對應(yīng)的粒子對象。

基本膜j的運(yùn)行規(guī)則：① 在區(qū)域地圖Map中隨機(jī)采樣1個(gè)點(diǎn)Prand[j]，比較該點(diǎn)與隨機(jī)采樣概率閾值的大小。如果Prand[j]≥Pth[j]，則基本膜內(nèi)采樣點(diǎn)就是Prand[j]；如果Prand[j]

表層膜0的運(yùn)行規(guī)則：分別計(jì)算m個(gè)基本膜輸出的新節(jié)點(diǎn)Pnew[j]與目標(biāo)點(diǎn)PT之間的距離L[j]，并選取最小值。

(6)

Lmin=minL[j]

(7)

將距離PT最近的節(jié)點(diǎn)作為新的路徑點(diǎn)加入到路徑點(diǎn)集合ν中，再將該節(jié)點(diǎn)作為當(dāng)前迭代次數(shù)尋找到的路徑點(diǎn)，重新同時(shí)輸入到m個(gè)基本膜中，不斷迭代循環(huán)，直至找到目標(biāo)點(diǎn)。當(dāng)滿足以下條件時(shí)認(rèn)為目標(biāo)點(diǎn)被找到。

(8)

式中Lnow為當(dāng)前路徑點(diǎn)與目標(biāo)點(diǎn)的距離。

判斷Lnow是否在閾值范圍內(nèi)，如果Lnow≤Ltarget，說明尋找的路徑已經(jīng)到達(dá)目標(biāo)點(diǎn)，將輸出的所有路徑點(diǎn)按順序連接起來，即可得到一條起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑。如果Lnow>Ltarget，說明此時(shí)找到的路徑點(diǎn)不是目標(biāo)點(diǎn)，進(jìn)行下一次迭代循環(huán)，直到滿足最大迭代次數(shù)為止。

快速連通階段工作流程如圖5所示。

圖5 快速連通階段工作流程

(1)設(shè)置表層膜0的當(dāng)前路徑點(diǎn)Pnow=P0，當(dāng)前迭代次數(shù)i=1，路徑點(diǎn)集合ν={P0}。

(2)將當(dāng)前路徑點(diǎn)Pnow復(fù)制到m個(gè)基本膜內(nèi),同時(shí)進(jìn)行基本膜計(jì)算，得到m個(gè)新路徑點(diǎn)Pnew[j]，將其輸出到表層膜0中。

(3)表層膜0分別計(jì)算每個(gè)新路徑點(diǎn)與目標(biāo)點(diǎn)之間的距離，獲取距離最小值對應(yīng)的新路徑點(diǎn)，將其作為當(dāng)前迭代次數(shù)尋找到的路徑點(diǎn)放入到路徑點(diǎn)集合ν中。

(4)計(jì)算當(dāng)前路徑點(diǎn)與目標(biāo)點(diǎn)的距離，判斷是否到達(dá)目標(biāo)點(diǎn)，到達(dá)則跳轉(zhuǎn)到步驟(6)，否則跳轉(zhuǎn)到步驟(5)。

(5)判斷迭代次數(shù)是否完成，即i是否等于K，若是，則認(rèn)為迭代完成，跳轉(zhuǎn)到步驟(6)；否則，令i=i+1，跳轉(zhuǎn)到步驟(2)。

(6)快速連通結(jié)束，輸出路徑點(diǎn)集合ν，按順序連接路徑點(diǎn)集合中所有路徑點(diǎn)，作為尋找到的起始點(diǎn)到目標(biāo)點(diǎn)的一條可行路徑。

快速連通階段采用多步長膜結(jié)構(gòu)，能夠根據(jù)空間區(qū)域的大小來調(diào)整步長。在可行空間較大的區(qū)域采用大步長搜索，加快搜索速度，而在狹小的空間使用小步長搜索，使搜索空間更加精細(xì)，提高狹小空間路徑搜索成功率，保證煤礦機(jī)器人在煤礦復(fù)雜多變的環(huán)境中安全高效行駛。

2.3 路徑尋優(yōu)階段

路徑尋優(yōu)階段的主要目的是基于快速連通階段已經(jīng)找到的一條可行路徑，進(jìn)行最短路徑優(yōu)化，從而尋找出從起始點(diǎn)到目標(biāo)點(diǎn)之間的最短可行路徑。將MC的并行性和高效性與Informed RRT*算法相結(jié)合，在第1階段尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑后，形成1個(gè)橢圓采樣區(qū)，然后通過構(gòu)建多采樣點(diǎn)膜結(jié)構(gòu)，讓m個(gè)基本膜同時(shí)在橢圓區(qū)域內(nèi)并行搜索最短可行路徑。

構(gòu)建多采樣點(diǎn)并行膜結(jié)構(gòu)，如圖6所示。

圖6 多采樣點(diǎn)并行膜結(jié)構(gòu)

多采樣點(diǎn)并行膜結(jié)構(gòu)可描述為

(9)

(10)

u′=[0[1]1,…,[j]j,…,[m]m]0

(11)

(12)

(13)

表層膜0的運(yùn)行規(guī)則：表層膜0根據(jù)Map，P0，PT和當(dāng)前路徑長度形成1個(gè)橢圓形采樣區(qū)域M，如圖7所示。橢圓的2個(gè)焦點(diǎn)為P0和PT，2個(gè)焦點(diǎn)之間的距離為Cfocus，當(dāng)前路徑長度為橢圓的長軸Clong，Cshort為橢圓的短軸，具體計(jì)算公式為

圖7 采樣橢圓

(14)

Pi∈ν′

(15)

(16)

路徑尋優(yōu)過程：

(1)將尋優(yōu)路徑點(diǎn)集合ν′[0]設(shè)置為快速連通階段找到的路徑點(diǎn)集合ν，即ν′=ν，當(dāng)前迭代次數(shù)i[0]=1。

(2)表層膜0根據(jù)區(qū)域地圖信息、起始點(diǎn)、目標(biāo)點(diǎn)和尋優(yōu)路徑點(diǎn)集合形成1個(gè)橢圓形區(qū)域作為隨機(jī)采樣區(qū)M，并輸入到所有基本膜中。

(3)m個(gè)基本膜分別同時(shí)按照Informed RRT*橢圓區(qū)域?qū)?yōu)過程進(jìn)行計(jì)算，并將得到的路徑點(diǎn)集合ν′[j]輸出到表層膜0。

(4)表層膜0分別計(jì)算當(dāng)前路徑點(diǎn)集合ν′[0]和ν′[j]內(nèi)路徑點(diǎn)距離之和，選取最小值對應(yīng)的路徑點(diǎn)集合作為新的最短路徑點(diǎn)集合,即ν′[0]=min{ν′[0],ν′[j]}。

(5)判斷迭代是否完成，即i[0]是否為N[0]，若是，則認(rèn)為迭代完成，跳轉(zhuǎn)到步驟(6)；否則，令i[0]=i[0]+1，跳轉(zhuǎn)到步驟(2)。

(6)最短路徑優(yōu)化結(jié)束，輸出ν′[0]，即起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)可行路徑。

路徑尋優(yōu)流程如圖8所示。

圖8 路徑尋優(yōu)流程

路徑尋優(yōu)階段的優(yōu)點(diǎn)是采用了多采樣點(diǎn)膜結(jié)構(gòu)，通過m個(gè)基本膜的并行計(jì)算，能夠同時(shí)在多個(gè)橢圓區(qū)域內(nèi)并行搜索最短可行路徑，節(jié)省了時(shí)間，提高了路徑優(yōu)化效率，可在最短時(shí)間內(nèi)找到一條起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑，大大增強(qiáng)了機(jī)器人路徑規(guī)劃的實(shí)時(shí)性，提高了機(jī)器人的行駛效率，為機(jī)器人在煤礦井下自主安全行駛提供了基礎(chǔ)保證。

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

3.1 實(shí)驗(yàn)條件

為了驗(yàn)證所提算法的有效性，在不同場景進(jìn)行對比實(shí)驗(yàn)。實(shí)驗(yàn)硬件系統(tǒng)為Intel(R)Core(TM)i5-7500 CPU@3.80 GHz，內(nèi)存為(RAM)16 GB，軟件采用Python 3.6編程。實(shí)驗(yàn)分為2個(gè)場景，如圖9所示，場景1為簡單環(huán)境，地圖大小設(shè)置為20 m×20 m，圖中x,y分別為地圖的橫坐標(biāo)和縱坐標(biāo),黑色代表障礙物，不能通行，白色區(qū)域?yàn)榭尚袇^(qū)域，環(huán)境布置時(shí)盡量在起始點(diǎn)和目標(biāo)點(diǎn)之間的路徑與周圍障礙物之間留下狹小的空間，主要分析改進(jìn)算法2個(gè)階段的運(yùn)行性能。煤礦井下空間狹小，加上大型生產(chǎn)設(shè)備，很容易出現(xiàn)連續(xù)狹小的極端環(huán)境，為了模擬煤礦井下環(huán)境，設(shè)計(jì)了場景2，地圖大小為22.5 m×22.5 m，黑色障礙物中間只留了距離為2 m的可通行區(qū)域，且連續(xù)設(shè)置3個(gè)可行區(qū)域不在一條直線的障礙物，以測試苛刻環(huán)境下的路徑規(guī)劃效果。場景1和場景2的起始點(diǎn)分別為(0,0)和(2.5 m,2.5 m)，目標(biāo)點(diǎn)都為(15 m,12 m)。

(a)場景1

3.2 簡單場景實(shí)驗(yàn)

在場景1中，使用MC-IRRT*算法和InformedRRT*算法進(jìn)行對比實(shí)驗(yàn)，測試MC-IRRT*算法的特性。在快速連通階段，考慮到采用的CPU為四核，設(shè)置多步長膜結(jié)構(gòu)為1個(gè)表層膜(表層膜0)、3個(gè)基本膜(基本膜1、基本膜2和基本膜3)，3個(gè)基本膜的步長分別設(shè)置為0.5,2.5和5，Informed RRT*算法在步長為0.5和2.5的條件下運(yùn)行，算法最大迭代次數(shù)為200。2種算法的實(shí)驗(yàn)結(jié)果如圖10所示。

圖10 快速連通階段實(shí)驗(yàn)結(jié)果對比

由圖10(a)可知，Informed RRT*算法在步長為0.5時(shí)運(yùn)行了170次，規(guī)劃出一條可行路徑，可以成功搜索到一條起始點(diǎn)到目標(biāo)點(diǎn)的路徑，但由于步長很小，需要多次迭代才能最終找到可行路徑。由圖10(b)可知，增大步長后Informed RRT*算法沒能到達(dá)目標(biāo)點(diǎn)，快速連通失敗，由于步長較大，很難搜索到較狹小的可行空間，最大迭代次數(shù)200次運(yùn)行結(jié)束后，也很難找到一條可行路徑。由圖10(c)可看出，由于改進(jìn)算法在此階段采用了多步長并行膜結(jié)構(gòu)，在狹小區(qū)域選擇小步長，能夠順利搜索到可行路徑，在較大可行空間選擇大步長，可以加快搜索速度，因此，在迭代40次之后便成功搜索到了一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑。相比于Informed RRT*算法，MC-IRRT*算法減少了130次迭代，搜索效率提高了76%。

在路徑尋優(yōu)階段，由2種算法得到的實(shí)驗(yàn)結(jié)果如圖11所示。對比圖11(a)和圖11(b)可看出，在迭代次數(shù)相同的情況下，MC-IRRT*算法得到的路徑更短，這是因?yàn)槠湓诼窂綄?yōu)階段使用了多采樣點(diǎn)膜結(jié)構(gòu)，利用MC的并行性，3個(gè)基本膜同時(shí)進(jìn)行采樣搜索，再與表層膜進(jìn)行信息粒子交換。對比圖11(a)和圖11(c)可看出，在找到相同優(yōu)化路徑的條件下，Informed RRT*算法需要迭代200次，而MC-IRRT*算法只需要迭代120次，路徑尋優(yōu)搜索效率提高了40%。

(a)MC-IRRT*,迭代120次

3.3 復(fù)雜場景實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證MC-IRRT*算法性能，選擇可通行路徑更加苛刻的場景2進(jìn)行實(shí)驗(yàn)，并與RRT*,Informed RRT*和PQ-RRT*算法進(jìn)行對比，4種算法均設(shè)置迭代次數(shù)為3 000次，實(shí)驗(yàn)結(jié)果如圖12所示。從圖12可看出：在迭代滿3 000次后，RRT*和Informed RRT*算法都無法找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑；PQ-RRT*算法成功尋找到了可行路徑并有所優(yōu)化，但由于使用的是固定步長增長樹，尋找到的路徑不夠平滑；MC-IRRT*算法由于使用了多步長，不僅可以迅速通過較窄可行區(qū)域，而且在路徑轉(zhuǎn)折處可以選擇使用較小步長，使路徑更加平滑。

(a)RRT*

復(fù)雜場景具體實(shí)驗(yàn)數(shù)據(jù)見表1?？梢钥闯?，PQ-RRT*算法和MC-IRRT*算法均能成功尋得可行路徑，PQ-RRT*算法平均用時(shí)17.05 s，而MC-IRRT*算法平均用時(shí)14.87 s，比PQ-RRT*算法少用了2.18 s，速率提高了12.79%。從尋到的平均路徑長度可以看出，MC-IRRT*算法規(guī)劃的路徑長度比PQ-IRRT*算法規(guī)劃的短8.18%。

表1 復(fù)雜場景具體實(shí)驗(yàn)數(shù)據(jù)

4 結(jié)論

(1)提出了一種基于MC的煤礦井下機(jī)器人路徑規(guī)劃算法，將MC的并行性與Informed RRT*算法相結(jié)合，使用多步長并行計(jì)算，既可以使用大步長進(jìn)行快速搜素，也可以采用小步長精細(xì)搜索，增加了在狹長空間中的搜索成功率，提高了搜索效率。使用多采樣點(diǎn)同時(shí)采樣搜索，在不大幅增加運(yùn)行時(shí)間成本的前提下，擴(kuò)展了采樣區(qū)域，提高了路徑優(yōu)化效率。

(2)簡單場景實(shí)驗(yàn)結(jié)果表明，與Informed RRT*算法相比，MC-IRRT*算法在快速連通階段和路徑尋優(yōu)階段的搜索效率分別提高了76%，40%。復(fù)雜場景實(shí)驗(yàn)結(jié)果表明：RRT*算法和Informed RRT*算法路徑規(guī)劃失敗，PQ-RRT*算法和MC-IRRT*算法均能成功尋得可行路徑；與PQ-RRT*算法相比，MC-IRRT*算法的速率提高了12.79%，規(guī)劃的路徑長度縮短了8.18%；MC-IRRT*算法不僅可以迅速通過較窄可行區(qū)域，而且在路徑轉(zhuǎn)折處可以選擇使用較小步長，使路徑更加平滑。

(3)MC-IRRT*算法充分利用了多核CPU的并行運(yùn)算能力，極大提高了路徑規(guī)劃的效率和實(shí)時(shí)性，為煤礦井下特殊環(huán)境下機(jī)器人的自主行駛提供了可靠保證。