基于變網(wǎng)格A*算法的機(jī)器人駕駛路徑規(guī)劃

任榮倫,衛(wèi) 沅,周志剛,李浩然

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

1 引言

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

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

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

2 A*算法

2.1 傳統(tǒng)A*算法

A*算法是一種在靜態(tài)地圖中解決路徑規(guī)劃問題比較優(yōu)良的算法,它是根據(jù)Dijkstra算法改進(jìn)的一種優(yōu)化算法。作為一種典型的啟發(fā)式搜索算法,A*算法的特點(diǎn)是搜索子節(jié)點(diǎn)時(shí)會(huì)根據(jù)當(dāng)前節(jié)點(diǎn)信息進(jìn)行對(duì)比,即每次搜索總是選取最小位置作為子節(jié)點(diǎn)。選取子節(jié)點(diǎn)需要根據(jù)特定的代價(jià)函數(shù),按照此方法執(zhí)行,直到搜索到目標(biāo)節(jié)點(diǎn)位置為止。A*算法的啟發(fā)式函數(shù)

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

(1)

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

2.2 變網(wǎng)格A*算法

2.2.1 變換網(wǎng)格

傳統(tǒng)A*算法主要有8個(gè)搜索方向,8個(gè)箭頭表示機(jī)器人在移動(dòng)過程中的8個(gè)搜索方向,機(jī)器人的最小轉(zhuǎn)角為90°。武義等將A*算法進(jìn)行改進(jìn),由8領(lǐng)域搜索方式改為48領(lǐng)域搜索方式,機(jī)器人的搜索方向變?yōu)?2個(gè)方向,移動(dòng)機(jī)器人的最小轉(zhuǎn)角為11.25°,提高了路徑平滑度。但是由于采用7×7鄰域搜索,搜索節(jié)點(diǎn)增多,大大增加了搜索時(shí)間。

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

1)程序開始,創(chuàng)建OPEN表與CLOSE表,將起點(diǎn)放入CLOSE表中。

2)搜索周圍8個(gè)子節(jié)點(diǎn)作為下一預(yù)備子節(jié)點(diǎn)放入OPEN表中,根據(jù)評(píng)價(jià)函數(shù)計(jì)算代價(jià)最小的節(jié)點(diǎn)放入CLOSE表中作為子節(jié)點(diǎn),繼續(xù)搜尋下一節(jié)點(diǎn)。

3)若周圍8個(gè)子節(jié)點(diǎn)出現(xiàn)障礙物,則打斷搜索8個(gè)子節(jié)點(diǎn)的函數(shù),進(jìn)入搜索48鄰域的搜索函數(shù),將上一父節(jié)點(diǎn)作為父節(jié)點(diǎn),搜索周圍48個(gè)節(jié)點(diǎn)作為預(yù)備節(jié)點(diǎn),放入OPEN表中,根據(jù)評(píng)價(jià)函數(shù)計(jì)算代價(jià)最小的節(jié)點(diǎn)放入CLOSE表中作為子節(jié)點(diǎn)繼續(xù)搜索。否則執(zhí)行2)。

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

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

2.2.2 改進(jìn)評(píng)價(jià)函數(shù)

2.2.2.1 改進(jìn)評(píng)價(jià)函數(shù)計(jì)算方式

傳統(tǒng)A*算法計(jì)算代價(jià)的方式常采用歐式距離,即兩個(gè)節(jié)點(diǎn)間的距離。所以2.1節(jié)中函數(shù)g(n)和h(n)分別為

(2)

(3)

其中g(shù)(n)是指起點(diǎn)到當(dāng)前節(jié)點(diǎn)的代價(jià),本文做出改進(jìn),借用上一節(jié)點(diǎn)的g(n)值加上一節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的代價(jià)來表示。具體計(jì)算如下

(4)

由于父節(jié)點(diǎn)Pn-1的g(n-1)是已知的,所以只需計(jì)算父節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的代價(jià)m(n),計(jì)算m(n)運(yùn)算量是一定小于g(n)的計(jì)算量。因此,在規(guī)劃中,此舉可節(jié)省大量計(jì)算時(shí)間。具體計(jì)算方式如下

