CN112223294A - 一种基于三维视觉的机械臂加工轨迹修正方法 - Google Patents

Abstract

本发明公开了一种基于三维视觉的机械臂加工轨迹修正方法，其包括以下步骤：S1.通过示教器编写两个示教程序，一个用于传感器扫描获取数据，另一个用于引导机器人沿着加工轨迹进行加工工作；S2.启动扫描程序，若为第一次扫描，则直接执行加工示教程序，若非第一次扫描获取点云数据并与模板点云数据进行配准，计算出两个点云数据集的平移向量和旋转矩阵；S3.利用S2计算的平移向量和旋转矩阵对示教文件中的示教点位进行修正生成新的示教文件，并传输至机器人控制器；S4.机器人执行修正后的示教文件，完成加工。

Description

一种基于三维视觉的机械臂加工轨迹修正方法

技术领域

本发明属于机械臂的工业应用领域，尤其涉及一种基于三维视觉的机械臂加工轨迹修正方法。

背景技术

随着科技进步和传感器精度的飞速发展，机器人的视觉感知能力已经成为衡量机器人智能化程度的重要指标。三维视觉的不断进步使机器人的视觉感知能力越来越强，它具有非接触、高精度、高效率等优点。传统的工业机器人在进行重复的工作前，需要操作人员对机器人运动的路径和轨迹进行点对点的编程，该方法较为直观，但耗时耗力。此外，如果工件位置发生变化，需要对示教点进行重新校准，即使此位置变化较为微小不易被察觉(例如2～3mm)，该偏差仍会直接影响最终的产品加工质量，造成经济上的损失。故本文提出工业机器人结合三维视觉的方法，使用激光扫描传感器的手眼系统采集工件点云数据，通过点云配准算法计算刚体工件前后位姿变换的齐次变换矩阵，利用刚体的坐标变换修正示教点。

发明内容

发明目的：为解决现有技术中存在的问题，本发明提出了一种基于3D视觉的机械臂加工轨迹修正方法，本发明可以提高机械臂的自动化程度，减少生产中的残次品的出现概率，减少因为工件位置微笑的变化而造成的经济损失。

技术方案：为了实现上述的目的，本发明提出了一种基于三维视觉的机械臂加工轨迹修正方法，该方法包括以下步骤：

(a)通过示教器点对点编写机器人工件加工的示教程序teach和扫描的示教程序scan并保存(示教程序名称可以根据用户需要自行修改)，示教程程序teach和scan由操作人员编写，teach示教文件中保存了加工轨迹的示教点位，操作人员根据待加工工件的加工轨迹编写示教程序；扫描示教程序scan需要对在传感器测量范围内对工件进行扫描以获取点云数据；示教程序teach的点位为加工轨迹，扫描示教程序scan用于扫描加工轨迹获取工件的点云数据，点云数据是计算机视觉领域常见的一种数据形式，可以通过三维视觉传感器采集获取，根据传感器的不同每一个点包含有三维坐标，有些可能含有颜色信息或反射强度信息；

(b)启动扫描程序scan，三维传感器开始采集点云数据，等待扫描结束，对点云数据进行坐标转换，再对点云数据进行预处理，上位机保存预处理后的扫描数据到指定文件夹；若此次扫描为第一次扫描，则把该数据作为点云配准的模板文件，机械臂直接执行teach示教文件开始加工工件，机械臂根据teach中的示教点位控制机械臂沿着加工轨迹进行运动，执行结束后完成一次加工；

若本次扫描不是第一次扫描，由于人工放置工件无法做到和第一次加工时的位置完全一致，尤其一些大型工件，此时工件的位置会和第一次加工的位置不同，会与第一次产生了旋转和平移的偏差，执行扫描示教程序则并保存数据后执行点云配准算法，将模板点云向本次扫描的点云做配准运算，计算得到工件在机器人坐标系下的齐次变换矩阵T_reg,齐次变换矩阵是描述坐标系平移变换和旋转变换的矩阵，它的维度位4×4，由一个3×3的旋转矩阵R和一个3×1的平移矩阵T组成，用T_4×4表示齐次变换矩阵，可以用下式表示：

