鄭 維，劉德垚，王洪斌，郝若蘭

(燕山大學 電氣工程學院，河北 秦皇島 066000)

E-mail：hb-wang@ysu.edu.cn

1 引 言

隨著移動機器人廣泛地被用于工業(yè)制造，醫(yī)療服務，農(nóng)業(yè)生產(chǎn)等領(lǐng)域，如何能在動態(tài)復雜環(huán)境中為機器人規(guī)劃路徑是現(xiàn)今無人駕駛領(lǐng)域研究的主流問題.軌跡規(guī)劃可以描述為在信息未知的靜態(tài)或動態(tài)場景中，自主移動機器人利用攜帶的傳感器檢測其周圍障礙物的實時分布情況從而及時處理環(huán)境的變化，從起點安全且無碰撞的行駛到達目標位置[1].機器人行駛時環(huán)境信息多變，在有不確定因素出現(xiàn)的環(huán)境中為機器人規(guī)劃出一條安全路徑極具實際研究意義和應用價值[2].

Buniyamin提出了用于軌跡規(guī)劃的簡單且有效的PointsBug算法[3].此外，普遍使用的基于模型的軌跡規(guī)劃的算法包括彈性帶結(jié)合模型預測軌跡規(guī)劃器算法[4]，多約束模型預測控制[5，6]等等.以上算法[4-6]能預測軌跡，但缺點是需要完善的規(guī)劃模型以獲得良好預測能力.

1996年Simmons首次將軌跡規(guī)劃問題公式化為速度約束空間的優(yōu)化問題，提出了曲率速度法[7]，在此基礎(chǔ)上，2002年Fox等人提出動態(tài)窗口法(Dynamic Window Approach，DWA)[8]，該方法可以考慮機器人的運動學特性，是一種將軌跡規(guī)劃問題公式化為速度約束空間的優(yōu)化問題的模型預測控制算法，能夠直接輸出速度控制指令，因此，規(guī)劃的軌跡更平滑且更符合實際對機器人的控制.隨后，國內(nèi)外大批學者加入對DWA算法的研究，2013年Maroti提出了具有目標點距離函數(shù)的DWA算法，通過消除多數(shù)情況下的局部極小問題提高DWA的評估性能，但由于權(quán)重不合理使機器人仍被困住[9].2017年Kintak提出了一種通過計算機器人到障礙物的最小距離和目標角度來計算角速度之差的局部軌跡規(guī)劃器[10].2018年Abubakr提出利用激光測距儀分析機器人的尺寸與機器人前方自由空間關(guān)系的DWA算法，在簡單環(huán)境中防止了機器人出現(xiàn)狹窄通道打圈震蕩問題[11].同年，Vakhshour比較了模糊邏輯和自適應神經(jīng)模糊推理系統(tǒng)的區(qū)別，證明模糊控制器具備自適應神經(jīng)模糊推理系統(tǒng)隨環(huán)境的變化聯(lián)合調(diào)整所有權(quán)重的功能[12].此外，GIS緩沖區(qū)技術(shù)是通過描述對象的空間關(guān)系和空間行為，為對象的空間查詢和空間分析提供參考，近而輔助空間決策的技術(shù)[13，14].2020年Jiang等人將緩沖區(qū)技術(shù)與A*算法結(jié)合，算法在靜態(tài)環(huán)境中取得良好的軌跡規(guī)劃效果[15].上述文獻中DWA只應用于靜態(tài)環(huán)境中，當環(huán)境是動態(tài)時，需要隨環(huán)境的變化更新DWA評價函數(shù)的權(quán)重參數(shù).2019年Wang等人提出密集障礙物中的模糊自適應DWA，根據(jù)機器人和障礙物間的距離調(diào)整速度子函數(shù)的權(quán)重，但算法中對障礙物的位置分析仍存在不足[16].目前對DWA算法的研究雖取得一定成果，但由于DWA軌跡評價功能的不足，且都未同時考慮根據(jù)機器人自身安全性，姿態(tài)，預測時間，行駛方向與平均障礙物方向及其與目標位置間的關(guān)系等信息調(diào)整DWA的軌跡評價值，在未知、動態(tài)且復雜的場景中，使用DWA規(guī)劃出機器人的有效局部行駛軌跡仍具有挑戰(zhàn)性.

基于上述研究不足，針對未知、動態(tài)且復雜的場景，為規(guī)劃出機器人的有效行駛軌跡，本文提出基于三級環(huán)境緩沖區(qū)結(jié)合模糊邏輯的DWA算法.首先，改進和增加了DWA的子功能函數(shù).其次，構(gòu)建三級緩沖區(qū)因子并將其納入DWA算法中，機器人根據(jù)自身安全等級調(diào)整線速度和角速度；全面考慮時變的環(huán)境因素和機器人自身狀態(tài)，設(shè)計模糊控制器，以動態(tài)調(diào)整機器人的姿態(tài)與速度.

2 基本的動態(tài)窗口法

2.1 非全向運動模型

建立機器人在時間間隔Δt內(nèi)運動的非全向運動學模型，t+1時刻移動機器人的位姿如式(1)，

