CN111267098B - 机器人关节层控制方法及系统 - Google Patents

Abstract

本发明公开了一种基于ROS2框架和EtherCAT总线的机器人关节层控制方法及系统，其中，方法包括以下步骤：生成满足预设条件的三维空间中的轨迹路径点；基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；基于EtherCAT总线并根据轨迹路径点和关节命令轨迹控制机器人的控制器和不同伺服驱动器，以对机器人的目标关节进行控制。该方法可以快速实现机器人底层关节的稳定、实时、高效和平滑控制，为机器人的快速开发提供可靠的技术方法和支持，简单易实现。

Description

机器人关节层控制方法及系统

技术领域

本发明涉及机器人技术领域，特别涉及一种基于ROS2框架和EtherCAT总线的机器人关节层控制方法及系统。

背景技术

相关技术，基于ROS1架构的方案，利用Moveit！插件模块进行笛卡尔层的轨迹规划，生成一系列的关节点，然后将关节点发送到关节轨迹控制器(关节轨迹控制器模块可以存在于控制器，也可以存在于驱动器)，输出位置、速度、力矩到驱动器控制电机的转动。存在最主要的问题是无法解决实时输出轨迹规划命令的问题。

相关技术存在如下缺陷：

(1)基于ROS1的方案无法保证实时性

如Moveit！插件生成一系列的关节角或编写逆向运动学生成关节角后，进行关节的轨迹规划，再利用ros_controller进行三次或者五次插值或编写关节层轨迹规划算法，插值输出时，因为系统无法保证传输数据的实时性，使得规划的轨迹命令不平滑，从而导致伺服电机运行不平顺。

(2)控制器和伺服驱动器的快速配置

传统控制器和伺服驱动器相互连接采用RS485、RS232、Can总线和模拟量等方式。EtherCAT总线相对其他的方式，关节配置更加的高效，连接更加的灵活和传输速率更快，而相关技术中没有将EtherCAT总线和ROS2结合的方案。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的一个目的在于提出一种基于ROS2框架和EtherCAT总线的机器人关节层控制方法，该方法可以快速实现机器人底层关节的稳定、实时、高效和平滑控制，为机器人的快速开发提供可靠的技术方法和支持，简单易实现。

本发明的另一个目的在于提出一种基于ROS2框架和EtherCAT总线的机器人关节层控制系统。

为达到上述目的，本发明一方面实施例提出了一种基于ROS2框架和EtherCAT总线的机器人关节层控制方法，包括以下步骤：生成满足预设条件的三维空间中的轨迹路径点；基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的控制器和不同伺服驱动器，以对所述机器人的目标关节进行控制。

本发明实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制方法，基于ROS2架构和EtherCAT总线的位置/速度/力矩控制方式，从而可以快速实现机器人底层关节的稳定、实时、高效和平滑控制，为机器人的快速开发提供可靠的技术方法和支持，简单易实现。

另外，根据本发明上述实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述生成满足预设条件的三维空间中的轨迹路径点，包括：规划笛卡尔空间的轨迹；将所述轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩；将所述关节数据组通过Action、消息或者服务的形式传输到所述控制器。

进一步地，在本发明的一个实施例中，所述基于ROS2架构生成满足预设条件的关节层的关节命令轨迹，包括：通过订阅、服务器或者Action服务器接收所述关节数据；根据所述关节数据构建高阶多项式轨迹；修改所述高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将所述高阶多项式轨迹转成C代码，以进行封装形成模块节点；输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

进一步地，在本发明的一个实施例中，所述基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的控制器和不同伺服驱动器，包括：接收所述控制信号，并根据所述控制信号控制驱动器与控制电机转动；通过服务数据对象对转矩、电流、速度和位置进行限幅；监测所述电机的当前转动状态，并根据预设频率发布所述当前转动状态。

为达到上述目的，本发明另一方面实施例提出了一种基于ROS2框架和EtherCAT总线的机器人关节层控制系统，包括：机器人笛卡尔层轨迹规划模块，用于生成满足预设条件的三维空间中的轨迹路径点；基于ROS2的关节层轨迹规划模块，用于基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；基于EtherCAT总线的机器人通讯模块，用于基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的控制器和不同伺服驱动器，以对所述机器人的目标关节进行控制。

本发明实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统，基于ROS2架构和EtherCAT总线的位置/速度/力矩控制方式，从而可以快速的实现机器人底层关节的稳定、实时、高效和平滑控制，为机器人的快速开发提供可靠的技术方法和支持，简单易实现。

另外，根据本发明上述实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述机器人笛卡尔层轨迹规划模块进一步用于规划笛卡尔空间的轨迹，并将所述轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩；，将关节数据组通过Action、消息或者服务的形式传输到所述控制器。

