Arduino BLDC 自主巡逻机器人：避障与路径规划

传感器局限性与环境适应性

• 极端环境影响：超声波传感器在强风或高温环境下测距不准；红外传感器受强光干扰严重；激光雷达在浓雾或灰尘环境中性能下降。必须通过软件滤波（如卡尔曼滤波）和多传感器数据融合来提高系统的鲁棒性。
• 盲区处理：传感器存在探测盲区（如地面附近或紧贴机身的区域）。在算法中应设置合理的安全距离裕度，并在机械设计上确保底盘离地高度能越过常见障碍物。

电源管理与电磁兼容（EMC）

• 电源隔离：BLDC 电机启动瞬间电流巨大，容易导致 Arduino 供电电压跌落而复位。必须使用独立的电源模块为电机和控制板供电，并在电源入口处并联大容量电解电容（如 1000μF）以吸收电流尖峰。
• 信号抗干扰：电机驱动线是主要的噪声源。信号线（如传感器数据线、编码器线）应远离电机动力线走线，必要时加装磁环或使用屏蔽线，防止电磁干扰导致传感器数据跳变或程序跑飞。

安全机制与异常处理

• 紧急制动：必须设计硬件级别的急停电路（如物理碰撞开关直连 ESC 的刹车信号），当软件失效或发生剧烈碰撞时，能立即切断电机动力。
• 低电量管理：算法需实时监测电池电量。当电量低于阈值时，应立即中断当前巡逻任务，规划一条最短路径返回充电站进行自动充电，确保任务的连续性。

4. 代码案例

4.1 基础避障巡逻（反应式控制）

场景：室内简单巡逻，遇到障碍物随机转向。 核心逻辑：超声波/红外测距 + 随机转向决策。

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

#define TRIG_PIN 9
#define ECHO_PIN 10
#define MAX_DISTANCE 200

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
BLDCMotor motorL = BLDCMotor(11); // 左电机
BLDCMotor motorR = BLDCMotor(11); // 右电机

void setup() {
  Serial.begin(115200);
  // BLDC 电机初始化代码（略）
  motorL.init();
  motorR.init();
}

void loop() {
  int distance = sonar.ping_cm(); // 获取前方距离
  if (distance > 0 && distance < 30) { // 30cm 内有障碍
    // 1. 停止
    motorL.move(0);
    motorR.move(0);
    delay(500);
    // 2. 随机转向（左转或右转）
    if (random(2) == 0) {
      // 左转：左轮反转，右轮正转
      motorL.move(-0.5);
      motorR.move(0.5);
    } else {
      // 右转：左轮正转，右轮反转
      motorL.move(0.5);
      motorR.move(-0.5);
    }
    delay(1000); // 转向 1 秒
  } else {
    // 无障碍，直行
    motorL.move(0.3);
    motorR.move(0.3);
  }
}

4.2 A*算法路径规划（静态地图）

场景：已知地图环境，从起点到终点的最优路径导航。 核心逻辑：A*算法计算路径 + 编码器定位跟踪。

// 假设已实现 A*算法类 AStarPlanner
AStarPlanner planner;
BLDCMotor motorL, motorR;
Encoder encoderL(2, 3), encoderR(18, 19); // 编码器用于定位
float currentX = 0, currentY = 0; // 当前位置
float targetX = 5.0, targetY = 5.0; // 目标位置
std::vector<Point> path; // 存储路径点

void setup() {
  // 初始化电机、编码器
  // ...
  // 规划路径
  path = planner.findPath(Point(0, 0), Point(5, 5));
}

void loop() {
  // 1. 更新当前位置（通过编码器里程计）
  updatePose();
  // 2. 获取当前目标点（路径中的下一个点）
  Point target = path.front();
  // 3. 计算角度偏差和距离
  float angleError = atan2(target.y - currentY, target.x - currentX);
  float distance = sqrt(pow(target.x - currentX, 2) + pow(target.y - currentY, 2));
  // 4. 控制逻辑：转向对准 + 直行
  if (abs(angleError) > 0.1) {
    // 角度偏差大，先转向
    // 差速转向：左轮速度 = 基础速度 - 修正量，右轮速度 = 基础速度 + 修正量
    motorL.move(0.1 - angleError * 0.5);
    motorR.move(0.1 + angleError * 0.5);
  } else if (distance > 0.1) {
    // 角度准了，直行
    motorL.move(0.3);
    motorR.move(0.3);
  } else {
    // 到达目标点，移除该点，准备去下一个点
    path.erase(path.begin());
  }
}

