CN114193440B - 基于3d视觉的机器人自动抓取***及方法 - Google Patents

Abstract

本发明公开了一种基于3D视觉的机器人自动抓取***及方法，包括抓取机器人向工控机发送请求数据指令，工控机接收请求数据指令判断是否缓存有处理结果，有则返回处理结果至抓取机器人，没有则控制3D相机拍照；工控机接收到抓取机器人的拍照指令则控制3D相机拍照；工控机接收原始三维点云数据，判断是否拍到目标零件；若拍到目标零件则基于原始三维点云数据，再进行点云处理计算得到零件空间位姿数据和状态信息，并缓存为处理结果；若未拍到目标零件则判断零件是否全部抓取完成，得到零件空间位姿数据和状态信息，并缓存为处理结果；抓取机器人根据处理结果，抓取零件、输出拍照指令和更换料框指令。本发明具有抓取成功率高和节拍快的优点。

Description

基于3D视觉的机器人自动抓取***及方法

技术领域

本发明涉及工业机器人、机器视觉技术领域，尤其涉及一种基于3D视觉的机器人自动抓取***及方法。

背景技术

传统的机器人应用是通过示教器来控制机器人完成预定的指令动作，但当机器人工作环境和工作任务发生改变时，机器人往往无法迅速适应这些变化，极大限制了工业机器人应用场景和应用效率。

随着机器视觉技术的发展，机器视觉技术逐渐应用到工业场景中。然而传统的二维视觉只能应用于平面物体的识别和定位，无法应对零件的倾斜、翻转等三维场景。

3D相机能够获取物体空间点云坐标，弥补传统二维视觉的不足，但三维点云数据量大，处理复杂，以及与机器人复杂的通讯逻辑，大大的限制了3D视觉的应用。例如：申请号为CN202010273493.1的中国发明专利“基于3D视觉与深度学习的智能搬运机械臂***及使用方法”提供了一种基于3D视觉与深度学习的智能搬运机械臂***，包括视觉检测模块、训练学习模块、运动规划模块以及控制模块。但该方法采用双目深度摄像头和RGB相机，并非直接采用三维点云数据，且基于深度学习的方法鲁棒性不高；申请号为CN202011256951.7的中国发明专利“一种基于3D视觉的机器人全自动无序上料***”基于3D视觉的机器人全自动无序上料***，包括上料装置、下料装置、摄影装置、活动机械手和中控***。但该方法需预先采集物料的种类及其空间六面特征，操作繁琐，效率不高。

因而，亟需解决上述问题。

发明内容

发明目的：本发明的第一目的是提供一种基于3D视觉的机器人自动抓取方法，该自动抓取方法具有抓取成功率高且具有节拍快和效率高的优点。

本发明的第二目的是提供一种基于3D视觉的机器人自动抓取***，该自动抓取***具有抓取成功率高且具有节拍快和效率高的优点。

技术方案：为实现以上目的，本发明公开了一种基于3D视觉的机器人自动抓取方法，包括如下步骤：

抓取机器人向工控机发送请求数据指令，工控机接收抓取机器人的请求数据指令，工控机判断是否缓存有处理结果，有则返回处理结果至抓取机器人，没有则控制3D相机拍照；工控机接收到抓取机器人的拍照指令时控制3D相机拍照；工控机接收3D相机返回的原始三维点云数据，基于原始三维点云数据，判断是否拍到目标零件，并输出判断结果；若拍到目标零件则基于目标零件的原始三维点云数据，再进行点云处理计算得到零件空间位姿数据和状态信息，并缓存为处理结果；若未拍到目标零件则另外判断零件是否全部抓取完成，得到零件空间位姿数据和状态信息，并缓存为处理结果；抓取机器人接收工控机返回的处理结果，根据处理结果，调整抓取点坐标与抓取角度并抓取零件、输出拍照指令和更换料框指令。

其中，若拍到目标零件，抓取机器人根据处理结果调整抓取点坐标与抓取角度并抓取零件，并向工控机发送拍照指令，然后抓取机器人将零件放置于工装板上；

若未拍到目标零件且未全部抓取完成，抓取机器人根据处理结果向工控机发送拍照指令；

若未拍到目标零件且全部抓取完成，抓取机器人根据处理结果向上料机器人发送更换料框指令。

优选的，判断是否拍到目标零件的具体方法为：

