CN116749198A - 一种基于双目立体视觉引导机械臂抓取方法 - Google Patents

Abstract

本发明提出了一种基于双目立体视觉引导机械臂抓取方法，包括步骤如下：S1、搭建眼在手外的手眼标定系统；S2、双目立体视觉标定；S3、扫描得到模型点云并记录当前机械臂夹取的位姿参数；S4、扫描实际场景得到实际场景点云；S5、将实际场景点云与模型点云进行点云匹配计算，得到模型点云向场景点云的变换矩阵；S6、将示教抓取位姿经过模型点云向场景点云变换矩阵变换计算，得到场景点云下的抓取位姿；S7、将场景点云下的抓取位姿坐标发送给机械臂，机械臂根据抓取位姿坐标实现场景下的物体抓取。本发明通过记录模型点云和示教位姿后通过点云匹配方式实现场景下的点云抓取，场景环境对计算过程影响较小，能够适用于不同场景下的抓取任务。

Description

一种基于双目立体视觉引导机械臂抓取方法

技术领域

本发明涉及机械臂智能抓取技术领域，尤其涉及一种基于双目立体视觉引导机械臂抓取方法。

背景技术

机械臂抓取在工业场景中有着广泛的应用，传统的实现方式是采用离线示教并通过预先设定运动轨迹来控制机械臂完成抓取动作，这种实现形式严重依赖于机械臂工作环境的结构化程度和抓取任务的可重复性，无法适用于现代柔性制造中的多规格产品的动态生产工艺。为了让机械臂的动作更具灵活性和主观性，通过机器视觉与机器臂相结合，给机械臂提供感知周围环境的能力，提高机械臂智能化水平，使其功能更加完备化。传统的机器视觉系统主要是2D视觉，通过采集二维图像配合机械臂完成一些简单的引导定位工作。但2D视觉缺乏深度信息，需要将工件平铺分开摆放，同时要求工件不能有较大倾斜，难以实现三维空间上的高精度识别和定位，发展遇到瓶颈。为了弥补2D视觉的缺点，近年来3D视觉逐渐应用在生产线上并成为当前热门的研究领域。

抓取操作是机器人最频繁的动作之一，但是如何解决机械臂的自主抓取问题一直是工业机器人和服务机器人的应用痛点。抓取目标的位姿识别与定位是实现机械臂自主抓取的必要前提，通过在机械人上添加3D视觉硬件，可以获取待检场景中的目标点云，如公开号为CN112509063A的专利，公开了一种基于边缘特征匹配的机械臂抓取系统及方法，其是将目标点云经过算法分析后得到目标位姿，虽然3D视觉技术能够直接给机器人提供位姿信息，但实际应用中存在着点云数据量大、点云处理耗时、点云提取位姿方法不能满足变换场景等问题，难以满足日益增长的柔性生产需求。现阶段工业上大多数3D视觉技术在机器人中的应用还处于初级尝试摸索阶段，未能得到大规模普遍应用。

发明内容

有鉴于此，本发明提出了一种基于双目立体视觉引导机械臂抓取方法，用于解决3D视觉中点云数据量大、处理耗时、提取位姿方法不能满足变换场景的问题。

本发明的技术方案是这样实现的：

本发明提供了一种基于双目立体视觉引导机械臂抓取方法，包括步骤如下：

S1、搭建眼在手外的手眼标定系统；

S2、双目立体视觉标定，用来计算双目立体视觉中两个相机的内参、畸变及相对位置；

S3、将待测物体移动到双目立体视觉视野中，计算出当前视野下的模型点云，然后调节机械臂的夹爪到期望夹取的位置夹取物体模型，记录当前机械臂夹取的位姿参数；

S4、扫描实际场景得到实际场景点云，并从实际场景点云中提取出待夹取的物体点云数据；

S5、将实际场景点云与模型点云进行点云匹配计算，得到模型点云向场景点云的变换矩阵；

S6、在步骤S5得到的变换矩阵和步骤S3记录的模型点云示教抓取位姿基础上，将示教抓取位姿经过模型点云向场景点云变换矩阵变换计算，得到场景点云下的抓取位姿；

S7、将步骤S6中计算得到的场景点云下的抓取位姿坐标发送给机械臂，机械臂运动到指定位置后，根据抓取位姿坐标实现场景下的物体抓取。