(c)通过文件传输协议取回teach示教文件，解析示教文件并读取示教点的位姿信息，每个示教点位姿信息包括偏移量X,Y,Z和ZYX欧拉角α、β、γ，其中α、β、γ分别对应着机器人末端执行器在机器人坐标系下绕Z轴、Y轴、X轴的旋转角度，偏移量X,Y,Z配合ZYX欧拉角α,β,γ数据可以和齐次变换矩阵相互转化，设转换后的齐次变换矩阵为T_before，则T_before可以由下式计算：

对于示教文件中的每个示教点位，根据齐次变换矩阵T_reg重新计算得到新示教点位的齐次变换矩阵T_new，并转换成偏移量X,Y,Z和ZYX欧拉角α,β,γ，将重新计算得到的示教点称为修正示教点，T_new的计算可以用下式进行计算：

T_new＝T_reg*T_before

T_new矩阵展开写出如下式所示：

将齐次变换矩阵T_new转换为偏移量X,Y,Z和ZYX欧拉角α,β,γ通过下述方法进行转换，其中偏移量X,Y,Z可以通过齐次变换矩阵直接读取出，ZYX欧拉角α,β,γ可以通过下式计算得到：

α＝arctan 2(r₂₁/r₁₁)

γ＝arctan 2(r₃₂/r₃₃)

(d)根据teach文件的格式，将修正示教点写入新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器；

(e)机器人控制器执行teach_plus示教文件，并开始对工件进行加工。

优选的，所述步骤a中需要人工编写两个示教程序teach和scan，示教程序名称可以根据需要修改，机械臂每次完整的运行首先执行scan示教程序对加工轨迹进行扫描，若第一次加工，即不存在模板文件，则执行teach示教文件，teach示教文件会引导机器人进行第一次的加工，若模板文件存在，即本次加工并非第一次加工，本次加工的工件位置已经发生变化，需要对加工轨迹的示教点进行重新计算修正，根据点云配准算法重新计算新的示教点位，并根据teach示教文件的内容，写入修正示教点到teach_plus文件，执行teach_plus示教文件，整个加工过程中都是扫描示教文件和加工示教文件交替进行。

优选的，所述步骤b中，点云采集完成后对点云数据进行坐标变换，三维传感器获取的原始点云数据为传感器坐标系下的，无法直接提供给机械臂使用，故应对点云数据中的每个点云数据进行坐标变换，变换关系可以通过下式(1)进行计算，其中，P_b表示点云数据在机器人基坐标下的数据，

表示机器人基坐标系到机械臂末端的工具坐标系的齐次变换矩阵，

表示机械臂末端的工具坐标系到传感器坐标系的齐次变换矩阵，该矩阵可以通过传感器和机械臂手眼标定进行获取，P_s表示原始的点云数据；

优选的，所述步骤b中对坐标变换后的点云数据进行预处理，预处理的方法包括点云滤波和点云降采样，滤除原始点云数据中的离群点及异常点，常用的点云滤波算法有统计滤波、直通滤波等，可以根据噪声的特点选择合适的滤波方法；点云降采样可以在保留点云原始形状的同时减少点云的数据量，这可以有效的减少后续的配准处理运算量。

优选的，所述步骤b中，若扫描次数不是第一次扫描，即已经存在模板文件，则执行点云配准，点云配准是找到对齐两个点云空间变换(例如缩放、旋转和平移)的过程。其目的是将多个数据集合并成全局一致的模型(或坐标系)，并将新的测量值映射到已知的数据集以识别特征或估计其姿态。点云配准问题可以被描述为：存在两个点云集，即源点云集P_s和目标点云集P_t，在本发明中，

源点云是指第一次加工时被存为模板点云的点云数据，目标点云是第一次加工后的每次加工中传感器采集的点云数据，这两个点云存在于不同的坐标系中，点云配准的过程是需要找到一个旋转矩阵R和平移矩阵T，将源点云P_s和目标点云P_t转换到同一坐标系下。假设源点云P_s和目标点云P_t中存在一对对应点，在本发明中，所述的一一对应点为工件点云数据的关键点，即点云数据的角点或曲率变化较大的点，对于两个点云数据中的一对对应点p_t和P_s，则存在下式(2)所示的转换关系，其中，p_t是目标点云中的一个点云，p_s为源点云中的一个点云：

