CN111347423B - 工业机器人传送带动态跟踪涂胶方法 - Google Patents

Abstract

本发明提出了一种工业机器人传送带动态跟踪涂胶方法，该方法可以使工业机器人在传送带上的涂胶物体到达操作范围后，定位到物体并且进行跟踪。在机器人与物体相对静止后实现对物体轮廓的涂胶操作。整个过程传送带无需停止。具体的方案设计包括以下几个阶段：物体刚开始进入机器人操作范围，机器人得到物体位置后定位物体，此时机器人是点到点运动；机器人定位到物体之后开始跟踪物体，直到与物体相对静止；机器人保持与物体相对静止同时实现对物体轮廓的涂胶操作，此时机器人是连续轨迹运动。本发明能够实现在传送带不停止的情况下跟踪物体涂胶，避免了传送带因频繁启停的损耗，提高了生产效率，节约了成本。

Description

工业机器人传送带动态跟踪涂胶方法

技术领域

本发明属于工业机器人控制技术领域，具体涉及一种工业机器人传送带动态跟踪涂胶方法。

背景技术

工业机器人广泛应用于工业自动化生产中。为了提高工作效率,降低生产成本,避免机器频繁启停的损耗,传送带需要时刻保持移动且工业机器人必须随着传送带一起运动，同时还得进行分拣、焊接、涂胶等工作。工业机器人需根据视觉系统以及编码器获得传送带上物体的位置和速度，以便对目标物体进行位置的实时同步和跟踪,最终完成分拣、焊接、涂胶等工作。

现在大部分的工业涂胶工作是依靠人工操作，人力成本大大增加，工作效率也得不到提高。因此使用机器人替代人力工作显得极其重要。目前使用机器人进行涂胶作业的实现方式比较简单，当传送带上的物体移动到指定工位时，传送带停止运行，机器人定位到物体后实现静态涂胶，然后传送带继续移动。这种方式虽然简单实用，但存在增加传送带损耗、工作效率低下、机器人等待时间过长等缺点。还有方法提出机器视觉辅助机器人进行实时跟踪与操作，相机每个周期循环拍摄物体位置与轮廓传送给机器人。这种方法对相机的硬件要求非常高，对采集图像时的光照要求严苛，最后的图像处理算法极其复杂，在实用性方面还有待考证。

发明内容

本发明的目的在于克服现有技术的不足，提出一种工业机器人传送带动态跟踪涂胶方法。该方法可以使工业机器人在传送带上的涂胶物体到达操作范围后，定位到物体并且进行跟踪。在机器人与物体相对静止后实现对物体轮廓的涂胶操作。整个过程传送带无需停止

本发明是通过以下技术方案实现的：

一种工业机器人传送带动态跟踪涂胶方法，包括以下步骤：

步骤一：开始跟踪涂胶工作之前，进行传送带标定和相机标定，将相机位置和传送带坐标系转换至机器人坐标系，实现对传送带上的目标物体的坐标位置进行转换，转换后的所有位置都基于机器人坐标系；

步骤二：当目标物体进入机器人操作范围时，此时相机拍摄到物体的位置P，编码器读取到传送带速度V，将其发送给机器人；经过坐标转换后，机器人得到其自身坐标系下的目标物体位置Po₁；

步骤三：机器人从等待位置Po₀移动到开始跟踪位置Po₂，此过程运用机器人逆运动学，将Po₀和Po₂的坐标逆解到每一个关节，在关节坐标系下运用五次多项式曲线进行轨迹规划，实现点到点的快速移动；

步骤四：当机器人已经移动到开始跟踪位置Po₂，此时可对目标物体进行跟踪，跟踪过程需要不断缩小机器人与目标物体的距离，使得机器人与目标物体处于相对静止状态，本步骤采用PID形式跟踪算法，设任意t时刻，机器人的位置为P_t0，目标物体的位置为P_tf，则两者的误差为

ε(t)＝P_tf-P_t0

则调整参量为

