改進蟻群算法的移動機器人路徑規劃

唐旭暉，辛紹杰

上海電機學院 機械學院，上海 201306

移動機器人的路徑規劃是指，在機器人位姿與環境信息都已知的情形下，尋找出一條自起點至目標點的無碰撞最優路徑。路徑規劃問題的核心便是路徑規劃算法，目前可分為傳統規劃算法和仿生群智能優化算法。其中，傳統路徑規劃方法有Dijkstra算法[1]、A*算法[2]、RRT算法[3]等，隨著群智能優化算法的發展，粒子群算法[4]、蟻群算法[5]、灰狼算法[6]等也逐漸在路徑規劃的應用中嶄露頭角。本文選擇的蟻群算法（ant colony optimization）是意大利學者Dorigo等人提出的一種基于狀態轉移概率和信息素更新機制的模擬蟻群覓食過程的智能算法，隨迭代次數增加，蟻群始終能在全局環境下搜索到一條最優路徑，但是該算法的收斂速度慢且易陷入局部最優。徐菱等人[7]提出一種基于16方向24鄰域的搜索方式，結合向量夾角改進啟發信息，增加蟻群搜索范圍，提高全局搜索能力。封聲飛等人[8]提出一種自適應蟻群算法，根據環境信息自適應調節初始信息素，并采取信息素排序擇優更新機制，提升了算法收斂速度。張強等人[9]以改進人工勢場算法為蟻群規劃出起始路徑，避免早期的盲目搜索，并構建負反饋機制，使信息素更新根據收斂次數自調節，增強收斂速度與全局尋優的協調性。針對傳統蟻群算法的收斂速度慢與易陷入局部最優問題，本文提出一種改進的蟻群算法，其中包括：根據環境信息構建目標點導向區，增強目標點導向區的初始信息素濃度，提高蟻群初期搜索方向的導向性；對地圖分塊處理并檢測路徑折點信息，通過叉積運算進行局部優化，改善路徑的曲折狀況；改進基于精英策略蟻群算法的精英信息素增強因子，并添加隨環境變化可自適應調節的衰減因子，同時引入隨機狀態轉移參數，提高蟻群搜索的隨機性，避免陷入局部最優，增強算法局部優化和全局尋優能力的平衡性。

1 路徑規劃環境建模

本文基于柵格地圖進行改進算法的路徑規劃分析，柵格地圖建模如下：構造環境矩陣G，定義矩陣G中元素0和1分別記為可行節點和障礙物節點。根據對應關系將矩陣G轉化為鄰接矩陣D，鄰接矩陣D中為柵格圖各節點間的通行代價，定義各節點間僅可相鄰或對角通行，相鄰和對角節點通行代價分別記為1和1.4，不可通行代價記為0，其轉換形式如圖1所示。

圖1 節點圖轉換示例Fig.1 Example of node diagram conversion

2 蟻群算法

傳統蟻群算法采取輪盤賭法的機制進行路徑節點選擇，其狀態轉移概率如式（1）、（2）所示：

式（3）為精英蟻群算法[10]信息素更新模型，其中，ρ為揮發系數，為第k只螞蟻經過(i,j)路段的信息素增量，w為螞蟻總數，e為最優路徑信息素增強權重系數，為最優路徑信息素增量。Ck是第k只螞蟻所走路徑總長，Cb為最優路徑Tbs的總路長。

3 改進蟻群算法

3.1 構建目標點導向區

蟻群算法最初用于解決TSP問題，現將其應用于移動機器人的路徑規劃問題后，路徑節點的選取過程中，部分小區域內的節點對產生最優路徑的影響效益小，如示意圖2所示。

圖2 20×20環境示例Fig.2 Example of 20×20 environment

