阅读说明：本技术 一种基于点云数据的工件位姿快速高精度估算方法及装置 (Method and device for quickly and accurately estimating pose of workpiece based on point cloud data ) 是由 楼云江 王瑶 杨先声 古维昆 董邓伟 于 2019-08-30 设计创作，主要内容包括：本发明公开一种基于点云的工件位姿估计方法,主要用以解决自动化装配领域,特别是小部件装配领域的工件位姿估计问题。该方法分为点云数据预处理、点云虚拟视图提取和位姿估计,其中位姿估计步骤还包括：基于迭代最近点算法的动态模型,采用粒子滤波算法进行迭代运算,如果达到迭代停止条件,则将有效粒子的位姿以加权均值的方式输出,然后通过求逆运算,计算所述的目标工件相对于相机坐标系下的位姿。本发明还涉及一种装置,其包括存储器和处理器,当所述处理器执行存储器中储存的程序时实现上述方法步骤。(The invention discloses a point cloud-based workpiece pose estimation method, which is mainly used for solving the problem of workpiece pose estimation in the field of automatic assembly, in particular small part assembly. The method comprises the steps of point cloud data preprocessing, point cloud virtual view extraction and pose estimation, wherein the pose estimation step further comprises the following steps: and (3) carrying out iterative operation by adopting a particle filter algorithm based on a dynamic model of an iterative closest point algorithm, outputting the pose of the effective particles in a weighted mean value mode if an iteration stopping condition is reached, and then calculating the pose of the target workpiece relative to a camera coordinate system through inversion operation. The invention also relates to an apparatus comprising a memory and a processor, which when executing a program stored in the memory performs the above-mentioned method steps.)

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

技术领域

本发明涉及机器视觉的物***姿估计技术领域，具体涉及一种基于点云数据的工件位姿快速高精度估算方法和装置。

背景技术

随着工业机器人技术的发展，越来越多的自动化生产线上采用机器人作业，在机器人自动分拣、自动装配等领域，视觉引导机器人作业越来越重要。传统的视觉引导基于2D图像，而近年来，使用三维扫描设备(如三维激光扫描仪、结构光相机等)获取工件表面的三维点云数据(即工件点云)，并将工件点云和工件CAD模型采样的模板点云进行配准得到两者之间的刚性变换，从而实现工件的位姿估计成为了机器视觉领域的研究热点。利用工件的点云信息做位姿估计，不需要在工件表面粘贴标志点，更高效和接近实际生产场景。

基于点云信息做位姿估计，传统的做法是采用迭代最近点算法(IterativeClosest Point，ICP)及其改进方法。但是其最大的一个缺点就是受初始给定位姿的精度影响，易陷入局部最优。目前解决的办法是采用全局配准加ICP的方法，来尽可能获得精确的位姿。全局位姿配准算法主要基于点云的特征，根据特征点对点云进行点对匹配，根据这些点可以计算点云的初始变换矩阵。描述点云特征的方法有很多，如局部特征描述方法有：SI、3D SC、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、根据公式x_t＝d(x_t-1,y_t-1)＝ICP_i(view(x_t-1),y_t-1)，驱动视点粒子进行状态更新，并且选取目标函数作为收敛条件，式中

ICP表示迭代最近点运算函数，

i表示迭代次数，

x_t表示t时刻视点粒子的状态，

x_t-1表示t-1时刻视点粒子的状态，

y_t-1表示t-1时刻视点粒子的观测数据，此处为目标点云，

view(x_t-1)表示t-1时刻视点粒子x_t-1的可见点云视图，

p表示目标点云坐标，

q表示源点云坐标，

R和T分别表示旋转和平移变换矩阵；

S3.4、将上一步骤计算得到的对应点对的欧式距离均方误差的负值作为自然底数的指数，完成视点粒子权值转移概率的计算，再将该概率乘以视点粒子上一时刻的权值，则得到当前时刻的权值；

