“Arduino BLDC 之基于模糊 PID 的膝关节柔顺力矩控制机器人”可以理解为：以 BLDC 为膝关节执行器，在 FOC 电流环基础上构建力矩/位置/速度分层闭环，并利用模糊逻辑根据力矩误差、角度误差、速度误差或交互力误差在线调整 PID 参数，使膝关节在不同负载、不同步态阶段和不同人机接触状态下保持柔顺、稳定、低冲击的力矩输出。这里的“柔顺力矩控制”不是单纯提高位置精度，而是让膝关节能够根据人体运动意图、地面负载和步态相位动态调节输出力矩，避免刚性对抗和拉扯感。
一、主要特点

1. 模糊 PID 解决膝关节负载时变问题
   膝关节在摆动相、支撑相、上下楼梯、起立坐下时负载差异很大。传统固定参数 PID 在某一工况下可能表现良好，但在负载突变时容易超调、振荡或响应迟钝。模糊 PID 以误差 e 和误差变化率 ec 为输入，通过模糊规则在线修正 Kp、Ki、Kd，使控制器在误差大时加快响应，在误差小时抑制振荡，在负载突变时增强鲁棒性。
2. 柔顺力矩控制通常采用“外环意图/位置 + 内环力矩/电流”结构
   典型分层为：
   最内环：FOC 电流环，直接控制 Iq，实现连续力矩输出；
   中间环：速度环或力矩环，用于抑制扰动、改善动态响应；
   外环：位置/轨迹跟踪、阻抗/导纳控制或人机交互力控制。
   模糊 PID 可以放在电流环、速度环或力矩环中，也可用于自适应调整阻抗参数，如刚度 Kp 和阻尼 Kd。
3. BLDC FOC 是实现柔顺性的底层条件
   普通开环 ESC 只能做速度或电压控制，力矩输出不线性，容易产生顿挫和拉扯。柔顺力矩控制需要稳定的电流采样、编码器反馈和 FOC 电流闭环，使期望力矩能平滑、可预测地转化为电机输出。
4. 人机交互力、步态相位和关节状态需要融合判断
   仅靠编码器角度不足以判断膝关节是否需要助力。更完整的方案会融合 IMU、足底压力、交互力传感器、编码器、电流反馈，甚至 EMG 信号，用于识别步态阶段、人体意图和接触状态。
5. 模糊规则需要与力矩限幅配合
   模糊 PID 能改善动态响应，但不能无限制提高输出。膝关节直接作用于人体或负载，必须设置最大力矩、最大角速度、角度软限位和急停保护，防止模糊推理输出过大造成冲击。
6. 嵌入式实现应分层运行
   电流环需要稳定高频周期，模糊 PID 和步态调度可以较低频运行。8 位 Arduino 适合验证单关节模糊 PID 和基础 FOC 闭环；若同时运行多传感器融合、步态识别、力矩分配和安全监控，更建议使用 ESP32-S3、Teensy 4.1、STM32 或“上位机 + 底层 MCU”架构。
   二、应用场景
7. 下肢康复外骨骼
   用于中风、脊髓损伤或术后康复患者的步态训练。模糊 PID 可根据患者主动发力程度调整辅助量，实现被动训练、主动辅助或抗阻训练。
8. 助力外骨骼
   在负重行走、搬运、爬楼或长时间站立时，膝关节根据负载变化提供柔顺助力，减少肌肉疲劳。
9. 单膝关节康复训练平台
   作为实验台验证模糊 PID、力矩闭环、轨迹跟踪和人机交互控制，适合科研和教学。
10. 双足机器人或人形机器人膝关节
    用于行走过程中的姿态稳定、冲击吸收和负载适应。模糊 PID 能改善起立、下蹲、上下台阶时的关节响应。
11. 智能假肢膝关节
    在步态摆动相和支撑相之间切换不同阻尼和力矩特性，使步态更自然、落地更稳定。
12. 人机协作训练装置
    例如膝部阻抗训练器、柔顺示教装置或触觉反馈平台，通过力矩闭环提供可控阻力或辅助。
    三、需要注意的事项
13. 安全优先级必须高于控制性能
    膝关节直接作用于人体或承载负载，任何误判都可能导致拉伤、摔倒或机械损伤。必须设置力矩上限、角度限位、急停按钮、看门狗和传感器异常检测，一旦数据异常立即进入零力矩或柔顺阻尼状态。
14. 电流环稳定是柔顺力矩控制的前提
    如果 FOC 电流环未校准，速度环和力矩环都会不稳定。应先保证电流采样、母线电压、编码器角度和 PWM 频率可靠，再引入模糊 PID。
15. 模糊规则不宜过度复杂
    嵌入式平台上，模糊 PID 通常采用 Mamdani 推理和查询表实现。规则过多会增加计算负担和调试难度。初期可用“误差大时提高 Kp、误差变化率大时提高 Kd、稳态误差存在时适度提高 Ki”的基础规则，再逐步细化。
16. 摩擦补偿和重力补偿不能省略
    减速器背隙、库仑摩擦、粘滞摩擦和重力矩都会影响柔顺性。若不补偿，控制器会把摩擦力误认为人机交互力，导致助力忽大忽小或低速卡顿。
17. 模式切换必须平滑
    从被动训练切到主动辅助、从零力切到阻抗控制、从行走切到爬楼时，目标力矩、刚度和阻尼应渐变，避免硬切换造成冲击。可加入斜率限制、置信度门槛和最小保持时间。
18. 传感器需要滤波但不能引入过大延迟
    IMU、力传感器和 EMG 都存在噪声。滤波过弱会导致模糊输入抖动，滤波过强会带来相位滞后，使力矩输出滞后于人体意图。应结合滑动平均、低通滤波、互补滤波或轻量卡尔曼滤波，并在实际穿戴中验证延迟感受。
