[发明专利]一种基于点云位姿估计的随机堆放工件定位方法

权利要求书

1.一种基于点云位姿估计的随机堆放工件定位方法，其特征在于，该方法包括以下步骤：

S1、使用数字建模软件获取工件3D点云数据，剔除部分冗余数据；

S2、计算步骤S1中工件点云数据法线，并约束法线方向统一指向模型外部，对带有法线的点云下采样，根据每个点的形状索引特征和可见概率计算投票权重；

S3、离线训练阶段，提取点对特征，将特征存储至查找表中的16邻域；计算每个模型点的局部参考系，用以后期假设验证；

S4、在线匹配阶段，计算场景点云的点对特征，在预训练查找表中寻找匹配关系，利用预计算的投票权重在投票表中投票；计算场景点局部参考系用以验证投票生成的位姿矩阵；对通过验证的位姿矩阵进行聚类获取更高精度的位姿估计结果；

S5、使用票数高的一定百分比的位姿假设进行欧式聚类，提取属于不同实例的位姿；对聚类结果，提取模型在粗位姿下的可见点，使用改进的ICP算法进行精确配准以定位目标；计算模型点与场景点的重叠率，筛选未被遮挡的实例作为待抓取对象。

2.如权利要求1所述的方法，其特征在于，步骤S2具体包括以下步骤：

S2-1、计算点云分辨率；

S2-2、使用主成分分析法计算所有点的法线；

S2-3、对所计算的法线重定向，将法线统一指向模型外部；

S2-4、对所述模型的点云进行体素栅格均匀采样，对下采样后的点云数据计算形状索引特征和可见概率；

S2-5、计算点对的投票权重。

3.如权利要求2所述的方法，其特征在于，所述步骤S2-3包括以下具体步骤：

S2-31、构造模型点云欧式距离最小生成树；

S2-32、在欧式最小生成树中加入邻域信息构造黎曼图；

S2-33、计算黎曼图最小生成树；

S2-34、选择初始点，以该点法线为参考，遍历黎曼最小生成树进行法向传播。

4.如权利要求1所述的方法，其特征在于，步骤S3包括以下具体步骤：

S3-1、计算点对特征

使用模型中两点及其法线计算点对特征，点m₁和其法线n₁，与点m₂和其法线n₂组成的点对特征F(m₁,m₂)计算方法为：

F(m₁,m₂)＝(||d||₂,∠(n₁,d),∠(n₂,d),∠(n₁,n₂)) (8)

式中，d表示两点连线，∠(n₁,d),∠(n₂,d),∠(n₁,n₂)为两法线与两点连线两两组合的夹角；

S3-2、计算模型点局部参考系：

查找模型点p半径R邻域内的临近点，假设有n个临近点，以点p近似作为重心，计算协方差矩阵M：

式(9)中，d_i表示临近点p_i到重心点p的欧氏距离；

使用奇异值分解计算协方差矩阵M的特征值和特征向量，对向量方向进行约束，作为点p的局部参考系，约束方法为：

将最小特征值对应的特征向量作为局部坐标系Z轴，将其约束至与法线n_p方向一致：

其中，Z⁺为最小特征值对应的特征向量的方向，Z^-为其相反方向，n_p为p点法线；

局部参考系X轴的方向约束方法为：

其中，X⁺为最大特征值对应的特征向量的方向，X^-为相反方向；和的计算方法为：

在Z轴与X轴方向固定的情况下，Y轴的方向由Z叉乘X计算：

Y＝Z×X (13)。