CN105598965A - 基于立体视觉的机器人欠驱动手自主抓取方法 - Google Patents

Abstract

基于立体视觉的机器人欠驱动手自主抓取方法，涉及一种机器人自主抓取方法。为了解决目前的机器人抓取方法需要事先得到物体的三维模型才可求解抓取点的问题和目前的机器人抓取方法只能对简单的物体进行识别，而对复杂的物体则不能获得其相应的抓取点的问题。本发明首先针对所要抓取的物体及其环境，通过Kinect传感器获取物体及环境的RGB-D点云，并对点云进行滤波处理；针对RGB-D点云，提取法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征和相应的约束方程；然后，基于高斯过程分类建立抓取规划方案；按照抓取方案驱动欠驱动手进行抓取，然后根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体。本发明适用于机器人的抓取领域。

Description

基于立体视觉的机器人欠驱动手自主抓取方法

技术领域

本发明涉及一种机器人自主抓取方法。

背景技术

自主抓取一直以来都被认为是机器人智能化的一个基本组成部分。目前的机器人基本上都采用主从操作的方式，操作人员通过操作杆控制机器人完成抓取任务。这种操作方式要求操作人员需经过专业的训练，而且操作费时费力，因此研究一种自主抓取方法显得尤为重要。

PlinioMoreno等人提出一种使用支持向量机学习物体可抓取点的方法，使用SVM对点云中的可抓取部位的局部特征进行学习，通过训练以后，他们的算法可以找出物体的可抓取部位和不可抓取部位。为了验证训练结果，他们在OCRA仿真器中进行抓取仿真。为了减少对机器学习模型的训练的工作量，Bohg等人提出采用合成图像进行训练的方法。他们通过提取物体的shapecontext特征，并采用支持向量机的机器学习方法。实验过程中，分别采用了线性分类器和非线性分类器两种机器学习模型，并对这两个分类器进行对比，实验结果表明，结合了shapecontext特征和非线性分类器的算法能够更好地找到物体的抓取点。然而上述算法需要事先得到物体的三维模型，而在实际生活中这是不现实的，因此上述算法有一定的局限性。且以上算法均基于对于一个抓取点的情况进行研究，规划出来的抓取点被认为是执行器的中心位置。而多个抓取点的情况是要求规划出执行器与物体的多个接触点，其结果更易用于在实际的抓取控制当中。同时，多个抓取点的规划方法同时考虑了抓取过程的力封闭性原则和抓取稳定性，因此获得的抓取点的抓取成功率更高，因此受到广大研究人员的青睐。

韩峥等人提出了一种基于Kinect的目标抓取方案。在他们的实验研究中，将Kinect固定在天花板上，能够得到一个固定的抓取场景，当有新物体出现时，采用背景相减法能快速得到物体点云信息。为了将人在放置物体时人手产生的点云的影响消除，采用帧查法对人手的运动进行判别，当人手离开后才识别物体点云。同时，通过简单的几何运算计算出物体抓取点。Vassilios等人提出了一种对平面物体的抓取点的选取方法，将物体形状的不确定性考虑到规划过程中，可以计算出抓取方案是否满足满足力封闭性原则。并提出了一个抓取质量矩阵，在存在物体形状误差和手指放置位置误差的情况下，通过抓取质量矩阵可以识别出抓取的稳定区域。然而上述方法只能获得物体一个面的信息，因此只能对简单的物体进行识别，而对复杂的物体则不能获得其相应的抓取点。

自主抓取系统的核心是抓取规划器的建立。目前很多研究人员采用了有教师的机器学习方法进行抓取规划，过对三维图片进行处理，计算边界上的点的特征，进而采用支持向量机对提取的特征进行机器学习。使用上述方法提取的特征考虑了抓取过程的力封闭性、稳定性和碰撞性，并且提取的都是物体的局部信息，因此具有通用性可以抓取其他的物体。然而由于其获得的抓取点都位于物体的边界上，当抓取方形的物体时，其抓取点都位于方形物体的楞上，因此抓取很不稳定。且上述方法采用支持向量机或者神经网络的方法，只能给出分类的结果，而不能给出属于这一类的概率，因此无法进一步获得其最优的抓取点。

发明内容

