CN104589367A - 基于EtherCAT的模块化机器人驱动器和控制方法 - Google Patents

Abstract

本发明涉及一种应用EtherCAT总线通信技术的模块化关节驱动器，包括核心控制器、电机驱动单元、HALL编码器电路、NRZ码盘电路、抱闸电路、转矩传感器电桥电路、RS232通信电路、EtherCAT通信电路、电源、状态显示电路、STO和I/O电路、应用层程序。方法包括：以核心控制器执行程序，运行算法，驱动电机，完成三闭环，实现与主机通信；用ESC控制芯片和PHY芯片实现EtherCAT协议的数据链路层和物理层通信；在核心控制器上编写程序实现应用层功能。本发明提供一种实时性强、可靠性高、通信速率高、集成度高、成本低、体积小的应用EtherCAT总线的模块化机器人关节驱动控制方法和装置。

Description

基于EtherCAT的模块化机器人驱动器和控制方法

技术领域

本发明涉及一种应用EtherCAT总线通信技术的模块化关节驱动器及其控制方法，可应用于模块化机器人和集成传动系统。

背景技术

模块化机器人的研究一直是机器人领域中的重要研究内容。采用模块化的设计方法可以提高机器人系统的可重用性、可扩展性并且便于维修，特别是随着机器人多功能性、灵活性和容错性研究的发展，机器人关节的模块化成为核心技术。

然而机器人模块化关节内部需要集成大量机电一体化设备，如电机、电机驱动器、电磁抱闸、编码器、限位开关、减速机和力矩传感器等。这些设备要相互配合，独自或协同实现特定功能，离不开高性能驱动控制器的作用。与此同时，作为机器人整体的一部份，关节还要与运动控制器、其它关节和外围传感器进行通信，这同样离不开关节驱动器的参与。

现有的模块化关节驱动器方案多采用各种分立电路单元组和的方式构成。由于关节内部空间十分有限，并且电磁环境复杂，使得关节性能难以提高，限制了机器人的整体性能。

发明内容

本发明的目的在于克服现有技术的不足，提供一种实时性强、可靠性高、通信速率高、集成度高、成本低、体积小的应用EtherCAT总线的模块化机器人关节驱动控制方法和装置。

本发明为实现上述目的所采用的技术方案是：基于EtherCAT的模块化机器人驱动器，包括控制器以及与其连接的电源、状态显示电路、RS232通信电路；还包括与控制器连接的电机驱动单元、霍尔编码器电路、码盘电路、抱闸电路、转矩传感器电桥电路、EtherCAT通信电路、I/O电路；

所述电机驱动单元与机器人关节的电机连接；霍尔编码器电路与电机的霍尔编码器连接；所述码盘电路与机器人关节的码盘连接；所述抱闸电路与机器人关节内的电磁抱闸连接；所述转矩传感器电桥电路与机器人关节的转矩传感器连接；所述EtherCAT通信电路与上位机连接；所述I/O电路与机器人关节的开关连接。

所述EtherCAT通信电路采用ASIC数据链路层芯片。

所述转矩传感器电桥电路采用放大器，放大器的反向输入端、正向输入端分别与电源负、正极连接且分别通过电容接地，正向输入端通过电阻与正电压、地连接，且与反向输入端之间连有电容；两个增益电阻端之间通过可调电阻相连接，正负电压参考端分别与正、负电压连接且分别通过电容接地；放大器的输出端连有顺序串联的电阻和电容，电容两端并联有二极管，二极管正端接地，负端通过另一个二极管与电源连接。

所述电源输出端通过二极管与电源转换芯片的输入端连接，电源转换芯片的输入端并联电容后经电阻接地；电源转换芯片的输出端连有正电压、与参考端之间连有并联的二极管和电阻，还通过并联的两个电容接地，参考端与地之间连有并联的电阻和电容。

正电压输出端与电源反相芯片的电源输入端连接，电源反相芯片的输出端与模拟地之间连有并联的电容，输出端连有负电压、与地之间连有并联的电容。

基于EtherCAT的模块化机器人控制方法，包括以下步骤：

控制器初始化EtherCAT通信电路参数、通道和同步方式；

控制器读EtherCAT通信电路内事件请求寄存器数据；如果接收到主站传来的周期性数据则写入到控制器，将其作为参考值控制电机运转；同时发送电机的周期性数据通过EtherCAT通信电路反馈至主站；

当控制器接收来自EtherCAT通信电路的中断并处理，进入应用层状态机，返回控制器读EtherCAT通信电路内事件请求寄存器数据步骤。

