2026-09-16

04 CiA 402 伺服控制

规范依据：CiA 402（IEC 61800-7-201/301, “Drives and motion control device profile”）、ETG.6010（CiA 402 在 EtherCAT 上的实现导则）
CiA 402 原是 CANopen 设备行规，在 EtherCAT 上通过 CoE（CANopen over EtherCAT） 承载：对象字典、SDO、PDO 概念原样沿用；EtherCAT 只是把 PDO 装进过程数据帧、SDO 走邮箱。市面绝大多数 EtherCAT 伺服（安川、松下、汇川、Elmo、科尔摩根、Copley…）都实现该行规。

1. 总体结构

主站运动控制 (插补/轨迹)
   │  每周期 RxPDO: 控制字 + 目标值        │ 每周期 TxPDO: 状态字 + 实际值
   ▼                                       ▲
EtherCAT 过程数据 (SM2/SM3, DC-SYNC0)──────┘
   │
从站 CoE 对象字典
   ├─ 0x6040/0x6041  控制字/状态字 ──→ 402 电源状态机
   ├─ 0x6060/0x6061  操作模式设定/显示
   └─ 0x60xx         各模式的目标值/实际值/参数
        │
        ▼
驱动器控制环 (位置环→速度环→电流环) → 电机

两条独立但正交的轴线：状态机（能不能动：上电→使能）与操作模式（怎么动：位置/速度/转矩）。

2. 402 电源状态机

stateDiagram-v2
    [*] --> NotReadyToSwitchOn : 上电自检
    NotReadyToSwitchOn --> SwitchOnDisabled : 自动
    SwitchOnDisabled --> ReadyToSwitchOn : Shutdown (0x06)
    ReadyToSwitchOn --> SwitchedOn : Switch On (0x07)
    SwitchedOn --> OperationEnabled : Enable Operation (0x0F)
    OperationEnabled --> SwitchedOn : Disable Operation (0x07)
    SwitchedOn --> ReadyToSwitchOn : Shutdown (0x06)
    ReadyToSwitchOn --> SwitchOnDisabled : Disable Voltage (0x00)
    OperationEnabled --> QuickStopActive : Quick Stop (0x02)
    QuickStopActive --> SwitchOnDisabled : 快停完成(视0x605A)
    QuickStopActive --> OperationEnabled : Enable Operation (0x0F)*
    OperationEnabled --> SwitchOnDisabled : Disable Voltage (0x00)
    NotReadyToSwitchOn --> FaultReactionActive : 故障
    OperationEnabled --> FaultReactionActive : 故障(任意状态)
    FaultReactionActive --> Fault : 故障反应完成
    Fault --> SwitchOnDisabled : Fault Reset (bit7 ↑沿)

（* Quick Stop 后能否直接回 Operation Enabled 取决于 0x605A Quick Stop Option Code；故障可从任意状态进入 Fault Reaction Active，图中只画一条。）

2.1 状态判定：状态字掩码表

状态由 0x6041 的低位按掩码判定（先与掩码，再比较值）：

状态	掩码	值	含义
Not Ready to Switch On	`0x004F`	`0x0000`	上电初始化中，不响应控制字
Switch On Disabled	`0x004F`	`0x0040`	初始化完成，等 Shutdown 命令
Ready to Switch On	`0x006F`	`0x0021`	主回路可上电
Switched On	`0x006F`	`0x0023`	主回路已上电，未使能
Operation Enabled	`0x006F`	`0x0027`	已使能，执行控制环
Quick Stop Active	`0x006F`	`0x0007`	快停减速中
Fault Reaction Active	`0x004F`	`0x000F`	故障反应（按 0x605E 减速）中
Fault	`0x004F`	`0x0008`	故障态，等 fault reset

2.2 状态迁移命令：控制字组合

