文章目录
- 优缺点
- 伪代码
- 具体流程
- 效率问题
- 代码
优缺点
优缺点先明说，优点RRT Star适用于任何地图，不像A Star，Dijkstra那样受限于栅格地图。
缺点：1.找到的路径可能不是最优的；2.路径可能不符合机器人的运动学动力学模型；3.效率问题。
伪代码
具体流程
- 给出起点和终点，以及设置好障碍物的地图，如下所示，将起点记作是根节点。
- 进行空间撒点采样，在空间中随机选择一点Xrand。（这里对应伪代码当中的Sample()函数）
- 接着寻找距离Xrand最近的一个已知节点Xnear（这一步对应伪代码当中的near()函数）。因为当前只有一个根节点（起点），所以根节点即为Xnear。
- “树的生长”（执行Steer函数）
将Xnear与Xrand两节点连接起来作为“树”生长的方向。
会设置一个步长作为“树枝”。
因为步长有长度，可能长度不会恰好等于Xnear到Xrand的距离。
所以称“树枝”的末端节点为Xnew。
现在，“树枝”的长度就是从Xnear到Xnew的长度。
将在“树枝”上的点（Xnear到Xnew这之间无限的点）都归为节点。
得到Xnew之后，之前的Xrand就舍弃了，只保留了“树枝”，之后重新进行采样。
- 重新采样
重新采样之后，发现连接的路径穿过了障碍物。
碰到连线穿过障碍物的，就抛弃这一次采样，重新采样。
发现，随着采样的进行，会越来越靠近终点。
但是要使算法停止，就必须要随机采样点刚好是终点，这样的概率是非常小的。所以会设置一个提前停止的条件：
因为每一段树枝的末端都是Xnew，所以每产生一次Xnew节点，我们都判断一下Xnew与终点之间的距离，看这个距离是否小于步长，如果小于步长且没有经过障碍物，则就直接把Xnew与终点进行相连。
综上，就能找到一条从起点到终点的路径。
效率问题
存在一个效率问题，如下图所示，按照每次Xnew之后进行Xnew与终点连线判断的情况，下图是可以直接按照这条轨迹到终点的。（因为在这没有碰到障碍物且只是步长不满足所给的条件）
这时候，改进一下采样的范围，就是直接沿着这个线的周围进行采样，限定范围，就可以加快算法导向终点的速度，这就是我的下篇Blog所要详解的Informed RRT*算法。
代码
代码在我的github以及gitee当中可下载。
