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;
    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); // 3ms 超时
    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 展示了'安全包围盒'的概念：不仅要看前面（跟随目标），还要看后面（防止被撞击）和侧面。一旦检测到任何方向的突发障碍，急停逻辑的优先级必须高于跟随逻辑，这是机器人安全设计的铁律。

技术解读

传感器融合与定位

• UWB：高精度室内定位（±10cm），但需部署锚点。
• 视觉：OpenMV 适合简单色块跟随，但易受光照影响。
• 激光雷达：SLAM 实现全局定位，但计算量大，需高性能 MCU（如 Portenta H7）。

运动学模型优化

• 差速驱动：通过左右轮速度差实现转向，需考虑轮距（L）对转弯半径的影响。

电机控制与 PID 调参

• 轮毂电机特性：6.5 寸电机惯性大，需降低 Kp 避免振荡，增加 Kd 抑制超调。
• 双闭环控制：外环（位置/速度）PID 输出作为内环（电流）PID 的输入（需 SimpleFOC 支持）。

动态避障与路径规划

• A*算法：适用于静态地图，但需离散化网格。
• DWA（动态窗口法）：实时避障，适合高速场景，但计算复杂度高。

硬件选型与电源管理

• 电机驱动：SimpleFOC Shield 支持高电流（≥10A），但需额外散热。
• 电源：6.5 寸电机峰值电流可能达 20A，建议使用 24V/10Ah 锂电池 + DC-DC 降压供电。