ROS平臺下三種常用的自主探索算法原理

1 前言

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

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

2 frontier_exploration

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

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

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

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

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

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

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

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

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

3 explorate_lite

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

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

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

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

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

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

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

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

explore_lite程序的框圖如下：

4 rrt_exploration

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

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

這里存在一些問題：

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

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

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

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

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