文章目錄

• 優缺點
• 偽代碼
• 具體流程
• 效率問題
• 代碼

優缺點

優缺點先明說，優點RRT Star適用于任何地圖，不像A Star，Dijkstra那樣受限于柵格地圖。
缺點：1.找到的路徑可能不是最優的；2.路徑可能不符合機器人的運動學動力學模型；3.效率問題。

偽代碼

具體流程

給出起點和終點，以及設置好障礙物的地圖，如下所示，將起點記作是根節點。

---

進行空間撒點采樣，在空間中隨機選擇一點Xrand。（這里對應偽代碼當中的Sample()函數）

---

接著尋找距離Xrand最近的一個已知節點Xnear（這一步對應偽代碼當中的near()函數）。因為當前只有一個根節點（起點），所以根節點即為Xnear。

---

“樹的生長”（執行Steer函數）
將Xnear與Xrand兩節點連接起來作為“樹”生長的方向。

---

會設置一個步長作為“樹枝”。
因為步長有長度，可能長度不會恰好等于Xnear到Xrand的距離。
所以稱“樹枝”的末端節點為Xnew。

---

現在，“樹枝”的長度就是從Xnear到Xnew的長度。
將在“樹枝”上的點（Xnear到Xnew這之間無限的點）都歸為節點。
得到Xnew之后，之前的Xrand就舍棄了，只保留了“樹枝”，之后重新進行采樣。

重新采樣

---

重新采樣之后，發現連接的路徑穿過了障礙物。

---

碰到連線穿過障礙物的，就拋棄這一次采樣，重新采樣。

---

發現，隨著采樣的進行，會越來越靠近終點。

---

但是要使算法停止，就必須要隨機采樣點剛好是終點，這樣的概率是非常小的。所以會設置一個提前停止的條件：

因為每一段樹枝的末端都是Xnew，所以每產生一次Xnew節點，我們都判斷一下Xnew與終點之間的距離，看這個距離是否小于步長，如果小于步長且沒有經過障礙物，則就直接把Xnew與終點進行相連。

---

綜上，就能找到一條從起點到終點的路徑。

---

效率問題

存在一個效率問題，如下圖所示，按照每次Xnew之后進行Xnew與終點連線判斷的情況，下圖是可以直接按照這條軌跡到終點的。（因為在這沒有碰到障礙物且只是步長不滿足所給的條件）

這時候，改進一下采樣的范圍，就是直接沿著這個線的周圍進行采樣，限定范圍，就可以加快算法導向終點的速度，這就是我的下篇Blog所要詳解的Informed RRT*算法。

代碼

代碼在我的github以及gitee當中可下載。

總結

以上是生活随笔為你收集整理的RRT算法原理和代码详解（快速扩展随机树）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。