李曉偉，于會山

（聊城大學物理科學與信息工程學院，聊城252059）

0 引言

在移動機器人的研究領域中，路徑規(guī)劃算法是最為重要而且不可缺少的組成部分，是移動機器人在障礙物環(huán)境下實現自主移動導航的基礎。隨著路徑規(guī)劃算法研究的不斷深入，研究人員已經改善并優(yōu)化了很多傳統的路徑規(guī)劃算法，并不斷提出了很多新的路徑規(guī)劃算法，使得路徑規(guī)劃算法的研究趨勢已經逐步偏向復雜化和智能化[1]。常用的路徑規(guī)劃算法包括A*、人工勢場法、概率路線圖（PRM）算法、蟻群算法、遺傳算法以及諸多啟發(fā)式算法等[2]。這些算法的收斂速度過于緩慢，需要提前對未知的障礙物空間環(huán)境進行建模，當環(huán)境改變時無法在未知的障礙空間中使用。

快速擴展隨機樹算法由Steven M.LaValle 于1998年首次提出來[3]。RRT（快速探索隨機樹）算法能有效解決這些傳統路徑規(guī)劃算法存在的問題。與其他傳統路徑規(guī)劃算法相比，RRT 算法不僅具有快速、高效的特點，而且不需要依賴于環(huán)境建模就能夠有效地解決未知復雜障礙物空間和高維動態(tài)環(huán)境的路徑規(guī)劃問題。RRT 是基于空間中樹木分支的創(chuàng)建，它迭代地對新的狀態(tài)進行采樣，然后將離每個樣本更近的現有節(jié)點指向一個新的樣本，從而形成一個具有分支的樹。即在空間里，從根節(jié)點開始，進行任意采樣，當采樣形成的新點為目標點或新點與目標點的距離小于一定步長時，則形成一條由根節(jié)點至目標點的路徑[4]。

1 RRT算法

在快速擴展隨機樹中，qinit為樹的根節(jié)點（起始點），qgoal為目標點，qrand為任意擴展的隨機節(jié)點，qnear為每次擴展時距離qrand最近的節(jié)點，qnew為新節(jié)點[5]。首先在搜索空間中采用隨機的方式生成隨機樹的隨機擴展節(jié)點qrand，然后遍歷當前已有的隨機樹，從樹中的節(jié)點里尋找距離qrand最近的節(jié)點qnear，在qnear向qrand延伸一定步長之后可以得到新節(jié)點qnew，之后需要對新節(jié)點進行碰撞檢測，若qnew碰到障礙物便把這個節(jié)點舍去；反之若qnew沒有碰到障礙物，且在規(guī)定空間內，即把qnew添加到擴展樹中，可知此時qnew的父節(jié)點為qnear，按照上述方式繼續(xù)擴展，直到qnew和qgoal之間的距離小于一定步長時，便規(guī)劃出了一條由根節(jié)點至目標點的路徑[6]。基本算法如下所示：

基本的RRT 算法是從根節(jié)點開始擴展的RRT 樹搜索規(guī)定空間。若有兩棵隨機樹，分別從根節(jié)點和目標點來搜索狀態(tài)空間，速度會相對較快。因此，提出一種基于雙向擴展的算法，即RRT-connect 算法。

在搜索空間內，定義了兩棵隨機樹。一棵從起始點開始擴展，另一棵從目標點擴展，兩棵隨機樹分別是Ta和Tb，任意選取兩隨機樹Ta和Tb中離qrand最近節(jié)點qnear，再從qnear向qrand擴展得到qnew，直到兩棵樹的qnew小于一定步長，則可確定Ta和Tb連通，即路徑規(guī)劃成功[7]。其算法如下所示：

RRT 算法在采樣時，使用啟發(fā)式采樣，即當隨機樹在擴展時，通過隨機概率p1（0≤p1＜1）確定qrand為qgoal的可能性。當目標點就是采樣點P（qgoal=qrand）=p1，能夠提高RRT 樹對目標點擴展的能力，使隨機樹在生長時更有指向性，樹的生長過程會相對收斂。

隨機樹在擴展新節(jié)點時，基本RRT 中qnew的計算如下：

qnew=qnear＋ρ（qrand-qnear）/||qrand-qnear||

ρ 為隨機樹生長時的步長，（qrand-qnear）表示兩向量單位化，||qrand-qnear|||qrand-qnear||為qrand和qnear的歐氏距離。

當把目標引力思想添加到基本RRT 后，qnew的計算如下：

qnew=qnear＋ρ1（qrand-qnear）/||qrand-qnear||+ρ2（qgoal-qnear）/||qgoal-qnear||

ρ1是隨機樹向任意點方向擴展的步長，ρ2是隨機樹向目標點方向擴展的步長，為qgoal和qnear的歐氏距離。

基本RRT 算法中的新節(jié)點是通過最近節(jié)點向隨機點延伸一個步長得到的，新節(jié)點只朝著隨機點生長，可能會偏離目標點，規(guī)劃出的路徑耗時長，距離遠。當添加目標引力函數后，qnew也會向qgoal方向生長。當ρ1＞ρ2時，新節(jié)點會偏向隨機點方向，這就比較接近基本RRT 算法；當ρ1＜ρ2時，新節(jié)點會偏向于目標點方向，加快隨機樹的收斂性。

2 基于雙向生長改進的RRT算法

雙向RRT 算法嘗試擴展兩棵樹到彼此，相比于基本RRT 提高了算法的收斂速度。不過因基本RRT 在擴展時有一定的隨機性，從而導致了雙向RRT 無指向性，缺乏穩(wěn)定性。針對于雙向生長具有不穩(wěn)定的問題，提出一種雙向生長改進的路徑規(guī)劃算法[8]。該算法將雙向生長和目標引力思想相結合起來。在RRT 雙向生長算法中引入目標吸引函數，目標吸引函數使隨機樹向目標生長。這不僅避免了隨機樹搜索全局空間，很大程度上降低算法的計算復雜度，提高了實時性，改善了路徑的平穩(wěn)性[9]。

其主要思想是兩棵隨機樹Ta和Tb分別從起始點和目標點開始擴展，若Ta從qinit開始生長，通過隨機擴展的方式得到采樣點，之后再結合目標引力思想擴展得到新節(jié)點。Tb從qgoal開始生長，生長時把Ta擴展得到的新節(jié)點當作目標點，采用和Ta相同的方式來擴展。Ta和Tb接下來在生長時都把彼此上一步擴展得到的新節(jié)點當成目標點，來進行下一步生長，直到兩棵樹的距離小于一定步長，即路徑規(guī)劃成功。

圖1 改進RRT構建過程

3 仿真實驗

通過用MATLAB 軟件來驗證文中改進的算法，設置根節(jié)點坐標是（0,0），目標點坐標是（490,490）。規(guī)劃一條由根節(jié)點至目標點的有效路徑。

圖2

表1

圖3

表2

圖4

表3

通過在不同環(huán)境下進行仿真試驗，得到如各表中的實驗數據。從而可以得出，相對于基本RRT 算法來說，Bi-RRT 路徑規(guī)劃用時和路徑長度更優(yōu)些。但本文改進之后的算法相比其余兩種算法路徑更優(yōu)，運行時間更短，接近最優(yōu)路徑。

4 結語

本文介紹了路徑規(guī)劃的原始算法、雙向擴展的方式和加入目標引力思想的雙向擴展算法。本文改進的路徑規(guī)劃算法在擴展時減少了路徑的隨機性，讓隨機樹生長更具有目標性和方向性，但其仍然存在許多不足，例如規(guī)劃出的路徑不平滑，需要進一步進行完善。