文章目录
优缺点伪代码具体流程效率问题代码优缺点
优缺点先明说,优点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当中可下载。