改進(jìn)A*算法在噴漿機(jī)器人路徑規(guī)劃中的研究

蘇 暢,袁 泉,李子強(qiáng),余曉帆

(安徽理工大學(xué) 機(jī)械工程學(xué)院,安徽 淮南 232001)

基于目前國家智慧礦山建設(shè)的背景,噴漿機(jī)器人的研發(fā)與應(yīng)用獲得了較大的進(jìn)步。作為一種電子、機(jī)械、人工智能等多學(xué)科交叉融合的復(fù)雜系統(tǒng),自主行走能力是噴漿機(jī)器人在礦井環(huán)境應(yīng)用中的重要性能標(biāo)準(zhǔn)[1]。由于井下巷道復(fù)雜的環(huán)境情況,噴漿機(jī)器人需要具備自主運(yùn)動能力,以及對巷道環(huán)境的感知和路徑規(guī)劃能力,以高效、可靠的完成噴漿作業(yè)。其中,路徑規(guī)劃是非常關(guān)鍵的,為了保障了機(jī)器人可以順利的抵達(dá)目標(biāo)點(diǎn)[2],需要機(jī)器人在未知環(huán)境中規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的平穩(wěn)路徑,常見的路徑規(guī)劃算法包括Dijkstra算法[3]、A*算法[4]、D*算法[5]、D*Lite算法[6]。其中,D*和D*Lite算法多用于不確定的動態(tài)環(huán)境,而在已知的靜態(tài)環(huán)境下,Dijkstra和A*算法可以更快速地找到最優(yōu)路徑。Dijkstra算法是以搜索點(diǎn)與起點(diǎn)之間的代價(jià)為基礎(chǔ)來搜索最短路徑,但由于其搜索范圍太大,使得搜索效率低下。A*算法是在Dijkstra算法的基礎(chǔ)上添加啟發(fā)函數(shù)的路徑搜索算法,并綜合考慮了搜索點(diǎn)與起點(diǎn)以及搜索點(diǎn)與目標(biāo)點(diǎn)兩種代價(jià),這樣可以平衡路徑代價(jià)和搜索時間。但是在實(shí)際的噴漿機(jī)器人路徑規(guī)劃中,A*算法存在安全性低,轉(zhuǎn)折路徑多,平滑性較差等問題。

針對以上問題,目前學(xué)者們已經(jīng)提出了許多改進(jìn)A*算法。王中玉等[7]通過調(diào)整計(jì)算方法與函數(shù)比例來減少路徑搜索時間和多余節(jié)點(diǎn)數(shù)目。辛煜等[8]在經(jīng)典A*算法的基礎(chǔ)上,將可搜索鄰域數(shù)目從八個擴(kuò)展到了無限個,并且可以在任何方向進(jìn)行搜索,既減少了求解的路徑長度,又極大的降低了拐點(diǎn)的數(shù)量。趙以毛等[9]重新設(shè)計(jì)啟發(fā)式函數(shù),減少了機(jī)器人路徑規(guī)劃的時間,且規(guī)劃的路徑更加平滑。吳飛龍等[10]對傳統(tǒng)評價(jià)函數(shù)作出改進(jìn),同時設(shè)置了安全閾值并對路徑節(jié)點(diǎn)進(jìn)行優(yōu)化。陳靖輝等[11]采用定向搜索的方法去除了對稱搜索路徑,由此減少了經(jīng)過的節(jié)點(diǎn)數(shù)量。ZHANG等[12]首先通過經(jīng)典A*算法規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的安全路徑,再利用關(guān)鍵點(diǎn)選擇方法進(jìn)行二次規(guī)劃,將路徑中多余的中間節(jié)點(diǎn)刪除,只保存起點(diǎn)、拐點(diǎn)和目標(biāo)點(diǎn),從而減少了搜索節(jié)點(diǎn)的數(shù)目和規(guī)劃時間。陳若男等[13]利用領(lǐng)域矩陣方法對障礙物進(jìn)行搜索,雖然改進(jìn)了路徑的安全性,但是路徑的搜索效率并沒有得到提高。本文在以上研究的基礎(chǔ)上,對A*算法的評價(jià)函數(shù),子節(jié)點(diǎn)的選取方式,以及路徑平滑等方面進(jìn)行改進(jìn),使改進(jìn)算法在路徑規(guī)劃中具有快速,高效,更平滑,安全性更強(qiáng)的特點(diǎn)。

