【花雕】6.5 寸轮毂电机自动跟随底盘・超声波最简方案

1、超声波跟随基础方案（单超声波）

#include <SimpleFOC.h>

// 6.5寸轮毂电机（约165mm直径）
BLDCMotor motorL = BLDCMotor(7); // 左轮毂电机
BLDCMotor motorR = BLDCMotor(7); // 右轮毂电机
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(5, 6, 7, 4);

// 超声波传感器
#define TRIG_PIN 2
#define ECHO_PIN 3

// 跟随参数
const float TARGET_DISTANCE = 50.0; // 目标跟随距离 50cm
const float MAX_DISTANCE = 200.0;   // 最大检测距离
const float WHEEL_DIAMETER = 0.165; // 6.5寸 = 16.5cm直径
const float WHEEL_BASE = 0.30;      // 轮距30cm

float readUltrasonic() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // 30ms超时
  float distance = duration * 0.034 / 2; // 声速340m/s
  
  if (distance > MAX_DISTANCE || distance < 2) {
    return -1; // 无效读数
  }
  return distance;
}

void followControl() {
  float distance = readUltrasonic();
  
  if (distance < 0) {
    // 传感器无效，停止
    motorL.move(0);
    motorR.move(0);
    return;
  }
  
  // 简单PID跟随控制
  float error = distance - TARGET_DISTANCE;
  
  // 基础控制策略
  float baseSpeed = 80; // 基础速度
  float speedAdjust = error * 1.0; // 比例控制
  
  if (error > 20) {
    // 距离过远，加速前进
    motorL.move(baseSpeed + speedAdjust);
    motorR.move(baseSpeed + speedAdjust);
  } else if (error < -10) {
    // 距离过近，减速或后退
    motorL.move(baseSpeed + speedAdjust);
    motorR.move(baseSpeed + speedAdjust);
  } else {
    // 保持距离，原地停止或低速维持
    motorL.move(10);
    motorR.move(10);
  }
}

void setup() {
  Serial.begin(115200);
  
  // 初始化超声波引脚
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // 初始化左电机
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();
  
  // 初始化右电机
  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();
  
  Serial.println("6.5寸轮毂电机跟随底盘就绪");
}

void loop() {
  // 电机FOC控制
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 跟随控制（10Hz控制频率）
  static unsigned long lastControl = 0;
  if (millis() - lastControl > 100) {
    followControl();
    lastControl = millis();
    
    // 调试输出
    Serial.print("距离:");
    Serial.print(readUltrasonic());
    Serial.print("cm 左速:");
    Serial.print(motorL.shaft_velocity);
    Serial.print(" 右速:");
    Serial.println(motorR.shaft_velocity);
  }
}
复制代码

2、双超声波角度跟随方案

#include <SimpleFOC.h>

// 6.5寸轮毂电机
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(2,3,4,5);
BLDCDriver3PWM driverR = BLDCDriver3PWM(6,7,8,9);

// 双超声波传感器
#define US_LEFT_TRIG 10
#define US_LEFT_ECHO 11
#define US_RIGHT_TRIG 12
#define US_RIGHT_ECHO 13

// 移动平均滤波器
class MovingAverageFilter {
private:
  float buffer[5];
  int index = 0;
  
public:
  MovingAverageFilter() {
    for(int i=0; i<5; i++) buffer[i] = 0;
  }
  
  float filter(float value) {
    buffer[index] = value;
    index = (index + 1) % 5;
    
    float sum = 0;
    for(int i=0; i<5; i++) sum += buffer[i];
    return sum / 5;
  }
};

MovingAverageFilter leftFilter, rightFilter;

float readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH, 25000); // 25ms超时
  return duration * 0.034 / 2;
}

