未知環境下的蟻群-聚類自適應動態路徑規劃*

劉新宇，譚力銘，楊春曦，翟 持

昆明理工大學 化學工程學院，昆明 650500

1 引言

未知環境下的機器人路徑規劃問題一直是機器人和人工智能領域研究的一個熱點課題[1-12]，機器人的路徑規劃是指在障礙物已知或未知的環境中，機器人按照一定的性能指標（如距離、時間等）規劃出一條由起始位置到目標位置的可通行路徑[2]。

蟻群算法（ant colony algorithm）是一種經典的智能優化算法[3]，因其在路徑規劃中的并行性、魯棒性和較易與其他算法相結合的特點，被廣泛應用在環境全部未知或部分未知的動態路徑規劃中[4-8]。然而，傳統的蟻群算法在動態路徑規劃中存在搜索時間長、收斂速度慢、易陷入局部最優等缺陷；如果在柵格環境下使用蟻群算法，所規劃的路徑還存在穿過對角障礙、轉彎次數多和累計轉折角大等問題。因此，一些學者提出了一些蟻群改進算法來緩解上述問題。文獻[9]通過改進距離啟發因子來增加目標節點對下一節點的影響，從而避免陷于局部最優解，提高收斂速度。文獻[10]利用遺傳算法的快速全局搜索能力和蟻群算法的并行、全局收斂能力，形成一種快速性和全局收斂性良好的混合算法。文獻[1]認為機器人具有一個視覺范圍，可以在視覺范圍內的一個子集中自由地選擇步長，提高搜索效率。文獻[11]采用占空比法來判斷環境復雜程度，從而選擇合適的尋優半徑，然后調用改進的蟻群算法來完成路徑的動態規劃。文獻[12]在蟻群算法中加入了平滑因子，平滑蟻群算法能夠在避開障礙物的情況下,有效地降低路徑長度,增大了轉折角度,并且路經規劃結果優于傳統蟻群算法的路徑規劃結果。

在上述改進的蟻群算法研究中，盡管不同程度地提高了搜索效率和收斂速度，減少了計算時間，但是卻沒有綜合考慮在動態環境下障礙的不確定性、搜索范圍有限和在柵格環境下規劃路徑存在轉彎次數多，累計轉折角大等問題。為此，本文提出了一種能在滿足實時性要求的前提下，基于聚類算法來準確判別障礙物分布的復雜程度，從而進行搜索半徑自動調整的自適應搜索半徑蟻群動態路徑規劃算法（self-adjust searching radius based on ant colonyclustering algorithm，SRL-CACA）。首先，通過對動態柵格障礙環境的判別，采用虛擬障礙生成法消除穿過對角障礙的路徑選擇；然后，借鑒滾動窗口法[13-14]思想設計了一種局部搜索半徑來描述機器人環境探索能力有限這一約束，并通過把局部搜索半徑設計成為可以根據環境復雜程度進行自適應調整的方式，以達到充分利用機器人有限的計算能力的目的；最后，通過在算法中加入平滑機制，減少轉彎次數和累計轉折角，提高規劃路徑的質量。

仿真實驗表明，本文所提的SRL-CACA算法在路徑距離優化、收斂優化、轉折角和動態復雜環境自適應能力方面都有較好的綜合性能。

2 環境建模

2.1 柵格法

對于機器人在任意二維地形中，有且存在著有限個障礙物；由于這些障礙物的坐標極易測繪，因而采用簡單易處理的柵格法來對搜索環境進行建模。

記G為機器人在二維平面上的有限運動區域，區域內的柵格編號如圖1所示，在G中建立直角坐標系，以G左下角為坐標原點，橫軸為X軸，縱軸為Y軸。設在相關區域內存在有限個障礙柵格，在圖1中用黑色柵格表示，自由柵格則用白色柵格表示。其中每個柵格為矩形，其邊長已知。該劃分策略從實用出發，使場景描述與實際環境嚴格相符，規劃出的路徑保證移動機器人暢通無阻。機器人僅在各個柵格內的中心點行走，則圖1機器人位置坐標的關系計算公式如式（1）和式（2）：

