馬向華，張 謙

上海應(yīng)用技術(shù)大學(xué) 電氣與電子工程學(xué)院，上海201418

隨著科學(xué)技術(shù)的發(fā)展，機(jī)器人被廣泛應(yīng)用在倉儲(chǔ)物流、現(xiàn)代化農(nóng)業(yè)、智能制造工廠、智慧醫(yī)療等領(lǐng)域[1]。而路徑規(guī)劃是移動(dòng)機(jī)器人研究的一個(gè)重要分支。路徑規(guī)劃是指規(guī)定移動(dòng)機(jī)器人在具有障礙物的環(huán)境中從初始位置出發(fā)尋找一條無碰撞，安全到達(dá)目標(biāo)位置最優(yōu)的一條路徑[2]。這里的最優(yōu)可以是路徑最短、時(shí)間最短或者功耗最小。迄今為止，國內(nèi)外學(xué)者圍繞移動(dòng)機(jī)器人路徑規(guī)劃展開了大量研究，比如傳統(tǒng)的路徑規(guī)劃方法Dijstra[3]、A*算法[4]等。隨著自動(dòng)化任務(wù)要求的提高，問題規(guī)模復(fù)雜度不斷增加，傳統(tǒng)算法難以滿足要求，許多學(xué)者提出了一系列的仿生智能算法比如遺傳算法[5]、粒子群算法[6]、蟻群算法等[7]。

蟻群算法（ACO）是由意大利學(xué)者M(jìn)aro Dorigo最早提出基于螞蟻覓食行為的仿生模擬進(jìn)化算法。由于算法自組織、分布式、正反饋等特點(diǎn)被廣泛應(yīng)用于機(jī)器人路徑規(guī)劃中。但是在機(jī)器人路徑規(guī)劃時(shí)仍然存在收斂速度慢、易陷入局部最優(yōu)等問題，國內(nèi)外許多學(xué)者對(duì)傳統(tǒng)蟻群算法進(jìn)行了改進(jìn)。文獻(xiàn)[8]提出建立起始點(diǎn)與目標(biāo)點(diǎn)構(gòu)成有利矩陣，矩陣內(nèi)具有相同的初始信息素，大于其他區(qū)域初始信息素。形成有利矩陣避免螞蟻盲目搜索，減少搜索時(shí)間。但是建立部分有利信息素矩陣不僅擴(kuò)大了環(huán)境柵格規(guī)模，提高了搜索范圍，搜索路徑問題復(fù)雜了，而且矩陣內(nèi)信息素均勻分布，對(duì)算法收斂速度的提升非常有限。文獻(xiàn)[9]在柵格地圖中根據(jù)起始點(diǎn)和目標(biāo)點(diǎn)位置劃出優(yōu)選區(qū)域，建立新的數(shù)學(xué)模型對(duì)初始信息素濃度在該區(qū)域差異化設(shè)置，有效地加快算法前期搜索的速度，減少了最優(yōu)迭代次數(shù)，但是算法建立復(fù)雜的數(shù)學(xué)模型以及比值節(jié)點(diǎn)的選取大大增加了算法的運(yùn)算量，運(yùn)行時(shí)間甚至超過傳統(tǒng)蟻群算法，特別是復(fù)雜環(huán)境該算法的搜索效率不高。文獻(xiàn)[10]將當(dāng)前柵格到待選柵格的距離dij與待選柵格到目標(biāo)柵格的距離dje之和平方的倒數(shù)作為啟發(fā)函數(shù)。增強(qiáng)了螞蟻搜索的目的性，也一定程度提高了算法的搜索效率。但是并未考慮dij遠(yuǎn)遠(yuǎn)小于dje所導(dǎo)致忽略局部最短路徑的影響。文獻(xiàn)[11]提出信息素?fù)]發(fā)因子自適應(yīng)策略，提高算法全局搜索能力。但是缺乏局部搜索優(yōu)化能力，而且算法搜索效率的提升非常有限。文獻(xiàn)[12]結(jié)合人工蜂群算法提出尋優(yōu)螞蟻和偵察螞蟻，分別賦予不同的信息素更新權(quán)重。提高了算法收斂速度，避免算法陷入局部最優(yōu)，保證解的多樣性。但是尋優(yōu)螞蟻數(shù)量減少導(dǎo)致解的多樣性有限，無法對(duì)死鎖螞蟻進(jìn)行處理或者補(bǔ)充螞蟻數(shù)量導(dǎo)致算法在復(fù)雜環(huán)境中路徑規(guī)劃容易失敗。文獻(xiàn)[13]引入“虛擬節(jié)點(diǎn)”縮小算法搜索空間，減少了迭代次數(shù)，提高算法收斂速度。但是設(shè)置中間節(jié)點(diǎn)不僅減少算法解的多樣性，而且又添加了一些新的拐點(diǎn)。這使得機(jī)器人在復(fù)雜環(huán)境面臨安全和功耗問題。

