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

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

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

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

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

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

1.1 A*算法

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

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

(1)

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

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

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

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

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

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

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

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

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

2.1 動態(tài)窗口法

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

(1)建立機器人運動模型

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

(2)

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

(2)機器人速度的運動約束

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

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

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

(3)

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

限制2：機器人動力加速度約束。

(4)

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

(5)

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

V=Vm∩Vr∩Va

(6)

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

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

(7)

式中：G(v，w)為機器人在速度(v，w)下的軌跡評價值；heading(v，w)為機器人運動方向與目標(biāo)點之間的夾角；dist(v，w)為機器人與障礙物最小的距離；v(v，w)為機器人當(dāng)前的速度值；α、β、γ分別為機器人指向權(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)速度選擇

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

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

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

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

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

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

(8)

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

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

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

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

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

3 融合算法

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

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

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

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

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

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

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

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

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

4 實驗

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

實驗的起始點、目標(biāo)點分別為世界坐標(biāo)系下的(1，1)、(9，9)；動態(tài)窗口法初始參數(shù)是：位置坐標(biāo)(1，1)，方向90°，初速度為0，角速度為0，評價函數(shù)參數(shù)為α=1，β=5，γ參數(shù)實驗時設(shè)定；機器人自身參數(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。

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

實驗1部分是分別對本次提出的算法進行無動態(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是在實時運動過程中，檢測到原規(guī)劃的路徑被障礙物堵塞，以當(dāng)前點為起點進行重新規(guī)劃最短路徑，直到機器人到達目標(biāo)點。

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

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

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

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

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

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

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

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

5 結(jié)論

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