李二超，齊款款

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

隨著科技迅速發(fā)展，人類(lèi)生活日愈趨向自動(dòng)化，智能移動(dòng)機(jī)器人已應(yīng)用于眾多領(lǐng)域，如移動(dòng)倉(cāng)儲(chǔ)機(jī)器人、服務(wù)移動(dòng)機(jī)器人等，其中移動(dòng)機(jī)器人路徑規(guī)劃是研究熱點(diǎn)之一。靜態(tài)環(huán)境移動(dòng)機(jī)器人路徑規(guī)劃是指在已知環(huán)境中根據(jù)目標(biāo)函數(shù)(如距離最短等)，尋找一條從起點(diǎn)到終點(diǎn)的無(wú)障礙的最優(yōu)或次優(yōu)路徑[1]。

移動(dòng)機(jī)器人路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[2]、A*算法[3]、蟻群算法[4]和遺傳算法[5-6]等。人工勢(shì)場(chǎng)法適用于動(dòng)態(tài)的局部路徑規(guī)劃，在靜態(tài)環(huán)境下易陷入極小值點(diǎn)，找不到最優(yōu)路徑；A*算法遍歷節(jié)點(diǎn)過(guò)多，占用內(nèi)存大，得到的路徑拐點(diǎn)較多，得到的路徑并非最優(yōu)；遺傳算法需要復(fù)雜的建模，路徑搜索效率低，占用很大的計(jì)算量和存儲(chǔ)空間；蟻群算法具有魯棒性較強(qiáng)，易與其他算法融合的優(yōu)點(diǎn)[7-8]，但該算法存在收斂速度慢、盲目性大、路徑無(wú)法做到最短的缺陷，學(xué)者們提出各自的改進(jìn)方法。王曉燕等利用人工勢(shì)場(chǎng)法求得的初始路徑信息對(duì)蟻群算法的啟發(fā)函數(shù)進(jìn)行改進(jìn)，增強(qiáng)路徑的引導(dǎo)作用，減小路徑搜索的盲目性[9]。陳志等在全局信息素更新方面，借鑒最大-最小蟻群系統(tǒng)和精英蟻群算法的思想，對(duì)最差路徑進(jìn)行懲罰，減少最差路徑對(duì)路徑搜索帶來(lái)的干擾，增強(qiáng)最優(yōu)路徑的指導(dǎo)作用，提高收斂速度[10]。王嬌嬌等采用基于中心點(diǎn)的平滑方法平滑尖峰的路徑，但此方法在平滑尖峰的同時(shí)增加了路徑的拐點(diǎn)，路徑依然不夠安全平滑[11]。王紅君等采用冗余點(diǎn)的刪除策略減少路徑上轉(zhuǎn)折點(diǎn)的數(shù)目，路徑相對(duì)平滑[12]。文獻(xiàn)[13]采用貝塞爾曲線優(yōu)化路徑，最大限度地平滑路徑，但出現(xiàn)了路徑碰撞。趙開(kāi)新等引入遺傳算法的路徑交叉策略得到更短路徑[14]。文獻(xiàn)[15]對(duì)初始信息素建立數(shù)學(xué)模型，信息素的非均勻分布使得螞蟻能夠在初始路徑搜索時(shí)，更傾向于選擇距離起點(diǎn)和終點(diǎn)連線較近的柵格作為下一節(jié)點(diǎn)，提高了算法的收斂速度，減少了路徑搜索的盲目性。Luo等使用偽隨機(jī)概率轉(zhuǎn)移公式提高算法的全局搜索能力和收斂速度[16]。

基于以上研究，針對(duì)傳統(tǒng)蟻群算法無(wú)法找到最短路徑、路徑搜索盲目性大、拐點(diǎn)多問(wèn)題，本文提出改進(jìn)貝塞爾曲線與雙向蟻群算法融合的機(jī)器人路徑規(guī)劃算法。首先，添加雙向螞蟻的路徑搜索策略，能夠找到最優(yōu)解。其次，采用路徑交叉策略產(chǎn)生新解，能夠獲得更優(yōu)解。然后，改進(jìn)了啟發(fā)式函數(shù)，使路徑搜索更具有目的性，采用只更新有效路徑上信息素的全局信息素更新方式，避免無(wú)效路徑的干擾，為防止信息素積累過(guò)多，自適應(yīng)調(diào)節(jié)信息素?fù)]發(fā)系數(shù)，設(shè)置信息素的取值范圍。最后，應(yīng)用改進(jìn)的貝塞爾曲線優(yōu)化路徑，能夠根據(jù)機(jī)器人需求調(diào)節(jié)平滑程度。

