基于邊界點(diǎn)優(yōu)化和多步路徑規(guī)劃的機(jī)器人自主探索

論文題目：Autonomous Robotic Exploration Based on Frontier Point Optimization and Multistep Path Planning

中文題目：基于邊界點(diǎn)優(yōu)化和多步路徑規(guī)劃的機(jī)器人自主探索

作者：Baofu Fang ；Jianfeng Ding ; Zaijun Wang

作者機(jī)構(gòu)：合肥工業(yè)大學(xué)計(jì)算機(jī)科學(xué)與信息工程學(xué)院

論文鏈接：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8681502

機(jī)器人對(duì)未知環(huán)境的自主探索是機(jī)器人智能化的關(guān)鍵技術(shù)。為了提高搜索效率，作者提出了一種基于邊界點(diǎn)優(yōu)化和多步路徑規(guī)劃的搜索策略。他們主要對(duì)邊界點(diǎn)優(yōu)化、邊界點(diǎn)選擇、路徑規(guī)劃三個(gè)方面對(duì)路徑規(guī)劃算法進(jìn)行改進(jìn)。在邊界點(diǎn)優(yōu)化部分，提出了一種隨機(jī)邊界點(diǎn)優(yōu)化(RFPO)算法，選擇評(píng)價(jià)值最高的邊界點(diǎn)作為目標(biāo)邊界點(diǎn)。綜合考慮信息增益、導(dǎo)航成本和機(jī)器人定位精度，來(lái)確定邊界點(diǎn)的評(píng)價(jià)函數(shù)。在路徑規(guī)劃部分，提出了一個(gè)多步探索策略。沒(méi)有直接規(guī)劃從機(jī)器人當(dāng)前位置到目標(biāo)邊界點(diǎn)的全局路徑，而是設(shè)置了局部探索路徑步長(zhǎng)。當(dāng)機(jī)器人的運(yùn)動(dòng)距離達(dá)到局部探索路徑步長(zhǎng)時(shí)，重新選擇當(dāng)前最優(yōu)邊界點(diǎn)進(jìn)行路徑規(guī)劃，以減少機(jī)器人走一些重復(fù)路徑的可能性。最后，通過(guò)相關(guān)實(shí)驗(yàn)驗(yàn)證了該策略的有效性。

1 前言

機(jī)器人自主探索是機(jī)器人領(lǐng)域的一個(gè)重要研究課題。主要目標(biāo)是讓機(jī)器人在有限時(shí)間且無(wú)需人工干預(yù)的情況下，獲得最完整、最準(zhǔn)確的環(huán)境地圖。許多現(xiàn)有的地圖探索策略都是基于邊界，邊界定義為未知空間與已知空間的分界線?；谶吔绲奶剿鞑呗缘乃枷胧且龑?dǎo)機(jī)器人到未知區(qū)域完成探索任務(wù)，因此自主探索任務(wù)一般分為三個(gè)步驟:生成邊界點(diǎn)、選擇評(píng)價(jià)值最高的邊界點(diǎn)、規(guī)劃前往所選邊界點(diǎn)的路徑。

邊界點(diǎn)的生成以基于邊界的探索策略為前提。在現(xiàn)有的研究中，常見(jiàn)地邊界點(diǎn)生成算法有：

基于數(shù)字圖像處理的邊緣檢測(cè)和區(qū)域提取技術(shù)：為了提取邊界邊緣，必須對(duì)整個(gè)地圖進(jìn)行處理，隨著地圖的擴(kuò)展，處理它將消耗越來(lái)越多的計(jì)算資源。

Keidar和Kaminka后來(lái)提出了一種只處理新的激光讀取數(shù)據(jù)的邊界檢測(cè)算法，加快了計(jì)算速度，降低了計(jì)算資源的消耗。

快速探索隨機(jī)樹(shù)(rapid-exploration Random Tree, RRT)算法：由于RRT算法的隨機(jī)性，生成的邊界點(diǎn)分布不均勻。

基于廣度優(yōu)先搜索(BFS)的方法：這種方法簡(jiǎn)單便捷，不重不漏，但在大地圖環(huán)境下，相對(duì)于其他算法可能計(jì)算速度稍慢

