机器人抓取系列——路径规划(RRT原理)

前面所述的点云分割+姿态估计能完成机器人抓取工作,但是如果抓取环境特比复杂,固定的抓取路径中有很多障碍物,那么就需要一种路径规划方法来规划出一条无碰撞的路线。

目前应用在机械臂上的路径规划算法主要分为人工势场法以及RRT(随机生成树)的方法,这里我用的是RRT的方法,因此主要介绍RRT方法的原理

RRT

RRT算法不仅可以在二维平面上适用,也适用于三维空间。今天主要记录一下二维的RRT算法原理,三维的RRT算法原理类似,只是增加一个维度即可。

RRT是一种通用的方法,不管什么机器人类型、不管自由度是多少、不管约束有多复杂都能用。而且它的原理很简单,这是它在机器人领域流行的主要原因之一。不过它的缺点也很明显,它得到的路径一般质量都不是很好,例如可能包含棱角,不够光滑,通常也远离最优路径。

那么下面就先介绍一下RRT的原理,然后再说一下路径不够光滑,以及远离最优距离是怎么解决的。

机器人抓取系列——路径规划(RRT原理)_第1张图片与其他方法相比较,RRT有什么优势呢?

其中一个很重要的优势就是快。RRT 的思想是快速扩张一群像树一样的路径以探索(填充)空间的大部分区域,伺机找到可行的路径。之所以选择“树”是因为它能够探索空间。在搜索轨迹的时候我们不知道出路应该在哪里,如果不在“确定”的搜索方向上,我们怎么找也找不到(找到的概率是0)。这时“随机”的好 处就体现出来了,虽然不知道出路在哪里,但是通过随机的反复试探还是能碰对的,而且碰对的概率随着试探次数的增多越来越大,就像买彩票一样,买的数量越多中奖的概率越大(RRT名字中“随机”的意思)。可是随机试探也讲究策略,如果我们从树中随机取一个点,然后向着随机的方向生长,那么结果是什么样的呢?见上图。可以看到,同样是随机树,但是这棵树并没很好地探索空间,它一直在起点(红点)附近打转。这可不好,我们希望树尽量经济地、均匀地探索空间,不要过度探索一个地方,更不能漏掉大部分地方。这样的一棵树怎么构造呢?

RRT的基本步骤是:
1、初始化整个空间,定义起始点,终点,采样点数,点与点之间的步长等信息
2、在空间中随机生成一个点Xrand
3、在已知树的点的集合中找到距离这个随机点最近的点Xnear
4、在Xnear到Xrand的直线方向上从Xnear以步长 t 截取点Xnew
5、判读Xnear和Xnew之间有没有障碍物,若存在障碍物则舍弃掉该点
6、将Xnew加入到树的集合中
7、循环2-6,循环截至的条件:有一个new点在终点的设定邻域内
机器人抓取系列——路径规划(RRT原理)_第2张图片

虽然这种方法能够找到有效的路径,但是同样也有一些问题
1、路径不够平滑
2、不是最优路径

RRT*

RRT*算法是相对于RRT算法的一个改进,相对于RRT来说,RRT *多了两个步骤:
1、重新为Xnew选择父节点
2、重布线随机树的过程

首先介绍RRT*重新选择父节点的过程

重新选择父节点过程

机器人抓取系列——路径规划(RRT原理)_第3张图片
这里贴一张图,图中9节点是新产生的节点Xnew,6节点是产生9节点的父节点,,图中节点和节点之间的数值代表的是两点之间的欧氏距离。
在重新找父节点的过程中,以9节点 Xnew为圆心,以事先规定好的半径,找到在这个圆的范围内 Xnew 的近邻,也就是4,5,8节点(6节点是9的父节点)。
原来的路径0-4-6-9的代价为10+5+1=16
备选的三个节点 4,5,8和Xnew9节点组成的路径0 - 1 - 5 - 9,0 - 4 - 9和0 - 1 - 5 - 8 - 9代价分别为3 + 5 + 3 = 11,10 + 4 = 14和3 + 5 + 1 + 3 = 12,因此如果5节点作为9节点的新父节点,则路径代价相对是最小的,因此我们把9节点的父节点由原来的节点6变为节点5,则重新生成的随机树如图 b)所示。

重布线随机树过程

机器人抓取系列——路径规划(RRT原理)_第4张图片
在为Xnew节点9重新选择父节点之后,为进一步使得随机树节点间连接的代价尽量小,为随机树进行重新布线。过程示意如上图所示,重布线的过程也可以被表述成:如果近邻节点的父节点改为 Xnew 可以减小路径代价,则进行更改。
比如上图,6节点一开始的路径代价为10+5=15,但是如果将他的父节点改成9,则他的路径代价则为3+5+3+1=12,因此更新6节点的父节点。
同理再看节点4和8,如果改变4和8的父节点变为9,那么他们的路径代价会变大,因此不改变4和8的父节点。

