基于有效拐點和最短最小路徑的蟻群路徑規劃方法

褚凱軒 常天慶 王全東 閆曉東

(1.陸軍裝甲兵學院兵器與控制系， 北京 100072； 2.軍事科學院評估論證研究中心， 北京 100091)

0 引言

路徑規劃是移動機器人在具有障礙物的環境約束下，根據給定的起點和終點，應用算法準則，找到一條最優或接近最優的、安全、無障礙路徑[1-2]。路徑規劃主要包括環境建模和路徑規劃算法兩部分。

柵格圖法是環境建模的經典方法[3]，柵格圖將地圖切割成相同大小且相互連接的柵格，每個柵格對應相應位置信息，能在考慮全局優化的基礎上兼顧較小的計算量, 對特定感知系統的假設參數不敏感，具有較強的魯棒性[4]，目前被廣泛運用于機器人、自動駕駛的運載器等領域[5-7]。柵格圖下，傳統的路徑搜索方式主要有4鄰域4方向搜索方式和8鄰域8方向搜索方式。徐菱等[8]提出一種24鄰域16方向的搜索方式，使路徑搜索具有更廣的搜索范圍和更多的方向選擇。但是有限方向有限鄰域使機器人每次只能在相鄰柵格間轉移，增大了搜索次數和步數。曾明如等[9]提出了一種搜索范圍不局限于相鄰柵格的多步長柵格圖法，能搜索出一條轉移步數更少的路徑,該方法在面臨較大規模地圖時計算量巨大。文獻[10-11]提出直接將信息素存放于柵格中而不是存放于路徑上，這種信息素存儲方式可以降低算法的復雜度，但是由于失去了方向性，在大規模柵格路徑規劃中易發生閉鎖和折回的現象。

路徑規劃算法主要有局部避障算法、基于幾何模型算法和智能搜索算法。局部避障算法包括人工勢場法[12]和動態窗口法[13]。人工勢場法通過模擬障礙物產生的斥力場和終點產生的引力場，推動機器人避障并向終點移動。動態窗口法是在速度空間中采樣多組數據，并模擬移動機器人以這些速度在下一時間段內的軌跡并對軌跡進行評價，選取最優軌跡對應的速度來驅動機器人運動。基于幾何模型算法主要有Dijkstra算法[14]和A*算法[15]，Dijkstra算法計算起始點到其他所有點的最短路徑長度，是廣度優先搜索，具有較高的空間復雜度和時間復雜度，而A*算法是一種啟發式算法，是一種深度優先的算法。智能搜索算法有蟻群算法[16-19]、遺傳算法[20]、粒子群算法[21]、人工蜂群算法[22]等，是利用群體智能的快速收斂特性尋找最優解。路徑規劃屬于NP-hard問題,因此很多學者采用啟發式群智能算法進行求解。蟻群算法模擬自然界螞蟻尋找最短覓食路徑，每只螞蟻在覓食時都會在路徑上留信息素，并且路徑越短，信息素濃度越高。螞蟻更傾向于沿著信息素濃度較高的路徑行走，并繼續在路徑上留下新的信息素。通過上述正反饋效應，最終蟻群路線收斂到信息素濃度最高的最短路徑上。蟻群算法具有魯棒性好、全局搜索能力強、環境約束表達方便等優勢，被廣泛應用于路徑規劃問題中。傳統的蟻群算法存在迭代次數多、搜索效率低等缺點。為了解決這些問題，LEE[16]提出了一種基于異構蟻群的路徑規劃方法，該方法改進了蟻群的轉移概率函數和信息素更新規則，在不需要后續平滑處理的情況下可以直接找到節點數較少的最優路徑；ZHANG等[17]提出了一種智能蟻群路徑規劃算法，該方法通過消除蟻群算法中禁忌表的約束，引入臨時權值矩陣，避免了小權值路徑的重復選擇，提高了算法的效率；LEE等[18]改變了蟻群算法生成初始種群的方式，縮短了尋找最優解的時間，加快了算法的收斂速度；SAIDI-MEHRABAD等[19]將蟻群算法分為兩個階段進行路徑搜索，有效縮短了路徑規劃的完成時間。劉可等[23]采用參數遷移的方法對蟻群算法進行參數優化，利用先驗知識有效解決蟻群算法路徑規劃時的參數選擇問題。張強等[24]在蟻群算法中構建負反饋通道，使全局信息素和局部信息素的更新速率跟隨收斂次數的變化自適應調節，使收斂速度與全局搜索精度協調統一。

