融合RRT-Connect和DWA算法的室內(nèi)移動(dòng)機(jī)器人單目標(biāo)點(diǎn)導(dǎo)航任務(wù)研究

王冠強(qiáng) ，張馳洲, ，陳明松, ，藺永誠(chéng) ，鄒奮揚(yáng) ，王秋, ，吳敏杰 ，曾維棟

(1. 中南大學(xué) 機(jī)電工程學(xué)院，湖南 長(zhǎng)沙，410083；2. 極端服役性能精準(zhǔn)制造全國(guó)重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙，410083；3. 中南大學(xué) 輕合金研究院，湖南 長(zhǎng)沙，410083)

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

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

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

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

1 路徑規(guī)劃算法

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

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

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

1.1.1 增設(shè)第三和第四擴(kuò)展樹

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

算法1：改進(jìn)后的RRT-Connect輸入：起始點(diǎn)pstart，目標(biāo)點(diǎn)pgoal，障礙物點(diǎn)pobstacle輸出：路徑Path(pstart，pgoal)1：按照式(1)和(2)計(jì)算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：//函數(shù)Extend( )和Connect( )的實(shí)現(xiàn)基于文獻(xiàn)[12]22：//函數(shù)Sampling_Area_Limitation( )表示限定采樣點(diǎn)生成范圍23：//函數(shù)Sampling_GRAR( )是指在引力場(chǎng)和排斥場(chǎng)的作用下產(chǎn)生隨機(jī)點(diǎn)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)

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

1.1.2 限定采樣點(diǎn)選取范圍

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

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

1.1.3 引入人工勢(shì)場(chǎng)法

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

式中：F(n)為新節(jié)點(diǎn)到目標(biāo)點(diǎn)的擴(kuò)展函數(shù)；G(n)為新節(jié)點(diǎn)的隨機(jī)擴(kuò)展函數(shù)；A(n)為目標(biāo)點(diǎn)到隨機(jī)樹中最近節(jié)點(diǎn)pnear(pne)的引力函數(shù)；R(n)為障礙物點(diǎn)到隨機(jī)樹節(jié)點(diǎn)的斥力函數(shù)。

新節(jié)點(diǎn)的隨機(jī)擴(kuò)展函數(shù)G(n)的表示方法為

式中：λ為擴(kuò)展的固定步長(zhǎng)。

目標(biāo)引力函數(shù)A(n)和斥力函數(shù)R(n)的表示方法為

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

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

疊加了引力場(chǎng)后的新算法每一個(gè)節(jié)點(diǎn)的擴(kuò)展都是按照式(7)的方法進(jìn)行，使兩棵雙向隨機(jī)樹在各自引力和斥力分量的引導(dǎo)下向目標(biāo)點(diǎn)方向擴(kuò)展，擴(kuò)展示意圖如圖2所示。

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

1.1.4 路徑平滑處理

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

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

同時(shí)考慮到機(jī)器人的轉(zhuǎn)向能力，期望最短的歐幾里得距離分布和較低的軌跡平滑度?；谝陨戏治?，本文選擇一個(gè)更合理的成本函數(shù)Cost，如式(7)所示。圖4所示為成本函數(shù)的節(jié)點(diǎn)示意圖。

圖4 改進(jìn)成本函數(shù)的節(jié)點(diǎn)示意圖Fig. 4 Node diagram of improved cost function

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

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

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

為更好地?cái)U(kuò)展機(jī)器人在移動(dòng)過程中的實(shí)時(shí)調(diào)優(yōu)能力，本節(jié)將介紹移動(dòng)機(jī)器人的實(shí)時(shí)軌跡計(jì)算方式，并基于DWA算法進(jìn)行評(píng)價(jià)函數(shù)優(yōu)化以加快函數(shù)收斂速度，減少路徑計(jì)算耗時(shí)，實(shí)現(xiàn)更快速的動(dòng)態(tài)決策。

1.2.1 機(jī)器人航跡推算

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

