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

黃家豪 郝潤科 呂剛震

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

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

DOI：10. 11907/rjdk. 192275 開放科學（資源服務）標識碼（OSID）：

中圖分類號：TP301文獻標識碼：A 文章編號：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ū)域并避開障礙物。該算法被廣泛應用于各種機器人平臺中，例如清潔機器人[1-2]、排雷機器人[3]、割草機器人[4]等。

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

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

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

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

1.1 占用柵格地圖構建

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

式中，m為地圖，[z1：t]為指導時刻t的所有測量值，[x1：t]為包含所有機器人位姿的路徑。

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

則式（1）轉化為：

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

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

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

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

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

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

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

1.3 Boustrophedon細胞分解

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

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

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

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

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

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

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

G=（C，L）構成一個有向圖，從G中尋找長度最短的Hamilton圈，即全遍歷路徑。

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

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