1 A*算法

1.1 環(huán)境建模

環(huán)境建模指的是在進(jìn)行路徑規(guī)劃之前,噴漿機(jī)器人通過視覺或激光傳感器將真實(shí)的物理環(huán)境轉(zhuǎn)變成計(jì)算機(jī)能夠識別的抽象環(huán)境。W.E.Howden在1968年提出了柵格法[14],因?yàn)檫@種方法在計(jì)算障礙物信息時較為簡單,而且容易實(shí)現(xiàn),因此被普遍地用于路徑規(guī)劃的環(huán)境建模上。通過柵格法把環(huán)境空間分割成若干個柵格單元,每個柵格的尺寸都是一樣的,并對其進(jìn)行定義來表達(dá)障礙物的信息。柵格單元的尺寸對路徑規(guī)劃的精度和效率有很大影響,當(dāng)柵格分割的較小時,柵格個數(shù)較多,搜索空間過大,導(dǎo)致路徑規(guī)劃耗時長,效率低下。當(dāng)柵格分割的較大時,環(huán)境空間劃分的不夠細(xì)致,導(dǎo)致路徑規(guī)劃的準(zhǔn)確性下降,但柵格數(shù)量少可以縮短路徑規(guī)劃時間,提高效率。因此,需要確定大小合適的柵格地圖,通常根據(jù)實(shí)際環(huán)境中障礙物的稀疏程度和路徑規(guī)劃的要求來確定。圖1是一個20*20的柵格地圖,白色柵格為空閑區(qū)域,能夠讓噴漿機(jī)器人自由通行,黑色柵格為障礙區(qū)域,機(jī)器人無法通行。

圖1 20×20柵格地圖

1.2 經(jīng)典A*算法

A*算法具有搜索效率高、規(guī)劃速度快等特點(diǎn),已經(jīng)廣泛應(yīng)用于路徑規(guī)劃。A*算法首先在起點(diǎn)開始尋找附近節(jié)點(diǎn),然后根據(jù)評價(jià)函數(shù)選擇移動代價(jià)最少的點(diǎn)作為下一個搜索節(jié)點(diǎn),直至擴(kuò)大到目標(biāo)點(diǎn)。A*算法的評價(jià)函數(shù)f(n)的表達(dá)式為:

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

(1)

式(1)中,g(n)為起點(diǎn)到當(dāng)前節(jié)點(diǎn)的移動代價(jià),h(n)為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià),也稱啟發(fā)函數(shù)。

若h(n)的評估值遠(yuǎn)小于g(n),則f(n)將近似等于g(n),此時A*算法的遍歷節(jié)點(diǎn)會增多,搜索效率將大大降低;若h(n)遠(yuǎn)大于g(n),此時A*算法路徑規(guī)劃的速度變快,但會出現(xiàn)穿過障礙物的情況。因此啟發(fā)函數(shù)h(n)決定了A*算法的搜索效率。歐幾里德距離、曼哈頓距離和對角線距離是常用h(n)的預(yù)測方法。相比于曼哈頓和對角線距離,歐幾里德距離具有更高的計(jì)算精度,而且更容易搜索最優(yōu)路徑,因此本文選取歐幾里德距離對h(n)進(jìn)行移動代價(jià)預(yù)估。假設(shè)起點(diǎn)坐標(biāo)為(s1,s2),目標(biāo)點(diǎn)坐標(biāo)為(g1,g2),則歐幾里德的啟發(fā)函數(shù)為:

(2)

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

2.1 評價(jià)函數(shù)的優(yōu)化

