基于3D点云数据的物体抓取位姿计算方法与流程

基于3d点云数据的物体抓取位姿计算方法
技术领域
1.本发明涉及物体抓取技术领域，具体涉及一种基于3d点云数据的物体抓取位姿计算方法。


背景技术：

2.物体的识别效率和抓取成功率是决定相关自动化产品是否研制成功的关键。目前的物体识别方法绝大多数都是采用相机获取图像进行处理，需要事先建立数据库且计算方法复杂，特别是对于零件等物体的摆放状态无法正确判断，造成抓取成功率低下，严重影响设备运转效率。近年来线激光传感器不断普及，点云数据的处理算法也不断丰富，相比于二维图像其优势在于精度高、有深度信息、受环境光照影响较小等优势，在定位与抓取领域拥有广阔的应用前景，如物流的分拣、行李的搬运、工厂的上下料等等。
3.如何利用点云数据进行物体抓取位姿的计算成为本领域研究的方向。


技术实现要素：

4.本发明的目的在于提供一种基于3d点云数据的物体抓取位姿计算方法，其通过自动判断流水线上物体的摆放状态和抓取位姿，实现高效抓取。
5.为实现上述目的，本发明采用的技术方案是：
6.一种基于3d点云数据的物体抓取位姿计算方法，其包括以下步骤：
7.步骤1、物体点云数据提取；
8.通过线激光位移传感器扫描获取被测物体与流水线，得到物体以及传送带平面的点云数据，然后基于高度信息提取出物体的点云数据；
9.步骤2、基于邻域点个数进行点云噪点过滤；
10.依次计算点云数据中每个点与周围邻域点的欧式距离，同时统计阈值距离内邻域点的数量，如果该数量小于设定阈值，则判断该点在整体点云数据中属于噪声点；
11.步骤3、点云分割聚类处理；
12.计算当前点与已知参考点的欧式距离，大于设定的距离阈值则为不同类；小于阈值则为同一类；
13.步骤4、物体摆放状态判断与抓取位姿提取；
14.物体摆放状态判断：首先基于单个物体点云高度信息提取物体部分点云数据，再计算其最小外接矩形获取物体局部的长宽；将该局部长宽与预设的长宽值进行对比来判断该物体局部的大小，进而确定物体的摆放状态：部分点云最小外接矩形的长宽与预设值的差值小于阈值时，则判断为正确摆放状态，否则判断为该物体摆放状态不是预想的摆放状态；
15.对于摆放状态正确的物体再计算其中心点以及其摆放的角度，由此获取机械臂抓取位姿；
16.物体抓取中心点计算：取正确摆放状态的物体点云数据在z轴的最高点，往下截取
一定高度的点集数据，计算这部分点集数据的中心点即为抓取中心点a；
17.物体角度计算：对于摆放状态正确的物体，求出整个物体点云模型的最小外接矩形的中点b，与上述抓取中心点a相减获得一个向量计算向量与x轴正方向形成的夹角，其取值范围是(-180
°
,180
°
]，其中向量位于第一、二象限为正，位于第三、四象限为负。
18.所述步骤2中，在判断某个点的邻域点数量时，点之间空间欧式距离的计算只判断该点所处的线状点集以及左右两条线状点集，即该点当前帧以及前后帧扫描得到的三条线状点集。
19.4、所述步骤3具体如下：
20.按照点云数据生成顺序依次判断点云每条线状点集中的每一个点与已知参考点的距离，即第一条线状点集的第一个点自动归为第一类，计算线状点集第二个点与第一个点的欧氏距离，若小于设定距离阈值则把第二个点归为第一类，同时设定该归类点为最新参考点；若大于阈值则搜索第一类点集的上一条线状点集是否有该点的邻域点，若有则归为第一类点集，并设该点为第一类最新参考点；若无则归为第二类，同时设该点为第二类最新参考点；
21.计算第三个点与第一类参考点及第二类参考点的欧式距离，当与第一类参考点的距离小于阈值时，则该点归到第一类；否则，搜索第一类点集的上一条线状点集是否有该点的邻域点，若有则归为第一类点集；若无则判断该点与第二类参考点的欧式距离，若小于阈值则归为第二类，若大于阈值则搜索第二类点集的上一条线状点集是否有该点的邻域点，若有则归为第二类点集，并设该点为第二类最新参考点；若无则归为第三类点集；
22.以此类推依次计算一条线状点集的每一个点，当第一条线状点集判断完则开始判断第二条线状点集，以此类推；
23.聚类完成后，判断每个类的中心点之间的距离，若小于阈值，则合并为同一类。
24.采用上述方案后，本发明首先获取物体的点云数据，然后基于邻域点个数对点云数据进行噪点滤波，接着再基于欧式距离阈值对点云数据进行分割聚类处理，以确定物体数量信息，最后基于最小外接矩形判断每组物体的状态是否符合抓取要求，对于符合要求的再计算其抓取位姿。可见，本发明在进行抓取位姿计算之前，先进行物体状态判断，只有符合抓取要求的物体才会进行抓取位姿计算，这样可以提高机械臂的抓取效率，实现物体的高效准确抓取。
附图说明
25.图1为本发明的流程图；
26.图2为物体点云数据提取示意图；
27.图3为点云数据现状点集示意图；
28.图4为物体点云滤波效果图；
29.图5为物体点云分割聚类效果图；
30.图6为物体的摆放状态示意图(不是预想摆放状态)；
31.图7为抓取中心点计算示意图；
32.图8为物体抓取位姿计算示意图。
具体实施方式
33.如图1所示本发明揭示了一种基于3d点云数据的物体抓取位姿计算方法，其包括以下步骤：
34.步骤1、物体点云数据提取；
35.物体摆放于流水线表面，通过线激光位移传感器扫描获取被测物体与流水线，得到物体以及传送带平面的点云数据，然后基于高度信息提取出物体的点云数据。因为物体是高于传送带平面的，所以直接根据高度信息可以直接提取出物体的点云数据。
36.图2所示的点云数据提取图中，物体为接线端子。
37.步骤2、基于邻域点个数进行点云噪点过滤；
38.基于邻域点个数的点云噪点过滤方法是基于欧氏距离实现的，其思想是：点分布相对比较稠密，统计学特征就是其与邻域点的距离较小。如果某个点与其邻域某些点距离过大，则可判定为噪声点。
39.具体地，依次计算点云数据中每个点与周围邻域点的欧式距离，同时统计阈值距离内邻域点的数量，如果该数量小于设定阈值，则判断该点在整体点云中属于噪声点。
40.本发明是基于线激光扫描获取点云数据，所以在判断某个点的邻域点数量时，点之间空间欧式距离的计算只判断该点所处的线状点集以及左右两条线状点集(图3示意)，即该点当前帧以及前后帧扫描得到的三条线状点集，这样可减小计算量。
41.过滤噪声点后的点云数据图如图4所示(以接线端子为例)，经过滤波的点云数据有效过滤了激光反射产生的噪声点，有助于后续物体聚类与抓取位姿的判断。
42.步骤3、点云分割聚类处理；
43.物体点云过滤后，需要判断点云数据由几组物体组成，物体间是否挨得太近而无法抓取，因此需要对点云数据进行分割聚类。
44.本发明中，点云分割聚类处理同样是基于欧氏距离法，其原理是：计算当前点(未知点)与已知参考点的欧式距离，大于设定的距离阈值则为不同类；小于阈值则为同一类。
45.具体地，按照点云数据生成顺序依次判断点云每条线状点集中的每一个点与已知参考点的距离，即第一条线状点集的第一个点自动归为第一类(归类后则为已知点)，计算线状点集第二个点与第一个点的欧氏距离，若小于设定距离阈值则把第二个点归为第一类，同时设定该归类点为最新参考点；若大于阈值则搜索第一类点集的上一条线状点集是否有该点的邻域点(即距离小于阈值的点)，若有则归为第一类点集，并设该点为第一类最新参考点；若无则归为第二类，同时设该点为第二类最新参考点。计算第三个点与第一类参考点及第二类参考点(若有)的欧式距离，当与第一类参考点的距离小于阈值时，则该点归到第一类；否则，搜索第一类点集的上一条线状点集是否有该点的邻域点(即距离小于阈值的点)，若有则归为第一类点集；若无则判断该点与第二类参考点的欧式距离，若小于阈值则归为第二类，若大于阈值则搜索第二类点集的上一条线状点集是否有该点的邻域点，若有则归为第二类点集，并设该点为第二类最新参考点；若无则归为第三类点集。以此类推依次计算一条线状点集的每一个点，当第一条线状点集判断完则开始判断第二条线状点集(此时开始存在上一条线状点集)，以此类推。聚类完成后，还需判断每个类的中心点之间的距离，若小于阈值，则合并为同一类。
46.进行数据处理后的点云数据图如图5所示(同样是以接线端子为例)，从图5中可以
清楚的看出，经过分割聚类处理后，可以清楚分割出不同物体的点云数据，即可获取单个物体的点云数据。
47.步骤4、物体摆放状态判断与抓取位姿提取；
48.获取单个物体的点云数据后，需要判断物体的摆放状态以确定是否可以抓取，本发明通过判断截取的部分点云数据的最小外接矩形的长宽来确定。
49.物体摆放状态判断：首先基于单个物体点云高度信息提取物体部分点云数据(根据点云高度信息从最高点往下截取一定高度的点云数据。高度根据夹爪大小等实际情况预设)，再计算其最小外接矩形获取物体局部的长宽；将该局部长宽与预设的长宽值进行对比来判断该物体局部的大小，进而确定物体的摆放状态：部分点云最小外接矩形的长宽与预设值的差值小于阈值时，则判断为正确摆放状态，否则判断为该物体摆放状态不是预想的摆放状态(如图6)。对于摆放状态正确的物体再计算其中心点以及其摆放的角度(最小外接矩形长边相对x轴正方向的角度)，由此获取机械臂抓取位姿。
50.物体抓取中心点计算：取正确摆放状态的物体点云数据在z轴的最高点，往下截取设定高度的点集数据，计算这部分点集数据的中心点即为抓取中心点a，如图7所示。设定高度根据夹爪大小等实际情况进行预设。
51.物体角度计算：对于摆放状态正确的物体，求出整个物体点云模型的最小外接矩形的中点b，与上述抓取中心点a相减获得一个向量计算向量与x轴正方向形成的夹角，其取值范围是(-180
°
,180
°
]，其中向量位于第一、二象限为正，位于第三、四象限为负。计算效果如图8所示。
52.通过上述抓取中心点和抓取角度可计算得到机械臂的抓取位姿，机械臂通过该抓取位姿可高效抓取物体。
53.综上，本发明首先获取物体的点云数据，然后基于邻域点个数对点云数据进行噪点滤波，接着再基于欧式距离阈值对点云数据进行分割聚类处理，以确定物体数量信息，最后基于最小外接矩形判断每组物体的状态是否符合抓取要求，对于符合要求的再计算其抓取位姿。可见，本发明在进行抓取位姿计算之前，先进行物体状态判断，只有符合抓取要求的物体才会进行抓取位姿计算，这样可以提高机械臂的抓取效率，实现物体的高效准确抓取。
54.以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。