该方案的核心特点是利用模糊逻辑（Fuzzy Logic）将多源传感器数据转化为仿人直觉的避障决策，结合BLDC电机配合FOC算法实现的高动态响应与平滑差速控制，使清洁机器人能够在复杂动态的室内环境中实现柔顺避障与精准的目标追踪；主要适用于家庭日常清洁、商业场所保洁、科研验证等场景；实际部署需重点解决Arduino算力瓶颈、模糊规则库整定、多传感器融合延迟及BLDC电机高频控制环实时性等问题。

一、主要特点
模糊逻辑避障——从“二值判断”到“仿人直觉决策”
传统避障算法常依赖精确的数学模型或简单的二值逻辑（如“距离<30cm则急停”），在面对传感器噪声或复杂环境时容易产生剧烈抖动或误判。模糊逻辑通过引入“隶属度函数”，将精确的距离值转化为“较近”、“适中”、“较远”等模糊概念，模拟人类驾驶员的直觉经验。
非线性适应性：模糊控制器通过预设的“If-Then”规则库（如“如果前方距离很近，且左侧距离较远，则向左急转”）进行决策，无需对机器人的运动学或动力学进行复杂的数学建模，能很好地处理传感器数据与环境反应之间的非线性关系。
强鲁棒性：面对未知或不确定的复杂环境（如光照变化、地面纹理、动态障碍物），模糊逻辑对传感器的微小噪声和测量误差不敏感，即使超声波传感器偶尔出现跳变，也能根据隶属度函数平滑地处理数据，避免机器人产生剧烈抖动。
计算高效：相比复杂的SLAM或路径规划算法，模糊逻辑的计算量相对较小，适合在Arduino等资源受限的嵌入式平台上进行快速决策，确保控制周期（Loop Time）足够短（如<50ms），满足实时避障需求。
BLDC+FOC柔顺驱动——从“刚性执行”到“平滑差速”
室内清洁机器人需要在狭窄空间内频繁启停、转向，对电机的动态响应和平稳性要求极高。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC（磁场定向控制）算法的高频电流环（通常>1kHz），能够毫秒级响应模糊控制器发出的加减速指令。通过差速驱动架构，机器人可以实现原地转向、急停和快速倒车等高难度避障动作。
平滑差速控制：结合FOC算法，BLDC电机可以实现精细的力矩闭环控制。通过Clark和Park变换，将三相交流电流解耦为励磁电流（d轴）和转矩电流（q轴），实现转矩脉动<1%的平稳输出。这使得机器人在低速和高速下都能平稳运行，避免传统有刷电机因碳刷摩擦产生的高频啸叫与电火花，运行噪音可控制在环境底噪之下（通常低于35分贝）。
高效稳定运行：相较于有刷电机，BLDC电机效率高（能量转换效率达80%以上）、发热低、寿命长，能够保证机器人在长时间、频繁启停的清洁任务中稳定运行，不易因过热而降额。
目标追踪——从“盲目清扫”到“主动寻的”
在清洁任务中，机器人需要主动寻找并追踪特定的目标（如灰尘聚集区、特定污渍或用户指定的清洁区域）。
多源融合定位：系统融合超声波、红外、视觉（如ESP32-CAM）等传感器数据，通过扩展卡尔曼滤波（EKF）进行紧耦合，实时解算目标的相对位置和运动趋势。
动态轨迹规划：在模糊避障的基础上，规划层实时调整机器人的期望轨迹，确保机器人在避开障碍物的同时，能够平滑地趋近目标。例如，当检测到前方有动态障碍物（如行人）时，系统会自动调整绕行路径，确保不中断对目标的追踪。
自适应速度控制：根据与目标的距离动态调整运动速度。当距离目标较远时，机器人以较高速度趋近；当接近目标时，自动切换到谨慎慢速模式，确保清洁效果的精准性。
分层控制架构
规划层（模糊逻辑+目标追踪）：负责环境感知、避障决策与目标轨迹规划。
控制层（FOC+差速控制）：负责将轨迹转化为关节力矩指令，并处理力交互与协同同步。
感知层（超声波+红外+视觉）：负责提供障碍物距离、目标位置与环境信息。

二、典型应用场景
家庭日常清洁
在家具布局复杂、存在宠物或儿童的室内环境中，清洁机器人需要安全导航并高效完成清洁任务。
模糊避障优势：通过模糊逻辑控制器，机器人能够灵活绕开随机走动的宠物或临时堆放的玩具，避免碰撞或卡死。
BLDC静音优势：无刷电机的低噪音特性使其可以在人员活动时段运行，不会打扰家庭成员的正常生活。
商业场所保洁
在商场、医院、图书馆等人群密集且移动轨迹不可预测的场所，清洁机器人需要适应动态环境。
动态适应优势：模糊逻辑对传感器的微小噪声和测量误差不敏感，即使超声波传感器偶尔出现跳变，也能根据隶属度函数平滑地处理数据，避免机器人产生剧烈抖动。
高效清洁优势：BLDC电机的高功率密度和快速动态响应能力，能够迅速执行模糊控制器发出的加减速指令，确保在人群密集区域快速通过，提高清洁效率。
智能仓储AGV
在货架林立、人员走动的非结构化仓库中，清洁机器人需要自主避障运输。
强鲁棒性优势：面对未知或不确定的复杂环境（如光照变化、地面纹理），系统表现稳定。
精准追踪优势：通过多源融合定位，机器人能够准确识别并追踪灰尘聚集区，实现精准清洁。
科研与教育验证
作为高校机器人学、嵌入式控制课程的实验平台，用于验证模糊逻辑控制、FOC驱动、多传感器融合等前沿技术。

