基于勢場-蟻群算法的平滑機器人路徑規劃

彭 湘，向鳳紅，毛劍琳

(昆明理工大學信息工程與自動化學院，云南，昆明 650500)

1 引言

隨著機器人智能化的不斷發展，移動機器人逐漸從最初僅應用于工業產業到如今廣泛應用在教育、娛樂、醫療等環境中[1]。因此人們對機器人的性能要求也越來越高，機器人在環境中的行走精確度要求也逐漸提高。移動機器人路徑規劃是指機器人在已知或未知環境中，規劃出一條從起始點到目標點的無碰撞路徑[2-3]，此路徑可以存在一個或多個約束條件，例如時間最優，路徑最優或次優等。路徑規劃主要分為全局路徑規劃和局部路徑規劃[3]，全局路徑規劃主要針對靜態環境，環境中障礙物是靜止的且位置確定；局部路徑規劃主要靠機器人攜帶的傳感器獲取周圍環境信息，主要應用于障礙物不斷變化的動態環境中。

針對移動機器人的路徑規劃，國內外學者提出了一系列方法[5]，常用的包括一些傳統算法，如Dijkstra算法、模擬退火算法，以及近年來新興的智能仿生算法[6]，如遺傳算法、蜂群算法、魚群算法等。不同的算法均根據不同的性能指標有著不同的優缺點。蟻群算法最早是由Marco Dorigo等人于1991年提出[7]，是一種通過模擬螞蟻的覓食行為，在給定空間進行路徑規劃的智能仿生算法，因其具有良好的魯棒性、正反饋性以及易與其它算法結合的優點，廣泛應用于機器人的路徑規劃中。文獻[8]針對復雜環境下的路徑規劃，改進了傳統蟻群算法，但規劃的最優路徑存在尖峰，算法的全局能力也有待提高；文獻[9]利用聚類算法對環境的識別能力，提出了一種自適應搜索半徑蟻群路徑規劃算法，解決了蟻群全局收斂速度較慢問題，但算法并未考慮蟻群搜索時陷入局部最優的情況；文獻[10]對柵格環境下機器人的規劃路徑進行平滑處理，減少了路徑的轉折次數和轉折角度，但文中平滑方法并未達到完全消除路徑拐角的作用，優化后的路徑仍然存在拐角。

鑒于以上特點，本文提出一種勢場-蟻群優化(Improve Ant Colony Optimization with Artificial Potential Field，IACO-APF)平滑機器人路徑規劃方法，主要思想是：考慮機器人所受的勢場合力和下一節點到目標點的距離，綜合構造啟發信息，不僅提高了算法的全局性和收斂速度，多種因素共同作用還能避免蟻群陷入“死鎖”；將信息素限制在一定范圍內，防止機器人在搜索過程中因信息素過高陷入局部極小值；最后引入三次B樣條曲線對最優路徑做平滑處理，消除了路徑拐角，機器人的行走也更加流暢。

2 建模

2.1 問題假設

為使移動機器人更精準、有效地規劃出一條符合實際工作環境的路徑，這里對機器人的工作環境作如下假設：①機器人僅靜止和勻速運動兩種狀態，且可在兩種狀態間自由切換；②機器人自身攜帶傳感器以獲取周圍環境信息，機器人在工作時間內均能檢測到附近障礙物位置；③忽略環境中除障礙物以外的力對機器人的影響；④機器人工作空間為二維有限空間。

2.2 環境建模

首先建立移動機器人的路徑規劃模型。假設機器人工作空間是一個二維平面，已知障礙物靜止且位置確定，為了更好的模擬實際工作環境，本文采用柵格法[11]來劃分移動機器人的工作環境。柵格法是機器人建模中的常用方法，但柵格法只能近似的模擬機器人的工作環境，因此這里采用膨脹法對障礙物進行處理，即對柵格內不足一個柵格的障礙物按一個柵格處理。

圖1 膨脹法處理障礙物

其中白色柵格表示機器人可自由通過的區域，黑色柵格表示障礙物，機器人無法通過此區域。柵格大小為R。以柵格圖左下角為原點建立笛卡爾坐標系，從左至右為x軸，從下至上為y軸，對建立的柵格圖從左至右，從上至下編碼，依次為1、2、3…..。