本发明为了解决目前的机器人抓取方法需要事先得到物体的三维模型才可求解抓取点的问题和目前的机器人抓取方法只能对简单的物体进行识别，而对复杂的物体则不能获得其相应的抓取点的问题。

基于立体视觉的机器人欠驱动手自主抓取方法，包括下述步骤：

步骤1、针对所要抓取的物体及其环境，通过Kinect传感器获取物体及环境的RGB-D点云，并对点云进行滤波处理；

步骤2、针对RGB-D点云，提取法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征和相应的约束方程；

(1)法向量夹角特征：

在抓住物体时，欠驱动手的两指是对称的，两指的分支点为O″，假设两指与物体的接触点为点C₁与点C₂，C₁与C₂的连线为中点与O″的连线与物体表面的交点记为O，n₁、n₂分别为点O、C₁处的法向量，n₁、n₂的夹角为α；

假设物体与欠驱动手之间的摩擦力系数为f，确定接触点C₁与点C₂处的摩擦锥，摩擦锥顶角θ的大小由θ＝arctan(f)确定；

定义法向量夹角特征为：

F₁＝cos(α)＝cos(n₁,n₂)

约束方程为：

F₁＝cos(α)<cos(θ)；

(2)共面特征：

定义共面特征为：

F₂＝distance(n₁，n₂)

约束方程为：

F₂＝distance(n₁，n₂)<F_2max；

其中，n₁和n₂分别表示点O与点C₁法线；distance(n₁，n₂)表示n₁和n₂所在直线的距离；F_2max为临界值；

(3)距离特征：

定义点O到点C₁所在的直线距离为d_pair；定义欠驱动手最合适的抓取距离为d_optimal，定义距离描述子为d＝|d_pair-d_optimal|；欠驱动手的最大距离为d_max，最小距离为d_min，假设 $d_{optimal}=\frac{d_{max}-d_{min}}{2};$

令ω为欠驱动手两指之间的最大距离，得到如下的约束条件：

$d<\frac{1}{2}\mathrm{\&omega;};$

(4)抓取稳定性特征：

稳定距离为欠驱动手中心与物体重心之间的距离；

通过Kinect传感器获得物体的局部三维点云，通过这局部三维点云找出物体在Kinect中的坐标系x轴、y轴和z轴方向上的最大和最小值，分别记为x_min、x_max、y_min、y_max、z_min、z_max；认为在Kinec的坐标系中物体中心点坐标为 $z=\frac{z_{\min }+z_{max}}{2};$

定义共面特征为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}$

其中x_g、y_g、z_g为在Kinect的坐标系中物体重心的坐标；

约束方程为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}<F_{3max};$

其中，F_3max为F₃对应的阈值；

(5)碰撞检测特征：

首先是通过对称的方法获得两个抓取点，在两个抓取点外侧分别建立两个圆柱体，圆柱体的半径大于欠驱动手的手指的半径，圆柱体的方向与抓取方向相同；

碰撞检测特征是通过计算点云中的点落入到两个圆柱体中的个数来计算的，分别记为h₁、h₂；

通过机器学习的方法来学习碰撞检测特征并获得阀值h_1max和h_2max；即h₁<h_1max、h₂<h_2max；

步骤3：基于高斯过程分类建立抓取规划方案；

步骤4：按照抓取方案驱动欠驱动手进行抓取，然后根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体。

本发明具有以下有益效果：

(1)在机器人抓取过程中，物体与物体之间可能存在一定的遮挡的问题，故实际自主抓取中很难获得物体的三维模型。本发明提出了通过平面提取法从Kinect获得的RGB-D图像中提取物体点云，无需物体的三维模型，提高了自主抓取在实际应用中的可操作性和实际的应用性。

(2)本发明通过对获得的物体点云进行特征提取，在此基础上，通过采用高斯过程分类器对提取的特征进行有教师的机器学习，求解出欠驱动手的最优抓取轨迹。所以本发明的抓取效果更好，抓取成功率更高。

