基于改進傳統RRT算法的機器人路徑規劃研究

買智強，辛舟，張雪琪

(蘭州理工大學 機電工程學院，甘肅 蘭州 730050)

0 引言

近年來，機器人技術的快速發展使得許多與機器人相關的研究變得熱門起來，路徑規劃就是其中之一。為了使機器人安全無障礙地從起始點平穩運行到目標點，給機器人規劃一條無障礙路徑就是路徑規劃的主要內容。在此基礎上，隨著國內外學者的不斷研究，逐漸對路徑規劃提出了新的要求，如最短、最優、最快等。為了達到這些更高的要求，不同學者提出了不同的解決方法。

快速擴展隨機樹RRT算法(rapidly-exploring random tree)是其中比較典型的一種算法，由LAVALLE S M于1998年提出[1]。這是一種典型的樹狀結構搜索算法，以初始狀態作為根節點，通過在狀態空間中進行隨機采樣，不斷擴展，逐步覆蓋狀態空間的自由區域，并最終覆蓋整個狀態空間，從而獲得一條從初始狀態到目標狀態的路徑。RRT算法結構簡單，不需要對障礙物進行精確的建模，不僅適用于二維空間，三維空間同樣可以[2]。其缺點在于得到的路徑并非最優且由于是隨機規劃，效率也并不高。FRAZZOLI E等針對RRT算法規劃出的路徑不是最優解，提出RRT*算法，通過對新節點至根節點的權值比較，找出最優解[3]，也有學者提出過刪除低性能節點規劃高緯機器人路徑[4]，或者是將局部優化引入，進而提高效率[5]。雖然在一定程度上提高了路徑的平滑性，但卻使得效率大大降低。LAVALLE S M等則提出雙向隨機樹(bidirectional RRT，bi-RRT)用來規劃高緯度路徑[6]。bi-RRT采用起始點和目標點分別生成一顆自己的隨機樹，直到二者相遇即得路徑。這種方法雖然縮短了時間，但卻未能得到最優路徑。

近年來，隨著智能算法的興起，結合強化學習，CABALLERO F[7]等提出基于RRT*擴展的快速學習隨機樹法(rapidly-exploring learning trees，RLT*)。對于RRT算法的隨機性，很多人選擇引入人工勢場法來進行改善，且取得了不錯的效果[8-9]。但對于密集障礙物附近的路徑仍不能做到動態調整以達到最優。本文則基于人工勢場引入障礙物對目標點的斥力函數，動態調整RRT算法在經過障礙物附近時的步長，以此來改善路徑。

1 算法描述

RRT算法基本原理如下：首先掃描全局地圖，獲取起始點Po、目標點Pg及障礙物等相關信息，然后以Po為樹的根節點，在地圖內隨機選取一個點Prand，比較隨機點Prand與每一個樹節點(包括根節點)的距離，選擇距離最短的那個。此時最短距離所對應的那個樹節點作為該隨機點Prand的父節點Pnear，沿Pnear到Prand方向以步長q為間隔選取一個新點Pnew，判斷Pnear到Pnew是否經過障礙物。若是則重新選取隨機點，再比較再判斷；若未經過障礙物，則將Pnew加入到樹節點，作為新的樹節點。不斷重復這個過程，直到找到目標點Pg，或者達到最大迭代次數[10]。原理圖如圖1所示。

圖1 原理圖

從圖1中可以看到，傳統RRT算法在規劃路徑時擴展步長是不變的，這樣在一定程度上大大簡化了算法，但其規劃出的路徑效果并不好。步長大小完全取決于使用者，長步長一般適用于那些路徑長且障礙物情況簡單的場合，短步長一般適用于路徑較短但路況復雜，需要躲避的障礙物多的情況，但實際應用中碰到的情況往往是以上二者混合的。對于一段長且復雜的路徑，既希望能以長步長迅速接近目標，又希望在經過復雜障礙物的時候步長能變短一些，好讓得出的軌跡在障礙物附近相對平穩。為解決這個問題，結合人工勢場中有關于障礙物斥力場及斥力函數的想法，本文提出在傳統RRT算法中引入斥力場及斥力函數。