圖2 柵格圖編碼

3 融合算法

單一算法本身往往存在各種不同的缺陷，而算法之間根據算法特性互相融合可以彌補算法的缺陷，得到更優的效果。蟻群在覓食時的搜索機制與移動機器人進行路徑規劃的相似性使得蟻群算法在機器人路徑規劃中有較好的應用，但傳統蟻群算法在前期搜索時由于信息素濃度較低，啟發信息僅受下一可能前往的節點作用等因素，導致搜索存在“盲目性”，收斂速度較慢，并且容易陷入“死鎖”，從而停滯搜索。在規劃過程中，隨著迭代次數增加，信息素濃度逐漸升高，蟻群又極易出現“早熟”現象。因此本文提出勢場-蟻群算法，利用人工勢場力的作用，使得蟻群在搜索前期，主要靠加入勢場合力的啟發信息作用，能夠加快蟻群前期的搜索速度，并在信息素更新時限定信息素范圍，避免蟻群因過快收斂而陷入局部最優，提高路徑規劃的效率。

3.1 人工勢場法

根據人工勢場法的思想，移動機器人在工作環境中受虛擬勢場的影響，機器人受到目標點引力和障礙物斥力的作用，從而規劃出一條無碰撞的路徑。在任一位置處，機器人受到的引力勢場和斥力勢場的和為

Usum=Uatt+Urep

(1)

其中，引力勢場表示為

(2)

其中：ξ是尺度因子，d2(P，G)表示物體當前位置Pi到目標點G的距離。對引力勢場求導即可得到引力：

Fatt=-ΔUatt(P)=ξd(P，G)

(3)

斥力勢場表示為

(4)

其中：d表示移動機器人到障礙物的距離，d0表示障礙物斥力場的作用范圍。同理，斥力表示為

Frep(P)=-?Urep(P)

(5)

機器人受到的勢場合力表示為

Fsum=Fatt+Frep

(6)

圖3 人工勢場受力圖

3.2 改進勢場-蟻群算法

3.2.1 構造啟發信息函數

傳統蟻群算法啟發信息函數表示為：

(7)

由于傳統蟻群算法在構造啟發信息函數僅考慮螞蟻當前節點i到下一節點j的距離dij，導致螞蟻的搜索全局性大大下降，故本文從算法的全局性出發，在啟發信息函數中考慮人工勢場合力、目標點在全局產生的引力勢場對機器人產生的吸引力，以及下一節點j與目標點G間的距離djG，綜合構造新的啟發信息函數為

(8)

3.2.2 信息素更新

蟻群在完成一次迭代，所有的螞蟻都到達目標點以后，需要對全局信息素進行更新。為了避免螞蟻在搜索過程中因為某路徑信息素過高而陷入搜索停滯狀態，本文受最大最小螞蟻系統(MMAS)[12-13]啟發，在信息素更新時，將信息素限定在一定范圍，用公式表示為

(9)

其中：τmin為規定最小信息素濃度，τmax為規定最大信息素濃度。當信息素小于最小信息時速濃度或大于最大信息素濃度時，信息素就會被按照式(9)重置。

4 平滑路徑規劃實現

由于柵格法的局限性，移動機器人只能從一個柵格的中心點移向另一個柵格的中心點，因此規劃出的最優路徑往往是一條帶有尖峰的不平滑路徑，若存在路徑拐角過大或拐角過多帶來的外力作用可能會造成機器人不必要的能量損失，不符合實際環境對機器人的性能要求。為了更好的模擬實際環境，得到一條平滑且優于最優路徑的結果，本文引入三次B樣條曲線對最優路徑拐點尖峰作平滑處理。

4.1 三次B樣條曲線

B樣條曲線具有可微性、凸包含性以及局部可修改性等特點，能夠滿足曲線節點間的狀態約束[14]。其中最廣泛使用的是三次B樣條曲線，三次B樣條曲線在節點具有二階連續性，滿足機器人在運動時速度與加速度連續，且曲線落在控制點所形成的三角形內[15]，因此可將三次B樣條曲線用于路徑規劃。三次B樣條曲線特性如圖4所示。

圖4 三次B樣條曲線特性

由n+1個控制點Pi構成的k次B樣條曲線方程的數學表達式為：

(10)

