龐永旭,袁德成

(沈陽化工大學信息工程學院,遼寧 沈陽 110142)

0 引 言

隨著科技的日益發(fā)展，移動機器人與人們的生活交集日漸增多。路徑規(guī)劃[1]作為當前移動機器人研究領域的前沿課題之一，其目的是在復雜的環(huán)境中躲避障礙，找尋一條能夠實現(xiàn)從起始點無碰撞快速到達目標點的最佳路徑。為解決路徑規(guī)劃中出現(xiàn)的問題，該領域內的學者進行了大量研究，提出了各種行之有效的算法。其中全局路徑規(guī)劃[2]算法有傳統(tǒng)的A*[3]算法、Dijkstra[4]算法；智能仿生的蟻群算法[5]、粒子群算法[6]、遺傳算法[7]等；局部路徑規(guī)劃算法有動態(tài)窗口法[8-9](DWA)和人工勢場法[10]等。王小紅等人[11]基于A*算法成功優(yōu)化其評價函數(shù)，提高了搜索效率，并且通過關鍵點選取策略去除冗余節(jié)點?；眲?chuàng)鋒等人[12]通過擴大傳統(tǒng)的A*算法搜索鄰域，將傳統(tǒng)的3×3鄰域分別擴展為5×5鄰域與7×7鄰域，優(yōu)化了搜索角度，顯著提高了其搜索效率。張超超等人[13]通過定向加權的方法改進A*算法，成功解決了機器人運動軌跡穿過障礙物的問題，但是仍然存在機器人運動軌跡與障礙物頂點相交的問題。張丹紅等人[14]提出了一種將A*算法與蟻群算法相融合的算法，此算法去除了冗余節(jié)點，使路徑更平滑，但是缺乏動態(tài)實時避障的功能，而且計算復雜度較大。徐保來等人[15]改進了動態(tài)窗口法，雖然能夠實現(xiàn)移動機器人的實時避障，軌跡卻無法滿足全局最優(yōu)。傳統(tǒng)的路徑規(guī)劃算法在面對復雜多變的障礙環(huán)境時已然力不從心。程傳奇等人[16]、吳飛龍等人[17]分別提出了2種基于A*與動態(tài)窗口的融合算法。根據(jù)上述路徑規(guī)劃問題，本文提出一種新的融合算法。通過引入障礙物占比優(yōu)化并設計A*算法的啟發(fā)函數(shù)，提高搜索效率，通過優(yōu)化子節(jié)點的選擇方式解決移動機器人路徑與障礙物頂點相交的問題，通過刪除軌跡中的冗余節(jié)點實現(xiàn)路徑平滑度的優(yōu)化，并且融合動態(tài)窗口法，在滿足全局最優(yōu)的情況下實現(xiàn)動態(tài)實時避障。

1 A*算法的改進

A*算法是一種能夠實現(xiàn)全局路徑規(guī)劃的啟發(fā)式搜索算法，可以在靜態(tài)網(wǎng)絡環(huán)境中根據(jù)評價函數(shù)來搜索最佳路徑。但是傳統(tǒng)的A*算法在復雜環(huán)境下存在以下問題：1)冗余節(jié)點多，搜索效率低；2)規(guī)劃后路徑的軌跡與障礙物頂點相交，安全性低；3)路徑拐點多，平滑度低。因此本文通過對傳統(tǒng)A*算法的評價函數(shù)做出改進，對子節(jié)點選擇方式和路徑平滑度進行優(yōu)化以解決上述問題。

1.1 傳統(tǒng)A*算法的基本原理

傳統(tǒng)A*算法搜索原理是將起始點加入open列表，將該點作為父節(jié)點加入close列表，搜索其相鄰的可到達節(jié)點加入open列表，依據(jù)評價函數(shù)計算open列表中節(jié)點的代價，選取代價最低的節(jié)點作為下一個父節(jié)點并放入close列表，再次搜索父節(jié)點可到達節(jié)點并計算其代價，依次循環(huán)，直到父節(jié)點為目標點的位置。

傳統(tǒng)A*算法的評價函數(shù)為：

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

(1)