重布线过程的意义在于每当生成了新的节点后,是否可以通过重新布线,使得某些节点的路径代价减少。如果以整体的眼光看,并不是每一个重新布线的节点都会出现在最终生成的路径中,但在生成随机树的过程中,每一次的重布线都尽可能的为最终路径代价减小创造机会。

那么根据上述的RRT*的 重新选择父节点重布线随机树过程 的过程,可以总结一下,RRT *相比于RRT来讲,他多了上述两个步骤:
重新选择父节点,就是以Xnew为中心画一个圈,假设在这个邻域中的点是Xnew的父节点,计算相应的路径代价,找到路径代价最小的那个父节点,然后对树进行一个更新。
重布线随机树过程:就是对Xnew的近邻点改变其父节点,让Xnew邻域内的节点的父节点都变成Xnew,因为这个时候Xnew的路径代价已经是最小的了,所以让Xnew邻域内的节点的父节点都变成Xnew,看是否能够减小各个节点的路径代价,如果路径代价减小了,则更新树。

RRT*算法的核心在于上述的两个过程:重新选择父节点和重布线。这两个过程相辅相成,重新选择父节点使新生成的节点路径代价尽可能小,重布线使得生成新节点后的随机树减少冗余通路,减小路径代价。

这里贴一个RRT* 的伪代码:(直接从别人那里截图过来的)
机器人抓取系列——路径规划(RRT原理)_第5张图片
机器人抓取系列——路径规划(RRT原理)_第6张图片
相对于RRT来讲,RRT*显然进行了一定程度的路径优化,但是这还不够,因此后续再介绍两种优化方式

双向RRT*

顾名思义就是从起始点和终点同时扩展树,直到两个树伸展到接近一定距离,然后直接进行拼接,这样减少了路径规划所消耗的时间。

贝塞尔曲线

这里先贴一个我用到的三阶贝塞尔曲线的python代码:

https://www.jb51.net/article/177375.htm

那么前面是对路径规划的路径距离的一个优化,但是生成的路径还是不平滑,因此利用研一所学的课程知识:贝塞尔曲线对规划的路径进行平滑处理:
机器人抓取系列——路径规划(RRT原理)_第7张图片
三阶贝塞尔曲线的公式为:

在这里插入图片描述

原理描述:
首先介绍一下一阶贝塞尔曲线
一阶贝塞尔曲线需要2个控制点,假设分别为 P0 , P1。则贝塞尔曲线上的生成点p1​(t)可以表达为:
在这里插入图片描述
t的取值范围为[0,1],下面不再重复说明。

一阶曲线很好理解, 就是根据 t 来的线性插值。

机器人抓取系列——路径规划(RRT原理)_第8张图片
二阶贝塞尔曲线:
二阶贝塞尔曲线有三个控制点,分别为P0,P1, P2。其中P0和P1构成一阶,P1和P2也构成一阶,即:
在这里插入图片描述
在生成两个一阶点的基础上,可以生成二阶贝塞尔点:
在这里插入图片描述
因此贝塞尔点与3个控制点的关系为:
在这里插入图片描述
机器人抓取系列——路径规划(RRT原理)_第9张图片
三阶贝塞尔曲线:
三阶贝塞尔曲线有4个控制点,假设分别为 P 0 , P 1 , P 2 , P 3 ,P0和P1,P1​和P2​、P2​和P3​ 都构成一阶,即:
机器人抓取系列——路径规划(RRT原理)_第10张图片
在生成的三个一阶点基础上,可以生成两个二 阶贝塞尔点:
在这里插入图片描述
在生成的两个二阶点基础上,可以生成三阶贝 塞尔点:
在这里插入图片描述
整理可得:
在这里插入图片描述
机器人抓取系列——路径规划(RRT原理)_第11张图片
将贝塞尔曲线应用到RRT中
机器人抓取系列——路径规划(RRT原理)_第12张图片
可以看出路径规划比较平滑。

扩展到三维

因为机械臂的工作空间不是在一个平面内,而是在三维空间中,因此需要将RRT*算法和贝塞尔平滑优化算法扩展到三维空间,其实很简单,就是简单的进行一个维度扩充就可以了,这里不仔细讲了,贴一下自己做出来的效果图:
机器人抓取系列——路径规划(RRT原理)_第13张图片

你可能感兴趣的:(机器人抓取,人工智能,自动驾驶)