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

文章目录

  • 优缺点
  • 伪代码
  • 具体流程
  • 效率问题
  • 代码

优缺点

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

伪代码

RRT算法原理和代码详解(快速扩展随机树)_第1张图片

具体流程

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

RRT算法原理和代码详解(快速扩展随机树)_第2张图片

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

RRT算法原理和代码详解(快速扩展随机树)_第3张图片

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

RRT算法原理和代码详解(快速扩展随机树)_第4张图片

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

RRT算法原理和代码详解(快速扩展随机树)_第5张图片

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


RRT算法原理和代码详解(快速扩展随机树)_第6张图片

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

  1. 重新采样

RRT算法原理和代码详解(快速扩展随机树)_第7张图片

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


RRT算法原理和代码详解(快速扩展随机树)_第8张图片

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


RRT算法原理和代码详解(快速扩展随机树)_第9张图片

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


RRT算法原理和代码详解(快速扩展随机树)_第10张图片

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

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


RRT算法原理和代码详解(快速扩展随机树)_第11张图片

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


RRT算法原理和代码详解(快速扩展随机树)_第12张图片

效率问题

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

RRT算法原理和代码详解(快速扩展随机树)_第13张图片

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

代码

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

你可能感兴趣的:(算法,算法,自动驾驶,图论,人工智能,matlab)