p_t＝R*p_s+T (2)

对于整个点集而言，点云配准就是让两个点云集中的对应点的欧氏距离之和最小，该过程可以表示为求下式(4)目标函数的最优解：

其中，

和

分别为目标点云和源点云中两个点集中的对应点，n代表两个点云集中对应点的数目，故此处的

x,y,z是指点云数据中每个点在机器人坐标系下的坐标值；本发明中点云数据的空间位置变化(旋转和平移)和实际工件的变化是保持一致的，在本发明中点云配准的主要作用是求取某次加工扫描的点云坐标系和模板点云坐标系的齐次变换矩阵，故该齐次变换矩阵实际上反映着工件的位置在机器人基座标系下的变化。

优选的，所述步骤d中，根据teach文件的格式，结合修正后的示教点位，生成新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器，teach_plus示教文件除了示教点信息和teach示教文件不同，其他均相同，teach_plus中的每个示教点位都经过(4)式进行了重新修正，机器人控制器执行teach_plus对工件进行加工，加工轨迹得到了准确的修正。

本发明的益处是：与现有技术相比，本发明的技术方案具有以下有益技术效果：

本发明的技术方案减少了机械臂的示教量，提高了工业加工的自动化程度，显著提高了生产成品的质量。传统的机械臂在实际生产的过程中，需要操作人员对准示教点位，若工件的位置一旦发生变化，就需要重新编写示教程序，应用本发明可以有效地解决上述问题，仅需编写一次示教程序，每次工件加工的过程中，自动修正示教轨迹；此外，针对实际加工过程中的工件发生了微小的变化，例如仅2～3mm的平移，该变化不易被察觉，若不对轨迹进行修正，可能会造成加工失败，进而造成经济上的损失，本发明对加工轨迹进行修正后，可以提高生产质量，减少经济损失；该发明的应用可以允许工件在一定范围内进行旋转和平移变化，这有效的提高了生产过程的灵活性。

附图说明

图1是本发明实施例轨迹修正的直线角焊缝示意图；

图2是本发明实施例轨迹修正的流程图；

图3是本发明实施例系统结构图；

图4是本发明实施例点云数据预处理后的点云数据可视化图；

图5是本发明实施例中前后两次点云数据的差异示例数据；

图6是本发明实施例中简化表示点云配准前后的关系图。

图7是本发明实施例中的工件配准前后的对比图；

图8是本发明实施例中的示教轨迹修正前后的对比图。

具体实施方式

下面结合说明书附图对本发明的实施例进行详细说明，本实施例以直线角焊缝的工业应用场景作为实施例，所用的传感器为激光轮廓传感器，系统的组成示意图如图3所示，但该场景仅作为具体实施方式的说明，不是本发明的限制，该方法用于其他工业生产场景也在本发明的保护范围之内。

实施例：

本实施例以直线角焊缝待焊工件进行说明，其示意图如图1所示，图中标记的红线AB处为焊缝：

如图2所示，一种基于三维视觉的机械臂加工轨迹修正方法，该方法包括以下步骤：

(a)通过示教器点对点编写机器人工件加工的示教程序teach和扫描的示教程序scan并保存(示教程序名称可以根据用户需要自行修改)，示教程程序teach和scan由操作人员编写，teach示教文件中保存了加工轨迹的示教点位，操作人员根据待加工工件的加工轨迹编写示教程序；扫描示教程序scan需要对在传感器测量范围内对工件进行扫描以获取点云数据；示教程序teach的点位为加工轨迹，扫描示教程序scan用于扫描加工轨迹获取工件的点云数据，点云数据是计算机视觉领域常见的一种数据形式，可以通过三维视觉传感器采集获取，根据传感器的不同每一个点包含有三维坐标，有些可能含有颜色信息或反射强度信息；

