CN105223868A - 一种Delta机器人分布式控制系统及控制方法 - Google Patents

Abstract

本发明公开了一种Delta机器人分布式控制系统，包括有主站和从站，所述主站中的用户程序模块用于执行用户编写的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中的目标点坐标，并发送到Delta反解计算模块，Delta反解计算模块对目标点的坐标进行一次反解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他轴的位置通过通讯控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴，驱动各轴的电子凸轮运行。本发明相比现有技术而言能够降低对主站计算能力的要求、降低对总线带宽需求、降低布线和排查难度以及提高抗干扰能力。

Description

一种Delta机器人分布式控制系统及控制方法

技术领域

本发明涉及机器人控制系统，尤其涉及一种Delta机器人分布式控制系统及控制方法。

背景技术

Delta机器人为一种典型的并联型机器人，由于其基座平台和运动平台都呈三角形而得名。Delta机器人由上下两个三角形平台组成，上方为固定平台，下方为运动平台，固定平台通过3条完全相同的运动链连接运动平台，每条运动链由1个杆件和1个平行四边形闭环机构组成，Delta机器人的运动平台始终与固定平台保持平行，只保留了空间的3个平移自由度，Delta机器人的特点是机构刚度强、承载能力大、精度高、自重负荷比小、动力性能好，是应用最为广泛的并联机器人。

由于Delta机器人结构和动作原理的特殊性，电机的运行位置与机器人末端执行器的位置无法简单直观的进行对应，已知末端执行器的位置，求取各个关节电机的运行位置，称之为反解，反解的计算比较复杂，因此控制机器人动作需要专用的控制器，目前主流的技术有以下两种：

1、基于PC+运动控制卡的方案：逻辑控制和Delta机器人相关的数学计算在PC平台上进行，运动控制卡在每个控制周期将计算的结果通过脉冲或者通讯总线发送给伺服电机驱动器；

2、基于专用运动控制器的方案：在运动控制器中采用比较高端的处理器，完成逻辑控制和Delta机器人相关的数学计算，在每个控制周期将计算的结果通过脉冲或者通讯总线发送给伺服电机驱动器。

然而，在上述现有技术中，存在以下问题：首先，现有技术属于集中控制，控制器需要进行周期性的反解计算，对处理器的计算能力要求很高；其次，使用脉冲周期性通讯，控制器和每一台电机驱动器都要连接脉冲线，在现场很容易受到干扰，出现干扰时排查和解决都比较困难，接线数量也较多；再次，使用高速现场总线周期性通讯，对总线的传输带宽要求比较高，需要选择高性能高成本的总线，实现方案复杂。

近年来，随着半导体技术的飞速发展，嵌入式CPU处理能力越来越强，存储空间越来越大，这使得电机驱动器在完成电机控制相关计算的前提下有富余的资源。但是目前电机驱动器富余的资源多用于增强电机的驱动性能，从系统的角度来看，驱动器CPU性能的提升并没有降低控制器CPU的压力。

发明内容

本发明要解决的技术问题在于，针对现有技术的不足，提供一种降低对主站计算能力的要求、降低对总线的带宽需求、降低布线和排查难度、提高抗干扰能力的Delta机器人分布式控制系统及其控制方法。

为解决上述技术问题，本发明采用如下技术方案。

一种Delta机器人分布式控制系统，所述Delta机器人包括有固定平台，所述固定平台下方设有运动平台，所述固定平台与运动平台通过三组驱动臂连接，所述Delta机器人分布式控制系统包括有主站和从站，其中：所述主站内设有用户程序模块、PTP命令执行模块、Delta反解计算模块、通讯控制模块、虚拟主轴模块、电子凸轮模块和电机控制模块，其中，所述用户程序模块用于执行用户编写的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中的目标点坐标，并发送到Delta反解计算模块，Delta反解计算模块对目标点的坐标进行一次反解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他轴的位置通过通讯控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴，驱动各轴的电子凸轮运行；所述从站内设有电机控制模块、电子凸轮模块和通讯模块，所述通讯模块接收到本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块根据这些参数，计算对应的电子凸轮曲线并保存到凸轮表，接收到虚拟主轴时，根据凸轮表周期性的计算本周期应该发出的周期脉冲量，电机控制模块每个周期接收电子凸轮模块的输出信号，控制电机运行。

