基于擬合優(yōu)先搜索的多場(chǎng)景自適應(yīng)改進(jìn)A*算法*

沈克宇,游志宇,劉永鑫

(西南民族大學(xué)電氣工程學(xué)院,四川 成都 610225)

1 引言

近年來,移動(dòng)機(jī)器人技術(shù)已經(jīng)逐漸替代傳統(tǒng)人工作業(yè),被廣泛應(yīng)用到工業(yè)、服務(wù)業(yè)、城市安全和空間探測(cè)等領(lǐng)域。路徑規(guī)劃是移動(dòng)機(jī)器人任務(wù)管理系統(tǒng)的關(guān)鍵技術(shù)之一,是指在有障礙物的工作環(huán)境中規(guī)劃出一條從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的無碰撞路徑,并使規(guī)劃路徑滿足某些優(yōu)化原則(如搜索速度更快、路徑距離更短等)成為最優(yōu)路徑[1]。根據(jù)對(duì)場(chǎng)景地圖信息掌握的程度,無人駕駛機(jī)器人路徑規(guī)劃可分為局部路徑規(guī)劃和全局路徑規(guī)劃[2,3]。局部路徑規(guī)劃需要硬件設(shè)備能實(shí)時(shí)采集周圍場(chǎng)景地圖信息,對(duì)環(huán)境誤差和噪聲有較高的魯棒性。全局路徑規(guī)劃基于先驗(yàn)全局地圖信息,規(guī)劃出的路徑精度取決于獲取的場(chǎng)景地圖信息的準(zhǔn)確度。

在全局路徑規(guī)劃算法中,基于啟發(fā)式搜索的A*算法[4,5]擁有出色的路徑最優(yōu)性、尋路完備性和搜索高效性,多被應(yīng)用于靜態(tài)場(chǎng)景地圖的路徑規(guī)劃中,一直是研究人員研究改進(jìn)的重點(diǎn)。文獻(xiàn)[6]通過改變距離計(jì)算方式與函數(shù)權(quán)重來縮短尋路時(shí)間、減少冗余節(jié)點(diǎn)數(shù)量。文獻(xiàn)[7]將切比雪夫距離作為啟發(fā)式函數(shù)因子,同時(shí)引入父節(jié)點(diǎn)增強(qiáng)其影響力,使得搜索速度極大提升。文獻(xiàn)[8]通過擴(kuò)展到20搜索鄰域,并設(shè)置了安全距離,使規(guī)劃出的路徑更平滑且距離更短。文獻(xiàn)[9]設(shè)計(jì)了新型距離計(jì)算方式,使得最終規(guī)劃出的路徑距離更短。文獻(xiàn)[10]針對(duì)復(fù)雜場(chǎng)景地圖引入安全因子使路徑盡量避開危險(xiǎn)區(qū)域,適用于機(jī)器人運(yùn)行。文獻(xiàn)[11,12]都采用雙向搜索方式以增強(qiáng)算法實(shí)時(shí)性,文獻(xiàn)[12]還引入了扇形鄰域擴(kuò)展,使搜索更有導(dǎo)向性。文獻(xiàn)[13]利用模擬退火法取代傳統(tǒng)算法的距離判斷方式,使得搜索更快地向目標(biāo)節(jié)點(diǎn)進(jìn)行。文獻(xiàn)[14]通過引進(jìn)時(shí)間因子對(duì)估價(jià)函數(shù)進(jìn)行改進(jìn),使得改進(jìn)算法可以節(jié)約規(guī)劃時(shí)間、降低路程成本。文獻(xiàn)[15]在傳統(tǒng)A*算法中引入動(dòng)態(tài)窗口法,提升路徑平滑處理的靈活性并滿足移動(dòng)機(jī)器人非完整性約束條件。