对原始三维点云数据进行点云滤波，得到滤波后的点云P′，获取模板零件对应的模板点云P_m，对比点云P′和模板点云P_m，点云P'中点的数量大于模板点云P_m中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件。

再者，点云滤波的具体方法为：

(1)采用统计滤波法对原始点云数据{P}进行滤波，去除离群点，得到点云{P₁’}；

(2)对点云{P₁’}进行体素降采样，得到点云{P₂’}；

(3)分割料框点云，采用直通滤波分割料框点云：设置供料台左上角为参考点，其在相机坐标系下的坐标(X，Y)，根据供料台高度H，料框高度h，料框长L，料框宽W，设定最大料框误差Δ＝{Δh，ΔL，ΔW}，则设置直通滤波参数为：

经过直通滤波得到滤波后的点云{P₃’}；

(4)去除料框平面，采用RANSAC平面模型分割方法去除料框平面，得到{P_c’}，

(4.1)计算法线，对点云{P₃’}计算法线，得到P₃‘_Normals；计算过程运用OpenMP技术进行加速；

(4.2)采用RANSAC方法进行平面分割；以点云{P₃’}及其法线P₃‘_Normals为输入，设置最大迭代次数M_itera＝1000，距离权重W＝0.1，距离阈值Δ＝12，提取出点云{P₃’}中平面点云{P’_plan}；

(4.3)去除平面，将点云{P’_plan}中的点从点云{P₃’}中去除，得到不含平面的点云{P_c’}；

(5)分割零件点云，根据料框左上角坐标(X′，Y’)，零件长l，宽w，摆放误差Δ‘＝{Δl’，Δw’}，对于第n次抓取，直通滤波参数为：

其中，N为抓取次数，C为料框列数，％为取余符号，[]为取整符号；依此分割出每次抓取零件的点云P’。

进一步，模板点云P_m的获取方法为：将抓取机器人移动到抓取点，在抓取点放置一个模板零件，使得机器人能够正确抓取；使用3D相机扫描模板零件，获得的模板零件点云即作为模板点云P_m。

优选的，再进行点云处理计算得到零件空间位姿数据和状态信息的具体方法为：采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；结合机器人手眼标定矩阵H，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1。

本发明一种基于3D视觉的机器人自动抓取***，包括工控机、3D相机和抓取机器人，

工控机，用于控制3D相机拍照，获取三维点云数据并进行处理，并将处理结果发送至抓取机器人；其包括，

指令接收模块，用于接收抓取机器人的请求数据指令和拍照指令；

数据缓存模块，用于存储状态信息和零件空间位姿数据为处理结果，并基于请求数据指令判断是否缓存有处理结果，有则将处理结果传输至抓取机器人，没有则输出拍照指令；

相机控制模块，用于基于拍照指令控制3D相机拍照，接收3D相机返回的原始三维点云数据；

确认零件模块，用于基于原始三维点云数据，判断是否拍到目标零件，拍到目标零件则输出判断结果至位姿计算模块，未拍到目标零件则输出判断结果至抓取状态模块；

位姿计算模块，用于基于确认零件模块的判断结果和原始三维点云数据，进行点云处理计算得到零件空间位姿数据和状态信息，并存储至数据缓存模块；

抓取状态模块，用于基于确认零件模块的判断结果，另外判断零件是否全部抓取完成，得到状态信息，并存储至数据缓存模块；

3D相机，用于拍照并输出原始三维点云数据至工控机；

抓取机器人，用于向工控机发送请求数据指令，接收工控机中数据缓存模块返回的处理结果；根据处理结果，调整抓取点坐标与抓取角度并抓取零件，输出拍照指令和更换料框指令。

再者，确认零件模块包括模板零件模块、点云滤波模块和对比模块，

模板零件模块，用于获取模板零件形成的模板点云P_m，

点云滤波模块，用于对原始三维点云数据进行点云滤波，得到滤波后的点云P'，

对比模块，用于对比点云P'和模板点云P_m，点云P'中点的数量大于模板点云P_m中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件。

进一步，所述位姿计算模块包括粗配准模块、精配准模块和坐标变换模块，

粗配准模块，用于采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；

精配准模块，用于采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；

坐标变换模块，用于结合机器人手眼标定矩阵H，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1。

优选的，还包括上料机器人、供料台、料框和工装板，

