馮勇超，萬廣喜，曾 鵬

(1.中國科學院 網(wǎng)絡化控制系統(tǒng)重點實驗室，遼寧 沈陽 110016；2.中國科學院沈陽自動化研究所 工業(yè)控制網(wǎng)絡與系統(tǒng)研究室，遼寧 沈陽 110016；3.中國科學院 機器人與智能制造創(chuàng)新研究院， 遼寧 沈陽 110169；4.中國科學院大學 計算機科學與技術(shù)學院，北京 100049)

0 引 言

路徑規(guī)劃是控制移動機器人的關(guān)鍵技術(shù)之一，深度優(yōu)先搜索算法、廣度優(yōu)先搜索算法[1，2]、Dijkstra算法[3]、遺傳算法[4]、粒子群算法[5]、神經(jīng)網(wǎng)絡法[6]、A*算法[7]等更適合用于二維環(huán)境中的路徑規(guī)劃；毛嘉琪[8]、陳志等[9]所提出的蟻群算法均在二維環(huán)境中有較好應用。但在實際加工制造環(huán)境中，研究蟻群算法三維路徑規(guī)劃問題更具有生產(chǎn)意義。常采用快速搜索隨機樹算法、人工勢場法[10]、蟻群算法[11]等算法研究三維路徑規(guī)劃問題，一般的三維路徑規(guī)劃算法具有計算過程復雜、信息存儲量大、難以直接進行全局規(guī)劃等問題。本文采用將離散點作為信息素載體的方式可較好解決此問題。蟻群算法具有分布式計算的特點，節(jié)約計算時間，提升整體計算效率，在路徑規(guī)劃上有很大發(fā)揮空間，目前有諸多學者將蟻群算法應用到不同機器人的三維路徑規(guī)劃之中。文獻[12]結(jié)合人工勢場法強化目標路徑，修改了蟻群算法的啟發(fā)值參數(shù)，并提出了吸引素概念，有效提高算法收斂速度。文獻[13]將機械臂末端限制在矩形區(qū)域內(nèi)，以此來簡化搜索空間，提高蟻群算法搜索效率；但這樣做限制了算法搜索出更優(yōu)路徑，增強了算法的局限性。文獻[14]在傳統(tǒng)的復雜山地環(huán)境下機器人路徑規(guī)劃中，針對機器人無法攀爬等問題，在啟發(fā)函數(shù)中引入了坡度相關(guān)函數(shù)，使算法所規(guī)劃出路徑的坡度更好適配機器人爬坡能力；并根據(jù)等高線原理改進算法，極大提高算法搜索速度。文獻[15]根據(jù)三維水下環(huán)境的海水流動因素，引入了一階導數(shù)和二階導數(shù)對蟻群算法的信息素更新規(guī)則進行改進，并分段修改螞蟻啟發(fā)函數(shù)，避免收斂速度慢和易于陷入局部最優(yōu)等問題，以上所介紹算法均有各自的優(yōu)點，但同時也具備局限性。傳統(tǒng)蟻群算法應用于復雜三維空間中仍具有搜索效率低的特點，故提出改進蟻群算法；本文針對三維空間避障路徑規(guī)劃問題的研究主要集中在以下方面：①應用蟻群算法路徑規(guī)劃，改變其規(guī)劃中選擇路徑的方式，建立與貪婪算法相結(jié)合的概率策略，能有效解決三維路徑規(guī)劃速度慢的問題；②調(diào)整適應度函數(shù)約束條件，改進信息素計算公式，使其隨迭代次數(shù)動態(tài)更新；減緩信息素前期的積累速度，可避免算法提前陷入早熟。仿真實驗結(jié)果表明，改進蟻群算法可較快規(guī)劃出合理的路徑，可有效解決復雜山地環(huán)境下機器人路徑規(guī)劃問題。

1 地圖建模

這部分通過數(shù)學模型的方式將本文所研究物體的運動空間進行合理表達。本文空間地圖采用柵格法建立，柵格的大小決定構(gòu)建地圖環(huán)境的精細程度，柵格越小，地圖所存儲的信息越多，但會降低規(guī)劃精度；柵格越大，地圖存儲的信息越少，路徑規(guī)劃速度會相應增快，但不利于規(guī)劃出有效的路徑規(guī)劃。合理選擇柵格大小有利于規(guī)劃出最優(yōu)路徑。本文柵格大小采用1 km*1 km*0.2 km。

1.1 三維空間建模

