常新新，胡 為，姬書得，岳玉梅

(沈陽(yáng)航空航天大學(xué) a.航空宇航學(xué)院； b.自動(dòng)化學(xué)院，沈陽(yáng) 110136)

0 引言

自主避障[1-3]是移動(dòng)機(jī)器人領(lǐng)域研究的重點(diǎn)及熱點(diǎn)問題。DWA算法[4-5]根據(jù)機(jī)器人的動(dòng)力學(xué)與運(yùn)動(dòng)學(xué)特性得到一系列速度的離散值，然后用這些速度模擬接下來(lái)一段時(shí)間內(nèi)機(jī)器人的軌跡，并采用評(píng)價(jià)函數(shù)評(píng)價(jià)這些軌跡，選取得分最高的軌跡對(duì)應(yīng)的速度作為下一階段的速度。DWA算法在每一階段需要評(píng)價(jià)的軌跡數(shù)目較多，增加了避障時(shí)間，而且在評(píng)價(jià)軌跡時(shí)，權(quán)重參數(shù)的選取不當(dāng)容易導(dǎo)致機(jī)器人在密集障礙物區(qū)無(wú)法選擇最佳路徑的問題[6]。文獻(xiàn)[7]提出了一種改進(jìn)的DWA算法，通過引入代價(jià)函數(shù)，使用線性回歸模型對(duì)避障路徑成本進(jìn)行估計(jì)，選擇較為合理的避障路徑。文獻(xiàn)[8]針對(duì)DWA算法，提出一種新的篩選允許速度的方法，針對(duì)非直接碰撞軌跡，在其終點(diǎn)處以其速度方向做射線，將與障礙物不相交的軌跡對(duì)應(yīng)的速度設(shè)為允許速度。該方法考慮了預(yù)軌跡終點(diǎn)速度的方向，避免了部分可用速度被舍棄，使得機(jī)器人能夠選擇較合理的速度快速通過障礙物密集區(qū)。

針對(duì)DWA避障算法存在的問題，本文提出一種較優(yōu)方位角范圍約束，即在模擬出預(yù)軌跡后，依據(jù)計(jì)算準(zhǔn)則計(jì)算出較優(yōu)方位角范圍，然后選取出下一時(shí)刻在較優(yōu)方位角范圍內(nèi)的預(yù)軌跡，最后通過軌跡評(píng)價(jià)函數(shù)確定最優(yōu)軌跡對(duì)應(yīng)的速度。該方法在原算法的基礎(chǔ)上進(jìn)一步優(yōu)化了允許速度的空間，一定程度上減少了需要評(píng)價(jià)的軌跡數(shù)目，減弱了權(quán)重參數(shù)在原算法中選擇最優(yōu)速度時(shí)的決定性作用，提高了避障規(guī)劃的效率。

1 DWA算法

1.1 DWA算法簡(jiǎn)介

DWA算法是基于機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)理論的一種局部避障[9]算法，它將對(duì)機(jī)器人的位置控制轉(zhuǎn)換為對(duì)機(jī)器人的速度控制。DWA算法可以概括為三步：一是根據(jù)機(jī)器人自身的限制以及環(huán)境制約將速度的采樣空間約束在一定范圍內(nèi)；二是根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)對(duì)采樣后的速度進(jìn)行模擬得到預(yù)軌跡；三是設(shè)定評(píng)價(jià)函數(shù)對(duì)預(yù)軌跡進(jìn)行評(píng)分以獲取最優(yōu)軌跡對(duì)應(yīng)的執(zhí)行速度。

1.1.1 速度采樣空間[10-11]

根據(jù)機(jī)器人自身最大速度和最小速度限制，速度Vs(v,w)(v表示機(jī)器人的線速度，w表示機(jī)器人的角速度)滿足：

Vs={(v,ω)|vmin≤v≤vmax,ωmin≤ω≤ωmax}

(1)