從上述文獻(xiàn)中可以看出,對(duì)A*算法的改進(jìn)主要包括優(yōu)化距離計(jì)算函數(shù)、擴(kuò)展鄰域以及與其它智能算法相結(jié)合,但這些改進(jìn)的普適性和魯棒性較弱,且部分對(duì)計(jì)算機(jī)性能要求較高。本文針對(duì)部分文獻(xiàn)中的改進(jìn)A*算法存在魯棒性差、遍歷節(jié)點(diǎn)數(shù)多、路徑轉(zhuǎn)折角度較大和搜索速度較慢的問題,基于傳統(tǒng)A*算法,在二維靜態(tài)環(huán)境下以移動(dòng)機(jī)器人為載體,提出一種兼顧算法魯棒性和普適性且計(jì)算效率較高的基于擬合優(yōu)先搜索的多場(chǎng)景自適應(yīng)改進(jìn)A*算法。首先,通過自適應(yīng)控制機(jī)制實(shí)現(xiàn)啟發(fā)式搜索,根據(jù)地圖變化適時(shí)調(diào)整權(quán)重。其次,引入擬合優(yōu)先搜索,增強(qiáng)算法的啟發(fā)性。接著,通過路徑剪枝和冗余節(jié)點(diǎn)刪除機(jī)制,對(duì)路徑進(jìn)行導(dǎo)向規(guī)劃和平滑處理。在相同柵格場(chǎng)景地圖中,傳統(tǒng)A*算法與本文改進(jìn)A*算法的仿真結(jié)果表明,本文改進(jìn)A*算法在路徑規(guī)劃時(shí)遍歷節(jié)點(diǎn)更少、路徑更平滑、搜索速度更快,對(duì)多場(chǎng)景地圖具有更強(qiáng)的魯棒性和普適性。

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

A*算法是由Hart等人[4]提出的,在Dijkstra算法基礎(chǔ)上發(fā)展而來。它根據(jù)定義的代價(jià)函數(shù)在靜態(tài)環(huán)境中尋找一條從起始位置到目標(biāo)位置的最小移動(dòng)距離路徑,其代價(jià)函數(shù)表達(dá)式如式(1)所示:

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

(1)

其中,n表示當(dāng)前節(jié)點(diǎn)所在位置;f(n)表示從起始節(jié)點(diǎn)位置到當(dāng)前節(jié)點(diǎn)位置的代價(jià)函數(shù);g(n)表示移動(dòng)機(jī)器人從起始節(jié)點(diǎn)位置到當(dāng)前節(jié)點(diǎn)位置的實(shí)際移動(dòng)代價(jià);h(n)表示從當(dāng)前節(jié)點(diǎn)位置到目標(biāo)節(jié)點(diǎn)位置的預(yù)估移動(dòng)代價(jià),稱為啟發(fā)式函數(shù)。在路徑規(guī)劃時(shí),若h(n)遠(yuǎn)小于g(n),此時(shí)A*算法近似于Dijkstra算法,遍歷節(jié)點(diǎn)會(huì)增多,搜索速度將大大降低;若h(n)遠(yuǎn)大于g(n),此時(shí)A*算法約等于最佳優(yōu)先搜索算法,搜索速度提高,但容易出現(xiàn)局部最優(yōu)解。因此,選擇合適的啟發(fā)式函數(shù)h(n)將會(huì)影響A*算法的規(guī)劃性能。

常用的h(n)計(jì)算方法有歐幾里得距離(Euclidean Distance)、曼哈頓距離(Manhattan Distance)和對(duì)角線距離(Diagonal Distance)。假設(shè)起點(diǎn)坐標(biāo)為(s1,s2),終點(diǎn)坐標(biāo)為(g1,g2),則歐幾里得距離的計(jì)算公式如式(2)所示:

(2)

曼哈頓距離的計(jì)算公式如式(3)所示:

h=|s1-g1|+|s2-g2|

(3)

對(duì)角線距離的計(jì)算公式如式(4)所示:

h=1.4×Diagonal+(Straight-2×Diagonal)

(4)

其中,Diagonal=min(|s1-g1|,|s2-g2|),Straight=|s1-g1|+|s2-g2|。

由于歐幾里得距離的計(jì)算精度高于曼哈頓距離和對(duì)角線距離的,得出最優(yōu)路徑的可能性更大,故A*算法選取歐幾里得距離進(jìn)行移動(dòng)代價(jià)預(yù)估。

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