(b)启动扫描程序scan，三维传感器开始采集点云数据，等待扫描结束，对点云数据进行坐标转换，再对点云数据进行预处理，上位机保存预处理后的扫描数据到指定文件夹；若此次扫描为第一次扫描，则把该数据作为点云配准的模板文件，机械臂直接执行teach示教文件开始加工工件，机械臂根据teach中的示教点位控制机械臂沿着加工轨迹进行运动，执行结束后完成一次加工；

若本次扫描不是第一次扫描，由于人工放置工件无法做到和第一次加工时的位置完全一致，尤其一些大型工件，此时工件的位置会和第一次加工的位置不同，会与第一次产生了旋转和平移的偏差，执行扫描示教程序则并保存数据后执行点云配准算法，将模板点云向本次扫描的点云做配准运算，计算得到工件在机器人坐标系下的齐次变换矩阵T_reg,齐次变换矩阵是描述坐标系平移变换和旋转变换的矩阵，它的维度位4×4，由一个3×3的旋转矩阵R和一个3×1的平移矩阵T组成，用T_4×4表示齐次变换矩阵，可以用下式表示：

(c)通过文件传输协议取回teach示教文件，解析示教文件并读取示教点的位姿信息，每个示教点位姿信息包括偏移量X,Y,Z和ZYX欧拉角α、β、γ，其中α、β、γ分别对应着机器人末端执行器在机器人坐标系下绕Z轴、Y轴、X轴的旋转角度，偏移量X,Y,Z配合ZYX欧拉角α,β,γ数据可以和齐次变换矩阵相互转化，设转换后的齐次变换矩阵为T_before，则T_before可以由下式计算：

对于示教文件中的每个示教点位，根据齐次变换矩阵T_reg重新计算得到新示教点位的齐次变换矩阵T_new，并转换成偏移量X,Y,Z和ZYX欧拉角α,β,γ，将重新计算得到的示教点称为修正示教点，T_new的计算可以用下式进行计算：

T_new＝T_reg*T_before

T_new矩阵展开写出如下式所示：

将齐次变换矩阵T_new转换为偏移量X,Y,Z和ZYX欧拉角α,β,γ通过下述方法进行转换，其中偏移量X,Y,Z可以通过齐次变换矩阵直接读取出，ZYX欧拉角α,β,γ可以通过下式计算得到：

α＝arctan 2(r₂₁/r₁₁)

γ＝arctan 2(r₃₂/r₃₃)

(d)根据teach文件的格式，将修正示教点写入新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器；

(e)机器人控制器执行teach_plus示教文件，并开始对工件进行加工。

优选的，所述步骤a中需要人工编写两个示教程序teach和scan，示教程序名称可以根据需要修改，机械臂每次完整的运行首先执行scan示教程序对加工轨迹进行扫描，若第一次加工，即不存在模板文件，则执行teach示教文件，teach示教文件会引导机器人进行第一次的加工，若模板文件存在，即本次加工并非第一次加工，本次加工的工件位置已经发生变化，需要对加工轨迹的示教点进行重新计算修正，根据点云配准算法重新计算新的示教点位，并根据teach示教文件的内容，写入修正示教点到teach_plus文件，执行teach_plus示教文件，整个加工过程中都是扫描示教文件和加工示教文件交替进行。

优选的，所述步骤b中，点云采集完成后对点云数据进行坐标变换，三维传感器获取的原始点云数据为传感器坐标系下的，无法直接提供给机械臂使用，故应对点云数据中的每个点云数据进行坐标变换，变换关系可以通过下式(1)进行计算，其中，P_b表示点云数据在机器人基坐标下的数据，

表示机器人基坐标系到机械臂末端的工具坐标系的齐次变换矩阵，

表示机械臂末端的工具坐标系到传感器坐标系的齐次变换矩阵，该矩阵可以通过传感器和机械臂手眼标定进行获取，P_s表示原始的点云数据；

优选的，所述步骤b中对坐标变换后的点云数据进行预处理，预处理的方法包括点云滤波和点云降采样，滤除原始点云数据中的离群点及异常点，常用的点云滤波算法有统计滤波、直通滤波等，可以根据噪声的特点选择合适的滤波方法；点云降采样可以在保留点云原始形状的同时减少点云的数据量，这可以有效的减少后续的配准处理运算量。