三、需要注意的事项
Arduino算力瓶颈与架构分工
模糊逻辑推理、目标追踪解算、FOC算法同时运行对算力要求极高。
平台选型：经典8位Arduino（如Uno）几乎无法胜任。必须选用ESP32-S3（双核240MHz）、STM32H7或Arduino Portenta H7等高性能平台，或采用“上位机+下位机”架构。
主从架构：建议采用“上位机+下位机”架构。上位机（如树莓派、Jetson或PC）负责模糊逻辑推理、目标追踪解算和轨迹规划；下位机（如专用FOC驱动板）负责高频电流环控制和电机换相。
控制频率与实时性
模糊避障与目标追踪要求极高的实时性，尤其是力矩同步与通道约束响应。
高频控制环：FOC的电流环频率建议≥1kHz，速度环和位置环≥500Hz。模糊逻辑的更新频率应与控制环匹配（如100Hz~500Hz），避免力矩指令滞后。
通信延迟：上位机与下位机之间的通信（如CAN、EtherCAT）延迟必须控制在毫秒级，否则会导致力矩指令滞后，引发机身抖动或碰撞。
模糊规则库设计与优化
模糊规则的质量和数量直接决定避障效果，需结合实际场景反复调试。
规则完备性：模糊规则库必须覆盖所有可能的环境状态组合（如“前方近、左侧远、右侧近”等）。规则缺失会导致在某些特定场景下无输出，机器人失控。
规则冲突与调整：初始规则通常基于经验试凑，可能不够优化。需要通过大量的实地测试，观察机器人的避障轨迹，并反复调整隶属度函数的形状和规则库的内容，以消除震荡或死锁现象。
多传感器融合与数据预处理
传感器数据的准确性与可靠性直接影响模糊逻辑的决策质量。
传感器选型与布局：单一传感器（如仅用超声波）存在盲区和精度限制。建议融合超声波（测距远）、红外/ToF（精度高、响应快）甚至激光雷达（构建局部地图）的数据。传感器的物理布局（前、左前、右前）应能构建完整的前方环境轮廓。
数据滤波：尽管模糊逻辑对噪声有容忍度，但剧烈的跳变仍会影响控制质量。在输入模糊控制器前，应对传感器数据进行滑动平均或中值滤波处理。
电机力矩响应与一致性
目标追踪的效果高度依赖电机的力矩响应性能和一致性。
电机选型：选用低齿槽效应、高扭矩密度的BLDC电机，确保低速下的力矩平稳性。
参数一致性：同一机器人上的所有电机参数（极对数、内阻、电感）必须一致，否则会导致各关节力矩输出不均，影响追踪精度。
电磁兼容（EMC）与电源管理
多电机高频PWM驱动会产生严重的电磁干扰，影响传感器精度。
电源隔离：动力电源与逻辑电源必须物理隔离，使用独立DC-DC模块。
滤波设计：电源入口并联大容量电解电容和高频陶瓷电容，吸收电压尖峰。
布线规范：强电与弱电严格分开，传感器信号线使用屏蔽线。

1、三向感知基础避障控制器
此案例聚焦于环境感知与基础决策。通过前、左、右三个方向的超声波传感器获取障碍物距离，利用模糊逻辑推断出最合适的转向动作，并以差速方式驱动BLDC电机，实现平滑避障。

#include <SimpleFOC.h> // 用于BLDC控制

// 定义BLDC电机 (差速驱动)
BLDCMotor motorLeft = BLDCMotor(7);
BLDCMotor motorRight = BLDCMotor(7);
BLDCDriver3PWM driverLeft = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM driverRight = BLDCDriver3PWM(5, 6, 8);

// 模拟传感器引脚定义
#define FRONT_SENSOR A0
#define LEFT_SENSOR A1
#define RIGHT_SENSOR A2

// 模糊控制输入变量：距离（经过map归一化）
float frontDist, leftDist, rightDist;
// 模糊控制输出变量：转向修正量 (-255 ~ 255)
int turnAdjust = 0;

void setup() {
  Serial.begin(115200);
  // 初始化电机及FOC
  driverLeft.init(); driverRight.init();
  motorLeft.linkDriver(&driverLeft); motorRight.linkDriver(&driverRight);
  motorLeft.init(); motorRight.init();
  motorLeft.initFOC(); motorRight.initFOC();
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;

  // 传感器引脚初始化
  pinMode(FRONT_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);
}