(3)为了使得在抓取过程中物体的移动量最小，需要欠驱动手接触到物体以后就立即停止运动。本发明在接触检测过程中采用基于电流的碰撞检测方法，通过高斯过程回归学习欠驱动手电流与电机转角之间的关系；当电流超出一定阀值时认为欠驱动手与物体接触，从而完成碰撞检测；在此基础上，完成欠驱动手的抓取控制。本发明提出的抓取方法可以有效的对复杂物体进行抓取，且抓取成功率在现有方法上有较大提高，成功率在95％以上。

附图说明

图1为法向量夹角特征选取过程的示意图；

图2为共面特征选取过程的示意图；

图3为Kinect传感器获得物体的局部三维点云图；

图4为实际抓取时适合抓取的抓取点示意图；

图5为实际抓取时不适合抓取的抓取点示意图；

具体实施方式

具体实施方式一：

基于立体视觉的机器人欠驱动手自主抓取方法，包括下述步骤：

步骤1、针对所要抓取的物体及其环境，通过Kinect传感器获取物体及环境的RGB-D点云，并对点云进行滤波处理；

Kinect传感器是微软于2010年11月推出的一款3D视觉传感器，其包含一个彩色摄像头和一个深度摄像头，可以直接获得场景内的彩色图和深度图，进而生成场景内的点云；然而，由于Kinect生成的点云中，包含场景内所有物体的点云，数量巨大且特征复杂，处理起来需要大量的机器时间，给后续的处埋带来麻烦；因此需要对所获得的点云进行一定的预处理，提取出点云中物体的点云，并进行滤波和法向量的提取，为后面的特征提取做准备；

步骤2、针对RGB-D点云，提取法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征和相应的约束方程；

(1)法向量夹角特征：

在抓住物体时，欠驱动手的两指是对称的，两指的分支点为O″，假设两指与物体的接触点为点C₁与点C₂，C₁与C₂的连线为中点与O″的连线与物体表面的交点记为O，n₁、n₂分别为点O、C₁处的法向量，n₁、n₂的夹角为α；

假设物体与欠驱动手之间的摩擦力系数为f，因此可以确定接触点C₁与点C₂处的摩擦锥，摩擦锥顶角θ的大小由θ＝arctan(f)确定；

定义法向量夹角特征为：

F₁＝cos(α)＝cos(n₁,n₂)

约束方程为：

F₁＝cos(α)<cos(θ)；

力封闭原则对于一个稳定的抓取来说是一个必备的条件；如图1所示，点C₁和点C₂的连线为如果分别在点C₁和点C₂的摩擦锥范围以内，那么从这两点抓取就可以符合力封闭原则；如果点C₁和点C′₂；的连线不在摩擦锥范围以内，那么从这两点的抓取时不稳定的；然而，由于只能得到物体的部分点云，点C₂和点C′₂实际是不可见的，因此通过测量点C₁与点O处的法线夹角来替代；点O处的法线代表了抓取方向；

在理想情况下，点C₁和点C₂的连线应该要与点C₁和点C₂的法线共线，而点C₁和点C₂是关于点O所在的法线对称的，在理想情况下点C₁处的法线应该要与点O处的法线垂直，因此可以通过测量点C₁处的法线与点O处的法线的夹角来衡量是否满足力封闭原则；定义法向量夹角特征为：

F₁＝cos(α)＝cos(n₁,n₂)

其中n₁、n₂分别为点O、C₁处的法向量，由于必须在点C₁和点C₂的摩擦锥范围以内，得到如下的约束方程：

F₁＝cos(α)<cos(θ)；

(2)共面特征：

定义共面特征为：

F₂＝distance(n₁，n₂)

约束方程为：

F₂＝distance(n₁，n₂)<F_2max；

其中，n₁和n₂分别表示点O与点C₁法线；distance(n₁，n₂)表示n₁和n₂所在直线的距离；F_2max为临界值；

如图2所示，由点O′C₁C₂′构成的抓取方案显然是不可行的，这种情况下，即使点O′与点C₁的法向量垂直，当点C₁与点C₂′的连线仍然不与点C₁的法向量和点C₂′的法向量共线，为了排除这种情况，要求点O′与点C₁所在的直线的直线距离尽可能小(理想状态为0)；当点O′与点C₁所在的空间直线的直线距离足够小时，则其位置关系转换成如图由点OC₁C₂构成的抓取方案，显然这种抓取方案是可行的；定义

F₂＝distance(n₁，n₂)<F_2max