进一步地，在本发明的一个实施例中，所述基于ROS2的关节层轨迹规划模块进一步用于通过订阅、服务器或者Action服务器接收所述关节数据，根据所述关节数据构建高阶多项式轨迹；修改所述高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将所述高阶多项式轨迹转成C代码，以进行封装形成模块节点；输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

进一步地，在本发明的一个实施例中，所述基于EtherCAT总线的机器人通讯模块进一步用于接收所述控制信号，并根据所述控制信号控制驱动器与控制电机转动，通过服务数据对象对转矩、电流、速度和位置进行限幅，并监测所述电机的当前转动状态，并根据预设频率发布所述当前转动状态。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制方法的流程图；

图2为根据本发明实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统的结构示意图；

图3为根据本发明一个实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统的结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

该发明主要解决以下难点：

(1)快速构建基于ROS构架的协作机器人控制系统，解决机器人实时控制、分布式控制和多机器人协作控制的问题；

(2)提升机器人关节层轨迹控制位置和速度的平滑性和连续性，并快速的将控制系统和电机伺服驱动系统相互连接。

下面参照附图描述根据本发明实施例提出的基于ROS2框架和EtherCAT总线的机器人关节层控制方法及系统，首先将参照附图描述根据本发明实施例提出的基于ROS2框架和EtherCAT总线的机器人关节层控制方法。

图1是本发明一个实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制方法的流程图。

如图1所示，该基于ROS2框架和EtherCAT总线的机器人关节层控制方法包括以下步骤：

在步骤S101中，生成满足预设条件的三维空间中的轨迹路径点。

可以理解的是，本发明实施例生成三维空间中连续、平滑的轨迹路径点。

进一步地，在本发明的一个实施例中，生成满足预设条件的三维空间中的轨迹路径点，包括：规划笛卡尔空间的轨迹；将轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩；将关节数据组通过Action、消息或者服务的形式传输到控制器。

可以理解的是，本发明实施例可以实现机器人在笛卡尔空间中的运动，如空间中直线或者圆弧轨迹规划，并将其解算到关节空间中。具体的实现步骤如下：

A.进行笛卡尔空间的轨迹规划，如构建空间中直线、圆弧和样条运动曲线，实现空间中连续、复杂、平滑的运行轨迹。

B.编写逆向运动学模块，将空间中的轨迹进行插值，通过逆向运动学求解得到多组关节转角和速度，并通过动力学迭代计算出相应的力矩。

C.将每次计算得到的关节数据组，通过Action、消息或者服务的形式传输到关节层控制器。对于这一部分的运行没有实时性的要求。

在步骤S102中，基于ROS2架构生成满足预设条件的关节层的关节命令轨迹。

可以理解的是，本发明实施例生成关节层连续、高阶、高动态响应和实时的关节命令轨迹。

进一步地，在本发明的一个实施例中，基于ROS2架构生成满足预设条件的关节层的关节命令轨迹，包括：通过订阅、服务器或者Action服务器接收关节数据；根据关节数据构建高阶多项式轨迹；修改高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将高阶多项式轨迹转成C代码，以进行封装形成模块节点；输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

可以理解的是，本发明实施例可以实现机器人关节层的实时轨迹规划。输出高阶、连续、平滑、实时和高动态响应的是命令轨迹。具体实现步骤如下：

A.通过订阅、服务器或者Action服务器接收机器人笛卡尔层轨迹规划模块传输的关节组，包括接收的位置、速度、力矩和间隔时间等信号；

B.构建高阶多项式轨迹规划，如三次和五次多项式、贝塞尔曲线等平滑、高阶、连续的轨迹；

C.就关节层轨迹规划模组可以修改Ros_control模组快速生成单点或者多点连续轨迹，也可以采用Matlab快速编写高阶轨迹转成C代码，进行封装形成模块节点。

D.实时输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块；通过基于ROS2的架构系统可以实时的产生命令信号，传输至驱动器，控制电机转动。

在步骤S103中，基于EtherCAT总线并根据轨迹路径点和关节命令轨迹控制机器人的控制器和不同伺服驱动器，以对机器人的目标关节进行控制。

可以理解的是，本发明实施例可以实现控制器和不同伺服驱动器的快速互联，实现代码有效复用和移植。

进一步地，在本发明的一个实施例中，基于EtherCAT总线并根据轨迹路径点和关节命令轨迹控制机器人的控制器和不同伺服驱动器，包括：接收控制信号，并根据控制信号控制驱动器与控制电机转动；通过服务数据对象对转矩、电流、速度和位置进行限幅；监测电机的当前转动状态，并根据预设频率发布当前转动状态。

可以理解的是，本发明实施例可以快速的将控制器和驱动器相互连接，实现伺服驱动的配置和驱动电机运动。具体实现步骤如下

