RRT路徑規划算法概述

本文主要記錄本人之前調研過在三維復雜環境下的路徑規划算法。

RRT快速隨機搜索樹

快速擴展隨機樹(Rapidly-exploring Random Trees，RRT)算法，是近十幾年得到廣泛發展與應用的基於采樣的運動規划算法，它由美國愛荷華州立大學的Steven M. LaValle教授在1998 年提出。RRT 算法是一種在多維空間中有效率的規划方法。原始的RRT 算法是通過一個初始點作為根節點，通過隨機采樣，增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由樹節點組成的從初始點到目標點的路徑。

RRT是一種通過隨機構建空間填充樹來有效搜索非凸，高維空間的算法。樹是從搜索空間中隨機抽取的樣本逐步構建的，並且本質上傾向於朝向大部分未探測區域生長。由於它可以輕松處理障礙物和差分約束（非完整和動力學）的問題，並被廣泛應用於自主機器人運動規划。

RRT也可以被看作是一種為具有狀態約束的非線性系統生成開環軌跡的技術。一個RRT也可以被認為是一個蒙特卡羅方法。用來將搜索偏向一個配置空間中圖形的最大Voronoi區域。一些變化甚至可以被認為是隨機分形。

改進的RRT算法：

1. 基於概率P的RRT;

2. RRT_Connect’

3. RRT*;

4. Parallel-RRT;

5. Real-time RRT;

6. Dynamic domain RRT;

 

 

基於RRT的運動規划算法綜述

介紹：

在過去的十多年中， 機器人的運動規划問題已經收到了大量的關注，因為機器人開始成為現代工業和日常生活的重要組成部分。最早的運動規划的問題只是考慮如何移動一架鋼琴從一個房間到另一個房間而沒有碰撞任何物體。早期的算法則關注研究一個最完備的運動規划算法（完備性指如果存在這么一條規划的路徑，那么算法一定能夠在有限時間找到它），例如用一個多邊形表示機器人，其他的多邊形表示障礙物體， 然后轉化為一個代數問題去求解。 但是這些算法遇到了計算的復雜性問題，他們有一個指數時間的復雜度。在 1979 年，Reif則證明了鋼琴搬運工問題的運動規划是一個 PSPACE-hard 問題[1]。所以這種完備的規划算法無法應用在實際中。

 

在實際應用中的運動規划算法有胞分法[2]，勢場法[3]，路徑圖法[4]等。這些算法在參
數設置的比較好的時候，可以保證規划的完備性，在復雜環境中也可以保證花費的時間上限。然而，這些算法在實際應用中有許多缺點。 例如在高維空間中這些算法就無法使用， 像胞分法會使得計算量過大。 勢場法會陷入局部極小值，導致規划失敗[5],[6]。

 

基於采樣的運動規划算法是最近十幾年提出的一種算法，並且已經吸引了極大的關注。
概括的講，基於采樣的運動規划算法一般是連接一系列從無障礙的空間中隨機采樣的點，試圖建立一條從初始狀態到目標狀態的路徑。 與最完備的運動規划算法相反，基於采樣的方法通過避免在狀態空間中顯式地構造障礙物來提供大量的計算節省。即使這些算法沒有實現完整性， 但是它們是概率完備， 這意味着規划算法不能返回解的概率隨着樣本的數量趨近無窮而衰減到零[7]，並且這個下降速率是指數型的。

快速擴展隨機樹(Rapidly-exploring Random Trees， RRT)算法，是近十幾年得到廣泛發展
與應用的基於采樣的運動規划算法，它由美國愛荷華州立大學的 Steven M. LaValle教授在1998 年提出， 他一直從事 RRT 算法的改進和應用研究，他的相關工作奠定了 RRT 算法的基礎。 RRT 算法是一種在多維空間中有效率的規划方法。原始的 RRT 算法是通過一個初始點作為根節點，通過隨機采樣，增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由樹節點組成的從初始點到目標點的路徑。

 

