黃勇強(qiáng)劉硯菊宋建輝劉曉陽

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

目前，移動(dòng)機(jī)器人在航空、物流、農(nóng)業(yè)以及醫(yī)療等諸多領(lǐng)域得到廣泛的應(yīng)用，其中差速移動(dòng)機(jī)器人對野外、室內(nèi)等不同環(huán)境的適應(yīng)能力較強(qiáng)，是一種集環(huán)境感知、動(dòng)態(tài)決策和執(zhí)行等多功能于一體的智能化機(jī)器人[1]，具有機(jī)械結(jié)構(gòu)簡單、驅(qū)動(dòng)和控制方便、機(jī)動(dòng)靈活、工作效率高等優(yōu)點(diǎn)[2]。

路徑規(guī)劃是移動(dòng)機(jī)器人研究的核心內(nèi)容之一。 為使機(jī)器人在不同環(huán)境中成功躲避障礙，需根據(jù)對環(huán)境的各種信息掌握程度，選擇相應(yīng)的算法規(guī)劃出合適的路徑[3－4]。 適用于差速移動(dòng)機(jī)器人的路徑規(guī)劃方法可分為全局路徑規(guī)劃和局部路徑規(guī)劃兩大類:全局路徑規(guī)劃屬于靜態(tài)規(guī)劃(又稱離線規(guī)劃)算法，包括A?算法[5]、Dijkstra 算法[6]、蟻群算法[7]、粒子群算法[8]、遺傳算法[9]等；局部路徑規(guī)劃屬于動(dòng)態(tài)規(guī)劃(又稱在線規(guī)劃)算法，包括動(dòng)態(tài)窗口法(DWA)[10]、D?算法[11]和人工勢場法[12]等。

文獻(xiàn)[13]在雙輪差速移動(dòng)機(jī)器人路徑規(guī)劃中先后利用Floyd 算法和圓弧平滑方法減少A?算法拐點(diǎn)，并使拐點(diǎn)處路徑更圓滑，減少了路徑長度以及轉(zhuǎn)折次數(shù)。 文獻(xiàn)[14]針對雙輪差速移動(dòng)機(jī)器人模型，提出一種基于Hermite 插值法的輪式機(jī)器人路徑規(guī)劃方法，提高了輪式機(jī)器人路徑規(guī)劃的平穩(wěn)性以及精確性。 文獻(xiàn)[15]提出平滑A?算法，并用于室內(nèi)移動(dòng)機(jī)器人的路徑規(guī)劃，提高了路徑的平滑度，但僅適用于全局路徑規(guī)劃。 文獻(xiàn)[16]改進(jìn)了DWA 算法，使移動(dòng)機(jī)器人能夠動(dòng)態(tài)地避開障礙物，但不能保證軌跡為全局最優(yōu)。 雖然路徑規(guī)劃算法種類繁多，但每種算法都存在一定的局限性[17]，由此融合算法開始出現(xiàn)并得到發(fā)展。

文獻(xiàn)[18]通過優(yōu)化傳統(tǒng)A?算法的搜索點(diǎn)選取策略及評價(jià)函數(shù)，去除路徑中的多余節(jié)點(diǎn)，且將改進(jìn)后的A?算法融合DWA 算法，在全局路徑規(guī)劃的基礎(chǔ)上對局部路徑規(guī)劃進(jìn)行修正。 文獻(xiàn)[19]針對傳統(tǒng)A?算法拐點(diǎn)多、無法避開障礙物等缺陷，通過擴(kuò)展領(lǐng)域和去除冗余節(jié)點(diǎn)優(yōu)化全局路徑，并利用改進(jìn)DWA 算法的速度評價(jià)功能，在提高機(jī)器人規(guī)劃路徑平滑度的同時(shí)有效避開障礙物。文獻(xiàn)[20]將A?算法與跳點(diǎn)算法結(jié)合，拓展跳躍子節(jié)點(diǎn)，同時(shí)利用Floyd 算法平滑路徑，再融合DWA 算法進(jìn)行機(jī)器人全局動(dòng)態(tài)路徑規(guī)劃，在多種柵格地圖中驗(yàn)證了算法的路徑規(guī)劃能力。

