劉紫燕，張 杰，袁 浩，梁 靜

（貴州大學(xué)大數(shù)據(jù)與信息工程學(xué)院，貴州 貴陽 550025）

1 引言

路徑規(guī)劃作為機器人領(lǐng)域的重要研究內(nèi)容之一［1］，其目標(biāo)是讓機器人按照一定準(zhǔn)則從出發(fā)點到目標(biāo)點搜索一條最優(yōu)的無碰撞路徑［2］。根據(jù)對環(huán)境中有用信息的掌握程度不同，機器人路徑規(guī)劃可分為全局路徑規(guī)劃及局部路徑規(guī)劃［3］。全局路徑規(guī)劃是機器人了解其所處環(huán)境的所有信息并能求解出靜態(tài)環(huán)境下的最短路徑；而局部路徑規(guī)劃則是不完全掌握環(huán)境信息，需在移動的過程中根據(jù)傳感器信息調(diào)整路徑［4］，主要用于解決動態(tài)場景下的避障問題。由于實際環(huán)境復(fù)雜多變且可能存在未知障礙物，全局路徑規(guī)劃無法滿足其性能要求。因而局部路徑規(guī)劃成為研究熱點，如何提高實時性和避障能力適應(yīng)環(huán)境是尚待解決的問題。

傳統(tǒng)的局部路徑規(guī)劃算法有人工勢場法［5］、神經(jīng)網(wǎng)絡(luò)算法等，這些算法的計算復(fù)雜度高，且在實際場景下存在缺陷［6］。文獻(xiàn)［7］提出一種基于Frenet坐標(biāo)系的局部路徑規(guī)劃方法，采用代價函數(shù)選取最優(yōu)軌跡，從而保證汽車行駛的安全性和平順性，但該方法在復(fù)雜環(huán)境下的性能有待提高。近年來，動態(tài)窗口法（Dynamic Window Approach，DWA）由于具有計算量小、反應(yīng)迅速、可操作性強等優(yōu)點被廣泛應(yīng)用于機器人局部路徑規(guī)劃中［8］。文獻(xiàn)［9］提出參數(shù)自適應(yīng)的DWA算法，通過自動調(diào)整速度權(quán)值以自適應(yīng)環(huán)境的變化，在稠密障礙物環(huán)境下表現(xiàn)出優(yōu)越的性能，但沒有考慮動態(tài)障礙物情況下的避障問題。此外，DWA常與其它優(yōu)化算法結(jié)合以獲得更好的性能［10］。文獻(xiàn)［11］將改進(jìn)A*算法與動態(tài)窗口法結(jié)合，在A*算法中引入關(guān)鍵點選取策略，基于動態(tài)窗口法構(gòu)造實現(xiàn)全局最優(yōu)路徑的評價函數(shù)，在避免動態(tài)窗口法陷入局部最優(yōu)的同時保證路徑規(guī)劃最優(yōu)，但是在高維場景下計算復(fù)雜度高。文獻(xiàn)［12］將目標(biāo)偏向RRT與動態(tài)窗口法結(jié)合，在提高路徑平滑度的同時縮短了行進(jìn)時間，但是規(guī)劃路徑距離過長。文獻(xiàn)［13］將角度變化因素添加到DWA軌跡評估函數(shù)中，使其具有更平滑的軌跡和更好的避障性能，但并未考慮到全局路徑最優(yōu)。

針對以上問題，提出一種融合改進(jìn)RRT與DWA的局部路徑規(guī)劃方法。首先在RRT算法基礎(chǔ)上，改進(jìn)采樣策略和優(yōu)化路徑以提高路徑規(guī)劃質(zhì)量；然后改進(jìn)DWA軌跡評價函數(shù)，將當(dāng)前軌跡與全局路徑的距離作為評價指標(biāo)，充分利用全局路徑，使行走路徑最優(yōu)的同時提高局部避障能力。

2 相關(guān)知識及建模

2.1 RRT算法原理