式(1)中，n代表當前節(jié)點，f(n)表示的是移動機器人在當前節(jié)點的評價函數(shù)，用來選取最優(yōu)路徑，g(n)代表移動機器人從起始點到當前點的實際代價值[18-20]，h(n)為啟發(fā)函數(shù)，代表從當前點n到目標點的估計代價值[21-22]。一般情況下，h(n)小于從當前點n到目標點的實際代價值，當h(n)值為0時，此時只有g(n)起作用，算法即為Dijkstra算法。h(n)的估計值比實際代價值小時，算法搜索時間會因搜索空間的增大而增加。h(n)估計值比實際代價值大時，算法搜索速度會增加，但是算法可能無法搜索到最短路徑。

不難看出h(n)對于搜索效率有很大的影響，h(n)有幾種常見的表現(xiàn)形式：1)曼哈頓距離；2)切比雪夫距離；3)歐幾里得距離。其幾種表現(xiàn)形式如下。

h(n)=abs(nx-gx)+abs(ny-gy)

(2)

h(n)=max[abs(nx-gx),abs(ny-gy)]

(3)

h(n)=sqrt[(nx-gx)2+(ny-gy)2]

(4)

在本文中h(n)采用的是歐幾里得距離，并且通過引入障礙物占比優(yōu)化改進啟發(fā)函數(shù)。

1.2 啟發(fā)函數(shù)的改進

當柵格地圖中的障礙物即障礙柵格適度增加時，地圖中從起始點到達目標點的路徑的選擇也會相應增多，優(yōu)化后過程中容易陷入局部最優(yōu)，導致搜索效率降低，依據(jù)傳統(tǒng)A*算法的基本原理可看出，評價函數(shù)影響著A*算法的搜索效率，因此對評價函數(shù)中的啟發(fā)函數(shù)進行改進。

1.2.1 引入障礙物占比P

通過引入障礙物占比P來影響啟發(fā)函數(shù)h(n)在不同環(huán)境下的權重。假設一局部區(qū)域中障礙物的數(shù)量為a，所有柵格數(shù)量為A，設當前點的坐標為(nx，ny)，目標點的坐標為(gx，gy)，則A可以由式(5)表示。

A=[1+abs(nx-gx)]×[1+abs(ny-gy)]

(5)

障礙物占比P表示在當前節(jié)點與目標節(jié)點所構成的局部區(qū)域中，障礙物在所有柵格中占的比重。則P如式(6)所示。

P=a/A

(6)

1.2.2 優(yōu)化啟發(fā)函數(shù)

局部區(qū)域內障礙物較多時，需要確保最優(yōu)路徑的精度，避免陷入局部最優(yōu)，此時應該減少h(n)的權重，降低搜索速度；局部區(qū)域內障礙物較少時，如果按照傳統(tǒng)A*算法的啟發(fā)函數(shù)h(n)尋找最優(yōu)路徑，會導致搜索節(jié)點過多，搜索效率降低，此時應該加大h(n)的權重。如式(7)所示，改進后的評價函數(shù)提高了A*算清的適應性。

f(n)=g(n)+h(n)-lgP×h(n)

(7)

1.2.3 子節(jié)點的優(yōu)化

傳統(tǒng)的A*算法在尋找最優(yōu)路徑時，會出現(xiàn)最優(yōu)路徑與障礙柵格頂點相交的情況，為解決這一問題，本文優(yōu)化子節(jié)點的選擇方式。如圖1所示，中間節(jié)點為父節(jié)點，父節(jié)點周圍相鄰的8個鄰域為8個子節(jié)點。根據(jù)8個子節(jié)點與父節(jié)點的位置關系，將子節(jié)點1、3、5、7歸類為A類子節(jié)點，將父節(jié)點上下方向上的子節(jié)點2、6歸類為B類子節(jié)點，將父節(jié)點左右方向上的子節(jié)點4、8歸類為C類子節(jié)點。

圖1 子節(jié)點圖

根據(jù)A、B、C這3類子節(jié)點，定義了以下子節(jié)點優(yōu)化選擇方式，如表1所示。

表1 子節(jié)點選擇規(guī)則