S3.5、执行视点粒子重采样，将权值大的视点粒子替换权值小的视点粒子；

S3.6、如果迭代运算达到所述的迭代停止条件，则返回执行步骤S3.2。

在根据本发明的一些实施例中，所述步骤S3.1还包括：

进行视点粒子初始化，使每个视点粒子对应一个虚拟3D相机，且每个初始化的视点粒子的权值为粒子数量的倒数；

斐波那契球面分布方式，使视点粒子均匀地分布在以物体模型点云中心为球心的球面上。

在根据本发明的一些实施例中，所述步骤S3.4还包括通过以下公式计算当前时刻的权值：

其中，

n表示视点粒子总数目，

fitness(view(x_t),y_t)为粒子适应性函数，由迭代最近点算法计算的配准均方误差得出，

w表示视点粒子权重。

在根据本发明的一些实施例中，所述步骤S3.5还包括：

根据公式：

其中，

计算有效的视点粒子的数量N_eff，当N_eff小于等于粒子总数的60％时，采用赌徒轮盘的算法，增加有效视点粒子数目。

本发明的技术方案第二方面为一种计算机装置，包括存储器和处理器。所述处理器执行存储器中储存的程序时实现上述方法。

本发明的技术方案第三方面为一种计算机可读介质，其上储存有计算机程序。所述计算机程序被处理器执行时实现上述方法。

本发明的有益效果为：

可在全局范围内搜索最佳位姿，解决传统ICP算法易陷入局部最优的问题，具有更高的位姿估计精度。其可见点分析采用HPR算法，可以在线生成某视点下的物体模型点云视图，相当于在线生成许多模板——即在大范围内使用少量稀疏模板，快速确定物体的大致位姿；在越接近物体真实位姿的范围内，生成更高密集的模板，以提高配准的精度。与传统的离线生成模板的配准方法相比，获得同等精度的前提下采用的模板更少，时间更快。同时HPR算法，具有对模型点云密度不敏感的特性。可以广泛适用于不同密度的点云模型。

附图说明

图1为在实施例中根据本发明方法的总体流程简图。

图2为点云预处理流程图。

图3为虚拟3D相机点云视图生成流程图。

图4为基于ICP动态过程，使用粒子滤波算法进行位姿估计的流程图。

图5为3D相机采集的场景点云示意图。

图6为在实施例中目标工件的CAD模型图。

图7为在实施例中所采集的目标工件的模型点云图。

图8为对本发明的位姿估算方法的验证平台。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

本文主要公开了一种基于点云的工件位姿估计方法，主要用以解决自动化装配领域，特别是小部件装配领域的工件位姿估计问题。如图1所示，该方法分为点云数据预处理、点云虚拟视图提取和位姿估计三大步骤。点云数据预处理，主要是将传感器获取的物体点云分割出来，作为目标点云；并将物体的CAD模型转换为点云数据，用于虚拟3D相机的点云视图生成。点云虚拟视图生成，主要模拟虚拟3D相机成像，获得该视角下的点云数据。位姿估计部分，基于ICP动态模型采用粒子滤波算法进行位姿估计，每个视点粒子代表一个可能的3D虚拟相机，利用ICP算法驱使视点粒子逼近最可能生成目标点云的相机位姿，经过求逆运算，实现相机坐标系下物体的位姿估计。该方法可在全局范围内搜索最佳位姿，解决传统ICP算法易陷入局部最优的问题，具有更高的精度。

下面结合图2至图7通过详细的实施例来描述上述方法步骤。

在一实施例中，一种基于点云的工件位姿估计方法，包括以下步骤：

S1：使用传感器获取目标工件的场景点云，并将目标工件从场景点云中分割出来，作为目标点云。利用待测量物体(即目标工件)的预先建模的CAD模型，转换为物体模型点云数据，用于虚拟3D相机的虚拟点云视图生成。

