RRT算法原理和代码详解（快速扩展随机树）

优缺点

优缺点先明说，优点RRT Star适用于任何地图，不像A Star，Dijkstra那样受限于栅格地图。
缺点：1.找到的路径可能不是最优的；2.路径可能不符合机器人的运动学动力学模型；3.效率问题。

伪代码

具体流程

1. 给出起点和终点，以及设置好障碍物的地图，如下所示，将起点记作是根节点。

---

2. 进行空间撒点采样，在空间中随机选择一点Xrand。（这里对应伪代码当中的Sample()函数）

---

3. 接着寻找距离Xrand最近的一个已知节点Xnear（这一步对应伪代码当中的near()函数）。因为当前只有一个根节点（起点），所以根节点即为Xnear。

---

4. “树的生长”（执行Steer函数）
   将Xnear与Xrand两节点连接起来作为“树”生长的方向。

---

会设置一个步长作为“树枝”。
因为步长有长度，可能长度不会恰好等于Xnear到Xrand的距离。
所以称“树枝”的末端节点为Xnew。

---

现在，“树枝”的长度就是从Xnear到Xnew的长度。
将在“树枝”上的点（Xnear到Xnew这之间无限的点）都归为节点。
得到Xnew之后，之前的Xrand就舍弃了，只保留了“树枝”，之后重新进行采样。

5. 重新采样

---

重新采样之后，发现连接的路径穿过了障碍物。

---

碰到连线穿过障碍物的，就抛弃这一次采样，重新采样。

---

发现，随着采样的进行，会越来越靠近终点。

---

但是要使算法停止，就必须要随机采样点刚好是终点，这样的概率是非常小的。所以会设置一个提前停止的条件：

因为每一段树枝的末端都是Xnew，所以每产生一次Xnew节点，我们都判断一下Xnew与终点之间的距离，看这个距离是否小于步长，如果小于步长且没有经过障碍物，则就直接把Xnew与终点进行相连。

---

综上，就能找到一条从起点到终点的路径。

---

效率问题

存在一个效率问题，如下图所示，按照每次Xnew之后进行Xnew与终点连线判断的情况，下图是可以直接按照这条轨迹到终点的。（因为在这没有碰到障碍物且只是步长不满足所给的条件）

这时候，改进一下采样的范围，就是直接沿着这个线的周围进行采样，限定范围，就可以加快算法导向终点的速度，这就是我的下篇Blog所要详解的Informed RRT*算法。

代码

代码在我的github以及gitee当中可下载。