一种六轴关节机器人分布式控制系统及方法
【专利摘要】本发明公开了一种六轴关节机器人分布式控制方法，该控制方法包括：主站将用户程序中的六轴关节机器人定位指令发送到机器人反解计算模块，机器人反解计算模块对目标点的位置进行反解计算，解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下进行周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。本发明能够降低对主站计算能力的要求、降低对总线的带宽需求、降低布线和排查难度、提高抗干扰能力。
【专利说明】
一种六轴关节机器人分布式控制系统及方法
技术领域
[0001] 本发明涉及机器人控制技术领域，尤其涉及一种六轴关节机器人分布式控制系统 及方法。
【背景技术】
[0002] 六轴关节机器人是一种典型的串联型工业机器人，是当今工业领域中最常见的工 业机器人的形态之一，适合用于诸多工业领域的自动化作业，例如，自动装配、自动喷涂、搬 运、码垛、焊接等作业。六轴关节机器人的反解，指根据给定的目标位姿及机械参数，计算出 各关节轴的角度，从而可以控制各关节电机运动到达目标位姿。反解的计算比较复杂，因此 控制机器人动作需要专用的控制器，目前主流的技术有以下两种：
[0003] 1、基于PC+运动控制卡的方案:逻辑控制和机器人相关的数学计算在PC平台上进 行，运动控制卡在每个控制周期将计算的结果通过脉冲或者通讯总线发送给伺服电机驱动 器；
[0004] 2、基于专用运动控制器的方案:在运动控制器中采用比较高端的处理器，完成逻 辑控制和机器人相关的数学计算，在每个控制周期将计算的结果通过脉冲或者通讯总线发 送给伺服电机驱动器。
[0005] 然而，在上述现有技术中，存在以下问题:首先，上述技术属于集中控制，控制器需 要进行周期性的反解计算，对处理器的计算能力要求很高;其次，使用脉冲周期性通讯，控 制器和每一台电机驱动器都要连接脉冲线，在现场很容易受到干扰，出现干扰时排查和解 决都比较困难，接线数量也较多；此外，使用高速现场总线周期性通讯，对总线的传输带宽 要求比较高，需要选择高性能高成本的总线，实现方案复杂。
[0006] 近年来，随着半导体技术的飞速发展，嵌入式CPU处理能力越来越强，存储空间越 来越大，这使得电机驱动器在完成电机控制相关计算的前提下有富余的资源。但是目前电 机驱动器富余的资源多用于增强电机的驱动性能，从系统的角度来看，驱动器CPU性能的提 升并没有降低控制器CHJ的压力。
【发明内容】
[0007] 本发明要解决的技术问题在于，针对现有技术的不足，提供一种六轴关节机器人 分布式控制系统及方法，用以降低对主站计算能力的要求、降低对总线的带宽需求、降低布 线和排查难度、提高抗干扰能力。
[0008] 为解决上述技术问题，本发明采用如下技术方案。
[0009] -种六轴关节机器人分布式控制系统，其包括有六轴关节机器人、一个主站及五 个从站，所述主站和从站分别对应驱动六轴关节机器人的六个关节，其中：所述主站内预置 有用户程序模块、PTP命令执行模块、机器人反解计算模块、通讯控制模块、虚拟主轴模块、 电子凸轮模块及电机控制模块，所述用户程序模块用于执行用户编写的逻辑程序，该逻辑 程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中的目标点坐标，发送 到机器人反解计算模块，机器人反解计算模块对目标点的位姿进行反解计算，得出目标点 对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他轴的位置通过通讯 控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴，驱动各轴的电子凸轮运 行;所述从站内预置有电机控制模块、电子凸轮模块和通讯控制模块，通讯控制模块接收到 本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块根据这些参数，计算对应的电 子凸轮曲线并保存到凸轮表，当接收到虚拟主轴时，根据凸轮表周期性的计算本周期应该 发出的周期脉冲量，所述电机控制模块接收电子凸轮模块每个周期的输出信号，并根据该 信号控制电机运行。
[0010] 优选地，所述主站中：所述用户程序模块由用户编程，在程序控制下调用六轴关节 机器人定位指令，所述定位指令包括坐标系下末端执行器的X、Y、Z坐标以及末端执行器的 旋转角度等参数，用户程序模块在定位指令生效后将这些参数传递给PTP命令执行模块;所 述通讯控制模块用于将参数打包发送到对应的电机驱动器;所述虚拟主轴模块用于在插补 运行中周期性的发出数量和速度可控的虚拟主轴。
[0011] 优选地，所述从站中：通讯控制模块用于接收主站发送给本机的参数，对通讯数据 进行校验，校验通过后将参数发送到电子凸轮模块;电子凸轮模块根据设定的曲线类型、目 标点位姿、当前位姿等参数自动计算生成凸轮曲线，运行时根据虚拟主轴周期计算得到每 个周期的周期脉冲量，发送到电机控制模块；电机控制模块用于根据周期脉冲量驱动电机 运行，同时反馈电机的运行状态。
[0012] 优选地，所述电子凸轮模块包括:参数缓冲区，用于保存通讯控制模块发来的定位 控制参数；电子凸轮曲线计算模块，用于根据参数计算对应的电子凸轮曲线保存到凸轮表； 凸轮表，用于保存电子凸轮曲线;虚拟主轴接收模块，用于在每个计算周期内接收控制器传 递过来的虚拟主轴，并计算主轴增量;电子凸轮运行控制模块，用于在每个计算周期内根据 虚拟主轴和凸轮表计算本周期应该发出的周期脉冲量，并发送给电机控制模块。
[0013] -种六轴关节机器人分布式控制方法，该控制方法包括:主站将用户程序中的六 轴关节机器人定位指令发送到机器人反解计算模块，机器人反解计算模块对目标点的位置 进行反解计算，解析成对应的参数，将参数通过总线发送到从站的电机驱动器中，电机驱动 器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮 模块在虚拟主轴的驱动下进行周期性的计算，每个周期计算出周期脉冲量，电机控制模块 根据周期脉冲量驱动电机运行。
[0014] 本发明公开的六轴关节机器人分布式控制系统及方法，其相比现有技术而言的有 益效果在于，首先，本发明在六轴关节机器人的各轴电机驱动器内进行周期计算，充分利用 电机驱动器富余的计算资源，对主站计算能力要求不高;其次，本发明在六轴关节机器人进 行定位控制时只需要在启动时通过总线传输一组参数，在插补过程中只需要发出虚拟主 轴，不需要周期性的传递数据，降低了对总线的带宽需求;此外，本发明只需要在主站和从 站之间连接通讯线路及虚拟主轴线路，降低了布线和排查的难度，不易受到干扰。
【附图说明】
[0015] 图1为本发明六轴关节机器人分布式控制方法中反解过程的流程图。
[0016] 图2为本发明六轴关节机器人分布式控制系统的组成框图。
[0017] 图3为电子凸轮模块的内部框图。
[0018] 图4为六轴关节机器人的关节分布示意图；
[0019] 图5为图4中六轴关节机器人各关节的坐标示意图。
【具体实施方式】
[0020] 下面结合附图和实施例对本发明作更加详细的描述。
[0021] 本发明公开了一种六轴关节机器人分布式控制系统，结合图1至图5所示，其包括 有六轴关节机器人、一个主站及五个从站，所述主站和从站分别对应驱动六轴关节机器人 的六个关节，其中：
[0022]所述主站内预置有用户程序模块、PTP命令执行模块、机器人反解计算模块、通讯 控制模块、虚拟主轴模块、电子凸轮模块及电机控制模块，所述用户程序模块用于执行用户 编写的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指 令中的目标点坐标，发送到机器人反解计算模块，机器人反解计算模块对目标点的位姿进 行反解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块， 其他轴的位置通过通讯控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴， 驱动各轴的电子凸轮运行；
[0023]所述从站内预置有与所述主站功能相同的电机控制模块、电子凸轮模块和通讯控 制模块，通讯控制模块接收到本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块 根据这些参数，计算对应的电子凸轮曲线并保存到凸轮表，当接收到虚拟主轴时，根据凸轮 表周期性的计算本周期应该发出的周期脉冲量，所述电机控制模块接收电子凸轮模块每个 周期的输出信号，并根据该信号控制电机运行。
[0024]作为一种优选方式，所述主站中：
[0025]所述用户程序模块由用户编程，在程序控制下调用六轴关节机器人定位指令，所 述定位指令包括坐标系下末端执行器的X、Y、Z坐标以及末端执行器的旋转角度等参数，用 户程序模块在定位指令生效后将这些参数传递给PTP命令执行模块;所述通讯控制模块用 于将参数打包发送到对应的电机驱动器;所述虚拟主轴模块用于在插补运行中周期性的发 出数量和速度可控的虚拟主轴。
[0026]作为一种优选方式，所述从站中：
[0027] 通讯控制模块用于接收主站发送给本机的参数，对通讯数据进行校验，校验通过 后将参数发送到电子凸轮模块;电子凸轮模块根据设定的曲线类型、目标点位姿、当前位姿 等参数自动计算生成凸轮曲线，运行时根据虚拟主轴周期计算得到每个周期的周期脉冲 量，发送到电机控制模块;电机控制模块用于根据周期脉冲量驱动电机运行，同时反馈电机 的运行状态。
[0028] 本实施例中，所述电子凸轮模块包括：
[0029] 参数缓冲区，用于保存通讯控制模块发来的定位控制参数；
[0030] 电子凸轮曲线计算模块，用于根据参数计算对应的电子凸轮曲线保存到凸轮表；
[0031] 凸轮表，用于保存电子凸轮曲线；
[0032] 虚拟主轴接收模块，用于在每个计算周期内接收控制器传递过来的虚拟主轴，并 计算主轴增量；
[0033] 电子凸轮运行控制模块，用于在每个计算周期内根据虚拟主轴和凸轮表计算本周 期应该发出的周期脉冲量，并发送给电机控制模块。
[0034] 基于上述六轴关节机器人分布式控制系统，本发明还公开一种六轴关节机器人分 布式控制方法，该控制方法包括:主站将用户程序中的六轴关节机器人定位指令发送到机 器人反解计算模块，机器人反解计算模块对目标点的位置进行反解计算，解析成对应的参 数，将参数通过总线发送到从站的电机驱动器中，电机驱动器接收到有效的数据后将其中 的定位运行参数解析出来，发送到电子凸轮模块，电子凸轮模块在虚拟主轴的驱动下进行 周期性的计算，每个周期计算出周期脉冲量，电机控制模块根据周期脉冲量驱动电机运行。
[0035] 进一步地，系统中的一个主站和五个从站分别对应六轴关节机器人的六个关节， 主站电机驱动器可以基于ARM等嵌入式处理器，处理器上运行的用户程序模块可以处理用 户编写的程序，用户编写程序控制六轴关节机器人的定位运行。程序判断定位指令满足执 行条件时，将参数送到PTP命令执行模块，PTP命令执行模块根据六轴关节机器人反解算法 计算出目标位置对应的各关节电机位置。所述机器人反解计算模块执行的反解算法如下： [0036]给定期望位姿： % I A. n o a p
[0037] y ' 7 Fy % % Pz 0 0 0 1
[0038] 反解计算各关节角度：
[0039] 9i = atan(py/px)
[0040] 0234 = atan(az/ (Ciax+Siay))
[0045] 04= 0234_02_93
[0046] 0 5 = atan (C234 (Ciax+Siay) +S234az) / (Siax-Ciay)
[0047] 9 6 = atan (-S234 (Cinx+Siriy) +C234nz) / (-S234 (Ci〇x+Si〇y) +C234〇z)
[0048] 其中：01为关节1角度，02为关节2角度，03为关节3角度，01为关节4角度，02为关节 5角度，03为关节6角度。计算出01、02、03、04、05、06后，机器人反解计算模块根据用户预设 的机械参数，如减速机减速比、臂长等，计算出对应各轴电机的目标位置，之后主站将这些 数据发送到本机电子凸轮模块和通讯控制模块，通讯控制模块由通讯处理程序及通讯接口 硬件组成，在接收到六轴多关节反解计算模块发送过来的数据后，将数据转换成通讯所需 的格式，通过接口硬件发送到对应的电机驱动器，当各从站接收完定位运行参数并生成凸 轮曲线后，PTP命令执行模块启动虚拟主轴，开始驱动定位运行。
[0049] 从站电机驱动器包括通讯控制模块、电子凸轮模块及电机控制模块。其中通讯控 制模块接收到通讯帧后，对数据进行校验，根据数据中的地址判断是否是发送给本机的数 据。如果是发送给本机的数据且校验正确，则将解析出的数据发送到电子凸轮模块。如果出 现了校验错误则要求重发。电机控制模块控制电机跟随电子凸轮模块输出的周期脉冲量， 包括位置环、速度环、电流环计算逻辑以及相应的硬件，以及必要的保护及监控功能。
[0050] 本实施例中，电子凸轮模块包括参数缓冲区、电子凸轮曲线计算模块、凸轮表、虚 拟主轴接收模块及电子凸轮运行控制模块。参数缓冲区用来保存通讯控制模块发送过来的 相关参数，典型的，包括曲线类型、轨迹参控制参数、目标点位姿等;定位启动时，电子凸轮 曲线计算模块根据参数缓冲区内容计算相应的电子凸轮曲线，保存到凸轮表中，之后虚拟 主轴接收模块开始接收虚拟主轴，电子凸轮运行控制模块根据虚拟主轴的位置和凸轮表计 算得到周期脉冲量。
[0051] 本发明公开的六轴关节机器人分布式控制系统及方法，其相比现有技术而言的有 益效果在于，首先，本发明在六轴关节机器人的各轴电机驱动器内进行周期计算，充分利用 电机驱动器富余的计算资源，对主站计算能力要求不高;其次，本发明在六轴关节机器人进 行定位控制时只需要在启动时通过总线传输一组参数，在插补过程中只需要发出虚拟主 轴，不需要周期性的传递数据，降低了对总线的带宽需求;此外，本发明只需要在主站和从 站之间连接通讯线路及虚拟主轴线路，降低了布线和排查的难度，不易受到干扰。
[0052]以上所述只是本发明较佳的实施例，并不用于限制本发明，凡在本发明的技术范 围内所做的修改、等同替换或者改进等，均应包含在本发明所保护的范围内。
【主权项】
1. 一种六轴关节机器人分布式控制系统，其特征在于，包括有六轴关节机器人、一个主 站及五个从站，所述主站和从站分别对应驱动六轴关节机器人的六个关节，其中： 所述主站内预置有用户程序模块、PTP命令执行模块、机器人反解计算模块、通讯控制 模块、虚拟主轴模块、电子凸轮模块及电机控制模块，所述用户程序模块用于执行用户编写 的逻辑程序，该逻辑程序包含定位指令，在执行定位指令时，PTP命令执行模块获取指令中 的目标点坐标，发送到机器人反解计算模块，机器人反解计算模块对目标点的位姿进行反 解计算，得出目标点对应的各个轴的位置，本轴位置直接发送给本轴的电子凸轮模块，其他 轴的位置通过通讯控制模块发送给各个从站，之后虚拟主轴模块持续发出虚拟主轴，驱动 各轴的电子凸轮运行； 所述从站内预置有电机控制模块、电子凸轮模块和通讯控制模块，通讯控制模块接收 到本轴目标位置后，将参数传递给电子凸轮模块，电子凸轮模块根据这些参数，计算对应的 电子凸轮曲线并保存到凸轮表，当接收到虚拟主轴时，根据凸轮表周期性的计算本周期应 该发出的周期脉冲量，所述电机控制模块接收电子凸轮模块每个周期的输出信号，并根据 该信号控制电机运行。2. 如权利要求1所述的六轴关节机器人分布式控制系统，其特征在于，所述主站中： 所述用户程序模块由用户编程，并在程序控制下调用六轴关节机器人定位指令，所述 定位指令包括坐标系下末端执行器的x、Y、z坐标以及末端执行器的旋转角度参数，用户程 序模块在定位指令生效后将这些参数传递给ΡΤΡ命令执行模块； 所述通讯控制模块用于将参数打包发送到对应的电机驱动器； 所述虚拟主轴模块用于在插补运行中周期性的发出数量和速度可控的虚拟主轴。3. 如权利要求1所述的六轴关节机器人分布式控制系统，其特征在于，所述从站中： 通讯控制模块用于接收主站发送给本机的参数，以及对通讯数据进行校验，校验通过 后将参数发送到电子凸轮模块； 电子凸轮模块根据设定的曲线类型、目标点位姿、当前位姿等参数自动计算生成凸轮 曲线，运行时根据虚拟主轴周期计算得出每个周期的周期脉冲量，发送到电机控制模块； 电机控制模块用于根据周期脉冲量驱动电机运行，同时反馈电机的运行状态。4. 如权利要求1所述的六轴关节机器人分布式控制系统，其特征在于，所述电子凸轮模 块包括： 参数缓冲区，用于保存通讯控制模块发来的定位控制参数； 电子凸轮曲线计算模块，用于根据参数计算对应的电子凸轮曲线保存到凸轮表； 凸轮表，用于保存电子凸轮曲线； 虚拟主轴接收模块，用于在每个计算周期内接收控制器传递过来的虚拟主轴，并计算 主轴增量； 电子凸轮运行控制模块，用于在每个计算周期内根据虚拟主轴和凸轮表计算本周期应 该发出的周期脉冲量，并发送给电机控制模块。5. -种六轴关节机器人分布式控制方法，其特征在于，该控制方法包括:主站将用户程 序中的六轴关节机器人定位指令发送到机器人反解计算模块，机器人反解计算模块对目标 点的位置进行反解计算，解析成对应的参数，将参数通过总线发送到从站的电机驱动器中， 电机驱动器接收到有效的数据后将其中的定位运行参数解析出来，发送到电子凸轮模块， 电子凸轮模块在虚拟主轴的驱动下进行周期性的计算，每个周期计算出周期脉冲量，电机 控制模块根据周期脉冲量驱动电机运行。6. 如权利要求5所述的六轴关节机器人分布式控制方法，其特征在于，所述机器人反解 计算模块执行的反解算法如下： 给定期望位姿： 反解计算各关节角度：其中：Θ1为关节1角度，Θ2为关节2角度，Θ3为关节3角度，Θ1为关节4角度，Θ2为关节5角 度，Θ3为关节6角度。7. 如权利要求6所述的六轴关节机器人分布式控制方法，其特征在于，计算出θ 1、Θ2、Θ 3、04、05、06后，机器人反解计算模块根据用户预设的机械参数计算出对应各轴电机的目标 位置，之后主站将这些数据发送到本机电子凸轮模块和通讯控制模块，通讯控制模块由通 讯处理程序及通讯接口硬件组成，在接收到六轴多关节反解计算模块发送过来的数据后， 将数据转换成通讯所需的格式，通过接口硬件发送到对应的电机驱动器，当各从站接收完 定位运行参数并生成凸轮曲线后，PTP命令执行模块启动虚拟主轴，开始驱动定位运行。
【文档编号】G05B19/414GK105929797SQ201610370888
【公开日】2016年9月7日
【申请日】2016年5月30日
【发明人】陈阳, 高大林
【申请人】深圳市微秒控制技术有限公司