具有目標(biāo)導(dǎo)向性的RRT路徑規(guī)劃研究

郭孝坤 吳玉秀 李景 程瑞嘉

摘? 要：在機(jī)器人全局路徑規(guī)劃中，首先對(duì)全局路徑規(guī)劃的各種探索路徑的算法做了總結(jié)，對(duì)RRT、A*和D*算法做了對(duì)比，分析了這些算法的優(yōu)缺點(diǎn)。鑒于RRT具有概率完備性，A*和D*具有深度優(yōu)先性，也就是目標(biāo)導(dǎo)向性，文章使用一種算法更改了原RRT算法中隨機(jī)點(diǎn)的選取方式，使RRT具有概率完備性的同時(shí)也擁有了目標(biāo)導(dǎo)向性，實(shí)驗(yàn)證明，改進(jìn)后的RRT算法能夠更快探索出到達(dá)目標(biāo)點(diǎn)的路徑。

關(guān)鍵詞：全局路徑規(guī)劃;RRT;A*;深度優(yōu)先性;概率完備性

中國(guó)分類號(hào)：TP242? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：2096-4706（2022）01-0069-05

Abstract： In the global path planning of robots， we first summarize the various path exploration algorithms of the global path planning， compare the RRT， A* and D* algorithms， and analyze their advantages and disadvantages. Given that RRT has probability completeness， A* and D* has depth priority， that is target orientation， this paper uses an algorithm to change the selection method of random points in the original RRT algorithm and make RRT have probability completeness and target orientation at the same time. The experiment proves that improved RRT algorithm can faster explore the path to reach the target point.

Keywords： global path planning; RRT; A*; depth priority; probability completeness

0? 引? 言

機(jī)器人應(yīng)用中，路徑規(guī)劃是必須且首先需要的核心技術(shù)[1]，很多后續(xù)任務(wù)也都是從路徑規(guī)劃開始。目前路徑規(guī)劃可分為局部路徑規(guī)劃和全局路徑規(guī)劃，本文主要研究了全局路徑規(guī)劃下的路徑探索問(wèn)題。全局路徑規(guī)劃意思是機(jī)器人已知全局環(huán)境信息，通過(guò)拓?fù)浞ā⒖梢晥D法、柵格法將全局信息轉(zhuǎn)化為機(jī)器人易處理的信息，并在此基礎(chǔ)上做路徑探索。常見的路徑探索方法主要有A*、D*、RRT法等。

A*算法在保證避障的前提下總是期望下一步盡可能接近目標(biāo)點(diǎn)[2]，這種路徑規(guī)劃方式有著深度優(yōu)先的特點(diǎn)，他能快速接近目標(biāo)點(diǎn)。但是缺點(diǎn)也很明顯，它容易陷入局部最優(yōu)，如遇上凹型障礙物規(guī)劃的路徑容易一直轉(zhuǎn)圈。D*算法[3]類似A*，不同的是D*算法從目標(biāo)點(diǎn)開始向起始點(diǎn)搜索，在動(dòng)態(tài)環(huán)境中有不俗的表現(xiàn)，他可以在環(huán)境發(fā)生變化時(shí)不斷遍歷節(jié)點(diǎn)從而找出新的最短路徑，但是這種算法是以犧牲效率為代價(jià)的。RRT算法的核心思路是以起始點(diǎn)開始不斷在地圖上擴(kuò)展探尋，直至找到目標(biāo)點(diǎn)為止，其作出的探索路徑像是一顆不斷生長(zhǎng)的樹[4]。RRT是一種具有概率完備性的路徑探索算法，也就是RRT算法理論上一定可以找到通往目標(biāo)點(diǎn)的路徑，并且依靠隨機(jī)性，RRT算法搜索目標(biāo)點(diǎn)的速度要遠(yuǎn)遠(yuǎn)高于完整搜索算法，且不會(huì)陷入局部最優(yōu)。RRT算法缺點(diǎn)是搜索全局路徑效率沒(méi)有A*和D*高，并且搜索出來(lái)的路徑也不是最優(yōu)最短。

本文基于上述全局路徑規(guī)劃方法，分析了各種路徑規(guī)劃算法的優(yōu)缺點(diǎn)，最終研究并改進(jìn)了RRT算法，使得RRT算法在擁有概率完備性的同時(shí)又能擁有A*和D*深度優(yōu)先的優(yōu)點(diǎn)。

