王冠強 ，張馳洲, ，陳明松, ，藺永誠 ，鄒奮揚 ，王秋, ，吳敏杰 ，曾維棟

(1. 中南大學 機電工程學院，湖南 長沙，410083；2. 極端服役性能精準制造全國重點實驗室，湖南 長沙，410083；3. 中南大學 輕合金研究院，湖南 長沙，410083)

路徑規(guī)劃技術是移動機器人領域的一項熱門研究[1-2]。路徑規(guī)劃的最終目的是尋找到一條可以從起始點到目標點的無碰撞路徑[3-4]。根據對環(huán)境信息的掌握程度不同，路徑規(guī)劃方法可分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃方法是在已知的地圖環(huán)境下，規(guī)劃出一條無碰撞的最優(yōu)路徑；而局部路徑軌跡規(guī)劃是在地圖環(huán)境信息完全未知或部分可知下，側重于考慮機器人當前的局部環(huán)境信息，讓機器人規(guī)劃出一條能夠動態(tài)避障的局部路徑[5-6]。在路徑規(guī)劃中面臨的問題有搜索節(jié)點數冗余、搜索方向不明確和搜索時間長等[7-8]，因此亟需設計合理的規(guī)劃器解決上述問題。

針對全局路徑規(guī)劃問題，傳統(tǒng)的路徑方法有RRT 算法、BFS 算法和A*算法[9]等。當地圖環(huán)境較大或者復雜時，使用BFS 和A*算法的規(guī)劃效率往往不如RRT 算法的規(guī)劃效率[10]。RRT(rapidexploring random tree，快速搜索隨機樹)算法首先由LAVALL[11]提出，該算法是以路徑起點為根節(jié)點，從空閑區(qū)域選取隨機節(jié)點引導樹的生長方向，當樹中的葉子節(jié)點擴散到目標點，完成整個搜索過程[12]。但是，該算法也存在諸多不足，如RRT算法的隨機采樣思想導致節(jié)點擴展無目標導向性，容易出現大量冗余節(jié)點，算法的收斂速度過慢[13]。同時路徑存在著轉折次數過多，路線不平滑、與障礙物安全距離過小和最大轉角過大等不符合車輛運動學要求的問題[14]。因此，針對上述問題，許多學者展開了系列研究。KUFFNER等[15]提出了RRT-Connect算法以提高收斂速度，但在復雜環(huán)境中該方法采樣范圍不確定和隨機性過大[16]。KANG等[17]提出了一種基于三角不等式RRT-Connect機器人路徑規(guī)劃算法，減少了規(guī)劃時間和路徑長度。韋玉海等[18]提出了一種AMRRT-Connect 算法以解決移動機器人在狹隘與復雜環(huán)境中非完整性約束問題。

針對局部路徑規(guī)劃問題，常用的方法有DWA、人工勢場法和時間彈性帶算法等。其中，人工勢場法在機器人與障礙物相近時無法生成路徑，時間彈性帶算法面對較為動態(tài)的場景時獲得的最優(yōu)路徑會頻繁變化。相比之下，DWA 算法更符合機器人的控制行為。DWA是FOX等[19]提出的一種局部路徑規(guī)劃方法，主要是在速度空間中進行速度采樣，并預測出這些速度在一定時間內的運動軌跡，隨后通過評價函數對預測軌跡進行評價，選取最優(yōu)軌跡對應的速度空間用以驅動機器人運動。郭烈等[20]提出了一種融合PID 控制的TEB 算法，減小了控制指令的波動范圍。ZHANG等[21]采用以目標距離代替DWA算法中方向角差的評價項，避免路徑因振動而不平滑。目前，DWA 算法所預測的軌跡在離障礙物較近時才開始避障，因此，往往需要結合其他避障算法進行優(yōu)化。

綜上，伴隨著移動機器人的發(fā)展，全局路徑規(guī)劃算法與局部路徑規(guī)劃算法得到了廣泛研究，但仍然存在不少問題而導致規(guī)劃的路徑質量不佳或效率不高。針對以上問題，本文作者提出一種人工勢場法與RRT-Connect算法相結合的全局路徑規(guī)劃算法，并針對移動機器人實際運行需求，對改進算法規(guī)劃出的路徑進行后處理，大幅度提高了輸出路徑的質量。同時為了提高機器人的動態(tài)避障能力，提出改進的DWA 局部路徑規(guī)劃算法，最終實現移動機器人在室內環(huán)境下的自主路徑規(guī)劃任務。

