劉奧博，袁 杰

新疆大學 電氣工程學院，烏魯木齊 830047

機器人路徑規(guī)劃可以定義為找到一條從起始點到目標點的無碰撞可行路徑的過程?；诓蓸拥乃惴ㄒ呀洃糜诮鉀Q高維規(guī)劃問題，且計算效率較高。

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

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

1 B-RRT*算法

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

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

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

2 目標偏置雙向RRT*算法

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

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

圖1 目標偏置雙向擴展隨機樹Fig.1 Goal biased bidirectional RRT*

算法4為改進的擴展函數Extend*()，引入目標偏置思想確定采樣點：

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

3 路徑簡化與平滑處理

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

3.1 路徑簡化

逆向尋優(yōu)結合轉角約束路徑簡化方法如圖2所示。

圖2 轉角約束與路徑簡化示意圖Fig.2 Corner constraint and path simplification

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

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

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

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

步驟2逆向尋優(yōu)，確定簡化路徑節(jié)點。簡化路徑的第一個節(jié)點為xgoal，由步驟1確定的節(jié)點x3，其與目標點xgoal之間滿足轉角約束，所以x3為簡化路徑第二個節(jié)點。同時將x3定義為新的目標節(jié)點xgoal。

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

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

3.2 平滑處理

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

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

4 仿真實驗對比分析

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

4.1 仿真實驗

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

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

表1 找到初始可行解的實驗結果Table 1 Experimental results of finding initial feasible solution

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

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

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

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

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

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

4.2 對比分析

圖8為上述實驗在找到初始可行路徑的數據對比柱狀圖，為了反映算法的規(guī)劃效率和規(guī)劃路徑質量，從平均擴展節(jié)點、平均搜索時間、平均路徑長度和簡化處理后路徑長度四個指標進行評價。

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

圖8 三種環(huán)境實驗數據對比Fig.8 Comparison of experimental data in three environments

5 結束語

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