在基于 Arduino + BLDC（无刷直流电机）​ 的机器人系统中，动态速度调节模糊控制器（Fuzzy Logic Speed Controller），是一种不依赖精确数学模型的智能调节方式。它针对 BLDC 驱动机器人在复杂工况（负载波动、地面摩擦变化、电池电压跌落、崎岖地形）下，传统 PID 参数固定难以兼顾快慢响应的痛点，利用语言规则（IF-THEN）将"速度偏差"和"偏差变化率"映射为对 BLDC 目标速度、加速度限制或 PID 增益的在线修正，实现自适应运动控制。

一、主要特点：模型无关 + 在线自适应

1. 模糊化输入与输出
   输入变量（通常两个）：
   速度偏差（Error）：期望速度 − 实际编码器速度
   偏差变化率（Delta Error）：本次偏差 − 上次偏差（反映加速或减速趋势）
   输出变量（可多选，依设计目的）：
   直接输出 BLDC 目标速度修正量
   或输出 PID 比例/积分/微分 增益调整量（模糊自整定 PID）
   或输出 加速度限制值（用于梯形速度规划）
2. 语言变量与隶属度函数
   输入/输出在代码中量化为几个模糊语言集，如：
   负大（NB）、负中（NM）、零（ZO）、正中（PM）、正大（PB）
   也可简化成 3 级：小、中、大
   隶属度函数常用三角形或梯形，在 Arduino 中用分段线性查表或 if 区间判断实现，无需浮指数运算。
3. 模糊规则库（核心）
   规则形如：
   若偏差为零 且 变化率为零 → 输出不变
   若偏差为正大 且 变化率为正 → 快速加速（增大目标速度或 Ki）
   若偏差为负（超速）且 变化率负大 → 强减速（限制加速度或减小目标）
   若偏差小 且 变化率大 → 接近目标，收紧积分防超调
   规则数量通常为 5×5=25 条左右，可裁剪到 9~15 条满足基本需求。
   规则体现专家经验：大偏差快追、近目标阻尼、超速抑、打滑松油门。
4. 解模糊（Defuzzification）
   常用 重心法（COG）或最大隶属度平均值（MOM）。
   在 Arduino 中可用查表 + 整数加权求和近似，避免复杂浮点除。
   输出结果经限幅（Clamp）后成最终：
   BLDC 速度环给定 RPM，或
   PID Kp/Ki/Kd 微调值，或
   最大允许加速度（喂给梯形规划器）
5. 与 BLDC 控制环关系
   模糊控制器一般运行在速度环同一周期（如 50~200Hz）或稍慢。
   不直接替代电流环，只在上层修正速度指令或 PID 参数，保留 FOC 电流环的高动态与稳定性。

二、应用场景
扫地/AGV 变负载变速场景
地毯 ↔ 瓷砖：摩擦力突变，固定 PID 易震荡或响应慢。
模糊控制器检测速度掉（负载突增）→ 短时放宽积分或提高目标扭矩上限，防卡死；平路时恢复保守参数降噪。
崎岖地形巡检机器人
单轮短暂悬空或打滑 → 编码器速度异常高。
模糊规则识别“偏差大 + 变化率剧变”判定打滑 → 限制该轮扭矩、增强其余轮驱动，防整机失稳。
人机共存/跟随机器人
跟随人时距离变化不规则。模糊控制器综合距离偏差与趋近速率，柔和调节 BLDC 速度，避免“忽快忽停”。
电池低压补偿
读 Vbat 输入模糊器 → 若电压低，自动降低最大速度上限或限制加速度，延寿并防驱动 UVLO 复位。

三、关键注意事项（设计红线）

1. 量化因子（Scale Factor）标定
   模糊控制器严重依赖输入缩放：
   速度偏差需除以“最大预期偏差”映射到论域（如 -1 ~ +1 或 -100 ~ +100 整型）
   偏差变化率同理
   注意：
   标定过小 → 对所有情况都当“小偏差”，规则不敏感
   标定过大 → 微扰被当“大偏差”，输出剧烈跳变 → BLDC 抖动
   建议：实测典型工况记录 max|Error|、max|ΔError|，取 80% 分位做基准，留余量。
2. 规则完备性与冲突
   必须覆盖常见组合（尤其 ZO/ZO、PB/NB 等）。
   多条规则可能被激活，解模糊会自然加权；但若存在矛盾极端规则同时高激活（极少见若论域划分合理），输出可能跳。
   可加输出滑动平均（3~5 点）平滑模糊量，防 BLDC 目标值阶跃。
3. 运算量与 MCU 选型
   完整 5×5 规则 + 浮点 COG 在 AVR Arduino（Uno/Mega）​ 可能拖慢主循环（尤其若还跑 FOC）。
   优化手段：
   用 8-bit 或 16-bit 整型查表（预计算激活强度和输出）
   简化为 3×3 规则（9 条）
   用 最大隶属度法（MOM）​ 代替 COG（省除）
   或改用 ESP32 / STM32（Arduino Core），有 FPU，可直写 float 版清晰规则
   模糊计算建议放在主循环定时调用（如每 10ms），不占 ADC/PWM ISR。