1 環(huán)境建模

本文機(jī)器人的工作環(huán)境為柵格地圖，即利用柵格法來(lái)建立二維靜態(tài)環(huán)境。柵格法由柵格取值為二進(jìn)制0和1的矩陣構(gòu)成，矩陣中0 代表自由柵格，機(jī)器人可自由移動(dòng)，用白色柵格表示；1 代表障礙柵格，機(jī)器人需要繞行前進(jìn)，用黑色柵格表示。

本文將柵格地圖進(jìn)行了劃分，劃分為Nx×Ny個(gè)小正方形柵格，地圖按照從左到右、從下到上的順序依次編號(hào)1、2、…，柵格序號(hào)與坐標(biāo)一一對(duì)應(yīng)，如圖1所示，坐標(biāo)與柵格編號(hào)的關(guān)系表達(dá)式如式(1)，求得的結(jié)果為柵格的中心點(diǎn)。

圖1 柵格地圖

(1)

式中:i代表柵格序號(hào);Nx為柵格地圖的行數(shù);Ny為柵格地圖的列數(shù)。

為防止機(jī)器人與障礙物發(fā)生碰撞，根據(jù)機(jī)器人的半徑將障礙物進(jìn)行膨化處理，處理方式如圖2所示。當(dāng)不規(guī)則障礙物不滿一個(gè)柵格時(shí)，將其填充為一個(gè)柵格，灰色部分為障礙物的膨化，其邊長(zhǎng)為機(jī)器人半徑。整體構(gòu)成了障礙物，此時(shí)把機(jī)器人看作質(zhì)點(diǎn)來(lái)處理。假設(shè)膨化后的正方形邊長(zhǎng)為1，即式(1)中a=1。

圖2 障礙物膨化處理圖

機(jī)器人運(yùn)動(dòng)方向?yàn)楫?dāng)前位置中心與其相鄰8個(gè)柵格中心連線的方向。為避免機(jī)器人走回頭路，去掉前一步走過(guò)的方向，只有7個(gè)方向可以選擇，具體情況如圖3所示。

圖3 機(jī)器人運(yùn)動(dòng)方式

本文研究的目標(biāo)函數(shù)為最短路徑

(2)

式中：{O1,O2,…,ON+1}為組成路徑經(jīng)過(guò)的柵格節(jié)點(diǎn)集合，且O1=S(起點(diǎn))，ON+1=E(終點(diǎn))；L為路徑長(zhǎng)度。

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

2.1 路徑選擇概率

狀態(tài)轉(zhuǎn)移概率

(3)

式中：j∈Gk表示螞蟻k下一步可到達(dá)的相鄰柵格集合；τij(t)表示在t時(shí)刻螞蟻從當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j之間的信息素濃度；ηij(t)表示在t時(shí)刻螞蟻從當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j之間的啟發(fā)信息；α和β分別表示信息素因子和啟發(fā)式因子。啟發(fā)式函數(shù)的表達(dá)式為

(4)

式中：dij為當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j的歐式距離。

2.2 信息素更新

全局信息素更新公式如下：

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

(5)

(6)

(7)

式中：τij(t+1)表示信息素更新后路徑(i,j)的信息素濃度；τij(t)表示當(dāng)前路徑(i,j)的信息素濃度；ρ為信息素?fù)]發(fā)系數(shù)；Δτij(t)表示所有螞蟻在路徑(i,j)留下的信息素濃度之和；Q為信息素強(qiáng)度；Lk為螞蟻k所走的路徑長(zhǎng)度。

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

3.1 雙向搜索策略

所謂雙向蟻群搜索方法就是將所有螞蟻平均分成兩組，一組從起始點(diǎn)向目標(biāo)點(diǎn)進(jìn)行搜索(正向搜索)，一組從目標(biāo)點(diǎn)向起始點(diǎn)進(jìn)行搜索(反向螞蟻)，路徑搜索時(shí)輪流交替從兩個(gè)方向進(jìn)行搜索，即先由正向的一只螞蟻進(jìn)行路徑搜索，到達(dá)目標(biāo)點(diǎn)或者發(fā)生“死鎖”，再由反向的一只螞蟻進(jìn)行路徑搜索，與前一只螞蟻相遇(包括起點(diǎn)和目標(biāo)點(diǎn)，在這兩點(diǎn)相遇，可理解為正向或反向螞蟻搜索路徑成功，是相遇情況中的特殊情況)或者發(fā)生“死鎖”，再由正向的一只螞蟻進(jìn)行路徑搜索，如此反復(fù)。為了區(qū)別正向螞蟻和反向螞蟻路徑搜索，用A0表示螞蟻從起點(diǎn)出發(fā)進(jìn)行路徑搜索，用A1表示螞蟻從目標(biāo)點(diǎn)出發(fā)進(jìn)行路徑搜索。

