一种控制机器人跟随传送带运动的方法和装置与流程

本发明涉及机器人的应用，具体涉及一种控制机器人跟随传送带运动的方法和装置。

背景技术：

在机器人的应用场合，常常使用传送带来运送工艺对象零件，这些零件往往是批量的，且有相同的作业要求。在喷涂、涂胶等行业中，常常需要机器人对这些零件的工艺轨迹进行作业。

现有的工艺具有如下缺陷：

(1)在传送带运送零件到位停止后，机器人对该零件的工艺轨迹进行作业直至作业完成，再启动传送带运送下一个零件到位并停止后，机器人再对该零件的工艺轨迹进行作业，如此反复，传送带和机器人错开的运动方式，造成零件单位作业耗时长，生产效率极低；

(2)目前的跟随方法中，如在搬运等应用领域，根据相机对零件拍摄定位的单个点跟随传送带运动，应用范围有限，无法满足喷涂、涂胶等应用领域，对零件的多个点跟随传送带运动的需求。

由此可见，目前的机器人跟随传送带的方法存在传送带和机器人错开运行、无法满足多点跟随的问题。

技术实现要素：

本发明要解决的技术问题是目前的机器人跟随传送带的方法存在传送带和机器人错开运行、无法满足多点跟随的问题。

为解决上述技术问题，本发明提供了一种控制机器人跟随传送带运动的方法，包括以下步骤：

步骤a，获取机器人在传送带上第一标记点和第二标记点分别对应的位姿p0(x0,y0,z0,w0,p0,r0)、p1(x1,y1,z1,w1,p1,r1)，第一标记点可通过传送带的运动完全重合到第二标记点，则：

运动方向向量为(x1-x0,y1-y0,z1-z0)；

进行单位化后的结果为：vector_belt(belt_dx,belt_dy,belt_dz)；

步骤b，在传送带运动状态下，测量N次传送带上安置的标杆触发第一IO传感器和第二IO传感器的时间差time_io，第一IO传感器和第二IO传感器分别对应固定安装在传送带基座的第一固定位、第二固定位上，已确定二者的固定距离为length_io，传送带的运动平均速度speed_belt为：

其中，time1_io,time2_io,timeN_io分别为第一次、第二次、第N次采集的时间差记录；

步骤c，在传送带停止状态下，在传送带上安置的标杆触发第一IO传感器时，对传送带的固定位置上放置的工艺对象，按作业的轨迹要求在笛卡尔空间对机器人进行轨迹编程，编程格式采用LFOLL ON，LFOLL OFF两关键字来标记跟随开始和跟随结束；

步骤d，在传送带运动状态下，在传送带上安置的标杆触发第一IO传感器时，时间t开始从0.0计数，经过tm时间完成机器人的速度V从0到speed_belt的规划，在0≤t≤tm，在笛卡尔空间实时计算机器人的位姿p(x,y,z,w,p,r)为：

其中，(x2,y2,z2,w2,p2,r2)为不考虑传送带的运动影响，对作业轨迹在时刻t为0时插补的位姿值；l为机器人在速度V从0到speed_belt的规划中t时刻的位移值；

在t≥tm后，在笛卡尔空间实时计算机器人跟随的位姿p(x,y,z,w,p,r)：

其中，(x3,y3,z3,w3,p3,r3)为不考虑传送带的运动影响，对作业轨迹在时刻(t-tm)插补的位姿值，(t-tm)*speed_belt为传送带在(t-tm)时间内的位移，tm为所规划的机器人的速度V从0变化为speed_belt时经过的时间，l_tm为所规划的机器人在tm时刻的位移值；

将该位姿转换为机器人的驱动控制量，发送给机器人驱动装置。

在上述方案中，步骤d中，机器人在速度V从0变化为speed_belt的规划完成后经过的位移值l_tm满足如下关系：l_tm＝speed_belt*tm，所规划的机器人的速度V从0变化为speed_belt的详细过程如下：

速度V为：当t∈[0.0,0.5tm]时，V＝V1＝3.0*speed_belt*t/tm，

当t∈[0.5tm,tm]时，V＝V2＝2.0*speed_belt-speed_belt*t/tm；

在t＝0.5tm时刻，所规划的机器人的速度V为1.5*speed_belt；

位移l为：当t∈[0.0,0.5tm]时，l＝1.5*speed_belt*t²/tm，

当t∈[0.5tm,tm]时，

