基于動(dòng)態(tài)障礙物的機(jī)器人避障路徑規(guī)劃方法

郭翰卿，付麗霞，張 勇，毛劍琳

（昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500）

0 引 言

隨著我國(guó)自動(dòng)化技術(shù)的發(fā)展，自主移動(dòng)機(jī)器人運(yùn)用越來(lái)越廣泛。路徑規(guī)劃[1]是移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一，路徑規(guī)劃問(wèn)題可分為全局路徑規(guī)劃和局部路徑規(guī)劃[2-3]。當(dāng)前，多因素復(fù)雜地形[4]和動(dòng)態(tài)障礙物環(huán)境[5]下的避障路徑規(guī)劃是最具有挑戰(zhàn)性的課題之一，是研究發(fā)展的新動(dòng)向。因此，研究一種能實(shí)現(xiàn)動(dòng)態(tài)避障的路徑規(guī)劃算法很有必要。

對(duì)于靜態(tài)全局路徑規(guī)劃，常用的算法有A*算法[6]、蟻群算法[7]、神經(jīng)網(wǎng)絡(luò)算法[8]等。A*算法由于其算法效率高而有著較為廣泛的應(yīng)用，對(duì)于改進(jìn)A*算法，文獻(xiàn)[9]對(duì)A*算法的搜索效率進(jìn)一步提升，并且結(jié)合視覺(jué)分析，但是其還是基于靜態(tài)環(huán)境，并未考慮動(dòng)態(tài)環(huán)境下的復(fù)雜因素。國(guó)內(nèi)外對(duì)于障礙物信息不確定的動(dòng)態(tài)局部路徑規(guī)劃也有所研究，文獻(xiàn)[10]在傳統(tǒng)動(dòng)態(tài)窗口法中引進(jìn)控制理念，使局部規(guī)劃更加精確，但該算法沒(méi)有考慮全局規(guī)劃。文獻(xiàn)[11]在動(dòng)態(tài)窗口法中結(jié)合全局考慮，實(shí)現(xiàn)了一部分隨機(jī)避障，但是該算法還是在障礙物固定的環(huán)境下對(duì)機(jī)器人移動(dòng)路徑進(jìn)行規(guī)劃，并未考慮運(yùn)動(dòng)障礙物，算法有一定的局限性。

上述改進(jìn)算法對(duì)路徑規(guī)劃問(wèn)題做了優(yōu)化，但是并未完全解決當(dāng)前復(fù)雜環(huán)境尤其是動(dòng)態(tài)障礙物環(huán)境下的路徑規(guī)劃需求。針對(duì)這個(gè)問(wèn)題，設(shè)計(jì)一種融合障礙物信息的動(dòng)態(tài)窗口法，并且引入改進(jìn)A*算法融合提升路徑規(guī)劃的全局最優(yōu)性。通過(guò)實(shí)驗(yàn)證明，本文所提的算法能在保證全局最優(yōu)的前提下實(shí)現(xiàn)移動(dòng)機(jī)器人動(dòng)態(tài)避障。

1 改進(jìn)A*算法

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

A*算法是可以實(shí)現(xiàn)全局路徑規(guī)劃的啟發(fā)式算法，主要應(yīng)用于柵格法環(huán)境建模的地圖中。其核心思想為：從路徑起點(diǎn)開(kāi)始對(duì)周?chē)徲蛩阉?，?jì)算當(dāng)前節(jié)點(diǎn)鄰域內(nèi)節(jié)點(diǎn)到起始點(diǎn)的實(shí)際代價(jià)和到目標(biāo)點(diǎn)的估值代價(jià)，選擇代價(jià)小的點(diǎn)作為下個(gè)待選節(jié)點(diǎn)，按照此搜索方式進(jìn)行搜索，直至找到目標(biāo)點(diǎn)。其估價(jià)函數(shù)為：

式中：F(k)是節(jié)點(diǎn)k的代價(jià)和，G(k)為機(jī)器人從節(jié)點(diǎn)k到達(dá)起點(diǎn)的實(shí)際代價(jià)，H(k)為機(jī)器人從節(jié)點(diǎn)k到達(dá)目標(biāo)點(diǎn)的估值代價(jià)。其中，H(k)的計(jì)算方法有曼哈頓距離、切比雪夫距離、歐式距離[12]等，本文采用歐氏距離作為H(k)，具體公式為：

式中：(x1,y1)和(x2,y2)分別表示當(dāng)前節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的位置坐標(biāo)。

1.2 改進(jìn)A*算法

傳統(tǒng)A*算法規(guī)劃的路徑?jīng)]有考慮到與障礙物的位置關(guān)系，不利于機(jī)器人在動(dòng)態(tài)障礙物環(huán)境下的避障規(guī)劃，因此本文對(duì)A*算法的實(shí)際代價(jià)函數(shù)G(k)進(jìn)行改進(jìn)，預(yù)設(shè)一個(gè)機(jī)器人與障礙物的距離函數(shù)，改進(jìn)后的G(k)為：

式中：si為路段i的安全估值，si=k(1-di/r)，0＜di≤r；li是機(jī)器人在路段i的長(zhǎng)度，ω是估值系數(shù)。由于傳統(tǒng)A*算法在路徑規(guī)劃時(shí)存在大量冗余節(jié)點(diǎn)且路徑轉(zhuǎn)折過(guò)多，本文采用一種雙向刪除轉(zhuǎn)折點(diǎn)算法，對(duì)傳統(tǒng)A*算法中的冗余轉(zhuǎn)折進(jìn)行優(yōu)化。該算法可有效減少路徑長(zhǎng)度，使得規(guī)劃出的路徑更加平滑，減少轉(zhuǎn)折。具體操作如下。

（1）獲取傳統(tǒng)A*算法路徑節(jié)點(diǎn)G{Si,1≤i≤n}，并創(chuàng)建一個(gè)只有起點(diǎn)S1和終點(diǎn)Sn的集合P。

（2）遍歷路徑中所有節(jié)點(diǎn)，刪除路徑中的中間點(diǎn)，保留轉(zhuǎn)折點(diǎn)。

（3）從起始點(diǎn)開(kāi)始，在在轉(zhuǎn)折點(diǎn)Si、Sj之間隔m取點(diǎn)，并判斷兩節(jié)點(diǎn)之間是否穿過(guò)障礙物。若沒(méi)有穿過(guò)障礙，選擇當(dāng)前節(jié)點(diǎn)Si+m替代集合P中的點(diǎn)為路徑節(jié)點(diǎn)，反之不做選擇。

（4）從目標(biāo)點(diǎn)反向遍歷，重復(fù)上一步判斷方法，并將所有取得的節(jié)點(diǎn)放入集合P中。

（5）依次連接P中所有節(jié)點(diǎn)，得到優(yōu)化后的最優(yōu)路徑。

路徑優(yōu)化過(guò)程如圖1所示。

圖1 路徑優(yōu)化過(guò)程示意圖

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

2.1 傳統(tǒng)動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口方法（Dynamic Window Approach，DWA）基于機(jī)器人所載的傳感器實(shí)時(shí)檢測(cè)局部環(huán)境信息，結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)進(jìn)行速度空間采樣，實(shí)現(xiàn)路徑規(guī)劃。該算法對(duì)多組速度進(jìn)行采樣來(lái)對(duì)機(jī)器人位置信息模擬，最后利用評(píng)價(jià)函數(shù)對(duì)軌跡進(jìn)行評(píng)估，選出最優(yōu)組，使得機(jī)器人能迅速到達(dá)目標(biāo)點(diǎn)。傳統(tǒng)動(dòng)態(tài)窗口法可最大限度地使機(jī)器人避開(kāi)障礙物并以最快的速度移至目標(biāo)點(diǎn)，評(píng)價(jià)函數(shù)的表達(dá)式為：

式中：head(v,w)是方位角評(píng)價(jià)函數(shù)，引導(dǎo)方位角始終朝向目標(biāo)節(jié)點(diǎn)，評(píng)估軌跡朝向和目標(biāo)的方位差；dist(v,w)是速度評(píng)價(jià)函數(shù)，vel(v,w)是距離評(píng)價(jià)函數(shù)，用于評(píng)價(jià)機(jī)器人與障礙物距離關(guān)系，如果沒(méi)有障礙物，則該參數(shù)為常數(shù)；α、β、γ為權(quán)重系數(shù)。

然而，傳統(tǒng)DWA算法缺少對(duì)全路徑的判斷，機(jī)器人容易陷入死鎖，導(dǎo)致路徑規(guī)劃失敗。并且當(dāng)障礙物為運(yùn)動(dòng)障礙物時(shí)，由于機(jī)器人缺少對(duì)障礙物信息的實(shí)時(shí)判斷，機(jī)器人無(wú)法安全地避開(kāi)障礙物。針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法的缺陷，設(shè)計(jì)一種基于動(dòng)態(tài)障礙物信息的改進(jìn)動(dòng)態(tài)窗口法。

2.2 改進(jìn)動(dòng)態(tài)窗口法

2.2.1 融合運(yùn)動(dòng)障礙物分析

