【规划】RRT Planer 以及 python实现

本文转载自头条号作者半杯茶的小酒杯，百度的研发工程师，文章多是自动驾驶相关，推介给大家。

RRT Planer

快速搜索随机树(RRT-Rapidly-ExploringRandom Trees)是一种常见的用于机器人路径规划的方法，他的原始算法思想很简单，以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。

相对于其它传统的路径规划算法，RRT通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。

 RRT算法伪代码

1.初始化起始点。比如设置机器人所在的位置为初始点；

2.随机生成目标点，遍历T，如果通过T能到达目标点，则路径搜索成功，扩展结束；否则继续扩展T；

3.挑选随机点到目标点最近的一个为Xnear;

4.沿着Xrand到Xnear的方向生长一段距离，生成一个新的节点Xnew;

5.判断Xnew进行碰撞检测，如果状态非法，则本次生长结束；否则，将新的状态添加到T；

6.返回树结构。

RRT算法实现-Python版本

Bidirectional RRT Planer

从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。为此，基于双向扩展平衡的连结型Bidirectional RRT算法被提出。

Bidirectional RRT算法伪代码

Bidirectional RRT算法同时从起点和终点建立扩展树，然后采样随机点进行扩展。在每一次迭代中，扩展完第一棵树的新节点q(new)后，以这个新目标点作为第二棵树的扩展方向，如果没有碰撞，继续往相同的方向扩展，直到扩展失败或者′()=()。

在迭代的过程中也同时考虑了两棵树的平衡性，即选择两棵树节点少的作为扩展的目标。Bidirectional RRT的这些属性使得它相对于原始的RRT算法，搜索效率有了大大提升；其次两棵树不断向着对方交替扩展，这种带有启发性的扩展使得树的扩展使得搜索区域可以逃离各自的约束区域，从而更快的实现算法收敛。