改進(jìn)雙向蟻群算法的移動機(jī)器人路徑規(guī)劃

李二超，齊款款

蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州730050

靜態(tài)環(huán)境下的移動機(jī)器人路徑規(guī)劃是指在已知環(huán)境中，按照一定的算法，根據(jù)目標(biāo)函數(shù)（如距離最短等），尋找一條從起點(diǎn)到終點(diǎn)的安全且最優(yōu)路徑[1]。

二維靜態(tài)環(huán)境下機(jī)器人路徑規(guī)劃算法有很多，如A*算法、遺傳算法、粒子群算法和蟻群算法，其中，A*算法速度快但轉(zhuǎn)折點(diǎn)較多，隨著環(huán)境復(fù)雜度的增加，搜索代價呈指數(shù)增長[2]，遺傳算法在種群初始化、算法迭代等環(huán)節(jié)代價函數(shù)建模困難，路徑搜索效率低，耗費(fèi)較大的計算和存儲資源，在復(fù)雜環(huán)境下的路徑規(guī)劃效率低下，需要較長時間才能規(guī)劃出可行路徑，且路徑并非最短[3]，粒子群算法簡單易行，通用性強(qiáng)，但在算法初期局部搜索能力較差，后期易陷入局部最優(yōu)等[4]，而選擇蟻群算法的原因是，蟻群算法是一種啟發(fā)式的隨機(jī)搜索算法，該算法由模擬自然界螞蟻行為而來，并通過信息素的積累產(chǎn)生的正向反饋來尋找最優(yōu)路徑，隨著環(huán)境復(fù)雜度的增加，路徑搜索代價不會指數(shù)增長，且具有較強(qiáng)的魯棒性，優(yōu)良的并行分布式計算能力、無中心控制、易于與其他算法融合的優(yōu)點(diǎn)[5-6]，但無法找到最短路徑，收斂速度慢，路徑搜索盲目性大、且路徑拐點(diǎn)較多。針對以上蟻群算法的缺陷，不同的學(xué)者有著各自的改進(jìn)方法。張?zhí)K英等人[7]采用雙向蟻群算法，但并未使用相遇條件，即起點(diǎn)螞蟻搜索路徑完成后，終點(diǎn)螞蟻才開始進(jìn)行路徑搜索，雖然能提高全局搜索能力，但并不能縮減算法運(yùn)行時間。文獻(xiàn)[8]采用初始信息素不平等分布的同時，在啟發(fā)函數(shù)中引入轉(zhuǎn)角因子，能夠較快得到拐點(diǎn)較少的最優(yōu)路徑。王紅君等人[9]使用冗余點(diǎn)的刪除策略，能夠減少路徑上的拐點(diǎn)數(shù)目。陳英俏[10]采用信息素非均勻分布的并行雙向蟻群算法，相遇條件為信息素相遇，即在同一柵格出現(xiàn)雙向不同的信息素視為相遇，同時在啟發(fā)函數(shù)中引入信息素相互引導(dǎo)策略，加強(qiáng)了信息素作用，減少了算法運(yùn)行時間。Luo等人[11]使用偽隨機(jī)概率轉(zhuǎn)移公式提高了算法的全局搜索能力和收斂速度。徐義晗等人[12]采用建立方向信息素矩陣的雙向蟻群算法，能夠提高路徑搜索速度和路徑的多樣性。王曉燕等人[13]利用人工勢場法求得的初始路徑信息對蟻群算法的啟發(fā)函數(shù)進(jìn)行改進(jìn)，增強(qiáng)路徑的引導(dǎo)作用，減小路徑搜索的盲目性。文獻(xiàn)[14]采用雙向蟻群算法路徑搜索策略，能夠加快算法運(yùn)行速度和提高全局搜索能力，但仍然是一小步的搜索，路徑轉(zhuǎn)折點(diǎn)依然較多。文獻(xiàn)[15]通過改進(jìn)信息素增強(qiáng)系數(shù)，信息素?fù)]發(fā)因子，建立信息素因子和啟發(fā)式因子的互鎖關(guān)系，減少了算法的迭代次數(shù)，提高了算法的收斂速度。本文算法與雙向蟻群算法（如文獻(xiàn)[14]）相比，優(yōu)勢在于減少路徑搜索的節(jié)點(diǎn)數(shù)（有效頂點(diǎn)數(shù)少于環(huán)境總柵格數(shù)），打破傳統(tǒng)總是一小步的路徑搜索方式以及轉(zhuǎn)移角度45?整數(shù)倍的限制，動態(tài)調(diào)節(jié)揮發(fā)系數(shù)，得到的路徑拐點(diǎn)較少，路徑更短。

