从零开始的OMPL库算法学习（4）RRT*算法
简介
RRT* 和RRTconnect一样，是对RRT算法的优化。RRT算法的一个问题在于，它只是找到了可行的路径，不能保证路径是相对优化的。RRT*算法在每次迭代后都会在局部更新搜索树，以优化路径。
它多了两个过程，为：
1.重新为 Xnew 选择父节点的过程， 比起RRT多了一个rewire的过程。
2.重布线随机树的过程
重新选择父节点过程
RRT*重选父节点过程
在新产生的节点 Xnew 附近以定义的半径范围内寻找“近邻”，作为替换Xnew 父节点的备选。依次计算“近邻”节点到起点的路径代价加上Xnew到每个“近邻”的路径代价，具体过程见图3。
RRT*重布线随机树过程
在为Xnew 重新选择父节点之后，为进一步使得随机树节点间连接的代价尽量小，为随机树进行重新布线。过程示意如图4重布线的过程也可以被表述成：如果近邻节点的父节点改为 Xnew可以减小路径代价，则进行更改。
其实很好理解，第一步为Xnew选择新的父节点，第二步为Xnew选择新的子节点，从而达到优化的效果。
伪代码
即多了chooseparent和rewire两个过程。