根據(jù)差速移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)律及運(yùn)動(dòng)原理，本文提出一種基于改進(jìn)A?算法和優(yōu)化DWA 的融合算法。 將矢量角余弦值加入傳統(tǒng)A?算法的啟發(fā)函數(shù)中，減少對無用拓展節(jié)點(diǎn)的搜尋，提高搜索效率；加入路徑優(yōu)化算法，去除全局路徑的冗余節(jié)點(diǎn)，提高機(jī)器人規(guī)劃路徑平滑度，縮短規(guī)劃時(shí)間；對DWA 算法的距離評價(jià)因子進(jìn)行改進(jìn)，區(qū)分已知的全局障礙物和新添未知障礙物；融合優(yōu)化后的兩種算法，將優(yōu)化后全局路徑的關(guān)鍵節(jié)點(diǎn)作為DWA 算法的局部規(guī)劃目標(biāo)點(diǎn)，引導(dǎo)局部規(guī)劃路徑更貼近全局最優(yōu)路徑，提高機(jī)器人動(dòng)態(tài)避障能力，同時(shí)滿足路徑全局最優(yōu)性。

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

1.1 改進(jìn)啟發(fā)函數(shù)

傳統(tǒng)A?算法是移動(dòng)機(jī)器人常用的一種全局路徑規(guī)劃方法[21]，通過連續(xù)計(jì)算路徑的評價(jià)函數(shù)值，啟發(fā)式地搜索節(jié)點(diǎn)來構(gòu)建最優(yōu)路徑。 評價(jià)函數(shù)為

式中:n表示路徑中的當(dāng)前節(jié)點(diǎn)；F(n)是從起點(diǎn)到目標(biāo)節(jié)點(diǎn)的評價(jià)函數(shù)；G(n)是狀態(tài)空間中從起點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)成本；H(n)是當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)成本。 本文代價(jià)成本采用曼哈頓距離表示，其計(jì)算式為

式中d(p1，p2)表示點(diǎn)p1(x1，y1)到點(diǎn)p2(x2，y2)的曼哈頓距離。

在傳統(tǒng)A?算法中節(jié)點(diǎn)的選擇僅取決于最小F(n)值，缺乏其他約束條件，故許多拓展節(jié)點(diǎn)并非最優(yōu)路徑節(jié)點(diǎn)，導(dǎo)致空間和時(shí)間成本增加。 為減少對無用節(jié)點(diǎn)的排查，引入矢量角余弦值改進(jìn)啟發(fā)函數(shù)，角度范圍限制在0 ~90°。 余弦函數(shù)在0~90°之間單調(diào)遞減，保證了啟發(fā)函數(shù)的單調(diào)性約束，使拓展節(jié)點(diǎn)的選擇更具方向性。 具體步驟如下。

1)構(gòu)建一個(gè)從初始點(diǎn)到結(jié)束節(jié)點(diǎn)的向量a，和一個(gè)從當(dāng)前節(jié)點(diǎn)的拓展節(jié)點(diǎn)到結(jié)束節(jié)點(diǎn)的向量b，如圖1 所示。 計(jì)算兩個(gè)向量之間角度θ的余弦，如式(3)所示。

圖1 矢量圖Fig.1 Vector diagram

2)過濾掉余弦值小于0.5 的拓展節(jié)點(diǎn)，使向量a和向量b之間的角度小于60°，確保搜索路徑更接近目的地。

3)使用η作為權(quán)重，其值為地圖單位長度乘以分辨率。 分辨率指1 m 單位長度包含的柵格數(shù)量，本文η＝1。 構(gòu)造函數(shù)H1(n)作為啟發(fā)函數(shù)，表達(dá)式為

由式(1)和式(4)可得到新的評價(jià)函數(shù)為

引入矢量角余弦值改進(jìn)啟發(fā)函數(shù)可有效將A?算法的搜索節(jié)點(diǎn)約束在一定范圍內(nèi)，減少無用節(jié)點(diǎn)的數(shù)量，提高A?算法的搜索效率。