根據(jù)上述分析中改進(jìn)蟻群算法在路徑規(guī)劃應(yīng)用中存在的不足，提出新的改進(jìn)蟻群算法：設(shè)置全局信息素矩陣，初始信息素濃度差異化分布，避免算法前期搜索盲目性；啟發(fā)函數(shù)融合A*算法，同時(shí)考慮當(dāng)前柵格到待選柵格距離dij與待選柵格到目標(biāo)柵格距離dje的數(shù)值差異，提高算法搜索方向性和收斂速度的同時(shí)考慮局部路徑的影響；提出回退策略保證螞蟻存活數(shù)量，提高算法解的多樣性以及算法魯棒性；添加拐點(diǎn)評(píng)價(jià)函數(shù)使得尋優(yōu)路徑更加光滑，減少路徑拐點(diǎn)，提高機(jī)器人行駛安全性以及降低能耗。

1 改進(jìn)蟻群算法的路徑規(guī)劃方法研究

1.1 蟻群算法原理

蟻群算法基本原理是螞蟻在覓食時(shí)在路上釋放信息素，隨著時(shí)間的增加，信息素?fù)]發(fā)的影響，最短路徑上的信息素濃度高于其他路徑[14]。蟻群算法的兩個(gè)關(guān)鍵過程為狀態(tài)轉(zhuǎn)移和信息素更新。

狀態(tài)轉(zhuǎn)移。螞蟻k(k=1,2,…,m)在t時(shí)刻從當(dāng)前節(jié)點(diǎn)i轉(zhuǎn)移到下一個(gè)節(jié)點(diǎn)j的轉(zhuǎn)移概率由路徑上信息素濃度和距離啟發(fā)函數(shù)確定。如公式（1）所示：

其中，τij表示路徑(i,j)信息素濃度，dij是當(dāng)前節(jié)點(diǎn)i到待選節(jié)點(diǎn)j的歐氏距離。ηij是啟發(fā)函數(shù)，α為信息素啟發(fā)因子，表示信息素濃度對(duì)轉(zhuǎn)移概率影響；β為期望啟發(fā)因子，表示路徑信息對(duì)轉(zhuǎn)移概率的影響。

信息素更新。所有的螞蟻完成一次迭代之后，路徑上信息素?fù)]發(fā)處理，在t+1時(shí)刻路徑(i,j)上信息素更新方式如下：

ρ表示信息素?fù)]發(fā)系數(shù)，1-ρ則表示路徑信息素殘留因子，其中△τk ij(t)為第k只螞蟻在此輪迭代過程中留下的信息素，公式如下：

Q為信息素強(qiáng)度常數(shù)，Lk表示螞蟻k在本輪循環(huán)中所走過的路徑總長度。

1.2 基于改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃

機(jī)器人工作環(huán)境為二維已知靜態(tài)空間，由于柵格法編碼簡單，易于實(shí)現(xiàn)，所以采用柵格法對(duì)環(huán)境建模[15]。為了提高蟻群算法搜索最優(yōu)路徑的效率和路徑規(guī)劃質(zhì)量，并且結(jié)合機(jī)器人路徑規(guī)劃安全性和低功耗等實(shí)際問題。本文對(duì)基本蟻群算法做以下改進(jìn)，具體包括：

（1）建立初始有利信息素矩陣