傳統(tǒng)A*算法在進(jìn)行路徑規(guī)劃時(shí),需要遍歷的節(jié)點(diǎn)數(shù)較多、搜索速度慢、魯棒性較差,而且規(guī)劃出的路徑存在許多冗余節(jié)點(diǎn),路徑總轉(zhuǎn)折角過大而不適合移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)原理,這些不足都限制了A*算法的應(yīng)用。

本文從自適應(yīng)權(quán)重調(diào)整、擬合優(yōu)先搜索以及局部剪枝與冗余節(jié)點(diǎn)刪除這3個(gè)方面對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn),以保證改進(jìn)后算法最終規(guī)劃出的路徑相對(duì)最優(yōu),增強(qiáng)魯棒性和搜索效率,減少遍歷節(jié)點(diǎn)數(shù)和總轉(zhuǎn)折角度,從而緩解移動(dòng)機(jī)器人的存儲(chǔ)壓力,降低能源損耗。

3.1 自適應(yīng)權(quán)重調(diào)整

傳統(tǒng)A*算法在遍歷節(jié)點(diǎn)時(shí)存在循環(huán)訪問判斷的情況,搜索效率較低。因此,本文引入父節(jié)點(diǎn)并增強(qiáng)其啟發(fā)能力,以減少遍歷節(jié)點(diǎn)數(shù),同時(shí)提高搜索速度。為了增強(qiáng)算法的魯棒性和普適性,同時(shí)避免因啟發(fā)函數(shù)過強(qiáng)導(dǎo)致規(guī)劃出的路徑出現(xiàn)更多局部最優(yōu)解,本文設(shè)計(jì)了能隨場(chǎng)景地圖變換的自適應(yīng)權(quán)重W,如式(5)所示:

(5)

在規(guī)劃之前先量化標(biāo)定障礙物所在柵格,將柵格地圖中可通行節(jié)點(diǎn)總數(shù)記作Sum0,障礙物節(jié)點(diǎn)總數(shù)記作Sum1,則式(5)中的P表示起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)所圍地圖的障礙物分布率,其計(jì)算如式(6)所示:

(6)

在場(chǎng)景地圖發(fā)生變化時(shí),障礙物分布率P也會(huì)變化,此時(shí)自適應(yīng)權(quán)重W會(huì)隨P的改變而自適應(yīng)地調(diào)整,使之能適應(yīng)多種場(chǎng)景地圖,此時(shí)能夠進(jìn)行自適應(yīng)權(quán)重調(diào)整的啟發(fā)式函數(shù)h′(n)如式(7)所示:

h′(n)=W×[h(n)+h(n-1)]

(7)

其中,(n-1)表示當(dāng)前節(jié)點(diǎn)位置n的父節(jié)點(diǎn)所在位置。

為了驗(yàn)證增加了自適應(yīng)權(quán)重調(diào)整的啟發(fā)式函數(shù)在遍歷節(jié)點(diǎn)和搜索速度上的性能,在圖1所示30×30柵格地圖中進(jìn)行對(duì)比測(cè)試,相關(guān)結(jié)果如表1所示。

Figure 1 Comparison of traditional A* algorithm and the algorithm with adaptive weight adjustment圖1 傳統(tǒng)A*算法與增加自適應(yīng)權(quán)重調(diào)整的A*算法對(duì)比

Table 1 Results comparison of traditional A* algorithm and the algorithm with adaptive weight adjustment

圖1中,圓形為起始節(jié)點(diǎn),五角星為目標(biāo)節(jié)點(diǎn),黑色為障礙物,灰色節(jié)點(diǎn)為遍歷且收錄的節(jié)點(diǎn),淺灰色為在判斷后丟棄的節(jié)點(diǎn),黑色實(shí)線為最終路徑。

根據(jù)圖1和表1的數(shù)據(jù)可知,增加自適應(yīng)權(quán)重調(diào)整的A*算法所遍歷的節(jié)點(diǎn)數(shù)遠(yuǎn)少于傳統(tǒng)A*算法的,且尋路時(shí)間更短。

3.2 擬合優(yōu)先搜索

