ROS平臺下三種自主探索算法總結(jié)及原理分析

作者：K.Fire | 來源：3DCV

1 前言

探索是指當(dāng)機(jī)器人處于一個(gè)完全未知或部分已知環(huán)境中，通過一定的方法，在合理的時(shí)間內(nèi)，盡可能多的獲得周圍環(huán)境的完整信息和自身的精確定位，以便于實(shí)現(xiàn)機(jī)器人在該環(huán)境中的導(dǎo)航，并實(shí)現(xiàn)后續(xù)工作任務(wù)。探索是移動機(jī)器人實(shí)現(xiàn)自主的關(guān)鍵功能，是移動機(jī)器人的一項(xiàng)重要任務(wù)，也是一個(gè)重要的研究領(lǐng)域。在許多潛在的應(yīng)用中，建筑物、洞穴、隧道和礦山內(nèi)的搜索操作有時(shí)是極其危險(xiǎn)的活動。使用自主機(jī)器人在復(fù)雜環(huán)境中執(zhí)行這些任務(wù)，降低了人類執(zhí)行這些任務(wù)的風(fēng)險(xiǎn)。

本文主要講解ROS平臺下三種常用的自主探索算法原理。

2 frontier_exploration

frontier_exploration是基于邊界的自主探索算法，邊界定義為已知空間和未知空間之間的區(qū)域（Frontiers are regions on the boundary between open space and unexplored space），在原文獻(xiàn)[1]中，作者通過BFS（深度優(yōu)先搜索）算法從機(jī)器人當(dāng)前位置搜索邊界。

圖a是在柵格地圖中檢測到了邊界，圖b是提取出來的屬于邊界的柵格點(diǎn)，然后使用一種“濾波”方法，濾除一些過小的邊界（因?yàn)檫@些邊界往往不需要特意去探索，機(jī)器人在行進(jìn)過程中會構(gòu)建出這里的地圖，或者它小到不影響導(dǎo)航和后續(xù)功能），一般這個(gè)閾值可以取機(jī)器人的自身尺寸（原文中是這樣的），就得到了圖c中的邊界區(qū)域。

這一系列邊界區(qū)域，需要通過某種方法選擇最合適的一個(gè)作為首先要前往的一個(gè)邊界區(qū)域，在frontier_exploration算法中僅選擇距離機(jī)器人當(dāng)前位置（歐氏距離）最近的一個(gè)作為首先要前往的目標(biāo)。

在文獻(xiàn)[2]中提出了選擇這些目標(biāo)點(diǎn)的方法，根據(jù)作者的結(jié)論，在單機(jī)器人探索中，Nearest Frontier Approach（選擇最近點(diǎn)）和Behaviour-Based Coordinated Approach（基于行為，融合了距離最近、避開障礙物、避開其他機(jī)器人三個(gè)懲罰項(xiàng)）兩個(gè)方法使用的時(shí)間相近都是最短，其中Nearest Frontier Approach用時(shí)更短。

在地圖質(zhì)量方面，使用Hybrid Integrated Coordinated Approach（該方法設(shè)計(jì)比較復(fù)雜，融合了SLAM算法，提高了定位精度，會在系統(tǒng)中評估并存儲精度較高的上一次位姿，當(dāng)機(jī)器人定位精度下降時(shí)，會回溯上一次精度較高的位姿，導(dǎo)致耗時(shí)嚴(yán)重）它的耗時(shí)嚴(yán)重超過其他方法，雖然總的來說，減少探索時(shí)間的目標(biāo)和良好的地圖質(zhì)量是相互沖突的，但我認(rèn)為綜合來看這種方法是不實(shí)用的，而地圖精度僅次于Hybrid Integrated Coordinated Approach的算法是Nearest Frontier Approach和 Cost-Utility Approach，分為成本函數(shù)（Cost）和效用函數(shù)（Utility），采用加權(quán)的思想，作者給出的表達(dá)式如下：

U(a)表示當(dāng)前往這個(gè)邊界點(diǎn)a時(shí)，能擴(kuò)大多少未知區(qū)域，以a為圓心，以一定長度Rs為半徑（一般設(shè)置為傳感器探測半徑）的圓覆蓋的未知柵格。

C(a) 表示當(dāng)前位置到邊界點(diǎn)的距離。

