ε-動態(tài)三角采樣和過渡遞歸回溯的RRT算法

馬智煥, 胡立坤*, 陶興華, 劉月洋, 胡正南

(1.廣西大學電氣工程學院, 廣西南寧530004;2.南寧學院智能制造學院, 廣西南寧530299)

0 引言

近年來,芯片制造技術、自動化控制技術的日臻成熟和人工智能概念的興起,機器人的功能越來越強大,使用場景也愈加多元化,同時路徑規(guī)劃是機器人領域內至關重要的研究方向之一。路徑規(guī)劃的需求是在明確了始發(fā)地和目標位置的情況下,同時滿足路徑長度短、規(guī)劃效率高等條件,并且搜索到與障礙物無碰撞的可行解。

常用的規(guī)劃算法包括Dijkstra算法[1]、A*算法[2]、人工勢場算法(artificial potential field, APF)[3]、遺傳算法[4]、人工魚群算法[5]、蟻群算法[6]、概率路線圖算法[7]和快速搜索隨機樹(rapidly exploring random trees, RRT)算法等。其中Dijkstra算法的主要特點是由中心向四周搜索,直到擴展到終點位置為止,由于算法遍歷節(jié)點多,因此容易消耗大量的時間。A*算法采用啟發(fā)式的搜索,提高了全局的搜索效率,卻難以滿足路徑最優(yōu)的要求;APF算法可以獲取平滑安全的路徑,但是容易陷入局部最優(yōu)的困境。遺傳算法可以有效的搜索整個地圖環(huán)境,但是實時性較差。人工魚群算法種群數(shù)與路徑質量成反比。蟻群算法雖然可以找到最優(yōu)路徑,但是收斂速度慢,容易發(fā)生死鎖現(xiàn)象。概率路線圖算法路徑的好壞取決于空間事先設置的采樣點,當采樣數(shù)量不多或者分布不均勻時,很難找到路徑。

文獻[8]提出了基于采樣的RRT算法,因為其結構簡單、路徑搜索效率高、擁有完整的概率完備性等特點,所以被運用在各類路徑規(guī)劃場景中,但是,RRT算法沒有考慮路徑節(jié)點的代價,導致生成的路徑通常是曲折的,無法保證路徑最優(yōu)。RRT*算法[9]為新點在一定范圍之中尋找最佳的父節(jié)點,同時將此范圍內的路徑點與新點重新連接,從而使得算法在采樣點數(shù)量接近無窮大時可以找到最佳路徑,但無法在短時間內找到最優(yōu)解。Informed-RRT*[10]通過將采樣點的生成范圍削減到一個超橢球狀態(tài)子集內來提升算法求解路徑的效率,但在狹窄通道、迷宮等復雜環(huán)境中通過性較差。譚建豪等[11]將啟發(fā)式思想運用到隨機采樣過程中,利用節(jié)點刪除策略對每一個路徑節(jié)點賦予相應的權重,使得算法可以保留權重更大的路徑節(jié)點,提高了算法的尋路性能。文獻[12]通過改變樹的生長策略,利用三角不等式原理減少了路徑的長度,同時在選擇最佳父節(jié)點時考慮相鄰節(jié)點的父節(jié)點,從而提出了收斂速度更快的Quick-RRT*算法。文獻[13]提出一種基于三角不等式改變樹中已存在節(jié)點連接方式,并利用轉角約束增強路徑的平滑度的RRT-Connect算法,在縮短尋找路徑時間的同時,還通過重新選擇祖節(jié)點減少路徑長度。雖然RRT-Connect顯示出良好的搜索環(huán)境的能力,但與RRT算法相似,其路徑質量并不穩(wěn)定,并且不能保證最佳。欒添添等[14]將地圖劃分四級采樣區(qū)域,并利用概率目標偏置改變隨機點的生成方向,同時設置安全距離且利用轉角限制和B樣條曲線擬合平滑路徑。Liao等[15]提出了F-RRT*算法,通過創(chuàng)造一個新節(jié)點使得路徑質量變得更優(yōu)。陳俠等[16]利用人工勢場算法削弱了RRT算法采樣時的隨機性,障礙物產(chǎn)生斥力,而終點產(chǎn)生引力,二者結合為隨機點提供擴展方向;但是由于人工勢場算法計算量較大,而且容易陷入局部陷阱,因此可能使得采樣點陷入局部最優(yōu)。荊澤成等[17]在RRT算法中加入目標偏置來加速算法搜索速度,但是對于復雜的環(huán)境目標偏置反而有可能限制樹的生長。

