B樣條曲線融合蟻群算法的機器人路徑規劃

李二超，齊款款

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

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

0 引言

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

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

1 環境建模

本文機器人工作環境為柵格地圖，具體如圖1所示。

圖1 柵格地圖Fig.1 Grid map

柵格法由柵格取值為二進制的0 和1 矩陣構成：0 代表自由柵格（白色柵格），機器人可自由移動；1 代表障礙柵格（黑色柵格），機器人需要繞行前進。地圖按照從左到右、從下到上的順序依次編號1、2、…，柵格序號與坐標一一對應，坐標與柵格編號的關系表達式如式（1），求得的結果為柵格的中心點。

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

為防止機器人與障礙物發生碰撞，當不規則障礙物不滿一個柵格時，將其填充為一個柵格，再將障礙物向外膨化，寬度為機器人的半徑，整體構成障礙物，此時把機器人看作質點來處理，假設膨化后的正方形邊長為1，即式（1）中的a=1。

機器人運動方向為8 個，去掉前一步走過的，只有7 個方向可以選擇。

2 傳統蟻群算法

2.1 路徑選擇概率

狀態轉移概率公式如式（2）：

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

其中：dij為當前節點i到下一節點j的歐氏距離。

2.2 信息素更新

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

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

3 改進蟻群算法

3.1 初始信息素不平等分布

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

其中：dSi表示起點與當前點的歐氏距離，djE表示下一節點與目標點的歐氏距離；C為信息素濃度常數；τ0為初始信息素。

3.2 改進啟發函數

本文的啟發函數是在文獻［16］中啟發函數的基礎上加入了動態調節因子（由最大迭代次數itermax和當前迭代次數iter構成）。在迭代前期，該調節因子促使啟發函數起主導作用；隨著迭代次數的增加，路徑上積累了一定量的信息素，此時調節因子會減弱啟發信息的引導作用，加強信息素的引導作用。

3.3 偽隨機狀態轉移策略

轉移概率公式如式（9）所示：

本文算法引入偽隨機狀態轉移策略，確定性概率用來減少路徑搜索的隨機性，同時也不能過于偏向于確定性概率而導致陷入局部最優值，因此引入任一性概率，如式（10）所示：

式中：rand為［0，1］區間的隨機數；q0、q1、q2由前人經驗以及反復實驗來確定的常數，范圍為（0，1）；randj為任意選擇下一可行節點。

3.4 改進揮發系數

由于蟻群算法的特殊性，在不同階段需要不同大小的揮發系數：如果ρ設置過大，螞蟻無法依靠信息素信息進行路徑搜索，導致收斂慢；如果ρ設置過小，信息素過度積累，則會使路徑搜索陷入局部最優。固定值的揮發系數無法動態調整，因此引入動態調整揮發系數如式（11）所示；同時，為防止算法陷入局部收斂，對信息素濃度進行限制，如式（12）所示。

式中：ρmin為揮發系數的最小值。

3.5 B樣條曲線平滑策略

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

由公式

可知，k=3時的B樣條曲線數學表達式為：

當三次B 樣條曲線各節點矢量間插值為常數時，為三次均勻B樣條曲線，第i段三次均勻B樣條曲線數學表達式為：

由式（13）、（15）、（16）可得三次均勻B 樣條曲線的基函數數學表達式：

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

將式（18）用矩陣形式表達為：

式（18）、（19）為三次均勻B樣條曲線數學表達式。

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

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

3.6 改進蟻群算法流程

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

圖3 改進蟻群算法流程Fig.3 Flowchart of improved ant colony algorithm

4 實驗仿真與分析

為驗證本文算法的可行性、有效性和優越性，在Matlab 2016a上進行仿真實驗。主要從以下幾個方面進行實驗驗證：1）對主要參數進行敏感性分析；2）在稍微復雜的柵格地圖環境中，在相同的參數條件下，為更方便驗證各個改進環節的可行性和有效性以及本文算法的優越性，在傳統算法上單獨添加改進的偽隨機轉移策略（方案1）、在方案1的基礎上加初始信息素不平等分布（方案2），在方案2的基礎上加改進啟發函數（方案3）和在方案3的基礎上加改進揮發系數（本文算法平滑前）以及在傳統算法上單獨添加平滑策略（傳統算法+平滑）進行對比分析，將整體改進方法（本文算法平滑前后）分別與傳統算法平滑前后和文獻［16］改進蟻群算法平滑前后進行仿真對比分析；3）在大型復雜柵格地圖環境下將本文算法與傳統算法和文獻［16］改進蟻群算法（使用文獻參數）進行對比分析，驗證本文算法的優點。

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

4.1 主要參數分析

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

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

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

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

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

4.2 20×20運行環境的最優路徑平滑效果對比

各方案的最優路徑圖如圖4 所示，本文算法與傳統算法和文獻［16］改進蟻群算法的最優路徑平滑前后圖如圖5 所示；上述各方案和三種算法仿真數據如表5所示。

表5 20×20運行環境下的仿真結果Tab.5 Simulation results in 20×20 running environment

圖4 20×20運行環境下不同方案的最優路徑對比Fig.6 Comparison of optimal paths of different schemes in 20×20 running environment

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

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

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

4.3 50×50復雜環境的最優路徑平滑效果對比

為進一步驗證本文算法也能適用于復雜環境，在50×50復雜環境進行仿真。傳統算法和文獻［16］算法與本文算法的平滑前后最優路徑圖和收斂曲線圖分別如圖6和圖7所示；上述三種算法仿真數據如表6所示。

表6 50×50復雜環境下三種算法的仿真結果Tab.6 Simulation results of three algorithms in 50×50 complex environment

圖6 50×50復雜環境下各算法平滑前后最優路徑對比Fig.6 Comparison of optimal paths of each algorithm before and after smoothing in 50×50 complex environment

圖7 50×50復雜環境下各算法收斂曲線Fig.7 Convergence curve of each algorithm in 50×50 complex environment

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

5 結語

在靜態的全局路徑規劃中，本文針對傳統蟻群算法的不足，提出了一種改進的蟻群算法。該算法對初始信息素進行不平等分布以降低盲目性；改進啟發式函數，使其包含當前節點、下一節點和目標點的信息，以增加路徑搜索的目的性；采用偽隨機狀態轉移策略，減少了路徑選擇的盲目性，提高算法收斂速度以及減少拐點數目；動態調整信息素揮發系數和設置信息素濃度范圍，避免算法陷入早熟；應用B 樣條平滑策略，在得到最優解的基礎上，進一步優化最優解。基于以上改進，本文算法能夠很好地適用于不同尺度和不同復雜程度的柵格地圖。