一种传送带跟踪机器人控制系统及其控制方法

本发明涉及自动化控制技术领域，尤其涉及一种传送带跟踪机器人控制系统及其控制方法。

随着机器人技术的发展，工业机器人已广泛应用于汽车、3C等制造领域，为了扩展工业机器人在制造业的应用范围，不断提高应用层次，研究开发具有一定感知、判断决策功能的信息反馈智能型的机器人，将是工业机器人重点发展方向之一；

现有的传送带追踪机器人控制系统，通过两个锚点模块将测量的距离数据传至目标模块，目标模块将接收到的距离数据通过无线模块传至上位机；上位机通过无线模块接收距离数据并根据算法计算出目标物体的坐标；上位机同时显示目标物体的实时位置坐标和移动轨迹；上位机同时将坐标信息传至服务器，实现了实时的跟踪定位运动的传送带上的目标物体的位置坐标和移动轨迹；上述操作方式虽然能够实现工件的追踪，但是，现有传送带追踪机器人控制系统，仅仅能够对单个目标工件进行追踪，使其不能够对连续的工件进行追踪，当需要对连续多个工件进行追踪时，需多次对单个工件进行追踪操作，需重复多次操作完成连续多个工件完成追踪，导致现有的传送带追踪机器人控制系统不能够对连续工件进行追踪，降低了传送带追踪机器人控制系统的工作效率；而且，现有的传送带追踪机器人控制系统内部无线模块结构较复杂，当该系统损坏时，需要对无线模块的内部进行逐个排查，维修成本大。

根据以上技术问题，本发明提供一种传送带跟踪机器人控制系统及其控制方法，其特征在于包括传送带、编码器、光电开关、控制器、机器人、工作区域，所述工作区域安装有传送带、控制器、机器人，所述传送带的前侧设置有控制器，所述控制器的右侧设置有机器人，所述传送带的中部安装有光电开关，用于检测工件是否通过，从而实现工件定位，所述传送带的左端安装有编码器，所述编码器与控制器连接，所述控制器与机器人连接，编码器用于记录传送带当前的位置以及计算传送带速度，从而推测出传送带上工件的位置，在机器人跟踪目标工件过程中，机器人控制系统根据编码器反馈的位置对机器人示教路径进行在线预测及实时修正，从而实现对目标工件的自动跟踪，降低用户示教操作难度，所述编码器内部设置有内存共享机制，通过内存共享机制，控制器外部接口通过实时获取传送带编码器信息，并将其放入到共享内存中，以便机器人规划模块调用；

本发明常用于对移动目标工件的自动抓取，这就要求机器人在抓取工件过程中，机器人的运动速度与目标工件的运行速度保持一致；这对机器人实时性提出了很大的要求，控制器通过在规划过程中引入实时算法，实时获取编码器位置，并根据编码器位置，实时纠正机器人规划路径，从而实现对目标工件的跟踪，此外考虑到机器人路径中的区域点和停止点，通过引入机器人启动调整参数和机器人停止调整参数，保证机器人运行平稳；

传送带跟踪工件的自动抓取也对机器人精度提出了很高的要求，控制机器人运动的控制器通过对传送带位置进行标定，并引入控制器延迟补偿因子，从而保证机器人抓取过程中的精度；

本发明在规划算法上采用分层实现，分别为几何插值层，动力学插值层以及在线执行层，本发明通过在几何层中引入传送带编码器数据，并在线预测机器人的位置用于动力学插值层参考，动力学插值层主要是根据机器人的动力学信息，确定机器人运行速度，保证机器人运行速度在电机最大可执行速度范围内，动力学插值层根据几何插值层引入的传感器数据，动态的修正机器人轨迹，再将所生成的数据发送到执行层，从而实现机器人对目标的动态跟踪，由于在机器人运动规划过程中引入了在线预测机制，大大提高了机器人跟踪精度，从而实现对传送带上的工件自动跟踪；

