为+8V。U2及其外围电路将输入+8V转为-8V。C1 到C10，E1到E5为滤波电容。
[0081] 在发明中，ESC控制芯片实现EtherCAT的数据链路层操作，通过在微处理器上编 写应用程序实现整个系统的应用层。应用层包括以下任务：初始化微处理器、ESC芯片寄存 器以及通信变量；处理EtherCAT通信状态机：查询状态控制器，读取相关配置寄存器，启动 或终止相关通信服务；处理驱动设备管理状态机：查询驱动器当前状态，根据需求将驱动 器切换到相应状态，完成各状态下驱动器相应的操作；周期性和非周期性数据的处理：驱 动器以查询模式或同步模式处理周期性数据和各种非周期性事件。
[0082] 根据上述应用层任务需求，本发明编写了相应的应用程序。
[0083] 如图6所示，以驱动器运行在查询模式为例，对应用层程序进行说明。驱动器的应 用程序分为初始化程序和主循环程序两个部分。在初始化部分，程序完成微处理器和ESC 芯片的相关硬件初始化，EtherCAT协议相关变量初始化以及读取ESC事件请求寄存器等操 作。在主循环部分，程序首先处理周期性数据，然后处理EtherCAT的状态机，接下来查询并 处理非周期性事件，最后会查询并处理应用层的驱动设备管理状态机。
[0084] 如图6所示，本驱动器根据CiA402伺服驱动器行规制定了设备管理状态机。 EtherCAT协议本身规定了物理层和数据链路层协议，在应用层EtherCAT支持CANopen协议 标准，即CoE(CANopen over EtherCAT)。CiA402是CANopen标准族中为伺服和运动控制行 业制定的行规标准。图5所示状态机正是根据CiA402行规所制定的，它详细规定了各个状 态的触发步骤和所执行的操作。只有相关操作正确完成之后，驱动器才能切换到新的状态。
[0085] 根据图7所示，驱动设备的状态由低到高分为三个等级，每个等级内完成不同的 工作。在区域A中，驱动器低级别的电源使能，即低电压的控制电路上电，动力部分并没有 上电，驱动器处于准备工作状态。在区域B中，驱动器高级别的电源使能，即高电压的动力 电路上电，但是此时电机并没有转矩输出，驱动器处于电源接通已开始供电状态。在区域C 中，设备完全准备好，电机有转矩输出，驱动器处于操作状态。此外，图中还有急停、出错反 应、出错等状态，用于应对伺服设备运行中可能遇到的错误，并从错误状态中恢复。
[0086] 主站通过写控制字传输给从站来控制从站的状态，从站通过状态字来反馈自己当 前的状态。开始阶段的未准备好接通电源状态（状态1)属于一个瞬间状态，整个通信网络 上看不到这个状态；在电源不可接通状态（状态2)驱动设备未激活，通过更改相应控制字 可使驱动设备切换到准备好接通电源状态；在准备好接通电源状态（状态3)，驱动设备被 激活，控制电路开始供电，此状态下可以任意更改驱动设备参数，更改相应控制字可以使驱 动设备切换到电源接通状态；在电源接通状态（状态4)，驱动设备动力电路上电，但是还未 向电机输出电压，此状态下可以更改驱动设备参数，但是更改后设备将返回状态3,更改相 应控制字可以使驱动设备进入可操作状态；在可操作状态（状态5)，驱动设备向电机输出 电压，电机开始运行；电机在运行时如果遇到紧急停止命令，则进入急停状态（状态6)，此 状态是一个过渡状态，根据驱动设备参数的设置它可以切换回可操作状态（状态5)或者切 换到电源不可接通状态（状态2);在设备运行的任何阶段，只要出现错误，驱动设备将进入 出错反应状态（状态7)，此状态将过渡到出错状态（状态8)最终返回电源不可接通状态 (状态2)。
【主权项】
1. 基于EtherCAT总线的机器人柔顺控制系统，其特征在于：包括主站、机器人驱动器、 电机和转矩传感器；主站通过EtherCAT总线与机器人驱动器连接，机器人驱动器连有电机 和转矩传感器； 所述主站用于接收机器人驱动器的位置、转矩信息进行柔顺控制，并输出控制信息至 机器人驱动器； 所述机器人驱动器用于将控制信息转换为电机的驱动命令控制电机的转矩和位置，并 将电机关节角转换为位置信息、将转矩的模拟量转换为数字量反馈至主站。2. 根据权利要求1所述的基于EtherCAT总线的机器人柔顺控制系统，其特征在于所 述机器人驱动器包括控制器（1)以及与其连接的电源（9)、状态显示电路（10)、RS232通信 电路（7);其特征在于还包括与控制器（1)连接的电机驱动单元（2)、霍尔编码器电路（3)、 码盘电路（4)、抱闸电路（5)、转矩传感器电桥电路出）、EtherCAT通信电路（8)、I/O电路 (11); 所述电机驱动单元（2)与机器人关节的电机连接；霍尔编码器电路（3)与电机的霍尔 编码器连接；所述码盘电路（4)与机器人关节的码盘连接；所述抱闸电路（5)与机器人关 节内的电磁抱闸连接；所述转矩传感器电桥电路（6)与机器人关节的转矩传感器连接；所 述EtherCAT通信电路（8)与上位机连接；所述I/O电路（11)与机器人关节的开关连接。3. 按权利要求2所述的基于EtherCAT总线的机器人柔顺控制系统，其特征在于所述 EtherCAT通信电路（8)采用ASIC数据链路层芯片。4. 按权利要求2所述的基于EtherCAT总线的机器人柔顺控制系统，其特征在于所述转 矩传感器电桥电路（6)采用放大器，放大器的反向输入端、正向输入端分别与电源负、正极 连接且分别通过电容接地，正向输入端通过电阻与正电压、地连接，且与反向输入端之间连 有电容；两个增益电阻端之间通过可调电阻相连接，正负电压参考端分别连有正电压、负电 压且分别通过电容接地；放大器的输出端连有顺序串联的电阻和电容，电容两端并联有二 极管，二极管正端接地，负端通过另一个二极管与电源连接。5. 按权利要求2所述的基于EtherCAT总线的机器人柔顺控制系统，其特征在于所述电 源（9)输出端通过二极管与电源转换芯片的输入端连接，电源转换芯片的输入端并联电容 后经电阻接地；电源转换芯片的输出端连有正电压、与参考端之间连有并联的二极管和电 阻，还通过并联的两个电容接地，参考端与地之间连有并联的电阻和电容。6. 按权利要求4或5所述的基于EtherCAT总线的机器人柔顺控制系统，其特征在于所 述正电压输入至电源反相芯片的电源输入端，电源反相芯片的输出端与模拟地之间连有并 联的电容，输出端连有负电压、与地之间连有并联的电容。7. 基于EtherCAT总线的机器人柔顺控制方法，其特征在于包括以下步骤： 机器人驱动器的控制器（1)初始化EtherCAT通信电路（8)参数、通道和同步方式； 主站将机器人驱动器采集的电机周期性数据与设定的期望值进行叠加并进行阻抗控 制得到参考力矩值，与设定的参考位置值共同输出至机器人驱动器； 机器人驱动器的控制器（1)读EtherCAT通信电路（8)内事件请求寄存器数据；如果接 收到主站传来的周期性数据则写入到控制器（1)，将其作为目标值控制电机运转；同时发 送电机的周期性数据通过EtherCAT通信电路（8)转换为数字量反馈至主站； 当机器人驱动器的控制器（1)接收来自EtherCAT通信电路（8)的中断并处理，进入应 用层状态机，返回控制器（1)读EtherCAT通信电路（8)内事件请求寄存器数据步骤。8. 按权利要求7所述的基于EtherCAT的模块化机器人控制方法，其特征在于所述周期 性数据包括电机的位置和转矩数据。9. 按权利要求7所述的基于EtherCAT的模块化机器人控制方法，其特征在于所述应用 层状态机包括： 初始状态为在电源不可接通状态，当接收到主站的控制命令则转入准备好接通电源状 态，再转入电源接通状态，然后通过可操作状态控制电机工作； 当电机处于急停状态时，则转入电源不可接通状态或者通过可操作状态控制电机停 止； 当电机处于出错反映状态时，则转入出错状态，返回电源不可接通状态。10. 按权利要求7所述的基于EtherCAT的模块化机器人控制方法，其特征在于所述主 站将机器人驱动器采集的电机周期性数据与设定的期望值进行叠加并进行阻抗控制包括 一下步骤： 操作者操纵手柄，接收经机器人驱动器将转矩数据转换的力信息、并发出期望的位置 指令和期望力； 然后将机器人的期望位置及机器人驱动器反馈的实际位置做差、并通过理想阻抗模型 得到计算力，将计算力与期望力叠加后作为参考力，再由雅可比矩阵解得出参考关节力矩， 将参考关节力矩发给机器人驱动器，实现对模块化机器人的阻抗控制。
【专利摘要】本发明涉及基于EtherCAT总线的机器人柔顺控制系统，包括主站，机器人驱动器，电机和转矩传感器；主站通过EtherCAT总线与机器人驱动器连接，机器人驱动器连有电机和转矩传感器；方法为主站将机器人驱动器采集的电机周期性数据与设定的期望值进行叠加并进行阻抗控制，输出参考力矩值至机器人驱动器。本发明提供一种实时性强、可靠性高、成本低、体积小的基于EtherCAT总线的模块化机器人柔顺控制方法,适应了现代机器人在执行接触作业中的要求。且该发明完全遵守EtherCAT协议，可以与其他EtherCAT驱动器混合使用，从而提高了该方法的适用范围。
【IPC分类】G05B19/18
【公开号】CN105700465
【申请号】CN201410696611
【发明人】张峰, 张涛, 李涛, 崔龙, 李洪谊, 李伟, 阳方平
【申请人】中国科学院沈阳自动化研究所
【公开日】2016年6月22日
【申请日】2014年11月26日