基于改進雙向RRT算法的機器人路徑規劃

韓豐鍵，邱書波，李慶華*，劉海英

(齊魯工業大學(山東省科學院)a. 電氣工程與自動化學院；b. 電子信息工程學院(大學物理教學部)，山東 濟南 250353)

隨著工業生產對圓盤移動機器人要求的不斷提高，路徑規劃已經成為一個重要研究領域，傳統路徑規劃算法主要有A*算法[1]、蟻群算法[2]、人工勢場算法[3]、遺傳算法[4]等。盡管這些算法在處理低維空間路徑規劃問題具有一定的優越性，但是當空間維度較高時，算法在精確表達構型空間上需要占用大量的計算資源。基于采樣思想的路徑規劃算法，如概率路線圖算法(probabilistic roadmap method,PRM)[5-7]、快速擴展隨機樹算法(rapidly exploring random trees，RRT)[8-10]，不需要精確表達構型空間，而是通過在構型空間內獲取自由構型形成構型圖，以描述構型空間的連通性，這類算法在機械臂、人型機器人等高維構型空間上所體現出的優勢更為明顯。

Lavalle等[9]首次提出了RRT算法，基于隨機采樣的思想獲取自由構型，用以構建一個樹形網絡表達自由構型空間。該算法避免了對整個環境空間建模，在高維構型空間的路徑規劃問題中優勢更為明顯。但是RRT算法采用的隨機采樣思想，也導致了節點的擴展無目標導向性，容易出現大量冗余節點，算法的收斂速度過慢。針對RRT算法生成構型無目標導向性、收斂速度慢、路徑拐點多的缺點，Urmson等[11]提出了路徑代價函數的概念以表征路徑的優化程度，面向目標路徑越優則路徑代價越小。該算法在擴展中，不再選擇距離隨機構型最近的節點，而是選擇k個較近的節點進行擴展，提升了已擴展RRT樹內距離隨機構型較近節點的搜索性能。代價函數的引入使得RRT樹的擴展算法具有較好的目標導向性，可有效提升算法的收斂速度。但是該算法在狹窄空間或障礙物密集等復雜環境下，算法收斂性能會有明顯下降。Rodriguez等[12]提出了B樣條平滑函數，考慮了路徑的曲率連續性和機器人自身的微分約束，但是該算法在步長增加的權重上沒有一個很好的導向性，使得算法收斂速度慢。Guo等[13]提出了一種魯棒動態多目標車輛路徑選擇方法，采用多目標粒子群優化算法，將動態客戶從魯棒虛擬路徑中移除，形成靜態車輛路徑，只有在找不到適合車輛路徑優化的位置時才會觸發動態路徑規劃，避免了耗時的車輛路徑規劃。上述改進算法都是以增加算法的計算成本來求解距離最優的搜索路徑。

在RRT算法研究初期，RRT及相應變種均采用單一隨機樹生成的思想，由初始構型作為隨機擴展樹的初始節點，在環境空間內進行擴展。單隨機擴展算法構造簡單，但是無論是基礎RRT算法還是其改進算法，均存在收斂速度過慢的缺點。