1 路徑規(guī)劃算法

全局路徑規(guī)劃要解決的首要問題是如何為機器人找到起點到目標點的可通行路徑，雖然在這個過程中還會構建一系列優(yōu)化目標，包括路徑最短、提前躲避障礙物等等，但是最關鍵的是獲得路徑的基本信息。機器人只有在全局路徑規(guī)劃算法下確定了路徑信息后，局部路徑規(guī)劃算法才會在局部范圍內不斷更新其自身的運動信息，從而完成路徑規(guī)劃的全部任務。

1.1 改進RRT-Connect算法的全局路徑規(guī)劃方法

在全局路徑規(guī)劃方法的開發(fā)中，RRT-Connect算法由于在RRT 的基礎上引入了雙樹擴展環(huán)節(jié)而得到了廣泛的推廣使用。該方法分別以起點和目標點為根節(jié)點同時擴展隨機樹，從而實現對狀態(tài)空間的快速搜索。然而，其靈敏度和實時性仍存在著較大的不足。此外，RRT-Connect算法由于采樣點選取范圍過廣，在采樣過程中會存在隨機性過大的問題。因此，本節(jié)以RRT-Connect算法為基礎進行系列開發(fā)與研究，擬改善上述不足，推動全局路徑規(guī)劃算法的優(yōu)化，如算法1 所示。首先，提出增設第三、第四棵擴展樹生成兩個RRTConnect 擴展區(qū)域進行擴展的方式，實現算法搜索效率和速度的提升。其次，提出以起點和終點所在矩形區(qū)域為采樣點限定選取范圍的方式，減少不必要擴展，加快算法收斂速度。最后，為減少RRT-Connect算法中節(jié)點選取的隨機性，引入人工勢場法用于引導擴展隨機樹朝著目標點進行啟發(fā)式擴展。在算法1 中給出了改進后的RRT-Connect整體流程。

1.1.1 增設第三和第四擴展樹

為了能夠增加第三和第四棵擴展樹，同時生成兩個RRT-Connect擴展區(qū)域，提升算法的搜索效率，首先應選取合適的第三和第四棵擴展樹的根節(jié)點坐標，本文取起點pstart(縮略為ps，下同)與終點pgoal(pg)連線的中點為第三根節(jié)點pcentral(pc)，同時以點pc為圓心，步長為半徑作圓弧與直線pspg相交，取靠近終點pg的交點為pcentral_r(pcr)，也即第四根節(jié)點。設起點坐標為ps(xs，ys)，終點坐標為pg(xg，yg)，則第三根節(jié)點與第四根節(jié)點的坐標計算公式為：

算法1：改進后的RRT-Connect輸入：起始點pstart，目標點pgoal，障礙物點pobstacle輸出：路徑Path(pstart，pgoal)1：按照式(1)和(2)計算pcenter，pcenter_r；T1.init(pstart)，T2.init(pcenter)，T3.init(pcenter_r)，T4.init(pgoal)//初始化2：Cr←Sampling_Area_Limitation(pstart，pgoal)3：for i=1 to n do 4：pnew1 ∈ Cr←Sampling_GRAR(pstart，pcenter，pobstacle，prand1)pnew2 ∈ Cr←Sampling_GRAR(pcenter_p，pgoal，pobstacle，prand2)if Extend (T1，prand1)≠Trapped then 5：6：7：8：9：10：11：12：13：14：15：16：17：18：19：end 20：return Fail 21：//函數Extend( )和Connect( )的實現基于文獻[12]22：//函數Sampling_Area_Limitation( )表示限定采樣點生成范圍23：//函數Sampling_GRAR( )是指在引力場和排斥場的作用下產生隨機點if Connect (T2，pnew1)=Reached then Path1=Path(T1，T2)end Swap(T1，T2)end if Extend (T3，prand2)≠Trapped then if Connect (T4，pnew2)=Reached then Path2=Path(T3，T4)end Swap(T3，T4)end return Path=Path.push(Path1，pcenter，pcenter_p，Path2)