如圖2所示，在該20×20環境下若定義左上角為起點，右下角為移動機器人需到達的終點，則可發現在最優路徑搜尋過程中，虛線橢圓范圍內的節點處于一種“尋路性價比”低的狀態，此類節點對路徑尋優的實用度和可靠性偏低。傳統蟻群算法采取初始信息素均勻分布策略，因此ηij是蟻群在初期狀態轉移過程中的唯一根據，所有節點的被搜索概率差異小，令蟻群初期搜索范圍遍布全局，使得面對路徑規劃實際應用時，蟻群在搜索初期會不可避免地耗時搜索于“低性價比區域節點”，形成初期搜索存在盲目性和初期收斂速度較慢等問題[11-13]。因此文獻[8]提出一種初始信息素差異化處理，以起點與目標點連線作為基準線，以各節點到該基準線的距離作為參考來對初始信息素進行差異化分布，越靠近基準線的節點的初始信息素濃度越高，由此即可形成范圍性非均勻信息素對比，令蟻群在路徑規劃的初期搜索過程中傾向于靠近尋優效益性價比高的范圍內搜索，避開低效益區域，可在搜索初期耗時更短地規劃出可靠性更高的較優路徑以作后續搜索參考。文獻[8]所設差異化區域在遇到地圖環境中心存在較大障礙物或障礙環境繁密復雜以及存在U型障礙區間等情況時，部分處于差異化區域內經過初始信息素濃度增強處理的節點，反而會影響蟻群路徑搜索的效果，造成路徑多彎折的問題。基于此本文提出在目標點附近設立導向區作為改進，并對該優化區內的初期信息素給予一定的增強，為蟻群的初期搜索起到引導作用，有效降低初期搜索盲目性。根據地圖信息，對地圖進行矩形分塊處理，以目標點E為圓心，R為半徑的圓周范圍設立為目標點導向區，計算方式如式（7）、（8），增強導向區信息素濃度，信息素差異化分布規則如式（6）所示：

式（6）中，d(j,E)為節點j到終點E的歐式距離，τ0為初始信息素濃度，λ為[1，2]的初始信息素增強權值。式（7）中，S為矩形分塊區邊長值，D為起點至目標點的歐氏距離，m和n分別為柵格地圖的長和寬，ξ為矩形分塊區域內的障礙物占比，ξ值越大，則目標點導向區范圍越大。式（8）中，O和V分別為矩形分塊區內的障礙物數量和可行節點數。以20×20規模地圖為例，取S=10進行分塊，目標點導向區示意如圖3所示。

圖3 目標點導向區Fig.3 Target point orientation area

如圖3所示，將20×20地圖進行分塊后，以目標點為中心的扇形區域為目標點導向區，由此可形成初期信息素差異化，蟻群在初期搜索過程中，當節點搜索行徑至2、3、4分塊區時，會傾向于靠近4分塊區的目標點附近范圍搜索，避開2、3分塊區的“低性價比”范圍，并且受環境障礙因素的影響小，更快找到達到目標點的路徑，減少初期搜索耗時。

3.2 局部路徑分塊優化

傳統蟻群算法的評價函數以全局路徑長度作為參考標準，從而忽視了對局部范圍路徑的好壞評價。如圖4所示路徑中，局部區域存在冗余折點狀況，同時影響全局路徑的長度評價。在移動機器人的實際工作環境下，多折點路線會對機器人的實體運動造成抖動、徘徊等負擔。因此提出局部路徑分塊優化策略，對冗余折點狀況進行改善。

圖4 冗余多折點路徑示意圖Fig.4 Redundant multi-fold path schematic

對地圖進行均勻的矩形劃分，隨后利用叉積運算依次檢測多個折點與附近障礙物節點間的位置關系，若存在冗余折點則采取禁忌搜索結合回退策略，將折點暫時加入禁忌搜索冗余節點集并回退至折線段起始點，螞蟻重新搜索路徑并與最初原路徑進行比較，經過循環多次檢測、判斷、比較后得出最終優化路徑，局部折點優化示意如圖5所示。

圖5 多折點示意圖Fig.5 Multi-fold diagram

圖5路徑中存在兩處冗余折點，取S=5對地圖進行局部分塊，利用叉積運算檢測，規則如式（9）所示：

其中，U為冗余節點集，i為中間點，i′、i′為兩端點，io為障礙節點。圖4中，點A、C、B之間點A、C互為鄰居節點位于分塊1區，點B位于分塊3區且AB連線無障礙節點，合并分塊區通過叉積運算檢測得C點為冗余節點，將點C暫時加入冗余節點集，螞蟻回退至節點A重新搜索，經過反復循環對比獲得安全路徑“A-H-B”，同理可優化冗余節點D、E得路徑“B-F”，最終完整局部路徑為“A-H-F”，折點數與路徑長度均比原路徑“A-C-B-D-E-F”有較大改善。

將傳統蟻群算法應用于20×20地圖路徑規劃，進行局部優化前后對比，如圖6所示。

圖6 20×20地圖局部優化對比Fig.6 Comparison of local optimisation of 20×20 maps

由圖6可知，原始路徑折點數為25個，存在6處冗余折點，經過局部優化后的路徑折點數為12個，僅存1處冗余折點，優化后路徑更貼合機器人運動方式。

