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