快速擴展隨機樹（RRT） 也是一種數據結構和算法，其設計用途是用來有效搜索高維非
凸空間，可應用於路徑規划、虛擬現實等研究。 RRT 是一種基於概率采樣的搜索方法，它采用一種特殊的增量方式進行構造，這種方式能迅速縮短一個隨機狀態點與樹的期望距離。該方法的特點是能夠快速有效的搜索高維空間，通過狀態空間的隨機采樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規划路徑。它通過對狀態空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效的解決高維空間和復雜約束的路徑規划問題。 RRT算法適合解決多自由度機器人在復雜環境下和動態環境中的路徑規划問題[8]。與其他的隨機路徑規划方法相比， RRT 算法更適用於非完整約束和多自由度的系統中[9]。

 

相比於最原始的 RRT 算法的一些缺點，又提出了許多改進的 RRT 算法，例如：

1. 基於概率 P 的 RRT

為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率（0.0 到 1.0 的隨機值 p）來選擇生長方向是目標點還是隨機點。2001 年，LaValle在采樣策略方面引入 RRT GoalBias與 RRT GoalZoom， RRT GoalBias方法中，規划器隨機采樣的同時,以一定概率向最終目標運動；RRTGoalZoom方法中，規划器分別在整個空間和目標點周圍的空間進行采樣[10]。

1. RRT_Connect

RRT_Connect即連接型 RRT， 2000 年由 LaValle教授和日本東京大學的 Kuffner教授聯合提出。該算法一開始同時從初始狀態點和目標狀態點生長兩棵隨機樹，每一次迭代過程中，其中一棵樹進行擴展，嘗試連接另一棵樹的最近節點來擴展新節點。然后，兩棵樹交換次序重復上一迭代過程[10]。這種雙向的 RRT 技術具有良好的搜索特性，相比原始快速擴展隨機樹算法，在搜索速度、搜索效率有了顯著提高。

1. RRT*

 

2010 年， MIT 的 Sertac和 Emilio 證明了在基於采樣的運動規划算法中，隨着 RRT 算法采樣點趨向於無窮，其收斂到最優解的概率為 0，為此他們提出了漸進最優（asymptoticoptimality）的 RRT*算法[11]。該算法在原有 RRT 算法基礎上，改進了父節點選擇的方式，采用代價函數來選取擴展節點鄰域內最小代價的節點為父節點，同時，每次迭代后都會重新連接現有樹上的節點，從而保證計算復雜度和漸進最優解。

 

定義：

位姿空間：

 

運動規划的狀態空間是應用於機器人變換的集合，稱為位姿空間（configuration space），
引入了 C-空間、 C-空間障礙物、自由空間等一系列概念， Latombe在他的著作[12]中對路徑規划的文獻進行了總結統一。對於路徑規划問題，位姿空間的引入是一次划時代的革命，一旦清楚的理解了位姿空間的概念和意義，許多諸如幾何學、運動學等各種以不同形式出現的運動規划問題都可以采用相同的規划算法加以解決，這種層次的抽象是非常重要的。 下面介紹一些概念：

 

 

定義 2.1 位姿（configuration） 機器人一個位姿指的是一組相互獨立的參數集，它能完
全確定機器人上所有的點在工作空間 W 中的位置，這些參數用來完整描述機器人在工作空間 W 中的狀態。一個位姿通常表示為帶有位置和方向參數的一個向量(vector)，用 q 表示。

定義 2.2 自由度（degrees of freedom） 機器人的自由度定義為機器人運動過程中決定其運動狀態的所有獨立參數的數目，即 q 的維數。

定義 2.3 位姿空間（configuration space） 位姿空間是機器人所有可能位姿組成的集合，
代表了機器人所有可能的運動結果，稱為 C-空間，也可簡記為 C。

定義 2.4 距離函數（distance function） C-空間中的距離函數定義為該空間中的一個映射。記為。

障礙物空間和自由空間

假設在工作空間中包含所有的障礙物區域，定義 A 為機器人， A的具體的含義可以理解為從機器人位姿空間到機器人工作空間的一一映射，它將位姿空間中的任意一個點映射成 2 維或者3 維工作空間中機器人各剛體段的位姿狀態。

 

定義 2.5 障礙物空間（obstacle space） 表達式定義了位姿空間當中的障礙物空間。

位姿空間中的無碰撞區域通常稱為自由空間，可用與集合運算定義（表示集合 A 與 B 的差集）

 

 

定義 2.6 自由空間（free space） 表達式定義了位姿空間中的自由空間。

