黃家豪 郝潤(rùn)科 呂剛震

摘 要：清掃機(jī)器人進(jìn)行全遍歷路徑規(guī)劃要求機(jī)器人能夠遍歷環(huán)境中所有的可清掃區(qū)域，因此提出一種基于蟻群系統(tǒng)算法的地圖全遍歷路徑規(guī)劃算法。使用搭載單線激光雷達(dá)傳感器的機(jī)器人進(jìn)行環(huán)境建圖，對(duì)每個(gè)柵格賦予不同概率值反映環(huán)境狀態(tài)信息;采用 Boustrophedon細(xì)胞分解方法將柵格地圖劃分為若干相鄰子模塊，并讓機(jī)器人從起始點(diǎn)開始遍歷所有子模塊后再回到起始位姿。為了提高各子模塊之間的銜接效率，引入蟻群系統(tǒng)算法實(shí)現(xiàn)機(jī)器人在到達(dá)每個(gè)子模塊的起始位姿后，對(duì)每個(gè)子模塊進(jìn)行高效的區(qū)域全覆蓋。實(shí)驗(yàn)結(jié)果表明，該算法相比傳統(tǒng)生成樹算法，清掃覆蓋率達(dá)到了96%，清掃效率提高了兩倍。

關(guān)鍵詞：全遍歷路徑規(guī)劃;清掃機(jī)器人;柵格地圖;蟻群系統(tǒng)算法;Boustrophedon細(xì)胞分解

DOI：10. 11907/rjdk. 192275 開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

中圖分類號(hào)：TP301文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-7800（2020）007-0041-05

Map Full Traversal Path Planning Based on Ant Colony System Algorithm

Huang Jia-hao， HAO Run-ke， LV Gang-zhen

（School of Mechanical Engineering， University of Shanghai for Science and Technology， Shanghai 200093， China）

Abstract： The full traversal path planning of the cleaning robot requires the robot to traverse all the cleanable areas in the environment. This paper proposes an algorithm for map full traversal path planning based on ant colony system algorithm. The robot is equipped with a single-line lidar sensor for environmental mapping. Each grid gives different probability values to reflect the state information of the environment. The Boustrophedon celluar decomposition method is used to divide the grid map into several adjacent sub-modules. And the robot traverses all submodules from the starting point and then return to their starting pose. In order to improve the connection efficiency between the various sub-modules， an ant colony system algorithm is introduced to realize that after the robot reaches the starting position of each sub-module， it covers the entire area of each sub-module efficiently. The experimental results show that compared with the traditional spanning tree algorithm， the proposed algorithm has a cleaning coverage rate of 96% and the cleaning efficiency is doubled.

Key Words： full traversal path planning; cleaning robot; grid map; ant colony system algorithm; Boustrophedon cell decomposition

0 引言

地圖全遍歷路徑規(guī)劃算法旨在解決如何覆蓋工作環(huán)境中的所有區(qū)域并避開障礙物。該算法被廣泛應(yīng)用于各種機(jī)器人平臺(tái)中，例如清潔機(jī)器人[1-2]、排雷機(jī)器人[3]、割草機(jī)器人[4]等。

根據(jù)是否有先驗(yàn)的環(huán)境信息存儲(chǔ)在系統(tǒng)中，全遍歷路徑規(guī)劃算法可分為在線方法[5-6]和離線方法[7]兩種。在線方法致力于未知環(huán)境實(shí)時(shí)規(guī)劃與覆蓋，但由于傳感器自身精度的局限性，存在無法覆蓋環(huán)境中所有區(qū)域的問題。離線方法雖具備更高的覆蓋率，并能生成更優(yōu)路徑，但在環(huán)境發(fā)生變化時(shí)極易出現(xiàn)算法失效的情況。Ribeiro 等[8]提出的生成樹算法和Hu等[9]提出的螺旋填充路徑算法雖然能基本實(shí)現(xiàn)地圖全遍歷路徑規(guī)劃，但因生成的路徑重復(fù)率太高，導(dǎo)致清掃效率較低;Karthikeyan等[10]提出的基于神經(jīng)網(wǎng)絡(luò)算法的路徑規(guī)劃和Yakoubi等[11]提出的基于遺傳算法的路徑規(guī)劃雖然能生成優(yōu)于前者、重復(fù)率較低的全遍歷路徑，但神經(jīng)網(wǎng)絡(luò)和遺傳算法主要用來解決最短路徑優(yōu)化問題，最短路徑優(yōu)化中的目標(biāo)函數(shù)通常是連續(xù)的，并且最終會(huì)收斂到特定的最優(yōu)值，且全遍歷路徑規(guī)劃中的最大覆蓋范圍會(huì)對(duì)目標(biāo)函數(shù)進(jìn)行嚴(yán)格約束，若面對(duì)多變的復(fù)雜環(huán)境，機(jī)器人也無法保持正常工作狀態(tài)。

