路径规划（二）快速扩展随机树（RRT）

1.1 RRT算法思路

我们有两个节点，一个绿色的起点，一个黄色的终点

对于RRT，我们做的第一件事就是将起点设置为随机树的根，那么我们就拥有了一颗只有根节点的树

这棵树光秃秃的，只有根节点的话不但难看，而且还没用。那么我们这时候就需要从这个根节点出发，向外拓展出新的叶⼦。拓展的方式很简单，就是随机采样+步⻓限制+碰撞检测。 RRT在每轮迭代中会⽣成⼀个随机采样点NewNode，如果NewNode位于自由区域，那么我们就可以遍历随机树中已有的全部节点，找出距离NewNode最近的节点ClosestNode。利⽤距离函数dist(NewNode, ClosestNode)得到二者之间的距离，如果满足步长限制的话，我们将接着对这两个节点进⾏碰撞检测，如果不满足步长限制的话，我们需要沿着NewNode和ClosestNode的连线⽅向，找出⼀个符合步长限制的中间点，用来替代NewNode。最后如果NewNode和ClosestNode通过了碰撞检测，就意味着二者之间存在边（edge），我们便可以将NewNode添加进随机树中。首先以第一轮迭代为例，因为刚开始我们的随机树中只有根节点，所以⽆论NewNode位于何处，遍历出的最近节点ClosestNode必然是根节点。 假设我们遇到下图这种情况，虽然采样点NewNode位于步⻓限制之内，但是却很不巧没有落在自由区域，即采样点落在障碍物的位置时，这个采样点会被算法舍弃。

假设我们的步⻓限制为R，也就是说对于每个ClosestNode节点来说，只有当NewNode落在其半径为R的圆的范围内时，这个随机采样点NewNode才有可能被直接采纳。如下图所示，该红⾊随机采样点虽然位于⾃由区域，但是明显在根节点的步⻓限制之外。不过这个节点并不会被简单粗暴地舍弃。⽽是会沿着ClosestNode和NewNode的连线，找出符合步⻓限制的中间点，将这个中间点作为新的采样点。如下图所示，蓝点就可以替代红点作为新的采样点。

那么假设我们已经通过第⼀轮迭代拓展出第⼀个叶⼦节点A，毫⽆疑问地A的⽗节点就是根节点，假设我们第⼆轮迭代的随机采样点NewNode为图中的点B，B落在A的步⻓限制范围内，但是A，B之间由于障碍物的阻挡，⽆法通过碰撞检测，于是B就会被算法舍弃。

假设我们的随机采样点是下图中的B’，明显B’位于⾃由区域，满⾜步⻓条件，并且可以通过与点A的碰撞检测，那么我们就在B’和A之间添加⼀条边，并且将A设置为B’的⽗节点。学过数据结构的⼩伙伴⼀定知道，在树结构中每个节点最多只有⼀个⽗节点，⽗节点可以拥有多个⼦节点。

在经历了N轮迭代后，我们已经获得了⼀颗如下图所示的随机树，这时我们发现此时的随机采样点竟然幸运地落在了终点的步⻓限制范围内，并且⼆者之间不存在障碍物。这时我们便可以认为，该采样点和终点之间存在⼀条边，于是将该节点设为终点的⽗节点，并把终点添加进随机树。此时算法就可以结束迭代了，即规划算法收敛。

当规划算法收敛以后，只需要从终点开始，沿着其⽗节点进⾏回溯，就可以找 到起点-终点之间的有效路径。

那么总结⼀下，RRT⽣成的每轮迭代中都包含以下这些流程：

1. ⽣成⼀个随机采样点NewNode，并判断采样点是否位于⾃由区域

2. 遍历随机树，找出距离NewNode最近的节点ClosestNode

3. 判断NewNode是否在ClosestNode的步⻓限制范围内，否则寻找中间点替代NewNode

4. 判断NewNode和ClosestNode之间是否存在障碍物，即碰撞检测。

5. 如果NewNode满⾜以上所有约束条件，则将NewNode添加进随机树，设置ClosestNode为NewNode的⽗节点。

6. 判断NewNode是否在终点的步⻓限制范围内，并对其⼆者做碰撞检测。如果满⾜条件则将该NewNode设为终点的⽗节点，并将终点加⼊随机树，即可结束迭代。否则继续迭代。

1.2 伪码

1.3 RRT的收敛性分析

本节主要介绍了该规划⽅法的理论特性。

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表示了规划器的收敛率，定理3建⽴了规划器是概率完备的（即，当迭代数趋于⽆穷时，找到解的概率趋于1

我们可以看出，由于算法在未达到收敛条件之前是在不断进⾏迭代的，所以只要在规划的起点和终点之间是存在有效的路径，那么只要迭代的次数够多，那么采样点就够多，随机树就长得越茂密，能探索到的区域就⾜够⼴，就必然可以找到有效的路径。所以RRT是概率完备的。但是由于采样点每次都是随机的，所以算法并不能保证找到的路径是最优的路径。因此RRT是⾮最优的。

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