定義 2.7 路徑長度（path cost） 定義pc:為一條路徑的非負長度。表示所有自由空間中的路徑集合。

 

2.3 運動規划的基本定義

用 C-空間的思路，運動規划問題在概念上變得非常簡單：任務是在中尋找一條從起始
位姿到目標位姿的路徑。 運動規划問題的示意圖如圖 2.1 所示,圖中整個水滴表示位姿空間

 

圖1運動規划示意圖

 

有了上述概念， C-空間中的運動規划問題可描述如下：

 

1. 定義一個工作空間 W；

2. 定義 W 中的障礙物區域 O 和機器人 A；

3. 所有可能的機器人位姿構成 C-空間，並且划分為和；

4. 指定機器人初始位姿和目標目標位姿；

5. 可行規划（Feasible planning） 一個完整的算法必須計算一條連續的路徑，使得，或者正確報告這樣的路徑不存在。

6. 最優規划（Optimal planning） 在所有的可行的規划的路徑里面花費代價最少的一條路徑，或者報告這樣的路徑不存在。

算法：

 

在介紹 RRT 算法之前，先說明一下路徑的表示方法。 我們用一個有向圖來表示路徑 G=（V,E）， 那么一條可行的路徑就是一個頂點的序列，，。同時，表示邊。 這樣子問題就變成了使用采樣到的點來擴展圖 G，使之能找到一條從初始節點到達目標節點的路徑。

 

3.1 原始的 RRT 算法

 

算法1：RRT算法主體部分

 

 

While do

 

 

 

算法2：原始RRT算法的Extend函數

 

 

 

If then

 

 

 

這里可以看到兩個算法， 一個是算法的主體部分，還有一個是 RRT 算法的 Extend 函數，主要是如何利用從采樣到點擴展圖 G。下面詳細介紹每一步驟：

 

初始化頂點為，邊集E；

進入while循環，迭代N次停止；

設置了為新的圖G；

Sample(i)采樣一個新的點；

利用新的點擴展圖G.

原始RRT算法Extend函數的步驟：

把V,E暫存

函數表示求圖G 中離歐式距離最近的點;一般情況下會采用來存儲圖中的節點，這樣會節約搜索的時間。

表示存在一個點它將最小化但是，為我們人為設定的一個值， 其實就是往 q 方向步進了一段距離；

 

進行碰撞檢測， 然后判斷這一段路徑，是否與障礙物發生碰撞即判斷路徑是否屬於中；

把加到頂點集中；

把加入到邊集中；

返回擴展后的圖。

從算法中可以看出， RRT 的擴展能夠趨向於位姿空間中沒有擴展到的部分。這就決定了RRT 一開始能夠快速的進行擴展，而且能夠形成對空間的全面覆蓋。 RRT 頂點是分配在位姿空間中是一致均勻的，如果路徑存在，在頂點數目一定的條件下是肯定可以找到一條路徑的。
當然 RRT 算法也有一些缺點，它是一種純粹的隨機搜索算法對環境類型不敏感，當 C 中包含大量障礙物和狹窄通道約束時，算法的收斂速度慢， 效率會大幅下降。 為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率（0.0 到1.0 的隨機值 p）來選擇生長方向是目標點還是隨機點。

 

基於概率P的RRT

算法3：基於概率P的RRT算法的Extend函數

1.

 

 

 

If  then

 

 

Return

算法主要的改變就是在於 Extend 函數時， 增加了一個ChoseTarget函數。這個函數不會全都使用作為擴展的方向，而是一個概率P來選擇進行擴展，以1-P的概率選擇來進行擴展。這樣的好處就會加快了收斂速度， 不過需要選擇好對應的 P 概率。

3.3 RRT_Connect算法

上述的 RRT 每次搜索都只有從初始狀態點生長的快速擴展隨機樹來搜索整個狀態空間，
如果從初始狀態點和目標狀態點同時生長兩棵快速擴展隨機樹來搜索狀態空間，效率會更高。為此， 在 2000 年由 LaValle教授和日本東京大學的 Kuffner教授聯合提出了基於雙向擴展平衡的連結型雙樹RRT 算法，即 RRT_Connect算法。 算法如下所示：

 

算法4：RRT_Connet算法

 

 

 

While do

 

 

 

If   then

 

 

 

;

If  then

 

 

Do

 

