“Arduino BLDC 之双足康复机器人步态相位阻抗切换辅助训练”可以理解为：以 BLDC 驱动髋、膝等下肢关节，在 FOC 电流环基础上构建阻抗控制；上层通过编码器、IMU、足底压力或交互力传感器识别步态相位；中层根据支撑相、摆动相、双支撑相以及患者康复等级动态调整刚度、阻尼和辅助轨迹；下层由 BLDC 柔顺执行，从而实现从被动带动到主动辅助、再到抗阻训练的分级康复。其核心不是让机器人“强行拖着腿走”，而是让机器人在不同步态阶段呈现不同的力学特性，既保证安全支撑，又鼓励患者主动参与。
一、主要特点

1. 步态相位驱动阻抗切换
   双足步态通常分为支撑相、摆动相和双支撑相。支撑相需要较高刚度与阻尼，保证承重和姿态稳定；摆动相需要较低刚度与阻尼，降低拖动感和能耗；双支撑相则需要平滑过渡，避免左右腿辅助力突变。
2. 阻抗控制比纯位置控制更柔顺
   纯位置控制会把关节刚性拉到目标角度，容易与患者主动发力对抗。阻抗控制把关节建模为“虚拟弹簧-阻尼系统”，通过刚度 Kp 和阻尼 Kd 调节人机交互力，使机器人既能提供轨迹引导，又允许患者参与发力。
3. 多传感器融合识别相位与意图
   常用信号包括关节编码器、IMU、足底压力/FSR、交互力矩传感器、电流反馈，部分方案还会引入 EMG。相位识别不能只依赖单一阈值，例如足底压力高不一定完全可靠，需结合关节角度、角速度、IMU 姿态和电流变化综合判断。
4. 康复等级可映射为辅助强度
   初期康复可提高辅助比例，采用较低刚度、较强轨迹引导；中期降低辅助，让患者更多参与；后期可进入轻辅助或抗阻训练。阻抗参数应随康复进度、疲劳状态和主动发力程度动态调整。
5. BLDC FOC 是实现柔顺辅助的执行基础
   FOC 电流环可提供低转矩脉动、可预测的力矩输出，适合低速、频繁加减速和人体交互场景。若使用开环 ESC，力矩不线性，容易造成顿挫、拉扯和安全隐患。
6. 相位切换必须平滑，不能硬跳变
   从摆动相切到支撑相、从轻辅助切到强辅助时，刚度、阻尼和目标轨迹应渐变。否则会出现“猛推”“猛拉”“膝盖突然锁定/解锁”等危险感受。
7. Arduino 平台适合做轻量原型或下位执行层
   ATmega328P 可承担单关节 FOC、简单相位判断和状态机；若同时运行多关节阻抗控制、IMU 融合、足底压力滤波、康复等级调度和安全监控，更建议使用 ESP32-S3、Teensy 4.1、STM32 或“上位机决策 + 下位机执行”架构。
   二、应用场景
8. 中风后偏瘫步态训练
   记录健侧腿轨迹，在患侧腿摆动相提供柔顺引导，在支撑相提供稳定支撑，帮助重建对称步态。
9. 脊髓损伤早期康复
   对主动发力较弱的患者采用较高辅助比例，完成髋膝屈伸和步态周期训练，维持关节活动度。
10. 术后下肢功能恢复
    用于膝关节、髋关节术后早期训练，限制活动范围并提供柔顺辅助，避免过度负荷。
11. 老年步态辅助与防跌倒训练
    通过支撑相增稳、摆动相减阻，改善抬腿不足、拖步和步态不稳。
12. 康复评估与数据记录平台
    记录关节角度、角速度、交互力矩、步频、步长和辅助力变化，为治疗师调整训练强度提供依据。
13. 科研与教学原型
    适合验证步态相位识别、阻抗参数调度、FOC 力矩跟踪、人机交互安全和康复等级自适应。
    三、需要注意的事项
14. 安全优先级高于控制性能
    康复机器人直接作用于人体，必须设置角度限位、力矩钳位、电流限幅、急停按钮、看门狗和传感器异常检测。一旦数据异常，应立即进入零力矩或低阻尼安全状态。
15. 相位识别要有滞回和时间窗口
    足底压力、IMU 和编码器信号都可能瞬时跳变。应使用滑动窗口、滞回阈值和多源一致性校验，避免相位频繁翻转。
16. 阻抗切换要渐变，禁止硬切换
    刚度、阻尼和目标轨迹变化应加斜率限制。尤其在足跟触地和足尖离地瞬间，若参数突变，容易造成膝冲击或髋部拉扯。
17. 摩擦补偿与重力补偿不能省略
    减速器摩擦、连杆重力和绑带预紧都会影响交互力估计。若不补偿，机器人可能把自身负载误判为患者发力，导致辅助过多或过少。
18. 电流环稳定是柔顺控制前提
    FOC 的电流采样、编码器角度、母线电压、PWM 频率和死区补偿必须先校准。电流环不稳定时，阻抗控制会表现为抖动、滞后或力矩漂移。
19. 传感器滤波要兼顾延迟
    IMU、足底压力和 EMG 都需要滤波，但滤波过强会引入相位滞后，使辅助力晚于患者意图。应通过实际穿戴测试平衡噪声抑制与响应延迟。
20. 机械贴合度直接影响体验
    绑带过松会导致力传递延迟，过紧会影响舒适性和血液循环。调试时不能只看角度误差，还要观察交互力波形、电流响应和受试者主观感受。
21. Arduino 算力边界要清晰
    单关节验证可行；双髋双膝四路 FOC、IMU 融合、足底压力滤波和阻抗调度同时运行时，8 位 MCU 容易成为瓶颈。建议把安全监控、相位调度和日志放在更高层，底层保持固定周期电流/位置闭环。
22. 电池压降和热管理必须监控
    站立、起步和支撑相力矩较大，电池压降可能引起 MCU 复位或传感器漂移。控制电源应独立稳压，动力线与控制线分开，并监控电机温度和母线电压。
23. 训练模式切换需有明确边界
    被动、主动辅助和抗阻训练之间不能随意跳转。应设置康复等级确认、最小保持时间、异常退出条件和治疗师授权，防止误切模式造成不适或二次损伤。