1? 傳統(tǒng)RRT算法概述

RRT全稱快速擴(kuò)展隨機(jī)樹，其算法的核心理論是以機(jī)器人初始起點(diǎn)作為樹的根節(jié)點(diǎn)，在向著地圖的任意方向隨機(jī)采樣，以采樣點(diǎn)連接樹中已存在且最靠近的節(jié)點(diǎn)向著采樣點(diǎn)延伸新節(jié)點(diǎn)的方式生長(zhǎng)樹的新節(jié)點(diǎn)，當(dāng)樹的新節(jié)點(diǎn)成功探尋到目標(biāo)點(diǎn)的時(shí)候停止。從該新節(jié)點(diǎn)開始找他的父節(jié)點(diǎn)，直到找到根節(jié)點(diǎn)。這樣就找到一條通往目標(biāo)點(diǎn)的可行路徑[5]。

1.1? 傳統(tǒng)RRT算法步驟

對(duì)地圖進(jìn)行SLAM建模，不考慮智能體所處環(huán)境出現(xiàn)上坡下坡等特殊環(huán)境，假定處于二維平面工作空間W（WorkSapce）中，W∈R2。對(duì)傳統(tǒng)RRT路徑算法步驟描述如下：將智能體的起點(diǎn)xinit作為隨機(jī)樹的根節(jié)點(diǎn);在Wfree中隨機(jī)選擇一個(gè)坐標(biāo)記做xrand;使用Nearest_Neihbor（）函數(shù)尋找隨機(jī)樹上所有節(jié)點(diǎn)，距離xrand最近的節(jié)點(diǎn)向著xrand的方向生長(zhǎng)出新的節(jié)點(diǎn)，記做xnear;反復(fù)調(diào)用EXTEND（）函數(shù)使xnear盡可能接近xrand，直到xnear完成到達(dá)xrand點(diǎn)、靠近障礙物、到達(dá)限制長(zhǎng)度和接近目標(biāo)點(diǎn)四個(gè)目標(biāo)中的一個(gè)為止，將這個(gè)xnear添加到隨機(jī)樹中，此時(shí)隨機(jī)樹成功產(chǎn)生一個(gè)新的節(jié)點(diǎn)xnew。重復(fù)選取xrand，重復(fù)添加新的xnear，直到上述算法成功找到使xnew到達(dá)目標(biāo)點(diǎn)區(qū)域范圍內(nèi)，程序結(jié)束，具體算法描述為：

Function：RRT（n∈Wfree，Δt∈R）

1：T.init（xinit）

2：for a=0; a<n;a++

3：xran←RANDOM_NODE（Wfree）

4：EXTEND（T， xrand）

5：end for

6：return T

Function：EXTEND（T， xrand）

1：xnear←Nearet_Neighbor （T， xrand）

2：u←Select_Input （xrand， xnew）

3：xnew←New_State（xnear， u， Δt）

4：if NoCollision Path（xnear， xnew） then

5：T. Add_Node（xnew）

6：T.Add_Edge（xnear， xnew， u）

7：end if

8：return T

1.2? 傳統(tǒng)RRT算法圖解

RRT大致過(guò)程如圖1所示，圖中綠色光點(diǎn)代表起始點(diǎn)，黃色光點(diǎn)代表終點(diǎn)，經(jīng)過(guò)一段時(shí)間的搜尋，RRT算法最終找到了目標(biāo)點(diǎn)，圖中粉紅色線段代表RRT算法找出的路徑?？梢钥闯鰝鹘y(tǒng)RRT路徑在地圖的每一個(gè)角落都進(jìn)行了搜索，他探索的路徑不一定是最優(yōu)的，但一定可以找到目標(biāo)點(diǎn)。

2? 改進(jìn)的RRT算法

2.1? 改進(jìn)的RRT算法介紹

通過(guò)圖1可以看出傳統(tǒng)RRT算法雖然通過(guò)概率完備性成功在地圖上找到了通往目標(biāo)點(diǎn)的路徑，但是RRT做了很多無(wú)用功，有些不必要搜索的地方RRT也做了探尋。如果給RRT算法提供一個(gè)朝向目標(biāo)的大體方向，使RRT向著目標(biāo)點(diǎn)的方向探尋，那么探尋目標(biāo)點(diǎn)的效率就會(huì)提高很多。因此本文對(duì)傳統(tǒng)RRT算法做了改進(jìn)，使之添加了一種目標(biāo)導(dǎo)向性的特性。