RRT算法是由S.M.Lavalle提出的一種高效的多維空間規(guī)劃方法［14］。其算法流程，如圖1所示。

圖1 RRT算法示意圖Fig.1 RRT Algorithm Diagram

具體步驟如下：（1）初始化隨機樹T，設(shè)定起點Qstart，終點Qgoal，搜索步長s；（2）在地圖中隨機產(chǎn)生節(jié)點Qrand；（3）在隨機樹中找到離Qrand最近的節(jié)點Qnear；（4）從Qnear向Qrand方向擴(kuò)展步長s得到新節(jié)點Qnew；（5）若Qnew在障礙物中或Qnew與Qnear連線與障礙物相交則舍棄Qnew；否則添加Qnew到隨機樹T中；（6）若‖Qnew-Qgoal‖＜s，則抵達(dá)終點，暫停搜索；否則返回步驟（2）。（7）從探索樹的目標(biāo)節(jié)點Qgoal回溯至Qstart，得到規(guī)劃路徑。

2.2 動態(tài)窗口法

動態(tài)窗口法是由文獻(xiàn)［15］提出的一種局部避障算法，在速度空間內(nèi)采樣多組速度，在模擬周期內(nèi)得到并評價所獲得的軌跡從而選取最優(yōu)軌跡［16］。最優(yōu)軌跡對應(yīng)的速度指令將驅(qū)動機器人移動，當(dāng)機器人到達(dá)終點時，所有運動軌跡則構(gòu)成了機器人實際行走的路徑。

2.2.1 機器人運動模型

機器人運動模型，如圖2所示。機器人在二維空間中移動，在機器人坐標(biāo)系中，X軸方向速度為vx，Y軸方向速度為vy，角速度為ω。設(shè)t-1 時刻機器人的位姿為Xt-1=(xt-1，yt-1，θt-1)T，Δt時間內(nèi)速度保持不變，將機器人坐標(biāo)系下的位移量投影到全局坐標(biāo)系，t時刻機器人位姿Xt［17］為式（1）所示：

圖2 機器人運動模型Fig.2 Robot Motion Model

2.2.2 速度采樣

在速度(v，ω)的二維空間內(nèi)存在無窮多組速度，可以通過機器人自身性能和環(huán)境條件將速度限制在一定范圍內(nèi)。通常由下述三個條件約束機器人的速度范圍［18］：

（1）速度范圍

機器人最大速度為vmax，最小速度vmin，最大角加速度為ωmax，最小角速度為ωmin，則機器人的速度(v，ω)如式（2）：

（2）電機性能

（3）制動距離

為了保證機器人在移動過程中的安全，防止機器人與障礙物的相撞，機器人的速度需保持在一定范圍內(nèi)，速度(v，ω)約束條件如式（4）所示：

式中：dist(v，ω)—速度(v，ω)對應(yīng)軌跡上與障礙物的最近距離，此條件的目的是使機器人在遇到碰撞障礙物時停止運動。

2.2.3 評價函數(shù)

如前文所述，DWA算法在速度空間內(nèi)采樣得到多組運動軌跡，然后通過軌跡評分從多組軌跡選取最優(yōu)軌跡，機器人運行最優(yōu)軌跡對應(yīng)的控制參數(shù)即可沿最優(yōu)軌跡運動直至到達(dá)目標(biāo)點。在速度(v，ω)下的軌跡總體評價函數(shù)，如式（5）所示：

式中：heading(v，ω)—方位角評估函數(shù)［11］，表示當(dāng)前速度產(chǎn)生的模擬軌跡末端方向與目標(biāo)點之間的角度［18］；dist(v，ω)—距離評價函數(shù)，表示當(dāng)前速度對應(yīng)軌跡上與障礙物的最近距離，采用曼哈頓（Manhattan）距離；velocity(v，ω)—速度評價函數(shù)，表示當(dāng)前速度大?。沪?、α、β、γ—各評價子函數(shù)的系數(shù)。設(shè)置該評價函數(shù)的目的是使機器人在避開障礙物的同時朝著目標(biāo)點以較快速度行駛，從而保證局部規(guī)劃效率。