在上述技术方案的基础上，优选的，在步骤S1中，根据待检物体大小和所在平面范围以及安装高度，选择合适的相机型号，确保双目立体视觉的扫描视野能够覆盖整个目标物体所在空间。

在上述技术方案的基础上，优选的，在步骤S2中，选用圆点阵列标定板进行双目立体视觉标定，将圆点阵列标定板正对相机并摆放在相机前聚焦位置附近，通过双目相机采集不同摆放位置下的圆点阵列标定板图像，计算出每张标定板的圆点图像坐标，将圆点阵列标定板的世界坐标和每个摆放姿态的图像坐标代入相机模型中，通过非线性优化计算出相机的内参、畸变和两个相机之间的旋转平移矩阵；

其中m_c表示每个摆放姿态下的圆点阵列图像坐标，m_w表示标定板圆点世界坐标经过相机模型投影后的图像坐标，K表示相机内参，k表示镜头畸变参数，r表示两个相机之间的旋转矩阵，t表示两个相机之间的平移矩阵，X_w表示圆点世界坐标。

在上述技术方案的基础上，优选的，在步骤S3中，将当前视野下扫描出的模型点云进行背景点云去除，并进一步对模型点云进行滤波操作，只保留显著特征的部分模型点云。

进一步，优选的，对模型点云进行的滤波操作采用曲率采样方式实现，根据曲面的曲率形式可以得到高斯曲率，通过设置高斯曲率阈值，保留高于阈值的点，得到经过曲率滤波后的点模型点云。

在上述技术方案的基础上，优选的，步骤S4中扫描并获取实际场景中物体点云的方法和步骤S3中扫描并获取模型点云的方法相同。

在上述技术方案的基础上，优选的，在步骤S5中，点云匹配过程分为点云粗匹配和点云精匹配，其中，点云粗匹配采用采样一致性初始配准算法，点云精匹配以点云粗匹配的结果为基础，采用迭代最近点算法求解。

进一步，优选的，点云粗匹配采用采样一致性初始配准算法，算法实现步骤如下：

第一步，从模型点云P中选取n个采样点，计算点特征直方图FPFH特征，FPFH是利用已估计出的点云法向特征，计算出该点与其邻域点的空间差异，并形成一个多维直方图对邻域几何属性进行描述；

第二步，在场景点云Q中查找与模型点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为模型点云P在场景点云Q中的一一对应点；

第三步，计算对应点之间旋转平移变换矩阵，通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能，找到使误差函数为最小值时的一组变换为最优变换。

在上述技术方案的基础上，优选的，点云精匹配方法是在传统点到点的最近点迭代算法上改进得到，将粗匹配结果作为精匹配的初始值，算法实现步骤如下：

第一步，将模型点云P经过粗匹配的旋转平移变换得到坐标系变换后的点云P′，将点云P′和场景点云Q作为精匹配的初始点集；

第二步，对点云P′中的每一点p_i在场景点云Q中寻找距离最近的对应点q_i，作为该点的对应点，组成初始对应点对；

第三步，采用方向向量阈值剔除错误对应点对，计算各对应点对法向量夹角，给定某一阈值，如果方向向量夹角小于某一阈值，则认为是正确的点对；

第四步，计算点云匹配中的旋转矩阵R和平移矩阵T，其计算过程为：使公式中d_k值最小时，对应的旋转矩阵R和平移矩阵T为所求匹配结果，其中N表示对应点个数，d_k表示第k次迭代对应点距离误差；

第五步，设定某一阈值ε＝d_k-d_k-1和最大迭代次数N_max，将上一步得到的刚体变换作用点云P′，得到新点云p″，计算p″和Q的距离误差，如果两次的迭代误差小于阈值ε或者当前迭代次数大于N_max，则结束迭代，否则将初始配准的点集更新为p″和Q，继续重复上述步骤，直至满足收敛条件，当达到收敛条件时得到的R和T即为精匹配结果，R和T改写成齐次矩阵形式为H_P ^Q。