同時，考慮到中點pc可能位于障礙物中的情況，采取中點偏置的方法，即當中點位于不可達區(qū)域時，以pc為圓心，在步長r為半徑的圓內搜索可達點pfree(pf)，用于替代中點pc，從而實現增加第三和第四根節(jié)點的目的。

1.1.2 限定采樣點選取范圍

在RRT-Connect算法中，存在許多隨機生成的擴展節(jié)點，而其中的大部分節(jié)點屬于無效節(jié)點，不參與組成最終路徑，但是生成這些節(jié)點會增加算法的總體時間成本。因此，為提升算法收斂速度和效率，減少迭代運算次數和無效節(jié)點，以更少的時間和更小的內存需求找到最佳路徑，本文以起點ps和終點pg所在區(qū)域為限定采樣區(qū)域Cr，采樣區(qū)域的確定原則為以起點ps與終點pg間連線斜率k為基礎，取斜率為且經過起點ps與終點pg的兩條直線與地圖邊界的交點分為Q1、Q2、Q3、Q4，連接這四個點所組成的矩形封閉區(qū)域為限定采樣區(qū)域，如圖1所示。當在自由空間C中隨機選取采樣點prandom(pran)時，若所選采樣點pran∈Cr，則保留為采樣點。反之，則刪除此點，取終點pg為本次擴展采樣點進行迭代運算。若在限定采樣區(qū)域Cr中無法搜索到可行路徑，則采取增大Q1與Q2、Q3與Q4坐標直線距離的方式擴大限定采樣區(qū)域，直到找到可行路徑。通過設立上述矩形限定采樣區(qū)域，使得無效采樣點數量大幅度減少，提高了擴展效率，縮短了總體時間成本。

圖1 矩形采樣區(qū)域示意圖Fig. 1 Schematic diagram of rectangular sampling area

1.1.3 引入人工勢場法

RRT-Connect算法在節(jié)點搜索時的隨機性導致節(jié)點對大量無效空間擴展，為此，本節(jié)將人工勢場法[22]引入至上述改進的RRT-Connect算法中，用于引導擴展隨機樹朝著目標點進行啟發(fā)式擴展。同時，選取合理斥力距離和系數，避免了與障礙物發(fā)生碰撞，提高了算法的安全性和實時性。具體地，在RRT-Connect算法的擴展節(jié)點上分別疊加一個引力場和斥力場去引導隨機節(jié)點的產生方向。疊加引力場和斥力場后的算法核心思想就是在任一個雙向擴展隨機樹中的單個節(jié)點n都增加一個目標引力函數A(n)和目標斥力函數R(n)，此時的n節(jié)點表示由根節(jié)點向外擴展的第n個新節(jié)點，擴展方式為

式中：F(n)為新節(jié)點到目標點的擴展函數；G(n)為新節(jié)點的隨機擴展函數；A(n)為目標點到隨機樹中最近節(jié)點pnear(pne)的引力函數；R(n)為障礙物點到隨機樹節(jié)點的斥力函數。

新節(jié)點的隨機擴展函數G(n)的表示方法為

式中：λ為擴展的固定步長。

目標引力函數A(n)和斥力函數R(n)的表示方法為

式中：ε和γ分別為引力系數和斥力系數；‖xg-xne‖為目標點到隨機數中最近點的歐式距離；‖xo-xne‖為障礙物點到隨機數中最近點的歐式距離；E(xg，xne)為目標點到隨機數中最近點的方向向量；E(xo，xne)為障礙物點到隨機數中最近點的方向向量。

由以上公式得到疊加了引力場和斥力場的新節(jié)點生成方法如下：

疊加了引力場后的新算法每一個節(jié)點的擴展都是按照式(7)的方法進行，使兩棵雙向隨機樹在各自引力和斥力分量的引導下向目標點方向擴展，擴展示意圖如圖2所示。