在A*算法的介紹中提出,A*算法的核心是f(n),其中g(shù)(n)的值是固定的,因此對啟發(fā)函數(shù)的優(yōu)化通常是圍繞h(n)實(shí)現(xiàn)的。針對實(shí)際的工作需求,h(n)越貼近實(shí)際的路徑代價(jià),其節(jié)點(diǎn)的選擇準(zhǔn)確度就越高,但h(n)的復(fù)雜程度越高,搜索節(jié)點(diǎn)就會越多,導(dǎo)致路徑規(guī)劃的運(yùn)算速度降低,所以選擇合適的h(n)是十分重要的。評價(jià)函數(shù)是移動代價(jià)與估計(jì)代價(jià)的和,通過更改移動代價(jià)與估計(jì)代價(jià)的權(quán)重,可以控制A*算法更偏向移動代價(jià)或是估計(jì)代價(jià),如果移動代價(jià)的權(quán)重過大,則路徑規(guī)劃的速度較慢但可以搜尋到最佳路徑;如果估計(jì)代價(jià)的權(quán)重過大,則路徑規(guī)劃的速度快但無法保證搜尋到最佳路徑。因此本文更改評價(jià)函數(shù)的公式為:

f(n)=g(n)+(1+r/R)*h(n)

(3)

式(3)中,r為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的長度,R為起點(diǎn)到目標(biāo)點(diǎn)的長度。即在估計(jì)代價(jià)h(n)前更改系數(shù),從而改變估計(jì)代價(jià)的權(quán)重,以提高算法搜索路徑的速度。

2.2 優(yōu)化子節(jié)點(diǎn)的選擇方式

A*算法在尋找從起始點(diǎn)到終點(diǎn)的路徑過程中,有4鄰接和8鄰接兩種常用的選擇子節(jié)點(diǎn)的方式,如圖2(a)、圖2(b)所示,但這兩種方式規(guī)劃的路徑拐點(diǎn)較多且路徑長。為此本文選擇16鄰接的方式,如圖2(c)所示,該方式規(guī)劃的路徑拐點(diǎn)少、路徑短,適合噴漿機(jī)器人的路徑規(guī)劃。

自費(fèi)疫苗并不是某些網(wǎng)友所說的試驗(yàn)性的，都是正式銷售的，而且基本是進(jìn)口疫苗（有些是國產(chǎn)的），這些進(jìn)口疫苗是美國FDA認(rèn)證過的，在歐美日等發(fā)達(dá)國家是免費(fèi)的計(jì)劃內(nèi)疫苗，所謂的自費(fèi)只是需要家長來付這筆費(fèi)用。

A*算法在生成其子節(jié)點(diǎn)后,對子節(jié)點(diǎn)進(jìn)行判斷,如果子節(jié)點(diǎn)存在障礙物,則不生成該位置的子節(jié)點(diǎn),而是選取評價(jià)函數(shù)值最小的子節(jié)點(diǎn)作為路徑節(jié)點(diǎn),因此,所規(guī)劃出的路徑會穿過圖3(a)所示障礙物的頂點(diǎn),或與圖3(b)所示的障礙物距離太近而發(fā)生碰撞,從而導(dǎo)致路徑規(guī)劃失敗,甚至造成機(jī)器人損壞。圖3中,三角形是機(jī)器人路徑規(guī)劃的起點(diǎn),圓形是目標(biāo)點(diǎn),黑色線段是規(guī)劃的路徑。

(a)

為了改善經(jīng)典A*算法安全性低的問題,改進(jìn)子節(jié)點(diǎn)的選擇方式,在父節(jié)點(diǎn)生成子節(jié)點(diǎn)的時候,對子節(jié)點(diǎn)與障礙物之間的位置關(guān)系進(jìn)行判斷,改變子節(jié)點(diǎn)的選擇方式,防止機(jī)器人在移動過程中出現(xiàn)斜穿障礙物頂點(diǎn)的問題,有效地避免了機(jī)器人與障礙物之間的碰撞,提高了噴漿機(jī)器人移動過程中的安全性。圖4所示為16個子節(jié)點(diǎn)的分布圖,本文設(shè)計(jì)的子節(jié)點(diǎn)選擇方式為:

圖4 子節(jié)點(diǎn)分布圖

(1)若子節(jié)點(diǎn)1或5為障礙物,那么子節(jié)點(diǎn)2、4、6、8、9、10、13、14均不作為預(yù)選節(jié)點(diǎn);

(2)若子節(jié)點(diǎn)3或7為障礙物,那么子節(jié)點(diǎn)2、4、6、8、11、12、15、16均不作為預(yù)選節(jié)點(diǎn);