本文在柵格地圖中提出有效拐點，使路徑規劃能夠以任何方向、任何距離的柵格作為下一步的目標柵格的方法，以提高搜索效率并降低轉彎頻率。提出最短最小路徑并給出計算方法，將最短最小距離作為蟻群算法的啟發值，作為閾值來指導信息素更新，以期提高引導效率和算法前期的收斂速度，并提高信息素更新的質量。

1 基于拐點的柵格圖

柵格圖將地圖切割成相同尺寸且相互連接的柵格，每個柵格對應相應位置信息。3種典型柵格圖搜索方式如圖1所示。

本文提出一種能夠以任何方向、任何距離的柵格作為下一步目標柵格的搜索方式，大大提高了搜索效率并降低轉彎頻率。如圖2所示，在某一無障礙的環境中，采用4種搜索方式搜索機器人運動軌跡，考慮到地圖設定簡單，采用常規蟻群算法[8]進行路徑搜索，蟻群算法關鍵參數有：迭代次數K=10，螞蟻數量m=20，信息素啟因子α=2；啟發函數因子β=3，信息素蒸發系數ρ=0.3。由圖2可以看出，4鄰域4方向搜索方式只能走直線拐直角，從起點到終點最短路徑長度為14，步數為14；8鄰域8方向搜索方式可以走45°，從起點到終點的最短路徑長度為9.1，步數為7；24鄰域16方向搜索方式從起點到終點的最短路徑為8.7，步數為5；本文的搜索方式可以取捷徑直達終點，路徑長度為8.6，步數為1。

鄰域數量的增多，意味著單步搜索空間增大，為了降低單步搜索的計算量，本文提出柵格拐點。考慮最短路徑一定會繞過障礙柵格，因此搜索過程中僅需考慮障礙物的鄰域柵格，即拐點一定是障礙物的鄰域柵格。采用滑動窗口的方法尋找拐點，設定2×2滑動窗，遍歷整個地圖，當窗內僅有1個障礙物柵格時，將窗內其他3個柵格均設定為拐點；當窗內有2個障礙物柵格，且位置處于對角，則將另外一對對角的2個柵格設定為拐點。幾種典型拐點如圖3所示，其中黑色表示障礙物柵格，白色表示可通行柵格，紅色表示拐點。路徑搜索時，只需要搜索拐點。通過設定拐點，可以大大減少單步搜索的工作量。

2 最短最小路徑

能夠快速、準確地估計各搜索節點到終點的距離對于全局的路徑規劃具有重要意義。如在A*算法中，當前節點i的代價函數為f(i)=g(i)+h(i)，式中g(i)是移動機器人從起點到當前節點i走過的實際距離，h(i)是從當前節點i到終點的估計值。g(i)是準確值，而h(i)是估計值，正確選擇h(i)函數可以提高A*算法中搜索和選擇的科學性。傳統方法采用歐氏距離[3]表示h(i)。

啟發式算法中，需要用搜索節點的啟發值計算選擇概率，而啟發值由搜索節點到終點的距離估計值決定。例如在蟻群算法中，選擇概率由信息素濃度和啟發值共同決定，在迭代初期，路徑信息素濃度差別不大，主要依靠啟發信息選擇節點，因此準確的啟發值有利于快速收斂。常見的蟻群算法[11，25]、蜂群算法[26]也多采用歐氏距離函數表示搜索節點到終點的距離估計值。

采用歐氏距離，即認為搜索節點可取捷徑到達終點，使得距離終點直線距離更近的點更容易被選擇，但是當捷徑路線上存在障礙物時，這種選擇傾向可能是錯誤的。如圖4所示，起點為S，終點為E,考慮A、B兩點，點A距離終點E的直線距離明顯小于點B距離終點E的直線距離，但是由于AE之間存在障礙物，無法通行，因此更近的路徑是S-B-E。如果能夠在搜索節點到終點的距離估計中，發現B到E的距離比A到E的距離更近，可大大提高節點搜索和選擇的科學性。

