路径规划（三）双向快速扩展随机树（RRT_CONNECT）

2.1 原理

双树RRT是在原本RRT的基础上多加了⼀颗随机探索树，各自从起点和终点向外探索拓展，直到两棵树相遇时规划算法收敛。这种改进过的探索策略可以⼤⼤提⾼RRT的运⾏效率。 双树RRT中存在两颗随机树，我们将其命名为A和B，A以起点为根节点，B以终点为根节点。两颗随机树的拓展方式和单树RRT的别无二致，同样都需要经历随机采样+步⻓限制+碰撞检测这三个步骤，但是不同的地⽅在于双树RRT的随机树是交替生长的，⽐⽅说第⼀轮迭代中A树向外⽣⻓，第⼆轮便切换为B树⽣⻓，如此循环。 在每轮迭代中，随机树除了向外拓展之外，还会多出⼀个步骤，就是遍历另一颗随机树中的所有节点，找出离NewNode最近的节点，用于判断两颗随机树是否相遇。 假设算法经历了N次迭代以后，已经拓展出如下图所示的两颗随机树。并且在下⼀轮迭代中，轮到A树进⾏拓展，A树在图中⽤绿⾊线条表示，B树⽤黄色线条表示。

当进⼊本轮迭代后，算法成功拓展出A树的新节点NewNode_A，此时算法将遍历B树中的所有节点，找出B树中离NewNode_A最近的节点ClosestNode_B。并判断⼆者是否满⾜步⻓限制以及是否可以通过步⻓检测。如下图所示，这种情况明显⽆法通过碰撞检测。那么A树和B树在这⼀轮迭代中⽆法相遇，需要接着下⼀轮迭代。

进⼊下⼀轮迭代，这次便切换为B树进⾏拓展，假设算法拓展出的NewNode_B以及遍历A树后得到的ClosestNode_A如下图所示，经过判断发现⼆者满⾜步⻓限制并且通过了碰撞检测，那么这时A树和B树就成功得相遇了，规划算法收敛

当算法收敛以后，只需在两棵树的相遇处分别沿着⽗节点回溯便可以找出从起点到终点的有效路径。

注意：

双向RRT和RRT的区别不仅仅是在于双向生长，双向RRT比RRT更“贪心”，相比于RRT在生长RRT树的时候，是每产生一个随机点，如果能通过碰撞检测，就往该随机点的方向生长一次，然后该随机点就被废弃了，下一步想继续生长RRT树的话，就只能继续生成新的随机点，每个随机点最多利用一次。而双向RRT在生长RRT树时，先先生成一个随机点，然后，该树往该随机点的方向生长，直到碰到障碍物或则生长到该随机点，这样，一个随机点就被多次利用，加快了速度。

2.2 伪码

2.3 程序示例

2.4 收敛性分析

双向RRT的收敛性分析可以应用RRT的收敛性分析

2.5 参考

1、RRT-Connect: An Efficient Approach to Single-Query Path Planning

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