(3)若子節(jié)點(diǎn)無障礙物則不作處理。

通過優(yōu)化子節(jié)點(diǎn)的選擇方式,改進(jìn)的A*算法規(guī)劃的路徑不會斜穿障礙物或離障礙物距離過近,如圖5(a)、圖5(b)所示,有效的避免了碰撞,機(jī)器人路徑規(guī)劃的安全性得到提高。

(a)

2.3 路徑平滑度優(yōu)化

經(jīng)典A*算法在規(guī)劃路徑時,由于路徑節(jié)點(diǎn)位于柵格中心,導(dǎo)致規(guī)劃的路徑具有多個拐點(diǎn)、平滑性差等問題。在噴漿機(jī)器人實(shí)際的作業(yè)環(huán)境中,上述問題對其工作效率會產(chǎn)生較大影響。為了解決這些問題,本文對路徑進(jìn)行了平滑度優(yōu)化,以減小路徑轉(zhuǎn)折的角度。本文選擇貝塞爾曲線法對路徑進(jìn)行平滑處理。貝塞爾曲線是一種應(yīng)用于二維圖像的曲線,它的取點(diǎn)方式是在兩條線段上尋找一個可以將兩條線段等分的點(diǎn),再在其連線上尋找到一個可以等比例平分這條連線的點(diǎn),這個點(diǎn)就是貝塞爾曲線上的一點(diǎn),貝塞爾曲線上的其他點(diǎn)則通過改變比例得到,這些點(diǎn)組成的曲線就是貝塞爾曲線。二階貝塞爾曲線公式如式(4)所示。

B(t)=(1-t)2P0+2t(1-t)P1+

t2P2,t∈(0,1)

(4)

式(4)中,P0是起點(diǎn)坐標(biāo),P1是目標(biāo)點(diǎn)坐標(biāo),P2是位置點(diǎn)坐標(biāo)。

貝塞爾曲線擬合的方法是:在一個平面上選擇3個不同線的點(diǎn)A、B、C,將三個點(diǎn)依次連成一條直線,在線段AB和BC上分別選擇點(diǎn)D和E,使AD/AB=BE/BC,連接DE,并在DE上確定一點(diǎn)F,使得DF/DE=AD/AB=BE/BC,如圖6所示,讓選取的點(diǎn)D在第一條線段上從點(diǎn)A移動到點(diǎn)B,然后確定所有點(diǎn)F,并將點(diǎn)F移動的軌跡連接起來,所得到的曲線就是貝塞爾曲線。

圖6 原理圖

3 實(shí)驗(yàn)與分析

3.1 仿真實(shí)驗(yàn)

在噴漿機(jī)器人工作過程中,煤礦環(huán)境中存在著大小不一的施工材料和障礙物。為了對本文提出的改進(jìn)A*算法的有效性進(jìn)行驗(yàn)證,使用經(jīng)典A*算法與本文所提的改進(jìn)A*算法在不同大小的柵格地圖上展開仿真試驗(yàn)。實(shí)驗(yàn)環(huán)境為CPU Inter Core i7 12700F,內(nèi)存16GB,實(shí)驗(yàn)工具M(jìn)ATLAB 2019a。本文首先在尺寸20×20的柵格地圖上展開實(shí)驗(yàn),每個柵格是邊長為1個單位的正方形,柵格地圖障礙物按照20%的比例隨機(jī)生成,其中不同形狀的黑色柵格表示煤礦環(huán)境中不同大小的施工材料和障礙物。傳統(tǒng)A*算法與本文改進(jìn)A*算法的路徑規(guī)劃的實(shí)驗(yàn)結(jié)果如圖7所示,圖中三角形是路徑規(guī)劃的起點(diǎn),圓形是路徑規(guī)劃的的目標(biāo)點(diǎn)。圖7(a)中實(shí)線是經(jīng)典A*算法規(guī)劃的路徑,圖7(b)中黑色實(shí)線是改進(jìn)A*算法平滑后的路徑。經(jīng)過比較可以看出,在相同環(huán)境下,與經(jīng)典A*算法規(guī)劃的路徑相比,改進(jìn)A*算法規(guī)劃的路徑轉(zhuǎn)折角度小,安全性高,路徑更加平滑。