整体来看，这套系统的核心链路是：多传感器识别步态相位 → 根据相位和康复等级调度刚度、阻尼与辅助轨迹 → BLDC FOC 阻抗/力矩柔顺执行 → 编码器、IMU 和交互力反馈闭环修正 → 安全降级保护。做原型时，建议按“单关节 FOC 电流环 → 固定参数阻抗控制 → 步态相位识别 → 相位相关阻抗切换 → 康复等级辅助调度 → 安全限位与急停”逐步推进。

1、基于足底FSR的相位识别 + 髋/膝关节阻抗参数查表切换
适用场景：患者穿戴双足外骨骼进行平地步行训练。足底压力传感器检测支撑/摆动相，髋关节在摆动相降低刚度以顺应前摆惯性，膝关节在支撑相提高刚度以稳定承重。核心逻辑是“相位识别→查表→阻抗力矩计算”。

#include <SimpleFOC.h>

// 左腿：髋关节 + 膝关节
BLDCMotor hipL(7), kneeL(7);
BLDCDriver3PWM hipDriverL(9, 10, 11, 8), kneeDriverL(3, 5, 6, 4);
Encoder hipEncL(18, 19, 2048), kneeEncL(20, 21, 2048);

// 右腿
BLDCMotor hipR(7), kneeR(7);
BLDCDriver3PWM hipDriverR(44, 45, 46, 43), kneeDriverR(41, 42, 40, 39);
Encoder hipEncR(30, 31, 2048), kneeEncR(32, 33, 2048);

// 足底压力
const int FSR_L = A0;
const int FSR_R = A1;
const int CONTACT_TH = 200;

// 步态相位
enum GaitPhase { STANCE, SWING };
GaitPhase phaseL = SWING, phaseR = STANCE;

// 阻抗参数表：{Kp, Kd}
struct ImpParams { float Kp, Kd; };
// 支撑相：高刚度稳定承重；摆动相：低刚度顺应惯性
ImpParams hipTable[2]  = {{8.0, 0.8}, {2.0, 0.3}};  // STANCE, SWING
ImpParams kneeTable[2] = {{12.0, 1.0}, {3.0, 0.4}};

// 参考角度（简化：上层步态规划给出）
float targetHipL = 0, targetKneeL = 0;
float targetHipR = 0, targetKneeR = 0;

void setup() {
  Serial.begin(115200);
  pinMode(FSR_L, INPUT); pinMode(FSR_R, INPUT);

  hipEncL.init(); kneeEncL.init();
  hipEncR.init(); kneeEncR.init();

  hipL.linkSensor(&hipEncL); kneeL.linkSensor(&kneeEncL);
  hipR.linkSensor(&hipEncR); kneeR.linkSensor(&kneeEncR);

  hipDriverL.voltage_power_supply = 24; kneeDriverL.voltage_power_supply = 24;
  hipDriverR.voltage_power_supply = 24; kneeDriverR.voltage_power_supply = 24;
  hipDriverL.init(); kneeDriverL.init();
  hipDriverR.init(); kneeDriverR.init();

  hipL.linkDriver(&hipDriverL); kneeL.linkDriver(&kneeDriverL);
  hipR.linkDriver(&hipDriverR); kneeR.linkDriver(&kneeDriverR);

  hipL.controller = MotionControlType::torque;
  kneeL.controller = MotionControlType::torque;
  hipR.controller = MotionControlType::torque;
  kneeR.controller = MotionControlType::torque;

  hipL.init(); hipL.initFOC();
  kneeL.init(); kneeL.initFOC();
  hipR.init(); hipR.initFOC();
  kneeR.init(); kneeR.initFOC();
}

void updatePhase() {
  int fsrL = analogRead(FSR_L);
  int fsrR = analogRead(FSR_R);

  // 左腿相位
  if (phaseL == SWING && fsrL > CONTACT_TH) {
    phaseL = STANCE;
    Serial.println("左腿 → 支撑相");
  } else if (phaseL == STANCE && fsrL < CONTACT_TH * 0.4) {
    phaseL = SWING;
    Serial.println("左腿 → 摆动相");
  }

  // 右腿相位
  if (phaseR == SWING && fsrR > CONTACT_TH) {
    phaseR = STANCE;
    Serial.println("右腿 → 支撑相");
  } else if (phaseR == STANCE && fsrR < CONTACT_TH * 0.4) {
    phaseR = SWING;
    Serial.println("右腿 → 摆动相");
  }
}

void loop() {
  hipL.loopFOC(); kneeL.loopFOC();
  hipR.loopFOC(); kneeR.loopFOC();

  updatePhase();

  // ===== 左腿阻抗执行 =====
  ImpParams hipP_L = hipTable[phaseL];
  ImpParams kneeP_L = kneeTable[phaseL];

  float tauHipL = hipP_L.Kp * (targetHipL - hipL.shaftAngle())
                  - hipP_L.Kd * hipL.shaftVelocity();
  float tauKneeL = kneeP_L.Kp * (targetKneeL - kneeL.shaftAngle())
                   - kneeP_L.Kd * kneeL.shaftVelocity();

  hipL.move(constrain(tauHipL, -3.0, 3.0));
  kneeL.move(constrain(tauKneeL, -3.0, 3.0));

  // ===== 右腿阻抗执行 =====
  ImpParams hipP_R = hipTable[phaseR];
  ImpParams kneeP_R = kneeTable[phaseR];

  float tauHipR = hipP_R.Kp * (targetHipR - hipR.shaftAngle())
                  - hipP_R.Kd * hipR.shaftVelocity();
  float tauKneeR = kneeP_R.Kp * (targetKneeR - kneeR.shaftAngle())
                   - kneeP_R.Kd * kneeR.shaftVelocity();

  hipR.move(constrain(tauHipR, -3.0, 3.0));
  kneeR.move(constrain(tauKneeR, -3.0, 3.0));

  delay(10);
}

解读：该案例直接采用了搜索结果中反复出现的“步态相位驱动阻抗参数切换”框架。hipTable/kneeTable 数组存储支撑相和摆动相的两组 {Kp, Kd}，updatePhase() 通过足底 FSR 阈值判定相位，执行层用 Kp*(目标-实际) - Kd*速度 计算力矩。BLDC 的 FOC 力矩模式使 move() 直接接收力矩指令，而非速度目标，这是阻抗控制能够“柔顺”的前提。搜索结果强调“支撑相初期提高刚度稳定承重，摆动相降低刚度增加阻尼”，与查表逻辑一致。

