基于 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){
  /* ... */
}

技术解读

• 传感器融合与定位
  • 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 降压供电。

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