19. Arduino 平台要严格控制实时负载
    电流环应放在硬件定时器中断中，串口打印、传感器初始化和复杂日志不应阻塞控制周期。若使用 ATmega328P，建议先做单关节验证；多关节或高带宽柔顺控制应迁移到 32 位 MCU。
20. 机械贴合度会直接影响控制效果
    绑带松动会导致力传递延迟，过紧则影响舒适性和血液循环。调试时不能只看误差曲线，还要结合穿戴者主观感受、交互力波形和电流响应综合判断。
21. 电池压降和热管理必须监控
    BLDC 在起立、爬楼或抗阻训练时电流较大，电池压降可能引起 MCU 复位或传感器异常。应使用独立稳压给控制板供电，加入母线电容，并监控电机温度和电池电压。
22. 模糊 PID 不能替代系统标定
    轮径、减速比、编码器方向、零点位置、力传感器零偏、重力矩曲线都需要标定。否则模糊自适应只能在错误模型上“补偿”，效果会受限甚至放大不稳定。

整体来看，这套系统的核心链路是：感知步态和负载 → 模糊 PID 在线调整 Kp/Ki/Kd → 力矩/电流环柔顺输出 → 编码器、IMU 和交互力反馈闭环修正。做原型时，建议按“单关节 FOC 电流环 → 固定参数力矩/位置 PID → 模糊 PID 自适应整定 → 摩擦与重力补偿 → 步态相位调度 → 人机安全降级”逐步推进。

1、基于误差与误差变化率的查表式模糊PID参数调度 + FOC力矩执行
适用场景：膝关节在支撑相与摆动相之间切换时，负载特性差异显著。支撑相需要高刚度抵抗重力，摆动相需要低刚度跟随人体残余运动。传统固定PID无法同时满足两种工况，模糊调度根据实时误差动态调整Kp/Kd。

#include <SimpleFOC.h>

// 膝关节 BLDC + FOC
BLDCMotor kneeMotor(7);
BLDCDriver3PWM kneeDriver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder kneeEnc(2, 3, 2048);

// 目标角度（由上层步态规划给出）
float targetAngle = 0.0;
float actualAngle = 0.0;
float angleError = 0.0;
float lastError = 0.0;
float errorRate = 0.0;

// 模糊调度参数（基准值）
float Kp_base = 3.0, Ki_base = 0.05, Kd_base = 0.3;
float Kp_adj = 1.0, Ki_adj = 1.0, Kd_adj = 1.0;

// 输出
float torqueCmd = 0.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.torque_controller = TorqueControlType::voltage;
  kneeMotor.voltage_limit = 2.0;  // 安全力矩上限
  kneeMotor.init();
  kneeMotor.initFOC();
}

// ===== 简化模糊推理：基于误差和误差变化率的查表调度 =====
// 误差论域归一化：|error| < 0.2 rad 为小，0.2~0.6 为中，>0.6 为大
// 误差变化率：|rate| < 1.0 为平稳，1.0~3.0 为变化中，>3.0 为剧烈
void fuzzyTune(float err, float rate) {
  float absErr = fabs(err);
  float absRate = fabs(rate);
  
  // 误差等级 (0=小, 1=中, 2=大)
  int eLevel = 0;
  if (absErr > 0.6) eLevel = 2;
  else if (absErr > 0.2) eLevel = 1;
  
  // 变化率等级 (0=平稳, 1=中, 2=剧烈)
  int rLevel = 0;
  if (absRate > 3.0) rLevel = 2;
  else if (absRate > 1.0) rLevel = 1;
  
  // ===== 模糊规则表（简化 3×3） =====
  // 规则：误差大且变化剧烈 → Kp大幅增大（快速响应），Kd减小（避免振荡）
  //       误差小且平稳     → Kp适中，Kd增大（抑制超调）
  //       中间区域         → 线性过渡
  
  if (eLevel == 2 && rLevel >= 1) {
    // 大误差、有变化：强P，弱D
    Kp_adj = 1.8; Kd_adj = 0.5;
  } else if (eLevel == 2 && rLevel == 0) {
    // 大误差、平稳：强P，适中D
    Kp_adj = 1.6; Kd_adj = 1.0;
  } else if (eLevel == 1 && rLevel == 2) {
    // 中误差、剧烈变化：适中P，弱D
    Kp_adj = 1.2; Kd_adj = 0.4;
  } else if (eLevel == 1 && rLevel == 1) {
    // 中误差、中变化：适中
    Kp_adj = 1.0; Kd_adj = 1.0;
  } else if (eLevel == 1 && rLevel == 0) {
    // 中误差、平稳：适中P，增强D
    Kp_adj = 0.9; Kd_adj = 1.3;
  } else if (eLevel == 0 && rLevel == 2) {
    // 小误差、剧烈变化：降低P，防抖
    Kp_adj = 0.6; Kd_adj = 1.5;
  } else {
    // 小误差、平稳：低P，强D
    Kp_adj = 0.5; Kd_adj = 1.8;
  }
}

void loop() {
  kneeMotor.loopFOC();
  
  // 感知
  actualAngle = kneeMotor.shaftAngle();
  angleError = targetAngle - actualAngle;
  errorRate = (angleError - lastError) / 0.01;
  lastError = angleError;
  
  // 模糊调度
  fuzzyTune(angleError, errorRate);
  
  // 实际 PID 参数
  float Kp = Kp_base * Kp_adj;
  float Kd = Kd_base * Kd_adj;
  
  // PID 计算（简化，无积分）
  torqueCmd = Kp * angleError - Kd * kneeMotor.shaftVelocity();
  
  // 安全限幅
  torqueCmd = constrain(torqueCmd, -1.5, 1.5);
  kneeMotor.move(torqueCmd);
  
  delay(10);  // 100Hz 调度周期
}

