Arduino 基于 6.5 寸轮毂电机的智能动态跟随机器人底盘设计

机械安装与轮胎选型

• 轴刚性固定：6.5 寸轮毂电机通常通过 16mm 轴或法兰固定。必须使用紧定螺钉 + 夹紧套或刚性连接板确保轴不松动，任何晃动都会导致霍尔测速不准和底盘跑偏。
• 轮胎匹配：PU 实心胎适合室内平坦地面，控制精准、不爆胎；橡胶充气胎适合室外不平地面，减震好、越障强，但需注意胎压，气压不足会影响里程计精度。

安全冗余机制

• 硬件急停：必须设计独立的硬件急停回路（如串联急停按钮），当触发时直接切断电机供电，优先级高于软件逻辑。
• 软件限幅：在代码中限制 PWM 输出最大值，防止因算法 bug 输出全速指令；同时设置软件限位，防止机器人冲出安全区域。

![图片描述]

1、基于 UWB 超宽带定位的差速跟随（PID 控制）

功能：通过 UWB 模块（如 DW1000）获取目标与机器人的相对位置，使用 PID 算法控制轮毂电机实现平滑跟随。 硬件：

• Arduino Mega/Due（支持多串口）
• 2×BLDC 轮毂电机（6.5 寸，带编码器）
• 2×SimpleFOC Shield（驱动电机）
• DW1000 UWB 模块×2（主从机）

#include <SimpleFOC.h>
#include <DW1000.h>

// UWB 库（需自行实现或使用第三方库）

// 左轮电机
BLDCMotor leftMotor(7);
BLDCDriver3PWM leftDriver(9, 10, 11, 8);
MagneticSensorI2C leftEncoder(AS5600_I2C);

// 右轮电机
BLDCMotor rightMotor(12);
BLDCDriver3PWM rightDriver(13, 14, 15, 16);
MagneticSensorI2C rightEncoder(AS5600_I2C);

// UWB 定位参数
float targetX = 0, targetY = 0; // 目标坐标（通过 UWB 更新）
float robotX = 0, robotY = 0;    // 机器人坐标
float desiredDistance = 1.0;     // 期望跟随距离（米）
float desiredAngle = 0;          // 期望朝向（弧度）

// PID 参数
float Kp_dist = 0.8, Ki_dist = 0.05, Kd_dist = 0.1; // 距离 PID
float Kp_angle = 1.0, Ki_angle = 0.1, Kd_angle = 0.2; // 角度 PID

void setup() {
    // 电机初始化（同前文案例）
    leftMotor.linkSensor(&leftEncoder);
    leftMotor.linkDriver(&leftDriver);
    leftMotor.controller = MotionControlType::velocity;
    leftMotor.PID_velocity.P = 0.2;
    leftMotor.initFOC();

    rightMotor.linkSensor(&rightEncoder);
    rightMotor.linkDriver(&rightDriver);
    rightMotor.controller = MotionControlType::velocity;
    rightMotor.PID_velocity.P = 0.2;
    rightMotor.initFOC();

    // UWB 初始化（简化示例）
    DW1000.begin(Serial1); // 使用硬件串口 1
    DW1000.setAddress(0x1234); // 机器人地址
    Serial.begin(115200);
}

