動態(tài)環(huán)境下蟻群算法的路徑規(guī)劃研究

胡國棟， 陳 波， 歐鳳琴

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

0 引言

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

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

1 全局路徑規(guī)劃

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

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

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

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

1.2 擴展樹隨機優(yōu)化原則

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

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

圖1 擴展樹隨機優(yōu)化流程圖

1.3 信息素更新素規(guī)則

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

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

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

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

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

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

圖2 解決螞蟻死鎖流程圖

1.4 改進蟻群算法步驟

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

圖3 改進蟻群算法步驟

2 動態(tài)環(huán)境局部路徑規(guī)劃

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

2.1 局部避障策略

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

圖4 滾動窗口監(jiān)測障礙物

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

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

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

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

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

圖5 機器人與障礙物可能發(fā)生側碰

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

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

2.2 動態(tài)規(guī)劃流程

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

圖7 滾動窗口規(guī)劃流程圖

3 仿真實驗及分析

3.1 改進蟻群算法全局靜態(tài)規(guī)劃

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

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

圖8 多算法路徑規(guī)劃路徑結果比較

圖9 多算法路徑規(guī)劃路徑長度比較

表1 單次運行結果統(tǒng)計數(shù)據(jù)

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

表2 多次運行結果統(tǒng)計數(shù)據(jù)

3.2 改進蟻群算法局部動態(tài)路徑規(guī)劃

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

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

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

圖11 動態(tài)環(huán)境下改進蟻群算法避障路徑規(guī)劃結果

4 結束語

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