實(shí)際生活中的障礙物大多是類(lèi)似汽車(chē)、動(dòng)物等運(yùn)動(dòng)的障礙物。當(dāng)人們遇見(jiàn)這類(lèi)障礙物，會(huì)分析判斷其歷史運(yùn)動(dòng)軌跡，然后根據(jù)障礙物的意圖規(guī)劃下一步路線?；谝陨显?，本文設(shè)計(jì)一種運(yùn)動(dòng)障礙物意圖分析法，對(duì)運(yùn)動(dòng)障礙物融合意圖分析，提高機(jī)器人避開(kāi)運(yùn)動(dòng)障礙物的安全性，如圖2所示。

圖2 動(dòng)態(tài)障礙物分析

機(jī)器人遇見(jiàn)運(yùn)動(dòng)障礙物時(shí)，首先對(duì)障礙物進(jìn)行運(yùn)動(dòng)學(xué)分析，根據(jù)障礙物的歷史運(yùn)動(dòng)軌跡，預(yù)估運(yùn)動(dòng)障礙物的速度和方向，整體規(guī)劃將要避障的區(qū)域，選擇在所有可行的軌跡中時(shí)間最優(yōu)的那一條作為機(jī)器人的最優(yōu)路徑。同時(shí)，機(jī)器人在運(yùn)動(dòng)過(guò)程中根據(jù)障礙物位置實(shí)時(shí)調(diào)整運(yùn)動(dòng)方向來(lái)實(shí)現(xiàn)更好的避障效果。機(jī)器人動(dòng)態(tài)避障方法如下。

（1）機(jī)器人根據(jù)傳感器實(shí)時(shí)采集運(yùn)動(dòng)障礙物信息，并確定運(yùn)動(dòng)障礙物歷史位置信息。

（2）對(duì)運(yùn)動(dòng)障礙物進(jìn)行適當(dāng)?shù)呐蛎浱幚恚沟脵C(jī)器人在避障時(shí)更加安全。

（3）根據(jù)移動(dòng)障礙物位置合理規(guī)劃將要行進(jìn)的軌跡。

（4）機(jī)器人在行進(jìn)過(guò)程中根據(jù)運(yùn)動(dòng)障礙物的位置變化合理地調(diào)整軌跡。

2.2.2 改進(jìn)動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)

傳統(tǒng)動(dòng)態(tài)窗口法對(duì)于障礙物固定的環(huán)境下規(guī)劃性能較好，在局部路徑規(guī)劃中有著較為廣泛的應(yīng)用。但是當(dāng)環(huán)境中存在運(yùn)動(dòng)障礙物時(shí)，由于傳統(tǒng)動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)的局限性，機(jī)器人容易出現(xiàn)局部死鎖、無(wú)法及時(shí)避開(kāi)障礙物等問(wèn)題。為此，本文對(duì)傳統(tǒng)動(dòng)態(tài)窗口法的評(píng)價(jià)函數(shù)進(jìn)行合理的拓展，在距離評(píng)價(jià)函數(shù)后增加一項(xiàng)拓展距離函數(shù)spdist(v,w)。

拓展之后的評(píng)價(jià)函數(shù)可以使得機(jī)器人在檢測(cè)到障礙物時(shí)綜合速度差值與距離參數(shù)來(lái)選擇合適的運(yùn)動(dòng)軌跡，增強(qiáng)機(jī)器人對(duì)動(dòng)態(tài)障礙物的避障能力。改進(jìn)后的評(píng)價(jià)函數(shù)如下：

基于運(yùn)動(dòng)障礙物的不確定性，為了使機(jī)器人更加安全地避障，對(duì)于不同速度的障礙物應(yīng)設(shè)定不同范圍的安全距離。運(yùn)動(dòng)速度快的障礙物，安全距離增大，反之減小。機(jī)器人在行駛過(guò)程中需要自主判斷運(yùn)動(dòng)障礙物的位置，在不影響障礙物的前提下運(yùn)動(dòng)，實(shí)行安全行進(jìn)策略。改進(jìn)后的算法流程如下。

（1）機(jī)器人在行進(jìn)過(guò)程中判斷軌跡方向是否與運(yùn)動(dòng)障礙物的危險(xiǎn)區(qū)域重疊。如果重疊，則進(jìn)行下一步操作。如果沒(méi)有，就按照原定軌跡運(yùn)動(dòng)。

（2）判定機(jī)器人與運(yùn)動(dòng)障礙物即將相遇，計(jì)算各個(gè)速度下機(jī)器人與運(yùn)動(dòng)障礙物的位置信息，將預(yù)測(cè)與障礙物危險(xiǎn)區(qū)域重疊的速度組舍棄。