在上述技术方案的基础上，优选的，在步骤S6中，将点云精匹配结果作用于模型点云的示教位姿，经过旋转平移变换得到场景下的抓取位姿，所述点云精匹配结果是在相机坐标系cam下，需要转换为机械臂基坐标系base下的匹配结果，相机坐标系cam向机械臂基坐标系base转换的齐次矩阵为机械臂基坐标系下的模型点云向场景点云转换矩阵获取步骤S3中模型点云抓取位姿为旋转矢量(r_xmodel,r_ymodel,r_zmodel)转换为旋转矩阵后与平移矩阵(t_xmodel,t_ymodel,t_zmodel)写成齐次矩阵形式为H_poseTeach，场景点云对应的抓取位姿齐次矩阵/>齐次矩阵H_poseGrab中取出旋转矩阵后变换为旋转矢量(r_xgrab,r_ygrab,r_zgrab)，取出H_poseGrab中的平移矩阵(t_xgrab,t_ygrab,t_zgrab)，合并得到抓取位姿(r_xgrab,r_ygrab,r_zgrab,t_xgrab,t_ygrab,t_zgrab)。

本发明相对于现有技术具有以下有益效果：

(1)本发明首先搭建眼在手外的手眼系统，通过双目立体视觉扫描待测物体点云建立模型点云，并记录下模型点云对应的机械臂位姿，然后通过采集实际场景中的点云，并经过点云处理后将模型点云与场景点云匹配，将匹配转换位姿作用于模型点云对应的机械臂位姿，即可得到场景点云中的抓取位姿。本发明采用眼在手外的双目视觉引导机械臂抓取方式，通过记录模型点云和示教位姿后通过点云匹配方式实现场景下的点云抓取，操作流程简单，能够在柔性化工业场景中应用；

(2)本发明在点云背景去除的基础上，采用点云滤波方法仅保留点云特征明显的数据，相比原始点云数据，大大减少了点云计算量，同时在计算中使用OMP并行加速和Kdtree快速查找方法，进一步提高计算速度，减少计算耗时；此外，本发明公开的方法仅记录模型点云数据和模型抓取位姿，通过点云匹配方法实现场景下抓取位姿计算，操作流程简单，场景环境对计算过程影响较小，能够适用于不同场景下的抓取任务；

(3)对点云进行背景去除和高斯曲率滤波处理，得到的滤波点云不仅数据量大大减少，而且还保留原点云特征信息，可以缩短计算耗时；

(4)通过使点云匹配过程分为点云粗匹配和点云精匹配两步，其中，点云粗匹配可以在点云相对位姿完全未知的情况下对点云进行配准，点云粗匹配为点云精匹配提供良好的初始值，同时点云精匹配可以在点云粗匹配基础上使点云之间的空间位置差异最小化，提高点云匹配精准度；

(5)采用一致性配准算法和最近点迭代算法能够快速计算出匹配结果，计算过程中采用OMP并行加速和Kdtree快速搜索方法，提高点云计算速度并快速得到匹配结果。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明公开的基于双目立体视觉引导机械臂抓取方法的流程图；

图2为本发明公开的双目立体视觉引导机械臂抓取示意图；

图3为本发明公开的通过圆点阵列标定板进行双目相机标定；

图4为本发明公开的点云采集及处理示意图像；

图5为本发明公开的点云匹配的示意图像。

具体实施方式

下面将结合本发明实施方式，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。

如图1，结合附图2-5所示，本发明实施例公开了一种基于双目立体视觉引导机械臂抓取方法，包括步骤如下：

S1、搭建眼在手外的手眼标定系统，眼在手外指的是将双目立体视觉安装在机械臂外的固定支架上，这种安装方式与双目立体视觉装置安装在机械臂上的眼在手上方式相比，机械臂的运动空间会更加灵活，不必考虑双目视觉装置与机械臂发生运动干涉的情况。

手眼标定指的是双目立体视觉与机械臂基座的相对位置转换关系，通过手眼标定结果可以将双目立体视觉中的点云坐标统一到机械臂基座坐标系中，双目立体视觉装置在安装时要确保视野范围能够覆盖整个目标物体所在空间。

作为一些优选实施方式，参照附图2所示，在双目立体视觉装置安装时，可以根据待检物体大小和所在平面范围以及安装高度，选择合适的相机型号，确保双目立体视觉的扫描视野能够覆盖整个目标物体所在空间。

S2、双目立体视觉标定，用来计算双目立体视觉中两个相机的内参、畸变及相对位置。

