Arduino BLDC 模糊逻辑避障控制机器人实战

2. 五传感器模糊巡墙

使用五个红外测距传感器，实现更精准的沿墙模糊控制。

// 传感器引脚 (模拟输入)
const int irLeftFar = A0, irLeftNear = A1;
const int irFront = A2;
const int irRightNear = A3, irRightFar = A4;

// 电机引脚
const int pwmLeft = 9, pwmRight = 10;

void setup() {
  pinMode(pwmLeft, OUTPUT); pinMode(pwmRight, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  int valLF = analogRead(irLeftFar);
  int valLN = analogRead(irLeftNear);
  int valF = analogRead(irFront);
  int valRN = analogRead(irRightNear);
  int valRF = analogRead(irRightFar);

  // 模糊化处理：计算误差
  int error = 0;
  if (valLN > 600) error = -2;      // 偏左严重
  else if (valLN > 400) error = -1; // 偏左一点
  else if (valRN > 600) error = 2;  // 偏右严重
  else if (valRN > 400) error = 1;  // 偏右一点
  else if (valF > 500) error = 10;  // 前方障碍
  else error = 0;                   // 居中

  // 模糊输出：计算转向修正量
  int correction = 0;
  switch (error) {
    case -2: correction = -120; break; // 急右转
    case -1: correction = -60; break;  // 缓右转
    case 0: correction = 0; break;     // 直行
    case 1: correction = 60; break;    // 缓左转
    case 2: correction = 120; break;   // 急左转
    case 10: correction = 180; break;  // 遇到前方障碍，左大转
  }

  int baseSpeed = 150;
  int leftSpeed = constrain(baseSpeed + correction, 0, 255);
  int rightSpeed = constrain(baseSpeed - correction, 0, 255);

  analogWrite(pwmLeft, leftSpeed);
  analogWrite(pwmRight, rightSpeed);

  Serial.print("Error: "); Serial.println(error);
  delay(50);
}

3. 带输出平滑滤波的模糊控制

在模糊逻辑输出后加入低通滤波，消除传感器噪声导致的电机高频抖动。

float alpha = 0.3; // 滤波系数
int filteredBias = 0;
int lastBias = 0;

void loop() {
  int distL = getDistance(trigL, echoL);
  int distF = getDistance(trigF, echoF);
  int distR = getDistance(trigR, echoR);

  // 模糊逻辑计算 rawBias
  int rawBias = 0;
  if (distF < 20) {
    rawBias = (distL > distR) ? -150 : 150;
  } else if (distL < 15) {
    rawBias = 80;
  } else if (distR < 15) {
    rawBias = -80;
  }

  // 一阶低通滤波：filtered = alpha * new + (1-alpha) * old
  filteredBias = alpha * rawBias + (1 - alpha) * lastBias;
  lastBias = filteredBias;

  // 只有当偏差变化足够大时才更新电机
  if (abs(filteredBias - lastBias) > 5) {
    int baseSpeed = 180;
    int leftSpeed = constrain(baseSpeed + filteredBias, 0, 255);
    int rightSpeed = constrain(baseSpeed - filteredBias, 0, 255);
    analogWrite(pwmLeft, leftSpeed);
    analogWrite(pwmRight, rightSpeed);
  }
  delay(50);
}

4. 进阶：SimpleFOC 与多传感器融合

对于更高性能需求，可结合 SimpleFOC 库和多种传感器进行融合决策。

#include <SimpleFOC.h>
#include <NewPing.h>

#define TRIGGER_PIN 12
#define ECHO_PIN 11
#define MAX_DISTANCE 200
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 6, 7);

void fuzzyControl(float distance) {
  float leftSpeed, rightSpeed;
  if (distance < 10) {       // 极近
    leftSpeed = -0.5; rightSpeed = -0.5; // 后退
  } else if (distance < 30) { // 近
    leftSpeed = 0.3; rightSpeed = -0.3;  // 右转
  } else if (distance < 50) { // 中等
    leftSpeed = 0.5; rightSpeed = 0.5;   // 前进
  } else {                    // 远
    leftSpeed = 0.8; rightSpeed = 0.8;   // 快速前进
  }
  motor.move(leftSpeed); // 简化示例，实际需双轮控制
}

void setup() {
  Serial.begin(115200);
  driver.init();
  motor.linkDriver(&driver);
  motor.controller = MotionControlType::velocity;
  motor.init();
  motor.initFOC();
}

void loop() {
  float distance = sonar.ping_cm();
  if (distance == 0) distance = MAX_DISTANCE;
  fuzzyControl(distance);
  delay(100);
}

调试与优化建议

• 模糊逻辑 vs 传统阈值：传统控制使用硬阈值（如 if (dist < 20)），动作生硬易振荡；模糊控制使用隶属度函数，输出平滑自然。
• 规则数量：通常 3-5 个输入变量，每个变量 3 个模糊集，规则数在 10-20 条左右即可覆盖大多数场景，并非越多越好。
• 去模糊化方法：重心法最常用，输出平滑；最大值平均法计算简单但可能跳变。资源受限平台可用查表法或简化规则直接输出数值。
• 调试技巧：利用 Arduino IDE 串口绘图器观察输入变量和模糊输出曲线，确保规则触发合理，无明显抖动。

硬件安全提示

以上案例旨在拓展思路，仅供参考。实际编程时，请务必根据自己的硬件配置、使用场景和具体需求进行调整，并进行多次实地测试。涉及硬件操作的代码，使用前请确认引脚和电平的参数正确性与安全性。务必加装急停保护机制，防止软件跑飞导致设备损坏或人身伤害。