基本蟻群算法中，信息素的初始化濃度均勻分配，導(dǎo)致搜索前期螞蟻可能會(huì)選擇與目標(biāo)點(diǎn)方向相反的區(qū)域行走，使得前期搜索時(shí)間較長且尋得的路徑偏長。為了解決算法初期搜索的盲目性，以及算法收斂速度慢的問題。提出建立有利信息矩陣，使得初始信息素濃度差異化分布，避免盲目搜索，減少搜索范圍，縮短搜索時(shí)間。首先連接起始點(diǎn)與目標(biāo)點(diǎn)，預(yù)規(guī)劃一條路徑Lse如圖1所示，受到柵格環(huán)境中障礙物的影響，最優(yōu)路徑在預(yù)規(guī)劃路徑Lse附近波動(dòng)，初始信息素濃度以預(yù)規(guī)劃路徑為中心向兩邊呈高斯分布。初始信息素濃度分布示意如圖2所示。

圖1 預(yù)規(guī)劃路徑

圖2 初始信息素濃度分布示意圖

初始信息素濃度矩陣為A100×100，根據(jù)toepliz矩陣規(guī)律，初始信息素濃度以對(duì)角線沿X軸、Y軸正態(tài)分布，公式如下：

根據(jù)公式（7）生成正態(tài)分布向量，再利用toepliz矩陣生成A100×100，ψ為平衡系數(shù)，X取值范圍為[μ:μ+100]，σ為方差。依據(jù)全局地圖起始點(diǎn)與目標(biāo)點(diǎn)位置信息可知，全局最優(yōu)路徑往往是一條從起始點(diǎn)到目標(biāo)點(diǎn)方向的路徑。因此初始信息素不均勻分布有利于減少螞蟻盲目搜索路徑，提高收斂速度，減少迭代次數(shù)。

（2）啟發(fā)函數(shù)融合A*算法

A*算法是一種啟發(fā)式算法，將初始點(diǎn)與當(dāng)前節(jié)點(diǎn)的真實(shí)代價(jià)以及當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的預(yù)估代價(jià)作為估價(jià)函數(shù)。A*算法估價(jià)函數(shù)f(n)如下：

其中，g(n)表示初始點(diǎn)到當(dāng)前節(jié)點(diǎn)的真實(shí)代價(jià)，h(n)表示當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的預(yù)估代價(jià)。h(n)指導(dǎo)算法搜索朝著目標(biāo)點(diǎn)最短路徑方向搜索。蟻群算法中，啟發(fā)函數(shù)η=1/dij只考慮節(jié)點(diǎn)i與待選節(jié)點(diǎn)j之間的真實(shí)代價(jià)，只考慮了局部最短路徑，不利于全局尋優(yōu)。在傳統(tǒng)蟻群算法中融合A*算法，引入預(yù)估代價(jià)dje，通過dij真實(shí)代價(jià)與預(yù)估代價(jià)dje之和的倒數(shù)作為啟發(fā)函數(shù)，新的啟發(fā)函數(shù)如下：

其中，ζ為路徑平衡因子，取值范圍（0，1），通過引入目標(biāo)節(jié)點(diǎn)，加強(qiáng)了算法搜索的效率，有利于全局路徑的尋優(yōu)，避免陷入局部最優(yōu)路徑。

（3）修改信息素更新規(guī)則

信息素更新規(guī)則修改，引入拐點(diǎn)評(píng)價(jià)函數(shù)，為使所尋路徑更加平滑，節(jié)省機(jī)器人行走時(shí)間和功耗，將拐點(diǎn)評(píng)價(jià)函數(shù)作為信息素更新影響因素之一，機(jī)器人行走路徑有不同的拐角，拐角大小也代表不同的功耗和時(shí)間，如公式（10）所示：

其中，Lm為第m只螞蟻?zhàn)咄曛蟮穆窂介L度，θ為機(jī)器人路徑拐角的大小，機(jī)器人移動(dòng)過程中左右拐角大小一致都是0°、45°、90°、135°。f(θ)為拐角評(píng)價(jià)函數(shù)，ξ為拐角平衡系數(shù)。

（4）螞蟻死鎖處理

螞蟻搜索路徑過程中，轉(zhuǎn)移到非終點(diǎn)的某個(gè)柵格時(shí)，以當(dāng)前柵格節(jié)點(diǎn)為中心，預(yù)選柵格無法滿足轉(zhuǎn)移搜索條件，此時(shí)刻螞蟻無路可走，搜索被迫停止，螞蟻進(jìn)入死亡狀態(tài)，其搜索路徑也無效。在復(fù)雜環(huán)境中凹型障礙物必然會(huì)給算法帶來挑戰(zhàn)，所以在搜索過程中提出回退策略有效減少螞蟻死鎖現(xiàn)象導(dǎo)致算法搜索性能的下降。

