参考博客：【规划】RRT*算法图解_笑扬轩逸的博客-CSDN博客_rrt算法

尽管RRT算法是一个相对高效率，同时可以较好的处理带有非完整约束的路径规划问题的算法，并且在很多方面有很大的优势，但是RRT算法并不能保证所得出的可行路径是相对优化的。因此许多关于RRT算法的改进也致力于解决路径优化的问题，RRT*算法就是其中一个。RRT*算法的主要特征是能快速的找出初始路径，之后随着采样点的增加，不断地进行优化直到找到目标点或者达到设定的最大循环次数。RRT*算法是渐进优化的，也就是随着迭代次数的增加，得出的路径是越来越优化的，而且永远不可能在有限的时间中得出最优的路径。因此换句话说，要想得出相对满意的优化路径，是需要一定的运算时间的。所以RRT*算法的收敛时间是一个比较突出的研究问题。但不可否认的是，RRT*算法计算出的路径的代价相比RRT来说减小了不少。RRT*算法与RRT算法的区别主要在于两个针对新节点  的重计算过程，分别为： 

RRT*重选父节点过程

在新产生的节点  附近以定义的半径范围内寻找“近邻”，作为替换  父节点的备选。依次计算“近邻”节点到起点的路径代价加上  到每个“近邻”的路径代价，具体过程见图3。

图 a)中表现的是随机树扩展过程中的一个时刻，节点标号表示产生该节点的顺序，0节点是初始节点，9节点是新产生的节点 ，4节点是产生9节点的 （抱歉这里6节点是9节点的  ，图中标错了），节点与节点之间连接的边上数字代表两个节点之间的欧氏距离(这里我们用欧氏距离来表示路径代价)。

在重新找父节点的过程中，以9节点  为圆心，以事先规定好的半径，找到在这个圆的范围内  的近邻，也就是4，5，8节点。

原来的路径0 - 4 - 6 - 9代价为10 + 5 + 1 = 16，备选的三个节点与  组成的路径0 - 1 - 5 - 9，0 - 4 - 9和0 - 1 - 5 - 8 - 9代价分别为3 + 5 + 3 = 11，10 + 4 = 14和3 + 5 + 1 + 3 = 12，因此如果5节点作为9节点的新父节点，则路径代价相对是最小的，因此我们把9节点的父节点由原来的节点4变为节点5，则重新生成的随机树如图 b)所示。

RRT*重布线过程

在为 重新选择父节点之后，为进一步使得随机树节点间连接的代价尽量小，为随机树进行重新布线。过程示意如图4重布线的过程也可以被表述成：如果近邻节点的父节点改为  可以减小路径代价，则进行更改。

如图4 a)，9节点为新生成的节点  ，近邻节点分别为节点4 , 6 , 8 。它们父节点分别为节点0 , 4 , 5。路径分别为0 - 4，0 - 4 - 6，0 - 1 - 5 - 8，代价分别为10，10 + 5 = 15 和3 + 5 + 1 = 9。

如果将4节点的父节点改为9节点  ，则到达节点4的路径变为0 - 1 - 5 - 9 - 4，代价为3 + 5 + 3 + 4 = 15 大于原来的路径代价10，因此不改变4节点的父节点。

同理，改变了8节点的父节点，路径代价将由原来的9变为14，也不改变8节点的父节点。如果改变6节点的父节点为  则路径变为0 - 1 - 5 - 9 - 6，代价为3 + 5 + 3 + 1 = 12小于原来的路径代价15，因此将6的父节点改为节点9，生成的新随机树如图4 b)。

重布线过程的意义在于每当生成了新的节点后，是否可以通过重新布线，使得某些节点的路径代价减少。如果以整体的眼光看，并不是每一个重新布线的节点都会出现在最终生成的路径中，但在生成随机树的过程中，每一次的重布线都尽可能的为最终路径代价减小创造机会。

RRT*算法的核心在于上述的两个过程：重新选择父节点和重布线。这两个过程相辅相成，重新选择父节点使新生成的节点路径代价尽可能小，重布线使得生成新节点后的随机树减少冗余通路，减小路径代价。

RRT*伪代码

其中部分函数与RRT算法中的定义和作用相同。

calculate(dist(,)+cost(,)) 中dist函数计算两点之间的距离，cost函数计算从给定点沿着随机树的各个边直到起点的路径代价。在这里路径代价是从给定点沿着随机树的各个边直到起点的欧氏距离之和。

min(dist( ,)+cost(,)) 表示选出使路径代价最小的 。

同理min(dist(,)+cost(,)) 表示选出使路径代价最小的。