5.1 原理

        在現(xiàn)實(shí)世界的場景中，通常會(huì)出現(xiàn)這樣的情況:有關(guān)環(huán)境的初始可用信息是不完整的，或者環(huán)境本身是動(dòng)態(tài)的。在這些情況下，當(dāng)接收到新信息時(shí)，初始解決方案可能會(huì)失效，例如通過機(jī)載傳感器。當(dāng)這種情況發(fā)生時(shí)，通常會(huì)放棄當(dāng)前的 RRT，并從零開始生長新的 RRT。這可能是一項(xiàng)非常耗時(shí)的操作，尤其是在規(guī)劃問題很復(fù)雜的情況下。另一方面，在確定性規(guī)劃界存在重規(guī)劃算法，當(dāng)這種變化發(fā)生時(shí)，它們能夠有效地修復(fù)之前的解決方案，而不需要從頭重新規(guī)劃。這就是通過連續(xù)域規(guī)劃路徑的問題，如果每次更新地圖，都用RRT重新規(guī)劃，效率相當(dāng)?shù)拖?。Extended_RRT則專門用于解決這種動(dòng)態(tài)路徑規(guī)劃問題。

        Extended_RRT的思路是這樣的：在老地圖中，由RRT算法得出的路徑，在障礙物動(dòng)態(tài)變化不太大的前提下，在新地圖中，大概率也是能通過的，就算障礙物變化很大，之前的路徑也或多或少包含了當(dāng)前障礙物區(qū)域的信息，所以，在新障礙物區(qū)域中，在重新規(guī)劃路徑時(shí)，原有的初始RRT路徑的信息可以利用上，而沒有必要完完全全重零開始用RRT來規(guī)劃。

        那么如何利用上初始RRT路徑的信息呢？

思路如下：

        在老地圖上先用RRT算法的處一條初始路徑，將這條初始路徑上的節(jié)點(diǎn)存儲(chǔ)在集合waypoints中，當(dāng)環(huán)境更新后，希望在新地圖上得出一條路徑，在隨機(jī)撒點(diǎn)的步驟，新的隨機(jī)點(diǎn)有概率p落在目標(biāo)節(jié)點(diǎn)處，此外，還有r的概率落在waypoints的節(jié)點(diǎn)中，剩余1-p-r的概率在目標(biāo)區(qū)域內(nèi)隨即撒點(diǎn)。這樣在重規(guī)劃時(shí)，就可以把初始路徑的信息利用進(jìn)來。完成隨機(jī)點(diǎn)選取后，剩余的碰撞檢測、樹的生長、路徑回溯步驟與RRT一致。

總結(jié)：

Extended_RRT適用于需要反復(fù)路徑重規(guī)劃的場景中，效率比直接重新進(jìn)行RRT要高得多，和RRT的主要區(qū)別在于，在選取新的隨機(jī)點(diǎn)時(shí)，利用上了初始路徑的信息，而不是完全隨機(jī)撒點(diǎn)。

5.2偽碼

5.3 程序示例

5.4 參考

1、Real-Time Randomized Path Planning for Robot Navigation* 

2、Extended RRT algorithm with dynamic N-dimensional cuboid domains

3、Replanning with RRTs