一种SCARA动态贴盒功能实现方法与流程

本发明涉及工业机器人控制技术领域，尤其涉及一种scara动态贴盒功能实现方法。

背景技术：

随着国民生活水平的提高，对礼品盒、包装盒等需求量也越来越大。在实际使用中，需要将盒子的外表皮和纸板内衬贴到一起，目前的包装企业，采取的主要是人工贴盒和视觉静态贴盒方案。人工贴盒增加了人力成本，效率也不高，且盒子的一致性不好，非熟练工人很难贴的精准。而视觉静态贴盒，需要传送带完全停止后才可以完成贴盒操作，这样频繁启停，加大了电机、传送带磨损，且此方案需要和前端送纸工序，后端盒子成型工序配合，系统出错风险大。

经检索，中国专利公开号为cn108674922a的专利，公开了pid跟随算法，需要根据传送带速度调pid参数，否则跟随精度不理想。

上述专利还存在有以下不足之处：利用pid跟随算法，使得应用场景受限，大大限制了包装厂家贴盒系统的升级改造，一些动态贴盒方案使用相机实时拍照，视觉系统计算出点位后传给机器人，这种办法对相机的要求比较高，对图像的采集环境比较苛刻，图像处理算法也很复杂，此种方法在工程上很难应用。

技术实现要素：

基于背景技术中提出的利用pid跟随算法，使得应用场景受限，大大限制了包装厂家贴盒系统的升级改造，一些动态贴盒方案使用相机实时拍照，视觉系统计算出点位后传给机器人，这种办法对相机的要求比较高，对图像的采集环境比较苛刻，图像处理算法也很复杂，此种方法在工程上很难应用的技术问题，本发明提出了一种scara动态贴盒功能实现方法。

本发明提出的一种scara动态贴盒功能实现方法，包括scara机器人动态贴盒方案、机械臂、盒子、相机模块、编码器、传送带、待贴礼品盒、动力线缆和编码器线缆，所述scara机器人动态贴盒方案包括机器人-传送带位置标定模块、传送带速度滤波模块、待机点-目标点轨迹规划模块、机械臂-传送带同步运动轨迹规划模块、动态跟踪完毕后脱离轨迹规划以及误差补偿模型建立模块，所述scara机器人动态贴盒方案还包括以下步骤：

s1：在开始作业前，完成机器人-传送带位置标定，不移动机器的情况下，标定参数一直有效；

s2：系统启动后，首先通过速度滤波模块获取了传送带运行速度，当盒子通过光电开关后，拍照获取目标点位置，然后完成待机点-目标点轨迹规划以及机械臂-传送带同步运动轨迹规划，则当待机点-目标点运动完毕后，机械臂末端跟随到盒子上方，此时可以完成吸放等工作；

s3：作业完毕后，进入脱离段，进行脱离轨迹规划，机械臂从跟踪点运动到跟踪点上方后，返回待机点。

优选地，所述机器人-传送带位置标定主要是通过在传送带上加装编码器，然后记录传动带上某点位p对应的机器人坐标位置以及编码器脉冲值，开启传送带，在机器人行程范围内运动一段路程，移动机器人，使得末端继续运动到p点，记录此时刻机器人位置坐标p1以及编码器的脉冲值，由两个点位机器人坐标差值和编码器脉冲差值的比例，计算出传送带的比例因子r，由两个点位机器人x、y方向坐标差值比例，计算出传送带相对于机器人x方向的偏转角θ，由此，可根据编码器脉冲反馈值，获得传送带的运行速度，根据偏转角θ获得传送带的运行方向。

优选地，所述传送带速度滤波模块主要是将每个周期计算的传送带速度进行滤波，首先在每个插补周期中，根据编码器反馈的脉冲值，差分计算当前脉冲速度，然后乘以传动带比例因子r，求得传动带实际运行速度v0，由于速度波动方向分布较为均匀，故使用算术平均滤波法即可取得较好效果，取n个采样周期内的速度平均值，以作为机械手和传动带同步速度规划值，机械臂和编码器连接有动力线缆和编码器线缆。

优选地，所述待机点-目标点轨迹规划模块是规划用户设定的待机点p2到相机模块拍照时刻得到的盒子坐标位置p3的轨迹，此过程使用机器人逆运动学，由点位坐标得到机器人关节角，在关节坐标系下使用五次多项式曲线进行轨迹规划，计算对应插补周期的关节角度，下发到伺服系统中，伺服系统驱动电机运动到目标角度，从而实现p2到p3点的运动。