本发明通过在应用层引入跟踪工件缓冲区，使得机器人具备缓冲区机制，使得机器人可同时对多个工件目标数据进行记录，只要当前工件未超出用户设置的最大跟踪范围，该范围需在机器人可达空间范围内，即可实现连续对多个目标进行跟踪。

本发明的操作步骤如下：

步骤一：本发明包括控制器，传送带，光电开关，编码器，机器人，传送带摆放在机器人可达工作区域内，光电开关安装在传送带上，用于检测工件是否通过，从而实现工件定位，编码器安装在传送带上，用于记录传送带当前的位置以及计算传送带速度，从而推测出传送带上工件的位置，编码器信息通过IO方式引入到控制器中；

步骤二：通过传送带跟踪标定界面对相关参数进行标定；

步骤三：配置传送带跟踪基础参数；

待整个系统标定完后，用户即可通过示教器编写传送带跟踪程序从而实现对传送带上的工件进行跟踪，用户在示教传送带跟踪程序之前也可根据自己需求对传送带跟踪参数进行配置。

本发明的有益效果为：本发明现有控制器的通用性的基础上，增加了编码器，实现了传送带跟踪过程中采集到的位置信息进行处理，本发明的构成简单，不要额外增加采集硬件，用户仅通过购买IO模块用于获取编码器位置，即可实现对工件进行跟踪，降低了本发明出现损坏的概率，减少了维修成本的支出；本发明通过在应用层引入跟踪工件缓冲区，使得机器人可同时对多个工件目标数据进行记录，只要当前工件未超出用户设置的最大跟踪范围，该范围需在机器人可达空间范围内，即可实现连续对多个目标工件进行跟踪，提高了工作效率；

本发明根据编码器反馈位置信息，并将该信息引入到计算机器人运动规划过程中，并对动力学插值进行引导，同时在线预测下一个规划周期内机器人所需的偏移路径，通过预测机制将工件移动数据引入到机器人运动规划中，从而实现机器人对传送带上的工件自动跟踪；

本发明通过采用内存共享机制，通过内存共享机制，控制器外部接口通过实时获取传送带编码器信息，并将其放入到共享内存中，以便机器人规划模块调用；

本发明通过采用缓冲区机制，可实现同时对多个未超出跟踪范围的工件进行跟踪。

图1为本发明整体结构示意图；

图2为本发明传送带的结构示意图；

图3为本发明跟踪流程图；

图4为本发明标定传送带基座标位置。

如图，1-传送带、2-编码器、3-光电开关、4-工件、5-控制器、6-机器人、7-工作区域。

实施例1

本发明提供一种传送带跟踪机器人控制系统及其控制方法，其特征在于包括传送带1、编码器2、光电开关3、控制器5、机器人6、工作区域7，工作区域7安装有传送带1、控制器5、机器人6，传送带1的前侧设置有控制器5，控制器5的右侧设置有机器人6，传送带1的中部安装有光电开关3，用于检测工件4是否通过，从而实现工件4定位，传送带1的左端安装有编码器2，编码器2与控制器5连接，控制器5与机器人6连接，编码器2用于记录传送带1当前的位置以及计算传送带1速度，从而推测出传送带1上工件4的位置，在机器人6跟踪目标工件4过程中，机器人6控制系统根据编码器2反馈的位置对机器人6示教路径进行在线预测及实时修正，从而实现对目标工件4的自动跟踪，降低用户示教操作难度，编码器2内部设置有内存共享机制，通过内存共享机制，控制器5外部接口通过实时获取传送带1编码器2信息，并将其放入到共享内存中，以便机器人6规划模块调用；

本发明常用于对移动目标工件4的自动抓取，这就要求机器人6在抓取工件4过程中，机器人6的运动速度与目标工件4的运行速度保持一致；这对机器人6实时性提出了很大的要求，控制器5通过在规划过程中引入实时算法，实时获取编码器2位置，并根据编码器2位置，实时纠正机器人6规划路径，从而实现对目标工件4的跟踪，此外考虑到机器人6路径中的区域点和停止点，通过引入机器人6启动调整参数和机器人6停止调整参数，保证机器人6运行平稳；

