B樣條曲線融合蟻群算法的機(jī)器人路徑規(guī)劃

李二超，齊款款

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

（?通信作者電子郵箱lecstarr@163.com）

0 引言

靜態(tài)環(huán)境下的移動(dòng)機(jī)器人路徑規(guī)劃是指在環(huán)境已知條件下，移動(dòng)機(jī)器人根據(jù)目標(biāo)函數(shù)（如距離最短等），按照已知算法尋找一條從起點(diǎn)到終點(diǎn)的安全且最優(yōu)路徑。移動(dòng)機(jī)器人路徑規(guī)劃算法有很多，如人工勢場法［1］、A*算法［2］、蟻群算法［3］、遺傳算法［4-5］等。其中，蟻群算法是一種啟發(fā)式的隨機(jī)搜索算法，魯棒性強(qiáng)，具有優(yōu)良的并行分布式計(jì)算能力和易于與其他算法融合的優(yōu)點(diǎn)［6-7］，但無法找到最短路徑，存在收斂速度慢、路徑搜索盲目性大、路徑拐點(diǎn)較多、路徑不平滑等不足。針對這些不足：孟冠軍等［8］利用A*算法搜索速度快的特點(diǎn)計(jì)算得到蟻群算法的初始路徑，實(shí)現(xiàn)了初始信息素的非均勻分布，減少了路徑搜索的盲目性；曹新亮等［9］對初始信息素建立數(shù)學(xué)模型，實(shí)現(xiàn)了信息素的非均勻分布，使螞蟻能夠在初始路徑搜索時(shí)更傾向于選擇距離起點(diǎn)和終點(diǎn)連線較近的柵格作為下一節(jié)點(diǎn)，以提高算法的收斂速度，減少路徑搜索的盲目性；王紅君等［10］使用冗余點(diǎn)的刪除策略減少了路徑上的拐點(diǎn)數(shù)目；Luo等［11］使用偽隨機(jī)概率轉(zhuǎn)移公式提高了算法的全局搜索能力和收斂速度；李志錕等［12］采用多步長路徑搜索策略，實(shí)現(xiàn)了拐點(diǎn)數(shù)目少且路徑最短；胡澮冕等［13］采用雙向蟻群算法路徑搜索策略加快了算法運(yùn)行速度，提高了全局搜索能力；張軍明等［14］采用自適應(yīng)調(diào)整揮發(fā)系數(shù)來加強(qiáng)較優(yōu)路徑信息素并減弱較差路徑信息素的方法，加快了算法的收斂；封聲飛等［15］使用分段三階貝塞爾曲線優(yōu)化最優(yōu)路徑，能夠得到更短路徑且拐點(diǎn)處的路徑平滑性較好。

基于前人的研究，本文主要解決傳統(tǒng)蟻群算法無法找到最短路徑、路徑搜索盲目性大、收斂速度慢和路徑不平滑問題，提出B 樣條曲線融合蟻群算法。與折線優(yōu)化路徑相比，B樣條曲線優(yōu)化的路徑較為光滑，而與其他曲線如貝塞爾曲線優(yōu)化路徑相比，B 樣條曲線除具有貝塞爾曲線的優(yōu)點(diǎn)外，還能夠克服其缺乏局部性質(zhì)的缺點(diǎn)。本文算法首先對路徑初始信息素進(jìn)行非均勻分布，在起點(diǎn)和終點(diǎn)連線附近設(shè)置最大濃度的信息素，且距離該線段越遠(yuǎn)，信息素濃度越低；其次，在啟發(fā)式函數(shù)中加入當(dāng)前節(jié)點(diǎn)、下一節(jié)點(diǎn)和目標(biāo)點(diǎn)的信息并加入動(dòng)態(tài)調(diào)節(jié)因子，實(shí)現(xiàn)前期路徑搜索主要依靠啟發(fā)信息，后期削弱，使路徑搜索更具有目的性，能夠朝著路徑最短方向上移動(dòng)，同時(shí)結(jié)合偽隨機(jī)（確定性概率、隨機(jī)性概率和任一性概率）狀態(tài)轉(zhuǎn)移策略，降低傳統(tǒng)算法路徑搜索的盲目性，加快收斂，減少轉(zhuǎn)折點(diǎn)數(shù)目；然后，為防止信息素積累過多，在自適應(yīng)調(diào)節(jié)信息素?fù)]發(fā)系數(shù)的同時(shí)，設(shè)置信息素濃度的取值范圍；最后，使用B樣條曲線對得到的最優(yōu)路徑進(jìn)行平滑處理，以進(jìn)一步使拐點(diǎn)處路徑更短、更光滑。