命令	bit7 fr	bit3 eo	bit2 qs	bit1 ev	bit0 so	典型值
Shutdown	0	×	1	1	0	`0x0006`
Switch On	0	0	1	1	1	`0x0007`
Enable Operation	0	1	1	1	1	`0x000F`
Disable Voltage	0	×	×	0	×	`0x0000`
Quick Stop	0	×	0	1	×	`0x0002`
Disable Operation	0	0	1	1	1	`0x0007`
Fault Reset	↑沿	×	×	×	×	`0x0080`

（so=switch on, ev=enable voltage, qs=quick stop 低有效, eo=enable operation, fr=fault reset。）

3. 控制字 0x6040 / 状态字 0x6041 位定义

3.1 控制字 `0x6040`（UINT16, RxPDO）

位	名称	说明
0	Switch On	状态机命令位
1	Enable Voltage	状态机命令位
2	Quick Stop	低有效：置 0 触发快停
3	Enable Operation	状态机命令位
4–6	模式相关	PP: bit4 new set-point / bit5 change set immediately / bit6 relative；HM: bit4 启动回零；CSP/CSV/CST 不用
7	Fault Reset	上升沿复位故障
8	Halt	暂停当前运动（按减速度停），模式相关
9	模式相关	如 PP 的 change on set-point
10	保留
11–15	厂商自定义

3.2 状态字 `0x6041`（UINT16, TxPDO）

位	名称	说明
0	Ready to Switch On	状态机状态位
1	Switched On	状态机状态位
2	Operation Enabled	状态机状态位
3	Fault	故障
4	Voltage Enabled	主回路电压已建立
5	Quick Stop	低有效：0 表示快停生效中
6	Switch On Disabled	状态机状态位
7	Warning	告警（不改变状态）
8	厂商自定义
9	Remote	控制字受总线控制（=1 才听主站的）
10	Target Reached	到达目标（PP/PV/HM 等；Halt 时表示已停）
11	Internal Limit Active	内部限制生效（软限位/转矩限幅等）
12	模式相关	PP: set-point acknowledge；HM: homing attained；CSP/CSV/CST: drive follows command（驱动器正在跟随目标值，使能后应检查此位）
13	模式相关	HM: homing error；CSP: following error
14–15	厂商自定义

4. 操作模式（0x6060 / 0x6061）

0x6060 Modes of Operation（INT8，写）设定模式；必须读 0x6061 Modes of Operation Display 确认生效（驱动器可能延迟切换或拒绝）。

值	模式	目标值对象	插补/轨迹在哪算	典型用途
1	PP 轮廓位置	`0x607A` + `0x6081` 轮廓速度等	驱动器（梯形/S 曲线自己生成）	点到点，主站只给终点
2	VL 速度（变频器）	`0x6042`	驱动器	变频器类
3	PV 轮廓速度	`0x60FF`	驱动器（按加减速斜坡）	恒速/调速
4	PT 轮廓转矩	`0x6071` + `0x6087` 转矩斜率	驱动器	张力/压装
6	HM 回零	`0x6098` 方法等	驱动器	上电找零
7	IP 插补位置	`0x60C1`	主站给点+驱动器插值	老式周期位置（多被 CSP 取代）
8	CSP 周期同步位置	`0x607A` 每周期更新	主站（细插补），驱动器仅在周期间做微插值	多轴插补主力
9	CSV 周期同步速度	`0x60FF` 每周期更新	主站	主站闭位置环
10	CST 周期同步转矩	`0x6071` 每周期更新	主站	力控/主站闭全环

支持哪些模式查 0x6502 Supported Drive Modes（位图）。

4.1 CSP vs PP：插补分工

	PP（轮廓位置）	CSP（周期同步位置）
主站每周期发什么	不需要每周期发；一次给目标位置+速度+加减速，bit4 触发	每个总线周期发一个新的 `0x607A`（轨迹上的采样点）
轨迹生成	驱动器内部轨迹发生器	主站运动规划器（多轴耦合插补只能在这做）
驱动器做什么	全部：轨迹+三环	位置环+速度环+电流环；周期间可做线性/二阶微插值平滑
多轴协调	差（各驱动器独立生成轨迹）	好（主站统一插补 + DC 同步执行）
对总线要求	低	周期性 + DC（见 03 篇）