解读：该案例的核心是 fuzzyTune() 函数。它没有实现完整的模糊化-推理-去模糊化流程，而是将误差和误差变化率离散化为 3×3 共 9 个工况，每个工况对应一组 Kp/Kd 调整系数。这种查表式模糊调度在 Arduino 上计算量极小（仅比较和赋值），且规则可读性强、易于根据实际膝关节负载特性调参。搜索结果中 DAMO 文章的肌电分级 PID 切换本质上也是同一种思路，只是其输入是肌电信号而非角度误差。FOC 力矩模式确保 torqueCmd 被精确执行为关节力矩，而非速度目标，这是柔顺控制的前提。

2、弹簧-阻尼模型 + 模糊刚度调度（被动柔顺 + 主动刚度调节）
适用场景：患者在康复训练中主动发力程度变化时，膝关节需要“该软时软、该硬时硬”。弹性耦合模型提供基础柔顺性，模糊调度根据关节运动速度动态调整虚拟刚度，使助力在患者主动运动时减弱、在患者无力时增强。

#include <SimpleFOC.h>

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

// 弹簧-阻尼基础参数
float stiffness_base = 0.8;   // 基础刚度 (Nm/rad)
float damping_base = 0.15;    // 基础阻尼 (Nm·s/rad)

// 模糊调度输出
float stiffness = 0.8;
float damping = 0.15;

// 感知
float jointAngle = 0.0;
float jointVelocity = 0.0;
float targetAngle = 0.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.torque_controller = TorqueControlType::voltage;
  kneeMotor.voltage_limit = 2.5;
  kneeMotor.init();
  kneeMotor.initFOC();
}

// ===== 模糊刚度调度 =====
// 输入：关节运动速度（反映患者主动意图强度）
// 规则：患者主动运动快 → 降低刚度（不阻碍）；运动慢/不动 → 提高刚度（提供支撑）
void fuzzyStiffnessTune(float velocity) {
  float absVel = fabs(velocity);
  
  // 速度分级
  if (absVel > 1.5) {
    // 快速主动运动：低刚度，高阻尼（柔顺跟随）
    stiffness = 0.3;
    damping = 0.25;
  } else if (absVel > 0.5) {
    // 中等运动：中等刚度
    stiffness = 0.6;
    damping = 0.18;
  } else {
    // 缓慢或静止：高刚度（支撑）
    stiffness = 1.2;
    damping = 0.10;
  }
}

void loop() {
  kneeMotor.loopFOC();
  
  jointAngle = kneeMotor.shaftAngle();
  jointVelocity = kneeMotor.shaftVelocity();
  
  // 模糊调度刚度/阻尼
  fuzzyStiffnessTune(jointVelocity);
  
  // ===== 弹簧-阻尼力矩计算 =====
  // F = K * (目标角度 - 实际角度) - B * 实际速度
  float angleError = targetAngle - jointAngle;
  float torqueCmd = stiffness * angleError - damping * jointVelocity;
  
  // 安全限幅
  torqueCmd = constrain(torqueCmd, -2.0, 2.0);
  kneeMotor.move(torqueCmd);
  
  delay(10);
}

解读：该案例的柔顺性来自弹簧-阻尼模型的物理直觉：刚度 K 决定“拉回目标角度有多用力”，阻尼 B 决定“对运动速度的阻力有多大”。搜索结果中康复机器人下肢辅助的代码正是使用了 F = stiffness * (IMU角度 - 电机角度) + damping * IMU速度 的弹性耦合模型，但其刚度/阻尼是固定值。本案例引入 fuzzyStiffnessTune()，根据关节速度动态切换刚度等级：患者主动运动时降低刚度避免“机器拖着人走”的对抗感，患者无力时提高刚度提供支撑。这种被动柔顺 + 主动刚度调度的结构比纯模糊PID更贴近膝关节康复的物理需求。

3、模糊PID + 力矩变化率限制（安全增强型柔顺控制）
适用场景：膝关节控制中，力矩的突变比力矩的大小更危险——突然的力矩阶跃可能造成关节冲击或患者恐慌。在模糊PID输出之后加入力矩变化率限制（jerk 限制），使柔顺性不仅体现在“力矩大小”，也体现在“力矩如何变化”。

#include <SimpleFOC.h>

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

float targetAngle = 0.0;
float lastError = 0.0;
float integral = 0.0;

// 力矩输出
float torqueCmd = 0.0;
float lastTorqueCmd = 0.0;

// 力矩变化率限制（安全关键参数）
const float MAX_TORQUE_RATE = 3.0;  // Nm/s
const float DT = 0.01;              // 10ms 控制周期
const float MAX_TORQUE_STEP = MAX_TORQUE_RATE * DT;  // 每周期最大变化 0.03 Nm

// 模糊调度
float Kp = 3.0, Ki = 0.05, Kd = 0.3;

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.torque_controller = TorqueControlType::voltage;
  kneeMotor.voltage_limit = 2.0;
  kneeMotor.init();
  kneeMotor.initFOC();
}

void fuzzyTune(float err, float rate) {
  float absErr = fabs(err);
  
  // 简化模糊：误差大 → 增P；误差变化剧烈 → 减D
  if (absErr > 0.5) {
    Kp = 5.0;
    Kd = 0.15;
  } else if (absErr > 0.2) {
    Kp = 3.5;
    Kd = 0.25;
  } else {
    Kp = 2.0;
    Kd = 0.4;
  }
  Ki = 0.05;  // 积分保持小值，防饱和
}