基于雙向搜索的思想，雙向隨機擴展算法(bidirectional RRT，BI-RRT)構造兩棵分別以起始構型和目標構型為初始點的隨機擴展樹，遞歸進行節點擴展以構建可表達構型空間的樹形網絡[14]。相較于RRT算法，BI-RRT算法的收斂速度更快。但是該算法采用RRT算法的隨機節點擴展思想，這導致BI-RRT算法也存在構型無目標導向性的缺點。為提升BI-RRT算法的收斂速度，結合貪婪思想，Kuffner等[10]提出了RRT-Connect算法。在BI-RRT算法擴展過程中，從最近點到隨機點僅步進一個固定步長，即使在無障礙物空間內也需多次擴展過程才可到達隨機點。在RRT-Connect算法的擴展過程中，從最近點到隨機點會持續步進，直至遇到障礙物或到達最近點。貪心思想的應用使得RRT-Connect算法具有更高的擴展效率，在自由構型空間內這一提升更為明顯。但該算法擴展過程是以自由采樣構型隨機點為目標點進行擴展，沒有改變其導向性差的缺點。Akgun等[15]基于啟發式采樣策略，提出了概率優化的RRT*算法，提升了RRT*算法的收斂速度及路徑質量，但是該算法采樣過程中使用的局部偏置思想在復雜環境中的適應性較差，存在導致算法收斂速度變緩慢的可能。張順等[16]提出PRRT-Connected算法,將人工勢場法與RRT-Connected算法結合，優化采樣和擴展策略；同時考慮到無人機的性能約束，對規劃出來的路徑修剪并采用三階貝塞爾曲線對最終路徑平滑，很好地解決復雜環境下無人機路徑規劃問題。這些改進的RRT算法均存在收斂速度緩慢的缺點，沒有考慮到圓盤移動機器人的避障思想。

針對BI-RRT算法研究中存在目標導向性差、收斂速度慢、路徑拐點多的問題，提出了一種改進的BI-RRT算法。該算法是在BI-RRT算法的基礎上，通過搜索兩棵樹的最近節點，利用目標導向思想產生隨機點，加快了路徑收斂速度；同時引入貪婪算法，解決了現在BI-RRT算法的“繞遠路”問題。本文以圓盤移動機器人為模型，還提出了k點碰撞檢測算法，可有效檢測新增節點是否為自由構型。最后，將本文方法與基本BI-RRT算法和目標導向的BI-RRT算法做仿真實驗對比，驗證所提出的改進算法在路徑規劃中的優勢。

1 BI-RRT算法

在BI-RRT中，定義了兩棵隨機樹Ta和Tb，樹Ta以qi為樹的根節點(起始點)開始擴展，樹Tb以qg為目標點開始擴展，p為每次延伸的步長，qr為任意擴展的隨機節點，qn為每次擴展時任選兩棵樹中距離qr最近的節點，以qx為新節點。首先在整個搜索空間中采取隨機的方式生成隨機樹的隨機擴展節點qr，然后遍歷當前已有的隨機樹，從樹中的節點尋找距離qr最近的節點qn，在qn向qr延伸一定步長p之后可以得到新節點qx，之后需要對新節點qx進行碰撞檢測，若qx碰到障礙物便將這個節點舍去，反之，即將qx添加到樹中，可知此時qx的父節點是qn，按照上述方式繼續擴展，直到兩棵樹的qx小于一定的步長閾值時，則可確定Ta和Tb連通，即路徑規劃成功。圖1表示BI-RRT算法隨機樹的生長過程。

圖1 BI-RRT算法隨機樹生長Fig.1 Growth of BI-RRT algorithm random tree

2 改進BI-RRT算法

改進BI-RRT算法傾向于解決目標導向性差和路徑拐點冗余的問題，本文通過目標導向、路徑平滑處理和圓盤k點碰撞檢測來實現對BI-RRT算法的改進。

2.1 目標導向算法

原方案僅采用隨機生成采樣點，以樹中最近點沿當前方向延伸一定步長p得到新的節點，該過程主要的計算任務在碰撞檢測階段。所提出的基于目標導向的方案，改進了BI-RRT算法只生成一個qr確定qx，增加目標導向思想是以隨機點qr和樹Ta的最近節點qn生成擴展方向，定義了樹的最近節點qn和樹Tb的最近節點qn′生成擴展方向，再分別以步長p和kp(k為導向系數)生成qr″和qn″,最后通過平行四邊形法則求新的節點qx。導向系數k的取值不同，會對目標導向算法的收斂性有一定的影響。盡管在目標導向階段增加了計算量，但節點的選擇更具有導向性，使樹的生成更偏向目標點。圖2表示基于目標導向思想生成新節點的過程。