本文提出最短最小路徑，并用最短最小距離代替歐氏距離，用于算法前期搜索節點的啟發值。當起點、終點和拐點確定后，根據柵格點之間是否可直達，構造從起點到終點的貫通樹。具體操作為：

增枝操作步驟如下：①將終點E置于最底層L0。②將所有與上一層Ln-1可通的點置于當前層Ln。③判斷起點S是否在當前層，如果S∈Ln，則算法結束，否則轉步驟②。

增枝操作得到貫通樹T1。增枝操作依據各點到達終點的步數，對有效柵格進行分層。最底層為終點柵格；第2層為所有與終點柵格可通的點；第3層為所有與第2層任一點可通的點；以此類推，最上層為起點柵格和所有與起點柵格處于同一層的點。

剪枝操作步驟如下：①最上層僅保留起點柵格，其它柵格從最上層中刪除。②保留當前層Ln中與上一層Ln+1可通的點，其它點從當前層刪除。③如果檢索到L0，則算法結束，否則轉步驟②。

剪枝操作得到貫通樹T2。

設從起點到終點的某條路徑Rout=[a1(S),a2,a3,…,a(E)],令ci表示點ai所在的層數，如果Rout為最短路徑，必滿足ci≥ci+1,即下一個點必須在當前點所在層或距離終點更近的層，不可能是遠離終點的一層。基于這一條規律，在啟發式路徑搜索中，只需要搜索與當前節點處在同一層的點和當前節點的下一層點，而不需要搜索所有與當前點可直達的拐點，可以大大減小搜索壓力。

設從起點到終點的某條路徑Rout=[a1(S),a2,a3,…,a(E)]，當對于任意i，均滿足ci>ci+1時，Rout為最小步數路徑。所有最小步數路徑中的最短路徑稱為最短最小路徑。本文用最短最小路徑的長度作為蟻群算法前期的啟發值。

3 蟻群算法求解最短路徑

為了配合前文的拐點柵格和最短最小路徑，本節對蟻群算法進行特定的設計和改進。蟻群算法路徑規劃流程見圖5。

3.1 有效拐點和最短最小路徑

首先刪減地圖中的無效拐點，并計算有效拐點到終點的最短最小距離，步驟如下：

(1)構造柵格地圖，并用兩種滑動窗循掃整幅地圖，找到所有拐點。

(2)采用增枝操作，得到貫通起點到終點的貫通樹T1，采用剪枝操作，得到貫通樹T2，保留貫通樹T1最上層的拐點和貫通樹T2中所有的拐點為有效拐點，其余拐點均視為無效拐點。

(3)初始化信息素矩陣，如果節點i、j滿足：Li≥Lj或柵格點i與柵格點j不可直達，則將從節點i到節點j的路徑上的信息素強制置零。

(4)計算所有有效拐點到終點的最短最小距離，存放于距離期望矩陣中。

3.2 轉移概率

螞蟻從當前柵格i到下一處柵格j的轉移概率為

(1)

(2)

式中α——信息素啟發因子

β——啟發函數因子

allowed——可選擇的柵格點集合

τij——點i到點j的信息素濃度

ηij——螞蟻從點i到點j的啟發信息

τis——點i到點s的信息素濃度

ηis——螞蟻從點i到點s的啟發信息

dij——點i到點j的距離

dj→end——點j到終點的距離期望

3.3 距離期望

初始時，用點j到終點的最短最小轉彎路徑長度作為距離期望。迭代過程中，實時更新距離期望矩陣，當螞蟻搜索到一條從起點到終點的可行路徑Rout=[a1(S),a2,a3,…,a(E)]，比較中間節點ai沿該條可行路徑到終點E的實際距離與節點ai到終點的當前距離期望，如果實際距離小于距離期望，則用該距離代替距離期望。

3.4 信息素更新

