基于變網格A*算法的機器人駕駛路徑規劃

任榮倫,衛 沅,周志剛,李浩然

(河南科技大學,河南 洛陽 471000)

1 引言

在有放射性污染物、廢棄核電站等的危險工況下,人類無法進入進行救援工作。因此救災機器人一直是各國研究領域熱點,但是當遇到復雜的環境比如樓梯、不平坦路面的情況且路徑距離較遠時,移動機器人無法到達,此時人形機器人可以駕駛汽車進行救援工作[1]。在救援開始前,需要對路徑進行規劃,機器人才能駕駛汽車進行救援[2]。因此研究機器人駕駛路徑規劃具有重大理論價值與應用前景。根據規劃范圍不同,可將路徑規劃分為靜態的全局路徑規劃和動態的局部路徑規劃[3],常見的全局路徑規劃算算法有Dijkstra算法、A*算法[4,5]、RRT算法[6]、遺傳算法[7]等;常見的局部路徑規劃算法有動態窗口法(DWA)[8]、人工勢場法[9]等。

全局規劃中遺傳算法在搜索最短路徑容易陷入局部最優的問題[10];RRT算法規劃出的路徑不一定是平滑的,且搜索效率較低[6];Dijkstra算法采用遍歷搜索方式,搜索節點數較多,算法效率低下,難以快速滿足快速規劃路徑的需求[11]。A* 算法原理簡單、易于實現、效率高、速度快且能搜索到最優解。且A*算法在Dijkstra算法的基礎上,根據估計代價決定搜索方向,提高了搜索效率。但是傳統A*算法有拐點過多,路徑不夠平滑、與障礙物距離近等問題。陳嬌等[12]在文獻中對A*算法刪除了多余節點,保證了路徑全局最優。Boroujeni.Zahra等[13]在文獻中將A*算法用于結構化道路圖路徑規劃。遲旭等[14]在文獻中優化了搜索策略減少了三個搜索方向,提高了搜索效率。武義等[15]在文獻中將搜索領域擴大為48方向,提高了路徑的平滑性。ZhangJing 等[16]在文獻中提出結合人工勢場法新型啟發式函數,并將算法用于陸地車輛,使其更容易控制。

本文提出一種變網格的A*算法。首先,改進評價函數的計算方式和代價計算方式,從而降低搜索時間。其次,將傳統的A*算法的3×3搜索鄰域和7×7鄰域結合起來,根據障礙物自適應調整網絡的結構,在無障礙物時,采用3×3鄰域快速搜索,增大h(n)比重使函數f(n) 的啟發式信息增強,增加搜索效率;遇到障礙物時自適應調整為7×7鄰域搜索,將h(n)比重調節略小于g(n),使其保證搜索效率的同時也兼顧路徑平滑度。同時為了避免汽車轉彎時邊緣發生碰撞,對障礙物進行膨脹化處理,保證了與障礙物之間的安全距離。最后對規劃出的路徑進行提取關鍵節點和圓弧平滑處理,保證規劃的路徑足夠平滑。

2 A*算法

2.1 傳統A*算法

A*算法是一種在靜態地圖中解決路徑規劃問題比較優良的算法,它是根據Dijkstra算法改進的一種優化算法。作為一種典型的啟發式搜索算法,A*算法的特點是搜索子節點時會根據當前節點信息進行對比,即每次搜索總是選取最小位置作為子節點。選取子節點需要根據特定的代價函數,按照此方法執行,直到搜索到目標節點位置為止。A*算法的啟發式函數

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

(1)

其中:f(n)為經過當前節點n的全局評估代價值;g(n)為起始節點到當前節點n的真實代價值;h(n)為當前節點n到目標節點的代價估計。當評價函數中h(n)所占比重較小時,A*傾向于廣度優先搜索,可以求解出最短路徑,但是耗時較長。反之,h(n)比重較大時,A*算法搜索啟發性更強,搜索時間較快,但規劃的路徑不一定是最優的,因此合理分配二者的比例十分重要。

2.2 變網格A*算法

2.2.1 變換網格