x(t+1)=x(t)+vtΔtcos(θt)
y(t+1)=y(t)+vtΔtsin(θt)
θ(t+1)=θt+wtΔt

(1)

式中，x(t)和y(t)為機器人在全局坐標系中t時刻的坐標，θt為機器人在t時刻的航向角，vt和wt分別為移動機器人在t時刻的線速度和角速度.

2.2 速度搜索空間

DWA算法中的3種速度空間約束為：

1)運動學約束：移動機器人的所有可達速度中，最大和最小速度限制為，

Vm=(v，ω)v∈[vmin，vmax]
ω∈[ωmin，ωmax]

(2)

2)電機動力學約束形成的速度約束：電機的有限力矩為機器人在軌跡的預測周期內(nèi)建立了一個電機的加減約束，從而形成了速度約束如式(3).

Vd=(v，ω)v∈[vc-aΔt，vc+aΔt]
ω∈[ωc-aΔt，ωc+aΔt]

(3)

式中，vc、wc表示當前速度；a、a表示機器人最大加速度.

3)移動機器人安全允許距離的速度約束：為確保機器人與障礙物碰撞前將速度降為0m/s，其制動的速度約束表示如式(4).

Vf=(v，ω)v≤(2d(v，ω)a)12
ω≤(2d(v，ω)a)12

(4)

式中，d(v，w)為(v，w)對應的軌跡與障礙物的最近距離.

速度V的搜索空間應定義如式(5).

V=Vm∩Vd∩Vf

(5)

2.3 評價函數(shù)

DWA的最佳速度指令需要滿足躲避障礙物、路徑長度短并且快速行駛到目標點的要求，因此，基本DWA的軌跡評價函數(shù)如式(6).

G(v，w)=σ(α·h(v，w)+β·Od(v，w)+γ·ve(v，w))

(6)

式中，α，β，γ分別表示目標方位角函數(shù)、與最近障礙物的距離函數(shù)和速度函數(shù)的權(quán)重系數(shù).σ表示歸一化處理.函數(shù)h(v，w)用來評估預測軌跡未端機器人的行駛方向與目標位置的角度差.函數(shù)Od(v，w)用來評估預測軌跡未端機器人與障礙物的最小距離.函數(shù)ve(v，w)是評估移動機器人速度大小的函數(shù)，表示如式(7)所示.

ve(v，w)=σ(v)

(7)

最佳的避障速度指令是與最大軌跡評價函數(shù)值對應的速度組合，如式(8)所示.

Vcmd=ArgMax{G(v，w)}

(8)

3 評價函數(shù)的改進

本文改進了DWA的3種原始子函數(shù)的評價功能以提高其軌跡評估能力，同時增加了兩種子函數(shù)以適應障礙物分布復雜的環(huán)境.

3.1 原始子函數(shù)的改進

原子函數(shù)h(v，w)和Od(v，w)以預測軌跡未端的機器人位置作為參考位置，該位置是機器人在預測時間段T=n·Δt內(nèi)行駛到的位置，由于速度每隔Δt秒被重新選擇，這意味著機器人每Δt秒內(nèi)只會沿整個預測軌跡的一小部分運動.因此，改進后的子函數(shù)h*(v，w)和Od*(v，w)將參考位置設(shè)置在接近當前位置的某個位置，與當前位置的距離稱為行進距離.首先設(shè)置行進距離d，需要根據(jù)行進距離d的值和圖1中的幾何關(guān)系確定預測時間段T的數(shù)學表達式，如式(9)所示.

T=π/w2r≤d
2arcsind2r/w2r>d

(9)

式中，v和w分別是機器人的線速度和角速度；r=v/w是預測軌跡的半徑.

圖1 預測時間T的計算方法Fig.1 Calculation method of time period T

改進的函數(shù)h*(v，w)和Od*(v，w)機器人的新參考位置都是在預測軌跡中經(jīng)過行進距離d后的位置.

圖2(a)和圖2(b)中，最小方形號表示目標位置.在對比軌跡A和軌跡B時，障礙物、目標位置和理想軌跡都相同.以上描述適用于圖3-圖8.

由圖2(a)可知，軌跡A比軌跡B更接近理想軌跡，當參考位置在預測軌跡的末端時，θB，0<θA，0，所以軌跡A 的h(v，w)值較低；若參考位置設(shè)置在接近當前位置的某個位置時，θB，n>θA，n，軌跡A的h*(v，w)值更高.

圖2 改進的子函數(shù)對軌跡的影響Fig.2 Effect of the improved sub-function on the trajectory

在圖2(b)中，軌跡A比軌跡B更接近理想軌跡.按照原函數(shù)Od(v，w)，因為dA，0小于安全距離，所以軌跡A應該被丟棄，且所有和軌跡A相似的軌跡都可能被丟棄，而類似軌跡B的軌跡評價值更高，但類似軌跡A卻不會與障礙物碰撞的軌跡不應被丟棄.改進的函數(shù)Od*(v，w)在丟棄軌跡時，從軌跡的起點到經(jīng)過幾個時間步長后的位置，計算機器人和障礙物的最近距離，由圖2(b)可知，dA，n≈dB，n，且都大于安全距離，因此可以保留這兩條軌跡并根據(jù)其他子函數(shù)進行軌跡評估.