（3）在其他不與運(yùn)動(dòng)障礙物危險(xiǎn)區(qū)域重疊的速度組中判斷，選擇距離合適速度組，向左或向右移動(dòng)機(jī)器人。

（4）根據(jù)機(jī)器人方向確定評(píng)價(jià)函數(shù)權(quán)重系數(shù)，并根據(jù)評(píng)價(jià)函數(shù)計(jì)算所有符合條件的軌跡，選出評(píng)估值最大的軌跡來(lái)最終確定機(jī)器人的移動(dòng)軌跡。

改進(jìn)后的動(dòng)態(tài)窗口法能更加適應(yīng)運(yùn)動(dòng)障礙物環(huán)境下的避障問(wèn)題，使得機(jī)器人行進(jìn)更加安全。

2.3 算法融合

動(dòng)態(tài)窗口法（DWA）是基于機(jī)器人檢測(cè)到的局部速度信息來(lái)實(shí)時(shí)規(guī)劃路徑的，在局部路徑中有良好的性能。但是該方法缺乏對(duì)最優(yōu)全局路徑的考慮，容易陷入局部最優(yōu)解。因此本文將改進(jìn)后的兩種算法融合，結(jié)合兩種算法的優(yōu)點(diǎn)，從而實(shí)現(xiàn)機(jī)器人沿全局最優(yōu)路徑行進(jìn)時(shí)能實(shí)時(shí)避開(kāi)運(yùn)動(dòng)的障礙物，獲得具有動(dòng)態(tài)自主避障特性的全局最優(yōu)路徑，并對(duì)動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)進(jìn)一步優(yōu)化。優(yōu)化后的DWA評(píng)價(jià)函數(shù)為：

式中：SHead(v,w)是當(dāng)前目標(biāo)點(diǎn)方向和機(jī)器人當(dāng)前軌跡方向的夾角，用來(lái)評(píng)價(jià)機(jī)器人在當(dāng)前速度下到達(dá)終點(diǎn)時(shí)的方向和當(dāng)前目標(biāo)點(diǎn)之間的角度下差；α、β、γ和δ分別 為SHead(v,w)、dist(v,w)、vel(v,w)和spdist(v,w)的參數(shù)權(quán)重。

改進(jìn)的評(píng)價(jià)函數(shù)可以使得動(dòng)態(tài)窗口法在局部路徑規(guī)劃時(shí)避免死鎖，實(shí)現(xiàn)全局最優(yōu)路徑，使得機(jī)器人能在復(fù)雜動(dòng)態(tài)障礙物環(huán)境下規(guī)劃避障。具體算法流程如圖3所示。

圖3 融合算法流程圖

3 仿真驗(yàn)證

為驗(yàn)證本文提出方法的有效性，對(duì)算法進(jìn)行仿真驗(yàn)證。仿真環(huán)境為Matlab 2018b軟件、8 GB內(nèi)存的64位Windows 10系統(tǒng)。

首先采用柵格法對(duì)實(shí)驗(yàn)環(huán)境建模，在20×20的柵格地圖環(huán)境下實(shí)驗(yàn)，如圖4所示。地圖中黑色為固定障礙物，白色是可行區(qū)域。實(shí)驗(yàn)中，動(dòng)態(tài)窗口法的參數(shù)設(shè)置為：最大線速度為1.5 m·s-1，最大角速度為20.0°·s-1，最大線加速度為0.2 m·s-2，最大角加速度為50.0°·s-2，采樣時(shí)間為0.1 s，評(píng)價(jià)函數(shù)參數(shù)α=0.05，β=0.2，γ=0.3，δ=0.3，ρ=0.4。

首先用傳統(tǒng)DWA算法進(jìn)行路徑規(guī)劃。如圖4所示，以（2.5，2.5）為起點(diǎn)，以（13.5，18.5）為終點(diǎn)。因?yàn)榈貓D中存在有U型障礙物，通過(guò)傳統(tǒng)DWA算法進(jìn)行路徑規(guī)劃，容易陷入局部最優(yōu)，導(dǎo)致機(jī)器人死鎖無(wú)法前進(jìn)。這種情況在地圖中存在半封閉障礙物時(shí)最為明顯，并且如果地圖中存在運(yùn)動(dòng)障礙物，傳統(tǒng)DWA算法也會(huì)因?yàn)樵u(píng)價(jià)函數(shù)的局限性，無(wú)法及時(shí)避開(kāi)動(dòng)態(tài)障礙物。