3.2 路徑交叉策略

雙向蟻群算法能夠快速搜索出一條有效路徑，在此基礎(chǔ)上，當(dāng)有2條有效路徑存在路徑重疊部分，即它們有除了起點(diǎn)和目標(biāo)點(diǎn)之外的交叉點(diǎn)，滿足上述條件可進(jìn)行路徑交叉。若存在多個(gè)交叉點(diǎn)，依次進(jìn)行判斷此路徑段是否更短，交叉完成后，整條路徑是否最短，若不是，繼續(xù)保留之前的最短路徑，若是，將其更新為當(dāng)前最優(yōu)路徑。若存在一個(gè)交叉點(diǎn)，方法同上，路徑交叉前后示意圖如圖4和圖5所示。

交叉操作如下：圖4實(shí)線線段{S,7,12,17,18,19,E}為有效路徑1，虛線線段{S,2,7,12,18,24,E}為有效路徑2，兩條路徑都經(jīng)過(guò)的交叉點(diǎn)為{7,12,18},從第一個(gè)交叉點(diǎn)7進(jìn)行交叉操作并判斷，很顯然實(shí)線部分長(zhǎng)度較短，暫時(shí)保留這一部分；交叉點(diǎn)7和12長(zhǎng)度相同，任選其一；交叉點(diǎn)12和18之間，虛線部分較短，暫時(shí)選擇此虛線部分；交叉點(diǎn)18到終點(diǎn)之間長(zhǎng)度相同，任選其一，整個(gè)交叉完成后，計(jì)算出交叉后路徑長(zhǎng)度，得到的路徑如圖5的實(shí)線線段，得到的更短路徑為{S,7,12,18,24,E}。

圖4 路徑交叉前

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

由于在傳統(tǒng)蟻群算法中引入了雙向策略，每個(gè)方向路徑搜索都有自己的轉(zhuǎn)移概率公式,兩個(gè)方向轉(zhuǎn)移概率公式最大的不同點(diǎn)在于啟發(fā)函數(shù)不同。傳統(tǒng)的啟發(fā)函數(shù)只與當(dāng)前點(diǎn)和下一節(jié)點(diǎn)之間的距離有關(guān)，啟發(fā)信息引導(dǎo)性較弱；本文采用的啟發(fā)函數(shù)能夠更好地引導(dǎo)螞蟻向目標(biāo)點(diǎn)前進(jìn)。

1) 正向螞蟻A0路徑搜索的啟發(fā)函數(shù)為

(8)

式中djE表示正向搜索時(shí)下一節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的歐氏距離。

2)反向螞蟻A1路徑搜索的啟發(fā)函數(shù)為

(9)

式中dbS表示反向搜索時(shí)下一節(jié)點(diǎn)與起點(diǎn)之間的歐氏距離。

根據(jù)式(8)和式(9)，兩個(gè)方向的轉(zhuǎn)移概率公式如下：

正向螞蟻A0路徑搜索時(shí)，

(10)

反向螞蟻A1路徑搜索時(shí)，

(11)

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

由于蟻群算法在不同階段需要不同大小的揮發(fā)系數(shù)，因此有著固定值的傳統(tǒng)揮發(fā)系數(shù)是行不通的，需要?jiǎng)討B(tài)調(diào)整其值，由此本文引入動(dòng)態(tài)調(diào)整揮發(fā)系數(shù)[13]

(12)

式中：ρmin為揮發(fā)系數(shù)的最小值；c、d為系數(shù)。

3.5 改進(jìn)信息素更新方式

由于本文算法設(shè)計(jì)的特殊性，只采用全局信息素更新方式且只更新有效路徑，如式(13)～(15)所示。

(13)

(14)

(15)

3.6 信息素限制策略

為防止算法陷入局部收斂，對(duì)信息素濃度進(jìn)行限制。在信息素?fù)]發(fā)階段，需要施加最小范圍的控制；在信息素更新階段，需要施加最大范圍的控制。具體公式如下：