子函數(shù)ve(v，w)的功能是評價線速度，改進后的子函數(shù)ve*(v，w)由機器人的線速度和角速度共同確定，目的是使機器人盡可能快地行駛并避免不必要的轉(zhuǎn)彎.在相同情況下，線速度越高機器人到達目標位置的速度就越快，因此，無需改變線速度的評價函數(shù)，僅在ve(v，w)中增加角速度.在有很多障礙物的環(huán)境中，機器人的最佳行為是在沒有平移運動的情況下轉(zhuǎn)向目標，因此，以較低的線速度和較高的角速度做轉(zhuǎn)彎運動可以降低與障礙物碰撞的風險.改進后的子函數(shù)ve*(v，w)如式(10)所示.

ve*(v，w)=v+(wmax-kvvmaxw)

(10)

式(10)中，角速度中增加vvmax項以測量當前的歸一化線速度；k是一個參數(shù)；wmax是角速度最大值.

對3項子函數(shù)分別進行歸一化處理，如式(11)-式(13).

n_h*(v，w)=(h*(v，w)-h*min)/(h*max-h*min)

(11)

n_Od*(v，w)=(Od*(v，w)-Od*min)/(Od*max-Od*min)

(12)

n_ve*(v，w)=(ve*(v，w)-v*emin)/(v*emax-v*emin)

(13)

3.2 增加的子函數(shù)

3.2.1 增加的成本函數(shù)

基于緩沖區(qū)技術(shù)建立成本函數(shù)osc(v，w)，其功能是評價移動機器人的歷史軌跡與當前軌跡的接近程度，其目的是增強移動機器人的穩(wěn)定性，防止機器人返回到已經(jīng)行駛過的地方或原地繞圈行駛，這在有狹窄通道的環(huán)境中避障時十分重要，該函數(shù)的權(quán)重為μ.成本函數(shù)osc(v，w)的評價值越高表示接近程度越低，機器人越不容易在該位置打圈震蕩.

圖3 像元成本的更新方法Fig.3 Update method of pixel cost圖4 像元計算成本函數(shù)值Fig.4 Pixel calculation cost function value

更新機器人位置上或其附近的像元成本的方法如圖3所示，圖3中的O是t時刻機器人中心位置，以O(shè)為原點建立坐標系，Rerc是受機器人影響的半圓形區(qū)域的半徑，此區(qū)域中的像元成本需要被更新；lp，q表示坐標為(p，q)的像元和O點的距離；osc(vt，wt)表示t時刻像元(p，q)應增加的成本，為了防止當機器人的速度變化較大時受機器人影響的周圍像元成本迅速增加，在成本的計算中添加速度項，osc(vt，wt)的計算公式如式(14)所示.

osc(vt，wt)=lp，q(1-vt-vt-1vmax)Rrec

(14)

圖4示出了歷史軌跡和它的像元格組，像元格的顏色越深表示成本越高.函數(shù)osc(v，w)的評價值是預測軌跡掃過的像元成本和，對于圖4中的預測軌跡，osc(v，w)的評價值是所有被實線圈中的像元格的成本和.圖5中對比了軌跡A和軌跡B，圖5中的θB>θA且dA，0

圖5 增加函數(shù)osc(v，w)的軌跡比較Fig.5 Comparison of trajectories of increasing function osc(v，w)

3.2.2 增加的距離函數(shù)

子函數(shù)Gd(v，w)表示與速度(v，w)對應的預測軌跡末端和目標位置的最小距離，距離越近，Gd(v，w)的評價值越高，該函數(shù)的權(quán)重為λ.Gd(v，w)的計算如式(15)所示.

Gd(v，w)=1-dGdGmax

(15)

式中，dG是預測軌跡末端和目標位置的最小距離；dGmax為設(shè)定的dG的最大值.如果dG大于dGmax，則Gd(v，w)為負數(shù)，此時該項的評價值為零.

改進并增加了子函數(shù)后的軌跡評價函數(shù)如式(16).

G*(v，w)=σα·h*(v，w)+β·Od*(v，w)
+γ·ve*(v，w)+μ·osc(v，w)
+λ·Gd(v，w)

(16)

4 三級緩沖區(qū)結(jié)合模糊邏輯的DWA

利用上文改進的DWA在動態(tài)環(huán)境中規(guī)劃軌跡時，對每個子功能的需求都是時變的，需要全面考慮環(huán)境因素的變化動態(tài)調(diào)整各項子函數(shù)的權(quán)重和預測時間，因此，提出一種三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法，提供了動態(tài)分析預測時間并使用更多場景信息的機會.首先，構(gòu)建基于三級緩沖區(qū)的DWA，使機器人初步根據(jù)安全程度調(diào)整速度和角速度；然后，分析動態(tài)預測時間內(nèi)的軌跡；最后，全面考慮時變的環(huán)境因素和機器人自身狀態(tài)，設(shè)計模糊控制器以動態(tài)調(diào)整機器人的姿態(tài)與速度.