(a)經(jīng)典A*算法

仿真實(shí)驗(yàn)在3種不同尺寸但障礙率都為20%的柵格地圖上分別成功運(yùn)行10次(實(shí)驗(yàn)中存在障礙物堵住機(jī)器人前進(jìn)路徑的情況),記錄傳統(tǒng)A*算法和改進(jìn)A*算法運(yùn)行時路徑搜索時間和路徑長度。不同尺寸地圖的起點(diǎn)和目標(biāo)點(diǎn)不同。

地圖尺寸為20×20時,起點(diǎn)坐標(biāo)為(6,4),目標(biāo)點(diǎn)坐標(biāo)為(18,19)。

地圖尺寸為50×50時,起點(diǎn)坐標(biāo)為(3,40),目標(biāo)點(diǎn)坐標(biāo)為(46,10)。

地圖尺寸為100×100時,起點(diǎn)坐標(biāo)為(20,80),目標(biāo)點(diǎn)坐標(biāo)為(90,20)。

仿真結(jié)果見表1。

表1 路徑規(guī)劃參數(shù)對比表

表1給出了在不同大小的柵格地圖上,經(jīng)典A*算法相對于改進(jìn)A*算法路徑規(guī)劃的時間和規(guī)劃路徑的長度。從表1中能夠發(fā)現(xiàn),與經(jīng)典A*算法相比,改進(jìn)A*算法的尋路時間節(jié)省了50%左右,路徑長度節(jié)省了20%左右,這說明改進(jìn)A*算法路徑規(guī)劃的效率更高、路徑更短。

3.2 結(jié)果分析

經(jīng)過多次仿真實(shí)驗(yàn)得出的結(jié)果,分析得出路徑優(yōu)化的三個主要因素。

(1)評價(jià)函數(shù)的優(yōu)化。通過更改估計(jì)代價(jià)的權(quán)重,以減少路徑規(guī)劃過程中搜索節(jié)點(diǎn)的數(shù)量,減少了搜索時間,增加路徑規(guī)劃的效率。

(2)優(yōu)化子節(jié)點(diǎn)的選擇方式。通過改變子節(jié)點(diǎn)的鄰接方式,降低了路徑的拐點(diǎn),使得路徑更短,同時優(yōu)化子節(jié)點(diǎn)的選擇方式,通過增加噴漿機(jī)器人與障礙物之間的距離,從而避免危險(xiǎn)情況發(fā)生。

(3)路徑平滑。選擇貝塞爾曲線對路徑進(jìn)行平滑處理,減少了路徑的轉(zhuǎn)折角度,保證了噴漿機(jī)器人運(yùn)動時的平穩(wěn)性。

通過以上實(shí)驗(yàn)和分析可知,噴漿機(jī)器人采用改進(jìn)A*算法進(jìn)行路徑規(guī)劃,可以提高工作效率,保證安全性和平穩(wěn)性,具有一定的應(yīng)用價(jià)值。

結(jié)語

為了解決經(jīng)典A*算法在路徑規(guī)劃時存在的缺陷,本文對A*算法的評價(jià)函數(shù)、子節(jié)點(diǎn)的選擇方式以及路徑平滑作出了一定的優(yōu)化。改進(jìn)A*算法通過改變估計(jì)代價(jià)的權(quán)重來減少搜索路徑的時間,提高路徑規(guī)劃的效率;然后為了使噴漿機(jī)器人不會斜穿障礙物頂點(diǎn),對子節(jié)點(diǎn)的選擇方式進(jìn)行改進(jìn),增加了機(jī)器人與障礙物之間的安全距離,改善了機(jī)器人運(yùn)動過程中的安全性;最后對路徑進(jìn)行平滑優(yōu)化,減少路徑的拐點(diǎn),規(guī)劃出更平穩(wěn)的路徑。經(jīng)過實(shí)驗(yàn)分析,本文改進(jìn)的A*算法搜索時間少、路徑短、安全性高、路徑更加平滑,證明了所提改進(jìn)算法的優(yōu)越性,可用于煤礦環(huán)境中的噴漿機(jī)器人路徑規(guī)劃。