驱控系统的控制协议及方式
本文主要介绍工业机器人的控制器与驱动器控制相关的知识点。专业有限，只作浅显的介绍。
涉及到的知识点：
CIA 402协议
对象字典
CoE协议
CIA 402状态机
伺服三环控制
同步位置周期控制
前述
驱控系统一般指控制端，驱动系统，执行机构组成的系统。控制端如PLC，控制卡，控制器等；驱动端如变频器，伺服驱动器，放大器等；执行机构用的较多的是电机。本文中的驱控系统主要指工业机器人所用到的控制器、伺服驱动器、伺服电机。
电机的运行由控制端发出运动命令，驱动系统进行执行。那么控制端与驱动端是如何建立控制的？两者交互的信息有哪些？本文基于上述两个问题，进行整理。说明不足之处，还望指正。
CIA 402协议
EtherCAT总线通信架构
本段是以EtherCAT作为通讯总线进行介绍，多数机器人厂家以及伺服驱动器厂家将EtherCAT作为基本总线。下图是控制端主站（Master）与驱动端从站（Slave）通讯的基本架构。主站控制从站，需要先对从站的ESI文件解析，该文件由从站供应商提供，主站对该文件进行解析，转换为自己控制从站的ENI文件。ESI标准文件格式为XML文件，部分控制厂家并不支持XML文件的解析，这就需要人工进行适配处理。
文件解释：EtherCAT Slave Information (ESI) :记载着从站固有的信息(供应商信息、产品信息、Profile、对象、过程数据、有无同步、SyncManager 设定等)的定义。
EtherCAT Network Information (ENI) : ENI载有识别从站信息(供应商信息等)、进行各从站初始化的信息，主站是基于 ENI 记载的信息进行网络的初始化、构筑。
Slave Information Interface (SII) :
ESC 连接保存 SII 数据的 EEPROM。此EEPROM(SII)中保持了ESC 的初始化信息、从站的应用通信设定的规格值(Mailbox 的数据大小值)、过程数据的映射等信息。
CANopen over EtherCAT (CoE)协议
在驱动端，除了要用到EtherCAT协议外，还要用到CANopen协议。EtherCAT协议设计时使用了OSI 7层模型中的三层，即应用层，数据链路层和物理层。CANopen协议是应用层协议，是EtherCAT在应用层使用的邮箱协议（Mailbox protocols）中的一种，即COE模型。CoE主要用于伺服系统通信，使用标准的CiA402规约。如下图所示：
解析：EtherCAT（CoE）网络参考模型由三部分组成：物理层、数据链路层和应用层。物理层参考相关硬件原理图，这里不作详细讲解数据链路层主要负责 EtherCAT 通讯协议应用层嵌入了 CANopen drive Profile（DS402）通讯规约。
SyncManager 同步管理器，控制对应用存储区的访问SM保证了主站和从站通信的一致性和安全性,并且通过产生中断来通知对方状态发生变化。SyncManager数据配置：SM0: 输出邮箱SM1: 输入邮箱SM2: 输出过程数据SM3: 输入过程数据
FMMU 现场总线存储映射管理单元：FMMU负责将主站分配的地址和Slave本身的物理地址建立映射关系（通过芯片内部地址映射的方法把主站分配的逻辑地址转换为本地的物理地址，每一个FMMU通道将一段连续的物理地址映射到一段连续的逻辑地址中。这样就实现了主站的逻辑寻址和实际物理地址的映射）， 主站在检测到所有的从站设备后，会按照链接的顺序给每一个总线上的Slave分配一段地址，并将这段地址下发给各个Slave从而建立关系。FMMU的配置工作由主站操作，对于从站的开发来说只需要核对好设备描述文件中的地址分配就可以了。FMMU映射步骤：1、主站读取每一个从站的硬件配置包括输入输出数据的长度。2、主站组织数据编址。3、主站将为每一个从站的分配好的逻辑地址下发到各个从站中（配置FMMU configuration register）。4、数据开始传输。FMMU数据配置：FMMU0: 映射到过程数据 (RxPDO) 输出区域FMMU1: 映射到过程数据 (TxPDO) 发送区域FMMU2: 映射到邮箱状态
CoE 中的对象字典包括了参数，应用数据以及 PDO 映射信息过程数据对象（PDO）由对象字典中能够进行 PDO 映射的对象构成，PDO 数据中的内容由 PDO 映射来定义。PDO 数据的读取与写入是周期性持续实时读取的，不需要查找对象字典。邮箱通讯Mailbox（SDO）是非周期性通讯，在读写它们时要查找对象字典。为了使 SDO 与 PDO 数据能在 EtherCAT 数据链路层上得到正确解析，需要 对 FMMU 与 SyncManager（同步管理器）进行如下配置。
对象字典
对象字典（OD：Object Dictionary）是CANopen协议最为核心的概念。所谓的对象字典就是一个有序的对象组，描述了对应CANopen节点的所有参数，包括通讯数据存放位置也列入其索引。这个表变成可以传递形式就叫做EDS文件（Electronic Date Sheet电子数据文档） 。对象字典中的每个对象都描述了它的功能、名字、索引、子索引、数据类型，以及这个对象是否必需、读写属性等等。下表为通用通讯对象的举例：
使用分组
| Index |
对象 |
| 0x0000 |
未使用 |
| 0x0001 ~ 0x009F |
数据类型区，存放常规数据类型和用户自定义数据类型 |
| 0x00A0 ~ 0x0FFF |
保留 |
| 0x1000 ~ 0x0FFF |
通信简表区，包含设备类型，错误寄存器和通信相关对象 |
| 0x2000 ~ 0x5FFF |
用户自定义区，存放用户自定义的对象 |
| 0x6000 ~ 0x9FFF |
标准设备简表区 ，存放标准行规中定义的对象 |
| 0xA000 ~ 0xFFFF |
保留 |
实际使用时，并不会用完所有的Index，都是根据需要选择一些Index来使用。例如标准设备简表区，用户开发的产品可能是用于某一行业的，那么就只需要该行业的标准行规定义的对象，如运动控制对应的DS-402。
对象字典的访问
数据交互时，分为两种数据。一种是过程数据对象(PDO) ,主要用于实时数据的传输，使用的数据分为两种：
- 用于主站向从站传输的RxPDO (收信 PDO) ,如控制字。
- 从站向主站传输的TxPDO（发信PDO）,如状态字。
另外一种是服务数据对象（SDO），主要用于非实时的数据传输。主站通过使用 SDO 传输对象字典的项目进行读取/写入操作，可以控制设备设置、监视等从站驱动器的各项参数。
SDO信息结构图
CIA 402协议
CiA402是CANopen标准协议中的子协议之一，全称CAN in Automation Draft Standard Proposed 402 Version。CANopen协议是一系列子协议组成，可分为通讯子协议和设备子协议。CIA 402属于设备子协议。
CIA 402协议用于运动控制功能逻辑，定义了运动控制相关的一些对象参数，状态机，和一些运行模式。
CIA 402功能作用
- 规定驱动器遵守一定的命令流程进行启停控制。（如状态机，下文会讲到）
- 把驱动器涉及到的功能定义好，开发厂家按照协议定义即可。（按照对象字典的格式进行定义）
- 适用于多种通信协议，如Modbus、Canopen、EtherCAT、EtherNET、Profibus、ProfiNET等。
CIA 402状态机
状态机描述了驱动端与控制端实时变化的状态交互。控制端通过控制字0x6040（对象字典）将控制指令下发给驱动端，驱动端通过状态字0x6041将状态发给控制端。
CIA 402状态机
区域A，控制单元运行正常；
区域B，驱动器内部主电使能正常，对电机输出端未接通。
区域C，驱动器需要连接了电机，并给电机上使能。
对控制端来说，在通信的每个周期种，它都需要向从站发送控制字，并接收从站的状态字进行确认。控制字和状态字用16位的二进制表示，每一位有着不同的作用。如控制字的0位为1时，代表着命令从站开启伺服。下表是控制字与状态字每一位代表的含义。
控制字
| bit |
名称 |
name |
描述 |
| 0 |
可以开启伺服运行 |
switch on |
1-有效，0-无效 |
| 1 |
接通主电 |
enable voltage |
1-有效，0-无效 |
| 2 |
快速停机 |
quick stop |
0-有效，1-无效 |
| 3 |
伺服运行 |
enable operation |
1-有效，0-无效 |
| 6 |
特定的操作模式 |
peration mode specific |
与各伺服运行模式有关 |
| 5 |
特定的操作模式 |
peration mode specific |
与各伺服运行模式有关 |
| 4 |
特定的操作模式 |
peration mode specific |
与各伺服运行模式有关 |
| 7 |
故障复位 |
fault reset |
0-无效，0至1:复位可清除的故障 |
| 8 |
暂停 |
halt |
1-有效，0-无效 |
| 9 |
特定的操作模式 |
peration mode specific |
与各伺服运行模式有关 |
| 10 |
保留 |
reserved |
未定义 |
| 11 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 12 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 13 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 14 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 15 |
厂家自定义 |
manufacturer-specific |
未定义 |
状态字
| bit |
名称 |
name |
描述 |
| 0 |
准备接通主电/伺服准备好 |
ready to switch on |
1-有效，0-无效 |
| 1 |
接通主电/伺服使能 |
switch on |
1-有效，0-无效 |
| 2 |
运行使能/伺服运行 |
operation enable |
1-有效，0-无效 |
| 3 |
故障 |
fault |
1-有效，0-无效 |
| 4 |
电压已启用/伺服主电接通 |
voltage enabled |
1-有效，0-无效 |
| 5 |
快速停机 |
quick stop |
0-有效，1-无效 |
| 6 |
禁止使能 |
Switch on disabled |
1-有效，0-无效 |
| 7 |
警告 |
warning |
1-有效，0-无效 |
| 8 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 9 |
远程控制 |
remote |
1-有效，0-无效 |
| 10 |
目标到达 |
target reached |
1-有效，0-无效 |
| 11 |
软件内部位置超限 |
internal limit active |
1-有效，0-无效 |
| 12 |
操作模式 |
operation mode specific |
未定义 |
| 13 |
操作模式 |
operation mode specific |
未定义 |
| 14 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 15 |
厂家自定义 |
manufacturer-specific |
未定义 |
我们判断上位端的指令或者驱动器的状态，并不需要知道控制字或者状态字的每一位，而是只关注其中关键的几位。如下表表示了对应关系。
状态字
| bit |
名称 |
name |
描述 |
| 0 |
准备接通主电/伺服准备好 |
ready to switch on |
1-有效，0-无效 |
| 1 |
接通主电/伺服使能 |
switch on |
1-有效，0-无效 |
| 2 |
运行使能/伺服运行 |
operation enable |
1-有效，0-无效 |
| 3 |
故障 |
fault |
1-有效，0-无效 |
| 4 |
电压已启用/伺服主电接通 |
voltage enabled |
1-有效，0-无效 |
| 5 |
快速停机 |
quick stop |
0-有效，1-无效 |
| 6 |
禁止使能 |
Switch on disabled |
1-有效，0-无效 |
| 7 |
警告 |
warning |
1-有效，0-无效 |
| 8 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 9 |
远程控制 |
remote |
1-有效，0-无效 |
| 10 |
目标到达 |
target reached |
1-有效，0-无效 |
| 11 |
软件内部位置超限 |
internal limit active |
1-有效，0-无效 |
| 12 |
操作模式 |
operation mode specific |
未定义 |
| 13 |
操作模式 |
operation mode specific |
未定义 |
| 14 |
厂家自定义 |
manufacturer-specific |
未定义 |
| 15 |
厂家自定义 |
manufacturer-specific |
未定义 |
我们判断上位端的指令或者驱动器的状态，并不需要知道控制字或者状态字的每一位，而是只关注其中关键的几位。如下表表示了对应关系。
控制字
| Command |
Bit 7 |
Bit 3 |
Bit 2 |
Bit 1 |
Bit 0 |
命令描述 |
Transitions |
| Shutdown |
0 |
X |
1 |
1 |
0 |
伺服主电未接通 |
2,6,8 |
| Switch on |
0 |
0 |
1 |
1 |
1 |
伺服使能 |
3 |
| Switch on + enable operation |
0 |
1 |
1 |
1 |
1 |
伺服使能+电机运行 |
3 + 4 (NOTE) |
| Disable voltage |
0 |
X |
X |
0 |
X |
禁止主电接通 |
7,9,10,12 |
| Quick stop |
0 |
X |
0 |
1 |
X |
快速停止 |
7,10,11 |
| Disable operation |
0 |
0 |
1 |
1 |
1 |
禁止运行 |
5 |
| Enable operation |
0 |
1 |
1 |
1 |
1 |
使能运行 |
4,16 |
| Fault reset |
上升沿 |
X |
X |
X |
X |
故障复位 |
15 |
NOTE： Automatic transition to Enable operation state after executing SWITCHED ON state functionality. 执行Switch on功能后，自动进入到Enable operation状态。
Transitions列中的数字与状态机中的数字是对应的，代表着不同状态间的过渡。
NOTE： Automatic transition to Enable operation state after executing SWITCHED ON state functionality. 执行Switch on功能后，自动进入到Enable operation状态。
Transitions列中的数字与状态机中的数字是对应的，代表着不同状态间的过渡。
状态字
| 值（二进制） |
描述 |
| xxxx xxxx x0xx 0000 |
未准备好（Not ready to switch on) |
| xxxx xxxx x1xx 0000 |
启动失效(Switch on disabled) |
| xxxx xxxx x01x 0001 |
准备好(Ready to switch on) |
| xxxx xxxx x01x 0011 |
启动(Switched on) |
| xxxx xxxx x01x 0111 |
操作使能(Operation enabled) |
| xxxx xxxx x00x 0111 |
快速停机有效(Quick stop active) |
| xxxx xxxx x0xx 1111 |
故障反应有效(Fault reaction active) |
| xxxx xxxx x0xx 1000 |
故障(Fault) |
驱动器从上电到给电机使能再让电机运行，是串行的逻辑控制，顺序不可以打乱。这就要求驱动器发送的控制字需要遵循一定的逻辑顺序。下图结合了控制字命令和状态字状态的状态机
图片来源倍福官网资料
详细可以参考++CiA402 行规状态机详解 - 知乎 (zhihu.com)++
控制端与驱动端的控制方式
常用的控制方式有位置控制模式（PP、CSP），速度控制模式(PV、CSV)，力矩控制模式(PT、CST)，统称伺服电机的三环控制。目前国内工业机器人领略中常用的控制方式为同步周期位置控制模式（CSP）。
伺服的三环控制关系
电流环为最内环，因为电流是驱动电机的直接因素。此环的计算完全在伺服驱动器内部进行，通过霍尔装置检测驱动器输出的各相电流，负反馈给电流的设定进行PID调节，从而达到输出电流尽量接近等于设定电流。转矩模式下驱动器的运算最小，动态响应最快。
速度环为第二个控制环，它的输入是期望转速，输出是期望电流，即电流环的输入。
位置环为最外层控制环，它的输入是期望的位置，输出是期望的转速，即速度环的输入。位置环控制时，驱动器的运算最大，动态响应最慢，但对上位机的要求较低。
电流环是控制电机的根本，不可缺少。电流对电机的转矩控制精度高，灵活动好，但是对速度和位置的控制却是比较难。当对速度要求较高时，可以用速度环进行控制，当对位置要求高时，可以用位置环进行控制。
速度环和电流环也可以放在上位机内进行运算，此时会对上位机的处理器及运算能力进一步提高要求。
在伺服控制方案中，若使用电流环控制时，可以采用将位置环、速度环的计算移到上位机上，输出期望的电流，达到对位置和速度的高精度要求。也可以采用动力学模型，直接计算出电流的大小，对伺服电流环进行控制。
同步周期位置控制
在同步周期位置模式下，轨迹发生器位于控制端，而不是驱动端。以循环同步方式，它向驱动端提供目标位置。控制端也可以选择提供位置偏移值，速度偏移值和扭矩偏移值，以便允许速度和/或扭矩达成运动前馈。通过传感器测量，驱动端可以向控制端提供位置、速度和扭矩的实际值。
图片来源 CiA-402-2-version-3.0.0
在位置控制模式中，规定也包含了对电机位置、速度、扭矩的限制功能，紧急情况下的快速停止功能，跟随误差功能等。
内插时间周期定义了目标位置和/或附加位置两次更新之间的时间段，应用于周期间内插。
目标位置应为编码器绝对值。
位置实际值是强制输入给控制端的，另外也可以选择速度实际值、扭矩实际值和速度传感器实际值。跟随误差实际值也可用作附加参数。
下图是清能德创RD系列驱动器说明书中的同步周期位置控制框图
CSP 模式下主要的对象字典参数见下表
位置模式下的动力学控制
工业机器人动力学控制的实现，在非集中控制中，运动学和动力学都是为底层电机控制服务的，运动学提供目标量，动力学提供前馈或者不用。在集中控制中，动力学作为系统模型来设计控制率，运动学作为辅助计算机器人的几何关系。目前包括四大家族均是使用第一种方案实现的。
详细见链接机器人运动学与动力学在控制上的区别与联系？
在位置模式下提供动力学前馈，即转矩偏移值0x60B2，速度偏移值。
动力学模型应用
- 计算控制环前馈信号和优化关节参考信号。基于动力学模型可以计算前馈通道的速度和力矩，提升机器人系统的响应性能。
- 改善机器人静态误差：基于机器人弹性动力学模型，对个体差异进行补偿。
- 碰撞检测和负载辨识。
- 支持机器人设计前的仿真验证
- 实施机器人故障辨识和诊断。分析机器人关键参数变化的趋势，判断机器人即将出现问题的可能性。
- 动力学模型是其他高级控制算法的基础。动力学模型是状态估计和控制算法设计的重要基础。
详细见链接机器人动力学模型角色 - 知乎
控制端输入的参数
机器人的杆长模型
不同结构的机器人，需要匹配不同的杆长模型。下面只简述较为通用的六轴工业机器人。
一般六轴工业机器人会涉及到7个杆长数据和旋转方向。杆长模型是机器人运动学模型和动力学模型的基础。其中旋转方向是由机器人的运动学决定的，在算法上可以将顺时针、逆时针任意设置为正方向。也有些厂家在算法上只支持单一的旋转方向，这时机器人实际的正负反向要与杆长的旋转方向一致。
图片来源纳博特官网
| 杆长 |
长度 |
| L1 |
基座中心到 2 轴中心的垂直高度。 |
| L2 |
2 轴中心到 3 轴中心的垂直高度。 |
| L3 |
3 轴中心到 5 轴中心的垂直高度。 |
| L4 |
3 轴中心到 5 轴中心的在机器人侧面观察的水平长度。 |
| L5 |
5 轴中心到 6 轴中心的垂直高度。 |
| L6 |
基座中心到 2 轴中心的在机器人侧面观察的水平长度。 |
| L7 |
2 轴中心到 5 轴中心在机器人正面或背面观察的水平长度。 |
由杆长数据我们自己也可以计算出零点位置时直角坐标系下 X、Z 轴值的组成。
| 轴 |
组成 |
| X 轴 |
L4+L6 |
| Y 轴 |
0 |
| Z 轴 |
L1+L2+L3-L5 |
轴参数及关节参数
| 参数名称 |
数据来源 |
数据作用 |
| 减速比 |
减速机数据 |
运动指令的传动比 |
| 耦合比（关节轴存在联动时，存在着耦合比） |
结构设计输入 |
运动指令的传动比 |
| 编码器分辨率 |
电机编码器数据 |
运动指令的传动比 |
| 电机的最大转速 |
电机数据 |
路径规划速度限制 |
| 电机的扭矩，最高扭矩 |
电机数据 |
动力学规划，运动学前馈，扭矩限制（无动力学规划时，无此参数） |
| 关节轴速度 |
结构设计输入 开发需求输入 |
关节路径规划速度限制 |
| 关节轴加速度/减速度 |
结构设计输入 开发需求输入 |
关节路径规划加速度限制 |
| 关节轴加加速度/加速度变化率 |
算法输入 动力学仿真输入 |
关节路径规划加加速度限制 |
| 笛卡尔速度 |
开发需求输入 结构设计输入 仿真输入 |
笛卡尔路径规划速度限制 |
| 笛卡尔轴加速度/减速度 |
开发需求输入 结构设计输入 仿真输入 |
笛卡尔路径规划加速度限制 |
| 笛卡尔加加速度/加速度变化率 |
开发需求输入 结构设计输入 仿真输入 |
笛卡尔路径规划加加速度限制 |
| 机器人各轴运动范围 |
开发需求输入 结构设计输入 |
机器人运动范围软限制 |
驱动端输入的参数
与上位机ESI的配置
与上位机进行EtherCAT通讯时，需要确认驱动端与控制端是否进行过匹配，这个需要精确到驱动器的系列，因为有些驱动器厂家，不同的系列有着不同的ESI文件。
外部制动电阻参数
电阻功率，电阻阻值，数据来源实际使用的电阻。
电机参数
包含电机的基本属性，如电流，转速，转子惯量等，电机规格数或者电机厂家进行提供。
编码器协议
这里需要关注驱动端与电机编码器的匹配。所填的参数为编码器的类型，分辨率等。
电机的PID调节
PID调节主要通过驱动器厂家提供的驱动器软件进行调节。一般主要调节位置环和速度环的比例积分。
其他参数
驱动端还有些参数需要根据实际应用中进行调节，滤波振动抑制等。
本文引用
CiA Draft Standard Proposal 402 Drives and motion control device profile Part 2: Operation modes and application data -CiA-402-2-version-3.0.0
EtherCAT总线通信架构模型以及CoE参考模型解析Aslier李xl的博客-CSDN博客_coe ethercat
EtherCAT和CANopen之间有什么关系？jacksong2021的博客-CSDN博客canopen和ethercat区别
机器人运动学与动力学在控制上的区别与联系？
机器人动力学模型角色 - 知乎