柳全澤 姚芝鳳

摘要：為解決多機(jī)器人探索算法存在的計(jì)算資源大，探索后期效率較低等問題，提出了一種基于復(fù)合探索策略的多機(jī)器人未知環(huán)境探索算法。該算法分為三3個(gè)模塊，首先，利用機(jī)器人傳感器構(gòu)建初始地圖，并使用基于快速邊界和基于RRT的探索算法來獲取邊界點(diǎn)，；其次，對(duì)邊界點(diǎn)進(jìn)行過濾和聚類以減少邊界點(diǎn)數(shù)量并降低計(jì)算量，；最后，通過任務(wù)分配模塊，綜合計(jì)算相對(duì)每個(gè)機(jī)器人的邊界點(diǎn)信息增益、導(dǎo)航成本和定位精度等因素后引導(dǎo)機(jī)器人前往未知區(qū)域。通過在不同開闊程度的仿真環(huán)境下進(jìn)行對(duì)比實(shí)驗(yàn)，結(jié)果表明，該算法提高了探索效率，并在復(fù)雜環(huán)境下表現(xiàn)出了更好的探索性能。

關(guān)鍵詞：RRT算法?快速邊界探索?任務(wù)分配?多機(jī)器人

中圖分類號(hào)：TP242?????文獻(xiàn)標(biāo)識(shí)碼：A

The?Multi-Robot?Unknown?Environment?Exploration?Algorithm?Based?on?the?Composite?Exploration?Strategy

LIU?QuanzZe???YAO?Zhifeng

（Qiqihar?University，Qiqihar，Heilongjiang?Province，161006?China）

Abstract：?A?multi-robot?unknown?environment?exploration?algorithm?based?on?the?composite?exploration?strategy?is?proposed?to?solve?the?problems?of?large?computing?resources?and?low?efficiency?in?the?later?stage?of?exploration?of?the?multi-robot?exploration?algorithm.?The?algorithm?is?divided?into?three?modules.?Firstly，?it?uses?robot?sensors?to?construct??the?initial?map，?and?uses?fast?boundary-based?and?RRT-based?exploration?algorithms?to?obtain?boundary?points.?Secondly，?it?filters?and?clusters?frontier?points?to?reduce?the?number?of?boundary?points?and?computing?effort.?Finally，?it?comprehensively?calculates?the?factors?such?as?the?boundary?point?information?gain，?navigation?cost?and?positioning?accuracy?relative?to?each?robot，?and?then?guides?robots?to?unknown?areas?by?the?task?assignment?module.Comparative?experiments?are?conducted?in?simulated?environments?with?different?degrees?of?openness，?and?results?show?that?the?algorithm?improves?exploration?efficiency?and?shows?better?exploration?performance?in?complex?environments.

Key?Words：?RRT?algorithm;?Fast?frontier?exploration;?Task?assignment;?Multi-robot

未知環(huán)境探索是機(jī)器人研究領(lǐng)域的一個(gè)重要方向，其目的是讓機(jī)器人在有限時(shí)間內(nèi)利用傳感器從地圖中獲取信息，以更小的成本和時(shí)間對(duì)未知環(huán)境進(jìn)行探索。

最早的自主探索策略是由Yamauchi提出的基于邊界的自主探索策略，其論文中提出將地圖分割為已知區(qū)域和未知區(qū)域，通過引導(dǎo)機(jī)器人前往已知區(qū)域與未知區(qū)域之間的邊界來完成探索任務(wù)。但該算法僅通過地圖來獲取邊界，對(duì)所有邊界都平等對(duì)待，使得計(jì)算資源巨大，也就導(dǎo)致了重復(fù)探索問題的出現(xiàn)。在此基礎(chǔ)上，孫旭東在其論文中針對(duì)邊界點(diǎn)選取問題提出了一種僅處理激光讀取數(shù)據(jù)的邊界檢測(cè)算法（（FFD算法）[1]FFD算法）。該算法雖然降低了邊界探索算法的時(shí)間，但是對(duì)候選邊界的評(píng)估有所欠缺，同樣容易產(chǎn)生無效不可達(dá)邊界。