根據(jù)上述算法描述，RRT算法每次下一節(jié)點(diǎn)xnear的確定都是從最靠近xrand的樹節(jié)點(diǎn)向xrand方向延伸。而xrand是通過(guò)概率函數(shù)隨機(jī)在地圖上撒點(diǎn)得到，所以改進(jìn)RRT算法的核心就是控制隨機(jī)點(diǎn)xrand在地圖上的落點(diǎn)。如果能設(shè)計(jì)一種機(jī)制，使得xrand的隨機(jī)性可控，即期望它下一次落點(diǎn)能夠幫助RRT算法更快朝著目標(biāo)探索，那么就可以有效提高RRT算法的效率。當(dāng)然，這種控制xrand隨機(jī)落點(diǎn)的行為會(huì)導(dǎo)致RRT算法丟失隨機(jī)性的特點(diǎn)，所以改進(jìn)機(jī)制需要在幫助RRT提高探索效率的同時(shí)，還要盡可能保留其隨機(jī)性的特點(diǎn)，也就是改進(jìn)RRT算法可以選擇犧牲少部分隨機(jī)性特點(diǎn)，來(lái)?yè)Q取其具有一部分目標(biāo)導(dǎo)向性的特點(diǎn)。

改進(jìn)RRT的地圖探索機(jī)制如圖2所示，綠色五角星表示RRT節(jié)點(diǎn)樹中距離目標(biāo)點(diǎn)最近的節(jié)點(diǎn)，紅色為目標(biāo)點(diǎn)。以兩點(diǎn)為中線（θ1=θ2=45°），按圖示意將地圖分為OAB、OBC、OCD、ODA四個(gè)區(qū)域，其中，∠AOB=∠BOC=∠COD=∠DOA=π/2。OAB為目標(biāo)域，OBC和ODA為鄰域，OCD為目標(biāo)反向域。

設(shè)總概率不變，改進(jìn)后的RRT算法將通過(guò)概率函數(shù)增大在目標(biāo)區(qū)域選擇xrand坐標(biāo)的概率，減小在目標(biāo)反向域選擇坐標(biāo)的概率，而xrand在兩個(gè)鄰域落點(diǎn)的概率保持不變。這樣改進(jìn)后的RRT算法，犧牲了一定的隨機(jī)性，添加了一定的目標(biāo)導(dǎo)向性，即樹的生長(zhǎng)被指定了大致的方向。

2.2? 改進(jìn)RRT的xrand節(jié)點(diǎn)的選取

改進(jìn)的RRT算法與未改進(jìn)的RRT算法比較，其他算法步驟不變，只更改隨機(jī)點(diǎn)xrand選取的方法[6]。在原始RRT算法中，xrand的選取機(jī)制是以地圖的長(zhǎng)和寬為基礎(chǔ)各乘以一個(gè)隨機(jī)的百分比得到地圖上的隨機(jī)點(diǎn)，改進(jìn)后的RRT算法在為xrand選取隨機(jī)點(diǎn)始終是以O(shè)點(diǎn)建立的極坐標(biāo)選取的，原始的方法并不適用。因此，改進(jìn)的方法是先以O(shè)點(diǎn)建立一個(gè)極坐標(biāo)系并求出角度和距離，隨后將極坐標(biāo)值轉(zhuǎn)換成原全局地圖上的坐標(biāo)值，隨后再加上O點(diǎn)的坐標(biāo)值即可表示出xrand在全局地圖下的坐標(biāo)點(diǎn)。

首先是xrand角度的選取，角度選取機(jī)制如下：

（1）遍歷當(dāng)前已存在的樹的節(jié)點(diǎn)，找出距離目標(biāo)點(diǎn)最近的節(jié)點(diǎn)，記為O點(diǎn);

（2）O點(diǎn)水平向右延伸，至地圖邊緣形成OE線段，以O(shè)E線段為極軸建立極坐標(biāo)系;

