孫嘉明,劉衛(wèi)朋,*,鞏祥瑞,山圣旗

(1.河北工業(yè)大學(xué) 省部共建電工裝備可靠性與智能化國(guó)家重點(diǎn)實(shí)驗(yàn)室,天津 300131;2.河北工業(yè)大學(xué) 電氣工程學(xué)院,天津 300131)

0 引言

路徑規(guī)劃是實(shí)現(xiàn)移動(dòng)機(jī)器人智能化的重要技術(shù),一直以來(lái)也是機(jī)器人控制領(lǐng)域的一個(gè)重要研究熱點(diǎn)[1],其核心思想就是使機(jī)器人依據(jù)一個(gè)或者多個(gè)評(píng)價(jià)指標(biāo),在所應(yīng)用的路徑規(guī)劃環(huán)境中找到一條從起點(diǎn)到目標(biāo)點(diǎn)無(wú)碰撞的最優(yōu)或次優(yōu)路徑。目前常用的路徑規(guī)劃指標(biāo)有最短路徑距離、最短運(yùn)行時(shí)間、最少運(yùn)動(dòng)耗能等。根據(jù)傳感器所接收環(huán)境信息的不同,路徑規(guī)劃問題也可分為全局路徑規(guī)劃和局部路徑規(guī)劃兩種。其中,全局路徑規(guī)劃側(cè)重于整體路徑上的最優(yōu)性,局部路徑規(guī)劃則側(cè)重于機(jī)器人在運(yùn)行過程中的安全性。目前常用的全局路徑規(guī)劃算法有A*[2]、D*、RRT[3]算法等,常用的局部路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[4]、動(dòng)態(tài)窗口法、蟻群算法[5]、遺傳算法等。

動(dòng)態(tài)窗口法(Dynamic Window Approach,DWA)是基于機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的一種局部路徑規(guī)劃算法,該算法將路徑規(guī)劃問題轉(zhuǎn)化為在速度矢量空間中的約束優(yōu)化問題,在速度二維空間中采樣多組速度(線速度、角速度),并在一定時(shí)間間隔內(nèi),模擬移動(dòng)機(jī)器人在這些速度下的軌跡,在獲取多組可行軌跡后,根據(jù)所設(shè)計(jì)的評(píng)價(jià)函數(shù),選取最優(yōu)軌跡所對(duì)應(yīng)的速度(線速度、角速度),驅(qū)動(dòng)機(jī)器人完成局部避障行駛,是一種常用的局部路徑規(guī)劃算法。

文獻(xiàn)[6]通過引入全局路徑規(guī)劃結(jié)果作為參考軌跡,提出了方向評(píng)價(jià)子函數(shù)、平滑速度評(píng)價(jià)子函數(shù)和加速度評(píng)價(jià)子函數(shù),優(yōu)化了移動(dòng)機(jī)器人的路徑,但只在較短路徑中效果顯著,在較長(zhǎng)路徑下仍然存在規(guī)劃路徑過長(zhǎng)的問題。文獻(xiàn)[7]提出了一種將傳統(tǒng)A*算法與動(dòng)態(tài)窗口法相融合的方法。針對(duì)移動(dòng)障礙物碰撞問題,提出速度障礙區(qū)的概念,改進(jìn)了動(dòng)態(tài)窗口法,實(shí)現(xiàn)了動(dòng)態(tài)和靜態(tài)的實(shí)時(shí)避障,但該算法計(jì)算效率較低,規(guī)劃路徑時(shí)所用時(shí)間過長(zhǎng)。文獻(xiàn)[8]提出一種融合改進(jìn)A*與動(dòng)態(tài)窗口法的隨機(jī)避障方法,在相鄰的節(jié)點(diǎn)之間采用動(dòng)態(tài)窗口法進(jìn)行局部規(guī)劃,實(shí)現(xiàn)了路徑平滑和隨機(jī)避障,但并沒有考慮運(yùn)行時(shí)出現(xiàn)動(dòng)態(tài)障礙物時(shí)的情況。