void loop() {
  // 1. 传感器数据采集与模糊化
  frontDist = analogRead(FRONT_SENSOR); // 原始值0~1023
  leftDist = analogRead(LEFT_SENSOR);
  rightDist = analogRead(RIGHT_SENSOR);
  
  // 2. 核心模糊逻辑推理 (极简规则，仅做演示)
  // 规则：前方越近，转向角度越大；左侧障碍大于右侧则右转，反之左转。
  float frontWeight = map(frontDist, 0, 500, 0, 255); // 映射为权重
  float turnBias = (rightDist - leftDist) / 4.0;      // 左右差值决定转向方向
  
  // 输出：将前方危险权重与方向偏置结合，生成转向修正量
  turnAdjust = constrain(frontWeight * (turnBias / 255.0), -255, 255);
  
  // 3. 输出驱动BLDC电机 (速度模式)
  int baseSpeed = 80; // 基础巡航速度
  
  // 差速转向：左轮减速、右轮加速实现右转，反之左转
  motorLeft.target = constrain(baseSpeed - turnAdjust, -200, 200);
  motorRight.target = constrain(baseSpeed + turnAdjust, -200, 200);
  
  // 4. 更新FOC
  motorLeft.move(motorLeft.target);
  motorRight.move(motorRight.target);
  motorLeft.loopFOC();
  motorRight.loopFOC();

  // 调试输出
  if (millis() % 200 < 10) {
    Serial.print("Front:"); Serial.print(frontDist);
    Serial.print(" | Turn:"); Serial.println(turnAdjust);
  }
  delay(20); // 50Hz控制周期
}

要点：此例展示了模糊逻辑最直观的应用——将连续的传感器数据（而非离散阈值）映射为平滑的转向动作，避免了“近了就急转、远了就猛冲”的机械感。

2、模糊避障 + 目标追踪融合
此案例在避障基础上引入了目标追踪任务。机器人会优先追踪一个目标（如UWB标签或红外信标），同时根据模糊逻辑判断路径上的障碍，在“追目标”和“绕障碍”之间做出柔性的优先级决策。

// ... (BLDC电机及传感器初始化同案例一) ...

// 假设的目标方位数据 (通过UWB或视觉获取)
float targetAngle = 0.0;    // 目标相对于机器人的角度 (-180~180)
float targetDistance = 0.0; // 与目标的距离 (m)

void loop() {
  // 1. 获取传感器和环境数据
  frontDist = analogRead(FRONT_SENSOR);
  leftDist = analogRead(LEFT_SENSOR);
  rightDist = analogRead(RIGHT_SENSOR);
  
  // 获取目标方位 (此处为模拟，实际需调用UWB或视觉库)
  targetAngle = getTargetAngle(); 
  targetDistance = getTargetDistance();

  // 2. 模糊决策：计算“转向”和“速度”指令
  // 输入1: 障碍物威胁度 (由frontDist映射)
  float obstacleThreat = constrain(map(frontDist, 0, 300, 100, 0), 0, 100);
  // 输入2: 目标方向偏差 (0表示正对目标)
  float targetBias = targetAngle / 180.0; 
  
  // 模糊推理: 根据障碍威胁度动态调整目标追踪的权重
  // 威胁度越高，越倾向于避障；威胁度低，则专心追踪目标
  float trackingWeight = 1.0 - (obstacleThreat / 100.0); // 0~1
  float obstacleWeight = 1.0 - trackingWeight;
  
  // 计算转向指令：融合目标方向和避障方向
  float targetTurn = targetBias * 200; // 转向目标
  float obstacleTurn = (rightDist - leftDist) / 4.0; // 避障转向
  
  int turnAdjust = (int)(trackingWeight * targetTurn + obstacleWeight * obstacleTurn);
  turnAdjust = constrain(turnAdjust, -255, 255);

  // 3. 速度控制：接近目标时减速，遇障时减速
  float speedScale = 1.0;
  if (targetDistance < 0.5) speedScale = 0.3; // 接近目标减速
  if (obstacleThreat > 60) speedScale *= 0.5; // 前方有障碍减速
  int baseSpeed = (int)(120 * speedScale);

  // 4. 驱动电机 (同案例一)
  motorLeft.target = constrain(baseSpeed - turnAdjust, -200, 200);
  motorRight.target = constrain(baseSpeed + turnAdjust, -200, 200);
  motorLeft.move(motorLeft.target);
  motorRight.move(motorRight.target);
  motorLeft.loopFOC();
  motorRight.loopFOC();

  delay(20);
}

要点：模糊逻辑的核心优势体现在此——它是一个“软决策”系统，能将“避障”和“追踪”这两个可能冲突的目标，通过权重动态融合成一个平滑的控制指令，而不是生硬地在两种模式间切换。

3、多模式自适应清洁机器人
此案例让机器人能够感知环境类型（开阔区域、窄通道、沿墙等），并自动切换不同的模糊规则集，实现更智能的清扫路径规划。这是更接近商业清洁机器人产品的设计思路。

// ... (BLDC电机及传感器初始化同案例一) ...

// 定义工作模式
enum RobotMode { EXPLORE, WALL_FOLLOW, NARROW_PASSAGE };
RobotMode currentMode = EXPLORE;

