目標(biāo)偏置雙向RRT*算法的機(jī)器人路徑規(guī)劃

劉奧博，袁 杰

新疆大學(xué) 電氣工程學(xué)院，烏魯木齊 830047

機(jī)器人路徑規(guī)劃可以定義為找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞可行路徑的過(guò)程。基于采樣的算法已經(jīng)應(yīng)用于解決高維規(guī)劃問(wèn)題，且計(jì)算效率較高。

LaValle提出經(jīng)典的基于采樣的RRT算法[1]，對(duì)整個(gè)環(huán)境空間進(jìn)行均勻隨機(jī)采樣，具有概率完備性，但是該算法只通過(guò)一棵隨機(jī)樹(shù)對(duì)空間進(jìn)行探索，在復(fù)雜環(huán)境中會(huì)消耗大量時(shí)間。Kuffner等[2]提出的RRT-connect算法，采用雙向搜索思想在起點(diǎn)和目標(biāo)點(diǎn)同時(shí)生成兩棵隨機(jī)樹(shù)。李成雷等[3]提出改進(jìn)的RRT-connect算法，在采樣過(guò)程中引入轉(zhuǎn)角約束，以減少搜索節(jié)點(diǎn)，但上述算法隨機(jī)樹(shù)的生長(zhǎng)是發(fā)散的，在不必要的區(qū)域進(jìn)行采樣會(huì)耗費(fèi)大量的時(shí)間，規(guī)劃出的路徑也不是最優(yōu)的。Karaman等[4]提出的RRT*算法，當(dāng)?shù)螖?shù)足夠多時(shí)，可找到一條最優(yōu)或接近最優(yōu)的路徑，但是搜索時(shí)間大大增加。Jordan等[5]提出雙向版的RRT*算法，稱為B-RRT*算法，搜索速度加快，但在復(fù)雜環(huán)境下計(jì)算節(jié)點(diǎn)量較大。Qureshi等[6]提出智能雙向搜索的IB-RRT*尋優(yōu)算法，專用于復(fù)雜環(huán)境搜索，但是采樣節(jié)點(diǎn)的擴(kuò)展是隨機(jī)的，在無(wú)用區(qū)域采樣造成不必要的時(shí)間消耗。為避免隨機(jī)樹(shù)擴(kuò)展過(guò)程的隨機(jī)性，LaValle等[7-11]采用目標(biāo)偏置思想，使隨機(jī)樹(shù)朝目標(biāo)點(diǎn)方向生長(zhǎng)。Nasir等[12]提出智能偏置采樣的RRT*-smart算法，但不同環(huán)境下需手動(dòng)調(diào)節(jié)偏置比，自適應(yīng)性較差。Liu等[13]提出將目標(biāo)偏置與曲線平滑結(jié)合的GB-RRT算法，減少搜索時(shí)間，但規(guī)劃路經(jīng)轉(zhuǎn)折處較多，路徑距離較長(zhǎng)。劉建宇等[14]提出改進(jìn)的RRT*-connect算法，在目標(biāo)偏置采樣基礎(chǔ)上對(duì)采樣區(qū)域進(jìn)行約束，但當(dāng)可行路徑需經(jīng)過(guò)約束區(qū)域之外時(shí)，算法不能確保找到可行解，有待進(jìn)一步完善。

