基于采样的规划算法之RRT家族（三）：RRT*

RRT算法是边随机产生节点边生长一棵路径树，当这棵树与目标点相遇，便从相遇点回溯到起点得到路径解。对于新产生的随机节点，原始RRT算法将路径树上距离它最近的节点作为它的父节点，并不能保证新节点通过该父节点就是最短路径。并且，就算新节点连接是最优的，也无法保证未来新节点出现时，原来的连接还是最优的，或许通过未来新节点时，节点能找到更短的路径。所以，对于原始RRT算法以及RRT-Connect算法来说，路径树是非最优的，生成的路径也是非最优的。RRT* 期望解决这个问题，找到同样节点下，生成最优的路径树。

一、原理

首先，假定一个构造一半的路径树如下图所示，节点9为利用RRT算法得到的新节点，按最近距离，它以6节点为父节点。

RRT* 与RRT算法的区别主要有两个：1）拓展新节点时增加了两个过程；2）算法的结束条件。

1.1 新增加的两个过程

首先介绍拓展新节点时增加的两个过程：

第一过程：拓展新节点时，会检查所有临近它的节点是否作为该新节的父节点会使新节点有更短的路径；

如下图所示，首先，我找到第9个节点领域内的节点1、5、6、4。然后，我们分