基于改進A_Star算法的移動機器人動態(tài)路徑規(guī)劃

李 杰

(安徽三聯(lián)學院機械工程學院,合肥 230601)

0 引言

隨著機器人各方面能力的不斷提高,移動機器人可充當勞動力,助人類完成工作,而路徑規(guī)劃是該領域的一項關鍵技術[1]。

智能規(guī)劃算法多種多樣,包括模擬退火法[2]、人工勢場法[3]、神經(jīng)網(wǎng)絡[4]、蟻群算法[5]、粒子群算法[6]、遺傳算法[7]等,其中A_Star算法作為一種智能啟發(fā)式算法,因其良好的通用性及拓展性被廣泛應用于物流運輸[8-10]、智能倉庫[11]、物聯(lián)網(wǎng)[12]、航空航天[13]及無人駕駛[14]等領域。A_Star算法也可應用于路徑規(guī)劃問題,并對其進行了大量研究。王忠玉等[15]拓展了A_Star算法的搜索領域,在優(yōu)化路徑、評價函數(shù)權重比例方面做出了貢獻。李二超[16]等通過改進人工勢場法對移動機器人避障軌跡進行了研究,但其避障不適合過多障礙物的復雜環(huán)境。而動態(tài)窗口法模型簡單,局部避障能力強,可以適應復雜環(huán)境,但容易陷入局部最優(yōu)。

根據(jù)上述問題,本研究提出一種將改進的A_Star算法與改進的動態(tài)窗口法相結合的方式,解決機器人在復雜環(huán)境下隨機避障問題。從減少搜索方向、平滑規(guī)劃路徑、優(yōu)化評價函數(shù)、障礙物拓展策略及路徑形式等方面對A_Star算法進行改進,得出最優(yōu)路徑,為動態(tài)窗口法提供全局指引點。動態(tài)窗口法在障礙物隨機分布的復雜環(huán)境下遇到障礙物時,能夠及時進行局部避障。該方法既可以規(guī)劃出全局最優(yōu)路徑,又能夠隨時避障。

1 環(huán)境建模

環(huán)境地圖的創(chuàng)建選擇使用柵格法,將空間地圖劃分成相等的二進制參數(shù)單元格,每個單元格的位置可以用二維坐標及序號表示出來[17]。移動機器人在柵格圖中被視為移動粒子,該粒子運動軌跡被視作規(guī)劃路徑。由于柵格的大小會影響算法的搜索速度及結果的準確度,故建立的柵格大小為46節(jié)點×46節(jié)點。地圖上共2116個節(jié)點,并做出了以下假設:

1)環(huán)境中的靜態(tài)障礙物是初始化地圖隨機生成的,動態(tài)障礙物是根據(jù)測試情況隨機添加的。假設靜態(tài)障礙物與動態(tài)障礙物是同時存在的,沒有障礙的柵格用白色表示,有障礙的柵格用黑色表示。

2)為了避免障礙物與移動機器人發(fā)生碰撞,將靜態(tài)障礙物與動態(tài)障礙物全部進行膨化處理,膨化程度大小為移動機器人半徑的大小,忽略障礙物的高度信息。

3)隨機添加動態(tài)障礙物選擇的坐標與對應柵格的坐標存在偏差,按照就近原則,在一定范圍內(nèi)返回柵格所在整數(shù)坐標位置,并進行填充標記。

4)移動機器人步長為兩個柵格中心點之間的距離,每一步都占滿整個柵格,忽略機器人的高度信息。

2 A_Star算法的改進

2.1 A_Star算法原理

A_Star算法是由Hart等在文獻[18]中提出的一種智能啟發(fā)式算法,通過廣度優(yōu)先搜索的Dijkstra與最佳優(yōu)先搜索的BFS優(yōu)點的結合,組成一種設計最短路徑的算法。在廣度搜索算法基礎上加入估價函數(shù),對結點的每一個方向進行評估,從中選擇消耗代價最低、最容易到達的路徑,解決了廣度搜索算法每一個節(jié)點都要走一遍的缺點,極大地提高了搜索效率。

在A_Star算法中,每一個結點位置的選擇都取決于其估價函數(shù)f(n),如式(1)所示:

f(n)=g(n)+h(n);h(n)≤h*(n)

(1)

其中,n表示當前所在的節(jié)點數(shù),f(n)代表機器人從起始點經(jīng)過相應節(jié)點到達目標點的估計總代價,g(n)代表機器人從起始點到達當前節(jié)點所花費的實際代價值,h(n)代表機器人從當前節(jié)點到達目標點的估計代價,h*(n)代表機器人從當前節(jié)點到達目標點的實際最小代價。

h(n)也叫做啟發(fā)函數(shù),啟發(fā)函數(shù)h(n)的選取與優(yōu)化直接影響著節(jié)點的搜索與擴展,進而影響算法的速度及精度。要提高算法的精度與速度,就需要對啟發(fā)函數(shù)h(n)進行優(yōu)化。

A_Star算法的評價函數(shù)是由Dijkstra與BFS兩種算法結合而成,當g(n)的權重為1、h(n)為0時,為Dijkstra算法,通過遍歷所有節(jié)點到起始點的距離,找出并記錄距離最短的節(jié)點,進而找出最短路徑。但遍歷的節(jié)點過多,導致算法運算速度緩慢。當g(n)的權重為0、h(n)為1時,為BFS算法,通過計算當前點節(jié)點到目標點的代價,按照代價最小原則向外擴展鄰近節(jié)點,直到找到目標點。雖然此算法運算速度較快,但很容易找不到目標點,因此分配合適的g(n)與h(n)比例權重,能夠使規(guī)劃的路徑更平滑、更短。

A_Star算法的啟發(fā)函數(shù)常用歐幾里得距離、切比雪夫距離、曼哈頓距離。究選取歐幾里得距離為啟發(fā)函數(shù)代價的計算標準。3種距離的計算如圖1所示。

圖1 3種距離示意圖

2.2 改進A_Star算法

2.2.1 優(yōu)化A_Star算法的評價函數(shù)

A_Star算法的評價函數(shù)主要由g(n)與啟發(fā)函數(shù)h(n)組成,其中起主導作用的是啟發(fā)函數(shù)h(n)。當h(n)h*(n)時,搜索的空間變小,搜索節(jié)點減少,計算量減少,效率變高,但搜索的路徑很難是最優(yōu)解;當h(n)=h*(n)時,算法運行效率是最高的。當前節(jié)點與目標點距離太遠、h(n)?h*(n)時,算法的運算效率很低,為了提高運算效率,應提高權重;反之,隨著當前節(jié)點與目標點越來越近,h(n)≈h*(n)時,為了防止找不到最優(yōu)規(guī)劃路徑,由于始終h(n)≤h*(n),故當障礙物較少時,應適當降低啟發(fā)函數(shù)h(n)的權重。綜上所述,將A_Star算法的評價函數(shù)改進為如式(2)所示:

(2)

其中,r為機器人當前位置到目標點的距離,R為起始點到目標點的距離。

2.2.2 選取最佳關鍵點

傳統(tǒng)的A_Star算法路徑規(guī)劃是由每個柵格的中心點連接而成的。當障礙物增多時,會導致路徑的彎折次數(shù)增多,使路徑不平滑。針對這一缺點,基于Floyd算法思想[19]對路徑的彎折問題做出以下優(yōu)化。

遍歷所有節(jié)點,刪除每一段路徑中間存在的多余節(jié)點,保留起始點與拐點。

遍歷起始點及拐點,從起始點開始將每一個節(jié)點與后面的節(jié)點相互連接作為可供選擇的路徑,判斷每條路徑與障礙物的距離,如果小于安全距離,則舍棄該路徑,如果大于安全距離,則保留該路徑。

提取連接剩余的節(jié)點,作為最優(yōu)路徑輸出。

傳統(tǒng)的A_Star算法規(guī)劃路徑是以斜線的形式靠近并通過障礙物頂點的,這種情況在真實環(huán)境下很容易發(fā)生障礙物與機器人的碰撞事件。經(jīng)過選取最佳關鍵點改進后,規(guī)劃出的路徑與障礙物保證了一定的安全距離,極大保證了移動機器人在真實環(huán)境中的安全。

2.2.3 平滑曲線路徑

