基于 EtherCAT + CiA402 的双机械臂回零流程解析：从状态字、控制字到目标位置

在 EtherCAT 伺服控制中，很多人一开始会把"上电使能""回零运动""目标位置更新"混在一起理解。尤其是在 CiA402 伺服驱动器里，状态字、控制字、目标位置这几个对象同时出现，如果不把它们的职责分清楚，就很难理解电机到底是怎么从"不能动"一步步进入"可以动"，再开始执行 homing 回零的。

代码实现在：ARM_aging\no_aging\homing_only_project

本文以一个双机械臂 EtherCAT 回零程序为例，讲清楚整个流程：

1. 主站启动 EtherCAT 任务

2. 注册 CiA402 相关 PDO

3. 通过 control_word 推进伺服进入 operation enabled

4. 等待人工确认安全

5. 发送 homing 命令

6. 实时回调消费 homing 命令

7. 在 CSP 模式下不断修改 target_position

8. 驱动器跟随目标位置完成软件回零

这里要先明确一个核心点：

本程序中的 homing 不是通过 CiA402 标准 Homing Mode 完成的，而是在 CSP 模式下，通过主站周期性发送平滑变化的 target_position 来实现回零。

也就是说：

control_word 负责让伺服进入可运动状态；
status_word 负责反馈伺服当前状态；
target_position 负责真正驱动电机按轨迹运动。

---

一、先认识几个关键 PDO 对象

在 CiA402 伺服控制中，最关键的几个对象如下：

对象	地址	方向	含义
control_word	0x6040	主站 → 从站	控制字，用于上电、使能、故障复位等
status_word	0x6041	从站 → 主站	状态字，用于反馈当前 CiA402 状态
mode_of_operation	0x6060	主站 → 从站	运行模式，例如 CSP 模式
mode_of_operation_display	0x6061	从站 → 主站	当前实际运行模式
position_actual_value	0x6064	从站 → 主站	实际位置
target_position	0x607A	主站 → 从站	目标位置

从控制方向上看，可以分成两类：

TxPDO：从站发给主站

• status_word
• position_actual_value
• mode_of_operation_display
• error_code

RxPDO：主站发给从站

• control_word
• mode_of_operation
• target_position

所以：

• status_word 是驱动器告诉主站"我现在是什么状态"；
• control_word 是主站告诉驱动器"你下一步该进入什么状态"；
• target_position 是主站告诉驱动器"你下一周期要运动到哪里"。

这三个变量的职责不同，不能混在一起看。

---

二、PDO 注册：把 C++ 指针绑定到 EtherCAT 对象

程序启动后，会扫描 EtherCAT 从站。如果发现某个从站是 CiA402 伺服驱动器，就会注册 PDO。

例如控制字注册：

task.try_register_pdo_entry(ax->control_word, sp,
    {static_cast<ecat::pdo_index_type>(0x6040 + off), 0});

目标位置注册：

task.try_register_pdo_entry(ax->target_position, sp,
    {static_cast<ecat::pdo_index_type>(0x607a + off), 0});

实际位置注册：

task.try_register_pdo_entry(ax->position_actual_value, sp,
    {static_cast<ecat::pdo_index_type>(0x6064 + off), 0});

这几句代码的含义是：

把 ax->control_word 绑定到从站 PDO 中的 0x6040 控制字；
把 ax->target_position 绑定到从站 PDO 中的 0x607A 目标位置；
把 ax->position_actual_value 绑定到从站 PDO 中的 0x6064 实际位置。

注册完成后，程序里写：

*axis->control_word = xxx;

本质上就是修改主站本地 PDO 输出区里 0x6040 对应的值。

程序里写：

*axis->target_position = xxx;

本质上就是修改主站本地 PDO 输出区里 0x607A 对应的值。

下一次 EtherCAT 周期发送时，这些数据会通过 PDO 发送给伺服驱动器。

---

三、主流程：先上电，再 homing

主程序大致流程如下：

master0.start();
master1.start();