优选地，所述主站中：所述用户程序模块供于用户编程，在程序的控制下调用Delta机器人定位指令，定位指令包括直接坐标系下末端执行器的X、Y、Z坐标以及末端执行器的旋转角度等参数，用户程序模块在定位指令生效后将这些参数传递给PTP命令执行模块；所述通讯控制模块用于将参数打包发送到对应的电机驱动器；所述虚拟主轴模块用于在插补运行中周期性的发出数量和速度可控的虚拟主轴。

优选地，所述从站中：通讯控制模块用于接收控制器发送给本机的参数，对通讯数据进行校验，校验通过后将参数发送到电子凸轮模块；电子凸轮模块用于根据设定的曲线类型、目标点位置、当前位置等参数自动计算生产凸轮曲线，运行时根据虚拟主轴周期计算得到每个周期的周期脉冲量，发送到电机控制模块；电机控制模块用于实现电机的控制算法，根据周期脉冲量驱动电机运行，同时反馈电机的运行状态。

优选地，所述电子凸轮模块包括：参数缓冲区，保存通讯控制模块发来的定位控制参数；电子凸轮曲线计算模块，根据参数计算对应的电子凸轮曲线并保存到凸轮表；凸轮表，用来保存电子凸轮曲线；虚拟主轴接收模块，每个周期接收控制器传递过来的虚拟主轴，计算主轴增量；电子凸轮运行控制模块，每个计算周期根据虚拟主轴和凸轮表计算本周期应该发出的周期脉冲量，发送给电机控制模块。

一种Delta机器人分布式控制方法，该控制方法包括：主站将用户编写程序中的Delta机器人定位指令解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。

优选地，控制系统由一个主站和两台从站构成，一个主站和两台从站分别对应Delta机器人的三组驱动臂，主站电机驱动器基于ARM等嵌入式处理器，处理器上运行的用户程序模块用于处理用户编写的程序，用户编写程序可控制Delta机器人的定位运行，程序判断定位指令满足执行条件时，将参数送到PTP命令执行模块，PTP命令执行模块根据Delta反解算法计算出目标位置对应的各关节电机位置。

优选地，所述Delta反解算法为：θi＝-atan(-Bi±sqrt(Bi²-4AiCi)/2Ai)，i＝1，2，3；其中：Px、Py、Pz为目标点直角坐标系下X、Y、Z坐标；L1为主动臂长度；L2为从动臂长度；θ1为关节B1角度；θ2为关节B2角度；θ3为关节B3角度；R为静平台关节外接圆半径；r为动平台关节外接圆半径。

优选地，计算出θ1、θ2、θ3后，Delta反解计算模块根据用户预设的机械参数计算出对应各轴电机的目标位置，之后主站将这些数据发送到本机电子凸轮模块和通讯控制模块，通讯控制模块由通讯的处理程序及相关通讯接口硬件组成，在接收到Delta反解计算模块发送过来的数据后，将数据转换成通讯需要的格式，通过接口硬件发送到对应的电机驱动器，在各从站接收完定位运行参数并生成凸轮曲线后，PTP命令执行模块启动虚拟主轴，开始驱动定位运行。

本发明公开的Delta机器人分布式控制系统中，主站将用户编写程序中的Delta机器人定位指令解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。本发明相比现有技术而言，本发明在Delta机器人的各轴电机驱动器内进行周期计算，充分利用电机驱动器富余的计算资源，对主站计算能力要求不高；同时，本发明在Delta机器人进行定位控制时只需要在启动时通过总线传输一组参数，在插补过程中只需要发出虚拟主轴，不需要周期性的传递数据，降低了对总线的带宽需求；此外，本发明只需要在主站和从站之间连接通讯线及虚拟主轴线，降低了布线和排查的难度，不易受到干扰。