1.2 路徑節(jié)點(diǎn)優(yōu)化

傳統(tǒng)A?算法規(guī)劃的路徑存在冗余節(jié)點(diǎn)多的問題，造成移動(dòng)機(jī)器人轉(zhuǎn)彎次數(shù)過多。 本文從起始點(diǎn)開始刪除冗余節(jié)點(diǎn)，平滑處理路徑。 具體步驟如下。

1)刪除冗余節(jié)點(diǎn)。 針對全局路徑，從第二個(gè)節(jié)點(diǎn)開始，判斷當(dāng)前節(jié)點(diǎn)和前一個(gè)節(jié)點(diǎn)的運(yùn)動(dòng)方向是否一致，若一致，則刪掉前一個(gè)節(jié)點(diǎn)，否則保留前一節(jié)點(diǎn)。 遍歷所有路徑節(jié)點(diǎn)，將所有冗余共線節(jié)點(diǎn)刪除，過程示意如圖2 所示，刪除后的路徑為X1→X5→X6→X7→X8。

圖2 刪除冗余節(jié)點(diǎn)示意圖Fig.2 Schematic diagram of removing redundant nodes

2)刪除多余拐點(diǎn)。 在剩余的拐點(diǎn)中取新的節(jié)點(diǎn)Xi，連接X1和Xi。 設(shè)定安全距離D＝0.75 m，障礙物柵格與路徑的距離為d1。 如果兩點(diǎn)之間沒有障礙物或d1>D，繼續(xù)連接X1和Xi＋1，直到d1＝D，選擇當(dāng)前節(jié)點(diǎn)作為新拐點(diǎn)；若d1

3)提取剩余節(jié)點(diǎn)，輸出平滑處理后的路徑。

2 優(yōu)化DWA 算法評價(jià)函數(shù)

2.1 差速移動(dòng)機(jī)器人運(yùn)動(dòng)模型分析

DWA 算法在動(dòng)態(tài)窗口范圍內(nèi)對機(jī)器人的線速度v以及角速度ω進(jìn)行采樣，首先需要建立移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型[22]。 本文以雙輪差速移動(dòng)機(jī)器人為例，該模型具有控制簡單、位姿計(jì)算簡單的特點(diǎn)，圖3 為雙輪差速移動(dòng)機(jī)器人底盤運(yùn)動(dòng)學(xué)模型。 圖中l(wèi)為兩輪間距，r為轉(zhuǎn)彎半徑，v1和v2分別為左右兩輪的線速度，d為左右兩輪在單位時(shí)間Δt內(nèi)的位移差，旋轉(zhuǎn)角度θ1＝θ2＝θ3。

圖3 雙輪差速移動(dòng)機(jī)器人底盤運(yùn)動(dòng)學(xué)模型Fig.3 Kinematic model of the chassis of a two-wheeled differential mobile robot

機(jī)器人線速度v為兩輪線速度的均值，即

在單位時(shí)間Δt內(nèi)θ2變化較小，故

機(jī)器人角速度ω即為Δt內(nèi)θ3的變化，結(jié)合式(7)可得

機(jī)器人在做弧線運(yùn)動(dòng)時(shí)，轉(zhuǎn)彎半徑r為線速度v和角速度ω的比值，結(jié)合式(6)和式(8)可得

雙輪差速移動(dòng)機(jī)器人可控制左右兩輪的線速度完成轉(zhuǎn)彎及非勻速運(yùn)動(dòng)，假設(shè)機(jī)器人在t－1 時(shí)刻的位姿坐標(biāo)為(xt－1，yt－1，θt－1)，結(jié)合t時(shí)刻的v和ω計(jì)算式，可得到機(jī)器人在t時(shí)刻的位姿坐標(biāo)為

由此得到機(jī)器人的位姿變化過程。

2.2 移動(dòng)機(jī)器人速度采樣