基于上述研究，本文針對(duì)較復(fù)雜環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃問(wèn)題，提出了一種目標(biāo)偏置雙向RRT*（goal biased bidirectional RRT*，GBB-RRT*）算法。首先借鑒雙向搜索思想，從起始和目標(biāo)位置同時(shí)構(gòu)造兩棵隨機(jī)樹(shù)，每次迭代，兩棵隨機(jī)樹(shù)都進(jìn)行新節(jié)點(diǎn)的擴(kuò)展，加快隨機(jī)樹(shù)擴(kuò)展速度，減少搜索時(shí)間。在采樣過(guò)程中，在一定概率下將兩棵隨機(jī)樹(shù)各自目標(biāo)點(diǎn)作為采樣點(diǎn)，引導(dǎo)兩棵隨機(jī)樹(shù)以一定的概率朝各自的目標(biāo)點(diǎn)方向生長(zhǎng)，減少在無(wú)用區(qū)域的搜索，提高搜索效率，減少時(shí)間消耗。針對(duì)規(guī)劃得到的初始路徑曲折問(wèn)題，設(shè)計(jì)一種逆向?qū)?yōu)結(jié)合轉(zhuǎn)角約束路徑簡(jiǎn)化方法，然后采用B樣條曲線對(duì)簡(jiǎn)化后的路徑進(jìn)行處理，生成機(jī)器人可跟蹤執(zhí)行的光滑路徑。通過(guò)仿真復(fù)雜環(huán)境實(shí)驗(yàn)對(duì)比，驗(yàn)證了所提算法的快速性和有效性。

1 B-RRT*算法

B-RRT*算法采用貪婪啟發(fā)式進(jìn)行兩棵隨機(jī)樹(shù)的連接，既具有概率完備性，且已證明具備漸進(jìn)最優(yōu)性，相比RRT*算法額外增加了Connect()函數(shù)，算法1給出了BRRT*算法的偽代碼。

B-RRT*算法以起始點(diǎn)xinit和目標(biāo)點(diǎn)xgoal為根節(jié)點(diǎn)生成兩棵隨機(jī)樹(shù)Ta、Tb。首先擴(kuò)展隨機(jī)樹(shù)Ta，初始階段與RRT*算法完全相同，在配置空間中隨機(jī)采樣得到xrand，然后執(zhí)行算法2。

搜索隨機(jī)樹(shù)Ta中距離最近的樹(shù)節(jié)點(diǎn)xnearest，其延伸步長(zhǎng)距離得到新節(jié)點(diǎn)xnew。以xnew為圓心r為半徑的圓確定鄰域集合Xnear，將從根節(jié)點(diǎn)經(jīng)過(guò)Xnear中節(jié)點(diǎn)到達(dá)xnew的無(wú)碰撞路徑代價(jià)與從根節(jié)點(diǎn)經(jīng)過(guò)xnearest到達(dá)xnew的路徑代價(jià)比較，確定路徑代價(jià)最小的節(jié)點(diǎn)為xnew的最佳父節(jié)點(diǎn)。然后將路徑與新節(jié)點(diǎn)xnew插入隨機(jī)樹(shù)，對(duì)Xnear中每個(gè)節(jié)點(diǎn)進(jìn)行重布線。在將新節(jié)點(diǎn)xnew插入隨機(jī)樹(shù)Ta后，接下來(lái)和RRT*有所不同，執(zhí)行函數(shù)Connect(xnew,Tb)，返回?zé)o碰撞可行路徑代價(jià)θnew，若θnew＜θbest，則兩棵樹(shù)連接成功。迭代結(jié)束時(shí)通過(guò)SwapTrees(Ta,Tb)交換兩棵隨機(jī)樹(shù)，在下次迭代中，進(jìn)行隨機(jī)樹(shù)Tb的擴(kuò)展，執(zhí)行和隨機(jī)樹(shù)Ta相同的擴(kuò)展過(guò)程。

2 目標(biāo)偏置雙向RRT*算法

機(jī)器人在存在大量障礙物等復(fù)雜環(huán)境中進(jìn)行路徑規(guī)劃時(shí)，使用RRT*和B-RRT*算法存在搜索時(shí)間長(zhǎng)、采樣點(diǎn)隨機(jī)性大和規(guī)劃路徑曲折的問(wèn)題。基于上述研究，本文提出了目標(biāo)偏置雙向RRT*算法（GBB-RRT*）。主要改進(jìn)是一次迭代中同時(shí)擴(kuò)展兩棵隨機(jī)樹(shù)、引入目標(biāo)偏置策略優(yōu)化采樣過(guò)程。算法3給出了GBB-RRT*算法偽代碼。