由于傳統(tǒng)動(dòng)態(tài)窗口法在局部路徑規(guī)劃時(shí)缺乏中間點(diǎn)的引導(dǎo),因此在機(jī)器人運(yùn)行至“L”型、“C”型等障礙物距離相近的區(qū)域時(shí)會(huì)出現(xiàn)評(píng)價(jià)函數(shù)失靈的問題。如在穿過障礙物數(shù)量較多區(qū)域時(shí)會(huì)出現(xiàn)繞行的情況,增大了總路徑長(zhǎng)度。同時(shí),原有的評(píng)價(jià)函數(shù)中并未考慮出現(xiàn)動(dòng)態(tài)障礙物的情況。

針對(duì)以上缺陷,本文提出距離抑制向量的概念。當(dāng)機(jī)器人運(yùn)行路徑上出現(xiàn)動(dòng)態(tài)障礙物時(shí),首先根據(jù)機(jī)器人的運(yùn)動(dòng)方向與障礙物的運(yùn)行方向預(yù)測(cè)是否可能發(fā)生碰撞,然后計(jì)算距離抑制向量,得到新的評(píng)價(jià)子函數(shù);在全局最優(yōu)性方面,將傳統(tǒng)A*算法與改進(jìn)灰狼優(yōu)化算法結(jié)合:首先使用傳統(tǒng)A*算法計(jì)算節(jié)點(diǎn)代價(jià)值并將其作為灰狼優(yōu)化算法的初值。其次,改進(jìn)灰狼優(yōu)化算法的更新步,在每次迭代時(shí),考慮距離加權(quán)進(jìn)行位置向量更新;提取全局路徑中的轉(zhuǎn)折點(diǎn)作為DWA算法的中間指引點(diǎn),定義中間點(diǎn)評(píng)價(jià)子函數(shù),最后得到新的評(píng)價(jià)函數(shù),實(shí)現(xiàn)對(duì)傳統(tǒng)DWA算法在局部和全局規(guī)劃能力上的優(yōu)化。

1 傳統(tǒng)DWA算法的基本原理及改進(jìn)

1.1 傳統(tǒng)DWA算法基本原理

傳統(tǒng)的動(dòng)態(tài)窗口法通過對(duì)機(jī)器人運(yùn)動(dòng)時(shí)的線速度、角速度進(jìn)行采樣,根據(jù)機(jī)器人本身的限制以及環(huán)境的限制,可以將機(jī)器人的速度控制在一個(gè)確定的范圍內(nèi),進(jìn)而模擬出移動(dòng)機(jī)器人在這些速度下的軌跡。在獲取多組可行軌跡后,根據(jù)評(píng)價(jià)函數(shù)G(v,ω),對(duì)每條軌跡進(jìn)行打分:

其中:h(v,ω)為方位角評(píng)價(jià)函數(shù),用來(lái)評(píng)價(jià)機(jī)器人在當(dāng)前采樣的速度下,達(dá)到模擬軌跡終點(diǎn)時(shí)的朝向與目標(biāo)點(diǎn)之間的夾角θ,θ越小,評(píng)分越高;d(v,ω)為距離評(píng)價(jià)函數(shù),當(dāng)所選路徑與障礙物無(wú)碰撞時(shí),該項(xiàng)設(shè)置為一個(gè)常數(shù);s(v,ω)為速度評(píng)價(jià)函數(shù),用來(lái)評(píng)價(jià)當(dāng)前速度大小。

為了防止評(píng)價(jià)函數(shù)不連續(xù),3個(gè)評(píng)價(jià)函數(shù)的結(jié)果在相加之前要進(jìn)行歸一化處理:

在對(duì)所有評(píng)價(jià)子函數(shù)進(jìn)行歸一化處理后,可以得到一條平滑的路徑。

通過對(duì)傳統(tǒng)DWA算法[9]的原理分析可知,在進(jìn)行局部規(guī)劃時(shí),該算法存在以下問題:

1)在進(jìn)行局部規(guī)劃時(shí),由于沒有全局信息做指引,僅使用機(jī)器人在局部環(huán)境下的信息做參考,使得算法所計(jì)算出的全局路徑容易陷入局部最優(yōu),進(jìn)而導(dǎo)致路徑規(guī)劃失敗。

2)當(dāng)機(jī)器人運(yùn)動(dòng)至障礙物數(shù)量密集區(qū)域或多個(gè)碰撞距離相近的障礙物區(qū)域時(shí),會(huì)選擇繞開密集障礙物區(qū),從而導(dǎo)致總路徑過長(zhǎng),并且對(duì)于障礙物的狀態(tài)并沒有進(jìn)行區(qū)分,對(duì)動(dòng)態(tài)障礙物的躲避能力有限。

1.2 距離抑制向量的提出與評(píng)價(jià)函數(shù)的改進(jìn)

為了改善機(jī)器人使用傳統(tǒng)DWA算法時(shí)出現(xiàn)的兩個(gè)問題,首先根據(jù)機(jī)器人在運(yùn)動(dòng)路徑附近可能出現(xiàn)的障礙物類型將其分為3類:靜態(tài)已知障礙物、靜態(tài)未知障礙物及動(dòng)態(tài)障礙物。

為了使機(jī)器人運(yùn)動(dòng)至相近障礙物時(shí),能夠順利避開局部最優(yōu)點(diǎn),并且不出現(xiàn)繞行現(xiàn)象,提出一種距離抑制向量的概念,下面結(jié)合圖1進(jìn)行說明。

圖1 動(dòng)態(tài)障礙物環(huán)境下的運(yùn)動(dòng)示意圖Fig.1 Schematic diagram of motion in dynamic obstacle

在機(jī)器人運(yùn)動(dòng)的路徑附近出現(xiàn)靜態(tài)未知或動(dòng)態(tài)障礙物時(shí),距離抑制向量PCOn由機(jī)器人此時(shí)的位置O’、預(yù)測(cè)發(fā)生碰撞的位置PZn、障礙物的速度Vn及機(jī)器人與障礙物之間的相對(duì)位置NXn決定,并滿足以下關(guān)系:

其中,PCOn的方向與障礙物速度方向相同取正,表示該障礙物即將與機(jī)器人在預(yù)測(cè)的軌跡上發(fā)生碰撞;PCOn的方向與障礙物速度方向相反取負(fù),表示障礙物遠(yuǎn)離預(yù)測(cè)的運(yùn)動(dòng)軌跡,即不會(huì)與機(jī)器人發(fā)生碰撞。若靜態(tài)未知障礙物出現(xiàn)后無(wú)速度,如障礙物2,則距離抑制向量不存在。為了將距離抑制向量引入原動(dòng)態(tài)窗口法中,定義距離抑制評(píng)價(jià)子函數(shù)

其值越大代表障礙物對(duì)機(jī)器人的運(yùn)動(dòng)軌跡影響越大,即發(fā)生碰撞的概率越高,從而可以得到基于距離抑制向量的改進(jìn)型DWA算法新的評(píng)價(jià)函數(shù):

式(5)通過引入距離抑制評(píng)價(jià)子函數(shù),改善了原算法對(duì)于不同障礙物類型的區(qū)分能力,從而解決了機(jī)器人在使用傳統(tǒng)DWA算法進(jìn)行局部路徑規(guī)劃時(shí)無(wú)法規(guī)避動(dòng)態(tài)障礙物的問題。

2 傳統(tǒng)A*算法的基本原理及改進(jìn)

2.1 融合改進(jìn)灰狼與A*算法的全局路徑規(guī)劃算法

作為一種啟發(fā)式算法,A*算法是在靜態(tài)環(huán)境中用于求解最優(yōu)路徑的最有效的直接搜索算法之一,因此在室內(nèi)機(jī)器人的路徑規(guī)劃領(lǐng)域中得到了廣泛應(yīng)用。

在進(jìn)行路徑規(guī)劃探索時(shí),A*算法主要依靠評(píng)價(jià)函數(shù)得到所有可能路徑權(quán)重的代價(jià)值:

其中,G(n)為機(jī)器人運(yùn)動(dòng)起點(diǎn)到節(jié)點(diǎn)n的實(shí)際距離,H(n)為節(jié)點(diǎn)n到終點(diǎn)的估計(jì)距離,也稱作啟發(fā)函數(shù)。

大量的實(shí)驗(yàn)證明,A*算法在小環(huán)境中搜索效率高,并且能夠搜索到最優(yōu)路徑,但是,由于其本身計(jì)算方法的局限性,導(dǎo)致理論上搜索出的路徑轉(zhuǎn)折角度太大,不利于將其直接應(yīng)用在實(shí)際工作中。

2.2 灰狼優(yōu)化算法

灰狼優(yōu)化算法是由Mirjalili等人[10]在2014年提出的一種新型智能群體算法,該算法受灰狼的捕食行為啟發(fā)而來(lái),依據(jù)灰狼內(nèi)部的等級(jí)制度及狩獵原理尋找最優(yōu)值。狼群內(nèi)部按階級(jí)高低可分為α狼、β狼、γ狼、ω狼四級(jí)。

α狼為狼群中的頭狼。β狼作為整個(gè)狼群的第二階級(jí),服從于α狼,協(xié)助其進(jìn)行決策,也可支配其他階級(jí)的狼。γ狼與ω狼的作用與β狼類似,α狼、β狼、γ狼可以分別看為待解決問題的最優(yōu)解、次優(yōu)解及第3優(yōu)解。

在尋優(yōu)過程中,該算法首先確定當(dāng)前種群中的前三個(gè)最優(yōu)解,然后依據(jù)下式進(jìn)行目標(biāo)搜索:

初次搜索后,使得狼群內(nèi)其他成員向最優(yōu)解處(獵物處)移動(dòng):

此外,在每次搜索后,群體內(nèi)各成員依據(jù)下式更新自己的位置向量:

在式(7)～(9)中,t代表當(dāng)前進(jìn)行的迭代次數(shù),X代表當(dāng)前灰狼的位置向量,D代表候選狼與最優(yōu)三頭狼之間的距離向量,A為位置調(diào)節(jié)系數(shù),C為距離調(diào)節(jié)向量。

2.3 改進(jìn)灰狼優(yōu)化算法與A*算法的結(jié)合

為了進(jìn)一步增強(qiáng)灰狼優(yōu)化算法的全局搜索能力,使其在進(jìn)行規(guī)劃時(shí)對(duì)終止點(diǎn)的敏感度進(jìn)一步提升。在算法每次迭代結(jié)束后,對(duì)成員進(jìn)行位置向量更新之前,根據(jù)成員與最優(yōu)狼之間的距離,對(duì)位置向量X進(jìn)行加權(quán)修正。定義非線性距離權(quán)重系數(shù)

其中,di為成員與最優(yōu)狼之間的距離大小。在每次迭代后,對(duì)非線性更新權(quán)重進(jìn)行位置更新:

最后,對(duì)最優(yōu)距離加權(quán)得到最優(yōu)位置向量:

作為一種非線性優(yōu)化算法,灰狼優(yōu)化算法的初值選取對(duì)于算法收斂性有很大影響。因此,考慮將傳統(tǒng)A*算法中的F(n)值作為灰狼優(yōu)化算法的初值,即將路徑起始點(diǎn)周圍可行柵格的F(n)值自大到小排列,并將其值前三位所在柵格初始化為狼群的α狼、β狼、γ狼,其余柵格作為ω狼進(jìn)行迭代,這樣不僅保證了算法的收斂性,同時(shí)在原理上對(duì)傳統(tǒng)A*算法的搜索方式也進(jìn)行了改善,從而得到了一種新的全局路徑規(guī)劃算法(GMA*),其計(jì)算流程如圖2所示。

圖2 GMA*算法計(jì)算流程Fig.2 The calculation process of the GMA*algorithm

2.4 基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃

針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法缺乏全局信息指引的問題,將改進(jìn)后的全局規(guī)劃算法(GMA*)所規(guī)劃出的路徑轉(zhuǎn)折點(diǎn)作為動(dòng)態(tài)窗口法的中間指引點(diǎn),在式(1)的基礎(chǔ)上,引入中間點(diǎn)評(píng)價(jià)子函數(shù)與距離抑制評(píng)價(jià)子函數(shù),從而得到了基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法評(píng)價(jià)函數(shù):

其中,m(v,ω)為中間點(diǎn)評(píng)價(jià)子函數(shù),表示動(dòng)態(tài)速度模擬軌跡上的點(diǎn)與全局路徑中間點(diǎn)之間距離。最終得到了基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法,其計(jì)算流程如下:

1)初始化地圖參數(shù),初始化GMA*算法初始參數(shù)。

2)建立環(huán)境地圖。

3)運(yùn)行GMA*算法,得到全局最優(yōu)路徑。

4)提取路徑中間點(diǎn),存儲(chǔ)進(jìn)中間點(diǎn)評(píng)價(jià)函數(shù)中。

5)初始化改進(jìn)型DWA算法參數(shù)。

6)確定速度窗口、形成運(yùn)動(dòng)軌跡。

7)評(píng)價(jià)函數(shù)歸一化,判斷是否到達(dá)目的地。

8)如到達(dá)目的地,則輸出最優(yōu)路徑,否則返回第6步,循環(huán)搜索直到到達(dá)終點(diǎn)。

圖3為該算法的整體流程圖。

圖3 基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法計(jì)算流程Fig.3 The calculation process of dynamic path planning algorithm based on distance suppression vector

3 仿真驗(yàn)證

為了驗(yàn)證改進(jìn)后的新算法的性能,在MATLAB 2018a環(huán)境中建立柵格地圖模型進(jìn)行仿真驗(yàn)證。

3.1 融合A*與改進(jìn)灰狼算法仿真實(shí)驗(yàn)

由于機(jī)器人在實(shí)際運(yùn)行中所在的環(huán)境較為復(fù)雜。為了簡(jiǎn)化計(jì)算,保證仿真的實(shí)時(shí)性和計(jì)算的精確性,本文通過建立柵格地圖的方式來(lái)模擬實(shí)際環(huán)境。其中,障礙物為柵格地圖中的黑色方格,位置隨機(jī)生成,地圖復(fù)雜度以黑色方格數(shù)量占總柵格數(shù)量的百分比及地圖邊長(zhǎng)決定。柵格地圖大小設(shè)置為50m×50 m,障礙物占總柵格數(shù)量的百分比設(shè)置為20%,為了保證算法規(guī)劃出路徑的一般性,選擇起點(diǎn)和終點(diǎn)盡量位于地圖的對(duì)角線位置。設(shè)置起始點(diǎn)為[0,5],目標(biāo)點(diǎn)為[27,20]。為了保證算法可靠收斂,最大迭代次數(shù)設(shè)置為500,仿真結(jié)果如圖4所示。

圖4 模擬復(fù)雜環(huán)境下融合算法(GMA*)與傳統(tǒng)A*算法仿真結(jié)果Fig.4 Simulation results of the fusion algorithm(GMA*)and the traditional A*algorithm in a simulated complex environment

圖5為傳統(tǒng)A*算法與本文算法在7組不同地圖復(fù)雜度環(huán)境下的仿真數(shù)據(jù)比較。

可以發(fā)現(xiàn),在環(huán)境地圖不變的條件下,融合算法(GMA*)在進(jìn)行全局路徑規(guī)劃時(shí),在路徑長(zhǎng)度以及轉(zhuǎn)折角度的減少方面,相比于傳統(tǒng)A*算法均有較大提升,從而證明了其理論上的優(yōu)越性。

圖5 改進(jìn)前后兩種算法的性能對(duì)比Fig.5 Performance comparison of the two algorithms before and after the improvement

3.2 改進(jìn)型DWA算法仿真實(shí)驗(yàn)