目前來看選擇距離最近的方法雖然很呆很簡單，但確實(shí)好用。另外，一個(gè)邊界區(qū)域由很多柵格點(diǎn)組成，這時(shí)就需要通過一些算法選擇目標(biāo)邊界點(diǎn)，作者在源代碼中列舉了三種常用方法：closest（BFS第一個(gè)檢測到的點(diǎn)）、middle（中點(diǎn)）、centroid（質(zhì)心）。-frontier_search.cpp的buildNewFrontier()函數(shù)中

另外，可應(yīng)采用插件的形式管理自主探索的算法，在源代碼中提供了兩個(gè)example插件，通過Base_Plugin接口類將自主探索算法集成到Exploration_Server中，便于修改，算法流程圖如下所示。

3 explorate_lite

explorate_lite的代碼是基于frontier_exploration16.04版本寫的，但是經(jīng)過作者幾次更新代碼有了很大的改進(jìn)，改進(jìn)點(diǎn)如下：

（1）它添加了一個(gè)frontierCost函數(shù)用來判斷計(jì)算邊界區(qū)域的代價(jià)，對 到邊界的距離 + 邊界大小 （+前沿方向分量） 幾個(gè)項(xiàng)加權(quán)后，排序，選擇代價(jià)最小的邊界的質(zhì)心作為目標(biāo)點(diǎn)。

（2）在frontier_exploration中對于邊界點(diǎn)Frontier的定義放在了Frontier.msg消息里面，而它定義為結(jié)構(gòu)體形式，其中也包含了更多信息。

（3）它添加了tf校驗(yàn)機(jī)制，確保了 map_frame-robot_base_frame 的通暢

（4）添加了rosconsole記錄器記錄Debug信息

（5）添加了更多接口方便通過launch文件對這些參數(shù)進(jìn)行修改

（6）這個(gè)修改的優(yōu)劣有待討論：explorate_lite在發(fā)布目標(biāo)邊界點(diǎn)時(shí)，使用的是定時(shí)發(fā)布，如下圖，當(dāng)機(jī)器人選擇了一個(gè)當(dāng)前的“最近點(diǎn)”時(shí)，行進(jìn)過程中遇到更近的會不會換一個(gè)目標(biāo)點(diǎn)，這樣會增加路徑重復(fù)率，也會造成機(jī)器人的頻繁起停。比如下圖所示，機(jī)器人運(yùn)行至左圖情況時(shí)選擇了紅色五角星為目標(biāo)點(diǎn)，但運(yùn)行過程中發(fā)現(xiàn)左圖中藍(lán)色五角星更優(yōu)，有可能會出現(xiàn)右圖所示更新選擇的情況，造成路徑重復(fù)和機(jī)器人的頻繁起停。

（7）設(shè)置了容忍時(shí)間，超時(shí)會把這個(gè)點(diǎn)加到黑名單中，可以類似仿照限定程序總運(yùn)行時(shí)間

explore_lite程序的框圖如下：

4 rrt_exploration

rrt_exploration[4]分為四個(gè)主要部分：Global Frontier Detector、Local Frontier Detector、Filter、Assigner。

（1）Global Frontier Detector、Local Frontier：使用全局和局部邊緣檢測點(diǎn)去選擇下一步應(yīng)該探索的邊界點(diǎn)，生長的RRT數(shù)到達(dá)的點(diǎn)如果位于未知區(qū)域，則這個(gè)的被認(rèn)為是邊界點(diǎn)，全局邊緣檢測是一種從初始位置開始一直執(zhí)行的RRT搜素，全局RRT樹不重置，隨著時(shí)間不斷生長、細(xì)化，主要是防止機(jī)器人錯(cuò)過在地圖上探索小角落的機(jī)會，也確保原理機(jī)器人當(dāng)前位置的點(diǎn)被檢測和探索[rrt_exploration]，而局部邊緣檢測是以機(jī)器人當(dāng)前位置為起始點(diǎn)拓展的RRT樹，這個(gè)局部樹搜索到一個(gè)邊界點(diǎn)后會重置樹。

這里存在一些問題：