If  then

 

 

 

Else break;

While not

If  then return ;

If  then Swap;

該算法與原始 RRT 相比， 在目標點區域建立第二棵樹進行擴展。 每一次迭代中， 開始步驟與原始的 RRT 算法一樣，都是采樣隨機點然后進行擴展。 然后擴展完第一棵樹的新節點后，以這個新的目標點作為第二棵樹擴展的方向。同時第二棵樹擴展的方式略有不同，首先它會擴展第一步得到，如果沒有碰撞，繼續往相同的方向擴展第二步，直到擴展失敗或者表示與第一棵樹相連了， 即 connect 了，整個算法結束。 當然每次迭代中必須考慮兩棵樹的平衡性，即兩棵樹的節點數的多少（也可以考慮兩棵樹總共花費的路徑長度）， 交換次序選擇“小”的那棵樹進行擴展。 這種雙向的 RRT 技術具有良好的搜索特性，比原始 RRT 算法的搜索速度、搜索效率有了顯著提高，被廣泛應用。首先， Connect 算法較之前的算法在擴展的步長上更長，使得樹的生長更快；其次，兩棵樹不斷朝向對方交替擴展，而不是采用隨機擴展的方式，特別當起始位姿和目標位姿處於約束區域時，兩棵樹可以通過朝向對方快速擴展而逃離各自的約束區域。這種帶有啟發性的擴展使得樹的擴展更加貪婪和明確，使得雙樹RRT 算法較之單樹RRT 算法更加有效。

 

3.4 RRT*算法

 

2010 年， MIT 的 Sertac和 Emilio 提出了 RRT*算法。這個算法與上面的算法相比，不但可以找到可行解，還可以找到一條相對次優的算法。 Extend 函數的算法如下：

 

算法5：RRT*算法的Extend

 

 

 

If  then

 

 

 

For all  do

If  then

 

If  then

 

For all  do

If  and

Return

這個算法增加了一些函數， 先來講解下增加的函數的作用。表示這兩個點
直接相連的一條路徑。函數表示從到的路徑花費。函數則返回 q 節點的父親節點。 near 函數是 RRT*里最關鍵的一步， 他會返回一個頂點的集合，表示這些頂點是靠近的。它以為球心的，為半徑的超球體，這些頂點就包括在超球體的內部，一般取，和是常數，表示節點維度的數，表示所有節點的個數[11]。

然后來分析這個算法的步驟： 前幾步采樣擴展步長與之前類似，只是當擴展出新的步長的時候，用 near 函數求鄰近點。遍歷鄰近點集合，尋找花費代價最小的節點，把加入到邊集中。 最后還要進行一步回溯的操作， 即如果存在這么一條路徑經過到達所需的花費少於經過的parent 到達的花費，那么就可以說明原來到的路徑不是最優的， 需要選擇經過到達的路徑作為新路徑。 對所有的鄰近點集合內的點都做相同的操作。 這樣經過 N 次迭代后得到就是接近最優的次優路徑了。

4.結語

RRT 算法是目前流行的基於增量采樣的運動規划算法。 本文介紹了它的原始算法和其他RRT 算法的變種。 為了優化 RRT 算法的收斂速度問題提出概率 P 選擇的 RRT 和 RRT-Connect算法。 還有為止還有仍在研究的基於多樹擴展的 RRT 算法。 針對 RRT 難以得到最優解， 而提出的 RRT*算法可以得到接近最優的次優解。 RRT 算法的優勢簡單易實現，可以運用在高維的運動規划中，並且采樣數量足夠多情況下總可以找到可行解，因為是隨機采樣可以避免陷入局部極小。 當然他也有不足，比如 RRT 算法收斂的速率是未知的， 找樹最鄰近點的效率上，同時還存在一個“長尾”效應比如剛開始擴展的一些點是有用的，后面隨機采樣的點可能完全用不到完全是浪費時間的。 還有許多 RRT 算法的變種本文未介紹，比如利用當下流行的 GPU 並行計算加速 RRT 求解速度的 Parallel-RRT 算法，改進實時性的 real-time RRT，針對動態環境的 dynamic domain RRT 算法等等。 希望以后對 RRT 算法開展更深入的研究。

 

 

 

參考文獻：

https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree

http://planning.cs.uiuc.edu/