所述周期性数据包括电机的位置和转矩数据。

基于EtherCAT的模块化机器人控制方法，所述应用层状态机包括：

初始状态为在电源不可接通状态，当接收到主站的控制命令则转入准备好接通电源状态，再转入电源接通状态，然后通过可操作状态控制电机工作；

当电机处于急停状态时，则转入电源不可接通状态或者通过可操作状态控制电机停止；

当电机处于出错反映状态时，则转入出错状态，返回电源不可接通状态。

本发明具有以下有益效果及优点：

1.本发明只用一个集成关节驱动器实现所有模块化关节的控制和高速通信，可以提高关节行动的可靠性、实时性，与主控制器的传输速率及传输距离，减少关节组成单元，节约总体成本。

2.本发明能驱动关节电机，读取两路编码器信号，实现电机的电流(力矩)、速度与位置闭环。

3.本发明跟据主控制器指令吸合或松开电磁抱闸，以锁死或使能关节运动。

4.本发明可由内置电桥处理电路，连接扭矩传感器，通过A/D转换，测量关节扭矩，并发送给主控制器，实现关节及整臂的精确力控制。

5.本发明利用EtherCAT技术的优异性能，实现每秒千次刷新率和多个电机的位置/速度/电流的闭环控制，而整个机械臂只需一条供电电缆和一条总线电缆。

6.本发明基于EtherCAT协议的CoE功能编写应用层程序，实现符合CiA402行规的伺服和运动控制功能。

7.本发明根据CiA402标准建立完整的伺服设备管理状态机，实现伺服设备的状态切换、使能操作、错误状态处理等操作。

附图说明

图1为本发明的功能单元构成框图；

图2为本发明的EtherCAT通信电路框图；

图3a为转矩传感器电桥电路图一；

图3b为转矩传感器电桥电路图二；

图3c为转矩传感器电桥电路图三；

图4为控制器软件流程图；

图5为驱动设备管理状态机原理图。

具体实施方式

下面结合附图及实施实例对本发明做进一步的详细说明。

一种应用EtherCAT总线通信技术的模块化关节驱动器，包括核心控制器、电机驱动单元、HALL编码器电路、NRZ码盘电路、抱闸电路、转矩传感器电桥电路、RS232通信电路、EtherCAT通信电路、电源、状态显示电路、STO和I/O电路、应用层程序。方法包括：以核心控制器执行程序，运行算法，驱动电机，完成三闭环，实现与主机通信；用ESC控制芯片和PHY芯片实现EtherCAT协议的数据链路层和物理层通信；在核心控制器上编写程序实现应用层功能。

如图1所示，本装置包括核心控制器1、电机驱动单元2、HALL编码器电路3、NRZ码盘电路4、抱闸电路5、转矩传感器电桥电路6；RS232通信电路7，EtherCAT通信电路8、电源9、状态显示电路10和安全转矩输出(STO)I/O电路11。

其中，核心控制器，执行程序，运行算法，驱动电机，完成三闭环，与主机通信；EtherCAT电路为百兆以太网的物理层，选用KS8721BL作为PHY芯片；RS232电路提供串口通信，为调试时使用，可使驱动器与调试上位机通信；转矩电桥配合转矩传感器使用，测量关节转矩，实现模、数转换，为了弥补转矩传感器或电路器件可能存在的参数波动，设计电路可调整增益和零偏；抱闸驱动电路将驱动器使能信号输出放大，驱动感性直流抱闸；NRZ码盘电路配合多圈绝对值编码器，读取关节转角，或连接其它通信传感器；HALL编码器电路连接电机霍尔信号，适合大多数直流无刷电机，完成电机换向；电机驱动电路应用PWM驱动电机，调节电机电流，主回路芯片采用IRSM005-301MHTH，可大幅度减小电路体积；I/O电路完成一般输入输出功能,STO(安全转矩关断)功能防止电机停止时产转矩，使机器人满足安全系统的安全性要求；状态显示电路通过指示灯显示关节工作状态；电源电路转换的24V直流电供抱闸使用，5V直流电供编码器使用，3.3V和1.8V直流电供MCU使用，2.5V直流电供网络变压器使用。