基于對傳統(tǒng)RRT采樣過程的分析,研究發(fā)現(xiàn)隨機采樣效率低,ε-DT-RRT利用ε-動態(tài)三角采樣區(qū)域劃分采樣空間,通過限制采樣區(qū)域減少無效節(jié)點的數(shù)量和在無價值區(qū)域的重復采樣。除此之外,通過對RRT算法結果的觀察,雖然采樣點是在整個地圖空間隨機生成的;但是很難生成最優(yōu)路徑上的最佳節(jié)點,因此采用創(chuàng)造過渡節(jié)點的方法解決了此問題。在此基礎上,通過遞歸回溯祖節(jié)點進一步去除路徑中冗余點,縮短路徑長度。

1 傳統(tǒng)RRT算法

RRT算法把已知的初始狀態(tài)起點選取為隨機樹的源節(jié)點,在定義的地圖中隨機產(chǎn)生一個采樣點,根據(jù)采樣點生成一個新的子節(jié)點,不斷重復此過程直至子節(jié)點到達終點時擴展終止。最后采取不斷尋找節(jié)點的父節(jié)點的方式即可尋找到一條完整路徑?；ARRT算法主要步驟如下：

步驟1：初始化空間信息,生成隨機樹T。

步驟2：在空間中均勻采樣生成Xran,并搜索最近點Xnea。

步驟3：利用最近點和隨機點,依據(jù)公式(1)合成Xnew。

步驟4：如果Xnew可以直接加入樹中進行步驟5,反之返回步驟2。

步驟5：循環(huán)執(zhí)行以上步驟,直到找到完整路徑。

(1)

從以上步驟可以看出,RRT算法結構簡單,但是也面臨以下問題：首先,隨機點是在整個地圖空間上隨機均勻選取的,地圖環(huán)境越大,隨機采樣點的選擇性越多,生成無用路徑點的數(shù)量與概率也隨之增多,消耗了大量的算法時間;其次,當Xnew與Xnea之間是直接連接,并沒有經(jīng)過優(yōu)化,導致算法生成的路徑曲折;最后,Xnew與Xnea之間存在障礙物時,算法會舍棄Xnew,而二者之間往往會有最優(yōu)路徑點,可能導致算法不能尋找到最優(yōu)路徑。

2 ε-DTSR-RRT算法設計

2.1 ε-動態(tài)三角采樣區(qū)域

傳統(tǒng)RRT算法中,因為在整個地圖中隨機采樣生成Xran,所以會產(chǎn)生大量無用的采樣點。針對此類問題,提出動態(tài)三角采樣區(qū)域的改進措施。首先,在生成Xran前,在樹中尋找距離目標點最近的節(jié)點1,根據(jù)新點1的坐標將采樣區(qū)域分為已探索區(qū)域和未探索區(qū)域。已探索區(qū)域為地圖中小于新點1坐標的區(qū)域,其余區(qū)域則記為未探索區(qū)域。其次,在未探索區(qū)域生成節(jié)點2,由于節(jié)點1和節(jié)點2是動態(tài)變化的,故節(jié)點1、節(jié)點2和終點構成的動態(tài)三角采樣區(qū)域,如圖1所示。最后,引入強化學習中ε-greedy動作選擇策略,隨機生成一個0～1的數(shù)記為rand,當rand小于ε(小于1的正數(shù))時,從整個地圖空間中隨機生成采樣點;反之,則在三角區(qū)域中隨機生成采用點,如公式(2)所示。

圖1 動態(tài)三角采樣區(qū)域Fig.1 Dynamic triangular sampling region

(2)

2.2 創(chuàng)建過渡節(jié)點