傳統(tǒng)A*算法的搜索范圍為4鄰域或圖2a中的8鄰域。研究人員針對(duì)搜索鄰域的擴(kuò)增與縮減展開研究,當(dāng)鄰域增大為16鄰域甚至無窮大時(shí),能保證尋路最優(yōu),但搜索速度變慢;當(dāng)縮減鄰域到3鄰域甚至單向搜索時(shí),搜索速度提高但無法保證尋路最優(yōu),且可能搜索失敗。由此可見,無論是擴(kuò)大鄰域還是縮減鄰域,都有一定的局限性,故本文并沒有針對(duì)可擴(kuò)展鄰域的數(shù)量進(jìn)行改進(jìn),而是重新設(shè)計(jì)傳統(tǒng)8鄰域中的動(dòng)態(tài)規(guī)劃矩陣Motion。

記(dx,dy)為當(dāng)前節(jié)點(diǎn)與鄰接節(jié)點(diǎn)之間橫縱坐標(biāo)的變化數(shù)值,S(i)為從當(dāng)前節(jié)點(diǎn)到鄰接節(jié)點(diǎn)的單步移動(dòng)距離,此時(shí)的動(dòng)態(tài)規(guī)劃矩陣如式(8)所示:

Motion=[dx,dy,S(i)]

(8)

Figure 2 Traditional 8 neighborhoods search and its limitations圖2 傳統(tǒng)8鄰域搜索與其局限性

如圖2b所示,以目標(biāo)節(jié)點(diǎn)為中心,向外畫圓時(shí),搜索過程中會(huì)出現(xiàn)h(n)相等的情況。由于A*算法是基于最小移動(dòng)距離原理進(jìn)行路徑搜索,所以當(dāng)h(n)相等時(shí),g(n)將占據(jù)算法的主導(dǎo)位置,起到引導(dǎo)搜索方向的決定性作用。故本文提出擬合優(yōu)先搜索策略,通過對(duì)搜索鄰域分配優(yōu)先級(jí),以增強(qiáng)g(n)的搜索啟發(fā)性,如圖3所示。當(dāng)移動(dòng)方向靠近搜索方向時(shí),實(shí)際移動(dòng)距離變小,此時(shí)尋優(yōu)路徑向最優(yōu)方向擬合,搜索速度得到提升。圖3中,黑色虛線表示從起始節(jié)點(diǎn)(父節(jié)點(diǎn))到目標(biāo)節(jié)點(diǎn)的搜索向量,黑色實(shí)線為從起始節(jié)點(diǎn)向子節(jié)點(diǎn)的移動(dòng)向量,向量間的夾角記作θ。

Figure 3 Vector angle representation圖3 向量夾角表示

記a=(ax,ay),b=(bx,by),向量間夾角θ的余弦如式(9)所示:

(9)

根據(jù)搜索向量與移動(dòng)向量的位置關(guān)系,本文對(duì)傳統(tǒng)8鄰域的動(dòng)態(tài)規(guī)劃矩陣Motion進(jìn)行設(shè)計(jì)。由式(9)可知,向量間夾角θ越小,其移動(dòng)方向與最優(yōu)搜索方向越擬合,故設(shè)計(jì)關(guān)于cosθ的函數(shù)作為搜索鄰域時(shí)單步移動(dòng)距離S(i)的權(quán)重,確保在尋路過程中優(yōu)先選擇與搜索向量擬合的鄰域,從而增強(qiáng)g(n)的啟發(fā)性和導(dǎo)向性,提高搜索速度。但該函數(shù)取值應(yīng)該適中,若是過大則無法用于復(fù)雜地圖環(huán)境,若是過小則路徑優(yōu)化不夠明顯。

改進(jìn)后的S′(i)如式(10)所示:

S′(i)=[(1-cosθ)/8+1]×S(i)=

(10)

此時(shí)擴(kuò)展鄰域并分配優(yōu)先級(jí)的g(n)動(dòng)態(tài)規(guī)劃矩陣如式(11)所示:

Motion=[dx,dy,S′(i)]

(11)

由式(1)及A*算法尋路原理可得擬合優(yōu)先搜索f(n)如式(12)所示:

(12)

