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

权利要求书

1.一种基于点云数据的工件位姿估算方法，所述工件用于被机器人抓取，其特征在于，所述方法包括以下步骤：

S1、通过3D传感器获取目标工件的场景点云，并从场景点云中分割出目标工件点云数据，其次，利用预先建模的目标工件CAD模型，转换得到物体模型的点云数据样板；

S2、基于物体模型点云坐标系，在球面上按照预设规则生成多个视点粒子，在每一个视点粒子位置模拟虚拟3D相机对物体模型点云数据进行成像，捕获在该虚拟3D相机视角下的物体模型点云数据样板的可见虚拟点云数据；

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

其中所述的迭代停止条件包括以下的任意一种或多种条件：

迭代次数大于预设的迭代次数；

目标点云与视点粒子形成的3D点云之间的欧式距离均方误差小于预设的阈值；

连续三次迭代过程的欧式距离均方误差之间的差值小于设定值；

其中，所述步骤S2包括：

S2.1、通过隐藏点移除算法，将物体模型的点云数据投影到双域上，提取在凸包上的点作为可视点，实施点云可见性的分析，在线生成用于确定物体大致位姿的物体稀疏模板，其中，在应用隐藏点移除算法时，选取切除球半径与实际工作距离的关系为r＝k*dis+b，其中，r为切除球半径，dis为实际工作距离，k和b是常数，且0.3m≤dis≤0.45m，k＝1.1094，b＝3.0133；

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)为粒子适应性函数，由迭代最近点算法计算的配准均方误差得出，

ω表示视点粒子权重；

所述步骤S3.5还包括：

根据公式：

其中，

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