优选的，所述步骤b中，若扫描次数不是第一次扫描，即已经存在模板文件，则执行点云配准，点云配准是找到对齐两个点云空间变换(例如缩放、旋转和平移)的过程。其目的是将多个数据集合并成全局一致的模型(或坐标系)，并将新的测量值映射到已知的数据集以识别特征或估计其姿态。点云配准问题可以被描述为：存在两个点云集，即源点云集P_s和目标点云集P_t，在本发明中，源点云是指第一次加工时被存为模板点云的点云数据，目标点云是第一次加工后的每次加工中传感器采集的点云数据，这两个点云存在于不同的坐标系中，点云配准的过程是需要找到一个旋转矩阵R和平移矩阵T，将源点云P_s和目标点云P_t转换到同一坐标系下。假设源点云P_s和目标点云P_t中存在一对对应点，在本发明中，所述的一一对应点为工件点云数据的关键点，即点云数据的角点或曲率变化较大的点，对于两个点云数据中的一对对应点p_t和P_s，则存在下式(2)所示的转换关系，其中，p_t是目标点云中的一个点云，p_s为源点云中的一个点云：

p_t＝R*p_s+T (2)

对于整个点集而言，点云配准就是让两个点云集中的对应点的欧氏距离之和最小，该过程可以表示为求下式(4)目标函数的最优解：

其中，

和

分别为目标点云和源点云中两个点集中的对应点，n代表两个点云集中对应点的数目，本发明中点云数据的空间位置变化(旋转和平移)和实际工件的变化是保持一致的，在本发明中点云配准的主要作用是求取某次加工扫描的点云坐标系和模板点云坐标系的齐次变换矩阵，故该齐次变换矩阵实际上反映着工件的位置在机器人基座标系下的变化。

优选的，所述步骤d中，根据teach文件的格式，结合修正后的示教点位，生成新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器，teach_plus示教文件除了示教点信息和teach示教文件不同，其他均相同，teach_plus中的每个示教点位都经过(4)式进行了重新修正，机器人控制器执行teach_plus对工件进行加工，加工轨迹得到了准确的修正。用一个矩形简化表示工件，则它们的关系如图6所示。

所述的文件传输协议为ftp协议，其中机器人控制器为ftp服务器，示教文件的上传和下载均以控制器为服务器，上位机为客户端。

所述方法在点云配准后，即可对齐两片点云，包括加工轨迹，对齐后的可视化图如图7所示，可以看出在旋转平移产生时，本发明提出的方法可以有效地进行轨迹修正，实验多次轨迹修正的可视化图如图8所示。实际生产中本发明提出的方法可以在工件移动范围在-20mm到20mm的区间内，对加工轨迹进行有效地修正。

以上所述的实施例仅表达了本发明的具体实施方式，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，以及应用于其他工业生产场景的轨迹修正，这些都属于本发明的保护范围。

Claims (5)

1.一种基于三维视觉的机械臂加工轨迹修正方法，其特征在于，该方法包括以下步骤：

(a)预设机器人工件加工的teach示教文件和扫描示教程序scan并保存，teach示教文件中保存了加工轨迹的示教点位，扫描示教程序scan需要对在传感器测量范围内对工件进行扫描以获取点云数据；

(b)启动扫描程序scan，三维传感器开始采集点云数据，等待扫描结束，对点云数据进行坐标转换，再对点云数据进行预处理，上位机保存预处理后的扫描数据到指定文件夹；若此次扫描为第一次扫描，则把该数据作为点云配准的模板文件，机械臂直接执行teach示教文件开始加工工件，机械臂根据teach中的示教点位控制机械臂沿着加工轨迹进行运动，执行结束后完成一次加工；

若本次扫描不是第一次扫描，执行扫描示教程序并保存数据后执行点云配准算法，将模板点云数据向本次扫描的点云数据做配准运算，计算得到工件在机器人坐标系下的齐次变换矩阵T_reg，用下式表示：

R为旋转矩阵，T为平移矩阵；

