融合A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃方法

于 洋，杜學(xué)歷，馮迎賓

(沈陽理工大學(xué) 自動化與電氣工程學(xué)院，沈陽 110159)

機(jī)器人路徑規(guī)劃是機(jī)器人實現(xiàn)自主導(dǎo)航的核心技術(shù)之一。機(jī)器人路徑規(guī)劃技術(shù)是根據(jù)某一個參數(shù)指標(biāo)(如工作代價值最低，選擇路徑最短，運(yùn)算時間消耗最短等)，在任務(wù)區(qū)域選擇出一條可從起點(diǎn)連接到終點(diǎn)的最優(yōu)或次優(yōu)的避障路徑。機(jī)器人路徑規(guī)劃環(huán)境分成兩種：靜態(tài)環(huán)境和動態(tài)環(huán)境。靜態(tài)環(huán)境是在已知先驗地圖模型下，在靜態(tài)障礙物中規(guī)劃一條全局路徑；動態(tài)環(huán)境是指通過機(jī)器人搭載的傳感器感知周圍環(huán)境中的動態(tài)障礙物，從而進(jìn)行安全避障。根據(jù)規(guī)劃環(huán)境研究路徑規(guī)劃，當(dāng)前的機(jī)器人路徑規(guī)劃研究多采用全局路徑和局部動態(tài)路徑融合的方法[1-4]。

在靜態(tài)環(huán)境下，最有效的路徑規(guī)劃算法是A*算法，該算法利用估計子函數(shù)在節(jié)點(diǎn)間快速搜索最優(yōu)路徑[5]。但A*算法規(guī)劃的路徑是通過節(jié)點(diǎn)進(jìn)行連接，存在路徑不連續(xù)的缺點(diǎn)，路徑冗長，無法在動態(tài)環(huán)境下完成避障規(guī)劃。程傳奇等[6]改進(jìn)了A*算法，剔除A*算法搜索的冗余節(jié)點(diǎn)及轉(zhuǎn)折節(jié)點(diǎn)，縮短和平滑了路徑，然后與動態(tài)窗口法融合進(jìn)行局部安全避障。王小紅等[7]針對機(jī)器人平滑及動態(tài)避障問題提出一種改進(jìn)A*算法，將8鄰域擴(kuò)展到24鄰域減少轉(zhuǎn)折點(diǎn)，路徑更加平滑，局部采用動態(tài)窗口法進(jìn)行動態(tài)避障。王洪斌等[8]采用對A*算法搜索的節(jié)點(diǎn)上進(jìn)行二次A*算法搜索，完成全局路徑規(guī)劃，局部采用人工勢場法進(jìn)行動態(tài)避障。王永雄等[9]提出參數(shù)自適應(yīng)的動態(tài)窗口法，根據(jù)機(jī)器人與局部障礙物距離及密集度調(diào)整參數(shù)，兼顧了速度與安全性，但未考慮全局路徑。

針對大面積的障礙物對已規(guī)劃全局路徑堵塞問題，以及涉及到機(jī)器人動力學(xué)時，速度與安全性無法同時滿足的問題，本文提出一種融合改進(jìn)A*算法和改進(jìn)動態(tài)窗口法的全局動態(tài)路徑規(guī)劃。該算法集成了兩種改進(jìn)算法的優(yōu)點(diǎn)，可以大量減少全局路徑的冗余節(jié)點(diǎn)，縮短路徑，遇到障礙物可及時重新規(guī)劃全局路徑，局部能夠自適應(yīng)調(diào)節(jié)速度，兼顧速度和安全性，路徑更加平滑。

1 改進(jìn)A*路徑規(guī)劃算法

1.1 A*算法

A*算法是基于節(jié)點(diǎn)的最佳搜索算法，可以快速搜索出節(jié)點(diǎn)路徑。A*算法的核心是估計子函數(shù)，公式為

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

(1)

式中：f(n)為第n個節(jié)點(diǎn)的估計子函數(shù)代價值；g(n)為第n個節(jié)點(diǎn)到初始節(jié)點(diǎn)的實際代價值；h(n)為第n個節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計代價值。在機(jī)器人中代價值是指兩節(jié)點(diǎn)之間的歐氏距離。