其中K_p是比例系数，T_i为积分时间常数，T_d为微分时间常数，则任意时刻t发送给机器人的位置为

P_t＝P_t0+δ(t)

步骤五：当机器人跟踪上目标物体后，两者位置偏差足够小能达到相对静止状态时，开始涂胶任务；此过程，根据相机发送的目标物体平面轮廓，进行机器人工作空间的连续运动轨迹规划，同时要保持与目标物体的同步速度，根据传送带速度V，物体轮廓的插补点坐标离散序列为p₁(x_p1+Δv，y_p1，z_p1)，…，p_i(x_pi+Δv，y_pi，z_pi)，…，p_n(x_pn+Δv，y_pn，z_pn)，其中Δv为每个插补周期速度增量值；采用上述插补点离散点序列对轨迹进行拟合；

步骤六：当完成涂胶任务后，机器人运动到目标物体上方，快速离开至等待位置，准备下一个物体的涂胶工作。

在上述技术方案中，在步骤1中，传送带标定方法为：需要确定转换矩阵及比例因子，设传送带运动方向为传送带坐标系的X轴方向，机器人坐标系一点P^R与传送带坐标系一点P^C转换方程为

为求得转换矩阵

首先在传送带上确定一点P₁，移动机器人末端触碰该点；接着移动传送带，使机器人末端再次触碰到点P₁，此时设为P₂；最后让机器人沿传送带垂直方向移动到点P₃，通过三点的坐标值求得编码器比例因子F和旋转矩阵

可得到转换矩阵

在上述技术方案中，在步骤三中，设Po₁到Po₂之间传送带移动距离为ΔL，所需时间Δt＝ΔL/V，因此机器人从等待位置Po₀运动到Po₂所用总时间为Δt，将位置Po₀、Po₂逆解到关节空间，使用五次多项式曲线拟合运动轨迹，五次多项式曲线表达式如下：

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F

设起始位置为θ₀，终止位置为θ_f；起始角速度为

终止角速度为

起始角加速度为

终止角加速度为

起始时间为t₀，终止时间为t_f，则Δt＝t_f-t₀，根据条件：

计算出五次多项式曲线表达式的各参数，

在上述技术方案中，在步骤五中，使用三次非均匀有理B样条曲线(NURBS)在工作空间内插补，对目标物体任意轮廓轨迹进行拟合，三次非均匀有理B样条曲线方程如下：

式中{ω_i}为权因子，{P_i}为控制点，N_i，3(u)为非周期节点U矢量上的三次B样条基函数，其中U＝{u₀，u₁，...，u_n+4}；插补分为以下步骤：首先根据新的离散点序列拟合得到权因子、节点矢量U以及控制点，生成样条曲线方程；接着对样条曲线方程进行插补，插补过程引入速度，建立样条曲线方程参数u和速度的方程式，根据实际所需速度和S型速度规划得到参数u离散序列，进而求得插补点；最后运用机器人运动学逆解算法，实现工作空间运动轨迹向关节空间运动轨迹转换，获得关节空间的插补点，使用五次多项式曲线拟合得到完成轨迹曲线，驱动机器人各关节运动。

本发明的优点和有益效果为：

本发明可以使工业机器人在传送带上的涂胶物体到达操作范围后，定位到物体并且进行跟踪，在机器人与物体相对静止后实现对物体轮廓的涂胶操作，整个过程传送带无需停止。具体的方案设计包括以下几个阶段：物体刚开始进入机器人操作范围，机器人得到物体位置后定位物体，此时机器人是点到点运动；机器人定位到物体之后开始跟踪物体，直到与物体相对静止；机器人保持与物体相对静止同时实现对物体轮廓的涂胶操作，此时机器人是连续轨迹运动。本发明能够实现在传送带不停止的情况下跟踪物体涂胶，避免了传送带因频繁启停的损耗，提高了生产效率，节约了成本。

附图说明

图1是本发明机器人传送带跟踪涂胶方案的硬件连接示意图；

