外骨骼柔顺控制：EtherCAT 实时总线与导纳控制

把上一篇选好的关节模组装成一条腿，跑一段录制好的步态轨迹，位置环增益拉满——会发生什么？人一穿上它就开始较劲：机器想让膝关节在 0.8 s 时到 40°，人今天累了想走慢一点，于是轨迹误差累积，位置控制器把误差翻译成越来越大的电机力矩，硬把腿往轨迹上拽。用户的感觉是"被机器人踩着走"，五分钟就累了，还得防着它突然发力。位置控制的外骨骼是矫形器——它强制人走机器的轨迹；力控的外骨骼才是助力器——它跟着人走，在需要时搭把手。

柔顺控制（compliant control）研究的就是怎么让机器"让"着人：不再是"到这个角度"，而是"呈现这么大的阻抗/导纳"，让交互力本身成为被控对象。这篇沿着"让着人"这条主线，从实时总线一路讲到 ros2_control 的控制器插件。

一、1 kHz 力控环：比人快，才有"助力感"

助力感有个生理学硬指标：人的肌电（EMG）反射延迟约 100 ms——从运动神经元放电到肌肉产生可测力矩，脊髓反射也要 50–80 ms。如果外骨骼的响应比这条神经通路还慢，人就会先靠自己发力、然后才等到机器的力，助力变成了迟到的拖拽，人机节律永远对不齐。

机器要"提前"或至少同步跟上人的意图，整条链路要压进 1 ms 以内：

交互力采样 + 抗混叠滤波：~100 μs
导纳/助力控制律解算：~100 μs
总线通信（指令下发 + 状态上传）：< 250 μs
FOC 电流环执行：~50 μs（持续在跑）

这就是 1 kHz 控制环的由来。倒不是什么玄学，就是机器的延迟必须埋进人的反射延迟里，人才感觉不到机器的存在、又感觉得到机器的帮忙。500 Hz 是体感下限，1 kHz 是舒适线，2 kHz 以上收益递减。

延迟不只是"慢"的问题，还有抖动（jitter）。控制环的均值延迟 0.8 ms 但抖动 ±0.5 ms，等于每个周期给导纳外环喂一个随机变化的等效刚度——虚拟阻尼参数 D 是按固定 dt 离散整定的，dt 一抖，阻尼有效值就抖，严重的会直接把稳定的导纳参数推到不稳定区。所以实时系统的两条硬指标——最坏情况延迟（WCET）和抖动上界——在外骨骼上不是性能参数，是安全参数。这也是为什么控制线程要 SCHED_FIFO 锁页内存、为什么内核要 PREEMPT_RT：那些配置不是仪式，是在给抖动上保险。

二、总线：EtherCAT 为什么赢了 CANopen

1 kHz 的环路要跨总线跑一圈，每一跳的延迟和抖动都在吃预算。

CANopen（CiA 402 驱动子协议）是上一代外骨骼的主流：1 Mbps 波特率下，一帧标准报文 8 字节，仲裁 + 传输约 100–200 μs，一条总线挂 6 个关节后再加 RPDO/TPDO 的调度，一个控制周期的确定性就没有保障了。更致命的是没有全网统一时钟：各节点各自采样、各自打时间戳，关节间的同步靠主站轮询的隐式对齐，抖动在几十到几百微秒级——多关节力控的相位一致性就毁在这里。CiA 402 的过程数据对象（PDO）映射和状态机（Profile/Enable Operation 那套）倒是很成熟，这要承认。

EtherCAT 的解法是把总线做成"现场总线里的实时确定性反例"：

分布式时钟（DC）：所有从站时钟锁到参考时钟，全网同步抖动 << 1 μs（典型 < 100 ns）。六个关节的力矩采样真正"同一瞬间"发生，多关节控制才有干净的相位基准。
飞行黑马机制：主站发出的帧经过每个从站时被"顺路"读写，整条总线的 IO 更新在一帧内完成。100 Mbps 线速下 ~1 kByte 过程数据全网刷新 < 50 μs，且抖动接近零。
周期确定性：250 μs 总线周期 + 控制器 1 kHz 解算，时间预算宽裕。

主站方案：上 PREEMPT_RT 实时内核（那篇的老本行）+ IgH EtherCAT 主站（开源、内核态、实时性最好），或 ROS 生态里基于 SOEM 的 ros2_control ethercat driver。硬件侧照旧走隔离 CAN/隔离 PHY 的防护经验。CiA 402 的状态机模型在 EtherCAT 之上以 CoE（CANopen over EtherCAT）形式完整保留——驱动器的使能序列、操作模式切换那套代码还能复用，迁移成本可控。

两种总线的关键指标对照：

