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

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

0 引言

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

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

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

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

1 基于拐點的柵格圖

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

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

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

2 最短最小路徑

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

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

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

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

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

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

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

剪枝操作得到貫通樹T2。

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

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

3 蟻群算法求解最短路徑

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

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

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

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

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

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

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

3.2 轉(zhuǎn)移概率

螞蟻從當前柵格i到下一處柵格j的轉(zhuǎn)移概率為

(1)

(2)

式中α——信息素啟發(fā)因子

β——啟發(fā)函數(shù)因子

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

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

ηij——螞蟻從點i到點j的啟發(fā)信息

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

ηis——螞蟻從點i到點s的啟發(fā)信息

dij——點i到點j的距離

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

3.3 距離期望

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

3.4 信息素更新

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

(3)

式中m——蟻群數(shù)量

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

(4)

式中Q——常數(shù)

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

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

4 仿真實驗

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

4.1 最短最小距離和歐氏距離統(tǒng)計實驗

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

表1 路徑距離Tab.1 Path distance

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

4.2 有效拐點數(shù)量和單步搜索空間統(tǒng)計實驗

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

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

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

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

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

4.3 對比實驗

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

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

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

表3 最優(yōu)路徑比較Tab.3 Comparison of optimal path

rand(‘state’,0);

G=rand(15);

Map=double(G<0.2)；

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

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

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

5 結(jié)論

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

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