图2是本发明传送带坐标系、相机坐标系、机器人坐标系示意图；

图3是本发明传送带标定示意图；

图4是本发明传送带、相机、机器人、目标物体位置关系示意图；

图5是本发明工业机器人传送带跟踪涂胶方案流程图。

对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据以上附图获得其他的相关附图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面结合具体实施例进一步说明本发明的技术方案。

本实施例以六轴协作机器人为实验对象，对其应用本发明所提出的机器人动态跟踪涂胶方案，其杆长参数为：L1＝140，L2＝380，L3＝350，L4＝110，L5＝132，L6＝100。实验环境及所需硬件如附图1所示，由传送带生产线1、相机视觉系统2、机器人本体3、机器人控制系统4这几大部分组成。

传送带生产线上设置有传送带编码器1-1，传送带编码器1-1与机器人控制系统4相连，用于检测传送带速度值；相机视觉系统2与机器人控制系统4相连，用于检测目标物体0初始位置和轮廓；机器人本体动作端设置有胶枪3-1，用于为目标物体0涂胶；机器人控制系统4包括六个电机驱动器和一台工控机，电机驱动器负责驱动机器人本体3的电机运动，工控机安装图像处理软件和EtherCAT主站，负责处理相机和编码器发送的数据以及控制机器人运动，所有运动控制算法在EtherCAT主站中编写完成；上位机5为普通PC，连接机器人控制系统4，作为工控机显示屏。

传送带、相机、机器人的坐标系如附图2所示，在机器人开始工作之前应进行传送带和相机的标定，规定传送带坐标系的X轴正方向为传送带移动方向，传送带匀速运动，速度为400mm/s，机器人控制系统插补周期ΔT为4ms。整个流程如附图5所示，具体包括以下步骤：

步骤一：传送带和相机标定。相机标定使用传统相机标定法，基于机器人进行标定；传送带标定需要确定转换矩阵及比例因子，使用以下方法：

设传送带运动方向为传送带坐标系的X轴方向，传送带坐标系为C，机器人坐标系为R。在传送带坐标系下的位置定义为P^C，机器人坐标系下的位置定义为P^R。两者的转换表达式

即为转换矩阵。首先在传送带上确定一点P₁，移动机器人末端触碰该点，同时记录此时传送带编码器值V₁，机器人末端位姿记为

接着移动传送带，使机器人末端再次触碰到点P₁，此时记为P₂，记录编码器的值V₂，机器人末端位姿记为

最后让机器人沿传送带垂直方向移动到点P₃，记录机器人末端位姿为

标定过程如附图3，通过以上方法得到编码器比例因子F，

还可得到机器人坐标系到传送带坐标系的旋转矩阵

设传送带坐标系原点为O′，机器人坐标系原点为O，则

因此可得到转换矩阵

在接下来的步骤中，会通过F和

对目标物体的坐标位置进行转换，转换后的所有位置都基于机器人坐标系。

步骤二：相机拍摄目标物体的初始位置和外观轮廓。如附图4所示，目标物体进入相机视觉范围，相机拍摄目标物体位置和轮廓，通过工控机转换至机器人坐标系后，发送给机器人，在机器人坐标系下此刻目标物体位置为Po₁。

步骤三：机器人从等待位置快速移动到开始跟踪位置。目标物体到达Po₁后进入机器人操作范围，机器人收到相机传来的信号并准备移动到开始跟踪位置Po₂，如附图4所示，设Po₁到Po₂之间传送带移动距离为ΔL，所需时间Δt＝ΔL/V，因此机器人从等待位置Po₀运动到Po₂所用总时间为Δt。将位置Po₀、Po₂逆解到关节空间，使用五次多项式曲线拟合运动轨迹。五次多项式曲线表达式如下：

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F

设起始位置为θ₀，终止位置为θ_f；起始角速度为

终止角速度为

起始角加速度为

终止角加速度为

起始时间为t₀，终止时间为t_f，则Δt＝t_f-t₀，根据条件：