上料机器人，用于接收更换指令从上料区抓取料框，将其放置于供料台上；

供料台，用于放置料框并对料框进行粗定位；

料框，待抓取的零件依次放置于料框中，每个料框包含固定数量的方格，每个方格可放置一个零件；

工装板，抓取机器人抓取零件后，将其放置于工装板上；其中工控机和抓取机器人采用Socket通讯，抓取机器人与上料机器人采用IO通讯。

有益效果：与现有技术相比，本发明具有以下显著优点：

(1)当机器人到达放置点时，发送信号给工控机进行拍照，工控机同步进行拍照和点云处理，等机器人进行下一次抓取时，工控机已经处理好结果放在缓存中，机器人不需要再等待拍照+点云处理的时间，缩短节拍，提高效率；

(2)工控机获取三维点云数据并进行点云滤波、点云粗配准、点云精配准后计算得到零件空间位姿信息，抓取机器人根据零件空间位姿信息精准抓取零件，本方法抓取效率高、鲁棒性强，在零件无堆叠场景下具有良好的抓取效果。

附图说明

图1为本发明的结构示意图，其中：1、上料机器人；2、上料台；3、料框；4、供料台；5、上料机器人夹爪；6、3D相机；7、相机支架；8、抓取机器人；9、抓取机器人夹爪；10、工控机；11、工装板；

图2为本发明中料框及其零件摆放的示意图；

图3为本发明中工控机和抓取机器人的处理流程图；

图4为本发明中点云滤波的流程示意图。

具体实施方式

下面结合附图对本发明的技术方案作进一步说明。

如图1所示，本发明一种基于3D视觉的机器人自动抓取方法，包括如下步骤：

抓取机器人向工控机发送请求数据指令，工控机接收抓取机器人的请求数据指令，工控机判断是否缓存有处理结果，有则返回处理结果至抓取机器人，没有则控制3D相机拍照；

工控机接收到抓取机器人的拍照指令则控制3D相机拍照；工控机接收3D相机返回的原始三维点云数据，基于原始三维点云数据，判断是否拍到目标零件，并输出判断结果；其中判断是否拍到目标零件的具体方法为：

如图4所示，对原始三维点云数据进行点云滤波，得到滤波后的点云P′；点云滤波的具体方法为：

(1)采用统计滤波法对原始点云数据{P}进行滤波，去除离群点，得到点云{P₁’}；

(2)对点云{P₁’}进行体素降采样，得到点云{P₂’}；

(3)分割料框点云，采用直通滤波分割料框点云：设置供料台左上角为参考点，其在相机坐标系下的坐标(X，Y)，根据供料台高度H，料框高度h，料框长L，料框宽W，设定最大料框误差Δ＝{Δh，ΔL，ΔW}，则设置直通滤波参数为：

经过直通滤波得到滤波后的点云{P₃’}；

(4)去除料框平面，采用RANSAC平面模型分割方法去除料框平面，得到{P_c’}，

(4.1)计算法线，对点云{P₃’}计算法线，得到P₃‘_Normals；计算过程运用OpenMP技术进行加速；

(4.2)采用RANSAC方法进行平面分割；以点云{P₃’}及其法线P₃‘_Normals为输入，设置最大迭代次数M_itera＝1000，距离权重W＝0.1，距离阈值Δ＝12，提取出点云{P₃’}中平面点云{P’_plan}；

(4.3)去除平面，将点云{P’_plan}中的点从点云{P₃’}中去除，得到不含平面的点云{P_c’}；

(5)分割零件点云，根据料框左上角坐标(X′，Y’)，零件长l，宽w，摆放误差Δ‘＝{Δl’，Δw’}，对于第n次抓取，直通滤波参数为：

其中，N为抓取次数，C为料框列数，％为取余符号，[]为取整符号；依此分割出每次抓取零件的点云P’；

获取模板零件对应的模板点云P_m，模板点云P_m的获取方法为：将抓取机器人移动到抓取点，在抓取点放置一个模板零件，使得机器人能够正确抓取；使用3D相机扫描模板零件，获得的模板零件点云即作为模板点云P_m；

对比点云P′和模板点云P_m，点云P'中点的数量大于模板点云P_m中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件，优先实施例中阀值百分比设置为80％，即点云P'中点的数量大于模板点云P_m中点的数量的80％，认为包含目标零件，P′.Size()≥P_m.Size()*k,k＝0.8；

