快速搜索随机树（RRT---Rapidly-exploring Random Trees）入门及在Matlab中演示
转载：http://blog.****.net/gpeng832/article/details/71249198?locationNum=1&fps=1
快速搜索随机树（RRT -Rapidly-ExploringRandom Trees），是一种常见的用于机器人路径（运动）规划的方法，它本质上是一种随机生成的数据结构—树，这种思想自从LaValle在[1]中提出以后已经得到了极大的发展，到现在依然有改进的RRT不断地被提出来。
机器人的路径（运动）规划的问题被定义为：给定机器人在运动区域的初始位姿qinit找到一条路径，即一个位姿的连续序列，使得机器人沿该路径能够从初始位姿运动到终点，且不与障碍物发生碰撞。
对于机械臂来说，一般的运动规划是在大于等于2的多维构型空间（C-Space）中进行的，然而对于初学者来说，可以首先以2维空间中的路径规划为例（例如图1所示的迷宫），掌握一个初步的概念。
图1 在一个迷宫中设置起点qinit
对于机器人运动规划问题，现在有很多开源的代码可供选择学习，例如OMPL，在****上也有人上传了一些RRT的代码，但是大部分是需要积分下载的。为了更加便捷地入门学习RRT的思路，我从Github上找到了一个在Matlab里编写和仿真的代码（源码连接），因为在Matlab中运行程序和可视化相对来说比较简单，所以现在简单介绍一下这个程序，算作是对RRT的初步入门学习。 它的伪代码可以表示成下表：
-
算法：构建RRT
———————————————————— -
输入：
map: 机器人所处环境的信息；
qinit：机器人的起始位置;
qgoal：机器人的终点位置；
k：尝试生成树节点的次数；
deltaq的距离；
———————————————————— -
输出
Vertices：RRT的顶点；
Edges：RRT的边；
Path的原始路径；
T的树;
PathSmooth的缩短后的路径；
———————————————————— - 1：qrand,qnear,qnew←∅；
2：for i=1 to k
3：按一定的概率设置qrand←qgoal;
4：qnear←findQNear(qrand,vertices);.
5：qnew←findQNew(qnear,qrand,deltaq);；
6：对qnew做碰撞检测；
7：if 没有碰撞
8： Vertices←Vertices∪}；
9： Edges←Edges∪}；
10：if qnew包围
11：path←fillSolutionPath(edges,vertices);//将Edges连接起来，即为生成的路径。
12：endif
13：endif
14：endfor
15：pathSmooth←smooth(map,path,vertices,delta);//使用贪心算法提取缩短后的路径。
16：Return T；
需要注意的是在步骤3中该程序使用的方法是以一定的概率将qgoal示意图如图2所示。
图2 生成qnew
对于步骤6的碰撞检测，可将qnew之间的连线插值出若干个点，如图3，对每个点做检测，如果所有的点都不在障碍区域，那么说明两点之间无障碍。
图3 碰撞检测原理
关于最后的一个缩短路径的步骤15（smooth），原理可见下图4，从起点qinit点连接后就得出了缩短后的路径（PathSmooth）。
图4 缩短路径的原理
运行程序之后得到的效果如图5所示，其中红色的路径为原始路径，黑色的路径为缩短（Smooth）后的路径。
图5 运动规划效果
[1]LaValle, S.M., Rapidly-exploring random trees: A new tool for path planning. 1998.
[2]https://github.com/emreozanalkan/RRT
快速搜索随机树（RRT -Rapidly-ExploringRandom Trees），是一种常见的用于机器人路径（运动）规划的方法，它本质上是一种随机生成的数据结构—树，这种思想自从LaValle在[1]中提出以后已经得到了极大的发展，到现在依然有改进的RRT不断地被提出来。
机器人的路径（运动）规划的问题被定义为：给定机器人在运动区域的初始位姿qinit找到一条路径，即一个位姿的连续序列，使得机器人沿该路径能够从初始位姿运动到终点，且不与障碍物发生碰撞。
对于机械臂来说，一般的运动规划是在大于等于2的多维构型空间（C-Space）中进行的，然而对于初学者来说，可以首先以2维空间中的路径规划为例（例如图1所示的迷宫），掌握一个初步的概念。
图1 在一个迷宫中设置起点qinit
对于机器人运动规划问题，现在有很多开源的代码可供选择学习，例如OMPL，在****上也有人上传了一些RRT的代码，但是大部分是需要积分下载的。为了更加便捷地入门学习RRT的思路，我从Github上找到了一个在Matlab里编写和仿真的代码（源码连接），因为在Matlab中运行程序和可视化相对来说比较简单，所以现在简单介绍一下这个程序，算作是对RRT的初步入门学习。 它的伪代码可以表示成下表：
-
算法：构建RRT
———————————————————— -
输入：
map: 机器人所处环境的信息；
qinit：机器人的起始位置;
qgoal：机器人的终点位置；
k：尝试生成树节点的次数；
deltaq的距离；
———————————————————— -
输出
Vertices：RRT的顶点；
Edges：RRT的边；
Path的原始路径；
T的树;
PathSmooth的缩短后的路径；
———————————————————— - 1：qrand,qnear,qnew←∅；
2：for i=1 to k
3：按一定的概率设置qrand←qgoal;
4：qnear←findQNear(qrand,vertices);.
5：qnew←findQNew(qnear,qrand,deltaq);；
6：对qnew做碰撞检测；
7：if 没有碰撞
8： Vertices←Vertices∪}；
9： Edges←Edges∪}；
10：if qnew包围
11：path←fillSolutionPath(edges,vertices);//将Edges连接起来，即为生成的路径。
12：endif
13：endif
14：endfor
15：pathSmooth←smooth(map,path,vertices,delta);//使用贪心算法提取缩短后的路径。
16：Return T；
需要注意的是在步骤3中该程序使用的方法是以一定的概率将qgoal示意图如图2所示。
图2 生成qnew
对于步骤6的碰撞检测，可将qnew之间的连线插值出若干个点，如图3，对每个点做检测，如果所有的点都不在障碍区域，那么说明两点之间无障碍。
图3 碰撞检测原理
关于最后的一个缩短路径的步骤15（smooth），原理可见下图4，从起点qinit点连接后就得出了缩短后的路径（PathSmooth）。
图4 缩短路径的原理
运行程序之后得到的效果如图5所示，其中红色的路径为原始路径，黑色的路径为缩短（Smooth）后的路径。
图5 运动规划效果
[1]LaValle, S.M., Rapidly-exploring random trees: A new tool for path planning. 1998.
[2]https://github.com/emreozanalkan/RRT