A.订阅控制器发送的命令信号，如控制器发送的是关节位置信号，可以将驱动器选择为轮廓周期位置模式或者同步周期位置模式；如接收的是速度信号，则驱动器可以切换为轮廓速度模式或者同步周期速度模式；如接收的是转矩信号或者电流信号，则驱动器可以切换为轮廓周期转矩或者同步周期力矩模式；

B.为防止电机突然遭到大的转矩、大的速度或者位置超限，通过SDO(服务数据对象)对转矩、电流、速度和位置进行限幅；

C.实时监测电机的转动状态，并将关节转动状态按照固定的频率发布，如构造joint_states消息用于仿真和控制。

综上，本发明实施例提出的基于ROS2框架和EtherCAT总线的机器人关节层控制方法，解决了ROS1无法实现关节层实时轨迹规划的问题。并在ROS2的架构的基础上，引入EtherCAT总线将控制器和驱动器快速连接，解决了与不同驱动器相互连接代码有效复用、移植和部署的问题。另外，由于ROS2架构设计，使得系统具有分布式、实时、多机协同和跨平台的特点；由于EtherCAT总线的配置使得系统具有高传输速率、高通讯性能、接线更加灵活和部署更加便捷。由于现有技术方案鲜有用于协作机器人的控制，因此，本发明实施例方案的提出为协作机器人的研发提供了重要的方法支持，尤其是力控和动力学前馈，需要实时解算出力矩发送到驱动器。目前采用ROS1加系统实时性补丁如Preempt-RT等方案无法实时输出轨迹规划命令，伺服电机转动就会出现波动，影响轨迹运动的平滑性；而本发明实施例提出的基于ROS2和EtherCAT总线的方案，能够获得好的控制性能，便于后续系统的扩展，并加快了系统构建的速度和提升了控制系统的品质。

其次参照附图描述根据本发明实施例提出的基于ROS2框架和EtherCAT总线的机器人关节层控制。

图2是本发明一个实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统的结构示意图。

如图2所示，该基于ROS2框架和EtherCAT总线的机器人关节层控制系统10包括：机器人笛卡尔层轨迹规划模块100、基于ROS2的关节层轨迹规划模块200和基于EtherCAT总线的机器人通讯模块300。

其中，机器人笛卡尔层轨迹规划模块100用于生成满足预设条件的三维空间中的轨迹路径点；基于ROS2的关节层轨迹规划模块200用于基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；基于EtherCAT总线的机器人通讯模块300用于基于EtherCAT总线并根据轨迹路径点和关节命令轨迹控制机器人的控制器和不同伺服驱动器，以对机器人的目标关节进行控制。本发明实施例的系统10可以快速实现机器人底层关节的稳定、实时、高效和平滑控制，为机器人的快速开发提供可靠的技术方法和支持，简单易实现。

可以理解的是，笛卡尔轨迹规划模块100，用于生成三维空间中连续、平滑的轨迹路径点；基于ROS2的关节层轨迹规划模块200，用于生成关节层连续、高阶、高动态响应和实时的关节命令轨迹；基于EtherCAT总线的机器人通讯模块300，用于控制器和不同伺服驱动器的快速互联，实现代码有效复用和移植。

具体而言，如图3所示，本发明实施例的系统10包含3个主要部分：第1部分是机器人笛卡尔层轨迹规划模块100；第2部分是基于ROS2的关节层轨迹规划模块200；第3部分是基于EtherCAT总线的机器人通讯模块300。其中，机器人笛卡尔层轨迹规划模块100是为了实现机器人在笛卡尔空间中的运动，如空间中直线或者圆弧轨迹规划，并将其解算到关节空间中；基于ROS2的关节层轨迹规划模块200主要是为了实现机器人关节层的实时轨迹规划。输出高阶、连续、平滑、实时和高动态响应的是命令轨迹；基于EtherCAT总线的机器人通讯模块300主要是为了快速的将控制器和驱动器相互连接，实现伺服驱动的配置和驱动电机运动。

进一步地，在本发明的一个实施例中，机器人笛卡尔层轨迹规划模块100进一步用于规划笛卡尔空间的轨迹，并将轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩；，将关节数据组通过Action、消息或者服务的形式传输到控制器。

进一步地，在本发明的一个实施例中，基于ROS2的关节层轨迹规划模块200进一步用于通过订阅、服务器或者Action服务器接收关节数据，根据关节数据构建高阶多项式轨迹；修改高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将高阶多项式轨迹转成C代码，以进行封装形成模块节点；输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

进一步地，在本发明的一个实施例中，基于EtherCAT总线的机器人通讯模块300进一步用于接收控制信号，并根据控制信号控制驱动器与控制电机转动，通过服务数据对象对转矩、电流、速度和位置进行限幅，并监测电机的当前转动状态，并根据预设频率发布当前转动状态。

