一種改進工業自動導引車路徑規劃算法

鄭 亮， 孫龍龍， 陳 雙

(1.哈爾濱工業大學蕪湖機器人產業技術研究院， 蕪湖 241000； 2.蕪湖哈特機器人產業技術研究院有限公司， 蕪湖 241000)

路徑規劃問題是移動機器人領域中的一個熱點問題，是指在移動機器人工作環境中，依據距離最短、時間最優、能效最高等一條或多條評價指標，規劃出一條從起始位置到目標位置的安全無碰撞路徑[1-2]。高質量的路徑規劃是確保移動機器人完成既定任務的關鍵所在[3]。

移動機器人路徑規劃的核心是路徑規劃算法，而構建移動機器人運行環境模型是路徑規劃算法的前提[4]。目前較為常用的環境建模方法包括柵格法[5]、可視圖法[6]、Voronoi圖法[7]等。其中，柵格法由于其建模簡單，構建的環境柵格地圖對不規則障礙物的表示能力較強，因而被廣泛應用于多種傳統或智能路徑規劃算法中。常用的基于柵格地圖的路徑規劃算法主要有：Dijkstra算法[8]、A*算法[9]及D*算法[10]等。相比于其他柵格地圖下的路徑規劃算法，A*算法計算量相對較小、路徑搜索時間較短且規劃路徑相對較優[11]。

A*算法是一種啟發式路徑規劃算法，一種在靜態網絡中求解最優路徑的有效搜索算法[12]。然而，傳統A*算法規劃路徑中存在折線多、累積轉角大且距離障礙物近等問題。因此，文獻[13]提出了一種平滑A*算法，該算法在傳統A*算法規劃路徑的基礎上，通過遍歷所有路徑節點，并刪除路徑節點間的冗余節點，以建立平滑A*模型。文獻[14]提出按較小的分割步長對傳統A*算法規劃路徑進行分割，在得到一系列路徑節點后，通過判斷節點間連線是否經過障礙物，以清除其中的冗余節點。然而，以上算法僅針對A*算法原有路徑節點進行處理，對應路徑長度和轉折角度減小幅度有限，并且未能解決算法規劃路徑距離障礙物較近的問題。

為此，通過引入Floyd算法生成路徑節點集合，同時基于A*算法對相鄰節點進行路徑規劃，并將生成路徑進行拼接，以解決傳統A*算法規劃路徑距離障礙物較近的問題，同時，針對傳統A*算法轉角較多、彎曲度較大的問題，引入貝塞爾曲線對拼接路徑進行平滑處理，以獲取全局路徑。

1 A*路徑規劃算法

A*算法是一種典型的啟發式搜索算法，基于A*算法實現AGV路徑規劃的過程通常可分為兩部分：首先，引入柵格法對自動導引車(automated guided vehicle,AGV)實際運行環境進行建模；其次，基于已構建環境柵格地圖模型確定AGV起始點和目標點，并引入A*算法實現路徑規劃。

1.1 環境柵格地圖構建

AGV運行環境柵格地圖構建是A*算法實現路徑規劃的重要前提，其基本思想是通過將有限的規劃空間劃分為若干個大小相同并賦有二進制數的柵格單元[15]，劃分后的工作空間如圖1所示。其中，柵格信息直接對應環境地圖信息，白色柵格表示可通行自由區域，灰色柵格表示障礙物區域。當AGV處于柵格地圖中任意柵格位置時，對應下一移動位置為該柵格相鄰8個柵格。

圖1 柵格地圖模型Fig.1 Grid map model

假設AGV二維工作空間長為L，寬為W，通過將此工作空間劃分為大小相同的R×S個柵格，若以Ω表示二維工作空間全體柵格信息，則有

Ω=∑Nij,i∈[1,R],j∈[1,S]

(1)

式(1)中：Nij表示柵格地圖中單個柵格信息：若Nij=0，則該柵格對應可通行自由區域；若Nij=1，則該柵格對應為障礙物區域；R表示橫向柵格個數，S表示縱向柵格個數。

1.2 路徑規劃

基于已構建的環境柵格地圖模型，在確定算法起始點和目標點后，A*算法即從起始點出發，通過評價函數確定算法搜索方向，同時對擴展的每個節點進行檢測，并選取其中代價最小的節點作為下一擴展節點，循環重復以上過程直至搜索到目標點，從而生成最終路徑。由于路徑上每個節點都是具有最小代價值的節點，因而A*最終生成路徑整體代價最小。A*算法的評價函數f(n)可表示為

f(n)=g(n)+h(n)

(2)

式(2)中：f(n)表示從起始點經由任意節點n到達目標點的評價值；g(n)表示在狀態空間中從起始點到節點n的實際代價值；h(n)表示從節點n到目標點的啟發式估計代價值。