S2：虚拟3D相机的点云视图生成，即模拟虚拟3D相机成像过程。在当前虚拟3D相机所在位置，对物体模型点云数据进行成像，获取在该视角下的模型的可见点的虚拟点云数据。

S3：目标工件位姿估计过程：基于ICP动态模型，采用粒子滤波算法，使分割出目标工件点云数据与多个虚拟点云数据进行匹配性迭代运算，如果达到迭代停止条件，则将有效视点粒子的位姿以加权均值的方式输出，然后通过求逆运算，计算所述的目标工件基于该有效视点粒子所关联的相机坐标系的位姿，输出最终位姿估计结果。

换句话说，上述步骤的关键点在于：预先通过目标工件的CAD模型生成一个样板性质的模型点云，然后配置多个虚拟相机，将每个虚拟相机看到的点云生成虚拟点云数据；与拍摄的实际点云做ICP迭代运算。如果发现所比较的虚拟点云数据与采集分割后的现实点云数据重合度高，就可以将该组有效的虚拟点云关联的虚拟相机与实际相机建立关系，然后通过坐标变换矩阵，利用该组虚拟相机间接求取目标工件的估算位姿。

步骤S1具体实现方式如下：

S1.1：3D传感器获取场景点云数据。3D传感器包括但不限于：3D激光扫描仪、结构光相机、双目(多目)立体视觉系统等。3D传感器直接获取的数据可以是RGB或灰度图像+深度信息，由此通过算法可以构建场景的点云信息。

S1.2：分割出目标点云。可以使用PCL(Point Cloud Library：C++实现的点云数据处理库)中的点云凸凹性分割函数，将目标点云从场景点云中进行分割。也可以根据背景颜色与物体的差异，从二维图像上分割后，将有效的二维点对应的点云数据提取出来。例如，对图7采集的背景点云中，将目标工件(比如金属壳的手机)放在纯色(比如绿色)布前，使得更容易分割出目标工件的点云。

S1.3：点云降噪。分割出的目标点云可能包含一些杂点，参考半径滤波的思想，通过设定一定的邻域半径，将不在该范围内的杂点滤除。

S1.4：将得到的目标点云与预先建模的目标工件CAD模型按照一定的点云密度进行采样。例如，可以通过目标工件的CAD模型的体积范围来限定点云采样的空间范围。而采样密度根据位姿估计精度和实际计算资源而定。在一实施例中，采用的点云密度为0.02mm/点云。

步骤S2具体实现方式如下：

S2.1：模型点云数据在不同视点下的可见点提取。

采用HPR(hidden point removal)算法，通过将物体模型的点云数据投影到双域上，提取在凸包上的点作为可视点，完成点云可见性的分析。

优选地，对于应用HPR算法时的切除球半径r的选取，使r与实际工作距离的关系为r＝k*dis+b，其中，dis在0.3至0.45m的工作范围内，优化后的k拟取：1.1094，b拟取：3.0133。

S2.2：将可见点集由物体模型坐标系转换到相机坐标系。

采用步骤S2.1中所述的HPR算法提取的可见点集{P}，在物体模型坐标系下，根据当前虚拟3D相机和物体之间的相对变换矩阵T，将提取的可见点集{P}转换到虚拟3D相机坐标系。

至此，虚拟3D相机点云视图提取完成。

步骤S3的具体实现方式如下：

S3.1：粒子初始化。

粒子初始化(此处的每个粒子都对应一个虚拟3D相机)是指：在以物体点云模型的原点为球心，在一定半径的球面上按照一定的规则撒上粒子，用于后续虚拟3D相机点云视图的生成。

如果初始化粒子数设置为K，即有K个虚拟3D相机。拟设置的K为10。初始化粒子权值相等，为1/K。粒子的分布采用斐波那契球面分布方式，使粒子均匀地分布在以物体模型点云中心为球心的球面上。粒子状态表达为：

