基于点云模板匹配技术的机械臂定位抓取系统及方法与设计方案

本技术公开一种基于点云模板匹配技术的机械臂定位抓取系统，包括依次连接的数据采集模块、数据处理和传输模块、机械臂控制模块。本技术还公开了一种基于点云模板匹配技术的机械臂定位抓取方法。本技术基于三维点云配准方法在场景中寻找目标物体，克服了利用普通彩色相机采集的场景二维图像信息难以检测存在相互遮挡的多个目标物体、缺乏纹理细节的目标物体等问题，准确性高，适用于货物分拣码垛场景，具有良好的应用前景。

技术要求

1.一种基于点云模板匹配技术的机械臂定位抓取系统，其特征在于，包括依次连接的数据采集模块、数据处理和传输模块、机械臂控制模块；

所述数据采集模块用于获取待抓取目标物体的场景点云数据；

所述数据处理和传输模块用于通过场景点云数据获取目标物体的位姿信息并将位姿信息

传输至机械臂控制模块；

所述机械臂控制模块用于根据位姿信息执行对目标物体的抓取。

2.根据权利要求1所示的一种基于点云模板匹配技术的机械臂定位抓取系统，其特征在

于，所述数据采集模块包括深度相机和电脑；

所述深度相机用于获取待抓取目标物体的场景点云数据并将所述场景点云数据保存至电

脑中。

3.一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述方法包括以下步骤：

采集待抓取目标物体的场景点云数据；

根据所述场景点云数据获取目标物体的目标点云数据；

根据所述目标点云数据计算目标物体在深度相机下的位姿信息；

根据所述深度相机下的位姿信息和机械臂示教计算目标物体在机械臂基坐标系下的位姿信息；

根据所述机械臂基坐标系下的位姿信息规划抓取目标物体的轨迹和相应姿态并实施抓取。

4.根据权利要求3所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述目标点云数据的获取方法包括：

对所述场景点云数据进行滤波处理获取无噪点和背景的点云；

根据所述无噪点和背景的点云所在的三维空间创建多个三维体素栅格，以所述体素栅格中所有点云的重心点作为采样点云；

对所述采样点云进行分割获取目标点云数据。

5.根据权利要求3所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述深度相机下的位姿信息计算方法包括：

提取所述目标点云数据的关键点；

将所述目标点云数据和模板点云数据的关键点进行初始配准、精配准获取齐次变换矩阵。

6.根据权利要求5所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述初始配准的方法包括：

从目标点云数据的关键点中选取样本点；

根据特征直方图获取所述样本点在模板点云数据的关键点中的初始对应点；

根据所述样本点和初始对应点获取初始齐次变换矩阵。

7.根据权利要求6所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述精配准的方法包括：

a、根据所述初始齐次变换矩阵获取所述样本点在模板点云数据的关键点中的精准对应点；

b、计算所述样本点和精准对应点的均方根，获取样本点和精准对应点之间均方根最小的变换；

c、重复所述步骤（a）、步骤（b），迭代计算直到达到设定的迭代次数或误差小于阈值即停止得到齐次变换矩阵。

8.根据权利要求7所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述机械臂基坐标系下的位姿信息计算方法包括：

根据机械臂示教的旋转矩阵、平移向量和所述齐次变换矩阵得到三者的组合齐次变换矩阵即机械臂基坐标系下的位姿信息。

9.根据权利要求3所述的一种基于点云模板匹配技术的机械臂定位抓取方法，其特征在于，所述场景点云数据的采集方法包括：

通过深度相机采集场景的深度图像，通过通用数据采集接口OpenNI将深度图像数据转换为场景点云数据。

技术说明书

基于点云模板匹配技术的机械臂定位抓取系统及方法

技术领域

本技术涉及点云处理算法领域，具体涉及一种基于点云模板匹配技术的机械臂定位抓取方法。

背景技术

机械臂可以替代人类进行高强度、简单重复的工作，例如流水线货物的分拣、码垛等；此外，还可以完成具有危险性、伤害性的工作，例如高空作业、扫雷、喷漆等。机械臂的应用不仅能降低人力成本、保证生产安全，还能大幅提高生产效率，有广泛的应用前景。

传统的机械臂应用，通常根据人为设定的指令行走固定的轨迹、执行固定的抓取姿态，机械臂无法自主的与环境发生交互，一旦环境发生变化，就需要人为重新设定指令，这降低了生产活动的效率。

技术内容

针对现有技术的不足，本技术的目的在于提供一种低成本高精度的机械臂定位抓取方法，以解决现有技术中存在的机械臂无法自主的与环境发生交互的问题。

为了实现上述目标，本技术采用如下的技术方案：

一种基于点云模板匹配技术的机械臂定位抓取系统，包括依次连接的数据采集模块、数据处理和传输模块、机械臂控制模块；

所述数据采集模块用于获取待抓取目标物体的场景点云数据；

所述数据处理和传输模块用于通过场景点云数据获取目标物体的位姿信息并将位姿信息传输至机械臂控制模块；

所述机械臂控制模块用于根据位姿信息执行对目标物体的抓取。