void loop() {
    // 1. 通过 UWB 获取目标位置（实际需实现通信协议）
    if (DW1000.available()) {
        float dist, angle;
        DW1000.getRelativePosition(&dist, &angle); // 假设 UWB 返回极坐标
        targetX = robotX + dist * cos(angle);
        targetY = robotY + dist * sin(angle);
    }

    // 2. 计算误差（距离和角度）
    float dx = targetX - robotX;
    float dy = targetY - robotY;
    float currentDistance = sqrt(dx * dx + dy * dy);
    float currentAngle = atan2(dy, dx); // 目标相对于机器人的方位角

    float errorDist = desiredDistance - currentDistance;
    float errorAngle = desiredAngle - currentAngle;
    errorAngle = (errorAngle > PI) ? errorAngle - 2 * PI : (errorAngle < -PI) ? errorAngle + 2 * PI : errorAngle;

    // 3. PID 控制（距离和角度解耦）
    static float integralDist = 0, integralAngle = 0;
    static float lastErrorDist = 0, lastErrorAngle = 0;
    float derivativeDist = (errorDist - lastErrorDist) / 0.02;
    float derivativeAngle = (errorAngle - lastErrorAngle) / 0.02;

    float outputDist = Kp_dist * errorDist + Ki_dist * integralDist + Kd_dist * derivativeDist;
    float outputAngle = Kp_angle * errorAngle + Ki_angle * integralAngle + Kd_angle * derivativeAngle;

    integralDist += errorDist * 0.02;
    integralAngle += errorAngle * 0.02;
    lastErrorDist = errorDist;
    lastErrorAngle = errorAngle;

    // 4. 差速转向控制（融合距离和角度输出）
    float baseSpeed = outputDist * 1.5; // 基础速度与距离误差成比例
    float turnRate = outputAngle * 0.8; // 转向速率与角度误差成比例
    float leftSpeed = baseSpeed - turnRate;
    float rightSpeed = baseSpeed + turnRate;

    leftMotor.move(leftSpeed);
    rightMotor.move(rightSpeed);

    // 5. 更新机器人位置（简化里程计模型）
    robotX += (leftSpeed + rightSpeed) * 0.02 * cos(currentAngle) / 2;
    robotY += (leftSpeed + rightSpeed) * 0.02 * sin(currentAngle) / 2;

    delay(20);
}

2、基于 OpenMV 视觉的色块跟随（纯追踪算法）

功能：通过 OpenMV 摄像头识别特定颜色色块，计算机器人与目标的相对位置，使用纯追踪算法控制轮毂电机。 硬件：

• Arduino Nano 33 BLE Sense（支持 I2C/UART）
• OpenMV Cam H7（带 UART 输出）
• 2×BLDC 轮毂电机（6.5 寸）
• SimpleFOC Shield

#include <SimpleFOC.h>

// 电机初始化（同案例 1）
BLDCMotor leftMotor(7);
BLDCDriver3PWM leftDriver(9, 10, 11, 8);
MagneticSensorI2C leftEncoder(AS5600_I2C);
BLDCMotor rightMotor(12);
BLDCDriver3PWM rightDriver(13, 14, 15, 16);
MagneticSensorI2C rightEncoder(AS5600_I2C);

// 视觉参数
float targetX = 160, targetY = 120; // OpenMV 图像中心坐标（320x240）
float lookaheadRatio = 0.5;         // 预瞄点比例（0~1）

void setup() {
    // 电机初始化
    leftMotor.linkSensor(&leftEncoder);
    leftMotor.linkDriver(&leftDriver);
    leftMotor.controller = MotionControlType::velocity;
    leftMotor.initFOC();

    rightMotor.linkSensor(&rightEncoder);
    rightMotor.linkDriver(&rightDriver);
    rightMotor.controller = MotionControlType::velocity;
    rightMotor.initFOC();

    Serial2.begin(115200); // OpenMV UART
    Serial.begin(115200);
}

void loop() {
    // 1. 从 OpenMV 获取目标坐标（通过 UART）
    if (Serial2.available() > 0) {
        String msg = Serial2.readStringUntil('\n');
        int commaIndex = msg.indexOf(',');
        targetX = msg.substring(0, commaIndex).toFloat();
        targetY = msg.substring(commaIndex + 1).toFloat();
    }

    // 2. 计算预瞄点（纯追踪算法）
    float imageCenterX = 160, imageCenterY = 120;
    float dx = targetX - imageCenterX;
    float dy = targetY - imageCenterY;
    float angleToTarget = atan2(dy, dx); // 目标在图像中的方位角（弧度）

    // 3. 转换为机器人坐标系（假设摄像头朝前）
    float robotAngle = 0; // 通过 IMU 或编码器估计（此处简化）
    float globalAngle = angleToTarget + robotAngle;

    // 4. 差速控制（简化版）
    float baseSpeed = 2.0; // 基础速度
    float turnGain = 0.5;  // 转向增益
    float errorAngle = globalAngle; // 直接使用图像角度误差

    leftMotor.move(baseSpeed - errorAngle * turnGain);
    rightMotor.move(baseSpeed + errorAngle * turnGain);

    Serial.print("Target: (");
    Serial.print(targetX);
    Serial.print(",");
    Serial.print(targetY);
    Serial.print(") Angle: ");
    Serial.println(angleToTarget * 180 / PI);

    delay(20);
}

3、基于激光雷达的 SLAM 跟随（动态避障）