1 環(huán)境建模

本文機(jī)器人工作環(huán)境為柵格地圖，具體如圖1所示。

圖1 柵格地圖Fig.1 Grid map

柵格法由柵格取值為二進(jìn)制的0 和1 矩陣構(gòu)成：0 代表自由柵格（白色柵格），機(jī)器人可自由移動(dòng)；1 代表障礙柵格（黑色柵格），機(jī)器人需要繞行前進(jìn)。地圖按照從左到右、從下到上的順序依次編號1、2、…，柵格序號與坐標(biāo)一一對應(yīng)，坐標(biāo)與柵格編號的關(guān)系表達(dá)式如式（1），求得的結(jié)果為柵格的中心點(diǎn)。

其中：i代表柵格序號；Nx和Ny分別代表柵格地圖的行數(shù)與列數(shù)；a表示一個(gè)單位的柵格邊長；mod（）是求余運(yùn)算，ceil（）是向上取整運(yùn)算。

為防止機(jī)器人與障礙物發(fā)生碰撞，當(dāng)不規(guī)則障礙物不滿一個(gè)柵格時(shí)，將其填充為一個(gè)柵格，再將障礙物向外膨化，寬度為機(jī)器人的半徑，整體構(gòu)成障礙物，此時(shí)把機(jī)器人看作質(zhì)點(diǎn)來處理，假設(shè)膨化后的正方形邊長為1，即式（1）中的a=1。

機(jī)器人運(yùn)動(dòng)方向?yàn)? 個(gè)，去掉前一步走過的，只有7 個(gè)方向可以選擇。

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

2.1 路徑選擇概率

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

其中：j∈allowedm表示螞蟻m下一步可到達(dá)的相鄰柵格集合；τij(t)表示信息素濃度；ηij(t)表示啟發(fā)信息；α和β分別表示信息素因子和啟發(fā)式因子，啟發(fā)式函數(shù)如式（3）所示。

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

2.2 信息素更新

信息素更新公式如式（4）～（6）所示：

其中：τij(t+1)表示信息素更新后的信息素濃度；ρ為揮發(fā)系數(shù)；Δτij(t)表示信息素濃度增量；Q為信息素強(qiáng)度；Lm為螞蟻m所走的路徑長度。

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

3.1 初始信息素不平等分布

傳統(tǒng)算法的初始信息素濃度相等，選擇路徑的概率差異不大，路徑選擇的盲目性很大，本文將初始信息素進(jìn)行差異化處理，在起點(diǎn)和終點(diǎn)連線附近信息素濃度最大，距離該線段越遠(yuǎn)，信息素濃度越低，在路徑搜索時(shí)，使得螞蟻更傾向于選擇該線段附近節(jié)點(diǎn)作為待選節(jié)點(diǎn)，得到的路徑更接近最優(yōu)解。初始信息素分布公式如式（7）所示：

其中：dSi表示起點(diǎn)與當(dāng)前點(diǎn)的歐氏距離，djE表示下一節(jié)點(diǎn)與目標(biāo)點(diǎn)的歐氏距離；C為信息素濃度常數(shù)；τ0為初始信息素。

3.2 改進(jìn)啟發(fā)函數(shù)

本文的啟發(fā)函數(shù)是在文獻(xiàn)［16］中啟發(fā)函數(shù)的基礎(chǔ)上加入了動(dòng)態(tài)調(diào)節(jié)因子（由最大迭代次數(shù)itermax和當(dāng)前迭代次數(shù)iter構(gòu)成）。在迭代前期，該調(diào)節(jié)因子促使啟發(fā)函數(shù)起主導(dǎo)作用；隨著迭代次數(shù)的增加，路徑上積累了一定量的信息素，此時(shí)調(diào)節(jié)因子會(huì)減弱啟發(fā)信息的引導(dǎo)作用，加強(qiáng)信息素的引導(dǎo)作用。

3.3 偽隨機(jī)狀態(tài)轉(zhuǎn)移策略

轉(zhuǎn)移概率公式如式（9）所示：

本文算法引入偽隨機(jī)狀態(tài)轉(zhuǎn)移策略，確定性概率用來減少路徑搜索的隨機(jī)性，同時(shí)也不能過于偏向于確定性概率而導(dǎo)致陷入局部最優(yōu)值，因此引入任一性概率，如式（10）所示：

式中：rand為［0，1］區(qū)間的隨機(jī)數(shù)；q0、q1、q2由前人經(jīng)驗(yàn)以及反復(fù)實(shí)驗(yàn)來確定的常數(shù)，范圍為（0，1）；randj為任意選擇下一可行節(jié)點(diǎn)。

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

