基于初始路徑封閉區域的機器人路徑規劃算法

蔣 美 云

(南京工業職業技術學院 江蘇 南京 210023)

0 引 言

移動機器人是一個集環境感知、動態決策與規劃、行為控制與執行等多功能于一體的綜合系統，是目前科學技術發展最活躍的領域之一，其不僅在工業、農業、服務業等行業中應用廣泛，而且在城市安全、國防和空間探測領域等有害與危險場合得到很好的應用。作為相對獨立工作的個體，其具有能量有限、時效性要求較高、工作環境特殊等特點，而有效的路徑規劃算法對于提高機器人工作效率，降低機器人能量消耗，延長機器人工作時間等具有重要意義。機器人路徑規劃是機器人技術研究領域的熱點及難點問題，旨在解決機器人如何在工作環境中安全到達目的地的問題，圍繞該問題國內外研究者開展了一系列研究。機器人路徑規劃過程主要是根據工作代價最小、路徑最短等準則，為機器人規劃出一條從起點到目的點的連續的、無碰撞的路徑[1]。當前，機器人路徑規劃步驟包含環境地圖構建及機器人路徑規劃。環境地圖構建主要有柵格地圖法[2]、可視圖法[3]、結構空間法[4]和拓撲圖法[5]。其中：柵格地圖法是一種十分有效的地圖描述方法，越來越受到研究者的重視，應用前景十分廣闊；可視圖法則將機器人作為工作環境中的一點，用直線將障礙物的頂點，起點與障礙物，障礙物與目標點連接起來，并保障直線不經過障礙物，但隨著障礙物數量的增加，可視圖中連線的數量也會成倍增加，導致路徑規劃效率大大下降。機器人路徑規劃方法主要有局部路徑規劃和全局路徑規劃法。局部規劃路徑方法屬于動態規劃，主要方法有遺傳算法、動態窗口法、模糊邏輯控制法和人工勢場法等[6]。A*算法[7]是全局路徑規劃方法的典型代表，由Dijkstra算法演變而來。A*算法用啟發函數評估待選路徑的優劣，啟發函數和評估函數的選取是該算法的難點。在A*算法的基礎上，又出現了LPA*[8]、D*Lite[9]等算法。隨著人工智能技術的普及應用，作為全局搜索算法的蟻群算法[10-11]在機器人路徑規劃的應用也越來越受到關注。然而，研究及應用結果表明，基于蟻群算法的機器人路徑規劃過程，往往需要較長的搜索時間，算法收斂速度較慢，易出現局部最優解等問題，除蟻群算法自身因素外，路徑搜索空間過大也是影響路徑搜索時間和算法收斂速度的主要因素之一。

本文為了降低路徑搜索空間大小，提高路徑搜索效率，在基于柵格地圖法對環境進行構建的基礎上，以起點和目的點間的連線為吸引路徑，采用精英螞蟻沿著靠近吸引路徑的方向探索出初始路徑封閉區域，后續螞蟻則在初始路徑封閉區域內搜索出最終路徑。本文算法有效降低了路徑搜索空間大小，能夠以更快的速度探索出有效路徑。

1 環境建模及相關定義

1.1 基于柵格法的環境建模

設定機器人工作的環境由m×n個大小相等的柵格構成，其中任一柵格單元可表示為Cxy，柵格地圖G可表示為:

G={Cxy|x∈[1,m],y∈[1,n]}

(1)

Cxy=0表示該柵格為無障礙物柵格區域，Cxy=1表示該柵格為存在障礙物柵格區域。此外，令G0為無障礙物柵格區域的集合，令G1為有障礙物柵格區域的集合，則柵格地圖G也可表示為:

G=G0∪G1

(2)

圖1為5×5的柵格地圖，無障礙物及存在障礙的柵格已在圖中標出。

圖1 5×5柵格地圖

1.2 相關定義

為便于進行后續算法描述，特給出如下定義：

定義1吸引路徑：機器人出發點S與目的點T間的連線ST。

定義2拐點：改變機器人前進方向的點。

定義3機器人當前位置：無障礙柵格的中心。

定義4機器人移動方向(見圖2)：機器人由當前無障礙柵格中心移動到下一無障礙柵格中心的方向，可為東、南、西、北、東北、東南、西南、西北八個方向。

圖2 機器人移動方向

定義5機器人單步移動距離：機器人從當前無障礙柵格中心移動到下一無障礙柵格中心所經過的路徑長度。

2 算法設計

2.1 算法思想