三維空間模型的具體建模方法如下：將三維空間中左下方頂點與三維地圖的坐標原點O重合，并在點O處建立三維坐標系，沿坐標軸方向構(gòu)建三維空間立方體區(qū)域ABCO-EFGH，該立方體區(qū)域是物體的運動空間，即為本次規(guī)劃的空間。采用等分法對空間進行劃分，首先沿著OC使用A1O1H1E1，A2O2H2E2，……，An-1On-1Hn-1En-1等n-1個平面將空間劃分為n個等分區(qū)域，同理沿著OA將空間等分為n個等分區(qū)域，最后沿著OZ將空間等分為m個等分區(qū)域。最終整個空間被劃分成具有n×n×m個柵格。螞蟻可移動的區(qū)域為每個柵格上的8個頂點，三維路徑規(guī)劃空間如圖1所示。

圖1 三維空間規(guī)劃網(wǎng)格

1.2 初始地圖建模

本文隨機生成的三維地圖如圖2所示，圖中下方點為路徑搜索的起點，上方點為路徑搜索終點。產(chǎn)生的地圖可作為復雜山地環(huán)境的建模應用，形狀起伏的表面模擬作為障礙物的山峰，x軸方向作為經(jīng)度方向，y軸方向作為維度方向，Z軸方向作為海拔高度方向。

如圖3所示，設置螞蟻每次尋找路徑沿x軸向終點方向移動一個單位長度，沿著y和z的正負軸方向最大可移動兩個單位長度，螞蟻在p點選擇下一節(jié)點時共有25種可選擇的移動方向。

圖3 下一節(jié)點選擇

定義1 可行節(jié)點定義：對平面AnOnHnEn上任一點pn(in，jn，kn)， 若線段pn-1pn不與地圖內(nèi)任何障礙物相交，則將pn存入集合allowed(i，j，k) 中，計算當前點pn可通過的所有可行節(jié)點并將其存入到集合allowed中作為待選點。

2 基本蟻群算法

螞蟻尋找食物過程中，會釋放一種名為信息素的生物信號，蟻群行進過程中，能夠識別信息素強弱程度，并根據(jù)識別出的結(jié)果來指引下一步的移動方向。相同時間內(nèi)，距離短的路徑上信息素濃度會更高，螞蟻會逐漸向這條距離短的路徑聚集，同時信息素強度會隨時間進行揮發(fā)。

蟻群算法的數(shù)學模型可描述如下：規(guī)劃空間中的所有節(jié)點由集合C={C1，C2…Cn} 表示，連接空間中n個節(jié)點間的直線路徑由集合P={Pij|i，j=1，2，…，n；pi，pj∈C} 表示，路徑的長度用集合Dij{i，j=1，2，…，n} 來表示。在t時刻每條路徑上的信息素用集合T={τij(t)|i，j=1，2…n；i，j∈C} 來表示，每條路徑在初始時刻都具有一定量的信息素濃度，且信息素濃度大小相同，本文規(guī)定初始信息素濃度τij(0)=1。 通過轉(zhuǎn)移概率、局部信息素更新和全局信息素更新規(guī)則構(gòu)成蟻群算法主體框架，并實現(xiàn)算法的尋優(yōu)過程。

在算法的搜索過程中，所有螞蟻組成的集合用K={1，2，……，m} 表示，螞蟻經(jīng)過的節(jié)點用集合ban={1，2……，n} 表示。螞蟻s(s∈K) 向下一點移動時，是根據(jù)所有可選點的概率進行選擇的，從當前點i選擇下一點j的概率可以表示為

(1)

式中：j∈{allowed}，k∈{K}，α為信息素啟發(fā)因子，反應信息素對選擇概率的作用程度；β為期望值啟發(fā)因子，決定環(huán)境影響選擇概率程度的大?。沪莍j為啟發(fā)函數(shù)，計算公式如式(2)所示

ηij(t)=1/dij

(2)

完成一次迭代過程后，此時所有螞蟻均完成從初始點到終點的路徑搜索，全局信息素在此時進行更新，更新規(guī)則如下所示

τij(t+Δt)=ρ1τij(t)+Δτij(t+Δt)

(3)

(4)

(5)

3 蟻群算法設計與改進

3.1 改進算法轉(zhuǎn)移概率規(guī)則

