基于改進RRT算法的路徑規(guī)劃研究*

韓金利

(山西機電職業(yè)技術(shù)學(xué)院 數(shù)控工程系，山西 長治 046011)

0 引言

路徑規(guī)劃是機器人智能化研究的重要方向之一,多年來,學(xué)者們對路徑規(guī)劃算法進行了很多探索。其中RRT(Rapidly Exploring Random Tree,快速擴展隨機樹)算法具有搜索能力強、算法簡單等特點,但是它也有冗余多等缺點,因此許多學(xué)者對RRT算法進行了優(yōu)化。欒添添等[1]提出了以生成的新節(jié)點與終點的距離來判斷自己所處的采樣區(qū)域,但距離公式計算效率較低。陳娟等[2]提出了候選點集策略,點集中的數(shù)據(jù)隨機性大。趙文龍等[3]提出了在均勻采樣得到的隨機點與目標點的連線上隨機取一個點作為新的隨機點進行樹的擴展,但沒有考慮到在障礙物附近新節(jié)點的生成問題。張恩東[4]提出了一次采樣多次擴展的方式,沒有對采樣點重復(fù)利用。董璐等[5]提出了歷史路徑緩存池概念,沒有對歷史路徑中的節(jié)點進行篩選。

針對以上問題,本文提出了基于改進RRT算法的路徑規(guī)劃。以地圖長、寬進行分區(qū),利用新節(jié)點的坐標值判斷處于何種分區(qū),并將首先進入最新區(qū)域的節(jié)點作為根節(jié)點,向著目標擴展,使得算法隨機性減弱,轉(zhuǎn)折點更少。

1 基本RRT算法

RRT算法的基本思想是:隨機點決定節(jié)點的擴展方向,擴展步長決定擴展速度。對問題定義如下:

假設(shè)C為探索空間,在探索空間中有自由空間qfree與障礙物空間qobs,并且qobs?C,qfree?C;探索空間中的起始點為qstart,qstart∈qfree,目標點為qgoal,qgoal∈qfree,qrand為隨機點。隨機點qrand初始路徑生長過程如圖1所示。

圖1 qrand初始路徑生長過程

2 改進RRT算法

2.1 節(jié)點擴展策略

這里采用進五取二的策略,當(dāng)隨機數(shù)大于閾值時,節(jié)點擴展步長數(shù)為1,當(dāng)小于閾值時,節(jié)點擴展步長數(shù)為5;在擴展過程中,若遇到障礙物,則停止擴展,否則則擴展5步。這里采用前進5步,只將其中的兩個節(jié)點加入隨機樹中,這里取第2步節(jié)點和第5步節(jié)點加入隨機樹中。

2.2 最近點分區(qū)搜索策略

根據(jù)地圖大小及目標點與開始點之間的距離劃分隨機樹擴展區(qū)域,只要一個新節(jié)點首次進入下一區(qū)域,就以進入該區(qū)域的最新點為根節(jié)點,并且在區(qū)域范圍內(nèi)生成隨機樹,將最近點搜索限制在區(qū)域范圍內(nèi),縮短搜索時間,一定程度上加快了可行路徑的生成。

2.3 隨機點分區(qū)采樣

根據(jù)起點與終點位置,將地圖分為3個區(qū)域,用虛線表示分界線,這里將一區(qū)、二區(qū)隨機點采樣區(qū)域指定為整個地圖,三區(qū)采樣空間指定為二區(qū)和三區(qū),既保證了隨機樹一定程度上可反向擴展,又保證了隨機點的選取使得隨機樹可向目標點擴展,最大程度上避免隨機樹剛剛進入新的區(qū)域時容易陷入局部震蕩。隨機采樣分區(qū)擴展示意圖如圖2所示。

圖2 隨機采樣分區(qū)擴展示意圖

2.4 節(jié)點試采樣回退策略

在分區(qū)采樣的過程中,提出了一種在擴展樹生長初期有限進入已探索區(qū)域的有限回退策略,即節(jié)點試采樣回退策略。設(shè)置一個標志位,當(dāng)節(jié)點采樣生成新節(jié)點未通過碰撞檢測1次則加1,當(dāng)采樣點生成新節(jié)點通過檢測,且新節(jié)點又在重復(fù)采樣分區(qū)則標志位減1,直到標志位數(shù)值達到閾值K,則允許重復(fù)采樣分區(qū)新生成的節(jié)點加入到隨機樹。

2.5 逆向剪枝與路徑平滑

改進RRT算法能夠迅速找到初始路徑,但生成了很多冗余節(jié)點,這里對初始路徑的節(jié)點集合進行剪枝操作,逆向剪枝示意圖如圖3所示。圖3中,黑色方塊為障礙物,實線為原始路徑,點劃線為碰撞檢測連線,虛線表示中間省略有很多節(jié)點。

圖3 逆向剪枝示意圖

路徑平滑采用二次貝塞爾曲線進行擬合處理。在路徑中選擇連續(xù)的3個節(jié)點qi-1,qi,qi+1,中間節(jié)點與前后兩個節(jié)點連線上選擇固定距離smooth_dis,并保證該距離不會超過兩條邊的中點。貝塞爾曲線擬合示意圖如圖4所示。其中,p0、p1、p2為擬合曲線上的3個點。

圖4 貝塞爾曲線擬合示意圖

3 實驗與分析

為了驗證本文改進算法的優(yōu)良性能,將本文算法與基本RRT算法和偏置RRT算法進行對比實驗。由于RRT具有隨機性,這里設(shè)置實驗次數(shù)為20次,各種算法指標求平均值,為保證算法驗證的客觀性。三種算法原始路徑如圖5所示,三種算法實驗結(jié)果如表1所示,本文算法路徑優(yōu)化效果及成本變化如圖6所示。

表1 三種算法實驗結(jié)果

圖5 三種算法原始路徑

圖6 本文算法路徑優(yōu)化效果及成本變化

從表1可以看出:三種算法的路徑成本相差不大,在采樣點數(shù)、運行時間、路徑節(jié)點數(shù)等指標方面本文算法優(yōu)勢十分明顯;本文算法相較于基本RRT算法采樣點數(shù)減少了67.65%,相較于偏置RRT算法采樣點數(shù)減少了51.19%;本文算法運行時間相較于基本RRT算法減少了60.34%,相較于偏置RRT算法減少了36.81%;本文算法路徑節(jié)點數(shù)相較于基本RRT算法減少了68.55%,相較于偏置RRT算法減少了52.08%。

由圖6可知:原始路徑平均長度為1 875.13 m,逆向?qū)?yōu)后路徑平均長度為1 424.85 m,平滑后路徑平均長度為1 398.48 m,最終路徑平均長度減少25.42%。由此看出,經(jīng)過擬合處理后,擬合路徑更加適合機器人運行。

4 總結(jié)

本文提出了分區(qū)采樣RRT算法,對探索空間進行分區(qū),僅在所在區(qū)域內(nèi)尋找最近點,提高了最近點的搜索效率;以進入下一區(qū)域的首個節(jié)點為根節(jié)點,開始在區(qū)域中進行探索,最大程度地避免在已采樣區(qū)域進行重復(fù)采樣;最終路徑由各個分區(qū)的搜索樹拼接而成。本文算法尋找可行路徑速度更快、節(jié)點更少、效率更高、導(dǎo)向性更強。