考慮到機(jī)器人驅(qū)動(dòng)力的約束，存在最大的加減速限制，進(jìn)一步將速度空間進(jìn)行約束，用Vd表示。假設(shè)機(jī)器人當(dāng)前線速度為Vc，當(dāng)前角速度為ωc，時(shí)間間隔為t，則下一時(shí)刻的可達(dá)速度Vd(v,w)滿足如下條件：

(2)

基于移動(dòng)機(jī)器人的安全考慮，為了能夠在碰到障礙物前停下來(lái)，因此在最大減速條件下，速度滿足：

(3)

式中，dist(v,ω)為速度(v,ω)對(duì)應(yīng)軌跡上離障礙物最近的距離。該碰撞條件是在機(jī)器人軌跡模擬出來(lái)之后，根據(jù)機(jī)器人與障礙物之間的距離，然后計(jì)算當(dāng)前采樣的速度能否在碰到障礙物之前停下，如果能夠停下來(lái)，該速度就是允許的速度。

機(jī)器人最終的速度采樣空間為:

Vr=Vs∩Vd∩Va

(4)

1.1.2 速度采樣及軌跡生成

人為設(shè)定速度采樣的線速度分辨率以及角速度分辨率，得到所有速度組合。根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型對(duì)采樣的速度進(jìn)行模擬，得到一系列下一時(shí)刻的預(yù)軌跡。

1.1.3 軌跡評(píng)價(jià)

采樣的速度中，不止一組速度對(duì)應(yīng)的軌跡是可行的，為了獲取最優(yōu)的執(zhí)行速度，采用評(píng)價(jià)函數(shù)對(duì)每條軌跡進(jìn)行評(píng)價(jià)。評(píng)價(jià)函數(shù)定義為：

G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))

(5)

式中，使得G(v,w)值最大的速度為最優(yōu)速度；函數(shù)項(xiàng)heading(v,w)中heading=π-φ(φ表示用來(lái)評(píng)價(jià)機(jī)器人在當(dāng)前設(shè)定的采樣速度下，達(dá)到模擬軌跡末端機(jī)器人的航向與目標(biāo)線的夾角)，如圖1所示。

圖1 機(jī)器人方位角

函數(shù)項(xiàng)dist(v,w)用來(lái)評(píng)價(jià)機(jī)器人在當(dāng)前軌跡上與最近的障礙物之間的距離，若這條軌跡上沒有障礙物，則設(shè)定一個(gè)常數(shù)；函數(shù)項(xiàng)velocity(v,w)評(píng)價(jià)當(dāng)前軌跡對(duì)應(yīng)的速度大小，速度越大的評(píng)分越高；α,β,γ為3個(gè)參數(shù)，代表每一個(gè)函數(shù)項(xiàng)的權(quán)值。

待評(píng)價(jià)項(xiàng)heading(v,w)、dist(v,w)和velocity(v,w)計(jì)算出來(lái)后，需要做歸一化處理，然后再相加，歸一化的準(zhǔn)則是每一項(xiàng)除以每一項(xiàng)的總和。即：

(6)

式中，i為待評(píng)價(jià)的當(dāng)前軌跡點(diǎn)，n為采樣的所有軌跡點(diǎn)。

1.2 改進(jìn)的DWA算法

本文提出一種改進(jìn)的DWA算法，在模擬出下一時(shí)刻的預(yù)軌跡后首先根據(jù)計(jì)算準(zhǔn)則確定機(jī)器人前進(jìn)的較優(yōu)方位角范圍，然后選取出較優(yōu)方位角范圍以內(nèi)的預(yù)軌跡，最后通過軌跡評(píng)價(jià)函數(shù)確定最優(yōu)軌跡對(duì)應(yīng)的速度。

當(dāng)機(jī)器人前進(jìn)方向上有多個(gè)障礙物時(shí)，根據(jù)機(jī)器人攜帶的單線激光雷達(dá)觀測(cè)到的障礙物的方位信息，計(jì)算可通行的較優(yōu)方位角范圍。具體步驟如下：

(1)坐標(biāo)系轉(zhuǎn)換

