无人驾驶路径规划之RRT算法

无人驾驶路径规划之RRT算法

简介

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

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

以下是RRT的伪代码：

主要步骤

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

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

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

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

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

6.返回树结构。