CN117706989A - 基于EtherCAT的工业机器人通用控制及调试系统 - Google Patents

Abstract

基于EtherCAT的工业机器人通用控制及调试系统，属于机器人控制技术领域。为了解决现有的工业机器人控制系统存在的兼容性差，软硬件耦合性强，控制与调试能力较弱的问题。本发明系统中，EtherCAT主站用于引导EtherCAT从站进入OP状态，主站通过PDO数据报与SDO数据报的形式与工业机器人驱动器硬件通信；主站的部分线程具有实时性；主站使用Socket通信API在机器人规划与控制模块、远程三维模拟模块、调试模块和工业机器人本体间传递数据；Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信；实时部分下的主站通过实时网络RTnet下发PDO和SDO数据报至配套的工业机器人驱动器，从而控制机器人关节的位置、速度和力矩目标值。

Description

基于EtherCAT的工业机器人通用控制及调试系统

技术领域

本发明属于机器人控制技术领域，涉及一种基于EtherCAT工业互联网总线的工业机器人控制系统。

背景技术

工业机器人在装配、喷涂、焊接等工业场景的应用拓展，对其定位稳定时间、各速度段下的轨迹精度和运动稳定性提出了更高的要求，需要开发一套算力高、健壮性好、实时性强、兼容性优、数据带宽大的机器人控制系统以承载复杂的规划控制算法。

EtherCAT(Ethernet for Control Automation Technology)是一种基于以太网的工业自动化领域实时通信技术，由倍福公司提出。相较于其他工业总线，EtherCAT采用主-从模式工作，具有实时性能好(通讯周期短)、灵活性高(拓扑形式多样)、设备简单(可使用通用网卡，无需引入额外硬件)、可靠性高(支持冗余)的特点，在工业互联网领域应用广泛。

目前基于EtherCAT总线的工业机器人控制系统通常采用驱控一体架构，使用专用的示教器进行运动规划，使用厂家提供的专有软件进行调试，并且仅对自身产品的控制器和机器人进行适配，具有以下缺点：

1.通用性差。EtherCAT主站向上和向下提供的接口中掺杂了大量专有协议和厂商自定义内容，通常仅能用于特定厂家的特定产品，没能根据通用规范开发通用的控制系统，兼容性糟糕。

2.控制系统架构落后，功能残缺。现有的机器人示教器功能以离线规划为主，缺乏对在线规划器的支持，且规划器深度绑定驱动器，无法随意更换。

3.控制器低速精度和高速运动性能差。虽然现有的控制器支持设置PID分组，但分组数量少，无法根据现实工况和任务需求调整PID参数；并且当前工业机器人控制领域常使用PI控制器，缺少微分环节，影响系统动态性能。

4.调试软件功能弱、兼容性差。国产工业机器人调试软件常为自家产品的专用软件，具体的功能支持与机器人驱动器紧密耦合；调试界面主要为字符监控窗口，缺乏直观的可视化调试能力。

发明内容

本发明为了解决现有的工业机器人控制系统存在的兼容性差，软硬件耦合性强，控制与调试能力较弱的问题。

基于EtherCAT的工业机器人通用控制及调试系统，包括EtherCAT主站、Socket通信API、实时网络RTnet、机器人规划与控制模块、远程三维模拟模块与调试模块；远程三维模拟模块采用C/S架构，其由本地计算机下的远程三维模拟模块服务器端与远程计算机下的远程三维模拟模块显示终端组成；

针对工业机器人控制系统运行环境，在Linux发行版中引入硬实时调度器，实现进程的实时调度；构建实时环境是通过引入Xenomai实时操作系统实现的：Xenomai在Linux内核的基础上，引入Cobalt内核，将单内核模式改为双内核模式，常规Linux内核负责非实时任务和实时任务的初始化，Xenomai下的Cobalt内核负责实时任务的运行；每当发出一个新的实时进程创建请求，先由Linux内核创建进程并分配内存空间，归类为非实时进程；随后切换至Cobalt内核下实时运行，成为实时进程；在双内核模式下，Linux内核优先级低于Cobalt内核，接受实时调度；同时在Cobalt内核中加入I-pipe补丁，I-pipe补丁位于计算机硬件与Cobalt内核之间，构建一层硬件抽象层，Xenomai系统运行时，由I-pipe补丁统一处理中断请求；通过对Linux启动引导程序的设置实现内核级的实时域与非实时域间的隔离；通过Linux的grub引导程序，将多处理器计算机系统逻辑核心分为第一处理器集和第二处理器集两部分，在操作系统的引导阶段将不同的系统引导至不同的逻辑CPU核上运行，第一处理器集用于运行Xenomai系统，承载实时任务，第二处理器集用于运行Linux发行版，承载非实时任务；

EtherCAT主站用于引导EtherCAT从站进入Operation Mode状态，即OP状态，EtherCAT从站即工业机器人驱动器，EtherCAT主站通过PDO数据报与SDO数据报的形式与工业机器人驱动器硬件通信；EtherCAT主站的部分线程具有实时性；EtherCAT主站使用Socket通信API在机器人规划与控制模块、远程三维模拟模块、调试模块和工业机器人本体间传递数据；Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信；在运行远程三维模拟模块显示终端的远程计算机和运行远程三维模拟模块服务器端的本地计算机之间实现单工的网络通信；实时部分下的EtherCAT主站通过实时网络RTnet下发EtherCAT的PDO和SDO数据报至配套的工业机器人驱动器，从而控制机器人关节的位置、速度和力矩目标值；

机器人规划与控制模块用于进行机器人的轨迹规划，并通过自适应PID控制器对机器人进行控制；机器人规划与控制模块具有实时性；

远程三维模拟模块远程显示机器人运行状态，通过模拟计算各关节力矩；远程三维模拟模块不具有实时性；

调试模块本地显示机器人关节空间下的位置、速度、力矩目标值与实际值；调试模块不具有实时性；

机器人规划与控制模块、EtherCAT主站的实时部分运行于Xenomai用户态，RTnet运行于Xenomai内核态，两者接受实时调度；远程三维模拟模块、EtherCAT主站的非实时部分与调试模块属于Linux下的非实时进程，运行于Linux用户态。

进一步地，调试模块为OpenGL调试模块。

进一步地，EtherCAT主站的网络架构实时化方式如下：

通过Cobalt内核下的Rtnet搭建实时网络，通过Cobalt内核提供的实时驱动模型RTDM对EtherCAT主站进行实时化改造，将Cobalt内核系统调用转换到实时域上，EtherCAT主站程序能够自由地调用实时网卡驱动收发EtherCAT数据包。

进一步地，EtherCAT主站程序分为初始化与终端信息打印线程、实时通信线程、EtherCAT状态机监测线程和键盘事件监测线程；

初始化与终端信息打印线程为主线程，主线程在启动时负责初始化部分的工作，在初始化完成后负责接收实时实时通信线程传递的信息，并将其打印到远程三维模拟模块服务器端或远程三维模拟模块显示终端；在初始化与终端信息打印线程的初始化过程中，EtherCAT主站会在主线程启动前通过shell脚本自动开启RTnet实时网络用以承载EtherCAT通信，随后启动EtherCAT主站；主线程读取EtherCAT从站信息，并根据从站驱动器设备名以字符串匹配的模式在数据库中检索对应的XML文件用以查找对应的ESI文件，根据CiA402标准化子协议和制造商特定子协议中的伪字节相关条目初始化PDO映射，引导EtherCAT从站进入OP状态，EtherCAT操作模式切换至CST模式，由此完成EtherCAT主站的初始化，并以有名信号量的方式发送信号以引导实时通信线程运行；随后主线程阻塞式地接收并解码主站实时通信线程发送的数据，并在终端打印输出；如果主线程收到了其他线程发出的线程结束信号，那么主线程负责将EtherCAT总线状态机恢复至Init状态，并结束主线程；

实时通信线程通过Xenomai提供的实时域内数据报协议IDDP实现同机器人规划与控制模块的点对点通信，通过Xenomai提供的跨域数据报协议XDDP实现与调试模块和主站初始化与终端信息打印线程的点对点通信，通过EtherCAT总线实现同各个EtherCAT从站的通信；

EtherCAT状态机监测线程负责定时监测EtherCAT总线状态机，并在从站报错时负责恢复将其至正常状态；

键盘事件监测线程负责监听用户的按键事件，并实现程序的退出和急停状态的退出功能。

进一步地，在EtherCAT主站初始化的过程中，通过扫描网络上的EtherCAT从站的方式获取当前的EtherCAT从站数量，通过读取从站XML配置文件的方式获取从站与关节数量的映射关系；随后以服务数据对象SDO写入的方式根据发送PDO与接收PDO信息进行PDO映射；各关节的PDO数据报存储于栈中，通过从站对应的内存地址加上对应数量减一的PDO数据报长度，得到指向该从站对应关节PDO数据报的指针地址；EtherCAT主站运行时，只需要向对应于机器人关节总数的指针中填入对应的PDO数据报数据，不关注EtherCAT从站的数量。

进一步地，实时通信线程的处理过程如下：

实时通信线程会在线程创建后首先执行CPU亲和力绑定与实时线程调度方式设置，以此提高此线程的优先级，同时最大化利用CPU核心和缓存资源；随后，实时通信线程将XDDP与IDDP通信初始化为监听对应的地址；之后，实时通信线程会阻塞式地等待主线程发送的信号量，并在信号量到达后进入主循环；实时通信线程在主循环中首先读取CLOCK_MONOTONIC高精度定时器的时间信息，并通过clock_nanosleep函数以纳秒精度休眠至下一个EtherCAT通讯周期；在休眠完成后，实时通信线程读取TxPDO数据报，根据CiA402协议解码出状态字对应的状态，随后根据此状态和EtherCAT总线状态机的状态切换过程编码对应的控制字，随后向与之通信的各个线程的地址发送IDDP或XDDP数据包；并将接收到的IDDP数据包进行解码，随后写入RxPDO数据包中，最后通过EtherCAT总线发送；IDDP数据包的收发均采用非阻塞的方式进行。

进一步地，EtherCAT主站程序中，实时通信线程赋予最高实时优先级；EtherCAT状态监测线程赋予低实时优先级；初始化与终端信息打印线程不添加实时优先级；键盘事件监测线程无实时优先级。

进一步地，Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信的过程如下：

基于Socket通信的方式，EtherCAT主站与上层机器人规划与控制模块中的规划器进行点对点的全双工通信，对应的通信使用Xenomai提供的实时域内数据报协议IDDP实现实时进程间的通信；全双工用于实现伺服控制，EtherCAT主站读取电机编码器数据并重新打包，随后通过IDDP转发至上层的规划器，规划器以此为起始位置开始规划任务；机器人规划与控制模块通过自适应PID控制器对机器人进行控制，自适应PID控制器向上接收规划器线程传递的目标位置和速度与转矩偏移量，向下接收电机位置、速度和力矩的实际值，然后计算出各关节的转矩目标值，通过EtherCAT主站下发至电机；EtherCAT主站通过XDDP向非实时进程打印数据，通过调试模块显示位置、速度、转矩的目标值与实际值，输出实际指令；

基于Socket通信的方式，实时域与非实时域间进行全双工通信，对应的通信使用Xenomai提供的跨域数据报协议XDDP实现实时进程与非实时进程间的通信，EtherCAT主站通过XDDP向EtherCAT主站非实时线程与调试模块输出信息；

XDDP和IDDP的通信周期相互独立。

进一步地，EtherCAT主站与上层机器人规划与控制模块中的规划器进行点对点的全双工通信的通信协议包括帧头、预留指令字、电机位置、电机速度、关节力矩信息和CRC校验位；预留的指令字接口用于功能拓展。

进一步地，机器人规划与控制模块通过XDDP协议单向地与远程三维模拟模块服务器端通信。

有益效果：

本发明向下实现了与工业机器人硬件系统解耦的EtherCAT总线实时通信，向上实现了面向调试的功能模块、通用的控制器与配套接口，主要体现在以下几个方面：

本发明实现了面向不同厂家XML配置文件的通用PDO初始化逻辑单元，能够自动根据不同厂家的EtherCAT从站驱动器信息选择对应的XML文件，并以此为依据开展PDO通信的初始化流程，引入了与驱动器内部从站数目无关的伺服轴软件兼容层，实现了对不同厂家不同型号的六轴工业机器人设备的兼容性。该方案在上层主机端使用通用硬件实现，成本低廉且供应充足，实现了软硬件解耦，在机器人端无需对硬件进行改造，容易在现有设备上实现。

本发明实现了对不同厂家不同型号的工业机器人的通用实时EtherCAT主站。通过CANOpen标准中规定的PDO数据报实现主站与从站驱动器间的数据交换，主站下发机器人各关节期望的操作模式和对应数据的目标值，从站返回电机的实际工作状态和各参数的实际值。主站具有高实时性，能够对不同构型和不同减速比配置的工业机器人进行实时控制，可满足快速高精度控制需求。

本发明实现了基于周期同步转矩模式控制的全速度段下的机器人自适应PID控制器。采用前馈、反馈与滤波相结合的方式，兼顾了低速状态下的精度和高速状态下的快速运动能力。

本发明实现了与底层硬件弱相关的通用API接口，能够通过socket实现点对点的阻塞式和非阻塞式全双工通信和异步的关键信息显示功能，并针对CiA402协议中规定的标准化工业机器人操作模式实现对不同种类的运动规划器的兼容。整体控制系统架构实现了对不同层级的实时任务和非实时任务处理，保证系统的实时性、健壮性、兼容性、拓展性。具有开发简单、使用方便的特点。

本发明实现了基于OpenGL的图形化二维本地调试软件与基于websocket与WebGL技术的远程三维模拟软件。本地调试软件可实时显示机器人关节空间下的位置、速度、力矩目标值与实际值，远程三维模拟软件通过基于mujoco的正逆动力学演算程序，能够实时显示机器人当前位姿。该系统提供了一套运行于模拟环境中的虚拟机械臂，开发人员可在此环境中进行算法验证与测试工作，验证无误后再对实际硬件进行控制，缩短了算法开发的时间，提高了开发过程中的安全性。远程访问的方式可通过局域网对工业现场的任一设备进行远程调试，节约了时间与成本，提高了对异常情况的响应速度，提高了调试的安全性，同时也提高了调试能力。

在本发明的基于EtherCAT的工业机器人通用控制及调试系统中，所述机器人控制单元采用基本频率为2.1GHz的8核x86处理器，EtherCAT通信周期为1ms，自适应PID控制器的控制周期为1ms。

附图说明

图1为工业机器人控制系统的应用环境示意图。

图2为工业机器人控制系统架构图。

图3为EtherCAT主站和机器人规划与控制模块的线程划分与优先级设置。

图4为EtherCAT主站初始化与终端信息打印线程的程序流程图。

图5为EtherCAT主站实时通信线程的程序流程图。

图6为EtherCAT主站EtherCAT状态监测线程的程序流程图。

图7为工业机器人自适应PID控制器架构图。

图8为工业机器人远程三维模拟模块服务器端的程序流程图。

图9为对6轴工业机器人驱动器的实时性测试结果。

具体实施方式

为了解决工业机器人控制系统的底层硬件兼容性和开发与使用的便利性问题，本发明提出了基于EtherCAT的工业机器人通用控制及调试系统。下面结合具体实施方式对本发明进行详细说明。

具体实施方式一：

本实施方式为一种基于EtherCAT的工业机器人通用控制及调试系统，如图2所示，其包括EtherCAT主站(包括实时部分与非实时部分)、Socket通信API、机器人规划与控制模块、远程三维模拟模块与调试模块；远程三维模拟模块采用C/S架构，其由本地计算机下的远程三维模拟模块服务器端与远程计算机下的远程三维模拟模块显示终端(客户端)组成；

如图1所示，EtherCAT主站用于引导EtherCAT从站，即工业机器人驱动器，进入Operation Mode(OP)状态，EtherCAT主站通过PDO数据报与SDO数据报的形式与工业机器人驱动器硬件通信；

EtherCAT主站的部分线程具有实时性，主站使用Socket通信API在机器人规划与控制模块、远程三维模拟模块、调试模块和工业机器人本体间传递数据；Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信，在远程计算机(负责承载远程三维模拟模块前端显示界面)和本地计算机(负责运行远程三维模拟模块后台服务器)之间实现单工的网络通信，如此既提供了远程调试的方法，又避免了图形用户界面(GUI)对整套控制系统实时性的影响；图2中的本地计算机与远程计算机之间通过Ethernet进行websocket通信。

机器人规划与控制模块用于进行机器人的轨迹规划，并通过自适应PID控制器对机器人进行控制；机器人规划与控制模块具有实时性；

远程三维模拟模块远程显示机器人运行状态，通过模拟计算各关节力矩；远程三维模拟模块不具有实时性；

调试模块本地显示机器人关节空间下的位置、速度、力矩目标值与实际值；调试模块为OpenGL调试模块，其不具有实时性；

机器人规划与控制模块、EtherCAT主站的实时部分运行于Xenomai用户态，RTnet运行于Xenomai内核态，两者接受实时调度；

远程三维模拟模块服务器、EtherCAT主站的非实时部分与OpenGL调试模块属于普通Linux下的非实时进程，运行于Linux用户态。

实时进程下的EtherCAT主站通过实时网络RTnet下发EtherCAT的PDO和SDO数据报至配套的工业机器人驱动器，从而控制六个关节的位置、速度和力矩目标值。

本发明设计了一种要求高实时性的EtherCAT主站。EtherCAT主站运行周期的抖动和延迟会影响工业机器人的精度和EtherCAT同步通信的稳定性，因此，首先在使用主线Linux内核的常规Linux发行版(以下均简称Linux)中引入硬实时调度器，实现进程的实时调度，以构建高实时性的EtherCAT主站运行环境；

构建实时环境是通过引入Xenomai实时操作系统实现的。Xenomai在Linux内核的基础上，引入I-pipe补丁和Cobalt内核，将传统的单内核模式改为双内核模式，常规Linux内核负责非实时任务和实时任务的初始化，Xenomai下的Cobalt内核负责实时任务的运行。每当发出一个新的实时进程创建请求，先由Linux内核创建进程并分配内存空间，此时进程虽然具备实时属性，但因运行于常规内核下，仍然归类为非实时进程；随后切换至Cobalt内核下实时运行，成为真正的实时进程。在双内核模式下，Linux内核优先级低于Cobalt内核，接受实时内核调度，实际上Linux内核同样受Cobalt内核调度，也就是Linux系统变成了Xenomai系统下的一个任务。

在常规的中断仲裁器中，分配了时间片的进程也有可能会被软件中断或硬件中断打断进入中断处理服务程序，并且不同的中断间可能还会互相嵌套，严重影响了实时进程的时序。并且实时任务和非实时任务均交由Linux内核运行，实时任务无法抢占非实时任务的CPU时间，在高负载时可能导致极大的抖动和延迟。为解决这一问题，在Cobalt内核中加入I-pipe补丁，该补丁位于计算机硬件与Cobalt内核之间，目的是构建一层硬件抽象层，Xenomai系统运行时，由I-pipe补丁统一处理中断请求(Interrupt ReQuest,IRQ)，避免对实时进程的影响。

其次通过对Linux启动引导程序的设置实现内核级的实时域与非实时域间的隔离。通过Linux的grub引导程序，将多处理器计算机系统逻辑核心分为第一处理器集和第二处理器集两部分，在操作系统的引导阶段将不同的系统引导至不同的逻辑CPU核上运行，第一处理器集用于运行Xenomai系统，承载实时任务，第二处理器集用于运行Linux发行版，承载非实时任务。由此，Xenomai内核下的实时进程和Linux内核下的非实时进程只能运行在指定的CPU逻辑核心下，提高了CPU不同核心的一级缓存和二级缓存命中率。当一个进程池中的进程请求CPU时间时，进程调度器会依据该进程的类型分配不同逻辑CPU核心的时间片，并最终写回运算结果，结束进程运行。

需要说明的是，基于Xenomai的双内核机制并非是搭建实时环境的唯一选择。通过在Linux内核中引入抢占式补丁PREEMPT_RT或者使用单Mecury内核版本的Xenomai同样可以实现Linux的实时化改造。还需要说明的是，由于双内核版本的Xenomai通过I-pipe补丁实现了硬实时，在实时性上要优于这里提到的其他解决方案，可提高工业机器人系统运行时的精度和稳定性；并且Xenomai对x86和ARM体系结构的平台支持度较高，可以优先考虑。

本发明涉及一种面向工业流水线现场的6轴串联机器人的EtherCAT主站。当前大部分工业机器人驱动器均支持通过EtherCAT工业现场总线进行通信。但在传统OSI模型下使用通用网卡(Network Interface Card，NIC)驱动程序配合TCP/IP协议簇的结构冗余且实时性低下，需要对网络架构进行实时化改造。

具体而言，通过Cobalt内核下的RTnet搭建实时网络。通过Cobalt内核提供的实时驱动模型RTDM(Real-Time Driver Model)对EtherCAT主站进行实时化改造，将Cobalt内核系统调用转换到实时域上。由此，EtherCAT主站程序可以自由地调用实时网卡驱动收发EtherCAT数据包。

实施例之一使用英特尔82575网卡配合rt-igb实时驱动组成数据链路层。需要说明的是，使用rt-igb实时驱动的其它型号的NIC或者使用其他实时驱动的NIC可实现相同的功能，如使用rt-e1000e实时驱动的英特尔82583网卡。

当前各型号工业机器人通常使用专有协议，不同品牌甚至不同型号间的工业机器人间虽使用相同的EtherCAT总线，但仍需配备支持特定应用层协议的示教器或主站才可控制机器人。对于一个常见的6轴机器人系统，其EtherCAT总线拓扑如图1所示。每个EtherCAT从站对应一个或若干个数量相等的关节(也即电机驱动器伺服轴，以下均简称关节)，而对于不同型号的工业机器人而言，此数目可能并不统一，也即EtherCAT从站可能采用单轴或多轴架构。如果简单依据从站数目进行初始化，会导致部分伺服轴无法被成功初始化。为解决上述问题，同时实现对不同型号工业机器人的兼容性，设计了EtherCAT主站初始化方式。

首先，不同型号的工业机器人构型不同，臂长不同，使用的过程数据对象PDO(Process Data Project)通信参数亦不尽相同，为了准确地描述从站信息，为PDO数据报收发做准备，需要根据EtherCAT从站信息(EtherCAT Slave Information,ESI)进行不同的初始化配置，达到兼容各厂商各型号EtherCAT从站的效果。具体而言，在EtherCAT主站初始化时，首先读取EtherCAT从站XML配置文件，根据其中的从站信息进行邮箱配置。随后以服务数据对象SDO(Service Data Project)写入的方式根据发送PDO(TxPDO)与接收PDO(RxPDO)信息进行PDO映射。

其次，工业机器人通常在应用层使用CAN in Automation(CiA)组织定义的CANOpen协议的分支CiA402协议进行通信。CiA402协议的对象字典中定义了制造商特定子协议区和标准化子协议区两部分，为实现广泛兼容，在不影响机器人功能的前提下，在PDO映射的配置过程中尽可能的使用标准化子协议部分。更具体地，由于部分厂商驱动器底层配置原因，需要使用伪字节进行强制内存对齐，而标准化子协议中并没有相关定义，由此在制造商特定子协议部分只使用伪字节相关条目，其余均不使用。

第三，EtherCAT从站单轴架构和多轴架构并不统一，如果将从站数量定义为关节数量，那在多轴架构下将无法控制各从站下除第一个关节外的其余关节。由此引入伺服轴软件兼容层，实现关节数量与从站数量间的解耦。具体而言，在EtherCAT主站初始化时，通过扫描网络上的EtherCAT从站的方式获取当前的EtherCAT从站数量，通过读取从站XML配置文件的方式获取由机器人厂商提供的从站与关节数量的映射关系。为了避免频繁向操作系统请求动态内存分配可能导致的实时性问题，各关节的PDO数据报存储于栈中。由于栈中的数据有固定的生长方向，因此通过从站对应的内存地址加上对应数量减一的PDO数据报长度，得到指向该从站对应关节PDO数据报的指针地址。EtherCAT主站运行时，只需要向对应于机器人关节总数的指针中填入对应的PDO数据报数据，而无需关注EtherCAT从站的数量。

在本实施例中，EtherCAT主站程序可分为初始化与终端信息打印线程、实时通信线程、EtherCAT状态机监测线程和键盘事件监测线程4个线程。EtherCAT主站的功能模块划分与实时优先级分配见图3，其中：

初始化与终端信息打印线程即主线程，主线程在启动时负责初始化部分的工作，在初始化完成后负责同非实时进程间的通信。EtherCAT主站的初始化与终端信息打印线程的流程如图4所示。EtherCAT主站同时支持规划位置模式(PP)、原点复位模式(HM)、周期同步位置模式(CSP)、周期同步速度模式(CSV)、周期同步转矩模式(CST)5种操作模式，可支持17-23位的电机编码器。

在本实施例中，EtherCAT主站主线程首先通过shell脚本自动开启RTnet实时网络用以承载EtherCAT通信，随后启动EtherCAT主站。主线程读取EtherCAT从站信息，并根据从站驱动器设备名以字符串匹配的模式在数据库中检索对应的XML文件用以查找对应的ESI文件，根据CiA402标准化子协议初始化PDO映射，引导EtherCAT从站进入OP状态，EtherCAT操作模式切换至CST模式，由此完成EtherCAT主站的初始化，并以有名信号量的方式发送信号以引导实时通信线程运行。随后主线程阻塞式地接收并解码主站实时通信线程发送的数据，并在终端打印输出。如果主线程收到了Xenomai系统发出的线程结束信号，那么主线程负责将EtherCAT总线状态机恢复至Init状态，并结束主线程。

实时通信线程负责沟通上层应用与下层的EtherCAT从站，实时通信线程的程序流程如图5所示。实时通信线程通过Xenomai提供的实时域内数据报协议(IDDP)实现同机器人规划与控制模块的点对点通信，通过Xenomai提供的跨域数据报协议(XDDP)实现与调试模块和主站初始化与终端信息打印线程的点对点通信，通过EtherCAT总线实现同各个从站的通信。实时通信线程会在线程创建后首先执行CPU亲和力绑定与实时线程调度方式设置，以此提高此线程的优先级，同时最大化利用CPU核心和缓存资源。随后，实时通信线程将XDDP与IDDP通信初始化为监听对应的地址，缓存池大小设置为128字节。之后，实时通信线程会阻塞式地等待主线程发送的信号量，并在信号量到达后进入主循环。实时通信线程在主循环中首先读取CLOCK_MONOTONIC高精度定时器的时间信息，并通过clock_nanosleep函数以纳秒精度休眠至下一个EtherCAT通讯周期。在休眠完成后，实时通信线程读取TxPDO数据报，根据CiA402协议解码出状态字对应的状态，随后根据此状态和EtherCAT总线状态机的状态切换过程编码对应的控制字，随后向与之通信的各个线程的地址发送IDDP或XDDP数据包。并将接收到的IDDP数据包进行解码，随后写入RxPDO数据包中，最后通过EtherCAT总线发送。为了避免丢包或信宿线程运行缓慢影响实时通信线程的实时性，IDDP数据包的收发均采用非阻塞的方式进行。

由于实时通信线程负责PDO与SDO数据报收发，考虑到EtherCAT总线对时间敏感的特性，为其赋予最高优先级，即RT；EtherCAT状态监测线程用于保证总线的正常工作，但考虑到实际运行过程中EtherCAT总线的故障率较低且恢复速度较慢，因此赋予低实时优先级；EtherCAT主站的初始化过程受驱动器硬件条件等多种因素影响，是一个不定长过程，因此本实施例中并未为其添加实时优先级；键盘事件监测线程仅用于检测用户的输入，因此无优先级。

EtherCAT状态机监测线程负责定时监测EtherCAT总线状态机，并在从站报错时负责将其恢复至正常状态，EtherCAT状态监测线程的程序流程如图6所示。

键盘事件监测线程负责监听用户的按键事件，并实现程序的退出和急停状态的退出等功能。

本发明涉及域内和跨域的全双工通信方式。域内指实时域内的通信，跨域指实时域和非实时域间的通信。

EtherCAT主站通过Socket通信的方式与上层机器人规划与控制模块中的规划器进行点对点的全双工通信。该通信使用Xenomai提供的实时域内数据报协议(IDDP)实现实时进程间的小数据量通信。该通信可以根据实际需要配置成阻塞式或者非阻塞式的。全双工用于实现伺服控制。通信协议包括帧头、预留指令字、电机位置、电机速度、关节力矩信息和CRC校验位，预留的指令字接口可方便地拓展功能。

实时域与非实时域间通过Socket通信的方式进行全双工通信。该通信使用Xenomai提供的跨域数据报协议(XDDP)实现实时进程与非实时进程间的小数据量通信，而不影响实时进程的实时性。XDDP和IDDP的通信周期相互独立。

位于实时域的机器人规划与控制模块通过XDDP协议单向地与远程三维模拟模块服务器通信。

在本实施例中，EtherCAT主站通过IDDP协议实时下发控制指令，并将各关节的实际状况报告至上层的规划器；EtherCAT主站同时通过XDDP向自身非实时线程与OpenGL调试模块输出信息。

具体而言，EtherCAT主站读取电机编码器数据并重新打包(信源是EtherCAT从站，从站负责读取电机编码器数据)，随后通过IDDP转发至上层的规划器，规划器以此为起始位置开始规划任务。控制器向上接收规划器线程传递的目标位置和速度与转矩偏移量，向下接收电机位置、速度和力矩的实际值，通过自适应PID控制器与前馈补偿控制系统计算出各关节的转矩目标值，通过EtherCAT主站下发至电机。EtherCAT主站通过XDDP向非实时进程打印数据，通过终端或终端模拟器输出实际指令，通过OpenGL图形化的调试模块显示位置、速度、转矩的目标值与实际值。

本发明中适用于工业机器人作业场景的机器人规划与控制模块分为运动规划线程，自适应PID控制线程以及IDDP与Socket通信线程，如图3所示。工业机器人典型作业场景包括低速情况下的高精度运动和高速情况下的快速响应两类，工业机器人本体关节的电机采用CST模式控制。相比于传统的比例-积分-微分PID控制器，自适应PID控制器无需手动调节PID参数，可获得更快的响应时间和更小的超调和稳态误差，并且对外部干扰的鲁棒性更强，适合在上述场景下使用。

针对机器人规划与控制模块通过自适应PID控制器对机器人进行控制的过程，在一些实施例中，使用反馈误差自适应PID(Feedback Error Adaptive PID，FEA-PID)作为PID控制器参数的调整方法。在本实施例中，自适应PID控制器的控制框图见图7。其中位置环和速度环控制位于控制器中，采用FEA-PID控制器；转矩环采用PI控制器，由工业机器人驱动器实现。

需要说明的是，PI控制器是工业机器人驱动器中内置的控制器，不属于自适应PID控制器，自适应PID控制器的输出必须经由驱动器的PI控制器才能发送至电机。

在本实施例中，自适应PID控制器的输入为目标位置值、目标速度值、目标力矩值、位置偏移量和速度偏移量，换算到力矩后发送给电机实现位置、速度和力矩三环控制。其中目标位置值、目标速度值和目标力矩值以反馈量的形式给出，位置偏移量和速度偏移量以前馈的形式输入。自适应位置环PID控制器的输入为目标位置值与位置偏移值的加和与位置实际值的差，自适应速度环PID控制器的输入为目标速度值与速度偏移值的加和与经低通滤波后的速度实际值的差。自适应位置环PID控制器与自适应速度环PID控制器的输出均为转矩值，位置环和速度环的转矩输出与目标转矩值先进行加和，后以转矩偏移量的形式下发至驱动器。

在本实施例中，为了解决电机编码器在短采样周期下反馈速度误差大的问题，采用速度低通滤波器对采集到的当前速度进行滤波，以过滤掉低频的噪声。

具体而言，为解决传统的工业机器人驱动器中自带的PID控制器在CSP模式下的低速精度和高速性能不足的缺点，本实施例提出了位于EtherCAT主站上层的工业机器人控制器，在CST模式下使用自适应PID控制器对电机的速度、位置和力矩进行自动调整的PID控制，其中位置环和速度环的控制周期为1000us，转矩环的控制周期为250us。控制器上预留了与在线规划器对接的Socket通信接口，可共同组成完整的上层应用。

针对工业机器人模拟和调试的设计，在不影响软件实时性的情况下提供本地和远程调试的手段。

在本地调试方面，使用OpenGL设计制作了通用的OpenGL调试模块，即调试软件。该软件运行于非实时模式下，可通过XDDP接收EtherCAT主站转发的来自电机编码器的实际值和来自上层应用的目标值，并将其显示在同一副二维框图中，直观地展示出两者的偏差。可同时显示位置值、速度值和力矩值。框图具有滚动刷新和自适应调整幅值的功能，可设置刷新周期。

在远程调试方面，基于软件架构设计中常见的客户端-服务器架构(C/S架构)开发了远程三维模拟模块，分为服务器和客户端两个部分，两部分间通过局域网相连。远程三维模拟模块服务器是一个集成了物理仿真引擎的后端程序，客户端(远程三维模拟模块显示终端)只负责前端显示部分。

在本实施例中，远程三维模拟模块服务器基于three.js技术和mujoco物理引擎构建。Three.js是一个使用Javascript语言的WebGL引擎，用于读取描述机器人构型的URDF(Unified Robot Description Format，统一机器人描述格式)文件和程序的前端构建；mujoco是一个开源的物理仿真引擎，可用于机器人的动力学和运动学仿真。

具体而言，不同构型的机器人描述文件预置在远程三维模拟模块服务器端的文件系统中，Three.js读取对应的URDF文件并传递给mujoco引擎，mujoco引擎的回调函数中预置了控制函数，由Linux下的高精度定时器触发中断实现调用；mujoco将正逆动力学的结算结果输出至Three.js，经由websocket通过网络发送至远程三维模拟模块客户端。

在本实施例中，工业机器人远程三维模拟模块服务器端的程序流程图见图8。需要说明的是，机器人构型的描述文件格式不仅限于URDF，采用MJCF等通用格式也可以达到相同的效果；物理仿真引擎不局限于mujoco，Bullet Physics、ODE、Newton和VortexDynamics等引擎同样可用于工业机器人的动力学和运动学计算。

在本实施例中，在远程三维模拟模块客户端方面，可通过指定IP地址和端口号的方式在浏览器中显示仿真画面，无需下载安装专有应用程序。客户端通过WebGL技术在本地渲染，这种渲染方式不占用服务器端的资源，可降低调试模块对EtherCAT主站实时性的影响。

在基本频率为2.1GHz的8核x86处理器下进行测试，使用Ubuntu 20.04操作系统配合Xenomai 3.1.3系统，使用经实时化改造的英特尔82575网卡，在1ms的EtherCAT通信周期下对清能德创CoolDrive RC6六轴工业机器人伺服驱动器进行60万次PDO数据包收发测试，使用RTcap驱动程序配合Wireshark进行抓包，通过时间戳判断主站的实时性，测试结果如图9所示。测试结果表明，99％的数据包抖动小于30us，全部的数据包抖动在60us以内。误差小于6％，可满足绝大部分工业场景的应用需求。

本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (10)

1.基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，包括EtherCAT主站、Socket通信API、实时网络RTnet、机器人规划与控制模块、远程三维模拟模块与调试模块；远程三维模拟模块采用C/S架构，其由本地计算机下的远程三维模拟模块服务器端与远程计算机下的远程三维模拟模块显示终端组成；

针对工业机器人控制系统运行环境，在Linux发行版中引入硬实时调度器，实现进程的实时调度；构建实时环境是通过引入Xenomai实时操作系统实现的：Xenomai在Linux内核的基础上，引入Cobalt内核，将单内核模式改为双内核模式，常规Linux内核负责非实时任务和实时任务的初始化，Xenomai下的Cobalt内核负责实时任务的运行；每当发出一个新的实时进程创建请求，先由Linux内核创建进程并分配内存空间，归类为非实时进程；随后切换至Cobalt内核下实时运行，成为实时进程；在双内核模式下，Linux内核优先级低于Cobalt内核，接受实时调度；同时在Cobalt内核中加入I-pipe补丁，I-pipe补丁位于计算机硬件与Cobalt内核之间，构建一层硬件抽象层，Xenomai系统运行时，由I-pipe补丁统一处理中断请求；通过对Linux启动引导程序的设置实现内核级的实时域与非实时域间的隔离；通过Linux的grub引导程序，将多处理器计算机系统逻辑核心分为第一处理器集和第二处理器集两部分，在操作系统的引导阶段将不同的系统引导至不同的逻辑CPU核上运行，第一处理器集用于运行Xenomai系统，承载实时任务，第二处理器集用于运行Linux发行版，承载非实时任务；

EtherCAT主站用于引导EtherCAT从站进入Operation Mode状态，即OP状态，EtherCAT从站即工业机器人驱动器，EtherCAT主站通过PDO数据报与SDO数据报的形式与工业机器人驱动器硬件通信；EtherCAT主站的部分线程具有实时性；EtherCAT主站使用Socket通信API在机器人规划与控制模块、远程三维模拟模块、调试模块和工业机器人本体间传递数据；Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信；在运行远程三维模拟模块显示终端的远程计算机和运行远程三维模拟模块服务器端的本地计算机之间实现单工的网络通信；实时部分下的EtherCAT主站通过实时网络RTnet下发EtherCAT的PDO和SDO数据报至配套的工业机器人驱动器，从而控制机器人关节的位置、速度和力矩目标值；

机器人规划与控制模块用于进行机器人的轨迹规划，并通过自适应PID控制器对机器人进行控制；机器人规划与控制模块具有实时性；

远程三维模拟模块远程显示机器人运行状态，通过模拟计算各关节力矩；远程三维模拟模块不具有实时性；

调试模块本地显示机器人关节空间下的位置、速度、力矩目标值与实际值；调试模块不具有实时性；

机器人规划与控制模块、EtherCAT主站的实时部分运行于Xenomai用户态，RTnet运行于Xenomai内核态，两者接受实时调度；远程三维模拟模块、EtherCAT主站的非实时部分与调试模块属于Linux下的非实时进程，运行于Linux用户态。

2.根据权利要求1所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，调试模块为OpenGL调试模块。

3.根据权利要求2所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，EtherCAT主站的网络架构实时化方式如下：

通过Cobalt内核下的Rtnet搭建实时网络，通过Cobalt内核提供的实时驱动模型RTDM对EtherCAT主站进行实时化改造，将Cobalt内核系统调用转换到实时域上，EtherCAT主站程序能够自由地调用实时网卡驱动收发EtherCAT数据包。

4.根据权利要求3所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，EtherCAT主站程序分为初始化与终端信息打印线程、实时通信线程、EtherCAT状态机监测线程和键盘事件监测线程；

初始化与终端信息打印线程为主线程，主线程在启动时负责初始化部分的工作，在初始化完成后负责接收实时实时通信线程传递的信息，并将其打印到远程三维模拟模块服务器端或远程三维模拟模块显示终端；在初始化与终端信息打印线程的初始化过程中，EtherCAT主站会在主线程启动前通过shell脚本自动开启RTnet实时网络用以承载EtherCAT通信，随后启动EtherCAT主站；主线程读取EtherCAT从站信息，并根据从站驱动器设备名以字符串匹配的模式在数据库中检索对应的XML文件用以查找对应的ESI文件，根据CiA402标准化子协议和制造商特定子协议中的伪字节相关条目初始化PDO映射，引导EtherCAT从站进入OP状态，EtherCAT操作模式切换至CST模式，由此完成EtherCAT主站的初始化，并以有名信号量的方式发送信号以引导实时通信线程运行；随后主线程阻塞式地接收并解码主站实时通信线程发送的数据，并在终端打印输出；如果主线程收到了其他线程发出的线程结束信号，那么主线程负责将EtherCAT总线状态机恢复至Init状态，并结束主线程；

实时通信线程通过Xenomai提供的实时域内数据报协议IDDP实现同机器人规划与控制模块的点对点通信，通过Xenomai提供的跨域数据报协议XDDP实现与调试模块和主站初始化与终端信息打印线程的点对点通信，通过EtherCAT总线实现同各个EtherCAT从站的通信；

EtherCAT状态机监测线程负责定时监测EtherCAT总线状态机，并在从站报错时负责恢复将其至正常状态；

键盘事件监测线程负责监听用户的按键事件，并实现程序的退出和急停状态的退出功能。

5.根据权利要求4所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，在EtherCAT主站初始化的过程中，通过扫描网络上的EtherCAT从站的方式获取当前的EtherCAT从站数量，通过读取从站XML配置文件的方式获取从站与关节数量的映射关系；随后以服务数据对象SDO写入的方式根据发送PDO与接收PDO信息进行PDO映射；各关节的PDO数据报存储于栈中，通过从站对应的内存地址加上对应数量减一的PDO数据报长度，得到指向该从站对应关节PDO数据报的指针地址；EtherCAT主站运行时，只需要向对应于机器人关节总数的指针中填入对应的PDO数据报数据，不关注EtherCAT从站的数量。

6.根据权利要求5所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，实时通信线程的处理过程如下：

实时通信线程会在线程创建后首先执行CPU亲和力绑定与实时线程调度方式设置，以此提高此线程的优先级，同时最大化利用CPU核心和缓存资源；随后，实时通信线程将XDDP与IDDP通信初始化为监听对应的地址；之后，实时通信线程会阻塞式地等待主线程发送的信号量，并在信号量到达后进入主循环；实时通信线程在主循环中首先读取CLOCK_MONOTONIC高精度定时器的时间信息，并通过clock_nanosleep函数以纳秒精度休眠至下一个EtherCAT通讯周期；在休眠完成后，实时通信线程读取TxPDO数据报，根据CiA402协议解码出状态字对应的状态，随后根据此状态和EtherCAT总线状态机的状态切换过程编码对应的控制字，随后向与之通信的各个线程的地址发送IDDP或XDDP数据包；并将接收到的IDDP数据包进行解码，随后写入RxPDO数据包中，最后通过EtherCAT总线发送；IDDP数据包的收发均采用非阻塞的方式进行。

7.根据权利要求6所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，EtherCAT主站程序中，实时通信线程赋予最高实时优先级；EtherCAT状态监测线程赋予低实时优先级；初始化与终端信息打印线程不添加实时优先级；键盘事件监测线程无实时优先级。

8.根据权利要求1至7任意一项所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，Socket通信API用于在实时与实时进程之间、实时与非实时进程之间实现域内和跨域的全双工通信的过程如下：

基于Socket通信的方式，EtherCAT主站与上层机器人规划与控制模块中的规划器进行点对点的全双工通信，对应的通信使用Xenomai提供的实时域内数据报协议IDDP实现实时进程间的通信；全双工用于实现伺服控制，EtherCAT主站读取电机编码器数据并重新打包，随后通过IDDP转发至上层的规划器，规划器以此为起始位置开始规划任务；机器人规划与控制模块通过自适应PID控制器对机器人进行控制，自适应PID控制器向上接收规划器线程传递的目标位置和速度与转矩偏移量，向下接收电机位置、速度和力矩的实际值，然后计算出各关节的转矩目标值，通过EtherCAT主站下发至电机；EtherCAT主站通过XDDP向非实时进程打印数据，通过调试模块显示位置、速度、转矩的目标值与实际值，输出实际指令；

基于Socket通信的方式，实时域与非实时域间进行全双工通信，对应的通信使用Xenomai提供的跨域数据报协议XDDP实现实时进程与非实时进程间的通信，EtherCAT主站通过XDDP向EtherCAT主站非实时线程与调试模块输出信息；

XDDP和IDDP的通信周期相互独立。

9.根据权利要求8所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，EtherCAT主站与上层机器人规划与控制模块中的规划器进行点对点的全双工通信的通信协议包括帧头、预留指令字、电机位置、电机速度、关节力矩信息和CRC校验位；预留的指令字接口用于功能拓展。

10.根据权利要求9所述的基于EtherCAT的工业机器人通用控制及调试系统，其特征在于，机器人规划与控制模块通过XDDP协议单向地与远程三维模拟模块服务器端通信。