4. 与 PID 结合方式选择
   方案 A：模糊修正目标速度（简单）
   不改 PID，只调速度给定 → 最安全，易调试
   方案 B：模糊自整定 PID 增益（高级）
   在线调 Kp/Ki/Kd → 适应性强，但需提防积分 wind‑up 与增益跳变致不稳定
   必须：
   对 Kp/Ki/Kd 做变化率限幅
   积分分离（仅在接近目标才启用 Ki）
   推荐初学/工程落地用方案 A，稳定后再尝试 B。
5. 安全限幅与失效保护
   模糊输出必须被物理极限钳位：
   最大速度 ≤ BLDC 额定 RPM
   最大加速度 ≤ 电机/电池承受
   若模糊模块异常（NaN/超时）→ 切回默认 PID 参数或固定保守速度
   急停、 cliff、过流仍具最高优先级硬件或中断级处理，可强切 EN。

1、三向超声波模糊避障速度调节
这是最经典的入门级应用——三个超声波传感器（左、前、右），根据距离模糊判断，动态调节左右BLDC电机速度实现差速避障。

#include <Fuzzy.h>

Fuzzy* fuzzy = new Fuzzy();

void setup() {
  Serial.begin(115200);
  
  // 定义距离输入：{很近, 近, 中, 远, 很远}
  FuzzySet* vClose   = new FuzzySet(0, 0, 15, 30);   // 0-15cm 很近
  FuzzySet* close    = new FuzzySet(15, 30, 40, 60);  // 15-40cm 近
  FuzzySet* mid      = new FuzzySet(30, 60, 80, 100); // 60-80cm 中
  FuzzySet* far      = new FuzzySet(80, 100, 120, 150);
  FuzzySet* vFar     = new FuzzySet(120, 150, 200, 300);
  
  FuzzyInput* distF = new FuzzyInput(1);
  distF->addFuzzySet(vClose);
  distF->addFuzzySet(close);
  distF->addFuzzySet(mid);
  distF->addFuzzySet(far);
  distF->addFuzzySet(vFar);
  
  // 定义转向输出：{左大转, 左小转, 直行, 右小转, 右大转}
  FuzzySet* turnLBig = new FuzzySet(-255, -255, -200, -100);
  FuzzySet* turnLSml = new FuzzySet(-150, -100, -50, 0);
  FuzzySet* straight = new FuzzySet(-50, 0, 0, 50);
  FuzzySet* turnRSml = new FuzzySet(0, 50, 100, 150);
  FuzzySet* turnRBig = new FuzzySet(100, 200, 255, 255);
  
  FuzzyOutput* turnOut = new FuzzyOutput(1);
  turnOut->addFuzzySet(turnLBig);
  turnOut->addFuzzySet(turnLSml);
  turnOut->addFuzzySet(straight);
  turnOut->addFuzzySet(turnRSml);
  turnOut->addFuzzySet(turnRBig);
  
  // 规则库
  // 规则1: 前方很近，左右差不多 → 随机大转
  fuzzy->addRule(new FuzzyRule(1, distF, vClose) && 
                 new FuzzyRule(1, distF, vClose), turnOut, turnLBig);
  // 规则2: 前方近，左边比右边远 → 左转
  fuzzy->addRule(new FuzzyRule(1, distF, close) && 
                 new FuzzyRule(1, distF, close), turnOut, turnLSml);
  // 规则3: 前方近，右边比左边远 → 右转
  fuzzy->addRule(new FuzzyRule(1, distF, close), turnOut, turnRSml);
  // 规则4: 前方中，左边很近 → 右小转
  fuzzy->addRule(new FuzzyRule(1, distF, mid) && 
                 new FuzzyRule(1, distF, vClose), turnOut, turnRSml);
  // 规则5: 前方中，右边很近 → 左小转
  fuzzy->addRule(new FuzzyRule(1, distF, mid) && 
                 new FuzzyRule(1, distF, vClose), turnOut, turnLSml);
  // 规则6: 其他情况 → 直行
  fuzzy->addRule(new FuzzyRule(1, distF, mid), turnOut, straight);
  
  pinMode(9, OUTPUT);  // 左电机PWM
  pinMode(10, OUTPUT); // 右电机PWM
}

void loop() {
  int distF = analogRead(A0);  // 前方超声波
  int distL = analogRead(A1);  // 左侧超声波
  int distR = analogRead(A2);  // 右侧超声波
  
  // 模糊化
  distF->setValue(distF);
  distL->setValue(distL);
  distR->setValue(distR);
  
  fuzzy->fuzzify();
  float bias = turnOut->defuzzify();
  
  // 应用输出到BLDC电机（差速转向）
  int baseSpeed = 180;
  int leftSpeed  = baseSpeed + bias;
  int rightSpeed = baseSpeed - bias;
  
  leftSpeed  = constrain(leftSpeed, 0, 255);
  rightSpeed = constrain(rightSpeed, 0, 255);
  
  analogWrite(9, leftSpeed);
  analogWrite(10, rightSpeed);
  
  delay(50);  // 控制周期50ms
}