傳統A*算法主要有8個搜索方向,8個箭頭表示機器人在移動過程中的8個搜索方向,機器人的最小轉角為90°。武義等將A*算法進行改進,由8領域搜索方式改為48領域搜索方式,機器人的搜索方向變為32個方向,移動機器人的最小轉角為11.25°,提高了路徑平滑度。但是由于采用7×7鄰域搜索,搜索節點增多,大大增加了搜索時間。

本文提出了一種變網格的A*算法,將兩者結合起來,在周圍沒有障礙物時采用3×3搜索鄰域快速搜索,遇到障礙物時自適應為7×7搜索鄰域,搜索出拐點少,更加平滑的路徑,通過障礙物時再采用3×3搜索鄰域快速搜索。具體算法流程如下:

1)程序開始,創建OPEN表與CLOSE表,將起點放入CLOSE表中。

2)搜索周圍8個子節點作為下一預備子節點放入OPEN表中,根據評價函數計算代價最小的節點放入CLOSE表中作為子節點,繼續搜尋下一節點。

3)若周圍8個子節點出現障礙物,則打斷搜索8個子節點的函數,進入搜索48鄰域的搜索函數,將上一父節點作為父節點,搜索周圍48個節點作為預備節點,放入OPEN表中,根據評價函數計算代價最小的節點放入CLOSE表中作為子節點繼續搜索。否則執行2)。

4)判斷CLOSE中新的子節點是否為終點,若是程序結束,否則返回2)。

5)已找到路徑,程序結束。

2.2.2 改進評價函數

2.2.2.1 改進評價函數計算方式

傳統A*算法計算代價的方式常采用歐式距離,即兩個節點間的距離。所以2.1節中函數g(n)和h(n)分別為

(2)

(3)

其中g(n)是指起點到當前節點的代價,本文做出改進,借用上一節點的g(n)值加上一節點到當前節點的代價來表示。具體計算如下

(4)

由于父節點Pn-1的g(n-1)是已知的,所以只需計算父節點到當前節點的代價m(n),計算m(n)運算量是一定小于g(n)的計算量。因此,在規劃中,此舉可節省大量計算時間。具體計算方式如下

(5)

因此,新的起始節點到當前節點n的真實代價值為

G(n)=g(n-1)+m(n)

(6)

h(n)是指當前節點到目標點的代價,本文不采用常用的歐式距離,而是采用曼哈頓距離,曼哈頓距離是指兩點之間橫、縱坐標差的絕對值之和。因此歐式距離的值是大于實際距離的,即評價函數中的h(n)的比重是較大的,搜索啟發性更強,搜索時間較快。對于汽車來說,顯然是更容易接受的。具體計算方式如下

H(n)=|xg-xi|+|yg-yi|

(7)

2.2.2.2 變換函數評價權重比例

由2.1可知在評價函數中g(n)比h(n)比重大時會導致評價函數f(n) 的啟發式信息比較弱,可生成更加優化的路徑,即與2.2.1小節的擴大搜索鄰域相一致。但這些直接導致路徑搜索工作量的增加,所以當上文變化網格時,由于擴大搜索鄰域使搜索時間變長,此時略微調小h(n)比例,使路徑進一步優化的同時保證時搜索效率。由于g(n)比h(n)比重小時,評價函數f(n)的啟發式信息強烈,盡管減少了路徑搜索的工作量,但規劃的路徑一般不是最佳,但是在沒有障礙物時變換網格時采用3×3搜索鄰域,且調整g(n)比例遠小于h(n),快速搜索找尋路徑。

sin2θ+cos2θ=1

(8)

本文構建的兩權重系數如式(8)所示,在式(8)中θ為自變量,取值范圍為[0,pi/2],兩權重之和為1,當θ從0到pi/2增大時,第一項權重會逐漸增大,第二項權重會逐漸減小,二者成反比關系。將權重系數引入A*算法評價函數中,可調節g(n)和h(n)比例。

F(n)=sin2θ×G(n)+cos2θ×H(n)

(9)