若拍到目标零件则基于目标零件的原始三维点云数据，再进行点云处理计算得到零件空间位姿数据和状态信息，并缓存为处理结果；其中再进行点云处理计算得到零件空间位姿数据和状态信息的具体方法为：

采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；采用RANSAC方法的具体步骤为：

计算法线：对点云P′与模板点云P_m分别计算法线，分别得到点云P’与模板点云P_m的法线P′_Normals和P_m_Normals；计算过程运用OpenMP技术进行加速；

计算FPFH描述子：根据点云P′与模板点云P_m，法线P′_Normals和P_m_Normals，分别计算点云P′与模板点云P_m的FPFH快速点云特征直方图，得到P′_FPFH和P_m_FPFH；计算过程运用OpenMP技术进行加速；

RANSAC粗配准：根据点云P′与模板点云P_m，以及对应的FPFH特征P′_FPFH和P_m_FPFH，采用RANSAC方法对模板点云P_m和点云P′进行粗配准，得到模板点云P_m到点云P′的位姿变换矩阵T_sac；

采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；ICP方法的具体步骤为：以粗配准得到的变换矩阵T_sac为初始位姿输入，设置ICP最大迭代次数M_itera＝1000，最大对应距离M_dis＝1000，迭代差值ε＝1e-10，最小误差ε_min＝0.0001；得到精配准之后的位姿变换矩阵T_icp；

结合机器人手眼标定矩阵H，手眼标定矩阵采用张正友标定法，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1；计算出零件空间位姿数据的具体步骤为：精配准后的位姿变换矩阵T_icp为相机坐标系下模板点云P_m到点云P′的坐标变换矩阵，将其转换到相机坐标系下，结合机器人到3D相机的手眼标定矩阵H，机器人坐标系下模板点云P_m到点云P′的坐标变换矩阵T_robot＝HT_icpH^-1；提取T_robot的平移分量(x,y,z)，并将其旋转分量转化为欧拉角(A，B，C)，即得到机器人坐标系下模板点云P_m到点云P′六自由度位姿变换参数Pos＝{x，y，z，A，B，C}；

若未拍到目标零件则另外判断零件是否全部抓取完成，得到零件空间位姿数据和状态信息，并缓存为处理结果；

若拍到目标零件，抓取机器人接收工控机返回的处理结果，根据处理结果调整抓取点坐标与抓取角度并抓取零件，并向工控机发送拍照指令，然后抓取机器人将零件放置于工装板上；

若未拍到目标零件且未全部抓取完成，抓取机器人接收工控机返回的处理结果，根据处理结果向工控机发送拍照指令；

若未拍到目标零件且全部抓取完成，抓取机器人接收工控机返回的处理结果，根据处理结果向上料机器人发送更换料框指令。

如图3所示，本发明一种基于3D视觉的机器人自动抓取方法，服务端工控机的数据处理流程如下：

步骤1：初始化抓取次数N＝0，开启Socket服务；

步骤2：等待机器人指令；

步骤3：是否接收到请求数据指令GetPos:C1T1，如果是，则跳转至步骤4；否则跳转至步骤7；

步骤4：判断当前缓存是否存在数据，如果存在，将缓存数据发送至抓取机器人，并清空缓存，跳转至步骤5；如果不存在，跳转至步骤7；

步骤5：将缓存数据发送至抓取机器人；

步骤6：清空缓存，跳转至步骤2；

步骤7：判断是否接收到拍照指令GetPos:C1T2，如果是，则跳转至步骤8；否则跳转至步骤2；

步骤8：工控机控制3D相机拍照，获取原始三维点云数据集{P}；

步骤9：对获取到的原始三维点云数据进行点云滤波，得到滤波后的点云P′；

步骤10：更新抓取次数N＝N+1；

步骤11：判断滤波后的点云P′是否包含目标零件，如果包含，跳转至步骤12；否则跳转至步骤16；

步骤12：点云粗配准，采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；

步骤13：点云精配准，采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数为T_sac，求得模板点云到目标点云的位姿变换矩阵S；

步骤14：坐标变换。结合机器人手眼标定矩阵H，计算出零件六自由度位姿Pos＝{x，y，z，A，B，C}，其中x，y，z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1；

步骤15：将状态信息Status和零件空间位姿数据Pos写入缓存，跳转至步骤2。