本发明所公开的双目立体视觉标定指的是求解双目立体视觉中两个相机的内参、畸变和相对位置关系。

具体的，参照附图3所示，本发明选用圆点阵列标定板进行双目立体视觉标定，不同姿态下采集得到的标定板图像圆点中心坐标经图像处理和椭圆拟合后计算得出，进一步根据相机成像模型和非线性优化方法计算出相机的内参、畸变以及两个相机之间的旋转平移矩阵。

具体的，在选用圆点阵列标定板进行双目立体视觉标定过程中，圆点阵列标定板上圆点世界坐标事先已通过高精度设备测出。标定具体流程是将圆点阵列标定板正对相机并摆放在相机前聚焦位置附近，通过双目相机采集不同摆放位置下的圆点阵列标定板图像，计算出每张标定板的圆点图像坐标，进一步，将圆点阵列标定板的世界坐标和每个摆放姿态的图像坐标代入相机模型中，通过非线性优化计算出相机的内参、畸变和两个相机之间的旋转平移矩阵，如下公式：

其中m_c表示每个摆放姿态下的圆点阵列图像坐标，m_w表示标定板圆点世界坐标经过相机模型投影后的图像坐标，K表示相机内参，k表示镜头畸变参数，r表示两个相机之间的旋转矩阵，t表示两个相机之间的平移矩阵，X_w表示圆点世界坐标。

通过上述实施例方式计算得到的标定结果是后续用于计算视野中点云坐标的重要参数。

S3、将待测物体移动到双目立体视觉视野中，计算出当前视野下的模型点云，然后调节机械臂的夹爪到期望夹取的位置夹取物体模型，记录当前机械臂夹取的位姿参数。

在步骤S3中，由于扫描出的模型点云中包含背景点云，为了减小背景点云干扰，需要将当前视野下扫描出的模型点云进行背景点云去除。参照附图4所示，本发明中双目立体视觉采用眼在手外的安装方式，双目立体视觉装置安装在物体场景的正上方，背景点云去除可以直接通过在点云数据上设置Z方向距离阈值实现，仅仅保留距离阈值范围的点云作为模型点云。为了提高点云计算速度，模型点云采用滤波处理，仅仅保留模型上显著特征的部分。

对模型点云进行的滤波操作采用曲率采样方式实现，根据曲面的曲率形式可以得到高斯曲率，通过设置高斯曲率阈值，保留高于阈值的点，得到经过曲率滤波后的点模型点云，由此设置，这样得到的滤波点云不仅数据量大大减少，而且还保留原点云特征信息，可以缩短计算耗时。

本发明所涉及的点云滤波采用曲率采样方法，具体步骤如下：

设点集中与某一待测点p_i的直线距离最短的k个待测点，称为p_i的k邻域，记为N(p_i)。设二次曲面方程为：

f(x,y)＝a₀x²+a₁y²+a₂x+a₃y+a₄

在N(p_i)范围内，根据最小二乘原理并分别对系数求导可以求出拟合参数a₀,a₁,a₂,a₃,a₄。把曲面方程表示为参数方程的形式：

r(x,y)＝(x,y,a₀x²+a₁y²+a₂x+a₃y+a₄)

r_x,r_y,r_xx,r_yy,r_xy为曲面r(x,y)的偏微分形式，曲面的单位法向量为n，根据曲面的第一基本形式和第二基本形式推导出:

E＝r_x·r_x

F＝r_x·r_y

G＝r_y·r_y

L＝r_xx·n

M＝r_xy·n

N＝r_yy·n

根据曲面的曲率形式可以得到高斯曲率K，公式如下：

本发明中将点云的高斯曲率平均值作为高斯曲率阈值，仅仅保留大于阈值部分点云，这样一来，可以大大减少点云数量，同时也大大减少计算耗时。

当模型点云经过滤波处理完后需要单独保存作为后续匹配中的模型点云，同时需要调节机械臂的夹爪到合适位置夹取物体模型，并记录当前机械臂夹取的位姿参数(r_xmodel,r_ymodel,r_zmodel,t_xmodel,t_ymodel,t_zmodel)，作为后续计算场景点云抓取变换的基准位置，其中，(r_xmodel,r_ymodel,r_zmodel)为旋转矢量，(txmodel,tymodel,t_zmodel)为平移矩阵。

