基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃

汪四新，譚功全，蔣 沁，蘇 暢

(四川輕化工大學(xué)自動(dòng)化與信息工程學(xué)院，四川自貢643000)

1 引言

移動(dòng)機(jī)器人首先要解決的問題就是如何有效快速地到達(dá)目標(biāo)點(diǎn)，而且能夠避開環(huán)境中的障礙物。路徑規(guī)劃是機(jī)器人導(dǎo)航中最基本的問題[1-2]，指的是如何規(guī)劃出一條由出發(fā)位置到目標(biāo)位置的無碰撞適宜軌跡。路徑規(guī)劃分為局部路徑規(guī)劃和全局路徑規(guī)劃[3]，局部路徑規(guī)劃指的是環(huán)境未知情況下的避障運(yùn)動(dòng)規(guī)劃，全局路徑規(guī)劃則是在環(huán)境已知的情況下搜索全局最優(yōu)的路徑規(guī)劃。常用的局部路徑規(guī)劃算法有動(dòng)態(tài)窗口法DWA[4]、人工勢(shì)場(chǎng)法[5]等。常用的全局路徑算法主要有神經(jīng)網(wǎng)絡(luò)[6]、遺傳算法[7]、柵格法[8]、A*算法[9]、RRT[10]以及仿生群搜索算法[11-12]等。

A*算法作為常用的全局算法中的一種，能夠在靜態(tài)環(huán)境中快速求解最優(yōu)路徑[13]。王淼弛[14]對(duì)雙向A*算法采用24領(lǐng)域擴(kuò)展方法，提高了算法單步搜索區(qū)域，但搜索節(jié)點(diǎn)過多且雙向搜索采用異步雙向搜索，導(dǎo)致算法的運(yùn)行時(shí)間過長(zhǎng)。王中玉等人[15]針對(duì)路徑冗余點(diǎn)和拐點(diǎn)改進(jìn)評(píng)價(jià)函數(shù)，提高了路徑的平滑性，但采用擴(kuò)展障礙物方法沒有從根本上解決拐點(diǎn)碰撞問題。陳若男等人[16]采用領(lǐng)域矩陣方法搜索障礙物，改善了路徑安全性，但沒有提高路徑搜索效率。

本文在已知靜態(tài)環(huán)境下，采用A*算法雙向搜索方式，并引入雙向中點(diǎn)虛擬目標(biāo)點(diǎn)引導(dǎo)雙向往點(diǎn)靠攏相遇，減少搜索時(shí)間。其次加入預(yù)估函數(shù)權(quán)重，兼顧搜索效率與路徑距離質(zhì)量。最后考慮安全性，進(jìn)行障礙物拐點(diǎn)領(lǐng)域擴(kuò)展切換。通過仿真，驗(yàn)證了算法改進(jìn)的有效性。

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

2.1 環(huán)境建模

移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃的首先要對(duì)地圖環(huán)境進(jìn)行建模，環(huán)境地圖目前使用較多的大概三類，分別為柵格地圖、拓?fù)涞貓D、可視地圖。柵格地圖具有易編程、結(jié)構(gòu)簡(jiǎn)單等特點(diǎn)，因此A*算法采用柵格法進(jìn)行環(huán)境建模。建立邊長(zhǎng)為k個(gè)柵格單元長(zhǎng)度的正方形地圖，即柵格地圖面積為k2。完成柵格地圖的框架建立后，需要對(duì)地圖進(jìn)行信息填充，在此之前需要確定出柵格地圖中每個(gè)柵格單元的坐標(biāo)位置。定義全局柵格信息為Φ，柵格地圖每個(gè)柵格單元為nxy可以根據(jù)其行列坐標(biāo)表示為

nxy=(x，y)(x，y∈[1，n])

(1)

式中x，y坐標(biāo)起點(diǎn)為柵格地圖左上頂點(diǎn)，起點(diǎn)值為1，x軸方向?yàn)槠瘘c(diǎn)垂直向下，y軸方向?yàn)槠瘘c(diǎn)垂直向右。