在關系式（1）、（2）中，a為每個柵格的邊長，橫（縱）坐標的最大柵格數值為MM，總柵格數為e=MM×MM，每個柵格的坐標為(xi,yi)，i為每個小正方形的柵格編號，mod為求余運算，而ceil為舍余取整運算。為不失一般性，這里假定機器人的起始位置和最終目的位置已知，則生成的柵格地圖環境如圖1所示。

Fig.1 Grid map圖1 柵格地圖

對于圖1中間的柵格，可以有多條可選擇路徑。這里假設任選一個柵格如圖2所示，其周圍沒有障礙物，因此處于該柵格的機器人下一步可以向鄰接的8個方位搜索，8個方位分別為右下、右、右上、上、左上、左、左下、下。按照柵格位置編號規律，容易預知這8個方位的序號及其當前位置柵格序號的差值。于是，對當前柵格位置，下一步搜索的方位如圖2表示。

2.2 動態障礙環境變化設置

為了便于理解，這里首先給出兩個定義。

定義1確認局部搜索半徑之后，需要進行局部路徑搜索，這樣局部搜索路徑規劃一次為一次規劃。局部路徑規劃的次數即規劃次數。

Fig.2 Possible routes圖2 可行路線

定義2步長為在同一條路徑中，一次規劃經過的柵格數就是步長長度。

為滿足在未知障礙環境中進行動態路徑規劃時的特點，本節設計了一種以時間步長為指標的動態障礙變化規則，即：以機器人的規劃的次數為指標來觸發障礙環境分布的變化。這里假設采用最大搜索半徑進行一次局部規劃所需時間為T,則障礙環境變化所需的時間為t≤nT,n∈Z+。其中n的取值大小與實時性有關，實時性要求越高，則n的取值越小。當機器人在時間t≤nT時，該子區域的障礙分布是固定不變的，并且機器人所在子區域外的障礙分布與本次路徑規劃無關。障礙環境變化流程如圖3所示。

Fig.3 Flow chart with time-varying obstacle environment圖3 障礙環境變化流程圖

本文設置了五種障礙環境：G1、G2、G3、G4、G5。同時采用不同的標記符號來辨別機器人在哪一個障礙環境下進行路徑規劃。同一個標記符號的個數代表了機器人在不同環境中的規劃次數。

3 蟻群算法的改進

3.1 對角障礙

當機器人處于中間柵格時，假設其鄰接周圍沒有障礙物，下一步可以向鄰接的8個方位搜索。由于在規劃過程中會遇到障礙，并且障礙的分布是不確定的，當遇到圖4所示的對角障礙時，所規劃的路徑會出現穿過對角障礙的情況。考慮到在實際情況中，這種情況屬于死角，機器人是無法通過的，因此在規劃好的路徑中需要避免出現這類情況。

Fig.4 Diagonal obstacle圖4 對角障礙

在圖4中，令紅點為機器人的當前位置，黑點為機器人可能選擇的節點位置。因此這里采用通過虛擬障礙生成法來避免產生這種情況。具體策略如下所示：

其中，MM為橫（縱）坐標的最大柵格數值；障礙柵格節點編號的集合為Ob，Ob(i)、Ob(j)分別為障礙柵格節點i和j的節點編號；D(i,j)為障礙柵格節點i和j的編號差值；F_Ob為虛擬障礙編號集合。即當兩個障礙的關系滿足式（3）時，這兩個障礙即為對角障礙，其虛擬障礙的節點編號滿足式（4），虛擬障礙生成如圖5所示。

Fig.5 Virtual obstacle圖5 虛擬障礙

在圖5中，紅色柵格是生成的虛擬障礙，即機器人在選擇下一步的目標點時，除了黑色柵格節點，紅色柵格節點也是不能選擇的。

3.2 基于聚類算法的局部搜索半徑的選擇