傳統(tǒng)蟻群算法采用輪盤賭方式選擇下一步路徑，這樣算法選擇的隨機性強但效率低下；貪婪算法是指在問題求解時中，選取局部最優(yōu)，不從整體上考慮最優(yōu)。改進貪婪蟻群算法的機制是每一次路徑，下一個節(jié)點選擇前按照傳統(tǒng)蟻群算法概率選擇式(1)，計算出所有可選點概率并取出概率最大點的值Pmax，后取隨機值rand∈(0，1)， 若rand≤Pmax， 下一點概率公式選擇式(6)，否則采取普通蟻群算法概率式(1)。按照此種方法算法能夠快速收斂，極大地提高算法的求解效率，且不失算法搜索隨機性

(6)

式中：τis為所有待選點的信息素濃度，ηis為啟發(fā)函數(shù)，α，β分別為適應度啟發(fā)因子和期望值啟發(fā)因子，{alloweds}=C-{ban}。

3.2 信息素更新規(guī)則改進與表示方法

信息素載體通常選擇相鄰兩節(jié)點間的路徑，但三維空間較為復雜，設每一個平面AnOnHnEn中有n2個節(jié)點，則相鄰兩個平面AnOnHnEn與An+1On+1Hn+1En+1的所有節(jié)點的連接路徑將會有n4種情況，且隨著劃分間隔距離的減小，n會逐漸增大，連線的情況也會隨之變多，故采用路徑作為載體，算法計算量過大，空間復雜度過高，計算速度十分緩慢，故信息素不適用于該方式進行表達。

本文采用離散點作為信息素的載體，這樣相鄰兩平面只有n2+n2個信息素載體量，若將整個地圖環(huán)境劃分為n×n×m大小的柵格，則只需n2×m個信息素載體，極大降低計算復雜度。

路徑對蟻群的吸引程度由信息素大小決定，螞蟻每前進一段距離，路徑上的信息素就需要進行一次局部更新。正是這樣的實時更新策略，使螞蟻可以快速的向信息素濃度高的路勁收斂，但若當前路徑非最優(yōu)路徑，則會導致算法陷入局部最優(yōu)。為了避免這種情況，本文新增信息素波動系數(shù)值，即隨迭代次數(shù)動態(tài)調(diào)節(jié)局部信息素值。局部信息素更新公式如下所示

τij(t+Δt)=ζτij(t)

(7)

(8)

其中，ζ∈[0，1] 為局部信息素波動系數(shù)值，Δt為算法運行時間，μ和σ為控制參數(shù)。

圖4顯示了局部信息素波動系數(shù)隨迭代次數(shù)變化的曲線，最大揮發(fā)系數(shù)設置為max=0.81。搜索初期波動系數(shù)小是為了讓螞蟻在初期獲得更多方向的搜索路徑，有利于找出全局最優(yōu)路徑，避免算法陷入早熟；后期波動系數(shù)隨迭代次數(shù)加大，可在搜尋出全局最優(yōu)路徑后，加快算法的收斂速度。

圖4 波動系數(shù)變化曲線圖

在進行一次完整的迭代之后，所有螞蟻均完成從初始點到終點的路徑搜索，全局信息素在此時進行更新，全局信息素的更新規(guī)則如式(9)所示

τij(N+1)=(1-ρ)τij(N)+ρΔτij(N)

(9)

(10)

3.3 改進啟發(fā)值判定規(guī)則

采用啟發(fā)值作為蟻群算法在三維路徑規(guī)劃中，下一節(jié)點能否被選擇的判定條件，傳統(tǒng)蟻群算法的啟發(fā)值與螞蟻選擇下一局部路徑的長度成反比，進而驅(qū)動螞蟻選擇短距離路徑，但所選路徑不一定是最優(yōu)路徑。文獻[16]采用障礙物約束條件、當前節(jié)點距下一可行節(jié)點距離和可行節(jié)點距目標節(jié)點的距離等3個因素作為啟發(fā)值評價函數(shù)，障礙物約束條件如式(11)所示

(11)

歐式距離作為最短路徑，啟發(fā)函數(shù)為式(12)所示

(12)

當前節(jié)點的下一可行節(jié)點距離目標點距離的啟發(fā)函數(shù)如式(13)所示

(13)

但下一節(jié)點距離目標點距離短并不能保證算法所選路徑一定是最短的，鑒于此，本文把局部路徑長度作為重要指標，采用當前節(jié)點距離下一節(jié)點的歐氏距離作為衡量啟發(fā)值函數(shù)的指標，啟發(fā)函數(shù)如式(14)所示

ηij(t)=f1·f2

(14)

3.4 適應度值計算公式