為方便計(jì)算，將激光雷達(dá)坐標(biāo)系原點(diǎn)與機(jī)器人坐標(biāo)系原點(diǎn)重合，建立機(jī)器人極坐標(biāo)系以及全局坐標(biāo)系，將機(jī)器人的方位角變換到全局直角坐標(biāo)系下。如圖2所示，機(jī)器人在全局坐標(biāo)系下的方位角為α，β為極坐標(biāo)系與全局坐標(biāo)系的變換角。

β=α-90°

(7)

已知機(jī)器人極坐標(biāo)系下終點(diǎn)的方位角為φ，則在全局坐標(biāo)系下的方位角為φ1，表示為：

φ1=φ-β

(8)

式(8)為機(jī)器人方位角由極坐標(biāo)系轉(zhuǎn)換到全局坐標(biāo)系下的轉(zhuǎn)換公式。

圖2 坐標(biāo)轉(zhuǎn)換圖

(2)可通行區(qū)域判定

如圖3所示，在機(jī)器人極坐標(biāo)系下，以θ2角度區(qū)域?yàn)槔绻噜徴系K物間的距離c≥dr(dr為機(jī)器人的寬度)，則可判定該區(qū)域?yàn)榭赏ㄐ袇^(qū)域[12]，否則為不可通行區(qū)域。圖4中a和b分別為機(jī)器人到障礙物A與障礙物B的距離，由余弦定理可得，兩障礙物間的最近距離c為：

(9)

圖3 可通行區(qū)域判定

(3)較優(yōu)方位角范圍確定

在機(jī)器人運(yùn)行環(huán)境已知的情況下，當(dāng)機(jī)器人距離目標(biāo)較遠(yuǎn)時(shí)，為避免機(jī)器人轉(zhuǎn)動(dòng)太大角度，優(yōu)先考慮與機(jī)器人航向較近的可通行區(qū)域，因此將機(jī)器人當(dāng)前航向與可通行區(qū)域夾角較小的方位角范圍設(shè)為較優(yōu)方位角范圍；反之，當(dāng)機(jī)器人與目標(biāo)點(diǎn)距離較近時(shí)，將目標(biāo)線與可通行區(qū)域夾角較小的方位角范圍設(shè)為較優(yōu)方位角范圍。

由于機(jī)器人在躲避障礙物過程中行走的路徑不規(guī)則，機(jī)器人與目標(biāo)點(diǎn)的距離可能不會(huì)呈單調(diào)遞減趨勢(shì)，因此設(shè)定距離準(zhǔn)則以描述機(jī)器人距離目標(biāo)點(diǎn)的遠(yuǎn)近。如圖4所示，在全局坐標(biāo)系下，機(jī)器人初始y坐標(biāo)為y0，以機(jī)器人和目標(biāo)點(diǎn)在y方向的坐標(biāo)差描述兩者距離的遠(yuǎn)近。較優(yōu)角度區(qū)域的計(jì)算準(zhǔn)則為：

(10)

yG-y0表示目標(biāo)點(diǎn)和機(jī)器人起始點(diǎn)在y方向的坐標(biāo)差；yr-y0表示機(jī)器人某時(shí)刻y方向的坐標(biāo)與起始點(diǎn)y坐標(biāo)的差值。

圖4 機(jī)器人與目標(biāo)點(diǎn)的距離準(zhǔn)則

較優(yōu)方位角范圍確定方法如下：

情景1：當(dāng)機(jī)器人與目標(biāo)方向較遠(yuǎn)時(shí)，計(jì)算機(jī)器人當(dāng)前航向與可通行區(qū)域夾角較小的方位角范圍。如圖5所示，在極坐標(biāo)系下，機(jī)器人共有4個(gè)可通行的候選方位區(qū)域：θ1,θ2,θ3,θ4，其中與機(jī)器人當(dāng)前位置最接近的角度區(qū)域?yàn)棣?和θ3。θ2和θ3均為機(jī)器人下一時(shí)刻的可通行區(qū)域，通過比較θ2和θ3區(qū)域與目標(biāo)線的角度差β1和β2，可知β1>β2。因此可將與航向較近的θ3區(qū)域設(shè)為機(jī)器人前進(jìn)的較優(yōu)方位區(qū)域。