目標(biāo)邊界點(diǎn)的選擇是有效探索的關(guān)鍵。以邊界為基礎(chǔ)的戰(zhàn)略是由Yamauchi首先提出的。所使用的探索策略是識(shí)別當(dāng)前地圖中的所有邊界區(qū)域，然后驅(qū)動(dòng)機(jī)器人前往最近的邊界點(diǎn)。這種方法對(duì)于探索任務(wù)有兩個(gè)缺點(diǎn)。首先，它平等對(duì)待所有邊界。其次，它僅限于一個(gè)信息來(lái)源:尋找新邊界區(qū)域。因此很多研究者提出了多種不同的邊界點(diǎn)選擇算法，來(lái)改善這一情況。這篇文章是綜合考慮信息增益、導(dǎo)航成本和機(jī)器人定位精度，來(lái)確定邊界點(diǎn)的評(píng)價(jià)函數(shù)。

最后是機(jī)器人的路徑規(guī)劃部分，對(duì)于機(jī)器人自主探索中路徑規(guī)劃的研究已經(jīng)存在很多高效的算法：

基于A*的路徑規(guī)劃算法

基于RRT算法生成機(jī)器人搜索路徑的算法

帶有信息論目標(biāo)函數(shù)的部分可觀察馬爾可夫決策過(guò)程(POMDP)

基于遺傳算法(GA)的路徑規(guī)劃方法

為傳統(tǒng)的基于邊界的探測(cè)策增加一個(gè)概率決策步驟，以決定在計(jì)劃路徑上進(jìn)一步移動(dòng)到下一個(gè)傳感位置是否可取。

2 最優(yōu)邊界點(diǎn)提取

2.1邊界點(diǎn)的生成

文中采用ROS平臺(tái)下常見(jiàn)的SLAM算法--GMapping構(gòu)建二維占用網(wǎng)格圖，然后使用RRT算法在地圖中生成邊界點(diǎn)，如果新生成的點(diǎn)位于未知區(qū)域，則認(rèn)為該點(diǎn)為邊界點(diǎn)。它會(huì)被標(biāo)記在地圖上，然后我們停止這棵樹(shù)的生長(zhǎng)。將當(dāng)前機(jī)器人的位置作為新的根節(jié)點(diǎn)，我們構(gòu)建一個(gè)新的快速探索隨機(jī)樹(shù)來(lái)生成邊界點(diǎn)。邊界點(diǎn)生成示例如下圖所示。

2.2邊界點(diǎn)評(píng)價(jià)函數(shù)

文章從邊界點(diǎn)的信息增益、導(dǎo)航成本和機(jī)器人定位精度三個(gè)方面對(duì)邊界點(diǎn)進(jìn)行評(píng)估。

信息增益被定義為對(duì)于一個(gè)給定邊界點(diǎn)預(yù)期被探索的未知區(qū)域面積。本文采用直接測(cè)量目標(biāo)邊界點(diǎn)可見(jiàn)區(qū)域內(nèi)未檢測(cè)到的空間大小的方法計(jì)算信息增益。以邊界點(diǎn)為圓心，以激光雷達(dá)探測(cè)距離為半徑形成圓。邊界點(diǎn)檢測(cè)圈如下圖所示。通過(guò)計(jì)算圓圈中未知單元格的數(shù)量來(lái)量化信息增益。

導(dǎo)航成本定義為機(jī)器人到達(dá)邊界點(diǎn)的預(yù)期距離，使用機(jī)器人當(dāng)前位置到目標(biāo)邊界點(diǎn)的歐氏距離來(lái)表示。

在目標(biāo)邊界點(diǎn)的探測(cè)范圍內(nèi)，如果能檢測(cè)到更多的直線特征或其他特征(如斷點(diǎn)、拐角、折線)，機(jī)器人就能更準(zhǔn)確地定位自己。文章用邊界點(diǎn)檢測(cè)圈內(nèi)障礙物的面積來(lái)表示定位精度。通過(guò)計(jì)算圓圈中被占用的單元數(shù)來(lái)量化。邊界點(diǎn)評(píng)價(jià)函數(shù)定義如下所示：

其中α、β、γ分別為信息增益、導(dǎo)航成本和障礙物面積的權(quán)重。這些權(quán)重用于調(diào)整不同因素的重要性,可根據(jù)不同的任務(wù)和環(huán)境進(jìn)行設(shè)置。如果探索任務(wù)要求盡快完成探索，則增加α，降低γ;如果探索任務(wù)更注重地圖的準(zhǔn)確性，則減少α，增加γ。β通常取1表示單位導(dǎo)航成本下的增益值。利用邊界點(diǎn)評(píng)價(jià)函數(shù)對(duì)所有邊界點(diǎn)進(jìn)行評(píng)價(jià)。選取值最大的點(diǎn)作為目標(biāo)邊界點(diǎn)。