圖2 在引力場和斥力場下節(jié)點擴展示意圖Fig. 2 Schematic diagram of node expansion under gravitational and repulsive fields

1.1.4 路徑平滑處理

對于RRT 系列算法，最終路徑是通過連接各擴展點來確定的。然而，由于各點之間連線是直線段，所以最終路徑是由許多段直線組成，呈不規(guī)則折線狀。這樣的路徑不符合移動機器人的運行要求。因此，本節(jié)提出引入Dijkstra 算法[23]進行路徑節(jié)點篩選，通過篩選路徑中的關鍵節(jié)點，去除不必要節(jié)點，使得路徑節(jié)點數大幅度減少，增加了路徑平滑度，如圖3所示。

圖3 節(jié)點篩選示意圖Fig. 3 Schematic diagram of node screening

同時考慮到機器人的轉向能力，期望最短的歐幾里得距離分布和較低的軌跡平滑度。基于以上分析，本文選擇一個更合理的成本函數Cost，如式(7)所示。圖4所示為成本函數的節(jié)點示意圖。

圖4 改進成本函數的節(jié)點示意圖Fig. 4 Node diagram of improved cost function

式中：pi和pj表示一對相鄰節(jié)點；‖pij‖表示相鄰節(jié)點的歐式距離；θij表示兩個相鄰節(jié)點的相對航向角；Normal表示歸一化；ω1和ω2分別表示距離和航向角的權重系數。

通過上述改進的RRT-connect 算法可獲得移動機器人在固定空間下的平滑全局移動路徑。然而，該路徑并不能作為最終的機器人移動軌跡。這是由于平滑后的路徑可能存在過平滑的現象以及平滑后路徑上存在障礙物。此外，該路徑上突然引入新障礙物，即面對未知空間，機器人依舊無法準確通行。因此，在全局路徑規(guī)劃的基礎上，還需結合局部路徑規(guī)劃算法進行路徑的實時調優(yōu)。

1.2 改進DWA算法的局部路徑規(guī)劃算法

為更好地擴展機器人在移動過程中的實時調優(yōu)能力，本節(jié)將介紹移動機器人的實時軌跡計算方式，并基于DWA算法進行評價函數優(yōu)化以加快函數收斂速度，減少路徑計算耗時，實現更快速的動態(tài)決策。

1.2.1 機器人航跡推算

本文中所研究的兩輪差速移動機器人運動學分析如圖5 所示(圖5 中：vc和wc分別為底盤中心點oc的線速度和角速度；x(t)、y(t)和θ(t)分別為當時時刻機器人的橫坐標、縱坐標和偏航角)。其中底盤有三個車輪，兩個后輪為驅動輪，分別由獨立的直流伺服電機來進行驅動，為移動機器人的運動提供動力；前輪為萬向輪，主要負責穩(wěn)定車體并保持平衡。

圖5 兩輪差速移動機器人運動學模型圖Fig. 5 Kinematic model diagram of two wheel differential mobile robot

假設機器人整個運動過程勻速行駛，經過很小的時間間隔Δt后(近似軌跡直線處理)，其航跡推算公式如下：

由式(9)可知：對于任意t時刻機器人的位姿，都可以通過速度集合(vc，wc)推算出采樣周期Δt內的位移位姿及其運動軌跡。而DWA算法作為一種簡單高效的算法，通過對機器人的速度矢量空間進行采樣，再評價產生軌跡的好壞以確定最佳軌跡，在差速移動機器人研究領域得到了廣泛使用。而為了進一步提高算法的高效性，接下來對DWA中的評價函數進行優(yōu)化。

1.2.2 評價函數優(yōu)化

在運動過程中，原軌跡評價函數中的角度差評價函數會選擇角度差值較小的速度集合來進行角度修正，這樣也會使得移動機器人需要進行反復的角度調整。因此，這里引入目標點距離評價函數替代航向角函數，以預測軌跡中離目標點最近的軌跡作為下一時刻的最優(yōu)軌跡，改進后的新評價函數如下式所示：