傳統(tǒng)的A_Star算法規(guī)劃路徑是以折線形式連接而成的,機器人在每次轉彎處都會出現(xiàn)停頓、再轉向、再繼續(xù)前進的現(xiàn)象。為了使機器人在每次轉彎處能平滑運行,在拐彎處以畫弧線的方式代替折線進行拐彎,這樣轉彎時機器人的運動就會相對平滑、連貫。圖2是優(yōu)化前后機器人通過障礙物頂點的對比圖。

圖2 優(yōu)化前后對比

2.2.4 優(yōu)化搜索方向選取策略

傳統(tǒng)A_Star算法當前節(jié)點到達鄰近節(jié)點的搜索方向為8個,如圖3表示。D代表當前點的位置,D1～D8代表當前節(jié)點可移動的8個方向。

圖3 節(jié)點移動方向

由于8個搜索方向會增加算法的運算時間,故將搜索方向根據(jù)當前節(jié)點與目標點的相對方向優(yōu)化為5個[20],以減少運算,提升算法運行速度及效率。不足之處是當5個搜索方向都存在障礙物時,搜索會陷入死區(qū),無法繼續(xù)實現(xiàn)路徑規(guī)劃。

將當前節(jié)點與目標點的連線與D1的夾角設為α,夾角α與保留的5個方向及舍棄的3個方向之間的對應關系如表1所示。

表1 夾角α與保留的5個方向的對應

3 DWA算法的原理與優(yōu)化

動態(tài)窗口法工作原理是對機器人運動過程中的線速度及角速度進行采樣分析,通過速度的數(shù)值計算出機器人下一步的運動軌跡,使用評價函數(shù)對獲得的每條軌跡進行打分評價,從中選出最安全、最平滑的局部最優(yōu)行駛路線。此時機器人的線速度及角速度為最佳行駛速度。

3.1 動力學模型

假設v(t)與w(t)分別表示Turtlebot2 在世界坐標系下t時刻的線速度及角速度,在采樣周期ht內(nèi),位移較小,可看做做勻速直線運動[18],則運動學模型的數(shù)學表達式

如式(3)表示:

(3)

式中,x(t),y(t),θ(t)分別代表時刻機器人在世界坐標系下的位置姿態(tài)。

3.2 速度采樣

動態(tài)窗口法將避障問題描述為速度空間內(nèi)帶約束的優(yōu)化問題,根據(jù)環(huán)境的變換對速度采樣的范圍進行相應的約束,主要約束機器人的速度、電機加速度及與障礙物的安全距離。

速度約束。移動機器人的速度可以分為線速度與角速度,對速度的約束相當于把線速度與角速度控制在合理的區(qū)間內(nèi),將其設置為線速度與角速度的集合,DWA算法搜索求解的范圍如式(4)所示:

(4)

其中,vmin與vmax分別為移動機器人的最小線速度與最大線速度,wmin與wmax分別為移動機器人的最小角速度與最大角速度。

電機加減速度約束。在對機器人速度進行采樣時,要充分考慮電機的性能問題,采樣速度單位時間內(nèi)的變化量應保持在電機最大加減速度規(guī)定范圍內(nèi),其約束條件如式(5)所示:

(5)

其中,admax與aimax為機器人線速度的最大減速度與最大加速度,αdmax與αimax為機器人角速度的最大減速度與最大加速度。vc與wc為當前機器人的線速度與角速度[19]。

對安全距離的約束。為了防止機器人與障礙物發(fā)生碰撞,機器人在距離障礙物一定距離時,要滿足機器人的線速度與角速度都降為零,其約束條件如式(6)所示:

(6)

其中,admax,αdmax分別為移動機器人的最大減速度與最大加速度,代表預測軌跡末端距離障礙物的距離[11]。

動態(tài)窗口算法的速度約束為上述3種速度約束的交集,動態(tài)窗口速度可以表示為VW=Vt∩Vs∩Vd。

3.3 評價函數(shù)優(yōu)化與改進