4.3 动态避障与局部重规划

场景：巡逻过程中遇到动态障碍物（如行人），实时绕行。 核心逻辑：动态窗口法（DWA）或向量场直方图（VFH）进行局部避障。

#include <RPLidar.h> // 激光雷达库，用于 360 度环境感知
RPLidar lidar;
BLDCMotor motorL, motorR;

void setup() {
  lidar.begin(Serial1); // 雷达连接串口 1
  // 电机初始化...
}

void loop() {
  // 1. 获取激光雷达数据（360 度点云）
  if (lidar.waitPoint()) {
    float angle = lidar.getCurrentPoint().angle;
    float distance = lidar.getCurrentPoint().distance;
    // 2. 动态避障决策（简化版：寻找最大空隙）
    float bestAngle = findBestGap(angle, distance);
    // 3. 控制电机转向该角度
    turnToAngle(bestAngle);
    // 4. 安全距离内前进
    if (getMinDistance() > 20) {
      moveForward(0.2);
    } else {
      stopMotors();
    }
  }
}

float findBestGap(float angle, float distance) {
  // 简化逻辑：寻找前方扇形区域中距离最远的方向
  // 实际应用中可使用 DWA 算法计算最优速度和角度
  return 0.0; // 返回最佳角度
}

4.4 基础避障巡逻（超声波 + 随机转向）

应用场景：室内简单环境下的随机路径避障巡逻。

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

// 电机与传感器配置
BLDCMotor motorL(7), motorR(7); // 左右电机（假设极对数相同）
BLDCDriver3PWM driverL(3, 5, 6, 11), driverR(9, 10, 12, 11);
Encoder encoderL(2, 4), encoderR(7, 8);
NewPing sonar(A0, A1, 200); // 超声波 Trigger(A0), Echo(A1), 最大距离 200cm

// 运动控制变量
float targetSpeed = 2.0; // 默认速度 (rad/s)
unsigned long lastTurnTime = 0;
bool isTurning = false;

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

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 超声波避障检测（每 100ms 一次）
  static unsigned long lastPing = 0;
  if (millis() - lastPing >= 100) {
    int distance = sonar.ping_cm();
    lastPing = millis();
    if (distance > 0 && distance < 30) {
      // 检测到障碍物
      isTurning = true;
      lastTurnTime = millis();
      // 随机选择左转或右转
      if (random(2) == 0) {
        motorL.target = -3.0;
        motorR.target = 3.0; // 原地右转
      } else {
        motorL.target = 3.0;
        motorR.target = -3.0; // 原地左转
      }
    }
  }
  
  // 避障完成后恢复直行
  if (isTurning && millis() - lastTurnTime >= 800) {
    isTurning = false;
    motorL.target = motorR.target = targetSpeed;
  }
  
  // 正常巡逻
  if (!isTurning) {
    motorL.target = motorR.target = targetSpeed;
  }
}

4.5 红外边界 + 超声波避障（有限区域巡逻）

应用场景：限定区域内的边界巡逻（如仓库货架间）。

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

// 硬件配置
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(3, 5, 6, 11), driverR(9, 10, 12, 11);
Encoder encoderL(2, 4), encoderR(7, 8);
NewPing sonar(A0, A1, 200);
#define IR_LEFT A2 // 左红外边界传感器
#define IR_RIGHT A3 // 右红外边界传感器

// 路径规划状态
enum State {
  FORWARD,
  TURN_LEFT,
  TURN_RIGHT,
  REVERSE
};
State currentState = FORWARD;
unsigned long stateChangeTime = 0;

void setup() {
  Serial.begin(115200);
  // 初始化电机（同案例 1）
  pinMode(IR_LEFT, INPUT);
  pinMode(IR_RIGHT, INPUT);
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 边界检测（红外信号为 LOW 表示触边）
  bool leftEdge = (digitalRead(IR_LEFT) == LOW);
  bool rightEdge = (digitalRead(IR_RIGHT) == LOW);
  
  // 超声波避障
  int obstacleDist = sonar.ping_cm();
  bool obstacleDetected = (obstacleDist > 0 && obstacleDist < 30);
  
  // 状态机切换逻辑
  switch (currentState) {
    case FORWARD:
      motorL.target = motorR.target = 2.0;
      if (obstacleDetected) {
        currentState = (random(2) ? TURN_LEFT : TURN_RIGHT);
        stateChangeTime = millis();
      } else if (leftEdge || rightEdge) {
        currentState = REVERSE;
        stateChangeTime = millis();
      }
      break;
    case TURN_LEFT:
      motorL.target = -3.0;
      motorR.target = 3.0;
      if (millis() - stateChangeTime >= 800) currentState = FORWARD;
      break;
    case TURN_RIGHT:
      motorL.target = 3.0;
      motorR.target = -3.0;
      if (millis() - stateChangeTime >= 800) currentState = FORWARD;
      break;
    case REVERSE:
      motorL.target = motorR.target = -1.5;
      if (millis() - stateChangeTime >= 1000) {
        currentState = (leftEdge ? TURN_RIGHT : TURN_LEFT);
        stateChangeTime = millis();
      }
      break;
  }
}