优选地，所述机械臂-传送带同步运动轨迹规划模块用于规划同步段轨迹，以相机拍照时刻得到的盒子坐标位置p3作为规划起点，沿传送带运动方向规划一个加速曲线，相邻两个插补周期，使用机器人逆运动学计算出两点对应的关节角度，计算它们的关节角度差下发至伺服系统，加速度到规划速度后，进入匀速段，则每个插补周期规划的位移为固定值，同样的使用机器人逆运动学计算机器人相邻两个插补周期对应的关节角度，计算差值，输入到伺服系统中。当机械臂进入同步运动段且待机点-目标点规划轨迹运动完成后，机械臂末端和工件同步运动，此时可关闭吸嘴气阀，放置盒子，完成贴盒操作。

优选地，所述脱离轨迹规划用于跟踪作业完毕后，机械臂停止运动，并回到待机点的轨迹规划，由于同步运动时z方向位置不变，故可以对z方向规划一个点到点运动，z轴抬高高度由用户设定。传送带运行方向，以传送带速度规划一个减速段运动，如此，机械臂脱离跟随，停在了传送带上方，再规划一个传送带上方到待机点的运动，机械臂运动到了待机点。

优选地，还包括以下计算步骤：

s11、首先完成机械手-传送带参数标定，在标定前确定传送带平面和机器人平面平行，则标定时只需考虑x、y方向，在机器人行程范围内找到一点p，移动机器人末端到该点位，记录对应的机器人坐标位置以及编码器脉冲值，开启传送带运动一段路程，移动机器人末端到p点，记录对应坐标位置以及编码器脉冲值，计算比例因子r，偏转角θ；

s12：为了简化操作步骤，方便操作人员快速标定，制作了一个标定软件，按步骤操作后，软件自动读取点位，计算标定结果，并写入控制器；

s13：传送带的实时速度可由编码器反馈值差分计算出来，速度滤波模块，记录最近n个插补周期的速度值，使用算术平均滤波法，求得经过滤波后的速度；

s14：传送带运行时，盒子跟随传动带运动，当光电开关检测到盒子到位后，触发相机拍照，相机将图像传入视觉系统中，视觉系统经过运算给出面纸目标位置p2，根据用户设定的待机点位置p2，运用机器人逆运动学，得到p2、p3关节角度q2、q3，在关节空间内进行轨迹规划，使用九次多项式拟合运动轨迹，九次多项式曲线表达式如下：

每个插补周期可根据该表达式，ethercat主站向机器人发送角度位置控制机器人的运动；

s15：为了实现机械臂跟随传送运动，需要在上述轨迹规划的基础上叠加一段同步运动轨迹，以相机拍照时刻得到的盒子坐标位置p3(x3,y3)作为规划起点，首先以测得的传动带速度为最大速度，使用九次多项式曲线规划一个加速段，根据插补时间，得到相应插补周期规划的位移s,则每个插补周期规划的位置坐标为p4(x3+,y3+)，使用机器人逆运动学计算出p3、p4对应的关节角度，然后计算它们的差值，在每个插补周期里，将该角度输出到伺服系统中。加速度到规划速度后，进入匀速段，则每个插补周期规划的位移s为固定值，其中，为系统每周期的插补时间，同样的，使用机器人逆运动学计算机器人相邻两个插补周期对应的关节角度，计算差值，输入到伺服系统中，当机械臂进入同步运动段且待机点-目标点规划轨迹运动完成后，机械臂末端和工件同步运动，此时可关闭吸嘴气阀，放置盒子，完成贴盒操作；

s16：脱离轨迹规划用于跟踪作业完毕后，机械臂停止运动，并回到待机点的轨迹规划，由于同步运动时z方向位置不变，故可以对z方向规划一个点到点运动，z轴抬高高度由用户设定，传送带运行方向，以传送带速度规划一个减速段运动，如此，机械臂脱离跟随，停在了传送带上方，再规划一个传送带上方到待机点的运动，机械臂运动到了待机点，scara动态贴盒动作完成，完成一整个循环，等待待贴礼品盒到位。

本发明中的有益效果为：

