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

韓金利

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

0 引言

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

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

1 基本RRT算法

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

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

圖1 qrand初始路徑生長過程

2 改進(jìn)RRT算法

2.1 節(jié)點(diǎn)擴(kuò)展策略

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

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

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

2.3 隨機(jī)點(diǎn)分區(qū)采樣

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

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

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

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

2.5 逆向剪枝與路徑平滑

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

圖3 逆向剪枝示意圖

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

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

3 實(shí)驗(yàn)與分析

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

表1 三種算法實(shí)驗(yàn)結(jié)果

圖5 三種算法原始路徑

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

從表1可以看出:三種算法的路徑成本相差不大,在采樣點(diǎn)數(shù)、運(yùn)行時(shí)間、路徑節(jié)點(diǎn)數(shù)等指標(biāo)方面本文算法優(yōu)勢十分明顯;本文算法相較于基本RRT算法采樣點(diǎn)數(shù)減少了67.65%,相較于偏置RRT算法采樣點(diǎn)數(shù)減少了51.19%;本文算法運(yùn)行時(shí)間相較于基本RRT算法減少了60.34%,相較于偏置RRT算法減少了36.81%;本文算法路徑節(jié)點(diǎn)數(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)過擬合處理后,擬合路徑更加適合機(jī)器人運(yùn)行。

4 總結(jié)

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