快速扩展随机树(RRT)算法

article/2025/10/4 8:23:06

RRT是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过随机构建Space Filling Tree实现对非凸高维空间快速搜索的算法。该算法可以很容易的处理包含障碍物和差分运动约束的场景，因而广泛的被应用在各种机器人的运动规划场景中。

1. Basic RRT算法

原始的RRT算法中将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域，就得到了从起点位置到目标位置的路径。

伪代码如下：

上述伪代码中，M是地图环境， $x_{init}$ 是起始位置， $x_{goal}$ 是目标位置。路径空间搜索的过程从起点开始，先随机撒点 $x_{rand}$ ；然后查找距离 $x_{rand}$ 最近的节点 $x_{near}$ ；然后沿着到方向前进stepsize的距离得到 $x_{new}$ ； $CollisionFree(M, E_i)$ 方法检测 $Edge(x_{new}, x_{near})$ 是否与地图环境中的障碍物有碰撞，如果没有碰撞，则将成功完成一次空间搜索拓展。重复上述过程，直至达到目标位置。

2. 基于概率的RRT算法

为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率概率p，根据概率p的值来选择树的生长方向是随机生长( $x_{rand}$ )还是朝向目标位置生长。引入向目标生长的机制可以加速路径搜索的收敛速度。

3. RRT Connect算法

RRT Connect算法从初始状态点和目标状态点同时扩展随机树从而实现对状态空间的快速搜索。

4. 算法说明

我们可以把RRT算法比较形象地看做“树型算法”。它从一个起始构型（对于二维图，就是一个点）出发，不断延伸树型数据，最终与目标点相连。先放一张规划的结果可能更加便于理解：

算法的步骤如下：

4.1. 初始化

选择或绘制一张bmp格式的图像，作为规划的构型空间，为了便于进行碰撞检测，将其二值化。选择左上角[0, 0]点作为起始点；右下角[499, 499]作为目标点。

4.2. 随机采样

我们已经确定了规划的起始点，按道理它需要不断地向着目标点进行生长。但需要注意的是，由于存在障碍物，如果我们让树型一味朝着目标点延伸，则可能会因为“撞墙”而失败。因此，我们采取了一种随机采样方法：在每次选择生长方向时，有一定的概率会向着目标点延伸，也有一定的概率会随机在地图内选择一个方向延伸一段距离，关键代码如下：

# 利用rand()函数在[0,1]区间内随机生成一个数
if np.random.rand() < 0.5:# 如果小于0.5，则在图 img_binary 的范围内随机采样一个点sample = np.mat(np.random.randint(0, img_binary.shape[0] - 1, (1, 2)))
else:# 否则用目标点作为采样点sample = self.point_goal

我们每一步让RRT树有0.5的概率直接采样终点向目标点前进，有0.5的概率向地图内任意方向前进。

4.3. 生长点选择与碰撞检测

从图 2 可以看到，由于每次生长都存在一定的随机性，因此RRT树会逐渐出现许多分支，那么每一步中我们该如何选择要延伸哪个分支呢？这里我们直接选择RRT树中离采样点最近的点，并向其延伸。

假设我们采样了空间中随机一个点，接下来从现有的RRT树中选择离采样点最近的一个点，并向采样点延伸一段距离。假如在这段延伸中没有发生碰撞（碰撞检测），而且新点与现有的所有点的距离大于某个判断阈值（防止生长到RRT已经探索过的位置），则将这个新点也加入RRT树。

4.4. 终止条件

由于我们每次延伸的距离是固定的，所以并不能保证最后一次延伸能够刚好到达终点的位置，更可能的情况是在终点周围来回跳动。因此我们设定一个阈值，假如本次延伸的新点与终点的距离小于这个阈值，我们就认为已经规划成功。

下面是随机采样概率0.5，步长20，采样上限20000次的结果

4.5. 分析

前面提到，RRT算法是概率完备的，预设参数可能对规划结果造成影响。那么有哪些参数会影响规划效果呢？这里我列举几个：

随机采样概率：

我们每一次采样，都有一定概率朝着任意方向走，或朝着终点走。这个概率显然会影响搜索效果。给人最直接的感觉是，随机采样的概率越大，RRT树的分支也就越多，反之则难以发生新的分支。下面我们修改随机采样概率来看看效果。

设随机采样的概率为0.01，采样上限20000次。可以看到，直到达到采样上限也没有成功找到解。这是因为RRT产生分支的概率太小，经历了许多次碰撞才能凭借分支绕过障碍物。

随机采样的概率为0.01，采样上限20000次

设随机采样的概率为1.0，采样上限20000次。可以看到，虽然规划得以成功，但由于生长缺乏方向性，其实是一种“碰运气”式的搜索。RRT树的分支填充了所有空间直至找到目标点。这样的搜索会消耗大量的时间。

随机采样的概率为1.0，采样上限20000次

生长步长：

我们的RRT树每一次延伸，都有一个固定的步长。这个步长的设置显然也会影响树的形状。当步长太大时，可能由于太过笨拙而无法成功绕过障碍物；当步长过小时，生长的速度显然会有所减慢（因为同样的距离要生长更多次）。一般来说，空间越复杂，步长越小。这里必须注意的是，生长步长一定要比判断是否为同一个采样点的阈值要大。

步长10，采样上限20000次。可以看到，采样点极其密集，消耗的时间更长。