2、基于触地冲击强度的自适应刚度软化（落地缓冲）
场景：患者在步行中足跟触地瞬间，地面反作用力会产生冲击。如果支撑相一开始就使用高刚度，冲击会沿机械结构传递到患者关节。需要根据触地瞬间的力变化率动态软化初始刚度，冲击衰减后再恢复高刚度支撑。

#include <SimpleFOC.h>

BLDCMotor kneeMotor(7);
BLDCDriver3PWM kneeDriver(9, 10, 11, 8);
Encoder kneeEnc(2, 3, 2048);

const int FSR_PIN = A0;
const int CONTACT_TH = 150;
const int IMPACT_TH = 400;

// 虚拟刚度
float kSoft = 1.5;      // 缓冲刚度
float kHard = 10.0;     // 支撑刚度
float kNow = kSoft;
float bDamp = 0.4;

float targetAngle = 0;
bool contact = false;
float prevForce = 0;

void setup() {
  Serial.begin(115200);
  kneeEnc.init();
  kneeMotor.linkSensor(&kneeEnc);
  kneeDriver.voltage_power_supply = 24;
  kneeDriver.init();
  kneeMotor.linkDriver(&kneeDriver);
  kneeMotor.controller = MotionControlType::torque;
  kneeMotor.voltage_limit = 6.0;
  kneeMotor.init();
  kneeMotor.initFOC();
}

void loop() {
  kneeMotor.loopFOC();

  float force = analogRead(FSR_PIN);
  float dForce = force - prevForce;
  prevForce = force;

  // 触地事件
  if (!contact && force > CONTACT_TH) {
    contact = true;
    float impact = (dForce > 0) ? dForce : 0;
    // 冲击越大，初始刚度越低（软化）
    kNow = kSoft / (1.0 + impact * 0.01);
    Serial.print("触地 冲击="); Serial.print(impact);
    Serial.print(" 初始刚度="); Serial.println(kNow);
  }

  // 离地
  if (contact && force < CONTACT_TH * 0.4) {
    contact = false;
    kNow = kSoft;
    Serial.println("离地");
  }

  // 刚度调度：冲击衰减后转高刚度支撑
  if (contact && force < IMPACT_TH) {
    kNow = kHard;
  }

  // 虚拟弹簧-阻尼力矩
  float tau = kNow * (targetAngle - kneeMotor.shaftAngle())
              - bDamp * kneeMotor.shaftVelocity();
  kneeMotor.move(constrain(tau, -2.5, 2.5));

  delay(10);
}

解读：该案例借鉴了搜索结果中单腿触觉落地缓冲的“虚拟弹簧-阻尼”逻辑。核心是 kNow = kSoft / (1.0 + impact * 0.01)：触地冲击越大，初始刚度越低，使膝关节在冲击瞬间“让一下”，衰减后再切到 kHard 提供稳定支撑。对于双足康复机器人，这一逻辑可应用于支撑相开始的 100-200ms 窗口内，避免步态相位切换瞬间的刚度阶跃对患者关节造成冲击。搜索结果中膝关节外骨骼的阻抗控制同样强调“支撑相初期需要缓冲而非刚性锁定”。

3、双足交替相位状态机 + 髋关节摆动相助力衰减
场景：患者在摆动相主动抬腿时，外骨骼不应过度助力，否则会“抢”患者的运动意图。需要根据摆动相内的关节角速度动态调整助力增益：患者摆得快，助力自动衰减；患者摆得慢，助力维持以帮助完成动作。

#include <SimpleFOC.h>

BLDCMotor hipL(7), hipR(7);
BLDCDriver3PWM hipDriverL(9, 10, 11, 8), hipDriverR(3, 5, 6, 4);
Encoder hipEncL(18, 19, 2048), hipEncR(20, 21, 2048);

const int FSR_L = A0, FSR_R = A1;
const int CONTACT_TH = 180;

enum Phase { STANCE, SWING };
Phase phaseL = SWING, phaseR = STANCE;

// 摆动相助力参数
float assistGain = 1.2;       // 基础助力
float assistDecay = 0.005;    // 速度衰减系数
float stanceDamping = 1.5;    // 支撑相阻尼

float targetHipL = 0, targetHipR = 0;

void setup() {
  Serial.begin(115200);
  pinMode(FSR_L, INPUT); pinMode(FSR_R, INPUT);

  hipEncL.init(); hipEncR.init();
  hipL.linkSensor(&hipEncL); hipR.linkSensor(&hipEncR);
  hipDriverL.voltage_power_supply = 24; hipDriverR.voltage_power_supply = 24;
  hipDriverL.init(); hipDriverR.init();
  hipL.linkDriver(&hipDriverL); hipR.linkDriver(&hipDriverR);
  hipL.controller = MotionControlType::torque;
  hipR.controller = MotionControlType::torque;
  hipL.init(); hipL.initFOC();
  hipR.init(); hipR.initFOC();
}

void updatePhases() {
  int fL = analogRead(FSR_L), fR = analogRead(FSR_R);
  if (phaseL == SWING && fL > CONTACT_TH) phaseL = STANCE;
  else if (phaseL == STANCE && fL < CONTACT_TH * 0.4) phaseL = SWING;
  if (phaseR == SWING && fR > CONTACT_TH) phaseR = STANCE;
  else if (phaseR == STANCE && fR < CONTACT_TH * 0.4) phaseR = SWING;
}

float computeHipTorque(Phase p, float target, BLDCMotor &m) {
  float vel = m.shaftVelocity();
  float angle = m.shaftAngle();

  if (p == STANCE) {
    // 支撑相：高阻尼，稳定承重
    return -stanceDamping * vel;
  } else {
    // 摆动相：助力 + 速度衰减
    float assist = assistGain;
    // 患者主动摆腿速度越快，助力越小（避免抢意图）
    if (fabs(vel) > 1.0) {
      assist *= max(0.2, 1.0 - (fabs(vel) - 1.0) * assistDecay * 10);
    }
    // 助力方向：朝向目标角度
    float dir = (target > angle) ? 1.0 : -1.0;
    return dir * assist - 0.3 * vel;
  }
}

void loop() {
  hipL.loopFOC(); hipR.loopFOC();
  updatePhases();

  float tauL = computeHipTorque(phaseL, targetHipL, hipL);
  float tauR = computeHipTorque(phaseR, targetHipR, hipR);

  hipL.move(constrain(tauL, -2.5, 2.5));
  hipR.move(constrain(tauR, -2.5, 2.5));

  delay(10);
}

