本文作者：計算機視覺工坊@K.Fire | 來源：計算機視覺工坊

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

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

作者：Baofu Fang ；Jianfeng Ding ; Zaijun Wang

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

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

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

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

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

• 基于數(shù)字圖像處理的邊緣檢測和區(qū)域提取技術(shù)：為了提取邊界邊緣，必須對整個地圖進行處理，隨著地圖的擴展，處理它將消耗越來越多的計算資源。
• Keidar和Kaminka后來提出了一種只處理新的激光讀取數(shù)據(jù)的邊界檢測算法，加快了計算速度，降低了計算資源的消耗。
• 快速探索隨機樹(rapid-exploration Random Tree, RRT)算法：由于RRT算法的隨機性，生成的邊界點分布不均勻。
• 基于廣度優(yōu)先搜索(BFS)的方法：這種方法簡單便捷，不重不漏，但在大地圖環(huán)境下，相對于其他算法可能計算速度稍慢

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

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

• 基于A*的路徑規(guī)劃算法
• 基于RRT算法生成機器人搜索路徑的算法
• 帶有信息論目標(biāo)函數(shù)的部分可觀察馬爾可夫決策過程(POMDP)
• 基于遺傳算法(GA)的路徑規(guī)劃方法
• 為傳統(tǒng)的基于邊界的探測策增加一個概率決策步驟，以決定在計劃路徑上進一步移動到下一個傳感位置是否可取。

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

文章從邊界點的信息增益、導(dǎo)航成本和機器人定位精度三個方面對邊界點進行評估。

• 信息增益被定義為對于一個給定邊界點預(yù)期被探索的未知區(qū)域面積。本文采用直接測量目標(biāo)邊界點可見區(qū)域內(nèi)未檢測到的空間大小的方法計算信息增益。以邊界點為圓心，以激光雷達探測距離為半徑形成圓。邊界點檢測圈如下圖所示。通過計算圓圈中未知單元格的數(shù)量來量化信息增益。
• 導(dǎo)航成本定義為機器人到達邊界點的預(yù)期距離，使用機器人當(dāng)前位置到目標(biāo)邊界點的歐氏距離來表示。
• 在目標(biāo)邊界點的探測范圍內(nèi)，如果能檢測到更多的直線特征或其他特征(如斷點、拐角、折線)，機器人就能更準(zhǔn)確地定位自己。文章用邊界點檢測圈內(nèi)障礙物的面積來表示定位精度。通過計算圓圈中被占用的單元數(shù)來量化。邊界點評價函數(shù)定義如下所示：

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

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

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

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

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

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

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

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

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