// 传感器阈值 (需根据实际调试)
#define WALL_THRESHOLD 150
#define NARROW_THRESHOLD 80 

void loop() {
  // 1. 读取多传感器 (增加侧向和后方传感器会更准确)
  frontDist = analogRead(FRONT_SENSOR);
  leftDist = analogRead(LEFT_SENSOR);
  rightDist = analogRead(RIGHT_SENSOR);

  // 2. 环境识别与模式切换 (状态机)
  if (frontDist < 200 && leftDist > 100 && rightDist > 100) {
    // 前方有障碍，但两侧开阔 → 执行避障探索
    currentMode = EXPLORE; 
  } else if (leftDist < WALL_THRESHOLD || rightDist < WALL_THRESHOLD) {
    // 检测到一侧有墙 → 进入沿墙模式 (沿墙清扫)
    currentMode = WALL_FOLLOW;
  } else if (leftDist < NARROW_THRESHOLD && rightDist < NARROW_THRESHOLD) {
    // 两侧都很近 → 进入窄通道模式 (减速直行)
    currentMode = NARROW_PASSAGE;
  } else {
    // 默认探索模式
    currentMode = EXPLORE;
  }

  // 3. 根据模式执行不同的模糊控制逻辑
  int turnAdjust = 0;
  int baseSpeed = 100;

  switch (currentMode) {
    case EXPLORE:
      // 标准避障逻辑 (同案例一)
      turnAdjust = (rightDist - leftDist) / 4.0;
      turnAdjust = constrain(turnAdjust, -255, 255);
      break;
      
    case WALL_FOLLOW:
      // 沿墙逻辑: 保持与墙的固定距离
      float wallError = (leftDist < rightDist) ? leftDist - 150 : rightDist - 150;
      turnAdjust = wallError * 1.5; // 误差越大，转向修正越大
      turnAdjust = constrain(turnAdjust, -100, 100);
      baseSpeed = 80; // 沿墙速度稍慢
      break;
      
    case NARROW_PASSAGE:
      // 窄通道逻辑: 尽量居中直行
      float centerError = leftDist - rightDist;
      turnAdjust = centerError * 2.0;
      turnAdjust = constrain(turnAdjust, -80, 80);
      baseSpeed = 60; // 狭窄空间慢速通过
      break;
  }

  // 4. 驱动电机
  motorLeft.target = constrain(baseSpeed - turnAdjust, -200, 200);
  motorRight.target = constrain(baseSpeed + turnAdjust, -200, 200);
  motorLeft.move(motorLeft.target);
  motorRight.move(motorRight.target);
  motorLeft.loopFOC();
  motorRight.loopFOC();

  delay(20);
}

要点：通过增加“环境感知”维度，模糊逻辑的价值从“底层控制”上升到“决策层”。机器人不再盲目避障，而是会根据环境类型（如沿墙、过窄门）采取最合理的策略，极大提升了任务效率。

要点解读
模糊逻辑是处理“不确定性”的利器：在真实室内环境中，传感器数据充满噪声。模糊逻辑允许你用“有点近”、“比较远”这类模糊概念来描述世界，而非必须用精确的数值（如“距离<15cm”），这使得系统对传感器误差有很高的容忍度。

“模糊化、规则库、解模糊”是核心三步：实现模糊控制系统，首先要把传感器数据转换为模糊语言变量（如“近/中/远”）；然后通过预设的“如果……那么……”规则进行推理（如“如果前方近且左侧远，则左转”）；最后将推理出的模糊结论（如“左转力度中等”）转化为电机可执行的精确数值（如PWM值）。你可以使用如Fuzzy库来简化这一过程。

BLDC+模糊控制构成“快决策+高精度执行”组合：模糊逻辑擅长快速决策，但其输出只是一个控制信号。BLDC电机配合FOC算法，则能高频、精准地将这个信号转化为平滑的力矩和转速，实现快速的响应和无顿挫的运动。

规则库设计是“灵魂”，需要根据场景“喂养”：模糊控制器的性能高度依赖其规则库。一个优秀的规则库是“经验”的数字化。对于清洁机器人，你不仅要设计“避障规则”，更需设计“沿墙清扫规则”、“脱困规则”等。这需要在实际场景中反复测试和调优。

警惕Arduino平台的算力瓶颈：模糊推理涉及多次浮点运算和查表。在资源受限的Arduino Uno上，复杂的规则库可能导致控制频率下降，影响实时性。优化方案包括：简化隶属度函数（多用三角形、梯形）、使用查表法替代实时推理，或升级至ESP32、Teensy等性能更强的平台。

4、桌面型清洁机器人——模糊逻辑避障+灰尘热源双重追踪
适用场景：办公桌面、小型操作台等狭窄空间，机器人需在清理灰尘的同时，自主追踪热源（如人体余温区域、咖啡杯余热）或高密度灰尘区域，核心解决“狭窄空间避障与精准目标追踪”的矛盾。

