CN112669385A - 基于三维点云特征的工业机器人工件识别与位姿估计方法 - Google Patents

Abstract

本发明公开了基于三维点云特征的工业机器人工件识别与位姿估计方法，所述方法包括步骤：(1)识别开始前，对需要识别的工件创建模型点云库，并进行训练，供识别过程使用；(2)识别开始时，对三维扫描仪得到的三维场景点云提取特征点，基于特征点的球邻域内建立SHOT描述符；采用改进矢量方法建立局部坐标系；(3)在三维空间中，根据场景点云和模板点云的SHOT描述符建立初始对应关系；根据向量空间中转换关系完成对工件的识别；计算场景点云中的工件实例相对模板点云的位姿；(4)将计算得到的场景点云位姿发送给机器人控制柜进行处理，并控制工业机器人完成识别并抓取工件。

Description

基于三维点云特征的工业机器人工件识别与位姿估计方法

技术领域

本发明属于机器人技术应用改进领域，主要涉及基于三维点云特征的工业机器人工件识别与位姿估计方法。

背景技术

在工业制造过程中，工件的定位与装配工艺是重要的组成部分之一。由于传统的抓取装配生产存在工作环境恶劣，工作重复性高，人工劳动强度大，生产效率低等不足，近年来，工业机器人逐渐应用于工业生产中的自动抓取及装配中。传统的工业机器人普遍采用图像的方法进行识别工作，如申请公布号为CN108225319A的中国发明专利，其利用工件的特征进行图像匹配来获取工件位姿，这种方法不适用于表面纹理较少、严重遮挡或干扰、目标物体过于平滑的情况，申请公布号为CN110634161A面对严重遮挡或干扰导致识别率低等问题，因而难以满足抓取与装配生产应用高效率、高精度和高质量的要求。通过三维扫描仪获取3D点云数据已经非常便捷，将三维点云与位姿已知的模板点云进行匹配得到两者之间的刚性变换，从而实现工件的识别和定位，成为位姿估计领域的研究热点。

发明内容

本发明的目的在于克服现有技术的不足，解决机器人抓取、装配工件过程中出现的噪声干扰，遮挡严重、无纹理等问题，使得机器人能够有效地识别工件及准确估计位姿。

本发明至少通过如下技术方案之一实现。

基于三维点云特征的工业机器人工件识别与位姿估计方法，该方法所基于的识别系统包括工业机器人、机器人控制柜、三维扫描仪、嵌入式工业控制机、工作台，所述方法包括以下步骤：

S1、识别开始前，对需要识别的工件创建三维工件模型，生成模型点云，建立模型数据库，对模型点云数据进行训练；

S2、识别开始时，采集当前场景点云，提取特征点，并基于特征点的球邻域内建立SHOT步骤

S3、在三维空间中，根据步骤S2得到的SHOT描述符将场景点云与模型点云建立初始对应关系，根据向量空间中转换关系完成对工件的识别，计算出场景点云中的工件实例相对于模板点云的相对位姿关系；

S4、获取场景点云的工件实例相对于工业机器人基坐标系下的位姿关系，并实时发送给机器人控制柜进行处理，控制机器人的末端执行器完成识别及抓取工件。

优选的，所述创建三维工件模型，生成模型点云是使用SolidWorks三维制图软件生成三维工件模型，保存为stl、obj格式文件。

优选的，所述模型点云是利用点云库中的库函数pcl_mesh_sampling.exe对三维工件模型每个面整体点云采样，得到模型点云，保存模型点云相对三维扫描仪坐标系的位姿关系。

优选的，建立模型数据库，对模型点云数据进行训练，具体包括以下步骤：

S11.在模型点云上使用统一的特征点采样，即通过建立空间体素栅格进行采样，再用每个体素内的重心代表该体素内的特征点F；

其中：n表示点云中点的个数，(q_xi,q_yi,q_zi)表示每个点的x、y、z的坐标值；

S12.基于特征点F建立该点球形支持域内的局部坐标系，建立局部坐标系；