(5)

因此,新的起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的真實(shí)代價(jià)值為

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

(6)

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

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

(7)

2.2.2.2 變換函數(shù)評(píng)價(jià)權(quán)重比例

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

sin2θ+cos2θ=1

(8)

本文構(gòu)建的兩權(quán)重系數(shù)如式(8)所示,在式(8)中θ為自變量,取值范圍為[0,pi/2],兩權(quán)重之和為1,當(dāng)θ從0到pi/2增大時(shí),第一項(xiàng)權(quán)重會(huì)逐漸增大,第二項(xiàng)權(quán)重會(huì)逐漸減小,二者成反比關(guān)系。將權(quán)重系數(shù)引入A*算法評(píng)價(jià)函數(shù)中,可調(diào)節(jié)g(n)和h(n)比例。

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

(9)

當(dāng)2.2.1小節(jié)中A*算法變換網(wǎng)格時(shí),同時(shí)給出信號(hào)對(duì)評(píng)價(jià)函數(shù)的權(quán)重比例也加以變換。當(dāng)以3×3搜索鄰域,快速搜索時(shí),此時(shí)將自變量設(shè)為較小值。當(dāng)以7×7搜索鄰域,搜索更加優(yōu)良路徑時(shí),將自變量設(shè)為略小值,保證路徑平滑的同時(shí)也保證搜索效率。最后經(jīng)過加權(quán)求平均處理取得合適值,使在有障礙物時(shí),搜索較優(yōu)路徑,無障礙物時(shí)快速搜索路徑,到達(dá)終點(diǎn)。

3 路徑優(yōu)化

3.1 提取關(guān)鍵節(jié)點(diǎn)

針對(duì)A*算法有多余的節(jié)點(diǎn)問題,采取提取關(guān)鍵節(jié)點(diǎn)算法,刪除冗余點(diǎn)處理,使路徑更短,轉(zhuǎn)折點(diǎn)更少,具體步驟如下:

1)將變網(wǎng)格的A*算法規(guī)劃路徑的所有節(jié)點(diǎn)放入一個(gè)集合P{P1,P2,P3,…,Pn(n為所有節(jié)點(diǎn)個(gè)數(shù))},P1是路徑規(guī)劃的起點(diǎn),Pn是路徑規(guī)劃的終點(diǎn)。

2)創(chuàng)建一個(gè)集合U,將P1,Pn放入集合U。

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

4)依次連接集合U中所有節(jié)點(diǎn),完成對(duì)關(guān)鍵節(jié)點(diǎn)的提取。

3.2 可調(diào)節(jié)圓弧優(yōu)化

由于受汽車本身結(jié)構(gòu)問題限制,當(dāng)汽車轉(zhuǎn)彎半徑過小時(shí),汽車是無法轉(zhuǎn)彎的。所以對(duì)路徑進(jìn)行圓弧優(yōu)化處理時(shí),當(dāng)轉(zhuǎn)彎半徑較小時(shí),汽車是無法通過的,因此圓弧半徑的選取十分重要。

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

(10)

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

由于A*算法所規(guī)劃的路徑轉(zhuǎn)折處不夠平滑,機(jī)器人駕駛汽車進(jìn)行行駛時(shí),路徑不夠平滑。因此,需要對(duì)所規(guī)劃的路徑進(jìn)行平滑處理,用圓弧代替轉(zhuǎn)折點(diǎn),其原理如圖3所示:

4 仿真研究

4.1 環(huán)境建模與膨脹化處理

4.1.1 環(huán)境建模

柵格法是一種經(jīng)典的環(huán)境建模方法,是A*算法常用的建模方法。其主要原理是將機(jī)器人的工作環(huán)境模擬為相同大小且相互連接的柵格,每個(gè)柵格對(duì)應(yīng)相應(yīng)位置信息。

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

4.1.2 膨脹化處理

由于在仿真環(huán)境中將機(jī)器人駕駛的汽車?yán)硐牖梢桓?轉(zhuǎn)彎時(shí)可能會(huì)在邊緣和障礙物發(fā)生碰撞,所以在構(gòu)建柵格地圖時(shí)將障礙物進(jìn)行膨脹化處理,膨脹距離的選取如圖5所示。