由于蟻群算法的特殊性，在不同階段需要不同大小的揮發(fā)系數(shù)：如果ρ設(shè)置過大，螞蟻無法依靠信息素信息進(jìn)行路徑搜索，導(dǎo)致收斂慢；如果ρ設(shè)置過小，信息素過度積累，則會(huì)使路徑搜索陷入局部最優(yōu)。固定值的揮發(fā)系數(shù)無法動(dòng)態(tài)調(diào)整，因此引入動(dòng)態(tài)調(diào)整揮發(fā)系數(shù)如式（11）所示；同時(shí)，為防止算法陷入局部收斂，對信息素濃度進(jìn)行限制，如式（12）所示。

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

3.5 B樣條曲線平滑策略

改進(jìn)蟻群算法生成的最優(yōu)路徑仍不夠平滑，部分路徑中還存在尖銳拐點(diǎn)。因此，在改進(jìn)蟻群算法基礎(chǔ)上，引入三次均勻B樣條曲線平滑優(yōu)化拐點(diǎn)處的路徑。

由公式

可知，k=3時(shí)的B樣條曲線數(shù)學(xué)表達(dá)式為：

當(dāng)三次B 樣條曲線各節(jié)點(diǎn)矢量間插值為常數(shù)時(shí)，為三次均勻B樣條曲線，第i段三次均勻B樣條曲線數(shù)學(xué)表達(dá)式為：

由式（13）、（15）、（16）可得三次均勻B 樣條曲線的基函數(shù)數(shù)學(xué)表達(dá)式：

將式（17）代入式（13）可得：

將式（18）用矩陣形式表達(dá)為：

式（18）、（19）為三次均勻B樣條曲線數(shù)學(xué)表達(dá)式。

圖2 為B 樣條曲線平滑最優(yōu)路徑仿真示意圖，折線為平滑前最優(yōu)路徑，曲線為B 樣條曲線，該平滑策略在拐點(diǎn)附近，以曲線代替折線，得到的路徑更短且平滑。

圖2 B樣條曲線平滑最優(yōu)路徑仿真示意圖Fig.2 Simulation diagram of B-spline curve smoothing optimal path

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

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

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

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

為驗(yàn)證本文算法的可行性、有效性和優(yōu)越性，在Matlab 2016a上進(jìn)行仿真實(shí)驗(yàn)。主要從以下幾個(gè)方面進(jìn)行實(shí)驗(yàn)驗(yàn)證：1）對主要參數(shù)進(jìn)行敏感性分析；2）在稍微復(fù)雜的柵格地圖環(huán)境中，在相同的參數(shù)條件下，為更方便驗(yàn)證各個(gè)改進(jìn)環(huán)節(jié)的可行性和有效性以及本文算法的優(yōu)越性，在傳統(tǒng)算法上單獨(dú)添加改進(jìn)的偽隨機(jī)轉(zhuǎn)移策略（方案1）、在方案1的基礎(chǔ)上加初始信息素不平等分布（方案2），在方案2的基礎(chǔ)上加改進(jìn)啟發(fā)函數(shù)（方案3）和在方案3的基礎(chǔ)上加改進(jìn)揮發(fā)系數(shù)（本文算法平滑前）以及在傳統(tǒng)算法上單獨(dú)添加平滑策略（傳統(tǒng)算法+平滑）進(jìn)行對比分析，將整體改進(jìn)方法（本文算法平滑前后）分別與傳統(tǒng)算法平滑前后和文獻(xiàn)［16］改進(jìn)蟻群算法平滑前后進(jìn)行仿真對比分析；3）在大型復(fù)雜柵格地圖環(huán)境下將本文算法與傳統(tǒng)算法和文獻(xiàn)［16］改進(jìn)蟻群算法（使用文獻(xiàn)參數(shù)）進(jìn)行對比分析，驗(yàn)證本文算法的優(yōu)點(diǎn)。

仿真參數(shù)設(shè)置：螞蟻數(shù)目為50，最大迭代次數(shù)為100，α=2，β=7，ρmin=0.1，ρ(iter=0)=0.9，Q=150，C=20，q0=0.8，q1=0.9，q2=1。除參數(shù)分析實(shí)驗(yàn)外，其他實(shí)驗(yàn)結(jié)果都是算法運(yùn)行50次得到的平均值。

4.1 主要參數(shù)分析

