動態環境下蟻群算法的路徑規劃研究

胡國棟， 陳 波， 歐鳳琴

（1.上海埃奇機器人技術有限公司， 上海 201615； 2.安徽工程大學機械工程學院， 安徽蕪湖 241000；3.埃華路（蕪湖）機器人工程有限公司， 安徽 蕪湖 241000）

0 引言

按照機器人對周圍環境的知曉程度， 移動機器人路徑規劃[1]大致可分為兩種：環境信息完全已知的全局路徑規劃，環境信息部分未知或完全未知的局部路徑規劃。人工勢場法[2]、粒子群算法[3]、遺傳算法[4]以及蟻群算法[5]對于環境信息完全已知的全局路徑規劃都體現出較大的應用價值。 在常用的路徑規劃算法中蟻群算法較為成熟和高效， 蟻群算法是一種搜索尋優強啟發算法， 蟻群算法在TSP 問題[6]、函數優化問題[7]和路徑規劃問題[8]中有著較好的表現， 尤其對于路徑規劃這類離散系統具有很強的適用性。 文獻[9]中提出迭代自適應因子去調節信息素的更新強度加快收斂速度以及信息素衰減因子避免算法早熟；文獻[10]通過提出兩個新的啟發式因子和一個偽隨機系數重新定義狀態轉移函數提高搜索隨機性， 并引入罰系數結合回退策略解決螞蟻死鎖問題， 但是頻繁回退有可能減緩算法的運行速度。

本文采用柵格法模擬移動機器人的工作環境， 針對基本蟻群算法存在的固有缺陷，如易陷入局部最優、收斂速度慢，及死鎖等問題，提出利用擴展樹隨機優化方法，創建虛擬路徑和蟻群更新域信息素更新原則， 改進信息素的更新方式；提出局部子起點再規劃策略，解決螞蟻死鎖問題；針對傳統蟻群算法對動態環境適應度低的問題，引入不斷刷新的滾動窗口以探測環境信息； 依據提出的避障策略，根據環境信息作出決策。

1 全局路徑規劃

傳統蟻群算法中螞蟻通過向著信息素濃度較高的路徑移動來尋找最優路徑， 這種正反饋機制可極大地加快收斂速度，但會導致蟻群多樣性減小，全局搜索能力減弱， 如何權衡收斂速度和種群多樣性是一個值得商榷的問題。 此外，在算法迭代早期，由于信息素矩陣中還沒有優勢路徑出現，螞蟻有著較大概率陷入死鎖狀態。針對這些問題，本文作出以下改進。

1.1 蟻群更新域信息素更新策略

在經典蟻群算法中， 信息素更新常采用在每一次迭代后對所有覓食成功螞蟻搜索到的路徑進行一次更新，種群多樣性大但收斂速度慢；在最大最小螞蟻系統中，每一次迭代后僅對全局最優螞蟻進行更新， 雖然加快了算法收斂速度，但失去種群多樣性，容易陷入局部最優，導致算法早熟。 本文提出介于兩者之間的權衡信息素更新方式，限制每次允許更新的閾值，每一次迭代后僅對閾值范圍內的螞蟻搜索路徑進行更新， 閾值范圍表示為（Upperlimit，Lowerlimit）。

式中，Lbest是當前代為止全局最優路徑長度；f 是表征更新域的系數，介于0～1 之間；Dg是全局規劃起點到規劃終點的歐式距離；n 為階數，0-inf， 值越大越接近最大最小螞蟻系統。

1.2 擴展樹隨機優化原則

蟻群算法作為一種強啟發式算法， 信息素對單一螞蟻行為的影響很大，信息素矩陣失衡，導致某條路徑信息素強度過于優勢， 由于正反饋機制導致螞蟻大部分集中該條路徑，導致算法早熟。 為了避免陷入局部最優，本文提出一種基于擴展樹的優化路徑方法， 藉由這種方法創建虛擬路徑，這些路徑僅用于更新信息素，不作為實際搜索出的路徑， 旨在調節信息素矩陣避免信息素矩陣過早出現失衡。在每次迭代結束后選取待優化路徑，優化后作為虛擬路徑用于更新信息素， 待優化路徑為每一次迭代中路徑長度最大和最小的兩條路徑以及區別于最大最小的隨機一條路徑。 同時，避免出現拖慢算法程序，定義啟動系數去控制算法的運行。

