路径规划算法流程

RRT算法流程：

1.初始化整个空间，定义初始点、终点、采样点数、点与点之间的步长t等信息
2.在空间中随机产生一个点xrand
3.在已知树的点集合中找到距离这个随机点最近的点xnear
4.在xnear到xrand的直线方向上从xnear以步长t截取点xnew
5.判断从xnear到xnew之间是否存在障碍物，若存在则舍弃该点
6.将new点加入到树集合中
7.循环2~6，循环结束条件：有一个new点在终点的设定邻域内

RRT*算法流程:

RRT算法只能找到一条可行路径，并不能保证找到一条最优路径，RRT* 算法在RRT算法的基础上增加了两步：rewrite和random relink。也就是重写和随机重连。
重写就是在新节点xnew加入到树种之后，重新为它选择父节点，好让它到起始点的路径长度（代价）更小。
随机重连就是在重写完成之后，对新节点xnew附近一定范围内的节点进行重连。重连就是，检查一下如果把xnew附近的这些节点的父节点设置为xnew，这些节点的代价会不会减小。如果能够减小，就把这些节点的父节点更改为xnew；否则，就不更改。RRT* 算法考虑每一个节点到出发点的距离，为此每一个节点会增加一个属性：distance_to_start，即到出发点的距离。相应地在每一个节点选择父节点地时候，新节点的距离等于父节点的距离加上父节点到子节点的直线距离。
遍历整个树，
1.rewrite：

获得到新节点xnew的距离小于一定阈值（比如1.5倍的步长，也就是1.5*t）的所有节点
将这些节点加入到一个名为candidate_parent_of_newpoint的列表中，
为了方便，这些节点的distance不再用来存储到出发点的距离，而是用来存储如果把该节点设置为xnew的父节点的话，xnew到出发点的距离。
找到candidate_parent_of_newpoint列表中具有最小distance的那个节点，返回他的索引index，将新节点newpoint的父节点设置为index。
2.random relink：

遍历整个列表，对每一个节点执行如下动作{
if(该节点到xnew的距离小于一定的阈值，比如1.6倍的步长，也就是1.6*t){
if（该节点现在的distance>把该节点的父节点更新为newpoint之后的distance){
把该节点的父节点设置为xnew，并更新该节点的distance值
更新以该节点为根节点的子树中的每个节点的distance。
}
}