【机器人学：运动规划】快速搜索随机树（RRT---Rapidly-exploring Random Trees）入门及在Matlab中演示

快速搜索随机树（RRT -Rapidly-ExploringRandom Trees）

gpeng832

16865人浏览 · 2017-05-06 10:46:25

gpeng832 · 2017-05-06 10:46:25 发布

快速搜索随机树（RRT -Rapidly-ExploringRandom Trees），是一种常见的用于机器人路径（运动）规划的方法，它本质上是一种随机生成的数据结构—树，这种思想自从LaValle在[1]中提出以后已经得到了极大的发展，到现在依然有改进的RRT不断地被提出来。

机器人的路径（运动）规划的问题被定义为：给定机器人在运动区域的初始位姿 $q_{init}$ 和终点位姿 $q_{goal}$ 找到一条路径，即一个位姿的连续序列，使得机器人沿该路径能够从初始位姿运动到终点，且不与障碍物发生碰撞。

对于机械臂来说，一般的运动规划是在大于等于2的多维构型空间（C-Space）中进行的，然而对于初学者来说，可以首先以2维空间中的路径规划为例（例如图1所示的迷宫），掌握一个初步的概念。

图1 在一个迷宫中设置起点

qinit $q_{init}$ 和终点

qgoal $q_{goal}$

对于机器人运动规划问题，现在有很多开源的代码可供选择学习，例如OMPL，在CSDN上也有人上传了一些RRT的代码，但是大部分是需要积分下载的。为了更加便捷地入门学习RRT的思路，我从Github上找到了一个在Matlab里编写和仿真的代码（源码连接），因为在Matlab中运行程序和可视化相对来说比较简单，所以现在简单介绍一下这个程序，算作是对RRT的初步入门学习。它的伪代码可以表示成下表：

算法：构建RRT
————————————————————
输入：
map: 机器人所处环境的信息；
$q_{init}$ ：机器人的起始位置;
$q_{goal}$ ：机器人的终点位置；
$k$ ：尝试生成树节点的次数；
$deltaq$ ： $q_{near}$ 和 $q_{new}$ 的距离；
————————————————————
输出
$Vertices$ ：RRT的顶点；
$Edges$ ：RRT的边；
$Path$ ：从 $q_{init}$ 到 $q_{goal}$ 的原始路径；
$T$ ：连接 $q_{init}$ 和 $q_{goal}$ 的树;
$PathSmooth$ ：连接 $q_{init}$ 和 $q_{goal}$ 的缩短后的路径；
————————————————————
1： $q_{rand},q_{near},q_{new}←∅$ ；
2：for i=1 to k
3：按一定的概率设置 $q_{rand}←q_{goal}$ 或在map中随机生成 $q_{rand}$ ;
4： $q_{near}←findQNear(q_{rand}, vertices);$ //在 $q_{rand}$ 附近找到距离其最近的 $q_{near}$ .
5： $q_{new}←findQNew(q_{near}, q_{rand}, deltaq);$ //生成沿 $q_{near}$ 和 $q_{rand}$ 方向上，距 $q_{near}$ 为 $deltaq$ 的 $q_{new}$ ；
6：对 $q_{new}$ 到 $q_{near}$ 做碰撞检测；
7：if 没有碰撞
8： $Vertices←Vertices∪$ { $q_{new}$ }；
9： $Edges←Edges∪$ { $q_{new},q_{near}$ }；
10：if $q_{new}$ = $q_{goal}$ or $q_{new}$ 和 $q_{near}$ 将 $q_{goal}$ 包围
11： $path←fillSolutionPath(edges, vertices);$ //将Edges连接起来，即为生成的路径。
12：endif
13：endif
14：endfor
15： $pathSmooth ← smooth(map, path, vertices, delta);$ //使用贪心算法提取缩短后的路径。
16：Return T；

需要注意的是在步骤3中该程序使用的方法是以一定的概率将 $q_{goal}$ 作为 $q_{rand}$ ，这样可以使树的生长方向偏向终点，这与RRT的原始文献[1]是不同的。步骤5生成 $q_{new}$ 示意图如图2所示。

图2 生成

qnew $q_{new}$

对于步骤6的碰撞检测，可将 $q_{new}$ 到 $q_{near}$ 之间的连线插值出若干个点，如图3，对每个点做检测，如果所有的点都不在障碍区域，那么说明两点之间无障碍。

图3 碰撞检测原理

关于最后的一个缩短路径的步骤15（smooth），原理可见下图4，从起点 $q_{init}$ 开始，依次寻找能够能够无碰撞连接终点 $q_{goal}$ 的顶点，记录此点 $q'$ ，再从起点 $q_{init}$ 开始，以 $q'$ 为终点寻找，直至起点 $q_{init}$ 和 $q'$ 能够无碰撞连接，将所有的 $q'$ 点连接后就得出了缩短后的路径（PathSmooth）。