void loop() {
  kneeMotor.loopFOC();
  
  float actualAngle = kneeMotor.shaftAngle();
  float error = targetAngle - actualAngle;
  float rate = (error - lastError) / DT;
  lastError = error;
  
  fuzzyTune(error, rate);
  
  // PID 计算
  integral += error * DT;
  integral = constrain(integral, -1.0, 1.0);  // 积分限幅
  float rawTorque = Kp * error + Ki * integral - Kd * kneeMotor.shaftVelocity();
  
  // ===== 力矩变化率限制（核心安全层） =====
  float torqueDelta = rawTorque - lastTorqueCmd;
  torqueDelta = constrain(torqueDelta, -MAX_TORQUE_STEP, MAX_TORQUE_STEP);
  torqueCmd = lastTorqueCmd + torqueDelta;
  
  // 总限幅
  torqueCmd = constrain(torqueCmd, -1.5, 1.5);
  
  kneeMotor.move(torqueCmd);
  lastTorqueCmd = torqueCmd;
  
  delay(10);
}

解读：该案例在模糊PID的输出端增加了一道 MAX_TORQUE_STEP 限制。rawTorque 是模糊PID计算出的“理想力矩”，但如果它与上一周期的 lastTorqueCmd 差异过大，强制按斜率限制逐步逼近。例如模糊调度在误差跨越阈值时突然将 Kp 从 3.5 跳到 5.0，rawTorque 会阶跃，但 MAX_TORQUE_STEP = 0.03 Nm/周期 使实际输出以约 3 Nm/s 的速率平滑过渡。对于膝关节外骨骼，力矩的变化率直接对应关节的冲击感，限制 jerk 比限制绝对力矩更能提升“柔顺”的主观体感。这一设计在搜索结果中未被直接提及，但它是将模糊PID从“实验室仿真”推向“人体穿戴”的关键工程补充。

要点解读

1. 搜索结果中的“模糊PID”多数是查表式参数调度，而非完整模糊推理
   DAMO 开发者矩阵的膝关节文章标题包含“模糊PID”，但代码实际展示的是 adjustControlParameters(intention, …) 根据肌电信号离散等级（0/1/2/3）直接赋值 Kp/Ki/Kd。这并非严格意义上的模糊控制（缺少隶属度函数、模糊化、去模糊化），但它在 Arduino 算力约束下是务实且有效的。案例一延续了这一思路：将误差和误差变化率离散化为 9 个工况，查表输出 Kp/Kd 调整系数。对于膝关节柔顺控制，工况的物理意义清晰（“大误差快响应”“小误差防超调”），比完整的模糊引擎更易于调试和临床验证。

2. 膝关节柔顺控制的核心不是“力矩大小”，而是“力矩如何随运动意图变化”
   搜索结果的康复机器人下肢辅助代码使用固定刚度/阻尼的弹簧模型，其柔顺性来自模型本身——角度误差大时力矩自然增大，患者主动运动时速度项产生阻尼。但固定刚度无法适应患者肌力波动。案例二的 fuzzyStiffnessTune() 根据关节速度切换刚度等级：患者主动运动快 → 刚度降低（助力退让），患者不动 → 刚度提高（提供支撑）。学术研究确认模糊控制相较固定参数在膝关节外骨骼中具有更好的抗干扰能力，其价值正在于此。

3. FOC 力矩模式是模糊PID输出被执行的前提，电压模式需要额外标定
   SimpleFOC 支持三种力矩控制类型：voltage、dc_current、foc_current。案例中使用了 TorqueControlType::voltage，因为它在 Arduino 上计算量最小，但 move(torqueCmd) 传入的实际上是电压给定，而非物理力矩（Nm）。这意味着模糊PID输出的“力矩指令”与真实关节力矩之间存在电机参数（相电阻、KV值）的非线性映射。如果对力矩精度要求高（如患者安全限力矩的临床场景），应使用 foc_current 模式，使 move() 的参数更接近真实电流/力矩。搜索结果的柔顺拖拽代码同样使用了电压限幅而非电流闭环，在原型阶段可接受，但工程化时必须升级。

4. 模糊调度的输入选择决定了它解决什么问题
   案例一以“角度误差 + 误差变化率”为输入，解决的是跟踪精度与稳定性的矛盾（大误差快速响应，小误差抑制振荡）。案例二以“关节运动速度”为输入，解决的是人机协同中的主从关系（人快则机器让，人慢则机器帮）。两者都是“模糊PID”，但物理含义完全不同。膝关节柔顺控制中，如果目标是康复训练，案例二的输入选择更贴合“柔顺”的本意；如果目标是仿人机器人站立稳定，案例一的输入选择更贴合“抗扰动”的需求。学术文献中膝关节外骨骼的模糊控制器设计也强调输入变量的选择应与具体任务目标对齐。

5. 力矩变化率限制是模糊PID从仿真走向穿戴的必备安全层
   搜索结果的模糊PID膝关节研究均基于 Simulink 仿真或台架实验，仿真中力矩可以任意跳变。但真实人体膝关节的惯性不可忽略，力矩的阶跃会通过减速器和绑带传递为冲击。案例三的 MAX_TORQUE_STEP 将力矩变化限制在 3 Nm/s 以内，使模糊调度即使发生参数跳变，执行层也不会产生冲击。这一设计在搜索结果中没有直接对应，但它是“柔顺”一词在工程上的必要含义：柔顺不仅是最终力矩的温和，更是力矩过渡的平滑。

4、双输入模糊 PID（误差 + 误差变化率）在线整定力矩环
适用硬件：ESP32 + SimpleFOC BLDC（膝关节，力矩模式）+ 编码器 + 电流采样 + FSR/力矩传感器。
核心思路：力矩环 PID 的三个增益不写死，而由“力矩误差 e”和“误差变化率 ec”经 7×7 模糊规则表实时整定。规则的设计基于工程直觉：大误差时加大 Kp 加快响应，误差小但变化快时加大 Kd 抑制超调，稳态时补 Ki 消残差。

