基于局部障礙率預獲取和雙向父節(jié)點變更的A*算法優(yōu)化

張志遠,陳海進,章一鳴

(南通大學信息科學技術學院,江蘇 南通 226019)

1 引言

隨著人工智能產業(yè)的高速發(fā)展,掃地機器人和送餐機器人等逐漸在人們的生活中普及,為現(xiàn)代化生活注入了“信息”基因。作為移動機器人[1]研究領域的核心,路徑規(guī)劃問題[2-4]的研究自20世紀70年代至今從未停止,研究人員積累了大量的研究成果。主流的傳統(tǒng)路徑規(guī)劃方法有可視圖法[5,6]、拓撲法、柵格法、Dijkstra算法和A*算法[7,8]等,其中可視圖法、拓撲法和柵格法通常應用于機器人感知環(huán)境信息的地圖建模[9],而Dijkstra算法和A*算法通常應用于最優(yōu)路徑的求解[10-13]。

近年來,相關學者為優(yōu)化機器人運動軌跡,提高機器人的運行效率,根據(jù)傳統(tǒng)A*算法的缺點進行了各自相應的改進。文獻[14]采用歐氏距離和曼哈頓距離的線性組合作為評價函數(shù),使得評價值更接近實際值,一定程度上提高了搜索效率,但并未明確給出評價函數(shù)各部分的權值的分配方案。文獻[15]根據(jù)機器人已走路徑在總路徑中的占比動態(tài)分配評價函數(shù)各個部分的權值,已達到優(yōu)化評價函數(shù)的目的,一定程度上優(yōu)化了軌跡平滑度。但是,該算法不考慮地圖局部障礙率的高低,可能導致在局部障礙率較低的區(qū)域搜索較大的空間,降低搜索效率;也可能導致在局部障礙率較高的區(qū)域搜索的空間較小,從而錯失最優(yōu)節(jié)點,陷入局部最優(yōu)。文獻[16]加權處理評價函數(shù),并通過人工搜索標記,縮小了搜索區(qū)域,犧牲搜索精度換取了搜索效率。文獻[17]提出一種動態(tài)啟發(fā)式算法,避免算法陷入局部最優(yōu),在保證搜索效率的前提下優(yōu)化了生成路徑。文獻[18]在復雜地形尋路的過程中,在評價函數(shù)中加入安全成本,在保證運行效率的同時,提高了機器人尋路的安全性。然而路徑規(guī)劃問題應不僅與搜索算法相關,更應該與環(huán)境地圖密切相關。

本文提出了基于局部障礙率預獲取和雙向父節(jié)點變更的改進A*算法,能有效地識別局部環(huán)境信息,依據(jù)環(huán)境復雜程度動態(tài)調整搜索空間,提高搜索效率,優(yōu)化路徑長度。首先,基于漂移矩陣預獲取地圖各個部分的局部障礙率信息,記錄各個矩陣內障礙率的值。其次,為矩陣內障礙率的值建立映射函數(shù),通過映射函數(shù)動態(tài)分配評價函數(shù)的權值,依據(jù)局部環(huán)境復雜程度靈活調整搜索空間。最后,依據(jù)改進的父節(jié)點變更方式,減少生成路徑的冗余點和拐點,進一步優(yōu)化路徑。實驗結果表明,本文改進后的算法有效縮短了路徑長度,提升了搜索效率,具有較強的適應性。

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

2.1 柵格法地圖建模

傳統(tǒng)A*算法用柵格法進行地圖建模,用二進制編碼0和1來表示環(huán)境地圖,0表示無障礙柵格,1表示障礙柵格。設機器人的工作區(qū)域為a′×b′的矩形區(qū)域,柵格單位長度為c,所得柵格地圖為m×n的矩陣,m和n數(shù)值如式(1)所示:

(1)

其中floor(·)為向下取整函數(shù)。

2.2 A*算法路徑規(guī)劃

A*算法每移動一格都要對周圍8鄰域柵格內的評價函數(shù)進行計算,其計算公式如式(2)所示:

(2)

其中,(x,y)為當前柵格s的坐標,(xS,yS)為起點S的坐標,(xD,yD)為終點D的坐標,g(s)為當前柵格s到起點的歐氏距離,h(s)為當前柵格s到終點的歐氏距離。依次將8鄰域內的可選柵格(無障礙柵格、非已走過柵格)放入open列表,并從open列表中選出評價函數(shù)f(·)值最小的柵格作為下一個節(jié)點柵格,并將當前節(jié)點柵格放入close列表。傳統(tǒng)A*算法從起點出發(fā)搜索至終點結束,生成路徑。