进一步的，所述数据采集模块包括深度相机和电脑；

所述深度相机用于获取待抓取目标物体的场景点云数据并将所述场景点云数据保存至电脑中。

一种基于点云模板匹配技术的机械臂定位抓取方法，所述方法包括以下步骤：

采集待抓取目标物体的场景点云数据；

根据所述场景点云数据获取目标物体的目标点云数据；

根据所述目标点云数据计算目标物体在深度相机下的位姿信息；

根据所述深度相机下的位姿信息和机械臂示教计算目标物体在机械臂基坐标系下的位姿信息；

根据所述机械臂基坐标系下的位姿信息规划抓取目标物体的轨迹和相应姿态并实施抓取。

进一步的，所述目标点云数据的获取方法包括：

对所述场景点云数据进行滤波处理获取无噪点和背景的点云；

根据所述无噪点和背景的点云所在的三维空间创建多个三维体素栅格，以所述体素栅格中所有点云的重心点作为采样点云；

对所述采样点云进行分割获取目标点云数据。

进一步的，所述深度相机下的位姿信息计算方法包括：

提取所述目标点云数据的关键点；

将所述目标点云数据和模板点云数据的关键点进行初始配准、精配准获取齐次变换矩阵。

进一步的，所述初始配准的方法包括：

从目标点云数据的关键点中选取样本点；

根据特征直方图获取所述样本点在模板点云数据的关键点中的初始对应点；

根据所述样本点和初始对应点获取初始齐次变换矩阵。

进一步的，所述精配准的方法包括：

a、根据所述初始齐次变换矩阵获取所述样本点在模板点云数据的关键点中的精准对应点；

b、计算所述样本点和精准对应点的均方根，获取样本点和精准对应点之间均方根最小的变换；

c、重复所述步骤（a）、步骤（b），迭代计算直到达到设定的迭代次数或误差小于阈值即停止得到齐次变换矩阵。

进一步的，所述机械臂基坐标系下的位姿信息计算方法包括：

根据机械臂示教的旋转矩阵、平移向量和所述齐次变换矩阵得到三者的组合齐次变换矩阵即机械臂基坐标系下的位姿信息。

进一步的，所述场景点云数据的采集方法包括：

通过深度相机采集场景的深度图像，通过通用数据采集接口OpenNI将深度图像数据转换为场景点云数据。

与现有技术相比，本技术的有益效果是：

本技术将深度相机与机械臂相结合，利用PrimeSense深度相机采集待抓取目标所在场景的点云数据，创建目标物体的三维点云模板，结合点云两两配准算法计算得到目标物体在深度相机坐标系下的位置、姿态信息，通过机械臂示教得到机械臂夹爪和模板物体之间的齐次变换矩阵，综上得到目标物体在机械臂坐标系下的坐标，从而规划出机械臂末端夹爪抓取目标的运动轨迹和姿态，实现了机械臂可以自主的与环境发生交互的功能；相对于传统的用昂贵的激光设备采集点云的方式，成本得到了大幅的降低，能促进低成本视觉方案在机械臂领域的应用。

附图说明

图1为机械臂的结构示意图；

图2为系统模块关系示意图；

图3为目标物体定位算法流程图。

具体实施方式

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

如图1、图2所示，一种基于点云模板匹配技术的机械臂定位抓取系统，包括依次连接的数据采集模块、数据处理和传输模块、机械臂控制模块。

所述数据采集模块用于获取待抓取目标物体的场景点云数据；

所述数据处理和传输模块用于通过场景点云数据获取目标物体的位姿信息并将位姿信息传输至机械臂控制模块；

所述机械臂控制模块用于根据位姿信息执行对目标物体的抓取。

所述数据采集模块包括深度相机和电脑；

所述深度相机用于获取待抓取目标物体所在场景的三维场景点云数据，并将获取的场景点云数据保存在电脑本地内存。

所述数据处理和传输模块，用于从场景点云数据中识别、定位出待抓取的目标物体，计算出目标物体在深度相机坐标系下的位置和姿态信息，分别对应于一个旋转矩阵和平移向量；然后，通过机械臂和深度相机联合示教，将深度相机下的位姿信息转换至机械臂基坐标系得到目标物体在机械臂基坐标系下的位姿信息；最后，将机械臂基坐标系下的位姿信息通过Socket网络通信传送至机械臂控制柜。

所述机械臂控制模块，用于接收目标物体的位姿信息，依据位姿信息规划机械臂末端夹爪的运动轨迹和相应姿态，执行对目标物体的抓取。

如图3所示，一种基于点云模板匹配技术的物体定位方法，所述定位抓取方法包括以下步骤：

步骤一、电脑端控制深度相机采集待抓取目标物体所在场景的场景点云数据；

步骤二、根据所述场景点云数据获取目标物体的目标点云数据；

步骤三、根据所述目标点云数据计算出目标物体在深度相机下的位姿信息；

步骤四、根据所述深度相机下的位姿信息结合机械臂示教的结果计算出目标物体在机械臂基坐标系下的位姿信息。