為了充分利用機器人有限的計算能力，需要根據環境的復雜程度進行局部搜索半徑的選擇，確保其計算能力在每次局部路徑規劃中均能夠充分利用，最終達到減少路徑規劃時間的目的。考慮到連接在一起的障礙實際上可以看成一個障礙，連接在一起的障礙越多，規劃的難度越小。而文獻[11]中的占空比法卻無法識別這類障礙。因此，這里采用聚類算法來進行障礙難易程度的準確識別。

3.2.1 K-means聚類算法思想

1967年，MacQueen提出了K-means算法。K-means算法是一種基于劃分的經典聚類算法。該算法最初隨機選擇K個數據樣本作為初始聚類中心，在每次的迭代過程中，根據計算相似度將每個數據樣本分配到最近的簇中，然后重新計算簇的中心，也就是每個簇中所有數據的平均值。該算法結束的條件為聚類準則函數達到最優即收斂，從而使生成的每個聚類內緊湊，類間獨立[15]。

如果把聚集在一起的障礙看成一個類，則該算法能夠根據障礙的聚集程度完成對環境中障礙類數的準確識別，從而可以根據搜索半徑中的障礙類數進行環境復雜程度的判別。為了區別于傳統K-means算法中簇類的定義，這里將簇類定義為柵格障礙環境中的障礙集群。

3.2.2 局部搜索半徑的選擇

在SRL-CACA算法中，為了保證選擇合適的局部搜索半徑，以獲得尋優距離、尋優時間和計算能力三個指標的綜合優化效果，所設計的局部搜索半徑選擇原則的具體步驟如下：

步驟1確定搜索邊界，本文的邊界確定原則采用文獻[11]中的填充邊界確定原則。因為其無障礙柵格占所覆蓋范圍總格數的比例較高，得到的可選節點更多更全面，全局最優路徑效果更優。

步驟2確定搜索半徑，這里設計了兩種搜索半徑確定原則：簇類最小選擇確定原則和同簇選大選擇確定原則。簇類最小選擇確定原則為：比較在不同搜索半徑所包含的柵格中，選擇聚類算法之后簇類最小的搜索半徑為本次局部路徑規劃搜索半徑。同簇選大選擇確定原則為：如果存在不同搜索半徑擁有相同的簇類值，則選擇相同簇類值中最大的搜索半徑為本次局部路徑規劃搜索半徑。簇類最小選擇確定原則可以表示為：

而同簇選大選擇確定原則可以表示為:

其中，f(·)表示對應簇類的搜索半徑函數；Φ(·)表示簇類相等的不同搜索半徑所對應的子集；g(·)表示對應簇類集合中的搜索半徑函數；φc表示具有相同簇類Kc的集合；Kc,c=1,2,…,n-1表示具有兩個以上相同簇類的簇類值。

在圖6中，紅色正方形柵格為機器人的當前位置，由內到外的兩個橙色矩形所包含的區域分別為兩步搜索半徑和四步搜索半徑所覆蓋的區域。2步搜索半徑的簇類數為4，4步為3。按照簇類最小的選擇原則應選擇4步搜索半徑。

在圖7中，紅色正方形柵格為機器人的當前位置，由內到外的兩個橙色矩形所包含的區域分別為2步搜索半徑和4步搜索半徑所覆蓋的區域。2步搜索半徑的簇類數為3，4步也為3。按照同簇選大搜索半徑選擇原則應選擇4步搜索半徑。

Fig.6 Cluster-based smallest win principle圖6 簇類最小選擇確定原則

Fig.7 Cluster-based biggest win principle from same cluster set圖7 同簇類選大選擇確定原則

在步驟2確定搜索半徑中，基于聚類算法確定簇類的具體步驟為：

步驟1初始化簇類K，即K值為局部搜索半徑內障礙柵格總數。

步驟2計算樣本集合節點之間的距離，根據鄰接矩陣計算出所有障礙柵格坐標，構成樣本集合Obstacle_data。從樣本集合Obstacle_data選擇K個數據對象作為初始聚類中心Cm,m=1,2,…,k；障礙樣本i和j之間的相異度用它們之間的坐標距離d(x,y)來表示。距離越小，則樣本i和j越相似；距離越大，兩個樣本越不相似。歐式距離公式如下：