4.6 SLAM 导航模拟（简化版）

应用场景：基于预设路径点的巡逻（需配合树莓派或更高级处理器实现完整 SLAM）。

#include <SimpleFOC.h>
#include <QueueArray.h>

// 硬件配置（同案例 1）
BLDCMotor motorL(7), motorR(7);
Encoder encoderL(2, 4), encoderR(7, 8);

// 路径点结构体
struct Waypoint {
  float x;
  float y;
};
QueueArray<Waypoint> pathQueue;
Waypoint currentTarget = {0, 0};
bool isMoving = false;

void setup() {
  Serial.begin(115200);
  // 初始化电机
  motorL.init();
  motorR.init();
  // 预设路径点（单位：米）
  pathQueue.push({1.0, 0.0}); // 点 1
  pathQueue.push({1.0, 1.0}); // 点 2
  pathQueue.push({0.0, 1.0}); // 点 3
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 模拟获取当前位置（实际需 IMU/编码器里程计）
  static float robotX = 0, robotY = 0;
  
  // 路径点导航逻辑
  if (!isMoving && !pathQueue.isEmpty()) {
    currentTarget = pathQueue.pop();
    isMoving = true;
    Serial.print("Moving to: (");
    Serial.print(currentTarget.x);
    Serial.print(", ");
    Serial.print(currentTarget.y);
    Serial.println(")");
  }
  
  // 简化控制：仅根据目标方向调整速度（实际需 PID 转向控制）
  if (isMoving) {
    float dx = currentTarget.x - robotX;
    float dy = currentTarget.y - robotY;
    float distance = sqrt(dx * dx + dy * dy);
    if (distance < 0.1) {
      // 到达目标点
      isMoving = false;
      motorL.target = motorR.target = 0;
    } else {
      // 简化转向：根据 dx/dy 比例分配电机速度
      float turnRatio = atan2(dy, dx) / PI; // -1 到 1
      motorL.target = 2.0 * (1 - abs(turnRatio));
      motorR.target = 2.0 * (1 + turnRatio);
      // 模拟位置更新（实际需编码器积分）
      robotX += 0.01 * dx / distance;
      robotY += 0.01 * dy / distance;
    }
  }
}

5. 技术解读

传感器选型决定'智能'程度

• 超声波/红外：成本低，适合案例一的简单避障，但探测范围窄，易误判。
• 激光雷达（Lidar）：适合案例三的复杂环境，能提供 360 度精确距离数据，是实现 SLAM（同步定位与地图构建）和高级路径规划的基础。

路径规划算法的'大脑'分层

• *全局规划（如 A）**：负责'从 A 到 B 怎么走最省时'，基于已知地图计算全局最优路径。
• 局部规划（如 DWA）：负责'眼前有个人怎么绕开'，实时处理传感器数据，避开动态障碍物。两者需结合使用。

BLDC 电机的'执行'精度

单纯的 analogWrite 控制直流电机转速不稳。推荐使用 FOC（磁场定向控制）库（如 SimpleFOC）驱动 BLDC，可实现平滑的转速和扭矩控制，这对于需要精确转向（如案例二的角度对准）至关重要。

定位是导航的'锚点'

没有定位，路径规划就是空谈。除了昂贵的 GPS，在室内通常依赖编码器里程计。通过读取左右轮编码器的脉冲数，可以估算出机器人走了多远、转了多少度，从而更新 (x, y) 坐标。

状态机思维避免'卡死'

机器人容易在角落或复杂地形卡住。代码中应设计状态机（如：搜索状态、前进状态、避障状态、恢复状态），并加入超时机制。例如，转向时间过长仍未找到路，则触发'随机探索'或'原路退回'策略。

6. 总结

以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和 Arduino 版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。