核心逻辑：采用模糊逻辑整合红外近距避障与灰尘传感器信号，以“距离误差+角度误差”为模糊输入，输出BLDC差速控制参数；同时融合热释电传感器追踪热源，通过模糊规则实现“避障优先级>追踪优先级”的动态平衡，在不碰撞的前提下逼近目标。

/* ===== 桌面清洁机器人：模糊避障 + 灰尘热源追踪 =====
 * 硬件：Arduino ESP32 + BLDC双电机 + 红外避障阵列 + 灰尘传感器 + 热释电传感器
 * 核心：模糊逻辑整合避障/追踪信号 + 差速驱动控制
 */
#include <SimpleFOC.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);       // 左右BLDC电机（控制轮速）
BLDCDriver3PWM driverL(9,10,11,8);    // 左驱动
BLDCDriver3PWM driverR(3,5,6,7);      // 右驱动
Encoder encoderL(18,19,2048), encoderR(20,21,2048); // 编码器测速

// 传感器接口
const int irPin[5] = {A0,A1,A2,A3,A4}; // 5路红外避障（前方+两侧）
const int dustPin = A5;                 // 灰尘传感器（输出电压与浓度正相关）
const int pirPin = 2;                   // 热释电传感器（检测热源）

// ==================== 模糊逻辑参数 ====================
// 输入变量：避障距离（近/中/远）、追踪偏差角（左/正/右）
// 输出变量：左轮速（慢/中/快）、右轮速（慢/中/快）
// 模糊规则：近障碍且偏差大→减速转向；远障碍且偏差小→全速追踪
const float MAX_SPEED = 1.2;           // 电机最大转速（rad/s）
const int IR_THRESHOLD = 300;          // 红外避障阈值（距离阈值，根据硬件标定）
const float DUST_THRESHOLD = 2.0;      // 灰尘浓度阈值（电压值）

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    pinMode(pirPin, INPUT);
}

// ==================== 传感器数据采集 ====================
// 获取避障距离状态（0=无障碍，1=侧方弱障碍，2=前方强障碍）
int getObstacleState() {
    int frontDist = analogRead(irPin[2]);
    int leftDist = analogRead(irPin[3]);
    int rightDist = analogRead(irPin[4]);
    if (frontDist < IR_THRESHOLD * 0.7) return 2; // 前方近障碍
    if (leftDist < IR_THRESHOLD || rightDist < IR_THRESHOLD) return 1; // 侧方障碍
    return 0; // 无障碍
}

// 获取追踪偏差角状态（0=正前方，1=左偏，2=右偏）
int getTrackingAngle() {
    int leftDist = analogRead(irPin[3]);
    int rightDist = analogRead(irPin[4]);
    int leftDust = analogRead(dustPin); // 左右灰尘浓度差间接反映角度偏差
    int rightDust = analogRead(irPin[0]);
    if (leftDust - rightDust > 100) return 1; // 左偏
    if (rightDust - leftDust > 100) return 2; // 右偏
    return 0; // 正前方
}

// ==================== 模糊逻辑推理核心 ====================
void fuzzyControl(int obsState, int angleState, bool isTarget, float &leftSpd, float &rightSpd) {
    // 模糊规则库推理（简化为查表法，适配Arduino算力）
    if (obsState == 2) { // 前方强障碍
        leftSpd = 0.3; rightSpd = 0.3; // 原地等待（需结合障碍方向调整）
        return;
    }
    if (obsState == 1) { // 侧方障碍
        if (angleState == 1) { // 目标左偏+左侧障碍
            leftSpd = 0.5; rightSpd = 0.8; // 右转规避
        } else if (angleState == 2) { // 目标右偏+右侧障碍
            leftSpd = 0.8; rightSpd = 0.5; // 左转规避
        } else {
            leftSpd = 0.6; rightSpd = 0.6; // 直行试探
        }
        return;
    }
    // 无障碍：优先追踪目标
    if (isTarget) {
        if (angleState == 1) { // 目标左偏
            leftSpd = MAX_SPEED * 0.3; rightSpd = MAX_SPEED * 0.9; // 左转追踪
        } else if (angleState == 2) { // 目标右偏
            leftSpd = MAX_SPEED * 0.9; rightSpd = MAX_SPEED * 0.3; // 右转追踪
        } else {
            leftSpd = MAX_SPEED; rightSpd = MAX_SPEED; // 全速直行追踪
        }
    } else { // 无目标：随机探索
        leftSpd = MAX_SPEED * 0.7; rightSpd = MAX_SPEED * 0.8; // 轻微偏转行走
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 传感器数据读取
    int obsState = getObstacleState();
    int angleState = getTrackingAngle();
    bool isTarget = (analogRead(dustPin) > DUST_THRESHOLD * 10) || digitalRead(pirPin) == HIGH; // 灰尘/热源触发追踪
    
    // 2. 模糊控制输出
    float leftSpd, rightSpd;
    fuzzyControl(obsState, angleState, isTarget, leftSpd, rightSpd);
    
    // 3. 电机执行
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 4. 状态调试输出
    Serial.print("Obs:");Serial.print(obsState);
    Serial.print(" Angle:");Serial.print(angleState);
    Serial.print(" Target:");Serial.print(isTarget);
    Serial.print(" L:");Serial.print(leftSpd);
    Serial.print(" R:");Serial.println(rightSpd);
    
    delay(50); // 稳定控制周期
}

5、地面自主清洁机器人——模糊逻辑动态避障+目标气味追踪
适用场景：地面清洁场景，需追踪厨房油污气味、宠物异味等挥发性目标，同时应对桌椅腿、地毯边缘等不规则障碍物，核心解决“动态避障与气味源追踪”的耦合问题。

核心逻辑：采用模糊逻辑融合超声波（远距避障）与气味传感器信号，输入为“避障距离误差”“气味浓度梯度”，输出为BLDC差速控制量；通过气味浓度变化调整模糊规则权重，浓度升高时优先追踪，浓度降低时扩大避障范围，实现避障与追踪的动态平衡。

/* ===== 地面清洁机器人：模糊动态避障 + 气味追踪 =====
 * 硬件：Arduino Mega + BLDC双电机 + 超声波阵列 + 气味传感器（MQ-135）+ 风机
 * 核心：模糊逻辑整合避障/气味信号 + 浓度梯度追踪
 */
#include <SimpleFOC.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// 传感器接口
const int sonarPin[3] = {2,3,4};       // 3路超声波（前/左前/右前）
const int smellPin = A0;               // 气味传感器
float lastSmellVal = 0;                // 上一周期气味浓度
float smellGradient = 0;                // 气味浓度梯度（变化率）

// ==================== 模糊逻辑参数 ====================
const float MAX_SPEED = 1.0;            // 最大转速
const int SONAR_SAFE_DIST = 30;         // 超声波安全距离（cm）
const float SMELL_BASE = 200;           // 气味基础阈值

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 超声波引脚初始化
    for(int i=0;i<3;i++) pinMode(sonarPin[i], OUTPUT);
}