(16)

3.7 路徑平滑策略

傳統(tǒng)蟻群算法在柵格環(huán)境下規(guī)劃的路徑轉(zhuǎn)折較多，機(jī)器人會(huì)消耗大量的能量，路徑并非最優(yōu)。因此，有必要進(jìn)行路徑平滑，使路徑更符合機(jī)器人實(shí)際行走的環(huán)境。本文采用文獻(xiàn)[13]的貝塞爾曲線，并加入了自己的改進(jìn)思想。

n階貝塞爾曲線公式如下：

(17)

式中：P(u)為貝塞爾曲線的運(yùn)動(dòng)控制點(diǎn)；P(i)為位置點(diǎn)，P(0)和P(1)分別為起點(diǎn)和終點(diǎn)。

i=0,1,2,…,n。

(18)

本文改進(jìn)思想如圖6和圖7所示。貝塞爾曲線是由無(wú)數(shù)個(gè)點(diǎn)組成的曲線，本文將曲線分成若干個(gè)點(diǎn)，根據(jù)機(jī)器人對(duì)路徑平滑精度的不同要求，可以合理選擇點(diǎn)的數(shù)目，點(diǎn)的數(shù)目越多，路徑平滑越接近曲線。極端情況下，可以將曲線變成折線段，曲線分段思想如圖6所示。另一方面的改進(jìn)是插入控制點(diǎn)，插入控制點(diǎn)的位置在路徑轉(zhuǎn)折處的線段上。插入的控制點(diǎn)越多，控制點(diǎn)越逼近拐點(diǎn)處，平滑的長(zhǎng)度就會(huì)減小，當(dāng)達(dá)到一定的數(shù)目時(shí)，平滑路徑與原路徑重合。這一改進(jìn)可以保證路徑最短并且安全性較高，插入控制點(diǎn)方法如圖7所示。圖6和圖7分別是同一柵格的放大，折線線段為平滑前的路徑，曲線為平滑后的路徑。

圖6 曲線分段思想示意圖

圖7 插入控制點(diǎn)示意圖

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

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

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

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

為了驗(yàn)證本文算法的可行性、有效性和優(yōu)越性，在MATLAB 2016a上進(jìn)行仿真實(shí)驗(yàn)。本文從以下幾個(gè)方面進(jìn)行實(shí)驗(yàn)驗(yàn)證：1)在文獻(xiàn)[13]的環(huán)境下進(jìn)行對(duì)比分析;2)在不同的柵格地圖尺寸和不同復(fù)雜程度的環(huán)境下，將本文改進(jìn)蟻群算法與傳統(tǒng)蟻群算法和文獻(xiàn)[13]進(jìn)行對(duì)比分析，驗(yàn)證本文算法的優(yōu)點(diǎn);3)應(yīng)用貝塞爾曲線平滑處理本文算法最優(yōu)路徑。

仿真參數(shù)設(shè)置分別為：螞蟻數(shù)目為50；最大迭代次數(shù)為50；揮發(fā)因子的系數(shù)c=0.8,d=25；α=1；β=10；Q=150。以下結(jié)果都是算法運(yùn)行50次的平均值。

5.1 文獻(xiàn)[13]的15×15運(yùn)行環(huán)境

傳統(tǒng)蟻群算法、文獻(xiàn)[13]和本文算法的最優(yōu)路徑圖與收斂曲線圖以及平滑前后的對(duì)比圖，分別如圖9～18所示。仿真結(jié)果見(jiàn)表1。

圖9 傳統(tǒng)蟻群算法的最優(yōu)路徑圖

圖10 傳統(tǒng)蟻群算法的收斂曲線圖

圖11 文獻(xiàn)[13]的最優(yōu)路徑圖

圖12 文獻(xiàn)[13]的收斂曲線圖

圖13 本文算法的最優(yōu)路徑圖

圖14 本文算法的收斂曲線圖

圖15 傳統(tǒng)蟻群算法的平滑前后對(duì)比圖

圖16 文獻(xiàn)[13]平滑前后對(duì)比圖

圖17 本文算法平滑前后對(duì)比圖

表1 算法仿真結(jié)果