DWA 算法的原理是對機(jī)器人的速度矢量空間進(jìn)行采樣，并根據(jù)機(jī)器人運(yùn)動(dòng)模型進(jìn)行前向模擬，以確定與采樣對應(yīng)的軌跡，然后依據(jù)評價(jià)函數(shù)為每一條軌跡打分，得分最高的軌跡就是最佳估計(jì)軌跡。 速度矢量作為控制信號發(fā)送給移動(dòng)機(jī)器人，速度采樣區(qū)間的確定是DWA 算法的核心，其受三個(gè)因素的限制，分別為本身最大(最小)速度Vm、電機(jī)加(減)速度Vd和安全制動(dòng)速度Va。 約束表達(dá)式如下。

式中:νmax、νmin分別為機(jī)器人最大、最小線速度；ωmax、ωmin分別為機(jī)器人最大、最小角速度。

式中:νc、ωc分別為機(jī)器人當(dāng)前時(shí)刻的線速度和角速度；νa1、ωa1分別為機(jī)器人最大線減速度、最大角減速度；νa2、ωa2分別為機(jī)器人最大線加速度、最大角加速度。

式中dist(ν，ω)表示最大安全制動(dòng)距離。

機(jī)器人的最終速度V是以上三個(gè)約束速度的交集，即

2.3 優(yōu)化評價(jià)函數(shù)

當(dāng)采樣樣本中有若干組可行軌跡時(shí)，采用評價(jià)函數(shù)對每條可行軌跡打分，以確定最佳路徑。評價(jià)函數(shù)G(v，ω)為

式中:heading(ν，ω)表示方位角評價(jià)函數(shù)，在當(dāng)前速度下，機(jī)器人當(dāng)前姿態(tài)方向與目標(biāo)位置的方位角偏差為δ，則heading(ν，ω) ＝180°－ ｜δ｜，機(jī)器人的方向越接近終點(diǎn)，δ越小，方位偏差越小，認(rèn)為其軌跡越好；dist(ν，ω)表示距離評價(jià)函數(shù)，用來評價(jià)模擬軌跡與障礙物的最近距離；vel(ν，ω)表示速度評價(jià)函數(shù)，用來評價(jià)機(jī)器人在模擬軌跡最后一段的速度，速度越大，得分越高；α、β、γ為三項(xiàng)加權(quán)系數(shù)；σ為歸一化平滑系數(shù)。

傳統(tǒng)DWA 算法采用前進(jìn)模擬軌跡中障礙物到移動(dòng)機(jī)器人之間的最短距離作為距離評價(jià)指標(biāo)，如權(quán)重過大會使軌跡與規(guī)劃路徑的偏差增大，權(quán)重過小則會導(dǎo)致兩者碰撞。 本文設(shè)定dist1(ν，ω)和dist2(ν，ω)作為新距離評價(jià)因子，以降低已知障礙物和未知障礙物之間的相互影響。dist1(ν，ω)用于評估前進(jìn)模擬軌跡終端到已知障礙物的最小距離，作用是控制已知全局障礙物對局部路徑規(guī)劃的干擾；dist2(ν，ω)則用于評價(jià)前進(jìn)模擬軌跡終端到未知障礙物的最小距離，作用是控制避障的靈敏度。 新的評價(jià)函數(shù)表達(dá)式為

式中λ為新增項(xiàng)的加權(quán)系數(shù)。

3 算法融合

改進(jìn)的A?算法在躲避全局未知障礙物方面仍顯性能不足，改進(jìn)后的DWA 算法在沒有全局路徑指引的情況下易陷入局部最優(yōu)，甚至在障礙物情況復(fù)雜的環(huán)境中會規(guī)劃失敗，兩者融合則可彌補(bǔ)各自的缺陷。 利用改進(jìn)A?算法的關(guān)鍵節(jié)點(diǎn)進(jìn)行規(guī)劃，將其應(yīng)用為DWA 算法局部規(guī)劃中的目標(biāo)點(diǎn)，以引導(dǎo)局部路徑規(guī)劃，兩者結(jié)合保證了動(dòng)態(tài)規(guī)劃的全局最優(yōu)性。 本文融合算法流程如圖4所示。