/*
 * 案例4：Mamdani 式模糊 PID（7×7 规则表）整定力矩环
 * 输入：e（力矩误差 Nm）、ec（误差变化率 Nm/s）
 * 输出：ΔKp, ΔKi, ΔKd（通过缩放因子叠加到基础 PID）
 */
#include <SimpleFOC.h>

BLDCMotor knee = BLDCMotor(7);
BLDCDriver3PWM drv(9,10,11,8);
Encoder enc(2,3,2048);
void onEnc(){ enc.handleA(); }

const int FSR_PIN = A0;           // 交互力矩传感器（或串联弹性体应变）
const float TAU_SCALE = 6.0f;     // FSR 读数 -> Nm 标定系数

// ---- 模糊域：7 个语言值 ----
// NB NM NS ZO PS PM PB  -> 索引 0..6，论域 [-3, +3]
const int NV = 7;

// 三角形隶属度（中心 c，半宽 w，输入域 [-3,3]）
float tri(float x, float c, float w) {
  float d = fabsf(x - c);
  return (d >= w) ? 0.0f : (1.0f - d / w);
}

// 计算 7 个语言值的隶属度（三角 + 端点饱和）
void fuzzify(float x, float mu[NV]) {
  const float C[NV] = {-3, -2, -1, 0, 1, 2, 3};
  const float W = 1.05f;              // 半宽（略大于 1，保证覆盖）
  for (int i = 0; i < NV; i++) mu[i] = tri(x, C[i], W);
  // 端点饱和：超出范围时最外侧语言值取 1
  if (x <= -3) { for(int i=0;i<NV;i++) mu[i]=0; mu[0]=1; }
  if (x >=  3) { for(int i=0;i<NV;i++) mu[i]=0; mu[6]=1; }
}

// ---- 规则表（行=e, 列=ec），值 = 输出语言值索引 0..6 ----
// Kp 规则：大误差大 Kp；误差小且变化大时也需较大 Kp
const int RULE_KP[NV][NV] = {
  {6,6,6,5,5,4,4},   // e=NB
  {6,6,5,5,4,4,3},   // e=NM
  {5,5,5,4,4,3,3},   // e=NS
  {4,4,4,3,3,2,2},   // e=ZO
  {5,5,4,4,3,3,3},   // e=PS
  {6,5,5,4,4,3,3},   // e=PM
  {6,6,5,5,4,4,4}    // e=PB
};
// Ki 规则：稳态（e=ZO）时补积分，其余减小以避免积分饱和
const int RULE_KI[NV][NV] = {
  {1,1,2,2,3,3,3},
  {1,2,2,3,3,3,4},
  {2,2,3,3,4,4,4},
  {3,3,3,5,3,3,3},   // ZO 行：中间加强积分
  {2,2,3,3,4,4,4},
  {1,2,2,3,3,3,4},
  {1,1,2,2,3,3,3}
};
// Kd 规则：误差变化大时加强微分，抑制超调
const int RULE_KD[NV][NV] = {
  {6,6,5,4,4,3,3},
  {6,5,5,4,3,3,2},
  {5,5,4,4,3,3,2},
  {4,4,4,5,4,4,4},   // ZO 行：中间略高
  {5,5,4,4,3,3,2},
  {6,5,5,4,3,3,2},
  {6,6,5,4,4,3,3}
};

// ---- 模糊推理（Mamdani，min-max 合成，重心法去模糊）----
float infer(const int rule[NV][NV], float muE[NV], float muEC[NV]) {
  float num = 0, den = 0;
  for (int i = 0; i < NV; i++) {
    if (muE[i] < 1e-4f) continue;
    for (int j = 0; j < NV; j++) {
      if (muEC[j] < 1e-4f) continue;
      float wgt = min(muE[i], muEC[j]);          // 前件取小
      float outVal = (rule[i][j] - 3.0f);        // 输出语言值 -> [-3,3]
      num += wgt * outVal;
      den += wgt;
    }
  }
  return (den > 1e-6f) ? num / den : 0.0f;       // 重心法
}

// ---- PID 状态 ----
const float KP0 = 3.2f, KI0 = 12.0f, KD0 = 0.08f;
const float S_KP = 1.5f, S_KI = 8.0f, S_KD = 0.04f;   // 输出缩放因子
float kpCur = KP0, kiCur = KI0, kdCur = KD0;
float iTerm = 0, prevErr = 0;

float fuzzyPID(float err, float dt, float eScale, float ecScale) {
  // 归一化到 [-3,3] 论域
  static float prevErrForEC = 0;
  float ec = (err - prevErrForEC) / dt;
  prevErrForEC = err;

  float eN  = constrain(err * eScale,  -3.0f, 3.0f);
  float ecN = constrain(ec  * ecScale, -3.0f, 3.0f);

  float muE[NV], muEC[NV];
  fuzzify(eN, muE); fuzzify(ecN, muEC);

  float dKp = infer(RULE_KP, muE, muEC) * S_KP;
  float dKi = infer(RULE_KI, muE, muEC) * S_KI;
  float dKd = infer(RULE_KD, muE, muEC) * S_KD;

  // 增益带上下限（防发散）
  kpCur = constrain(KP0 + dKp, 0.8f, 8.0f);
  kiCur = constrain(KI0 + dKi, 1.0f, 30.0f);
  kdCur = constrain(KD0 + dKd, 0.01f, 0.30f);

  // 标准 PID（积分抗饱和）
  iTerm = constrain(iTerm + kiCur * err * dt, -4.0f, 4.0f);
  float d = -kdCur * (err - prevErr) / dt;      // 微分作用于误差
  prevErr = err;
  return constrain(kpCur*err + iTerm + d, -8.0f, 8.0f);
}

