最优阈值生长算法_【机器人路径规划】快速扩展随机树(RRT)算法

内容无关：最近的课题内容和机器人运动规划方法有关，我把学习的内容整理成为工具箱上传到了我的github仓库，稍后将会发一篇说明介绍使用方法。

XM522706601/robotics_tutorial_for_zhihu​github.com

---

上一篇文章 小明工坊：【基础知识】机器人运动规划原理与实现（一）——概率路线图(PRM)方法 我们讲到了机器人运动规划中一个比较重要的方法——概率路线图（PRM）方法，并通过编程进行了演示和分析，下面简单回顾如下：

• 机器人运动规划的基本任务为从开始位置运动到目标位置
• 主要难点有二：躲避障碍物（全局约束）和满足自身运动性能（微分约束）
• 抽样规划是解决全局约束问题的重要方法
• 抽样规划算法分为综合查询方法和单一查询方法
• 综合查询方法的代表为概率路线图算法(PRM)，单一查询方法的代表为快速扩展随机树算法(RRT)

本章我们同样用一个例子来讲解快速扩展随机树算法(RRT)算法。

其实RRT算法与PRM算法十分类似，都是通过抽样来在已知的地图上建立无向图，进而通过搜索方法寻找相对最优的路径。不同点在于，PRM算法在一开始就通过抽样在地图上构建出完整的无向图，再进行图搜索；而RRT算法则是从某个点出发一边搜索，一边抽样并建图。

与PRM算法相同，RRT算法也是概率完备的：只要路径存在，且规划的时间足够长，就一定能确保找到一条路径解。注意“且规划的时间足够长”这一前提条件，说明了如果规划器的参数设置不合理（如搜索次数限制太少、采样点过少等），就可能找不到解。

算法说明

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

图 1 RRT 算法，从左上角出发呈树型向目标点延伸

算法的步骤如下：

1.初始化

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

原图像

二值化图像

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的概率向地图内任意方向前进。

图 2 随机采样一个点，或直接采样终点，概率各一半

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

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

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

4. 终止条件

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

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

成功找到路径

分析

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

随机采样概率：

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

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

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

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

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

生长步长：

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

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

步长10，采样上限20000次

步长200，采样上限20000次。没有搜索到最终结果，可以看到，由于步长太大，生长点在障碍物与终点之间来回跳动，始终不能满足碰撞检测或终止条件的要求。

步长200，采样上限20000次

更多演示

RRT算法的适用性同样很广，举例如下：

参考文献：

[1] Siciliano B, Oussama K. Springer Handbook of Robotics[M]. 2007.

[2] RRT路径规划算法 XXX已失联