机器人关节控制几乎都用 CSP（或力控用 CST）：ros2_control 的周期性 write/read 模型与 CSP 一一对应。

5. 关键对象速查表

索引	名称	类型	方向	说明
`0x603F`	Error Code	U16	T	最近故障码（沿用 CiA 301/402 错误码，如 0x2310 过流、0x3210 过压、0x7305 编码器故障、0x8611 跟随误差过大；厂商可扩展）
`0x6040`	Controlword	U16	R	控制字
`0x6041`	Statusword	U16	T	状态字
`0x605A`	Quick Stop Option Code	I16	SDO	快停后行为（减速停后保持/断使能等）
`0x605E`	Fault Reaction Option Code	I16	SDO	故障时的减速策略
`0x6060` / `0x6061`	模式设定 / 显示	I8	R / T	见上表
`0x6064`	Position Actual Value	I32	T	实际位置（用户单位/counts）
`0x6065`	Following Error Window	U32	SDO	跟随误差报警窗口
`0x606C`	Velocity Actual Value	I32	T	实际速度
`0x6071`	Target Torque	I16	R	目标转矩（千分之额定转矩）
`0x6072`	Max Torque	U16	SDO	转矩限幅
`0x6077`	Torque Actual Value	I16	T	实际转矩
`0x607A`	Target Position	I32	R	目标位置（PP/CSP）
`0x607C`	Home Offset	I32	SDO	零点偏置：机械零 = 回零参考点 + offset
`0x607D`	Software Position Limit	I32×2	SDO	软限位 min/max（sub1/sub2）
`0x6081`	Profile Velocity	U32	SDO/R	PP 轮廓速度
`0x6083` / `0x6084`	Profile Accel / Decel	U32	SDO	PP 加/减速度
`0x6085`	Quick Stop Deceleration	U32	SDO	快停减速度
`0x6098`	Homing Method	I8	SDO	回零方法（1–37 标准 + 负值厂商法）
`0x6099`	Homing Speeds	U32×2	SDO	sub1 找开关速度 / sub2 找零脉冲速度
`0x609A`	Homing Acceleration	U32	SDO	回零加速度
`0x60B0`/`0x60B1`/`0x60B2`	Position/Velocity/Torque Offset	—	R	周期同步模式下的前馈叠加量
`0x60C2`	Interpolation Time Period	—	SDO	插补周期（须与总线周期一致，如 1 ms）
`0x60F4`	Following Error Actual Value	I32	T	实际跟随误差
`0x60FF`	Target Velocity	I32	R	目标速度（PV/CSV）
`0x6502`	Supported Drive Modes	U32	SDO	支持的模式位图

（方向：R = RxPDO 常映射（主→从），T = TxPDO 常映射（从→主），SDO = 通常初始化时用 SDO 配置。多轴一体驱动器第二轴对象在 0x6800+，按 CiA 402 axis offset 0x800 递增。）

6. 典型 PDO 映射示例（CSP 模式）

PDO 映射对象：RxPDO 映射 0x1600~~、TxPDO 映射 0x1A00~~；SM 分配对象 0x1C12（SM2/输出）、0x1C13（SM3/输入）。映射条目格式 = 索引(16) | 子索引(8) | 位长(8)。

RxPDO 0x1600（主站 → 驱动器，共 6 字节）
  0x6040:00 /16bit   控制字
  0x607A:00 /32bit   目标位置

TxPDO 0x1A00（驱动器 → 主站，共 10 字节）
  0x6041:00 /16bit   状态字
  0x6064:00 /32bit   实际位置
  0x60F4:00 /32bit   跟随误差

