基于復(fù)合探索策略的多機器人未知環(huán)境探索算法

柳全澤 姚芝鳳

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

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

中圖分類號：TP242?????文獻標識碼：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)境探索是機器人研究領(lǐng)域的一個重要方向，其目的是讓機器人在有限時間內(nèi)利用傳感器從地圖中獲取信息，以更小的成本和時間對未知環(huán)境進行探索。

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

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

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

1?基于簡化快速邊界探索的局部自主探索策略

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

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

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

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

（1）當(dāng)前像素點不是邊界單元格，則代表它可以被忽略，因為該點不存在新的有用信息。

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

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

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

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

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

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

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

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

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

3?過濾模塊

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

4任務(wù)分配模塊

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

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

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

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

所述的邊界點評估函數(shù)如公式（2）所示：

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

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

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

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

5仿真實驗

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

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

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

6結(jié)語

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

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