步骤16：判断零件是否抓取完成，如果未完成，即S<N-1，设置状态信息Status＝2，零件空间位姿信息Pos＝0；跳转至步骤15；否则设置状态信息Status＝3，零件空间位姿信息Pos＝0，重置抓取次数N＝0，跳转至步骤15。

如图3所示，本发明中一种基于3D视觉的机器人自动抓取方法，客户端抓取机器人的数据处理流程如下：

步骤1、抓取机器人运动到Home点；

步骤2、建立通讯：抓取机器人与工控机建立Socket通讯；

步骤3、发送指令：抓取机器人向工控机发送请求数据指令GetPos:C1T1，工控机接收到指令后执行数据处理，并将处理结果返回至抓取机器人；

步骤4、接收数据：抓取机器人接收工控机返回的结果数据，结果数据包含处理状态Status以及零件空间位姿信息Pos。

步骤5、数据解析：如果Status＝1，表示成功获取零件空间位姿信息，跳转至步骤6；如果Status＝2，表示本次拍照未识别到零件，抓取机器人向工控机重新发送拍照指令GetPos:C1T2，跳转至步骤1；如果Status＝3，表示当前料框未含有零件或已经全部抓取，抓取机器人向上料机器人发送更换料框IO信号，并跳转至步骤1；

步骤6、抓取零件：抓取机器人移动到抓取点，并根据步骤4中获取的Pos数据调整零件抓取位置和角度，抓取零件；

步骤7、重新拍照：抓取机器人抓取零件后，移动到放置点，并向工控机发送拍照指令GetPos:C1T2；

步骤8、放置零件：抓取机器人将零件放置于工装板上；

步骤9、断开通讯：抓取机器人关闭与工控机的Socket通讯；并跳转至步骤1。

如图1所示，本发明一种基于3D视觉的机器人自动抓取***，包括工控机10、3D相机6、抓取机器人8、上料机器人1、供料台4、上料台2、料框3和工装板11，

工控机，用于控制3D相机拍照，获取三维点云数据并进行处理，并将处理结果发送至抓取机器人；

3D相机6，用于拍照并输出原始三维点云数据至工控机，本实施例中使用的是基于双目结构光的3D相机，可直接输出点云；3D相机6设置在相机支架7上，

抓取机器人，用于向工控机发送请求数据指令，接收工控机中数据缓存模块返回的处理结果；根据处理结果，调整抓取机器人夹爪9抓取点坐标与抓取角度并抓取零件，输出拍照指令和更换料框指令；

上料机器人，用于接收更换指令，上料机器人夹爪5从上料台抓取料框，将其放置于供料台上；

供料台，用于放置料框并对料框进行粗定位，上料机器人每次将料框放在固定位置；

料框，待抓取的零件依次放置于料框中，每个料框包含固定数量的方格，每个方格可放置一个零件；料框划分有方格，零件依次放置于料框方格上，每个料框可存放S＝R*C个零件，其中，R为料框行数，C为料框列数，如图2所示；

工装板，抓取机器人从料框里抓零件，放到工装板上；料框上有多个零件，工装板上只放一个零件，放完之后工装板通过传送带送走；其中工控机和抓取机器人采用Socket通讯，抓取机器人与上料机器人采用IO通讯。

工控机包括，指令接收模块，用于接收抓取机器人的请求数据指令和拍照指令；

数据缓存模块，用于存储状态信息和零件空间位姿数据为处理结果，并基于请求数据指令判断是否缓存有处理结果，有则将处理结果传输至抓取机器人，没有则输出拍照指令；

相机控制模块，用于基于拍照指令控制3D相机拍照，接收3D相机返回的原始三维点云数据；

确认零件模块，用于基于原始三维点云数据，判断是否拍到目标零件，拍到目标零件则输出判断结果至位姿计算模块，未拍到目标零件则输出判断结果至抓取状态模块；确认零件模块包括模板零件模块、点云滤波模块和对比模块，

模板零件模块，用于获取模板零件形成的模板点云Pm，模板点云P_m的获取方法为：将抓取机器人移动到抓取点，在抓取点放置一个模板零件，使得机器人能够正确抓取；使用3D相机扫描模板零件，获得的模板零件点云即作为模板点云P_m；