在仿真之前首先設(shè)置算法所需的基本參數(shù),包括機(jī)器人的機(jī)械特性參數(shù)、DWA算法的評(píng)價(jià)函數(shù)參數(shù)以及障礙物參數(shù)。為了使仿真過程中產(chǎn)生的誤差在可接受的閾值內(nèi),機(jī)器人的機(jī)械特性參數(shù)設(shè)置為實(shí)驗(yàn)用機(jī)器人Turtlebot3-Burger的機(jī)械特性參數(shù),如表1所示。

表1 機(jī)器人機(jī)械特性參數(shù)Tab.1 Robot mechanical characteristic parameters

由于機(jī)器人通過激光雷達(dá)獲取外界地圖,并采用基于粒子濾波的GMapping-SLAM算法建立環(huán)境地圖,故最大線加速度與角加速度不宜設(shè)置過大,以免出現(xiàn)環(huán)境柵格地圖在轉(zhuǎn)角處失真的現(xiàn)象。

改進(jìn)后的DWA算法引入距離抑制評(píng)價(jià)子函數(shù)及中間點(diǎn)評(píng)價(jià)子函數(shù),為了使距離抑制向量以及中間點(diǎn)在局部規(guī)劃和全局規(guī)劃時(shí)發(fā)揮更明顯的作用,考慮將以上兩個(gè)子函數(shù)的權(quán)重設(shè)置為0.50。DWA算法基本參數(shù)設(shè)置如表2所示。

表2 DWA算法評(píng)價(jià)函數(shù)參數(shù)Tab.2 DWA algorithm evaluation function parameters

考慮到機(jī)器人自身尺寸與電機(jī)最大減速度,為了保證在躲避障礙物時(shí)的安全性,將障礙物尺寸參數(shù)設(shè)置為0.50 m,最小碰撞閾值設(shè)置為0.50 m,障礙物位置橫坐標(biāo)X、縱坐標(biāo)Y設(shè)置范圍不超過地圖邊長(zhǎng),即X、Y的范圍均為0～50 m。

3.2.1 靜態(tài)環(huán)境規(guī)劃仿真

首先進(jìn)行靜態(tài)環(huán)境下的算法測(cè)試。在仿真環(huán)境中首先進(jìn)行環(huán)境地圖參數(shù)的設(shè)置,并初始化DWA算法所需參數(shù)。

圖6為傳統(tǒng)DWA算法所規(guī)劃出的路徑,可以發(fā)現(xiàn),在進(jìn)入虛線所示區(qū)域內(nèi)時(shí),因?yàn)闄C(jī)器人與四周障礙物的距離相差不大,出現(xiàn)了評(píng)價(jià)函數(shù)失靈的現(xiàn)象。

圖6 傳統(tǒng)DWA算法所規(guī)劃出的路徑Fig.6 The path planned by the traditional DWA algorithm

此外,在全局路徑的選擇上,當(dāng)使用傳統(tǒng)DWA算法進(jìn)行路徑規(guī)劃時(shí),由于算法并沒有考慮全局環(huán)境信息,只對(duì)局部環(huán)境信息進(jìn)行了提取,因此機(jī)器人選擇以繞行的方式避開障礙物密度大的區(qū)域,導(dǎo)致機(jī)器人整體路徑過長(zhǎng)。

在相同障礙物柵格環(huán)境下,使用改進(jìn)A*算法(GMA*)進(jìn)行全局規(guī)劃,并提取路徑中的轉(zhuǎn)折點(diǎn)作為中間指引點(diǎn),得到的全局規(guī)劃路徑如圖7所示。

圖7 GMA*算法所規(guī)劃出的路徑及中間指引點(diǎn)Fig.7 The path planned by the GMA*algorithm and the intermediate guide point