1、该scara动态贴盒功能实现方法，通过提出一种位置叠加的方案，将动态贴盒过程分解成传送带方向的同步运动和待机点到贴盒点的点到点运动。为了实现机械手和传送带的同步运动，需要完成传动带测速模块。为了获取贴盒点坐标位置，需要完成相机和机械臂的手眼标定、传送带和机械臂的位置标定。为了使机械手和传送带在跟随段时达到较理想的跟踪精度，建立一个同步运动位置补偿模型，补偿跟随段的位置偏差。

2、该scara动态贴盒功能实现方法，关键在于将动态运动分解为一个点到点的运动和一个传送带方向的同步运动。实现原理通俗易懂，算法处理效率高，可以较好地保证机器人快速跟踪上目标物体，且可以达到较好的跟踪精度，同时也避免了繁琐的参数调整。使用该方法，各关节速度曲线平滑，避免了由于目标物体的变化引起的关节速度剧烈变化，从而出现末端振动的现象。

该装置中未涉及部分均与现有技术相同或可采用现有技术加以实现。

附图说明

图1为本发明提出的一种scara动态贴盒功能实现方法的各模块执行流程图图；

图2为本发明提出的一种scara动态贴盒功能实现方法的动态贴盒系统布局图；

图3为本发明提出的一种scara动态贴盒功能实现方法的rob2s-40型机器人示意图；

图4为本发明提出的一种scara动态贴盒功能实现方法的机械臂-传送带参数标定软件界面；

图5为本发明提出的一种scara动态贴盒功能实现方法的传送带运行方向和机器人坐标系关系图。

图中：1机械臂、2盒子、3相机模块、4编码器、5传送带、6待贴礼品盒、7动力线缆、8编码器线缆。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

在本发明的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

2、参照图1-5，一种scara动态贴盒功能实现方法，包括scara机器人动态贴盒方案、机械臂1、盒子2、相机模块3、编码器4、传送带5、待贴礼品盒6、动力线缆7和编码器线缆8，scara机器人动态贴盒方案包括机器人-传送带位置标定模块、传送带速度滤波模块、待机点-目标点轨迹规划模块、机械臂-传送带同步运动轨迹规划模块、动态跟踪完毕后脱离轨迹规划以及误差补偿模型建立模块，scara机器人动态贴盒方案还包括以下步骤：

s1：在开始作业前，完成机器人-传送带位置标定，不移动机器的情况下，标定参数一直有效；

s2：系统启动后，首先通过速度滤波模块获取了传送带运行速度，当盒子2通过光电开关后，拍照获取目标点位置，然后完成待机点-目标点轨迹规划以及机械臂-传送带同步运动轨迹规划，则当待机点-目标点运动完毕后，机械臂1末端跟随到盒子2上方，此时可以完成吸放等工作；

s3：作业完毕后，进入脱离段，进行脱离轨迹规划，机械臂1从跟踪点运动到跟踪点上方后，返回待机点。

本发明中，机器人-传送带位置标定主要是通过在传送带5上加装编码器4，然后记录传动带5上某点位p对应的机器人坐标位置以及编码器脉冲值，开启传送带5，在机器人行程范围内运动一段路程，移动机器人，使得末端继续运动到p点，记录此时刻机器人位置坐标p1以及编码器的脉冲值，由两个点位机器人坐标差值和编码器脉冲差值的比例，计算出传送带的比例因子r，由两个点位机器人x、y方向坐标差值比例，计算出传送带相对于机器人x方向的偏转角θ，由此，可根据编码器脉冲反馈值，获得传送带5的运行速度，根据偏转角θ获得传送带5的运行方向。

本发明中，传送带速度滤波模块主要是将每个周期计算的传送带速度进行滤波，首先在每个插补周期中，根据编码器4反馈的脉冲值，差分计算当前脉冲速度，然后乘以传动带比例因子r，求得传动带实际运行速度v0，由于速度波动方向分布较为均匀，故使用算术平均滤波法即可取得较好效果，取n个采样周期内的速度平均值，以作为机械手和传动带5同步速度规划值，机械臂1和编码器4连接有动力线缆7和编码器线缆8。

本发明中，待机点-目标点轨迹规划模块是规划用户设定的待机点p2到相机模块3拍照时刻得到的盒子2坐标位置p3的轨迹，此过程使用机器人逆运动学，由点位坐标得到机器人关节角，在关节坐标系下使用五次多项式曲线进行轨迹规划，计算对应插补周期的关节角度，下发到伺服系统中，伺服系统驱动电机运动到目标角度，从而实现p2到p3点的运动。