2、基于模糊PID的BLDC速度自适应控制器
将模糊逻辑与PID融合，根据速度误差动态调整Kp、Ki、Kd参数，解决传统PID在负载突变时的超调与震荡问题。

#include <Fuzzy.h>

Fuzzy* fuzzyPID = new Fuzzy();
float integral = 0;
float lastError = 0;

void setupFuzzyPID() {
  // 输入1: 速度误差 e (目标速度 - 实际速度)
  FuzzySet* nb = new FuzzySet(-100, -100, -50, -20);  // 负大
  FuzzySet* ns = new FuzzySet(-50, -20, -10, 0);       // 负小
  FuzzySet* ze = new FuzzySet(-10, 0, 0, 10);          // 零
  FuzzySet* ps = new FuzzySet(0, 10, 20, 50);           // 正小
  FuzzySet* pb = new FuzzySet(20, 50, 100, 100);       // 正大
  
  FuzzyInput* errorInput = new FuzzyInput(1);
  errorInput->addFuzzySet(nb);
  errorInput->addFuzzySet(ns);
  errorInput->addFuzzySet(ze);
  errorInput->addFuzzySet(ps);
  errorInput->addFuzzySet(pb);
  
  // 输入2: 误差变化率 ec
  FuzzyInput* ecInput = new FuzzyInput(2);
  ecInput->addFuzzySet(nb);
  ecInput->addFuzzySet(ns);
  ecInput->addFuzzySet(ze);
  ecInput->addFuzzySet(ps);
  ecInput->addFuzzySet(pb);
  
  // 输出: Kp增益
  FuzzySet* kpVL = new FuzzySet(3.0, 3.0, 4.0, 5.0);   // 很大
  FuzzySet* kpL  = new FuzzySet(1.5, 2.0, 2.5, 3.0);   // 大
  FuzzySet* kpM  = new FuzzySet(1.0, 1.5, 2.0, 2.5);   // 中
  FuzzySet* kpS  = new FuzzySet(0.5, 0.8, 1.2, 1.5);   // 小
  
  FuzzyOutput* kpOut = new FuzzyOutput(1);
  kpOut->addFuzzySet(kpVL);
  kpOut->addFuzzySet(kpL);
  kpOut->addFuzzySet(kpM);
  kpOut->addFuzzySet(kpS);
  
  // 模糊规则
  fuzzyPID->addRule(new FuzzyRule(1, errorInput, nb) && 
                    new FuzzyRule(2, ecInput, nb), kpOut, kpVL);
  fuzzyPID->addRule(new FuzzyRule(1, errorInput, ze) && 
                    new FuzzyRule(2, ecInput, ns), kpOut, kpM);
  fuzzyPID->addRule(new FuzzyRule(1, errorInput, pb) && 
                    new FuzzyRule(2, ecInput, ps), kpOut, kpS);
  
  pinMode(9, OUTPUT);  // BLDC ESC PWM
}

void loop() {
  float targetSpeed = 100.0;
  float actualSpeed = readEncoder();  // 读取编码器速度
  
  float error = targetSpeed - actualSpeed;
  float ec = error - lastError;
  integral += error;
  integral = constrain(integral, -500, 500);  // 积分限幅
  
  // 模糊推理获取自适应Kp
  errorInput->setValue(error);
  ecInput->setValue(ec);
  fuzzyPID->fuzzify();
  float Kp = kpOut->defuzzify();
  
  float Ki = 0.1;   // 固定积分增益
  float Kd = 0.05;  // 固定微分增益
  
  // PID计算
  float output = Kp * error + Ki * integral + Kd * ec;
  output = constrain(output, 1000, 2000);  // ESC PWM范围
  
  analogWrite(9, map(output, 1000, 2000, 0, 255));
  
  lastError = error;
  delay(10);  // 100Hz控制周期
}

3、多传感器融合动态速度模糊调度（含安全本能）
这是工业级方案——融合超声波距离、电池电压、IMU倾角，采用"本能层+智能层"双层架构，硬阈值急停 + 模糊速度调节。

#include <Fuzzy.h>

Fuzzy* speedFuzzy = new Fuzzy();
const int EMERGENCY_DIST = 15;    // 硬急停阈值 cm
const int CRITICAL_DIST  = 40;    // 模糊介入阈值 cm