附图说明

图1为本发明Delta机器人分布式控制系统的结构示意图。

图2为电子凸轮模块的的工作流程图。

图3为Delta反解计算过程的流程图。

图4为Delta机器人的结构示意图。

图5为Delta机器人活动坐标系示意图。

具体实施方式

下面结合附图和实施例对本发明作更加详细的描述。

本发明公开了一种Delta机器人分布式控制系统，结合图1至图5所示，所述Delta机器人包括有固定平台100，所述固定平台100下方设有运动平台101，所述固定平台100与运动平台101通过三组驱动臂102连接，所述Delta机器人分布式控制系统包括有主站1和从站2，其中：

所述主站1内设有用户程序模块、PTP命令执行模块、Delta反解计算模块、通讯控制模块、虚拟主轴模块、电子凸轮模块和电机控制模块，其中，所述用户程序模块用于执行用户编写的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中的目标点坐标，并发送到Delta反解计算模块，Delta反解计算模块对目标点的坐标进行一次反解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他轴的位置通过通讯控制模块发送给各个从站2，之后虚拟主轴模块持续发出虚拟主轴，驱动各轴的电子凸轮运行；

所述从站2内设有电机控制模块、电子凸轮模块和通讯模块，所述通讯模块接收到本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块根据这些参数，计算对应的电子凸轮曲线并保存到凸轮表，接收到虚拟主轴时，根据凸轮表周期性的计算本周期应该发出的周期脉冲量，电机控制模块每个周期接收电子凸轮模块的输出信号，控制电机运行。

上述Delta机器人分布式控制系统中，主站将用户编写程序中的Delta机器人定位指令解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。本发明相比现有技术而言，本发明在Delta机器人的各轴电机驱动器内进行周期计算，充分利用电机驱动器富余的计算资源，对主站计算能力要求不高；同时，本发明在Delta机器人进行定位控制时只需要在启动时通过总线传输一组参数，在插补过程中只需要发出虚拟主轴，不需要周期性的传递数据，降低了对总线的带宽需求；此外，本发明只需要在主站和从站之间连接通讯线及虚拟主轴线，降低了布线和排查的难度，不易受到干扰。

作为一种优选方式，所述主站中：所述用户程序模块供于用户编程，在程序的控制下调用Delta机器人定位指令，定位指令包括直接坐标系下末端执行器的X、Y、Z坐标以及末端执行器的旋转角度等参数，用户程序模块在定位指令生效后将这些参数传递给PTP命令执行模块；所述通讯控制模块用于将参数打包发送到对应的电机驱动器；所述虚拟主轴模块用于在插补运行中周期性的发出数量和速度可控的虚拟主轴。

进一步地，所述从站中：通讯控制模块用于接收控制器发送给本机的参数，对通讯数据进行校验，校验通过后将参数发送到电子凸轮模块；电子凸轮模块用于根据设定的曲线类型、目标点位置、当前位置等参数自动计算生产凸轮曲线，运行时根据虚拟主轴周期计算得到每个周期的周期脉冲量，发送到电机控制模块；电机控制模块用于实现电机的控制算法，根据周期脉冲量驱动电机运行，同时反馈电机的运行状态。

其中，所述电子凸轮模块包括：参数缓冲区，保存通讯控制模块发来的定位控制参数；电子凸轮曲线计算模块，根据参数计算对应的电子凸轮曲线并保存到凸轮表；凸轮表，用来保存电子凸轮曲线；虚拟主轴接收模块，每个周期接收控制器传递过来的虚拟主轴，计算主轴增量；电子凸轮运行控制模块，每个计算周期根据虚拟主轴和凸轮表计算本周期应该发出的周期脉冲量，发送给电机控制模块。