在之后的研究中，鄧志超在其論文中提出了另一種基于快速探索隨機(jī)樹的自主探索策略，，簡(jiǎn)稱RFD算法[2]。該探索策略基于RRT算法，利用RRT算法對(duì)于未知區(qū)域的傾向性，通過RRT樹的生長(zhǎng)獲取到邊界點(diǎn)來引導(dǎo)機(jī)器人前往未知區(qū)域。但由于RRT算法的隨機(jī)性，生成的邊界點(diǎn)分布并不均勻，這會(huì)導(dǎo)致在狹窄地圖環(huán)境中探索效率下降，不能很好地完成自主探索任務(wù)。

針對(duì)以上問題，本該文提出了一種基于復(fù)合探索策略的多機(jī)器人邊界探測(cè)算法（Multi-robot?Frontier?Detection?Algorithm?Based?on?a?Composite?Exploration?Strategy，簡(jiǎn)稱MFD算法）。該算法將基于邊界的探索算法和基于RRT的探索算法進(jìn)行結(jié)合。首先，每個(gè)機(jī)器人單獨(dú)執(zhí)行一個(gè)基于快速邊界探索算法的局部探索模塊，通過傳感器獲取自身雷達(dá)探測(cè)范圍內(nèi)的邊界點(diǎn)；同時(shí)，以每個(gè)機(jī)器人的初始位置為根節(jié)點(diǎn)在整個(gè)地圖中生成RRT樹，構(gòu)建了一種多根節(jié)點(diǎn)的RRT全局探索算法。并引入動(dòng)態(tài)步長(zhǎng)機(jī)制，提高了RRT樹在已知區(qū)域的生長(zhǎng)速度。其次將兩種探索算法獲取到的邊界點(diǎn)發(fā)送到過濾模塊進(jìn)行聚類過濾。最后通過任務(wù)分配模塊，綜合考慮相對(duì)每個(gè)機(jī)器人的邊界點(diǎn)信息增益、導(dǎo)航成本和定位精度等因素，對(duì)每個(gè)機(jī)器人分配對(duì)應(yīng)的最優(yōu)邊界點(diǎn)以實(shí)現(xiàn)整個(gè)多機(jī)器人自主探索策略。整個(gè)探索策略流程如下圖1所示。

1?基于簡(jiǎn)化快速邊界探索的局部自主探索策略

基于邊界的自主探索策略是目前較為常用的一種自主探索策略，當(dāng)機(jī)器人地圖中移動(dòng)同時(shí)構(gòu)建新地圖時(shí)，基于邊界的檢測(cè)算法可以立即識(shí)別地圖上的邊界點(diǎn)。本文該文采用一種基于快速邊界探索的探索算法[13]并對(duì)其進(jìn)行改進(jìn)。由于可以通過后續(xù)的邊界點(diǎn)過濾模塊對(duì)探索到的邊界點(diǎn)進(jìn)行過濾，從而不需要對(duì)先前探測(cè)到的邊界點(diǎn)進(jìn)行維護(hù)。因此，改進(jìn)后的快速邊界探測(cè)算法僅包含三3個(gè)步驟：（1），分別為排序分類；（2）、構(gòu)建邊界輪廓；（3）、探測(cè)新邊界。

整體探索流程如下圖2所示。

首先（步驟1）以每個(gè)機(jī)器人自身為中心作為極坐標(biāo)的原點(diǎn)，根據(jù)角度對(duì)傳感器范圍內(nèi)的讀數(shù)進(jìn)行排序。通過SORT_POLAR函數(shù)進(jìn)行坐標(biāo)轉(zhuǎn)換，隨后根據(jù)排序好的激光數(shù)據(jù)讀數(shù)構(gòu)建邊界輪廓（步驟2-7），根據(jù)激光雷達(dá)獲取的數(shù)據(jù)，通過調(diào)用GET_LINE函數(shù)計(jì)算每個(gè)相鄰像素點(diǎn)之間的連線，并將所有的連線與激光讀數(shù)獲取的點(diǎn)融合構(gòu)成邊界輪廓。