A*算法實現(xiàn)流程是：(1)在環(huán)境地圖中，初始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)設(shè)置，初始化OPEN集合表和CLOSE表；(2)選擇OPEN表中最小估計子函數(shù)代價值的節(jié)點(diǎn)，進(jìn)行鄰域擴(kuò)充，擴(kuò)充后，該節(jié)點(diǎn)放置CLOSE表中，擴(kuò)充的節(jié)點(diǎn)放置OPEN表中；(3)迭代第二步，直到找到目標(biāo)節(jié)點(diǎn)；(4)在CLOSE表中，反向得到最終的路徑節(jié)點(diǎn)集合，將節(jié)點(diǎn)連接，形成路徑。

A*算法進(jìn)行實驗仿真，實驗結(jié)果如圖1所示，小圓圈是搜索節(jié)點(diǎn)，黑色實心圓是障礙物的膨脹區(qū)域，圖中線段是路徑。

從圖1中可以看出，A*規(guī)劃的路徑中存在大量的折線、冗余節(jié)點(diǎn)及冗余轉(zhuǎn)折點(diǎn)，且路徑距離方面仍然存在可縮短的空間。

1.2 二次A*算法優(yōu)化

針對A*算法的缺點(diǎn)進(jìn)行優(yōu)化，優(yōu)化的方法是對路徑中的節(jié)點(diǎn)進(jìn)行二次A*算法搜索，剔除A*算法規(guī)劃路徑中節(jié)點(diǎn)及轉(zhuǎn)折點(diǎn)，保留關(guān)鍵節(jié)點(diǎn)，減少路徑距離。

優(yōu)化算法步驟：首先以第一次A*算法搜索的節(jié)點(diǎn)作為新的節(jié)點(diǎn)進(jìn)行搜索；然后在當(dāng)前節(jié)點(diǎn)后的節(jié)點(diǎn)進(jìn)行鄰域擴(kuò)充，擴(kuò)充標(biāo)準(zhǔn)是中間不存在障礙物；最后根據(jù)估計子函數(shù)進(jìn)行評定，不斷的迭代，最終得到優(yōu)化路徑。通過對圖1的A*算法規(guī)劃的路徑進(jìn)行優(yōu)化，得到圖2所示的優(yōu)化路徑。

由圖2可以看出，原A*算法中11個節(jié)點(diǎn)優(yōu)化為4個節(jié)點(diǎn)，路徑由12.48優(yōu)化為11.7，轉(zhuǎn)折點(diǎn)由7個優(yōu)化為2個，節(jié)點(diǎn)、轉(zhuǎn)折點(diǎn)數(shù)量大幅度減少，路徑縮短。節(jié)點(diǎn)和轉(zhuǎn)折點(diǎn)的減少，縮短路徑，便于機(jī)器人實際運(yùn)動控制及快速到達(dá)目標(biāo)，所以本文采用二次A*算法優(yōu)化后的路徑作為機(jī)器人的全局路徑。

2 改進(jìn)動態(tài)窗口法

2.1 動態(tài)窗口法

動態(tài)窗口法是局部動態(tài)路徑規(guī)劃算法，將機(jī)器人的位置變化控制轉(zhuǎn)化成速度控制，避障問題轉(zhuǎn)變成空間中的運(yùn)動約束問題，這樣可以根據(jù)運(yùn)動約束條件選擇局部最優(yōu)的路徑。

(1)建立機(jī)器人運(yùn)動模型

假設(shè)在Δt時間內(nèi)，機(jī)器人可視為做勻速直線運(yùn)動來模擬機(jī)器人運(yùn)動，以其中一組狀態(tài)值(vt，wt)進(jìn)行軌跡推算，其運(yùn)動模型為

(2)

式中：[xt+Δtyt+Δtθt+Δt]T是t+Δt時刻世界坐標(biāo)系下的機(jī)器人位姿；[xtytθt]T是t時刻世界坐標(biāo)系下的機(jī)器人位姿；[vxΔtvyΔtwtΔt]]T是t時刻機(jī)器人坐標(biāo)系下的位姿變化。

(2)機(jī)器人速度的運(yùn)動約束

機(jī)器人在實際運(yùn)動中，存在自身的限制及傳感器檢測的周圍障礙物距離的限制，速度的范圍受到限制。

限制1：機(jī)器人自身最大速度、最小速度。