傳統(tǒng)蟻群算法存在很多的缺陷，針對傳統(tǒng)蟻群算法無法找到最短路徑，路徑搜索盲目性大，收斂速度慢，拐點(diǎn)多問題，提出基于改進(jìn)雙向蟻群算法的機(jī)器人路徑規(guī)劃。首先，改進(jìn)路徑搜索方式，即基于障礙物有效頂點(diǎn)編碼的路徑搜索，不再使用傳統(tǒng)的從當(dāng)前柵格中心到下一柵格中心一步步的路徑搜索，而是從當(dāng)前障礙物有效頂點(diǎn)到下一可選障礙物有效頂點(diǎn)的大步的路徑搜索，結(jié)合雙向蟻群算法路徑搜索策略，能夠加快收斂速度且能夠找到拐點(diǎn)相對更少的最短路徑。其次，由于采用障礙物有效頂點(diǎn)編碼的路徑搜索，啟發(fā)式函數(shù)也隨之做了改進(jìn)，即當(dāng)前障礙物有效頂點(diǎn)到下一可選障礙物有效頂點(diǎn)的歐氏距離的倒數(shù)，并在公式分母中加入調(diào)節(jié)參數(shù)，有助于找到更短路徑。同時，引入偽隨機(jī)（隨機(jī)概率和任一性概率）的狀態(tài)轉(zhuǎn)移策略，能夠降低傳統(tǒng)算法路徑搜索的盲目性。最后，為防止信息素積累過多，自適應(yīng)調(diào)節(jié)信息素?fù)]發(fā)系數(shù)的同時，設(shè)置信息素濃度的取值范圍。

1 環(huán)境建模

機(jī)器人工作環(huán)境為靜態(tài)柵格地圖，如圖1所示。黑色柵格代表障礙物，用1表示，白色柵格代表自由柵格，用0表示。地圖按照從左到右，從下到上的順序依次編號1、2、…，柵格序號與坐標(biāo)一一對應(yīng)，坐標(biāo)與柵格編號的關(guān)系表達(dá)式如式（1）：

圖1 柵格地圖Fig.1 Grid map

其中，i代表柵格序號，Nx柵格地圖的行數(shù)，Ny柵格地圖的列數(shù)，mod是求余運(yùn)算，ceil是向上取整運(yùn)算。

將障礙物進(jìn)行膨化處理，如圖2所示。最里面黑色正方形為原始障礙物，當(dāng)不規(guī)則障礙物不滿一個柵格時，將其填充為一個柵格，白色部分為障礙物的膨化，寬度為機(jī)器人半徑，最外層黑色部分為安全距離，整體構(gòu)成障礙物，此時把機(jī)器人看作質(zhì)點(diǎn)來處理，假設(shè)膨化后的正方形邊長為1，即式（1）中的a=1。本文研究的目標(biāo)為路徑最短。

圖2 障礙物膨化處理Fig.2 Expansion treatment of obstacles

2 傳統(tǒng)蟻群算法

狀態(tài)轉(zhuǎn)移概率公式如式（2）所示：

式中，j∈allowedm為待選節(jié)點(diǎn)集合；τij(t)為信息素濃度；ηij(t)為啟發(fā)信息，用當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j的歐式距離的倒數(shù)表示；α和β分別表示信息素因子和啟發(fā)式因子。

信息素更新公式如式（3）~式（5）所示：

式中，τij(t+1)為更新后的信息素濃度；ρ為信息素?fù)]發(fā)系數(shù)；Δτij(t)為所有螞蟻信息素濃度之和；Q為信息素強(qiáng)度；Lm為螞蟻所走的路徑長度。

3 改進(jìn)蟻群算法

3.1 障礙物有效頂點(diǎn)

障礙物有效頂點(diǎn)定義及條件：

（1）柵格地圖最外圍邊界上的所有點(diǎn)都不可能成為障礙物的有效頂點(diǎn)。