点云滤波模块，用于对原始三维点云数据进行点云滤波，得到滤波后的点云P'，点云滤波的具体方法为：

(1)采用统计滤波法对原始点云数据{P}进行滤波，去除离群点，得到点云{P₁’}；

(2)对点云{P₁’}进行体素降采样，得到点云{P₂’}；

(3)分割料框点云，采用直通滤波分割料框点云：设置供料台左上角为参考点，其在相机坐标系下的坐标(X，Y)，根据供料台高度H，料框高度h，料框长L，料框宽W，设定最大料框误差Δ＝{Δh，ΔL，ΔW}，则设置直通滤波参数为：

经过直通滤波得到滤波后的点云{P₃’}；

(4)去除料框平面，采用RANSAC平面模型分割方法去除料框平面，得到{P_c’}，

(4.1)计算法线，对点云{P₃’}计算法线，得到P₃‘_Normals；计算过程运用OpenMP技术进行加速；

(4.2)采用RANSAC方法进行平面分割；以点云{P₃’}及其法线P₃‘_Normals为输入，设置最大迭代次数M_itera＝1000，距离权重W＝0.1，距离阈值Δ＝12，提取出点云{P₃’}中平面点云{P’_Plan}；

(4.3)去除平面，将点云{P’_plan}中的点从点云{P₃’}中去除，得到不含平面的点云{P_c’}；

(5)分割零件点云，根据料框左上角坐标(X′，Y’)，零件长l，宽w，摆放误差Δ‘＝{Δl’，Δw’}，对于第N次抓取，直通滤波参数为：

其中，N为抓取次数，C为料框列数，％为取余符号，[]为取整符号；依此分割出每次抓取零件的点云P’；

对比模块，用于对比点云P'和模板点云Pm，点云P'中点的数量大于模板点云Pm中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件；

位姿计算模块，用于基于确认零件模块的判断结果和原始三维点云数据，进行点云处理计算得到零件空间位姿数据和状态信息，并存储至数据缓存模块；位姿计算模块包括粗配准模块、精配准模块和坐标变换模块，

粗配准模块，用于采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；采用RANSAC方法的具体步骤为：

计算法线：对点云p′与模板点云P_m分别计算法线，分别得到点云P’与模板点云P_m的法线P′_Normals和P_m_Normals；计算过程运用OpenMP技术进行加速；

计算FPFH描述子：根据点云P′与模板点云P_m，法线P′_Normals和P_m_Normals，分别计算点云P′与模板点云P_m的FPFH快速点云特征直方图，得到P′_FPFH和P_m_FPFH；计算过程运用OpenMP技术进行加速；

RANSAC粗配准：根据点云P′与模板点云P_m，以及对应的FPFH特征P′_FPFH和P_m_FPFH，采用RANSAC方法对模板点云P_m和点云P′进行粗配准，得到模板点云P_m到点云P′的位姿变换矩阵T_sac；

精配准模块，用于采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；ICP方法的具体步骤为：以粗配准得到的变换矩阵T_sac为初始位姿输入，设置ICP最大迭代次数M_itera＝1000，最大对应距离M_dis＝1000，迭代差值ε＝1e-10，最小误差ε_min＝0.0001；得到精配准之后的位姿变换矩阵T_icp；

坐标变换模块，用于结合机器人手眼标定矩阵H，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1；计算出零件空间位姿数据的具体步骤为：精配准后的位姿变换矩阵T_icp为相机坐标系下模板点云P_m到点云P′的坐标变换矩阵，将其转换到相机坐标系下，结合机器人到3D相机的手眼标定矩阵H，机器人坐标系下模板点云P_m到点云P′的坐标变换矩阵T_robot＝HT_icpH^-1；提取T_robot的平移分量(x,y,z)，并将其旋转分量转化为欧拉角(A，B，C)，即得到机器人坐标系下模板点云P_m到点云P′六自由度位姿变换参数Pos＝{x，y，z，A，B，C}；

抓取状态模块，用于基于确认零件模块的判断结果，另外判断零件是否全部抓取完成，得到状态信息，并存储至数据缓存模块。

本发明通过3D视觉和机器人相结合，获取目标零件的六自由度空间位姿，实现目标工件的智能抓取、分拣和搬运等作业，极大的增强了生产的智能化和柔性化，将工人从脏乱、危险和重复性机械劳动中解放出来。

Claims (7)