θ3角度區(qū)域的起始為γ1，終止角度區(qū)域?yàn)棣?，轉(zhuǎn)換到全局直角坐標(biāo)系下表示為：

(11)

即機(jī)器人下一時(shí)刻前進(jìn)的較優(yōu)方位角范圍為[γ1-β,γ2-β]。

圖5 機(jī)器人航向與可通行區(qū)域關(guān)系示意圖

假設(shè)機(jī)器人坐標(biāo)系下，預(yù)測(cè)軌跡的終點(diǎn)的方位角為η，預(yù)測(cè)軌跡的終點(diǎn)和機(jī)器人當(dāng)前位置的連線與較優(yōu)角度區(qū)域邊界的夾角滿足：

(12)

情景2：當(dāng)機(jī)器人與目標(biāo)點(diǎn)距離較近時(shí)，計(jì)算目標(biāo)線與可通行區(qū)域夾角較小的方位角范圍。同理，由圖6可知，通過計(jì)算，θ2和θ3均為可通行的區(qū)域，通過比較θ2和θ3區(qū)域與機(jī)器人航向的角度差β3和β4，可知β3<β4。因此可將與目標(biāo)點(diǎn)較近θ2區(qū)域設(shè)為機(jī)器人前進(jìn)的較優(yōu)方位區(qū)域。

θ2角度區(qū)域的起始為γ3，終止角度區(qū)域?yàn)棣?，轉(zhuǎn)換到全局直角坐標(biāo)系下表示為：

(13)

即機(jī)器人下一時(shí)刻前進(jìn)的較優(yōu)方位角范圍為[γ3-β,γ4-β]。

圖6 目標(biāo)線與可通行區(qū)域關(guān)系示意圖

此時(shí)預(yù)測(cè)軌跡的終點(diǎn)和機(jī)器人當(dāng)前位置的連線與較優(yōu)角度區(qū)域邊界的夾角滿足：

(14)

改進(jìn)后的DWA算法步驟如下：

(1) 根據(jù)傳感器信息，得到機(jī)器人與障礙物的距離及方位，確定可通行區(qū)域；

(2) 根據(jù)機(jī)器人的動(dòng)力學(xué)與運(yùn)動(dòng)學(xué)特性以及機(jī)器人當(dāng)前速度和加速度，在速度采樣空間中對(duì)v與ω進(jìn)行離散采樣，得到一系列速度(v，ω)；

(3) 根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)特性對(duì)采樣的速度進(jìn)行模擬，進(jìn)而得到一系列預(yù)軌跡。根據(jù)以上準(zhǔn)則及方法計(jì)算下一時(shí)刻機(jī)器人前進(jìn)的較優(yōu)方位角范圍，剔除較優(yōu)方位角范圍以外的軌跡對(duì)應(yīng)的速度；

(4) 采用評(píng)價(jià)函數(shù)對(duì)剩余的軌跡進(jìn)行評(píng)價(jià)，首先將評(píng)價(jià)項(xiàng)按照式(6)的方法進(jìn)行歸一化處理，然后根據(jù)式(5)求出評(píng)價(jià)得分最高的一組軌跡對(duì)應(yīng)的速度(v，ω)作為下一時(shí)刻的執(zhí)行速度。

(5) 執(zhí)行最優(yōu)速度，判斷是否到達(dá)目標(biāo)點(diǎn)，若是，則結(jié)束運(yùn)行；若否，則返回第一步，進(jìn)入下一步循環(huán)。

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

對(duì)原DWA算法以及改進(jìn)的DWA算法進(jìn)行仿真，仿真環(huán)境為MATLAB 2019b，并對(duì)結(jié)果進(jìn)行分析。DWA算法涉及了機(jī)器人自身的多個(gè)參數(shù)以及算法參數(shù)，選取的參數(shù)如表1和表2所示。

表1 機(jī)器人自身參數(shù)

表2 原DWA算法參數(shù)