最后，我們根據(jù)算法計(jì)算獲得的輪廓構(gòu)建新邊界（步驟8-21），對(duì)于輪廓中的相連的每個(gè)像素點(diǎn)，存在三3種情況。

（1）當(dāng)前像素點(diǎn)不是邊界單元格，則代表它可以被忽略，因?yàn)樵擖c(diǎn)不存在新的有用信息。

（2）當(dāng)前像素點(diǎn)和其相連的點(diǎn)都是邊界單元格，則證明兩個(gè)點(diǎn)都屬于同一邊界點(diǎn)，則將當(dāng)前像素點(diǎn)作為邊界點(diǎn)發(fā)送到過濾模塊，并略過剩余與其相連的單元格。

（3）當(dāng)前像素點(diǎn)為邊界點(diǎn)，而與其相連的像素點(diǎn)不為邊界單元格，則僅將當(dāng)前像素點(diǎn)作為新的邊界點(diǎn)發(fā)送到過濾模塊。

通過上述流程，我們可以從每個(gè)機(jī)器人通過激光雷達(dá)獲取的數(shù)據(jù)中獲取其附近的邊界點(diǎn)，并通過過濾模塊對(duì)其進(jìn)行聚類和過濾，對(duì)冗余邊界點(diǎn)和已探索過的邊界點(diǎn)進(jìn)行刪除，將剩余的邊界點(diǎn)發(fā)送給任務(wù)分配模塊分配給每個(gè)機(jī)器人以實(shí)現(xiàn)自主探索。

2?基于多根節(jié)點(diǎn)的改進(jìn)RRT自主探索策略

該探索策略與寧宇銘等人論文[24]中的全局探測(cè)模塊算法流程基本相同，我們?cè)谄浠A(chǔ)上進(jìn)行了一定程度的改進(jìn)。RRT樹首先以每個(gè)機(jī)器人的初始位置作為初始頂點(diǎn)和邊集合開始，并且在每次迭代時(shí)都會(huì)通過函數(shù)在地圖中隨機(jī)選取一個(gè)點(diǎn)，在RRT樹中找到距離最近的一個(gè)頂點(diǎn)。然后，通過函數(shù)判斷與之間是否存在障礙物或未知區(qū)域，根據(jù)判斷結(jié)果，分三3種情況決定RRT樹的生長(zhǎng)步長(zhǎng)。

（1）如果函數(shù)返回值為1，即與之間不存在障礙物和未探索區(qū)域，那么就將直接作為新節(jié)點(diǎn)加入到樹中。

（2）如果函數(shù)返回值為-1，即與之間存在未知區(qū)域，則進(jìn)行二次判斷，首先沿向xrand以最大步長(zhǎng)進(jìn)行延伸獲得點(diǎn)，之后再次通過函數(shù)判斷與之間是否仍存在未知區(qū)域，若仍舊存在未知區(qū)域，那么就沿向方向獲取距離最近的處于未知區(qū)域的點(diǎn)作為邊界點(diǎn)發(fā)送到過濾器，隨后重置RRT樹并刪除所有節(jié)點(diǎn)，并以機(jī)器人當(dāng)前位置的坐標(biāo)為初始點(diǎn)重新生長(zhǎng)；若與之間為已知區(qū)域，則將作為新節(jié)點(diǎn)加入到樹中。

（3）如果函數(shù)返回值為0，即與之間存在障礙物，則計(jì)算沿著方向與最近的障礙物之間的距離，記為，如果大于步長(zhǎng)的最大值，則取為步長(zhǎng)從向方向生長(zhǎng)得到新節(jié)點(diǎn)加入樹中，如果小于步長(zhǎng)的最小值，則放棄這個(gè)點(diǎn)，重新選擇新的點(diǎn)，也可以認(rèn)為步長(zhǎng)取0；否則以為步長(zhǎng)獲取新節(jié)點(diǎn)加入到樹中，如公式（1）所示。之后再次判定與之間是否與障礙物發(fā)生碰撞，如果仍舊發(fā)生碰撞則舍棄這條路徑，如果不發(fā)生碰撞則將作為新節(jié)點(diǎn)加入樹中。根據(jù)上述三3種情況的描述，就可以實(shí)現(xiàn)RRT樹的動(dòng)態(tài)步長(zhǎng)生長(zhǎng)，隨著已知區(qū)域的不斷擴(kuò)大，引入動(dòng)態(tài)步長(zhǎng)機(jī)制的RRT樹會(huì)在已知區(qū)域快速生長(zhǎng)，同時(shí)降低向障礙區(qū)域生長(zhǎng)的步長(zhǎng)，從而提高對(duì)未知區(qū)域的探索速度。