1.一种基于3D视觉的机器人自动抓取方法，其特征在于，包括如下步骤：

抓取机器人向工控机发送请求数据指令，工控机接收抓取机器人的请求数据指令，工控机判断是否缓存有处理结果，有则返回处理结果至抓取机器人，没有则控制3D相机拍照；工控机接收到抓取机器人的拍照指令则控制3D相机拍照；工控机接收3D相机返回的原始三维点云数据，基于原始三维点云数据，判断是否拍到目标零件；若拍到目标零件则基于目标零件的原始三维点云数据，再进行点云处理计算得到零件空间位姿数据和状态信息，并缓存为处理结果；若未拍到目标零件则另外判断零件是否全部抓取完成，得到零件空间位姿数据和状态信息，并缓存为处理结果；抓取机器人接收工控机返回的处理结果，根据处理结果，调整抓取点坐标与抓取角度并抓取零件、输出拍照指令和更换料框指令；

所述判断是否拍到目标零件的具体方法为：

对原始三维点云数据进行点云滤波，得到滤波后的点云P′，获取模板零件对应的模板点云P_m，对比点云P′和模板点云P_m，点云P'中点的数量大于模板点云P_m中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件；

所述点云滤波的具体方法为：

(1)采用统计滤波法对原始点云数据{P}进行滤波，去除离群点，得到点云{P₁’}；

(2)对点云{P₁’}进行体素降采样，得到点云{P₂’}；

(3)分割料框点云，采用直通滤波分割料框点云：设置供料台左上角为参考点，其在相机坐标系下的坐标(X，Y)，根据供料台高度H，料框高度h，料框长L，料框宽W，设定最大料框误差Δ＝{Δh，ΔL，ΔW}，则设置直通滤波参数为：

经过直通滤波得到滤波后的点云{P₃’}；

(4)去除料框平面，采用RANSAC平面模型分割方法去除料框平面，得到{P_c’}，

(4.1)计算法线，对点云{P₃’}计算法线，得到P₃‘_Normals；计算过程运用OpenMP技术进行加速；

(4.2)采用RANSAC方法进行平面分割；以点云{P₃’}及其法线P₃‘_Normals为输入，设置最大迭代次数M_itera＝1000，距离权重W＝0.1，距离阈值Δ＝12，提取出点云{P₃’}中平面点云{P’_plan}；

(4.3)去除平面，将点云{P’_plan}中的点从点云{P₃’}中去除，得到不含平面的点云{P_c’}；

(5)分割零件点云，根据料框左上角坐标(X′，Y’)，零件长l，宽w，摆放误差Δ‘＝{Δl’，Δw’}，对于第n次抓取，直通滤波参数为：

其中，N为抓取次数，C为料框列数，％为取余符号，[]为取整符号；依此分割出每次抓取零件的点云P’。

2.根据权利要求1所述的一种基于3D视觉的机器人自动抓取方法，其特征在于：

若拍到目标零件，抓取机器人根据处理结果调整抓取点坐标与抓取角度并抓取零件，并向工控机发送拍照指令，然后抓取机器人将零件放置于工装板上；

若未拍到目标零件且未全部抓取完成，抓取机器人根据处理结果向工控机发送拍照指令；

若未拍到目标零件且全部抓取完成，抓取机器人根据处理结果向上料机器人发送更换料框指令。

3.根据权利要求1所述的一种基于3D视觉的机器人自动抓取方法，其特征在于：模板点云P_m的获取方法为：将抓取机器人移动到抓取点，在抓取点放置一个模板零件，使得机器人能够正确抓取；使用3D相机扫描模板零件，获得的模板零件点云即作为模板点云P_m。

4.根据权利要求1所述的一种基于3D视觉的机器人自动抓取方法，其特征在于：所述再进行点云处理计算得到零件空间位姿数据和状态信息的具体方法为：采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；结合机器人手眼标定矩阵H，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1。

5.一种基于3D视觉的机器人自动抓取***，其特征在于：包括工控机、3D相机和抓取机器人，

所述工控机，用于控制3D相机拍照，获取三维点云数据并进行处理，并将处理结果发送至抓取机器人；其包括，

指令接收模块，用于接收抓取机器人的请求数据指令和拍照指令；

数据缓存模块，用于存储状态信息和零件空间位姿数据为处理结果，并基于请求数据指令判断是否缓存有处理结果，有则将处理结果传输至抓取机器人，没有则输出拍照指令；