針對RRT算法中,當Xnea和Xnew之間存在障礙物時,便會放棄Xnew,重新生成采樣點的問題,如圖2(a)所示,提出了創(chuàng)造過渡節(jié)點的方法。而在這種情況下,Xnew往往處于未探索區(qū)域中,為了加快對地圖空間的搜索,需要聯(lián)通最近點和新點,同時將新點添加到路徑中。以Xnea和Xnew為對角頂點構造矩形[18]。在矩形另外兩端點中任選一個處于可行區(qū)域中的點作為臨時過渡節(jié)點Xt,并利用二分法優(yōu)化調整Xt的位置,如圖 2(b)所示。當優(yōu)化完成時,進一步利用三角不等式遞歸回溯Xt的無碰撞祖節(jié)點Xanc,并將Xanc作為Xt的父節(jié)點、Xt作為Xnew的父節(jié)點插入樹中,最終路徑如圖2(c)所示。由圖2可知,構建的過渡節(jié)點Xt既保留了原算法中被舍棄的Xnew,又增加了算法獲取最佳路徑節(jié)點的機會,并聯(lián)通了新的探索區(qū)域。

(a) 生成Xnew

2.3 遞歸回溯祖節(jié)點

針對RRT算法中頂點利用率低、局部最小值等情況,提出了遞歸回溯祖節(jié)點的方法優(yōu)化路徑。在Xnew作為可行路徑點被添加樹中之前,對最近點進行祖節(jié)點溯源。通過循環(huán)溯源操作直至擇出可以與Xnew直接連接的路徑點記作Xanc,將其父節(jié)點記作Xp,如圖3(a)所示。由于新點和Xp之間存在障礙物,不可直接加入路徑,利用Xanc,Xnew和Xp建立一個連通路徑的過渡節(jié)點Xt。首先,在Xanc與新點之間搭建臨時點;其次,在臨時點和Xp之間生成Xt,如圖3(b)所示。在此基礎上,為了進一步刪除拐點數(shù)量和削減路徑代價,繼續(xù)從樹中搜集Xt可以直接連接的路徑點Xtp,并加入樹中。由圖3(c)所示,經(jīng)過遞歸回溯祖節(jié)點之后的路徑質量明顯優(yōu)于初始路徑。

(a) 回溯祖節(jié)點

2.4 改進連接終點判別方式

傳統(tǒng)RRT算法找到路徑的標準是Xnew與終點的距離小于閾值條件即可,其局限在于不能保證新點是一直沿著終點方向拓展的,并且閾值大小也不好確定,可能會在重復的地點來回搜索,導致路徑出現(xiàn)震蕩。綜合起點與終點的長度并乘以系數(shù),記作THR。如果新點與終點的距離小于THR,啟動終點直連環(huán)節(jié),若能直接連接,說明算法已經(jīng)找到路徑。相對于在地圖中盲目拓展的時間而言,直接連接環(huán)節(jié)消耗的時間可以忽略。

2.5 ε-DTSR-RRT算法實現(xiàn)

ε-DT-RRT算法具體步驟如下：

步驟1：初始化地圖,設置起始點、目標點、步長等參數(shù)。

步驟2：動態(tài)三角采樣區(qū)域中生成隨機點Xran,在已知樹中找到Xnea,并生成新點Xnew。

步驟3：判斷Xnea和Xnew之間是否有障礙物。如果有跳至步驟7,否則進行步驟4。

步驟4：尋找Xnea祖節(jié)點中與Xnew沒有碰撞的節(jié)點Xanc、Xanc父節(jié)點Xp。

步驟5：基于Xp和Xnew之間創(chuàng)造過渡節(jié)點Xt,利用二分法優(yōu)化Xt。

步驟6：尋找Xp祖節(jié)點中可以直接與Xt相連的路徑點Xtp,將相關節(jié)點插入樹中。

步驟7：如果可以建立過渡節(jié)點Xt進行下一步,不能跳至步驟2。

步驟8：利用二分法在障礙物邊緣生成Xt,并在樹中尋找可以直接與Xt相連的路徑點,并插入樹中。

步驟9：判斷Xnew與目標點之間的距離是否大于設置閾值,若大于則返回步驟2,否則,結束程序并輸出路徑。

3 仿真與驗證

為了驗證ε-DT-RRT算法路徑規(guī)劃效果,利用MATLAB軟件仿真對比原算法和RRT類相關算法中所得路徑的指標。算法的運行環(huán)境配置為筆記本電腦,操作系統(tǒng)版本為Windows 10 64位;CPU為i5;主頻為2.5 GHz。RRT算法得到的路徑都存在或多或少的差異,這是因為其具有很強的不確定性。為了減少算法的不確定性帶來的實驗誤差,在不同的空間中運行RRT、Bias-RRT、RRT*、ε-DT-RRT算法各50次,整理實驗結果對比分析。