4.1 三級緩沖區(qū)結(jié)合模糊邏輯的DWA

以往研究中認為只要移動機器人不與障礙物相撞，空間就是安全的，但這個想法不夠嚴謹，本文提出了一種更合理的方式是，為機器人構(gòu)建三級緩沖區(qū)，分析不同的安全區(qū)域并建立不同的安全系數(shù)，機器人距離障礙物較遠的區(qū)域安全系數(shù)更高，而接近障礙物位置的區(qū)域安全系數(shù)較低.

圖6 激光雷達掃描區(qū)域劃分Fig.6 Lidar scanning area division

面型緩沖區(qū)是以面要素的邊緣向外擴展所生成的新多邊形，三級緩沖區(qū)的生成方式是，將機器人用面要素表示，根據(jù)激光雷達傳感器獲取的實際障礙物信息數(shù)據(jù)設(shè)置機器人的緩沖區(qū)半徑，在DWA軌跡評價函數(shù)中構(gòu)建不同屬性的緩沖區(qū)因子，從而生成面要素的不同安全程度的緩沖區(qū).具體實現(xiàn)為，機器人通過激光雷達感知障礙物的位置，根據(jù)角度的分布情況將激光雷達掃描的前方區(qū)域分為6個扇形區(qū)域，如圖6所示.圖6描述了機器人尺寸和車載激光雷達傳感器掃描區(qū)域.以激光雷達質(zhì)心為圓心，激光雷達掃描區(qū)域橫向平均劃分成6個角弧度為π/6的扇形區(qū)域：A區(qū)(A1，A2，A3)，B區(qū)(B1，B2，B3)，C區(qū)(C1，C2，C3)，D區(qū)(D1，D2，D3)，E區(qū)(E1，E2，E3)，F(xiàn)區(qū)(F1，F(xiàn)2，F(xiàn)3)；縱向劃分成三級緩沖區(qū)：A1，B1，…，F(xiàn)1表示一級緩沖區(qū)，一級緩沖區(qū)半徑為R1；A2，B2，…，F(xiàn)2表示二級緩沖區(qū)，二級緩沖區(qū)半徑為R2；A3，B3，…，F(xiàn)3表示三級緩沖區(qū)，三級緩沖區(qū)半徑R3.

移動機器人上的激光雷達在執(zhí)行任務過程中通過判斷障礙物到機器人的最近距離ρmin占各級緩沖區(qū)的半徑的比例，設(shè)計緩沖區(qū)因子q，如式(17)所示.

q=(2e-k1·ρmin-1)/2，R1<ρmin≤R2
(2e-k2·ρmin-1)/2，R2<ρmin≤R3
k3，ρmin>R3

(17)

式中，k1，k2，k3∈(0，1)是緩沖區(qū)因子的可調(diào)參數(shù).

將設(shè)計的緩沖區(qū)因子q作為參數(shù)納入DWA的速度子函數(shù)，使機器人能夠初步根據(jù)障礙物分布在幾級緩沖區(qū)，判斷自身的安全程度，從而根據(jù)當前的安全程度調(diào)整速度和角速度.定義三級緩沖區(qū)的優(yōu)勢為，避免兩個絕對安全的機器人位置由于與障礙物的距離不同而導致二者獲取到不同的權(quán)值這種情況發(fā)生.經(jīng)過反復試驗與調(diào)試可知，選擇三級緩沖區(qū)外部區(qū)域的概率更大，這將更有利于產(chǎn)生安全的軌跡.

基于三級緩沖區(qū)定義DWA的新軌跡評價函數(shù)如式(18)所示.

G**(v，w)=σα·h*(v，w)+β·Od*(v，w)

+q·γ·ve*(v，w)+μ·osc(v，w)

+λ·Gd(v，w)

(18)

4.2 動態(tài)預測時間內(nèi)的軌跡分析

在DWA執(zhí)行預測步驟時，計算最佳軌跡是必需的，此軌跡被認為是機器人的新可能位置.需要注意的是，采用較長的固定預測時間，計算成本將大大增加，且環(huán)境條件可能會改變.而且，如圖7所示，當機器人周圍存在許多障礙物時，預測時間段T過長會導致預測軌跡過長，函數(shù)Od*(v，w)的誤判使得部分可行的速度被丟棄，如軌跡A；某些軌跡朝向目標點但和障礙物距離太近，導致函數(shù)Gd(v，w)的誤判，如軌跡B.因此，為避免上述問題，需要動態(tài)調(diào)整d值從而動態(tài)調(diào)整預測時間T.

圖7 時間段T的軌跡分析Fig.7 Trajectory analysis of time period T

綜上，在軌跡評價函數(shù)中需要動態(tài)調(diào)整的6個參數(shù)分別是：函數(shù)的權(quán)重組合α、β、γ、λ、μ和行進距離d.

4.3 設(shè)計模糊控制器

4.3.1 定義模糊控制器的輸入函數(shù)

本文定義的4個輸入函數(shù)如下：