算法每次迭代中,讓所有螞蟻各自完成一次路徑搜索,并在每只到達終點或出現死鎖時停止搜索，每次迭代結束后,根據各螞蟻搜索到的路徑，各柵格的信息素濃度為

(3)

式中m——蟻群數量

此處改進在于，由于已經有了最短最小轉彎路徑作為參考，本文認為只有螞蟻成功搜索到終點，且長度小于等于最短最小轉彎路徑的長度才算對群體有貢獻，因此信息素濃度增量取

(4)

式中Q——常數

dk——螞蟻k從起點到終點走過路徑的距離

dm——起點到終點的最短最小距離

4 仿真實驗

為了驗證本文算法的有效性和可行性,通過仿真實驗對算法的相關性能進行驗證，并與文獻[8]中提出的方法進行對比。仿真實驗運行環境:操作系統Windows 7(64位),處理器Inter(R) Core(TM) i5-4590U, CPU3.30 GHz,內存8 GB,仿真平臺Matlab R2016a。

4.1 最短最小距離和歐氏距離統計實驗

為了驗證本文提出的用最短最小距離作為啟發值的方法的科學性，進行最短最小距離和歐氏距離統計實驗。隨機生成100幅規模為30×30、障礙比例為0.2～0.4的柵格地圖并從中隨機選取起點和終點(每幅地圖可取多組起點和終點)，求取起點和終點間的最短最小路徑和歐氏距離，同時用本文的蟻群算法計算起點到終點的最近路徑，蟻群算法關鍵參數：迭代次數K=50，螞蟻數量m=50，信息素啟因子α=3；啟發函數因子β=6，信息素蒸發系數ρ=0.3。路徑距離見表1，其中R表示最短最小距離(歐氏距離)與用本文蟻群算法求得的最短距離的比值。

表1 路徑距離Tab.1 Path distance

由表1可以看出，即使拐點層數達到12，最短最小距離和最短距離的比值仍小于1.08，說明最短最小距離可以很大程度上體現起點到終點的距離；而歐氏距離與最短距離的比值隨著層數的增多，比值減小到0.354，說明二者差距逐漸加大。由此可以看出，用最短最小距離作為搜索節點的啟發值更準確可靠。

4.2 有效拐點數量和單步搜索空間統計實驗

螞蟻在選擇下一步節點時,長遠的視野范圍可以幫助螞蟻更好地規劃路徑，但是也造成了單步搜索空間巨大而不利于快速收斂。為解決這個問題，本文基于最短路徑和障礙柵格的關系，提出拐點柵格，并通過增枝操作進一步刪減掉無效拐點；同時，基于增枝貫通樹中柵格點的層級關系，進一步減小了螞蟻單步移動需要搜索的柵格數量。

為了便于直觀地理解增枝操作在減少有效拐點數量和減小單步搜索空間方面的作用，仿真算例如圖6所示。圖6a為原始柵格地圖，白色表示可通行，黑色表示障礙物，左上角為起點，右下角為終點。圖6b為拐點地圖，紅色柵格為拐點。圖6c為經過增枝操作生成的分層地圖，圖中紫色柵格為L0層的點，也就是終點，綠色柵格為L1層的點，也就是所有與L0層的點可直達的點，同理深藍色為L3層的點，淺藍色為L4層的點，起點位于L4層中，而圖6c中的紅色點即為淘汰的無效拐點。可以看出，通過增枝操作減小了有效拐點的數量。由于在最短路徑中，下一步到達的點必須是當前點所在層的點或距離終點更近的層中的點，而不可能是遠離終點的一層中的點，由此可以明顯減少單步搜索空間，以起點為例，當前單步搜索只需要搜索L4層的點(淺藍色)和L3層的點(深藍色)，而不需要搜索其他的有效拐點。同時通過節點間是否可直達的限制，進一步刪減一部分待搜索的拐點，可以明顯減少單步搜索的壓力。

為了進一步驗證本文方法在減少有效拐點數量和減小單步搜索空間方面的作用，設定地圖規模為15×15、30×30、60×60，障礙柵格比例為0.2、0.3、0.4、0.5，共計12種地圖下，重復實驗20次，統計有效拐點數量和單步搜索空間的平均值。平均有效拐點數量見圖7，平均單步搜索空間見圖8。