来描述点O′与点C₁所在的直线的直线距离，其必须小于一定的临界值F_2max，其中n₁和n₂分别表示点O′与点C₁法线，求解n₁和n₂的距离实则为求解n₁、n₂所在的直线的距离；

通过使用法向量夹角特征和共面特征，可以使得本文提出的方法考虑到了力封闭原则；

(3)距离特征：

由于欠驱动手机构等方面的限制，两指之间的距离不能太大也不能太小，因此需要定义两指之间的距离特征；数学描述如下，通过求取点O到点C₁所在的直线的距离，既可以得到一个抓取点到欠驱动手中心距离，乘以2即为两个接触点之间的距离，即点C₁与点C₂之间的距离；

定义点O到点C₁所在的直线距离为d_pair；同时，对于一个欠驱动手，尽管欠驱动手能在一定的范围内实现抓取，然而欠驱动手有一个最合适的抓取距离，定义欠驱动手最合适的抓取距离为d_optimal，定义距离描述子为d＝|d_pair-d_optimal|；欠驱动手的最大距离为d_max，最小距离为d_min，则为了方便起见，不妨假设

令ω为欠驱动手两指之间的最大距离，得到如下的约束条件：

$d<\frac{1}{2}\mathrm{\&omega;};$

(4)抓取稳定性特征：

对于一个稳定的抓取，要求手爪与物体之间的摩擦力与物体的重力相平衡；这种情况下要求手爪的抓取中心与物体的重心之间的距离越小越好，这样的话物体重力产生的外力矩才更小；因此定义一个稳定距离来作为稳定性的评价指标，稳定距离为欠驱动手中心与物体重心之间的距离；

然而由于无法获得物体的全部三维点云，而只能获得其局部信息，因此无法真正的求解出物体的中心或者重心的位置；本文采用如下的简化方法，如图3可以看出，虽然无法获得其完整的点云信息，但是却可以通过Kinect传感器获得物体的局部三维点云，通过这局部三维点云找出物体在Kinect中的坐标系x轴、y轴和z轴方向上的最大和最小值，分别记为x_min、x_max、y_min、y_max、z_min、z_max；认为在Kinect的坐标系中物体中心点坐标为 $x=\frac{x_{\min }+x_{max}}{2},y=\frac{y_{\min }+y_{max}}{2},z=\frac{z_{\min }+z_{max}}{2};$

定义共面特征为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}$

其中x_g、y_g、z_g为在Kinect的坐标系中物体重心的坐标；

约束方程为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}<F_{3max};$

其中，F_3max为F₃对应的阈值；

(5)碰撞检测特征：

首先是通过对称的方法获得两个抓取点，在两个抓取点外侧分别建立两个圆柱体，圆柱体的半径大于欠驱动手的手指的半径，圆柱体的方向与抓取方向相同；

碰撞检测特征是通过计算点云中的点落入到两个圆柱体中的个数来计算的，分别记为h₁、h₂；

如果两个圆柱体中都没有点，那么这个抓取方案在抓取过程中很有可能就不会与物体发生碰撞；反之，当圆柱体中有点时，则可能与物体发生碰撞；然而，由于点云存在一定的噪声，有时候尽管不会产生碰撞，圆柱体中也会有少量的点云；因此，有必要通过机器学习的方法来学习碰撞检测特征并获得合理的阀值h_1max和h_2max；即h₁<h_1max、h₂<h_2max；

通过以上5点特征，可以确定出一个稳定的抓取；第1、2个特征保证了抓取的力封闭原则，第3、5个特征保证了这种抓取方案对本研究中所使用的欠驱动手来说是可行的，第4个特征则保证了抓取过程的稳定性，保证了抓取的质量；

步骤3：基于高斯过程分类建立抓取规划方案；

步骤4：按照抓取方案驱动欠驱动手进行抓取，然后根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体。

具体实施方式二：

本实施方式步骤1所述的对点云进行滤波处理的过程的具体步骤如下：

步骤1.1、使用半径离群点移除滤波器(RadiusOutlierRemoval滤波器)移除离群点；

