RRT算法和RRT*算法是一種基於隨機采樣的路徑規划算法，其中RRT*是眾多RRT變種中比較出名的算法，RRT*解決了RRT無法得出最優路徑的問題，只要RRT*算法迭代的次數足夠多，就一定能找出最優的路徑，但是隨之而來的就是規划需要的時間變長。筆者在做本科畢設的時候在為SLAM移動機器人規划路徑時用的就是OMPL中的RRT*，如果要得到最佳的路徑的話，時間得幾秒，但是如果想降低迭代的時間，得出的路徑就不是最優的，如起始點和目標點沒有障礙物但是規划的路徑仍然不是一條直線。但是無論如何，RRT都是一種有效的機器人路徑規划算法。
Ⅰ.RRT Algorithm
RRT全名叫做 Rapidly-exploring random tree，
1. 起始時樹的根節點就是起始點，然后在圖的有效空間內隨機采樣，采樣點可能落在搜索空間內的任一個位置。
2. 一旦一個隨機點被選定，算法就會嘗試將該隨機點和離RRT樹最近的一個元素連接，如果該連接在限制條件以內，如連接長度小於設定的步長且兩點之間無障礙物，則將該隨機點納入RRT樹，成為其一個新的元素，該元素的父節點為離它最近的那個點；如果該點到樹中某點的最近的距離比設定的步長要大，則可以選擇以兩點相連的直線為方向，以設定的最大步長為長度，重新選擇這個直線的端點作為樹的新元素，而不再選用生成的隨機點。
3. 迭代的過程中會對每個點判斷點和目標點是否能夠直接相連，如果直接相連，則可以提前退出搜索。
偽代碼如下：
1 Algorithm BuildRRT 2 Input: Initial configuration qinit, number of vertices in RRT K, incremental distance Δq) 3 Output: RRT graph G 4 5 G.init(qinit) 6 for k = 1 to K do 7 qrand ← RAND_CONF() 8 qnear ← NEAREST_VERTEX(qrand, G) 9 qnew ← NEW_CONF(qnear, qrand, Δq) 10 G.add_vertex(qnew) 11 G.add_edge(qnear, qnew) 12 return G 13 "←" denotes assignment. For instance, "largest ← item" means that the value of largest changes to the value of item. 14 "return" terminates the algorithm and outputs the following value.
Ⅱ. RRT* Algorithm
先來看一段RRT*的偽代碼：
RRT*大體步驟和RRT相同，只不過多了一個1.Connect along a minimum-cost和一個2.Rewire the tree 的步驟，就是這兩個步驟使得RRT*可以得到最優的路徑，
1. 步驟1的就是為隨機選中的點重新選擇父節點，使得該點到起點的cost能夠最小，至於cost的定義，就是路徑的長度，看具體情況選什么類型，其中父節點的選擇可以是該節點附近相連的所有點
2. 步驟2就是在重新選完父節點后，為該節點附近區域的所有樹中的節點重新布線，即rewire，布線的原則是使所有節點到起始點的cost最小
經過以上兩個步驟，隨着迭代次數的增加，就一定能夠找到最佳的路徑。
在知乎上看到一個很好描述上述兩個過程的圖，分享如下：原文鏈接：https://zhuanlan.zhihu.com/p/51087819
Reference:
[1] https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree#cite_note-incremental-8