void setup() {
  Serial.begin(115200);
  
  // === 本能层：硬阈值急停 ===
  // 前方距离 < 15cm → 立即切断电机（硬件中断级）
  
  // === 智能层：模糊速度调节 ===
  // 输入1: 前方距离
  FuzzySet* vClose  = new FuzzySet(0, 0, 15, 30);
  FuzzySet* close   = new FuzzySet(15, 30, 50, 80);
  FuzzySet* mid     = new FuzzySet(50, 80, 120, 160);
  FuzzySet* far     = new FuzzySet(120, 160, 200, 300);
  
  FuzzyInput* distIn = new FuzzyInput(1);
  distIn->addFuzzySet(vClose);
  distIn->addFuzzySet(close);
  distIn->addFuzzySet(mid);
  distIn->addFuzzySet(far);
  
  // 输入2: 电池电压（归一化到0-1）
  FuzzySet* batLow  = new FuzzySet(0, 0, 0.2, 0.4);   // <3.7V
  FuzzySet* batMid  = new FuzzySet(0.3, 0.5, 0.7, 0.8);
  FuzzySet* batHigh = new FuzzySet(0.7, 0.8, 1.0, 1.0); // >4.2V
  
  FuzzyInput* batIn = new FuzzyInput(2);
  batIn->addFuzzySet(batLow);
  batIn->addFuzzySet(batMid);
  batIn->addFuzzySet(batHigh);
  
  // 输出: 速度修正系数 (0.0 ~ 1.0)
  FuzzySet* slow   = new FuzzySet(0.0, 0.0, 0.2, 0.4);
  FuzzySet* medium = new FuzzySet(0.3, 0.5, 0.7, 0.8);
  FuzzySet* fast   = new FuzzySet(0.7, 0.8, 1.0, 1.0);
  
  FuzzyOutput* speedOut = new FuzzyOutput(1);
  speedOut->addFuzzySet(slow);
  speedOut->addFuzzySet(medium);
  speedOut->addFuzzySet(fast);
  
  // 规则库
  fuzzy->addRule(new FuzzyRule(1, distIn, vClose), speedOut, slow);
  fuzzy->addRule(new FuzzyRule(1, distIn, close) && 
                 new FuzzyRule(2, batIn, batLow), speedOut, slow);
  fuzzy->addRule(new FuzzyRule(1, distIn, mid) && 
                 new FuzzyRule(2, batIn, batMid), speedOut, medium);
  fuzzy->addRule(new FuzzyRule(1, distIn, far) && 
                 new FuzzyRule(2, batIn, batHigh), speedOut, fast);
  
  pinMode(9, OUTPUT);  // BLDC ESC
}

void loop() {
  int dist = readUltrasonic();          // 前方距离 cm
  float batV = readBattery() / 5.0;      // 归一化电压
  
  // ========== 本能层：硬急停 ==========
  if (dist < EMERGENCY_DIST) {
    analogWrite(9, 0);  // 立即停转！
    return;
  }
  
  // ========== 智能层：模糊速度调节 ==========
  if (dist < CRITICAL_DIST) {
    distIn->setValue(dist);
    batIn->setValue(batV);
    
    fuzzy->fuzzify();
    float speedFactor = speedOut->defuzzify();
    
    int baseSpeed = 200;  // 基础速度
    int targetPWM = map(baseSpeed * speedFactor, 0, 200, 1000, 2000);
    targetPWM = constrain(targetPWM, 1000, 2000);
    
    analogWrite(9, map(targetPWM, 1000, 2000, 0, 255));
  } else {
    analogWrite(9, 255);  // 全速巡航
  }
  
  delay(30);  // 33Hz调度周期
}

要点解读
一、模糊控制器的本质是"用语言规则替代精确数学模型"
传统PID依赖被控对象的精确传递函数，而模糊控制器通过"如果距离很近且电池充足，则速度为慢"这类人类经验规则直接驱动电机。这使得系统对电机死区、静摩擦→动摩擦转换、轮胎-地面滑移等非线性环节具有天然鲁棒性——这是PID在Arduino上难以企及的优势。

二、BLDC+FOC是模糊输出的最佳物理载体
模糊控制器输出的是连续平滑的速度曲线，这对执行机构极为苛刻。BLDC配合FOC（磁场定向控制）能实现0.1RPM级别的速度分辨率，低速时正弦波驱动依然平稳无扭矩脉动，全速域保持线性转矩特性。相比有刷电机，模糊控制的细腻调节在BLDC上才能真正发挥——这是"软逻辑"与"硬执行"的完美匹配。

三、查表法（LUT）是Arduino上的唯一可行之路
标准Arduino Uno（16MHz, 8位AVR）跑完整模糊推理可能超过100ms。实战中必须采用查表法：在系统初始化阶段预计算所有输入组合对应的输出，存储在二维/三维数组中，运行时仅需output = lookupTable[distIdx][batIdx]。输入量化（如每10cm一级）+ 规则库剪枝可将查表大小控制在几KB以内，确保控制周期<10ms。