3種算法都能找到各自的最優(yōu)路徑，但傳統(tǒng)蟻群算法找到的不是最短路徑，并且拐點(diǎn)很多，從而增加了路徑長(zhǎng)度。本文算法得到的最短路徑、拐點(diǎn)數(shù)目和路徑搜索的目的性都優(yōu)于傳統(tǒng)算法。和文獻(xiàn)[13]算法相比，本文算法得到的最短路徑一樣，拐點(diǎn)的穩(wěn)定性(標(biāo)準(zhǔn)差)僅次于文獻(xiàn)[13],但本文算法的迭代次數(shù)優(yōu)于文獻(xiàn)[13]。值得注意的是，文獻(xiàn)[13]所使用的平滑方法使得路徑與障礙物發(fā)生碰撞(如圖16和圖18所示)。為此，本文對(duì)此碰撞問(wèn)題進(jìn)行了改進(jìn)，如前文路徑平滑策略所述，最終得到的路徑更適合機(jī)器人運(yùn)行。

圖18 圖16的拐點(diǎn)放大圖

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

為進(jìn)一步驗(yàn)證本文算法的可行性、有效性和優(yōu)越性，在其他不同尺度的地圖和復(fù)雜程度不同環(huán)境中，將本文算法與傳統(tǒng)算法、文獻(xiàn)[13]進(jìn)行性能比較。因?yàn)槲墨I(xiàn)[13]平滑方法容易與路徑發(fā)生碰撞，為方便比較平滑后路徑，對(duì)文獻(xiàn)[13]采用本文的路徑平滑方法。傳統(tǒng)算法和文獻(xiàn)[13]與本文算法的最優(yōu)路徑圖和收斂曲線圖以及平滑前后的對(duì)比圖，分別如圖19～27所示。仿真結(jié)果見(jiàn)表2。

表2 3種算法的仿真結(jié)果

圖19 傳統(tǒng)算法的最優(yōu)路徑圖

圖20 傳統(tǒng)算法的收斂曲線圖

圖21 文獻(xiàn)[13]算法的最優(yōu)路徑圖

圖22 文獻(xiàn)[13]算法的收斂曲線圖

圖23 本文算法的最優(yōu)路徑圖

圖24 本文算法的收斂曲線圖

圖25 傳統(tǒng)蟻群算法的平滑前后對(duì)比圖

圖26 文獻(xiàn)[13]平滑前后對(duì)比圖

圖27 本文算法平滑前后對(duì)比圖

顯而易見(jiàn)，3種算法都能找到各自的最優(yōu)路徑，但傳統(tǒng)蟻群算法找到的不是最短路徑，并且在復(fù)雜的環(huán)境中拐點(diǎn)更多，從而增加了路徑長(zhǎng)度，路徑搜索時(shí)盲目性太大。本文算法在數(shù)值穩(wěn)定性(平均值和標(biāo)準(zhǔn)差都最小)方面優(yōu)于2種對(duì)比算法，得到的最短路徑、拐點(diǎn)數(shù)目和路徑搜索的目的性都優(yōu)于傳統(tǒng)算法；和文獻(xiàn)[13]算法相比，本文算法得到的最短路徑更短。本文算法在某些方面仍有改進(jìn)空間，后續(xù)會(huì)繼續(xù)進(jìn)行研究。

6 結(jié)論

在靜態(tài)的全局路徑規(guī)劃中，針對(duì)傳統(tǒng)蟻群算法無(wú)法找到最短路徑、拐點(diǎn)多和盲目性大等缺陷，本文提出了一種基于貝塞爾曲線融合雙向蟻群算法。在傳統(tǒng)蟻群算法的基礎(chǔ)上，引入了雙向的路徑搜索方式，能夠找到最優(yōu)解；雙向策略與路徑交叉策略的結(jié)合能夠優(yōu)化最優(yōu)解；把目標(biāo)點(diǎn)的信息加入啟發(fā)式函數(shù)中，增加路徑搜索的目的性，間接改進(jìn)了概率轉(zhuǎn)移公式，使得螞蟻更傾向于距離目標(biāo)點(diǎn)近的下一節(jié)點(diǎn)轉(zhuǎn)移；采用有效路徑的信息素更新方式，添加動(dòng)態(tài)調(diào)整揮發(fā)系數(shù)公式。對(duì)貝塞爾曲線進(jìn)行了相應(yīng)改進(jìn)，增加控制點(diǎn)使曲線優(yōu)化的路徑更加安全，也對(duì)曲線進(jìn)行了分割，以滿足對(duì)路徑平滑度不同的需求。同時(shí)，通過(guò)實(shí)驗(yàn)驗(yàn)證了本文算法的可行性、有效性和優(yōu)越性。