计算出五次多项式曲线表达式的各参数，

据此可求得五次多项式曲线方程的六个参数值，每个插补周期EtherCAT主站向机器人发送位置控制机器人运动。

步骤四：当机器人已经移动到开始跟踪位置Po₂，此时可对目标物体进行跟踪。跟踪过程需要不断缩小机器人与目标物体的距离，使得机器人与目标物体处于相对静止状态，根据传送带速度V，可以预测每个插补周期ΔT的物体移动距离为Δq＝V*ΔT，设P_i(x_i，y_i，z_i)为插补前物体的位置，P_i+1(x_i+1，y_i+1，z_i+1)为插补后物体的位置，则P_i+1(x_i+1，y_i+1，z_i+1)表示为P_i+1(x_i+Δq，y_i，z_i)，通过此式，可以预测每个插补周期物体的位置。设任意t时刻，机器人的位置为P_t0，目标物体的位置为P_tf，则两者的误差为

ε(t)＝P_tf-P_t0

采用PID形式的跟踪算法，任意时刻t位置的调整参量为

每个周期EtherCAT主站发送给机器人的位置为

P_t＝P_t0+δ(t)

步骤五：当机器人跟踪上目标物体后，两者位置偏差足够小能达到相对静止状态时，开始涂胶任务。根据相机发送的目标物体平面轮廓，进行机器人工作空间的连续运动轨迹规划，同时要保持与目标物体的同步速度。当目标物体不随传送带移动时，采用机器人示教方式得到的物体轮廓插补离散点序列为p₁(x_p1，y_p1，z_p1)，…，p_i(x_pi，y_pi，z_pi)，…，p_n(x_pn，y_pn，z_pn)。当目标物体达到与传送带相同的速度时，为保持与物体的相对静止，根据传送带速度V，插补点X轴上的坐标值应加上一个速度增量值Δv。对于插补周期为ΔT的第i个插补点

Δv＝(i-1)V*ΔT

因此插补点坐标离散点序列变为p₁(x_p1+Δv，y_p1，z_p1)，…，p_i(x_pi+Δv，y_pi，z_pi)，…，p_n(x_pn+Δv，y_pn，z_pn)。采用新的插补离散点序列对轨迹进行拟合。

由于目标物体平面轮廓为任意曲线，使用直线插补和圆弧插补并不能很好拟合运动轨迹。本发明使用三次非均匀有理B样条曲线(NURBS)在工作空间内插补，对目标物体任意轮廓轨迹进行拟合。三次非均匀有理B样条曲线方程如下

式中{ω_i}为权因子，{P_i}为控制点，N_i，3(u)为非周期节点U矢量上的三次B样条基函数，其中U＝{u₀，u₁，...，u_n+4}。三次非均匀有理B样条曲线的插补分为以下步骤：首先根据新的离散点序列使用最小二乘法拟合得到权因子、节点矢量U以及控制点，生成样条曲线方程；接着对样条曲线方程进行插补，插补过程引入速度，建立样条曲线方程参数u和速度的方程式。根据实际所需速度和S型速度规划得到参数u离散序列，进而求得插补点；最后运用机器人运动学逆解算法，实现工作空间运动轨迹向关节空间运动轨迹转换，获得关节空间的插补点，参照步骤2使用五次多项式曲线拟合得到完成轨迹曲线，驱动机器人各关节运动。

步骤六：机器人完成涂胶任务回到等待位置。完成涂胶任务后，机器人运动到目标物体上方，快速离开至等待位置，准备下一个物体的涂胶工作。

Claims (1)

1.一种工业机器人传送带动态跟踪涂胶方法，其特征在于，包括以下步骤：

步骤一：开始跟踪涂胶工作之前，进行传送带标定和相机标定，用以将相机位置和传送带坐标系转换至机器人坐标系，实现对传送带上的目标物体的坐标位置进行转换，转换后的所有位置都基于机器人坐标系；