1.2.4 優(yōu)化路徑平滑度

傳統(tǒng)的A*算法在路徑規(guī)劃過程中會出現(xiàn)冗余節(jié)點，影響移動機器人的正常工作。冗余節(jié)點包括共線節(jié)點和多余拐點，如圖2所示，X1→X2→X3→X4→X5→X6→X7→X8路徑存在較多冗余節(jié)點，為傳統(tǒng)A*算法所尋路徑。本文通過從起始點到目標點刪除冗余節(jié)點，然后從目標點反向到起始點再次刪除多余節(jié)點進行路徑平滑度優(yōu)化。

圖2 刪除冗余節(jié)點圖

1)刪除共線節(jié)點，從目標點方向在X2開始，沿著傳統(tǒng)A*算法的路徑判斷當前節(jié)點與前一節(jié)點是否共線，若共線，則前一節(jié)點被認為需要刪除的冗余節(jié)點。優(yōu)化后軌跡變?yōu)閄1→X5→X6→X7→X8。

2 動態(tài)窗口法

動態(tài)窗口法(DWA)是一種解決移動機器人局部避障時的常用算法。此算法是指在速度空間中，依據(jù)移動機器人的運動模型，對多組速度進行采樣，分析并預測出一段時間內在各組速度下移動機器人的移動軌跡，然后根據(jù)算法的評價函數(shù)選出最優(yōu)軌跡所對應的速度，根據(jù)最優(yōu)軌跡對應的速度驅動移動機器人進行局部路徑規(guī)劃。

2.1 移動機器人的運動模型

DWA對窗口區(qū)域內移動機器人的線速度和角速度進行采樣，因此第一步需要對移動機器人進行運動學建模。假設在一段時間內(Δt)移動機器人做勻速直線運動。式(8)表示移動機器人在該段時間內的運動學模型。

(8)

2.2 移動機器人速度采樣

在移動機器人速度組空間中，理論上有無窮多組速度集[23](v,ω)，但是移動機器人容易受到自身的硬件、工作環(huán)境的制約，因此獲得機器人運動模型后需要根據(jù)實際情況對采樣的速度集范圍進行約束。

1)移動機器人受自身條件制約的最大、最小速度范圍可表示為：

Vm={(v,ω)|v∈[vmin,vmax]∩ω∈[ωmin,ωmax]}

(9)

2)移動機器人受到自身電機的影響，存在加速減速約束，在動態(tài)窗口顯示內，移動機器人受加減速影響的最大、最小速度范圍為：

(10)

式中，vt代表移動機器人的當前線速度，ωt代表機器人當前角的速度，va、ωa為移動機器人的最大加減速度，Δt為模擬預測時間。

3)移動機器人制動距離約束。實現(xiàn)動態(tài)避障主要依賴制動距離的約束。動態(tài)窗口法在選擇速度和軌跡評價的過程中尋找障礙物，為保證移動機器人工作時的可靠性與安全性，需要其在與障礙物發(fā)生碰撞前，在最大減速度條件下，把速度減到0。約束公式如式(11)所示。

(11)

式(11)中dist(v，ω)表示此速度下該軌跡與障礙物之間的最短距離。根據(jù)上述3種速度約束可得移動機器人所取速度集為滿足上述3種約束條件的速度矢量空間交集，如圖3所示。

圖3 速度采樣示意圖

2.3 評價函數(shù)

動態(tài)窗口法中的評價函數(shù)用于選取最優(yōu)軌跡。軌跡評判的準則為：準確避開障礙物，耗時最短靠近目標點。設計評價函數(shù)為：

G(v,ω)=σ(α·head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)

(12)

式中head(v，ω)為方位角評價函數(shù)[24]，表示目標位置與預測軌跡終點位置方向的方位角偏差大小；dist(v,ω)為距離評價函數(shù)；vel(v，ω)為速度評價函數(shù)；α、β、γ為3項加權系數(shù)，σ為歸一化平滑系數(shù)。

3 算法融合