S4、扫描实际场景得到实际场景点云，并从实际场景点云中提取出待夹取的物体点云数据。其中，步骤S4中扫描并获取实际场景中物体点云的方法和步骤S3中扫描并获取模型点云的方法相同。在实际场景下的对物体进行扫描，扫描得到的场景点云同样包含着背景点云，点云去除背景和点云滤波操作与步骤S3中过程一致。

S5、将实际场景点云与模型点云进行点云匹配计算，得到模型点云向场景点云的变换矩阵。

在步骤S5中，点云匹配过程分为点云粗匹配和点云精匹配两步，其中，点云粗匹配是指在点云相对位姿完全未知的情况下对点云进行配准，点云粗匹配为点云精匹配提供良好的初始值。点云精匹配目的是在点云粗匹配基础上使点云之间的空间位置差异最小化，提高点云匹配精准度。参照附图5所示，图中，右下方图像为场景点云，左上方图像为模型点云，二者重叠部分图像为匹配结构。

本发明中点云粗匹配方法采用一致性初始配准算法，算法实现步骤如下：

第一步，从模型点云P中选取n个采样点，计算点特征直方图FPFH特征，FPFH是利用已估计出的点云法向特征，计算出该点与其邻域点的空间差异，并形成一个多维直方图对邻域几何属性进行描述；

第二步，在场景点云Q中查找与模型点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在模型点云Q中的一一对应点；

第三步，计算对应点之间旋转平移变换矩阵，通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能，找到使误差函数为最小值时的一组的变换为最优变换。旋转平移矩阵可以通过对应点坐标进行奇异值分解求解，距离误差和函数表示对应点经过旋转平移变换后对应点之间的距离误差，当物理误差和函数值最小时，认为当前计算得到的旋转平移矩阵结果最优。

在计算FPFH的过程中使用OMP库多线程加速，通过点云的特征描述子与Kdtree快速搜索相结合，可以获得较快的匹配速度。

本发明中点云精匹配方法是在传统点到点的最近点迭代算法上改进得到，将粗匹配结果作为精匹配的初始值，算法具体实现步骤如下：

第一步，将模型点云P经过粗匹配的旋转平移变换得到坐标系变换后的点云P′，将点云P′和场景点云Q作为精匹配的初始点集；

第二步，对点云P′中的每一点p_i在场景点云Q中寻找距离最近的对应点q_i，作为该点的对应点，组成初始对应点对；

第三步，采用方向向量阈值剔除错误对应点对，计算各对应点对法向量夹角，给定某一阈值，如果方向向量夹角小于某一阈值，则认为是正确的点对；

第四步，计算点云匹配中的旋转矩阵R和平移矩阵T，其计算过程为：使公式中d_k值最小时，对应的旋转矩阵R和平移矩阵T为所求匹配结果，其中N表示对应点个数，d_k表示第k次迭代对应点距离误差；

需要说明的是：点云匹配就是求两个点云之间的转换关系，转换关系包括旋转变换和平移变换，对应旋转矩阵R和平移矩阵T。

第五步，设定某一阈值ε＝d_k-d_k-1和最大迭代次数N_max，将上一步得到的刚体变换作用点云P′，得到新点云p″，计算p″和Q的距离误差，如果两次的迭代误差小于阈值ε或者当前迭代次数大于N_max，则结束迭代，否则将初始配准的点集更新为p″和Q，继续重复上述步骤，直至满足收敛条件，当达到收敛条件时得到的R和T即为精匹配结果，R和T改写成齐次矩阵形式为H_P ^Q。

S6、在步骤S5得到的变换矩阵和步骤S3记录的模型点云示教抓取位姿基础上，将示教抓取位姿经过模型点云向场景点云变换矩阵变换计算，得到场景点云下的抓取位姿。