全局柵格信息Φ可表示為

Φ=∑nxy

(2)

為便于對(duì)A*路徑算法的理解分析，對(duì)普遍使用的黑白柵格地圖彩色化。柵格地圖進(jìn)行彩色化處理，需要知道對(duì)應(yīng)每個(gè)柵格單元的線性化坐標(biāo)，線性坐標(biāo)是以柵格地圖左上角為坐標(biāo)起點(diǎn)，采用自上而下，自左往右的方式排序。因此將容易直觀觀察的行列坐標(biāo)nxy轉(zhuǎn)換為線性坐標(biāo)m

m=x+(y-1)*k

(3)

式中x，y為柵格單元的行列坐標(biāo)，k為地圖邊長(zhǎng)。

根據(jù)A*算法需求，對(duì)線性坐標(biāo)柵格進(jìn)行相應(yīng)顏色三元素RGB矩陣配置，分別對(duì)空地、障礙、起點(diǎn)、目標(biāo)點(diǎn)、已搜索點(diǎn)、待搜索點(diǎn)、路徑進(jìn)行顏色矩陣設(shè)定，如表1所示。

表1 柵格地圖RGB顏色配置

2.2 A*算法原理

A*算法是P.E.Hart、N.J.Nilsson和B.Raphael于1968年提出的一種啟發(fā)式搜索算法。A*算法是結(jié)合Dijkstra搜索算法[17]與最佳優(yōu)先搜索(BFS)算法的優(yōu)點(diǎn)所提出的。Dijkstra算法與BFS算法都是從出發(fā)點(diǎn)開始，向四周發(fā)散擴(kuò)展，直至搜索到目標(biāo)點(diǎn)。區(qū)別在于Dijkstra算法是以擴(kuò)展點(diǎn)與起點(diǎn)距離為代價(jià)進(jìn)行路徑搜索，沒有考慮與目標(biāo)點(diǎn)之間的距離關(guān)系，所以搜索出目標(biāo)點(diǎn)花費(fèi)時(shí)間很長(zhǎng)，但搜索出的路徑是最優(yōu)路徑。而BFS算法則相反，考慮了擴(kuò)展點(diǎn)與目標(biāo)點(diǎn)的關(guān)系，搜索出目標(biāo)點(diǎn)花費(fèi)時(shí)間很短，但路徑不一定是最優(yōu)的。A*算法兼顧Dijkstra算法與BFS算法距離代價(jià)，建立了新的搜索評(píng)價(jià)函數(shù)，如下式

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

(4)

式中g(shù)(n)為當(dāng)前點(diǎn)n與起點(diǎn)的實(shí)際距離代價(jià)，h(n)為當(dāng)前點(diǎn)n與目標(biāo)點(diǎn)的估計(jì)距離代價(jià)。其中代價(jià)距離均采用歐幾里德距離，g(n)函數(shù)與h(n)函數(shù)計(jì)算公式如下

(5)

(6)

式中x、y分別代表當(dāng)前擴(kuò)展點(diǎn)的橫縱坐標(biāo)，xs、ys分別代表起始點(diǎn)橫縱坐標(biāo)，xg、yg分別代表目標(biāo)點(diǎn)橫縱坐標(biāo)。當(dāng)g(n)取值接近0時(shí)，A*算法實(shí)則變成了BFS搜索算法，當(dāng)h(n)取值接近0時(shí)，A*算法則相當(dāng)于Dijkstra搜索算法。

A*算法在當(dāng)前點(diǎn)擴(kuò)展領(lǐng)域節(jié)點(diǎn)時(shí)，通常采用四向、八向領(lǐng)域擴(kuò)展，四向擴(kuò)展偏向于倉儲(chǔ)結(jié)構(gòu)化的搜索方式，方向?yàn)楫?dāng)前點(diǎn)的上、下、左、右方向。八向擴(kuò)展方式則是在四向擴(kuò)展基礎(chǔ)上加入了左上、右上、坐下、右下方向，擴(kuò)展節(jié)點(diǎn)如圖1。