PS：在計(jì)算導(dǎo)航成本時(shí)，如果環(huán)境地圖過(guò)于復(fù)雜，可以選擇使用A*規(guī)劃出的路徑長(zhǎng)度作為導(dǎo)航成本項(xiàng)，但會(huì)造成過(guò)大的資源計(jì)算消耗；另外，作者提出的這種計(jì)算模型是比較合理的，導(dǎo)航成本在分母上，也就是說(shuō)距離越大得分越低，信息增益和障礙物面積在分子上，機(jī)器人在選擇目標(biāo)點(diǎn)時(shí)，肯定傾向于選擇信息增益高、導(dǎo)航精度高的目標(biāo)點(diǎn)。

2.3隨機(jī)邊界點(diǎn)優(yōu)化算法

由于邊界點(diǎn)的生成部分始終運(yùn)行在整個(gè)探測(cè)過(guò)程中，因此隨著探測(cè)任務(wù)的執(zhí)行，將得到許多邊界點(diǎn)。然而，由于RRT算法的隨機(jī)性，這些邊界點(diǎn)的分布是不均勻的。因此，需要對(duì)生成的邊界點(diǎn)進(jìn)行優(yōu)化。文中借鑒GSO算法的思想，提出RFPO算法。GSO算法是一種新型的仿生群體智能優(yōu)化算法。它模擬了高亮度螢火蟲(chóng)會(huì)吸引低亮度螢火蟲(chóng)向其移動(dòng)的自然現(xiàn)象，使所有螢火蟲(chóng)集中在一個(gè)更好的位置，從而實(shí)現(xiàn)問(wèn)題的優(yōu)化。在RFPO算法中，將每個(gè)邊界點(diǎn)視為一只螢火蟲(chóng)，并將邊界點(diǎn)評(píng)價(jià)函數(shù)的值E作為其絕對(duì)亮度值L：

螢火蟲(chóng)會(huì)被絕對(duì)亮度值更大的螢火蟲(chóng)所吸引，并向其移動(dòng)。這種吸引力的大小由螢火蟲(chóng)對(duì)螢火蟲(chóng)的相對(duì)亮度值決定，螢火蟲(chóng)在螢火蟲(chóng)所在位置的亮度強(qiáng)度定義為螢火蟲(chóng)對(duì)螢火蟲(chóng)的相對(duì)亮度，相對(duì)亮度值越大，吸引力越大。然后對(duì)相對(duì)亮度進(jìn)行建模。對(duì)于每個(gè)邊界點(diǎn)，都有一個(gè)感知半徑。它的值應(yīng)根據(jù)感知傳感器的范圍來(lái)設(shè)置。在這個(gè)范圍內(nèi)，每一個(gè)邊界點(diǎn)都會(huì)找到絕對(duì)亮度值大于自己的其他邊界點(diǎn)，形成自己的鄰域集。在鄰域集中使用輪盤(pán)賭的方法，選擇下一個(gè)要移動(dòng)的目標(biāo)點(diǎn)。

PS：這里的建模過(guò)程不是文章主要內(nèi)容，就不過(guò)多展開(kāi)，感興趣可以在原文查看詳細(xì)推導(dǎo)過(guò)程。

3 多步探索策略

采用多步探索的原因：在機(jī)器人運(yùn)動(dòng)的過(guò)程中，會(huì)產(chǎn)生一些新的邊界點(diǎn)，一些舊的邊界點(diǎn)會(huì)失效，而新生成的邊界點(diǎn)可能會(huì)優(yōu)于當(dāng)前的最優(yōu)目標(biāo)邊界點(diǎn)。這可能會(huì)導(dǎo)致機(jī)器人選擇一些重復(fù)的路徑。

文中解決方法：定義了一個(gè)局部探索路徑步長(zhǎng)。每次當(dāng)機(jī)器人的運(yùn)動(dòng)距離達(dá)到步長(zhǎng)時(shí)，就清除無(wú)效點(diǎn)，并對(duì)所有剩余的邊界點(diǎn)進(jìn)行重新優(yōu)化和重新選擇。在每個(gè)局部探索路徑步長(zhǎng)內(nèi)，采用動(dòng)態(tài)窗口法進(jìn)行機(jī)器人避障局部路徑規(guī)劃。

PS：動(dòng)態(tài)窗口法是ROS中常見(jiàn)地局部路徑規(guī)劃方法，主要思想是在速度空間中進(jìn)行采樣，生成下一時(shí)間步的模擬軌跡，然后根據(jù)評(píng)估函數(shù)選擇得分最高的路徑，驅(qū)動(dòng)機(jī)器人移動(dòng)。