功能：通过 RPLIDAR A1 获取环境点云，使用 SLAM 算法定位机器人与目标，结合 A*路径规划实现动态避障跟随。 硬件：

• Arduino Portenta H7（高性能 MCU）
• RPLIDAR A1（通过 UART 连接）
• 2×BLDC 轮毂电机（6.5 寸）
• SimpleFOC Shield

#include <SimpleFOC.h>
#include <RPLidar.h> // 需自行实现或使用第三方库

// 电机初始化（同案例 1）
BLDCMotor leftMotor(7);
BLDCDriver3PWM leftDriver(9, 10, 11, 8);
MagneticSensorI2C leftEncoder(AS5600_I2C);
BLDCMotor rightMotor(12);
BLDCDriver3PWM rightDriver(13, 14, 15, 16);
MagneticSensorI2C rightEncoder(AS5600_I2C);

// 激光雷达参数
RPLidar lidar;
float robotPos[2] = {0, 0}; // 机器人位置（通过 SLAM 更新）
float targetPos[2] = {2, 0}; // 目标位置
float path[100][2];          // 存储路径点
int pathLength = 0;

void setup() {
    // 电机初始化
    leftMotor.linkSensor(&leftEncoder);
    leftMotor.linkDriver(&leftDriver);
    leftMotor.controller = MotionControlType::velocity;
    leftMotor.initFOC();

    rightMotor.linkSensor(&rightEncoder);
    rightMotor.linkDriver(&rightDriver);
    rightMotor.controller = MotionControlType::velocity;
    rightMotor.initFOC();

    // 激光雷达初始化
    lidar.begin(Serial2); // 使用硬件串口 2
    Serial.begin(115200);
}

void loop() {
    // 1. 获取激光雷达数据并更新 SLAM 地图（简化示例）
    if (lidar.available()) {
        float scanData[360][2]; // 存储极坐标点
        lidar.getScan(scanData); // 假设通过 SLAM 算法更新机器人位置和目标路径
        updateSLAM(scanData);
        generatePath(robotPos, targetPos, path, &pathLength);
    }

    // 2. 纯追踪算法跟踪路径
    if (pathLength > 0) {
        Vector2 currentPos(robotPos[0], robotPos[1]);
        Vector2 nextPoint(path[0][0], path[0][1]);
        // 计算预瞄点（动态调整预瞄距离）
        float lookaheadDist = 0.3 + (pathLength > 5 ? 0.2 : 0); // 根据路径长度调整
        Vector2 lookahead = getLookaheadPoint(currentPos, nextPoint, lookaheadDist);

        // 差速控制
        float dx = lookahead.x - robotPos[0];
        float dy = lookahead.y - robotPos[1];
        float angleToTarget = atan2(dy, dx);
        float baseSpeed = 1.5;
        float turnRate = angleToTarget * 0.8;

        leftMotor.move(baseSpeed - turnRate);
        rightMotor.move(baseSpeed + turnRate);
    }
    delay(20);
}

// 简化版 SLAM 和路径规划函数（需实际实现）
void updateSLAM(float scanData[360][2]) {/* ... */}
void generatePath(float start[2], float end[2], float path[100][2], int* length) {/* ... */}
Vector2 getLookaheadPoint(Vector2 pos, Vector2 next, float dist) {/* ... */}

4、基于超声波与 PID 的恒距平滑跟随

功能描述：这是最经典的跟随方案。利用超声波测距，通过 PID 算法将距离误差转化为电机速度指令。相比简单的'近了就退，远了就进'，PID 能让底盘在接近目标时自动减速，实现平滑停靠，避免急停急启造成的晃动。

#include <SimpleFOC.h>

// --- 硬件定义 ---
// 假设使用 SimpleFOC 驱动 6.5 寸轮毂电机
BLDCMotor motorL(7); // 7 对极电机
BLDCMotor motorR(7);
// 驱动器引脚需根据实际板卡定义
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(5, 6, 7, 4);

// 超声波引脚
#define TRIG_PIN 2
#define ECHO_PIN 3

// --- 跟随参数 ---
const float TARGET_DIST = 50.0; // 目标跟随距离 (cm)
const float MAX_DIST = 200.0;   // 最大检测距离
const float MIN_DIST = 20.0;    // 最小安全距离

// --- PID 控制器 ---
// 输入是距离误差，输出是电机速度
float Kp = 2.0, Ki = 0.0, Kd = 1.0; // 需实测整定
float lastError = 0;
float integral = 0;

