1 前言

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

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

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

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

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

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

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

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

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

3 explorate_lite

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

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

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

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

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

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

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

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

explore_lite程序的框圖如下：

4 rrt_exploration

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

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

這里存在一些問題：

全局RRT樹的生長速度隨著樹的生長而變慢

RRT是一種漸進(jìn)最優(yōu)的算法，理論上時間足夠長可以覆蓋整個地圖，但其具有隨機(jī)性，有限時間內(nèi)很可能無法完全探測所有未知區(qū)域，特別是實際應(yīng)用中往往是有時間要求的

RRT本身的生長問題：當(dāng)一個分支無法生長延伸時，需要回溯到之前的未知，這會增加搜索時間和重復(fù)區(qū)域

如下圖，RRT具有隨機(jī)性，可能在機(jī)器人前往一個目前“最優(yōu)”的目標(biāo)點時，突然發(fā)現(xiàn)了一個更好的點，機(jī)器人需要折返，盡管這種可能性很小，但可能會出現(xiàn)這種情況，會增加時間和重復(fù)度，而frontier_exploration這種算法往往有更好的傾向性和啟發(fā)性，而且在探索過程中使用了BFS雖然時間較慢（我認(rèn)為是微小的），但是可以搜索到所有可能的邊界，并通過權(quán)重合理選擇探索順序。

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

（2）Filter

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

（3）Assigner

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

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

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

5 總結(jié)

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

審核編輯：劉清