ROS平臺(tái)下三種常用的自主探索算法原理

1 前言

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

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

2 frontier_exploration

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

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

這一系列邊界區(qū)域，需要通過(guò)某種方法選擇最合適的一個(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（基于行為，融合了距離最近、避開(kāi)障礙物、避開(kāi)其他機(jī)器人三個(gè)懲罰項(xiàng)）兩個(gè)方法使用的時(shí)間相近都是最短，其中Nearest Frontier Approach用時(shí)更短。

在地圖質(zhì)量方面，使用Hybrid Integrated Coordinated Approach（該方法設(shè)計(jì)比較復(fù)雜，融合了SLAM算法，提高了定位精度，會(huì)在系統(tǒng)中評(píng)估并存儲(chǔ)精度較高的上一次位姿，當(dāng)機(jī)器人定位精度下降時(shí)，會(huì)回溯上一次精度較高的位姿，導(dǎo)致耗時(shí)嚴(yán)重）它的耗時(shí)嚴(yán)重超過(guò)其他方法，雖然總的來(lái)說(shuō)，減少探索時(shí)間的目標(biāo)和良好的地圖質(zhì)量是相互沖突的，但我認(rèn)為綜合來(lái)看這種方法是不實(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為圓心，以一定長(zhǎng)度Rs為半徑（一般設(shè)置為傳感器探測(cè)半徑）的圓覆蓋的未知柵格。

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

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

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

3 explorate_lite

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

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

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

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

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

（5）添加了更多接口方便通過(guò)launch文件對(duì)這些參數(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)過(guò)程中遇到更近的會(huì)不會(huì)換一個(gè)目標(biāo)點(diǎn)，這樣會(huì)增加路徑重復(fù)率，也會(huì)造成機(jī)器人的頻繁起停。比如下圖所示，機(jī)器人運(yùn)行至左圖情況時(shí)選擇了紅色五角星為目標(biāo)點(diǎn)，但運(yùn)行過(guò)程中發(fā)現(xiàn)左圖中藍(lán)色五角星更優(yōu)，有可能會(huì)出現(xiàn)右圖所示更新選擇的情況，造成路徑重復(fù)和機(jī)器人的頻繁起停。

（7）設(shè)置了容忍時(shí)間，超時(shí)會(huì)把這個(gè)點(diǎn)加到黑名單中，可以類(lèi)似仿照限定程序總運(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：使用全局和局部邊緣檢測(cè)點(diǎn)去選擇下一步應(yīng)該探索的邊界點(diǎn)，生長(zhǎng)的RRT數(shù)到達(dá)的點(diǎn)如果位于未知區(qū)域，則這個(gè)的被認(rèn)為是邊界點(diǎn)，全局邊緣檢測(cè)是一種從初始位置開(kāi)始一直執(zhí)行的RRT搜素，全局RRT樹(shù)不重置，隨著時(shí)間不斷生長(zhǎng)、細(xì)化，主要是防止機(jī)器人錯(cuò)過(guò)在地圖上探索小角落的機(jī)會(huì)，也確保原理機(jī)器人當(dāng)前位置的點(diǎn)被檢測(cè)和探索[rrt_exploration]，而局部邊緣檢測(cè)是以機(jī)器人當(dāng)前位置為起始點(diǎn)拓展的RRT樹(shù)，這個(gè)局部樹(shù)搜索到一個(gè)邊界點(diǎn)后會(huì)重置樹(shù)。

這里存在一些問(wèn)題：

全局RRT樹(shù)的生長(zhǎng)速度隨著樹(shù)的生長(zhǎng)而變慢

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

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

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

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

（2）Filter

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

（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)過(guò)大的邊界點(diǎn)把機(jī)器人吸引過(guò)去。

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

5 總結(jié)

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

審核編輯：劉清