基于激光雷达的机器人实时控制方法与流程

1.本发明涉及机械手控制技术领域，具体涉及一种基于激光雷达的机器人实时控制方法。


背景技术：

2.随着机器人技术的发展，利用视觉感知控制机器人的机械手抓取零件已经比较普遍，随着技术的发展，基于二维图像进行机械手的控制已经不能满足机械手的准确度要求，于是出现了三维点云数据与机械手控制相结合的控制方法。
3.但是对于一些非常规形状特征的零件，其摆放姿态无法确定，并且可能存在零件特征被部分遮挡，此时通过激光雷达无法获取精确的结果，进而造成了在点云数据下机器人机械手的抓取不精准的问题，无法满足现代工厂对零件的精准抓取。


技术实现要素：

4.为了解决上述技术问题，本发明提供一种基于激光雷达的机器人实时控制方法，所采用的技术方案具体如下：本发明一个实施例提供了一种基于激光雷达的机器人实时控制方法，该方法包括以下步骤：接收待抓取零件的点云数据和对应标准零件的cad三维图，在cad三维图中获取标准零件的标准特征边，在点云数据中获取待抓取零件的目标特征边；特征边为对应零件的边缘；根据目标特征边与标准特征边的匹配结果移动所述点云数据得到更新点云数据，所述更新点云数据与cad三维图方向相同；基于更新点云数据中每两个点之间的距离将所述更新点云数据分组，并在每个组内获取每个点的局部离群因子；对于相邻的两个组，分别计算组中每个点到两个组中心的距离并求差值，当所述差值小于预设的距离阈值时，对应的点为异常点；将异常点对应的相邻的两个组记为异常组，去除异常组中的异常点，根据剩余点之间的距离获取该异常组的映射参考值；在该异常组中任意添加一个异常点，再获取对应的映射参考值作为映射值，对于每个异常点，计算对应的两个异常组的映射值和映射参考值的差值作为映射差异，进而获取该异常点的分布特征值；所述剩余点的分布特征值为预设值；根据所述局部离群因子和所述分布特征值获取每个点的偏移概率；基于所述偏移概率结合ransac算法获取每个组对应的点云平面；以优选度最高的点云平面作为抓取平面，发送抓取指令以使机械手抓取所述待抓取零件。
5.优选的，所述匹配结果的获取方法为：根据每两条相邻的目标特征边的方向向量获取方向参数；获取cad三维图的标准方向参数；根据方向参数和标准方向参数的比例关系将点云数据和cad三维图进行匹配得
到所述匹配结果。
6.优选的，所述方向参数的获取方法为：获取每条目标特征边的方向向量以及对应的模长，对于每两条相邻的目标特征边的方向向量，令两个方向向量中对应元素相乘再求和得到的结果作为分子，计算两条相邻的目标特征边的模长的和作为分母，以分子和分母的比值作为所述方向参数。
7.优选的，所述将点云数据和cad三维图进行匹配得到所述匹配结果的过程包括：对于每个方向参数，计算该方向参数与每个标准方向参数的比例，在所有方向参数对应的比例中，选取数值差异最小的一组比例作为匹配比例，所述匹配比例所对应的目标特征边与标准特征边为一对匹配边，所有匹配边组成所述匹配结果。
8.优选的，所述更新点云数据的获取方法为：基于相互匹配的目标特征边和标准特征边的差异获取待抓取零件的旋转角度和平移距离；基于所述旋转角度和平移距离对所述点云数据进行旋转和平移，成为与所述cad三维图方向相同的所述更新点云数据。
9.优选的，所述映射参考值的获取方法为：对于异常组中的每个剩余点，获取该剩余点在该异常组中与其他剩余点之间的最短距离，所有最短距离的平均值即为所述映射参考值。
10.优选的，所述分布特征值的获取方法为：将异常点所在的异常组的映射差异记为，将异常点对应的相邻的异常组的映射差异记为，当时，该异常点的分布特征值为所述预设值；当时，该异常点的分布特征值为0.01；当时，该异常点的分布特征值为0.5；所述预设值为1。
11.优选的，所述偏移概率的获取方法为：计算每个点对应的局部离群因子和分布特征值的比值，选取最大比值和最小比值，以每个比值与最小比值的差作为分子，以最大比值和最小比值的差作为分母，得到的分子和分母的比值为所述偏移概率。
12.优选的，所述基于所述偏移概率结合ransac算法获取每个组对应的点云平面，包括：在每个组中选取预设数量的点拟合为一个初始平面，利用预设的距离拟合阈值和所述偏移概率获取每个点的自适应距离阈值，当每个点与所述初始平面之间的距离小于对应的自适应距离阈值时，该点为所述初始平面的内点，初始平面以及对应的所有内点组成对应组的所述点云平面。
13.优选的，所述优选度的获取方法为：通过模拟器进行模拟获取机械手到达点云平面的调整次数；基于点云平面的质心和机械手的质心之间的连线获取机械手的行进距离；计算所述调整次数和所述行进距离的乘积，获取点云平面的面积，以面积和所述乘积的相反数的比值作为所述优选度。
14.本发明实施例至少具有如下有益效果：根据目标特征边与标准特征边的匹配结果对点云数据进行移动，使待抓取零件的点云数据与cad三维图方向相同，待抓取零件的摆放姿态可能比较随机，先将点云数据转换
至与标准零件的cad三维图同方向，方便后续分析；然后将更新点云数据分组，获取每组内的每个点的局部离群因子，局部离群因子代表了该点在所在组中的离散程度，离散程度越大说明该点越偏离所在的组，可能并不属于当前所在的组；通过计算组中每个点到两个组中心的距离并求差值，来筛选异常点，选取的异常点可能属于相邻的另一个组，因此通过计算每个组包含异常点与不包含异常点下的映射差异获取异常点的分布特征值，分布特征值代表了异常点属于当前所在组以及相邻组的分布情况；根据局部离群因子和分布特征值共同获取异常点的偏移概率，偏移概率越大，该异常点越有可能属于相邻组；基于偏移概率结合ransac算法获取每个组对应的点云平面，将偏移概率与ransac算法，考虑到同一组内可能存在其他组的点，使得同属一个组的点拟合的平面更加准确；然后选取优选度最高的点云平面作为抓取平面，控制机械手抓取待抓取零件，能够提高机械手的抓取精准度和实时性。
附图说明
15.为了更清楚地说明本发明实施例或现有技术中的技术方案和优点，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它附图。
16.图1为本发明一个实施例提供的一种基于激光雷达的机器人实时控制方法的步骤流程图。
具体实施方式
17.为了更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对依据本发明提出的一种基于激光雷达的机器人实时控制方法，其具体实施方式、结构、特征及其功效，详细说明如下。在下述说明中，不同的“一个实施例”或“另一个实施例”指的不一定是同一实施例。此外，一或多个实施例中的特定特征、结构或特点可由任何合适形式组合。
18.除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。
19.下面结合附图具体的说明本发明所提供的一种基于激光雷达的机器人实时控制方法的具体方案。
20.请参阅图1，其示出了本发明一个实施例提供的一种基于激光雷达的机器人实时控制方法的步骤流程图，该方法包括以下步骤：在本发明中通过机器人端进行信息接收、数据处理和指令输出，机器人端为控制机器人的执行系统如机械手的远程服务器端，通过本发明的方法获取机械手的调整参数，基于调整参数生成机械手的控制指令，发送至机械手，机械手依据接收的指令执行抓取任务。其中机器人端可以搭载于机械手上，也可以与机械手分离，即机器人端未搭载于机械手上，通过输出控制指令实现机器人端和机械手之间的交互。
21.步骤s001，接收待抓取零件的点云数据和对应标准零件的cad三维图，在cad三维图中获取标准零件的标准特征边，在点云数据中获取待抓取零件的目标特征边；特征边为
对应零件的边缘。
22.具体的步骤包括：获取待抓取零件的类型、零件号，待抓取零件通过水平方向的传送带传送，激光雷达固定在传送带一侧，扫描方向与传送带方向垂直，用于获取传送带上待抓取零件的点云数据，扫描高度与待抓取零件的高度相匹配，机械手固定在传送带上方用于抓取待抓取零件。通过激光雷达获取待抓取零件的点云数据，获取与待抓取零件同类型的标准零件的cad三维图，将待抓取零件的类型、零件号、点云数据和对应标准零件的cad三维图发送至机器人端，机器人端接收上述信息之后，通过angle criterion算法获取点云数据的目标特征边，angle criterion算法为估计点云边界的一种经典算法，为现有技术，在本发明实施例中不再详细描述具体过程。
23.cad三维图是由标准零件的边缘线构成，通过人工标注的方式进行标准特征边的标注。
24.目标特征边为待抓取零件的点云数据的边缘，标准特征边为cad三维图标注的边缘。
25.步骤s002，根据目标特征边与标准特征边的匹配结果移动点云数据得到更新点云数据，更新点云数据与cad三维图方向相同。
26.具体的步骤包括：根据每两条相邻的目标特征边的方向向量获取方向参数；获取cad三维图的标准方向参数；根据方向参数和标准方向参数的比例关系将点云数据和cad三维图进行匹配得到匹配结果。
27.目标特征边和标准特征边是一一对应的，因此通过计算上述两者之间的方向参数进行匹配。
28.获取每条目标特征边的方向向量以及对应的模长，对于每两条相邻的目标特征边的方向向量，令两个方向向量中对应元素相乘再求和得到的结果作为分子，计算两条相邻的目标特征边的模长的和作为分母，以分子和分母的比值作为方向参数。
29.由于cad三维图和点云数据拥有各自的坐标系，因此将点云数据转换到cad三维图的坐标系中以使坐标系统一。记待抓取零件的点云数据的第条目标特征边的方向向量为，则点云数据中第条目标特征边和相邻的第条目标特征边之间的方向参数为：其中，表示点云数据中的第条目标特征边的方向向量的坐标；表示点云数据中的第条目标特征边的方向向量的坐标；
表示第条目标特征边的方向向量的模长；表示第条目标特征边的方向向量的模长。
30.计算点云数据中每两条相邻目标特征边之间的方向参数，然后以同样的方法计算cad三维图中的标准方向参数。
31.由于待抓取零件的摆放姿态不同，即点云数据的目标特征边与cad三维图的标准特征边无法对应，但是待抓取零件的点云数据的目标特征边与cad三维图的标准特征边本身存在一一对应的关系，由于点云数据与cad三维图的大小不一定相同，因此计算两个方向参数的比例，当存在多个比例一致时，说明点云数据和cad三维图相互匹配，则通过计算点云数据的每个目标特征边，分别与cad三维图的每个标准特征边之间的方向参数的比例进行匹配。
32.匹配过程为：对于每个方向参数，计算该方向参数与每个标准方向参数的比例，在所有方向参数对应的比例中，选取数值差异最小的一组比例作为匹配比例，匹配比例所对应的目标特征边与标准特征边为一对匹配边，所有匹配边组成匹配结果。
33.例如，共有n个方向参数和n个标准方向参数，计算每个方向参数与n个标准方向参数的比例，每个方向参数均对应n个比例，在每个方向参数对应的n个比例中选取一个比例进行比较，选取n个方向参数对应的比例差异最小的一组比例，此时每个比例对应的目标特征边与标准特征边为一对匹配边，所有匹配边组成匹配结果。
34.在n个方向参数和n个标准方向参数中仅存在一种匹配结果，即仅存在一组相似比例，在这组相似比例中，点云数据的目标特征边和cad三维图的标准特征边的比例基本相同。
35.通过上述匹配过程获取待抓取零件的点云数据目标特征边和cad三维图的标准特征边的匹配成功的匹配边。计算匹配边中两个特征边之间的余弦相似度获取两个特征边之间的角度作为点云数据的旋转角度；计算匹配边中两个特征边之间的欧式距离作为点云数据的平移距离。
36.基于相互匹配的目标特征边和标准特征边的差异获取待抓取零件的旋转角度和平移距离；基于旋转角度和平移距离对点云数据进行旋转和平移，成为与cad三维图方向相同的更新点云数据。
37.步骤s003，基于更新点云数据中每两个点之间的距离将更新点云数据分组，并在每个组内获取每个点的局部离群因子；对于相邻的两个组，分别计算组中每个点到两个组中心的距离并求差值，当差值小于预设的距离阈值时，对应的点为异常点。
38.具体的步骤包括：为了更好地拟合点云平面，先根据聚类算法，将更新点云数据分为多个组。在本发明实施例中的聚类算法采用k-means聚类算法，其中聚类的组数k根据待抓取零件的面的数量确定，即待抓取零件共有k个面，聚类时预设的组数即为k，聚类时以更新点云数据中每两个点之间的欧式距离作为聚类距离，最终得到k个组。
39.然后计算每个组中每个点的局部离群因子，局部离群因子代表了每个点在所在的组中的离散程度，局部离群因子越大，说明该点越有可能不属于当前所在的组，即有可能属
于另一个组。局部离群因子通过lof算法计算得到，lof算法为现有技术，本实施例中不再描述具体的计算方法，在本发明实施例中lof算法中的第k邻域的k值为5。
40.由于每个组中可能存在部分点属于另一个组，即该点对于当前所属组确定的拟合平面是异常的数据。因此对于两个相邻组中的点，计算每个点至两个组中心之间的欧式距离：记第个组中第个点与第个组的中心点的欧式距离为，第个点与第个组的中心点的欧式距离为。则计算两个欧式距离的差值，设置距离阈值，若两个欧式距离之间的差值小于该距离阈值，则表明第个组中第个点为异常点。类似此操作，可以得到所有组中的异常点。
41.异常点离两个组的中心的距离比较接近，有可能并不属于当前的所属组，而是属于所属组的相邻组。
42.步骤s004，将异常点对应的相邻的两个组记为异常组，去除异常组中的异常点，根据剩余点之间的距离获取该异常组的映射参考值；在该异常组中任意添加一个异常点，再获取对应的映射参考值作为映射值，对于每个异常点，计算对应的两个异常组的映射值和映射参考值的差值作为映射差异，进而获取该异常点的分布特征值；剩余点的分布特征值为预设值。
43.具体的步骤包括：由于异常点可能属于对应的两个相邻组中的任意一个组，因此将异常点对应的两个相邻组均记为异常组，通过计算异常组的映射差异来获取异常点的分布特征值来表征异常点的分布情况。
44.首先去除异常组中的所有异常点，对于异常组中的每个剩余点，获取该剩余点在该异常组中与其他剩余点之间的最短距离，所有最短距离的平均值即为映射参考值。
45.通过计算每个剩余点与其他剩余点之间的距离获取其中的最短距离，即每个剩余点均对应一个最短距离，再计算所有剩余点对应的最短距离的平均值作为对应异常组的映射参考值。
46.然后在该异常组中任意添加一个异常点，再获取对应的映射参考值作为映射值。即添加任意一个异常点，按照计算映射参考值同样的方法，计算所有剩余点和添加的异常点对应的最短距离的平均值作为添加该异常点时的映射值，则每个异常点在每个异常组中均对应一个映射值。
47.对于每个异常点，对应了两个异常组，且在这两个异常组中均存在相应的映射值，分别计算这两个组中的映射值和映射参考值的差值作为映射差异。将异常点所在的异常组的映射差异记为，将异常点对应的相邻的异常组的映射差异记为，当时，该异常点更有可能属于当前所在的异常组，此时的分布特征值为预设值；当时，该异常点更有可能属于对应的相邻的异常组，此时的分布特征值为0.01；当时，该异常点属于两个异常组的概率相同，此时的分布特征值为0.5；预设值为1。
48.所有剩余点均为正常点，即均属于当前所在的组，因此所有剩余点的分布特征值均为预设值1。分布特征值越大，对应的点越有可能属于当前所在的组。
49.步骤s005，根据局部离群因子和分布特征值获取每个点的偏移概率；基于偏移概率结合ransac算法获取每个组对应的点云平面；以优选度最高的点云平面作为抓取平面，发送抓取指令以使机械手抓取待抓取零件。
50.具体的步骤包括：计算每个点对应的局部离群因子和分布特征值的比值，选取最大比值和最小比值，以每个比值与最小比值的差作为分子，以最大比值和最小比值的差作为分母，得到的分子和分母的比值为偏移概率。
51.局部离群因子越大，对应的点越偏离当前所在的组；分布特征值越大，越可能属于当前所在的组，因此基于每个点对应的局部离群因子和分布特征值获取对应的偏移概率。以第l组中第w个点为例，计算该点的偏移概率：其中，表示第l组中第w个点的局部离群因子和分布特征值的比值，表示第l组中第w个点的局部离群因子，表示第l组中第w个点的分布特征值；表示最大比值，表示最小比值。
52.在每个组中选取预设数量的点拟合为一个初始平面，利用预设的距离拟合阈值和偏移概率获取每个点的自适应距离阈值，当每个点与初始平面之间的距离小于对应的自适应距离阈值时，该点为初始平面的内点，初始平面以及对应的所有内点组成对应组的点云平面。
53.传统的ransac算法的具体过程为：随机采样预设数量的点拟合模型，然后计算其他点到拟合模型的距离，当该距离小于一定阈值时，当作内点，并统计内点数量，重复多次，选择内点数最多的模型，再利用所有内点重新估计模型。其中的阈值为设定的固定值，对所有点来说均相同，本发明实施例结合偏移概率获取每个点的自适应距离阈值进行ransac算法的模型估计。
54.在本发明实施例中预设数量为20，首先在每个组中选取20个点拟合为一个初始平面，然后预设距离拟合阈值t=5，在ransac拟合过程中，对于偏移概率大的点，对距离拟合阈值t进行校正，其中对于第个组中第个点校正后的自适应距离阈值，根据所有点的自适应距离阈值进行ransac算法计算。当每个点与初始平面之间的距离小于对应的自适应距离阈值时，该点为初始平面的内点，初始平面以及对应的所有内点组成对应组的点云平面。每组均对应一个点云平面。
55.在机械手抓取过程中，移动旋转的次数较小，并且在抓取过程中对应的抓取面较
大，能够实现更好的抓取。因此基于机械手到达点云平面的调整次数、行进距离和点云平面的面积筛选最优平面作为抓取平面。
56.通过模拟器进行模拟获取机械手到达点云平面的调整次数；基于点云平面的质心和机械手的质心之间的连线获取机械手的行进距离；计算调整次数和行进距离的乘积，获取点云平面的面积，以面积和乘积的相反数的比值作为优选度。
57.为了精准调节机器人的机械手的旋转角度，以机器人的机械手的中心为坐标原点，以激光雷达的扫描方向为x轴方向，以传送带的传送方向为y轴方向，以机械手正上方竖直方向为z轴方向构建三维坐标系。
58.计算点云平面的质心以及机械手的质心的连线，将该连线分别投影至x轴、y轴和z轴坐标轴上，三个轴投影后的距离即为对应方向的行进距离、和。
59.通过每个点云平面与轴和轴构成的平面的夹角获取对应点云平面的法向量的方向的角度，由于机械手的起始位置在零件的正上方，因此通过每个点云平面的法向量的方向的角度以及机械手的行进方向即可确定自由度位姿调整的次数。具体的：通过模拟器根据当前机械手的行进方向与每个点云平面法向量的角度之间获取自由度位姿调整的次数，即对于机械手的不同的行进方向，以及不同点云平面的不同角度，将机械手所在位置、机械手行进方向、不同点云平面的不同法向量角度，输入至模拟器中进行训练模拟，从而输出不同点云平面的法向量的角度下，对应的机械手调整位姿的调整次数。
60.点云平面的大小根据凸包检测获取，其凸包的大小对应抓取该点云平面时机械手的张开大小。
61.然后计算每个点云平面抓取的优选度，以第个点云平面为例，计算优选度，其中，表示第个点云平面的面积；表示第个点云平面的行进距离的总和，即；表示机械手对于第个点云平面的调整次数。
62.机械手抓取过程中，对应的行进距离的总和越小，机械手位姿的调整次数越少，点云平面的面积越大，越容易抓取，对应的该平面的优选度就越大。
63.以优选度最高的点云平面作为抓取平面，机器人端发送抓取指令以使机械手执行抓取行为，从抓取平面抓取待抓取零件。需要说明的是，每次抓取行为完成后，机械手会初始化进行复位。
64.综上所述，本发明实施例接收待抓取零件的点云数据和对应标准零件的cad三维图，在cad三维图中获取标准零件的标准特征边，在点云数据中获取待抓取零件的目标特征边；特征边为对应零件的边缘；根据目标特征边与标准特征边的匹配结果移动点云数据得到更新点云数据，更新点云数据与cad三维图方向相同；基于更新点云数据中每两个点之间的距离将更新点云数据分组，并在每个组内获取每个点的局部离群因子；对于相邻的两个组，分别计算组中每个点到两个组中心的距离并求差值，当差值小于预设的距离阈值时，对应的点为异常点；将异常点对应的相邻的两个组记为异常组，去除异常组中的异常点，根据剩余点之间的距离获取该异常组的映射参考值；在该异常组中任意添加一个异常点，再获
取对应的映射参考值作为映射值，对于每个异常点，计算对应的两个异常组的映射值和映射参考值的差值作为映射差异，进而获取该异常点的分布特征值；剩余点的分布特征值为预设值；根据局部离群因子和分布特征值获取每个点的偏移概率；基于偏移概率结合ransac算法获取每个组对应的点云平面；以优选度最高的点云平面作为抓取平面，发送抓取指令以使机械手抓取待抓取零件。本发明实施例能够提高机械手的抓取精准度和实时性。
65.需要说明的是：上述本发明实施例先后顺序仅仅为了描述，不代表实施例的优劣。且上述对本说明书特定实施例进行了描述。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
66.本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同或相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。
67.以上所述实施例仅用以说明本技术的技术方案，而非对其限制；对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的范围，均应包含在本技术的保护范围之内。