本文采用控制變量法，設(shè)置一系列的組合，每種組合運(yùn)行20 次，對均值進(jìn)行比較，原始參數(shù)組合為：α=1，β=6，Q=50，ρmin=0.3，ρmax=0.8，q0=0.6，q1=0.8，q2=1，本文算法參數(shù)分析結(jié)果如表1 所示。從表1 中可知，適當(dāng)增加Q值，能夠改善路徑長度；由表2可知，q0的值較大時(shí)，路徑長度和迭代次數(shù)較優(yōu)；表3 中最小值取值不宜過小，最大值不宜過大，路徑長度和迭代次數(shù)較優(yōu)；表4 中參數(shù)α的值較小，β值適當(dāng)較大時(shí)效果較好。

表1 參數(shù)Q對路徑長度和迭代次數(shù)的影響Tab.1 Influence of parameter Q on path length and iteration times

表2 參數(shù)［q0，q1］對路徑長度和迭代次數(shù)的影響Tab.2 Influence of parameters［q0，q1］on path length and iteration times

表3 參數(shù)［ρmin，ρmax］對路徑長度和迭代次數(shù)的影響Tab.3 Influence of parameters［ρmin，ρmax］on path length and iteration times

表4 參數(shù)［α，β］對路徑長度和迭代次數(shù)的影響Tab.4 Influence of parameters［α，β］on path length and iteration times

4.2 20×20運(yùn)行環(huán)境的最優(yōu)路徑平滑效果對比

各方案的最優(yōu)路徑圖如圖4 所示，本文算法與傳統(tǒng)算法和文獻(xiàn)［16］改進(jìn)蟻群算法的最優(yōu)路徑平滑前后圖如圖5 所示；上述各方案和三種算法仿真數(shù)據(jù)如表5所示。

表5 20×20運(yùn)行環(huán)境下的仿真結(jié)果Tab.5 Simulation results in 20×20 running environment

圖4 20×20運(yùn)行環(huán)境下不同方案的最優(yōu)路徑對比Fig.6 Comparison of optimal paths of different schemes in 20×20 running environment

圖5 20×20運(yùn)行環(huán)境下各算法平滑前后最優(yōu)路徑對比Fig.5 Comparison of optimal paths of each algorithm before and after smoothing in 20×20 running environment

從每一改進(jìn)部分得到的數(shù)據(jù)可知，傳統(tǒng)算法最優(yōu)路徑長度為41.414 2，經(jīng)過B 樣條曲線對拐點(diǎn)附近平滑優(yōu)化后最優(yōu)路徑長度為39.241 0，路徑長度縮短了5.2%，說明了本文平滑策略的可行性和有效性；在傳統(tǒng)算法基礎(chǔ)上單獨(dú)添加偽隨機(jī)轉(zhuǎn)移策略（方案1），最優(yōu)路徑長度為36.242 6，優(yōu)于傳統(tǒng)算法，算法運(yùn)行時(shí)間有效縮短，拐點(diǎn)數(shù)目較少，迭代次數(shù)欠佳，說明了偽隨機(jī)轉(zhuǎn)移策略可行性和有效性；在方案1的基礎(chǔ)上加信息素不平等分布（方案2），最優(yōu)路徑長度為35.071 1，優(yōu)于方案1，算法運(yùn)行時(shí)間進(jìn)一步縮短，路徑搜索的目的性有所增強(qiáng)（在起點(diǎn)和終點(diǎn)連線附近搜索），啟發(fā)信息較弱，說明了本文初始信息素不平等分布的可行性和有效性；在方案2的基礎(chǔ)上加改進(jìn)啟發(fā)函數(shù)（方案3），最短路徑、算法運(yùn)行時(shí)間和拐點(diǎn)數(shù)等指標(biāo)全面改善，說明本文改進(jìn)啟發(fā)函數(shù)可行性和有效性；在方案3的基礎(chǔ)上加改進(jìn)揮發(fā)系數(shù)（本文平滑前），拐點(diǎn)數(shù)和運(yùn)行時(shí)間優(yōu)于方案3，說明本文改進(jìn)揮發(fā)系數(shù)可行性和有效性。