3 分配權重的A*算法及其相關性質證明

分配權重的A*算法對h(s)和g(s)賦予不同的權重a和b,以達到增大或縮小搜索區(qū)域的目的,其評價函數(shù)如式(3)所示:

fweight(s)=a·h(s)+b·g(s),a+b=1

(3)

對于分配權重的A*算法,有如下結論:

結論1當a<0.5

證明如圖1所示,當a<0.5g(B),且點B滿足以下關系:

Figure 1 Proof of the relationship between function weight and search area size

BS+BD=AS+AD=

g(B)+h(B)=g(A)+h(A)

(4)

由式(3)可得:

(5)

式(5)中兩式相減后,令b=a+a1,經變換得:

fweight(A)-fweight(B)=

a·(h(A)+g(A)-(h(B)+g(B)))+

a1·(g(A)-g(B))

(6)

由式(4)可得:

fweight(A)-fweight(B)=a1·(g(A)-g(B))

(7)

因為g(A)-g(B)>0,故在點B處,fweight(A)-fweight(B)>0。

過點B作起終點連線SD的垂線,當點B在垂線上向遠離SD方向移動時,h(B)和g(B)值也隨之增大,記增大后的值為h′(B)和g′(B),則:

(8)

其中,Δh(B)和Δg(B)為點B遠離SD帶來的增量。

由式(8)可將式(6)變換為:

fweight(A)-fweight(B)=-a·(Δh(B)+Δg(B))-

a1·Δg(B)+a1·(g(A)-g(B))

(9)

式(9)中第1項和第2項為負值增量,第3項為正值定量,可以看出點B移動產生的負值增量使得式(9)的值逐漸向0逼近,直至式(9)的值為0,點B停止運動,記此時點B運動至點P,fweight(A)=fweight(B)。