（2）由當(dāng)前小柵格與相鄰的三個小柵格組成的“田”字形中，當(dāng)且僅當(dāng)只有一個障礙物時，田字格的中心點(diǎn)為障礙物的有效頂點(diǎn)。

必須同時滿足以上條件才能成為障礙物的有效頂點(diǎn)。例如在圖3中柵格13、14、19和20四個小柵格組成“田”字形較大柵格9個點(diǎn)中，點(diǎn)（0，2）、（0，3）和（0，4）為柵格地圖最外圍邊界上的點(diǎn)，不滿足條件（1），不是障礙物的有效頂點(diǎn)，分別以點(diǎn)（1，2）、（1，4）和（2，4）為中心的田字格中分別有3個障礙物、無障礙物和2個障礙物，不滿足條件（2），不是障礙物的有效頂點(diǎn)，點(diǎn)（1，3）、（2，2）和（2，3）滿足上述的兩個條件，故為障礙物的有效頂點(diǎn)。圖3中有14個障礙物有效頂點(diǎn)，用紅色的五角星表示，紅色三角形分別為起點(diǎn)（S）和終點(diǎn)（E），定義為廣義的障礙物有效頂點(diǎn)（起點(diǎn)和終點(diǎn)分別是有效頂點(diǎn)連接路徑的首端和末端），因此圖中共有16個有效頂點(diǎn)。

圖3 障礙物有效頂點(diǎn)示意圖Fig.3 Diagram of effective vertex of obstacles

基于障礙物有效頂點(diǎn)的路徑搜索需要對每個有效頂點(diǎn)進(jìn)行編碼，以便于路徑的識別和搜索，從下向上，從左向右依次進(jìn)行編號（起點(diǎn)和終點(diǎn)編號為1和2除外），例如圖3中的16個有效頂點(diǎn)，以先后順序從1編號到16，順序?yàn)镾→E→（1，3）→（2，1）→（2，2）→（2，3）→（2，5）→（3，1）→（3，2）→（3，3）→（3，5）→（4，3）→（4，4）→（5，2）→（5，3）→（5，4）。

3.2 改進(jìn)雙向搜索策略

雙向蟻群算法路徑相遇條件不同于文獻(xiàn)[14]信息素相遇條件，即有效頂點(diǎn)相遇條件，具體如圖4所示。

圖4 障礙物有效頂點(diǎn)路徑規(guī)劃仿真示意圖Fig.4 Simulation diagram of effective vertex path planning of obstacles

螞蟻平均分成兩組，一組放在起點(diǎn)位置，從起始點(diǎn)（S）向目標(biāo)點(diǎn)（E）進(jìn)行搜索（正向搜索），一組放在目標(biāo)點(diǎn)位置，從目標(biāo)點(diǎn)向起始點(diǎn)進(jìn)行搜索（反向搜索），采用輪流交替從兩個方向進(jìn)行搜索，即先從起點(diǎn)派出一只螞蟻向目標(biāo)點(diǎn)進(jìn)行一步路徑搜索（此時的一步并非傳統(tǒng)算法的一小步，有可能是多步），然后從目標(biāo)點(diǎn)派出一只螞蟻向起點(diǎn)進(jìn)行一步路徑搜索。

當(dāng)正向搜索一步后，判斷與反向搜索的路徑有無相同有效頂點(diǎn)，若有，則結(jié)束搜索，若沒有，則從目標(biāo)點(diǎn)派出一只螞蟻向起點(diǎn)進(jìn)行一步路徑搜索，判斷與正向搜索的路徑有無相同有效頂點(diǎn)，若有，則結(jié)束搜索，若沒有，則從起點(diǎn)派出一只螞蟻向終點(diǎn)進(jìn)行一步路徑搜索，再進(jìn)行判斷有無相同的有效頂點(diǎn)，如此反復(fù)進(jìn)行，直到相遇在相同的有效頂點(diǎn)然后從相同的有效頂點(diǎn)分別向起點(diǎn)和終點(diǎn)回溯，形成一條完整的路徑并記錄。