空間分解技術(shù)[13]是全遍歷路徑規(guī)劃中的關(guān)鍵一環(huán)，選擇適當(dāng)?shù)目臻g分解技術(shù)能極大地簡(jiǎn)化環(huán)境模型并降低算法整體復(fù)雜度?；跂鸥竦姆纸夥椒╗14]將環(huán)境空間劃分為較小單位元素的并集，其中所有單元的大小與形狀相同，且不存在任何重疊區(qū)域。同時(shí)，每個(gè)柵格大小決定了地圖分辨率，高分辨率的地圖可使算法對(duì)工作環(huán)境進(jìn)行更好的估計(jì)，達(dá)到更高的覆蓋率。

故本文通過引入蟻群系統(tǒng)算法[15-16]，使用搭載單線激光雷達(dá)傳感器的清掃機(jī)器人構(gòu)建環(huán)境的高分辨率地圖，并對(duì)地圖進(jìn)行Boustrophedon細(xì)胞分解[17]，規(guī)劃出全覆蓋的全局路徑。該算法相比傳統(tǒng)算法具有更高的覆蓋率與更低的重復(fù)率，彌補(bǔ)了傳統(tǒng)算法在動(dòng)態(tài)環(huán)境中容易失效的缺陷。

1 基于蟻群系統(tǒng)算法的全遍歷路徑規(guī)劃算法

1.1 占用柵格地圖構(gòu)建

柵格地圖由Moravec等[18]提出，該方法簡(jiǎn)單、直觀且數(shù)據(jù)容易表示，所以被廣泛應(yīng)用于機(jī)器人避障導(dǎo)航、位姿估計(jì)及路徑規(guī)劃等方面，特別適合于處理激光雷達(dá)采集的數(shù)據(jù)。占用柵格地圖構(gòu)建算法可根據(jù)給定數(shù)據(jù)計(jì)算整個(gè)地圖的后驗(yàn)概率。

式中，m為地圖，[z1：t]為指導(dǎo)時(shí)刻t的所有測(cè)量值，[x1：t]為包含所有機(jī)器人位姿的路徑。

這里采用對(duì)數(shù)表達(dá)形式，以避免0和1附近數(shù)值的不穩(wěn)定性，即：

則式（1）轉(zhuǎn)化為：

因此，將求解整個(gè)地圖轉(zhuǎn)換成求解每個(gè)柵格的概率問題，并將每個(gè)單元被占有的概率記為置信度。

當(dāng)機(jī)器人位姿發(fā)生改變時(shí)，再將當(dāng)前觀測(cè)的局部地圖融入到已有的全局地圖中。圖1為單線激光雷達(dá)構(gòu)建的實(shí)際環(huán)境1的柵格地圖，圖2為實(shí)際環(huán)境2的柵格地圖。

1.2 環(huán)境柵格地圖噪聲處理

如圖1、圖2所示，使用單線激光雷達(dá)傳感器構(gòu)建的占用柵格地圖不可避免地會(huì)存在許多噪聲點(diǎn)和發(fā)散點(diǎn)，并且地圖中有許多穿透玻璃門掃到的室外區(qū)域，這些區(qū)域都不在室內(nèi)清掃機(jī)器人的清掃范圍內(nèi)，所以需要去除柵格地圖中的噪聲。

本文使用OpenCV中的膨脹與腐蝕算法[19]對(duì)柵格地圖進(jìn)行去噪，并確定清掃機(jī)器人作業(yè)范圍。膨脹和腐蝕是圖像形態(tài)學(xué)中的重要操作，原理為對(duì)圖像進(jìn)行卷積操作，其需要有一個(gè)kernel卷積核，常見的是3*3的矩陣。但與卷積不同的是，如果矩陣中的像素點(diǎn)有任意一個(gè)點(diǎn)的值是前景色，則設(shè)置中心像素點(diǎn)為前景色。在算法處理前，先將地圖變成二值圖像，用數(shù)值1表示障礙物，數(shù)值0表示空閑區(qū)域。

采用腐蝕算法，用3*3的kernel掃描圖像每一個(gè)像素，并用kernel與其覆蓋的二值圖像作“與”操作，如果都為1，最終圖像的該像素為1，否則為0;采用膨脹算法，用3*3的kernel與其覆蓋的二值圖像作“與”操作，如果都為0，最終圖像的該像素為0，否則為1。

圖1中的原始柵格地圖經(jīng)過OpenCV中的算法處理后得到如圖3所示地圖。

1.3 Boustrophedon細(xì)胞分解

