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

要点解读总结
（1）传感器融合与抗干扰：
超声波测距需分时触发，避免相互干扰；MPU6050需刚性固定并校准零偏。
电源分区设计（驱动/传感器/控制）减少电机噪声对敏感电路的影响。
（2）运动控制解耦：
避障、转向、前进等动作需明确分离，避免电机指令冲突。
差速控制实现精确转向，减少机械应力，延长轮毂电机寿命。
（3）PID参数整定：
需通过“Ziegler-Nichols法”或实验调整参数，适应不同底盘质量分布。
积分项需限幅防止风扰等持续误差导致电机饱和。
（4）硬件选型与配置：
6.5寸轮毂电机需匹配BLDC驱动器（如VESC），支持闭环控制。
Arduino需外接稳压模块，避免电机启动电流导致电压跌落重启。
（5）安全冗余机制：
急停逻辑需优先于所有运动指令，防止碰撞损坏。
电机PWM限幅与失控保护（如通信丢失时自动停机）提升系统可靠性。
注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。

4、基础避障与定向跟随
[code]#include
#include
// 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);
}
}[/code]
要点：
硬件抗干扰：3PWM驱动模式减少电源噪声，超声波测量间隔30ms抑制电磁干扰。
运动解耦：后退与转向动作分离，避免电机指令冲突。
阈值动态校准：30cm阈值需根据实际环境调整，适应不同地面材质。
电机保护：PWM限幅（-200~300）防止电机过流，延长BLDC寿命。
模块化设计：驱动层与控制逻辑分离，便于功能扩展（如添加IMU）。
5、双超声波环向避障
[code]#include
#include
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); // 前进
}
}[/code]
要点：
多向感知：前后双超声波实现360°避障，避免单传感器盲区。
运动模式切换：急停/转向/后退多模式协同，提升复杂环境适应性。
控制频率优化：25ms周期匹配BLDC响应特性，避免控制滞后。
电源管理：双超声波分时工作降低功耗，延长续航时间。
故障安全：前后同时检测到障碍物时急停，防止碰撞损坏。
6、PID姿态稳定与路径跟随
[code]#include
#include
#include
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控制频率
}[/code]
要点：
姿态稳定：卡尔曼滤波融合加速度计与陀螺仪数据，抑制振动噪声。
PID参数整定：需通过实验调整Kp/Ki/Kd，避免振荡或响应迟缓。
差速控制：通过左右电机转速差实现姿态补偿，保持底盘稳定。
高频控制：20ms周期匹配IMU数据更新率，实现平滑控制。
电源隔离：IMU供电加π型滤波器，减少电机噪声干扰。

1、超声波跟随基础方案（单超声波）
[code]#include
// 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);
}
}[/code]
2、双超声波角度跟随方案
[code]#include
// 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();
}
}[/code]
3、超声波+IMU融合跟随方案
[code]#include
#include
#include
// 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();
}
}[/code]
要点解读：
（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个轮毂电机）实现了基本的自动跟随功能，适合快速原型开发和入门级应用。后续可根据需求增加更多传感器或优化算法。