圖1為GBB-RRT*算法擴(kuò)展示意圖。如圖1（a）和算法3所示，算法在每次迭代中，以起始點(diǎn)xinit和目標(biāo)點(diǎn)xgoal為根節(jié)點(diǎn)生成兩棵隨機(jī)樹(shù)Ta、Tb，這兩棵隨機(jī)樹(shù)將同時(shí)在自由環(huán)境中擴(kuò)展，以隨機(jī)樹(shù)Ta向其目標(biāo)點(diǎn)xgoal方向擴(kuò)展過(guò)程來(lái)說(shuō)明。首先，通過(guò)算法4生成新節(jié)點(diǎn)

圖1 目標(biāo)偏置雙向擴(kuò)展隨機(jī)樹(shù)Fig.1 Goal biased bidirectional RRT*

算法4為改進(jìn)的擴(kuò)展函數(shù)Extend*()，引入目標(biāo)偏置思想確定采樣點(diǎn)：

如式（1），設(shè)定一個(gè)目標(biāo)偏向閾值Ptarget（0.6），然后按均勻概率分布隨機(jī)獲取一個(gè)概率值P（0-1），若其值大于閾值，則將目標(biāo)點(diǎn)xgoal作為采樣節(jié)點(diǎn)；若小于閾值，則將SampleFree()函數(shù)產(chǎn)生的隨機(jī)點(diǎn)作為采樣節(jié)點(diǎn)。圖1中樹(shù)Ta將隨機(jī)點(diǎn)xrand作為采樣節(jié)點(diǎn)。

3 路徑簡(jiǎn)化與平滑處理

在存在大量障礙物等較復(fù)雜環(huán)境中，本文所提GBB-RRT*算法規(guī)劃出的初始路徑仍然曲折。針對(duì)這個(gè)問(wèn)題，設(shè)計(jì)一種逆向?qū)?yōu)結(jié)合轉(zhuǎn)角約束路徑簡(jiǎn)化方法。與文獻(xiàn)[15-17]采用正向簡(jiǎn)化方法相比，本文采用正向遍歷、逆向簡(jiǎn)化的方法。從起點(diǎn)開(kāi)始若有節(jié)點(diǎn)與目標(biāo)點(diǎn)之間連線可無(wú)碰撞直達(dá)，則直接連接該節(jié)點(diǎn)與目標(biāo)點(diǎn)，省去后面節(jié)點(diǎn)的簡(jiǎn)化，減少時(shí)間消耗，對(duì)狹窄通道、大量障礙物等復(fù)雜環(huán)境有快速通過(guò)性，簡(jiǎn)化路徑更短。與文獻(xiàn)[18]在隨機(jī)樹(shù)生長(zhǎng)過(guò)程引入轉(zhuǎn)角約束相比，本文在進(jìn)行簡(jiǎn)化路徑時(shí)引入轉(zhuǎn)角約束，可在滿足機(jī)器人約束條件的同時(shí)使簡(jiǎn)化后路徑長(zhǎng)度更短，然后采用B樣條曲線對(duì)簡(jiǎn)化路徑進(jìn)行平滑處理，生成移動(dòng)機(jī)器人可跟蹤執(zhí)行的光滑路徑。

3.1 路徑簡(jiǎn)化

逆向?qū)?yōu)結(jié)合轉(zhuǎn)角約束路徑簡(jiǎn)化方法如圖2所示。

圖2 轉(zhuǎn)角約束與路徑簡(jiǎn)化示意圖Fig.2 Corner constraint and path simplification

圖2（a）為轉(zhuǎn)角約束示意圖，以xgoal(x1)為起點(diǎn)，其中xi+1是正向遍歷時(shí)第一個(gè)和xgoal連線不與障礙物碰撞的路徑點(diǎn)。φ1為xgoal(x1)對(duì)應(yīng)的初始角度，φ1(i+1)為目標(biāo)點(diǎn)xgoal與路徑點(diǎn)xi+1連線與x軸的夾角，φi+1為路徑點(diǎn)xi+2和xi+1連線與x軸的夾角，φi+2為路徑點(diǎn)xi+3和xi+2連線與x軸的夾角。然后判斷上述參數(shù)是否滿足約束條件：