四、“安全本能"双层架构是工程落地的底线
纯模糊控制存在逻辑死循环风险，必须叠加硬阈值本能层：距离<15cm时绕过所有模糊计算，直接切断电机动力。这不是理论问题，而是工业级机器人设计的基本准则。同时，硬件看门狗必须启用，传感器失效时自动降级到安全模式（减速停止），模糊逻辑是"大脑”，硬线急停是"脊髓反射"。

五、从模糊控制到模糊Q学习——智能的进化路径
基础模糊控制器依赖人工规则，而模糊Q学习将强化学习与模糊逻辑融合，利用模糊化压缩连续状态空间，使Q表能存储在Arduino有限的RAM中（<2KB）。系统通过与环境交互自主优化调度策略，奖励函数综合考量任务完成度、能耗、安全性，实现从"专家经验"到"自主学习"的跨越。建议分阶段部署：仿真验证→离线训练→受限在线学习→完全在线学习，切勿一步到位。

4、负载突变下的模糊速度补偿系统（基于电流检测的自适应调速）
场景定位：针对机器人爬坡、顶推重物、穿越松软地面等负载突变场景，传统PID控制器易因负载突变导致速度波动大、响应滞后，模糊控制器可实时感知负载变化，动态调整BLDC电机转速，保障速度稳定性。

核心逻辑：以负载变化误差（当前电流-额定电流）为输入，通过模糊控制器输出速度补偿量，叠加到基础转速上。模糊控制核心是将电流误差的模糊值转化为精确的速度补偿指令，解决负载突变时的转速过冲或不足问题。

#include <SimpleFOC.h>

// 硬件定义
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver;

// 模糊控制参数
const int CURRENT_NORM = 500;  // 额定工作电流（电机空载稳定电流）
const float BASE_SPEED = 0.6;  // 基础目标转速（归一化：0~1）
float currentVal = 0;
float error;
float compensation;

// 模糊输入变量（论域与隶属度函数）
#define NB -3, -2, -1, 0  // 负大：负载极轻
#define NS -2, -1, 0, 1   // 负小：负载偏轻
#define ZE  -1, 0, 1, 2   // 零：负载匹配
#define PS 0, 1, 2, 3      // 正小：负载略重
#define PB 1, 2, 3, 4      // 正大：负载极重

// 电流采样（简化为模拟采样，实际需接电流采样电路）
float readCurrent() {
  // 假设通过INA219电流传感器采样，此处简化为模拟口读取（需匹配硬件连接）
  return analogRead(A0) * (3.3 / 1023.0) * 1000; // 转换为mA
}

// 模糊化函数：将误差映射到模糊论域
float fuzzify(float error) {
  // 归一化误差到[-3,4]范围
  float normError = map(error, -CURRENT_NORM, 1000, -3, 4);
  normError = constrain(normError, -3, 4);
  return normError;
}

// 模糊推理：基于误差隶属度计算补偿量
float fuzzyInference(float errorNorm) {
  float muNB = 0, muNS = 0, muZE = 0, muPS = 0, muPB = 0;
  
  // 计算误差对各模糊集的隶属度
  if (errorNorm <= -3) muNB = 1;
  else if (errorNorm <= -2) muNB = (-3 - errorNorm)/1.0;
  
  if (errorNorm >= -2 && errorNorm <= 0) muNS = 1 - abs(errorNorm+1)/1.0;
  
  if (errorNorm >= -1 && errorNorm <=1) muZE = 1 - abs(errorNorm)/1.0;
  
  if (errorNorm >=0 && errorNorm <=2) muPS = 1 - abs(errorNorm-1)/1.0;
  
  if (errorNorm >=2) muPB = 1;
  else if (errorNorm >=1) muPB = (errorNorm-1)/1.0;
  
  // 重心法去模糊化（补偿量论域：-0.2~0.2）
  float numerator = muNB*(-0.2) + muNS*(-0.1) + muZE*0 + muPS*0.1 + muPB*0.2;
  float denominator = muNB + muNS + muZE + muPS + muPB;
  
  if (denominator == 0) return 0;
  return numerator / denominator;
}

void setup() {
  Serial.begin(115200);
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  pinMode(A0, INPUT);
}

void loop() {
  // 1. 采样电流与计算误差
  currentVal = readCurrent();
  error = currentVal - CURRENT_NORM;
  
  // 2. 模糊化与推理
  float errorNorm = fuzzify(error);
  compensation = fuzzyInference(errorNorm);
  
  // 3. 速度调节：基础转速+补偿量
  float targetSpeed = BASE_SPEED + compensation;
  targetSpeed = constrain(targetSpeed, 0.2, 1.0);
  motor.move(targetSpeed);
  
  // 调试输出
  Serial.print("电流:"); Serial.print(currentVal);
  Serial.print(" 误差:"); Serial.print(error);
  Serial.print(" 补偿量:"); Serial.print(compensation);
  Serial.print(" 目标转速:"); Serial.println(targetSpeed);
  
  delay(50); // 20Hz控制周期
}