由圖7、8可以看出，雖然本文采取了無限鄰域的搜索方式，但是由于有效拐點概念的提出刪減了大量無效柵格點，使得每幅地圖的平均有效拐點數和平均單步搜索空間都在可接受的范圍內。對比常規的有限鄰域有限方向搜索方式，可通行柵格數和平均單步搜索空間見表2，本文搜索方式的有效拐點數量遠遠小于常規搜索方式的可通行柵格數，平均單步搜索空間略高于常規搜索方式的平均單步搜索空間。

表2 常規搜索方式可通行柵格數和平均單步搜索空間Tab.2 Number of passable grids and average single step search domain of conventional search methods

4.3 對比實驗

文獻[8]采用16方向24鄰域的螞蟻搜索方式,并結合向量夾角的思想設計2種啟發信息的計算方法，并在轉移選擇時采用δ-貪婪策略。首先采用文獻中的地圖進行對比實驗。對比方法和本文方法搜索到最優路徑如圖9所示。

文獻[8]方法和本文方法搜索到最優路徑的長度和步數比較見表3。經人工分析，圖9a、9b、9d、9e中的路徑都是相應搜索規則下的最短路徑，可以看出，本文方法得到的最短路徑，在路徑長度和步數上均優于文獻[8]方法的最優路徑。

為了進一步驗證本文算法的優越性，設定地圖規模為15×15、30×30、60×60，障礙柵格比例0.2、0.4，隨機生成6幅地圖。為了便于讀者復現和驗證，采用rand(‘state’,0)鎖定隨機地圖。如生成規模為15×15、障礙比例為0.2的地圖，代碼為

表3 最優路徑比較Tab.3 Comparison of optimal path

rand(‘state’,0);

G=rand(15);

Map=double(G<0.2)；

分別用本文算法和文獻[8]方法求解每幅地圖的最短路徑，每幅地圖重復實驗10次。蟻群算法關鍵參數：迭代次數K=50，螞蟻數量m=50，信息素啟因子α=3；啟發函數因子β=6，信息素蒸發系數ρ=0.3，文獻[8]算法采用method 1作為啟發信息計算方式，貪婪選擇概率δ=0.8。本文算法和文獻[8]算法最優路徑對比見表4。

可以看出，本文方法在路徑長度、步數、迭代次數方面均優于文獻[8]方法。由于本文提出的搜索方式不受距離限制，隨著地圖規模增大，所需步數的增加速度較慢，而對比算法的步數隨地圖規模的增大快速增多,因此隨著地圖規模增大，本文算法相對于對比算法，步數這一指標的提升愈加明顯，更短的步數意味著更少的出錯概率和更高的可靠性，利于快速收斂。文獻[8]算法在貪婪選擇中增加了參數δ，有一定概率隨機選擇柵格，有利于跳出局部最優解但減緩了收斂速度。因此本文算法比文獻[8]算法需要更少的迭代次數。本文方法求得的路徑距離更短的原因有：①由于搜索方式的改進，對于遠距離的可直達柵格點，機器人可以直接取捷徑通過，避免不必要的彎折。②本文方法步數少而帶來的低錯誤率和高收斂精度優勢。

表4 本文方法和文獻[8]方法最優路徑對比Tab.4 Comparison of optimal path of proposed method and Literature[8] method

5 結論

(1)針對柵格圖環境下的路徑規劃問題，提出了一種基于有效拐點和最短最小路徑的蟻群路徑規劃方法。在柵格地圖中采取無限鄰域的搜索方式，使機器人可取捷徑直達任何可直通的柵格點，同時為了減小搜索量，提出有效拐點的概念，大大減小了搜索空間。提出最短最小路徑的概念，并用其取代歐氏距離作為啟發值，提高了啟發值的準確度和可靠性，同時用起點到終點的最短最小距離，指導信息素更新，提高了蟻群算法迭代的質量。

(2)不同規模和復雜度環境的仿真實驗表明,本文方法與常規柵格搜索方式下的蟻群路徑規劃方法相比有較大的提升，有效減少了路徑長度和步數,加快了算法的迭代收斂速度。