φmax為最大轉(zhuǎn)角約束，如果公式（2）成立，則可將xgoal和xi+1之間的節(jié)點(diǎn)剪除。

圖2（b）為路徑簡(jiǎn)化示意圖。其中xinit為初始點(diǎn)，xgoal為終點(diǎn)，xi為可行路徑上的節(jié)點(diǎn)，黑實(shí)線為規(guī)劃得到的初始可行路徑。對(duì)初始可行路徑：

步驟1采用正向遍歷方法，確定首個(gè)無(wú)碰撞點(diǎn)。首先從起點(diǎn)xinit開(kāi)始判斷與目標(biāo)點(diǎn)xgoal之間是否存在障礙物。若無(wú)障礙物，則直接連接起點(diǎn)與目標(biāo)點(diǎn)即為最優(yōu)簡(jiǎn)化路徑；若存在障礙物，進(jìn)行下一個(gè)節(jié)點(diǎn)x1與目標(biāo)點(diǎn)xgoal碰撞檢測(cè)。從圖2（b）可看出，xinit、x1、x2和xgoal連線均與障礙物碰撞，x3為初始路徑正向遍歷時(shí)第一個(gè)與目標(biāo)節(jié)點(diǎn)xgoal無(wú)碰撞的路徑節(jié)點(diǎn)。

步驟2逆向?qū)?yōu)，確定簡(jiǎn)化路徑節(jié)點(diǎn)。簡(jiǎn)化路徑的第一個(gè)節(jié)點(diǎn)為xgoal，由步驟1確定的節(jié)點(diǎn)x3，其與目標(biāo)點(diǎn)xgoal之間滿足轉(zhuǎn)角約束，所以x3為簡(jiǎn)化路徑第二個(gè)節(jié)點(diǎn)。同時(shí)將x3定義為新的目標(biāo)節(jié)點(diǎn)xgoal。

重復(fù)步驟1、2，直到完成對(duì)整條初始可行路徑的簡(jiǎn)化。圖2（b）中xinit和新目標(biāo)點(diǎn)x3連線不與障礙物碰撞，且滿足轉(zhuǎn)角約束，因此，xinit為簡(jiǎn)化路徑第三個(gè)節(jié)點(diǎn)。簡(jiǎn)化后的路徑即圖2（b）中從起點(diǎn)xinit經(jīng)過(guò)x3到目標(biāo)點(diǎn)xgoal的路徑。具體的流程如圖3所示。

圖3 路徑簡(jiǎn)化與平滑處理流程圖Fig.3 Flow chart of path simplification and smoothing

3.2 平滑處理

對(duì)上述簡(jiǎn)化方法生成的仍然曲折的簡(jiǎn)化路徑，考慮對(duì)簡(jiǎn)化路徑的平滑要求，采用二次B樣條曲線對(duì)簡(jiǎn)化路徑進(jìn)行平滑處理。由于B樣條曲線的連續(xù)性和局部性特點(diǎn)，可在不改變整條路徑形狀的情況下，只在局部對(duì)簡(jiǎn)化路徑進(jìn)行平滑處理。如圖4（a）中藍(lán)色實(shí)線為GBBRRT*規(guī)劃得到的初始可行路徑；圖4（b）中綠色實(shí)線為簡(jiǎn)化后路徑；圖4（c）中紅色實(shí)線為B樣條曲線擬合生成的平滑路徑。

圖4 路徑簡(jiǎn)化與平滑處理效果圖Fig.4 Effect picture of path simplification and smoothing

4 仿真實(shí)驗(yàn)對(duì)比分析

在筆記本電腦上使用Matlab R2018a對(duì)本文所提算法進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。計(jì)算機(jī)為Win10系統(tǒng)，處理器Intel Core i5-4120M，主頻2.6 GHz，運(yùn)行內(nèi)存8 GB。

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