如上文所述，除蟻群算法自身因素外，路徑搜索空間過大也是導致算法執行效率較低的主要原因之一。在蟻群算法優化方面，已有眾多研究者提出有效的改進算法，本文則從降低路徑搜索空間大小的角度出發，基于平面上兩點間直線距離最短的原則，以起點和目的點間的連線為吸引路徑，從起點出發采用精英螞蟻分別在吸引路徑的兩側沿著靠近吸引路徑的方向朝目的點前進，而后通過吸引路徑兩側的精英螞蟻探索出初始路徑，構建起初始路徑封閉區域，有效降低后續螞蟻路徑搜索空間的大小，進而有效降低路徑搜索時間。

2.2 目標函數

設定機器人在柵格地圖中的起點為S(xs,ys)、目的點為T(xt,yt)，從當前位置Pi(x,y)到下一位置Pi+1(x,y)的距離為di(Pi→Pi+1)，機器人從起點S(xs,ys)經過N段路徑后到達目的點T(xt,yt)的路徑距離之和表示為：

(3)

且Pi(x,y)∈G0。定義目標函數F為：

(4)

由式(4)可知機器人路徑目標函數可描述為：從起始點S(xs,ys)到目的點T(xt,yt)為機器人找到一條最短且無碰撞的路徑。

2.3 初始路徑探索算法

算法主要步驟如下：

Step1以起點S(xs,ys)與目的點T(xt,yt)間的連線ST為吸引路徑。

Step2將精英螞蟻A和B置于起點S(xs,ys)，兩只精英螞蟻均靠近并沿吸引路徑方向向目的點T(xt,yt)前進。

Step3當遇到障礙柵格時，螞蟻A在吸引路徑的右側靠近并沿吸引路徑方向前進；螞蟻B則在吸引路徑的左側靠近并沿吸引路徑方向前進；螞蟻A和螞蟻B在前進過程中均記錄其路徑拐點PA→i(x,y)和PB→j(x,y)；根據吸引路徑的吸引效應積極向吸引路徑靠攏，并沿吸引路徑方向前進。

Step4螞蟻A或螞蟻B繼續靠近并沿吸引路徑方向朝目的點T(xt,yt)前進，遇到障礙柵格時，執行Step3。

Step5螞蟻A或螞蟻B未到達目的點時，重復執行Step3和Step4，直到螞蟻A和B均到達目的點，螞蟻A和B獲得的包含拐點的路徑分別表示為：

(5)

式中：GAi表示螞蟻A從起始點S出發遇到的第i個拐點且1≤i≤m；GBj表示螞蟻B從起始點S出發遇到的第j個拐點且1≤j≤n。

2.4 初始路徑平滑算法

采用2.3節算法得到的路徑雖保證了盡量靠近吸引路徑，但仍存在較多的拐點導致機器人付出較高的轉彎代價，且路徑長度也有待優化。下面以平滑螞蟻A的路徑PathA=(S,GA1,GA2,…,GAm,T)為例，為便于描述，不妨令出發點S為路徑上的第0個拐點即S=GA0，目的點T為路徑上的第m+1個拐點即T=GA(m+1)，則原始路徑可表示為：

PathA=(GA0,GA1,GA2,…,GAm,GA(m+1))

(6)

初始路徑平滑算法步驟如下：

Step1以路徑PathA的第一個拐點GA0為基準，分別構建GA0與其不相鄰拐點GAi間的路徑LA0Ai=d(GA0,GAi)，其中2≤i≤m+1。

Step2當存在拐點GAi滿足max(LA0Ai)且路徑LA0Ai上無障礙柵格時，將拐點GA0存入已優化路徑(GA0)，構建待優化路徑：

(7)

(8)

令精英螞蟻B的初始路徑為：

PathB=(S,GB1,GB2,…,GBn,T)

(9)

同樣在采用初始路徑平滑算法后，可得到精英螞蟻B的優化路徑：

(10)

結論1地圖中的點S和T間只存在一個障礙柵格區域時，其最短路徑包含在精英螞蟻A和螞蟻B初始路徑形成的封閉區域內。

證明：構建如圖3所示柵格地圖，根據2.3節中提出的算法，其中一只精英螞蟻的路徑可表示為：

Path0=(S,A,B,D,E,T)

(11)

圖3 證明圖例

采用2.4節中優化算法后，得到的優化路徑為：

Path1=(S,B,D,T)

(12)

此時，機器人所經過的路徑之和為：

L1=d(S,B)+d(B,D)+d(D,T)

(13)

如圖3所示，在拐點B處，機器人除沿拐點D的方向運動外，剩余可運動方向為水平方向，假定其沿水平方向運動到拐點C處，此時拐點C與目的點T間存在直接路徑，拐點C和T間不存在拐點使得拐點間路徑更短，則路徑可表示為：

Path2=(S,C,T)

(14)

機器人所經過的路徑之和為：

L2=d(S,C)+d(C,T)

(15)

假定每個柵格的寬度和長度均為1，則有：

(16)

顯然L1

