貝塞爾曲線融合雙向蟻群算法的移動機器人路徑規(guī)劃

李二超，齊款款

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

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

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

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

1 環(huán)境建模

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

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

圖1 柵格地圖

(1)

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

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

圖2 障礙物膨化處理圖

機器人運動方向為當前位置中心與其相鄰8個柵格中心連線的方向。為避免機器人走回頭路，去掉前一步走過的方向，只有7個方向可以選擇，具體情況如圖3所示。

圖3 機器人運動方式

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

(2)

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

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

2.1 路徑選擇概率

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

(3)

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

(4)

式中：dij為當前節(jié)點i到下一節(jié)點j的歐式距離。

2.2 信息素更新

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

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

(5)

(6)

(7)

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

3 改進蟻群算法

3.1 雙向搜索策略

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

3.2 路徑交叉策略

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

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

圖4 路徑交叉前

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

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

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

(8)

式中djE表示正向搜索時下一節(jié)點與目標點之間的歐氏距離。

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

(9)

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

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

正向螞蟻A0路徑搜索時，

(10)

反向螞蟻A1路徑搜索時，

(11)

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

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

(12)

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

3.5 改進信息素更新方式

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

(13)

(14)

(15)

3.6 信息素限制策略

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

(16)

3.7 路徑平滑策略

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

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

(17)

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

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

(18)

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

圖6 曲線分段思想示意圖

圖7 插入控制點示意圖

4 改進蟻群算法流程

改進后的蟻群算法流程圖如圖8所示。

圖8 改進蟻群算法流程圖

5 實驗仿真與分析

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

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

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

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

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

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

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

圖12 文獻[13]的收斂曲線圖

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

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

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

圖16 文獻[13]平滑前后對比圖

圖17 本文算法平滑前后對比圖

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

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

圖18 圖16的拐點放大圖

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

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

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

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

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

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

圖22 文獻[13]算法的收斂曲線圖

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

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

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

圖26 文獻[13]平滑前后對比圖

圖27 本文算法平滑前后對比圖

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

6 結(jié)論

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