RRT、RRT_Connect、RRT*

RRT

节点包括：在地图上的坐标，父节点的指针，从起始点移动到该节点的代价值。
• 将地图坐标点处理为节点。
• 树上节点集合A。
• 开始节点S。
• 目标节点G。
• 随机节点R。
• 树上距离R最近的节点N。
• 搜索距离p。

1、将节点S加入树A中。
2、在地图上产生一个随机节点R。
3、找到A中距离R最近的节点N。
4、由N朝着R搜索p得到一个新的节点Q。
5、判断Q与G的距离是否小于一定阀值，若是则执行10。
6、判断Q是否为可行点，若是则将Q的父节点设置为N。
7、重复2-6。
10、得出路径。
11、结束。
参考链接：https://jingyan.baidu.com/article/574c521957eb406c8d9dc1bc.html

优化

为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率来决定随机节点是目标点还是随机点。设定参数Prob，每次得到一个0到1.0的随机值p，当0<p<Prob的时候，随机树朝目标点生长行；当Prob<p<1.0时，随机树朝一个随机方向生长。

RRT_Connect

基本的RRT每次搜索都只有从初始状态点生长的快速扩展随机树来搜索整个状态空间，如果从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。为此，基于双向扩展平衡的连结型双树RRT算法，即RRT_Connect算法被提出。
该算法与原始RRT相比，在目标点区域建立第二棵树进行扩展。每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点𝑞𝑛𝑒𝑤后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同（15~22行），首先它会扩展第一步得到𝑞′𝑛𝑒𝑤，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤表示与第一棵树相连了，即connect了，整个算法结束。当然每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少（也可以考虑两棵树总共花费的路径长度），交换次序选择“小”的那棵树进行扩展。这种双向的RRT技术具有良好的搜索特性，比原始RRT算法的搜索速度、搜索效率有了显著提高，被广泛应用。首先，Connect算法较之前的算法在扩展的步长上更长，使得树的生长更快；其次，两棵树不断朝向对方交替扩展，而不是采用随机扩展的方式，特别当起始位姿和目标位姿处于约束区域时，两棵树可以通过朝向对方快速扩展而逃离各自的约束区域。这种带有启发性的扩展使得树的扩展更加贪婪和明确，使得双树RRT算法较之单树RRT算法更加有效。
参考：https://www.cnblogs.com/flyinggod/p/8727951.html

RRT*

第一阶段：将Xnew用最小的成本连接到树上。
选取Xnew的方法与RRT是相同的，然后以Xnew为圆心，选取半径r画圆。得到的圆形区域内的所有点，都选入集合Znear。在上图中，这些点都用绿色表示,叫做Xnear。然后，迭代计算所有Xnew->Xnear+Xnear->Xinit的成本，选最小的那个Xnear与Xnew链接。到此第一阶段结束。
第二阶段：检查通过Xnew到达集合Znear的其他点的成本是否比原来更小，是的话，链接新线。
能看到上图，Xnew向另外两个Xnear连虚线。从Xinit经过Xnew到达Xnear，如果比从Xinit到Xnear的成本更低，就采用新边链接，删除旧边。
原文链接：https://blog.csdn.net/c19961227/article/details/87930850
参考链接：https://blog.csdn.net/weixin_43795921/article/details/88557317#t0