5、路径曲率自适应模糊调速系统（基于IMU与编码器的动态调速）
场景定位：针对机器人在S形弯道、直角转弯等复杂路径行驶时，传统固定速度易因转弯半径过小导致侧翻或失控，模糊控制器可根据路径曲率实时调整速度，保障行驶稳定性与效率。

核心逻辑：以路径曲率误差（期望曲率-当前实际曲率）和当前速度误差（期望速度-实际速度）为输入，模糊控制器输出速度调节量。曲率通过IMU的航向角变化率与编码器的线速度联合计算，实现路径曲率的实时感知，进而动态优化速度。

#include <SimpleFOC.h>
#include <MPU6050.h>

// 硬件定义
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver;
MPU6050 imu;

// 编码器引脚（简化示例，实际需接正交编码器）
Encoder enc;
float linearVel = 0;
float angularVel = 0;

// 模糊控制参数
const float TARGET_SPEED = 0.5;   // 期望直行速度
const float MAX_CURVATURE = 1.0;  // 最大曲率（对应最小转弯半径）
float curvatureError;
float speedError;
float speedDelta;

// 模糊输入论域
#define ERROR_NB -2, -1, 0
#define ERROR_ZE  -1, 0, 1
#define ERROR_PB 0, 1, 2

#define DELTA_NB -0.15, -0.1, -0.05
#define DELTA_ZE -0.05, 0, 0.05
#define DELTA_PB 0.05, 0.1, 0.15

// 曲率计算：曲率=角速度/线速度
float calculateCurvature() {
  if (abs(linearVel) < 0.01) return 0;
  return abs(angularVel / linearVel);
}

// 模糊化曲率误差（误差范围：-2~2，曲率误差=实际曲率-期望曲率，期望曲率默认0（直行））
float fuzzifyCurvature(float error) {
  return map(error, -MAX_CURVATURE, MAX_CURVATURE, -2, 2);
}

// 模糊推理：双输入（曲率误差、速度误差）到单输出（速度增量）
float fuzzySpeedAdjust(float cError, float sError) {
  // 曲率误差隶属度
  float muNB_c = 0, muZE_c = 0, muPB_c = 0;
  if (cError <= -2) muNB_c = 1;
  else if (cError <= 0) muNB_c = 1 - (cError+2)/2.0;
  if (cError >= -1 && cError <=1) muZE_c = 1 - abs(cError)/1.0;
  if (cError >=0 && cError <=2) muPB_c = 1 - (2 - cError)/2.0;
  if (cError >=2) muPB_c = 1;
  
  // 速度误差隶属度
  float muNB_s = 0, muZE_s = 0, muPB_s = 0;
  if (sError <= -1) muNB_s = 1;
  else if (sError <=0) muNB_s = 1 - (sError+1)/1.0;
  if (sError >= -0.5 && sError <=0.5) muZE_s = 1 - abs(sError*2);
  if (sError >=0 && sError <=1) muPB_s = 1 - (1 - sError)/1.0;
  if (sError >=1) muPB_s = 1;
  
  // 规则推理与重心法去模糊化
  float numerator = 0, denominator = 0;
  // 规则1：曲率负大+速度负大→速度增量负大
  numerator += muNB_c * muNB_s * (-0.15);
  denominator += muNB_c * muNB_s;
  // 规则2：曲率零+速度零→速度增量零
  numerator += muZE_c * muZE_s * 0;
  denominator += muZE_c * muZE_s;
  // 规则3：曲率正大+速度正大→速度增量负大（曲率大减速）
  numerator += muPB_c * muPB_s * (-0.15);
  denominator += muPB_c * muPB_s;
  
  // 更多规则可扩展，此处简化为典型组合
  if (denominator == 0) return 0;
  return numerator / denominator;
}

void setup() {
  Serial.begin(115200);
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  imu.initialize();
  enc.init();
  motor.move(TARGET_SPEED);
}

void loop() {
  // 1. 传感器数据获取
  sensors_event_t accel, gyro;
  imu.getEvent(&accel, &gyro);
  angularVel = gyro.gyro.z; // 绕z轴的角速度（航向角变化率）
  linearVel = enc.getVelocity(); // 线速度（从编码器获取）
  
  // 2. 计算曲率与误差
  float curvature = calculateCurvature();
  curvatureError = curvature - 0; // 期望曲率为0（直行）
  speedError = TARGET_SPEED - linearVel;
  
  // 3. 模糊推理输出速度增量
  float cErrorNorm = fuzzifyCurvature(curvatureError);
  speedDelta = fuzzySpeedAdjust(cErrorNorm, speedError);
  
  // 4. 速度调节
  float targetSpeed = TARGET_SPEED + speedDelta;
  targetSpeed = constrain(targetSpeed, 0.1, 0.8); // 限制安全速度范围
  motor.move(targetSpeed);
  
  // 调试输出
  Serial.print("曲率:"); Serial.print(curvature);
  Serial.print(" 曲率误差:"); Serial.print(cErrorNorm);
  Serial.print(" 速度误差:"); Serial.print(speedError);
  Serial.print(" 速度增量:"); Serial.print(speedDelta);
  Serial.print(" 目标速度:"); Serial.println(targetSpeed);
  
  delay(30); // 约33Hz控制周期
}

