本文轉載自頭條號作者半杯茶的小酒杯，百度的研發工程師，文章多是自動駕駛相關，推介給大家。
RRT Planer
快速搜索隨機樹(RRT-Rapidly-ExploringRandom Trees)是一種常見的用於機器人路徑規劃的方法，他的原始算法思想很簡單，以一個初始點作爲根節點，通過隨機採樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。
相對於其它傳統的路徑規劃算法，RRT通過對狀態空間中的採樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和複雜約束的路徑規劃問題。該方法能夠快速有效地搜索高維空間，通過狀態空間的隨機採樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規劃路徑，適合解決多自由度機器人在複雜環境下和動態環境中的路徑規劃。
RRT算法僞代碼
1.初始化起始點。比如設置機器人所在的位置爲初始點；
2.隨機生成目標點，遍歷T，如果通過T能到達目標點，則路徑搜索成功，擴展結束；否則繼續擴展T；
3.挑選隨機點到目標點最近的一個爲Xnear;
4.沿着Xrand到Xnear的方向生長一段距離，生成一個新的節點Xnew;
5.判斷Xnew進行碰撞檢測，如果狀態非法，則本次生長結束；否則，將新的狀態添加到T；
6.返回樹結構。
RRT算法實現-Python版本
Bidirectional RRT Planer
從初始狀態點和目標狀態點同時生長兩棵快速擴展隨機樹來搜索狀態空間，效率會更高。爲此，基於雙向擴展平衡的連結型Bidirectional RRT算法被提出。
Bidirectional RRT算法僞代碼
Bidirectional RRT算法同時從起點和終點建立擴展樹，然後採樣隨機點進行擴展。在每一次迭代中，擴展完第一棵樹的新節點q(new)後，以這個新目標點作爲第二棵樹的擴展方向，如果沒有碰撞，繼續往相同的方向擴展，直到擴展失敗或者′()=()。
在迭代的過程中也同時考慮了兩棵樹的平衡性，即選擇兩棵樹節點少的作爲擴展的目標。Bidirectional RRT的這些屬性使得它相對於原始的RRT算法，搜索效率有了大大提升；其次兩棵樹不斷向着對方交替擴展，這種帶有啓發性的擴展使得樹的擴展使得搜索區域可以逃離各自的約束區域，從而更快的實現算法收斂。