S13.在局部坐标系确定后，对球形支持域中的结构形状信息进行统计，建立3-D描述符SHOT特征，根据在特征点F处建立的局部坐标系将r半径球领域划分为H空间区域，其中空间区域包括若干个径向区、若干个方位角区、若干个仰角区；

对于每个区域由局部直方图表示，每个直方图分为h个小单元，各单元的值根据网格中相应部分的每个点的法线n_vi与特征点法线n_u之间的夹角θ_i余弦值来累计点数而构建，即

cosθ_i＝n_u·n_vi

最终SHOT描述符的维度为H×h，根据局部坐标系将所有直方图并排在一起；

S14.采用四次线性插值法对局部直方图中相邻的单元格和相邻直方图同一单元格内进行插值，每个计数乘以每个维度的权重1-d，对于法矢夹角余弦，直方图相邻单元格插值d的定义为：

在方位角方向、仰角方向和径向方向插值类似，其中s表示相邻单元格间距；

根据法矢直方图完成SHOT特征的描述，存储每个工件的模型点云的SHOT描述符特征后供后续的识别过程使用。

优选的，所述建立局部坐标系包括以下步骤：

1)由特征点F的k个最近邻域点集P＝{p_i,i＝1,2...k}，建立协方差矩阵M：

其中，p_i表示k邻域中的第i个点，

表示点集的质心：

对M进行特征值分解，采用总体最小二乘法估计法线方向，依次计算加权协方差矩阵M：

其中：d_i＝||p_i-F||₂表示p_i与特征点F之间的欧几里德距离，r是球面领域的半径，求得加权协方差矩阵M特征向量即为局部坐标系的三个单位向量；

2)对协方差矩阵M的特征向量采用符号一致性的方法消除歧义，将最大和最小特征值及对应的特征向量分别定义为局部坐标系x轴和z轴的单位向量，则消除歧义的x轴定义为：

其中，x⁺表示x轴的正半轴方向，x^-表示x轴的负半轴方向，

表示特征点F指向p_i的向量与x正半轴同向的累计数，

表示特征点F指向p_i的向量与x负半轴同向的累计数，表示；

使用相同的过程消除z轴的歧义，最后y轴通过标准正交约束下的叉乘得到：

y＝z×x。

优选的，所述步骤S2中包括以下步骤：

S21.使用三维扫描仪采集到的场景点云，采用随机样本一致性(RANSAC)算法拟合实验平台上的点云，并将平面点云作为局外点去除，采用直通滤波器沿着三维扫描仪相机坐标系的x轴、y轴去除噪声点，使用体素化网格滤波器减少点的数量；

S22.对步骤S21处理后的场景点云采用步骤S12、步骤S13、步骤S14，依次提取特征点，建立局部坐标系，得到SHOT三维描述符。

优选的，步骤S3中，场景点云和SHOT描述符的初始对应关系建立为：根据当前场景中的描述符，将每个场景描述符与数据库中所有的描述符进行匹配，通过K维树关联描述符空间中接近的模型-场景描述符确定点对点的对应关系。

优选的，对工件的识别是通过加强几何一致性消除伪对应，在三维空间中，利用模型点云与当前场景点云之间的对应关系，通过局部坐标系将每个场景特征点与模型质心的相对位置相关联，使得每个对应的场景特征在三维霍夫空间中进行投票，根据票数确定工件在三维空间中的位置。

优选的，计算出场景点云中的工件实例相对于模板点云的相对位姿关系具体如下：

设模型特征点为F_i ^M、场景特征点为

模型点云和场景点云的质心分别为C^M、C^S，C^M和F_i ^M之间的向量表示为：

其中，质心的定义为

为保证向量在不同坐标系下具有旋转平移不变性，通过以下变换将向量

转化到局部参考系即为特征点坐标系：

其中符号·表示矩阵乘法，

是模型全局坐标系到局部坐标系间的变换矩阵，其每一行是模型特征点F_i ^M的局部参考系的单位向量，表示为：

于是，每个特征点F_i ^M与其对应的向量

关联在一起，其中

表示局部坐标系；

对每对场景特征点

与模型特征点F_i ^M建立的联系：

由于局部参考系的旋转平移不变性，所以有

其中

表示场景点云中的特征点指向质心的向量，

表示模型点云中的特征点指向质心的向量；

将

转换到场景的全局参考系中：

其中，

是通过以场景特征

的局部坐标系

的单位向量的列排列组成的变换矩阵，表示为：

根据以上转换关系，场景特征点

通过向量

在3D霍夫空间中投票，在霍夫参数空间中，峰值的存在表明场景中存在目标；

根据上述转换关系，得到的三组及以上模型-场景特征点对应关系，求得场景点云中的工件实例相对于工件模板点云的相对位姿关系。

优选的，步骤S4是先根据仿射变换得到模板点云相对三维扫描仪坐标系下的位姿关系，以及场景点云的工件实例相对于三维扫描仪坐标系下的位姿关系，再通过手眼标定得到三维扫描仪相对工业机器人基坐标系下的位姿关系，最终求得场景点云的工件实例相对于工业机器人基坐标系下的位姿关系。

相对于现有技术，本发明是一种简单、有效、可用于基于点云特征的工件识别方法，克服了基于平面二维图像识别中受光照强度变化、成像距离及视点变化的影响，有效解决时识别中遮挡、存在噪声干扰问题，能够极大提高识别的准确率。

附图说明

图1是本发明实施例的工业机器人及三维扫描仪识别系统总体结构示意图；

图2是SHOT描述符的特征结构图；

图3是法矢夹角余弦插值直方图；

图4是场景特征点与模型特征点配对关系图；

图5是引入局部坐标系的坐标变换示意图；

图6是三维物体识别与位姿估计框图；

图7是机器人视觉系统各坐标系关系示意图；

图中：1-三维扫描仪；2-嵌入式工业控制机；3-工业机器人；4-工件；5-工作台；6-机器人控制柜。

具体实施方式

下面结合具体实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1和图6所示，本实施例的基于三维点云特征的工业机器人工件识别与位姿估计方法所基于的识别系统包括工业机器人3、机器人控制柜6、三维扫描仪1、嵌入式工业控制机2、工作台5，本实施例的工业机器人工件识别及位姿估计方法，包括以下步骤：

S1、识别开始前，采用嵌入式工业控制机2中的SolidWorks对需要识别的工件4创建三维模型，生成模型点云，最后建立模型数据库，对系统中的模型点云数据进行训练；具体而言，供识别过程使用；所述步骤S1包括：

S11、使用SolidWorks三维制图软件生成三维工件模型，保存为stl,obj等格式文件，利用点云库中的库函数pcl_mesh_sampling.exe对模型每个面整体点云采样，得到模型点云，保存模型点云相对三维扫描仪坐标系的位姿关系；

S12、得到工件模型点云后，在模型点云上使用统一的特征点采样，即通过建立一定宽度的(10-30mm)空间体素栅格进行采样，然后用每个体素内的重心代表该体素内的特征点F；

其中：n表示点云中点的个数，(q_xi,q_yi,q_zi)表示每个点的x、y、z的坐标值；

S13、基于特征点F建立该点球形支持域内的局部坐标系，建立局部坐标系的过程分为两步：

1)由特征点F的k个最近邻域点集P＝{p_i,i＝1,2...k}，建立协方差矩阵M：

其中，p_i表示k邻域中的第i个点，

表示点集的质心：

对M进行特征值分解，采用总体最小二乘法估计法线方向。为了提高局部坐标系在杂乱场景中的可重复性，给较远的点分配较小的权重，依次计算加权协方差矩阵M，可得，

其中：d_i＝||p_i-F||₂表示p_i与F之间的欧几里德距离；r是球面领域的半径；

求得加权协方差矩阵M特征向量即为局部坐标系的三个单位向量。

2)对到协方差矩阵M的特征向量采用符号一致性的方法消除歧义，将最大和最小特征值及对应的特征向量分别定义为局部坐标系x轴和z轴的单位向量，则消除歧义的x轴定义为：

其中，x⁺表示x轴的正半轴方向，x^-表示x轴的负半轴方向；

使用相同的过程消除z轴的歧义。最后y轴通过标准正交约束下的叉乘得到：

y＝z×x；

S14、在局部坐标系确定后，对球形支持域中的结构形状信息进行统计，建立3-D描述符SHOT特征，如附图2所示，根据在特征点F处建立的局部坐标系将r半径球领域划分为32个空间区域，其中2个径向区、8个方位角区、2个仰角区；

对于每个区域由局部直方图表示，每个直方图分为11个小单元，各单元的值根据网格中相应部分的每个点的法线n_vi与特征点法线n_u之间的夹角θ_i余弦值来累计点数而构建，即

cosθ_i＝n_u·n_vi

最终SHOT描述符的维度为32×11＝352。根据局部坐标系将所有直方图有序地并排在一起。

S15、为了防止局部直方图组合在一起时会产生边界效应，采用四次线性插值法对局部直方图中相邻的单元格和相邻直方图同一单元格内进行插值。每个计数乘以每个维度的权重1-d，对于法矢夹角余弦，直方图相邻单元格插值如附图3所示，d的定义为：

在方位角方向、仰角方向和径向方向插值类似，其中s表示相邻单元格间距；

最后，根据法矢直方图完成了SHOT特征的描述。存储每个模型的SHOT描述符特征后供后续的识别过程使用。

S2、识别开始时，采集当前场景点云，提取特征点，并基于特征点的球邻域内建立SHOT，具体而言，包括以下步骤：

S21、对三维扫描仪1采集到的场景点云，采用RANSAC(随机样本一致性)算法拟合实验平台上的点云，并将其作为局外点去除，采用直通滤波器沿着相机坐标系(Cam)的x轴、y轴去除噪声点，使用体素化网格滤波器减少点的数量，提高计算效率，同时保持点云的形状特征。

S22.对步骤S21预处理后的场景点云采用步骤S12、步骤S13、步骤S14，依次提取特征点，建立局部坐标系，得到SHOT三维描述符。

S3、在三维空间中，根据步骤S2得到的SHOT描述符将场景点云与模型点云建立初始对应关系，根据向量空间中转换关系完成对工件的识别，计算出场景点云中的工件实例相对于模板点云的相对位姿关系；

场景点云和SHOT描述符的初始对应关系建立为：根据当前场景中的描述符，将每个场景描述符与数据库中所有的描述符进行匹配，通过K维树关联描述符空间中接近的模型-场景描述符确定点对点的对应关系。

根据向量空间中转换关系完成对工件的识别具体如下：

由于复杂场景遮挡干扰和场景中相似目标物体的存在，上述得到的对点的对应关系存在伪对应对，通过加强几何一致性来消除伪对应，如附图5和图7所示，在三维空间中，利用模型点云与当前场景点云之间的对应关系，通过局部坐标系将每个场景特征点与模型质心的相对位置相关联，使得每个对应的场景特征可以在三维霍夫空间中进行投票，足够多的票数来支持工件在三维空间中给定位置的存在，其具体的实现过程如下：

在附图5中，设模型特征点为F_i ^M，场景特征点为

模型点云和场景点云的质心分别为C^M、C^S，C^M和F_i ^M之间的向量表示为，

其中质心的定义为

为保证向量在不同坐标系下具有旋转平移不变性，将向量

转化到局部参考系(即为特征点坐标系)中，通过以下变换：

其中符号·表示矩阵乘法，

是模型全局坐标系到局部坐标系间的变换矩阵，其每一行是特征点F_i ^M的局部参考系的单位向量，表示为：

于是，每个特征点F_i ^M与其对应的向量

关联在一起，其中

表示局部坐标系；

如图4所示，实线表示正确的匹配，虚线表示伪对应(伪对应表示场景点云与模型点云的特征点之间错误的配对关系，正确的匹配表示示场景点云与模型点云的特征点之间正确的配对关系)，对每对场景特征点

与模型特征点F_i ^M建立的联系

由于局部参考系的旋转平移不变性，所以有

其中

表示场景点云中的特征点指向质心的向量，

表示模型点云中的特征点指向质心的向量；

将

转换到场景的全局参考系中，可得

其中，

是通过以场景特征

的局部参考系的单位向量的列排列组成的变换矩阵，表示为：

根据这些转换关系，场景特征点

通过向量

在3D霍夫空间中投票，在霍夫参数空间中，峰值的存在表明场景中存在目标。

根据上述转换关系得到场景点云与工件模板点云的三组及三组以上模型-场景特征点对应关系，求得场景点云中的工件实例相对于模板点云的相对位姿关系；

S4、根据仿射变换得到的模板点云相对三维扫描仪坐标系下的位姿关系，得到场景点云的工件实例相对于三维扫描仪坐标系下的位姿关系，通过手眼标定得到三维扫描仪相对工业机器人基坐标系(RB)下的位姿关系，最终求得场景点云的工件实例相对于工业机器人基坐标系下的位姿关系，并实时发送给机器人控制柜6进行处理，控制机器人3的末端执行器完成识别及抓取工件。

本发明的上述实施例仅是为清楚地说明本发明所作的举例，并非是对本发明的实施方式的限定。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.基于三维点云特征的工业机器人工件识别与位姿估计方法，该方法所基于的识别系统包括工业机器人、机器人控制柜、三维扫描仪、嵌入式工业控制机、工作台，其特征在于，所述方法包括以下步骤：

S1、识别开始前，对需要识别的工件创建三维工件模型，生成模型点云，建立模型数据库，对模型点云数据进行训练；

S2、识别开始时，采集当前场景点云，提取特征点，并基于特征点的球邻域内建立SHOT步骤

S3、在三维空间中，根据步骤S2得到的SHOT描述符将场景点云与模型点云建立初始对应关系，根据向量空间中转换关系完成对工件的识别，计算出场景点云中的工件实例相对于模板点云的相对位姿关系；

S4、获取场景点云的工件实例相对于工业机器人基坐标系下的位姿关系，并实时发送给机器人控制柜进行处理，控制机器人的末端执行器完成识别及抓取工件。

2.根据权利要求1所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，所述创建三维工件模型，生成模型点云是使用SolidWorks三维制图软件生成三维工件模型，保存为stl、obj格式文件。

3.根据权利要求2所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，所述模型点云是利用点云库中的库函数pcl_mesh_sampling.exe对三维工件模型每个面整体点云采样，得到模型点云，保存模型点云相对三维扫描仪坐标系的位姿关系。

4.根据权利要求3所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，建立模型数据库，对模型点云数据进行训练，具体包括以下步骤：

S11.在模型点云上使用统一的特征点采样，即通过建立空间体素栅格进行采样，再用每个体素内的重心代表该体素内的特征点F；

其中：n表示点云中点的个数，(q_xi,q_yi,q_zi)表示每个点的x、y、z的坐标值；

S12.基于特征点F建立该点球形支持域内的局部坐标系，建立局部坐标系；

S13.在局部坐标系确定后，对球形支持域中的结构形状信息进行统计，建立3-D描述符SHOT特征，根据在特征点F处建立的局部坐标系将r半径球领域划分为H空间区域，其中空间区域包括若干个径向区、若干个方位角区、若干个仰角区；

对于每个区域由局部直方图表示，每个直方图分为h个小单元，各单元的值根据网格中相应部分的每个点的法线n_vi与特征点法线n_u之间的夹角θ_i余弦值来累计点数而构建，即

cosθ_i＝n_u·n_vi

最终SHOT描述符的维度为H×h，根据局部坐标系将所有直方图并排在一起；

S14.采用四次线性插值法对局部直方图中相邻的单元格和相邻直方图同一单元格内进行插值，每个计数乘以每个维度的权重1-d，对于法矢夹角余弦，直方图相邻单元格插值d的定义为：

在方位角方向、仰角方向和径向方向插值类似，其中s表示相邻单元格间距；

根据法矢直方图完成SHOT特征的描述，存储每个工件的模型点云的SHOT描述符特征后供后续的识别过程使用。

5.根据权利要求4所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，所述建立局部坐标系包括以下步骤：

1)由特征点F的k个最近邻域点集P＝{p_i,i＝1,2...k}，建立协方差矩阵M：

