写在前面

这个系列的文章将会分享rrt算法的实现结果以及伪代码思路。rrt算法是基于采样的路径规划算法,与A*算法的基于搜索是不同的。

算法实现过程中遇到了一些困难,类似距离计算公式写错等低级错误,也有路径探索方向写错等算法理解错误,这些我都将在后续文章中提出。

为了算法的运行,我还写了一些调试函数,包括atan2()使用的使用可视化,这些我也会在后续文章中进行展示。

结果展示

黑色:障碍物(可由鼠标绘制);
绿色线段:探索路径;
绿色方块:终点
红色方块:起点
棕色线段:最终路径
点划线:路径探索下边界
绿色圆点:代表算法开始运行

算法逻辑及伪代码

rrt算法之所以叫做基于采样的路径规划算法,也是因为他的探索方向是随机的,也就是概率完备的,在路径存在的情况下,只要搜索的次数够多,内存够大,rrt算法总能找到路径。

rrt算法我也是基于C++,利用vector来进行实现的。

rrt算法用自然语言来描述的话就是从当前节点出发,然后设定概率阈值,通过生成随机数与概率阈值的大小关系确定搜索方法,当生成随机数大于概率阈值时,算法就将运行的目标点设置成终点,当生成随机数小于等于概率阈值时,算法就将运动的目标点设置成地图内的任意一点做为目标点。

选定目标点之后就是将当前节点向目标节点延展特定距离,如果说将延展节点没有触碰到障碍物而且是合法坐标点的话,则将延展节点指向当前节点。那么如何选择当前节点呢?很简单,目标点如果是终点,则在所有节点中寻找离终点最近的点,如果目标点是随机点,则在所有节点中寻找离随机点最近的点。

但是这样的话搜索点很难刚好搜索到重点上,因此搜索的中止条件应该是搜索到某一点,该点到终点的距离小于特定阈值了,就可以认为算法结束了。

状态分别如下图所示:

下一搜索节点的搜索方法:

算法终止条件:

有了自然语言的描述之后,算法的伪代码就是呼之欲出了:


INPUT:START,END,RRT_PATH

RRT_PATH_ADD(START)

WHILE(1){
    if(RANDOM > RANDOM_THRESHOLD){
        TARGET = INIT_RANDOM_NODE()
    }ELSE{
        TARGET = END
    }

    NEW_P = GENERATE_NEXT_NODE(NOW_P, TARGET)

    IF(NOT_OBSTACLES(NEW_P) && IS_LEGAL(NEW_P)){
        RRT_PATH_ADD(NEW_P)
    }

    NOW_P = GET_NEAREST_END()
    IF(DISTANCE(NOW_P , END) < DIST_THRESHOLD) BREAK
}

前瞻

在利用atan2()的时候,因为我用的xy坐标系可能和算法所用的不一样,我特地写了一个转换程序和可视化程序,在之后的文章中我会进行讲解分析,现在先放个界面给大家看一下:

然后在做可视化搜索路径的时候,为了在运行过程中查看搜索路径,我也写了一个可与鼠标交互可视化程序,效果如下:

线段都是从鼠标指向终点的,长度是设定好的数值,下面2个坐标分别是线段端点的坐标。

这个系列将会很有意思,敬请期待~