3.1 地圖1的仿真驗證

地圖1設定為的狹窄環(huán)境地圖(180×180),用于驗證算法狹窄環(huán)境下的路徑指標。設置左上角為坐標原點,起始點坐標為[10,10],目標點坐標為[180,180]。選取RRT、Bias-RRT、RRT*、ε-DT-RRT算法最優(yōu)路徑規(guī)劃結果如圖4所示。虛線為迭代過程,實線段為最終路徑。為了更直觀地對比,將50次實驗所得4種算法數(shù)據(jù)列于表1。

表1 地圖1的4種算法數(shù)據(jù)對比Tab.1 Four algorithm data comparison in Map 1

(a) RRT算法

從表1可以得到,ε-DT-RRT算法在狹窄通道中的采樣次數(shù)相較于RRT算法、Bias-RRT算法分別由原來的1 062.82、1 735.06縮減到225.80,分別減少了78.75%、86.99%。路徑長度參數(shù)雖然與RRT*算法相差不大,但是在時間消耗和拐點數(shù)量方面各自減少了96.23%和58.00%。可以看出在地圖1環(huán)境下,改進算法性能均優(yōu)于對比算法。

結合表1數(shù)據(jù)和路徑規(guī)劃結果,改進算法在快速尋找到最優(yōu)路徑有一定的優(yōu)勢,這是因為所提算法使用了ε-動態(tài)三角采樣區(qū)域一方面限制了采樣點的隨機性,另一方面指出了最優(yōu)路徑點可能存在范圍,減少了生成低價值節(jié)點的時間,進而提升了算法的效率。

3.2 地圖2的仿真驗證

地圖2設置為局部陷阱環(huán)境(500×500),用于驗證算法跳出局部陷阱環(huán)境的能力。設置左上角為坐標原點,起始點坐標為[10,10],目標點坐標為[500,500]。選取RRT、Bias-RRT、RRT*、ε-DT-RRT算法最優(yōu)路徑規(guī)劃結果如圖5所示。虛線為迭代過程,實線段為最終路徑。為了更直觀地對比,將50次實驗所得4種算法數(shù)據(jù)于表2。

表2 地圖2的4種算法數(shù)據(jù)對比Tab.2 Four algorithm data comparison in Map 2

從表2可以看出,ε-DT-RRT算法采取動態(tài)三角區(qū)域采樣的方式加快了算法的尋路速度,原算法需要3.14 s才能找到路徑,而相對較快的Bias-RRT也需要2.09 s,但改進算法僅需要1.76 s,分別提升了43.95%和15.89%。同時從迭代次數(shù)中也可以觀察到ε-DT-RRT算法的有效性,相較對比算法分別減少了51.21%、66.21%、60.46%,而迭代次數(shù)越少,也說明算法效率越高。

地圖2的作用是驗證算法逃離陷阱的能力,由圖5可以看出,RRT算法、Bias-RRT算法、RRT*算法在面對地圖左上角半包圍型障礙物時均有不同程度地陷入其中,導致生成大量的冗余節(jié)點,消耗了算法的大量時間,而改進算法利用動態(tài)區(qū)域采樣迅速逃離局部陷阱,提升了算法的效率。從圖5(d)中,可以看出改進算法所得路徑長度短,這是由于在隨機樹擴展的過程中和遞歸回溯祖節(jié)點生成了過渡節(jié)點,而過渡節(jié)點則有極大的概率是最優(yōu)路徑上的節(jié)點。

3.3 地圖3的仿真驗證

將地圖3設定為復雜環(huán)境地圖(500×500),除地圖環(huán)境設置以外,其余設置同上,4種算法路徑對比如圖6所示。虛線為迭代過程,實線段為最終路徑,將50次實驗所得4種算法數(shù)據(jù)列于表3。

表3 地圖3的4種算法數(shù)據(jù)對比Tab.3 Four algorithm data comparison in Map 3

(a) RRT算法