式中：GD(vc，wc)表示當移動機器人速度集合為(vc，wc)，地圖中的障礙物與移動機器人車體中心的最短距離；Do(vc，wc)主要用于篩選出能使移動機器人規(guī)避地圖中障礙物的軌跡，保證運動安全；V(vc，wc)是在滿足上述兩個篩選條件的前提下，盡量選出當前速度集合中較大的角速度與線速度，保證移動機器人能夠以較快的速度運行；α、β、γ分別為軌跡評價函數三個性能指標的權重系數；GD(vc，wc)為距離評價函數，主要用于獲取當前時刻下移動機器人的速度空間中各個速度集合所對應的軌跡末端終點與目標點之間的歐式距離，其計算公式如下：

式中：g(x)與g(y)分別為目標點的橫坐標與縱坐標；xt(x)與xt(y)分別為當前時刻下預測軌跡末端的橫坐標與縱坐標。GD(vc，wc)通過依次對比各個速度集合(vc，wc)所對應的預測軌跡末端與目標點之間的距離，取距離目標點最近的軌跡為最優(yōu)軌跡，不需要反復進行角度調整，加快了算法收斂速度。

2 單目標點導航算法

由于全局和局部路徑規(guī)劃都有各自的缺陷，RRT-Connect算法可獲取全局范圍內的最佳路徑卻無法躲避動態(tài)障礙物，DWA 算法能躲避動態(tài)障礙物卻容易陷入局部最優(yōu)。鑒于此，本文以改進RRT-Connect算法規(guī)劃的全局路徑為指引，融合改進評價函數的DWA算法實現移動機器人的動態(tài)避障，從而實現安全有效的單目標點導航任務。單目標點算法流程圖如圖6所示。

圖6 單目標導航算法流程圖Fig. 6 Flow chart of single target navigation algorithm

3 實驗與分析

3.1 仿真實驗

仿真實驗硬件平臺配置為AMD Ryzen 5 3600 6-Core Processor 3.59 GHz CPU，RAM 為16 GB，采用matlab2020a軟件平臺編程實現。

3.1.1 改進RRT-Connect算法仿真對比實驗與分析

為了證明本文所提出的全局路徑規(guī)劃算法可行性與穩(wěn)定性，本仿真實驗基于長度×寬度為640 m×600 m 的二維環(huán)境地圖進行。圖7 和圖8 所示分別為簡單環(huán)境和復雜環(huán)境效果對比圖。圖中，白色區(qū)域為可通行區(qū)域，黑色區(qū)域為不可通行區(qū)域。仿真實驗中起點ps坐標為(20，20)，終點坐標pg為(580，580)，結合文獻[24]設置各項參數λ=20、ε=1、γ=20、ρ0=120、P=20。同時，對比改進RRT-Connect 算法與傳統(tǒng)RRT 算法、RRTConnect 算法在相同環(huán)境中規(guī)劃出的路徑長度和運行時間，如表1所示。

圖7 簡單環(huán)境效果對比圖Fig. 7 Performance comparison of diverse algorithms under complex environment

圖8 復雜環(huán)境效果對比圖Fig. 8 Performance comparison of diverse algorithms under complex environment

由圖7 和8 及表1 可知：在簡單環(huán)境中，與傳統(tǒng)的RRT算法和RRT-Connect算法相比，改進后的RRT-Connect 算法的全局路徑長度分別減少了311 m 和142 m，算法運行時間分別減少了29.37 s和3.36 s。即改進RRT-Connect 算法路徑長度最高縮短了27.3%，運行時間最高縮短了92.88%。而在復雜環(huán)境中，與傳統(tǒng)的RRT算法和RRT-Connect算法相比，改進后的RRT-Connect算法所獲得的全局路徑長度分別減少了297 m 和236 m，算法運行時間分別減少36.12 s和3.74 s。即路徑長度最高縮短了26.92%，運行時間最高縮短了89.78%。

此外，改進后的RRT-Connect算法進行了路徑節(jié)點篩選，解決了輸出路徑折線段和轉角過多的問題，因此，所規(guī)劃的路徑質量顯著比傳統(tǒng)的RRT 和RRT-Connect 算法的優(yōu)。改進后的RRTConnect 算法規(guī)劃出的路徑長度和運行時間均明顯比傳統(tǒng)的RRT算法和RRT-Connect算法的少。