void setup() {
    Serial.begin(115200);
    // 初始化超声波
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);

    // 初始化电机 (SimpleFOC)
    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();
}

float readDistance() {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long duration = pulseIn(ECHO_PIN, HIGH, 30000); // 30ms 超时
    return (duration * 0.034) / 2; // 转换为 cm
}

void loop() {
    // 1. 读取距离
    float distance = readDistance();

    // 2. 有效性检查
    if (distance > MAX_DIST || distance < 2) {
        motorL.move(0);
        motorR.move(0);
        return;
    }

    // 3. PID 计算
    float error = distance - TARGET_DIST;
    integral += error;
    float derivative = error - lastError;

    // 计算速度指令
    float speedCmd = (Kp * error) + (Ki * integral) + (Kd * derivative);
    lastError = error;

    // 4. 速度限制与死区处理
    // 简单的死区补偿，防止低速不动
    if (abs(speedCmd) < 0.5) speedCmd = 0;
    // 限制最大速度 (rad/s)
    speedCmd = constrain(speedCmd, -5.0, 5.0);

    // 5. 差速电机控制
    motorL.move(speedCmd);
    motorR.move(speedCmd);

    // SimpleFOC 核心循环
    motorL.loopFOC();
    motorR.loopFOC();
    delay(10); // 100Hz 控制频率
}

5、基于蓝牙 RSSI 信号强度的定向跟随

功能描述：利用蓝牙模块（如 HC-05）的信号强度指示（RSSI）来判断目标的远近。信号越强（数值越接近 0）代表越近，信号越弱（负值越大）代表越远。这是一种低成本、无需视觉的'盲跟随'方案。

#include <SoftwareSerial.h>

// --- 硬件定义 ---
SoftwareSerial btSerial(10, 11); // RX, TX 连接蓝牙模块

// --- 跟随参数 ---
const int RSSI_TARGET = -40;    // 目标信号强度 (dBm)，需实测校准
const int RSSI_TOO_CLOSE = -20; // 太近阈值
const int RSSI_TOO_FAR = -70;   // 太远阈值

// --- 电机控制 ---
const int MOTOR_L_PWM = 9;
const int MOTOR_R_PWM = 10;
const int MOTOR_L_DIR = 2;
const int MOTOR_R_DIR = 3;

void setup() {
    Serial.begin(9600);
    btSerial.begin(9600); // 蓝牙波特率通常为 9600 或 38400
    pinMode(MOTOR_L_PWM, OUTPUT);
    pinMode(MOTOR_R_PWM, OUTPUT);
    pinMode(MOTOR_L_DIR, OUTPUT);
    pinMode(MOTOR_R_DIR, OUTPUT);
    // 初始化电机方向...
}

void loop() {
    // 1. 读取 RSSI (AT 指令模式或特定模块会自动上报)
    // 这里假设通过串口发送 "AT+CSQ" 或解析模块自动上报的数据
    // 实际代码需根据蓝牙模块协议解析
    int rssi = readRSSI(); // 模拟函数，返回 -100 到 0 之间的整数
    if (rssi == 0) return; // 无信号

    // 2. 逻辑判断
    int speed = 0;
    if (rssi > RSSI_TOO_CLOSE) {
        // 信号太强 -> 距离太近 -> 后退
        speed = -100;
        digitalWrite(MOTOR_L_DIR, LOW); // 设置方向
        digitalWrite(MOTOR_R_DIR, LOW);
    } else if (rssi < RSSI_TOO_FAR) {
        // 信号太弱 -> 距离太远 -> 前进
        speed = 150;
        digitalWrite(MOTOR_L_DIR, HIGH);
        digitalWrite(MOTOR_R_DIR, HIGH);
    } else {
        // 信号适中 -> 保持或微调
        // 简单的比例控制：信号越弱，速度越快
        speed = map(rssi, RSSI_TOO_FAR, RSSI_TARGET, 150, 0);
        digitalWrite(MOTOR_L_DIR, HIGH);
        digitalWrite(MOTOR_R_DIR, HIGH);
    }

    // 3. 输出 PWM
    analogWrite(MOTOR_L_PWM, abs(speed));
    analogWrite(MOTOR_R_PWM, abs(speed));

    Serial.print("RSSI: ");
    Serial.print(rssi);
    Serial.print(" Speed: ");
    Serial.println(speed);
    delay(100); // 蓝牙读取不宜过快
}