l＝1.5*speed_belt*0.25tm+1.5*speed_belt*(t-0.5tm)-0.5*speed_belt*(t-0.5tm)²/tm。

在上述方案中，步骤a中，第一标记点和第二标记点分别对应的位姿p0、p1的位置距离Ds为500mm以上，否则作为失败数据采集并发出报警。

在上述方案中，步骤b中，标杆触发第一IO传感器和第二IO传感器的触发时间差time_io为10s以上，否则作为失败数据采集并发出报警。

在上述方案中，步骤b中，测量次数N为5。

本发明还提供了一种机器人跟随传送带运动的装置，包括：

传送带方向计算单元，用于获取机器人在传送带上第一标记点和第二标记点分别对应的位姿来计算传送带在机器人坐标系下的运动方向向量；

传送带速度计算单元，用于在传送带运动状态下，测量N次传送带上安置的标杆触发第一IO传感器和第二IO传感器的时间差，根据第一IO传感器和第二IO传感器的距离，计算传送带的运动平均速度；

轨迹编程单元，用于在传送带停止状态下，在传送带上安置的标杆触发第一IO传感器时，对传送带的固定位置上放置的工艺对象，按作业的轨迹要求在笛卡尔空间对机器人进行轨迹编程；

跟随控制单元，用于在传送带运动状态下，传送带上安置的标杆触发第一IO传感器，在机器人的速度V完成从0到传送带的运动平均速度的规划后，在笛卡尔空间实时计算机器人跟随的位姿，并将该位姿转换为机器人的驱动控制量，发送给机器人驱动装置。

在上述方案中，所述第一IO传感器和所述第二IO传感器均为对射型光电传感器。

本发明，在传送带运动状态下，基于时间差计算得到传送带的平均运动速度，对传送带相对机器人无摆放方向的要求，在传送带停止状态机器人可对工艺作业的零件进行轨迹编程，改善了传送带和机器人完全错开的运作方式，并可对工艺对象的零件的多个点跟随传送带运动作业，对作业应用的范围限制小，简单易用，适用于机器人和传送带的流水线生产，有利于缩短工艺作业时间，提高生产效率。

附图说明

图1为根据本发明的控制机器人跟随传送带运动的方法的流程图；

图2为根据本发明的机器人与传送带的布局示意图；

图3为根据本发明的机器人跟随传送带运动的速度示意图；

图4为根据本发明的控制机器人跟随传送带运动的装置的示意图。

具体实施方式

下面结合具体实施例和说明书附图对本发明予以详细说明。

如图1～图3所示，本发明提供了一种控制机器人跟随传送带运动的方法，结合结构来说明，具体包括以下步骤：

步骤a，获取机器人20在传送带21上的第一标记点26和第二标记点27分别对应的位姿p0(x0,y0,z0,w0,p0,r0)、p1(x1,y1,z1,w1,p1,r1)，第一标记点26可通过传送带21的运动完全重合到第二标记点27，则：

运动方向向量为(x1-x0,y1-y0,z1-z0)，方向为图2中的箭头方向；

进行单位化后的结果为：vector_belt(belt_dx,belt_dy,belt_dz)；

步骤b，在传送带21的运动状态下，测量N次传送带21上安置的标杆22触发第一IO传感器29和第二IO传感器30的时间差time_io，第一IO传感器29、第二IO传感器30分别对应固定安装在传送带21基座的第一固定位、第二固定位上，已确定二者的固定距离为length_io，传送带21的运动平均速度speed_belt为：

其中，time1_io,time2_io,timeN_io分别为第一次、第二次、第N次采集的时间差记录；

步骤c，在传送带21停止状态下，在传送带21上安置的标杆22触发第一IO传感器29时，对传送带21的固定位置23上放置的工艺对象，按作业轨迹24的要求在笛卡尔空间对机器人20进行轨迹编程，编程格式采用LFOLL ON，LFOLL OFF两关键字来标记跟随开始和跟随结束；

步骤d，在传送带21的运动状态下，在传送带21上安置的标杆22触发第一IO传感器29时，时间t开始从0.0计数，经过tm时间完成机器人20的速度V从0到speed_belt的规划，在0≤t≤tm，在笛卡尔空间实时计算机器人20的位姿p(x,y,z,w,p,r)为：

其中，(x2,y2,z2,w2,p2,r2)为不考虑传送带的运动影响，对作业轨迹在时刻t为0时插补的位姿值；l为机器人在速度V从0到speed_belt的规划中t时刻的位移值；