對于任一條待優化路徑， 路徑上的節點稱為元擴展節點，向外擴展的節點稱為擴展節點，擴展樹優化步驟見圖1。①初始化；②輸入待優化路徑，元擴展節點序號設為1；③元擴展節點位置向外擴展。 若擴展節點觸碰邊界或者障礙物則該方向停止擴展并且將該方向的值設為INF，停止擴展； 若擴展節點屬于待優化路徑且元擴展節點與擴展節點之間直線可達， 用元擴展節點與擴展節點之間直線路徑代替待優化路徑中元擴展節點與擴展節點之間路徑，生成一條待選擇路徑， 將值設為路徑長度，停止擴展；④元節點所有方向達到停止條件，選擇值最小的待選擇路徑更新待優化路徑；⑤元擴展節點序號自增，重復③和④， 直到元擴展節點序號達到最大，輸出優化路徑。

圖1 擴展樹隨機優化流程圖

1.3 信息素更新素規則

如前所述， 每次迭代結束可獲得若干條覓食路徑，t+1 時刻與t 時刻的關系見式（3）。

式中，τmax，τmin分別為預設的信息素最大值和最小值。

當環境信息較為復雜時，在蟻群算法早期，螞蟻隨機搜索尋優時極易死鎖狀態。針對死鎖問題，文獻[11]直接令處于死鎖狀態的螞蟻死亡，不對其進行任何處理，這樣當環境很復雜有很多螞蟻陷入死鎖狀態時，迭代早期無或者僅有少量有效路徑，信息素矩陣無或者僅有少量增益，造成下一次迭代，死鎖率仍然很高，陷入惡性循環，因此該方法不利于螞蟻尋優也不能解決復雜環境的死鎖問題。

本文結合回退策略提出局部子起點再規劃方法，來解決螞蟻死鎖問題。其中子起點的代價函數如式（5）所示。

S（N）=（η（N）μ）·（f（N）v）（5）式中，N—柵格節點；η—啟發式信息矩陣；f—節點附近可行節點數的歸一化系數矩陣；μ—表述啟發式信息的重要程度；υ—表述歸一化系數的重要程度。

對于任一只螞蟻陷入死鎖時， 將其走過的路徑分為前部和后部， 其中前部是全局規劃起點和上一次局部再規劃起點之間的路徑，后部是余下的除死鎖點的路徑。如圖2 所示，局部子起點再規劃步驟：①初始化參數；②螞蟻按照狀態轉移概率選擇下一節點； ③若螞蟻在當前節點有可選下一節點，繼續運行蟻群算法；④若螞蟻在當前節點無可選下一節點。 若后部為空，采取回退策略，子起點不變，并從禁忌表中刪除死鎖節點；若后部非空，則根據價函數在后部中選擇值最大的節點作為子起點， 將螞蟻路徑更新為全局規劃起點和當前局部再規劃起點之間的路徑； ⑤螞蟻選擇下一節點。若仍然無可選節點，令當前螞蟻死亡；若有可行節點，重復②、③和④，直到螞蟻死亡或者到達目標點結束。

圖2 解決螞蟻死鎖流程圖

1.4 改進蟻群算法步驟

移動機器人路徑規劃步驟見圖3。 ①初始化：初始化移動機器人的柵格環境信息并設置基本參數；②螞蟻由起始點開始轉移；③判斷柵格環境是否可行。若下一柵格可行且未到達終止點，則按照轉移規則繼續前進，計算并保存路徑長度，將螞蟻每次到達的柵格節點從禁忌表中刪除；若無可行下一柵格，執行螞蟻死鎖解決策略，直到到達目標點或者螞蟻死亡；④擴展樹隨機優化；⑤按照式（3）和式（4）更新信息素；⑥判斷是否達到算法最大迭代次數。 如果已達到，輸出最優路徑并退出；否則進入下一次循環。

圖3 改進蟻群算法步驟

2 動態環境局部路徑規劃

滾動窗口算法基本思想是依靠機器人實時探測到的局部環境信息，以滾動的方式進行在線規劃。滾動窗口隨著機器人的移動不斷行進而刷新，通過采集到的信息，預測是否會發生碰撞，若有可能發生碰撞，根據不同的局部信息采取相應的避障策略在窗口內重新規劃路徑， 否則按照規劃好的路徑行走。

2.1 局部避障策略

本節討論滾動窗口中不同局部信息應采取的避障策略：①滾動窗口內不存在動態障礙物或者障礙物遠離機器人，機器人沿著全局規劃路徑行進即可；②滾動窗口內存在動態障礙物時，軌跡規劃周期內，機器人和障礙物均可視為勻速直線運動。 如圖4 所示，為簡單起見， 將障礙物適當膨化后，視為圓形。

圖4 滾動窗口監測障礙物

根據余弦定理， 在一個采樣周期（dt）內的移動距離可由式（6）表示。

式中，θ 是vobs與L1的夾角（銳角）。

根據d 的大小可分為以下幾種情況：

（1）d＞robs+r+step。 動態障礙物在規劃窗口外運動，不會發生碰撞；機器人沿著全局規劃路徑行走即可；