如下圖所示，黃色點(diǎn)為目標(biāo)邊界點(diǎn)。作者在不同的速度集上模擬了許多軌跡。根據(jù)他們定義的軌跡評(píng)價(jià)函數(shù)，選取得分最高的軌跡（下圖中用紅色標(biāo)記的路徑）?？梢钥闯觯瑱C(jī)器人執(zhí)行紅色軌跡可以快速到達(dá)目標(biāo)邊界點(diǎn)。同時(shí)，軌跡與墻體有一定的安全距離，墻體邊界可以幫助機(jī)器人更準(zhǔn)確地定位自身。

4 實(shí)驗(yàn)與結(jié)果

4.1 實(shí)驗(yàn)設(shè)置

通過(guò)仿真地圖和真實(shí)地圖對(duì)所提出策略的性能進(jìn)行了實(shí)驗(yàn)驗(yàn)證，并與其他策略進(jìn)行了比較。所有用于比較的策略都是在運(yùn)行Ubuntu 14.04的Intel core i7 3.60GHz處理器和8GB RAM的計(jì)算機(jī)上使用ROS庫(kù)在c++中開(kāi)發(fā)為ROS組件。

文章實(shí)驗(yàn)參數(shù)表如下：

對(duì)于仿真環(huán)境，我們使用Gazebo模擬器構(gòu)建一個(gè)封閉空間，如下圖4(a)所示?？紤]到地圖尺寸變化的影響，使用了不同的地圖尺寸(2020m, 4040m, 6060m)。機(jī)器人的半徑為0.2m，激光傳感器的范圍設(shè)置為10m。圖4(c)為在2020m的模擬環(huán)境中建立的二維占用網(wǎng)格圖。

在真實(shí)環(huán)境中，作者利用擋板構(gòu)建了一個(gè)10m * 10m的空間，如下圖4(b)所示。實(shí)驗(yàn)中使用的移動(dòng)機(jī)器人平臺(tái)為EAIBOT Dashgo-D1。它配備了一個(gè)Hokuyo UST-10LX 2D激光傳感器(10米的檢測(cè)范圍和270°視野)。圖4(d)為在真實(shí)環(huán)境中構(gòu)建的二維占用網(wǎng)格圖。

4.2 邊界點(diǎn)優(yōu)化結(jié)果

如下圖5(a)所示。由于RRT算法的隨機(jī)性，邊界點(diǎn)的位置都是隨機(jī)的?？梢钥闯觯械倪吔缬泻芏噙吔琰c(diǎn)，有的邊界只有很少的邊界點(diǎn)。此外，在地圖的各個(gè)邊界上，邊界點(diǎn)的分布也不均勻。圖5(b)是用文中提出的算法生成的邊界點(diǎn)。優(yōu)化后邊界點(diǎn)數(shù)量大大減少，各邊界上的邊界點(diǎn)分布基本均勻。黃色點(diǎn)是根據(jù)定義的邊界點(diǎn)評(píng)價(jià)函數(shù)計(jì)算出的當(dāng)前情況下的最優(yōu)邊界點(diǎn)。

4.3 多步探索策略的結(jié)果

如圖6(a)和圖6(b)所示，在傳統(tǒng)的全局路徑規(guī)劃策略下，機(jī)器人直接規(guī)劃從當(dāng)前位置到目標(biāo)邊界點(diǎn)的路徑，并搜索下一個(gè)目標(biāo)邊界點(diǎn)，直到到達(dá)前一個(gè)目標(biāo)邊界點(diǎn)。

在多步路徑規(guī)劃策略中，每當(dāng)機(jī)器人的運(yùn)動(dòng)距離達(dá)到確定的局部探索路徑步長(zhǎng)時(shí)，都會(huì)重新計(jì)算并重新選擇最優(yōu)邊界點(diǎn)。從下面的圖6(c)可以看出，在機(jī)器人到達(dá)圖6(a)中的目標(biāo)邊界點(diǎn)之前，當(dāng)前的最優(yōu)邊界點(diǎn)已經(jīng)發(fā)生了變化。因此，機(jī)器人已經(jīng)規(guī)劃了一條新的路徑。結(jié)果是圖6(c)的路徑長(zhǎng)度明顯短于圖6(a)和圖6(b)。

4.4 與其他策略的比較