改進(jìn)后的RRT自主探索策略算法流程圖3所示。

3?過濾模塊

過濾模塊的目的是接收上述兩種探索策略所獲取到的所有邊界點(diǎn)并對(duì)其進(jìn)行聚類過濾。由于這些邊界點(diǎn)有些可能彼此非常接近，如果將這些邊界點(diǎn)全部都發(fā)送到任務(wù)分配模塊來進(jìn)行任務(wù)分配將占用大量計(jì)算資源，因此，我們采用mean-shift算法對(duì)這些邊界點(diǎn)進(jìn)行聚類選取其中較優(yōu)的邊界點(diǎn)發(fā)送到任務(wù)分配模塊，同時(shí)刪除無效邊界點(diǎn)和已探索過的邊界點(diǎn)以減少計(jì)算量。

4任務(wù)分配模塊

該模塊從過濾模塊獲取過濾后的邊界點(diǎn)，并將其分配給機(jī)器人，采用與陰賀生等人論文[35]類似的任務(wù)分配算法，任務(wù)分配模塊對(duì)每個(gè)過濾后的邊界點(diǎn)將考慮以下因素來構(gòu)建邊界點(diǎn)評(píng)估函數(shù)，用于分配每個(gè)機(jī)器人要探索的邊界點(diǎn)具體敘述如下。

信息增益（I）：信息增益定義為到達(dá)給定邊界點(diǎn)預(yù)計(jì)能夠探索到的未知區(qū)域的面積。該面積通過計(jì)算以該邊界點(diǎn)為中心，以半徑（為機(jī)器人雷達(dá)探索半徑）所畫的圓內(nèi)的未知單元格的數(shù)量乘以每個(gè)單元格的面積來確定。

導(dǎo)航成本（N）：導(dǎo)航成本為機(jī)器人到達(dá)該邊界點(diǎn)的預(yù)期距離。為了簡(jiǎn)化計(jì)算，本文僅考慮機(jī)器人當(dāng)前位置與邊界點(diǎn)之間的距離的范數(shù)作為導(dǎo)航成本。

建圖精度（F）：建圖精度定義為給定邊界點(diǎn)預(yù)計(jì)能夠探索到的障礙區(qū)域的面積。是以邊界點(diǎn)為圓心，以半徑所畫的已知區(qū)域內(nèi)障礙物的面積。由于地圖中的障礙物可以作為構(gòu)建地圖的某些標(biāo)定點(diǎn)，通過考慮該參數(shù)，在提升機(jī)器人自身定位精度的同時(shí)也提高了地圖繪制的精度。

所述的邊界點(diǎn)評(píng)估函數(shù)如公式（2）所示：

式（2）中，：α、β為權(quán)重參數(shù)，分別用于控制建圖精度的權(quán)重以及控制導(dǎo)航成本的權(quán)重，若需要提高建圖精度則提高α值并降低β值；若需要更快的建圖速度則提高β值并降低α值。；為當(dāng)前機(jī)器人坐標(biāo)到邊界點(diǎn)的滯后增益[46]。

滯后增益的計(jì)算公式如（3）所示：。

式（3）中，|為機(jī)器人當(dāng)前坐標(biāo)到邊界點(diǎn)的直線距離，若該距離小于機(jī)器人傳感器的半徑長(zhǎng)度，就將該參數(shù)設(shè)為1；若該距離小于機(jī)器人傳感器的半徑長(zhǎng)度，則該參數(shù)設(shè)為（為用戶設(shè)置的一個(gè)大于1的常數(shù)）。該參數(shù)能夠使機(jī)器人優(yōu)先探索自身附近的邊界點(diǎn)。