步骤五、利用Socket网络通信将机械臂基坐标系下的位姿信息通过网线传送至机械臂。步骤六、机械臂根据接收到的机械臂基坐标系下的位姿信息，规划抓取目标物体的轨迹和相应姿态并实施抓取。

所述目标点云数据的获取方法包括：

2.1、场景点云数据滤波；

通过统计滤波去除场景点云数据中的噪点，通过直通滤波去除场景点云数据场景的背景获取得到无噪点和背景的点云。统计滤波步骤通过对每个点的邻域进行统计分析，计算它和它邻近点的平均距离，得到的结果服从正态分布，所以能得到临近点距离的均值。将距离大于均值的点定义为离群点，将其从场景点云数据中移除；直通滤波步骤通过指定一个需要滤波的方向，设置一个阈值，然后按照此方向遍历点云，并判断每个点是否在阈值区间内，将不在阈值内的点删除。

2.1、获取采样点云；

通过在无噪点和背景的点云所在的三维空间内创建多个三维体素栅格，体素栅格可视为微小的空间三维立方体，点云依据空间分布的不同落入相应的体素内，计算体素中所有点的重心，以重心点近似替代该体素中的所有点作为采样点云。以此降低点云场景密度但同时依然保持点云形状特征。

2.3对所述采样点云进行分割获取目标点云数据；

通过随机采样一致性算法区分不同特征、不同区域的点云，主要过程为从所有数据中选取m个样本点，在m维空间中去定义点和点之间一些性质的亲或疏，据此完成聚类操作。假设m个点云数据可以聚合成n个类，然后将类间距离最小的两个类再聚合成一个类，聚合之后需要重新计算类间距离，因为样本点发生了变化。类间距重新计算之后，再一次比较所有类间距离，再次合并距离最小的两个类，如此反复迭代，直到任何两个类之间的距离都大于设定的阈值或者类的数目小于预定义的数目时，停止迭代，完成点云分割，得到目标点云数据。

所述深度相机下的位姿信息计算方法包括：

3.1、目标点云数据的关键点提取及关键点特征描述子计算；

通过以下步骤提取目标点云数据的关键点：遍历深度图像的每个点，重点关注邻近区域有较明显深度变化的点，以此进行边缘检测；依据邻近区域表面的数值波动计算表面变化的系数，找到变化的主方向；根据主方向计算兴趣值，该值可以表征变化的主方向和其它方向的不同；对计算得到的所有兴趣值进行平滑滤波；对滤波的数据进行无最大值压缩，以此找到最终的NARF（Normal Aligned Radial Feature，法向对齐径向特征）关键点。通过计算关键点的FPFH（fast point feature histograms，快速点特征直方图）描数子描述关键点周围的形状特征，以此作为模板点云与目标点云的配准依据。

3.2、点云粗配准；

通过采样一致性初始配准算法完成模板点云和目标点云的关键点之间的粗配准，流程包括：从目标点云数据的关键点数据集P中选取适当样本点，设定最小的配对距离值dmin，确保样本点的配对距离大于dmin；对于从数据集P中选取的样本点，需要根据特征直方图找到在对应的模板点云数据的关键点数据集Q中的初始对应点并存储，从而确定对应关系；通过样本点及其对应的初始对应点可以计算出初始齐次变换矩阵。

3.3、点云精配准。

通过迭代最近点算法完成模板点云有目标点云之间的精配准，流程包括：a、根据所述初始齐次变换矩阵获取所述样本点在模板点云数据的关键点中的精准对应点；b、对得到的样本点和精准对应点计算均方根，寻找使得样本点和精准对应点之间均方根最小的变换；c、从重复上述步骤（a）、（b）的操作，迭代计算直到达到设定的迭代次数或误差小于阈值即停止，得到齐次变换矩阵，误差最小是指计算的相邻两次均方根的差的绝对值小于设定的目标误差阈值。

所述机械臂基坐标系下的位姿信息计算方法包括：

根据机械臂示教的旋转矩阵、平移向量和所述齐次变换矩阵得到三者的组合齐次变换矩阵即机械臂基坐标系下的位姿信息。

所述方法还包括目标物体模板点云制作及场景点云获取和机械臂与深度相机联合示教；

目标物体模板点云制作及场景点云获取：

通过深度相机采集场景的深度图像，通过通用数据采集接口OpenNI（开放自然交互接口）将深度数据转换为点云数据并保存到电脑本地；将模板物体Model放入场景，重新采集点云数据，手动剪裁点云得到目标物体的点云模板ModelCloud。

机械臂与深度相机联合示教：将深度相机绑定在机械臂上，设定机械臂的初始位置，手动调节机械臂至抓取模板目标物体Model的位置，通过机械臂控制柜读取旋转矩阵R1、平移向量T1。

本技术并不受上述实施方式的限制，在未背离本技术的精神实质与原理下所作的任何改变、修饰、替代、组合和简化，均应为等效的置换方式，都包含在本技术的保护范围之内。尽管已经示出和描述了本技术的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本技术的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本技术的范围由所附权利要求及其等同物限定。