解读：该案例借鉴了搜索结果中康复机器人“摆腿速度过快时自动减少助力”的设计。computeHipTorque() 在摆动相中根据 fabs(vel) 衰减 assist：患者主动摆腿越快，外骨骼助力越小，避免“机器抢着动”的对抗感；患者摆得慢时助力维持，帮助完成动作。支撑相则切换为纯阻尼模式（-stanceDamping * vel），只提供稳定阻力而不主动驱动，符合“按需辅助”的康复原则。双足交替的相位状态机确保左右腿独立判定，不依赖对称假设。

要点解读

1. 步态相位识别是阻抗切换的触发条件，FSR阈值法在Arduino上最务实
   搜索结果中多篇康复外骨骼文章均将“步态相位识别”列为阻抗调度的前置模块。双足场景下，足底FSR的触地/离地阈值判定虽然粗糙（无法区分足跟/足尖着地细节），但在Arduino算力约束下是唯一可实时运行的方案。案例一的 CONTACT_TH 和 0.4*CONTACT_TH 形成滞回区，避免压力在阈值附近波动时相位反复跳变。更精细的相位划分（足跟 strike / 足平 / 足尖 off）需要多路FSR或IMU融合，在Arduino上应降级为查表或交由协处理器。

2. 支撑相与摆动相的阻抗目标完全不同：支撑要“稳”，摆动要“顺”
   搜索结果明确指出“支撑相初期提高关节刚度以稳定承重；摆动相降低刚度、增加阻尼，使腿部顺应人体惯性自然前摆”。案例一的查表参数体现了这一原则：支撑相 Kp=8~12，摆动相 Kp=2~3。如果反过来（支撑相低刚度），患者站立时关节会“发软”；如果摆动相高刚度，患者摆腿时会感到“机器在拽”。案例二进一步在支撑相开始时用低刚度缓冲冲击，衰减后再切高刚度，这是对“支撑要稳”的时序细化。

3. 摆动相助力的“衰减机制”是避免人机对抗的关键
   康复训练的目标是促进患者主动运动，而非替代。案例三的 assistDecay 逻辑来自搜索结果中“摆腿速度过快时自动减少助力”的设计。如果外骨骼在摆动相始终输出固定助力，患者越快摆腿，机器就越“抢”，患者会感到被拖着走，主动参与度下降。速度衰减机制使助力在患者快速运动时退让，在患者无力时维持，符合“按需辅助”（assist-as-needed）的康复理念。

4. 双足交替的相位状态机必须独立判定，不能假设对称
   案例一和案例三中 phaseL 与 phaseR 是独立的状态变量，各自根据同侧 FSR 更新。如果简化为“左支撑时右摆动”的对称逻辑，在患者步态不对称（偏瘫患者常见）时会错误切换相位。搜索结果中的双足机器人CPG方案虽然用相位耦合生成对称步态，但康复外骨骼面对的是有运动障碍的人，其步态本身可能不对称，外骨骼应跟随而非强制对称。独立相位判定是安全底线。

5. Arduino算力边界要求阻抗环降频、FOC环保频
   案例中的 delay(10) 对应约100Hz的阻抗计算与相位更新频率。SimpleFOC 的 loopFOC() 应尽可能高频运行（去掉 delay 或使用定时器），而相位判定和阻抗参数计算可以降频。搜索结果明确指出“底层（1-5ms）运行FOC电流环，中层（10-20ms）执行阻抗算法，高层（50-100ms）进行步态相位识别”。在Arduino Uno上同时运行双足四关节的FOC和阻抗计算已接近极限，建议使用ESP32-S3或Teensy 4.x；若必须用Uno，应将四关节的FOC分拆到两个从控MCU，主控只负责相位判定和参数下发。

4\FSR+IMU 步态相位识别 + 相位-阻抗表切换（髋/膝）
适用硬件：ESP32 + SimpleFOC BLDC（左/右 × 髋/膝，共 4 关节）+ 编码器 + 双足 FSR（脚跟/前掌）+ MPU6050。

核心思路：康复训练的共同流程是“减重—带动—按需助力”。相位识别把连续步行切成 4 个区间（双支撑/单支撑/预摆/摆动），每个区间对应一组阻抗参数（K、B、期望轨迹）。支撑期高刚度防塌陷，摆动期低刚度留主动空间——刚度按相位变化，是步行康复与被动牵伸的本质区别。