其中，xi、yi和xj、yj分別為樣本i、j的橫縱坐標。

步驟3更新簇類K，根據d(x,y)=1，計算出d(x,y)=1的數p，K=K-p。

步驟4計算新簇的聚類中心，簇中心指一個簇中所有對象組成的幾何中心點，簇的平均值K-means算法中也稱為簇中心，簇中心的公式如下：

其中，Nk是簇k的樣本數目，Ck是指簇k的中心，Xq是樣本集合Obstacle_data中的每個數據對象。

步驟5計算聚類準則函數（sum of the squared error，SSE），K-means聚類算法使用聚類準則函數來評價聚類性能的好壞。聚類準則函數公式為：

步驟6給出一個足夠小的閾值ε＞0，在聚類過程中，如果滿足，則表示算法結束，否則返回步驟2繼續執行。

3.3 平滑機制

在柵格環境下利用蟻群算法規劃出來的機器人路徑存在轉彎次數多，累計轉折角大等問題。針對這些問題，這里提出了中點平滑機制方法，對規劃出的路徑進行轉角平滑處理，從而使得路徑相對平滑，縮短起點到終點的距離。

中點平滑機制方法的基本原則如圖8所示。考慮規劃路徑中相鄰的三個位置z(i-1)、z(i)、z(i+1)，如果夾角∠α≤δ，則對該折線啟動平滑機制：以位置z(i-1)、z(i+1)為起始位置，產生一條同時通過兩個位置的折線來替換原有折線。這里δ為角度閥值。平滑機制的具體步驟如下：

Fig.8 Smoothing optimization schematic圖8 平滑機制原理圖

步驟1計算當前節點與上一個節點的斜率k1和當前節點與下一個節點的斜率k2。

步驟2如果k1=k2，跳過這個節點，計算下個節點。

步驟3計算圖8中的a、b、c值。

步驟4根據余弦公式cosα=(a2+b2-c2)/2ab，計算出α值。

步驟5給出一個閾值δ，判斷α≤δ；如果等式成立，計算出上一節點與當前點的中點的橫縱坐標和當前點與下一節點的中點的橫縱坐標；這里給出的δ=155°。

步驟6連接節點時，連接生成的新節點，舍去z(i)節點，達到平滑目的。

由圖9可知，加入平滑機制能顯著提高線路質量，有效地降低了機器人規劃路徑的長度和累計轉彎角度。

Fig.9 Smoothing optimization圖9 平滑優化

以上三個改進，使得在動態路徑規劃過程中，對角障礙識別能有效避免出現“死角”的情況，保證路徑的成功；聚類算法能在復雜的障礙環境下，更準確地匹配步長與環境復雜度的關系；平滑優化能保證機器人在行進過程中，有效減少轉折角的幅度，降低機器人偏離規劃路線的機率。

4 改進蟻群-聚類算法的實現流程

綜上所述，機器人基于蟻群-聚類算法的自適應動態路徑規劃具體分為以下步驟。蟻群-聚類算法的流程圖如圖10所示。

步驟1首先采用柵格法對機器人的工作環境進行建模。

步驟2動態障礙變化設置。

步驟3初始化蟻群。設置螞蟻的當前位置為起點位置。初始化蟻群禁忌表、路徑表、路徑長度：禁忌表中存放的數據代表所有柵格的禁忌狀況，用來記錄柵格當前狀態；路徑表中存放蟻群經過的柵格的編號，用來記錄蟻群尋找到的路徑。

步驟4對柵格地圖進行識別對角障礙，并生成虛擬障礙。

步驟5根據規劃實時性要求確定本次動態路徑規劃的搜索半徑上界，即機器人在一次局部動態路徑規劃中所允許行走的最大步長。

步驟6機器人以當前位置為出發點，采用搜索半徑的選擇規則確定本次局部動態路徑規劃的搜索半徑值。

步驟7采用文獻[11]的方法調用隨機輪盤賭方法確定本次動態路徑規劃的最優局部目標點。