// ==================== 超声波距离获取 ====================
int getSonarDist(int pin) {
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW); delayMicroseconds(2);
    digitalWrite(pin, HIGH); delayMicroseconds(10);
    digitalWrite(pin, LOW);
    pinMode(pin, INPUT);
    return pulseIn(pin, HIGH) / 58; // 转换为cm（声速340m/s）
}

// ==================== 传感器数据融合 ====================
void getSensorData(float &distError, int &sideState, float &smellVal) {
    int frontDist = getSonarDist(sonarPin[0]);
    int leftDist = getSonarDist(sonarPin[1]);
    int rightDist = getSonarDist(sonarPin[2]);
    
    distError = SONAR_SAFE_DIST - frontDist; // 避障距离误差（正为远，负为近）
    // 侧方状态：0=均远，1=左近，2=右近
    if (leftDist < SONAR_SAFE_DIST && leftDist < rightDist) sideState = 1;
    else if (rightDist < SONAR_SAFE_DIST && rightDist < leftDist) sideState = 2;
    else sideState = 0;
    
    // 气味浓度与梯度
    smellVal = analogRead(smellPin);
    smellGradient = smellVal - lastSmellVal;
    lastSmellVal = smellVal;
}

// ==================== 模糊控制核心（浓度加权避障） ====================
void fuzzyControl(float distError, int sideState, float smellVal, float &leftSpd, float &rightSpd) {
    // 气味浓度权重：浓度越高，追踪优先级越高，避障容错越大
    float trackWeight = map(smellVal, SMELL_BASE, 800, 0.3, 1.0);
    trackWeight = constrain(trackWeight, 0.3, 1.0);
    
    // 避障规则：误差小→减速，侧方障碍→转向
    if (distError < 0) { // 近障碍
        if (sideState == 1) { // 左近障碍
            leftSpd = (0.2 * (1-trackWeight)) + (0.5 * trackWeight); // 权重平衡
            rightSpd = (0.8 * (1-trackWeight)) + (0.8 * trackWeight);
        } else if (sideState == 2) { // 右近障碍
            leftSpd = (0.8 * (1-trackWeight)) + (0.8 * trackWeight);
            rightSpd = (0.2 * (1-trackWeight)) + (0.5 * trackWeight);
        } else { // 正前方近障碍
            leftSpd = 0.3 * (1-trackWeight);
            rightSpd = 0.3 * (1-trackWeight);
        }
    } else { // 安全距离
        // 追踪规则：浓度梯度正→向气味源转向，梯度负→扩大搜索
        if (smellGradient > 0) { // 浓度上升（向源靠近）
            if (sideState == 1) { // 左近且浓度上升→减速左
                leftSpd = 0.7 * trackWeight;
                rightSpd = 1.0 * trackWeight;
            } else if (sideState == 2) {
                leftSpd = 1.0 * trackWeight;
                rightSpd = 0.7 * trackWeight;
            } else {
                leftSpd = 1.0 * trackWeight;
                rightSpd = 1.0 * trackWeight;
            }
        } else if (smellGradient < -50) { // 浓度骤降→随机转向搜索
            leftSpd = 0.3 * (1-trackWeight);
            rightSpd = 0.8 * (1-trackWeight);
            delay(200); // 转向延时
        } else { // 浓度稳定→缓慢前进
            leftSpd = 0.6; rightSpd = 0.6;
        }
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 传感器数据融合
    float distError; int sideState; float smellVal;
    getSensorData(distError, sideState, smellVal);
    
    // 2. 模糊控制计算
    float leftSpd, rightSpd;
    fuzzyControl(distError, sideState, smellVal, leftSpd, rightSpd);
    
    // 3. 电机执行
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    
    // 4. 调试输出
    Serial.print("DistErr:");Serial.print(distError);
    Serial.print(" Side:");Serial.print(sideState);
    Serial.print(" Smell:");Serial.print(smellVal);
    Serial.print(" Grad:");Serial.print(smellGradient);
    Serial.print(" L:");Serial.print(leftSpd);
    Serial.print(" R:");Serial.println(rightSpd);
    
    delay(80); // 控制周期适配传感器响应
}

6、高空幕墙清洁机器人——模糊逻辑边缘避障+目标轨迹追踪
适用场景：高层建筑幕墙、玻璃幕墙清洁，机器人需贴合墙面移动，避开边缘与突出障碍物，同时追踪预设清洁轨迹（如从顶部到底部的直线路径），核心解决“高空边缘避障+精准轨迹追踪”的平衡问题。

核心逻辑：采用模糊逻辑融合墙面传感器（边缘检测）与视觉轨迹传感器信号，输入为“墙面距离偏差”“轨迹角度偏差”，输出为BLDC电机速度与吸附装置控制信号；通过模糊规则实现“贴合墙面+跟随轨迹”，边缘传感器触发时优先避障，轨迹偏差大时调整转向，确保清洁效率与高空安全。

/* ===== 幕墙清洁机器人：模糊边缘避障 + 轨迹追踪 =====
 * 硬件：Arduino Due + BLDC双电机 + 激光墙面传感器 + 视觉轨迹传感器 + 电磁吸附模块
 * 核心：模糊逻辑整合边缘/轨迹信号 + 吸附-避障联动
 */
#include <SimpleFOC.h>

// ==================== 硬件驱动初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// 传感器与执行器接口
const int wallSensor = A0;              // 墙面距离传感器（激光）
const int trackSensor = A1;              // 视觉轨迹传感器（角度输出）
const int adsorbPin = 2;                // 电磁吸附控制

// ==================== 模糊逻辑参数 ====================
const float MAX_SPEED = 0.8;            // 高空安全转速（避免晃动）
const int WALL_SAFE_DIST = 5;           // 墙面安全距离（cm）
const float TRACK_ANGLE_TOL = 15;       // 轨迹角度容差（度）

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    pinMode(adsorbPin, OUTPUT);
    digitalWrite(adsorbPin, HIGH); // 初始吸附
}

