基于目標導向的雙向RRT路徑規(guī)劃算法

韓豐鍵 ,邱書波,馮 超 ,曹啟賀,李慶華*

1.齊魯工業(yè)大學(山東省科學院) 電氣工程與自動化學院,濟南 250353 2.齊魯工業(yè)大學(山東省科學院) 電子信息工程學院(大學物理教學部),濟南 250353 3.濟南市人機智能協(xié)同工程實驗室,濟南 250353

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

Lavalle等[9]首次提出了快速擴展隨機樹(rapidly exploring random trees,RRT)算法,基于隨機采樣的思想獲取自由構型qfree,用以構建一個樹形網(wǎng)絡表達自由構型空間。該算法避免了對整個環(huán)境空間建模,在高維構型空間的路徑規(guī)劃問題中優(yōu)勢更為明顯,得到了廣泛的關注。但是RRT算法采用的隨機采樣思想,也導致了節(jié)點的擴展無目標導向性,容易出現(xiàn)大量冗余節(jié)點,算法的收斂速度過慢。針對RRT算法生成構型無目標導向性、收斂速度慢等缺點,Urmson等[11]提出了路徑代價函數(shù)的概念以表征路徑的優(yōu)化程度,面向目標路徑越優(yōu)則路徑代價越小。該算法在擴展中,不再選擇距離隨機構型qfree距離最近的節(jié)點,而是選擇k個較近的節(jié)點進行擴展,提升了已擴展RRT樹內(nèi)距離隨機構型較近節(jié)點的搜索性能。代價函數(shù)的引入使得RRT樹的擴展算法具有較好的目標導向性,可有效提升算法的收斂速度。但是該算法在狹窄空間或障礙物密集等復雜環(huán)境下,算法收斂性能會有明顯下降。

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

基于雙向搜索的思想,Pohi等[12]提出了雙向隨機擴展算法(bidirectional RRT,BI-RRT),構造兩棵分別以起始構型和目標構型為初始點的隨機擴展樹,遞歸進行節(jié)點擴展以構建可表達構型空間的樹形網(wǎng)絡。相較于RRT算法,BI-RRT算法的收斂速度更快。但是該算法采用RRT算法的隨機節(jié)點擴展思想,這導致BI-RRT算法也存在構型無目標導向性的缺點。為提升BI-RRT算法的收斂速度,結合貪心思想,Kuffner等[10]提出了RRT-connect算法。在BI-RRT算法擴展過程中,從qnear到qrand僅步進一個固定步長,即使在無障礙物空間內(nèi)也需多次擴展過程才可到達qrand。在RRT-connect算法的擴展過程中,從qnear到qrand會持續(xù)步進,直至遇到障礙物或到達qrand。貪心思想的應用使得RRT-connect算法具有更高的擴展效率,在自由構型空間內(nèi)這一提升更為明顯。但該算法擴展過程是以自由采樣構型qrand為目標點進行擴展,沒有改變其導向性差的缺點。Akgun等[13]基于目標導向采樣策略,提出了概率優(yōu)化的RRT*算法,提升了RRT*算法的收斂速度及路徑質量,但是該算法采樣過程中使用的局部偏置思想在復雜環(huán)境中的適應性較差,存在導致算法收斂速度變緩慢的可能。李曉偉等[14]將一種目標偏向的思想加入到BI-RRT,避免了隨機樹搜索全局空間,很大程度降低了算法的復雜度,但是該算法采樣過程中選取的目標點是另一棵樹的上一個節(jié)點,目標導向性不明確,存在導致算法收斂速度緩慢的可能。上述所提的RRT算法均存在收斂速度緩慢的缺點,沒有考慮到圓盤移動機器人的碰撞檢測問題。

針對BI-RRT算法研究中存在目標導向性差、收斂速度慢的問題,提出了一種目標導向的BI-RRT算法(goal-oriented BI-RRT,GOBI-RRT)。該算法是在BI-RRT算法的基礎上引入了目標導向的思想。通過搜索兩棵樹的最近節(jié)點,利用目標導向思想產(chǎn)生隨機點,新增節(jié)點具有良好的目標導向性,加快了路徑收斂速度。本文在機器人模型選擇中,以圓盤移動機器人為模型,替代質點模型,可更好的匹配真實機器人構型。為適應這一改進,還提出了適用于圓盤機器人的k點碰撞檢測算法,該碰撞算法可有效檢測新增節(jié)點是否為自由構型。本文算法不同于文獻[14]中的算法,該算法新增節(jié)點是通過搜索一棵樹的最近節(jié)點和另一棵樹的上一節(jié)點來生成,而本文中的GOBI-RRT算法新增節(jié)點是通過搜索兩棵樹的最近節(jié)點來生成。最后,將本文方法與文獻[12]基本BI-RRT算法和文獻[14]中的算法作仿真實驗對比,驗證所提出GOBI-RRT算法在路徑規(guī)劃中的優(yōu)勢。通過實驗可知,所提出的方法可以更加快速地尋得路徑。本文算法的這一優(yōu)勢,可促進圓盤底座移動機器人能夠在較短時間內(nèi)獲取可行路徑,節(jié)約能源消耗。

