RRT算法是一种比较高效、能较好处理具有不完全约束的路径规划问题的算法，在许多方面具有很大的优点，但RRT算法并不能保证得到的可行路径相对优化。 因此，RRT*算法就是其中之一，改进RRT算法也致力于解决路径优化问题。 rt*算法的主要特点是快速找到初始路径，然后随着采样点的增加，不断优化直到找到目标点或达到设定的最大循环次数。 rt*算法经过了渐进优化。 也就是说，随着迭代次数的增加，得到的路径越来越优化，永远不可能在有限的时间内得到最佳路径。 也就是说，要获得相对满意的优化路径，需要一定的运算时间。 所以RRT*算法的收敛时间是一个比较突出的研究问题。 但是，不可否认，用RRT*算法计算的路径成本与RRT相比减少了很多。 RRT*算法与RRT算法的区别主要在于对以下两个新节点的重新计算过程：
用于重新选择父节点的进程比RRT多一个rewire进程。 随机树重路由过程重新选择父节点过程RRT*父节点重选过程
在新生成的节点附近，在定义的半径范围内查找“邻居”，以代替替换父节点。 在从“近邻”节点到出发点的路径成本中依次计算各“近邻”的路径成本。 具体过程见图3。
图a )中，表示随机树扩展中的某个时间点，节点标签表示生成节点的顺序，0节点是初始节点，9节点是新生成的节点，http://www.Sina.com/http://www.Sina.com
在新寻找父节点的过程中，以9个节点为圆心，按预定半径，找到此圆范围内的近邻，即4、5、8个节点。
原始路径0 - 4 - 6 - 9的成本为105-1=16，而构成替代的三个节点的路径0 - 4 - 6 - 9、0 - 4 - 6 - 9和0 - 1 - 5 - 8 - 9的成本分别为3(5)3=11、10、10 如果将5个节点作为9个节点的新的母节点，路径成本相对最小，所以将9个节点的母节点从原来的节点4变更为节点5的话，新生成的随机树如图b )所示。
4节点是产生9节点的 RRT*重新布线流程
重新选择父节点后，为了进一步尽量减少随机树节点之间的连接成本，对随机树进行重新布线。 程序图可以表示，如图4所示的再布线过程，在相邻节点的父节点被变更为可以削减路径成本时也会发生变更。
图4 a )，9个节点是新生成的节点，相邻节点分别是节点4、6、8。 它们的父节点分别是节点0、4、5。 路径分别为0-4、0-4-6、0-1-5-8，成本分别为10、105=15、3 5 1=9。
如果将4个节点的父节点更改为9个节点，则4个节点的父节点不会更改，因为到达节点4的路径为0 - 1 - 5 - 9 - 4，成本为3 5 3 4=15，且大于原始路径成本10。
同样，改变8个节点的父节点时，路径成本从原来的9变为14，8个节点的父节点也不变。 如果变更6个节点父节点，则路径为0 - 1 - 5 - 9 - 6，成本为3531=12，比原路径成本15小，所以将6个父节点变更为节点9，生成新的随机树如图4 b所示)。
重路由过程的含义是，在每次生成新节点时重新路由是否可以降低部分节点的路径代价。 从整体上看，重新布线的节点并不显示在最终生成的路径中，但在生成随机树的过程中，每次重新布线都提供了最小化最终路径成本的机会。
RRT*算法的核心在于上述两个过程。 父节点的重新选择和重新路由。 这2个过程是互补的，为了尽量减少新生成的节点的路径成本，重新选择母节点，通过重新布线生成新节点后的随机树减少冗长路径，减少路径成本。
RRT*伪码的部分函数与RRT算法的定义和作用相同。
calculate(dist )、() cost ()、() ) )的dist函数计算两点之间的距离，cost函数计算从给定点到随机树的每一边到起点的路径成本。 在此，路径成本是从给定点沿着随机树的各边到起点的fndkn距离之和。
min(dist )、() cost )、() )意味着选择路径成本最小的。
同样，min(dist )、(cost )、() )表示选择路径成本最低的选项。
（抱歉这里6节点是9节点的 为了简化问题，路径规划的障碍物采用圆、多边形等比较规则的几何形状。 对圆形障碍物来说，圆形边界的判断是一个非线性问题，通常对圆形进行线性化处理(变换为多边形)。 仅判断应该判断的横纵轴是否在圆内，在此如果在圆形障碍物的外接正方形内，则视为碰撞。 如果圆形障碍物的圆的中心坐标为，半径为，则考虑移动机器人的尺寸，相对于障碍物
碍物进行膨化处理，膨胀尺寸inf，所以碰撞条件为：因为圆形障碍物的避障策略相对简单，在仿真中并未设置圆形障碍物。
仿真环境中搭建的迷宫，主要选取长方形障碍物。长方形的碰撞机制研究相对复杂一些，具体碰撞机制阐述如下：在扩展随机树的过程中，由 到 连接的边不能与长方形障碍物的任何一边相交，即将长方形障碍物碰撞检测问题转化为直线与矩形相交问题。直线与矩形相交的判断主要分为两步：
第一步，判断 与 是否在矩形某一条边的一侧。如果 与 在矩形任意一条边的同侧，则不用进行后续判断， 与 连线必定不与矩形相交。这里不存在两点位于矩形内部的情况，因为 由 产生，而 必位于矩形外侧，如果 与 位于某一条边的两侧，则进行第二步判断。
第二步， 与 在矩形任意一边的不同侧时，分为两种情况：
第一种情况是 位于矩形内部，则 与 连线必定与矩形相交。第二种情况是两点均在矩形外部。在这种情况下并不能保证两点连线不与矩形相交，图6情况所示，两点位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交。在这种情况下，利用直线与矩形的性质进行避碰计算。 图6 两点均位于矩形外侧且位于某一条边两侧与矩形相交情况图7 碰撞机制数学模型
如图7所示， 与 位于矩形障碍物AB 边的两侧且均位于矩形的外侧，两点连线与矩形相交， 与 是两个边界，即当 与 连线位于 与 两直线下方时， 与 连线必定与矩形相交。反之，若不在 与 两直线下方则表现为不与障碍物发生碰撞。对于AD边来说必发生碰撞的过程可以用如下式子表示，
k 表示直线的斜率。
整个碰撞检测过程的逻辑如下:
对于矩形的一条边设定一个布尔值 ，当 时，表示发生碰撞，当 时，表示不发生碰撞。
定义一个判断函数 ，其中：
所以 函数是一个布尔函数，当等式右边为真时， ，反之 。则对于每一个边，判断逻辑可以写成
其中 表示矩形障碍物某一条边的两个定点。
针对图7中的具体情况:
布尔函数表示为：
首先判断且逻辑左半部分，
所以 ，意味着 与 位于AD边的两侧。
接着判断且逻辑的右半部分，
所以 ，意味着 与 连线位于 与 两直线下方。根据此判断条件发现， 与 属于同一种情况，不必再分情况讨论。
因此 ， 与 连线与矩形这一条边相交，发生碰撞。
bool 函数且逻辑左半部分与 相同，对于 而言，因为且逻辑左半部分判断两点是否在一条边两侧。且逻辑右半部分
所以，故 。
因此 与 连线不会与矩形这一条边相交，不会发生碰撞。
其他边的判断方法是相同的，当且仅当
才表明 与 连线不与矩形相交，不会发生碰撞，也将其视为有效点插入随机树中。
通过对每一个障碍物进行上述逻辑判断即可以使随机树的扩展避开障碍物，在 中搜索路径。
算法图解：1. 产生一个随机点xrand。
2. 在树上找到与xrand最近的节点xnearest。
3. 连接xrand与xnearest。
4. 以xrand为中心，ri为半径，在树上搜索节点。
5. 找出潜在的父节点集合Xpotential_parent，其目的是要更新xrand，看看有没有比它更好的父节点。
6. 从某一个潜在的父节点xpotential_parent开始考虑。
7. 计算出xparent作为父节点时的代价。
8. 先不进行碰撞检测，而是将xpotential_parent与xchild（也就是xrand）连接起来。
9. 计算出这条路径的代价。
10. 将新的这条路径的代价与原路径的代价作比较，如果新的这条路径的代价更小则进行碰撞检测，如果新的这条路径代价更大则换为下一个潜在的父节点。
11. 碰撞检测失败，该潜在父节点不作为新的父节点。
12. 开始考虑下一个潜在父节点。
13. 将潜在父节点和xchild连接起来
14. 计算出这条路径的代价。
15. 将新的这条路径的代价与原路径的代价作比较，如果新的这条路径的代价更小则进行碰撞检测，如果新的这条路径代价更大则换为下一个潜在的父节点。
16. 碰撞检测通过。
17. 在树中将之前的边删掉。
18. 在树中将新的边添加进去，将xpotential_parent作为xparent。
19. 遍历所有的潜在父节点，得到更新后的树。
引用文章：
[1] 机器人运动规划RRT*算法图解
[2] 路径规划——改进RRT算法