基于蟻群系統算法的地圖全遍歷路徑規劃

黃家豪 郝潤科 呂剛震

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

關鍵詞：全遍歷路徑規劃;清掃機器人;柵格地圖;蟻群系統算法;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 引言

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

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

空間分解技術[13]是全遍歷路徑規劃中的關鍵一環，選擇適當的空間分解技術能極大地簡化環境模型并降低算法整體復雜度。基于柵格的分解方法[14]將環境空間劃分為較小單位元素的并集，其中所有單元的大小與形狀相同，且不存在任何重疊區域。同時，每個柵格大小決定了地圖分辨率，高分辨率的地圖可使算法對工作環境進行更好的估計，達到更高的覆蓋率。

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

1 基于蟻群系統算法的全遍歷路徑規劃算法

1.1 占用柵格地圖構建

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

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

這里采用對數表達形式，以避免0和1附近數值的不穩定性，即：

則式（1）轉化為：

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

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

1.2 環境柵格地圖噪聲處理

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

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

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

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

1.3 Boustrophedon細胞分解

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

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

1.4 蟻群系統算法

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

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

根據圖8得到一個有向圖G的三元組為（V，E，f），其中V是一個非空集合，其元素稱為有向圖節點;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圈，即全遍歷路徑。

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

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