[发明专利]基于3D点云数据的物体抓取位姿计算方法

权利要求书

1.一种基于3D点云数据的物体抓取位姿计算方法，其特征在于：所述方法包括以下步骤：

步骤1、物体点云数据提取；

通过线激光位移传感器扫描获取被测物体与流水线，得到物体以及传送带平面的点云数据，然后基于高度信息提取出物体的点云数据；

步骤2、基于邻域点个数进行点云噪点过滤；

依次计算点云数据中每个点与周围邻域点的欧式距离，同时统计阈值距离内邻域点的数量，如果该数量小于设定阈值，则判断该点在整体点云数据中属于噪声点；

步骤3、点云分割聚类处理；

计算当前点与已知参考点的欧式距离，大于设定的距离阈值则为不同类；小于阈值则为同一类；

步骤4、物体摆放状态判断与抓取位姿提取；

物体摆放状态判断：首先基于单个物体点云高度信息提取物体部分点云数据，再计算其最小外接矩形获取物体局部的长宽；将该局部长宽与预设的长宽值进行对比来判断该物体局部的大小，进而确定物体的摆放状态：部分点云最小外接矩形的长宽与预设值的差值小于阈值时，则判断为正确摆放状态，否则判断为该物体摆放状态不是预想的摆放状态；

对于摆放状态正确的物体再计算其中心点以及其摆放的角度，由此获取机械臂抓取位姿；

物体抓取中心点计算：取正确摆放状态的物体点云数据在Z轴的最高点，往下截取一定高度的点集数据，计算这部分点集数据的中心点即为抓取中心点A；

物体角度计算：对于摆放状态正确的物体，求出整个物体点云模型的最小外接矩形的中点B，与上述抓取中心点A相减获得一个向量计算向量与X轴正方向形成的夹角，其取值范围是(-180°,180°]，其中向量位于第一、二象限为正，位于第三、四象限为负。

2.根据权利要求1所述的基于3D点云数据的物体抓取位姿计算方法，其特征在于：所述步骤2中，在判断某个点的邻域点数量时，点之间空间欧式距离的计算只判断该点所处的线状点集以及左右两条线状点集，即该点当前帧以及前后帧扫描得到的三条线状点集。

3.根据权利要求1所述的基于3D点云数据的物体抓取位姿计算方法，其特征在于：所述步骤3具体如下：

按照点云数据生成顺序依次判断点云每条线状点集中的每一个点与已知参考点的距离，即第一条线状点集的第一个点自动归为第一类，计算线状点集第二个点与第一个点的欧氏距离，若小于设定距离阈值则把第二个点归为第一类，同时设定该归类点为最新参考点；若大于阈值则搜索第一类点集的上一条线状点集是否有该点的邻域点，若有则归为第一类点集，并设该点为第一类最新参考点；若无则归为第二类，同时设该点为第二类最新参考点；

计算第三个点与第一类参考点及第二类参考点的欧式距离，当与第一类参考点的距离小于阈值时，则该点归到第一类；否则，搜索第一类点集的上一条线状点集是否有该点的邻域点，若有则归为第一类点集；若无则判断该点与第二类参考点的欧式距离，若小于阈值则归为第二类，若大于阈值则搜索第二类点集的上一条线状点集是否有该点的邻域点，若有则归为第二类点集，并设该点为第二类最新参考点；若无则归为第三类点集；

以此类推依次计算一条线状点集的每一个点，当第一条线状点集判断完则开始判断第二条线状点集，以此类推；

聚类完成后，判断每个类的中心点之间的距离，若小于阈值，则合并为同一类。