對于傳統(tǒng)A*算法,由式(2)和式(4)可得,在點B處就已經滿足f(A)=f(B)的關系,所以三角形SBD區(qū)域內的f(·)值均小于f(A)。故在點A被選取之前,三角形SBD區(qū)域內的點依次會被回溯選取為當前節(jié)點。三角形SBD即為此時新增的搜索區(qū)域。而對于分配權重的A*算法(a<0.5

□

結論2當a>0.5>b時,分配權重的A*算法搜索區(qū)域小于傳統(tǒng)A*算法的搜索區(qū)域。

證明式(5)中兩式相減后令a=b+b1,經變換可得:

fweight(A)-fweight(B)=

b·(h(A)+g(A)-(h(B)+

g(B)))+b1·(h(A)-h(B))

(10)

由式(4)得:

fweight(A)-fweight(B)=b1·(h(A)-h(B))

(11)

因為h(A)-h(B)<0,故在點B處,fweight(A)-fweight(B)<0。

將垂線上的點B向靠近SD方向移動,h(B)和g(B)的值隨之減小,記減小后的值為:

(12)

隨點B向靠近SD方向移動,由式(12)可將式(10)改寫為:

fweight(A)-fweight(B)=

b·(Δh(B)+Δg(B))+

b1·Δg(B)+b1·(h(A)-h(B))

(13)

式(13)中第1和第2項為正值增量,第3項為負值定量,可以看出點B向靠近SD方向移動產生的正值增量使式(13)的值逐漸向0逼近,直至其值為0,點B停止運動,記此時點B運動至點C,fweight(A)=fweight(B)。顯然,a>0.5>b時,三角形SCD即為新增的搜索區(qū)域,其面積小于三角形SBD,結論2得證。

□

結論3分配權重的A*算法搜索區(qū)域大小與a的值成反比,與b的值成正比。

證明當aΔg(B),所以總的負值增量減小,故點B需要運動到比點P更遠的位置才能使式(9)的值為0。在此種情況下,算法搜索區(qū)域增大,搜索區(qū)域大小與b值成正比,與a值成反比。

當a>b>0.5,a=b+b1時,增大a的值,b1的值增大,b的值減小,使得式(13)中,第1項正值增量系b數(shù)減小,第2項b1正值增量系數(shù)增大,由于Δh(B)+Δg(B)>Δg(B),總的正值增量減小,故點B需要運動至比點C更靠近SD的位置才能使式(13)的值為0。此時算法搜索區(qū)域縮小,搜索區(qū)域大小仍與b值成正比,與a值成反比,結論3得證。

□

4 基于局部障礙率預獲取的改進A*算法

本文提出基于局部障礙率預獲取的改進A*算法,利用漂移矩陣在對地圖的局部障礙率信息進行預獲取后,通過第3節(jié)分配權重評價函數(shù)已證明的相關結論,將量化后的局部障礙率信息融入評價函數(shù)中,進一步對權重的分配方式進行改進,使得在障礙物復雜程度不同的各個區(qū)域內,算法可依據(jù)環(huán)境信息自適應調整搜索空間,在生成路徑最優(yōu)化的同時,也提升了搜索的靈活性,確保算法高效運行。

4.1 基于漂移矩陣獲取地圖局部障礙率

基于柵格法對機器人工作環(huán)境進行地圖建模。設所獲柵格地圖為m×n的矩形。構建漂移矩陣A1。在同一地圖范圍內,漂移矩陣的邊長l取值越小,漂移矩陣數(shù)量越多,所獲取的局部障礙率信息越豐富。顯然,在同一位置用不同大小的漂移矩陣獲取局部障礙率,其邊長l越小,局部障礙率的精度越高,越能準確反映當前位置附近的環(huán)境信息。本文中l(wèi)取3個單位柵格長度,為可取范圍內的最小取值,所得漂移矩陣為當前位置鄰域內3×3大小的柵格矩陣。

用式(3)作為評價函數(shù)的A*算法,給予h(s)高權重,快速規(guī)劃出一條自起點至終點的路徑,漂移矩陣在A*算法尋路過程中跟隨移動并記錄當前矩陣內部障礙率,每當尋路所在當前柵格超出漂移矩陣范圍時,則重新構建漂移矩陣,記當前柵格坐標為(x,y),記第i個漂移矩陣Ai的障礙率值如式(14)所示:

(14)

其中,oi是矩陣中值為1的柵格(障礙柵格)的數(shù)目。記最后一個漂移矩陣的局部障礙率為pk,從p1到pk中獲得地圖局部障礙率p的取值在[pmin,pmax]。基于漂移矩陣獲取地圖局部障礙率具體流程如圖2所示。

Figure 2 Flow chart of drift matrix algorithm

圖3所示為基于漂移矩陣獲取地圖局部障礙率的實例,設柵格法建模所獲地圖為30×30矩陣,起點S坐標為(1,1),終點D坐標為(30,30),漂移矩陣為3×3大小的柵格矩陣。按圖3所示流程,由于起點(1,1)的橫縱坐標值均小于2,故算法首先以a1(2,2)為中心構建包含起點的漂移矩陣A1,并記錄當前矩陣內的障礙率值p1。當A*算法尋路至a2點時,當前位置坐標(4,4)超出漂移矩陣A1所在范圍,且其橫縱坐標值均大于2,故以當前位置(4,4)為中心構建新的漂移矩陣A2,同樣記錄當前矩陣內的障礙率值p2,以此類推,當算法尋路至終點D時,統(tǒng)計漂移矩陣A1至An內障礙率的極大值pmax和極小值pmin,獲取局部障礙率p值的取值區(qū)間[pmin,pmax],算法結束。

Figure 3 Schematic diagram of map local obstacle rate pre-acquisition

4.2 評價函數(shù)權重比例的改進

由第3節(jié)已證明的相關結論可知,分配權重的評價函數(shù)通過改變各項的權重比例可以擴大或縮小搜索空間。當h(s)的權重增大時,算法的表現(xiàn)為搜索區(qū)域縮小,運行效率提升,更偏向于選擇距終點代價更小的路徑,會陷入局部最優(yōu);當h(s)的權重減小時,搜索區(qū)域逐漸增大,運行效率降低,但往往能夠優(yōu)化生成路徑的長度。為了平衡啟發(fā)式信息強度和優(yōu)化生成的路徑,本文將4.1節(jié)基于漂移矩陣預獲取的障礙率信息融入評價函數(shù)中,設計如式(15)所示的權重關系函數(shù):

(15)

其中,c和d為權重調節(jié)因子,p為局部障礙率。

將式(15)代入式(3)可得:

(16)

當p值變化時,g(s)的權重與h(s)的權重始終成反比,且二者之和為1。當d/(c+p)取值為0.5時,式(16)改寫為:

fweight(s)=0.5·g(s)+0.5·h(s)

(17)

式(17)中2項權重均為0.5,搜索空間和生成路徑與傳統(tǒng)A*算法一致。c、d為常數(shù),與預獲取的局部障礙率信息pmin、pmax滿足以下關系:

(18)

通過式(18)確定常數(shù)c、d的值,同時對應p的取值在[pmin,pmax]內,權重d/(c+p)和(c-d+p)/(c+p)的取值始終在[λ1,λ2],λ1和λ2為搜索區(qū)域調節(jié)因子,當λ1和λ2分別處于0.45和0.55的鄰域內時,算法既不會因為g(s)的權重過低(<0.45)而產生較多的冗余點和拐點,也不會因為g(s)的權重過高(>0.55)而產生過多無用的搜索空間。評價函數(shù)的權重始終維持在一個合理的范圍之內,在不同的漂移矩陣內依據(jù)其內部的局部障礙信息,自適應地調整權重,按需產生不同的有效的搜索空間,在對路徑優(yōu)化的同時,也保證了算法高效、靈活地運行。

5 父節(jié)點變更方式優(yōu)化

A*算法作為一種啟發(fā)式搜索算法,在尋路過程中會產生多條子路徑,而最先找到終點的路徑為最終生成的規(guī)劃路徑,該條件下,各個子路徑之間柵格父節(jié)點的變更對路徑優(yōu)化起著至關重要的作用。本文在傳統(tǒng)A*算法父節(jié)點變更的方式上進行改進,以有效減少生成路徑的冗余點,進一步優(yōu)化路徑。

如圖4所示,柵格V及其它空白柵格為未選柵格,Xn+1～Xn+7按下標順序先后被選為當前柵格,設置子父節(jié)點的鏈接后放入close列表(圖中黑色箭頭的起點為父節(jié)點,終點為指向子節(jié)點)。設由起點start→柵格V為路徑route1,由起點start→柵格Xn為路徑route2,且route1的長度遠小于route2的。顯然,路徑route1→Xn+4→Xn+2→Xn+5要優(yōu)于實際路徑route2→Xn+1→Xn+2→Xn+5。這是由于分配給h(s)較高的權值,算法遵循終點最優(yōu)原則,會優(yōu)先選取靠近終點的柵格作為當前柵格,搜索區(qū)域也相對較小,因此錯失更優(yōu)路徑上的柵格V,因而錯失全局最優(yōu)路徑,陷入局部最優(yōu)。

Figure 4 Pointing diagram of child and parent nodes in traditional A* algorithm

針對上述問題,本文對傳統(tǒng)A*算法的父節(jié)點變更方式進行改進,在檢測由起點至當前柵格到子路徑柵格是否存在更優(yōu)路徑的同時,也檢測子路徑柵格所屬路徑到當前柵格是否存在更優(yōu)路徑,由原本的單向訪問優(yōu)化為雙向訪問,在保證搜索空間有限的前提下不遺漏更優(yōu)路徑,確保路徑的全局最優(yōu)性。

步驟1從待選柵格列表選取柵格作為當前柵格,并記當前柵格current的8鄰域內的柵格依次為currenti(i=1,…,8),判斷當前柵格是否為終點,若是,算法運行結束,若不是,轉到步驟2。

步驟2判斷i是否等于8,若等于返回步驟1,若不等于,i=i+1,轉到步驟3。

步驟3判斷currenti是否為障礙,若為障礙,返回步驟2;若不為障礙,轉到步驟4。

步驟4判斷currenti是否在open列表中,若在open列表中,轉到步驟5;若不在,則將currenti父節(jié)點設為當前柵格current,i=i+1,轉到步驟2。

步驟5判斷經由所屬路徑到達currenti是否更優(yōu),若更優(yōu)則將currenti的父節(jié)點變更為當前柵格current,轉到步驟2。

步驟6判斷經由currenti所屬路徑到達當前柵格current是否更優(yōu),若更優(yōu),則將當前柵格的父節(jié)點變更為currenti,同時將currenti作為下一個待選柵格放入待選柵格列表,轉到步驟2。

圖5a所示為改進前的示意圖,當選中Xn+1為當前柵格時,經其右上角柵格V所屬路徑route1到達當前柵格Xn+1,顯然比Xn+1所屬路徑route2代價更小。改進后,經由route1→Xn+4→Xn+2→Xn+5的路徑要優(yōu)于原先由route2→Xn+1→Xn+2→Xn+5的路徑。可以看出,在h(s)權值較高,搜索區(qū)域較小的情況下,即使所屬更優(yōu)路徑的柵格V沒有被選為當前柵格,按改進后的父節(jié)點變更方式,也可以通過其相鄰柵格檢測出V所在的更優(yōu)路徑,并以V為當前柵格進一步對該條路徑進行延續(xù)性擴展,由局部路徑優(yōu)化延續(xù)為全局路徑優(yōu)化,有效減少生成路徑的冗余點。

Figure 5 Comparison diagram of parent node changes before and after improvement

6 仿真與結果分析

6.1 基于局部障礙率預獲取的改進A*算法仿真

為驗證基于局部障礙率預獲取的改進A*算法的適應性和有效性,本節(jié)選取3個不同尺寸和障礙物密度的柵格地圖,將本文算法和傳統(tǒng)A*算法及文獻[15]引入路徑權值的改進A*算法進行對比,3組實驗的對比仿真結果如圖6所示。

Figure 6 Comparison of simulation results among three algorithms

本次對比實驗是理想仿真,建立了30×30的移動機器人工作環(huán)境地圖,假定環(huán)境地圖的柵格邊長為1 m,對角線長度為1.42 m,為測算實際情況下機器人按實驗路徑尋路所花費的時間,假設機器人的移動速度v=0.5 m/s,原地旋轉45°所需時間為1 s。3種算法的評價指標為:柵格搜索個數(shù)(搜索區(qū)域大小)、路徑長度、轉角次數(shù)和機器人實際尋路時間。本文具體實驗數(shù)據(jù)統(tǒng)計結果如表1所示。

Table 1 Analysis of three algorithms simulation results

從表1的對比仿真數(shù)據(jù)可以看出,傳統(tǒng)A*算法不能很好地識別地圖信息,陷入了局部最優(yōu),增加了不必要的冗余節(jié)點,轉彎次數(shù)多,消耗了機器人的移動時間。對于文獻[15]中引入路徑權值的改進A*算法,相較于傳統(tǒng)A*算法擴大了搜索空間,縮短了路徑長度,減少了轉角次數(shù)。但是,依靠路徑權值對搜索空間的調整,沒有達到依據(jù)具體環(huán)境按需分配搜索空間的目的。比較表1中數(shù)據(jù)可以看出,本文算法在局部復雜地區(qū),擴大了搜索空間,確保路徑最優(yōu),在局部障礙率低的區(qū)域,縮小了搜索空間,提升了搜索效率,不但在搜索空間個數(shù)這一指標上明顯下降,較文獻[15]中引入路徑權值的A*算法,在3種不同地圖環(huán)境中的路徑長度分別由50.34 m縮短為47.82 m,47.08 m縮短為44.66 m,46.4 m縮短為45.24 m,且轉彎次數(shù)這一指標上也有明顯提升,有效縮短了機器人在實際環(huán)境工作中的尋路時間。

6.2 優(yōu)化父節(jié)點變更方式算法仿真

為驗證改進的父節(jié)點變更方式可進一步優(yōu)化生成路徑,本節(jié)在6.1節(jié)實驗的基礎上將改進的父節(jié)點變更方式融入算法,其仿真結果對比如圖7所示。

Figure 7 Comparison before and after optimizing parent node

圖7中,S為起點,D為終點,黑色柵格為障礙,白色柵格為無障礙區(qū)域,黑色實線為尋路路徑。圖7a即為優(yōu)化父節(jié)點前圖6c的算法仿真結果,圖7b為優(yōu)化父節(jié)點后的算法仿真結果。可以看出,在地圖左上角的局部區(qū)域內,障礙率較低,依據(jù)式(16),本文算法分配給h(s)較高的權值,以達到縮小搜索區(qū)域,提高運行效率的目的,但也因此錯過更優(yōu)路徑,陷入局部最優(yōu)。在這種情況下,改進的父節(jié)點變更方式很好地彌補了這一弊端,將路徑總長度由47.82 m縮短為46.98 m,在保證運行效率的同時,篩選出了當前區(qū)域內的最優(yōu)路徑并延續(xù)擴展至終點,由局部最優(yōu)上升為全局最優(yōu),有效地減少了生成路徑的冗余點。

7 結束語

本文就機器人如何更高效地識別和利用已知地圖進行路徑規(guī)劃的問題,提出了一種基于漂移矩陣地圖預獲取的改進A*算法。通過漂移矩陣預獲取地圖各個部分的局部障礙信息,并融入評價函數(shù),使得算法能在不同復雜程度區(qū)域按需調整搜索空間,并用改進的父節(jié)點變更方式進一步優(yōu)化生成路徑。仿真結果表明,本文算法較傳統(tǒng)算法更具適應性和有效性,在相同大小的搜索區(qū)域內,本文算法可以獲得更短的路徑長度,更少的轉角次數(shù),有效減少生成路徑的冗余點;在獲得同等路徑長度的條件下,可以有效縮小搜索空間,縮短運行時間,提高機器人的運行效率。