步驟8調用加入了平滑機制的可調用蟻群算法（called ant colony algorithm，C-ACA），規劃出前往最優局部目標點路徑。

步驟9通過計算本次最優局部目標點與終點的二范數是否為零來判斷該節點是否為終點。如果是終點則本次路徑規劃完成，反之則返回步驟3。

步驟10如果達到最大迭代次數，迭代終止。

步驟11篩選出最優解，然后輸出最短距離所在的尋優路徑。

Fig.10 Flow chart of ant colony-clustering algorithm圖10 蟻群-聚類算法流程圖

通過上述步驟的不斷循環，即可實現全局的動態路徑規劃。

5 仿真與分析

為了驗證提出算法的有效性，本文將和以下幾個典型算法進行對比。

（1）基于精英策略的蟻群算法（elite ant system,EAS）[3]：一種根據基本的蟻群優化算法，精英策略改進了信息素的更新方式的算法。

（2）基于柵格法的機器人路徑規劃蟻群算法（ant colony algorithm，ACA）[16]：一種靜態環境下的機器人路徑規劃仿生算法。

（3）基于統計分析的自適應蟻群算法（optimal ant colony based on statistical analysis，ACO）[17]：一種借鑒統計分析的方法提取了每一代蟻群的三個特征參數，進而改進了局部信息素更新方式的算法。

（4）自適應搜索半徑蟻群動態路徑規劃算法（selfadjust searching radius based on ant colony algorithm，SRL-ACA）[11]：一種能在實時性要求內，根據障礙物占空比進行搜索半徑自動調整的算法。

所有算法程序均采用Matlab語言進行編程，障礙變化規則為時間變換規則，路徑規劃范圍為20×20的柵格面積，為實現在同等條件下比較，五種算法的公共參數初始值設置如表1所示。

Table 1 Algorithm's public parameter settings表1 算法的公共參數設置

5.1 動態性能比較

首先，在同等條件下，圖11對比了SRL-CACA算法、SRL-CACA+算法（聚類與占空比權值各占50%的方案）和經典ACA算法。圖12對比了SRL-CACA算法、文獻[17]ACO算法和文獻[3]EAS算法。由圖11、圖12可知，五種算法均很好地適應障礙變化，各自規劃出了一條從起始點到全局目標點的優化路徑。相比較而言，SRL-CACA算法、SRL-CACA+算法在步長選擇上更合理、規劃的次數更少且路徑更短。

Fig.11 Optimization comparison among SRL-CACA,SRL-CACA+andACA圖11 SRL-CACA、SRL-CACA+與ACA尋優對比圖

Fig.12 Optimization comparison among SRL-CACA,ACO and EAS圖12 SRL-CACA、ACO與EAS尋優對比圖

其次，圖13對比了SRL-CACA算法、SRL-CACA+算法和文獻[11]的SRL-ACA算法。SRL-CACA算法比文獻[11]的SRL-ACA算法在最優路徑上有所提高，并且在步長選擇上更合理、規劃的次數更少。SRL-CACA+算法在SRL-CACA算法上也有所提高，說明聚類法和占空比法在不同的柵格障礙環境下占有不同的優勢。例如：在柵格障礙圖中，如果障礙聚集比較明顯，那么聚類法比較占優勢；而在障礙聚集不明顯，比較分散的情況下，占空比法比較占優勢。

Fig.13 Optimization comparison among SRL-CACA,SRL-CACA+and SRL-ACA圖13 SRL-CACA、SRL-CACA+與SRL-ACA尋優對比圖

在尋優對比圖11～圖13中，均出現穿越障礙的情況，不是路徑規劃失敗了，而是因為進行路徑規劃的柵格障礙圖進行周期性變化，顯示生成的圖為最后一個場景障礙圖。

這里以圖11中黑色矩形框標記的路徑進行說明，標記的路徑應為第一個尋優路徑環境下的規劃路徑，圖14同樣也是第一個尋優路徑環境下的規劃路徑，很顯然并沒有穿過障礙圖。