當2.2.1小節中A*算法變換網格時,同時給出信號對評價函數的權重比例也加以變換。當以3×3搜索鄰域,快速搜索時,此時將自變量設為較小值。當以7×7搜索鄰域,搜索更加優良路徑時,將自變量設為略小值,保證路徑平滑的同時也保證搜索效率。最后經過加權求平均處理取得合適值,使在有障礙物時,搜索較優路徑,無障礙物時快速搜索路徑,到達終點。

3 路徑優化

3.1 提取關鍵節點

針對A*算法有多余的節點問題,采取提取關鍵節點算法,刪除冗余點處理,使路徑更短,轉折點更少,具體步驟如下:

1)將變網格的A*算法規劃路徑的所有節點放入一個集合P{P1,P2,P3,…,Pn(n為所有節點個數)},P1是路徑規劃的起點,Pn是路徑規劃的終點。

2)創建一個集合U,將P1,Pn放入集合U。

3)從P1開始作直線連接P2,判斷直線P1P2是否經過障礙物,若沒有經過障礙物,連接P1P3,判斷是否經過障礙物,若經過障礙物,則P2為關鍵節點,放入集合U,連接P2P4,以此類推,直到遍布所有節點。

4)依次連接集合U中所有節點,完成對關鍵節點的提取。

3.2 可調節圓弧優化

由于受汽車本身結構問題限制,當汽車轉彎半徑過小時,汽車是無法轉彎的。所以對路徑進行圓弧優化處理時,當轉彎半徑較小時,汽車是無法通過的,因此圓弧半徑的選取十分重要。

本文選取汽車最小轉彎半徑作為圓弧優化算法的半徑,就可以避免無法轉彎問題。由于不同的汽車最小轉彎半徑不同,且汽車最小轉彎半徑與汽車寬度無關,與車長和汽車最大轉角有關。所以根據不同車型選取不同的半徑,具體計算如下

(10)

其中R為汽車最小轉彎半徑,L為車長,Ψ為汽車方向最大轉角。

由于A*算法所規劃的路徑轉折處不夠平滑,機器人駕駛汽車進行行駛時,路徑不夠平滑。因此,需要對所規劃的路徑進行平滑處理,用圓弧代替轉折點,其原理如圖3所示:

4 仿真研究

4.1 環境建模與膨脹化處理

4.1.1 環境建模

柵格法是一種經典的環境建模方法,是A*算法常用的建模方法。其主要原理是將機器人的工作環境模擬為相同大小且相互連接的柵格,每個柵格對應相應位置信息。

如圖4所示,建立一個30×30的地圖仿真柵格圖。每個柵格長度為汽車車長的正方形,障礙物不滿一格按照一格計算。紅色為起點,綠色為終點,黑色為障礙物,白色柵格表示可以通行的區域。

4.1.2 膨脹化處理

由于在仿真環境中將機器人駕駛的汽車理想化成一格,轉彎時可能會在邊緣和障礙物發生碰撞,所以在構建柵格地圖時將障礙物進行膨脹化處理,膨脹距離的選取如圖5所示。

本文定義了一種膨脹化距離RX,根據汽車轉彎時的三角函數關系推導,具體公式如下:

(11)

其中R為上文的最小轉彎半徑,即圖中的OM,d為汽車的寬度,YM為輪距,Ψ為汽車方向最大轉角,k為安全系數,根據汽車的實際工況進行選擇,取值范圍為1～2。膨脹距離的確定可以保證汽車與障礙物間的安全距離,提高路徑的可行性。

利用膨脹化距離將障礙物一定距離外設為不可行區域,由于U形和L形障礙物凹進去區域依舊不可行或降低平滑度,亦將其設為不可行區域,具體如圖6所示。

4.2 仿真對比

4.2.1 A*算法仿真

為了驗證變網格A*算法的有效性,在Matlab 2019b環境下對提出的算法進行仿真驗證。構建50×50的柵格地圖場景,柵格地圖每格邊長為1米(汽車車長為1米)。由于車長寬為1m×0.51 m,汽車方向最大轉角為30°,輪距為0.44m。安全系數k選取為1.6,由式(11)可得膨脹化的距離為0.5m。其中黑色為障礙物、白色為可行區域,紅色為起點,坐標為(1,1)、綠色為終點,坐標為(49,49)。將傳統的A*、7×7A*算法與變網格A*算法進行對比仿真,其仿真結果如圖7所示。