針對(duì)含有凹型障礙物的復(fù)雜環(huán)境，一種常見方法就是人為處理將環(huán)境中凹型障礙物填充或者變成凸型障礙物。這樣膨脹化處理較為簡單，但是會(huì)犧牲環(huán)境的精度，在實(shí)際應(yīng)用中存在局限性，不利于算法的適用性和魯棒性，因此提出螞蟻回退策略。假設(shè)螞蟻k在t時(shí)刻進(jìn)入當(dāng)前節(jié)點(diǎn)，根據(jù)搜索條件預(yù)選節(jié)點(diǎn)集合如果為空集，判斷螞蟻k落入陷阱。此時(shí)螞蟻返回上一節(jié)點(diǎn)(i-1)并把當(dāng)前節(jié)點(diǎn)加入禁忌集合并清除當(dāng)前節(jié)點(diǎn)信息素，重新判斷(i-1)是否落入陷阱，如果是，重復(fù)上述操作，直到當(dāng)前節(jié)點(diǎn)根據(jù)搜索條件預(yù)選的節(jié)點(diǎn)集合為非空集，則螞蟻跳出陷阱?；赝瞬呗杂行У乇苊饬宋浵佀劳觯岣咚惴ㄊ諗克俣?，提升算法魯棒性。

2 改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃實(shí)現(xiàn)

改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃步驟如下：

步驟1已知靜態(tài)二維空間建立柵格地圖，設(shè)置柵格序號(hào)，選擇起始點(diǎn)和目標(biāo)點(diǎn)。

步驟2參數(shù)初始化，設(shè)置螞蟻數(shù)量m，最大迭代次數(shù)Nmax，信息素啟發(fā)因子α，期望啟發(fā)因子β，信息素?fù)]發(fā)因子ρ，初始信息素平衡因子ψ，路徑平衡因子ζ，拐角平衡系數(shù)ξ。

步驟3將螞蟻m(m=1,2,…,M)放在起始位置，并把初始點(diǎn)放入禁忌表Tabu中。

步驟4根據(jù)轉(zhuǎn)移概率公式（1）計(jì)算可行節(jié)點(diǎn)的概率，采用輪盤賭法尋找下一可達(dá)節(jié)點(diǎn)，并將其加入到禁忌表中，然后更新路徑長度。

步驟5判斷螞蟻是否已經(jīng)到達(dá)目標(biāo)點(diǎn)E，如果已經(jīng)到達(dá)則記錄螞蟻?zhàn)哌^的柵格序號(hào)和路徑長度。如果沒有到達(dá)目標(biāo)點(diǎn)，則重復(fù)步驟4直到找到目標(biāo)點(diǎn)E為止。若某個(gè)螞蟻k搜索陷入停滯，則根據(jù)螞蟻回退策略進(jìn)行處理。所有螞蟻搜索完畢，轉(zhuǎn)到步驟6。

步驟6判斷是否達(dá)到最大迭代次數(shù)Nmax。如果算法達(dá)到最大迭代次數(shù)，則輸出當(dāng)前蟻群搜索結(jié)果最短路徑長度，否則，清空禁忌表，令N=N+1，轉(zhuǎn)到步驟3，繼續(xù)搜索路徑，直到達(dá)到最大迭代次數(shù)Nmax，算法運(yùn)行結(jié)束，輸出當(dāng)前搜索最短路徑的長度。

3 仿真與分析

通過MATLAB軟件對(duì)改進(jìn)蟻群算法的路徑規(guī)劃方法進(jìn)行仿真驗(yàn)證。利用柵格法進(jìn)行環(huán)境建模，算法參數(shù)設(shè)置如表1所示，分別在兩種實(shí)驗(yàn)環(huán)境下對(duì)基本蟻群算法路徑規(guī)劃方法和文獻(xiàn)[11]信息素?fù)]發(fā)因子自適應(yīng)蟻群算法的路徑規(guī)劃方法和本文提出的改進(jìn)蟻群算法路徑規(guī)劃方法進(jìn)行對(duì)比。結(jié)果如圖3～6所示。