(c)通过文件传输协议取回teach示教文件，解析示教文件并读取示教点的位姿信息，每个示教点位姿信息包括偏移量X，Y，Z和ZYX欧拉角α、β、γ，其中α、β、γ分别对应着机器人末端执行器在机器人坐标系下绕Z轴、Y轴、X轴的旋转角度，偏移量X，Y，Z配合ZYX欧拉角α，β，γ数据可以和齐次变换矩阵相互转化，设转换后的齐次变换矩阵为P_before，则T_before可以由下式计算：

对于示教文件中的每个示教点位，根据齐次变换矩阵T_reg重新计算得到新示教点位的齐次变换矩阵T_new，并转换成偏移量X，Y，Z和ZYX欧拉角α，β，γ，将重新计算得到的示教点称为修正示教点，T_new的计算可以用下式进行计算：

T_new＝T_reg*T_before

T_new矩阵展开写出如下式所示：

将齐次变换矩阵T_new转换为偏移量X，Y，Z和ZYX欧拉角α，β，γ通过下述方法进行转换，其中偏移量X，Y，Z可以通过齐次变换矩阵直接读取出，ZYX欧拉角α，β，γ可以通过下式计算得到：

α＝arctan 2(r₂₁/r₀₁)

γ＝arctan 2(r₃₂/r₃₃)

(d)根据teach文件的格式，将修正示教点写入新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器；

(e)机器人控制器执行teach_plus示教文件，并开始对工件进行加工。

2.根据权利要求1所述的一种基于三维视觉的机械臂加工轨迹修正方法，其特征在于，所述步骤b中，点云采集完成后对点云数据进行坐标变换，三维传感器获取的原始点云数据为传感器坐标系下的，无法直接提供给机械臂使用，故应对点云数据中的每个点云数据进行坐标变换，变换关系可以通过下式(1)进行计算，其中，P_b表示点云数据在机器人基坐标下的数据，

表示机器人基坐标系到机械臂末端的工具坐标系的齐次变换矩阵，

表示机械臂末端的工具坐标系到传感器坐标系的齐次变换矩阵，该矩阵可以通过传感器和机械臂手眼标定进行获取，P_s表示原始的点云数据；

3.根据权利要求1所述的一种基于三维视觉的机械臂加工轨迹修正方法，其特征在于，所述步骤b中对坐标变换后的点云数据进行预处理，预处理的方法包括点云滤波和点云降采样，滤除原始点云数据中的离群点及异常点。

4.根据权利要求1所述的一种基于三维视觉的机械臂加工轨迹修正方法，其特征在于，所述步骤b中，若扫描次数不是第一次扫描，即已经存在模板文件，则执行点云配准，点云配准问题被描述为：存在两个点云集，即源点云集P_s和目标点云集P_t，源点云是指第一次加工时被存为模板点云的点云数据，目标点云是第一次加工后的每次加工中传感器采集的点云数据，点云配准的过程是需要找到一个旋转矩阵R和平移矩阵T，将源点云P_s和目标点云P_t转换到同一坐标系下，假设源点云P_s和目标点云P_t中存在一对对应点，所述的一一对应点为工件点云数据的预设关键点，即点云数据的角点或曲率变化大于阈值的点，对于两个点云数据中的一对对应点p_t和P_s，则存在下式(2)所示的转换关系，其中，p_t是目标点云中的一个点云，p_s为源点云中的一个点云：

p_t＝R*p_s+T (2)

对于整个点集，点云配准就是让两个点云集中的对应点的欧氏距离之和最小，该过程可以表示为求下式(4)目标函数的最优解：

其中，

和

分别为目标点云和源点云中两个点集中的对应点，点云中的每个点可以用一个向量表示，故此处的

x，y，z是指点云数据中每个点在机器人坐标系下的坐标值，n代表两个点云集中对应点的数目。

5.根据权利要求1所述的一种基于三维视觉的机械臂加工轨迹修正方法，其特征在于，所述步骤d中，根据teach文件的格式，结合修正后的示教点位，生成新的示教文件teach_plus，并通过文件传输协议传输到机器人控制器，teach_plus示教文件除了示教点信息和teach示教文件不同，其它均相同，teach_plus中的每个示教点位都经过(4)式进行了重新修正，机器人控制器执行teach_plus对工件进行加工，加工轨迹得到了准确的修正。

Images