圖2 目標導向生成qxFig.2 qx generated from target orientation

y=[(yr-yn)(xr-xn)]x+b，

(1)

假設qr′的坐標為(xr′,yr′)，步長p表示為：

(2)

(yr-yn)(xr-xn)=(yr-yn)(xr-xn)。

(3)

由式(2)和(3)得到qr′的坐標。

假設qr″的坐標為(xn″,yn″)，步長kp表示為：

(4)

(yn′-yn)(xn′-xn)=(yn″-yn)(xn″-xn) 。

(5)

由式(4)和(5)得到qn″的坐標。

依據qr′和qn″的坐標，qn的坐標，假設qx的坐標為(xa,ya)，依據平行四邊形法則可得：

(6)

針對一次節點擴展，利用目標導向得到新節點qx的計算過程如下：

Step1：給定樹Ta的最近點qn和隨機點qr的坐標，給定樹Tb的最近點qn′的坐標。

Step2：根據給定的步長p和kp求解式(2)、(3)、(4)和(5)得到qr′qn″的坐標。

2.2 路徑平滑算法

加入目標導向算法的BI-RRT樹，能快速向目標節點“生長”，但是由于隨機點的生成有很強的隨機性，路徑會有很多拐點，特別是在障礙物較多的復雜環境中，BI-RRT生成的路徑有很多拐點，如圖3所示。

圖3 多拐點的路徑規劃Fig.3 Path planning of multiple inflection points

為了消除冗余節點，減少圓盤移動機器人在不必要轉向的機械損耗，需要對規劃出來的路徑進行平滑處理，如圖4所示，紅色線表示平滑之后的路徑。通過規劃出來的路徑，對目標點qg嘗試依次連接前面的路徑點，若兩點之間沒有障礙物則將該路徑點刪除，連接上一個節點，直到碰撞發生。如果發生碰撞就將發生碰撞節點的上一個未發生碰撞的節點保存，并且以該點作為父節點再次執行上述操作，直到連接到起始點qi。

圖4 路徑平滑后的路徑規劃Fig.4 Path planning after path smoothing

2.3 圓盤k點碰撞檢測算法

假設圓盤的圓心坐標為(x0,y0)，半徑為r。將圓盤以圓心將周長k等份，設圓上任意一點坐標為(xi,yi)，i=1,2,3,…,k,設該點與圓心的連線和平行于x軸的直線y=y0的夾角為α。圖5表示圓盤k點碰撞檢測算法。

坐標(xi,yi)表示為：

(7)

圖5 圓盤k點碰撞檢測算法Fig.5 Collision detection algorithm for k point of disk

2.4 復雜度分析

單節點擴展總的計算復雜度由目標導向的復雜度和圓盤k點碰撞檢測復雜度兩部分組成。

2.4.1 目標導向的復雜度

由式(3)可以得到

yr′=[(yr-yn)(xr-xn)](xr′-xn)，

(8)

再將yr′帶入到式(2)中得到

其次，要對種子進行處理，在處理的過程中要分為幾個階段。其一，選擇高質量的種子放于55-60℃的溫水中進行攪拌，使溫度降到30℃左右，之后將種子浸泡2 h；其二，種子浸泡后取出風干，風干后將其置于200 mg/kg赤霉素溶液中浸泡24 h后催芽，并用1%的高錳酸鉀溶液浸種30 min，撈出淘干凈，再放入55℃溫水中浸種，用水量為種子的5倍；其三，在用藥水浸泡種子之后，用25℃左右的溫水將種子浸泡8-12 h，用細砂搓去種皮上的黏液，洗凈后攤開晾一晾，準備播種。

p=(xr′-xn)2+[(xr′-xn)(yr-yn)(xr-xn)-yn]2，

(9)

圖6 正、反向生長區Fig.6 Positive and negative growth zones

2.4.2 圓盤k點碰撞檢測復雜度