為了驗(yàn)證所提的GBB-RRT*算法具有更好的性能，本文將RRT*、B-RRT*和GBB-RRT*算法在3種二維仿真復(fù)雜環(huán)境下進(jìn)行對(duì)比實(shí)驗(yàn)，地圖大小均為500 m×500 m。由于在復(fù)雜環(huán)境下RRT*找到初始可行路徑需經(jīng)過(guò)大量迭代，時(shí)間成本消耗過(guò)大，為了更好展示這兩類算法在搜索速度上的區(qū)別，下面只描述B-RRT*和GBB-RRT*的對(duì)比實(shí)驗(yàn)。最大采樣點(diǎn)(N)個(gè)數(shù)限制為1萬(wàn)個(gè)。在三種環(huán)境下均進(jìn)行50次重復(fù)實(shí)驗(yàn)。表1列出了兩種算法找到初始可行路徑解的平均擴(kuò)展節(jié)點(diǎn)數(shù)、平均時(shí)間、平均初始路徑和簡(jiǎn)化路徑長(zhǎng)度。

圖5為存在大量隨機(jī)障礙物環(huán)境，圖6為狹窄通道較多的迷宮式復(fù)雜環(huán)境。起點(diǎn)（10，10），終點(diǎn)分別為（490，490）和（460，460）。從圖5和圖6中可以明顯看出，由于B-RRT*采樣隨機(jī)性，隨機(jī)樹(shù)更多地往碰撞概率小的區(qū)域擴(kuò)展，生長(zhǎng)較為發(fā)散。由于GBB-RRT*引入了目標(biāo)偏置策略，使隨機(jī)樹(shù)擴(kuò)展具有一定的方向性，有向目標(biāo)點(diǎn)生長(zhǎng)的趨勢(shì)，有更大的概率對(duì)內(nèi)部通道狹窄區(qū)域進(jìn)行探索，能更快速地通過(guò)通道狹窄區(qū)域，減少對(duì)不必要區(qū)域的探索，搜索時(shí)間更少，路徑長(zhǎng)度更短，規(guī)劃出的初始可行路徑質(zhì)量更高。如表1所示，在第一種大量障礙物環(huán)境下找到初始可行路徑時(shí)，GBB-RRT*算法平均擴(kuò)展節(jié)點(diǎn)和初始路徑長(zhǎng)度比B-RRT*分別減少10.52%和1.58%。GBB-RRT*平均搜索時(shí)間（0.768 7 s）比B-RRT*（1.296 9 s）縮短40.72%。在第二種狹窄通道迷宮環(huán)境下找到初始可行路徑時(shí)，GBB-RRT*平均擴(kuò)展節(jié)點(diǎn)和初始路徑長(zhǎng)度比B-RRT*分別減少14.77%和3.37%。GBB-RRT*平均搜索時(shí)間（0.557 s）比B-RRT*（1.006 4 s）縮短44.65%。

表1 找到初始可行解的實(shí)驗(yàn)結(jié)果Table 1 Experimental results of finding initial feasible solution

圖5 大量障礙物環(huán)境規(guī)劃結(jié)果對(duì)比Fig.5 Comparison of environmental planning results for a large number of obstacles

圖6 狹窄通道迷宮環(huán)境規(guī)劃結(jié)果對(duì)比Fig.6 Comparison of labyrinth environmental planning results for narrow passage

圖7是非常具有挑戰(zhàn)性的單通道迷宮環(huán)境，起點(diǎn)（10，10），終點(diǎn)（390，430）。特點(diǎn)是只有一條可行路徑通道，且通道轉(zhuǎn)折多，對(duì)算法快速通過(guò)性要求較高。由于B-RRT*和GBB-RRT*經(jīng)過(guò)大量迭代后對(duì)配置空間的探索相似，所以擴(kuò)展節(jié)點(diǎn)數(shù)和路徑長(zhǎng)度很接近。不同的是GBB-RRT*算法具有一定方向性，能以更短的時(shí)間逃離迷宮。如表1所示，GBB-RRT*算法平均搜索時(shí)間（4.827 1 s）比B-RRT*（7.533 3 s）縮短35.99%。