传送带1跟踪工件4的自动抓取也对机器人6精度提出了很高的要求，控制机器人6运动的控制器5通过对传送带1位置进行标定，并引入控制器5延迟补偿因子，从而保证机器人6抓取过程中的精度；

本发明在规划算法上采用分层实现，分别为几何插值层，动力学插值层以及在线执行层，本发明通过在几何层中引入传送带1编码器2数据，并在线预测机器人6的位置用于动力学插值层参考，动力学插值层主要是根据机器人6的动力学信息，确定机器人6运行速度，保证机器人6运行速度在电机最大可执行速度范围内，动力学插值层根据几何插值层引入的传感器数据，动态的修正机器人6轨迹，再将所生成的数据发送到执行层，从而实现机器人6对目标的动态跟踪，由于在机器人6运动规划过程中引入了在线预测机制，大大提高了机器人6跟踪精度，从而实现对传送带1上的工件4自动跟踪；

本发明通过在应用层引入跟踪工件4缓冲区，使得机器人6具备缓冲区机制，使得机器人6可同时对多个工件4目标数据进行记录，只要当前工件4未超出用户设置的最大跟踪范围，该范围需在机器人6可达空间范围内，即可实现连续对多个目标进行跟踪。

本发明的操作步骤如下：

步骤一：本发明包括控制器5，传送带1，光电开关3，编码器2，机器人6，传送带1摆放在机器人6可达工作区域7内，光电开关3安装在传送带1上，用于检测工件4是否通过，从而实现工件4定位，编码器2安装在传送带1上，用于记录传送带1当前的位置以及计算传送带1速度，从而推测出传送带1上工件4的位置，编码器2信息通过IO方式引入到控制器5中；

步骤二：通过传送带1跟踪标定界面对相关参数进行标定；

步骤三：配置传送带1跟踪基础参数；

待整个系统标定完后，用户即可通过示教器编写传送带1跟踪程序从而实现对传送带1上的工件4进行跟踪，用户在示教传送带1跟踪程序之前也可根据自己需求对传送带1跟踪参数进行配置。

实施例2

本发明在使用前，对本发明进行安装，步骤一：系统包括控制器55，传送带1，光电开关3，编码器2，机器人6，传送带1摆放在机器人6可达工作区域7内，光电开关3安装在传送带1上，用于检测工件4是否通过，从而实现工件4定位，编码器2安装在传送带1上，用于记录传送带1当前的位置以及计算传送带1速度，从而推测出传送带1上工件4的位置，编码器2信息通过IO方式引入到控制器5中；

步骤二：通过传送带1跟踪标定界面对相关参数进行标定，标定参数分别说明如下：

标定编码器2与位置转换关系：

当整个环境搭建好后，通常我们不知道编码器2和传送带1之间的传动比，因此我们需要对编码器2和传送带1之间的关系进行校验，把工件4放在传送带1上，并在工件4上做一个标记点，选择指定的tool和wobj，通过机器人6示教该标记点，并记录当前机器人6位置为target1，同时读取当前编码器2的值count1启动传送带1，并允许传送带1移动一段距离，一般来讲大于500mm,并保证机器人6能够到达新的标记点位置，此时，停止传送带1运动，选择示教target1时采用的tool和wobj，重新示教标记点，并记录当前机器人6位置为target2，同时记录当前编码器2的值为count2，此时我们可以计算得出taget2和target1之间的几何距离

diff＝sqrt(pow(target2.x-target1.x,2)+pow(target2.y-target1.y,2)+pow(target2.z-target1.z,2)),单位为m

编码器2与传送带1的齿轮比：

Gear＝(count2-count1)/diff