while (!master0.is_all_enabled() || !master1.is_all_enabled())
{
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

wait_for_enter("Press Enter to start homing...");

master0.begin_homing();
master1.begin_homing();

while (!master0.is_homing_done() || !master1.is_homing_done())
{
    ...
}

master0.hold_position();
master1.hold_position();

这段流程说明了一个重要设计：

程序不会一启动就 homing，而是先等待所有关节进入 operation enabled，再让用户确认安全，最后才开始回零。

这里的 is_all_enabled() 判断的是：

(*ax->status_word & 0x006f) == 0x0027

也就是说，只要状态字满足：

status_word & 0x006F = 0x0027

程序就认为该轴已经进入：

operation enabled

此时伺服才真正具备运动条件。

---

四、CiA402 上电阶段：状态字和控制字怎么变化

CiA402 伺服不是直接写目标位置就能运动。它必须先经过状态机。

程序中通过状态字识别当前状态：

{{0x004f, 0x0040}, switch_on_disabled},
{{0x006f, 0x0021}, ready_to_switch_on},
{{0x006f, 0x0023}, switched_on},
{{0x006f, 0x0027}, operation_enabled},

这些匹配不是简单判断：

status_word == 0x0027

而是判断：

(status_word & mask) == expected

例如 operation enabled 的判断是：

(status_word & 0x006f) == 0x0027

这样可以忽略一些无关 bit，例如 warning、remote、target reached 等。

状态字和控制字的正常变化如下：

周期	从站反馈 status_word	主站识别状态	主站写入 control_word	含义
1	0x0040	switch_on_disabled	0x0006	shutdown
2	0x0021	ready_to_switch_on	0x0007	switch on
3	0x0023	switched_on	0x000F	enable operation
4	0x0027	operation_enabled	保持 0x000F	已经使能成功

所以完整上电链路是：

status_word:   0x0040 → 0x0021 → 0x0023 → 0x0027
control_word:  0x0006 → 0x0007 → 0x000F → 0x000F

这里要注意一个细节：

本周期写入 control_word 后，驱动器通常要到下一个 EtherCAT 周期才反馈新的 status_word。

也就是说，状态字和控制字之间存在周期性的反馈延迟。

---

五、control_word 到底在哪里被修改？

在 power::on_cycle() 中，控制字通过这句被修改：

cmd->set(*axis->control_word);

cmd 是根据当前 CiA402 状态查表得到的位模式。例如当前状态是 switched_on，上电命令是：

mask     = 0x008f
expected = 0x000f

执行：

cmd->set(*axis->control_word);

等价于：

*axis->control_word &= ~0x008f;
*axis->control_word |=  0x000f;

也就是把控制字中与 CiA402 状态机相关的 bit 设置成 0x000F。

0x000F 的作用就是：

enable operation

让驱动器进入：

operation enabled

所以，control_word 的修改位置不在主函数里，也不直接出现在回调函数表面，而是在每周期调用的 power::on_cycle() 里。

---

六、为什么 homing 前要设置 CSP 模式？

当状态进入 switched_on 时，程序会执行：

if (state == cia402_state::switched_on && axis->mode_of_operation)
    *axis->mode_of_operation = 8;

这里的：

mode_of_operation = 8

表示：

CSP：Cyclic Synchronous Position，周期同步位置模式

在 CSP 模式下，主站每个周期都可以发送新的目标位置：

target_position

驱动器根据这个目标位置进行位置控制。

因此，这个程序里的 homing 本质是：

先把驱动器使能到 operation enabled；
再进入 CSP 模式；
主站每周期修改 target_position；
驱动器跟随 target_position 回到软件零点。

它不是使用 CiA402 标准 Homing Mode，也就是不是通过：

mode_of_operation = 6

让驱动器自己执行回零。

---

七、homing 命令是怎么发出的？

当用户确认机械臂安全后，主程序调用：

master0.begin_homing();
master1.begin_homing();

进入：

void ArmProgram::begin_homing()
{
    g_cmd_homing[master_id].store(true, std::memory_order_relaxed);
}

也就是说，主线程只是把一个原子标志置为 true：

g_cmd_homing[0] = true
g_cmd_homing[1] = true

主线程不会直接改电机目标位置，也不会直接写控制字。

这样设计的好处是：

主线程只负责发命令；
实时 EtherCAT 回调线程负责真正消费命令并修改运动状态。

这可以避免主线程和实时线程同时修改同一个运动对象，降低线程竞争风险。

---

八、实时回调：消费 homing 命令

EtherCAT 每个周期收到从站反馈后，会执行 receive callback：

task.set_receive_callback([mid, &progs] {
    if (g_cmd_homing[mid].load(std::memory_order_relaxed))
    {
        g_cmd_homing[mid].store(false, std::memory_order_relaxed);
        for (auto &p : progs)
        {
            if (p.axis && p.axis->position_actual_value)
                p.motion.start_homing(*p.axis->position_actual_value);
        }
    }

    if (g_cmd_hold[mid].load(std::memory_order_relaxed))
    {
        g_cmd_hold[mid].store(false, std::memory_order_relaxed);
        for (auto &p : progs)
            p.motion.hold();
    }

    for (auto &p : progs)
        p();
});

这里做了三件事：

1. 检查是否收到 homing 命令
2. 如果收到，就以当前实际位置作为回零起点
3. 最后执行每个轴的周期控制逻辑 p()

例如某个轴当前位置是：

position_actual_value = 100000

那么执行：

p.motion.start_homing(*p.axis->position_actual_value);

就等价于：

p.motion.start_homing(100000);

start_homing() 内部会做：

mode = MotionMode::HOMING;
cur_target = actual_pos;
homing_done = false;

也就是：

mode = HOMING
cur_target = 100000
homing_done = false

注意，这里仍然没有直接写 target_position。

它只是告诉 motion 模块：

从当前位置开始，准备往 0 运动。

---

九、target_position 到底在哪里改变？

真正改变目标位置的是 program::operator()() 中这一句：

*axis->target_position = motion.update(actual);

完整逻辑如下：

void operator()()
{
    if (!axis)
        return;

    power_.axis = axis;
    power_.enable = axis->power_enable;
    power_.on_cycle();

    if (!axis->target_position || !axis->position_actual_value)
        return;

    if (!power_.status)
    {
        *axis->target_position = *axis->position_actual_value;
        return;
    }

    const std::int32_t actual = *axis->position_actual_value;
    *axis->target_position = motion.update(actual);
}

这里有两个阶段。

第一阶段，如果伺服还没有进入 operation enabled：

*axis->target_position = *axis->position_actual_value;

这样可以让目标位置跟随实际位置，防止刚上电瞬间目标位置突变。

第二阶段，如果伺服已经进入 operation enabled：

*axis->target_position = motion.update(actual);

这时 motion 模块会根据当前模式生成新的目标位置。

如果 mode 是 HOMING，目标位置就会逐步朝 0 靠近。

---

十、HomingMotion 如何生成目标位置？

HomingMotion 的核心逻辑是：

const std::int32_t err = 0 - cur_target;

if (err > 0)
    cur_target += std::min(homing_step_counts, err);
else if (err < 0)
    cur_target -= std::min(homing_step_counts, -err);

homing_done = (cur_target == 0);
if (homing_done)
    mode = MotionMode::HOLDING;

return cur_target;

这段代码的含义是：

如果当前目标位置大于 0，就每周期减小一点；
如果当前目标位置小于 0，就每周期增大一点；
直到 cur_target 等于 0；
到 0 后进入 HOLDING 状态。

所以 homing 不是一步跳到 0，而是平滑靠近 0。

---

十一、用具体数值看一次 homing

假设当前已经完成上电：

status_word = 0x0027
control_word = 0x000F
mode_of_operation = 8

假设当前位置是：

position_actual_value = 100000

执行 homing 后：

start_homing(100000);

motion 内部变成：

mode = HOMING
cur_target = 100000

假设每周期步长：

homing_step_counts = 1000

那么每个 EtherCAT 周期执行：

*axis->target_position = motion.update(actual);

目标位置变化如下：

周期	status_word	control_word	target_position
homing 前	0x0027	0x000F	100000
第 1 周期	0x0027	0x000F	99000
第 2 周期	0x0027	0x000F	98000
第 3 周期	0x0027	0x000F	97000
第 4 周期	0x0027	0x000F	96000
...	0x0027	0x000F	...
最后	0x0027	0x000F	0

可以看到：

homing 过程中 status_word 基本保持 0x0027；
homing 过程中 control_word 基本保持 0x000F；
真正变化的是 target_position。

这就是 CSP 回零的核心。

---

十二、真实代码中的步长如何计算？

程序中 homing 步长不是写死的，而是根据：

homing_speed_deg_s
counts_per_deg
cycle_dt

计算：

homing_step_counts =
    homing_speed_deg_s * counts_per_deg * cycle_dt;

默认参数是：

homing_speed_deg_s = 10 deg/s
cycle_dt = 0.001 s

因此每周期角度变化是：

10 deg/s × 0.001 s = 0.01 deg

也就是说，默认情况下，主站每 1 ms 让目标位置变化 0.01 度对应的编码器 counts。

如果某个轴的 counts_per_deg 越大，那么每周期变化的 counts 就越大；如果某个轴的 counts_per_deg 越小，那么每周期变化的 counts 就越小。

所以 homing 的真实数值由编码器分辨率、减速比和周期共同决定。

---

十三、homing 完成后发生什么？

当 cur_target == 0 时，motion 内部会设置：

homing_done = true
mode = HOLDING

主程序判断 homing 是否完成时，看的是实际位置：

abs(position_actual_value) <= 0.5 deg 对应的 counts

也就是说，程序不是只看目标位置到没到 0，而是看实际位置是否已经接近 0。

当所有轴都完成后，主程序调用：

master0.hold_position();
master1.hold_position();

这会设置：

g_cmd_hold[0] = true
g_cmd_hold[1] = true

下一个 EtherCAT 回调中，motion 会进入 HOLDING 状态。

此时：

status_word 仍然保持 0x0027
control_word 仍然保持 0x000F
target_position 保持最后的目标位置

也就是说，hold 并不是立刻下电，而是让电机保持当前位置。

---

十四、什么时候 control_word 会从 0x000F 变掉？

homing 完成后，如果只是 hold，控制字仍然保持：

0x000F

只有当程序准备退出并调用：

setPowerDisabled();

才会把：

axis->power_enable = false

然后 power::on_cycle() 会走 disable 逻辑。

如果当前状态是 operation_enabled，disable 命令会把控制字改成：

0x0007

对应关系是：

operation_enabled + disable → control_word = 0x0007

驱动器会从：

operation enabled

退回到：

switched on

所以完整生命周期可以总结为：

上电阶段：
control_word: 0x0006 → 0x0007 → 0x000F
status_word:  0x0040 → 0x0021 → 0x0023 → 0x0027

homing 阶段：
control_word: 0x000F → 0x000F → 0x000F
status_word:  0x0027 → 0x0027 → 0x0027
target_position: actual → ... → 0

hold 阶段：
control_word: 0x000F
status_word:  0x0027
target_position: 保持最后值

下电阶段：
control_word: 0x000F → 0x0007 → 可能继续到 0x0000
status_word:  根据驱动器状态逐步变化

---

十五、完整数据流总结

整个系统可以按照下面这条链路理解：

main.cpp
    ↓
启动 master0 / master1
    ↓
EtherCAT task 周期运行
    ↓
注册 PDO：
    0x6040 control_word
    0x6041 status_word
    0x607A target_position
    0x6064 position_actual_value
    ↓
power::on_cycle()
    ↓
根据 status_word 判断 CiA402 状态
    ↓
写 control_word：
    0x0006 → 0x0007 → 0x000F
    ↓
伺服进入 operation enabled
    ↓
用户按 Enter
    ↓
begin_homing()
    ↓
g_cmd_homing[mid] = true
    ↓
receive_callback 消费 homing 命令
    ↓
motion.start_homing(position_actual_value)
    ↓
每周期 p()
    ↓
power.on_cycle() 保持 control_word = 0x000F
    ↓
motion.update(actual)
    ↓
写 target_position
    ↓
驱动器在 CSP 模式下跟随目标位置回零
    ↓
实际位置接近 0
    ↓
hold_position()
    ↓
保持当前位置

---

十六、最后总结

这个 EtherCAT homing-only 程序的关键点可以总结成三句话。

第一，状态字 status_word 是从站反馈给主站的，它告诉主站伺服当前处于哪个 CiA402 状态。

典型上电过程是：

0x0040 → 0x0021 → 0x0023 → 0x0027

第二，控制字 control_word 是主站写给从站的，它负责推动伺服进入可运动状态。

典型控制过程是：

0x0006 → 0x0007 → 0x000F

第三，homing 过程中真正变化的是 target_position，不是 control_word。

当伺服已经进入 operation enabled 后，控制字保持：

0x000F

状态字保持：

0x0027

主站在 CSP 模式下不断更新：

target_position

让目标位置从当前位置平滑靠近 0，从而实现软件回零。

所以，这个程序的本质不是"用控制字完成 homing"，而是：

用控制字完成伺服使能；
用 CSP 模式和 target_position 完成回零运动。