圖8為使用改進(jìn)后DWA算法所規(guī)劃出的路徑。可以明顯發(fā)現(xiàn),在機(jī)器人運(yùn)行至全局規(guī)劃路徑的中間點(diǎn)時(shí),在中間點(diǎn)評(píng)價(jià)子函數(shù)的作用下,機(jī)器人的路徑長(zhǎng)度得到了大幅度減小,也沒有出現(xiàn)繞行障礙物區(qū)域的現(xiàn)象,因此改進(jìn)后的算法實(shí)現(xiàn)了對(duì)局部評(píng)價(jià)函數(shù)失靈問題和繞行稠密障礙物區(qū)域兩個(gè)主要問題的優(yōu)化。

圖8 改進(jìn)型DWA算法規(guī)劃出的路徑Fig.8 The path planned by the improved DWA algorithm

3.2.2 動(dòng)態(tài)障礙物規(guī)劃仿真

為了測(cè)試機(jī)器人在運(yùn)行中出現(xiàn)動(dòng)態(tài)障礙物時(shí)的算法性能,在仿真環(huán)境中分別加入不同類型的障礙物進(jìn)行了仿真分析,如圖9所示。

圖9 出現(xiàn)動(dòng)態(tài)障礙物時(shí)的仿真結(jié)果Fig.9 Simulation result with dynamic obstacles

在全局規(guī)劃路徑上加入臨時(shí)出現(xiàn)的靜態(tài)未知障礙物,并在機(jī)器人運(yùn)行過程中,設(shè)置一個(gè)運(yùn)動(dòng)軌跡與機(jī)器人路徑存在交點(diǎn)的動(dòng)態(tài)障礙物。如圖9(a)所示,當(dāng)機(jī)器人運(yùn)行至動(dòng)態(tài)障礙物附近時(shí),對(duì)于動(dòng)態(tài)障礙物可以有效地進(jìn)行躲避。

由圖9(b)中可以看出,基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法不僅實(shí)現(xiàn)了對(duì)全局路徑中中間點(diǎn)的提取,得到了一條光滑的全局路徑,而且在機(jī)器人路徑上出現(xiàn)動(dòng)態(tài)與靜態(tài)位置障礙物時(shí),也能夠順利進(jìn)行規(guī)避,從而提高了運(yùn)行過程中的安全性。

4 實(shí)驗(yàn)驗(yàn)證

為了進(jìn)一步驗(yàn)證改進(jìn)后的優(yōu)化算法在機(jī)器人實(shí)際運(yùn)行中的性能,本文選擇使用Willow Garage公司出品的Turtlebot3-Burger機(jī)器人平臺(tái)進(jìn)行實(shí)驗(yàn)[11],實(shí)驗(yàn)場(chǎng)地為5 m×8 m的實(shí)驗(yàn)室。通過擺放紙箱及擋板模擬環(huán)境中的障礙物。在實(shí)驗(yàn)開始前首先通過Wi-Fi建立樹莓派與上位筆記本電腦之間的通信,然后使用基本SLAM功能包GMapping-SLAM建立室內(nèi)環(huán)境的柵格地圖[12]。

4.1 傳統(tǒng)A*算法與改進(jìn)A*算法(GMA*)實(shí)驗(yàn)對(duì)比

在獲得環(huán)境地圖的先驗(yàn)信息后,通過RViz中的2D Pose Estimate按鈕進(jìn)行動(dòng)態(tài)實(shí)時(shí)地圖與靜態(tài)地圖的矯正,使小車?yán)走_(dá)所掃描到的實(shí)時(shí)點(diǎn)云數(shù)據(jù)與柵格地圖重合。首先使用傳統(tǒng)A*算法[13]進(jìn)行路徑規(guī)劃實(shí)驗(yàn),通過點(diǎn)擊2D Nav Goal[14]按鈕設(shè)定路徑終點(diǎn),所規(guī)劃的路線如圖10(a)所示;同理設(shè)置相同的起點(diǎn)和終點(diǎn),使用改進(jìn)后的算法(GMA*)進(jìn)行實(shí)驗(yàn),規(guī)劃出的路線如圖10(b)所示。

圖10 改進(jìn)前后算法實(shí)驗(yàn)路徑對(duì)比Fig.10 Comparison of algorithm test paths before and after the improvement