需要说明的是，基于上述三个模块能够实现快速的构建机器人控制系统，尤其针对于关节层控制系统构建。本发明实施例提出的方法不仅仅适用于机器人位置控制，也适用于速度控制和力矩控制，同时也为多机器人系统互联提供了技术方案。本发明实施例研究借助ROS2解决了节点实时运算的问题，依赖EtherCAT总线解决了控制和伺服高效传输和代码复用的问题。

另外，前述对基于ROS2框架和EtherCAT总线的机器人关节层控制方法实施例的解释说明也适用于该实施例的基于ROS2框架和EtherCAT总线的机器人关节层控制系统，此处不再赘述。

根据本发明实施例提出的基于ROS2框架和EtherCAT总线的机器人关节层控制系统，解决了ROS1无法实现关节层实时轨迹规划的问题。并在ROS2的架构的基础上，引入EtherCAT总线将控制器和驱动器快速连接，解决了与不同驱动器相互连接代码有效复用、移植和部署的问题。另外，由于ROS2架构设计，使得系统具有分布式、实时、多机协同和跨平台的特点；由于EtherCAT总线的配置使得系统具有高传输速率、高通讯性能、接线更加灵活和部署更加便捷。由于现有技术方案鲜有用于协作机器人的控制，因此，本发明实施例方案的提出为协作机器人的研发提供了重要的方法支持，尤其是力控和动力学前馈，需要实时解算出力矩发送到驱动器。目前采用ROS1加系统实时性补丁如Preempt-RT等方案无法实时输出轨迹规划命令，伺服电机转动就会出现波动，影响轨迹运动的平滑性；而本发明实施例提出的基于ROS2和EtherCAT总线的方案，能够获得好的控制性能，便于后续系统的扩展，并加快了系统构建的速度和提升了控制系统的品质。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (4)

1.一种基于ROS2框架和EtherCAT总线的机器人关节层控制方法，其特征在于，所述方法可以实现力控和动力学前馈效果，包括以下步骤：

规划笛卡尔空间的轨迹；

将所述轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩；

将所述关节数据组通过Action、消息或者服务的形式传输到控制器；

基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；以及

基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的所述控制器和不同伺服驱动器，以对所述机器人的目标关节进行控制；

其中，所述基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的控制器和不同伺服驱动器，包括：

订阅所述控制器发送的控制信号，并根据所述控制信号控制驱动器与控制电机转动；其中，所述控制信号包括关节位置信号、速度信号、转矩信号或者电流信号；

通过服务数据对象对转矩、电流、速度和位置进行限幅；

实时监测所述电机的当前转动状态，并根据预设频率通过joint_states消息发布所述当前转动状态。

2.根据权利要求1所述的方法，其特征在于，所述基于ROS2架构生成满足预设条件的关节层的关节命令轨迹，包括：

通过订阅、服务器或者Action服务器接收所述关节数据；

根据所述关节数据构建高阶多项式轨迹；

修改所述高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将所述高阶多项式轨迹转成C代码，以进行封装形成模块节点；

输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

3.一种基于ROS2框架和EtherCAT总线的机器人关节层控制系统，其特征在于，所述控制系统可以实现力控和动力学前馈效果，包括：

机器人笛卡尔层轨迹规划模块，用于规划笛卡尔空间的轨迹，并将所述轨迹进行插值，并通过逆向运动学求解得到多组关节数据，以及通过动力学迭代计算出相应的力矩，并将关节数据组通过Action、消息或者服务的形式传输到控制器；

基于ROS2的关节层轨迹规划模块，用于基于ROS2架构生成满足预设条件的关节层的关节命令轨迹；以及

基于EtherCAT总线的机器人通讯模块，用于基于EtherCAT总线并根据所述轨迹路径点和所述关节命令轨迹控制机器人的所述控制器和不同伺服驱动器，以对所述机器人的目标关节进行控制；

其中，所述基于EtherCAT总线的机器人通讯模块进一步用于订阅所述控制器发送的控制信号，并根据所述控制信号控制驱动器与控制电机转动，通过服务数据对象对转矩、电流、速度和位置进行限幅，并实时监测所述电机的当前转动状态，并根据预设频率通过joint_states消息发布所述当前转动状态；其中，所述控制信号包括关节位置信号、速度信号、转矩信号或者电流信号。

4.根据权利要求3 所述的系统，其特征在于，所述基于ROS2的关节层轨迹规划模块进一步用于通过订阅、服务器或者Action服务器接收所述关节数据，根据所述关节数据构建高阶多项式轨迹；修改所述高阶多项式轨迹，生成单点或者多点连续轨迹，或者，采用Matlab编写将所述高阶多项式轨迹转成C代码，以进行封装形成模块节点；输出关节规划位置、速度或力矩信号到基于EtherCAT总线的机器人通讯模块，以生成控制信号。

Images