從整體上看，本文改進(jìn)蟻群算法和文獻(xiàn)［16］改進(jìn)蟻群算法都能夠找到比傳統(tǒng)算法得到的路徑更短、拐點(diǎn)相對較少的路徑，而且運(yùn)行時(shí)間較短，能夠快速收斂，路徑搜索的目的性加強(qiáng)。本文改進(jìn)算法和文獻(xiàn)［16］改進(jìn)蟻群算法平滑前最優(yōu)路徑相同，但本文采用了B樣條曲線對路徑進(jìn)行二次優(yōu)化，平滑后的路徑優(yōu)于文獻(xiàn)［16］改進(jìn)算法。為說明本文平滑策略的有效性，將本文平滑策略加入到無平滑策略的文獻(xiàn)［16］算法中，結(jié)果顯示能使文獻(xiàn)［16］算法的路徑進(jìn)一步縮短，進(jìn)一步驗(yàn)證了本文算法平滑策略的有效性。本文引入初始信息素不平等分布策略，減少了路徑盲目性；引入偽隨機(jī)轉(zhuǎn)移策略，提高了收斂速度，得到的路徑長度均值與標(biāo)準(zhǔn)差和拐點(diǎn)的均值與標(biāo)準(zhǔn)差都較小。均值與標(biāo)準(zhǔn)差可以衡量在某一數(shù)值附近的波動(dòng)程度和穩(wěn)定性，其值越小代表越好，上述實(shí)驗(yàn)結(jié)果驗(yàn)證了本文算法改進(jìn)的可行性、有效性和優(yōu)越性。

4.3 50×50復(fù)雜環(huán)境的最優(yōu)路徑平滑效果對比

為進(jìn)一步驗(yàn)證本文算法也能適用于復(fù)雜環(huán)境，在50×50復(fù)雜環(huán)境進(jìn)行仿真。傳統(tǒng)算法和文獻(xiàn)［16］算法與本文算法的平滑前后最優(yōu)路徑圖和收斂曲線圖分別如圖6和圖7所示；上述三種算法仿真數(shù)據(jù)如表6所示。

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

圖6 50×50復(fù)雜環(huán)境下各算法平滑前后最優(yōu)路徑對比Fig.6 Comparison of optimal paths of each algorithm before and after smoothing in 50×50 complex environment

圖7 50×50復(fù)雜環(huán)境下各算法收斂曲線Fig.7 Convergence curve of each algorithm in 50×50 complex environment

由實(shí)驗(yàn)結(jié)果可知，三種算法都能找到各自的最短路徑，但本文算法所得出的路徑最短，且穩(wěn)定在71.639 6，仿真50次最短路徑出現(xiàn)50 次；而傳統(tǒng)算法和文獻(xiàn)［16］算法搜索不到最短路徑且最優(yōu)解各出現(xiàn)1 次，由于傳統(tǒng)算法盲目性大，啟發(fā)信息較弱，在路徑搜索時(shí)，拐點(diǎn)較多，有時(shí)出現(xiàn)回環(huán)交叉，遠(yuǎn)離目標(biāo)行走，導(dǎo)致路徑長度增加。文獻(xiàn)［16］算法的啟發(fā)函數(shù)加入當(dāng)前節(jié)點(diǎn)、下一節(jié)點(diǎn)和目標(biāo)點(diǎn)信息，得到的最短路徑比傳統(tǒng)算法的更短，但由于地圖過于復(fù)雜，非常有必要進(jìn)行初始信息素不平等分布，由于本文算法加入該策略，本文改進(jìn)算法的拐點(diǎn)數(shù)目最少，路徑搜索往往選擇距離起點(diǎn)和終點(diǎn)連線最近的柵格。在大型復(fù)雜地圖中，傳統(tǒng)算法缺陷充分暴露，而本文算法優(yōu)勢更加突出?？傊?，本文算法的平滑前后最短路徑、收斂速度、拐點(diǎn)數(shù)目和路徑搜索的目的性都優(yōu)于傳統(tǒng)算法和文獻(xiàn)［16］算法。

5 結(jié)語

在靜態(tài)的全局路徑規(guī)劃中，本文針對傳統(tǒng)蟻群算法的不足，提出了一種改進(jìn)的蟻群算法。該算法對初始信息素進(jìn)行不平等分布以降低盲目性；改進(jìn)啟發(fā)式函數(shù)，使其包含當(dāng)前節(jié)點(diǎn)、下一節(jié)點(diǎn)和目標(biāo)點(diǎn)的信息，以增加路徑搜索的目的性；采用偽隨機(jī)狀態(tài)轉(zhuǎn)移策略，減少了路徑選擇的盲目性，提高算法收斂速度以及減少拐點(diǎn)數(shù)目；動(dòng)態(tài)調(diào)整信息素?fù)]發(fā)系數(shù)和設(shè)置信息素濃度范圍，避免算法陷入早熟；應(yīng)用B 樣條平滑策略，在得到最優(yōu)解的基礎(chǔ)上，進(jìn)一步優(yōu)化最優(yōu)解?；谝陨细倪M(jìn)，本文算法能夠很好地適用于不同尺度和不同復(fù)雜程度的柵格地圖。