3 改進(jìn)路徑規(guī)劃算法

3.1 改進(jìn)RRT算法

3.1.1 改進(jìn)采樣策略

傳統(tǒng)RRT算法在機器人狀態(tài)空間中隨機產(chǎn)生新節(jié)點，采樣效率低且路徑質(zhì)量較差［19］。這里提出改進(jìn)采樣策略，其核心思想是目標(biāo)點Qgoal和隨機產(chǎn)生的節(jié)點Qrand對隨機樹產(chǎn)生引力，兩個節(jié)點一同引導(dǎo)隨機樹生長［20］。新節(jié)點Qnew不再只由隨機節(jié)點Qrand決定，而且還受Qgoal的影響，如圖3所示。通過此方式以改善原算法的隨機特性從而提高搜索效率。新節(jié)點Qnew的產(chǎn)生，如式（6）所示。

圖3 采樣策略示意圖Fig.3 Sampling Strategy Diagram

式中：new.X—在二維地圖中新節(jié)點的橫坐標(biāo)；new.Y—新節(jié)點的縱坐標(biāo)；near.X—最近節(jié)點的橫坐標(biāo)；near.Y—最近節(jié)點的縱坐標(biāo)；s—擴(kuò)展步長；θ1—Qnear與Qrand連線與X軸的夾角；θ2—Qnear與Qgoal連線與X軸的夾角；α、β—比例系數(shù)。

3.1.2 路徑優(yōu)化

改進(jìn)RRT算法生成的路徑包含多個轉(zhuǎn)折點，并非最優(yōu)。為提高路徑質(zhì)量，可優(yōu)化處理路徑。假設(shè)算法輸出的路徑由一系列路徑點P={si}(1 ≤i≤N)組成，其中，si表示第i個路徑點坐標(biāo)(xi，yi)，為了縮短路徑，取si+2和si的中間點代替原來的路徑點si+1，如式（7）所示：

其中，得到的路徑仍不夠平滑，為了提高路徑平滑度以便機器人轉(zhuǎn)彎和移動，采用三次貝塞爾曲線對路徑進(jìn)行平滑處理。從路徑P中確定（n+1）個點的位置Fi=(i=0，1，…，n)，可構(gòu)造（n+1）階貝塞爾曲線，如式（8）所示：

式中：Fi—Bezier曲線的控制點；n—Bezier曲線的階數(shù)；t—控制參數(shù)，其取值范圍為［0，1］；基函數(shù)Bi，n定義如式（9）所示：

若式中n=3，則得到三次貝塞爾基函數(shù)，如式（10）所示：

所以，三次貝塞爾曲線可表示為式（11）：

3.2 融合改進(jìn)RRT算法和動態(tài)窗口法

傳統(tǒng)DWA算法只在當(dāng)前環(huán)境下選擇最優(yōu)速度，得到的路徑是局部最優(yōu)并非全局最優(yōu)，若遇到U形障礙物或角落，機器人有可能陷入困境［21］，從而降低導(dǎo)航效率甚至導(dǎo)航失敗。

結(jié)合全局路徑規(guī)劃和局部路徑規(guī)劃的優(yōu)點，這里提出改進(jìn)RRT和DWA融合的局部路徑規(guī)劃算法［22］。改進(jìn)DWA的軌跡評價函數(shù)，使機器人在避障的同時跟隨全局路徑，從而充分利用全局路徑最短。

在（10×10）m的二維地圖上測試DWA算法的避障性能，如圖4所示。設(shè)定起點為（0m，0m），一種色標(biāo)記點為終點位置（10m，10m），黑色區(qū)域代表障礙物，在起點到終點的必經(jīng)路徑中存在U形障礙物。機器人從起點向終點運動，到達(dá)U形障礙物區(qū)域內(nèi)時速度逐漸變慢直至止步于此，究其原因是當(dāng)機器人靠近U形障礙物時無法再搜索到有效路徑而無法離開U形障礙物，則路徑規(guī)劃失敗。上述實驗表明DWA算法易陷入U形障礙物困境，不適用于室內(nèi)復(fù)雜環(huán)境下的路徑規(guī)劃。