1)輸入函數(shù)Put1：

Put1=D

(19)

式(19)中，D表示移動機器人中心與目標點的距離.

D=ρGdmin+(a(I+2δ))/2

(20)

式中，D∈((-a(I+2δ))/2，R3+(a(I+2δ))/2)，參數(shù)a表示機器人自身的物理結(jié)構(gòu)參數(shù)；I表示機器人的長度；2δ表示在機器人兩側(cè)的安全空間余量；ρGdmin表示激光雷達傳感器到目標位置的最近距離.

根據(jù)目標位置是否在三級緩沖區(qū)內(nèi)，將Put1的模糊集定義為{S，B}，即{正小，正大}，如式(21)，

Put1=S，DB，D≥R3

(21)

式中，R3是機器人三級緩沖區(qū)的半徑.

2)輸入函數(shù)Put2：

Put2=θr，g

(22)

式中，θr，g表示機器人的運動方向與從機器人位置指向目標位置的直線間的夾角.

根據(jù)機器人朝目標位置移動的近似偏差將Put2定義為{W，Z，N，O}，即{正外，正中，正內(nèi)，零}，如式(23)，示意圖如圖8.

Put2=W，θr，g∈[0，π/6)∪[5π/6，π)
Z，θr，g∈[π/6，π/3)∪[2π/3，5π/6)
N，θr，g∈[π/3，π/2)∪[π/2，2π/3)
O，θr，g∈else

(23)

圖8 Put2的模糊集示意圖Fig.8 Schematic diagram of fuzzy set of Put2

3)輸入函數(shù)Put3：

Put3=d

(24)

式中，d表示機器人的行進距離，定義d如式(25)，

d=2vw，w>π
2vwsinw2，w≤π

(25)

式中，v和w分別是機器人的線速度和角速度.

根據(jù)行進距離的長短將Put3的模糊集定義為{S，B}，即{正小，正大}，見式(26)，示意圖如圖9所示.

Put3=S，d>0.5m
B，d≤0.5m

(26)

圖9 Put3的模糊集示意圖Fig.9 Schematic diagram of fuzzy set of Put3

4)輸入函數(shù)Put4：

Put4考慮機器人的尺寸，定義障礙物的主方向描述機器人周圍障礙物的近似分布.

激光雷達的兩個激光束li、lj與障礙物相交的兩點間的距離do的計算公式如式(27).

do=ρ2j+ρ2i-2cos(θj-θi)ρjρi

(27)

式中，ρi和ρj分別是兩個激光束li、lj檢測到機器人與障礙物的距離，θi和θj分別表示li、lj與水平線形成的角度.

找到近端區(qū)A區(qū)和F區(qū)中的兩個最短距離ρAmin，ρFmin和與其對應角度θAmin，θFmin；中端區(qū)B區(qū)和E區(qū)中的兩個最短距離ρBmin，ρEmin和與其對應角度θBmin，θEmin；遠端區(qū)C區(qū)和D區(qū)中的兩個最短距離ρCmin，ρDmin和與其對應角度θCmin，θDmin.根據(jù)式(27)計算近端區(qū)A和F的dAFo min.同理依次得中端區(qū)B和E的dBEo min和遠端區(qū)C和D的dCDo min，分別計算dAFo min、dBEo min和dCDo min與移動機器人寬度的差，得到DAFmin、DBEmin和DCDmin如式(28)-式(30).

DAFmin=dAFo min-a(2b+2δ)

(28)

DBEmin=dBEo min-a(2b+2δ)

(29)

DCDmin=dCDo min-a(2b+2δ)

(30)

式中，DAFmin，DBEmin，DCDmin∈(-a(2b+2δ)，2r-a(2b+2δ))；參數(shù)a表示機器人自身的物理結(jié)構(gòu)參數(shù)；2b和I分別表示機器人寬度和長度； 2δ表示在機器人兩側(cè)的安全空間余量.

通常障礙物間的距離越小，機器人靠近障礙物時越危險，所以該距離對應的角度占障礙物平均角度的比例應該越大，因此，用各區(qū)的最近障礙物夾角對應的距離倒數(shù)作為獲得平均角度的權(quán)重，障礙物的平均角度θoab是近端區(qū)、中端區(qū)和遠端區(qū)角度的加權(quán)和，如式(31)所示.

θoab=θFmin-θAminDAFmin+θEmin-θBminDBEmin+θDmin-θCminDCDmin1DAFmin+1DBEmin+1DCDmin

(31)

圖10 障礙物主導方向分析Fig.10 Analysis of the dominant direction of obstacles

圖10中θa和θb分別為θoab的兩邊與水平線的夾角，θmin為最近障礙物方向與水平線的夾角，θn，a和θn，b分別是平均障礙物方向和最近障礙物方向所夾的角度.分析圖10中的情況a和b可知，在情況a中，θn，a和θn，b較小，平均障礙物方向可以更準確地反映障礙物的分布；而情況b中在平均方向上沒有障礙物，因此平均方向不能反映障礙物的分布，此時，θn，a或θn，b通常較大，用最近障礙物方向θmin反映周圍障礙物的分布.障礙物的主方向θo如式(32)所示.