1 目標導向的雙向快速搜索隨機樹算法

1.1 BI-RRT算法

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

圖1 BI-RRT算法隨機樹生長圖

1.2 GOBI-RRT算法

1.2.1 改進節(jié)點生成方式

基本思想:原方案僅采用隨機生成采樣點,以樹中最近點沿當前方向擴展得到新的節(jié)點,該過程主要的計算任務在碰撞檢測階段。所提出的基于目標導向的方案,盡管在目標導向階段增加了計算量,但節(jié)點的選擇更具有導向性,使樹的生成更偏向目標點。圖2表示基于目標導向思想生成新節(jié)點的過程。

圖2 GOBI-RRT算法生成qnew

改進了BI-RRT算法只生成一個qrand確定qnew,增加目標導向思想是以隨機點qrand和樹Ta的最近節(jié)點qnear生成擴展方向,樹的最近節(jié)點qnear和樹Tb的最近節(jié)點qnear'生成擴展方向,再分別以步長p和kp(k為導向系數(shù))生成qrand'和qnear'',最后通過平行四邊形法則求新的節(jié)點qnew。

y=(yrand-ynear)/(xrand-xnear)*x+b,

(1)

假設qrand'的坐標為(xrand',yrand'),步長p表示為:

(2)

(yrand-ynear)/(xrand-xnear)=

(yrand-ynear)/(xrand-xnear),

(3)

由(2)和(3)得到qrand'的坐標。

假設qnear''的坐標為(xnear'',ynear''),步長kp表示為:

(4)

(ynear'-ynear)/(xnear'-xnear)=

(ynear''-ynear)/(xnear''-xnear),

(5)

由(4)和(5)得到qnear''的坐標。

依據(jù)qrand'和qnear''的坐標,qnear的坐標,假設qnew的坐標為(xnew,ynew),依據(jù)平行四邊形法則可得:

(6)

針對一次節(jié)點擴展,利用目標導向得到新節(jié)點qnew的計算過程如下:

Step1:給定樹Ta的最近點qnear和隨機點qrand的坐標,給定樹Tb的最近點qnear'的坐標。

Step2:根據(jù)給定的步長p和kp求解式(2)、(3)、(4)和(5)得到qrand'、qnear''的坐標。

1.2.2 圓盤k點碰撞檢測算法

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

坐標(xi,yi)表示為:

(7)

圖3 圓盤k點碰撞檢測算法

1.3 復雜度分析

單節(jié)點擴展總的計算復雜度由兩部分組成:

第一部分:目標導向的復雜度

由式(3)可以得到

yrand'=(yrand-ynear)(xrand-xnear)·

(xrand'-xnear),

(8)

再將yrand'帶入到式(2)中得到

p=(xrand'-xnear)2+[(xrand'-xnear)·

(yrand-ynear)(xrand-xnear)-ynear]2。

圖4 正、反向生長區(qū)

第二部分:圓盤k點碰撞檢測復雜度

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

圖5為BI-RRT和GOBI-RRT復雜度對比圖,藍色線為BI-RRT計算復雜度,紅色線為GOBI-RRT計算復雜度,與隨機采樣比較,目標導向可以減少無效隨機采樣點生成,隨著擴展節(jié)點數(shù)量的增長,通過目標導向思想的算法改進的更明顯。

圖5 BI-RRT和GOBI-RRT復雜度對比圖

2 偽代碼實現(xiàn)

2.1 BI-RRT算法

算法1給出了BI-RRT算法的輪廓,首先初始狀態(tài)被添加到搜索樹,主循環(huán)是line3-24,在n次迭代后終止。顯然,BI-RRT算法通過采樣隨機點,擴展完樹Ta的新節(jié)點qnew后,以qnew作為Tb的擴展方向。同時樹Tb首先會擴展第一步得到qnew',如果沒有碰撞,繼續(xù)向著相同的方向擴展第二步,直到擴展失敗或者qnew=qnew'表示與樹Ta相連了,即整個算法結束。每次迭代中必須考慮兩棵樹的平衡性,即兩棵樹的節(jié)點數(shù)的多少(也可以考慮兩棵樹總共花費的路徑長度),交換次序選擇“小”的那棵樹進行擴展,BI-RRT的構造方法如表1所示。

表1 BI-RRT算法

2.2 GOBI-RRT算法

2.2.1 改進節(jié)點生成方式

表2 節(jié)點生成

圖6 目標導向生成qnew

2.2.2 圓盤k點碰撞檢測算法

Checkpath:改進了單點碰撞檢測函數(shù),提出了一種圓盤多點碰撞檢測算法,具體操作:假設圓盤機器人的圓心為xd,首先將圓盤機器人分割成k等份,本文中取k=50,再對分割出來點的坐標合成一個集合{qi},i=1,2,3…50,對{qi}碰撞檢測,來檢測圓盤上的點是否在障礙物上,當這50個點都不在障礙物里和地圖外時,將qnew加入到路徑中。Checkpath構造方法如表3所示。

表3 k點碰撞檢測

3 實驗與分析