Vm={(v，w)|v∈[vmin，vmax]，w∈[wmin，wmax]}

(3)

式中：Vm為機(jī)器人自身最大速度、最小速度的限制下的速度范圍集合；vmin、vmax分別為最小、最大線速度；wmin、wmax分別為最小、最大角速度。

限制2：機(jī)器人動力加速度約束。

(4)

限制3：安全距離限制，即機(jī)器人到障礙物之間，使用最大減速度可以在障礙物前停下來。

(5)

三個限制的交集即為機(jī)器人最終速度的范圍V為

V=Vm∩Vr∩Va

(6)

(3)速度采樣與軌跡推算

在速度空間中進(jìn)行采樣，采樣出不同組的速度，然后根據(jù)機(jī)器人運(yùn)動模型預(yù)測機(jī)器人的軌跡，通過評價函數(shù)對這些軌跡進(jìn)行最優(yōu)選擇，其評價函數(shù)是

(7)

式中：G(v，w)為機(jī)器人在速度(v，w)下的軌跡評價值；heading(v，w)為機(jī)器人運(yùn)動方向與目標(biāo)點(diǎn)之間的夾角；dist(v，w)為機(jī)器人與障礙物最小的距離；v(v，w)為機(jī)器人當(dāng)前的速度值；α、β、γ分別為機(jī)器人指向權(quán)值參數(shù)、安全距離權(quán)值參數(shù)、速度權(quán)值參數(shù)。heading(v，w)、dist(v，w)、v(v，w)都是歸一化后的數(shù)據(jù)。

(4)當(dāng)前最優(yōu)速度選擇

通過對不同組速度推算的軌跡進(jìn)行評價，取評價函數(shù)值最大的(v，w)進(jìn)行控制機(jī)器人運(yùn)動。

2.2 自適應(yīng)動態(tài)窗口法

在機(jī)器人運(yùn)動中，目標(biāo)是機(jī)器人快速運(yùn)動。在動態(tài)窗口法中，速度控制是由γ參數(shù)決定，如果γ設(shè)定過大，所需時間短，步數(shù)少，但機(jī)器人的安全性降低；如果γ設(shè)定過小，所需時間長，步數(shù)多，但機(jī)器人安全性較高。

為在整體上兼顧機(jī)器人的安全性和速度，采用一種基于障礙物信息的動態(tài)參數(shù)調(diào)整方法進(jìn)行機(jī)器人速度權(quán)值參數(shù)γ的調(diào)整。整體思想是：根據(jù)機(jī)器人自身所帶有的傳感器對周圍信息的獲取，對機(jī)器人與周圍障礙物的密集度及距離進(jìn)行自適應(yīng)設(shè)定權(quán)值，這樣可以在安全區(qū)域有較大的速度；危險區(qū)域，降低速度，增加安全性。

(1)檢測障礙物安全距離

一般機(jī)器人傳感器檢測角度為扇形，假設(shè)當(dāng)檢測的障礙物數(shù)量為m時，當(dāng)m大于閾值，表示進(jìn)入密集障礙區(qū)，本次設(shè)定閾值為2。檢測該范圍中障礙物與機(jī)器人距離及角度，然后計算不同障礙物之間的距離Intij，其計算公式為

(8)

式中：Di、Dj是第i、j個障礙物與機(jī)器人的距離；θi、θj是機(jī)器人與第i、j個障礙物朝向的角度值。

通過兩點(diǎn)間的Intij值判斷機(jī)器人是否能夠通過這些障礙物，當(dāng)Intij值大于兩個障礙物膨脹距離的2倍，表示可以通過這兩個障礙物。

(2)計算自適應(yīng)閾值

基于安全考慮，機(jī)器人與障礙物最短的距離Dmin作為自適應(yīng)動態(tài)函數(shù)的輸入，當(dāng)Dmin大于閾值Ds時，機(jī)器人選擇最大的權(quán)值。根據(jù)機(jī)器人自身來定義閾值Ds。

(9)

(3)自適應(yīng)函數(shù)計算

動態(tài)的速度權(quán)值與輸入Dmin關(guān)系是

(10)

式中：γd是輸出的動態(tài)速度權(quán)值參數(shù)；γmin是輸出的最小速度權(quán)值；γmax是輸出的最大速度權(quán)值；k、a分別是調(diào)整參數(shù)。