圖4 本文融合算法流程圖Fig.4 Flowchart of fusion algorithm in this paper

4 仿真實(shí)驗(yàn)和算法驗(yàn)證

采用Matlab 進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證改進(jìn)A?算法以及本文融合算法的有效性。

4.1 改進(jìn)A?算法的仿真結(jié)果

本文分別在Matlab 中設(shè)置環(huán)境1 和環(huán)境2 兩個(gè)不同的柵格地圖，柵格地圖的單位長度為1 m，黑色柵格為障礙物，不能通行，白色柵格為空曠區(qū)域，可以自由通行。S標(biāo)記為起始點(diǎn)位置，T標(biāo)記為目標(biāo)點(diǎn)位置。 圖5 為A?算法改進(jìn)前后在環(huán)境1中規(guī)劃路徑的仿真結(jié)果；圖6 為A?算法改進(jìn)前后在環(huán)境2 中規(guī)劃路徑的仿真結(jié)果。

圖6 環(huán)境2 中規(guī)劃路徑的仿真結(jié)果Fig.6 Simulation results of the planned path in environment 2

表1 分別從拐點(diǎn)個(gè)數(shù)、路徑長度以及運(yùn)行時(shí)間三個(gè)方面進(jìn)行A?算法改進(jìn)前后對比，表中時(shí)間為機(jī)器人離線規(guī)劃的時(shí)間。

表1 A?算法改進(jìn)前后路徑規(guī)劃對比Table 1 Comparisons of path planning before and after improvement of A?algorithm

通過圖5 和圖6 中A?算法改進(jìn)前后的規(guī)劃路徑對比，并結(jié)合表1 數(shù)據(jù)可知，改進(jìn)A?算法可以適應(yīng)簡單環(huán)境和復(fù)雜環(huán)境，且改進(jìn)A?算法搜索更具方向性，轉(zhuǎn)折減少，路徑平滑度得到很大改善，減少了靜態(tài)全局最優(yōu)路徑長度和離線規(guī)劃時(shí)間。

4.2 本文融合算法的仿真結(jié)果

在融合算法的仿真實(shí)驗(yàn)中，全局已知地圖添加了未知的靜態(tài)障礙物和動(dòng)態(tài)障礙物，DWA 算法評價(jià)函數(shù)中四個(gè)參數(shù)分別取值為α＝0.3、β＝0.5、λ＝0.5、γ＝0.2。 移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)設(shè)置如表2 所示。

表2 機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)設(shè)置Table 2 Robot kinematic parameters settings

圖7 為融合算法路徑規(guī)劃過程仿真圖。圖7(a)中移動(dòng)機(jī)器人正在避開新增的靜態(tài)障礙物；圖7(b)中移動(dòng)機(jī)器人正在避開未知?jiǎng)討B(tài)障礙物；圖7(c)中移動(dòng)機(jī)器人即將到達(dá)第一個(gè)局部目標(biāo)點(diǎn)；圖7(d)中移動(dòng)機(jī)器人到達(dá)目標(biāo)點(diǎn)。

圖7 融合算法路徑規(guī)劃過程仿真圖Fig.7 Simulation of the path planning process of the fusion algorithm

仿真圖中左上角“Δ”標(biāo)記表示機(jī)器人的起始點(diǎn)位置，右下角“○”標(biāo)記表示目標(biāo)點(diǎn)位置，融合算法路徑上不斷移動(dòng)的小圓圈表示雙輪差速機(jī)器人模型，其前面的一簇線代表前進(jìn)模擬軌跡，改進(jìn)A?算法路徑上的“?”代表改進(jìn)A?算法的關(guān)鍵點(diǎn)，也是局部規(guī)劃目標(biāo)點(diǎn)，新增的靜態(tài)障礙物在圖中用較大灰色方塊表示，環(huán)境中隨機(jī)出現(xiàn)的動(dòng)態(tài)障礙物在圖中用較小灰色方塊表示。