6、障碍规避与速度协同模糊控制系统（基于距离传感器的动态降速）
场景定位：针对机器人在密集障碍场景（如仓库货架间、家庭家具环境）行驶时，需实时调整速度：障碍近时降速规避，障碍远时提速高效行驶，传统阈值切换易导致速度突变，模糊控制可实现速度平滑过渡。

核心逻辑：以障碍距离和当前速度为输入，模糊控制器输出速度调节系数，实现从“紧急制动”到“匀速行驶”的平滑过渡。障碍距离通过超声波/激光雷达获取，模糊控制规则模拟人工避障的直觉经验，保障避障效率与行驶平稳性。

#include <SimpleFOC.h>
#include <HCSR04.h> // 超声波传感器库

// 硬件定义
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver;
HCSR04 ultrasonic(TRIG_PIN, ECHO_PIN); // 超声波传感器引脚

// 模糊控制参数
const float MAX_SPEED = 0.7;      // 最高行驶速度
const float SAFE_DIST = 0.3;      // 安全距离阈值（30cm）
const float MIN_SPEED = 0.1;      // 最低行驶速度（蠕动）
float dist;
float currentSpeed;
float speedCoef;

// 模糊论域与隶属度函数
#define DIST_NEAR 0, 0.1, 0.2   // 近（0~20cm）
#define DIST_MID  0.1, 0.3, 0.5 // 中（10~50cm）
#define DIST_FAR  0.4, 0.6, 1.0 // 远（40cm以上）

#define SPEED_LOW  0, 0.3, 0.5  // 低速（0~0.5）
#define SPEED_MID  0.3, 0.5, 0.7// 中速（0.3~0.7）
#define SPEED_HIGH 0.5, 0.7, 1.0// 高速（0.5~1.0）

#define COEF_DEC -0.5, -0.2, 0  // 减速系数
#define COEF_KEEP 0, 0.3, 0.6   // 保持系数
#define COEF_INC 0.3, 0.6, 1.0  // 加速系数

// 距离归一化（0~1：0为最近，1为最远，映射到100cm量程）
float normalizeDist(float distCM) {
  return constrain(map(distCM, 0, 100, 0, 1), 0, 1);
}

// 模糊推理：距离+速度→速度系数
float fuzzyObstacleSpeed(float normDist, float speed) {
  // 距离隶属度
  float muNear = 0, muMid = 0, muFar = 0;
  if (normDist <= 0.1) muNear = 1;
  else if (normDist <= 0.2) muNear = 1 - (normDist - 0.1)/0.1;
  if (normDist >= 0.1 && normDist <=0.3) muMid = 1 - abs(normDist-0.2)/0.2;
  if (normDist >= 0.4) muFar = 1;
  else if (normDist >=0.3) muFar = (normDist - 0.3)/0.1;
  
  // 速度隶属度
  float muLow = 0, muMidS = 0, muHigh = 0;
  if (speed <= 0.3) muLow = 1;
  else if (speed <=0.5) muLow = 1 - (speed-0.3)/0.2;
  if (speed >=0.3 && speed <=0.7) muMidS = 1 - abs(speed-0.5)/0.4;
  if (speed >=0.7) muHigh = 1;
  else if (speed >=0.5) muHigh = (speed-0.5)/0.2;
  
  // 规则推理（核心逻辑：近且速高→强减速；远且速低→强加速）
  float numerator = 0, denominator = 0;
  // 规则1：近+高速→强减速（系数-0.5）
  numerator += muNear * muHigh * (-0.5);
  denominator += muNear * muHigh;
  // 规则2：中+中速→保持速度（系数0.3）
  numerator += muMid * muMidS * 0.3;
  denominator += muMid * muMidS;
  // 规则3：远+低速→强加速（系数1.0）
  numerator += muFar * muLow * 1.0;
  denominator += muFar * muLow;
  
  // 补充规则：近+低速→弱减速；远+高速→弱加速
  numerator += muNear * muLow * (-0.2);
  denominator += muNear * muLow;
  numerator += muFar * muHigh * 0.6;
  denominator += muFar * muHigh;
  
  if (denominator == 0) return 0.3;
  return numerator / denominator;
}

void setup() {
  Serial.begin(115200);
  driver.init();
  motor.linkDriver(&driver);
  motor.init();
  ultrasonic.init();
  motor.move(MAX_SPEED); // 初始高速
}