人工勢場法是一種虛擬力法，多用于機器人路徑規劃，其基本原理是在障礙物和目標點附近構建勢力場，通過對機器人位置的勢場強度求負梯度后得到勢場力。其中，目標點產生引力，障礙物產生斥力，二者的合力方向為機器人運動方向[11]。本文不涉及引力勢及引力函數，對其不做介紹。目前常用的斥力函數如下：

(1)

式中：Ur(X)表示在X點處的斥力場；η是斥力度因子；ρ0是障礙物的影響范圍；X0表示障礙物位置；ρ(X,X0)表示位置點X與障礙物之間的最短距離。當距離>ρ0時，機器人的運動不受障礙物的影響。斥力場產生的斥力為斥力場的負梯度，其表達式為

Fr=-?[Ur(X)]=

(2)

式中：Fr表示斥力；?ρ(X,X0)表示障礙物位置指向X位置的單位向量。

當機器人距離障礙物較遠時，采用長步長迅速靠近以減少時間，一旦進入障礙物影響范圍ρ0，斥力函數啟動，減小步長。具體變化如下：

(3)

式中：S為改進后的步長；k為步長系數；|Fr|為斥力Fr取標量。當ρ≥ρ0，即未進入障礙物影響范圍時，步長固定為q；當ρ<ρ0機器人進入障礙物影響范圍時，動態步長啟動，距離障礙物越近，斥力Fr越大，步長S越小。

2 仿真結果及分析

1)Matlab仿真平臺

圖2、圖3分別是是由Matlab搭建的兩張地圖。地圖規模均是[500，500]，方框即為地圖邊界，黑色區域代表障礙物，路徑規劃的起始點為[1，1]，目標點為[490，490]，固定步長q=30，障礙物影響范圍ρ0取50。

圖2 仿真地圖1

圖3 仿真地圖2

2)傳統RRT算法與改進算法

傳統RRT算法路徑規劃結果如圖4、圖5所示。圖中，與目標點相連的黑色細線表示最終路徑。為了便于觀察，程序設定在每一次打點后都停頓1 s，最終圖4用時40.230 s，圖5用時37.560 s。觀察圖中傳統RRT算法所得路徑，可以明顯看出，在路徑經過障礙物附近時平滑性大大降低，尤其是在地圖2這種障礙物空間復雜的情況下，路徑點轉折更大。

圖4 地圖1規劃結果

圖5 地圖2規劃結果

改進后的RRT算法規劃路徑如圖6、圖7所示。為了獲取更加準確的距離信息，包絡障礙物邊緣，加入動態步長后，路徑點在障礙物附近的路徑平滑程度大大提高。地圖1用時48.532 s，地圖2用時52.548 s。從時間上看，圖6相比于圖4，改進后算法多用時25.58%，圖7相比于圖5多用時22.4%。

圖6 地圖1規劃結果

圖7 地圖2規劃結果

3)曲線擬合比較

由于RRT算法得出的軌跡是一條折線段，機器人在經過拐角時會出現運動不平穩的現象。為了解決這個問題，選擇對路徑進行光滑處理。貝塞爾曲線是于1962年由法國工程師皮埃爾·貝塞爾(Pierre Bézier)所發表，用貝塞爾曲線來為汽車的主體進行設計。后經人不斷改良[12]，廣泛應用于二維平面。本文采用該方法進行路徑曲線的擬合。圖8、圖9為經過光滑處理后的最終路徑。顯然，改進后的RRT算法得到的路徑在經過障礙物附近時更為光滑。

圖8 RRT算法擬合

圖9 改進RRT算法擬合

3 結語

本文完成了對傳統RRT算法的改進，基于人工勢場法中的斥力場及斥力函數，加入了動態步長，對RRT算法經過障礙物附近時的路徑進行優化，再對得到的路徑進行貝塞爾曲線擬合，使得改進后的算法在經過障礙物附近時路徑的平滑程度大大提高，缺點是在一定程度上延長了規劃時間。后續的研究將會更加注重規劃時間的縮短。