θo=θoab，θn，a<π/4 andθn，b<π/4
θmin，else

(32)

將輸入函數(shù)Put4用公式表示為：

Put4=θo

(33)

將輸入函數(shù)Put4的模糊集定義為{O，N，M，F(xiàn)}，即{零，正近，正中，正遠}，如式(34)，

Put4=N，θo∈[0，π/6)∪[5π/6，π)
M，θo∈[π/6，π/3)∪[2π/3，5π/6)
F，θo∈[π/3，π/2)∪[π/2，2π/3)
O，ρ>R3

(34)

式中，ρ表示機器人與障礙物的距離；R3表示三級緩沖區(qū)半徑.

Put1，Put2，Put3和Put4作為4個輸入變量，構(gòu)成C12C14C12C14=64組輸入組合，每組輸入組合對應一組輸出.

4.3.2 定義模糊控制器的輸出函數(shù)

α、β、γ、λ、μ和行進距離d作為輸出函數(shù)，它們的模糊集分為五個等級，為{VS，S，M，B，VB}，即{正極小，正小，正中，正大，正極大}.

4.4 三級緩沖區(qū)結(jié)合模糊邏輯的DWA 算法結(jié)構(gòu)

本文提出的三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法提供了分析預測時間并使用更多場景信息的機會，算法結(jié)構(gòu)如圖11所示.

圖11 三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法結(jié)構(gòu)Fig.11 DWA algorithm structure based on three-level buffer and fuzzy logic

5 算法的仿真驗證

5.1 仿真平臺

操作系統(tǒng)：Windows10，處理器I5，內(nèi)存8GB及以上，開發(fā)環(huán)境采用python3.7.

5.2 改進評價函數(shù)后算法的仿真驗證

靜態(tài)環(huán)境參數(shù)設(shè)置：復雜的靜態(tài)環(huán)境參數(shù)如表1所示，結(jié)合實際，設(shè)計地圖尺寸為120m×120m的仿真環(huán)境空間.

表1 靜態(tài)環(huán)境參數(shù)Table 1 Environmental parameter

機器人參數(shù)設(shè)置(機器人參數(shù)設(shè)置在全文的仿真中一致)：速度分辨率平衡了導航性能與計算成本，機器人的初始速度和姿態(tài)角為零，機器人參數(shù)設(shè)置如表2所示.

表2 機器人參數(shù)Table 2 Robot parameter

DWA算法初始參數(shù)設(shè)置(全文統(tǒng)一)：

α=0.1，β=0.6，γ=0.1，λ=0.1，μ=0.1，d=0.5m，dGmax=50m.

在同一由障礙物離散分布形成的多個狹窄通道的大環(huán)境中展示了固定參數(shù)下改進評價函數(shù)前、后的DWA算法的整體性能，以證明改進評價函數(shù)的DWA算法在復雜靜態(tài)環(huán)境的有效性.圖12(a)和圖12(b)分別示出了基本DWA算法和改進DWA算法對應的移動機器人行駛的軌跡.

圖12 復雜的靜態(tài)環(huán)境中移動機器人的行駛軌跡Fig.12 Driving trajectory of the mobile robot

由圖12可知，圖12(b)中機器人在A處穿過障礙物的狹窄通道，而非繞開A處行駛，機器人在B處穿過障礙物間隙并直接行駛到終點，因此由于子函數(shù)Od*(v，w)更改了軌跡的丟棄方式，改進后的軌跡明顯更優(yōu).圖12(a)和圖12(b)的C處證明改進后的子函數(shù)ve*(v，w)使機器人可以同時考慮線速度和角速度，改進后的DWA算法使機器人在C處以更短的軌跡繞過障礙物.圖12(a)和圖12(b)的A處軌跡改進前后的對比證明了增加的函數(shù)osc(v，w)防止了預測軌跡嚴重偏向歷史軌跡，改進的軌跡在A處并未出現(xiàn)打圈震蕩現(xiàn)象，并成功穿過A處；而未增加函數(shù)osc(v，w)前，改進前的軌跡落入局部陷阱并保持轉(zhuǎn)彎，并且機器人無法穿過通道A處.對比圖12(a)的D處和圖12(b)的B處，證明由于增加了子功能Gd(v，w)，使機器人的周圍有障礙物時向終點運動的趨勢增強，改進的軌跡穿過B處后直接向終點行駛，而改進前的軌跡在通道D處的轉(zhuǎn)彎角度明顯.改進評價函數(shù)的DWA的軌跡長度為198.654m，而基本DWA的軌跡長度為233.758m，改進后的DWA規(guī)劃軌跡更短.

圖12(a)和圖12(b)對比證明了固定參數(shù)下改進評價函數(shù)的DWA的整體性能，改進后的DWA規(guī)劃軌跡更短且更平滑，提高機器人在復雜環(huán)境中到達終點的能力和效率.

5.3 三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法的仿真驗證