图1为本发明的EtherCAT通信电路框图。其核心为MCU芯片PIC24C16MJ306A，关节驱动控制器的主要程序全部在其中运行。MCU芯片通过专用的电机驱动PWM接口控制由IRSM005-301MH组成的换流电路；通过SPI接口或同步、异步接口与ESC芯片通信实现EtherCAT通信，其中ET1200芯片起到打包协议数据的作用；在物理层，以PHY芯片KS8721BL为核心，辅以外围电路。一个完整的EtherCAT从站由此搭建。

上位机可采用高性能的工控PC机，下位机采用基于DSP的伺服运动控制系统，上下位机之间通过EtherCAT进行信息的传输。每个模块化关节控制系统主要由DSP伺服运动控制器、EtherCAT总线接口电路、直流无刷电机和制动器驱动等部分组成。整条机械臂控制通信全部基于实时总线，不仅简化接线，还可实现高速、高可靠性的全变量控制系统。

图2为EtherCAT通信物理层电路图。它包括PHY芯片U1，其型号为KS8721BL；内含网络变压器芯片的RJ45接头J1，其型号为HY911168C；外围配置电路如RP1、RP2、R1至R7和电源滤波电容C1至C14。图3中电路需要2.5VDC和3.3VDC电源，由电源电路提供。U1通过MIDATA、MICLK和RXD[0…3]等引脚与MCU相连接，实现MII通信。U1通过TX+、TX-、RX+和RX-与J1相连接，通过RJ45接头中的网络变压器，将信号输入、输出。

如图3a～3c所示，J1连接转矩传感器的应变片，有8V供电、模拟地、sign+和sign-四路电连接。其中sign+连接电桥信号正，sign-连接电桥信号负。U1为AD620精密仪用放大器，以其为核心构成电桥放大电路。在R3的为放大后的信号输出。VR1可以调节信号增益，VR2可以调节信号零点偏置。D1、D2为钳位二极管，ADC防止电压超过DSP输入阈值。P1及其外围电路将输入+24V转为+8V。U2及其外围电路将输入+8V转为-8V。C1到C10，E1到E5为滤波电容。

在发明中，ESC控制芯片实现EtherCAT的数据链路层操作，通过在微处理器上编写应用程序实现整个系统的应用层。应用层包括以下任务：初始化微处理器、ESC芯片寄存器以及通信变量；处理EtherCAT通信状态机：查询状态控制器，读取相关配置寄存器，启动或终止相关通信服务；处理驱动设备管理状态机：查询驱动器当前状态，根据需求将驱动器切换到相应状态，完成各状态下驱动器相应的操作；周期性和非周期性数据的处理：驱动器以查询模式或同步模式处理周期性数据和各种非周期性事件。

根据上述应用层任务需求，本发明编写了相应的应用程序。

如图4所示，以驱动器运行在查询模式为例，对应用层程序进行说明。驱动器的应用程序分为初始化程序和主循环程序两个部分。在初始化部分，程序完成微处理器和ESC芯片的相关硬件初始化，EtherCAT协议相关变量初始化以及读取ESC事件请求寄存器等操作。在主循环部分，程序首先处理周期性数据，然后处理EtherCAT的状态机，接下来查询并处理非周期性事件，最后会查询并处理应用层的驱动设备管理状态机。

如图4所示，本驱动器根据CiA402伺服驱动器行规制定了设备管理状态机。EtherCAT协议本身规定了物理层和数据链路层协议，在应用层EtherCAT支持CANopen协议标准，即CoE(CANopen over EtherCAT)。CiA402是CANopen标准族中为伺服和运动控制行业制定的行规标准。图5所示状态机正是根据CiA402行规所制定的，它详细规定了各个状态的触发步骤和所执行的操作。只有相关操作正确完成之后，驱动器才能切换到新的状态。

根据图5所示，驱动设备的状态由低到高分为三个等级，每个等级内完成不同的工作。在区域A中，驱动器低级别的电源使能，即低电压的控制电路上电，动力部分并没有上电，驱动器处于准备工作状态。在区域B中，驱动器高级别的电源使能，即高电压的动力电路上电，但是此时电机并没有转矩输出，驱动器处于电源接通已开始供电状态。在区域C中，设备完全准备好，电机有转矩输出，驱动器处于操作状态。此外，图中还有急停、出错反应、出错等状态，用于应对伺服设备运行中可能遇到的错误，并从错误状态中恢复。