圖1 擴(kuò)展節(jié)點(diǎn)

3 改進(jìn)A*算法

3.1 雙向同步搜索

改進(jìn)的A*算法在傳統(tǒng)A*算法基礎(chǔ)上采用雙向同步搜索的方式，分別將此次路徑搜索的起點(diǎn)、目標(biāo)點(diǎn)作為雙向搜索的起點(diǎn)，同時(shí)向各自的目標(biāo)點(diǎn)進(jìn)行擴(kuò)展，最終雙向擴(kuò)展點(diǎn)相交，終止搜索，得到全局路徑。其步驟可總結(jié)如下：

1)算法運(yùn)行開始時(shí)，首先創(chuàng)建雙向OPEN表、CLOSE表、PARENT表，后標(biāo)1表示正向表，后標(biāo)2表示反向標(biāo)，將全局路徑的起始點(diǎn)、目標(biāo)點(diǎn)(即雙向搜索的兩端點(diǎn))放入對(duì)應(yīng)的OPEN1表和OPEN2表，作為各自的搜索起點(diǎn)，準(zhǔn)備擴(kuò)展；

2)判斷OPEN1表和OPEN2表是否為空。為空跳8)，否則執(zhí)行3)；

3)判斷OPEN1表和OPEN2表是否包含各自搜索方向目標(biāo)點(diǎn)。包含跳7)，否則執(zhí)行4)；

4)分別選取OPEN1表和OPEN2表中f(n)值最小值點(diǎn)存入對(duì)應(yīng)CLOSE表，成為新的擴(kuò)展當(dāng)前點(diǎn)；

5)將新的擴(kuò)展當(dāng)前點(diǎn)，擴(kuò)展領(lǐng)域范圍內(nèi)的點(diǎn)存入OPEN1表和OPEN2表中，更新其父指針，存入對(duì)應(yīng)正反向PARENT表；跳過障礙物點(diǎn)與CLOSE表中的點(diǎn)擴(kuò)展其它點(diǎn)，并跳至2)。若已存在于OPEN表中的點(diǎn)，則比較其評(píng)價(jià)函數(shù)的當(dāng)前值與過去值，若低于過去值則刷新父指針；

6)判斷正反向擴(kuò)展點(diǎn)是否存在交集。存在交集則執(zhí)行7)，否則跳2)；

7)已找到路徑，連接正向父指針PARENT1與顛倒的反向父指針PARENT2，獲得路徑；

8)程序運(yùn)行結(jié)束。

因?yàn)殡p向A*算法，在范圍過大時(shí)往往存在雙向搜索不能再地圖中間區(qū)域相遇，導(dǎo)致搜索所花費(fèi)的時(shí)間加長(zhǎng)，甚至?xí)霈F(xiàn)無法相遇的情況，無法得到一條可行路徑。針對(duì)以上雙向搜索相遇問題，以人工勢(shì)場(chǎng)法[18]牽引力思想，將目標(biāo)點(diǎn)看作對(duì)移動(dòng)機(jī)器人的引力作用，從而引導(dǎo)機(jī)器人往目標(biāo)點(diǎn)啟發(fā)式搜索。建立一個(gè)兩端點(diǎn)歐式距離中點(diǎn)位置的虛擬牽引點(diǎn)如圖2。

圖2 虛擬牽引

虛擬點(diǎn)為雙向搜索提供一個(gè)往中間點(diǎn)位置的虛擬牽引力作用，使得二者能在可行路徑條件下，盡可能相遇在中間位置，縮短搜索時(shí)間，提高雙向搜索的效率，如下式(7)、(8)

(7)

h′(n)=ln(d(n))*h(n)

(8)

式(7)中d(n)為虛擬牽引點(diǎn)與機(jī)器人當(dāng)前點(diǎn)的歐氏距離，式(8)中h′(n)為加入虛擬牽引點(diǎn)牽引后的預(yù)估函數(shù)，其中虛擬引力點(diǎn)作用大小隨著與雙向搜索當(dāng)前點(diǎn)距離縮短而減小，加入對(duì)數(shù)函數(shù)使得整個(gè)距離衰減過程更加平滑。