// ---- 交互意图（FSR）-> 目标力矩 ----
float targetTorque = 0;
float readIntent() {
  float f = analogRead(FSR_PIN) / 1023.0f;
  if (f < 0.08f) return 0;                        // 死区：无意图
  return constrain((f - 0.08f) * 5.0f, 0.0f, 1.6f);   // 0~1.6 Nm
}

void setup() {
  enc.init(); enc.enableInterrupts(onEnc);
  knee.linkSensor(&enc);
  knee.controller = MotionControlType::torque;
  knee.PID_current.P = 0.5f; knee.PID_current.I = 10.0f;
  knee.voltage_limit = 12.0f;
  knee.current_limit = 3.0f;                       // 力矩硬限（安全底线）
  knee.init();
  drv.voltage_power_supply = 12.0f; drv.init();
  knee.linkDriver(&drv);
  knee.initFOC();
  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.2f) dt = 0.02f;

  // 1) 意图 -> 目标力矩
  targetTorque = readIntent();

  // 2) 实际力矩估计（电流 × 力矩常数，或用串联弹性体测量）
  float tauAct = knee.current * 0.38f;             // Kt≈0.38 Nm/A（标定）
  float err = targetTorque - tauAct;

  // 3) 模糊 PID -> 力矩指令
  float tauCmd = fuzzyPID(err, dt, 2.0f, 0.5f);

  // 4) 力矩指令叠加到目标力矩（前馈 + 反馈）
  float tauOut = constrain(targetTorque + tauCmd, -3.0f, 3.0f);

  static float tauPrev = 0;
  tauOut = tauPrev + constrain(tauOut - tauPrev, -0.02f, 0.02f);  // 斜率限幅
  tauPrev = tauOut;

  knee.loopFOC();
  knee.move(tauOut);

  Serial.print("tgt="); Serial.print(targetTorque, 2);
  Serial.print(" act="); Serial.print(tauAct, 2);
  Serial.print(" kp="); Serial.print(kpCur, 2);
  Serial.print(" ki="); Serial.print(kiCur, 1);
  Serial.print(" kd="); Serial.println(kdCur, 3);
  delay(10);   // 100Hz 外环；力矩环 1kHz 可移入定时器
}

关键：模糊输出只做“增量”（ΔKp、ΔKi、ΔKd），并叠加到基础增益上；这样即使模糊表调得不好，系统也在基础 PID 附近工作，不会发散。kdCur 在力矩环里必须很小（0.01~0.3）；力矩环的微分项过大，会放大电流噪声。

5、FreeRTOS 三层 + 模糊增益调度（按步态相位/负载切换整定域）
适用硬件：ESP32 双核 + 膝关节 BLDC + 编码器 + IMU + 足底 FSR + 力矩传感器。

核心思路：膝关节力矩在不同工况下特性差异极大，支撑期要求高刚度抗塌陷，摆动期要求低刚度跟随，因此采用单一模糊表无法覆盖这些差异。这套设计让模糊推理器本身可切换：按步态相位选三套规则表（支撑/摆动/转换），并通过共享会话量平滑过渡。

