机器人抓取系列——路径规划（RRT原理）

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

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

RRT

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

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

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

与其他方法相比较，RRT有什么优势呢？

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

RRT