相机控制模块，用于基于拍照指令控制3D相机拍照，接收3D相机返回的原始三维点云数据；

确认零件模块，用于基于原始三维点云数据，判断是否拍到目标零件，拍到目标零件则输出判断结果至位姿计算模块，未拍到目标零件则输出判断结果至抓取状态模块；

所述确认零件模块包括模板零件模块、点云滤波模块和对比模块，

模板零件模块，用于获取模板零件形成的模板点云P_m，

点云滤波模块，用于对原始三维点云数据进行点云滤波，得到滤波后的点云P'，

点云滤波具体执行如下操作：

采用统计滤波法对原始点云数据{P}进行滤波，去除离群点，得到点云{P₁’}；

对点云{P₁’}进行体素降采样，得到点云{P₂’}；

分割料框点云，采用直通滤波分割料框点云：设置供料台左上角为参考点，其在相机坐标系下的坐标(X，Y)，根据供料台高度H，料框高度h，料框长L，料框宽W，设定最大料框误差Δ＝{Δh，ΔL，ΔW}，则设置直通滤波参数为：

经过直通滤波得到滤波后的点云{P₃’}；

去除料框平面，采用RANSAC平面模型分割方法去除料框平面，得到{P_c’}，

计算法线，对点云{P₃’}计算法线，得到P₃‘_Normals；计算过程运用OpenMP技术进行加速；

采用RANSAC方法进行平面分割；以点云{P₃’}及其法线P₃‘_Normals为输入，设置最大迭代次数M_itera＝1000，距离权重W＝0.1，距离阈值Δ＝12，提取出点云{P₃’}中平面点云{P’_plan}；

去除平面，将点云{P’_plan}中的点从点云{P₃’}中去除，得到不含平面的点云{P_c’}；

分割零件点云，根据料框左上角坐标(X′，Y’)，零件长l，宽w，摆放误差Δ‘＝{Δl’，Δw’}，对于第n次抓取，直通滤波参数为：

其中，N为抓取次数，C为料框列数，％为取余符号，[]为取整符号；依此分割出每次抓取零件的点云P’；

对比模块，用于对比点云P'和模板点云P_m，点云P'中点的数量大于模板点云P_m中点的数量的阈值百分比，则判断滤波后的点云P'包含目标零件；

位姿计算模块，用于基于确认零件模块的判断结果和原始三维点云数据，进行点云处理计算得到零件空间位姿数据和状态信息，并存储至数据缓存模块；

抓取状态模块，用于基于确认零件模块的判断结果，另外判断零件是否全部抓取完成，得到状态信息，并存储至数据缓存模块；

所述3D相机，用于拍照并输出原始三维点云数据至工控机；

所述抓取机器人，用于向工控机发送请求数据指令，接收工控机中数据缓存模块返回的处理结果；根据处理结果，调整抓取点坐标与抓取角度并抓取零件，输出拍照指令和更换料框指令。

6.根据权利要求5所述的基于3D视觉的机器人自动抓取***，其特征在于：所述位姿计算模块包括粗配准模块、精配准模块和坐标变换模块，

粗配准模块，用于采用RANSAC方法对点云P′与模板点云P_m进行全局粗配准，求得位姿变换矩阵T_sac；

精配准模块，用于采用ICP方法对点云P′与模板点云P_m进行精配准，初始输入参数位姿变换矩阵T_sac，求得模板点云到目标点云的位姿变换矩阵T_icp；

坐标变换模块，用于结合机器人手眼标定矩阵H，计算出零件空间位姿数据Pos＝{x，y，z，A，B，C}，其中x,y,z为位置坐标，A，B，C为旋转欧拉角，同时设置状态信息Status＝1。

7.根据权利要求5所述的基于3D视觉的机器人自动抓取***，其特征在于：还包括上料机器人、供料台、料框和工装板，

上料机器人，用于接收更换指令从上料区抓取料框，将其放置于供料台上；

供料台，用于放置料框并对料框进行粗定位；

料框，待抓取的零件依次放置于料框中，每个料框包含固定数量的方格，每个方格可放置一个零件；

工装板，抓取机器人抓取零件后，将其放置于工装板上；其中工控机和抓取机器人采用Socket通讯，抓取机器人与上料机器人采用IO通讯。