圖4 DWA陷入U形障礙物Fig.4 DWA Trapped in U-Shaped Obstacle

為了解決上述問題，使用DWA算法在驅(qū)動機器人運動時充分利用全局路徑信息，即在全局路徑的指導(dǎo)下生成局部路徑。在圖4所示的地圖進(jìn)行實驗，機器人在運動過程中跟隨全局路徑成功到達(dá)終點，其路徑，如圖5所示。圖中一種色曲線為改進(jìn)RRT的算法生成的全局路徑，另一種色曲線為DWA算法在全局路徑引導(dǎo)下生成的運動軌跡。對比圖4、圖5 可知，在復(fù)雜場景下，DWA算法與全局路徑規(guī)劃算法結(jié)合能根據(jù)全局路徑信息可有效避開U形障礙物到達(dá)目標(biāo)點，并且規(guī)劃的路徑為最優(yōu)［23］。

圖5 全局路徑下DWA逃離U形障礙物Fig.5 DWA Escaping from U-Shaped Obstacles Under Global Path Planning

機器人在運動過程中朝向目標(biāo)點運動，但是由于環(huán)境中存在障礙物，使得機器人易陷入障礙物區(qū)域中，式（5）中的方位角函數(shù)heading的作用是避免DWA算法陷入局部最優(yōu)，用路徑跟隨評價函數(shù)Pdist代替方位角函數(shù)heading，提出改進(jìn)的目標(biāo)函數(shù)如式（12）所示：

式中：Obs—生成軌跡末端與障礙物的距離；

Gdist—生成軌跡末端與目標(biāo)點的距離；

Pdist—生成軌跡末端與全局路徑點的最短距離；

μ、ξ、ζ—系數(shù)。

所選擇的軌跡使式（12）取得最小值，機器人選擇一條與最佳路徑保持接近的安全路線，并朝目標(biāo)點以較快速度行進(jìn)。

這里提出的改進(jìn)融合算法流程圖，如圖6所示。包括全局路徑規(guī)劃和局部路徑規(guī)劃。使用改進(jìn)RRT算法規(guī)劃出從起點到終點的全局最優(yōu)路徑，DWA算法依此路徑和環(huán)境狀況輸出機器人的最佳速度和運動方向，生成機器人運動的真實軌跡。

圖6 融合算法流程圖Fig.6 Flow Chart of Fusion Algorithm

4 實驗結(jié)果與分析

4.1 仿真實驗

仿真實驗的硬件配置：筆記本電腦安裝Ubuntu 14.04操作系統(tǒng)，CPU為i3-3110M，主頻2.4GHz，運行內(nèi)存為4GB。

本實驗在ROS平臺的Stage仿真平臺，選取Maze迷宮地圖。機器人工作環(huán)境為（10×10）m 的迷宮，如圖7所示。三種算法的起始點坐標(biāo)統(tǒng)一設(shè)置為（2m，2m），終點坐標(biāo)為（9m，3m）。A*-DWA、Dijkstra-DWA 和改進(jìn)RRT-DWA 算法的規(guī)劃結(jié)果，如圖7～圖9所示。其中，一條路徑代表全局規(guī)劃路徑，另一條路徑為機器人的實際運動軌跡［24］。

圖8 Dijkstra-DWA規(guī)劃結(jié)果Fig.8 Path Planning Results with Dijkstra DWA

圖9 改進(jìn)RRT-DWA規(guī)劃結(jié)果Fig.9 Path Planning Results with Improved RRT-DWA