3.3 改進信息素更新機制

精英蟻群易受精英信息素的持續增強而過早陷入局部最優，因此提出一種隨迭代次數增加而自適應調節的增強因子e(t)，起到各螞蟻信息素之間的平衡作用。考慮到機器人需避開障礙物完成行徑動作，提出一種隨障礙物數量自適應的衰減因子，作為路段信息素增量的權重系數，改進的信息素更新公式如式（10）、（11）、（12）所示：

式（11）中，φ為(i,j)路段鄰居節點集中障礙物的占比率，e(t)為自適應增強因子；式（12）中ω為[0,1]可選取系數，t為迭代次數。可知隨障礙物數量增加，(1-φ)逐步衰減，降低相對應障礙物路段的信息素含量；隨迭代次數的增加e(t)逐次降低，保障了初期精英螞蟻影響力的同時降低后期的過度影響，有效避免蟻群過早陷入局部最優，增強全局搜索能力。

3.4 隨機狀態轉移機制

傳統蟻群的路徑節點選取，只取決于Pk ij(t)的大小，迭代至后期，信息素濃度趨于穩定，高濃度信息素的路段被選取概率大，路徑搜索出現停滯現象[14-16]。本文借鑒游曉明等人[17]提出的動態誘導策略，結合鯨魚優化算法中的隨機捕獵思想，提出一種隨機狀態轉移機制，通過設置狀態轉移參數σ∈(0,1)和隨機數μ=rand(0,1)對比，對蟻群的搜索狀態進行調控，增加蟻群搜索機制的隨機性，避免算法過早陷入停滯狀態。隨機狀態轉移規則為：當μ≥σ時，蟻群的狀態轉移根據Pk ij(t)輪盤賭策略決定，當μ＜σ時，蟻群根據式（13）、（14）更新下一路徑節點位置。

其中，xt為當前位置點，x(t+1)為下一位移點，xτl為當前位置點所屬鄰居節點集中信息素含量最低節點，L為隨迭代次數增長而下降的步長值。由式（13）可知，當μ＜σ時，蟻群被迫強制選取最差鄰居節點并以一定步長位移。本文在移動機器人路徑規劃中采取式（15）作為路徑評價目標函數。

3.5 算法步驟

本文改進蟻群算法的路徑規劃流程如圖7所示。

圖7 改進算法路徑規劃流程圖Fig.7 Improved algorithm path planning flow chart

在改進算法中，實現路徑規劃的具體步驟如下：

步驟1初始化地圖矩陣G、種群數量K、最大迭代次數tmax、初始信息素τ0、冗余節點集U、初始信息素增強系數λ、分塊邊長值S、D、信息素揮發系數ρ、ω、φ等參數。

步驟2根據式（6）、（7）、（8）和S值，對地圖進行分塊并設立目標點導向區，調整初始信息素。

步驟3將第k只螞蟻放置于起始點。

步驟4根據隨機狀態轉移機制與式（1）、（2）、（13）和（14）更新下一步位移節點。

步驟5判斷是否達到目標點或陷入死胡同，若未到達目標點且未陷入死胡同轉至步驟4；若陷入死胡同則回退至上一節點，并將當前節點加入禁忌表后轉至步驟5重新判斷；若已達到目標點轉至步驟6。

為了驗證所提控制策略的有效性,在PSIM環境下對MPDPC、定系數降頻MPDPC和變系數降頻MPDPC做了仿真對比研究,系統的參數如表1所示。為了簡化,將上述3個控制策略分別依次定義為MPDPC.I,MPDPC.II和MPDPC.III。給定有功功率為1 000 W,給定無功功率為0 VAr,以保證系統單位功率因數運行。

步驟6判斷是否所有K只螞蟻已完成此次迭代中路徑規劃任務，若未完成則令k=k+1并轉至步驟3；若均已完成則轉至步驟7。

步驟7根據式（15）判斷并記錄每一代最優路徑長及路線點。

步驟8根據分塊優化策略及式（9）對每一代最優路徑進行局部優化和全局整合，并記錄對應節點信息。

步驟9根據式（10）和（11）更新信息素矩陣。

步驟10若迭代未滿最大值，則令t=t+1,k=1，轉至步驟3；反之轉至步驟11。

步驟11輸出全局最優路徑和最佳適應度值。

4 仿真實驗及數據分析

為了驗證改進算法的有效性，搭建柵格地圖進行實驗，仿真實驗環境：操作系統Windows 10（64 bit），處理器Core?i7-7700，CPU3.60 GHz，運行內存16 GB，仿真平臺MatlabR2018b。