由圖7 可以看出，改進(jìn)A?算法先規(guī)劃出一條靜態(tài)全局最優(yōu)路徑，融合算法的路徑盡可能地靠近靜態(tài)全局最優(yōu)路徑，在增加全局未知障礙物的情況下，融合算法可以同時(shí)成功避開新增的未知?jiǎng)討B(tài)和靜態(tài)障礙物。

4.3 不同融合算法對比

在Matlab 中分別構(gòu)建與文獻(xiàn)[18]中地圖1、文獻(xiàn)[19]中地圖2 和文獻(xiàn)[20]中地圖3 環(huán)境相同的地圖，分別設(shè)置起點(diǎn)和終點(diǎn)一致、路徑障礙物情況一致、線速度和角速度一致，對比不同環(huán)境下融合算法規(guī)劃的路徑長度、運(yùn)行時(shí)間、路徑平滑度以及路徑全局一致性。

圖8 ~10是本文融合算法分別在地圖1 ~3中的規(guī)劃路徑。 機(jī)器人最大線速度和最大角速度在地圖1 中分別為1 m/s 和0.35 rad/s，在地圖2中分別為2 m/s 和0.7 rad/s，在地圖3 中分別為2 m/s和0.35 rad/s。

圖8 地圖1 中規(guī)劃路徑Fig.8 A planning path in Map 1

表3 為本文融合算法與文獻(xiàn)中融合算法仿真結(jié)果對比，表中時(shí)間為機(jī)器人抵達(dá)目標(biāo)點(diǎn)時(shí)間。

表3 融合算法對比Table 3 Comparisons of fusion algorithms

由圖8 和表3 可知，本文融合算法規(guī)劃的路徑始終貼近靜態(tài)全局最優(yōu)路徑，路徑全局最優(yōu)性更好，且路徑長度及運(yùn)行時(shí)間更短。 由圖9 和表3可知，本文融合算法路徑全局最優(yōu)性較好，在檢測到?jīng)]有新增障礙物時(shí)，保證平滑性和安全距離的條件下始終盡可能靠近靜態(tài)全局最優(yōu)路徑，路徑長度及運(yùn)行時(shí)間更短。 由圖10 和表3 可知，在障礙物密集時(shí)，本文融合算法轉(zhuǎn)折更少，且保證路徑具有全局最優(yōu)性的前提下，與文獻(xiàn)[20]選擇從密集障礙物中間穿過的路徑不同，在經(jīng)過第一個(gè)和最后一個(gè)未知障礙物時(shí)，本文融合算法選擇了安全性更高的路徑。

圖9 地圖2 中規(guī)劃路徑Fig.9 A planning path in Map 2

圖10 地圖3 中規(guī)劃路徑Fig.10 A planning path in Map 3

5 結(jié)論

提出了一種改進(jìn)A?算法和優(yōu)化DWA 的融合算法。 將矢量角余弦值引入A?算法的啟發(fā)函數(shù)中，使拓展節(jié)點(diǎn)的搜索更具選擇性，并對路徑進(jìn)行節(jié)點(diǎn)優(yōu)化處理，減少了多余的拐點(diǎn)和共線節(jié)點(diǎn)。仿真實(shí)驗(yàn)結(jié)果表明，改進(jìn)A?算法規(guī)劃路徑的平滑度得到很大提升，且路徑長度平均減少了4.97%，計(jì)算時(shí)間平均減少了30.5%。 將改進(jìn)A?算法的路徑關(guān)鍵節(jié)點(diǎn)提取出來，并作為局部路徑規(guī)劃的目標(biāo)點(diǎn)，實(shí)現(xiàn)了基于全局最優(yōu)的算法融合。 仿真實(shí)驗(yàn)結(jié)果顯示，該融合算法的路徑最大程度地接近于靜態(tài)全局最優(yōu)路徑，且對新增的動(dòng)態(tài)和靜態(tài)障礙物實(shí)現(xiàn)較好的避障效果，驗(yàn)證了融合算法的有效性。 對比其他常用融合算法，本文融合算法在路徑平滑度、安全性及路徑全局最優(yōu)性方面表現(xiàn)更出色，規(guī)劃路徑長度平均減少了3.7%，規(guī)劃時(shí)間平均減少了5.1%。