（3）根據(jù)圖2展示的改進(jìn)RRT探索機(jī)制，利用概率函數(shù)分配xrand在對(duì)應(yīng)區(qū)域出現(xiàn)的概率;

（4）概率函數(shù)執(zhí)行，得到xrand的角度值。

在選取了角度之后需要對(duì)xrand節(jié)點(diǎn)選取長(zhǎng)度。為了讓xrand能夠覆蓋地圖所有區(qū)域，需要對(duì)不同角度xrand長(zhǎng)度選取做計(jì)算，xrand的長(zhǎng)度根據(jù)可表示為：

式中rand（）為隨機(jī)百分比函數(shù)，OVn他表示了O點(diǎn)到四個(gè)頂點(diǎn)其中一個(gè)的距離，若長(zhǎng)度超出地圖邊界時(shí)，算法重新執(zhí)行式（1），直至長(zhǎng)度不會(huì)超出地圖。OVn為xrand取不同角度時(shí)應(yīng)取得的最長(zhǎng)模長(zhǎng)，如圖3所示改進(jìn)RRT中xrand長(zhǎng)度選取機(jī)制，圖中OE、OF、OG和OH將地圖分為四個(gè)部分，每個(gè)部分最長(zhǎng)的模長(zhǎng)即為O點(diǎn)至其斜邊的距離。設(shè)式（1）中xrand取得的角度為θ，若θ∈[0，π/2]，則n為2;θ∈[π/2，π]，n為3;θ∈[π，3/2π]，n為4;θ∈[3/2π，2π]，n為1。

在xrand以O(shè)E為極坐標(biāo)系時(shí)的角度和距離都確定情況下，設(shè)O點(diǎn)在地圖上的坐標(biāo)為（xo，yo），則xrand在地圖上的坐標(biāo)可表示為：

式（2）表示在全局地圖坐標(biāo)下xrand的坐標(biāo)，改進(jìn)RRT算法相對(duì)于原始RRT算法需要對(duì)表1算法第三點(diǎn)xrand←RANDOM_NODE（Wfree）修改為本節(jié)所描述算法，其他步驟不變。

3? 實(shí)驗(yàn)仿真

在一個(gè)20×20的方形地圖內(nèi)，左上角坐標(biāo)為（0，0），右下角坐標(biāo)為（20，20），地圖中有障礙物若干，其中還包含一個(gè)凹型障礙物陷阱。RRT算法探索的起始點(diǎn)是（5，0），在地圖中以綠色光點(diǎn)標(biāo)明，目標(biāo)點(diǎn)是（5，20），在地圖中以黃色光點(diǎn)標(biāo)明。在matlab上基于上述地圖對(duì)原始RRT算法和改進(jìn)后RRT算法進(jìn)行實(shí)驗(yàn)仿真。

如圖4所示進(jìn)行了九次原始RRT算法仿真，如圖5所示進(jìn)行了九次改進(jìn)后RRT算法仿真。表2為前后兩次仿真實(shí)驗(yàn)相關(guān)數(shù)據(jù)。

從圖4和圖5可以看出，圖4中的實(shí)驗(yàn)每次總是試圖探尋整個(gè)地圖以發(fā)掘找到目標(biāo)點(diǎn)的路徑，圖5中的實(shí)驗(yàn)明顯減少了對(duì)不必要搜索區(qū)域的探索。但是這種對(duì)比不是絕對(duì)的，原始RRT算法也可能比改進(jìn)后RRT更先更快探索到目標(biāo)點(diǎn)，如圖4（a）對(duì)比圖5（a），圖4（g）對(duì)比圖5（g），但是大概率對(duì)比，改進(jìn)后的RRT是優(yōu)于原始RRT算法的，如其余七幅圖的對(duì)比。

如表2所示，原始RRT算法平均每次的總時(shí)長(zhǎng)為5.27 s，平均每次步數(shù)為259步，改進(jìn)后RRT算法平均每次總時(shí)長(zhǎng)為3.34 s，平均每次為步數(shù)189步。表2數(shù)據(jù)表明改進(jìn)后的RRT算法在平均步數(shù)和平均用時(shí)上大大少于原始RRT算法，并且他繼承了原始RRT算法隨機(jī)性的同時(shí)也吸收了A*和D*算法深度優(yōu)先性的特點(diǎn)。