// ==================== 传感器数据解析 ====================
void getSensorData(float &wallDist, float &trackAngle) {
    wallDist = analogRead(wallSensor) * 0.1; // 转换为cm（需硬件标定）
    trackAngle = map(analogRead(trackSensor), 0, 1023, -45, 45); // 轨迹角度
}

// ==================== 模糊控制核心（边缘-轨迹联动） ====================
void fuzzyControl(float wallDist, float trackAngle, float &leftSpd, float &rightSpd, bool &adsorb) {
    // 墙面距离误差（正为远离，负为靠近）
    float wallErr = wallDist - WALL_SAFE_DIST;
    // 轨迹角度误差（正为右偏，负为左偏）
    float angleErr = trackAngle;
    
    // 边缘避障优先级最高
    if (wallDist < 3) { // 近边缘（危险）
        adsorb = true;
        // 远离边缘转向
        if (wallErr < 0) { // 贴墙过近
            if (angleErr > 0) { // 轨迹右偏→左转避墙
                leftSpd = 0.2; rightSpd = 0.6;
            } else { // 轨迹左偏→右转避墙
                leftSpd = 0.6; rightSpd = 0.2;
            }
        }
        return;
    }
    
    // 轨迹追踪：角度偏差调整转向
    if (abs(angleErr) > TRACK_ANGLE_TOL) {
        if (angleErr > 0) { // 轨迹右偏→右转修正
            leftSpd = 0.7; rightSpd = 0.5;
        } else { // 轨迹左偏→左转修正
            leftSpd = 0.5; rightSpd = 0.7;
        }
    } else { // 轨迹对齐→直行
        leftSpd = MAX_SPEED; rightSpd = MAX_SPEED;
    }
    
    // 墙面距离控制：过远时减速靠近，过近时轻微远离
    if (wallErr > 3) { // 远离墙面→减速靠近
        leftSpd *= 0.8; rightSpd *= 0.8;
    } else if (wallErr < -2) { // 过近墙面→轻微远离
        if (angleErr > 0) {
            leftSpd = 0.6; rightSpd = 0.4;
        } else {
            leftSpd = 0.4; rightSpd = 0.6;
        }
    }
    adsorb = (wallDist > 2); // 非边缘时保持吸附
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 传感器数据获取
    float wallDist, trackAngle;
    getSensorData(wallDist, trackAngle);
    
    // 2. 模糊控制计算
    float leftSpd, rightSpd;
    bool adsorbState;
    fuzzyControl(wallDist, trackAngle, leftSpd, rightSpd, adsorbState);
    
    // 3. 执行器动作
    motorL.move(leftSpd);
    motorR.move(rightSpd);
    digitalWrite(adsorbPin, adsorbState ? HIGH : LOW);
    
    // 4. 调试输出
    Serial.print("WallDist:");Serial.print(wallDist);
    Serial.print(" TrackAng:");Serial.print(trackAngle);
    Serial.print(" Adsorb:");Serial.print(adsorbState);
    Serial.print(" L:");Serial.print(leftSpd);
    Serial.print(" R:");Serial.println(rightSpd);
    
    delay(100); // 高空稳定控制周期
}