3.2 改進(jìn)預(yù)估函數(shù)

對(duì)評(píng)價(jià)函數(shù)中加入虛擬引力點(diǎn)作用后的預(yù)估函數(shù)h′(n)加入權(quán)值函數(shù)，從而改變預(yù)估函數(shù)在整個(gè)評(píng)價(jià)函數(shù)中的比例，減少了算法搜索中無效搜索區(qū)域，提高算法的搜索效率。權(quán)值函數(shù)采用當(dāng)前點(diǎn)與目標(biāo)點(diǎn)歐幾里德距離的衰減對(duì)數(shù)函數(shù)，加入權(quán)值函數(shù)的預(yù)估函數(shù)h″(n)如下式(9)

(9)

式中加入的對(duì)數(shù)權(quán)值函數(shù)隨著機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)的距離變化而變化，當(dāng)機(jī)器人與目標(biāo)點(diǎn)距離很遠(yuǎn)時(shí)，預(yù)估距離要比真實(shí)距離小得多，因此權(quán)重函數(shù)值應(yīng)較大，加快搜索趨近目標(biāo)點(diǎn)速度。接近目標(biāo)點(diǎn)時(shí)，預(yù)估距離則接近真實(shí)距離，權(quán)重函數(shù)值接近于1。

3.3 避障策略

在障礙物環(huán)境下，以機(jī)器人為柵格單位，會(huì)出現(xiàn)在仿真路線可行的情況下，實(shí)際機(jī)器人會(huì)與障礙物則會(huì)碰撞的安全問題，本文不考慮機(jī)器人與障礙物擦邊的情況如圖3。

圖3 避障擴(kuò)展

圖3中灰色柵格為機(jī)器人當(dāng)前位置，黑色為障礙物，綠色為目標(biāo)點(diǎn)位置，此時(shí)采取8向領(lǐng)域擴(kuò)展機(jī)器人的下一位置則是5號(hào)柵格，仿真路線可行，但實(shí)際機(jī)器人則會(huì)與障礙物發(fā)生碰撞。針對(duì)此類障礙物拐角點(diǎn)碰撞問題，采用領(lǐng)域擴(kuò)展切換的方式。在進(jìn)行擴(kuò)展時(shí)，判斷是否會(huì)出現(xiàn)此類問題，如果會(huì)出現(xiàn)則將下一步擴(kuò)展由原來的8向領(lǐng)域擴(kuò)展切換為4向領(lǐng)域擴(kuò)展，待下一步擴(kuò)展完成后在恢復(fù)到原來的四向擴(kuò)展，這樣就可以有效解決機(jī)器人在障礙物拐角碰撞問題。

4 仿真分析

為了驗(yàn)證本方法的有效性，在Matlab R2017b仿真平臺(tái)下，進(jìn)行了不同環(huán)境的仿真。并且通過彩色地圖進(jìn)行顯示，各顏色表示信息如表1，能夠更直觀表達(dá)出改進(jìn)算法的效果。

實(shí)驗(yàn)一首先對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn)，引入雙向搜索A*算法，分別將起始點(diǎn)和目標(biāo)點(diǎn)作為各自的起點(diǎn)開始搜索，往各自目標(biāo)點(diǎn)搜索，仿真如圖4。

圖4 實(shí)驗(yàn)一傳統(tǒng)A*算法和雙向A*算法仿真

圖4中灰色表示機(jī)器人的最終搜索到的路徑，可以看出改進(jìn)后的雙向搜索在路徑長(zhǎng)度上更短，通過搜索時(shí)間、長(zhǎng)度數(shù)據(jù)更為詳細(xì)地比較傳統(tǒng)A*算法與雙向A*算法見表2，引入雙向搜索后的A*算法搜索時(shí)間得到了顯著縮短，且路徑長(zhǎng)度也得到的略微縮短改善。

表2 實(shí)驗(yàn)一仿真結(jié)果數(shù)據(jù)