/*
 * 案例4：步态相位 -> 阻抗表 -> 阻抗环执行
 * 关节：hipL, kneeL, hipR, kneeR（各一台 BLDC，力矩模式）
 * 相位：D_SWING(摆动) / D_PRE_SWING(预摆) / S_SINGLE(单支撑) / S_DOUBLE(双支撑)
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor hipL(7), kneeL(7), hipR(7), kneeR(7);
BLDCDriver3PWM dHL(9,10,11,8), dKL(13,14,15,12),
               dHR(22,23,24,25), dKR(26,27,28,29);
Encoder eHL(2,3,2048), eKL(4,5,2048), eHR(16,17,2048), eKR(18,19,2048);
void oHL(){eHL.handleA();} void oKL(){eKL.handleA();}
void oHR(){eHR.handleA();} void oKR(){eKR.handleA();}
MPU6050 imu;

// ---- 足底压力 ----
const int FSR_LH = 34, FSR_LT = 35;    // 左 脚跟/前掌
const int FSR_RH = 36, FSR_RT = 39;
const int TH_H = 400, TH_T = 320;      // 触地阈值（实测标定）

// ---- 机构参数（重力补偿用）----
const float M_THIGH = 6.8f, M_SHANK = 3.4f;
const float L_THIGH = 0.42f, L_SHANK = 0.40f, G = 9.81f;

// ---- 训练参数 ----
float assistLevel = 0.6f;              // 训练强度 0~1（治疗师设定）
const float TAU_LIM = 18.0f;           // 单关节力矩硬限 Nm

// ---- 相位 ----
enum Phase { P_DOUBLE, P_SINGLE_L, P_PRE_SWING_L, P_SWING_L,
             P_SINGLE_R, P_PRE_SWING_R, P_SWING_R };
Phase ph = P_DOUBLE;
unsigned long phT0 = 0;
float phT() { return (millis() - phT0) / 1000.0f; }

// ---- 阻抗表：相位 -> (K, B, 期望角偏移) ----
struct Imp { float kHip, kKnee, bHip, bKnee, offHip, offKnee; };
// K: Nm/rad, B: Nm·s/rad, off: 相对中立位的期望偏角(rad)
Imp tableFor(Phase p, bool isSwingLeg) {
  // 摆动腿：低刚度、允许主动、期望轨迹略屈
  if (isSwingLeg) return { 4.0f, 3.0f, 0.6f, 0.5f, -0.12f, 0.35f };
  // 支撑腿：高刚度抗塌陷、阻尼较大
  return { 22.0f, 20.0f, 2.4f, 2.2f, 0.05f, 0.05f };
}

// ---- 相位机：FSR 事件驱动 + 超时兜底 ----
void updatePhase() {
  bool L = (analogRead(FSR_LH) > TH_H) || (analogRead(FSR_LT) > TH_T);
  bool R = (analogRead(FSR_RH) > TH_H) || (analogRead(FSR_RT) > TH_T);
  float t = phT();

  switch (ph) {
    case P_DOUBLE:
      if (!L && R)       { ph = P_SWING_L; phT0 = millis(); }        // 左脚离地
      else if (L && !R)  { ph = P_SWING_R; phT0 = millis(); }
      break;
    case P_SWING_L:
      if (L)             { ph = P_SINGLE_R; phT0 = millis(); }       // 左脚落地
      else if (t > 0.8f) { ph = P_PRE_SWING_L; phT0 = millis(); }    // 兜底
      break;
    case P_SINGLE_R:
      if (t > 0.15f)     { ph = P_PRE_SWING_R; phT0 = millis(); }    // 进入预摆
      break;
    case P_PRE_SWING_R:
      if (!R)            { ph = P_SWING_R; phT0 = millis(); }
      else if (t > 0.6f) { ph = P_DOUBLE; phT0 = millis(); }
      break;
    case P_SWING_R:
      if (R)             { ph = P_SINGLE_L; phT0 = millis(); }
      else if (t > 0.8f) { ph = P_PRE_SWING_L; phT0 = millis(); }
      break;
    case P_SINGLE_L:
      if (t > 0.15f)     { ph = P_PRE_SWING_L; phT0 = millis(); }
      break;
    case P_PRE_SWING_L:
      if (!L)            { ph = P_SWING_L; phT0 = millis(); }
      else if (t > 0.6f) { ph = P_DOUBLE; phT0 = millis(); }
      break;
  }
}

// ---- 重力补偿（关节角 -> 重力矩，简化两连杆模型）----
void gravityTorque(float qHip, float qKnee, float pitchBody,
                   float &tauG_H, float &tauG_K) {
  // 大腿与竖直夹角 = qHip + 躯干前倾；小腿 = 大腿 + 膝角
  float aT = qHip + pitchBody;
  float aS = aT + qKnee;
  tauG_H = (M_THIGH*L_THIGH*0.5f + M_SHANK*L_THIGH) * G * sinf(aT)
         + M_SHANK*L_SHANK*0.5f * G * sinf(aS);
  tauG_K = M_SHANK*L_SHANK*0.5f * G * sinf(aS);
}

// ---- 单关节阻抗控制 ----
float impedance(float q, float qd, float qDes, float k, float b, float tauG) {
  float tau = k * (qDes - q) - b * qd + tauG;
  return constrain(tau, -TAU_LIM, TAU_LIM);
}

// ---- 力矩斜率限幅（升慢降快）----
struct Ramp { float v = 0; };
Ramp rHipL, rKneeL, rHipR, rKneeR;
float slew(Ramp &r, float tgt, float dt) {
  float up = 40.0f*dt, dn = 200.0f*dt;      // 升 40 Nm/s，降 200 Nm/s
  r.v += constrain(tgt - r.v, -dn, up);
  return r.v;
}

void setup() {
  eHL.init(); eKL.init(); eHR.init(); eKR.init();
  BLDCMotor *ms[4] = {&hipL, &kneeL, &hipR, &kneeR};
  Encoder   *es[4] = {&eHL, &eKL, &eHR, &eKR};
  BLDCDriver3PWM *ds[4] = {&dHL, &dKL, &dHR, &dKR};
  for (int i = 0; i < 4; i++) {
    ms[i]->linkSensor(es[i]);
    ms[i]->controller = MotionControlType::torque;
    ms[i]->PID_current.P = 0.6f; ms[i]->PID_current.I = 12.0f;
    ms[i]->voltage_limit = 24.0f;
    ms[i]->current_limit = 8.0f;
    ms[i]->init();
    ds[i]->voltage_power_supply = 24.0f; ds[i]->init();
    ms[i]->linkDriver(ds[i]);
    ms[i]->initFOC();
  }
  eHL.enableInterrupts(oHL); eKL.enableInterrupts(oKL);
  eHR.enableInterrupts(oHR); eKR.enableInterrupts(oKR);
  Wire.begin(); imu.initialize();
  phT0 = millis();
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.15f) dt = 0.02f;

  updatePhase();
  imu.update();
  float pitch = imu.getPitch() * DEG_TO_RAD;

  // 判断两侧是否为摆动腿
  bool Lswing = (ph == P_SWING_L || ph == P_PRE_SWING_L);
  bool Rswing = (ph == P_SWING_R || ph == P_PRE_SWING_R);
  Imp iL = tableFor(ph, Lswing);
  Imp iR = tableFor(ph, Rswing);

  // 中立位 + 相位偏置
  const float Q_HIP_N = 0.10f, Q_KNEE_N = 0.15f;
  float qdHL = Q_HIP_N + iL.offHip,  qdKL = Q_KNEE_N + iL.offKnee;
  float qdHR = Q_HIP_N + iR.offHip,  qdKR = Q_KNEE_N + iR.offKnee;

  // 训练强度缩放刚度（0=透明跟随便可，1=全助力）
  float kScale = 0.15f + 0.85f * assistLevel;

  float gHL, gKL, gHR, gKR;
  gravityTorque(hipL.shaft_angle, kneeL.shaft_angle, pitch, gHL, gKL);
  gravityTorque(hipR.shaft_angle, kneeR.shaft_angle, pitch, gHR, gKR);

  float tHL = impedance(hipL.shaft_angle, hipL.shaft_velocity, qdHL,
                        iL.kHip*kScale, iL.bHip, gHL);
  float tKL = impedance(kneeL.shaft_angle, kneeL.shaft_velocity, qdKL,
                        iL.kKnee*kScale, iL.bKnee, gKL);
  float tHR = impedance(hipR.shaft_angle, hipR.shaft_velocity, qdHR,
                        iR.kHip*kScale, iR.bHip, gHR);
  float tKR = impedance(kneeR.shaft_angle, kneeR.shaft_velocity, qdKR,
                        iR.kKnee*kScale, iR.bKnee, gKR);

  tHL = slew(rHipL, tHL, dt);  tKL = slew(rKneeL, tKL, dt);
  tHR = slew(rHipR, tHR, dt);  tKR = slew(rKneeR, tKR, dt);

  hipL.loopFOC();  hipL.move(tHL);
  kneeL.loopFOC(); kneeL.move(tKL);
  hipR.loopFOC();  hipR.move(tHR);
  kneeR.loopFOC(); kneeR.move(tKR);

  Serial.print("ph="); Serial.print(ph);
  Serial.print(" kH_L="); Serial.print(iL.kHip*kScale, 1);
  Serial.print(" tL="); Serial.print(tHL, 1);
  Serial.print("/"); Serial.println(tKL, 1);
  delay(10);   // 100Hz 外环；FOC 内环建议 1kHz 定时器
}

要点提示：tableFor 返回的两组参数相差 4~5 倍（22 对 4.0），相位一旦硬切换就会有明显顿挫；工程上必须按案例二的插值方式做过渡，案例一为了说明结构先写直接切换。

5、FreeRTOS 三层 + 阻抗参数平滑插值 + 训练强度分级
适用硬件：ESP32 双核 + 4 关节 BLDC + 编码器 + FSR + IMU。

核心思路：把“相位判定（100 Hz）”“参数调度（100 Hz）”“力矩执行（1 kHz）”分开。参数调度层不直接输出 K/B，而是输出目标阻抗参数 + 一阶低通，让 K 与 B 按时间常数过渡（约 150 ms），从根本上消除相位切换产生的力矩冲击。训练强度是另一个乘性系数，可在线调整而不打断步态。

/*
 * 案例5：三层分离 + 阻抗参数插值 + 训练强度在线调整
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor hip[2], knee[2];      // [0]=左 [1]=右
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;
SemaphoreHandle_t mux;

// ---- 阻抗参数（调度层写，执行层读）----
struct ImpParam { float kHip, kKnee, bHip, bKnee, offHip, offKnee; };
volatile ImpParam g_imp[2];           // 平滑后的参数
volatile float g_assist = 0.6f;       // 训练强度 0~1

// ---- 相位 ----
enum Ph { PH_DOUBLE, PH_SWING_L, PH_SWING_R };
volatile Ph g_ph = PH_DOUBLE;

// ---- 执行输出 ----
volatile bool g_estop = false;

// ---- 感知 ----
volatile bool g_fsrL = false, g_fsrR = false;
volatile float g_pitch = 0;

// ---- 目标参数（按相位）----
ImpParam targetFor(Ph p, int side) {
  bool swing = (p == PH_SWING_L && side == 0) || (p == PH_SWING_R && side == 1);
  if (swing) return { 5.0f, 4.0f, 0.7f, 0.6f, -0.14f, 0.38f };
  return { 20.0f, 18.0f, 2.2f, 2.0f, 0.04f, 0.04f };
}

// ---- 任务1：感知 100Hz ----
void taskSense(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    bool L = (analogRead(34) > 400) || (analogRead(35) > 320);
    bool R = (analogRead(36) > 400) || (analogRead(39) > 320);
    imu.update();
    xSemaphoreTake(mux, portMAX_DELAY);
    g_fsrL = L; g_fsrR = R;
    g_pitch = imu.getPitch() * DEG_TO_RAD;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---- 任务2：相位 + 参数调度 100Hz ----
void taskSchedule(void*) {
  TickType_t last = xTaskGetTickCount();
  unsigned long phT0 = millis();
  const float TAU_IMP = 0.15f;          // 参数过渡时间常数

  for (;;) {
    bool L, R;
    xSemaphoreTake(mux, portMAX_DELAY);
    L = g_fsrL; R = g_fsrR;
    xSemaphoreGive(mux);

    // 相位机（事件 + 超时）
    float t = (millis() - phT0) / 1000.0f;
    Ph np = g_ph;
    switch (g_ph) {
      case PH_DOUBLE:
        if (!L && R)      { np = PH_SWING_L; phT0 = millis(); }
        else if (L && !R) { np = PH_SWING_R; phT0 = millis(); }
        break;
      case PH_SWING_L:
        if (L || t > 1.0f) { np = PH_DOUBLE; phT0 = millis(); }
        break;
      case PH_SWING_R:
        if (R || t > 1.0f) { np = PH_DOUBLE; phT0 = millis(); }
        break;
    }

    // 参数一阶低通（关键：消除切换冲击）
    float a = 1.0f - expf(-0.01f / TAU_IMP);      // dt=10ms
    xSemaphoreTake(mux, portMAX_DELAY);
    g_ph = np;
    for (int s = 0; s < 2; s++) {
      ImpParam tg = targetFor(np, s);
      g_imp[s].kHip  += (tg.kHip  - g_imp[s].kHip ) * a;
      g_imp[s].kKnee += (tg.kKnee - g_imp[s].kKnee) * a;
      g_imp[s].bHip  += (tg.bHip  - g_imp[s].bHip ) * a;
      g_imp[s].bKnee += (tg.bKnee - g_imp[s].bKnee) * a;
      g_imp[s].offHip  += (tg.offHip  - g_imp[s].offHip ) * a;
      g_imp[s].offKnee += (tg.offKnee - g_imp[s].offKnee) * a;
    }
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float prev[4] = {0,0,0,0};
  for (;;) {
    ImpParam ip[2]; float assist, pitch; bool es;
    xSemaphoreTake(mux, portMAX_DELAY);
    ip[0] = g_imp[0]; ip[1] = g_imp[1];
    assist = g_assist; pitch = g_pitch; es = g_estop;
    xSemaphoreGive(mux);

    BLDCMotor *m[4] = {&hip[0], &knee[0], &hip[1], &knee[1]};
    float kScale = 0.15f + 0.85f * assist;

    for (int s = 0; s < 2; s++) {
      float qH = m[s*2]->shaft_angle,   vH = m[s*2]->shaft_velocity;
      float qK = m[s*2+1]->shaft_angle, vK = m[s*2+1]->shaft_velocity;

      // 重力补偿（与案例一相同逻辑，略去展开）
      float aT = qH + pitch, aS = aT + qK;
      float gH = (6.8f*0.21f + 3.4f*0.42f)*9.81f*sinf(aT)
               + 3.4f*0.20f*9.81f*sinf(aS);
      float gK = 3.4f*0.20f*9.81f*sinf(aS);

      float qdH = 0.10f + ip[s].offHip;
      float qdK = 0.15f + ip[s].offKnee;

      float tH = ip[s].kHip *kScale*(qdH - qH) - ip[s].bHip *vH + gH;
      float tK = ip[s].kKnee*kScale*(qdK - qK) - ip[s].bKnee*vK + gK;

      tH = constrain(tH, -18.0f, 18.0f);
      tK = constrain(tK, -18.0f, 18.0f);
      if (es) { tH = 0; tK = 0; }

      // 斜率限幅
      int iH = s*2, iK = s*2+1;
      prev[iH] += constrain(tH - prev[iH], -0.2f, 0.04f);
      prev[iK] += constrain(tK - prev[iK], -0.2f, 0.04f);

      m[iH]->loopFOC(); m[iH]->move(prev[iH]);
      m[iK]->loopFOC(); m[iK]->move(prev[iK]);
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（torque 模式）
  Wire.begin(); imu.initialize();
  for (int s = 0; s < 2; s++) {
    ImpParam t = targetFor(PH_DOUBLE, s);
    g_imp[s] = t;
  }
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskSense,    "sens", 4096, NULL, 3, NULL, 0);
  xTaskCreatePinnedToCore(taskSchedule, "sch",  4096, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(taskExec,     "exec", 4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

要点提示：参数低通用指数形式（1-exp(-dt/τ)），而不是固定系数。这样修改调度周期（10 ms 改为 20 ms）时过渡时间不会变；固定系数则需要重新调参。

6、双侧不对称 + 按需助力 + 痉挛检测降级
适用硬件：ESP32 + 4~6 关节 BLDC + 编码器 + FSR + IMU + 表面肌电（患侧，可选）+ 急停按钮。

核心思路：康复患者双侧能力差异大，患侧需要更高助力、更慢节拍、更宽容的跟踪误差，不能按对称设计。这里做三件事：① 左右独立参数集（患侧 K 更低、B 更高、期望轨迹更保守）；② 按需助力——助力大小随跟踪误差自适应，患者能跟上时减少助力，否则增加，把主动权还给患者；③ 痉挛/异常检测（力矩突变 + 角速度反向 + 肌电爆发），触发后立即软化关节并暂停助力。

/*
 * 案例6：双侧不对称 + 按需助力 + 痉挛检测
 * τ = τ_grav + K(e)·(qd-q) - B·qd ，其中 K 随跟踪误差自适应
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor hip[2], knee[2];
// 初始化同案例一（略）
MPU6050 imu;

// ---- 侧别参数（0=健侧 1=患侧）----
struct SideCfg {
  float kBase;      // 基础刚度
  float kMin, kMax; // 按需助力的刚度范围
  float bBase;      // 阻尼
  float gaitScale;  // 步态节拍缩放（患侧 >1 = 更慢）
  float errTarget;  // 期望跟踪误差（用于按需调节）
};
const SideCfg CFG[2] = {
  { 22.0f, 10.0f, 30.0f, 2.0f, 1.00f, 0.06f },   // 健侧：刚度高、响应快
  { 9.0f,  9.0f,  22.0f, 3.0f, 1.25f, 0.14f },   // 患侧：刚度低、节奏慢
};

// ---- 步态相位（简化：支撑/摆动，患侧周期延长）----
enum Ph { PH_STANCE, PH_SWING };
Ph ph[2] = {PH_STANCE, PH_STANCE};
unsigned long phT0[2] = {0, 0};
const float T_STANCE = 0.70f, T_SWING = 0.50f;    // 标称秒数

// ---- 关节状态 ----
const float Q_HIP_MID = 0.10f, Q_KNEE_MID = 0.15f;

// ---- 按需助力：误差 -> 刚度系数 ----
float kAdaptive(const SideCfg &c, float err) {
  // 误差小于目标：降刚度（让患者自己走）；误差大于目标：升刚度（兜住）
  float r = fabsf(err) / max(c.errTarget, 1e-3f);
  float k;
  if (r < 1.0f) k = c.kBase * (0.75f + 0.25f * r);        // 跟得好 -> 少帮
  else          k = c.kBase * (1.0f + 0.9f * (r - 1.0f)); // 跟不上 -> 多帮
  return constrain(k, c.kMin, c.kMax);
}

// ---- 痉挛 / 异常检测 ----
// 三判据：力矩突变、角速度反向（关节被拉回）、肌电爆发（可选）
const int EMG_PIN = 32;                 // 患侧股直肌
float emgBaseline = 0;
bool spasm = false;
unsigned long spasmT0 = 0;

bool detectSpasm(int side, float tauCmd, float tauPrev, float qd) {
  bool mechHit = (fabsf(tauCmd - tauPrev) > 6.0f) && (fabsf(qd) > 1.2f);
  int emg = analogRead(EMG_PIN);
  emgBaseline = 0.995f*emgBaseline + 0.005f*emg;      // 慢速基线
  bool emgBurst = (emg > emgBaseline + 250) && (side == 1);
  return mechHit || emgBurst;
}

// ---- 力矩斜率限幅 ----
struct Ramp { float v = 0; };
Ramp r[4];
float slew(Ramp &rr, float tgt, float dt, bool soft) {
  float up = (soft ? 15.0f : 40.0f) * dt;
  float dn = (soft ? 60.0f : 200.0f) * dt;
  rr.v += constrain(tgt - rr.v, -dn, up);
  return rr.v;
}

void setup() {
  // 电机初始化同案例一
  Wire.begin(); imu.initialize();
  pinMode(EMG_PIN, INPUT);
  for (int s = 0; s < 2; s++) phT0[s] = millis();
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.15f) dt = 0.02f;

  imu.update();
  float pitch = imu.getPitch() * DEG_TO_RAD;

  float tauOut[4];
  BLDCMotor *m[4] = {&hip[0], &knee[0], &hip[1], &knee[1]};

  for (int s = 0; s < 2; s++) {
    const SideCfg &c = CFG[s];

    // ---- 相位推进（患侧周期按 gaitScale 延长）----
    float t = (millis() - phT0[s]) / 1000.0f;
    float tStance = T_STANCE * c.gaitScale;
    float tSwing  = T_SWING  * c.gaitScale;
    if (ph[s] == PH_STANCE && t > tStance) { ph[s] = PH_SWING; phT0[s] = millis(); }
    else if (ph[s] == PH_SWING && t > tSwing) { ph[s] = PH_STANCE; phT0[s] = millis(); }

    // ---- 期望轨迹（摆动屈髋抬膝，支撑伸展）----
    float u = t / ((ph[s]==PH_STANCE) ? tStance : tSwing);
    u = constrain(u, 0.0f, 1.0f);
    float qdH, qdK;
    if (ph[s] == PH_SWING) {
      qdH = Q_HIP_MID - 0.18f * sinf(PI * u);       // 屈髋
      qdK = Q_KNEE_MID + 0.42f * sinf(PI * u);      // 屈膝抬脚
    } else {
      qdH = Q_HIP_MID + 0.10f * sinf(PI * u);       // 伸髋蹬离
      qdK = Q_KNEE_MID + 0.06f * sinf(PI * u);
    }

    float qH = m[s*2]->shaft_angle,   vH = m[s*2]->shaft_velocity;
    float qK = m[s*2+1]->shaft_angle, vK = m[s*2+1]->shaft_velocity;

    // ---- 按需刚度 ----
    float eH = qdH - qH, eK = qdK - qK;
    float kH = kAdaptive(c, eH);
    float kK = kAdaptive(c, eK);

    // ---- 重力补偿 ----
    float aT = qH + pitch, aS = aT + qK;
    float gH = (6.8f*0.21f + 3.4f*0.42f)*9.81f*sinf(aT)
             + 3.4f*0.20f*9.81f*sinf(aS);
    float gK = 3.4f*0.20f*9.81f*sinf(aS);

    // ---- 痉挛检测（优先于助力）----
    float tHraw = kH*eH - c.bBase*vH + gH;
    float tKraw = kK*eK - c.bBase*vK + gK;
    if (!spasm && detectSpasm(s, tKraw, r[s*2+1].v, vK)) {
      spasm = true; spasmT0 = millis();
      Serial.println("SPASM DETECTED - softening");
    }
    if (spasm && millis() - spasmT0 > 1500) spasm = false;   // 1.5s 后恢复

    if (spasm) {
      // 软化：极低刚度 + 高阻尼 + 仅重力补偿（让关节可被动活动）
      tHraw = 1.0f*eH - 4.0f*vH + gH;
      tKraw = 1.0f*eK - 4.0f*vK + gK;
    }

    tHraw = constrain(tHraw, -18.0f, 18.0f);
    tKraw = constrain(tKraw, -18.0f, 18.0f);

    // ---- 斜率限幅（痉挛时更柔）----
    tauOut[s*2]   = slew(r[s*2],   tHraw, dt, spasm);
    tauOut[s*2+1] = slew(r[s*2+1], tKraw, dt, spasm);
  }

  for (int i = 0; i < 4; i++) { m[i]->loopFOC(); m[i]->move(tauOut[i]); }

  Serial.print("phL="); Serial.print(ph[0]);
  Serial.print(" phR="); Serial.print(ph[1]);
  Serial.print(" spasm="); Serial.print(spasm);
  Serial.print(" kK_R="); Serial.println(kAdaptive(CFG[1], 0.1f), 1);
  delay(10);
}

要点提示：痉挛时“软化 + 高阻尼”是正确的响应，绝不能只是撤力。撤力会让支撑腿瞬间塌陷，高阻尼能保持关节可控且让治疗师用手法带过去；响应窗口 1.5s 后自动恢复，避免一直停留在保护态。

要点解读
一、阻抗必须按步态相位调度，不能全周期用一组参数
步行时同一条腿在支撑期和摆动期的力学任务完全相反：支撑期要“硬”（抵抗体重、防膝塌陷），摆动期要“软”（留出主动屈伸空间）。案例一的表中支撑腿 K=20~22、摆动腿 K=4~5，相差 4~5 倍。若全周期用一组参数，取大值则摆动期僵硬、患者拖不动腿；取小值则支撑期塌陷、有跌倒风险。所以相位识别是康复步态控制的前置条件，不是可选优化。

二、参数切换必须“插值过渡”，硬切换等于给患者一次冲击
相位切换是毫秒级事件，但刚度是持续性物理量。K 从 22 瞬间跳到 4，等于在 10 ms 内撤掉了 18 Nm/rad 的支撑，力矩会出现一个“台阶”，患者感受到的是关节突然发软或弹回。案例二用一阶低通把参数过渡时间设为 150 ms，且用指数形式（1-exp(-dt/τ)）保证修改调度周期时过渡时间不变。这条对康复机器人的重要性高于工业机器人：工业上是精度问题，康复上是舒适与安全（可能诱发疼痛或恐惧性肌紧张）。

三、“按需助力”是康复的核心原则，助力要随表现变化而非固定
康复训练的目标不是“把腿搬过去”，而是让患者主动参与。固定助力会导致两种失效：助力过大 → 患者完全被动、肌肉不激活、训练无效；助力过小 → 患者跟不上、代偿模式（划圈步态）固化。案例三的做法是让刚度随跟踪误差自适应：误差小于目标值就降刚度（患者能自己走），超出就升刚度（兜住防止跌倒）。这条原则的工程含义是：助力强度必须是一个由患者表现驱动的变量，而不是治疗师一次性设定的常数。

四、双侧不对称是常态，不要用对称设计掩盖它
偏瘫、单侧置换、单侧疼痛的患者，两侧能力差异可达 3~5 倍。如果代码里只写一组参数并镜像到另一侧，患侧必然要么“带不动”（助力不足）要么“被拽着走”（助力过度）。案例三为每侧独立配置：基础刚度（9 对 22）、步态周期缩放（1.25 对 1.0）、期望误差（0.14 对 0.06）。特别要注意步态周期延长这一项——患侧摆动相需要更多时间，强行同步到健侧节奏等于逼患者代偿。

五、痉挛/异常检测与安全降级，是康复机器人不可让渡的底线
康复机器人直接与人的肢体绑定，任何失控都会造成伤害。必须有三层防护：① 痉挛检测（力矩突变 + 角速度反向 + 肌电爆发，案例三三判据融合），响应是“软化 + 高阻尼 + 仅重力补偿”，而不是撤力（撤力会让支撑腿塌陷）；② 斜率限幅（升 15~40 Nm/s、降 60~200 Nm/s），保证任何指令变化都是连续的；③ 力矩硬限幅 + 电流限幅 + 物理急停按钮，三重独立，任何一层失效其余仍有效。康复场景里“能安全地停下来”比“能精准地走”优先级更高。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。