4.1 狀態轉移參數σ分析

為驗證狀態轉移參數σ對本文改進算法在搜索方式及全局搜索能力方面的影響效果，本節在30×30地圖中以傳統蟻群算法為基礎模型，僅引入隨機狀態轉移機制，模擬文獻[7]中參數仿真分析實驗的控制變量方法等。控制如：蟻群數量K、迭代次數tmax、全局初始信息素濃度τ0、揮發因子ρ、信息素啟發因子α、能見度啟發因子β等參數值固定不變，依次改變σ的取值為{0.1，0.3，0.5，0.7，0.9}，進行五種情況下的分批多次實驗，觀察σ取值不同時對蟻群搜索的影響效果，并根據實驗結果選取出σ的合適取值。其余參數固定取值如表1所示。

表1 仿真參數Table 1 Simulation parameters

隨機狀態轉移機制仿真結果如圖8所示。

圖8 σ取值不同時螞蟻搜索路徑Fig.8 Ant search path whenσtakes different values

由圖8可知，當σ取值過大時，蟻群的搜索路徑過于發散；當σ取值過小時，蟻群搜索能力受限，驗證了σ對蟻群搜索范圍可調控的有效性，綜上考慮取σ=0.3作為參數設定值平衡改進算法的全局搜索能力與收斂性。

4.2 算法仿真對比

傳統蟻群算法遵循標準蟻群的搜索機制，在初期搜索范圍遍布全局，初期蟻群的搜索方向無引導性，收斂速度慢；文獻[8]采取以始末兩點基準線為參考的初始信息素差異化分布，提升收斂速度，但最終路徑搜尋效果易受地圖中心區障礙或U型障礙環境影響；樽海鞘群算法以領導者結合追隨者的更新機制尋求目標函數的最優問題，該算法中采取隨機位置更新機制系數c3來決定位置更新方向和隨機步長系數c2來決定移動長度。為驗證本文改進算法中初始信息素差異化策略、折點優化策略及隨機狀態轉移機制在路徑規劃應用中的綜合效果，將本文改進算法與傳統蟻群算法、樽海鞘群[18]算法及文獻[8]改進算法分別在30×30和50×50地圖中進行路徑規劃的實驗對比分析，驗證算法收斂速度、路徑長度及路徑彎折提升的有效性，仿真參數如表2、3所示。

表2 30×30地圖仿真參數Table 2 30×30 map simulation parameters

表3 50×50地圖仿真參數Table 3 50×50 map simulation parameters

4.2.1 簡單環境30×30地圖實驗對比

在障礙物占比率為20%的30×30地圖中，實驗結果如圖9、圖10所示。

圖9 簡單環境30×30地圖路徑對比Fig.9 Comparison of 30×30 map paths in simple environment

圖10 簡單環境30×30地圖收斂曲線對比Fig.10 Comparison of 30×30 map convergence curves in simple environment

由表4可知：在30×30簡單環境地圖中，由于障礙物占比率低且分散，蟻群可探索范圍空曠且可探索節點數多，傳統蟻群和樽海鞘群因初期搜索無方向引導性，導致在面對可搜范圍空曠的環境下收斂速度慢，最終未能實現完全收斂；文獻[8]算法與本文改進算法在迭代次數達到90～95時呈逐漸收斂趨勢。本文改進算法拐點個數比傳統蟻群和樽海鞘群分別減少了82%和70%；路徑長度分別縮短了31%和22%。文獻[8]采取初始信息差異化分布，并采用排序擇優方式更新信息素濃度提高收斂速度，但初始搜索范圍會偏向于始末點間的連線狹小區域，且在后續信息素更新中該趨勢逐步增加，如圖9（c）中路徑對比可知，當地圖中央區域存在有較大障礙物使得起點和目標點連線區形成阻斷形勢時，文獻[8]的搜索效果受到干擾，出現向中心線靠近的多折點現象。而本文改進算法在引入目標點導向區范圍的初始信息素差異化分布后，對比傳統蟻群在收斂速度方面有所提升，且應對此類中間區存有障礙物阻斷的地圖環境中有較好效果，本文改進算法的路徑長度雖僅比文獻[8]路徑縮短12%，但拐點個數比文獻[8]減少了62%，本文改進算法應對地圖環境抗干擾性有所提高，折點優化效果良好。

表4 簡單環境30×30地圖數據對比Table 4 Comparison of 30×30 map data in simple environment

