当前位置:   article > 正文

RRT算法原理和代码详解(快速扩展随机树)_rrt伪代码

rrt伪代码

优缺点

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

伪代码

在这里插入图片描述

具体流程

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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


在这里插入图片描述

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

  1. 重新采样

在这里插入图片描述

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


在这里插入图片描述

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


在这里插入图片描述

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


在这里插入图片描述

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

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


在这里插入图片描述

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


在这里插入图片描述

效率问题

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

在这里插入图片描述

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

代码

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

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/Li_阴宅/article/detail/808632
推荐阅读
相关标签
  

闽ICP备14008679号