void loop() {
  // 1. 读取障碍距离
  dist = ultrasonic.getDistance() / 100.0; // 转换为米
  currentSpeed = motor.target; // 获取当前设定速度
  
  // 2. 模糊推理
  float normDist = normalizeDist(dist*100); // 归一化到0~1
  speedCoef = fuzzyObstacleSpeed(normDist, currentSpeed);
  
  // 3. 速度计算：基础速度*系数，限制范围
  float targetSpeed = MAX_SPEED * speedCoef;
  targetSpeed = constrain(targetSpeed, MIN_SPEED, MAX_SPEED);
  motor.move(targetSpeed);
  
  // 调试输出
  Serial.print("距离:"); Serial.print(dist);
  Serial.print(" 归一化距离:"); Serial.print(normDist);
  Serial.print(" 当前速度:"); Serial.print(currentSpeed);
  Serial.print(" 速度系数:"); Serial.print(speedCoef);
  Serial.print(" 目标速度:"); Serial.println(targetSpeed);
  
  delay(40); // 25Hz控制周期
}

要点解读

1. 模糊控制核心：从“精确建模”到“直觉推理”的思维转变
   模糊控制的本质是将人类的直觉经验转化为可执行的规则，无需被控对象（如BLDC电机+负载+机器人运动学）的精确数学模型，完美解决BLDC机器人在复杂动态场景下难以建模的问题：
   输入模糊化：将精确的电流、曲率、距离等物理量，转化为“大/小、近/远、快/慢”等模糊语言变量，契合人类对不确定场景的认知逻辑。
   规则驱动决策：通过IF-THEN形式的模糊规则，模拟人类驾驶/操控的直觉反应，无需推导复杂的物理方程，规则设计简单直观。
   输出去模糊化：将模糊的控制指令转化为精确的电机转速调节量，核心是“重心法”，确保控制指令的连续性，避免阈值切换的突变问题。

2. 动态调速适配：模糊控制器与BLDC电机的“快响应+强鲁棒”协同
   BLDC电机动态调速的核心需求是快速响应负载/环境变化，维持速度稳定，模糊控制器通过以下设计实现与BLDC电机的协同：
   输入适配BLDC特性：选择与BLDC调速强关联的物理量作为模糊输入，如电流直接反映负载变化、曲率反映路径复杂度，避免冗余输入导致推理滞后。
   输出对接FOC控制：模糊控制器输出的速度补偿量直接叠加到BLDC电机的FOC控制环（通过motor.move()指令），FOC的快速电流环保障速度指令的实时执行，二者形成“感知-决策-执行”的高速闭环。
   参数自适应设计：模糊规则可根据电机特性动态调整，如重载电机的补偿系数可适当增大，轻载电机可减小，适配不同功率的BLDC电机。

3. 输入变量设计：聚焦“强关联、可感知”的核心物理量
   模糊控制器的输入直接决定控制性能，需避免“为复杂而复杂”，应聚焦与速度调节强关联、易通过传感器获取的物理量：
   负载场景：选择电流作为输入，电流与BLDC电机输出扭矩直接正相关，能精准反映负载变化，且电流采样电路简单（如INA219），成本低、响应快。
   路径场景：选择曲率作为输入，曲率是路径的核心特征，直接影响机器人转弯时的安全速度，通过IMU+编码器联合计算，无需额外复杂传感器。
   避障场景：选择距离作为输入，距离是避障的直接决策依据，超声波/激光雷达成本低、测距可靠，与速度结合可实现动态避障。

4. 规则设计：遵循“直觉优先、层次分明”的原则，避免规则冗余
   模糊规则是控制器的核心，设计需兼顾有效性与简洁性，避免规则过多导致推理延迟或矛盾：
   直觉贴合人类经验：规则设计模拟人类操控机器人的直觉，无需复杂的理论推导，核心逻辑“当负载重→补偿正速度，负载轻→补偿负速度”“曲率大→降速，曲率小→提速”“障碍近→降速，障碍远→提速”。
   层次化覆盖论域：将输入变量划分为3~5个模糊子集（如NB/ZE/PB），确保覆盖整个论域，每个子集对应明确的物理含义，避免子集重叠过多导致规则冲突。
   规则数量精简：每个案例的规则控制在5~10条，覆盖核心工况，避免冗余规则导致Arduino的算力不足，确保控制周期稳定。

5. 实现与落地：兼顾Arduino算力与控制实时性的工程优化
   Arduino资源有限，模糊控制的落地需通过软硬件优化，平衡控制效果与系统性能：
   算力优化：简化隶属度函数计算，避免使用浮点数复杂运算，同时合理设置控制周期，确保模糊推理能在Arduino的算力范围内完成，不影响BLDC电机的实时控制。
   接口适配传感器：根据输入物理量选择低成本、易适配的传感器，通过库函数简化传感器数据采集，减少底层驱动开发工作量，专注于模糊控制逻辑。
   调试与迭代：通过串口打印关键变量实时调试，快速定位规则不合理、参数不匹配的问题。实际落地时，需通过现场测试调整隶属度函数参数和规则，适配机器人的具体机械结构与电机特性。

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