4? 結(jié)? 論

路徑規(guī)劃是機(jī)器人應(yīng)用中非常重要的一環(huán)，本文對(duì)全局路徑規(guī)劃中的幾種常見的路徑探索算法的優(yōu)缺點(diǎn)進(jìn)行了歸納總結(jié)，最后用RRT算法結(jié)合A*和D*算法的優(yōu)點(diǎn)進(jìn)行了改進(jìn)，讓RRT算法既有隨機(jī)性又有目標(biāo)導(dǎo)向性，實(shí)驗(yàn)證明這種改進(jìn)方式能利用隨機(jī)性幫助智能體逃離局部最優(yōu)的同時(shí)，也能更快速探索出通往目標(biāo)點(diǎn)的路徑。

改進(jìn)后的RRT算法依然存在不足：

其一，對(duì)于目標(biāo)區(qū)域、鄰域和目標(biāo)反向域的xrand落點(diǎn)概率值并沒(méi)有嚴(yán)格的算法支持，在本文中依靠的是多次試驗(yàn)獲得的相對(duì)較優(yōu)的經(jīng)驗(yàn)值，后續(xù)的改進(jìn)中可以利用自適應(yīng)等算法鎖定最優(yōu)概率值。

其二，如圖5改進(jìn)的RRT部分實(shí)驗(yàn)展示（圖5（d），圖5（g），圖5（h），圖5（i）），在犧牲了一部分的隨機(jī)性之后，這可能會(huì)導(dǎo)致算法在大概率情況下會(huì)嘗試目標(biāo)方向的路徑探索，即使目標(biāo)方向會(huì)有障礙，這將導(dǎo)致一些不必要的探索出現(xiàn)，此時(shí)路徑探索會(huì)變得集中，后續(xù)的改進(jìn)中仍需要減少這種不必要的路徑探索。

其三，改進(jìn)后RRT算法中xrand的最長(zhǎng)模長(zhǎng)選取機(jī)制不是很合理，有時(shí)隨機(jī)長(zhǎng)度可能會(huì)超出地圖邊緣，這時(shí)xrand的長(zhǎng)度需要重新選擇，這使得改進(jìn)后RRT算法的總用時(shí)可能會(huì)加長(zhǎng)，后續(xù)改進(jìn)中可以選擇新的模長(zhǎng)選取機(jī)制，以減少總時(shí)長(zhǎng)。

參考文獻(xiàn)：

[1] 張捍東，鄭睿，岑豫皖.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)的現(xiàn)狀與展望 [J].系統(tǒng)仿真學(xué)報(bào)，2005（2）：439-443.

[2] 張海濤，程蔭杭.基于A*算法的全局路徑搜索 [J].微計(jì)算機(jī)信息，2007（17）：238-239+308.

[3] 谷潤(rùn)平，崔朋，唐建勛，等.基于D*算法的場(chǎng)面滑行動(dòng)態(tài)規(guī)劃研究 [J].科學(xué)技術(shù)與工程，2015，15（1）：315-319+328.

[4] DU M B，MEI T，CHEN J J，et al.RRT-based Motion Planning Algorithm for Intelligent Vehicle in Complex Environments [J].Robot，2015，37（4）：443-450.

[5] 張捍東，陳陽(yáng)，吳玉秀.未知環(huán)境下移動(dòng)機(jī)器人實(shí)時(shí)路徑規(guī)劃 [J].計(jì)算機(jī)工程與應(yīng)用，2018，54（19）：140-146.

[6] 劉永紅，劉明雍，謝波.航位推算組合導(dǎo)航系統(tǒng)在線標(biāo)定技術(shù) [J].中國(guó)慣性技術(shù)學(xué)報(bào)，2015，23（4）：434-437.

作者簡(jiǎn)介：郭孝坤（1997—），男，安徽馬鞍山人，碩士在讀，研究方向：路徑規(guī)劃;吳玉秀（1982—），男，漢族，河南安陽(yáng)人，講師，博士，研究方向：機(jī)器視覺(jué)與圖像檢測(cè);李景（1997—），男，安徽六安人，碩士在讀，研究方向：雙目視覺(jué);程瑞嘉（1997—），男，山東濱州人，碩士在讀，研究方向：深度學(xué)習(xí)。