4.2.2 復雜環境30×30地圖實驗對比

在障礙物占比率為23.77%的30×30地圖中，實驗結果如圖11、圖12所示。

圖12 復雜環境30×30地圖收斂曲線對比Fig.12 Comparison of 30×30 map convergence curves in complex environments

由表5可知：在30×30復雜環境地圖中傳統蟻群和樽海鞘群分別在迭代次數達到95次和52次后趨于完全收斂，本文改進算法與文獻[8]均在迭代至約46次實現完全收斂，本文改進算法在路徑規劃應用中，收斂速度方面對比傳統蟻群采取初始信息素均勻分布策略有較大提升；路徑長度平均縮短約24%，拐點數平均減少約53%。由圖11（b）觀察得，在面對復雜環境下，本文改進算法的隨機狀態轉移機制對比樽海鞘群算法中的隨機移動步長結合隨機移動方向機制，樽海鞘群因搜索過度隨機而出現路徑回環，而本文改進算法在迭代次數與其相近的條件下，良好地避免了路徑回環及死鎖的現象。由圖11（c）觀察得，文獻[8]路徑在復雜環境下仍受地圖中心區障礙物阻斷影響，在柵格圖節點搜索中出現穿越障礙物狀況，本文改進算法受地圖障礙干擾性小，路徑長度、迭代次數與文獻[8]相近的情況下，拐點個數更少且路徑良好完成了避障效果。

圖11 復雜環境30×30地圖路徑對比Fig.11 Comparison of 30×30 map paths in complex environments

表5 復雜環境30×30地圖數據對比Table 5 Comparison of 30×30 map data in complex environments

4.2.3 50×50地圖實驗對比

在障礙物占比率為23.24%的50×50地圖中，實驗結果如圖13、圖14所示。

圖13 50×50地圖各算法路徑對比Fig.13 Comparison of 50×50 map paths of algorithms

圖14 50×50地圖各算法收斂曲線對比Fig.14 Comparison of 50×50 map convergence curves of algorithms

由表6可知：在50×50地圖中，本文改進算法在收斂迭代次數方面比傳統蟻群和樽海鞘群平均減少45%；在拐點數方面平均減少81%；在路徑長度方面平均減少21%。在50×50復雜環境下，傳統蟻群與樽海鞘群初期搜索方向均無引導性，遍布全局地圖，致使收斂速度慢，樽海鞘群因隨機移動方向和隨機移動步長機制以及隨機參數取值問題，導致在復雜環境下，路徑的后期搜索未實現最終收斂。本文改進算法通過初始信息素差異化加快了算法初期收斂速度，并由隨機狀態轉移機制和對隨機因子σ的取值分析，較好地平衡了搜索隨機性與算法收斂性。本文改進算法與文獻[8]的收斂迭代次數和路徑長度方面均相近，但在50×50的復雜環境地圖下，由圖13（c）可知，障礙物不規則的復雜環境對文獻[8]算法的實現存在干擾性，面對U型障礙區和對角相鄰特征的障礙存在回拐和穿越等現象。本文改進算法在路徑搜索過程中較好地完成了對U型區及復雜鄰接障礙的避障效果，完整路徑的拐點個數比文獻[8]減少約58%。

表6 50×50地圖數據對比Table 6 Comparison of 50×50 map data

5 結語

在傳統蟻群算法中初期搜索存在盲目性，本文提出設立目標點導向區形成初始信息素差異化分布，為蟻群初期搜索提供向目標點移動方向的參考導向，且導向區范圍影響力隨環境因素變化而改變，并非固定比例的類橢圓型優化區，避免了蟻群搜索偏于向地圖中心靠攏，為蟻群指引目標方向的同時也具有一定的搜索隨機性和尋優能力；改進最優路徑信息素增強因子為隨迭代次數變化的自適應調節因子，并引入隨機狀態轉移控制參數及鄰居節點轉移規則，解決傳統蟻群算法初期搜索收斂慢的同時避免算法中后期過早收斂陷入局部最優；引入局部分塊優化策略結合叉積運算規則，改善算法規劃路徑產生的冗余節點，使得在面臨地圖環境大、障礙分布復雜及存在類U型障礙和鄰接障礙集的情況下，規劃出可有效避障且折點數少的最優路徑，更符合移動機器人運動方式。最后，通過在多種地圖下與其他算法（傳統蟻群、樽海鞘群、文獻[8]改進算法）進行對比實驗仿真驗證了本文改進算法的有效性。