基于上述Delta机器人分布式控制系统的控制方法，该控制方法包括：主站将用户编写程序中的Delta机器人定位指令解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。

本实施例中，控制系统由一个主站和两台从站构成，一个主站和两台从站分别对应Delta机器人的三组驱动臂102，主站电机驱动器基于ARM等嵌入式处理器，处理器上运行的用户程序模块用于处理用户编写的程序，用户编写程序可控制Delta机器人的定位运行，程序判断定位指令满足执行条件时，将参数送到PTP命令执行模块，PTP命令执行模块根据Delta反解算法计算出目标位置对应的各关节电机位置。进一步地，所述Delta反解算法为：

θi＝-atan(-Bi±sqrt(Bi²-4AiCi)/2Ai)，i＝1，2，3

其中：

Px、Py、Pz为目标点直角坐标系下X、Y、Z坐标；

L1为主动臂长度；

L2为从动臂长度；

θ1为关节B1角度；

θ2为关节B2角度；

θ3为关节B3角度；

R为静平台关节外接圆半径；

r为动平台关节外接圆半径。

本发明在计算出θ1、θ2、θ3后，Delta反解计算模块根据用户预设的机械参数计算出对应各轴电机的目标位置，之后主站将这些数据发送到本机电子凸轮模块和通讯控制模块，通讯控制模块由通讯的处理程序及相关通讯接口硬件组成，在接收到Delta反解计算模块发送过来的数据后，将数据转换成通讯需要的格式，通过接口硬件发送到对应的电机驱动器，在各从站接收完定位运行参数并生成凸轮曲线后，PTP命令执行模块启动虚拟主轴，开始驱动定位运行。

本发明技术方案中，从站电机驱动器包括通讯控制模块、电子凸轮模块及电机控制模块。其中通讯控制模块接收到通讯帧后，对数据进行校验，根据数据中的地址判断是否是发送给本机的数据。如果是发送给本机的数据且校验正确，则将解析出的数据发送到电子凸轮模块，如果出现了校验错误则要求重发。电机控制模块控制电机跟随电子凸轮模块输出的周期脉冲量，包括位置环、速度环、电流环计算逻辑以及相应的硬件，以及必要的保护及监控功能，启动一个定位运行时，Delta反解计算模块的运行流程图。

本发明公开的Delta机器人分布式控制系统及其控制方法，进行定位运行时，在主站里进行一次目标点的反解计算，然后通过简单的异步通讯总线将反解得到的各个轴对应目标点的位置发送到各个轴，各轴根据当前所处位置和目标位置生成凸轮曲线；主站和从站之间只需要一条虚拟主轴连接，定位运行时，主站发出虚拟主轴，各轴根据虚拟主轴和凸轮表周期性的计算位置增量，驱动电机运行；主轴发送完成时，机器人运行到目标位置。结合以上所述可以看出，本发明相比现有技术而言能够降低对主站计算能力的要求、降低对总线带宽需求、降低布线和排查难度以及提高抗干扰能力。

以上所述只是本发明较佳的实施例，并不用于限制本发明，凡在本发明的技术范围内所做的修改、等同替换或者改进等，均应包含在本发明所保护的范围内。

Claims (8)

1.一种Delta机器人分布式控制系统，其特征在于，所述Delta机器人包括有固定平台，所述固定平台下方设有运动平台，所述固定平台与运动平台通过三组驱动臂连接，所述Delta机器人分布式控制系统包括有主站和从站，其中：

所述主站内设有用户程序模块、PTP命令执行模块、Delta反解计算模块、通讯控制模块、虚拟主轴模块、电子凸轮模块和电机控制模块，其中，所述用户程序模块用于执行用户编写的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中的目标点坐标，并发送到Delta反解计算模块，Delta反解计算模块对目标点的坐标进行一次反解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他轴的位置通过通讯控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴，驱动各轴的电子凸轮运行；