對比圖7～圖9可知，A*算法與Dijkstra算法生成的路徑不夠平滑，部分路徑呈鋸齒狀；而改進(jìn)RRT 算法生成路徑平滑度提高，證明了改進(jìn)算法的有效性。這里算法與Dijkstra-DWA 和A*-DWA 算法相比的結(jié)果，如表1所示。由表1數(shù)據(jù)可知，這里提出的改進(jìn)RRT-DWA 算法生成的實際運動軌跡長度更短，較A*-DWA算法縮短了約10.14%；同時，這里提出方法的行進(jìn)時間更短，提高了規(guī)劃效率。

表1 Maze地圖仿真實驗數(shù)據(jù)對比Tab.1 Numerical Results on Maze Map

4.2 實際環(huán)境實驗

在完成仿真實驗后，在實際場景下驗證了改進(jìn)算法的性能。以筆記本電腦作為上位機，將融合算法應(yīng)用到基于ROS平臺的移動機器人Turtlebot2上，使用深度相機Kinect獲取外界環(huán)境信息，amcl 和gmapping 模塊實現(xiàn)定位和構(gòu)建二維地圖，如圖10 所示。這里的實驗場景為實驗室，首先用無線控制器控制Turtlebot2掃描室內(nèi)環(huán)境，利用gmapping算法構(gòu)建二維地圖，地圖尺寸為576*512像素；然后在建好的地圖上實現(xiàn)路徑規(guī)劃，并將實驗結(jié)果與A*-DWA和Dijkstra-DWA算法進(jìn)行對比。

圖10 Turtlebot2機器人平臺Fig.10 Turnlebot2 Robot Platform

在ROS的Rviz可視化工具中顯示二維地圖和三種算法的規(guī)劃路徑［21］，A*-DWA算法、Dijkstra-DWA和改進(jìn)RRT-DWA算法的仿真結(jié)果，如圖11～圖13所示?？梢钥闯鲞@里所提算法所規(guī)劃的路徑更短。三種算法在相同起始點下生成軌跡的路徑長度、行進(jìn)時間和平均速度，如表2所示。由表2的實驗數(shù)據(jù)可知，與A*-DWA 和Dijkstra-DWA 相比，改進(jìn)RRT-DWA 的路徑長度縮短，行進(jìn)時間明顯減少，平均行駛速度較高，體現(xiàn)了提出算法在真實環(huán)境下的優(yōu)良性能。因此改進(jìn)RRT-DWA算法優(yōu)于A*-DWA和Dijkstra-DWA。

表2 實驗室環(huán)境下三種算法實驗數(shù)據(jù)對比Tab.2 Numerical Results in Laboratory Environment

圖11 A*-DWA規(guī)劃結(jié)果Fig.11 Path Planning Results with A*-DWA

圖12 Dijkstra-DWA規(guī)劃結(jié)果Fig.12 Path Planning Results with Dijkstra DWA

圖13 改進(jìn)RRT-DWA規(guī)劃結(jié)果Fig.13 Path Planning Results with Improved RRT-DWA

5 結(jié)論

這里將改進(jìn)RRT算法與DWA算法融合，在兼顧全局路徑最優(yōu)的同時選擇最優(yōu)速度驅(qū)動機器人運動，不僅改善了路徑質(zhì)量，而且顯著提升規(guī)劃效率。與傳統(tǒng)全局路徑規(guī)劃算法相比，這里算法由于能輸出控制參數(shù)直接驅(qū)動機器人運動而具有較好的實時性，且規(guī)劃的路徑更優(yōu)；與局部路徑規(guī)劃算法相比，這里算法不易陷入U形障礙物等困境，在復(fù)雜環(huán)境下適應(yīng)性較高，且能保證規(guī)劃路徑的全局最優(yōu)。這里算法充分利用全局和局部規(guī)劃方法的優(yōu)點，有效地解決了傳統(tǒng)導(dǎo)航算法在已經(jīng)規(guī)劃好全局路線的情況下無法局部避障的問題，提高了機器人避障性能。