（2）robs+r＜d≤robs+r+step。 如圖5 所示，動態障礙物運行在規劃窗口內，可能發生側碰。 此時可選擇等待障礙物運行完再運動， 或者根據式（9）計算可行域，符合式（9）的所有ω 的集合即為可行域。

圖5 機器人與障礙物可能發生側碰

（3）0＜d≤robs+r。 如圖6 所示，機器人處在動態障礙物的運行軌跡上， 可能發生正碰或者追尾； 此時可通過式（10）計算可行域，符合式（10）的所有ω 的集合即為可行域。

圖6 機器人與障礙物可能發生正碰或者追尾

2.2 動態規劃流程

圖7 所示的滾動窗口規劃流程：①初始化參數；②利用全局先驗靜態環境運行改進蟻群算法進行路徑規劃；③對滾動窗口里環境數據持續監測采集。 若無動態障礙物，按規劃好的路徑行走；若有動態障礙物，根據采集數據預測障礙物軌跡和即將可能發生的碰撞類型， 選擇相應的策略；④根據選擇的策略為機器人規劃局部路徑，使得機器人安全避開障礙物； ⑤刷新滾動窗口，重復步驟③和④，直到機器人到達目標點。

圖7 滾動窗口規劃流程圖

3 仿真實驗及分析

3.1 改進蟻群算法全局靜態規劃

為驗證算法性能，在20×20 的柵格環境實施路徑規劃仿真實驗， 移動機器人的起始點坐標為（0.5，19.5），目標點坐標為（19.5，0.5）。 通用參數如下：迭代次數k=200，螞蟻數量m=50，信息素重要程度α=1，啟發式重要程度β=7，信息素蒸發系數ρ=0.3。本文信息素上界設置為1，下界設置為0，μ=1，ν=7；對于文獻[9]算法，信息素上界設置為120，下界設置為0，懲罰系數設置為0.85，隨機系數設置為0.1。

多算法路徑規劃結果見圖8 和圖9， 結合表1 可以看出，本文改進蟻群算法相對另外兩種算法，規劃出最優路徑的收斂迭代次數明顯減少，路徑的最優值也是最小的；見表2，多次均值試驗表明，從最優值均值和最優率看，本文改進蟻群算法具有很強的穩定性，并且從一代死鎖率看本文提出的局部子起點再規劃策略相對另外兩種算法可以明顯降低迭代早期螞蟻的死鎖率，且優于另外兩種算法。

圖8 多算法路徑規劃路徑結果比較

圖9 多算法路徑規劃路徑長度比較

表1 單次運行結果統計數據

對比各算法穩定性， 對三種算法分別獨立運行100次，結果見表2。

表2 多次運行結果統計數據

3.2 改進蟻群算法局部動態路徑規劃

用20×20 柵格模擬移動機器人所處的運行環境， 設置起始點為（19.5，0.5），目標點坐標為（0.5，19.5），參數設置與前述靜態環境下改進蟻群算法保持一致， 初始環境下使用改進蟻群算法規劃出全局最優或者接近最優路徑，見圖10。

圖10 改進蟻群算法全局路徑規劃結果

直線障礙物1、2 和3運行軌跡見圖11，在t1 時刻， 障礙物1 運行到（13.5，4.5）點，滾動窗口內移動機器人檢測到障礙物， 并預測出其速度大小及方向， 計算出預測航跡線， 機器人到航跡的距離觸發側碰避障策略； 在t2時刻， 障礙物2 運行到（10.5，14.5）點，滾動窗口內移動機器人檢測到障礙物， 并預測出其速度大小及方向， 計算出預測航跡線， 機器人到航跡的距離觸發正碰避障策略； 在t3時刻， 障礙物3 運行到（5.5，19.5）點，滾動窗口內移動機器人檢測到障礙物，并預測出其速度大小及方向，計算出預測航跡線，機器人到航跡的距離觸發追尾避障策略， 作出如圖11 所示避障軌跡。

圖11 動態環境下改進蟻群算法避障路徑規劃結果

4 結束語

本文針對傳統蟻群算法在復雜環境中路徑規劃的缺陷，提出蟻群自適應更新域信息更新原則、隨機擴展樹優化原則、局部子起點規劃以及滾動窗口的改進蟻群算法，用柵格圖模擬移動機器人的運動環境。仿真分析表明，本文改進蟻群算法，能夠在不損失程序運行時間的前提下，加快算法收斂速度， 能夠改善陷入局部最優和迭代早期螞蟻陷入死鎖的情況；通過加入滾動窗口，為移動機器人規劃出一條能實時避開環境中動態障礙物的最優或者近似最優無碰路徑，更加適合移動機器人實際所處環境。