所述从站内设有电机控制模块、电子凸轮模块和通讯模块，所述通讯模块接收到本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块根据这些参数，计算对应的电子凸轮曲线并保存到凸轮表，接收到虚拟主轴时，根据凸轮表周期性的计算本周期应该发出的周期脉冲量，电机控制模块每个周期接收电子凸轮模块的输出信号，控制电机运行。

2.如权利要求1所述的Delta机器人分布式控制系统，其特征在于，所述主站中：所述用户程序模块供于用户编程，在程序的控制下调用Delta机器人定位指令，定位指令包括直接坐标系下末端执行器的X、Y、Z坐标以及末端执行器的旋转角度等参数，用户程序模块在定位指令生效后将这些参数传递给PTP命令执行模块；

所述通讯控制模块用于将参数打包发送到对应的电机驱动器；

所述虚拟主轴模块用于在插补运行中周期性的发出数量和速度可控的虚拟主轴。

3.如权利要求1所述的Delta机器人分布式控制系统，其特征在于，所述从站中：通讯控制模块用于接收控制器发送给本机的参数，对通讯数据进行校验，校验通过后将参数发送到电子凸轮模块；

电子凸轮模块用于根据设定的曲线类型、目标点位置、当前位置等参数自动计算生产凸轮曲线，运行时根据虚拟主轴周期计算得到每个周期的周期脉冲量，发送到电机控制模块；

电机控制模块用于实现电机的控制算法，根据周期脉冲量驱动电机运行，同时反馈电机的运行状态。

4.如权利要求3所述的Delta机器人分布式控制系统，其特征在于，所述电子凸轮模块包括：

参数缓冲区，保存通讯控制模块发来的定位控制参数；

电子凸轮曲线计算模块，根据参数计算对应的电子凸轮曲线并保存到凸轮表；

凸轮表，用来保存电子凸轮曲线；

虚拟主轴接收模块，每个周期接收控制器传递过来的虚拟主轴，计算主轴增量；

电子凸轮运行控制模块，每个计算周期根据虚拟主轴和凸轮表计算本周期应该发出的周期脉冲量，发送给电机控制模块。

5.一种Delta机器人分布式控制方法，其特征在于，该控制方法包括：主站将用户编写程序中的Delta机器人定位指令解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。

6.如权利要求5所述的Delta机器人分布式控制方法，其特征在于，控制系统由一个主站和两台从站构成，一个主站和两台从站分别对应Delta机器人的三组驱动臂，主站电机驱动器基于ARM等嵌入式处理器，处理器上运行的用户程序模块用于处理用户编写的程序，用户编写程序可控制Delta机器人的定位运行，程序判断定位指令满足执行条件时，将参数送到PTP命令执行模块，PTP命令执行模块根据Delta反解算法计算出目标位置对应的各关节电机位置。

7.如权利要求6所述的Delta机器人分布式控制方法，其特征在于，所述Delta反解算法为：

θi＝-atan(-Bi±sqrt(Bi²-4AiCi)/2Ai)，i＝1，2，3

其中：

Px、Py、Pz为目标点直角坐标系下X、Y、Z坐标；

L1为主动臂长度；

L2为从动臂长度；

θ1为关节B1角度；

θ2为关节B2角度；

θ3为关节B3角度；

R为静平台关节外接圆半径；

r为动平台关节外接圆半径。

8.如权利要求7所述的Delta机器人分布式控制方法，其特征在于，计算出θ1、θ2、θ3后，Delta反解计算模块根据用户预设的机械参数计算出对应各轴电机的目标位置，之后主站将这些数据发送到本机电子凸轮模块和通讯控制模块，通讯控制模块由通讯的处理程序及相关通讯接口硬件组成，在接收到Delta反解计算模块发送过来的数据后，将数据转换成通讯需要的格式，通过接口硬件发送到对应的电机驱动器，在各从站接收完定位运行参数并生成凸轮曲线后，PTP命令执行模块启动虚拟主轴，开始驱动定位运行。