針對未知且復雜的動態(tài)場景，為驗證基于三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法規(guī)劃軌跡的有效性，在仿真時使用足夠數(shù)量且隨機分布的動態(tài)障礙，動態(tài)障礙物空間包括：朝向主機器人行駛的兩個圓形移動機器人，速度為1.0m / s；50個位置隨機變化的動態(tài)障礙物.靜態(tài)障礙物分布和圖12中的相同.復雜的動態(tài)環(huán)境參數(shù)設(shè)置如表3所示.

表3 動態(tài)環(huán)境參數(shù)Table 3 Environmental parameter

在圖13中，在圖12的靜態(tài)環(huán)境基礎(chǔ)上，增加了離散分布的動態(tài)障礙和兩個圓形移動機器人，其中，動態(tài)障礙的尺寸和主機器人相同，在圖13中可由尺寸的大小明顯區(qū)分靜態(tài)障礙和動態(tài)障礙；帶局部軌跡的方形表示主移動機器人，其周圍的圓圈表示一級緩沖區(qū)，呈放射型分布的線段表示激光雷達的掃描區(qū)域，為更直觀地描述障礙物分布位置，激光雷達的顯示頻率設(shè)為每隔2秒顯示一次；主機器人的預測軌跡指向反映了其轉(zhuǎn)彎方向.圖13(a)-圖13(h)分別是提出的三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法對應不同時刻的主機器人的行駛軌跡，對應的主機器人的速度v和角速度w變化圖如圖15(a)所示.

如圖13(a)和圖13(b)所示，在60s和62s時，圓形機器人以1m/s的速度從對面駛來，落入了主機器人的二級緩沖區(qū)，同時主機器人周圍存在靜態(tài)障礙和隨機出現(xiàn)的動態(tài)障礙，主機器人此時的安全度很低，且機器人距離終點較遠，動態(tài)窗口中的預測軌跡覆蓋范圍小，在機器人的近端區(qū)存在障礙物.這種情況時在緩沖區(qū)和模糊控制器的共同作用下，機器人減速行駛，速度由3m/s 降低至1.2m/ s，提高角速度w，不需要考慮因行駛方向偏離終點引起的震蕩，因此可以增大預測軌跡與已經(jīng)行駛過的軌跡的接近程度，減小行進距離d以防止行進距離過長造成障礙物位置誤判，保證主機器人有較多的時間盡快轉(zhuǎn)彎，主機器人躲避開第一個圓形移動機器人和其他障礙物，其速度v和角速度w變化圖如圖15(a)(第8代左右時).同理，如圖13(c)所示，在72s時主機器人提前預判第二個圓形移動機器人和隨機動態(tài)障礙物位置從而調(diào)整自身轉(zhuǎn)向，降低速度，提高角速度，順利躲避了第二個圓形移動機器人和隨機動態(tài)障礙物，主機器人沒有打圈、繞路、碰撞或停車，主機器人的速度v和角速度w變化圖如圖15(a)(第20左右時).如圖13(d)-圖13(f)所示，在116-120s時，主機器人需要通過障礙物區(qū)B，必須同時躲避周圍存在的許多大塊障礙物和隨機出現(xiàn)的動態(tài)障礙物，主機器人調(diào)整姿態(tài)，降低速度，加強避障作用逃脫了障礙物陷阱區(qū)B.如圖13(g)所示，在162s時，終點在主機器人三級緩沖區(qū)內(nèi)，且主機器人周圍分布許多隨機出現(xiàn)的動態(tài)障礙，在緩沖區(qū)和模糊控制器的共同作用下，主機器人大大降低預測軌跡與已經(jīng)行駛過軌跡的接近程度以修改主機器人的行駛方向使其指向終點，主機器人順利躲避了終點附近隨機出現(xiàn)的所有動態(tài)障礙成功到達終點.主機器人行駛的最終軌跡如圖13(h)所示.

圖14示出了近些年其他優(yōu)秀的DWA擴展算法在動態(tài)環(huán)境中規(guī)劃的主機器人最終行駛軌跡.圖14(a)對應的主機器人的速度v和角速度w變化如圖15(b).在圖14(a)中，P-DWA ANFIS算法由于的權(quán)重系數(shù)固定，其對應的主機器人躲避隨機出現(xiàn)的圓形移動機器人時需要轉(zhuǎn)彎和減速，主機器人動態(tài)避障時繞開障礙物的自由度大，但由于權(quán)重固定，雖成功躲避圓形機器人，當主機器人面對密集的障礙物時，從障礙區(qū)域的外部繞路到達終點，導致路徑長度增加.圖14(b)中，A-DWA算法雖采用動態(tài)權(quán)重，但在這種未知的動態(tài)復雜環(huán)境下，該算法只考慮障礙物與機器人間的最小距離調(diào)整速度子函數(shù)的權(quán)重，由于機器人對障礙物位置判斷不準確，導致該算法對應的主機器人雖然躲避了動態(tài)障礙，但無法穿過窄通道A，同時為穿過窄通道B，被強迫選擇更能靠近目標點的采樣速度，機器人由于朝向和終點方向夾角θr，g>90°而偏離終點，在道口B處出現(xiàn)打圈停滯現(xiàn)象，無法到達終點.