蟻群算法采用適應度值的大小作為算法性能評估的指標，適應度值是由當前點與被選擇點兩點間距離與被選擇路徑點的高度值組成的[17]，其中 (Yi-1，Zi-1)，(Yi，Zi)， 是路徑上選擇點與被選點的Y，Z坐標值，Y∈[1，J]，Z∈[1，J]。 適應度值公式如式(15)所示

(15)

4 算法流程

本文算法執(zhí)行的流程如圖5所示：

圖5 改進蟻群算法流程

步驟1 初始化參數(shù)：初始信息素濃度τ(0)=1， 全局信息素強度Q2=100，螞蟻數(shù)量Nmax=10，迭代次數(shù)N=200，其中初始位置與目標位置由實驗網(wǎng)格環(huán)境決定，適應度啟發(fā)因子α，期望值啟發(fā)因子β，全局信息素的衰減系數(shù)ρ，作為變量根據(jù)實驗現(xiàn)象分別進行設置。

步驟2 將Nmax只螞蟻隨機放置在起點處。

步驟3 根據(jù)改進蟻群算法的轉(zhuǎn)移概率公式，選擇下一節(jié)點并更新局部信息素，搜索到終點后，一只螞蟻的完整路徑搜索結(jié)束。

步驟4 判斷循環(huán)次數(shù)是否小于等于螞蟻數(shù)量Nmax，若滿足條件重復步驟3，否則停止循環(huán)，一次迭代完成，選出Nmax條路徑中的最佳適應度值，對該路徑進行全局信息素更新。

步驟5 判斷迭代次數(shù)是否小于等于N，若小于跳回到步驟2，否則循環(huán)結(jié)束，找出N次循環(huán)中最優(yōu)的適應度值作為程序最終運行結(jié)果。

5 仿真結(jié)果

為驗證貪婪改進蟻群算法的有效性，選擇軟件MATLAB2020a作為仿真環(huán)境，CPU型號為Intel i5-10400，內(nèi)存16 GB的計算機。蟻群算法中，算法的參數(shù)極大影響著算法的性能，其中主要起作用的參數(shù)分別為：信息素啟發(fā)因子α，期望值啟發(fā)因子β，全局信息素揮發(fā)系數(shù)ρ。為了得出最佳參數(shù)配置本文采用圖2(a)初始地圖1的環(huán)境進行實驗，采用修改一個參數(shù)值，其它參數(shù)不變的原則。初始默認參數(shù)為：螞蟻數(shù)量Nmax=10，迭代次數(shù)N=200，信息素啟發(fā)因子α=1，期望值啟發(fā)因子β=1，全局信息素揮發(fā)系數(shù)ρ=0.8，實驗結(jié)果如圖6所示。

圖6 α，β，ρ不同取值對算法性能的影響

結(jié)果表明在第一次迭代后改進蟻群算法的適應度值遠優(yōu)于文獻[12]算法，改進算法最終迭代的適應度值平均值也是較低的，且算法性能隨著α，β，ρ等參數(shù)的波動變化較小。以上實驗結(jié)果可以得出，3個關(guān)鍵參數(shù)的最優(yōu)配置為：α=1.5，β=2.5，ρ=0.7。

為驗證本文算法的有效性，將其與文獻[12]的算法在不同環(huán)境下進行對比，首先設置所有參數(shù)環(huán)境均相同的第一組對比實驗。首先選擇圖2(a)初始地圖1，規(guī)劃路徑起點坐標為(1，15，1000)，終點坐標為(21，4，1600)，起點和終點相同的情況下，對比結(jié)果如圖7所示，其中圓形線為改進蟻群算法，星形線為文獻[12]算法。算法的收斂曲線如圖7(c)所示，虛線代表文獻[12]算法最佳個體適應度收斂曲線，實線代表改進蟻群算法最佳個體適應度收斂曲線。

圖7 兩種算法路徑規(guī)劃結(jié)果對比

第一組實驗對比見表1，結(jié)果表明改進蟻群算法最終適應度值收斂到110.4840，文獻[12]算法最終適應度值收斂到118.0732，適應度值縮小7.5892；改進蟻群算法比文獻[12]算法規(guī)劃出的路徑更加簡潔，路徑轉(zhuǎn)彎次數(shù)較少，整體較為平滑。由適應度迭代曲線可以看出，在算法的初期改進算法的迭代速度要優(yōu)于文獻[12]的算法，這是因為改進算法加大了所有待選點中概率最大點被選擇的概率，即信息素與啟發(fā)值大的這些點更易被選擇，同時減少了算法向較差路徑方向搜索的概率，可以更快搜尋出最優(yōu)路徑。而在經(jīng)歷多次迭代，適應度值不變后，在150次迭代處，本文改進算法仍在進行優(yōu)化，即說明改進算法具備跳出局部最優(yōu)解的能力，實驗結(jié)果表明改進算法擁有極快收斂速度的同時不失搜索隨機性。