1. 全局RRT樹的生長速度隨著樹的生長而變慢
2. RRT是一種漸進(jìn)最優(yōu)的算法，理論上時(shí)間足夠長可以覆蓋整個(gè)地圖，但其具有隨機(jī)性，有限時(shí)間內(nèi)很可能無法完全探測所有未知區(qū)域，特別是實(shí)際應(yīng)用中往往是有時(shí)間要求的
3. RRT本身的生長問題：當(dāng)一個(gè)分支無法生長延伸時(shí)，需要回溯到之前的未知，這會增加搜索時(shí)間和重復(fù)區(qū)域
4. 如下圖，RRT具有隨機(jī)性，可能在機(jī)器人前往一個(gè)目前“最優(yōu)”的目標(biāo)點(diǎn)時(shí)，突然發(fā)現(xiàn)了一個(gè)更好的點(diǎn)，機(jī)器人需要折返，盡管這種可能性很小，但可能會出現(xiàn)這種情況，會增加時(shí)間和重復(fù)度，而frontier_exploration這種算法往往有更好的傾向性和啟發(fā)性，而且在探索過程中使用了BFS雖然時(shí)間較慢（我認(rèn)為是微小的），但是可以搜索到所有可能的邊界，并通過權(quán)重合理選擇探索順序。

RRT源代碼中提供了使用opencv進(jìn)行邊界檢測的方法作為比較，將使用cv2.findContours()方法檢測出來的邊界序列求質(zhì)心后打包發(fā)送，發(fā)送給Filter濾波后再發(fā)送給Assigner，在Assigner中求出最終的得分，選擇得分高的節(jié)點(diǎn)發(fā)送個(gè)Move_base節(jié)點(diǎn)，這就出現(xiàn)一個(gè)情況在每一次檢測節(jié)點(diǎn)輪詢，目標(biāo)點(diǎn)是不斷發(fā)生變化的（很具有隨機(jī)性），也就是說有可能還沒到當(dāng)前發(fā)布的位置時(shí)，隨著地圖更新，又會產(chǎn)生新的目標(biāo)點(diǎn)，如果向一個(gè)方向即將（還沒）探索完（這時(shí)它的信息增益I很小了），還是可能會被其他位置的目標(biāo)邊界點(diǎn)吸引，這樣會增加重復(fù)性和探索時(shí)間，及時(shí)作者加了滯回增益h，也不能避免發(fā)生這種情況。

（2）Filter

對搜索到的點(diǎn)進(jìn)行聚類，形成邊界，這里的聚類方法也是使用最簡單的質(zhì)心聚類法，質(zhì)心聚類公式如下：

（3）Assigner

從現(xiàn)有的目標(biāo)點(diǎn)中根據(jù)導(dǎo)航成本（N）和信息增益（I）兩項(xiàng)選擇機(jī)器人下一步需要前往的目標(biāo)點(diǎn)

可以借鑒的一點(diǎn)是RRT選擇點(diǎn)的公式，在信息增益I這一項(xiàng)前加了一個(gè)滯回增益項(xiàng)h，h也跟距離有關(guān)，防止一個(gè)很遠(yuǎn)但I(xiàn)過大的邊界點(diǎn)把機(jī)器人吸引過去。

RRT相較于frontier_exploration的優(yōu)勢就是速度快，但作者也在文獻(xiàn)中實(shí)驗(yàn)指出，探索時(shí)間比基于圖像處理的時(shí)間稍長，但是優(yōu)勢在于可以方便的拓展到三維。

5 總結(jié)

最后，就是我經(jīng)過看論文和閱讀源代碼，分析總結(jié)的自主探索算法可以改進(jìn)的點(diǎn)，通過我閱讀一些文獻(xiàn)，我發(fā)現(xiàn)對算法的改進(jìn)確實(shí)就是從這幾個(gè)方面入手的。

6 參考文獻(xiàn)

[1]B.Yamauchi, A frontier-based approach for autonomous exploration, Proceedings 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA'97. 'Towards New Computational Principles for Robotics and Automation', Monterey, CA, USA, 1997, pp. 146-151, doi 10.1109CIRA.1997.613851. [2]Juliá, M., Gil, A. & Reinoso, O. A comparison of path planning strategies for autonomous exploration and mapping of unknown environments. Auton Robot 33, 427–444 (2012). httpsdoi.org10.1007s10514-012-9298-8

[3]H?rner, Ji?í. “Map-merging for multi-robot system.” (2016).

[4]H. Umari and S. Mukhopadhyay, Autonomous robotic exploration based on multiple rapidly-exploring randomized trees, 2017 IEEERSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 2017, pp. 1396-1402, doi 10.1109IROS.2017.8202319.

工坊618特惠活動

[1]課程618優(yōu)惠活動:工業(yè)3D視覺/SLAM/自動駕駛課程，我該如何選?

[2]知識星球618優(yōu)惠活動：工業(yè)3D視覺或SLAM，到底該如何高效學(xué)習(xí)?