綜上，采用優化算法后，可確保精英螞蟻所構建的初始路徑封閉區域外不存在更短的從起點至目的點的路徑。因此，最短路徑只能包含在初始路徑封閉區域內。

證畢。

2.5 最終路徑規劃

執行初始路徑及其優化算法后，由精英螞蟻A和B的路徑可獲得包含吸引路徑的封閉區域，不妨令機器人轉彎次數n為精英螞蟻A和B路徑拐點數的均值。當障礙柵格密集分布于封閉區域內時，可能會導致優化后的路徑中仍存在大量的拐點且最優路徑被封閉區域隔離，直接體現為機器人轉彎代價的增加。在此，給出基于機器人轉彎代價的最終路徑規劃方法：

(1) 當機器人轉彎代價α>t時，表明采用初始路徑優化算法后，路徑中仍存在較多拐點，導致機器人轉彎代價過大且路徑長度較長。因此，在全局范圍內，采用文獻[11]提出的改進蟻群算法進行路徑搜索，并進一步采用2.4節的路徑平滑算法對路徑進行優化。

(2) 當機器人轉彎代價α≤t時，表明障礙柵格在吸引路徑上分布較為稀疏。此時，將封閉區域以外的柵格統一標記為障礙柵格，而后續螞蟻的尋路過程也將在此封閉區域內進行。后續路徑搜索采用文獻[11]算法，在此不再贅述。當通過蟻群算法在初始區域中尋找到最優路徑后，進一步采用2.4節的路徑平滑算法對路徑進行優化。

特別地，轉彎代價門限值t可根據本文算法與傳統算法的對比實驗予以確定。

3 仿真實驗

3.1 相關設置

算法仿真過程在MATLAB環境中實現。為便于展示，首先構建15×15的柵格地圖，起點S及目的點T如圖4所示。同時，隨機生成50幅100×100的柵格地圖，根據本文算法與文獻[11]算法的對比結果，設定轉彎代價門限值為t=0.61，以確保采用本文算法與文獻[11]算法均能搜索到最優路徑。在相同的參數和環境下，對文獻[11]算法和本文算法進行了對比分析。

圖4 仿真實驗柵格地圖

3.2 初始路徑規劃算法仿真結果

采用本文提出的初始路徑探索算法后，獲得了如圖5所示的初始路徑，精英螞蟻A和B經過的初始路徑存在較多拐點，路徑有待平滑優化。精英螞蟻A和B經過的初始路徑長度分別為23.14、23.73，拐點個數分別為15、13。

圖5 初始路徑規劃算法執行結果

3.3 初始路徑規劃優化算法仿真結果

采用本文算法的初始路徑優化結果如圖6所示，精英螞蟻A和B平滑優化后的初始路徑長度分別為19.97、21.03，拐點數分別為6、8，平滑優化后的初始路徑長度減少了13.70%、11.38%，拐點數量減少了60%、38%。初始路徑封閉區域如圖7所示，其大小占柵格地圖的36%，這表明本文算法有效降低了路徑搜索空間的大小。

圖6 初始路徑平滑結果

圖7 初始路徑封閉區域

3.4 最終路徑規劃結果

最終路徑規劃過程中，由精英螞蟻A和B確定的機器人轉彎代價為t≈0.39，小于門限值t，故在初始路徑封閉區域內搜索最終路徑，其結果如圖8所示。最終路徑長度為19.01，較初始路徑規劃過程中精英螞蟻A平滑優化后的初始路徑長度19.97減少了0.96。圖9為本文算法與文獻[11]算法的收斂性對比結果。可以看出，迭代次數達到19次時，本文算法已找到最優路徑，耗時0.490 2 s。由于路徑搜索空間較大，文獻[11]算法在迭代次數達到40時，才能找到最優路徑，耗時1.966 2 s，約為本文算法的4倍。此外，在前面隨機生成的50幅100×100柵格地圖中，有48幅柵格地圖的最終路徑包含在初始路徑封閉區域內，而剩余2個柵格地圖的機器人轉彎代價均大于門限值0.61，也根據全局搜索算法確定了最終路徑。

圖8 初始路徑封閉區域內的最終路徑

4 結 語

針對柵格地圖環境中的機器人路徑規劃問題，從減少路徑搜索空間大小的角度出發，確定了基于初始路徑封閉區域的路徑規劃思想。為獲得初始路徑封閉區域，首先采用精英螞蟻沿起始點與目的點間的吸引路徑前進，再采用路徑平滑算法優化初始路徑探尋結果。最終路徑搜索過程中，根據機器人轉彎代價，決定后續螞蟻的路徑探尋區域是否限定在初始路徑封閉區域內。與傳統算法在仿真環境下的對比結果表明，在96%的情況下本文算法能夠在更短的時間內找到最終路徑。