x_tk＝(x,y,z,R,P,Y)

式中x,y,z,R,P,Y分别代表该粒子在模型点云坐标系下的x,y,z坐标和Roll、Pitch、Yaw角度。

S3.2：虚拟3D相机点云视图生成。对每一个粒子进行虚拟3D相机点云视图生成，过程与步骤S2类似。

S3.3：粒子状态转移。

利用ICP算法，驱动粒子进行状态更新。

x_t＝d(x_t-1,y_t-1)＝ICP_i(view(x_t-1),y_t-1) (1)

式中：

i表示ICP迭代次数；

x_t表示t时刻粒子的状态；

x_t-1表示t-1时刻粒子的状态；

y_t-1表示t-1时刻粒子的观测数据，此处为目标点云；

view(x_t-1)表示t-1时刻粒子x_t-1的可见点云视图。

此处应用ICP算法时，原理为：给定两块在不同坐标系下的点云数据，目标点云P＝{p1,p1,…,pn}，源点云Q＝{q1,q2,…,qn}。对于源点云中的每一个点q1，在目标点云中找到距离其最近的点pk,根据下面公式的目标函数，找到最优的刚体变换矩阵的R和T。根据得到的R和T，变换源点云Q，直到满足收敛条件，则会将最终的R，T输出。

上述ICP目标函数为：

S3.4：粒子权值计算。将步骤S3.3中ICP计算得到的对应点对的欧式距离均方误差(MSE)的负值作为自然底数的指数，完成粒子权值转移概率的计算。再将概率乘以粒子上一时刻的权值，则得到当前时刻的权值。

式中：

n表示粒子总数目；

fitness(view(x_t),y_t)为粒子适应性函数，由ICP计算的配准均方误差计算得出；

w表示粒子权重。

S3.5：粒子重采样。在迭代过程中，会出现粒子权值退化的现象，解决的办法就是粒子重采样。

首先根据公式：

计算有效的粒子数，当Neff小于等于粒子总数的60％时，采用赌徒轮盘的方法，增加有效粒子数目；其过程相当于使用权值大的粒子替换权值小粒子，粒子重采样可以减少权重小的粒子对状态估计的影响，增加权重大的粒子对状态估计的影响。以下为使用赌徒轮盘方式后的粒子权值转移概率公式：

S3.6：判定算法迭代条件。

本算法在粒子状态转移过程中，采用ICP算法，因此采用ICP算法的迭代停止条件。根据需配准的两部分点云自身特征和精度要求，可设置为几种停止条件的组合，包括但不限于：ICP迭代次数小于k；目标点云与粒子形成的3D点云视图之间的MSE小于某阈值；连续三次ICP迭代MSE之差小于设定值。

如果算法运行过程达到迭代停止条件，则将有效粒子的位姿以加权均值的方式输出，该位姿则为目标工件坐标系下相机所处的位姿，通过求逆运算，可以获得该目标工件相对于相机坐标系下的位姿；若未达到迭代停止条件，则重复步骤S3.2-3.6。

在本发明的验证实例中，采用了手机后盖作为待估计的工件。以下为五组目标工件的位姿估计结果：

表1位姿估计算法结果

为衡量本发明的位姿估计精度，参照图8，通过控制机器10抓取目标工件20。可以利用多自由度运动底座30，将目标工件从位姿1移动到位姿2。将机器人10手持盒上的运动数据作为基准，衡量本发明的位姿估计精度。需注意的是，为避免引入机器人10和3D传感器40的手眼标定误差，因此，误差衡量指标转化为总的平移误差和轴角误差：

表2平移误差分析

表3轴角误差分析

表中，T₁₂表示从机器人从位姿1到位姿2的运动过程，如此类推。从表中可以看出，本发明提出的位姿估计算法具有平移精度在0.1mm级别，角度误差最大在1°左右；能够满足机器人装配等应用场景的需求。

以上所述的仅是本发明的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。