要点解读

1. 模糊逻辑的“规则-输入-输出”适配：解决场景模糊性的核心
   模糊逻辑的核心价值是用“人类经验式规则”替代精准数学建模，适配室内清洁的模糊环境（障碍物大小、距离无严格边界，目标强度模糊），关键在于：
   输入变量设计：需紧扣场景核心矛盾，如案例4的“避障距离+追踪偏差角”、案例5的“避障误差+气味梯度”，避免变量冗余导致算力不足；
   规则库构建：以“安全优先+任务优先”为核心，例如案例1中“近障碍>追踪目标”，案例3中“边缘避障>轨迹跟随”，确保决策符合场景安全逻辑；
   输出量化简化：针对Arduino算力限制，采用“查表法”“权重映射”替代复杂的模糊推理和解模糊，用map、constrain等函数快速输出电机速度，平衡算法复杂度与控制实时性。
2. 避障与追踪的优先级动态平衡：应对多目标冲突的关键
   室内清洁机器人的核心冲突是“避障安全”与“追踪任务”的矛盾，单一优先级会导致效率或安全隐患，核心是动态优先级调整：
   安全兜底原则：任何情况下，避障优先级需高于追踪，如案例4-6中，触发近障碍、边缘检测时，强制降低电机速度或转向，确保安全；
   任务加权平衡：无安全威胁时，根据任务紧急度调整追踪权重，如案例2中“气味浓度越高，追踪权重越大，避障容错越大”，避免因避障过度保守导致追踪失效；
   平滑切换逻辑：优先级切换需避免突兀，如案例6中“边缘避障→轨迹追踪”切换时，速度逐步提升，避免电机急加速导致高空晃动，保障控制平滑性。
3. 传感器多源融合：弥补单一传感器的局限性
   单一传感器无法满足“避障+追踪”的双重需求，核心是多源传感器互补融合，通过数据互补解决环境感知盲区：
   避障互补：案例4用红外阵列覆盖近距盲区，案例5用超声波覆盖远距，案例3用激光覆盖墙面距离，弥补单一传感器的量程局限；
   目标互补：案例4用灰尘+热源双目标覆盖不同清洁场景，案例2用气味传感器追踪挥发性目标，案例3用视觉传感器追踪轨迹，覆盖不同类型的清洁目标；
   信号融合：将不同传感器的信号通过模糊规则统一整合，而非简单“与/或”逻辑，例如案例2中“超声波避障信号”与“气味浓度信号”通过权重系数融合，实现更智能的决策。
4. BLDC控制与模糊输出的精准匹配：保障执行可靠性
   模糊逻辑输出的是抽象的“控制意图”，需精准匹配BLDC电机的执行特性，才能避免“算法理想化，执行失效”，核心是：
   控制模式适配：清洁机器人多采用差速驱动，需将模糊输出的线速度、角速度，通过差速公式转换为左右电机转速，适配底盘运动学，如案例中leftSpd = 线速度 - 角速度*轮距/2的解算；
   执行约束匹配：对模糊输出的速度进行电机物理约束（如最大转速、最小转速），避免模糊规则输出超出电机性能导致堵转或失控，如案例中用constrain限制速度范围；
   FOC控制闭环：采用SimpleFOC的磁场定向控制，对BLDC电机进行速度闭环控制，抵消负载波动（如爬坡、避障时的阻力）对电机转速的影响，保证模糊输出的意图能精准执行。
5. 算力适配与代码优化：保障Arduino平台的落地可行性
   模糊逻辑推理和多传感器处理对算力有要求，而Arduino平台算力有限，核心是算力优化与落地适配：
   算法简化降维：将复杂的模糊推理（如Mamdani模糊系统）简化为查表法、权重映射法，避免浮点运算和复杂循环，如案例1的规则直接转为if-else语句；
   采样周期适配：根据传感器响应速度和电机控制周期，合理设置控制循环延迟（如50-100ms），既保证系统响应，又避免频繁采样导致算力溢出；
   资源复用与预处理：将传感器标定数据（如超声波转换系数、气味阈值）预处理为常量，避免运行时重复计算；对传感器数据进行简单滤波（如滑动平均），减少噪声对模糊推理的影响，同时控制滤波复杂度。

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