通过使用PCL库提供的半径离群点移除滤波器可以去除由于噪声产生的少量离群点；滤波过程如下，假设点A是需要通过滤波的点，则首先采用Kd_tree的搜索算法统计出以点A为中心，r为半径的球内部的点的总数，当点数小于阀值n时，则认为其是离群点；

步骤1.2、使用平均值滤波器使得物体的表面更为平滑。

通过使用平均值滤波器可以去除白噪声的影响；滤波过程如下，假设点A是需要通过滤波的点，则首先采用Kd_tree的搜索算法求取出以点A为中心，r为半径的球内部的点，将这些点的坐标值取平均数作为点A的坐标；通过循环的方法对点云中所有的点都进行滤波。

其他步骤和参数与具体实施方式一相同。

具体实施方式三：

本实施方式步骤3所述基于高斯过程分类建立抓取规划方案的具体步骤如下：

求取完以上各个特征以后，可以通过有教师的机器学习方法来获得抓取方案；通过采用高斯过程分类器这一机器学习方法来获得抓取方案的原因：1)实际特征与理想特征之间的误差是由噪声产生的，因此他们服从高斯分布；所以这些误差都可以被高斯过程所学习；2)相对比支持向量机和神经网络来说，高斯过程分类器构造更为简单，只需要确定其核函数和均值函数即可，同时使用的参数更少，这使得参数优化更为简单，同时参数也更容易收敛；3)高斯过程分类器不仅可以给出分类的结果，而且给出其概率的大小，这个概率在本研究中也就代表抓取成功的概率，这为后面的排序算法创造了条件；

步骤3.1、基于所述的抓取特征，通过一个六维向量来描述一对抓取点的抓取情况，设X_i∈R⁶为抓取特征向量；抓取特征向量中的每一个元素分别对应于法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征，由于碰撞检测特征需要检测两个接触点的碰撞检测特征，因此碰撞检测特征对应抓取特征向量中的两个元素；

步骤3.2、通过实际抓取判断抓取点是否合适抓取，利用高斯过程分类器获得训练结果；

步骤3.3、通过高斯过程分类器计算得出各抓取点适合抓取的概率，在0到1之间变化，概率越大抓取成功率越高；并按概率从高到低指定抓取方案。

其他步骤和参数与具体实施方式一或二相同。

具体实施方式四：

本实施方式步骤3.2所述利用高斯过程分类器获得训练结果的具体步骤如下：

步骤3.2.1、将物体放置在Kinect的可视范围以内，通过Kinect采集物体的点云，采用随机采样法从物体的点云中采集两点作为一对抓取点，并计算相应的抓取特征向量X_i∈R⁶，并将这对抓取点在RGB-D点云中绘制出来，通过实际抓取对抓取点进行判别，如图4所示，当抓取点适合抓取时，令输出Y_i＝1；如图5所示，当抓取点不适合抓取时，令输出Y_i＝-1；

步骤3.2.2、按照步骤3.2.1的操作，重复多次，获得训练集

步骤3.2.3、根据训练集验证点云中所有的点是否适合抓取，并不断实时在线更新训练集的训练结果。

其他步骤和参数与具体实施方式一至三之一相同。

具体实施方式五：

本实施方式步骤4所述根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体的具体步骤如下：

步骤4.1、通过欠驱动的运动学、准静力学和高斯过程分类器，确定抓取轨迹，并对抓取轨迹进行优化，使得实际抓取点与物体抓取点之间的距离最小；根据优化后的抓取轨迹对物体进行抓取；

步骤4.2、驱动手指按照轨迹接触抓取物，基于接触检测的抓取控制器完成抓取过程；抓取过程主要分为三个阶段：预抓取阶段、夹紧阶段、释放阶段；释放阶段完成后，抓取过程结束。

其他步骤和参数与具体实施方式一至四之一相同。

具体实施方式六：

本实施方式步骤4.2所述预抓取阶段、夹紧阶段、释放阶段具体如下：

预抓取阶段是指两个欠驱动手手指没有都与物体接触时的情况，此时欠驱动手按照指定的方式进行运动；

夹紧阶段是在预抓取的基础上进行的；当两个欠驱动手手指都与物体接触时，两个手指的欠驱动关节开始运动对物体进行夹紧，并不断读取欠驱动手关机的转角以及驱动电流，并通过高斯过程回归检测电流变化，当电流满足以下式子时认为物体已经夹紧，当欠驱动手夹紧物体后，停止运动；