采用歐幾里得距離作為啟發式函數，若以(xn,yn)表示節點n的坐標，(xd,yd)表示目標點d的坐標，則有

(3)

A*算法在朝向目標點搜索的過程中，將對搜索方向附近的相關節點進行評價。在抵達狀態空間某一節點時，A*算法將其周圍節點添加至OpenList列表，并從中選取代價值最小的節點作為下一擴展節點，將其添加到ClosedList列表后，重復執行以上過程，直至目標點添加至OpenList列表中。

A*算法路徑搜索過程如圖2所示。通過啟發式評價函數指引搜索方向，A*算法能夠快速搜索到從起始點到目標點的最優路徑，但該路徑存在轉角較大，并且距離障礙物較近等問題。

綠色柵格表示算法起始點；紅色柵格表示算法目標點； 黃色折線為A*算法最終搜索路徑圖2 A*算法路徑規劃結果Fig.2 Path planning results of A* algorithm

2 改進路徑規劃算法

基于已構建的AGV運行環境柵格地圖確定算法起始點rs和目標點re后，首先通過在柵格地圖中設置路徑關鍵節點，同時引入Floyd算法進行最短路徑規劃，并輸出路徑節點集合；其次，將起始點rs和目標點re計入路徑節點集合，并基于A*算法對集合中相鄰節點進行路徑規劃，同時對生成路徑進行拼接；最后，引入貝塞爾曲線對拼接路徑進行平滑處理，以生成最終路徑。

2.1 Floyd最短路徑算法

Floyd算法是典型的多源最短路徑算法，又稱為插點法，是一種用于尋找給定加權圖中多源點之間最短路徑的算法[16]。算法基本思想為：直接在圖的帶權鄰接矩陣中以插入節點的方式依次構造v個矩陣D(1),D(2),…,D(v)，最終生成的矩陣D(v)即為圖的距離矩陣，同時求取插入點矩陣R(1),R(2),…,R(v)以獲得兩點間的最短路徑。

(4)

(5)

通過不斷迭代插入節點vk，即可求得最短距離矩陣D(n)和最短路徑矩陣R(n)，其中R(n)保存了任意兩個節點間最短路徑，即插入點。因而，通過對最短路徑矩陣進行回溯，即可得到從節點vi到節點vj的路徑節點集合。假設基于路徑關鍵節點集合v1,v2,…,vn，生成最短路徑節點集合為r1,r2,…,rm。

2.2 貝塞爾曲線

通過對最短路徑節點集合r1,r2,…,rm進行處理，并將起始點rs和目標點re計入路徑節點集合后，引入A*算法對集合中相鄰節點進行路徑規劃，并對生成路徑進行拼接。然而，柵格地圖下的A*算法拼接路徑仍存在折線較多、轉角較大的問題，因而AGV實際運行效率相對較低，亟需對拼接路徑進行平滑處理。本文通過引入貝塞爾曲線以實現對A*算法拼接路徑的平滑處理。

貝塞爾曲線是由伯恩斯坦多項式發展而來，由于其控制簡單、圖形描述能力較強，且位置點連接平滑，因而在工業上得到了較為廣泛的應用[17]。假設P(0)和P(1)分別為貝塞爾曲線起點和終點，P(u)表示貝塞爾曲線運動控制點，則n次貝塞爾曲線可表示為

(6)

式(6)中：u表示獨立變量；P(i)表示位置點，對應所有位置點P(i)構成了一個特征多邊形；Bi,n(u)為n次伯恩斯坦多項式，滿足：

(7)

式(7)中：i∈{0,1,…,n}。

對n次貝塞爾曲線表達式進行求導，可得

(8)

貝塞爾曲線具有以下性質：①起始于P(0)，終止于P(1)；②曲線起點和終點即為特征多邊形的起點和終點，且曲線起點和終點的切線與特征多邊形的起始線和結尾線方向一致；③對應特征多邊形一旦確定，貝塞爾曲線的形狀也唯一確定，其不依賴于選取的參考系。

2.3 改進路徑規劃算法步驟

步驟1基于柵格法構建AGV運行環境柵格地圖模型，并確定算法起始點rs，目標點re。

步驟4通過對Floyd算法生成的最短路徑矩陣R={R(1),R(2),…,R(n)}進行回溯，即可獲取任意從節點vk到節點vj的最短路徑節點集合，假設為r1,r2，…,rm。

步驟5將算法起始點rs和目標點re計入節點集合中，處理后得rs、r1,r2，…,rm、re。

步驟6基于A*算法對集合rs、r1,r2，…,rm、re中相鄰節點進行路徑規劃，并對規劃路徑進行拼接處理。

步驟7引入貝塞爾曲線對A*算法拼接路徑進行平滑處理。改進路徑規劃算法流程圖如圖3所示。

圖3 改進路徑規劃算法流程圖Fig.3 The flow chart of the improved path planning algorithm