工程常见扩展：RxPDO 加 0x6060(8bit) 以便在线切模式、0x60B1/0x60B2 速度/转矩前馈、0x6072 转矩限幅；TxPDO 加 0x6061(8bit)、0x606C 实际速度、0x6077 实际转矩、0x603F 错误码、0x60FD 数字输入。修改映射须在 PREOP 状态下通过 SDO 完成（先清 0x1C12/0x1C13 与映射对象的 sub0，写条目，再恢复 sub0）。

7. 使能时序完整示例

前提：从站已进 EtherCAT OP 状态、模式已设为 8（CSP）且 0x6061 读回 8、DC 已收敛。每一步写控制字后轮询状态字直到掩码匹配（每个总线周期查一次，带超时如 500 ms）：

步骤 0: 若 (SW & 0x004F) == 0x0008        // Fault
          写 CW = 0x0080 (fault reset, 需上升沿，之后写回 0x0000)
          等待 (SW & 0x004F) == 0x0040    // Switch On Disabled

步骤 1: 写 CW = 0x0006                    // Shutdown
        等待 (SW & 0x006F) == 0x0021      // Ready to Switch On

步骤 2: 写 CW = 0x0007                    // Switch On（主回路上电）
        等待 (SW & 0x006F) == 0x0023      // Switched On

步骤 3: 【CSP 关键】把 0x607A 预置为当前实际位置 0x6064
        （否则使能瞬间目标≠实际 → 电机飞车或立刻报跟随误差）

步骤 4: 写 CW = 0x000F                    // Enable Operation
        等待 (SW & 0x006F) == 0x0027      // Operation Enabled
        （可再确认 SW bit12 = 1, drive follows command）

之后: 每周期 CW 保持 0x000F，0x607A 按插补轨迹更新（相邻周期增量 = 目标速度×周期）
下使能: 写 0x0006（Shutdown, 自由停/抱闸按驱动器配置）或 0x0002（Quick Stop, 按 0x6085 减速）

许多主站栈把这段写成状态机每周期跑一步（非阻塞），而不是阻塞等待——CSP 下控制字本身就在周期 PDO 里。

8. 故障处理

状态字 bit3=1 进入 Fault；先经 Fault Reaction Active（按 0x605E 定义减速），停稳后落到 Fault 态。
读 0x603F Error Code 获取原因；多数驱动器还提供 0x1003（预定义错误历史，CiA 301）和厂商诊断对象；EtherCAT 侧可结合 AL Status Code 与 CoE Emergency 报文（邮箱，含同样的错误码）定位。
复位：排除故障源后控制字 bit7 打一个上升沿（0x0080 → 0x0000），状态回到 Switch On Disabled，再走一遍使能时序。
注意：有些故障（编码器断线、STO 触发）复位无效，需断电或恢复安全回路；跟随误差类故障复位前务必把 0x607A 重新对齐 0x6064。

9. 回零（Homing）要点

切模式 0x6060 = 6，确认 0x6061 = 6；
SDO 配置：0x6098 方法（常用：1/2 负/正限位+Index，17/18 限位开关不找 Index，33/34 仅 Index，35/37 = 当前位置即零点——绝对值编码器系统常用 35/37）、0x6099 两段速度、0x609A 加速度、0x607C 零点偏置；
在 Operation Enabled 下置控制字 bit4=1 启动回零；
监视状态字：bit12=1 且 bit10=1 = 回零成功（homing attained + target reached）；bit13=1 = 回零出错（未找到开关等）；
完成后把 bit4 清零，切回工作模式（如 CSP），并再次将 0x607A 对齐 0x6064。

方法编号 35 与 37 语义相同（”以当前位置为零”），37 是 CiA 402 后期版本为一致性新增的推荐编号；负值方法为厂商自定义。

10. 多轴同步要点（DC + CSP）