圖5～7的圖（d）中藍(lán)色實(shí)線為初始路徑，綠色實(shí)線為簡(jiǎn)化路徑，紅色實(shí)線為光滑路徑。從圖5（d）、圖6（d）和圖7（d）可以看出，算法規(guī)劃出的初始可行路徑較為曲折，采用所設(shè)計(jì)的路徑簡(jiǎn)化方法對(duì)初始路徑進(jìn)行簡(jiǎn)化，然后采用B樣條曲線進(jìn)行平滑處理后，路徑明顯更為簡(jiǎn)短和光滑。尤其對(duì)圖7環(huán)境，初始路徑曲折較多，由表1可知，簡(jiǎn)化前B-RRT*和GBB-RRT*規(guī)劃路徑相差不大，但經(jīng)過(guò)簡(jiǎn)化處理后路徑長(zhǎng)度較B-RRT*縮短4.3%。所以，本文所提路徑簡(jiǎn)化平滑處理方法效果良好，并且是十分必要的。

圖7 單通道迷宮環(huán)境規(guī)劃結(jié)果對(duì)比Fig.7 Comparison of single channel maze environmental planning results

通過(guò)表1可以看出，雖然B-RRT*算法能在較短時(shí)間找到初始可行路徑，但是時(shí)間耗費(fèi)很大，規(guī)劃路徑長(zhǎng)度也更長(zhǎng)。本文所提出的目標(biāo)偏置雙向RRT*（GBBRRT*）算法在3種復(fù)雜環(huán)境下，找到初始可行路徑解所需的擴(kuò)展節(jié)點(diǎn)更少、搜索時(shí)間更短，經(jīng)過(guò)路徑簡(jiǎn)化與平滑處理方法處理后，路徑長(zhǎng)度更短、更光滑。

4.2 對(duì)比分析

圖8為上述實(shí)驗(yàn)在找到初始可行路徑的數(shù)據(jù)對(duì)比柱狀圖，為了反映算法的規(guī)劃效率和規(guī)劃路徑質(zhì)量，從平均擴(kuò)展節(jié)點(diǎn)、平均搜索時(shí)間、平均路徑長(zhǎng)度和簡(jiǎn)化處理后路徑長(zhǎng)度四個(gè)指標(biāo)進(jìn)行評(píng)價(jià)。

從圖8（a）、（b）中可以看出，與B-RRT*算法相比，本文所提GBB-RRT*算法有較少的擴(kuò)展節(jié)點(diǎn)、較短的搜索時(shí)間，特別是搜索時(shí)間大大減少。圖8（c）中綠色為簡(jiǎn)化后路徑，路徑長(zhǎng)度明顯縮短，說(shuō)明了路徑簡(jiǎn)化與光滑處理的必要性和有效性。

圖8 三種環(huán)境實(shí)驗(yàn)數(shù)據(jù)對(duì)比Fig.8 Comparison of experimental data in three environments

5 結(jié)束語(yǔ)

本文針對(duì)較復(fù)雜環(huán)境下采用RRT*算法和B-RRT*算法進(jìn)行移動(dòng)機(jī)器人的路徑規(guī)劃，存在搜索時(shí)間長(zhǎng)、采樣效率低和規(guī)劃路徑曲折的問(wèn)題，提出了目標(biāo)偏置雙向RRT*（GBB-RRT*）算法。將雙向搜索與目標(biāo)偏置思想相結(jié)合，加快隨機(jī)樹(shù)的生長(zhǎng)速度，提高算法搜索效率，大大減少搜索時(shí)間。所設(shè)計(jì)的逆向?qū)?yōu)結(jié)合轉(zhuǎn)角約束路徑簡(jiǎn)化方法，可結(jié)合B樣條曲線生成機(jī)器人可跟蹤執(zhí)行的光滑路徑。通過(guò)在三種仿真環(huán)境下與B-RRT*算法進(jìn)行對(duì)比實(shí)驗(yàn)，結(jié)果表明所提GBB-RRT*算法搜索時(shí)間縮短40%左右、擴(kuò)展節(jié)點(diǎn)減少10%左右，結(jié)合路徑簡(jiǎn)化方法后路徑長(zhǎng)度縮短3%左右。證明了所提算法具有一定的優(yōu)越性和有效性。