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

方案一：基于 UWB 超宽带定位的差速跟随

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

#include <SimpleFOC.h>
// 注意：DW1000 库需根据具体硬件型号引入
#include <DW1000.h>

// 左轮电机配置
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;
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;
float Kp_angle = 1.0, Ki_angle = 0.1, Kd_angle = 0.2;

float lastErrorDist = 0;
float lastErrorAngle = 0;

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);
    DW1000.setAddress(0x1234);
    Serial.begin(115200);
}

void loop() {
    // 1. 获取目标位置
    if (DW1000.available()) {
        float dist, angle;
        DW1000.getRelativePosition(&dist, &angle);
        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;
    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);
}

方案二：基于超声波与 PID 的恒距平滑跟随

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

#include <SimpleFOC.h>

// --- 硬件定义 ---
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);

    // 初始化电机
    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);
    return (duration * 0.034) / 2;
}

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;
    speedCmd = constrain(speedCmd, -5.0, 5.0);

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

    motorL.loopFOC();
    motorR.loopFOC();
    delay(10); // 100Hz 控制频率
}

方案三：多传感器融合与防碰撞急停

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

#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 setMotors(int left, int right) {
    // 实际驱动函数
}

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. 正常跟随逻辑
    if (distFront > 0 && distFront < MAX_DIST) {
        if (distFront > 100) {
            motorSpeed = 150;
        } else {
            motorSpeed = 0;
        }
        setMotors(motorSpeed, motorSpeed);
    }

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

五、技术解读与建议

1. FOC 控制优势

使用 SimpleFOC 库驱动 6.5 寸轮毂电机，相比传统直流电机，FOC（磁场定向控制）能实现极低速下的平稳运行和精准扭矩控制。这对于跟随机器人至关重要，因为它能避免低速跟随时的'顿挫感'。

2. PID 在距离控制中的应用

我们不是简单地控制'动'或'停'，而是控制'速度'。PID 算法将距离误差转化为速度指令。当距离远时速度快，距离近时速度慢，到达目标距离时速度为 0，这种平滑过渡是提升用户体验的关键。

3. 安全冗余机制

在涉及大功率轮毂电机的应用中，惯性很大，刹车距离长。展示了'安全包围盒'的概念：不仅要看前面（跟随目标），还要看后面（防止被撞击）。一旦检测到任何方向的突发障碍，急停逻辑的优先级必须高于跟随逻辑，这是机器人安全设计的铁律。

4. 开发建议

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