void angleFollowing() {
  // 读取左右距离
  float leftDist = readUltrasonic(US_LEFT_TRIG, US_LEFT_ECHO);
  float rightDist = readUltrasonic(US_RIGHT_TRIG, US_RIGHT_ECHO);
  
  // 滤波处理
  leftDist = leftFilter.filter(leftDist);
  rightDist = rightFilter.filter(rightDist);
  
  // 有效性检查
  if (leftDist > 200 || leftDist < 5) leftDist = 0;
  if (rightDist > 200 || rightDist < 5) rightDist = 0;
  
  // 计算角度偏差
  float angleError = 0;
  if (leftDist > 0 && rightDist > 0) {
    // 左右都有有效读数，计算角度偏差
    angleError = (leftDist - rightDist) * 0.5; // 转换为角度修正
  } else if (leftDist > 0) {
    // 只有左侧有读数，右转
    angleError = -30;
  } else if (rightDist > 0) {
    // 只有右侧有读数，左转
    angleError = 30;
  } else {
    // 没有有效读数，停止
    motorL.move(0);
    motorR.move(0);
    return;
  }
  
  // 距离控制
  float avgDist = (leftDist + rightDist) / 2;
  float distanceError = avgDist - 60; // 目标距离60cm
  
  // 速度控制
  float baseSpeed = constrain(100 - distanceError * 0.5, 30, 150);
  
  // 转向控制
  float turnAdjust = constrain(angleError * 0.8, -50, 50);
  
  // 差速控制
  motorL.move(baseSpeed + turnAdjust);
  motorR.move(baseSpeed - turnAdjust);
  
  // 调试输出
  Serial.print("左:");
  Serial.print(leftDist);
  Serial.print("cm 右:");
  Serial.print(rightDist);
  Serial.print("cm 角度修正:");
  Serial.println(turnAdjust);
}

void setup() {
  Serial.begin(115200);
  
  // 初始化超声波引脚
  pinMode(US_LEFT_TRIG, OUTPUT);
  pinMode(US_LEFT_ECHO, INPUT);
  pinMode(US_RIGHT_TRIG, OUTPUT);
  pinMode(US_RIGHT_ECHO, INPUT);
  
  // 初始化电机
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();
  
  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();
  
  Serial.println("双超声波角度跟随就绪");
}

void loop() {
  // 必须的FOC循环
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 8Hz控制频率（125ms）
  static unsigned long lastControl = 0;
  if (millis() - lastControl > 125) {
    angleFollowing();
    lastControl = millis();
  }
}
复制代码

3、超声波+IMU融合跟随方案

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

// 6.5寸轮毂电机底盘
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(2,3,4,5);
BLDCDriver3PWM driverR = BLDCDriver3PWM(6,7,8,9);

// 超声波（前向）
#define US_FRONT_TRIG A0
#define US_FRONT_ECHO A1

// 超声波（侧向，检测目标方位）
#define US_SIDE_TRIG A2
#define US_SIDE_ECHO A3

// MPU6050 IMU
MPU6050 mpu;

// 状态变量
float targetDistance = 80.0; // 80cm跟随距离
float currentYaw = 0;
float targetYaw = 0;

void setupIMU() {
  Wire.begin();
  mpu.initialize();
  
  if (!mpu.testConnection()) {
    Serial.println("MPU6050连接失败");
  }
  
  // 校准（简化）
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
}

float readIMUYaw() {
  // 读取陀螺仪Z轴，积分得到偏航角
  static float yaw = 0;
  static unsigned long lastTime = 0;
  
  unsigned long currentTime = micros();
  float dt = (currentTime - lastTime) / 1e6;
  lastTime = currentTime;
  
  int16_t gz = mpu.getRotationZ();
  float gyroZ = gz / 131.0 * PI/180.0; // 转换为rad/s
  
  yaw += gyroZ * dt;
  
  // 角度归一化
  if (yaw > PI) yaw -= 2*PI;
  if (yaw < -PI) yaw += 2*PI;
  
  return yaw;
}

void fusionFollowing() {
  // 前向距离测量
  float frontDist = readUltrasonic(US_FRONT_TRIG, US_FRONT_ECHO);
  float sideDist = readUltrasonic(US_SIDE_TRIG, US_SIDE_ECHO);
  
  // IMU数据
  currentYaw = readIMUYaw();
  
  // 状态决策
  if (frontDist < 30) {
    // 距离太近，后退
    motorL.move(-80);
    motorR.move(-80);
    
    // 同时尝试转向
    if (sideDist > 0 && sideDist < 100) {
      float sideError = sideDist - 40; // 期望侧向40cm
      motorL.move(-80 + sideError);
      motorR.move(-80 - sideError);
    }
  } 
  else if (frontDist > 30 && frontDist < 150) {
    // 正常跟随模式
    
    // 距离控制
    float distanceError = frontDist - targetDistance;
    float speedBase = constrain(100 - distanceError * 1.0, 40, 150);
    
    // 方向控制（使用侧向超声波）
    float directionError = 0;
    if (sideDist > 10 && sideDist < 200) {
      directionError = (sideDist - 50) * 0.5; // 期望50cm侧距
    }
    
    // IMU辅助稳定
    float yawStabilization = (targetYaw - currentYaw) * 10.0;
    
    // 综合控制
    float leftSpeed = speedBase + directionError - yawStabilization;
    float rightSpeed = speedBase - directionError + yawStabilization;
    
    // 限幅
    leftSpeed = constrain(leftSpeed, 0, 200);
    rightSpeed = constrain(rightSpeed, 0, 200);
    
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);
  }
  else {
    // 距离过远或无读数，停止
    motorL.move(20);
    motorR.move(20);
  }
  
  // 调试信息
  Serial.print("前:");
  Serial.print(frontDist);
  Serial.print("cm 侧:");
  Serial.print(sideDist);
  Serial.print("cm 偏航:");
  Serial.print(currentYaw * 180/PI);
  Serial.println("度");
}