// 模拟 RSSI 读取函数
int readRSSI() {
    // 实际需解析 AT+CSQ 返回的值
    // 这里仅做演示
    return -45;
}

6、多传感器融合与防碰撞急停

功能描述：单纯的跟随容易发生碰撞。本案例在跟随的基础上，增加了前后双超声波检测，构建了一个简单的'安全包围盒'。一旦检测到前方有突发障碍物或后方有人靠近，立即触发急停或避让逻辑，提升安全性。

#include <NewPing.h>

// --- 传感器定义 ---
#define SONAR_NUM 2
#define TRIG_PIN_FRONT 2
#define ECHO_PIN_FRONT 3
#define TRIG_PIN_REAR 4
#define ECHO_PIN_REAR 5
#define MAX_DIST 200
NewPing sonar[SONAR_NUM] = {
    NewPing(TRIG_PIN_FRONT, ECHO_PIN_FRONT, MAX_DIST),
    NewPing(TRIG_PIN_REAR, ECHO_PIN_REAR, MAX_DIST)
};

// --- 安全阈值 ---
const int STOP_DIST_FRONT = 30; // 前方急停距离
const int STOP_DIST_REAR = 20;  // 后方急停距离

// --- 电机状态 ---
int motorSpeed = 0;

void setup() {
    Serial.begin(9600);
    // 初始化电机驱动...
}

void loop() {
    // 1. 读取传感器 (分时触发避免干扰)
    delay(50);
    unsigned int distFront = sonar[0].ping_cm();
    delay(50);
    unsigned int distRear = sonar[1].ping_cm();

    // 2. 安全逻辑判断 (优先级最高)
    if ((distFront > 0 && distFront < STOP_DIST_FRONT) || (distRear > 0 && distRear < STOP_DIST_REAR)) {
        // 触发急停
        motorSpeed = 0;
        setMotors(0, 0);
        Serial.println("⚠️ 障碍物检测！紧急制动");
        return;
    }

    // 3. 正常跟随逻辑 (此处可结合案例一的 PID)
    // 假设 distFront 是跟随目标的距离
    if (distFront > 0 && distFront < MAX_DIST) {
        // 简单的 P 控制演示
        if (distFront > 100) {
            motorSpeed = 150; // 远了加速
        } else {
            motorSpeed = 0;   // 近了停止
        }
        setMotors(motorSpeed, motorSpeed);
    }

    Serial.print("Front: ");
    Serial.print(distFront);
    Serial.print(" Rear: ");
    Serial.println(distRear);
}

void setMotors(int left, int right) {
    // 驱动电机函数
}

要点解读

• 轮毂电机的 FOC 控制优势：案例 4 使用了 SimpleFOC 库。6.5 寸轮毂电机通常是无刷电机（BLDC），相比传统直流电机，使用 FOC（磁场定向控制）能实现极低速下的平稳运行和精准扭矩控制。这对于跟随机器人至关重要，因为它能避免低速跟随时的'顿挫感'。
• PID 在距离控制中的应用：在案例 4 中，我们不是简单地控制'动'或'停'，而是控制'速度'。PID 算法将距离误差转化为速度指令。当距离远时速度快，距离近时速度慢，到达目标距离时速度为 0，这种平滑过渡是提升用户体验的关键。
• RSSI 跟随的局限与校准：案例 5 利用蓝牙信号强度（RSSI）跟随，成本极低，但受环境影响大（多径效应）。关键点在于校准：不同的手机、不同的蓝牙模块，其信号强度基准不同。必须在实际环境中测试，找到'舒适距离'对应的 RSSI 值（如 -40dBm），并设置合理的死区。
• 多传感器抗干扰设计：案例 6 使用了 NewPing 库并加入了 delay 分时触发。这是因为超声波传感器在发射声波时会产生机械振动，如果多个传感器同时触发，回波会相互干扰（串扰）。通过分时读取（例如间隔 50ms），可以确保数据的准确性。
• 安全冗余机制：在涉及大功率轮毂电机的应用中，惯性很大，刹车距离长。案例 6 展示了'安全包围盒'的概念：不仅要看前面（跟随目标），还要看后面（防止被撞击）和侧面。一旦检测到任何方向的突发障碍，急停逻辑的优先级必须高于跟随逻辑，这是机器人安全设计的铁律。

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

![图片描述]