3.1.2 融合算法仿真對比實驗

1) 確定最優(yōu)評價函數權重參數。為了驗證融合算法的有效性，需要對局部規(guī)劃器(改進DWA算法)評價函數的參數進行整定，其中權重參數有α，β和γ，設置了如圖9所示的二維仿真環(huán)境地圖，該環(huán)境地圖長度×寬度為12 m×12 m，其中白色圓圈所在區(qū)域代表此處為障礙物區(qū)域，起點坐標為(0，0)，終點坐標為(10，10)。對目前繁瑣的反復調參情況，本文選用待定系數法，先取GD(v，w)的權重系數α為經驗值0.5，通過設置以下實驗參數對照組，以運行時間最短為評價標準，選取最優(yōu)的參數β和γ。

圖9 二維仿真場景示意圖Fig. 9 Schematic diagram of two-dimensional simulation scene

α為0.5 時不同權重參數組合的路徑規(guī)劃耗時結果如表2 所示。由表2 可知：當γ為0 時，DWA算法不考慮軌跡評價函數V(v，w)的約束效果，此時由于移動機器人沒有前行速度參考，所以會出現時間t無窮大而導致規(guī)劃失敗的情況，無法進行有效的局部路徑規(guī)劃。當β和γ分別為0.1 和0.9時，移動機器人局部路徑規(guī)劃所用的時間t最短。

表2 α為0.5時不同權重參數組合的路徑規(guī)劃耗時結果Table 2 Time-consuming results of path planning with different weight parameter combinations when α is 0.5 s

2) 動態(tài)避障測試實驗對比。為驗證融合算法的動態(tài)避障能力，使用長度×寬度為20 m×20 m環(huán)境地圖作為仿真實驗環(huán)境，設置起始點坐標為(1，1)，目標點坐標為(19，19)，在改進融合算法規(guī)劃的路線上添加2個動態(tài)障礙物A和B，融合方法驗證結果如圖10所示。其中第一個動態(tài)障礙物A和第二個動態(tài)障礙物B分別以v=1 m/s的速度沿x軸負方向和y軸方向從a和b點移動至a′和b′點，虛線空心橢圓框為機器人與障礙物相遇時障礙物的位置。機器人參數反饋如圖11所示。

圖10 存在動態(tài)障礙物時的融合算法仿真實驗Fig. 10 Simulation experiment of fusion algorithm under dynamic obstacle avoidance

圖11 融合算法機器人線速度和角速度輸出結果圖Fig. 11 Robot linear and angular velocities of fusion algorithm

從圖10 可知：改進后的路徑規(guī)劃算法所規(guī)劃的路徑能夠有效避開動態(tài)障礙物，且與障礙物之間始終保持安全距離。由圖11 可知：在與障礙物相遇時，融合算法能夠降低機器人的線速度，當避開障礙物時恢復最大移動速度，有效避免了與動態(tài)障礙物相碰的情況。

3.2 物理實驗

為驗證融合導航算法在真實環(huán)境中的動態(tài)避障效果，使用兩輪差速移動機器人Turtlebot2 進行實驗。其中，機器人具體結構如圖12(a)所示，真實實驗場地如圖12(b)所示。以個人PC為主機、機器人控制器為從機進行局域網內主從機網絡配置。其中，主機端開啟鍵盤控制節(jié)點，而從機端開啟基于Gmapping算法的建圖節(jié)點，構建如圖12(c)所示的柵格地圖。在機器人上搭載的主要器件有：1個STM32F103 運動控制器、4 個驅動電機、1 個Ls01b型激光雷達等，同時在機器人控制器和個人PC中預裝基于Ubuntu18.04的機器人操作系統(tǒng)ROS(Melodic)。

圖12 室內實驗條件Fig. 12 Indoor experimental conditions

3.2.1 改進RRT-Connect算法物理對比實驗

控制機器人完成真實環(huán)境下的導航功能。在圖13 所示的真實環(huán)境中分別對原始RRT-Connect算法、改進RRT-Connect算法進行導航實驗。打開Rviz 可視化界面，完成相應顯示配置，進行初始位姿估計并指定目標點，基于建好的環(huán)境地圖對原始RRT-Connect 算法、改進RRT-Connect 算法進行導航實驗，運行結果如圖13所示。