文中總共進(jìn)行了200組實(shí)驗(yàn)，將提出的策略與其他四種策略進(jìn)行比較。策略1的思想是隨機(jī)選擇邊界點(diǎn)進(jìn)行探索，稱之為RANDOM。策略2的思想是選擇離機(jī)器人最近的邊界點(diǎn)，用NEAREST來(lái)表示它。策略3采用了貪婪算法的思想，因此將其記為GREEDY。策略4是用UMARI來(lái)描述。本文中提出的策略稱為RFPO。為了比較不同地圖尺寸對(duì)探索策略的影響，作者使用了4張不同尺寸的地圖進(jìn)行實(shí)驗(yàn)(一張真實(shí)地圖和3張不同尺寸的模擬地圖)。每張地圖都要進(jìn)行50組探索作業(yè)。這50次探索分為5組，每組代表一種探索策略。在40*40 m的模擬地圖中，對(duì)于每種策略，從10次探索運(yùn)行中選擇一個(gè)實(shí)驗(yàn)結(jié)果來(lái)顯示機(jī)器人的探索軌跡。結(jié)果如圖7所示。

圖8為四種不同地圖中不同探索策略探索結(jié)束時(shí)的探索時(shí)間，圖9為四種不同地圖中不同探索策略探索結(jié)束時(shí)的探索距離。

分析：從圖中可以看出，地圖的尺寸越大，不同策略之間探索效率的差異就越明顯。在60*60m的模擬地圖上，文中提出的策略與其他四種策略相比，平均探索時(shí)間分別減少了26.71%、7.36%、5.56%、1.62%，平均探索距離分別減少了31.22%、15.56%、14.61%、8.43%。

對(duì)于RANDOM策略來(lái)說(shuō)，由于每次的目標(biāo)邊界點(diǎn)都是隨機(jī)選擇的，機(jī)器人會(huì)走很多重復(fù)的路線，所以探測(cè)時(shí)間和探測(cè)距離都會(huì)增加。

而NEAREST策略和GREEDY策略會(huì)導(dǎo)致搜索變成局部最優(yōu)問(wèn)題，影響搜索的效率。

UMARI的策略直接規(guī)劃了機(jī)器人從當(dāng)前位置到探測(cè)目標(biāo)點(diǎn)的路徑，這可能會(huì)導(dǎo)致圖6中的問(wèn)題。

實(shí)驗(yàn)結(jié)果表明，無(wú)論是與探測(cè)時(shí)間相比，還是與探測(cè)距離相比，文中提出的探測(cè)策略的效果都優(yōu)于其他策略，證明了所提出策略的有效性。

5 總結(jié)

本文提出了一種基于邊界點(diǎn)優(yōu)化和多步路徑規(guī)劃的機(jī)器人自主探索策略。該策略可以驅(qū)動(dòng)機(jī)器人探索未知環(huán)境，并在無(wú)需人工干預(yù)的情況下高效地構(gòu)建相應(yīng)的二維占用柵格地圖。在這個(gè)探索策略中，作者使用RRT算法來(lái)生成邊界點(diǎn)，并提出RFPO算法來(lái)優(yōu)化這些邊界點(diǎn)。然后定義了邊界點(diǎn)評(píng)價(jià)函數(shù)，選取當(dāng)前最優(yōu)邊界點(diǎn)進(jìn)行探索。在路徑規(guī)劃部分，設(shè)置了一個(gè)局部探索路徑步長(zhǎng)，當(dāng)機(jī)器人的運(yùn)動(dòng)距離達(dá)到局部探索路徑步長(zhǎng)時(shí)，重新選擇目標(biāo)邊界點(diǎn)進(jìn)行探索，以減少機(jī)器人走一些重復(fù)路徑的可能性。最后通過(guò)實(shí)驗(yàn)，驗(yàn)證了所提策略的有效性。

未來(lái)改進(jìn)：目前只使用里程計(jì)數(shù)據(jù)結(jié)合激光傳感器數(shù)據(jù)來(lái)構(gòu)建二維占用網(wǎng)格地圖，地圖中包含的信息相對(duì)較少。

可以將視覺(jué)傳感器數(shù)據(jù)融合到自主探索中，視覺(jué)傳感器數(shù)據(jù)的優(yōu)點(diǎn)是可以獲得更多的環(huán)境信息，這些數(shù)據(jù)可以被融合在一起，為以后的導(dǎo)航任務(wù)和其他相關(guān)工作構(gòu)建具有更豐富信息的地圖。

此外，可以嘗試協(xié)調(diào)多個(gè)機(jī)器人進(jìn)行高效探索。