Arduino 6.5 寸轮毂电机智能跟随机器人底盘设计与实现

2. OpenMV 视觉色块跟随

利用 OpenMV 摄像头识别特定颜色色块，计算机器人与目标的相对位置，使用纯追踪算法控制轮毂电机。OpenMV 通过 UART 发送坐标数据，Arduino 解析后计算预瞄点并控制电机。

#include<SimpleFOC.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);

float targetX = 160, targetY = 120;
float lookaheadRatio = 0.5;

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

void loop() {
    // 读取视觉数据
    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();
    }

    // 纯追踪简化版
    float imageCenterX = 160, imageCenterY = 120;
    float dx = targetX - imageCenterX;
    float dy = targetY - imageCenterY;
    float angleToTarget = atan2(dy, dx);

    float baseSpeed = 2.0;
    float turnGain = 0.5;
    float errorAngle = angleToTarget;

    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 算法定位机器人与目标，结合路径规划实现动态避障跟随。这需要高性能 MCU（如 Portenta H7）来处理点云数据。

#include<SimpleFOC.h>
#include<RPLidar.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);

RPLidar lidar;
float robotPos[2] = {0, 0};
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);
    Serial.begin(115200);
}

void loop() {
    if (lidar.available()) {
        float scanData[360][2];
        lidar.getScan(scanData);
        updateSLAM(scanData);
        generatePath(robotPos, targetPos, path, &pathLength);
    }

    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);
}

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 能让底盘在接近目标时自动减速，实现平滑停靠。

#include<SimpleFOC.h>

BLDCMotor motorL(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;
const float MAX_DIST = 200.0;
const float MIN_DIST = 20.0;

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() {
    float distance = readDistance();

    if (distance > MAX_DIST || distance < 2) {
        motorL.move(0);
        motorR.move(0);
        return;
    }

    float error = distance - TARGET_DIST;
    integral += error;
    float derivative = error - lastError;

    float speedCmd = (Kp * error) + (Ki * integral) + (Kd * derivative);
    lastError = error;

    if (abs(speedCmd) < 0.5) speedCmd = 0;
    speedCmd = constrain(speedCmd, -5.0, 5.0);

    motorL.move(speedCmd);
    motorR.move(speedCmd);

    motorL.loopFOC();
    motorR.loopFOC();
    delay(10);
}

5. 蓝牙 RSSI 定向跟随

利用蓝牙模块的信号强度指示（RSSI）来判断目标的远近。这是一种低成本、无需视觉的'盲跟随'方案。需注意不同设备的信号基准差异，必须在实际环境中校准。

#include<SoftwareSerial.h>

SoftwareSerial btSerial(10, 11);

const int RSSI_TARGET = -40;
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);
    pinMode(MOTOR_L_PWM, OUTPUT);
    pinMode(MOTOR_R_PWM, OUTPUT);
    pinMode(MOTOR_L_DIR, OUTPUT);
    pinMode(MOTOR_R_DIR, OUTPUT);
}

int readRSSI() {
    // 模拟函数，实际需解析 AT+CSQ
    return -45;
}

void loop() {
    int rssi = readRSSI();
    if (rssi == 0) return;

    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);
    }

    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);
}

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

void loop() {
    delay(50);
    unsigned int distFront = sonar[0].ping_cm();
    delay(50);
    unsigned int distRear = sonar[1].ping_cm();

    if ((distFront > 0 && distFront < STOP_DIST_FRONT) ||
        (distRear > 0 && distRear < STOP_DIST_REAR)) {
        motorSpeed = 0;
        setMotors(0, 0);
        Serial.println("⚠️ 障碍物检测！紧急制动");
        return;
    }

    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);
}

技术总结

轮毂电机的 FOC 控制优势在于极低速下的平稳运行和精准扭矩控制，这对于跟随机器人至关重要，能避免低速跟随时的'顿挫感'。PID 在距离控制中将距离误差转化为速度指令，当距离远时速度快，距离近时速度慢，到达目标距离时速度为 0，这种平滑过渡是提升用户体验的关键。

RSSI 跟随成本极低，但受环境影响大，关键在于校准。多传感器设计中，分时触发超声波传感器可以避免回波串扰。涉及大功率轮毂电机时，惯性很大，刹车距离长，安全冗余机制必须遵循'急停逻辑优先级高于跟随逻辑'的铁律。