指标	CANopen (CiA 402)	EtherCAT + CoE
典型周期（6 关节）	`1–2.5 ms`，抖动大	`250 μs`，抖动 `< 1 μs`
节点间同步	主站轮询隐式对齐，`~100 μs` 级	分布式时钟，`<< 1 μs`
帧机制	逐帧仲裁（CSMA/CA）	飞行黑马，一帧扫全环
时间戳可信度	各节点自由时钟	全网统一参考时钟
生态成熟度	驱动器普遍支持，老而稳	工业标准，CoE 复用 CiA 402 模型
适配场景	单关节调试、低速康复	多关节 1 kHz 力控

最后一行是结论也是提醒：CANopen 没有死，单关节台架、低速康复设备上它依然是性价比之选；当你要做的是"跟人较劲的毫秒级游戏"，EtherCAT 才是入场券。

三、重力补偿与"零力矩透明模式"

有了 1 kHz 的确定性环路，第一件该做的事不是助力，是抵消外骨骼自己。

透明模式（zero-torque / transparency mode）的定义：关节力矩控制器的目标力矩 τ_cmd = τ_gravity + τ_friction_comp，即恰好抵消设备自身的重力和摩擦。做完这一步，用户感觉外骨骼"消失了"，腿上只剩自己身体的惯性——这是验证整套机电系统的金标准（一份重力补偿做得糟糕的外骨骼，透明模式下绑带会持续漂移，人机交互力方差立刻暴露）。

各关节的重力矩 τ_gravity(q) 由动力学模型计算：给定关节角 q，遍历运动链上每个连杆的质量/质心/惯量，把重力投影到关节轴。手写这段又长又容易错——这正是动力学库的战场。Pinocchio（C++，带解析导数，RBDL 的同源后辈）或 RBDL，从 URDF 直接生成：

// 透明模式：Pinocchio 重力补偿 + 摩擦前馈（1 kHz 实时线程内）
#include <pinocchio/algorithm/rnea.hpp>   // 逆动力学
pinocchio::Model model;  pinocchio::Data data(model);
pinocchio::urdf::buildModel("exo_leg.urdf", model);
const int NV = model.nv;

// 预分配，禁止在实时循环里 new
Eigen::VectorXd q(model.nq), v(NV), a(NV), tau_cmd(NV);
Eigen::VectorXd tau_gravity(NV), tau_fric(NV);
v.setZero();  a.setZero();          // 透明模式只补重力，不追踪轨迹

while (running.load(std::memory_order_relaxed)) {
    auto t0 = clock::now();
    readJointStates(q);                       // 负载端编码器，双编码器校正后

    pinocchio::computeGeneralizedGravity(model, data, q);   // ~7 us (6-DoF)
    tau_gravity = data.g;

    for (int i = 0; i < NV; ++i)               // 库仑+粘滞摩擦前馈
        tau_fric(i) = fc[i] * std::tanh(v(i) / 0.05) + fv[i] * v(i);

    tau_cmd = tau_gravity + tau_fric;
    saturate(tau_cmd, tau_limit);              // 限幅，见安全监控
    sendTorqueCommands(tau_cmd);               // EtherCAT CoE, 250 us 周期

    sleep_until(t0 + microseconds(1000));      // 严格 1 kHz
}

computeGeneralizedGravity 对 6 自由度腿型模型只要几微秒，1 kHz 预算内绰绰有余。URDF 里的惯量参数从哪来——CAD 导出再实测校准，那篇数字孪生的方法直接搬过来。

透明模式是基线，助力是在这个基线上做加法。

四、阻抗与导纳：一对对偶，一个选择

柔顺控制的经典框架是 Hogan 1985 的阻抗控制，四十年来的变体都绕着"阻抗/导纳"这对对偶转。一句话讲清对偶关系：

阻抗控制：输入交互力，输出位置偏移——F_ext 进来，位置"让"出去多少由阻抗参数决定。
导纳控制：输入交互力，输出位置/速度轨迹——测量 F_ext，把它喂给一个虚拟质量-阻尼-弹簧模型，解出参考轨迹给底层位置环去跟。

上面两条读着像一句话？差在实现结构：阻抗控制要求执行器本身是力源（直接力矩控制，透明性来自低减速比），导纳控制允许执行器是位置源（内层跑刚性位置环，外层算"该往哪让"）。

导纳的典型形式（虚拟模型方程，行内写法）：

M × a + D × v + K × x = F_ext

M/D/K 分别是虚拟惯量/阻尼/刚度，F_ext 是测到的人机交互力，解出的 x 作为位置环参考。参数怎么调：M 大了感觉"推不动"，K 大了感觉"弹"，康复早期 M 和 K 都压低，让患者觉得推一台漂浮的机器。

两种控制结构的一句话对比与选择依据：