具体的，在步骤S6中，将点云精匹配结果作用于模型点云的示教位姿，经过旋转平移变换得到场景下的抓取位姿，所述点云精匹配结果是在相机坐标系cam下，需要转换为机械臂基坐标系base下的匹配结果，相机坐标系cam向机械臂基坐标系base转换的齐次矩阵为机械臂基坐标系下的模型点云向场景点云转换矩阵/>获取步骤S3中模型点云抓取位姿为(r_xmodel,r_ymodel,r_zmodel,t_xmodel,t_ymodel,t_zmodel)，旋转矢量(r_xmodel,r_ymodel,r_zmodel)转换为旋转矩阵后与平移矩阵(txmodel,t_ymodel,t_zmodel)写成齐次矩阵形式为H_poseTeach，场景点云对应的抓取位姿齐次矩阵/>齐次矩阵H_poseGrab中取出旋转矩阵后变换为旋转矢量(r_xgrab,r_ygrab,r_zgrab)，取出H_poseGrab中的平移矩阵(t_xgrab,t_ygrab,t_zgrab)，合并得到抓取位姿(r_xgrab,r_ygrab,r_zgrab,t_xgrab,t_ygrab,t_zgrab)。

上述实施例中，在点云匹配过程中，采用一致性配准算法和最近点迭代算法能够快速计算出匹配结果，计算过程中采用OMP并行加速和Kdtree快速搜索方法，提高点云计算速度并快速得到匹配结果。

S7、将步骤S6中计算得到的场景点云下的抓取位姿(r_xgrab,r_ygrab,r_zgrab,t_xgrab,t_ygrab,t_zgrab)发送给机械臂，机械臂运动到指定位置后，根据抓取位姿坐标实现场景下的物体抓取。

本发明首先搭建眼在手外的手眼系统，通过双目立体视觉扫描待测物体点云建立模型点云，并记录下模型点云对应的机械臂位姿，然后通过采集实际场景中的点云，并经过点云处理后将模型点云与场景点云匹配，将匹配转换位姿作用于模型点云对应的机械臂位姿，即可得到场景点云中的抓取位姿。在点云背景去除的基础上，采用点云滤波方法仅保留点云特征明显的数据，相比原始点云数据，大大减少了点云计算量，同时在计算中使用OMP并行加速和Kdtree快速查找方法，进一步提高计算速度，减少计算耗时；此外，本发明公开的方法仅记录模型点云数据和模型抓取位姿，通过点云匹配方法实现场景下抓取位姿计算，操作流程简单，场景环境对计算过程影响较小，能够适用于不同场景下的抓取任务。

以上所述仅为本发明的较佳实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于双目立体视觉引导机械臂抓取方法，其特征在于，包括步骤如下：

S1、搭建眼在手外的手眼标定系统；

S2、双目立体视觉标定，用来计算双目立体视觉中两个相机的内参、畸变及相对位置；

S3、将待测物体移动到双目立体视觉视野中，计算出当前视野下的模型点云，然后调节机械臂的夹爪到期望夹取的位置夹取物体模型，记录当前机械臂夹取的位姿参数；

S4、扫描实际场景得到实际场景点云，并从实际场景点云中提取出待夹取的物体点云数据；

S5、将实际场景点云与模型点云进行点云匹配计算，得到模型点云向场景点云的变换矩阵；

S6、在步骤S5得到的变换矩阵和步骤S3记录的模型点云示教抓取位姿基础上，将示教抓取位姿经过模型点云向场景点云变换矩阵变换计算，得到场景点云下的抓取位姿；

S7、将步骤S6中计算得到的场景点云下的抓取位姿坐标发送给机械臂，机械臂运动到指定位置后，根据抓取位姿坐标实现场景下的物体抓取。

2.如权利要求1所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：在步骤S1中，根据待检物体大小和所在平面范围以及安装高度，选择合适的相机型号，确保双目立体视觉的扫描视野能够覆盖整个目标物体所在空间。

3.如权利要求1所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：在步骤S2中，选用圆点阵列标定板进行双目立体视觉标定，将圆点阵列标定板正对相机并摆放在相机前聚焦位置附近，通过双目相机采集不同摆放位置下的圆点阵列标定板图像，计算出每张标定板的圆点图像坐标，将圆点阵列标定板的世界坐标和每个摆放姿态的图像坐标代入相机模型中，通过非线性优化计算出相机的内参、畸变和两个相机之间的旋转平移矩阵，如下公式：

其中m_c表示每个摆放姿态下的圆点阵列图像坐标，m_w表示标定板圆点世界坐标经过相机模型投影后的图像坐标，K表示相机内参，k表示镜头畸变参数，r表示两个相机之间的旋转矩阵，t表示两个相机之间的平移矩阵，X_w表示圆点世界坐标。