如圖7所示,圖中藍綠色路徑為3×3鄰域搜索的路徑,紫色路徑為7×7鄰域搜索的路徑,黑色路徑為變網格算法搜索的路徑。取每個算法10次運行時間,進行均值處理,記錄結果如表1所示。

表1 改進A*算法時間對比

從表1可以得知。變網格A*算法的路徑長度比傳統算法多了1.61%,比7×7A*算法多了0.61%。總轉折角度比傳統A*算法優化了13.95%,比7×7A*算法多了10.63%。但是尋路時間相比傳統A*算法優化了61.23%,比7×7A*算法優化了66.96%。如果路徑轉折角度少,路徑更加平滑,機器人操作汽車時,動作就更加簡易,所以對于機器人駕駛來說,犧牲部分距離換來更好的駕駛體驗是可行的。

4.2.2 路徑優化仿真

本文在第3節中對路徑進行了優化,首先剔除A*不重要的節點,只留下必要的節點。然后對路徑轉折點進行可調節圓弧優化,具體結果如圖8和表2所示。圖中紫色為優化之前的路徑,黑色為優化后路徑,從圖8和表2可以得知優化之后的路徑在拐點處變為了弧線且是根據駕駛汽車最小轉彎半徑設置的半徑,提高了機器人駕駛汽車的可操作性。同時3個場景路徑長度也有所優化。

表2 路徑優化仿真

4.3 實驗驗證

為了驗證本文改進的A*算法具有一定的實效性,本文采用25自由度的NAO機器人駕駛微型寶馬Z4電動車在走廊進行實驗。NAO機器人身高0.58m,重量為5.4kg,寶馬Z4電動車長寬為1m×0.51 m,汽車方向最大轉角為30°,由式(10)可得最小轉彎半徑R為1m,膨脹距離亦采取仿真的0.5m。

實驗環境為9m×5m的矩形區域,柵格地圖每格邊長為1m,設起點坐標為(1,1),終點坐標為(9,5),障礙物坐標為(5,1)和(6,5)。實驗中,4個椅子組成邊長為1m的方形障礙物,膠帶區域為安全區域,紫色路線為規劃路徑。實驗結果表明,NAO機器人駕駛微型寶馬電動車能夠安全到達目標點。因此可以證明算法的可行性。具體結果如圖9所示。

機器人駕駛部分是由matlab輸出仿真結果,choregraphy軟件通過仿真路線,將機器人控制方向盤和踩油門的動作集成為一個模塊化包。由于篇幅原因,駕駛部分不做詳細介紹,流程如圖10所示。

圖1 變網格A*算法

圖2 最小轉彎半徑

圖3 圓弧優化原理示意圖

圖4 環境建模

圖5 膨脹距離選取

圖6 膨脹處理

圖7 算法仿真結果

圖8 優化仿真結果

圖9 仿真環境與實驗環境

圖10 機器人駕駛過程

5 結論與展望

本文提出一種基于變網格的A*算法,該算法根據不同情況自適應不同的搜索方式。為了解決駕駛過程中可能出現的邊緣碰撞問題,對障礙物進行膨脹化處理,留出安全距離,保證汽車行駛的安全性。同時對路徑進行關鍵節點和圓弧處理,增加搜索效率的同時也使路徑更加平滑。為了驗證算法的有效性,在Matlab2019b的仿真環境下,對比傳統A*算法和7×7的A*算法,變網格A*算法結合了兩種算法的優勢,使其可以快速搜索的同時也使路徑更加平滑。最后采用NAO機器人駕駛微型寶馬Z4電動車進行實驗驗證,證明了算法的可行性。

本文算法尚不能處理突然出現的動態障礙物,未來將對這一問題進行探索,且結合機器視覺,對更為復雜條件下的路徑規劃進行研究。