维度	阻抗控制（力源）	导纳控制（位置源+外环）
执行器要求	低减速比、高带宽力矩	任意位置执行器（含高减速比）
环境刚度要求	接触环境要"软"且已知	几乎不敏感（位置环隔离）
透明度上限	高（QDD 直接力控）	受位置环迟滞限制
稳定性风险	环境刚/控制刚比失衡时失稳	虚拟惯量过小或 `dt` 抖动时失稳
典型失效模式	高刚度接触振荡	参数整定不当的低频晃动

外骨骼为什么多用导纳？决定性因素是人机接触环境的刚度不确定。阻抗控制要求交互环境相对"软"才稳定——机械臂磨削、装配的对象刚度已知。而外骨骼绑带+软组织的等效刚度因人而异（胖瘦、穿着层次、绑带松紧），且人在不同时刻会主动绷紧或放松肌肉，环境刚度是时变的。导纳控制在结构上避开了这个雷区：内层位置环刚而快，外层导纳参数只决定"让"的动态，接触刚度的不确定性被位置环的高带宽隔离了。代价是极限透明度不如高成本 QDD 直接力控（多了一层位置环的迟滞），工业实现里这个代价可以接受——稳定压倒一切，尤其对象是人。

实现骨架（实时线程里的导纳外环）：

// 导纳外环：交互力 -> 虚拟模型积分 -> 位置环参考
struct AdmittanceGains { double M, D, K; };        // 每关节独立参数
AdmittanceGains G[NJ] = /* 康复处方下发 */;

double x_ref[NJ] = {0}, v_ref[NJ] = {0};           // 虚拟状态
double x_home[NJ] = {0};                            // 中立位/轨迹偏移基准

for (int j = 0; j < NJ; ++j) {
    double F = interaction_force[j];                // 六维力/SEA 估计
    double dx = x_ref[j] - x_home[j];
    // 虚拟模型数值积分 (半隐式欧拉, dt = 1 ms)
    double a = (F - G[j].D * v_ref[j] - G[j].K * dx) / G[j].M;
    v_ref[j] += a * dt;
    x_ref[j] += v_ref[j] * dt;
    position_loop_setpoint(j, x_ref[j]);            // 交给 1 kHz 位置内环
}

五、按需助力：透明之上做加法

透明模式让机器"让"着人，助力要更进一步：主动出力，但只在人需要时、只出人需要的份量。检测人的意图是第一步，两条技术路线：

力/力矩路线：透明模式下，交互力本身就是人主动力矩的直接测量——人想抬腿，外骨骼"滞后"产生的偏差力就是意图信号。可靠、线性、无创。
肌电路线：sEMG 电极贴股四头肌/胫骨前肌，50–2000 Hz 采样，整流+包络后做意图估计。比力信号早 ~100 ms（力是结果，EMG 是命令），但个体差异大、电极易位就失准，实际系统多作为融合源而非唯一源。

按需助力（assist-as-needed）的策略骨架：估计人的主动力矩 τ_human，机器输出 τ_assist = α × τ_human，α 是助力比例（踝泵 0.3–0.6 起步）。核心设计约束是随节奏不抢节奏：助力做前馈叠加在透明模式之上，而不是接管轨迹——一旦机器抢了主导权，用户会本能减少主动发力（"滑降效应"），康复场景里这直接毁掉训练效果。

落到实时循环里，助力层就是透明基座上的一层加法：

// 按需助力：透明基座 + 比例前馈，绝不接管轨迹
struct AssistGains { double alpha; };          // 每关节助力比例
AssistGains AG[NJ] = /* 处方: 踝 0.5, 膝 0.3, 髋 0.4 起步 */;

for (int j = 0; j < NJ; ++j) {
    // 意图估计：交互力路线（透明模式下偏差力即人的主动力矩）
    double tau_human = interaction_force[j] * moment_arm[j];
    // 可选融合: sEMG 包络做前移补偿, 权重随信噪比自适应
    if (emg_valid[j])
        tau_human = (1 - emg_w) * tau_human + emg_w * emg_map[j];

    double tau_assist = AG[j].alpha * tau_human;
    tau_assist = slew_limit(tau_assist, last_assist[j], dt);  // 限摆率,防突跳
    last_assist[j] = tau_assist;

    tau_cmd[j] = tau_gravity[j] + tau_fric[j] + tau_assist;   // 透明基座之上
}

两个细节值得点破：slew_limit 限制助力的变化率——人的意图估计天然带噪声，直接比例放大会把噪声变成电机的高频抖动；助力叠加在 τ_gravity + τ_fric 基座上而不是替代它——透明是助力的一部分，一台自重都要人扛的机器，助力比例再高也是在"先帮机器再帮人"。

康复阻抗训练是同一套框架的另一个处方：不给助力比例，把导纳参数调"重"——K 给治疗师设定的阻力，患者推着虚拟弹簧做功。助力模式和阻力模式共用一套导纳底座，只换参数包，这是架构上最优雅的地方。