從表3可知,ε-DT-RRT算法有效地剔除了冗余節(jié)點,相較于RRT算法、Bias-RRT算法及RRT*算法分別減少了72.29%、71.01%、65.70%,同時迭代次數(shù)也收縮了64.91%、77.48%、70.62%。最佳路徑代價和平均路徑代價分別為841.96、888.84 m,比原算法減少了16.76%和26.27%。搜索時間也由原來的2.19 s縮短到1.35 s,減少了38.36%。

地圖3為復雜環(huán)境,其中設計了狹窄通道,局部陷阱等復雜地形。由表3可知,改進算法在復雜地形環(huán)境平均僅用了440次迭代就達到了遠低于具有最優(yōu)性的RRT*算法1 500次迭代的效果。這是因為,ε-動態(tài)三角采樣區(qū)域結合將地圖劃分為最優(yōu)區(qū)域和次優(yōu)區(qū)域,最優(yōu)區(qū)域中的點往往是最優(yōu)節(jié)點,可以快速通過狹窄通道和逃離局部陷阱。從圖6中算法的路徑對比可以看出,改進算法通過在障礙物附近創(chuàng)造過渡節(jié)點,通過連接過渡節(jié)點避開障礙物,使得路徑更短;進一步利用遞歸回溯祖節(jié)點的方式,去除路徑上的冗余節(jié)點,減少了路徑上的轉折點,從而得到平直的路徑,進一步減少移動機器人轉彎時的能耗。

通過在不同地圖上測試分析,ε-DT-RRT算法在所選取的路徑指標中均優(yōu)于基礎RRT、Bias-RRT、和RRT*。在相同的地圖環(huán)境中,改進算法能夠生成更為簡潔的規(guī)劃軌跡,路徑搜索時間更短,路徑迭代效率更高,路徑規(guī)劃效果更為顯著。

3.4 算法的概率完備性

諸多學者已經(jīng)論證過RRT算法是概率完備的,即在任意存在路徑的地圖中,只要RRT算法迭代的次數(shù)足夠多,Xnew總會進入到終點目標范圍內,即找到一條無碰撞的路徑。所提算法在傳統(tǒng)RRT算法上改進,提出ε-動態(tài)三角采樣區(qū)域來限制采樣點的區(qū)域,利用過渡節(jié)點創(chuàng)造無障礙路徑,采用遞歸回溯的方式消除路徑的轉折點,使得算法可以更快地找到路徑。改進算法以ε的概率探索未知的區(qū)域,1-ε的概率探索最優(yōu)區(qū)域,在采樣點足夠多的情況下,采樣點可以遍歷整個地圖,故ε-DTSR-RRT算法依舊是概率完備的。

4 結論

為了解決傳統(tǒng)RRT在路徑規(guī)劃存在的問題,提出了一種ε-DT-RRT算法,通過理論分析和實驗仿真,得出以下結論：

①通過ε-動態(tài)三角區(qū)域劃分地圖采樣空間,限制隨機采樣點的生成位置,從而減少次優(yōu)區(qū)域的搜索次數(shù)和無效節(jié)點的數(shù)量,提升了采樣效率,同時兼顧了采樣點的全局性和最優(yōu)性,加快了算法的收斂速度。

②針對傳統(tǒng)RRT算法在Xnea和Xnew之間存在障礙物時舍棄Xnew的問題,提出了創(chuàng)造過渡節(jié)點的方法。過渡節(jié)點通常是最優(yōu)路徑上的節(jié)點,能夠靠近障礙物并保持安全的距離,提升了獲取最優(yōu)路徑節(jié)點的概率,有效地縮短了路徑長度。

③運用遞歸回溯祖節(jié)點的方法,通過連接新點和祖節(jié)點達到縮短路徑長度和減少冗余點的效果。在此基礎上,為無碰撞祖節(jié)點的父節(jié)點和新點之間創(chuàng)造過渡節(jié)點,能夠進一步改善路徑質量。

從上述的仿真實驗結果來看,在3種不同的地圖中,相比于RRT算法、Bias-RRT算法及RRT*算法,改進算法減少了路徑中節(jié)點的數(shù)量,優(yōu)化了路徑的長度,得到了更短的路徑;同時減少了迭代次數(shù),縮短搜索的時間。通過上述的數(shù)據(jù)對比可以看出,ε-DT-RRT算法極大地提高了算法搜索路徑的速度、質量和效率,證明了其在路徑規(guī)劃方面具有很強的優(yōu)越性。