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

沈克宇,游志宇,劉永鑫

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

1 引言

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

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

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

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

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

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

(1)

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

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

(2)

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

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

(3)

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

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

(4)

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

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

3 改進A*算法

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

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

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

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

(5)

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

(6)

在場景地圖發(fā)生變化時,障礙物分布率P也會變化,此時自適應(yīng)權(quán)重W會隨P的改變而自適應(yīng)地調(diào)整,使之能適應(yīng)多種場景地圖,此時能夠進行自適應(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é)點位置n的父節(jié)點所在位置。

為了驗證增加了自適應(yīng)權(quán)重調(diào)整的啟發(fā)式函數(shù)在遍歷節(jié)點和搜索速度上的性能,在圖1所示30×30柵格地圖中進行對比測試,相關(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*算法對比

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

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

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

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

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

記(dx,dy)為當(dāng)前節(jié)點與鄰接節(jié)點之間橫縱坐標(biāo)的變化數(shù)值,S(i)為從當(dāng)前節(jié)點到鄰接節(jié)點的單步移動距離,此時的動態(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é)點為中心,向外畫圓時,搜索過程中會出現(xiàn)h(n)相等的情況。由于A*算法是基于最小移動距離原理進行路徑搜索,所以當(dāng)h(n)相等時,g(n)將占據(jù)算法的主導(dǎo)位置,起到引導(dǎo)搜索方向的決定性作用。故本文提出擬合優(yōu)先搜索策略,通過對搜索鄰域分配優(yōu)先級,以增強g(n)的搜索啟發(fā)性,如圖3所示。當(dāng)移動方向靠近搜索方向時,實際移動距離變小,此時尋優(yōu)路徑向最優(yōu)方向擬合,搜索速度得到提升。圖3中,黑色虛線表示從起始節(jié)點(父節(jié)點)到目標(biāo)節(jié)點的搜索向量,黑色實線為從起始節(jié)點向子節(jié)點的移動向量,向量間的夾角記作θ。

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

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

(9)

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

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

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

(10)

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

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

(11)

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

(12)

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

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

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

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

3.3.1 局部剪枝

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

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

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

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

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

Judge_Tri=

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

(13)

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

剪枝完三角形局部冗余后,再對梯形冗余進行剪枝操作。記從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,說明此時局部路徑呈梯形,需要對從a到d的節(jié)點進行判斷。若經(jīng)過障礙物不處理,否則直接對路徑剪枝。

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

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

3.3.2 冗余節(jié)點刪除

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

冗余節(jié)點刪除原理示意圖如圖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)路徑。

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

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

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

4 實驗仿真

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

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

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

Table 2 Experimental results of path planning in figure 8

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

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

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

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

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

Table 3 Testing results of five different starting and target nodes

5 結(jié)束語

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