1.1 RRT算法思路

我們有兩個(gè)節(jié)點(diǎn)，一個(gè)綠色的起點(diǎn)，一個(gè)黃色的終點(diǎn)

對(duì)于RRT，我們做的第一件事就是將起點(diǎn)設(shè)置為隨機(jī)樹(shù)的根，那么我們就擁有了一顆只有根節(jié)點(diǎn)的樹(shù)

這棵樹(shù)光禿禿的，只有根節(jié)點(diǎn)的話不但難看，而且還沒(méi)用。那么我們這時(shí)候就需要從這個(gè)根節(jié)點(diǎn)出發(fā)，向外拓展出新的葉?。拓展的方式很簡(jiǎn)單，就是隨機(jī)采樣+步?限制+碰撞檢測(cè)。 RRT在每輪迭代中會(huì)?成?個(gè)隨機(jī)采樣點(diǎn)NewNode，如果NewNode位于自由區(qū)域，那么我們就可以遍歷隨機(jī)樹(shù)中已有的全部節(jié)點(diǎn)，找出距離NewNode最近的節(jié)點(diǎn)ClosestNode。利?距離函數(shù)dist(NewNode, ClosestNode)得到二者之間的距離，如果滿足步長(zhǎng)限制的話，我們將接著對(duì)這兩個(gè)節(jié)點(diǎn)進(jìn)?碰撞檢測(cè)，如果不滿足步長(zhǎng)限制的話，我們需要沿著NewNode和ClosestNode的連線?向，找出?個(gè)符合步長(zhǎng)限制的中間點(diǎn)，用來(lái)替代NewNode。最后如果NewNode和ClosestNode通過(guò)了碰撞檢測(cè)，就意味著二者之間存在邊（edge），我們便可以將NewNode添加進(jìn)隨機(jī)樹(shù)中。首先以第一輪迭代為例，因?yàn)閯傞_(kāi)始我們的隨機(jī)樹(shù)中只有根節(jié)點(diǎn)，所以?論NewNode位于何處，遍歷出的最近節(jié)點(diǎn)ClosestNode必然是根節(jié)點(diǎn)。 假設(shè)我們遇到下圖這種情況，雖然采樣點(diǎn)NewNode位于步?限制之內(nèi)，但是卻很不巧沒(méi)有落在自由區(qū)域，即采樣點(diǎn)落在障礙物的位置時(shí)，這個(gè)采樣點(diǎn)會(huì)被算法舍棄。

假設(shè)我們的步?限制為R，也就是說(shuō)對(duì)于每個(gè)ClosestNode節(jié)點(diǎn)來(lái)說(shuō)，只有當(dāng)NewNode落在其半徑為R的圓的范圍內(nèi)時(shí)，這個(gè)隨機(jī)采樣點(diǎn)NewNode才有可能被直接采納。如下圖所示，該紅?隨機(jī)采樣點(diǎn)雖然位于?由區(qū)域，但是明顯在根節(jié)點(diǎn)的步?限制之外。不過(guò)這個(gè)節(jié)點(diǎn)并不會(huì)被簡(jiǎn)單粗暴地舍棄。?是會(huì)沿著ClosestNode和NewNode的連線，找出符合步?限制的中間點(diǎn)，將這個(gè)中間點(diǎn)作為新的采樣點(diǎn)。如下圖所示，藍(lán)點(diǎn)就可以替代紅點(diǎn)作為新的采樣點(diǎn)。

那么假設(shè)我們已經(jīng)通過(guò)第?輪迭代拓展出第?個(gè)葉?節(jié)點(diǎn)A，毫?疑問(wèn)地A的?節(jié)點(diǎn)就是根節(jié)點(diǎn)，假設(shè)我們第?輪迭代的隨機(jī)采樣點(diǎn)NewNode為圖中的點(diǎn)B，B落在A的步?限制范圍內(nèi)，但是A，B之間由于障礙物的阻擋，?法通過(guò)碰撞檢測(cè)，于是B就會(huì)被算法舍棄。