對(duì)于每個(gè)邊界點(diǎn)，我們都使用公式（2）進(jìn)行收益計(jì)算，并將其中收益最高的邊界點(diǎn)分配給相對(duì)應(yīng)的機(jī)器人，引導(dǎo)機(jī)器人前往該點(diǎn)進(jìn)行探索。

5仿真實(shí)驗(yàn)

由于本該文提出了將兩種探索策略進(jìn)行結(jié)合的復(fù)合式自主探索策略，因此，我們首先針對(duì)探索策略方面與孫旭東論文[51]提出的FFD探索算法以及鄧志超論文[62]提出的RFD算法進(jìn)行對(duì)比實(shí)驗(yàn)，為了便于對(duì)比，三這3種探索策略我們都選用了與鄧志超論文[62]相同的路徑規(guī)劃模塊和SLAM模塊以便于進(jìn)行對(duì)比實(shí)驗(yàn)。具體相關(guān)信息請(qǐng)參考[6]。

本該文通過Gazebo仿真實(shí)驗(yàn)平臺(tái)進(jìn)行模擬仿真實(shí)驗(yàn)，仿真機(jī)器人采用三3臺(tái)kubuki移動(dòng)機(jī)器人，同時(shí)根據(jù)地圖的復(fù)雜程度，建立復(fù)雜程度不同的兩種仿真環(huán)境，一種為障礙物較稀疏的20?m×*20?m仿真環(huán)境，一種為障礙物較稠密的20?m×*40?m仿真環(huán)境。圖4和圖5分別表示探索完成后的環(huán)境和相應(yīng)的柵格地圖模型。在兩種仿真實(shí)驗(yàn)環(huán)境下，我們?cè)O(shè)定改進(jìn)后的全局RRT探測(cè)模塊與RFD算法[62]中的全局探測(cè)模塊的步長(zhǎng)相同，分別設(shè)置為1?m、2?m、4?m、8?m、10?m；同時(shí)，在每個(gè)環(huán)境中，在每個(gè)機(jī)器人隨機(jī)初始位置的情況下每步長(zhǎng)進(jìn)行20次實(shí)驗(yàn)。將構(gòu)建地圖所用的平均時(shí)間繪制為直方圖，具體情況如圖6所示。

根據(jù)圖6可以看出，對(duì)比貪婪邊界探索算法[7]和RFD算法，在開闊地形仿真環(huán)境中，完成探索所需平均時(shí)間分別提高了14.4%和10.2%，同時(shí)在狹窄地形仿真環(huán)境中分別提高了22.5%和33.8%。

6結(jié)語

在本文中，我們?cè)撗芯刻岢隽艘环N基于復(fù)合探索策略的多機(jī)器人探索算法。通過結(jié)合基于邊界的探索算法和基于RRT的探索算法。首先，每個(gè)機(jī)器人單獨(dú)執(zhí)行一個(gè)基于快速邊界的探索算法，僅通過傳感器獲取自身探測(cè)范圍內(nèi)的邊界點(diǎn)；同時(shí)，以每個(gè)機(jī)器人的初始位置為根坐標(biāo)，構(gòu)建了一種多根節(jié)點(diǎn)的RRT自主探索策略，并引入動(dòng)態(tài)步長(zhǎng)機(jī)制提高了RRT樹的生長(zhǎng)速度。最后在仿真實(shí)驗(yàn)環(huán)境下建立了仿真實(shí)驗(yàn)環(huán)境，實(shí)驗(yàn)結(jié)果表明，本該算法在提高了探索效率，在復(fù)雜環(huán)境下能夠表現(xiàn)出更好的性能。證明了該方法的可行性及優(yōu)越性。

參考文獻(xiàn)[1] 孫旭東.基于ROS的輪式機(jī)器人自主融合探索建圖與路徑規(guī)劃[D].石家莊：石家莊鐵道大學(xué)，2017.