為了充分證明改進(jìn)的DWA算法的可行性以及避免算法適用環(huán)境的偶然性，本文設(shè)計(jì)了兩種機(jī)器人運(yùn)行環(huán)境，并進(jìn)行仿真實(shí)驗(yàn)，如圖7、圖8所示，驗(yàn)證在不同環(huán)境下，改進(jìn)的DWA算法在局部避障中的可行性及優(yōu)勢(shì)。

在仿真時(shí)，將機(jī)器人當(dāng)作質(zhì)點(diǎn)處理，但是在實(shí)際運(yùn)行中，機(jī)器人是有一定體積的，因此采用障礙膨脹處理的方法將機(jī)器人的尺寸考慮到障礙物上，仿真中采取的障礙物膨脹半徑為0.4 m。兩種運(yùn)行環(huán)境中機(jī)器人起點(diǎn)分別為(0,0)和(12,0)，目標(biāo)點(diǎn)分別為(12,14)和(0,14)，為便于區(qū)分對(duì)比，將原DWA算法實(shí)驗(yàn)的路徑軌跡設(shè)置為藍(lán)色，將改進(jìn)的DWA算法實(shí)驗(yàn)的路徑軌跡設(shè)置為綠色。由原DWA算法仿真的軌跡圖可知，機(jī)器人在運(yùn)行過程中容易陷入局部最優(yōu)，而且在密集障礙物區(qū)選擇的路徑不是最佳路徑，規(guī)劃的避障路徑相對(duì)較遠(yuǎn)，若在實(shí)際運(yùn)行中可能會(huì)增加機(jī)器人遇到動(dòng)態(tài)障礙物的概率，從而降低運(yùn)行的效率。由改進(jìn)后的DWA算法仿真結(jié)果可以觀察出，兩種不同的運(yùn)行環(huán)境下，機(jī)器人在密集障礙物區(qū)能夠選擇較優(yōu)方位角區(qū)域的速度運(yùn)行，機(jī)器人付出的的代價(jià)較小，而且運(yùn)行后的軌跡較為平滑。

(a) 原DWA算法仿真軌跡圖 (b) 改進(jìn)的DWA算法仿真軌跡圖圖7 運(yùn)行環(huán)境1

(a) 原DWA算法仿真軌跡圖 (b) 改進(jìn)的DWA算法仿真軌跡圖圖8 運(yùn)行環(huán)境2

對(duì)算法改進(jìn)前后的迭代次數(shù)與運(yùn)行時(shí)間進(jìn)行定量分析，如表3所示。由于運(yùn)行環(huán)境設(shè)置的復(fù)雜性以及時(shí)間分辨率較低等因素，導(dǎo)致避障中算法迭代的次數(shù)較多、運(yùn)行時(shí)間比較長(zhǎng)。但在參數(shù)設(shè)置相同的情況下，改進(jìn)后的DWA算法迭代次數(shù)相比原DWA算法要少，減少了需要評(píng)價(jià)的軌跡數(shù)目，而且運(yùn)行所用的時(shí)間少，在避障效率上有所提高。

表3 改進(jìn)前后迭代次數(shù)與運(yùn)行時(shí)間對(duì)比

3 結(jié)論

本文通過改進(jìn)DWA算法對(duì)移動(dòng)機(jī)器人避障效果進(jìn)行優(yōu)化，針對(duì)DWA算法提出了一種較優(yōu)方位角范圍約束及其計(jì)算準(zhǔn)則優(yōu)化允許速度空間。改進(jìn)后的DWA算法在一定程度上減少了需要評(píng)價(jià)的軌跡數(shù)目，減弱了原算法中權(quán)重參數(shù)對(duì)最優(yōu)軌跡的決定性作用。仿真結(jié)果表明：在密集障礙物區(qū)，采用改進(jìn)的DWA算法，機(jī)器人能夠以最佳路徑選擇較優(yōu)的區(qū)域通過，避障效率得到提高。本文只是將改進(jìn)的DWA算法在仿真環(huán)境下進(jìn)行了避障規(guī)劃實(shí)驗(yàn)，下一步將進(jìn)行實(shí)物實(shí)驗(yàn)驗(yàn)證改進(jìn)算法的有效性。