路径规划（八）RRT*-smart

8.1 原理

        最初，RRT*-Smart 像 RRT* 一样随机搜索状态空间。类似地，找到第一条路径就像 RRT* 会尝试通过配置空间中的随机采样来找到路径一样。一旦找到第一条路径，它就会通过互连直接可见的节点来优化它。此优化路径产生用于智能采样的偏置点。在这些偏置点，采样以规则的间隔进行

        随着算法的进展和路径的不断优化，此过程将继续进行。每当找到更短的路径时，偏差就会转向新路径。

        RRT*是一边生长一边优化的，RRT*的重心在于找到最优路径。RRT*树生长到能连接起点和终点后，这就已经有一条初始路径了。

        这颗RRT*树可以继续生长，越生长可以得到的路径相比初始路径会越优。然而，这个继续生长的过程对于RRT*而言效率非常低，由此衍生出RRT*-smart算法，专门解决这一问题。

        注意到，RRT*-smart是在RRT*算法已生成初始路径后，在此基础上，想对初始路径继续优化的步骤才起作用，所以RRT*-smart对于生成初始路径并没有加速帮助。

        RRT*-smart的优势在于：它专注于提升路径接近障碍物拐点处的优化速度。RRT*-smart算法的思路是这样的：

在原始RRT*算法的基础上加了两步：

①路径优化

路径优化的本质是利用三角形两边之和大于第三边

假设RRT*生成的初始路径长这样

具体操作如下：

        一旦RRT*给出了一条初始路径，将初始路径中彼此可见的节点直接相连。迭代过程从xgoal开始，向xinit检查与每个节点的连续父节点的直接连接，直到无冲突条件失败。下图给出一个示例。

信标（Z_beacons）：经过路径优化后的路径中除了起点和终点之外的节点，标记为信标（Z_beacons），如上图中的绿点。

②智能采样

在RRT*算法中，在生成初始路径后，在此RRT*树的基础上继续采点，采点越多，路径优化效果越好，但此采点，是完全随机的采点，因此效率低下，RRT*-smart则不是完全随机的采点。

在RRT*-smart算法中，利用了这样一种思想：智能采样背后的想法是通过生成尽可能靠近障碍物顶点的节点来接近最优性。

在基于采样的RRT*-smart中，路径仅沿着靠近障碍物拐点的外围进行优化，解决的办法就是：在障碍物拐点的外围多多采点。如下图所示：

问：那么RRT*-smart如何实现在障碍物拐点的外围多多采点呢？

答：利用①路径优化过程中给出的信标（Z_beacons）。

一旦找到初始路径，智能采样就会开始，在以Z beacons为中心的半径为R beacons的球中直接生成一定数量的样本。采样偏向于这些信标，因为它们提供了有关障碍物顶点（或圆形障碍物的外围）位置的有用线索。因此，这些信标需要被最大节点包围，以优化这些转弯处的路径。与 RRT* 相比，这一特征迫使所提出的算法以更少的迭代次数达到最优解。

注意：

信标（Z_beacons）是需要随着优化路径的更新而更新的。即当z_goal.cost变小时，说明路径得到了优化，那么就要启用之前①路径优化算法来重新确定新的z_beacons。

8.2 伪码

8.3 程序示例

下图中的线段代表由RRT*生成的初始RRT树，圆圈代表在初始RRT树基础上，继续采点的分布，可见在几个“拐点处”的圆形领域内我们有额外的采点以加强在这部分的采点

路径优化后确定出拐点

经过路径优化后的路径：

8.4 参考

1、RRT*-SMART: A Rapid Convergence Implementation of RRT*

2、Rapid convergence implementation of RRT* towards optimal solution