基于擬合優先搜索的多場景自適應改進A*算法*

沈克宇,游志宇,劉永鑫

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

1 引言

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

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

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

2 傳統A*算法

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

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

(1)

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

常用的h(n)計算方法有歐幾里得距離(Euclidean Distance)、曼哈頓距離(Manhattan Distance)和對角線距離(Diagonal Distance)。假設起點坐標為(s1,s2),終點坐標為(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|。

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

3 改進A*算法

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

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

3.1 自適應權重調整

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

(5)

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

(6)

在場景地圖發生變化時,障礙物分布率P也會變化,此時自適應權重W會隨P的改變而自適應地調整,使之能適應多種場景地圖,此時能夠進行自適應權重調整的啟發式函數h′(n)如式(7)所示:

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

(7)

其中,(n-1)表示當前節點位置n的父節點所在位置。

為了驗證增加了自適應權重調整的啟發式函數在遍歷節點和搜索速度上的性能,在圖1所示30×30柵格地圖中進行對比測試,相關結果如表1所示。

Figure 1 Comparison of traditional A* algorithm and the algorithm with adaptive weight adjustment圖1 傳統A*算法與增加自適應權重調整的A*算法對比

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

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

根據圖1和表1的數據可知,增加自適應權重調整的A*算法所遍歷的節點數遠少于傳統A*算法的,且尋路時間更短。

3.2 擬合優先搜索

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

記(dx,dy)為當前節點與鄰接節點之間橫縱坐標的變化數值,S(i)為從當前節點到鄰接節點的單步移動距離,此時的動態規劃矩陣如式(8)所示:

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

(8)

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

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

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

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

(9)

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

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

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

(10)

此時擴展鄰域并分配優先級的g(n)動態規劃矩陣如式(11)所示:

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

(11)

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

(12)

在圖1的場景地圖中進行驗證,測試結果如圖4所示。圖4中黑色實線為在自適應權重調整基礎上增加擬合優先搜索策略后規劃出的路徑。與圖1b對比,圖4中灰色柵格覆蓋面積更少,可以看出路徑向目標節點擬合程度較高。結果表明,遍歷節點數比表1中自適應權重調整算法的減少10個,尋路時間由表1中的0.161 s減少到0.142 s,此時算法搜索效率更高?？梢娫谧赃m應權重調整算法的基礎上對其進行優先擬合搜索使得改進A*算法的性能更為優越。

Figure 4 Test result of algorithm introducing fitting-first search圖4 引入擬合優先搜索的算法測試結果

3.3 局部剪枝與冗余節點刪除

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

3.3.1 局部剪枝

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

圖5a所示為三角形局部冗余路徑,圖5b為梯形局部冗余路徑。其中,灰色圓球代表的a、b、c、d均是路徑上相鄰的節點,黑色為障礙物。圖5a中若需要從a點到達c點,可以直接從a到c,并不需要經過中間節點b。圖5b中若需要從a點到達d點,可以直接從a到d,并不需要經過中間節點b和c。圖5的三角形和梯形路徑存在局部最優解,導致全局路徑無法最優,因此需要對局部冗余路徑進行剪枝處理,以避免出現局部最優。 記4個相鄰節點a、b、c、d的坐標分別為(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搜索鄰域進行路徑規劃,一旦路徑出現三角形局部最優的情況,該三角形一定是等腰直角三角形,此時可以通過坐標位置關系或向量垂直原理進行判斷。

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

Judge_Tri=

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

(13)

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

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

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

(d2-a2)×(c1-b1)

(14)

若Judge_Tra為0,說明此時局部路徑呈梯形,需要對從a到d的節點進行判斷。若經過障礙物不處理,否則直接對路徑剪枝。

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

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

3.3.2 冗余節點刪除

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

冗余節點刪除原理示意圖如圖7a所示,圖中A→B→C→D為規劃出的路徑。假設A→D、A→C和B→D均未經過障礙物,可直接得出A→D為最優路徑。若A→D經過障礙物,再比較A→C→D與A→B→D這2條路徑的總距離,距離最小的為最優路徑。

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

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

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

4 實驗仿真

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

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

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

Table 2 Experimental results of path planning in figure 8

從本文改進A*算法與傳統A*算法在以上3種不同場景地圖中的規劃路線和表2中相關數據可知,本文改進A*算法魯棒性和實時性更強,能適應多種場景地圖,且規劃出的路徑更優。

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

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

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

Figure 9 Test results of different path planning algorithms圖9 不同路徑規劃算法測試結果

Table 3 Testing results of five different starting and target nodes

5 結束語

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