在圖1的場(chǎng)景地圖中進(jìn)行驗(yàn)證,測(cè)試結(jié)果如圖4所示。圖4中黑色實(shí)線為在自適應(yīng)權(quán)重調(diào)整基礎(chǔ)上增加擬合優(yōu)先搜索策略后規(guī)劃出的路徑。與圖1b對(duì)比,圖4中灰色柵格覆蓋面積更少,可以看出路徑向目標(biāo)節(jié)點(diǎn)擬合程度較高。結(jié)果表明,遍歷節(jié)點(diǎn)數(shù)比表1中自適應(yīng)權(quán)重調(diào)整算法的減少10個(gè),尋路時(shí)間由表1中的0.161 s減少到0.142 s,此時(shí)算法搜索效率更高?？梢娫谧赃m應(yīng)權(quán)重調(diào)整算法的基礎(chǔ)上對(duì)其進(jìn)行優(yōu)先擬合搜索使得改進(jìn)A*算法的性能更為優(yōu)越。

Figure 4 Test result of algorithm introducing fitting-first search圖4 引入擬合優(yōu)先搜索的算法測(cè)試結(jié)果

3.3 局部剪枝與冗余節(jié)點(diǎn)刪除

從圖4中可以明顯看到,路徑存在局部最優(yōu)解,而且經(jīng)過了大量的冗余節(jié)點(diǎn),故還需要對(duì)之前規(guī)劃出的路徑進(jìn)行平滑處理。首先通過局部剪枝去除路徑上的局部最優(yōu)解,之后再刪除路徑上的冗余節(jié)點(diǎn),以保證最終規(guī)劃出的路徑經(jīng)過的節(jié)點(diǎn)數(shù)更少且路徑更為平滑。

3.3.1 局部剪枝

因?yàn)閳?chǎng)景地圖中存在多種障礙物分布情況以及8鄰域搜索的角度局限性,故可能出現(xiàn)三角形和梯形局部冗余。即使是更復(fù)雜的局部冗余路徑,也可以先將三角形剪枝為梯形,再對(duì)梯形剪枝,從而形成平滑路徑。

圖5a所示為三角形局部冗余路徑,圖5b為梯形局部冗余路徑。其中,灰色圓球代表的a、b、c、d均是路徑上相鄰的節(jié)點(diǎn),黑色為障礙物。圖5a中若需要從a點(diǎn)到達(dá)c點(diǎn),可以直接從a到c,并不需要經(jīng)過中間節(jié)點(diǎn)b。圖5b中若需要從a點(diǎn)到達(dá)d點(diǎn),可以直接從a到d,并不需要經(jīng)過中間節(jié)點(diǎn)b和c。圖5的三角形和梯形路徑存在局部最優(yōu)解,導(dǎo)致全局路徑無法最優(yōu),因此需要對(duì)局部冗余路徑進(jìn)行剪枝處理,以避免出現(xiàn)局部最優(yōu)。 記4個(gè)相鄰節(jié)點(diǎn)a、b、c、d的坐標(biāo)分別為(a1,a2),(b1,b2),(c1,c2)和(d1,d2)。

Figure 5 Schematic of local redundant paths圖5 局部冗余路徑示意圖

對(duì)于圖5a的三角形局部冗余路徑,將從a到b的向量記作AB,將從b到c的向量記作BC,將從a到c的向量記作AC。本文采用8搜索鄰域進(jìn)行路徑規(guī)劃,一旦路徑出現(xiàn)三角形局部最優(yōu)的情況,該三角形一定是等腰直角三角形,此時(shí)可以通過坐標(biāo)位置關(guān)系或向量垂直原理進(jìn)行判斷。

向量垂直判斷函數(shù)如式(13)所示:

Judge_Tri=

(AB·BC)×(AC·BC)×(AC·AB)

(13)

若Judge_Tri為0,表示此時(shí)路徑可能存在三角形局部冗余,需要對(duì)路徑進(jìn)行判斷:若對(duì)局部最優(yōu)路徑剪枝后經(jīng)過障礙物,不處理;若a與c可直連且未經(jīng)障礙物,可以直接沿著從a到c的方向剪枝。