仿真實驗是在Windows10,內(nèi)存為16GB的電腦安裝有Mtalab R2019a仿真平臺。仿真實驗環(huán)境設定在不同的環(huán)境(寬闊環(huán)境、通道環(huán)境、柵格環(huán)境、迷宮環(huán)境)中的路徑規(guī)劃,設置實驗空間尺寸為500 m×500 m,起始點坐標設置成(10,10),終點坐標設置成(490,490)?？紤]到是現(xiàn)實生活,本文取圓盤移動機器人直徑為1 m。分別對文獻[12]BI-RRT算法、文獻[14]目標偏向BI-RRT算法和本文GOBI-RRT算法在四種不同地圖上進行仿真對比,每種對比實驗進行50次仿真,取均值進行比較。圖7至圖10,分別表示寬闊環(huán)境、通道環(huán)境、柵格環(huán)境、迷宮環(huán)境下的路徑規(guī)劃圖,圖 a)表示文獻[11],圖 b)表示文獻[14],圖 c)表示本文算法,藍色線為樹Ta的路徑規(guī)劃,紅色線為樹Tb的路徑規(guī)劃,綠色線為樹Ta和樹Tb節(jié)點的相連。

a)BI-RRT b)目標偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目標偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目標偏向BI-RRT c)GOBI-RRT

a)BI-RRT b)目標偏向BI-RRT c)GOBI-RRT

通過在四種不同的環(huán)境下的仿真實驗,可以明顯看出,相比BI-RRT算法和目標偏向BI-RRT算法,GOBI-RRT算法使隨機樹向著目標點生長,提高了收斂速度并減少了大量的節(jié)點。

表4至表7表示分別在寬闊地圖、通道地圖、柵格地圖、迷宮地圖四種環(huán)境下,選取路徑長度、航跡點數(shù)、規(guī)劃時間三方面進行對比并得到的仿真實驗數(shù)據(jù)。上述實驗數(shù)據(jù),均通過Matlab內(nèi)運行仿真算法提取。

表4 寬闊地圖下的仿真數(shù)據(jù)比較

在寬闊地圖上,GOBI-RRT算法相比BI-RRT算法平均軌跡點減少了53.49%、平均規(guī)劃時間減少了14.2%,相比目標偏向BI-RRT算法平均軌跡點減少了5%、平均規(guī)劃時間減少了6.5%。

表5 通道地圖下的仿真數(shù)據(jù)比較

在通道環(huán)境上,GOBI-RRT算法相比BI-RRT算法平均軌跡點減少了50%、平均規(guī)劃時間減少了25.58%,相比目標偏向BI-RRT算法平均軌跡點減少了3.8%、平均規(guī)劃時間減少了4.78%。

表6 柵格地圖下的仿真數(shù)據(jù)比較

在柵格環(huán)境上,GOBI-RRT算法相比BI-RRT算法平均軌跡點減少了51.16%、平均規(guī)劃時間減少了30.71%,相比目標偏向BI-RRT算法平均軌跡點減少了4.5%、平均規(guī)劃時間減少了6.6%。

表7 迷宮地圖下的仿真數(shù)據(jù)比較

在迷宮環(huán)境上,GOBI-RRT算法相比BI-RRT算法平均軌跡點減少了54.54%、平均規(guī)劃時間減少了30.5%,相比目標偏向BI-RRT算法平均軌跡點減少了3.8%、平均規(guī)劃時間減少了10.78%。

由表4至表7可知,GOBI-RRT算法能夠在很短的時間里尋得路徑。在同樣的環(huán)境和參數(shù)下,本文算法的平均軌跡點的個數(shù)約為BI-RRT算法的一半,相比目標偏向BI-RRT算法平均減少了4.28%；在平均規(guī)劃時間上,本文算法相比BI-RRT算法平均減少了25.25%,相比目標偏向BI-RRT算法平均減少了6.64%。GOBI-RRT算法相比目標偏向BI-RRT算法更具有目標導向性,隨著節(jié)點數(shù)量的增加,GOBI-RRT算法相比BI-RRT算法復雜度得到了優(yōu)化。因此本文提出的GOBI-RRT算法適用于多種不同環(huán)境,能夠以更少的搜索節(jié)點、更快的收斂速度得到路徑,有較大的實用價值。

4 結 論

針對BI-RRT算法路徑規(guī)劃中存在目標導向性差、收斂速度緩慢的問題提出了GOBI-RRT算法。該算法通過目標導向思想對隨機樹中隨機點的產(chǎn)生進行改進,同時和圓盤k點碰撞算法相結合,通過數(shù)學模型分析,相比于BI-RRT算法,降低了算法復雜度。在4種不同地圖環(huán)境的仿真實驗中,驗證了GOBI-RRT算法在圓盤移動機器人上的優(yōu)勢。通過與BI-RRT算法和目標偏向BI-RRT算法相比,該算法大幅減少了平均航跡點的個數(shù)和平均規(guī)劃時間,在路徑規(guī)劃中,有較大的實用價值。在后續(xù)的研究中,將考慮在平均路徑長度與路徑平滑上進行持續(xù)優(yōu)化。