模式参数化之后，临床侧的接口变得极简——治疗师面对的不是控制参数，是处方：

模式	`M`	`D`	`K`	`α` 助力比	临床用途
透明模式	低	低	低	0	系统验证、早期适应
按需助力	低	低	低	`0.3–0.6`	肌力衰退患者的步行辅助
恒定阻力	低	高	高	0	抗阻训练、肌力强化
渐进阻抗	低	中	处方曲线	负值（反向）	肌力评估、等速训练
轨迹导引	高	高	高	0	重瘫患者的被动步态训练

注意最后一行——轨迹导引其实就是本篇开头批判的"位置控制"，但在重瘫场景它是正确的：患者没有主动力可跟随，机器必须主导。控制模式没有高下，只有与患者状态的匹配，这张表从上到下大致对应患者从康复早期到出院的旅程。

六、ros2_control：把控制律做成插件

以上所有模式——透明、助力、阻力——在 ros2_control 里就是三个 controller 插件，共享同一个 ExoSystemInterface 硬件接口（对下封装 IgH 主站的 PDO 读写）。切换模式 = controller_manager 重载资源组合，不用重编译：

# exo_controllers.yaml：模式即插件组合
controller_manager:
  ros__parameters:
    update_rate: 1000                     # 1 kHz，实时线程
    joint_state_broadcaster: { type: joint_state_broadcaster/JointStateBroadcaster }
    gravity_comp:  { type: exo_ctrl/GravityComp }      # 透明模式
    admittance:    { type: exo_ctrl/AdmittanceCtl }    # 柔顺外环
    assist_as_needed: { type: exo_ctrl/AssistPD }      # 按需助力

admittance:
  ros__parameters:
    joints: [hip_l, knee_l, ankle_l, hip_r, knee_r, ankle_r]
    M: [0.20, 0.15, 0.10, 0.20, 0.15, 0.10]   # 虚拟惯量 kg·m²
    D: [2.0, 1.5, 1.0, 2.0, 1.5, 1.0]         # 阻尼
    K: [50.0, 40.0, 30.0, 50.0, 40.0, 30.0]   # 刚度 Nm/rad
    command_interfaces: [position, effort]      # 导纳位置 + 安全力矩钳
    hardware_plugin: exo_hw/EthercatSystem     # IgH 主站封装

控制器先在仿真里跑通，再一行配置切到实机——Gazebo 仿真与 EtherCAT 实机共用同一套控制器代码的完整方法，见ros2_control 硬件在环：仿真与实机无缝切换。

软硬件联动

力控体系再精巧，也有失效的时刻——软件失控、总线断链、控制器死机。力控失效的最后防线是硬件：STO 通道直接切断驱动级功率使能，双通道冗余加医疗级隔离，这条通路与本文的整个软件栈完全独立，见穿戴式安全电路：STO 急停与医疗级隔离。

而导纳外环输出的每一条力矩指令，最终都由那块 FOC 驱动板的电流环兑现——见三相 FOC 驱动板：栅极驱动与电流采样。

参考文献

Hogan, N. "Impedance Control: An Approach to Manipulation: Part I—Theory." Journal of Dynamic Systems, Measurement, and Control, vol. 107, no. 1, pp. 1–7, 1985.（阻抗控制的奠基论文，柔顺控制四十年的起点）
Keemink, A. Q. L., van der Kooij, H., & Stienen, A. H. A. "Admittance Control for Physical Human–Robot Interaction." The International Journal of Robotics Research, vol. 37, no. 11, pp. 1421–1444, 2018.（导纳控制的工程化综述，参数整定与环境刚度稳定性分析）
Carpentier, J., Saurel, G., Buondonno, G., et al. "The Pinocchio C++ Library: A Fast and Flexible Implementation of Rigid Body Dynamics Algorithms and Their Analytical Derivatives." IEEE SII, 2019.（本文重力补偿所用动力学库）
EtherCAT Technology Group. EtherCAT Specification (ETG.1000), IEC 61158/61784 国际标准对应文本.（分布式时钟与飞行黑马机制的权威定义）
CAN in Automation. CiA 402: CANopen Device Profile for Drives and Motion Control, v4.2.（驱动状态机与 PDO 模型，CoE 之上得以复用的遗产）
Tucker, M. R., Olivier, J., Pagel, A., et al. "Control Strategies for Active Lower-Extremity Prosthetics and Orthoses: A Review." Journal of NeuroEngineering and Rehabilitation, vol. 12, no. 1, 2015.（意图检测与按需助力策略的系统性综述）

回头看这条主线：矫形器与助力器的分水岭不在机械，在控制——位置环管住的是角度，力环托住的是人。配合上一篇的机电架构，一套外骨骼的骨与肉就齐了。

本文采用署名-非商业性使用-相同方式共享 4.0 国际许可协议，转载请注明出处。