剪枝完三角形局部冗余后,再對(duì)梯形冗余進(jìn)行剪枝操作。記從a到d的向量AD=(d1-a1,d2-a2),從b到c的向量記作BC=(c1-b1,c2-b2)。通過向量平行原理判斷路徑中的梯形冗余,其判斷函數(shù)如式(14)所示:

Judge_Tra=(d1-a1)×(c2-b2)-

(d2-a2)×(c1-b1)

(14)

若Judge_Tra為0,說明此時(shí)局部路徑呈梯形,需要對(duì)從a到d的節(jié)點(diǎn)進(jìn)行判斷。若經(jīng)過障礙物不處理,否則直接對(duì)路徑剪枝。

Figure 6 Path pruning demonstration圖6 路徑剪枝演示

為驗(yàn)證路徑剪枝的有效性,針對(duì)圖1所示地圖進(jìn)行剪枝處理仿真測(cè)試,測(cè)試結(jié)果如圖6所示。其中虛線為原始路徑,實(shí)線為剪枝后路徑。從圖6中可知,剪枝可以有效解決復(fù)雜地圖中出現(xiàn)的局部最優(yōu)路徑的問題。

3.3.2 冗余節(jié)點(diǎn)刪除

剪枝完成后,還需要對(duì)路徑上的冗余節(jié)點(diǎn)進(jìn)行處理,從而減少路徑經(jīng)過的節(jié)點(diǎn)數(shù),使得最終路徑更平滑。首先,計(jì)算初始路徑中鄰近3個(gè)節(jié)點(diǎn)間的位置關(guān)系,通過向量共線原理判斷并存儲(chǔ)路徑中的拐點(diǎn);接著,依次判斷鄰近4個(gè)拐點(diǎn)是否可以直連,直連后若未經(jīng)過障礙物且總距離最短,則記錄該拐點(diǎn)位置,并刪除初始路徑中位于直連拐點(diǎn)之間的冗余節(jié)點(diǎn);隨后,以記錄拐點(diǎn)為起點(diǎn),再與隨后3個(gè)鄰近拐點(diǎn)進(jìn)行直連判斷,直到目標(biāo)節(jié)點(diǎn)為止。更新路徑節(jié)點(diǎn)作為新規(guī)劃路徑,再次循環(huán)判斷,直到路徑上沒有冗余節(jié)點(diǎn),此時(shí)得到的路徑即為最終路徑。

冗余節(jié)點(diǎn)刪除原理示意圖如圖7a所示,圖中A→B→C→D為規(guī)劃出的路徑。假設(shè)A→D、A→C和B→D均未經(jīng)過障礙物,可直接得出A→D為最優(yōu)路徑。若A→D經(jīng)過障礙物,再比較A→C→D與A→B→D這2條路徑的總距離,距離最小的為最優(yōu)路徑。

為驗(yàn)證路徑剪枝和冗余節(jié)點(diǎn)刪除策略的有效性,在圖4場(chǎng)景中進(jìn)行路徑規(guī)劃測(cè)試,結(jié)果如圖7b所示。其中,虛線為初始路徑,實(shí)線為經(jīng)過路徑剪枝與冗余節(jié)點(diǎn)刪除之后的路徑,深灰色方格表示最終經(jīng)過的節(jié)點(diǎn)。

從圖7b中可知,刪除冗余節(jié)點(diǎn)后的規(guī)劃路徑需要存儲(chǔ)的遍歷節(jié)點(diǎn)數(shù)大大減少,路徑更為平滑,經(jīng)過的節(jié)點(diǎn)數(shù)也更少。此時(shí)僅需要記憶幾個(gè)拐點(diǎn)就可以規(guī)劃出完整的路徑,有效緩解了移動(dòng)機(jī)器人的存儲(chǔ)壓力。

Figure 7 Removing redundant node principle and path smoothing test圖7 刪除冗余節(jié)點(diǎn)原理與路徑平滑測(cè)試

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

為了驗(yàn)證本文改進(jìn)A*算法的優(yōu)越性,在Intel?CoreTMi5-7300HQ CPU @ 2.50 GHz計(jì)算機(jī)上利用MATLAB 2020b對(duì)傳統(tǒng)A*算法和本文改進(jìn)A*算法進(jìn)行仿真測(cè)試。

