RRT算法流程、注意事项

一、RRT算法简介

二、算法流程

1） 算法初始化；
2） 是否到达目标位置，未到达则转向步骤3），否则说明随机树已构造完成，进而转向
步骤 7）;
3） 生成一个[0,1]内随机数 p ，判断 p<pg ，是则转向步骤 4），否则转向步骤 5）;
4） 选择最终目标作为 qrand ，确定 qnear ，进而得到 qnew ;
5） 在搜索空间内生成一个随机节点 qrand ，相应地计算出候选新节点 qnew;
6） 判断 qnew 与 qnear 之间是否存在威胁/障碍物，若无威胁/障碍物则将 xnew 添加到随机树之中，实现随机树的一步扩展，转向步骤 2），若存在威胁则转向步骤 3）;
7） 从构造出的随机树之中，寻找从起始点 qinit 到最终目标点 qgoal 的路径。

三、注意事项

1）代码实现关键主要在判断qnew 与 qnear 之间是否存在威胁/障碍物，对于圆形障碍物是线段和圆之间是否重合，对于不规则障碍物则较难判断，一般是将不规则障碍物填充为凸障碍物，然后判断线段和线段之间是否重合，因此这种情况较为麻烦，需要额外考虑代码编写；

2）是否到达目标位置一般为判断生成的点与终点的距离是否小于步长距离，小于步长距离就结束。

四、实现效果及代码