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

1 前言

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

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

2 frontier_exploration

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

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

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

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

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

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

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

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

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

3 explorate_lite

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

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

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

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

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

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

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

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

explore_lite程序的框圖如下：

4 rrt_exploration

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

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

這里存在一些問題：

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

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

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

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

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