圖4為某兩只螞蟻路徑搜索仿真示意圖，起點(diǎn)和終點(diǎn)分別編號為1和2，分別加入到road0和road1路徑集合中。有一只正向搜索的螞蟻在起點(diǎn)，根據(jù)有效頂點(diǎn)的鄰接矩陣可知，下一步有2個可選的有效頂點(diǎn)4和8，再根據(jù)狀態(tài)轉(zhuǎn)移規(guī)則，選擇有效頂點(diǎn)4，并加入road0集合（此時集合中有1和4兩個元素），判斷與反向路徑無相同的有效頂點(diǎn)，此時反向搜索的螞蟻從終點(diǎn)出發(fā)，根據(jù)有效頂點(diǎn)的鄰接矩陣，下一步有8個可選的有效頂點(diǎn)5、7、10、11、13、14、15和16，再根據(jù)狀態(tài)轉(zhuǎn)移規(guī)則，選擇有效頂點(diǎn)13，并加入road1集合（此時集合中有2和13兩個元素），判斷與正向路徑無相同的有效頂點(diǎn)，此時執(zhí)行正向的路徑搜索，根據(jù)有效頂點(diǎn)的鄰接矩陣，下一步有8個可選的有效頂點(diǎn)5、6、7、8、9、10、12和13，再根據(jù)狀態(tài)轉(zhuǎn)移規(guī)則，選擇有效頂點(diǎn)9，并加入road0集合。判斷與反向路徑無相同的有效頂點(diǎn)，此時反向搜索的螞蟻從終點(diǎn)出發(fā)，根據(jù)有效頂點(diǎn)的鄰接矩陣，下一步有7個可選的有效頂點(diǎn)4、5、9、10、11、12和16，選擇有效頂點(diǎn)4，并加入road1集合，判斷與正向路徑有相同的有效頂點(diǎn)4，則結(jié)束搜索，此時road0：S→4→9，road1：E→13→4，最終路徑roadlast：S→4→13→E（具體編號規(guī)則見上文）。

3.3 改進(jìn)轉(zhuǎn)移概率公式

改進(jìn)的啟發(fā)函數(shù)如式（6）所示：

式中，dij表示當(dāng)前有效頂點(diǎn)與下一有效頂點(diǎn)之間的歐氏距離。a,b為正數(shù)。roadxij表示正向或反向搜索，其中x=0,表示正向搜索，x=1,表示反向搜索。

啟發(fā)函數(shù)的改進(jìn)，相應(yīng)的轉(zhuǎn)移概率也跟著改變，并引入任一性概率。具體改進(jìn)轉(zhuǎn)移概率公式如式（7）、（8）所示：

式中，q為[0，1]之間的隨機(jī)數(shù)，q0由反復(fù)實(shí)驗(yàn)來確定的常數(shù)，范圍為（0，1），randj為在可選有效頂點(diǎn)中任選其一。

3.4 改進(jìn)揮發(fā)系數(shù)

改進(jìn)的揮發(fā)系數(shù)能夠?qū)崿F(xiàn)動態(tài)調(diào)節(jié)，如式（9）所示：

式中，ρmin為揮發(fā)系數(shù)的最小值，T為最大迭代次數(shù)，t為當(dāng)前迭代次數(shù)。

3.5 信息素限制策略

為防止算法陷入局部收斂，對信息素濃度進(jìn)行限制，信息素大小限制范圍如式（10）所示：

式中，τmin為信息素最小值，τmax為信息素最大值。

4 改進(jìn)蟻群算法流程

改進(jìn)后的蟻群算法流程圖如圖5所示。

圖5 改進(jìn)蟻群算法流程圖Fig.5 Flow chart of improved ant colony algorithm

5 實(shí)驗(yàn)仿真與分析