$\left|\right|I_{{\mathrm{\&alpha;}}^{\&prime;}}\left|\right|>I_{\lim it}={\mathrm{\&omega;}}^{\&prime;}\&CenterDot;g_{cr}+\left|\right|{\hat{I}}_{{\mathrm{\&alpha;}}^{\&prime;}}\left|\right|$

其中，α′为欠驱动单元的转角，I_α′为相应时刻的电流，为相应时刻通过高斯过程回归计算的电流的理论平均值，ω′为对应夹紧判据所对应的权重；I_limit表示的值，g_cr为检测电流偏差最大值；

释放阶段，当欠驱动手控制器接收到释放命令以后，欠驱动手的舵机以及电机均运动到零位，从而完成物体的释放。

其他步骤和参数与具体实施方式一至五之一相同。

Claims (6)

1.基于立体视觉的机器人欠驱动手自主抓取方法，其特征在于包括下述步骤：

步骤1、针对所要抓取的物体及其环境，通过Kinect传感器获取物体及环境的RGB-D点云，并对点云进行滤波处理；

步骤2、针对RGB-D点云，提取法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征和相应的约束方程；

(1)法向量夹角特征：

在抓住物体时，欠驱动手的两指是对称的，两指的分支点为O″，假设两指与物体的接触点为点C₁与点C₂，C₁与C₂的连线为中点与O″的连线与物体表面的交点记为O，n₁、n₂分别为点O、C₁处的法向量，n₁、n₂的夹角为α；

假设物体与欠驱动手之间的摩擦力系数为f，确定接触点C₁与点C₂处的摩擦锥，摩擦锥顶角θ的大小由θ＝arctan(f)确定；

定义法向量夹角特征为：

F₁＝cos(α)＝cos(n₁,n₂)

约束方程为：

F₁＝cos(α)<cos(θ)；

(2)共面特征：

定义共面特征为：

F₂＝distance(n₁，n₂)

约束方程为：

F₂＝distance(n₁，n₂)<F_2max；

其中，n₁和n₂分别表示点O与点C₁法线；distance(n₁，n₂)表示n₁和n₂所在直线的距离；F_2max为临界值；

(3)距离特征：

定义点O到点C₁所在的直线距离为d_pair；定义欠驱动手最合适的抓取距离为d_optimal，定义距离描述子为d＝|d_pair-d_optimal|；欠驱动手的最大距离为d_max，最小距离为d_min，假设 $d_{optimal}=\frac{d_{max}-d_{min}}{2};$

令ω为欠驱动手两指之间的最大距离，得到如下的约束条件：

$d<\frac{1}{2}\mathrm{\&omega;};$

(4)抓取稳定性特征：

稳定距离为欠驱动手中心与物体重心之间的距离；

通过Kinect传感器获得物体的局部三维点云，通过这局部三维点云找出物体在Kinect中的坐标系x轴、y轴和z轴方向上的最大和最小值，分别记为x_min、x_max、y_min、y_max、z_min、z_max；认为在Kinec的坐标系中物体中心点坐标为 $z=\frac{z_{\min }+z_{max}}{2};$

定义共面特征为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}$

其中x_g、y_g、z_g为在Kinect的坐标系中物体重心的坐标；

约束方程为：

$F_3=\sqrt{{(x-x_g)}^2+{(y-y_g)}^2+{(z-z_g)}^2}<F_{3max};$

其中，F_3max为F₃对应的阈值；

(5)碰撞检测特征：

首先是通过对称的方法获得两个抓取点，在两个抓取点外侧分别建立两个圆柱体，圆柱体的半径大于欠驱动手的手指的半径，圆柱体的方向与抓取方向相同；

碰撞检测特征是通过计算点云中的点落入到两个圆柱体中的个数来计算的，分别记为h₁、h₂；

通过机器学习的方法来学习碰撞检测特征并获得阀值h_1max和h_2max；即h₁<h_1max、h₂<h_2max；

步骤3：基于高斯过程分类建立抓取规划方案；

步骤4：按照抓取方案驱动欠驱动手进行抓取，然后根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体。