本发明中，机械臂-传送带同步运动轨迹规划模块用于规划同步段轨迹，以相机拍照时刻得到的盒子2坐标位置p3作为规划起点，沿传送带5运动方向规划一个加速曲线，相邻两个插补周期，使用机器人逆运动学计算出两点对应的关节角度，计算它们的关节角度差下发至伺服系统，加速度到规划速度后，进入匀速段，则每个插补周期规划的位移为固定值，同样的使用机器人逆运动学计算机器人相邻两个插补周期对应的关节角度，计算差值，输入到伺服系统中。当机械臂1进入同步运动段且待机点-目标点规划轨迹运动完成后，机械臂1末端和工件同步运动，此时可关闭吸嘴气阀，放置盒子2，完成贴盒操作。

本发明中，脱离轨迹规划用于跟踪作业完毕后，机械臂1停止运动，并回到待机点的轨迹规划，由于同步运动时z方向位置不变，故可以对z方向规划一个点到点运动，z轴抬高高度由用户设定。传送带5运行方向，以传送带5速度规划一个减速段运动，如此，机械臂1脱离跟随，停在了传送带5上方，再规划一个传送带5上方到待机点的运动，机械臂1运动到了待机点。

本发明中，还包括以下计算步骤：

s11、首先完成机械手-传送带参数标定，在标定前确定传送带平面和机器人平面平行，则标定时只需考虑x、y方向，在机器人行程范围内找到一点p，移动机器人末端到该点位，记录对应的机器人坐标位置以及编码器脉冲值，开启传送带运动一段路程，移动机器人末端到p点，记录对应坐标位置以及编码器脉冲值，计算比例因子r，偏转角θ；

s12：为了简化操作步骤，方便操作人员快速标定，制作了一个标定软件，按步骤操作后，软件自动读取点位，计算标定结果，并写入控制器；

s13：传送带5的实时速度可由编码器4反馈值差分计算出来，速度滤波模块，记录最近n个插补周期的速度值，使用算术平均滤波法，求得经过滤波后的速度；

s14：传送带5运行时，盒子2跟随传动带5运动，当光电开关检测到盒子2到位后，触发相机拍照，相机将图像传入视觉系统中，视觉系统经过运算给出面纸目标位置p2，根据用户设定的待机点位置p2，运用机器人逆运动学，得到p2、p3关节角度q2、q3，在关节空间内进行轨迹规划，使用九次多项式拟合运动轨迹，九次多项式曲线表达式如下：

每个插补周期可根据该表达式，ethercat主站向机器人发送角度位置控制机器人的运动；

s15：为了实现机械臂1跟随传送运动，需要在上述轨迹规划的基础上叠加一段同步运动轨迹，以相机拍照时刻得到的盒子坐标位置p3x3,y3作为规划起点，首先以测得的传动带速度为最大速度，使用九次多项式曲线规划一个加速段，根据插补时间，得到相应插补周期规划的位移s,则每个插补周期规划的位置坐标为p4x3+,y3+，使用机器人逆运动学计算出p3、p4对应的关节角度，然后计算它们的差值，在每个插补周期里，将该角度输出到伺服系统中。加速度到规划速度后，进入匀速段，则每个插补周期规划的位移s为固定值，其中，为系统每周期的插补时间，同样的，使用机器人逆运动学计算机器人相邻两个插补周期对应的关节角度，计算差值，输入到伺服系统中，当机械臂1进入同步运动段且待机点-目标点规划轨迹运动完成后，机械臂1末端和工件同步运动，此时可关闭吸嘴气阀，放置盒子，完成贴盒操作；

s16：脱离轨迹规划用于跟踪作业完毕后，机械臂1停止运动，并回到待机点的轨迹规划，由于同步运动时z方向位置不变，故可以对z方向规划一个点到点运动，z轴抬高高度由用户设定，传送带运行方向，以传送带速度规划一个减速段运动，如此，机械臂1脱离跟随，停在了传送带5上方，再规划一个传送带5上方到待机点的运动，机械臂1运动到了待机点，scara动态贴盒动作完成，完成一整个循环，等待待贴礼品盒6到位。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。