為驗(yàn)證改進(jìn)算法的可行性、有效性和優(yōu)越性，在MATLAB 2016a上進(jìn)行仿真實(shí)驗(yàn)。從以下幾個方面進(jìn)行實(shí)驗(yàn)驗(yàn)證：在稍微復(fù)雜的柵格地圖環(huán)境中，在相同的參數(shù)條件下，將單獨(dú)改進(jìn)的單向有效頂點(diǎn)（方案1）、雙向有效頂點(diǎn)（方案2）、雙向有效頂點(diǎn)+改進(jìn)啟發(fā)函數(shù)（方案3）、雙向有效頂點(diǎn)+改進(jìn)狀態(tài)轉(zhuǎn)移規(guī)則（方案4）和雙向有效頂點(diǎn)+改進(jìn)揮發(fā)系數(shù)（包括限制信息素范圍）（方案5）依次和傳統(tǒng)算法進(jìn)行仿真對比，然后將整體改進(jìn)方法分別與傳統(tǒng)算法和文獻(xiàn)[16]進(jìn)行仿真對比分析，驗(yàn)證改進(jìn)方法的可行性以及改進(jìn)算法的優(yōu)越性。在大型復(fù)雜柵格地圖環(huán)境下，將改進(jìn)算法與傳統(tǒng)算法和文獻(xiàn)[16]算法（使用文獻(xiàn)參數(shù)）進(jìn)行對比分析，驗(yàn)證改進(jìn)算法的優(yōu)點(diǎn)。

仿真參數(shù)設(shè)置分別為：

螞蟻數(shù)目為80，最大迭代次數(shù)為100，α=1,β=2,Q=50,q0=0.4,a=1.5,b=2。以下結(jié)果都是算法運(yùn)行50次的平均值。

5.1 20×20復(fù)雜環(huán)境

方案1~5、傳統(tǒng)算法、文獻(xiàn)[16]算法和本文算法最優(yōu)路徑圖分別如圖6~13所示，以及仿真結(jié)果數(shù)據(jù)如表1所示。

表1 20×20復(fù)雜環(huán)境下的仿真結(jié)果Table 1 Simulation results under 20×20 complex environment

圖6 方案1的最優(yōu)路徑圖Fig.6 Optimal path diagram of scheme 1

圖7 方案2的最優(yōu)路徑圖Fig.7 Optimal path diagram of scheme 2

圖8 方案3的最優(yōu)路徑圖Fig.8 Optimal path diagram of scheme 3

圖9 方案4的最優(yōu)路徑圖Fig.9 Optimal path diagram of scheme 4

圖10 方案5的最優(yōu)路徑圖Fig.10 Optimal path diagram of scheme 5

圖11 傳統(tǒng)算法的最優(yōu)路徑圖Fig.11 Optimal path graph of traditional algorithm

在驗(yàn)證改進(jìn)算法整體改進(jìn)的優(yōu)越性前，設(shè)置梯度方案，即由于改進(jìn)算法的特殊性，不能直接將某一個改進(jìn)的點(diǎn)加入傳統(tǒng)算法進(jìn)行比較，而是在單向有效頂點(diǎn)路徑搜索基礎(chǔ)上設(shè)置梯度方案，驗(yàn)證改進(jìn)點(diǎn)的可行性。梯度方案為方案1~5，分別對應(yīng)單獨(dú)添加有效頂點(diǎn)，單獨(dú)使用有效頂點(diǎn)，雙向有效頂點(diǎn)+改進(jìn)啟發(fā)函數(shù)，雙向有效頂點(diǎn)+改進(jìn)轉(zhuǎn)移規(guī)則和雙向有效頂點(diǎn)+改進(jìn)揮發(fā)系數(shù)。

經(jīng)仿真數(shù)據(jù)可知，在最優(yōu)路徑、拐點(diǎn)數(shù)和收斂速度方面，單向有效頂點(diǎn)都優(yōu)于傳統(tǒng)算法，驗(yàn)證有效頂點(diǎn)的可行性和優(yōu)越性，在單向基礎(chǔ)上添加反向，即雙向有效頂點(diǎn)，在最優(yōu)路徑、拐點(diǎn)數(shù)和收斂速度方面進(jìn)一步改善，說明雙向搜索的優(yōu)越性，然后在雙向的基礎(chǔ)上分別加入改進(jìn)啟發(fā)函數(shù)、改進(jìn)轉(zhuǎn)移規(guī)則和改進(jìn)揮發(fā)系數(shù)，在拐點(diǎn)數(shù)和算法運(yùn)行時間上，再一次改善，在最短路徑或者最短路徑的穩(wěn)定性上也得到相應(yīng)的改善，驗(yàn)證改進(jìn)點(diǎn)可行性和有效性。