void setup() {
  Serial.begin(115200);
  
  // 初始化超声波
  pinMode(US_FRONT_TRIG, OUTPUT);
  pinMode(US_FRONT_ECHO, INPUT);
  pinMode(US_SIDE_TRIG, OUTPUT);
  pinMode(US_SIDE_ECHO, INPUT);
  
  // 初始化IMU
  setupIMU();
  
  // 初始化电机
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();
  
  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();
  
  Serial.println("超声波+IMU融合跟随就绪");
}

void loop() {
  // FOC控制循环
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 10Hz控制频率
  static unsigned long lastControl = 0;
  if (millis() - lastControl > 100) {
    fusionFollowing();
    lastControl = millis();
  }
}
复制代码

要点解读：
（1）6.5寸轮毂电机特性与配置：
物理参数：直径约16.5cm，周长约51.8cm，适合中小型机器人
功率匹配：12V供电，每轮约50-100W功率，提供足够扭矩
控制需求：需要FOC控制实现平稳启停和低速控制
安装优势：轮毂电机结构紧凑，集成度高
（2）超声波传感器最小可行配置：
单传感器方案：最简单，只能检测距离，无法判断角度
双传感器方案：可检测角度偏差，实现更好的跟随效果
传感器布局：前方+侧向布局可同时检测距离和方位
滤波处理：移动平均滤波消除超声波的随机误差
（3）跟随控制算法核心：
距离控制环：PID或比例控制维持目标距离
角度控制环：差速转向保持正确方位
状态机设计：根据不同距离范围采取不同策略
防撞保护：近距离时自动后退或停止
（4）性能优化关键点：
控制频率：8-10Hz足够，过高会增加计算负担
响应速度：6.5寸轮毂响应快，需防止过冲
滤波参数：平衡响应速度和稳定性
死区设置：避免在目标距离附近振荡
（5）可靠性增强措施：
超时处理：超声波读数超时返回无效值
范围限制：只处理有效范围内的读数（2-200cm）
错误恢复：传感器失效时安全停止
调试输出：串口监控实时状态，便于调试
这个最简方案使用最少的硬件（1-2个超声波+2个轮毂电机）实现了基本的自动跟随功能，适合快速原型开发和入门级应用。后续可根据需求增加更多传感器或优化算法。

4、基础避障与定向跟随

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

// 6.5寸轮毂电机配置（假设电机极对数7，额定电压12V）
BLDCMotor motorLeft(7, 1000);  // 左轮电机（引脚7，PWM频率1000Hz）
BLDCMotor motorRight(8, 1000); // 右轮电机
NewPing sonarFront(12, 13, 200); // 超声波传感器（触发12，回波13，量程200cm）

void setup() {
  Serial.begin(115200);
  motorLeft.linkDriver(new BLDCDriver3PWM(3,5,6,11)); // 3PWM驱动（3,5,6引脚，使能11）
  motorRight.linkDriver(new BLDCDriver3PWM(9,10,12,13));
  motorLeft.init(); motorRight.init();
}

void loop() {
  // 超声波测距（抗干扰：连续测量3次取中值）
  int dist = sonarFront.ping_cm();
  delay(30); // 抑制电磁干扰
  
  // 避障逻辑：前方<30cm时后退+转向
  if(dist > 0 && dist < 30) {
    motorLeft.move(-200);  // 后退
    motorRight.move(-200);
    delay(500);
    motorLeft.move(150);   // 左转
    motorRight.move(-150);
    delay(300);
  } 
  // 正常前进
  else {
    motorLeft.move(300);
    motorRight.move(300);
  }
}
复制代码

要点：
硬件抗干扰：3PWM驱动模式减少电源噪声，超声波测量间隔30ms抑制电磁干扰。
运动解耦：后退与转向动作分离，避免电机指令冲突。
阈值动态校准：30cm阈值需根据实际环境调整，适应不同地面材质。
电机保护：PWM限幅（-200~300）防止电机过流，延长BLDC寿命。
模块化设计：驱动层与控制逻辑分离，便于功能扩展（如添加IMU）。