在同樣的地圖環(huán)境中加入動(dòng)態(tài)障礙物，將對(duì)融合改進(jìn)后算法進(jìn)行路徑規(guī)劃。首先用改進(jìn)A*算法進(jìn)行全局路徑規(guī)劃，作為動(dòng)態(tài)窗口法的全局最優(yōu)序列指引。如圖5所示，機(jī)器人首先規(guī)劃出了一條虛線作為全局最優(yōu)路徑，然后在全局最優(yōu)路徑上加入運(yùn)動(dòng)障礙物和隨機(jī)出現(xiàn)的障礙物。其中虛線上的兩層方塊表示動(dòng)態(tài)障礙物，兩層方塊表示對(duì)動(dòng)態(tài)障礙物做了一定的膨脹處理，虛線中靠近終點(diǎn)的方塊表示隨機(jī)出現(xiàn)的障礙物。融合后的算法首先按照全局最優(yōu)序列點(diǎn)行進(jìn)，避免了傳統(tǒng)DWA算法的死鎖情況。由圖6、圖7可知，當(dāng)機(jī)器人行進(jìn)到運(yùn)動(dòng)障礙物附近開(kāi)始進(jìn)行障礙物軌跡歷史分析，結(jié)合改進(jìn)動(dòng)態(tài)窗口法綜合分析障礙物運(yùn)動(dòng)速度與機(jī)器人當(dāng)前速度，決策出一條能安全快速的避開(kāi)障礙物軌跡。由圖6可知，機(jī)器人遇見(jiàn)動(dòng)態(tài)障礙物時(shí)會(huì)選擇一條既遠(yuǎn)離運(yùn)動(dòng)障礙物、又遠(yuǎn)離靜態(tài)障礙物且盡可能貼近全局最優(yōu)路徑的軌跡，符合實(shí)際生活中機(jī)器人的安全行進(jìn)規(guī)則。

圖5 融合算法起始規(guī)劃位置

圖6 避開(kāi)動(dòng)態(tài)障礙物

圖7 全局避障路徑規(guī)劃

對(duì)本文所提出算法和傳統(tǒng)動(dòng)態(tài)窗口法進(jìn)行多組仿真實(shí)驗(yàn)，以證明實(shí)驗(yàn)的準(zhǔn)確性，得到機(jī)器人碰撞次數(shù)、路徑長(zhǎng)度、運(yùn)行時(shí)間的對(duì)比數(shù)據(jù)，如表1所示。由表1的數(shù)據(jù)可知，改進(jìn)后的融合算法大幅度降低了碰撞次數(shù)，實(shí)現(xiàn)了機(jī)器人在運(yùn)動(dòng)障礙物環(huán)境下的避障規(guī)劃，同時(shí)也減少了機(jī)器人運(yùn)動(dòng)路徑長(zhǎng)度，提升了機(jī)器人的運(yùn)動(dòng)效率。

表1 改進(jìn)算法性能提升效果

4 結(jié) 語(yǔ)

本文針對(duì)傳統(tǒng)機(jī)器人路徑規(guī)劃沒(méi)有考慮到動(dòng)態(tài)障礙物環(huán)境、不符合實(shí)際需求且容易陷入局部最優(yōu)等問(wèn)題，提出一種基于動(dòng)態(tài)障礙物的機(jī)器人避障路徑規(guī)劃方法。首先通過(guò)對(duì)傳統(tǒng)A*算法的改進(jìn)得到路徑更短且安全性更高的全局規(guī)劃路徑軌跡；其次融合運(yùn)動(dòng)障礙物軌跡分析，改進(jìn)動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)，使得動(dòng)態(tài)窗口法能夠在存在動(dòng)態(tài)障礙物的環(huán)境下實(shí)現(xiàn)避障；最后將改進(jìn)后的動(dòng)態(tài)窗口法和改進(jìn)A*算法相融合，將改進(jìn)A*算法的最優(yōu)路徑序列點(diǎn)作為動(dòng)態(tài)窗口法的目標(biāo)，避免了DWA在局部路徑規(guī)劃容易陷入局部最優(yōu)的問(wèn)題，最終得到一種兼顧全局最優(yōu)且能實(shí)時(shí)避開(kāi)動(dòng)態(tài)障礙物的路徑規(guī)劃算法。通過(guò)Matlab仿真實(shí)驗(yàn)分析，結(jié)果表明該算法對(duì)動(dòng)態(tài)復(fù)雜環(huán)境的適應(yīng)度高，能夠滿(mǎn)足移動(dòng)機(jī)器人的實(shí)際避障需求，具有一定的應(yīng)用價(jià)值。