圖13 Rviz環(huán)境下移動機器人路徑規(guī)劃算法效果對比Fig. 13 Comparison of diverse algorithms for mobile robot path planning in Rviz environment

由圖13可知：原始RRT-connect全局路徑規(guī)劃算法在所設置的實驗室場景下，雖然能夠規(guī)劃出一條從起點到目標點的安全無碰撞行駛路徑，但是其所規(guī)劃出的全局路徑存在較多的冗余路徑節(jié)點，使得移動機器人整體行走路徑的質量不高，限制了移動機器人的運行效率。改進后的RRTConnect 全局路徑規(guī)劃算法相對于原始算法而言，移動機器人行走路線大為簡化，除轉彎處以外均能保持規(guī)則直線，行走路徑質量得到了顯著的提升。路徑規(guī)劃實驗結果對比如表3所示。由表3可知：在相同環(huán)境下，改進RRT-Connect全局路徑規(guī)劃算法相比于原始RRT-Connect 全局路徑規(guī)劃算法，路徑長度縮短了7.37%，運行時間縮短了11.59%?？芍?，改進后的RRT-Connect全局路徑規(guī)劃算法路徑短、質量高，表明該改進算法在實際運行過程中的路徑規(guī)劃效果提升較為明顯。

表3 路徑規(guī)劃實驗結果對比Table 3 Comparison of path planning experiment results

3.2.2 融合導航物理對比實驗

為驗證融合算法的實際效果，在室內環(huán)境下進行單目標點導航實驗，以添加動態(tài)障礙物的方式驗證改進算法的動態(tài)規(guī)劃效果，改進融合算法實驗過程圖如圖14 所示，實驗結果對比如表4所示。

表4 動態(tài)避障過程中規(guī)劃算法對比Table 4 Comparison of planning algorithms during dynamic obstacle avoidance

圖14 改進融合算法路徑規(guī)劃實驗過程圖Fig. 14 Improved fusion algorithm path planning experiment process diagram

在該實驗場景下，機器人導航目標點為(3.68，-1.50) m，機器人最終停止點為(3.85，-1.78) m，停止點與目標點的距離為0.33 m，最終距離均小于0.4 m，故機器人成功到達目標點誤差允許范圍。

在導航過程中，除了主要定位誤差外，還需要對比DWA算法和融合算法的路徑長度和導航時間，如表4所示。由表4可知：本文融合算法相比于原始DWA算法，最終的路徑長度基本相同，但是運行時間縮短了22.56%。因此，改進融合的算法擁有較好的局部規(guī)劃能力，且效果穩(wěn)定。

4 結論

1) 為了獲取符合室內結構化場景運行需求的移動機器人全局路徑與局部路徑，提出了一種基于改進RRT-Connect算法的全局路徑規(guī)劃算法和一種基于改進DWA算法的局部路徑規(guī)劃算法。提出的基于人工勢場法和范圍限定條件的RRT-Connect算法，與原算法相比運行時間最高縮短了92.88%，算法的收斂速度明顯加快；提出的基于Dijkstra 算法的節(jié)點篩選機制，使最終的全局路徑避免了折線和轉角過多的問題，與原算法相比路徑長度最高縮短了27.3%，路徑簡化效果明顯；提出的融合算法能有效避開動態(tài)障礙物且速度波動相對平順。

2) 為驗證所提融合算法的有效性，基于Turtlebot2移動機器人開展了實驗測試。改進RRTConnect 算法相比于原始算法，路徑長度縮短了7.37%，運行時間縮短了11.59%，效果提升較為明顯；改進融合算法相比DWA算法，最終的路徑長度基本相同，運行時間縮短了22.56%，效果提升較為明顯。

3) 本文所提出的算法目前主要用于室內較為復雜環(huán)境下的實現快速路徑規(guī)劃，下一步將嘗試針對室外復雜環(huán)境以及三維導航下的情況進行改進，從而提高算法的泛化性，以便更好地滿足工程實踐的需要。