5、双超声波环向避障

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

BLDCMotor motorLeft(7, 1000);
BLDCMotor motorRight(8, 1000);
NewPing sonarFront(12,13,200);
NewPing sonarRear(14,15,200); // 后向超声波

void setup() {
  motorLeft.linkDriver(new BLDCDriver3PWM(3,5,6,11));
  motorRight.linkDriver(new BLDCDriver3PWM(9,10,12,13));
  motorLeft.init(); motorRight.init();
}

void loop() {
  int frontDist = sonarFront.ping_cm();
  int rearDist = sonarRear.ping_cm();
  delay(25); // 25ms控制周期
  
  // 前后障碍物检测逻辑
  if(frontDist < 30 && rearDist < 30) {
    motorLeft.move(0); motorRight.move(0); // 急停
  } 
  else if(frontDist < 30) {
    motorLeft.move(-250); motorRight.move(250); // 原地转向避障
  } 
  else if(rearDist < 30) {
    motorLeft.move(200); motorRight.move(200); // 后退
  } 
  else {
    motorLeft.move(350); motorRight.move(350); // 前进
  }
}
复制代码

要点：
多向感知：前后双超声波实现360°避障，避免单传感器盲区。
运动模式切换：急停/转向/后退多模式协同，提升复杂环境适应性。
控制频率优化：25ms周期匹配BLDC响应特性，避免控制滞后。
电源管理：双超声波分时工作降低功耗，延长续航时间。
故障安全：前后同时检测到障碍物时急停，防止碰撞损坏。

6、PID姿态稳定与路径跟随

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

BLDCMotor motorLeft(7, 1000);
BLDCMotor motorRight(8, 1000);
MPU6050 mpu;
Kalman kalman;

// PID参数（需实测整定）
float Kp = 1.2, Ki = 0.05, Kd = 0.1;
float targetAngle = 0; // 保持水平姿态

void setup() {
  motorLeft.linkDriver(new BLDCDriver3PWM(3,5,6,11));
  motorRight.linkDriver(new BLDCDriver3PWM(9,10,12,13));
  motorLeft.init(); motorRight.init();
  mpu.initialize();
  kalman.setAngle(0);
}

void loop() {
  // MPU6050数据读取与卡尔曼滤波
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float angle = kalman.getAngle(atan2(-ay, az)*180/PI, gx/131.0);
  
  // PID控制计算
  float error = targetAngle - angle;
  static float integral = 0, lastError = 0;
  integral = constrain(integral + error*0.02, -100, 100);
  float derivative = (error - lastError)/0.02;
  float output = Kp*error + Ki*integral + Kd*derivative;
  lastError = error;
  
  // 电机差速控制（姿态补偿）
  motorLeft.move(300 + output);
  motorRight.move(300 - output);
  delay(20); // 50Hz控制频率
}
复制代码

要点：
姿态稳定：卡尔曼滤波融合加速度计与陀螺仪数据，抑制振动噪声。
PID参数整定：需通过实验调整Kp/Ki/Kd，避免振荡或响应迟缓。
差速控制：通过左右电机转速差实现姿态补偿，保持底盘稳定。
高频控制：20ms周期匹配IMU数据更新率，实现平滑控制。
电源隔离：IMU供电加π型滤波器，减少电机噪声干扰。

要点解读总结
（1）传感器融合与抗干扰：
超声波测距需分时触发，避免相互干扰；MPU6050需刚性固定并校准零偏。
电源分区设计（驱动/传感器/控制）减少电机噪声对敏感电路的影响。
（2）运动控制解耦：
避障、转向、前进等动作需明确分离，避免电机指令冲突。
差速控制实现精确转向，减少机械应力，延长轮毂电机寿命。
（3）PID参数整定：
需通过“Ziegler-Nichols法”或实验调整参数，适应不同底盘质量分布。
积分项需限幅防止风扰等持续误差导致电机饱和。
（4）硬件选型与配置：
6.5寸轮毂电机需匹配BLDC驱动器（如VESC），支持闭环控制。
Arduino需外接稳压模块，避免电机启动电流导致电压跌落重启。
（5）安全冗余机制：
急停逻辑需优先于所有运动指令，防止碰撞损坏。
电机PWM限幅与失控保护（如通信丢失时自动停机）提升系统可靠性。

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