规划算法专栏之渐进最优快速随机搜索树（RRT star)

RRT算法是一个典型的基于采样的高效率的算法，但是上篇文章的分析中提到了，RRT的规划的结果通常不最优，这里虽然说的是通常不最优，实际上可以说是非常不优。因此在RRT的基础上，有很多改进版本，RRT_star就是针对RRT的规划结果不最优的局限性提出的。RRT*原论文
RRT_star的伪代码如下，来源链接

在原始RRT的基础上，RRT_star主要有两点改进:

• 为新节点选择代价最小的父节点
• 重布线

通俗点来说，就是为新扩展出来的节点选择一个最好的爸爸，使得新节点的代价最小，然后，在一定范围内选择合适的儿子，使得儿子的代价也最小。上述两个步骤分别随影伪代码中的10-13行和14-16行。

下面结合图和文字一起来描述这个过程：

按照流程，在规划过程中随机进行采样，根据采样得到的采样点Xrand寻找目前的树中最近的节点Xnear，然后扩展得到了一个新节点Xnew。
按照经典RRT的做法，就是将新节点Xnew的父节点赋值为Xnear，然后代价为起点到Xnear的代价与Xnear到Xnew的代价之和。而RRT_star中这一步则是通过伪代码中10-13行完成的，也就是先以新节点为圆心，一定半径画出一个圈，圈内的节点都是可能的父节点，然后挨个计算起点圈内的节点的代价与圈内节点到新节点代价之和，选其中最小的代价为新节点Xnew的代价，相应的节点为Xnew的父节点。
如下图，图中节点之间的数字表示节点之间的距离。

节点d为原始树中距离采样点最近的节点，节点c根据采样点和最近节点生成的新节点Xnew，在圈中共有a、b、d、e四个节点，挨个将其当作c的父节点然后计算c的代价，发现当节点a作为c的父节点时新节点c的代价最小，那么就将c的父节点赋值为a。通俗的说，这里就是新节点找爸爸的过程。结果如下：

这一步完成之后，还有重布线过程，也就是新节点找儿子。还是在刚刚的圈中，对圈中任意一个节点，计算一下如果以新节点为圈中的节点的父节点时圈中的节点的代价是否会降低，如果会，那么将该节点的父节点赋值为新节点，对应的代价更新为新的代价。
从图中来看，节点e原来的代价为15，如果以节点c为父节点，那么新的代价为12，小于原来的代价，那么将节点e的父节点改为节点c，结果如下：

当然，图示过程是假设均无碰撞的情况，如果上述过程中要计算的两个节点之间会发生碰撞，那么这两个节点之间不可能连接。

虽然RRT_star同样是基于采样的规划方法，但是他是渐进最优的，也就是当迭代次数足够多，时间足够长，RRT_star是可以得到一条最优路径的，他与RRT的效果区别可以直观的从下面两张图中看出(图片来源)：

可以看出，RRT_star倾向于规划处最短路径，而RRT只是得到一条可行路径。
由于RRT_star在路径优化上提出了改进方法，为了得到较好的路径，RRT_star花费的时间一般要比RRT多很多，毕竟没有一种规划算法是万能的，必须根据应用场景来选择最合适的规划算法。
最后，给出PythonRobitics中的RRT_star代码：
python版本代码
以及参考资料：
berkeley 机器人课程
以及，示意图懒得自己画，找了一个中文期刊用的他里面的图，图片连接
最后，大家一起加油哇！