其中：Ni，k(u)表示第i個k階的B樣條基函數，表示為

(11)

當k為3時，此時即為三次B樣條曲線，基函數表示為

(12)

因此，三次B樣條曲線方程為：

0≤u≤1

(13)

4.2 路徑平滑實現

三次B樣條曲線平滑路徑流程圖如圖5所示。

圖5 三次B樣條曲線平滑路徑

5 仿真

5.1 算法步驟

Step1：柵格法建模，設置移動機器人的起始點S、目標點G；初始化勢場-蟻群算法各參數；設置三次B樣條曲線參數；

Step3：判斷螞蟻是否到達目標點G：若是，則一次迭代結束，否則，繼續搜索；

Step4：完成一次迭代后，更新全局信息素；

Step5：判斷當前迭代次數Nc是否達到最大迭代次數Ncmax，若是，則輸出最優路徑，否則，轉到Step2，直到Nc=Ncmax；

Step6：對輸出的最優路徑作平滑處理，輸出最終優化結果。

5.2 仿真及分析

本文采用MATLAB 2014a為仿真工具，移動機器人工作環境設置為mxn的柵格地圖，起始點柵格序號為1，終止點柵格序號為mxn。設定仿真參數為：Ncmax=200次，螞蟻個數m=50只，信息素因子α=1，啟發因子β=7，信息素揮發系數ρ=0.3，信息素增強系數Q=1。為了驗證算法的可行性，首先在10x10的柵格地圖環境下進行仿真，仿真結果如圖6所示。

圖6 勢場-蟻群算法(10x10)

可以看到，在仿真結果圖6中，算法在10x10柵格環境下運行成功且找到一條從起始點到目標點的有效路徑，路徑長度為14.7279，驗證了本文提出的算法是有效可行的。

為了進一步驗證算法的性能，實驗繼續在20x20的柵格環境下對比傳統蟻群算法和本文提出的勢場-蟻群算法。仿真結果如圖7、圖8所示。

圖7 傳統蟻群算法(20x20)

圖8 勢場-蟻群算法(20x20)

對比圖7、8仿真結果，傳統蟻群算法收斂圖曲線在找到最優路徑后仍舊出現了幾次波動，此后才趨于平穩，最終迭代28次后進入收斂狀態，而改進勢場-蟻群算法僅用6次迭代就已收斂，迭代次數上減少了78.6%。在路徑長度方面，傳統蟻群算法路徑較改進勢場-蟻群算法長1.1715，并且傳統蟻群算法規劃的路徑拐點較多，假設將此應用在實際環境中，拐點過多會導致機器人在行走過程中耗能較多，而本文算法規劃的路徑拐點數僅為4，較傳統蟻群減少了69.2%。仿真結果表明，改進勢場-蟻群算法在搜索效率和收斂速度上較傳統蟻群算法都有較大改進，同時改進勢場-蟻群算法規劃出的路徑更短，路徑拐點也大大減少。實驗在20x20柵格環境下的運行情況對比見表1。

表1 算法結果對比

針對算法規劃的最優路徑，采用三次B樣條曲線優化的最終結果如圖8所示，可以看出，針對拐點處的尖峰路徑，通過設置三次B樣條曲線的控制點，可以使平滑后的路徑成功落在控制點與拐點包圍的三角形內，并且路徑長度比原最優路徑減少了9.0603，對比文獻[16]使用的平滑方法，采用三次B樣條曲線處理路徑能夠完全消除路徑上任意拐點，達到100%平滑路徑的目的。

圖9 平滑優化結果

6 結束語

針對機器人在靜態環境下的全局路徑規劃，本文在傳統蟻群算法的基礎上提出勢場-蟻群融合算法。實驗結果表明，相較于傳統蟻群算法而言，勢場-蟻群算法在收斂速度、全局性等問題上都有較大的改進，同時算法還解決了傳統蟻群算法存在的易陷入“死鎖”和局部最優解的問題。在規劃出最優路徑后，采取三次B樣條曲線對路徑進行優化處理，完全平滑了路徑尖峰，還縮短了路徑長度。整個算法在優化后運行效率和運算結果都得到了大大的提升。在今后的后續工作中，將針對參數變化和動態環境對規劃過程帶來的影響等問題做進一步研究。