写在前面
这个系列的文章将会分享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个坐标分别是线段端点的坐标。
这个系列将会很有意思,敬请期待~
评论(0)
您还未登录,请登录后发表或查看评论