6.1原理 Dynamic RRT和Extended RRT一樣，也是用來解決動態(tài)路徑規(guī)劃問題，它們的思想有一點是共通的，那就是不要完全放棄初始RRT生成的樹或初始路徑的信息，而是在此基礎上重新規(guī)劃。Dynamic RRT和Extended RRT的區(qū)別在于，Extended RRT利用的是RRT生成的初始路徑的信息，而Dynamic RRT利用的是RRT生成的初始RRT樹的信息。思路如下： 在老地圖中，用RRT算法生成了一個RRT樹，在新地圖中，原始RRT樹的節(jié)點信息（坐標、父節(jié)點）存儲在一個節(jié)點集合中。在新地圖中，先檢測新地圖中比老地圖多出的障礙物，然后，以碰撞檢測為評判根據，刪除老節(jié)點集合中與新障礙物無法通過碰撞檢測的節(jié)點和邊。的到一顆修建過后的與新地形無碰撞的修剪后RRT樹，然后再在這顆修剪后的額RRT樹的基礎上，繼續(xù)生長這棵樹，直到這棵樹連接起點和終點，然后回溯路徑，得出新路徑。①從從初始配置到目標配置生成的 RRT 開始（圖（a））。 ②當配置空間發(fā)生變化時（例如通過接收新信息），將RRT中因這些變化而失效的所有部分標記為無效（圖（b）和（c））。 ③然后我們修剪樹以去除所有這些無效部分（圖（d））。 ④此時，保證樹中剩余的所有節(jié)點和邊都是有效的，但樹可能不再達到目標。最后，我們把樹長出來，直到再次達到目標6.2 偽碼6.3 程序示例加入新的障礙物后，被該障礙物折斷的剩余的圖：在原來的樹的基礎上，繼續(xù)生長后的圖：6.4 參考Replanning with RRTs
5.1 原理 在現實世界的場景中，通常會出現這樣的情況:有關環(huán)境的初始可用信息是不完整的，或者環(huán)境本身是動態(tài)的。在這些情況下，當接收到新信息時，初始解決方案可能會失效，例如通過機載傳感器。當這種情況發(fā)生時，通常會放棄當前的 RRT，并從零開始生長新的 RRT。這可能是一項非常耗時的操作，尤其是在規(guī)劃問題很復雜的情況下。另一方面，在確定性規(guī)劃界存在重規(guī)劃算法，當這種變化發(fā)生時，它們能夠有效地修復之前的解決方案，而不需要從頭重新規(guī)劃。這就是通過連續(xù)域規(guī)劃路徑的問題，如果每次更新地圖，都用RRT重新規(guī)劃，效率相當低下。Extended_RRT則專門用于解決這種動態(tài)路徑規(guī)劃問題。 Extended_RRT的思路是這樣的：在老地圖中，由RRT算法得出的路徑，在障礙物動態(tài)變化不太大的前提下，在新地圖中，大概率也是能通過的，就算障礙物變化很大，之前的路徑也或多或少包含了當前障礙物區(qū)域的信息，所以，在新障礙物區(qū)域中，在重新規(guī)劃路徑時，原有的初始RRT路徑的信息可以利用上，而沒有必要完完全全重零開始用RRT來規(guī)劃。 那么如何利用上初始RRT路徑的信息呢？思路如下： 在老地圖上先用RRT算法的處一條初始路徑，將這條初始路徑上的節(jié)點存儲在集合waypoints中，當環(huán)境更新后，希望在新地圖上得出一條路徑，在隨機撒點的步驟，新的隨機點有概率p落在目標節(jié)點處，此外，還有r的概率落在waypoints的節(jié)點中，剩余1-p-r的概率在目標區(qū)域內隨即撒點。這樣在重規(guī)劃時，就可以把初始路徑的信息利用進來。完成隨機點選取后，剩余的碰撞檢測、樹的生長、路徑回溯步驟與RRT一致。總結：Extended_RRT適用于需要反復路徑重規(guī)劃的場景中，效率比直接重新進行RRT要高得多，和RRT的主要區(qū)別在于，在選取新的隨機點時，利用上了初始路徑的信息，而不是完全隨機撒點。5.2偽碼5.3 程序示例5.4 參考1、Real-Time Randomized Path Planning for Robot Navigation* 2、Extended RRT algorithm with dynamic N-dimensional cuboid domains3、Replanning with RRTs
原理相比于最原始的 RRT 算法的一些缺點，提出的一種改進的 RRT 算法 為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率（0.0 到 1.0 的隨機值 p）來選擇生長方向是目標點還是隨機點。2001 年，LaValle在采樣策略方面引入 RRT GoalBias 與 RRT GoalZoom，RRT GoalBias 方法中，規(guī)劃器隨機采樣的同時,以一定概率向最終目標運動；RRTGoalZoom 方法中，規(guī)劃器分別在整個空間和目標點周圍的空間進行采樣。 和普通RRT的區(qū)別僅在于隨機撒點的時候有區(qū)別，這個p越大，算法越快，但對于復雜地形，可能會陷入局部極小處，反而變慢。一般取p=0.1程序示例參考Rapidly-Exploring Random Trees: A New Tool for Path Planning
2.1 原理雙樹RRT是在原本RRT的基礎上多加了?顆隨機探索樹，各自從起點和終點向外探索拓展，直到兩棵樹相遇時規(guī)劃算法收斂。這種改進過的探索策略可以??提?RRT的運?效率。 雙樹RRT中存在兩顆隨機樹，我們將其命名為A和B，A以起點為根節(jié)點，B以終點為根節(jié)點。兩顆隨機樹的拓展方式和單樹RRT的別無二致，同樣都需要經歷隨機采樣+步?限制+碰撞檢測這三個步驟，但是不同的地?在于雙樹RRT的隨機樹是交替生長的，??說第?輪迭代中A樹向外??，第?輪便切換為B樹??，如此循環(huán)。 在每輪迭代中，隨機樹除了向外拓展之外，還會多出?個步驟，就是遍歷另一顆隨機樹中的所有節(jié)點，找出離NewNode最近的節(jié)點，用于判斷兩顆隨機樹是否相遇。 假設算法經歷了N次迭代以后，已經拓展出如下圖所示的兩顆隨機樹。并且在下?輪迭代中，輪到A樹進?拓展，A樹在圖中?綠?線條表示，B樹?黃色線條表示。當進?本輪迭代后，算法成功拓展出A樹的新節(jié)點NewNode_A，此時算法將遍歷B樹中的所有節(jié)點，找出B樹中離NewNode_A最近的節(jié)點ClosestNode_B。并判斷?者是否滿?步?限制以及是否可以通過步?檢測。如下圖所示，這種情況明顯?法通過碰撞檢測。那么A樹和B樹在這?輪迭代中?法相遇，需要接著下?輪迭代。進?下?輪迭代，這次便切換為B樹進?拓展，假設算法拓展出的NewNode_B以及遍歷A樹后得到的ClosestNode_A如下圖所示，經過判斷發(fā)現?者滿?步?限制并且通過了碰撞檢測，那么這時A樹和B樹就成功得相遇了，規(guī)劃算法收斂當算法收斂以后，只需在兩棵樹的相遇處分別沿著?節(jié)點回溯便可以找出從起點到終點的有效路徑。注意：雙向RRT和RRT的區(qū)別不僅僅是在于雙向生長，雙向RRT比RRT更“貪心”，相比于RRT在生長RRT樹的時候，是每產生一個隨機點，如果能通過碰撞檢測，就往該隨機點的方向生長一次，然后該隨機點就被廢棄了，下一步想繼續(xù)生長RRT樹的話，就只能繼續(xù)生成新的隨機點，每個隨機點最多利用一次。而雙向RRT在生長RRT樹時，先先生成一個隨機點，然后，該樹往該隨機點的方向生長，直到碰到障礙物或則生長到該隨機點，這樣，一個隨機點就被多次利用，加快了速度。2.2 偽碼2.3 程序示例2.4 收斂性分析雙向RRT的收斂性分析可以應用RRT的收斂性分析2.5 參考1、RRT-Connect: An Efficient Approach to Single-Query Path Planning2、https://www.guyuehome.com/9405
1.1 RRT算法思路我們有兩個節(jié)點，一個綠色的起點，一個黃色的終點對于RRT，我們做的第一件事就是將起點設置為隨機樹的根，那么我們就擁有了一顆只有根節(jié)點的樹這棵樹光禿禿的，只有根節(jié)點的話不但難看，而且還沒用。那么我們這時候就需要從這個根節(jié)點出發(fā)，向外拓展出新的葉?。拓展的方式很簡單，就是隨機采樣+步?限制+碰撞檢測。 RRT在每輪迭代中會?成?個隨機采樣點NewNode，如果NewNode位于自由區(qū)域，那么我們就可以遍歷隨機樹中已有的全部節(jié)點，找出距離NewNode最近的節(jié)點ClosestNode。利?距離函數dist(NewNode, ClosestNode)得到二者之間的距離，如果滿足步長限制的話，我們將接著對這兩個節(jié)點進?碰撞檢測，如果不滿足步長限制的話，我們需要沿著NewNode和ClosestNode的連線?向，找出?個符合步長限制的中間點，用來替代NewNode。最后如果NewNode和ClosestNode通過了碰撞檢測，就意味著二者之間存在邊（edge），我們便可以將NewNode添加進隨機樹中。首先以第一輪迭代為例，因為剛開始我們的隨機樹中只有根節(jié)點，所以?論NewNode位于何處，遍歷出的最近節(jié)點ClosestNode必然是根節(jié)點。 假設我們遇到下圖這種情況，雖然采樣點NewNode位于步?限制之內，但是卻很不巧沒有落在自由區(qū)域，即采樣點落在障礙物的位置時，這個采樣點會被算法舍棄。假設我們的步?限制為R，也就是說對于每個ClosestNode節(jié)點來說，只有當NewNode落在其半徑為R的圓的范圍內時，這個隨機采樣點NewNode才有可能被直接采納。如下圖所示，該紅?隨機采樣點雖然位于?由區(qū)域，但是明顯在根節(jié)點的步?限制之外。不過這個節(jié)點并不會被簡單粗暴地舍棄。?是會沿著ClosestNode和NewNode的連線，找出符合步?限制的中間點，將這個中間點作為新的采樣點。如下圖所示，藍點就可以替代紅點作為新的采樣點。那么假設我們已經通過第?輪迭代拓展出第?個葉?節(jié)點A，毫?疑問地A的?節(jié)點就是根節(jié)點，假設我們第?輪迭代的隨機采樣點NewNode為圖中的點B，B落在A的步?限制范圍內，但是A，B之間由于障礙物的阻擋，?法通過碰撞檢測，于是B就會被算法舍棄。假設我們的隨機采樣點是下圖中的B’，明顯B’位于?由區(qū)域，滿?步?條件，并且可以通過與點A的碰撞檢測，那么我們就在B’和A之間添加?條邊，并且將A設置為B’的?節(jié)點。學過數據結構的?伙伴?定知道，在樹結構中每個節(jié)點最多只有?個?節(jié)點，?節(jié)點可以擁有多個?節(jié)點。在經歷了N輪迭代后，我們已經獲得了?顆如下圖所示的隨機樹，這時我們發(fā)現此時的隨機采樣點竟然幸運地落在了終點的步?限制范圍內，并且?者之間不存在障礙物。這時我們便可以認為，該采樣點和終點之間存在?條邊，于是將該節(jié)點設為終點的?節(jié)點，并把終點添加進隨機樹。此時算法就可以結束迭代了，即規(guī)劃算法收斂。當規(guī)劃算法收斂以后，只需要從終點開始，沿著其?節(jié)點進?回溯，就可以找 到起點-終點之間的有效路徑。那么總結?下，RRT?成的每輪迭代中都包含以下這些流程：1. ?成?個隨機采樣點NewNode，并判斷采樣點是否位于?由區(qū)域2. 遍歷隨機樹，找出距離NewNode最近的節(jié)點ClosestNode3. 判斷NewNode是否在ClosestNode的步?限制范圍內，否則尋找中間點替代NewNode4. 判斷NewNode和ClosestNode之間是否存在障礙物，即碰撞檢測。5. 如果NewNode滿?以上所有約束條件，則將NewNode添加進隨機樹，設置ClosestNode為NewNode的?節(jié)點。6. 判斷NewNode是否在終點的步?限制范圍內，并對其?者做碰撞檢測。如果滿?條件則將該NewNode設為終點的?節(jié)點，并將終點加?隨機樹，即可結束迭代。否則繼續(xù)迭代。1.2 偽碼1.3 RRT的收斂性分析本節(jié)主要介紹了該規(guī)劃?法的理論特性。Theorem 1 如果存在長度為k的連接序列，則將 x_init 連接到 x_goal 所預期的迭代次數不超過k/p Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第392頁Theorem 2 Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第393頁Theorem 3 當頂點數趨于無窮時，在 x_init 處初始化的RRT包含 x_goal 的概率將趨近于1 Pf. 參考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第394頁定理1和定理2表示了規(guī)劃器的收斂率，定理3建?了規(guī)劃器是概率完備的（即，當迭代數趨于?窮時，找到解的概率趨于1我們可以看出，由于算法在未達到收斂條件之前是在不斷進?迭代的，所以只要在規(guī)劃的起點和終點之間是存在有效的路徑，那么只要迭代的次數夠多，那么采樣點就夠多，隨機樹就長得越茂密，能探索到的區(qū)域就?夠?，就必然可以找到有效的路徑。所以RRT是概率完備的。但是由于采樣點每次都是隨機的，所以算法并不能保證找到的路徑是最優(yōu)的路徑。因此RRT是?最優(yōu)的。1.4 程序示例1.5 參考1、S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.2、Rapidly-Exploring Random Trees: A New Tool for Path Planning3、https://www.guyuehome.com/9405
什么是路徑規(guī)劃？ 路徑規(guī)劃（也叫運動學規(guī)劃），任務是確定控制輸入，以驅動機器人從初始配置和速度到目標配置和速度，同時服從基于物理的動力學模型，且能確保機器人在環(huán)境中避開障礙。說白了，就是給你一張地圖，且已知障礙物分布，以及起始點和目標點的坐標，希望你根據這些信息，找到一條從起點到終點的能繞開障礙物的有效路徑，如果可以，還希望這條有效路徑盡可能最優(yōu)（最短），并且希望找到這條有效路徑的時間盡可能短（算法足夠高效） 目前流行的路徑規(guī)劃分為兩大類：基于采樣的路徑規(guī)劃和基于搜索的路徑規(guī)劃。運動規(guī)劃的狀態(tài)空間是應用于機器人變換的集合，稱為位姿空間（configuration space），引入了 C-空間、C-空間障礙物、自由空間等一系列概念，下面介紹一些概念：位姿（configuration）機器人一個位姿指的是一組相互獨立的參數集，它能完全確定機器人上所有的點在工作空間 W 中的位置，這些參數用來完整描述機器人在工作空間 W 中的狀態(tài)。一個位姿通常表示為帶有位置和方向參數的一個向量(vector)，用 q 表示。自由度（degrees of freedom）機器人的自由度定義為機器人運動過程中決定其運動狀態(tài)的所有獨立參數的數目，即 q 的維數。位姿空間（configuration space）位姿空間是機器人所有可能位姿組成的集合，代表了機器人所有可能的運動結果，稱為 C-空間，也可簡記為 C。距離函數（distance function）C-空間中的距離函數定義為該空間中的一個映射
公眾號