在t≥tm后，在笛卡尔空间实时计算机器人20跟随的位姿p(x,y,z,w,p,r)：

其中，(x3,y3,z3,w3,p3,r3)为不考虑传送带21的运动影响，对作业轨迹24在时刻(t-tm)插补的位姿值，(t-tm)*speed_belt为传送带21在(t-tm)时间内的位移S3，S3为图3中标注的块在对应时刻t的面积，tm为所规划的机器人20的速度V从0变化为speed_belt时经过的时间，l_tm为所规划的机器人在tm时刻的位移值；

将该位姿转换为机器人20的驱动控制量，发送给机器人驱动装置。

优选地，步骤d中，机器人20在速度V从0变化为speed_belt的规划完成后经过的位移l_tm满足如下关系：l_tm＝speed_belt*tm，在tm时刻，作业轨迹24已随传送带21的运动变化为后续作业轨迹25，所规划的机器人20的速度V从0变化为speed_belt的详细过程如下：

速度V为：当t∈[0.0,0.5tm]时，V＝V1＝3.0*speed_belt*t/tm，

当t∈[0.5tm,tm]时，V＝V2＝2.0*speed_belt-speed_belt*t/tm；

在t＝0.5tm时刻，所规划的机器人20的速度V为1.5*speed_belt；

V1和V2分别为图3中标注的两条直线上对应时刻t的点的高度；

位移l为：当t∈[0.0,0.5tm]时，l＝1.5*speed_belt*t²/tm，

当t∈[0.5tm,tm]时，

l＝1.5*speed_belt*0.25tm+1.5*speed_belt*(t-0.5tm)-0.5*speed_belt*(t-0.5tm)²/tm。l为图3中标注的块在对应时刻t的面积。

优选地，步骤a中，第一标记点26和第二标记点27分别对应的位姿p0、p1的位置距离Ds为500mm以上，否则作为失败数据采集并发出报警。

优选地，步骤b中，标杆22触发第一IO传感器29和第二IO传感器30的触发时间差time_io为10s以上，否则作为失败数据采集并发出报警。位置距离Ds为

优选地，步骤b中，测量次数N为5，保证一定的测量次数才能保证速度测算的精确性。

如图4所示，本发明还提供了一种机器人跟随传送带运动的装置400，包括传送带方向计算单元41、传送带速度计算单元42、轨迹编程单元43和跟随控制单元44。

传送带方向计算单元41用于获取机器人20在传送带21上第一标记点26和第二标记点27分别对应的位姿，来计算传送带21在机器人20坐标系下的运动方向向量。

传送带速度计算单元42用于在传送带21的运动状态下，测量N次传送带21上安置的标杆22触发第一IO传感器29和第二IO传感器30的时间差，根据第一IO传感器29和第二IO传感器30的距离，计算传送带21的运动平均速度。

轨迹编程单元43用于在传送带21停止状态下，在传送带21上安置的标杆22触发第一IO传感器29时，对传送带21的固定位置23上放置的工艺对象，按作业的轨迹要求在笛卡尔空间对机器人20进行轨迹编程。

跟随控制单元用于44在传送带21的运动状态下，传送带21上安置的标杆22触发第一IO传感器29，在机器人20的速度V完成从0到传送带21的运动平均速度的规划后，在笛卡尔空间实时计算机器人20跟随的位姿，并将该位姿转换为机器人20的驱动控制量，发送给机器人驱动装置21，使机器人按照既定轨迹进行动作。

优选地，第一IO传感器29和第二IO传感器30均为对射型光电传感器，对射型光电传感器安全性能胜过激光型，且降低了成本，使用了可清楚辨明光点位置的红色点光源LED，使用耐曲折导线，可放心地安置于可动部，具有超高速应答能力。

本发明，在传送带运动状态下，基于时间差计算得到传送带的平均运动速度，对传送带相对机器人无摆放方向的要求，在传送带停止状态机器人可对工艺作业的零件进行轨迹编程，改善了传送带和机器人完全错开的运作方式，并可对工艺对象的零件的多个点跟随传送带运动作业，对作业应用的范围限制小，简单易用，适用于机器人和传送带的流水线生产，有利于缩短工艺作业时间，提高生产效率。

本发明不局限于上述最佳实施方式，任何人应该得知在本发明的启示下作出的结构变化，凡是与本发明具有相同或相近的技术方案，均落入本发明的保护范围之内。