/*
 * 案例5：模糊增益调度 —— 三套规则表按步态相位切换
 * 支撑期：高刚度、高 Kp、低 Kd（抗塌陷）
 * 摆动期：低刚度、低 Kp、高 Kd（跟随柔和）
 * 转换期：线性混合两套参数（避免切换冲击）
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor knee(7);
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;
SemaphoreHandle_t mux;

// ---- 步态相位 ----
enum Ph { PH_STANCE, PH_SWING, PH_TRANS };
volatile Ph phase = PH_TRANS;
volatile float phaseBlend = 0.0f;    // 0=纯支撑, 1=纯摆动

// ---- 三套基础增益 ----
struct Gains { float kp, ki, kd; };
const Gains G_STANCE = {5.5f, 15.0f, 0.03f};
const Gains G_SWING  = {1.8f,  6.0f, 0.09f};

// ---- 共享量 ----
volatile float s_tauTgt = 0, s_tauAct = 0;
volatile float c_tauOut = 0;
volatile bool  c_stop = false;

// 简化模糊推理（复用案例一结构，略去表定义）
float fuzzyDelta(float eN, float ecN, int table);   // table: 0=Kp 1=Ki 2=Kd

// ---- 任务1：感知 100Hz ----
void taskSense(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    imu.update();
    float f = analogRead(A0) / 1023.0f;              // 足底 FSR
    bool toeOn = (f > 0.35f);                        // 前掌触地
    float pitchRate = imu.getAngularVelocityY();      // 膝屈伸角速度

    xSemaphoreTake(mux, portMAX_DELAY);
    s_tauAct = knee.current * 0.38f;
    s_tauTgt = toeOn ? 0.9f : 0.25f;                 // 支撑期加载
    // 相位判定：触地 + 角速度小 = 支撑；离地 = 摆动
    Ph want = toeOn ? PH_STANCE : PH_SWING;
    if (want != phase) {
      phase = PH_TRANS;
      phaseBlend += (want == PH_SWING) ? 0.15f : -0.15f;   // 平滑过渡
      phaseBlend = constrain(phaseBlend, 0.0f, 1.0f);
      if (phaseBlend >= 0.99f || phaseBlend <= 0.01f) phase = want;
    }
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---- 任务2：力矩控制 100Hz（外环）----
void taskCtrl(void*) {
  TickType_t last = xTaskGetTickCount();
  float iTerm = 0, prevE = 0;
  for (;;) {
    float tgt, act, blend;
    xSemaphoreTake(mux, portMAX_DELAY);
    tgt = s_tauTgt; act = s_tauAct; blend = phaseBlend;
    xSemaphoreGive(mux);

    float e = tgt - act;
    float dt = 0.01f;
    float ec = (e - prevE) / dt;
    prevE = e;

    // 基础增益：按 blend 线性插值（转换期平滑）
    float kp = G_STANCE.kp + (G_SWING.kp - G_STANCE.kp) * blend;
    float ki = G_STANCE.ki + (G_SWING.ki - G_STANCE.ki) * blend;
    float kd = G_STANCE.kd + (G_SWING.kd - G_STANCE.kd) * blend;

    // 模糊增量：误差归一化后推理
    float eN = constrain(e * 2.5f, -3, 3);
    float ecN = constrain(ec * 0.4f, -3, 3);
    kp += fuzzyDelta(eN, ecN, 0) * 1.2f;
    ki += fuzzyDelta(eN, ecN, 1) * 5.0f;
    kd += fuzzyDelta(eN, ecN, 2) * 0.03f;
    kp = constrain(kp, 0.6f, 9.0f);
    ki = constrain(ki, 0.5f, 25.0f);
    kd = constrain(kd, 0.01f, 0.25f);

    iTerm = constrain(iTerm + ki*e*dt, -3.5f, 3.5f);
    float tau = constrain(kp*e + iTerm - kd*ec, -3.0f, 3.0f);

    xSemaphoreTake(mux, portMAX_DELAY);
    c_tauOut = tau;
    c_stop = false;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---- 任务3：执行 1kHz ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float tPrev = 0;
  for (;;) {
    float t; bool stop;
    xSemaphoreTake(mux, portMAX_DELAY);
    t = c_tauOut; stop = c_stop;
    xSemaphoreGive(mux);
    if (stop) t = 0;
    tPrev = tPrev + constrain(t - tPrev, -0.015f, 0.015f);
    knee.loopFOC();
    knee.move(tPrev);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（torque 模式 + current_limit=3A）
  Wire.begin(); imu.initialize();
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskSense, "sens", 4096, NULL, 3, NULL, 0);
  xTaskCreatePinnedToCore(taskCtrl,  "ctrl", 4096, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(taskExec,  "exec", 4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

相位切换的关键是 phaseBlend 的渐变（每 10ms 变化 0.15，约 70ms 完成过渡），因此增益不会跳变，力矩也不会因刚度突变产生冲击。如果直接硬切换，会因为刚度差 3 倍而出现明显“打膝”感。

6、模糊自适应 + 负载/速度前馈补偿（上楼梯、坐站转换）
适用硬件：ESP32 + 膝关节 BLDC + 编码器 + IMU + 足底 FSR + 膝关节力矩传感器（串联弹性体）。

核心思路：模糊 PID 只解决“误差怎么消”，而膝关节还有一个更关键的问题：重力矩随关节角剧烈变化（正弦关系）。所以真正柔顺的控制结构是“重力前馈 + 摩擦前馈 + 速度前馈 + 模糊 PID 反馈”。前馈负责让力矩大体正确，模糊 PID 只负责修正残差；这样 PID 可以在小增益下工作，柔顺性自然更好。

/*
 * 案例6：前馈（重力+摩擦+速度）+ 模糊PID反馈
 * τ_out = τ_grav(q) + τ_fric(q̇) + τ_ff(v) + fuzzyPID(e)
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

BLDCMotor knee(7);
// 驱动/编码器初始化同案例一（略）
MPU6050 imu;

// ---- 机构参数 ----
const float M_SHANK = 3.2f;      // 小腿质量 kg
const float L_CG    = 0.20f;     // 质心距膝轴 m
const float G        = 9.81f;
const float TAU_FRIC_C = 0.35f;  // 库仑摩擦 Nm
const float B_VISC     = 0.08f;  // 粘性阻尼 Nm·s/rad

// ---- 重力前馈：τ = m·g·L·cos(关节角相对竖直) ----
float gravityFF(float qKnee, float pitchBody) {
  // qKnee: 膝关节角（0=伸直），pitchBody: 大腿与水平夹角
  float angFromVertical = qKnee + pitchBody;       // 小腿与竖直夹角
  return M_SHANK * G * L_CG * sinf(angFromVertical);
}

// ---- 摩擦前馈：库仑 + 粘性 ----
float frictionFF(float qd) {
  float sign = (qd > 0.02f) ? 1.0f : (qd < -0.02f) ? -1.0f : 0.0f;
  return TAU_FRIC_C * sign + B_VISC * qd;
}

// ---- 速度前馈：期望角速度对应的力矩 ----
float speedFF(float qdDes) {
  return 0.12f * qdDes;          // 简化：等效惯量项
}

// ---- 模糊PID（复用案例一结构，参数按膝关节调）----
float kpF = 2.0f, kiF = 8.0f, kdF = 0.05f;
float iTermF = 0, prevErrF = 0;
float fuzzyFeedback(float err, float dt) {
  // 简化实现：模糊输出直接调制增益（完整表实现同案例一）
  float eN = constrain(err * 3.0f, -3, 3);
  float ec = (err - prevErrF) / dt;
  float ecN = constrain(ec * 0.5f, -3, 3);
  prevErrF = err;

  // 简化规则：大误差->大Kp；小误差快变化->大Kd；稳态->稍加Ki
  float aKp = 1.0f + 0.35f * fabsf(eN);
  float aKd = 1.0f + 0.40f * fabsf(ecN) * (1.0f - fabsf(eN)/3.0f);
  float aKi = 1.0f + 0.30f * (1.0f - fabsf(eN)/3.0f);

  float kp = constrain(kpF * aKp, 0.8f, 9.0f);
  float ki = constrain(kiF * aKi, 1.0f, 25.0f);
  float kd = constrain(kdF * aKd, 0.01f, 0.25f);

  iTermF = constrain(iTermF + ki*err*dt, -3.0f, 3.0f);
  return constrain(kp*err + iTermF - kd*ec, -3.0f, 3.0f);
}

// ---- 目标：坐站转换 / 上楼梯 / 行走 三档 ----
enum Task { T_SIT2STAND, T_STAIR, T_WALK };
Task task = T_WALK;
const int FSR_PIN = A0;

float targetTorqueFor(Task t) {
  float f = analogRead(FSR_PIN) / 1023.0f;
  switch (t) {
    case T_SIT2STAND: return 1.6f + 0.8f * f;      // 高力矩支撑起身
    case T_STAIR:     return 1.2f + 0.5f * f;      // 中等助力
    default:          return 0.4f + 0.3f * f;      // 行走：轻助力
  }
}

void setup() {
  // 电机初始化同案例一
  Wire.begin(); imu.initialize();
  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.2f) dt = 0.01f;

  imu.update();
  float qKnee = knee.shaft_angle;
  float qd    = knee.shaft_velocity;
  float pitch = imu.getPitch() * DEG_TO_RAD;

  // 1) 目标力矩（按任务）
  float tgt = targetTorqueFor(task);

  // 2) 实际力矩
  float act = knee.current * 0.38f;

  // 3) 前馈三件套
  float tauG = gravityFF(qKnee, pitch);
  float tauF = frictionFF(qd);
  float tauV = speedFF(0.0f);            // 期望角速度=0（保持位）

  // 4) 模糊反馈（只修正残差）
  float err = tgt - act;
  float tauFB = fuzzyFeedback(err, dt);

  // 5) 合成 + 硬限幅 + 斜率限幅
  float tauOut = tauG + tauF + tauV + tauFB;
  tauOut = constrain(tauOut, -3.5f, 3.5f);

  static float tPrevOut = 0;
  tauOut = tPrevOut + constrain(tauOut - tPrevOut, -0.03f, 0.03f);
  tPrevOut = tauOut;

  knee.loopFOC();
  knee.move(tauOut);

  Serial.print("q="); Serial.print(qKnee, 2);
  Serial.print(" tauG="); Serial.print(tauG, 2);
  Serial.print(" tauFB="); Serial.print(tauFB, 2);
  Serial.print(" out="); Serial.println(tauOut, 2);
  delay(10);
}

前馈与反馈的比例是关键：前馈越准，反馈就可以越弱，柔顺性自然越好。工程经验是前馈应承担 80~90% 的稳态力矩，模糊 PID 只负责最后的 10~20%。如果反过来让 PID 承担主要力矩，就必然要用大增益，那么一遇到外部扰动（如人碰一下）就会产生“顶回去”的僵硬感。

要点解读
一、模糊 PID 的价值在“变工况”，不在“省调参”
如果膝关节只在单一工况（比如恒定负载的坐站转换）下工作，精心整定的固定 PID 完全够用，模糊推理反而是负担。模糊 PID 真正不可替代的场景是特性随工况大幅漂移：支撑期要求高刚度（Kp 大），摆动期要求低刚度（Kp 小 3 倍），负载从 0 变到 40kg，这些都让单一增益失效。所以设计模糊 PID 的第一步，是明确“你要覆盖哪几个工况”，并让规则表的设计以此为目标，而不是先写表再想用途。

二、规则表要遵循“物理直觉”，不要追求数学最优
模糊规则表的价值在于把工程师的经验写成可执行的形式。三条基本直觉足够覆盖大部分场景：① 误差大 → 加大 Kp，先快速消除偏差；② 误差变化快 → 加大 Kd，防止冲过头；③ 误差接近零 → 适度补 Ki，消残差。案例一的 7×7 表就是这三条的展开。追求“最优规则表”（比如用遗传算法离线优化）在实验室有效，但换一个受试者、换一双鞋，最优就变了；而物理直觉型的规则表泛化性更好，也更容易向临床医生解释。

三、模糊输出必须做“增量 + 限幅”，绝不能直接输出增益
模糊推理直接输出 Kp（比如 0~10）会让系统在规则表边界处跳变；正确做法是输出增量 ΔKp，并叠加到一个可靠的基础 PID 上（案例一 kpCur = KP0 + dKp），再加硬上下限。这样即使模糊推理出错（比如隶属度全为 0、除零），系统仍退化为基础 PID 工作，不会失控。这条对医疗器械类应用是刚需：任何“智能”环节都必须是可退化、可旁路的。

四、力矩环的微分项要极小，噪声是它的天敌
位置环的 Kd 可以到 0.5，力矩环的 Kd 通常只有 0.01~0.3（差一个数量级）。原因在于力矩反馈来自电流采样，本身含开关噪声与 PWM 纹波；微分会把这个噪声放大成力矩抖动，直接传到人腿上。同理，力矩环的 output_ramp（斜率限幅）比位置环更重要：案例一、三都用 0.015~0.03 Nm/周期 的斜率，保证力矩是“长出来”的而不是“跳出来”的。柔顺的物理本质就是力矩的时间连续性。

五、前馈优先，反馈兜底——这是柔顺控制的最高原则
膝关节最容易被忽视的事实是：重力矩是关节角的确定函数（τ = m·g·L·sin θ），完全可以算出来，不需要靠 PID 去“试”。案例三的结构给出了标准答案：重力前馈 + 摩擦前馈 + 速度前馈承担主要力矩，模糊 PID 只修残差。这样做有三个好处：① 前馈无延迟，人一动就能立刻得到正确的支撑；② PID 增益可以很小，对外力干扰不“顶牛”，柔顺性自然好；③ 系统对参数变化的鲁棒性由前馈保证，不依赖高增益。反过来，若把主要力矩交给 PID，就必须用大增益才能跟上重力变化，一旦有人推一下，机器人立刻“硬”起来——这正是很多外骨骼穿起来“硌人”的根本原因。

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