从零开始的OMPL库算法学习（4）RRT*算法

从零开始的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两个过程。