3 實驗與分析

為了驗證本文算法的性能，選取兩種不同的實驗環境，并與傳統A*算法進行了對比實驗，對應的環境柵格地圖分別如圖4、圖5所示。

圖4 長廊環境柵格地圖Fig.4 The grid map ofthe promenade environment

圖5 室內環境柵格地圖Fig.5 The grid map of the indoor environment

圖4為建立的長廊環境對應柵格地圖，其中灰線部分表示環境障礙物輪廓，空白部分對應為當前環境中空閑區域；圖5表示存在較多障礙物的室內環境柵格地圖，將以此驗證本文算法的路徑規劃效果。運行程序電腦配置為：CPU為i5處理器，8 GB內存，主頻2.5 GHz，Windows 10操作系統。

3.1 算法性能評估

由于在實際運行過程中，應當充分考慮AGV的尺寸以免AGV碰撞環境中的障礙物，因此首先根據相應的膨脹半徑對柵格地圖中障礙物所處的柵格進行膨脹處理，其中長廊環境對應的膨脹地圖如圖6所示。

圖6 長廊環境柵格膨脹地圖Fig.6 The inflated grid map of the promenade environment

為了驗證本文算法的相關性能，通過在長廊環境膨脹地圖中設置起始點S和目標點E，并將本文算法與傳統A*算法以及Floyd+A*算法進行路徑搜索對比實驗，其中傳統A*算法搜索路徑如圖7(a)所示，Floyd+A*算法生成路徑如圖7(b)所示，本文算法搜索路徑如圖7(c)所示。

圖7 不同算法搜索路徑Fig.7 The research path of different algorithms

如圖7(a)所示，基于給定起始點S和目標點E，傳統A*算法搜索生成的路徑在轉彎處彎曲度較大，且距離障礙物較近，同時該路徑經過柵格個數為1 121。而基于Floyd算法進行最短路徑規劃，生成路徑節點集合，并利用A*算法對處理后的路徑節點集合進行路徑規劃，最終生成的路徑如圖7(b)所示，該路徑經過柵格個數為1 016，有效減少了AGV運行時間，且規劃的路徑能有效避開障礙物，改進了傳統A*算法中“貼墻走”的問題，但仍存在路徑轉折點彎曲度較高，AGV實際運行效率較差的問題。而本文改進算法通過引入貝塞爾曲線進行平滑處理，最終生成的平滑路徑在轉折點處彎曲度較小，更加符合AGV的運行規則，AGV實際運行效率更高。

3.2 室內環境改進算法路徑規劃分析

根據設置的膨脹半徑對圖5室內環境柵格地圖中障礙物所處的柵格進行相應膨脹處理，并基于環境中給定的起始點S和目標點E，運用傳統A*算法進行路徑搜索，最終搜索到的路徑如圖8(a)所示。基于相同起始點S和目標點E，本文算法規劃的路徑如圖8(b)所示。

圖8 室內環境下傳統A*算法、本文算法搜索路徑Fig.8 The research path of the traditional A* algorithm and the proposed algorithm under the indoor environment

如圖8(b)所示，存在較多障礙物的室內環境中，基于給定的起始點S和目標點E，本文算法能夠完全避開環境中的障礙物，并規劃出合理的路徑，且生成的路徑較為平滑，同時在轉折點處彎曲度更小，更加符合AGV運行規則，AGV實際運行效率更高。

3.3 搜索時間分析

為了驗證改進路徑規劃算法的路徑搜索時間效率，針對兩種不同環境進行了5次路徑搜索實驗，對應的搜索時間如表1所示。

表1 各種路徑規劃算法路徑搜索時間比較

如表1所示，針對兩種不同環境，在給定起始點S和目標點E后，相比于A*算法，Floyd+A*算法路徑搜索時間相對較短，路徑搜索較快。然而，由于本文算法增添了貝塞爾路徑平滑處理，因而相對于Floyd+A*算法，本文算法對應的路徑搜索時間相對較長，但相比A*算法，本文方法路徑搜索時間整體相對較短，路徑搜索較快。

4 結論

在A*算法的基礎上，提出了一種改進路徑規劃算法。該算法通過在全局地圖中設置路徑關鍵節點，繼而引入Floyd算法進行最短路徑規劃，并輸出路徑節點集合，同時基于A*算法對集合中相鄰節點進行路徑搜索，并對生成路徑進行拼接，從而解決A*算法規劃路徑距離障礙物較近的問題。其次，針對傳統A*算法轉角較多、彎曲度較大的問題，引入貝塞爾曲線對拼接路徑進行平滑處理，以獲取全局平滑路徑。實驗結果表明本文算法規劃的路徑轉彎更少、彎曲度更小、搜索時間更短且能完全避開障礙物行走，更加符合工業AGV的應用環境。