表1 參數(shù)設(shè)置

圖3 實(shí)驗(yàn)1基于三種蟻群算法最短路徑規(guī)劃圖

圖4 實(shí)驗(yàn)1三種蟻群算法路徑規(guī)劃迭代收斂曲線

圖5 實(shí)驗(yàn)2基于三種蟻群算法最短路徑規(guī)劃圖

圖6 實(shí)驗(yàn)2三種蟻群算法路徑規(guī)劃迭代收斂曲線

實(shí)驗(yàn)1中20×20柵格環(huán)境下仿真結(jié)果如圖3、4所示，分別是基本蟻群算法、文獻(xiàn)[11]信息素?fù)]發(fā)因子自適應(yīng)蟻群算法和本文提出的改進(jìn)蟻群算法的路徑規(guī)劃圖和迭代收斂曲線。如表2所示，本文改進(jìn)蟻群算法搜索效率（迭代次數(shù)減少率）較基本蟻群算法提高了51.7%，較文獻(xiàn)[11]算法提高了44%，路徑規(guī)劃結(jié)果本文最優(yōu)路徑長度較基本蟻群算法減少了9.7%，較文獻(xiàn)[11]中的算法減少了3.8%，拐點(diǎn)數(shù)目較基本蟻群算法和文獻(xiàn)[11]改進(jìn)蟻群算法分別減少了51.7%、47.1%。綜上所述，在實(shí)驗(yàn)1環(huán)境下本文提出的路徑規(guī)劃方法具有明顯的改進(jìn)效果和較優(yōu)的搜索性能。

表2 實(shí)驗(yàn)1三種蟻群算法仿真結(jié)果對(duì)比

選擇含有U型、H型障礙物的30×30復(fù)雜環(huán)境驗(yàn)證改進(jìn)算法的適用性。保證算法的可靠性，將螞蟻數(shù)目增加到100只，最大迭代次數(shù)增加到150?；鞠伻核惴ā⑽墨I(xiàn)[11]信息素?fù)]發(fā)因子自適應(yīng)蟻群算法和本文提出的改進(jìn)蟻群算法路徑規(guī)劃圖和迭代收斂曲線如圖5、6所示，基本蟻群算法收斂速度慢而且易陷入U(xiǎn)型障礙物，搜索最優(yōu)路徑較長，算法難以找到最優(yōu)解。圖6（b）所示文獻(xiàn)[11]改進(jìn)蟻群算法搜索前期不能盡快鎖定搜索方向，具有一定盲目性導(dǎo)致收斂速度較慢。如表3所示本文改進(jìn)算法的搜索最優(yōu)路徑長度較基本蟻群算法和文獻(xiàn)[11]算法分別減少了12.8%和8.7%，拐點(diǎn)數(shù)較后者均減少了52.2%。綜上所述，本文改進(jìn)蟻群算法在前期能快速鎖定搜索方向，搜索效率有較大提升，拐點(diǎn)數(shù)大幅減少算法搜索路徑光滑，減少機(jī)器人運(yùn)行的危險(xiǎn)和轉(zhuǎn)彎造成的能量損耗。

表3 實(shí)驗(yàn)2三種蟻群算法仿真結(jié)果對(duì)比

4 結(jié)語

本文基于蟻群算法的機(jī)器人路徑規(guī)劃進(jìn)行分析，根據(jù)蟻群算法路徑規(guī)劃過程中存在的問題進(jìn)行改進(jìn)：初期預(yù)規(guī)劃路徑，建立初始信息素濃度差異化分布矩陣，避免盲目搜索；然后引入A*算法，提高搜索過程的方向性和收斂速度，以及對(duì)搜索過程出現(xiàn)死鎖現(xiàn)象提出回退方法；最后結(jié)合拐點(diǎn)評(píng)價(jià)函數(shù)對(duì)搜索路徑進(jìn)行優(yōu)化。通過不同環(huán)境下對(duì)比實(shí)驗(yàn)發(fā)現(xiàn)，改進(jìn)后的蟻群算法進(jìn)行移動(dòng)機(jī)器人路徑規(guī)劃時(shí)具有良好的搜索效率，尤其在復(fù)雜環(huán)境下仍然具有較優(yōu)的性能，迭代次數(shù)更少，收斂速度更快，搜索路徑更光滑。