主站通过写控制字传输给从站来控制从站的状态，从站通过状态字来反馈自己当前的状态。开始阶段的未准备好接通电源状态(状态1)属于一个瞬间状态，整个通信网络上看不到这个状态；在电源不可接通状态(状态2)驱动设备未激活，通过更改相应控制字可使驱动设备切换到准备好接通电源状态；在准备好接通电源状态(状态3)，驱动设备被激活，控制电路开始供电，此状态下可以任意更改驱动设备参数，更改相应控制字可以使驱动设备切换到电源接通状态；在电源接通状态(状态4)，驱动设备动力电路上电，但是还未向电机输出电压，此状态下可以更改驱动设备参数，但是更改后设备将返回状态3，更改相应控制字可以使驱动设备进入可操作状态；在可操作状态(状态5)，驱动设备向电机输出电压，电机开始运行；电机在运行时如果遇到紧急停止命令，则进入急停状态(状态6)，此状态是一个过渡状态，根据驱动设备参数的设置它可以切换回可操作状态(状态5)或者切换到电源不可接通状态(状态2)；在设备运行的任何阶段，只要出现错误，驱动设备将进入出错反应状态(状态7)，此状态将过渡到出错状态(状态8)最终返回电源不可接通状态(状态2)。

Claims (8)

1.基于EtherCAT的模块化机器人驱动器，包括控制器(1)以及与其连接的电源(9)、状态显示电路(10)、RS232通信电路(7)；其特征在于还包括与控制器(1)连接的电机驱动单元(2)、霍尔编码器电路(3)、码盘电路(4)、抱闸电路(5)、转矩传感器电桥电路(6)、EtherCAT通信电路(8)、I/O电路(11)；

所述电机驱动单元(2)与机器人关节的电机连接；霍尔编码器电路(3)与电机的霍尔编码器连接；所述码盘电路(4)与机器人关节的码盘连接；所述抱闸电路(5)与机器人关节内的电磁抱闸连接；所述转矩传感器电桥电路(6)与机器人关节的转矩传感器连接；所述EtherCAT通信电路(8)与上位机连接；所述I/O电路(11)与机器人关节的开关连接。

2.按权利要求1所述的基于EtherCAT的模块化机器人驱动器，其特征在于所述EtherCAT通信电路(8)采用ASIC数据链路层芯片。

3.按权利要求1所述的基于EtherCAT的模块化机器人驱动器，其特征在于所述转矩传感器电桥电路(6)采用放大器，放大器的反向输入端、正向输入端分别与电源负、正极连接且分别通过电容接地，正向输入端通过电阻与正电压、地连接，且与反向输入端之间连有电容；两个增益电阻端之间通过可调电阻相连接，正负电压参考端分别连有正电压、负电压且分别通过电容接地；放大器的输出端连有顺序串联的电阻和电容，电容两端并联有二极管，二极管正端接地，负端通过另一个二极管与电源连接。

4.按权利要求1所述的基于EtherCAT的模块化机器人驱动器，其特征在于所述电源(9)输出端通过二极管与电源转换芯片的输入端连接，电源转换芯片的输入端并联电容后经电阻接地；电源转换芯片的输出端连有正电压、与参考端之间连有并联的二极管和电阻，还通过并联的两个电容接地，参考端与地之间连有并联的电阻和电容。

5.按权利要求3或4所述的基于EtherCAT的模块化机器人驱动器，其特征在于所述正电压输入至电源反相芯片的电源输入端，电源反相芯片的输出端与模拟地之间连有并联的电容，输出端连有负电压、与地之间连有并联的电容。

6.基于EtherCAT的模块化机器人控制方法，其特征在于包括以下步骤：

控制器(1)初始化EtherCAT通信电路(8)参数、通道和同步方式；

控制器(1)读EtherCAT通信电路(8)内事件请求寄存器数据；如果接收到主站传来的周期性数据则写入到控制器(1)，将其作为参考值控制电机运转；同时发送电机的周期性数据通过EtherCAT通信电路(8)反馈至主站；

当控制器(1)接收来自EtherCAT通信电路(8)的中断并处理，进入应用层状态机，返回控制器(1)读EtherCAT通信电路(8)内事件请求寄存器数据步骤。

7.基于EtherCAT的模块化机器人控制方法，其特征在于所述周期性数据包括电机的位置和转矩数据。

8.基于EtherCAT的模块化机器人控制方法，其特征在于所述应用层状态机包括：

初始状态为在电源不可接通状态，当接收到主站的控制命令则转入准备好接通电源状态，再转入电源接通状态，然后通过可操作状态控制电机工作；

当电机处于急停状态时，则转入电源不可接通状态或者通过可操作状态控制电机停止；

当电机处于出错反映状态时，则转入出错状态，返回电源不可接通状态。