RRT* 算法简介

快速探索随机树（RRT：Rapidly exploring Random Tree）是一种基于随机采样的路基规划算法，对高维状态空间的路径规划非常高效。本文将从随机游走的思想展开，介绍 RRT 的核心思想，并介绍考虑代价之后的 RRT* 算法。

随机游走简介

假想你身处一片迷雾之中，视线范围有限，如何对周边进行探索？直觉可能会诱导你进行"随机游走“，即每次任意选择一个方向移动一个单位的距离。用程序模拟 66 次随机移动，走过的路线可能如下视频所示：

显然随机游走能够对周边的环境进行探索，但存在两个比较严重的问题：

1. 游走的随机性可能导致局部范围内的反复横跳，导致相同的移动次数下探索的范围较小，即图中的路径没有“充分膨胀”；
2. 随机游走每次都是基于上一步，只能形成一条路径，没有生成分支从而看不到实际存在的捷径。

RRT 基本原理

考虑这么一个问题：给定已经探索到的路径，对于随机生成的目标点，如何移动能够更容易接近目标点？RRT 算法的核心步骤如下：

1. 在给定的范围内随机生成一个样本点，如图中绿色圆点；
2. 在已经探索到的地图中找到离这个样本点最近的节点，为图中蓝色圆点；
3. 从上述找到的最近点向样本点进行扩展。

重复上述步骤，同样地进行 66 次随机采样，可以看到 RRT 探索到的地图更广，分支更多，对指定区域的探索更充分。

上述示例和动图以二维空间的路径搜索为例，实际上 RRT 可以引入更高阶、更复杂的动态模型，并能够对障碍物进行规避。完整的 RRT 算法伪代码如下：

设定起始状态 x_start 和目标状态 x_goal
初始化地图 T，将起点放入地图（数据结构图可采用树）
直到达成终止条件，重复执行：
    随机生成一个样本点 x_rand
    找到地图中离样本点最近的节点 x_nearest
    如果从 x_nearest 到 x_rand 存在碰撞：
        进入下一次循环

    从 x_nearest 向 x_rand 生成新状态 x_new
    如果从 x_nearest 到 x_new 不存在碰撞：
        将从 x_nearest 到 x_new 的路径添加到地图

注意到伪代码将示例中的“位置”泛化成了“状态”，因此 RRT 适用于状态空间描述的动态系统，使用时有必要注意以下几点：

• 如何生成随机的样本点：一般来说只需要在关注的状态空间范围内随机生成即可，如有必要，可以在目标点附近的空间设置较大的概率分布密度，从而以较高的概率牵引随机树向目标点附近生长；
• 如何定义节点之间的距离：数学上通常可以直接取状态向量的模长，但考虑实际的信息，可以只取部分分量（例如在考虑位置和速度的状态空间中，只考虑位置的差距），也可以对状态进行加权（对应后面 RRT* 中的代价设置）；
• 如何生长新的状态：可以直接使用状态空间方程；
• 终止条件是什么：理想情况下为“达到目标状态”，实际上应当考虑随机采样的误差，认为距离目标状态在“一定范围内“且向目标状态直接转移不会发生碰撞；在算法的实现上，还应当设置最大迭代次数或者超时保护，避免进入死循环。

RRT* 最优路径

敏锐的读者可能已经意识到，RRT 算法虽然是从”最近节点“向样本点生长新的节点，但是新生长的节点周围可能存在很多其他的”相距并不远“的节点，忽略这样的路径可能会错失更优的路径。因此，RRT* 首先要做的就是设置一定的”半径“，在生长出新的可行节点后检查该半径范围内是否有其他已知的节点，如果有则尝试构建两者之间的路径。

为了定义最优路径，应当设置一定的代价函数，以代价小的路径为优。从上层的需求来看，（节点对应的）状态之间的”距离“可以设置为状态误差加权后的模长，而状态之间转移的”代价“可以设置成另一组不同的权重。RRT* 以”最近距离“ 生长出来的节点并不是代价最小的路径，因此有必要在设定的半径范围内检查是否存在代价更小的父节点。

此外，生长出新的节点后，对以一定范围内的其他节点，可能出现”从新节点过来的代价更小“的情况，因此还要进一步考虑重写路径。

综合上面的三方面考量，RRT* 算法的伪代码为：

设定起始状态 x_start 和目标状态 x_goal
初始化地图 T，将起点放入地图（数据结构图可采用树）
直到达成终止条件，重复执行：
    随机生成一个样本点 x_rand
    找到地图中离样本点最近的节点 x_nearest
    如果从 x_nearest 到 x_rand 存在碰撞：
        进入下一次循环

    从 x_nearest 向 x_rand 生成新状态 x_new
    如果从 x_nearest 到 x_new 存在碰撞：
        进入下一次循环

    # 检查更优的父节点
    暂时记录 x_new 的最优父节点 x_min = x_nearest
    找到已有地图在 x_new 一定半径范围内的所有节点 x_neighbors
    对于 x_neighbors 内的每个节点 x_neighbor：
        检查 x_neighbor 到 x_new 是否存在碰撞，是则跳过
        如果从 x_neighbor 到 x_new 的累计代价小于 x_min 到 x_new 的代价，则将 x_min 设置为当前节点
    向地图中添加从 x_min 到 x_new 的路径

    # 考虑更换周围节点的父节点
    对于 x_neighbors 内的每个节点 x_neighbor（除了 x_min）：
        检查 x_new 到 x_neighbor 是否存在碰撞，是则跳过
        如果从 x_new 到 x_neighbor 的累计代价小于 x_neighbor 目前的累计代价，则将 x_neighbor 的父节点设置为 x_new
        更新 x_neighbor 及其子节点的代价

参考文献

1. S. M. LaValle and J. J. Kuffner, Randomized kinodynamic planning. Proceedings 1999 IEEE International Conference on Robotics and Automation. 1999. 473-479(1).
2. S. Karaman, E. Frazzoli. Incremental sampling-based algorithms for optimal motion planning.