4.如权利要求1所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：在步骤S3中，将当前视野下扫描出的模型点云进行背景点云去除，并进一步对模型点云进行滤波操作，只保留显著特征的部分模型点云。

5.如权利要求4所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：对模型点云进行的滤波操作采用曲率采样方式实现，根据曲面的曲率形式可以得到高斯曲率，通过设置高斯曲率阈值，保留高于阈值的点，得到经过曲率滤波后的点模型点云。

6.如权利要求1所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：步骤S4中扫描并获取实际场景中物体点云的方法和步骤S3中扫描并获取模型点云的方法相同。

7.如权利要求1所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：在步骤S5中，点云匹配过程分为点云粗匹配和点云精匹配，其中，点云粗匹配采用采样一致性初始配准算法，点云精匹配以点云粗匹配的结果为基础，采用迭代最近点算法求解。

8.如权利要求7所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：点云粗匹配采用采样一致性初始配准算法，算法实现步骤如下：

第一步，从模型点云P中选取n个采样点，计算点特征直方图FPFH特征，FPFH是利用已估计出的点云法向特征，计算出该点与其邻域点的空间差异，并形成一个多维直方图对邻域几何属性进行描述；

第二步，在场景点云Q中查找与模型点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为模型点云P在场景点云Q中的一一对应点；

第三步，计算对应点之间旋转平移变换矩阵，通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能，找到使误差函数为最小值时的一组的变换为最优变换。

9.如权利要求7所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：点云精匹配方法是在传统点到点的最近点迭代算法上改进得到，将粗匹配结果作为精匹配的初始值，算法实现步骤如下：

第一步，将模型点云P经过粗匹配的旋转平移变换得到坐标系变换后的点云P′，将点云P′和场景点云Q作为精匹配的初始点集；

第二步，对点云P′中的每一点p_i在场景点云Q中寻找距离最近的对应点q_i，作为该点的对应点，组成初始对应点对；

第三步，采用方向向量阈值剔除错误对应点对，计算各对应点对法向量夹角，给定某一阈值，如果方向向量夹角小于某一阈值，则认为是正确的点对；

第四步，计算点云匹配中的旋转矩阵R和平移矩阵T，其计算过程为：使公式中d_k值最小时，对应的旋转矩阵R和平移矩阵T为所求匹配结果，其中N表示对应点个数，d_k表示第k次迭代对应点距离误差；

第五步，设定某一阈值ε＝d_k-d_k-1和最大迭代次数N_max，将上一步得到的刚体变换作用点云P′，得到新点云p″，计算p″和Q的距离误差，如果两次的迭代误差小于阈值ε或者当前迭代次数大于N_max，则结束迭代，否则将初始配准的点集更新为p″和Q，继续重复上述步骤，直至满足收敛条件，当达到收敛条件时得到的R和T即为精匹配结果，R和T改写成齐次矩阵形式为

10.如权利要求9所述的基于双目立体视觉引导机械臂抓取方法，其特征在于：在步骤S6中，将点云精匹配结果作用于模型点云的示教位姿，经过旋转平移变换得到场景下的抓取位姿，所述点云精匹配结果是在相机坐标系cam下，需要转换为机械臂基坐标系base下的匹配结果，相机坐标系cam向机械臂基坐标系base转换的齐次矩阵为机械臂基坐标系下的模型点云向场景点云转换矩阵/>获取步骤S3中模型点云抓取位姿为(r_xmodel,r_ymodel,r_zmodel,t_xmodel,t_ymodel,t_zmodel)，旋转矢量(r_xmodel,r_ymodel,r_zmodel)转换为旋转矩阵后与平移矩阵(t_xmodel,t_ymodel,t_zmodel)写成齐次矩阵形式为H_poseTeach，场景点云对应的抓取位姿齐次矩阵/>齐次矩阵H_poseGrab中取出旋转矩阵后变换为旋转矢量(r_xgrab,r_ygrab,r_zgrab)，取出H_poseGrab中的平移矩阵(t_xgrab,t_ygrab,t_zgrab)，合并得到抓取位姿(r_xgrab,r_ygrab,r_zgrab,t_xgrab,t_ygrab,t_zgrab)。