從圖10(a)中可以看出,傳統(tǒng)A*算法在障礙物密集的環(huán)境中進(jìn)行路徑規(guī)劃時(shí),規(guī)劃出的路徑長(zhǎng)度較長(zhǎng),并且路徑轉(zhuǎn)折角度不平滑,在進(jìn)入密集障礙物區(qū)域時(shí),出現(xiàn)了繞行的現(xiàn)象。

圖10(b)為本文提出的改進(jìn)后的全局路徑規(guī)劃算法GMA*所規(guī)劃的路徑,相比原A*算法能夠有效地縮短路徑長(zhǎng)度,當(dāng)機(jī)器人駛?cè)胝系K物密集區(qū)域時(shí),也能夠保持全局路徑最短的性能指標(biāo),路徑轉(zhuǎn)折角度也得到了減小。

4.2 基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證改進(jìn)后的DWA算法性能,將實(shí)驗(yàn)場(chǎng)景分別設(shè)置為室內(nèi)實(shí)驗(yàn)室及長(zhǎng)距離走廊,動(dòng)態(tài)未知障礙物由另一臺(tái)上位機(jī)控制的工業(yè)機(jī)器人底盤代替,靜態(tài)未知障礙物由臨時(shí)增加的擋板代替。實(shí)驗(yàn)結(jié)果如圖11所示。

圖11 動(dòng)態(tài)路徑規(guī)劃實(shí)驗(yàn)環(huán)境及規(guī)劃路徑Fig.11 Dynamic path planning experimental environment and planned path

由圖11可知,Turtlebot3-Burger在使用本文提出的基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法進(jìn)行路徑規(guī)劃時(shí),不僅能在地圖全局信息的基礎(chǔ)之上完成全局路徑規(guī)劃,并且在機(jī)器人運(yùn)行過程中也能夠躲避未知的靜態(tài)與動(dòng)態(tài)障礙物。

以上實(shí)驗(yàn)結(jié)果表明,本文提出的基于距離抑制向量的機(jī)器人動(dòng)態(tài)路徑規(guī)劃算法能夠在復(fù)雜環(huán)境中更加高效完成動(dòng)態(tài)避障與路徑規(guī)劃?？紤]到實(shí)驗(yàn)用機(jī)器人本體質(zhì)量較輕,可能會(huì)出現(xiàn)輪胎打滑、轉(zhuǎn)彎不穩(wěn)定導(dǎo)致定位與環(huán)境地圖不重合等問題,在每次實(shí)驗(yàn)后可通過對(duì)機(jī)器人舵機(jī)進(jìn)行校正減小實(shí)驗(yàn)誤差。

5 結(jié)論

針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法進(jìn)行局部路徑規(guī)劃時(shí)出現(xiàn)評(píng)價(jià)函數(shù)失靈及繞行障礙物的缺點(diǎn),本文提出了一種基于距離抑制向量的改進(jìn)型DWA路徑規(guī)劃算法。通過引入距離抑制向量及全局中間點(diǎn),使得機(jī)器人在運(yùn)行過程中能夠?qū)ξ粗膭?dòng)態(tài)障礙物完成躲避,從而提升了機(jī)器人在運(yùn)行時(shí)的可靠性;針對(duì)傳統(tǒng)A*算法在路徑長(zhǎng)度和轉(zhuǎn)折角度上存在的問題,將其與改進(jìn)后的灰狼優(yōu)化算法相結(jié)合,利用距離權(quán)重系數(shù)改進(jìn)更新步,大幅度減少了機(jī)器人在復(fù)雜環(huán)境中進(jìn)行路徑規(guī)劃時(shí)的路徑長(zhǎng)度及運(yùn)行時(shí)產(chǎn)生的轉(zhuǎn)折角度,最后將兩種改進(jìn)算法結(jié)合,得到了基于距離抑制向量的動(dòng)態(tài)路徑規(guī)劃算法。仿真及實(shí)驗(yàn)表明,本文算法在實(shí)際應(yīng)用中表現(xiàn)出較好的路徑規(guī)劃性能。