對比圖15(a)和圖15(b)可知 ，固定參數(shù)的P-DWA ANFIS算法對應的主機器人速度和角速度在主機器人行駛過程中出現(xiàn)大幅度波動，且由于權(quán)重系數(shù)固定，機器人行駛在這種未知動態(tài)且復雜的情況下，主機器人動態(tài)避障時繞開障礙物的自由度大，h*(v，w)和ve*(v，w)的權(quán)重小，導致機器人靠近終點時，主機器人在終點附近仍具有1m/s速度，為避免v較大的軌跡末端超過終點，主機器人不斷地調(diào)整自身角度導致了w的波動.而提出的三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法對應的主機器人行駛過程中只有在需要避開兩個圓形移動機器人時(第8代和第20代左右)和在通過障礙物B區(qū)的時(第37代-77代)減速行駛，在躲避其他動、靜態(tài)障礙物時保持3m/s的最高速度行駛，在靠近終點時，主機器人的v和w逐漸減至0m/s，且w無上下波動，到達終點后停車，完成了盡快行駛的任務.

圖16 從實驗視頻中截取的真實避障情況Fig.16 Actual obstacle avoidance situation intercepted from the experimental video

綜上仿真對比可證，所提出的三級緩沖區(qū)結(jié)合模糊邏輯的動態(tài)預測性DWA算法可以找到未知的動態(tài)復雜環(huán)境狀態(tài)下的最佳速度指令，增強了算法的靈活性.

6 真實實驗驗證

本文實驗對象是滑動差分機器人，最大車速為1m/s.車載工控機(迷你小電腦)的CPU類型為英特爾 酷睿i7-5557U，二維激光雷達傳感器型號為OBD30M-R2000-B23-V1V17-1L，擁有10m，360°的測量范圍，左右兩側(cè)各有用于驅(qū)動車輪的直流電機.

為驗證所提出的基于三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法在未知的動態(tài)復雜環(huán)境中軌跡規(guī)劃中的有效性，在12m×12m的空間中進行了實驗，真實實驗的視頻鏈接(1)真實實驗的視頻鏈接(密碼：52119999)：https：//v.youku.com/v_show/id_XNTAzNTk3NjE1Mg==.html為(密碼：52119999)：實驗環(huán)境中放置了多個靜態(tài)障礙物，兩個人依次從車的對面走過模擬仿真環(huán)境中出現(xiàn)的兩個圓形移動機器人，從實驗視頻中截取的時刻圖如圖16(a)-圖16(l)所示，移動機器人通過二維激光探測的環(huán)境信息，當檢測到對面第1個人走來，機器人調(diào)整行駛方向，避開第1個人(圖16(a)-圖16(d))；當檢測到第2個人從對面走來，此時由于空間狹窄，主機器人周圍有鐵柱和方桌，同時需要躲避第2個人，因此主機器人減速行駛(圖16(e)-圖16(h))；機器人通過狹窄通道，并躲避開鐵柱(圖16(i)-圖16(k))；直至行駛到終點，減速停車(圖16(l)).主機器人在整個過程中避開了所有靜態(tài)障礙物和隨機出現(xiàn)的人.與真實實驗所對應的主機器人避障軌跡曲線仿真圖如圖17所示.與真實實驗對應的主機器人速度與角速度變化曲線圖如圖18所示，由圖18可知，主機器人在行駛過程中除躲避第2個人時減速(第74代左右)，其他時刻均保持以最大速度1m/s行駛.

圖17 與實驗所對應的主機器人避障軌跡曲線仿真圖Fig.17 Simulation diagram of the obstacle avoidance trajectory curve of the robot corresponding to the experiment

圖18 與真實實驗對應的主機器人速度與角速度變化曲線圖Fig.18 Main robot speed and angular velocity change curve corresponding to the real experiment

通過反復實驗表明移動機器人可以利用三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法成功避開動、靜態(tài)障礙，成功到達終點，整個過程中機器人不會窄通道振蕩，打圈和繞路，并與行走的人和靜態(tài)障礙物保持著安全距離，同時盡快行駛.

7 結(jié) 論

本文通過修改和擴展評價函數(shù)對基本的DWA進行改進，增強了DWA的規(guī)劃能力.此外，利用激光雷達技術(shù)結(jié)合三級緩沖區(qū)分析方法創(chuàng)建不同屬性的緩沖區(qū)因子為機器人設(shè)置安全等級；通過設(shè)計四輸入六輸出模糊控制器的方式將目標位置是否在機器人三級緩沖區(qū)內(nèi)、機器人偏向目標位置的程度、行進距離和障礙物的分布情況信息充分地可視化，提出三級緩沖區(qū)結(jié)合模糊邏輯的DWA算法.使得機器人在實際行駛時進行實時未知的動態(tài)復雜避障，同時盡快行駛.最后利用仿真實驗對比和實物實驗證明三級緩沖區(qū)結(jié)合模糊邏輯的DWA在未知且復雜的動態(tài)環(huán)境中規(guī)劃的有效性.