路径规划RRT算法学习

RRT算法基本实现过程

RRT伪代码

1.初始化整个空间，定义初始点，终点、采样点数、点与点之间的步长t等信息
2.在空间中随机产生一个点xrand
3.在已知树的点集合中找到距离这个随机点最近的点xnear
4.在xnear到xrand的直线方向上从xnear以步长t截取点xnew
5.判断从xnear到xnew之间是否存在障碍物（此伪代码中未包含有障碍物的情况），若存在则舍弃该点。
6.将xnew点加入到树集合中

RRT*算法的基本实现过程

RRT*伪代码
1.初始化整个空间，定义初始点，终点、采样点数、点与点之间的步长t等信息
2.在空间中随机产生一个点xrand
3.在已知树的点集合中找到距离这个随机点最近的点xnear
4.在xnear到xrand的直线方向上从xnear以步长t截取点xnew
5.如果xnear到xnew之间没有障碍物，则在xnear周围以固定半径V寻找xnear的备选父节点
6.计算从起点到各个备选父节点再到xnear节点的代价，找出处于最小代价的（备选）父节点作为xnew的父节点（重选父节点）
7.计算xnew邻近（V半径范围内）的节点，如果从起始点到xnew节点再到周围节点的代价比原先从起点到xnew周围节点的代价更小，则将xnew作为这个周围节点的父节点。（重新布线）
8.将xnew点加入到树集合中

参考

[1] https://blog.csdn.net/baidu_36645485/article/details/79090718
[2] https://zhuanlan.zhihu.com/p/51087819
[3] http://save.seecs.nust.edu.pk/pubs/ICMA2012.pdf
[4] http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf