一种基于点云数据的工件位姿快速高精度估算方法及装置制造方法及图纸

本发明专利技术公开一种基于点云的工件位姿估计方法，主要用以解决自动化装配领域，特别是小部件装配领域的工件位姿估计问题。该方法分为点云数据预处理、点云虚拟视图提取和位姿估计，其中位姿估计步骤还包括：基于迭代最近点算法的动态模型，采用粒子滤波算法进行迭代运算，如果达到迭代停止条件，则将有效粒子的位姿以加权均值的方式输出，然后通过求逆运算，计算所述的目标工件相对于相机坐标系下的位姿。本发明专利技术还涉及一种装置，其包括存储器和处理器，当所述处理器执行存储器中储存的程序时实现上述方法步骤。

A fast and high precision method and device of workpiece pose estimation based on point cloud data

【技术实现步骤摘要】
一种基于点云数据的工件位姿快速高精度估算方法及装置
本专利技术涉及机器视觉的物体位姿估计
，具体涉及一种基于点云数据的工件位姿快速高精度估算方法和装置。
技术介绍
随着工业机器人技术的发展，越来越多的自动化生产线上采用机器人作业，在机器人自动分拣、自动装配等领域，视觉引导机器人作业越来越重要。传统的视觉引导基于2D图像，而近年来，使用三维扫描设备(如三维激光扫描仪、结构光相机等)获取工件表面的三维点云数据(即工件点云)，并将工件点云和工件CAD模型采样的模板点云进行配准得到两者之间的刚性变换，从而实现工件的位姿估计成为了机器视觉领域的研究热点。利用工件的点云信息做位姿估计，不需要在工件表面粘贴标志点，更高效和接近实际生产场景。基于点云信息做位姿估计，传统的做法是采用迭代最近点算法(IterativeClosestPoint，ICP)及其改进方法。但是其最大的一个缺点就是受初始给定位姿的精度影响，易陷入局部最优。目前解决的办法是采用全局配准加ICP的方法，来尽可能获得精确的位姿。全局位姿配准算法主要基于点云的特征，根据特征点对点云进行点对匹配，根据这些点可以计算点云的初始变换矩阵。描述点云特征的方法有很多，如局部特征描述方法有：SI、3DSC、LSP、PFH、FPFH、USC、SHOT、TriSI、RoPS等；全局的特征描述方法有：VFH、CVFH。除此之外主成分分析PCA也可以获得点云的初始位姿。但是基于点云特征求解初始位姿的方法都有一个问题，就是对于几何特征本来就不丰富的点云，求解效果不佳，因此导致ICP求解的位姿精度不高。对于基于点云位姿估计问题，目前也有学者通过深度学习的方法进行尝试。通过将点云数据输入到三维的卷积神经网络中，可以直接预测物体的3D位姿。但是基于深度学习的方法，需要使用大量的数据进行训练，目前所见的采用深度学习的方法做基于点云的位姿估计，主要还是采用公开的数据集中的数据，主要针对生活场景下的一些物品的位姿估计，其位姿估计精度太低，并不能满足工业场景下机器人抓取和装配精度需求。同时深度学习的方法，需要消耗较多的时间做训练，并且其计算设备的价格都是极其昂贵的，目前工业界还并不普及。针对以上传统方法和深度学习的方法存在的问题，如何做到可以应用与机器人抓取和装配精度的位姿估计，成为当前机器视觉领域的研究热点。
技术实现思路
本专利技术的技术方案第一方面为一种基于点云数据的工件位姿估算方法，所述方法包括以下步骤：S1、通过3D传感器获取目标工件的场景点云，并从场景点云中分割出目标工件点云数据，其次，利用预先建模的目标工件CAD模型，转换得到物体模型的点云数据样板；S2、基于物体模型点云坐标系，在球面上按照预设规则生成多个视点粒子，在每一个视点粒子位置模拟虚拟3D相机对物体模型点云数据进行成像，捕获在该虚拟3D相机视角下的物体模型点云数据样板的可见虚拟点云数据；S3、基于迭代最近点算法的动态模型，并采用粒子滤波算法，使分割出目标工件点云数据与多个虚拟点云数据进行匹配性迭代运算，如果达到迭代停止条件，则将有效视点粒子的位姿以加权均值的方式输出，然后通过求逆运算，计算所述的目标工件基于该有效视点粒子所关联的相机坐标系的位姿，以作为实际的目标工件位姿的估算结果.其中所述的迭代停止条件包括以下的任意一种或多种条件：迭代次数大于预设的迭代次数；目标点云与视点粒子形成的3D点云之间的欧式距离均方误差小于预设的阈值；连续三次迭代过程的欧式距离均方误差之间的差值小于设定值。在根据本专利技术的一些实施例中，所述步骤S1包括：S1.1、通过3D传感器获取目标工件上的点的颜色信息和深度信息，以构建场景点云；S1.2、利用点云凸凹性分割函数将目标点云从场景点云中进行分割，同时根据背景颜色与物体的差异，从二维图像上进一步进行分割，将有效的二维点对应的点云数据提取；S1.3、利用半径滤波算法，将不在邻域半径范围内的杂点滤除；S1.4、将得到的目标点云与CAD模型按照预设的点云密度进行采样，其中的采样密度根据位姿估计精度和实际计算资源情况来调整。在根据本专利技术的一些实施例中，所述步骤S2包括：S2.1、通过隐藏点移除算法，将物体模型的点云数据投影到双域上，提取在凸包上的点作为可视点，实施点云可见性的分析；S2.2、将可见点集由物体模型坐标系转换到相机坐标系。在根据本专利技术的一些实施例中，所述步骤S3包括：S3.1、以物体点云模型的原点为球心，在球面上均匀生成多个视点粒子；S3.2、在每一个视点粒子处，对模型点云进行虚拟3D相机点云视图生成；S3.3、根据公式xt＝d(xt-1,yt-1)＝ICPi(view(xt-1),yt-1)，驱动视点粒子进行状态更新，并且选取目标函数作为收敛条件，式中ICP表示迭代最近点运算函数，i表示迭代次数，xt表示t时刻视点粒子的状态，xt-1表示t-1时刻视点粒子的状态，yt-1表示t-1时刻视点粒子的观测数据，此处为目标点云，view(xt-1)表示t-1时刻视点粒子xt-1的可见点云视图，p表示目标点云坐标，q表示源点云坐标，R和T分别表示旋转和平移变换矩阵；S3.4、将上一步骤计算得到的对应点对的欧式距离均方误差的负值作为自然底数的指数，完成视点粒子权值转移概率的计算，再将该概率乘以视点粒子上一时刻的权值，则得到当前时刻的权值；S3.5、执行视点粒子重采样，将权值大的视点粒子替换权值小的视点粒子；S3.6、如果迭代运算达到所述的迭代停止条件，则返回执行步骤S3.2。在根据本专利技术的一些实施例中，所述步骤S3.1还包括：进行视点粒子初始化，使每个视点粒子对应一个虚拟3D相机，且每个初始化的视点粒子的权值为粒子数量的倒数；斐波那契球面分布方式，使视点粒子均匀地分布在以物体模型点云中心为球心的球面上。在根据本专利技术的一些实施例中，所述步骤S3.4还包括通过以下公式计算当前时刻的权值：其中，n表示视点粒子总数目，fitness(view(xt),yt)为粒子适应性函数，由迭代最近点算法计算的配准均方误差得出，w表示视点粒子权重。在根据本专利技术的一些实施例中，所述步骤S3.5还包括：根据公式：其中，计算有效的视点粒子的数量Neff，当Neff小于等于粒子总数的60％时，采用赌徒轮盘的算法，增加有效视点粒子数目。本专利技术的技术方案第二方面为一种计算机装置，包括存储器和处理器。所述处理器执行存储器中储存的程序时实现上述方法。本专利技术的技术方案第三方面为一种计算机可读介质，其上储存有计算机程序。所述计算机程序被处理器执行时实现上述方法。本专利技术的有益效果为：可在全局范围内搜索最佳位姿，解决传统ICP算法易陷入局部最优的问题，具有更高的位姿估计精度。其可见点本文档来自技高网...

【技术保护点】
1.一种基于点云数据的工件位姿估算方法，其特征在于，所述方法包括以下步骤：/nS1、通过3D传感器获取目标工件的场景点云，并从场景点云中分割出目标工件点云数据，其次，利用预先建模的目标工件CAD模型，转换得到物体模型的点云数据样板；/nS2、基于物体模型点云坐标系，在球面上按照预设规则生成多个视点粒子，在每一个视点粒子位置模拟虚拟3D相机对物体模型点云数据进行成像，捕获在该虚拟3D相机视角下的物体模型点云数据样板的可见虚拟点云数据；/nS3、基于迭代最近点算法的动态模型，并采用粒子滤波算法，使分割出目标工件点云数据与多个虚拟点云数据进行匹配性迭代运算，如果达到迭代停止条件，则将有效视点粒子的位姿以加权均值的方式输出，然后通过求逆运算，计算所述的目标工件基于该有效视点粒子所关联的相机坐标系的位姿，以作为实际的目标工件位姿的估算结果，/n其中所述的迭代停止条件包括以下的任意一种或多种条件：/n迭代次数大于预设的迭代次数；/n目标点云与视点粒子形成的3D点云之间的欧式距离均方误差小于预设的阈值；/n连续三次迭代过程的欧式距离均方误差之间的差值小于设定值。/n

【技术特征摘要】
1.一种基于点云数据的工件位姿估算方法，其特征在于，所述方法包括以下步骤：
S1、通过3D传感器获取目标工件的场景点云，并从场景点云中分割出目标工件点云数据，其次，利用预先建模的目标工件CAD模型，转换得到物体模型的点云数据样板；
S2、基于物体模型点云坐标系，在球面上按照预设规则生成多个视点粒子，在每一个视点粒子位置模拟虚拟3D相机对物体模型点云数据进行成像，捕获在该虚拟3D相机视角下的物体模型点云数据样板的可见虚拟点云数据；
S3、基于迭代最近点算法的动态模型，并采用粒子滤波算法，使分割出目标工件点云数据与多个虚拟点云数据进行匹配性迭代运算，如果达到迭代停止条件，则将有效视点粒子的位姿以加权均值的方式输出，然后通过求逆运算，计算所述的目标工件基于该有效视点粒子所关联的相机坐标系的位姿，以作为实际的目标工件位姿的估算结果，
其中所述的迭代停止条件包括以下的任意一种或多种条件：
迭代次数大于预设的迭代次数；
目标点云与视点粒子形成的3D点云之间的欧式距离均方误差小于预设的阈值；
连续三次迭代过程的欧式距离均方误差之间的差值小于设定值。


2.根据权利要求1所述的方法，其特征在于，所述步骤S1包括：
S1.1、通过3D传感器获取目标工件上的点的颜色信息和深度信息，以构建场景点云；
S1.2、利用点云凸凹性分割函数将目标点云从场景点云中进行分割，同时根据背景颜色与物体的差异，从二维图像上进一步进行分割，将有效的二维点对应的点云数据提取；
S1.3、利用半径滤波算法，将不在邻域半径范围内的杂点滤除；
S1.4、将得到的目标点云与CAD模型按照预设的点云密度进行采样，其中的采样密度根据位姿估计精度和实际计算资源情况来调整。


3.根据权利要求1所述的方法，其特征在于，所述步骤S2包括：
S2.1、通过隐藏点移除算法，将物体模型的点云数据投影到双域上，提取在凸包上的点作为可视点，实施点云可见性的分析；
S2.2、将可见点集由物体模型坐标系转换到相机坐标系。


4.根据权利要求1所述的方法，其特征在于，所述步骤S3包括：
S3.1、以物体点云模型的原点为球心，在球面上均匀生成多个视点粒子；
S3.2、在每一个视点粒子处，对模型点云进行虚拟3D相机点云视图生成；
S3.3、根据公式x...

【专利技术属性】
技术研发人员：楼云江，王瑶，杨先声，古维昆，董邓伟，
申请(专利权)人：哈尔滨工业大学深圳，
类型：发明
国别省市：广东;44