在圖8所示的3種場(chǎng)景中進(jìn)行路徑規(guī)劃,圖中黑色虛線表示傳統(tǒng)A*算法規(guī)劃出的路徑,黑色實(shí)線為本文改進(jìn)A*算法規(guī)劃出的路徑。相關(guān)仿真實(shí)驗(yàn)結(jié)果如表2所示。

Figure 8 Test results of the proposed improved A* algorithm in different maps圖8 本文改進(jìn)A*算法在不同場(chǎng)景地圖中的測(cè)試結(jié)果

Table 2 Experimental results of path planning in figure 8

從本文改進(jìn)A*算法與傳統(tǒng)A*算法在以上3種不同場(chǎng)景地圖中的規(guī)劃路線和表2中相關(guān)數(shù)據(jù)可知,本文改進(jìn)A*算法魯棒性和實(shí)時(shí)性更強(qiáng),能適應(yīng)多種場(chǎng)景地圖,且規(guī)劃出的路徑更優(yōu)。

為了進(jìn)一步論述本文改進(jìn)A*算法的優(yōu)越性和穩(wěn)定性,在模仿小區(qū)環(huán)境構(gòu)建的50×50柵格地圖中選取5組不同起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)進(jìn)行測(cè)試,分別為[ (7,7),(42,43)],[(7,47),(47,7)],[(37,33),(14,13)],[(26,25),(7,18)],[(49,7),(13,47)]。

以第1組起始節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)為例,分別使用傳統(tǒng)A*算法、文獻(xiàn)[6]改進(jìn)A*算法和本文改進(jìn)A*算法進(jìn)行路徑搜索,最終規(guī)劃出的路徑如圖9所示。5組不同起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)測(cè)試的相關(guān)結(jié)果如表3所示。

根據(jù)表3可知,本文改進(jìn)A*算法與傳統(tǒng)A*算法和文獻(xiàn)[6]改進(jìn)A*算法相比,遍歷節(jié)點(diǎn)數(shù)平均減少了72.19%和28.36%,在刪除冗余節(jié)點(diǎn)后極大緩解了計(jì)算機(jī)存儲(chǔ)壓力;總轉(zhuǎn)折角度平均減少了64.33%和28.59%,規(guī)劃出的路徑更為平滑,降低了移動(dòng)機(jī)器人與障礙物發(fā)生碰撞的幾率;尋路時(shí)間平均減少了68.51%和27.15%,實(shí)時(shí)性更強(qiáng),搜索速度更快,一定程度上解決了機(jī)器人在轉(zhuǎn)彎時(shí)額外產(chǎn)生的能源損耗和時(shí)間等待問題。

Figure 9 Test results of different path planning algorithms圖9 不同路徑規(guī)劃算法測(cè)試結(jié)果

Table 3 Testing results of five different starting and target nodes

5 結(jié)束語

本文針對(duì)傳統(tǒng)A*算法存在遍歷節(jié)點(diǎn)數(shù)較多、總轉(zhuǎn)折角度過大和搜索速度較慢等不足,提出了基于擬合優(yōu)先搜索的多場(chǎng)景自適應(yīng)改進(jìn)A*算法。首先,引入父節(jié)點(diǎn)的啟發(fā)距離,設(shè)計(jì)能自適應(yīng)地圖變化的啟發(fā)式搜索權(quán)重,以減少遍歷的節(jié)點(diǎn)數(shù),提高搜索速度;然后,引入擬合優(yōu)先搜索進(jìn)一步增強(qiáng)算法啟發(fā)性;最后,對(duì)初步規(guī)劃出的路徑進(jìn)行局部剪枝并刪除冗余節(jié)點(diǎn),最后得出最終路徑。仿真測(cè)試結(jié)果表明,本文提出的改進(jìn)A*算法的魯棒性更強(qiáng),遍歷節(jié)點(diǎn)數(shù)更少,路徑更平滑,搜索速度更快,更適用于移動(dòng)機(jī)器人的日常工作。