相关参数会保存在编码器2位置转换系数中，其数据类型是float型，取值范围-1000000,1000000。

本发明上编辑程序如下：

ActUnit(“conveyor”)

WaitWObj(wobj1)；

1)启动传送带1，启动程序，将工件4放置在传送带1上，当工件41通过同步开关，程序执行完，停止传送带1运动。并示教工件41上的标记点，并记录target1。

2)继续启动传送带1，并停止，选用相同的tool和Wobj获得target2。

3)重复上述操作两次，获得target3和target4。

最后在标定界面上点击确定可计算得传送带1基座标。

步骤三：配置传送带1跟踪基础参数：

待整个系统标定完后，用户即可通过示教器编写传送带1跟踪程序从而实现对传送带1上的工件4进行跟踪。用户在示教传送带1跟踪程序之前也可根据自己需求对传送带1跟踪参数进行配置：

传送带1跟踪参数说明如下：

路径层滤波参数(path_filter_order)：数据类型是int型，取值范围1-10，用户可修改，默认值为5。

执行层滤波参数(exectue_filter_order)：数据类型是int型，取值范围1-15，用户可修改，默认值为5。

控制器5延迟时间(controller_delay_time)：数据类型是float型,取值范围-0.1到0.1，单位是秒，默认值为0.0，用户可修改

传送带1名称(name)：数据类型是string型，用户通过配置的外部轴机械单元进行选取。用户可修改，但不该修改为机械臂的机械单元。

传送带1激活状态(active):数据类型是bool型，默认设置为true，用户可对该参数进行修改。

编码器2分辨率(encoder_resolution_ratio):数据类型是int型，默认值为16。用户可根据选择的编码器2位数对该参数进行修改。

缓冲区长度(start_window_width)：数据类型是float型，取值范围0.0到1000.0，默认值为100，单位是mm，用户设置的缓冲区长度，当机器人6跟踪完当前目标时，则对接下来缓冲区沿传送带1运行方向的第一个目标进行跟踪。

最小跟踪距离(min_dist)：数据类型是float型，取值范围-50000到10000，默认值-0.1，单位mm，用户可设置，当前该参数没啥用。

最大允许跟踪距离(max_dist)：数据类型是float型，取值范围10.0到100000.0，默认值为1000，单位mm，用户设置用于保证机器人6在可达工作范围内，最大的可跟踪的距离。

传送带1速度(adjustment_speed)：数据类型是float型，取值范围10-500，默认值为200.用户可对其进行设置。

机器人6启动调整参数(correction_vector_ramp_length")：数据类型是int型，取值范围2-100，默认值为20，程序启动时，机器人6用于调整参数，设置越大，机器人6跟踪上目标的时间越长，机器人6运行越平稳，用户可对其进行处理。

机器人6停止调整参数(correction_vector_stop_ramp)：数据类型是int型，取值范围2-100，默认值为50，程序停止时，机器人6用于调整的参数，设置越大，机器人6停止越平稳，用户可对其进行处理。

同步开关信号：数据类型是string型，用户可根据所配置同步开关IO进行选择，默认值为空，用户不配置的话，TPU报错处理，用户可对其进行处理。

编码器2位置信号：数据类型是string型，用户可根据配置的编码器2信号进行选择，默认值为空，用户不配置的话，TPU报错处理，用户可对其进行处理。

等待目标信号：数据类型是string型，用户可根据配置的等待工件4信号进行选择，默认值为空，用户不配置的话，TPU报错处理，用户可对其进行处理。

放弃目标信号：数据类型是string型，用户可根据配置的放弃工件4信号进行选择，默认值为空，用户不配置的话，TPU报错处理，用户可对其进行处理；

待整个系统标定完后，用户即可通过示教器编写传送带1跟踪程序从而实现对传送带1上的工件4进行跟踪。用户在示教传送带1跟踪程序之前也可根据自己需求对传送带1跟踪参数进行配置。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本发明提到的各个部件为现有领域常见技术，本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。