本文定義了一種膨脹化距離RX,根據(jù)汽車轉(zhuǎn)彎時(shí)的三角函數(shù)關(guān)系推導(dǎo),具體公式如下:

(11)

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

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

4.2 仿真對(duì)比

4.2.1 A*算法仿真

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

如圖7所示,圖中藍(lán)綠色路徑為3×3鄰域搜索的路徑,紫色路徑為7×7鄰域搜索的路徑,黑色路徑為變網(wǎng)格算法搜索的路徑。取每個(gè)算法10次運(yùn)行時(shí)間,進(jìn)行均值處理,記錄結(jié)果如表1所示。

表1 改進(jìn)A*算法時(shí)間對(duì)比

從表1可以得知。變網(wǎng)格A*算法的路徑長度比傳統(tǒng)算法多了1.61%,比7×7A*算法多了0.61%?？傓D(zhuǎn)折角度比傳統(tǒng)A*算法優(yōu)化了13.95%,比7×7A*算法多了10.63%。但是尋路時(shí)間相比傳統(tǒng)A*算法優(yōu)化了61.23%,比7×7A*算法優(yōu)化了66.96%。如果路徑轉(zhuǎn)折角度少,路徑更加平滑,機(jī)器人操作汽車時(shí),動(dòng)作就更加簡易,所以對(duì)于機(jī)器人駕駛來說,犧牲部分距離換來更好的駕駛體驗(yàn)是可行的。

4.2.2 路徑優(yōu)化仿真

本文在第3節(jié)中對(duì)路徑進(jìn)行了優(yōu)化,首先剔除A*不重要的節(jié)點(diǎn),只留下必要的節(jié)點(diǎn)。然后對(duì)路徑轉(zhuǎn)折點(diǎn)進(jìn)行可調(diào)節(jié)圓弧優(yōu)化,具體結(jié)果如圖8和表2所示。圖中紫色為優(yōu)化之前的路徑,黑色為優(yōu)化后路徑,從圖8和表2可以得知優(yōu)化之后的路徑在拐點(diǎn)處變?yōu)榱嘶【€且是根據(jù)駕駛汽車最小轉(zhuǎn)彎半徑設(shè)置的半徑,提高了機(jī)器人駕駛汽車的可操作性。同時(shí)3個(gè)場景路徑長度也有所優(yōu)化。

表2 路徑優(yōu)化仿真

4.3 實(shí)驗(yàn)驗(yàn)證

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

實(shí)驗(yàn)環(huán)境為9m×5m的矩形區(qū)域,柵格地圖每格邊長為1m,設(shè)起點(diǎn)坐標(biāo)為(1,1),終點(diǎn)坐標(biāo)為(9,5),障礙物坐標(biāo)為(5,1)和(6,5)。實(shí)驗(yàn)中,4個(gè)椅子組成邊長為1m的方形障礙物,膠帶區(qū)域?yàn)榘踩珔^(qū)域,紫色路線為規(guī)劃路徑。實(shí)驗(yàn)結(jié)果表明,NAO機(jī)器人駕駛微型寶馬電動(dòng)車能夠安全到達(dá)目標(biāo)點(diǎn)。因此可以證明算法的可行性。具體結(jié)果如圖9所示。

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

圖1 變網(wǎng)格A*算法

圖2 最小轉(zhuǎn)彎半徑

圖3 圓弧優(yōu)化原理示意圖

圖4 環(huán)境建模

圖5 膨脹距離選取

圖6 膨脹處理

圖7 算法仿真結(jié)果

圖8 優(yōu)化仿真結(jié)果

圖9 仿真環(huán)境與實(shí)驗(yàn)環(huán)境

圖10 機(jī)器人駕駛過程

5 結(jié)論與展望

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

本文算法尚不能處理突然出現(xiàn)的動(dòng)態(tài)障礙物,未來將對(duì)這一問題進(jìn)行探索,且結(jié)合機(jī)器視覺,對(duì)更為復(fù)雜條件下的路徑規(guī)劃進(jìn)行研究。