由式(7)，可見求解(xi,yi)包含二次乘法和二次加法，圓盤k點碰撞檢測算法的復雜度為2k次乘法和2k次加法。圓盤多點碰撞檢測的復雜度為O(n2log2k)。

圖7為BI-RRT和改進的BI-RRT復雜度對比圖與隨機采樣比較，目標導向可以減少無效隨機采樣點生成，隨著擴展節點數量的增長，通過目標導向思想的算法改進得更明顯。

圖7 BI-RRT和改進的BI-RRT復雜度對比圖Fig.7 Comparison of complexity between BI-RRT and improved BI-RRT

3 偽代碼實現

3.1 BI-RRT算法

BI-RRT算法的原理為首先初始狀態被添加到搜索樹，主循環是line3～line24，在n次迭代后終止。顯然，BI-RRT算法通過采樣隨機點，擴展完樹Ta的新節點qx后，以qx作為Tb的擴展方向。按照上述方式繼續擴展，直到兩棵樹的qx小于一定的步長閾值時，則可確定Ta和Tb連通，即整個算法結束。每次迭代中必須考慮兩棵樹的平衡性，即兩棵樹的節點數的多少(也可以考慮兩棵樹總共花費的路徑長度)，交換次序選擇“小”的那棵樹進行擴展。BI-RRT算法的偽代碼見OSID。

3.2 改進BI-RRT算法

3.2.1 目標導向算法

圖8 目標導向生成qxFig.8 qx generated from target orientation

3.2.2 路徑平滑算法

路徑平滑處理的步驟如下：

(1)上一程序周期中，從目標節點qg，追溯到隨機樹的根節點qi，所生成的路徑，形成路徑節點path(q0,q1,…,qn)。其中q0為起始節點qi，qn為目標節點qg。

(2)搜索樹的平滑。在程序下一周期中對已生成的路徑進行貪婪算法的平滑處理，令qt=q0，依次嘗試用q0連接q1,…,qn，直到碰到障礙物，即qt無法連接到第一個節點qi，但qt到qi-1之間是可以直接連接的，則將qi-1存入到路徑緩存區域T0中。

(3)令qt=qi-1，依次嘗試用qt連接qi,qi+1,…,qn,若qt無法連接到節點qm，則將qm-1存入到路徑緩存區域T1中，重復以上的步驟，直到qt=qn。

(4)最后，根據路徑緩存數組中的節點，生成平滑后的路徑。

路徑平滑算法的偽代碼見OSID。

3.2.3 圓盤k點碰撞檢測算法

圓盤k點碰撞檢測算法:改進了單點碰撞檢測函數，提出了一種圓盤k點碰撞檢測算法，具體操作：假設圓盤機器人的圓心為xd，半徑為r，首先將圓盤機器人分割成k等份，本文中取k=50，再對分割出來點的坐標合成一個集合{qi}，i=1,2,3，…，50，對{qi}碰撞檢測，來檢測圓盤上的點是否在障礙物上，當這50個點都不在障礙物里和地圖外時，將qx加入到路徑中。圓盤k點碰撞檢測算法的偽代碼見OSID。

4 實驗與分析

仿真分析平臺由Matlab開發，并在主頻為2.3 GHz的PC上運行。通過分析障礙物數量、迭代次數及導向系數k等參數對方案收斂性的影響，同時本方案還通過分析拐點個數對三種算法進行了比較，驗證了所提的改進BI-RRT算法有較好的優勢。

4.1 障礙物數量

按照障礙物的數量和密度，仿真實驗環境設定在不同的環境(寬闊環境、通道環境、柵格環境、迷宮環境)中的路徑規劃，設置實驗空間尺寸為500 m×500 m，起始點坐標設置成(10,10), 終點坐標設置成(490,490)。考慮到是現實生活，本文取圓盤移動機器人直徑為1 m。分別對BI-RRT算法、目標導向BI-RRT算法和改進的BI-RRT算法在4種不同地圖上進行仿真對比，每種對比實驗進行50次仿真，取均值進行比較。圖9～12分別表示寬闊環境、通道環境、柵格環境、迷宮環境下的路徑規劃圖，綠色線表示BI-RRT算法的規劃路徑，藍色線表示目標導向BI-RRT算法的規劃路徑，紅色線表示對目標導向BI-RRT算法路徑規劃加入平滑后的改進BI-RRT算法。