假設(shè)我們的隨機(jī)采樣點(diǎn)是下圖中的B’，明顯B’位于?由區(qū)域，滿?步?條件，并且可以通過(guò)與點(diǎn)A的碰撞檢測(cè)，那么我們就在B’和A之間添加?條邊，并且將A設(shè)置為B’的?節(jié)點(diǎn)。學(xué)過(guò)數(shù)據(jù)結(jié)構(gòu)的?伙伴?定知道，在樹(shù)結(jié)構(gòu)中每個(gè)節(jié)點(diǎn)最多只有?個(gè)?節(jié)點(diǎn)，?節(jié)點(diǎn)可以擁有多個(gè)?節(jié)點(diǎn)。

在經(jīng)歷了N輪迭代后，我們已經(jīng)獲得了?顆如下圖所示的隨機(jī)樹(shù)，這時(shí)我們發(fā)現(xiàn)此時(shí)的隨機(jī)采樣點(diǎn)竟然幸運(yùn)地落在了終點(diǎn)的步?限制范圍內(nèi)，并且?者之間不存在障礙物。這時(shí)我們便可以認(rèn)為，該采樣點(diǎn)和終點(diǎn)之間存在?條邊，于是將該節(jié)點(diǎn)設(shè)為終點(diǎn)的?節(jié)點(diǎn)，并把終點(diǎn)添加進(jìn)隨機(jī)樹(shù)。此時(shí)算法就可以結(jié)束迭代了，即規(guī)劃算法收斂。

當(dāng)規(guī)劃算法收斂以后，只需要從終點(diǎn)開(kāi)始，沿著其?節(jié)點(diǎn)進(jìn)?回溯，就可以找 到起點(diǎn)-終點(diǎn)之間的有效路徑。

那么總結(jié)?下，RRT?成的每輪迭代中都包含以下這些流程：

1. ?成?個(gè)隨機(jī)采樣點(diǎn)NewNode，并判斷采樣點(diǎn)是否位于?由區(qū)域

2. 遍歷隨機(jī)樹(shù)，找出距離NewNode最近的節(jié)點(diǎn)ClosestNode

3. 判斷NewNode是否在ClosestNode的步?限制范圍內(nèi)，否則尋找中間點(diǎn)替代NewNode

4. 判斷NewNode和ClosestNode之間是否存在障礙物，即碰撞檢測(cè)。

5. 如果NewNode滿?以上所有約束條件，則將NewNode添加進(jìn)隨機(jī)樹(shù)，設(shè)置ClosestNode為NewNode的?節(jié)點(diǎn)。

6. 判斷NewNode是否在終點(diǎn)的步?限制范圍內(nèi)，并對(duì)其?者做碰撞檢測(cè)。如果滿?條件則將該NewNode設(shè)為終點(diǎn)的?節(jié)點(diǎn)，并將終點(diǎn)加?隨機(jī)樹(shù)，即可結(jié)束迭代。否則繼續(xù)迭代。

1.2 偽碼

1.3 RRT的收斂性分析

本節(jié)主要介紹了該規(guī)劃?法的理論特性。

Theorem 1 

    如果存在長(zhǎng)度為k的連接序列，則將 x_init 連接到 x_goal 所預(yù)期的迭代次數(shù)不超過(guò)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頁(yè)

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頁(yè)

Theorem 3

    當(dāng)頂點(diǎn)數(shù)趨于無(wú)窮時(shí)，在 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頁(yè)

定理1和定理2表示了規(guī)劃器的收斂率，定理3建?了規(guī)劃器是概率完備的（即，當(dāng)?shù)鷶?shù)趨于?窮時(shí)，找到解的概率趨于1

我們可以看出，由于算法在未達(dá)到收斂條件之前是在不斷進(jìn)?迭代的，所以只要在規(guī)劃的起點(diǎn)和終點(diǎn)之間是存在有效的路徑，那么只要迭代的次數(shù)夠多，那么采樣點(diǎn)就夠多，隨機(jī)樹(shù)就長(zhǎng)得越茂密，能探索到的區(qū)域就?夠?，就必然可以找到有效的路徑。所以RRT是概率完備的。但是由于采樣點(diǎn)每次都是隨機(jī)的，所以算法并不能保證找到的路徑是最優(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 Planning

3、https://www.guyuehome.com/9405