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

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

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

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

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

在RRT算法研究初期，RRT及相應(yīng)變種均采用單一隨機樹生成的思想，由初始構(gòu)型作為隨機擴展樹的初始節(jié)點，在環(huán)境空間內(nèi)進行擴展。單隨機擴展算法構(gòu)造簡單，但是無論是基礎(chǔ)RRT算法還是其改進算法，均存在收斂速度過慢的缺點。

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

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

1 BI-RRT算法

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

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

2 改進BI-RRT算法

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

2.1 目標(biāo)導(dǎo)向算法

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

圖2 目標(biāo)導(dǎo)向生成qxFig.2 qx generated from target orientation

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

(1)

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

(2)

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

(3)

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

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

(4)

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

(5)

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

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

(6)

針對一次節(jié)點擴展，利用目標(biāo)導(dǎo)向得到新節(jié)點qx的計算過程如下：

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

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

2.2 路徑平滑算法

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

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

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

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

2.3 圓盤k點碰撞檢測算法

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

坐標(biāo)(xi,yi)表示為：

(7)

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

2.4 復(fù)雜度分析

單節(jié)點擴展總的計算復(fù)雜度由目標(biāo)導(dǎo)向的復(fù)雜度和圓盤k點碰撞檢測復(fù)雜度兩部分組成。

2.4.1 目標(biāo)導(dǎo)向的復(fù)雜度

由式(3)可以得到

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

(8)

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

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

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

(9)

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

2.4.2 圓盤k點碰撞檢測復(fù)雜度

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

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

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

3 偽代碼實現(xiàn)

3.1 BI-RRT算法

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

3.2 改進BI-RRT算法

3.2.1 目標(biāo)導(dǎo)向算法

圖8 目標(biāo)導(dǎo)向生成qxFig.8 qx generated from target orientation

3.2.2 路徑平滑算法

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

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

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

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

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

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

3.2.3 圓盤k點碰撞檢測算法

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

4 實驗與分析

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

4.1 障礙物數(shù)量

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

圖9 寬闊環(huán)境下的路徑規(guī)劃Fig.9 Path planning in broad environment

圖10 通道環(huán)境下的路徑規(guī)劃Fig.10 Path planning in channel environment

圖11 柵格環(huán)境下的路徑規(guī)劃Fig.11 Path planning in raster environment

圖12 迷宮環(huán)境下的路徑規(guī)劃Fig.12 Path planning in maze environment

4.2 參數(shù)分析

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

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

表1 4種環(huán)境下的仿真數(shù)據(jù)比較

4.2.2 拐點個數(shù)分析對比

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

表2 4種環(huán)境下的拐點個數(shù)比較

4.2.3 導(dǎo)向系數(shù)k分析對比

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

表3 不同導(dǎo)向系數(shù)k對時間的影響

5 結(jié)論

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