Fig.14 Path planning in the first obstacle environment圖14 第一種障礙環境下的路徑規劃

5.2 對角障礙的準確性

由圖15可知，圖中紅色柵格為生成的虛擬障礙，綠色實線為生成虛擬障礙前的路徑規劃路線，紅色實線為生成虛擬障礙后的路徑規劃路線。在進行路徑規劃時，未生成虛擬障礙的路徑規劃直接穿過對角障礙；而生成虛擬障礙的則會繞過對角障礙。

Fig.15 Virtual obstacle generation diagram圖15 虛擬障礙生成圖

5.3 搜索半徑選擇規則對優化參數的影響

為了測試所涉及算法對環境變化的自適應能力，在這里通過設置相同的算法參數和環境參數，比較了這五種算法在路徑尋優關鍵參數上的差異。為確保尋優關鍵參數的有效性，這里的最優路徑長和最短時間均是50次尋優結果的平均值。

首先，表2分別對比了五種算法的不同性能，最優路徑、尋優時間以及步數，并分析了算法每次得到結果的平均值。

Table 2 Performance analysis of different algorithms表2 不同算法的性能分析

由表2知，SRL-CACA算法與ACA算法對比，在最優路徑上提高了22.2%，尋優時間縮短了52.7%，步數減少了57.1%；與SRL-ACA算法對比，在最優路徑上提高了7.8%，尋優時間縮短了8.3%，步數減少了14.3%；與ACO算法對比，在最優路徑上提高了10.1%，尋優時間縮短了83.8%，步數減少了52%；與EAS算法對比，在最優路徑上提高了24.3%，尋優時間縮短了94%，步數減少了61.3%。

其次，為了進一步分析SRL-CACA算法與SRLACA算法在計算能力和路徑規劃能力的差異，這里通過一次具體的動態規劃結果來說明，如表3所示。

Table 3 Path optimization parameter between SRLCACAand SRL-ACA表3 SRL-CACA和SRL-ACA路徑尋優參數表

由表3知，尋優距離方面，本文的總尋優距離為30.06，要小于文獻[11]的36.62；尋優時間方面，本文的總尋優時間為4.30，要小于文獻[11]的4.95；計算能力方面，兩種算法均舍去異常值，文獻[11]中一次局部規劃所需的最大尋優時間與最小尋優時間的差值為0.027，而本文為0.016，顯然本文的計算能力分配更合理。并且在第7次規劃中，本文算法步長為5，文獻[11]算法步長為1，但是本文算法所消耗的時間還少，說明聚類算法對環境復雜程度的判斷是準確、有效的。

最后，為了比較聚類與占空比權值各占50%（SRL-CACA+）的方案與本文提出的SRL-CACA算法和文獻[11]的SRL-ACA算法的性能差異，這里分別進行了對比仿真實驗。具體結果如表4所示。對應的尋優對比圖如圖13。

Table 4 Path optimization parameter among SRL-CACA+,SRL-CACAand SRL-ACA表4 SRL-CACA+、SRL-CACA和SRL-ACA路徑尋優參數表

由表4可知，SRL-CACA+算法比SRL-CACA算法和SRL-ACA算法在最優路徑、尋優時間和步數三個指標上都有所提高，說明加入聚類和占空比權值分配方案在復雜未知環境下的動態路徑規劃上更具優勢，因此，如何實現合理的權值分配具有良好的研究價值。

6 結束語

本文主要在復雜動態環境進行動態路徑規劃的過程中，提出了基于蟻群-聚類算法的一種新型的搜索半徑自適應蟻群動態路徑規劃方法。通過自適應調整搜索半徑，使得機器人的計算能力始終得到充分的利用，從而進一步縮短路徑距離，加快算法收斂速度。通過對對角障礙的識別，生成虛擬障礙，避免了穿過死角的情形。通過對算法規劃出的路徑進行平滑優化，顯著提高了線路質量，降低了機器人規劃路徑的長度和累計轉彎次數。通過仿真實驗驗證了文中改進算法的有效性。后續研究中，將針對聚類與占空比權值自動分配問題，進行下一步研究。