自主导航系列16-rrt源码

自主导航系列16-rrt源码

参考

1，理论基础

简单来说，Rapidly Exploring Random Trees 快速探索随机树是基于随机采样的规划算法。RRT的起点为一个根节点root
拓展根节点的方法为随机采样+步长限制+碰撞检测，每一轮迭代都会由父节点产生子节点
N轮迭代后，我们已经获得了一颗如下图所示的随机树，这时我们发现此时的随机采样点竟然幸运地落在了终点的步长限制范围内，并且二者之间不存在障碍物
总结一下就是

1. 生成一个随机采样点NewNode，并判断采样点是否位于自由区域
2. 遍历随机树，找出距离NewNode最近的节点ClosestNode
3. 判断NewNode是否在ClosestNode的步长限制范围内，否则寻找中间点替代NewNode
4. 判断NewNode和ClosestNode之间是否存在障碍物，即碰撞检测。
5. 如果NewNode满足以上所有约束条件，则将NewNode添加进随机树，设置ClosestNode为NewNode的父节点
6. 判断NewNode是否在终点的步长限制范围内，并对其二者做碰撞检测。如果满足条件则将该NewNode设为终点的父节点，并将终点加入随机树，即可结束迭代。否则继续迭代。

2，节点关系

roslaunch rrt_exploration_tutorials single_simulated_MTR.launch 
roslaunch rrt_exploration single.launch