傳統(tǒng)的DWA評價函數(shù)主要由指向終點的方位角、機器人速度大小、模擬軌跡末端與障礙物的距離3個指標組成。但存在以下不足:由于目標點只有一個,中間缺少指引的臨時目標點,在大面積環(huán)境中容易陷入局部最優(yōu)路徑。將DWA算法與改進后的A_Star算法相結合,可以提供中間缺少的引導點,極大程度地改善此問題。其未將障礙物進行區(qū)分,導致動態(tài)避障過程中靈敏度降低[19]。針對此不足,對傳統(tǒng)的DWA評價函數(shù)進行優(yōu)化,增加了機器人在動態(tài)避障過程中的靈敏度,得出改進后的評價函數(shù)如式(7)所示:

G(v,w)=αheading(v,w)+βvel(v,w)

+σpath(v,w)+δdist_1(v,w)+μdist_2(v,w)

(7)

其中,α,β,σ,δ,μ分別為各個子函數(shù)的加權系數(shù)。heading(v,w)為模擬軌跡終點不斷朝向目標點的方向角偏差;vel(v,w)用來評價當前機器人運動速度的大小;path(v,w)用來評價模擬軌跡終點與全局規(guī)劃路徑的距離,使其在局部避障后,能快速回歸全局規(guī)劃路徑;dist_1(v,w)用來評價模擬軌跡終點到靜態(tài)障礙物的最近距離,控制障礙物對局部障礙的干擾;dist_2(v,w)用來評價模擬軌跡終點到動態(tài)障礙物的最近距離,通過增加對障礙物的識別能力,提高避障的靈敏度,體現(xiàn)了機器人的避障能力,只有距離大于機器人的半徑,模擬軌跡才能通過該評價條件。

4 融合算法

將改進后的A_Star算法與改進后的DWA算法進行融合,主要目的是針對兩者算法的優(yōu)勢與不足,達到優(yōu)勢互補的效果。

改進后的A_Star算法雖然在已知障礙物靜態(tài)環(huán)境下能得到全局規(guī)劃的最優(yōu)解,但在未知障礙物的環(huán)境中無法避障,僅能達到局部的路徑規(guī)劃。DWA算法由于只有一個目標點指引,全程缺少局部方向的指引,當障礙物較多時很容易陷入局部最優(yōu),導致無法進行全局的路徑規(guī)劃。

通過兩種算法的融合,改進后的A_Star算法能夠提取全局規(guī)劃路徑上的關鍵點為DWA算法作為中間的指引點,在動態(tài)環(huán)境下為DWA算法提供方向,避免陷入局部最優(yōu)的狀況。優(yōu)化后的DWA算法也能夠對動態(tài)障礙物與靜態(tài)障礙物進行區(qū)分,消除一些已知障礙物對路徑的干擾,進一步提高算法運算速度。融合算法能夠結合兩種算法的優(yōu)點,既具有避障功能,又能規(guī)劃出最短路徑。融合算法的流程如圖4所示。

圖4 融合算法流程

5 仿真實驗

5.1 改進后的A_Star算法仿真實驗

仿真實驗在MATLAB 2016b環(huán)境下進行驗證,為了驗證算法的適應性及有效性,隨機建立了柵格地圖,每個柵格設置為面積相等的小正方形,白色柵格代表無障礙區(qū),黑色柵格代表有障礙區(qū)?！啊鳌贝頇C器人的起始點,“○”代表目標點。將改進的A_Star算法、Dijkstra算法、BFS算法進行對比仿真實驗,如圖5所示。

圖5 4種算法仿真實驗對比圖

5.2 仿真實驗結果分析

從仿真圖上來看,Dijkstra算法的拐點是最多的、最不平滑的路徑,改進的A-Star算法的拐點是最少的,路徑是最平滑的。當機器人從一個節(jié)點移動到另一個節(jié)點的過程中,需要進行原位置轉向,轉向下一節(jié)點方向后再繼續(xù)前行,故路徑過于曲折或過長都會造成機器人能量與時間消耗。由此可以看出,改進的A_Star算法在能量消耗與時間花費上都優(yōu)于其余3種路徑規(guī)劃算法,便于機器人在實際情況下更好地進行路徑規(guī)劃,到達目標點。

從表2可知,4種不同的路徑規(guī)劃算法最終都能規(guī)劃出路徑,但從各項數(shù)據(jù)來看還是存在差異的。從計算時間與遍歷節(jié)點數(shù)來看,BFS算法計算時間是最短的,但規(guī)劃路徑是最長的;Dijkstra算法計算時間是最長的,但規(guī)劃路徑是最優(yōu)的;改進的A_Star算法相比于傳統(tǒng)的A_Star,計算時間上平均提高了70%,但路徑長度也略微增加了2.4%。改進的A_Star算法性能大幅度優(yōu)于其他3類算法。

表2 4種路徑規(guī)劃算法性能比較

5.3 融合算法的仿真實驗與分析

為了驗證融合算法的有效性,在仿真軟件中建立了46 m×46 m、柵格間距為1 m的模擬真實場景的柵格地圖,進行了1組仿真實驗,環(huán)境中的靜態(tài)障礙物覆蓋率為30%,隨機添加了5組動態(tài)障礙物分布在全局規(guī)劃軌跡附近,目的是最大程度地增加避障難度。實驗中,起點坐標為(42 m,45 m),目標點坐標為(11 m,2 m)。實驗參數(shù)設置如下:最大角速度為15.0 °/s,最大線速度為0.8 m/s,最大角加速度為45.0°/s,最大線加速度為0.25 m/s2。評價函數(shù)各項參數(shù)如下:α=0.1,β=0.2,σ=0.1,σ=0.05,μ=0.05。融合算法仿真實現(xiàn)過程如圖6所示。

圖6 融合算法仿真實現(xiàn)過程

從圖6可以看出,圖6(a)中的虛線為改進后A_Star算法在未添加動態(tài)障礙物前所規(guī)劃的全局路徑。圖6(b)中方塊用來表示全局路徑中隨機添加的未知障礙物點。圖6(c)融合算法路徑規(guī)劃曲線末端的短曲線代表模擬軌跡,路徑上的“*”代表提取關鍵點,作為融合算法的中間引導點。圖6(d)代表DWA算法正在繞過了隨機添加的動態(tài)障礙物,與虛線不吻合的弧線部分代表DWA算法繞過障礙物與全局路徑之間的偏差。圖6(d)曲線代表機器人在保證規(guī)劃路徑長度最優(yōu)的基礎上使用DWA算法在完美躲避所有障礙物的情況下完成了從起始點到目標點的路徑規(guī)劃。

從最終結果分析,全局靜態(tài)規(guī)劃路徑的長度為55.87 m,局部動態(tài)規(guī)劃的路徑長度為57.32 m,僅僅增加了2.6%。由此可以得出,融合算法既具有避障功能又具有全局最優(yōu)路徑的規(guī)劃功能,充分地將改進后的A_Star算法與DWA算法的優(yōu)勢相融合。

6 結論

隨著機器人領域的蓬勃興起,路徑規(guī)劃的應用場景越來越廣闊,路徑規(guī)劃功能需求也越來越多。傳統(tǒng)的A_Star算法存在效率低、路徑規(guī)劃不平滑、只能適應于已知障礙的環(huán)境等問題。為了滿足實際需求,改進了傳統(tǒng)的A_Star算法,利用優(yōu)先搜索策略將搜索方向從8減少到5,以提高算法的搜索效率。設計了一種路徑平滑優(yōu)化算法,刪除多余節(jié)點與拐點,提高路徑的平滑度,并對自適應函數(shù)進行優(yōu)化,使其隨著環(huán)境的復雜度變化進行自適應調(diào)整,提高了算法的效率及靈活性,加快了收斂速度。通過與Dijkstra算法、DFS算法、傳統(tǒng)A_Star算法的對比實驗可知,改進后的A_Star算法提高了70%的運算效率。

通過提取改進后的A_Star算法關鍵點作為DWA算法中間目標點的方式,在全局規(guī)劃路徑最優(yōu)的基礎上實現(xiàn)融合算法。實驗結果表明,融合算法規(guī)劃的全局路徑不僅實現(xiàn)了與全局最優(yōu)路徑基本吻合的效果,還能夠躲避環(huán)境中出現(xiàn)的動態(tài)障礙物,進一步提高了路徑規(guī)劃算法的功能,可應用于更多復雜的環(huán)境。