圖9 寬闊環境下的路徑規劃Fig.9 Path planning in broad environment

圖10 通道環境下的路徑規劃Fig.10 Path planning in channel environment

圖11 柵格環境下的路徑規劃Fig.11 Path planning in raster environment

圖12 迷宮環境下的路徑規劃Fig.12 Path planning in maze environment

4.2 參數分析

4.2.1 迭代次數對三種算法收斂速度的影響

表1分別記錄了4種不同環境下的統計結果。由表1可知，在路徑規劃過程中，改進BI-RRT算法相比BI-RRT算法和目標導向BI-RRT算法平均路徑長度上能夠在很短的時間里尋得路徑。在同樣的環境和參數下，在平均迭代次數上，改進BI-RRT算法與目標導向BI-RRT算法次數差不多，相比BI-RRT算法，在4種環境下，迭代次數平均增加了44.24%。對于平均規劃時間，改進BI-RRT算法相比目標導向BI-RRT算法增加了路徑平滑算法，路徑規劃過程中增加了規劃時間，4種環境時間平均增加了3.38%，相比BI-RRT算法，在平均規劃時間上有著很明顯的優勢，4種環境時間平均縮短了27.82%，加快了收斂速度。

表1 4種環境下的仿真數據比較

4.2.2 拐點個數分析對比

由表2可知，在寬闊環境的路徑規劃過程中，改進BI-RRT算法相比BI-RRT算法和目標導向BI-RRT算法拐點個數分別減少了95.12%和89.47%；在通道環境的路徑規劃過程中，改進BI-RRT算法相比BI-RRT算法和目標導向BI-RRT算法拐點個數分別減少了87.5%和75%；在柵格環境的路徑規劃過程中，改進BI-RRT算法相比BI-RRT算法和目標導向BI-RRT算法拐點個數分別減少了95.12%和90%；在迷宮環境的路徑規劃過程中，改進BI-RRT算法相比BI-RRT算法和目標導向BI-RRT算法拐點個數分別減少了90.90%和83.33%。

表2 4種環境下的拐點個數比較

4.2.3 導向系數k分析對比

不同的導向系數k會對算法的收斂性有一定的影響。為了驗證導向系數對實驗數據的影響，在上述提到的4種環境對不同導向系數k進行50次的仿真實驗，取均值對規劃時間進行比較。由表3可知，在4種環境下，導向系數k=1.0，相比k=0.5，1.5，2.0時，平均規劃時間相對較短。

表3 不同導向系數k對時間的影響

5 結論

本文針對BI-RRT算法路徑規劃中存在目標導向性差、收斂速度緩慢、路徑拐點多的問題提出了改進BI-RRT算法。該算法通過目標導向啟發式思想對隨機樹中隨機點的產生進行改進，并加入了貪婪算法對路徑進行平滑處理，同時和圓盤k點碰撞算法相結合，通過數學模型分析，相比于BI-RRT算法，降低了算法復雜度。在4種不同地圖環境的仿真實驗中，驗證了改進BI-RRT算法在圓盤移動機器人上的優勢。雖然，相比目標導向BI-RRT算法增加了平均規劃時間，但使路徑更加的平滑，提高了整體的路徑規劃。因此本文提出的改進BI-RRT算法適用于多種不同環境，能夠以更少的搜索節點、更快的收斂速度得到路徑，有較大的實用價值。在后續的研究中，將考慮在平均路徑長度上的優化和考慮應用到移動智能車的路徑轉角上進行改進。