总线周期常用 1 ms / 500 µs / 250 µs（高动态场景 125 µs）；周期越短跟随误差越小、主站实时性要求越高。0x60C2 插补周期须与之一致。
全部轴配 DC-Synchronous（0x1C32:01 = 2），SYNC0 周期 = 总线周期；各轴在同一 SYNC0 沿锁存位置、切换目标值 → 轴间一致性 < 1 µs（见 03 篇）。
主站每周期做一次多轴插补，把同一时刻的各轴目标位置写入各自 RxPDO，一帧带全所有轴（EtherCAT 单帧多从站寻址天然支持）。
主站发帧相位须领先 SYNC0 足够裕量（shift time），否则轴收到旧目标值 → 表现为周期性抖动或 AL 0x001A。
检查项：所有轴 0x6061 = 8、状态字 bit12 = 1、跟随误差 0x60F4 稳定且远小于 0x6065 窗口。
ros2_control 集成时：read() 取 0x6064/0x606C/0x6077 映射到 state interface，write() 把 command interface 写入 0x607A（或 0x60FF/0x6071），周期任务与 EtherCAT 周期同源。

11. 排障 checklist

现象	可能原因	查什么
控制字写了没反应	状态字 bit9 (remote)=0，或从站没进 OP	AL 状态、PDO 是否在刷新
卡在 Switched On 进不了 Enabled	STO 未闭合、主回路未上电（bit4=0）、驱动器条件不满足	状态字 bit4、驱动器面板/诊断对象
使能瞬间飞车 / 立即报警	CSP 下 `0x607A` 未预置为实际位置	使能时序步骤 3
运行中报跟随误差	目标增量超速度极限、周期抖动、增益不足	`0x60F4` 趋势、主站抖动、`0x6065` 窗口
切模式不生效	只写了 `0x6060` 没确认 `0x6061`；部分驱动器要求非使能态切换	读 `0x6061`、查手册约束
Fault 复位不掉	bit7 没有产生上升沿（一直保持 0x80）、故障源仍在	写 0x80 后回 0x00；读 `0x603F`
多轴轨迹错位	未启 DC、shift 配置不当、某轴仍 SM-Sync	各轴 `0x1C32:01`、`0x092C` 收敛情况
PDO 映射改不了	不在 PREOP、映射对象只读（固定映射的驱动器）	ESI/手册确认是否支持动态映射

下一篇：05-主站实现-IgH与SOEM.md

文章互动

阅读 --

留言

0 条留言

正在加载留言…

fishBlog

专注嵌入式开发

04 CiA 402 伺服控制

04 CiA 402 伺服控制

1. 总体结构

2. 402 电源状态机

2.1 状态判定：状态字掩码表

2.2 状态迁移命令：控制字组合

3. 控制字 0x6040 / 状态字 0x6041 位定义

3.1 控制字 `0x6040`（UINT16, RxPDO）

3.2 状态字 `0x6041`（UINT16, TxPDO）

4. 操作模式（0x6060 / 0x6061）

4.1 CSP vs PP：插补分工

5. 关键对象速查表

6. 典型 PDO 映射示例（CSP 模式）

7. 使能时序完整示例

8. 故障处理

9. 回零（Homing）要点

10. 多轴同步要点（DC + CSP）

11. 排障 checklist

文章互动

留言

04 CiA 402 伺服控制

1. 总体结构

2. 402 电源状态机

2.1 状态判定：状态字掩码表

2.2 状态迁移命令：控制字组合

3. 控制字 0x6040 / 状态字 0x6041 位定义

3.1 控制字 0x6040（UINT16, RxPDO）

3.2 状态字 0x6041（UINT16, TxPDO）

4. 操作模式（0x6060 / 0x6061）

4.1 CSP vs PP：插补分工

5. 关键对象速查表

6. 典型 PDO 映射示例（CSP 模式）

7. 使能时序完整示例

8. 故障处理

9. 回零（Homing）要点

10. 多轴同步要点（DC + CSP）

11. 排障 checklist

文章互动

留言

3.1 控制字 `0x6040`（UINT16, RxPDO）

3.2 状态字 `0x6041`（UINT16, TxPDO）