在步骤1中，传送带标定方法为：需要确定转换矩阵及比例因子，设传送带运动方向为传送带坐标系的X轴方向，机器人坐标系一点P^R与传送带坐标系一点P^C转换方程为：

为求得转换矩阵

首先在传送带上确定一点P₁，移动机器人末端触碰该点；接着移动传送带，使机器人末端再次触碰到点P₁，此时设为P₂；最后让机器人沿传送带垂直方向移动到点P₃，通过三点的坐标值求得编码器比例因子F和旋转矩阵

可得到转换矩阵：

其中，O′为传送带坐标系原点，O为机器人坐标系原点；

步骤二：当目标物体进入机器人操作范围时，此时相机拍摄到目标物体的位置P，编码器读取到传送带速度V，将其发送给机器人；经过坐标转换后，机器人得到其自身坐标系下的目标物体位置

步骤三：机器人从等待位置

移动到开始跟踪位置

此过程运用机器人逆运动学，将

和

的坐标逆解到每一个关节，在关节坐标系下运用五次多项式曲线进行轨迹规划，实现点到点的快速移动；

在步骤三中，设

到

之间传送带移动距离为ΔL，所需时间Δt＝ΔL/V，因此机器人从等待位置

运动到

所用总时间为Δt，将位置

逆解到关节空间，使用五次多项式曲线拟合运动轨迹，五次多项式曲线表达式如下：

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F

设起始位置为θ₀，终止位置为θ_f；起始角速度为

终止角速度为

起始角加速度为

终止角加速度为

起始时间为t₀，终止时间为t_f，则Δt＝t_f-t₀，根据条件：

计算出五次多项式曲线表达式的各参数，

步骤四：当机器人已经移动到开始跟踪位置

此时可对目标物体进行跟踪，跟踪过程需要不断缩小机器人与目标物体的距离，使得机器人与目标物体处于相对静止状态，本步骤采用PID形式跟踪算法，设任意t时刻，机器人的位置为P_t0，目标物体的位置为P_tf，则两者的误差为：

ε(t)＝P_tf-P_t0

则调整参量为：

其中K_p是比例系数，T_i为积分时间常数，T_d为微分时间常数，则任意时刻t发送给机器人的位置为：

P_t＝P_t0+δ(t)

步骤五：当机器人跟踪上目标物体后，两者位置偏差足够小能达到相对静止状态时，开始涂胶任务；此过程，根据相机发送的目标物体平面轮廓，进行机器人工作空间的连续运动轨迹规划，同时要保持与目标物体的同步速度，根据传送带速度V，物体轮廓的插补点坐标离散序列为p₁(x_p1+Δv,y_p1,z_p1)，…，p_i(x_pi+Δv,y_pi,z_pi)，…，p_n(x_pn+Δv,y_pn,z_pn)，其中Δv为每个插补周期速度增量值；采用上述插补点离散点序列对轨迹进行拟合；

在步骤五中，使用三次非均匀有理B样条曲线(NURBS)在工作空间内插补，对目标物体任意轮廓轨迹进行拟合，三次非均匀有理B样条曲线方程如下：

式中{ω_i}为权因子，{P_i}为控制点，N_i,3(u)为非周期节点U矢量上的三次B样条基函数，其中U＝{u₀,u₁,…,u_n+4}；插补分为以下步骤：首先根据新的离散点序列拟合得到权因子、节点矢量U以及控制点，生成样条曲线方程；接着对样条曲线方程进行插补，插补过程引入速度，建立样条曲线方程参数u和速度的方程式，根据实际所需速度和S型速度规划得到参数u离散序列，进而求得插补点；最后运用机器人运动学逆解算法，实现工作空间运动轨迹向关节空间运动轨迹转换，获得关节空间的插补点，使用五次多项式曲线拟合得到完成轨迹曲线，驱动机器人各关节运动；

步骤六：当完成涂胶任务后，机器人运动到目标物体上方，快速离开至等待位置，准备下一个物体的涂胶工作。

Images