其中，p_i表示k邻域中的第i个点，

表示点集的质心：

对M进行特征值分解，采用总体最小二乘法估计法线方向，依次计算加权协方差矩阵M：

其中：d_i＝||p_i-F||₂表示p_i与特征点F之间的欧几里德距离，r是球面领域的半径，求得加权协方差矩阵M特征向量即为局部坐标系的三个单位向量；

2)对协方差矩阵M的特征向量采用符号一致性的方法消除歧义，将最大和最小特征值及对应的特征向量分别定义为局部坐标系x轴和z轴的单位向量，则消除歧义的x轴定义为：

其中，x⁺表示x轴的正半轴方向，x^-表示x轴的负半轴方向，

表示特征点F指向p_i的向量与x正半轴同向的累计数，

表示特征点F指向p_i的向量与x负半轴同向的累计数，表示；

使用相同的过程消除z轴的歧义，最后y轴通过标准正交约束下的叉乘得到：

y＝z×x。

6.根据权利要求5所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，所述步骤S2中包括以下步骤：

S21.使用三维扫描仪采集到的场景点云，采用随机样本一致性(RANSAC)算法拟合实验平台上的点云，并将平面点云作为局外点去除，采用直通滤波器沿着三维扫描仪相机坐标系的x轴、y轴去除噪声点，使用体素化网格滤波器减少点的数量；

S22.对步骤S21处理后的场景点云采用步骤S12、步骤S13、步骤S14，依次提取特征点，建立局部坐标系，得到SHOT三维描述符。

7.根据权利要求6所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，步骤S3中，场景点云和SHOT描述符的初始对应关系建立为：根据当前场景中的描述符，将每个场景描述符与数据库中所有的描述符进行匹配，通过K维树关联描述符空间中接近的模型-场景描述符确定点对点的对应关系。

8.根据权利要求7所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，对工件的识别是通过加强几何一致性消除伪对应，在三维空间中，利用模型点云与当前场景点云之间的对应关系，通过局部坐标系将每个场景特征点与模型质心的相对位置相关联，使得每个对应的场景特征在三维霍夫空间中进行投票，根据票数确定工件在三维空间中的位置。

9.根据权利要求8所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，计算出场景点云中的工件实例相对于模板点云的相对位姿关系具体如下：

设模型特征点为F_i ^M、场景特征点为

模型点云和场景点云的质心分别为C^M、C^S，C^M和F_i ^M之间的向量表示为：

其中，质心的定义为

为保证向量在不同坐标系下具有旋转平移不变性，通过以下变换将向量

转化到局部参考系即为特征点坐标系：

其中符号·表示矩阵乘法，

是模型全局坐标系到局部坐标系间的变换矩阵，其每一行是模型特征点F_i ^M的局部参考系的单位向量，表示为：

于是，每个特征点F_i ^M与其对应的向量

关联在一起，其中

表示局部坐标系；

对每对场景特征点

与模型特征点F_i ^M建立的联系：

由于局部参考系的旋转平移不变性，所以有

其中

表示场景点云中的特征点指向质心的向量，

表示模型点云中的特征点指向质心的向量；

将

转换到场景的全局参考系中：

其中，

是通过以场景特征

的局部坐标系

的单位向量的列排列组成的变换矩阵，表示为：

根据以上转换关系，场景特征点

通过向量

在3D霍夫空间中投票，在霍夫参数空间中，峰值的存在表明场景中存在目标；

根据上述转换关系，得到的三组及以上模型-场景特征点对应关系，求得场景点云中的工件实例相对于工件模板点云的相对位姿关系。

10.根据权利要求9所述的基于三维点云特征的工业机器人工件识别与位姿估计方法，其特征在于，步骤S4是先根据仿射变换得到模板点云相对三维扫描仪坐标系下的位姿关系，以及场景点云的工件实例相对于三维扫描仪坐标系下的位姿关系，再通过手眼标定得到三维扫描仪相对工业机器人基坐标系下的位姿关系，最终求得场景点云的工件实例相对于工业机器人基坐标系下的位姿关系。

Images