Boustrophedon細(xì)胞分解是一種柵格地圖分解方法，算法原理為：切片掃過有界自由空間，從“負(fù)無窮大”開始，在切片最初遇到自由空間時(shí)生成第一個(gè)細(xì)胞;然后切片繼續(xù)掃過自由空間。在IN事件中，切片連通性增加，當(dāng)前細(xì)胞關(guān)閉，兩個(gè)新細(xì)胞被打開。相反，在切片連通性降低的OUT事件中，兩個(gè)當(dāng)前細(xì)胞關(guān)閉，一個(gè)新細(xì)胞被打開。當(dāng)切片離開有界自由空間時(shí)，該過程終止。在計(jì)算分解時(shí)，還要確定鄰接圖。同樣，每個(gè)細(xì)胞是圖中的節(jié)點(diǎn)，并且是邊連接的相鄰節(jié)點(diǎn)。用類似深度優(yōu)先的圖搜索算法輸出表示通過鄰接圖的窮舉遍歷路徑列表，遍歷路徑列表構(gòu)成了對(duì)鄰接圖的詳盡遍歷;最后，使用上述路徑列表計(jì)算機(jī)器人的實(shí)際路徑。當(dāng)機(jī)器人進(jìn)入“未清理”單元時(shí)，進(jìn)行Boustrophedon運(yùn)動(dòng)，然后計(jì)算路徑列表中的下一個(gè)單元路徑。當(dāng)機(jī)器人進(jìn)入“已清理”單元時(shí)，只是計(jì)算通過當(dāng)前單元格到路徑列表中下一個(gè)單元的路徑。重復(fù)這兩個(gè)動(dòng)作，直到到達(dá)路徑列表末尾，即直到每個(gè)單元都已被“清理”。IN事件和OUT事件分別如圖4、圖5所示。

在對(duì)圖3完成Boustrophedon細(xì)胞分解后，得到如圖6所示分塊地圖。對(duì)實(shí)驗(yàn)環(huán)境2也進(jìn)行Boustrophedon細(xì)胞分解，得到如圖7所示分塊地圖。

1.4 蟻群系統(tǒng)算法

將環(huán)境柵格地圖分解為多個(gè)子區(qū)域后，需要給清掃機(jī)器人規(guī)劃出一條從起點(diǎn)開始訪問所有子區(qū)域，最后又回到起點(diǎn)的最優(yōu)路徑，也即組合優(yōu)化問題中的旅行商問題。該問題是在一個(gè)帶權(quán)完全無向圖中尋找一個(gè)權(quán)值最小的Hamilton回路[20]。由于該問題的可行解是所有頂點(diǎn)全排列，隨著頂點(diǎn)數(shù)的增加，會(huì)產(chǎn)生組合爆炸，因此是一個(gè)NP完全問題。圖8是實(shí)驗(yàn)環(huán)境1的區(qū)域結(jié)構(gòu)圖，根據(jù)該圖直接實(shí)現(xiàn)全遍歷規(guī)劃算法是不現(xiàn)實(shí)的。

蟻群系統(tǒng)算法（Ant Colony System，ACS）是一種基于群體、用于求解復(fù)雜優(yōu)化問題的元啟發(fā)式算法。與真實(shí)螞蟻通過外激素的留存、跟隨行為進(jìn)行間接通信類似，ACS中一群簡(jiǎn)單的人工螞蟻之間通過媒介質(zhì)進(jìn)行間接通信，并利用該信息以及與問題相關(guān)的啟發(fā)式信息逐步構(gòu)造問題的解，實(shí)現(xiàn)對(duì)問題的優(yōu)化。算法流程如圖9所示。

根據(jù)圖8得到一個(gè)有向圖G的三元組為（V，E，f），其中V是一個(gè)非空集合，其元素稱為有向圖節(jié)點(diǎn);E是一個(gè)集合，其元素稱為有向圖的邊;f是從E到V×V的一個(gè)映射。E中元素總是與V中的序偶有對(duì)應(yīng)關(guān)系，可用V中的序偶代替E中元素，則G簡(jiǎn)記為（V，E）。

設(shè)[C=c1，c2，？cn]為n個(gè)城市的集合，[L=lij|ci，cj∈C]是集合C中元素兩兩連接的集合，[diji，j=1，2，？，n]是[lij]的Euclidean距離，如式（6）所示。

G=（C，L）構(gòu)成一個(gè)有向圖，從G中尋找長(zhǎng)度最短的Hamilton圈，即全遍歷路徑。

在蟻群系統(tǒng)算法中，螞蟻采用偽隨機(jī)比例規(guī)則策略。根據(jù)該策略，一只位于子區(qū)域1的螞蟻k通過公式（5）選擇下一個(gè)要訪問的區(qū)域j。

q為[0，1]區(qū)間均勻分布的隨機(jī)數(shù)，[q0]為一個(gè)參數(shù)，[0q01]。螞蟻k以[q