最后將整體改進(jìn)算法與傳統(tǒng)算法和文獻(xiàn)[16]算法進(jìn)行比較，三種算法都能找到各自的最短路徑，分別為27.932 0、35.071 1和29.799 0，由此可知，改進(jìn)算法找到的路徑最短，在拐點(diǎn)數(shù)和收斂速度方面占有較大優(yōu)勢，總之，改進(jìn)算法得到的最短路徑、拐點(diǎn)數(shù)和收斂速度都優(yōu)于傳統(tǒng)算法和文獻(xiàn)[16]算法。

圖12 文獻(xiàn)[16]的最優(yōu)路徑圖Fig.12 Optimal path graph of reference[16]

圖13 本文算法的最優(yōu)路徑圖Fig.13 Optimal path graph of proposed algorithm

圖15 文獻(xiàn)[16]算法最優(yōu)路徑圖Fig.15 Optimal path of algorithm in reference[16]

5.2 30×30復(fù)雜的環(huán)境

為進(jìn)一步驗(yàn)證改進(jìn)算法也能適用于更加復(fù)雜環(huán)境，因此在復(fù)雜環(huán)境下進(jìn)行仿真。傳統(tǒng)算法、文獻(xiàn)[16]算法和本文算法的最優(yōu)路徑圖分別如圖14~16所示，以及三種算法仿真結(jié)果數(shù)據(jù)如表2所示。

圖14 傳統(tǒng)算法的最優(yōu)路徑圖Fig.14 Optimal path graph of traditional algorithm

表2 30×30復(fù)雜的環(huán)境下三種算法的仿真結(jié)果Table 2 Simulation results of three algorithms in 30×30 complex environment

通過比較仿真數(shù)據(jù)，改進(jìn)算法得到的路徑最短為41.061 1，拐點(diǎn)數(shù)最少為2，路徑幾乎接近起點(diǎn)和終點(diǎn)的連線，且它們的平均值和標(biāo)準(zhǔn)差最小，文獻(xiàn)[16]次之，傳統(tǒng)算法最大，平均值和標(biāo)準(zhǔn)差的大小能夠反映在最小值附近的波動大小以及穩(wěn)定性，也可以反映該最小值出現(xiàn)機(jī)會的大小，如在50次的運(yùn)行結(jié)果中，傳統(tǒng)算法找到的最短路徑59.899 5出現(xiàn)1次，相應(yīng)的平均值和標(biāo)準(zhǔn)差較大，改進(jìn)算法找到的最短路徑41.061 1出現(xiàn)32次，相應(yīng)的平均值和標(biāo)準(zhǔn)差較小。由于傳統(tǒng)算法盲目性大，啟發(fā)信息較弱，在路徑搜索時，拐點(diǎn)較多，有時出現(xiàn)回環(huán)交叉，遠(yuǎn)離目標(biāo)行走，導(dǎo)致路徑長度增加。在收斂速度和算法運(yùn)行時間方面，改進(jìn)算法和文獻(xiàn)[16]算法都優(yōu)于傳統(tǒng)算法，其中改進(jìn)算法稍遜于文獻(xiàn)[16]算法。

圖16 本文算法最優(yōu)路徑圖Fig.16 Optimal path graph of proposed algorithm

6 結(jié)束語

在靜態(tài)的全局路徑規(guī)劃中，本文對傳統(tǒng)蟻群算法的不足，提出了一種改進(jìn)雙向蟻群算法。雙向蟻群算法結(jié)合障礙物有效頂點(diǎn)進(jìn)行路徑搜索，能夠快速找到最優(yōu)解且得到的路徑拐點(diǎn)相對較少；改進(jìn)的啟發(fā)函數(shù)與當(dāng)前有效頂點(diǎn)和下一可選有效頂點(diǎn)有關(guān)，能夠?qū)崿F(xiàn)一步或多步行走（相對于傳統(tǒng)算法）；改進(jìn)的狀態(tài)轉(zhuǎn)移規(guī)則能夠加快收斂速度；改進(jìn)的揮發(fā)系數(shù)能夠避免陷入早熟?；谝陨系母倪M(jìn)，本文算法能夠很好地適用于不同尺度和不同復(fù)雜程度的柵格地圖?？傊?，從小型簡單的柵格地圖中，已經(jīng)證明了改進(jìn)算法的優(yōu)越性，在大型復(fù)雜柵格地圖中能夠明顯地驗(yàn)證改進(jìn)算法可行性、有效性和優(yōu)越性。