2.根据权利要求1所述的基于立体视觉的机器人欠驱动手自主抓取方法，其特征在于步骤1所述的对点云进行滤波处理的过程的具体步骤如下：

步骤1.1、使用半径离群点移除滤波器移除离群点；

步骤1.2、使用平均值滤波器使得物体的表面更为平滑。

3.根据权利要求4所述的基于立体视觉的机器人欠驱动手自主抓取方法其特征在于步骤3所述基于高斯过程分类建立抓取规划方案的具体步骤如下：

步骤3.1、基于所述的抓取特征，通过一个六维向量来描述一对抓取点的抓取情况，设X_i∈R⁶为抓取特征向量；抓取特征向量中的每一个元素分别对应于法向量夹角特征、共面特征、距离特征、抓取稳定性特征及碰撞检测特征，碰撞检测特征需要检测两个接触点的碰撞检测特征，碰撞检测特征对应抓取特征向量中的两个元素；

步骤3.2、通过实际抓取判断抓取点是否合适抓取，利用高斯过程分类器获得训练结果；

步骤3.3、通过高斯过程分类器计算得出各抓取点适合抓取的概率；并按概率从高到低指定抓取方案。

4.根据权利要求3所述的基于立体视觉的机器人欠驱动手自主抓取方法，其特征在于步骤3.2所述利用高斯过程分类器获得训练结果的具体步骤如下：

步骤3.2.1、将物体放置在Kinect的可视范围以内，通过Kinect采集物体的点云，采用随机采样法从物体的点云中采集两点作为一对抓取点，并计算相应的抓取特征向量X_i∈R⁶，并将这对抓取点在RGB-D点云中绘制出来，通过实际抓取对抓取点进行判别，当抓取点合适抓取时，令输出Y_i＝1；当抓取点不合适抓取时，令输出Y_i＝-1；

步骤3.2.2、按照步骤3.2.1的操作，重复多次，获得训练集

步骤3.2.3、根据训练集验证点云中所有的点是否适合抓取，并不断实时在线更新训练集的训练结果。

5.根据权利要求4所述的基于立体视觉的机器人欠驱动手自主抓取方法，其特征在于步骤4所述根据电流检测判断欠驱动手是否已经抓住物体，直至欠驱动手抓住物体，完成抓取任务后释放物体的具体步骤如下：

步骤4.1、通过欠驱动的运动学、准静力学和高斯过程分类器，确定抓取轨迹，并对抓取轨迹进行优化，使得实际抓取点与物体抓取点之间的距离最小；根据优化后的抓取轨迹对物体进行抓取；

步骤4.2、驱动手指按照轨迹接触抓取物，基于接触检测的抓取控制器完成抓取过程；抓取过程主要分为三个阶段：预抓取阶段、夹紧阶段、释放阶段；释放阶段完成后，抓取过程结束。

6.根据权利要求5所述的基于立体视觉的机器人欠驱动手自主抓取方法，其特征在于步骤4.2所述预抓取阶段、夹紧阶段、释放阶段具体如下：

预抓取阶段是指两个欠驱动手手指没有都与物体接触时的情况，此时欠驱动手按照指定的方式进行运动；

夹紧阶段是在预抓取的基础上进行的；当两个欠驱动手手指都与物体接触时，两个手指的欠驱动关节开始运动对物体进行夹紧，并不断读取欠驱动手关机的转角以及驱动电流，并通过高斯过程回归检测电流变化，当电流满足以下式子时认为物体已经夹紧，当欠驱动手夹紧物体后，停止运动；

$\left|\right|I_{{\mathrm{\&alpha;}}^{\&prime;}}\left|\right|>I_{\lim it}={\mathrm{\&omega;}}^{\&prime;}\&CenterDot;g_{cr}+\left|\right|{\hat{I}}_{{\mathrm{\&alpha;}}^{\&prime;}}\left|\right|$

其中，α′为欠驱动单元的转角，I_α′为相应时刻的电流，为相应时刻通过高斯过程回归计算的电流的理论平均值，ω′为对应夹紧判据所对应的权重；I_limit表示的值，g_cr为检测电流偏差最大值；

释放阶段，当欠驱动手控制器接收到释放命令以后，欠驱动手的舵机以及电机均运动到零位，从而完成物体的释放。