機(jī)器人在Ds大于Dmin時，動態(tài)的速度權(quán)值與輸入Dmin關(guān)系是單調(diào)遞增的。其中γmax、γmin的取值方法是：γmax是安全通過密集障礙物最大的速度權(quán)值，γmin是通過狹窄的通道且最安全的速度權(quán)值。

(4)調(diào)整評價函數(shù)

調(diào)整后的動態(tài)窗口法的評價函數(shù)是

(11)

設(shè)置速度權(quán)值參數(shù)γ=2、γ=20、γ=γd進(jìn)行實驗，實驗中其他參數(shù)不變。實驗結(jié)果如圖3所示，分別得到了總路程、耗時、步數(shù)、安全距離的數(shù)據(jù)。

圖3a、3b、3c三個實驗的數(shù)據(jù)分析如表1、表2所示，可以得到速度權(quán)值參數(shù)γ=2、γ=20、γ=γd的對比結(jié)果。

表1 圖3a與圖3c實驗數(shù)據(jù)分析表

表2 圖3b與圖3c實驗數(shù)據(jù)分析表

根據(jù)表1、表2可知，速度權(quán)值參數(shù)γ=γd與γ=2實驗效果相比，保證機(jī)器人安全性的基礎(chǔ)上，降低機(jī)器人步數(shù)及耗時，快速達(dá)到目標(biāo)點(diǎn)；速度權(quán)值參數(shù)γ=γd與γ=20實驗效果相比，保證速度情況下，大幅度提升機(jī)器人的安全性。故機(jī)器人在局部路徑規(guī)劃時，采用自適應(yīng)動態(tài)窗口法可以兼顧速度與安全。

3 融合算法

使用改進(jìn)的A*算法進(jìn)行全局路徑規(guī)劃，在局部規(guī)劃時采用改進(jìn)動態(tài)窗口法，通過傳感器檢測周圍信息，實時規(guī)劃最優(yōu)路徑，如果遇到大面積動態(tài)障礙物堵塞全局路徑規(guī)劃的路徑時，重新規(guī)劃全局路徑，防止堵塞的全局路徑對局部實時路徑規(guī)劃造成較大的影響。在大面積動態(tài)障礙物環(huán)境中，融合算法可以保證機(jī)器人在整體規(guī)劃最優(yōu)，又可以兼顧機(jī)器人速度與安全性。

融合算法的實現(xiàn)步驟是：

(1)在靜態(tài)離散地圖中，采用二次A*算法優(yōu)化后的關(guān)鍵節(jié)點(diǎn)作為全局路徑；

(2)將關(guān)鍵點(diǎn)作為機(jī)器人運(yùn)動的局部目標(biāo)點(diǎn)，當(dāng)機(jī)器人與關(guān)鍵點(diǎn)距離超過0.5m時，設(shè)定下一個關(guān)鍵點(diǎn)作為局部目標(biāo)點(diǎn)；

(3)傳感器檢測到動態(tài)障礙堵塞目標(biāo)關(guān)鍵點(diǎn)或規(guī)劃的全局路徑時，重新進(jìn)行步驟(1)規(guī)劃；

(4)局部的目標(biāo)點(diǎn)作為自適應(yīng)動態(tài)窗口法的目標(biāo)輸入，輸出的局部速度值進(jìn)行軌跡預(yù)測，使用評價函數(shù)選擇最優(yōu)軌跡對應(yīng)的速度運(yùn)動；

(5)是否到達(dá)局部目標(biāo)點(diǎn)，如果沒有到達(dá)局部目標(biāo)點(diǎn)，迭代執(zhí)行步驟(4)，直到到達(dá)局部目標(biāo)點(diǎn)；

(6)是否到達(dá)目標(biāo)節(jié)點(diǎn)，如果沒有到達(dá)，執(zhí)行步驟(2)，通過不斷地迭代，直到到達(dá)目標(biāo)節(jié)點(diǎn)。

融合算法的流程如圖4所示。

4 實驗

為驗證算法的有效性，在Matlab 16.0軟件實驗環(huán)境下，構(gòu)建一個離散化的地圖，構(gòu)建的面積為12m×12m，其中的障礙物進(jìn)行0.5m膨脹，障礙物分成靜態(tài)障礙物和動態(tài)障礙物，采用黑色圓代表靜態(tài)障礙物膨脹后的區(qū)域，白色圓代表動態(tài)障礙物膨脹后的區(qū)域。