由于實(shí)驗(yàn)一柵格地圖為10*10，不能暴露出雙向A*算法可能出現(xiàn)的問題，且搜索區(qū)域較小，實(shí)驗(yàn)二在雙向A*算法基礎(chǔ)上，擴(kuò)大柵格地圖，建立30*30柵格地圖，并增加起始點(diǎn)與目標(biāo)點(diǎn)之間的障礙物，將傳統(tǒng)A*算法與改進(jìn)方法進(jìn)行對(duì)比，仿真結(jié)果如圖5。

圖5 實(shí)驗(yàn)二傳統(tǒng)A*算法和改進(jìn)A*算法仿真

實(shí)驗(yàn)二在擴(kuò)展的地圖中，分別對(duì)傳統(tǒng)A*算法和本文逐漸改進(jìn)后的算法進(jìn)行了對(duì)比仿真，對(duì)比各自算法搜索的時(shí)間、路徑長(zhǎng)度及避障性能指標(biāo)來進(jìn)行分析。實(shí)驗(yàn)二各算法仿真結(jié)果數(shù)據(jù)見表3，在擴(kuò)大了地圖面積并增加了障礙物后，通過與傳統(tǒng)A*算法對(duì)比，單純的雙向A*算法如圖5(b)雖然在路徑長(zhǎng)度上提升了4.4%，但在搜索時(shí)間上花費(fèi)嚴(yán)重，增高了42.6%，原因是地圖中間巨大的障礙物使得雙向搜索久久不能相遇。通過引入兩端點(diǎn)中部虛擬引力后如圖5(c)，使得雙向搜索即使在有障礙物影響下，也能盡快相遇，較傳統(tǒng)A*算法搜索時(shí)間降低11.6%，路徑長(zhǎng)度降低4.9%，較單純雙向A*算法搜索時(shí)間更是降低了38.0%。在此基礎(chǔ)上再引入衰減距離對(duì)數(shù)函數(shù)加權(quán)后如圖5(d)，在保障了路徑長(zhǎng)度前提下，較傳統(tǒng)A*算法搜索時(shí)間降低了33.5%。在改善算法搜索時(shí)間與路徑長(zhǎng)度同時(shí)，還考慮到路徑可行性，即與障礙物之間的安全問題，對(duì)機(jī)器人通過障礙物拐點(diǎn)進(jìn)行安全判據(jù)，并通過領(lǐng)域擴(kuò)展切換方式如圖5(e)，有效避免了機(jī)器人與障礙物發(fā)生碰撞，提高了路徑的安全性。

表3 實(shí)驗(yàn)二仿真結(jié)果數(shù)據(jù)

5 結(jié)論

針對(duì)傳統(tǒng)A*算法搜索效率低，且路徑長(zhǎng)度一般不是最優(yōu)問題。本文首先引入同步雙向搜索方式提高搜索效率，并考慮同步雙向搜索中隨著地圖擴(kuò)大存在的長(zhǎng)久不能相遇問題，參考人工勢(shì)場(chǎng)法思想，引入虛擬牽引點(diǎn)，在確保路徑長(zhǎng)度前提下，使得雙向搜索能盡快相遇。并且對(duì)預(yù)估函數(shù)加入到搜索目標(biāo)點(diǎn)的距離衰減函數(shù)，進(jìn)一步提高了算法搜索速度。此外考慮到搜索路徑安全可行問題，通過改變領(lǐng)域擴(kuò)展方式改善避障。本文最終改進(jìn)方法提高了算法搜索效率，改善了搜索路徑，證明了算法改進(jìn)的有效性。但本文算法不是完全優(yōu)于傳統(tǒng)方法，在特殊情況下，比如起始點(diǎn)與目標(biāo)點(diǎn)之間障礙物較少或者無障礙物情況下，傳統(tǒng)方法性能還是會(huì)優(yōu)于本文方法，而且沒在實(shí)際機(jī)器人中進(jìn)行測(cè)試，下一步將結(jié)合實(shí)用生產(chǎn)環(huán)境進(jìn)行研究。