表1 第一組實驗對比

我們在同一地圖不同起點與終點設置第二組對比實驗。規(guī)劃路徑起點坐標為(1，17，800)，終點坐標為(21，18，1200)時，對比結(jié)果如圖8所示。

圖8 不同起止點兩種算法路徑規(guī)劃結(jié)果對比

第二組實驗對比見表2，結(jié)果表明改進蟻群算法最終適應度值收斂到118.0698，文獻[12]算法最終適應度值收斂到124.9016，適應度值縮小6.8318；由俯視圖可以觀察到，本文改進算法規(guī)劃出路徑更加簡潔。改進算法前期，由于波動系數(shù)較低，故算法會向多個方向搜索，有助于快速的找尋到最優(yōu)路徑，此時最優(yōu)適應度值處于急速降低狀態(tài)；隨著迭代次數(shù)增加，波動系數(shù)值增大幅度減緩并趨于穩(wěn)定，此時算法迅速向最優(yōu)路徑遞進，最優(yōu)適應度值更新趨于平穩(wěn)。另外采用本文的啟發(fā)值判定法則，削弱了距終點近的，非最優(yōu)路徑上的點對算法尋優(yōu)的影響。第二組對比實驗驗證了本文算法在同一地圖不同起點的情況中，同樣擁有較好的性能，改進蟻群算法只需很短的時間與迭代次數(shù)即可達到文獻[12]算法迭代200次所產(chǎn)生的適應度值。但文獻[12]算法的迭代200次所消耗時間比本文算法少，本文算法犧牲少部分時間獲得更快地收斂速度與更優(yōu)的適應度值。

表2 第二組實驗對比

當采用圖2(b)的初始地形，起點與終點改變，起點坐標為(1，11，1400)，終點坐標為(21，5，1000)參數(shù)不變的第三組實驗，兩算法對比結(jié)果如圖9所示。

圖9 場景二兩種算法路徑規(guī)劃結(jié)果對比

第三組實驗對比見表3，結(jié)果表明改進蟻群算法最終適應度值收斂到133.9033，文獻[12]算法最終適應度值收斂到140.7637，適應度值縮小6.8604；路徑圖可以看出改進算法在y方向上是逐漸向終點靠擾，而文獻[12]算法在y方向上有越過終點的選擇，且只需較小的迭代次數(shù)(50次以下)，與運行時間，改進算法就可規(guī)劃出與文獻[12]算法迭代完成所產(chǎn)生的適應度值；實驗三驗證了改進算法在不同環(huán)境中仍然具有較好的性能。

為了驗證算法的適應性，設置與圖9中起點相同，算法參數(shù)相同，初始地形相同的10組對比實驗，結(jié)果見表4。

表3 第三組實驗對比

表4 10組對比實驗

相同地圖10組對比實驗見表4，第一列為實驗序號；第二列和第三列是迭代次數(shù)為200時，兩種算法規(guī)劃出的適應度值；第四列和第五列為兩種算法達到200次迭代時所運行的時間。實驗仿真結(jié)果表明，文獻[12]算法優(yōu)化能力較差，需要更多的時間才可以迭代出較好的適應度值；本文所提出的改進蟻群算法在三維路徑規(guī)劃中具有更快的收斂速度、較優(yōu)的適應度值和更好的穩(wěn)定性。

6 結(jié)束語

移動機器人執(zhí)行任務過程中，在復雜環(huán)境下進行合理的三維避障路徑規(guī)劃能夠有效的提升機器人的工作效率與安全性。本文針對蟻群算法在三維路徑規(guī)劃中的缺點，如過多的迭代次數(shù)，規(guī)劃出的適應度值較大、路徑過曲折等問題提出了改進貪婪蟻群算法。使用柵格法構(gòu)建初始地形，通過實驗確定算法參數(shù)最優(yōu)組合，通過模擬不同起點終點與地圖環(huán)境與傳統(tǒng)的蟻群算法進行比較。仿真結(jié)果表明改進算法具有更快的收斂速度與更優(yōu)的適應度值。合理解決了復雜山地環(huán)境下機器人路徑規(guī)劃問題。本文所研究障礙物局限于靜態(tài)環(huán)境，地圖參數(shù)已知的情況，這為下一步動態(tài)障礙物避障算法的研究打下基礎。