RRT算法

Rapidly Exploring Random Trees 快速探索随机树，RRT和PRM的相同之处是，它们都是基于随机采样的规划算法，不同的是PRM生成的是一个“图”（Graph），而RRT生成的是“树”，RRT的一大显著特征就是它具备探索空间的能力，即从一点（根）出发向外探索拓展的特征。  简单的RRT有单树和双树两种类型，单树RRT将规划起点作为随机树的根节点，通过随机采样、碰撞检测的方式为随机树增加叶子节点，最终生成一颗随机树。而双RRT则拥有两颗随机树，分别以起点和终点为根节点，以同样的方式进行向外的探索，直到两颗随机树相遇，以达到提高规划效率的目的。RRT在每轮迭代中会生成一个随机采样点NewNode，如果NewNode位于自由区域，那么我们就可以遍历随机树中已有的全部节点，找出距离NewNode最近的节点ClosestNode。利用距离函数dist(NewNode, ClosestNode)得到二者之间的距离，如果满足步长限制的话，我们将接着对这两个节点进行碰撞检测，如果不满足步长限制的话，我们需要沿着NewNode和ClosestNode的连线方向，找出一个符合步长限制的中间点，用来替代NewNode。最后如果NewNode和ClosestNode通过了碰撞检测，就意味着二者之间存在边（edge），我们便可以将NewNode添加进随机树中。  首先以第一轮迭代为例，因为刚开始我们的随机树中只有根节点，所以无论NewNode位于何处，遍历出的最近节点ClosestNode必然是根节点。  假设我们遇到下图这种情况，虽然采样点NewNode位于步长限制之内，但是却很不巧没有落在自由区域，即采样点落在障碍物的位置时，这个采样点会被算法舍弃。