改進后的A*算法本質上仍然是一種全局路徑規(guī)劃算法，在錯綜復雜的環(huán)境下無法進行動態(tài)避障，路徑也不夠平滑。而動態(tài)窗口法作為一種解決移動機器人局部避障時的常用算法，能夠動態(tài)實時規(guī)劃路徑，從而彌補改進A*算法的不足。而且傳統(tǒng)的動態(tài)窗口法缺少全局路徑規(guī)劃的指引，在障礙環(huán)境復雜時容易陷入局部最優(yōu)，對此提取改進后A*算法的全局路徑節(jié)點，設計一種融合全局路徑信息的評價函數(shù)。

G(v,ω)=σ(α·Head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)+λDist(v,ω)

(13)

式中Head(v，ω)為方位角評價函數(shù)，表示模擬預測路徑終點位置方向與全局路徑節(jié)點的方位角偏差，依照改進后A*算法的全局路徑節(jié)點順序設置當前目標節(jié)點，在抵達目標點時更新，依次動態(tài)設置目標節(jié)點。dist(v,ω)、Dist(v,ω)為距離評價函數(shù)，分別表示全局已知障礙物與模擬軌跡的最短距離和局部未知障礙物與模擬軌跡的最短距離。本文采用改進后的A*算法做全局路徑規(guī)劃，然后采用融合全局路徑信息的DWA進行局部路徑規(guī)劃，最終實現(xiàn)移動機器人動態(tài)避障，尋找最優(yōu)路徑，融合算法流程如圖4所示。

圖4 融合算法流程圖

4 實驗仿真

本文在MATLAB下進行仿真實驗，從而驗證改進算法及融合算法的有效性。

4.1 A*算法優(yōu)化后仿真結果

上述操作實現(xiàn)了搜索效率的提高，刪除了多余節(jié)點并且優(yōu)化了路徑平滑度，本文在MATLAB下進行不同環(huán)境的仿真，分別為環(huán)境1(簡易環(huán)境)、環(huán)境2(復雜環(huán)境)，其中仿真環(huán)境2的尺寸相比環(huán)境1擴大了4倍。通過圖5、圖6可看出簡易環(huán)境下優(yōu)化后的A*算法有障礙物信息的引導，搜索空間減少，搜索時間更快，全局路徑效果更佳；通過圖7、圖8可看出復雜環(huán)境下的改進A*算法路徑長度上短于傳統(tǒng)A*算法，避免了路徑與障礙物頂點相切，安全系數(shù)提高，路徑精度高于傳統(tǒng)A*算法。表2為不同環(huán)境下路徑規(guī)劃后的指標，改進后的A*算法優(yōu)于傳統(tǒng)算法。

圖5 環(huán)境1下傳統(tǒng)A*算法仿真

圖6 環(huán)境1下改進A*算法仿真

圖7 環(huán)境2下傳統(tǒng)A*算法仿真

圖8 環(huán)境2下改進A*算法仿真

表2 路徑規(guī)劃對比

4.2 融合算法仿真結果

圖9 避障過程仿真圖1

圖10 避障過程仿真圖2

圖11 改進A*算法仿真圖

圖12 融合算法仿真圖

58 結束語

本文提出了一種融合改進A*算法與動態(tài)窗口法的融合算法，以提升移動機器人在不同復雜度環(huán)境下工作的靈活性，提高其工作效率。與傳統(tǒng)算法相比，改進后的A*算法解決了軌跡與障礙柵格頂點相交的問題，路徑節(jié)點減少，路徑更加平滑，引入障礙物占比，提高了不同環(huán)境下的靈活性與搜索效率。并采用動態(tài)窗口算法進行局部避障，結合全局路徑規(guī)劃信息的指引規(guī)劃出最優(yōu)路徑，彌補了A*算法無法完成動態(tài)避障的缺點。通過MATLAB仿真對比實驗，表明了本文所提出融合算法的有效性。未來預計從以下幾個方面進行研究：1)針對復雜動態(tài)環(huán)境下多數(shù)目移動機器人相互協(xié)調，多目標移動機器人路徑規(guī)劃問題進行探索；2)針對未知動態(tài)環(huán)境，結合機器學習進行移動機器人路徑規(guī)劃的進一步研究。