圖5 兩輪差速移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型圖Fig. 5 Kinematic model diagram of two wheel differential mobile robot

假設(shè)機(jī)器人整個(gè)運(yùn)動(dòng)過程勻速行駛，經(jīng)過很小的時(shí)間間隔Δt后(近似軌跡直線處理)，其航跡推算公式如下：

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

1.2.2 評(píng)價(jià)函數(shù)優(yōu)化

在運(yùn)動(dòng)過程中，原軌跡評(píng)價(jià)函數(shù)中的角度差評(píng)價(jià)函數(shù)會(huì)選擇角度差值較小的速度集合來進(jìn)行角度修正，這樣也會(huì)使得移動(dòng)機(jī)器人需要進(jìn)行反復(fù)的角度調(diào)整。因此，這里引入目標(biāo)點(diǎn)距離評(píng)價(jià)函數(shù)替代航向角函數(shù)，以預(yù)測(cè)軌跡中離目標(biāo)點(diǎn)最近的軌跡作為下一時(shí)刻的最優(yōu)軌跡，改進(jìn)后的新評(píng)價(jià)函數(shù)如下式所示：

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

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

2 單目標(biāo)點(diǎn)導(dǎo)航算法

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

圖6 單目標(biāo)導(dǎo)航算法流程圖Fig. 6 Flow chart of single target navigation algorithm

3 實(shí)驗(yàn)與分析

3.1 仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)硬件平臺(tái)配置為AMD Ryzen 5 3600 6-Core Processor 3.59 GHz CPU，RAM 為16 GB，采用matlab2020a軟件平臺(tái)編程實(shí)現(xiàn)。

3.1.1 改進(jìn)RRT-Connect算法仿真對(duì)比實(shí)驗(yàn)與分析

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

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

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

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

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

3.1.2 融合算法仿真對(duì)比實(shí)驗(yàn)

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

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

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

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

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

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

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

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

3.2 物理實(shí)驗(yàn)

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

圖12 室內(nèi)實(shí)驗(yàn)條件Fig. 12 Indoor experimental conditions

3.2.1 改進(jìn)RRT-Connect算法物理對(duì)比實(shí)驗(yàn)

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

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

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

表3 路徑規(guī)劃實(shí)驗(yàn)結(jié)果對(duì)比Table 3 Comparison of path planning experiment results

3.2.2 融合導(dǎo)航物理對(duì)比實(shí)驗(yàn)

為驗(yàn)證融合算法的實(shí)際效果，在室內(nèi)環(huán)境下進(jìn)行單目標(biāo)點(diǎn)導(dǎo)航實(shí)驗(yàn)，以添加動(dòng)態(tài)障礙物的方式驗(yàn)證改進(jìn)算法的動(dòng)態(tài)規(guī)劃效果，改進(jìn)融合算法實(shí)驗(yàn)過程圖如圖14 所示，實(shí)驗(yàn)結(jié)果對(duì)比如表4所示。

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

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

在該實(shí)驗(yàn)場(chǎng)景下，機(jī)器人導(dǎo)航目標(biāo)點(diǎn)為(3.68，-1.50) m，機(jī)器人最終停止點(diǎn)為(3.85，-1.78) m，停止點(diǎn)與目標(biāo)點(diǎn)的距離為0.33 m，最終距離均小于0.4 m，故機(jī)器人成功到達(dá)目標(biāo)點(diǎn)誤差允許范圍。

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

4 結(jié)論

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

2) 為驗(yàn)證所提融合算法的有效性，基于Turtlebot2移動(dòng)機(jī)器人開展了實(shí)驗(yàn)測(cè)試。改進(jìn)RRTConnect 算法相比于原始算法，路徑長(zhǎng)度縮短了7.37%，運(yùn)行時(shí)間縮短了11.59%，效果提升較為明顯；改進(jìn)融合算法相比DWA算法，最終的路徑長(zhǎng)度基本相同，運(yùn)行時(shí)間縮短了22.56%，效果提升較為明顯。

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