實驗的起始點(diǎn)、目標(biāo)點(diǎn)分別為世界坐標(biāo)系下的(1，1)、(9，9)；動態(tài)窗口法初始參數(shù)是：位置坐標(biāo)(1，1)，方向90°，初速度為0，角速度為0，評價函數(shù)參數(shù)為α=1，β=5，γ參數(shù)實驗時設(shè)定；機(jī)器人自身參數(shù)為速度(0～1.0)m/s，速度變化率0.02m/s，最大加減速度是0.4m/s2，角速度是(-50～+50)°/s，最大角加速度是80°/s2，角速度變化率1°/s，軌跡采樣時間是0.05s，軌跡預(yù)測時間是3s，自適應(yīng)動態(tài)窗口法的參數(shù)為γmin=2，γmax=20，a=1，k=1，l=0.9。

實驗分成兩部分進(jìn)行對比，實驗1部分進(jìn)行無動態(tài)障礙物堵塞全局路徑和有動態(tài)障礙物堵塞路徑的實驗；實驗2部分進(jìn)行兼顧安全性和速度實驗，分別采用固定的γ參數(shù)和自適應(yīng)的γ參數(shù)。

實驗1部分是分別對本次提出的算法進(jìn)行無動態(tài)障礙物的實驗和有大面積動態(tài)障礙物的實驗。實驗中僅僅是實驗環(huán)境改變，動態(tài)窗口法評價函數(shù)中各個參數(shù)不變，分別是α=1、β=5、γ=γd，具體實驗結(jié)果如圖5所示。

圖5a中是無動態(tài)障礙物的實驗，實驗中不存在動態(tài)障礙物，所以已經(jīng)規(guī)劃的全局路徑不會發(fā)生改變。圖5b是在實時運(yùn)動過程中，檢測到原規(guī)劃的路徑被障礙物堵塞，以當(dāng)前點(diǎn)為起點(diǎn)進(jìn)行重新規(guī)劃最短路徑，直到機(jī)器人到達(dá)目標(biāo)點(diǎn)。

根據(jù)實驗1可以看出，出現(xiàn)大面積障礙物時，實時規(guī)劃全局路徑，可以有效解決大面積的動態(tài)障礙物問題。

實驗2是進(jìn)行速度與安全的兼顧性實驗，實驗環(huán)境采用與圖5b一致的大面積動態(tài)障礙物的環(huán)境，全局路徑一致，障礙物一致，僅速度權(quán)值參數(shù)γ不同。

動態(tài)窗口法評價函數(shù)中速度權(quán)值參數(shù)γ=2、γ=20分別是最安全和速度最快的權(quán)值參數(shù)值，分別對這兩個參數(shù)進(jìn)行實驗，具體實驗如圖6所示。

圖6a、6b實驗結(jié)果分別與已經(jīng)完成的圖5b實驗數(shù)據(jù)進(jìn)行分析，對比分析如表3、表4所示。

表3 圖6a與圖5b實驗數(shù)據(jù)分析表

表4 圖6b與圖5b實驗數(shù)據(jù)分析表

根據(jù)實驗2中γ=2與γd進(jìn)行對比分析，該算法保證安全性的基礎(chǔ)上，可以減少運(yùn)動的步數(shù)、消耗時間；γ=20與γd進(jìn)行對比分析，該算法可以大幅度提高機(jī)器人的安全性。

由實驗1、實驗2可知，該算法解決了大面積動態(tài)障礙物堵塞全局路徑的問題，同時改進(jìn)局部的動態(tài)窗口法，得到一條兼顧速度和安全的路徑。

5 結(jié)論

針對機(jī)器人在規(guī)劃全局路徑后，動態(tài)場景中會堵塞全局路徑的問題，以及運(yùn)動中與障礙物的安全距離和運(yùn)動速度的兼顧性問題，本文設(shè)計一種全局動態(tài)路徑規(guī)劃方法；實驗表明：該全局動態(tài)路徑規(guī)劃方法可以解決大面積動態(tài)障礙物規(guī)劃問題，且在通過大面積障礙物時，機(jī)器人能夠兼顧安全與速度。