【花雕学编程】Arduino BLDC 之UWB精准定位跟随机器人

驴友花雕

27人浏览 · 2026-07-10 20:48:04

驴友花雕 · 2026-07-10 20:48:04 发布

在这里插入图片描述
基于Arduino与BLDC（无刷直流电机）的“UWB精准定位跟随机器人”，是一种专为复杂、动态环境设计的先进机电一体化系统。该系统利用超宽带（UWB）技术实现厘米级的空间感知，结合BLDC电机的高动态响应，能够在无需视觉或地图建模的情况下，实现对特定目标的稳定、抗干扰跟随。以下是该系统的详细专业解析：
一、主要特点
厘米级高精度与强抗干扰定位
UWB技术利用纳秒级脉冲信号，测距精度可达10–30 cm级别，远优于蓝牙和Wi-Fi。其超宽频带（通常>500 MHz）赋予了系统极强的抗多径效应和电磁干扰能力，即使在金属密集、电磁环境复杂的工厂车间或仓库中，也能保持极高的定位稳定性。
多基站协同与TDOA/ToF解算
系统依赖预先部署的固定基站（Anchor）与机器人上的标签（Tag）协同工作。通过至少3个非共线布置的基站，利用TDOA（到达时间差）或ToF（飞行时间）原理，解算出目标在全局坐标系中的精确二维或三维位置。这种架构无需改造地面环境，具备极高的部署灵活性。
BLDC高动态响应与平滑运动控制
底层采用BLDC电机作为执行器，具有高效率、高扭矩密度和低噪音的特点。在接收到上层算法根据UWB数据计算出的期望速度和转向角后，BLDC电机能够迅速、平滑地执行差速转向或加减速指令，确保在目标突然启停或急转弯时，机器人能够平稳跟随且不发生剧烈晃动。
低功耗与动态自适应跟随
系统可通过“主动唤醒+低频轮询”机制优化功耗，在目标静止时降低测距频率，加速时恢复高频。结合卡尔曼滤波（KF）或扩展卡尔曼滤波（EKF），系统能够平滑原始定位数据的跳动，预测目标轨迹，即使在目标被短暂遮挡时也能维持跟随的连续性。
二、典型应用场景
智能仓储与物流拣选（人机协同AGV）
在大型仓库中，将UWB标签贴于托盘或佩戴在拣货员身上，机器人自动识别并跟随目标移动，实现“货到人”的高效搬运，显著提升分拣效率并减轻人工劳动强度。
工厂车间物料配送与工具跟随
在柔性制造产线中，机器人可实时跟随携带标签的物料小车或操作员，自动完成从原料库到装配工位的运输任务。UWB的高抗干扰能力使其能在复杂的电磁与金属环境中稳定运行。
智慧零售与超市购物车
作为智能购物车跟随顾客，自动避障穿行于货架之间。结合UWB的身份绑定与预测算法，确保机器人只跟随“自己的主人”，提供无感跟随的购物体验，甚至可结合RFID实现无感支付。
医疗康复与助行辅助
在医院或康复中心，作为智能助行器或医疗设备运载车跟随患者移动。UWB的低延迟与高精度确保了设备在患者改变步速或转向时能及时响应，提供安全的支撑与跟随体验。
三、需要注意的关键事项
基站几何布局与视距（LoS）保障
基站的部署直接决定定位精度。至少需要3个基站呈非共线布置以包围工作区域，且必须保证机器人与基站之间无严重的物理遮挡（视距要求）。基站坐标需经过高精度物理测量并输入系统，任何标定误差都会直接放大定位偏差。
主控算力瓶颈与架构分工
UWB的原始测距数据解算（如求解非线性方程组）及后续的滤波算法对算力要求极高，标准Arduino Uno/Nano极易因内存溢出或浮点运算过载而崩溃。强烈建议采用ESP32、Teensy 4.1或树莓派等高算力主控，或采用“上位机解算坐标 + Arduino底层控制”的分布式架构。
数据滤波与多传感器融合
尽管UWB抗干扰能力强，但在极端环境或人体遮挡时仍会出现测距跳变。系统必须引入滤波算法（如卡尔曼滤波）对原始坐标进行平滑处理。同时，建议融合IMU（惯性测量单元）和轮式里程计，在UWB信号短暂丢失时通过航位推算维持跟随的连续性。
严格的电源隔离与电磁兼容（EMC）
UWB模块对电源噪声和射频干扰极其敏感，而BLDC电机在启停时会产生巨大的电流冲击和高频PWM噪声。必须为UWB模块和Arduino主控提供独立的电源，严禁与电机共用；信号线需远离动力线，防止电机干扰导致定位数据乱跳。
安全机制与容错设计
必须设计完善的安全策略。即便UWB定位准确，也必须配备超声波或激光雷达等传感器实现局部避障，设置紧急停止按钮与碰撞检测逻辑。当检测到与目标的距离过近或UWB信号完全丢失时，机器人应自动触发减速或急停机制，防止盲目跟随导致碰撞。

在这里插入图片描述

1、三基站三角定位 + PID跟随
适用场景：室内AGV跟随人员或货物。三个UWB基站部署在固定位置，机器人通过三边测量法定位目标并PID控制跟随。

/* ===== Arduino BLDC UWB三基站三角定位 + PID跟随 =====
 * 硬件：2×BLDC差速底盘 + 3个UWB基站(DWM1000) + 1个UWB标签
 * 核心：三边测量定位 → 卡尔曼滤波平滑 → PID位置跟随
 */
#include <SimpleFOC.h>
#include <Math.h>

BLDCMotor motorL(5), motorR(6);
BLDCDriver3PWM drvL, drvR;
Encoder encL(2,3,2048), encR(4,5,2048);

// 三个基站坐标(已知，单位：米)
const float ANCHOR[3][2] = {
  {0.0, 0.0},    // 基站0
  {6.0, 0.0},    // 基站1
  {3.0, 5.0}     // 基站2
};

// UWB测距值(米)
float ranges[3] = {0, 0, 0};

// 目标位置(待求)
float targetX = 0, targetY = 0;

// 机器人位置(里程计)
float robotX = 0, robotY = 0, robotYaw = 0;

// 卡尔曼滤波
float kfX = 0, kfY = 0;
float kfP = 1.0;  // 估计误差协方差
const float KF_Q = 0.1;  // 过程噪声
const float KF_R = 0.3;  // 测量噪声

// PID参数
float Kp_dist = 0.8, Ki_dist = 0.02, Kd_dist = 0.1;
float distIntegral = 0, lastDistError = 0;
float Kp_ang = 1.2, Ki_ang = 0.01;
float angIntegral = 0;

const float DESIRED_DIST = 1.2;  // 期望跟随距离(m)
const float MAX_SPEED = 250;

// ========== 三边测量定位 ==========
bool trilaterate(float d0, float d1, float d2, float* x, float* y) {
  float A = 2 * (ANCHOR[1][0] - ANCHOR[0][0]);
  float B = 2 * (ANCHOR[1][1] - ANCHOR[0][1]);
  float C = d0*d0 - d1*d1 - ANCHOR[0][0]*ANCHOR[0][0] + ANCHOR[1][0]*ANCHOR[1][0] 
            - ANCHOR[0][1]*ANCHOR[0][1] + ANCHOR[1][1]*ANCHOR[1][1];
  float D = 2 * (ANCHOR[2][0] - ANCHOR[1][0]);
  float E = 2 * (ANCHOR[2][1] - ANCHOR[1][1]);
  float F = d1*d1 - d2*d2 - ANCHOR[1][0]*ANCHOR[1][0] + ANCHOR[2][0]*ANCHOR[2][0]
            - ANCHOR[1][1]*ANCHOR[1][1] + ANCHOR[2][1]*ANCHOR[2][1];
  
  float det = A * E - B * D;
  if(abs(det) < 0.0001) return false;
  
  *x = (C * E - B * F) / det;
  *y = (A * F - C * D) / det;
  return true;
}

// ========== 卡尔曼滤波 ==========
void kalmanFilter(float measurement, float* state, float* cov) {
  // 预测
  *cov += KF_Q;
  
  // 更新
  float K = *cov / (*cov + KF_R);
  *state += K * (measurement - *state);
  *cov = (1 - K) * *cov;
}

// ========== 读取UWB测距 ==========
bool readUWB() {
  // 从UWB模块读取三个基站的距离(伪代码)
  ranges[0] = getRange(0);
  ranges[1] = getRange(1);
  ranges[2] = getRange(2);
  
  for(int i=0; i<3; i++) {
    if(ranges[i] < 0.1 || ranges[i] > 15.0) return false;
  }
  return true;
}

void setup() {
  Serial.begin(115200);
  initUWB();  // 初始化UWB模块
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 读取UWB测距
  if(!readUWB()) {
    Serial.println("UWB data invalid");
    delay(20);
    return;
  }
  
  // 2. 三边测量定位
  float rawX, rawY;
  if(!trilaterate(ranges[0], ranges[1], ranges[2], &rawX, &rawY)) {
    Serial.println("Trilateration failed");
    delay(20);
    return;
  }
  
  // 3. 卡尔曼滤波平滑
  kalmanFilter(rawX, &kfX, &kfP);
  kalmanFilter(rawY, &kfY, &kfP);
  targetX = kfX;
  targetY = kfY;
  
  // 4. 计算相对位置
  float dx = targetX - robotX;
  float dy = targetY - robotY;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx);
  
  // 5. PID跟随控制
  float distError = dist - DESIRED_DIST;
  distIntegral += distError * dt;
  distIntegral = constrain(distIntegral, -50, 50);
  float distDerivative = (distError - lastDistError) / dt;
  lastDistError = distError;
  
  float vLin = Kp_dist * distError + Ki_dist * distIntegral + Kd_dist * distDerivative;
  vLin = constrain(vLin, -100, MAX_SPEED);
  
  float angError = angle - robotYaw;
  angIntegral += angError * dt;
  angIntegral = constrain(angIntegral, -20, 20);
  float vAng = Kp_ang * angError + Ki_ang * angIntegral;
  vAng = constrain(vAng, -1.5, 1.5);
  
  // 6. 执行
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
  
  // 7. 更新里程计
  float speed = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
  robotX += speed * cos(robotYaw) * dt;
  robotY += speed * sin(robotYaw) * dt;
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
三边测量法利用三个基站距离解算二维位置
卡尔曼滤波平滑UWB测量噪声
PID控制器分别控制距离和角度
里程计辅助更新机器人自身位置

2、四基站TDOA定位 + 预测跟随
适用场景：高精度跟随（如自动充电对接）。四个基站通过到达时间差(TDOA)定位，结合目标速度预测实现超前跟随。

/* ===== Arduino BLDC 四基站TDOA + 预测跟随 =====
 * 硬件：2×BLDC差速底盘 + 4个UWB基站 + 1个UWB标签
 * 核心：TDOA定位 → 速度估计 → 位置预测 → 超前跟随
 */
#include <SimpleFOC.h>
#include <Math.h>

BLDCMotor motorL(5), motorR(6);

// 四个基站坐标
const float ANCHOR[4][2] = {
  {0.0, 0.0}, {8.0, 0.0},
  {8.0, 6.0}, {0.0, 6.0}
};

// 目标状态(含速度和预测)
struct TargetState {
  float x, y;
  float vx, vy;
  float px, py;  // 预测位置
  unsigned long lastUpdate;
};
TargetState target;

// 预测参数
const float PREDICT_TIME = 0.5;  // 预测时间(秒)
const float MAX_SPEED = 260;

// TDOA定位(简化)
bool tdoaLocate(float* x, float* y) {
  float tdoa[4];
  for(int i=0; i<4; i++) {
    tdoa[i] = getTOA(0, i);  // 标签0到基站i的到达时间
  }
  
  // 以基站0为参考
  float c = 299792458;  // 光速
  float d1 = (tdoa[1] - tdoa[0]) * c;
  float d2 = (tdoa[2] - tdoa[0]) * c;
  float d3 = (tdoa[3] - tdoa[0]) * c;
  
  // Chan算法简化
  float A = (ANCHOR[1][0]*ANCHOR[1][0] + ANCHOR[1][1]*ANCHOR[1][1] - d1*d1) / 2.0;
  float B = (ANCHOR[2][0]*ANCHOR[2][0] + ANCHOR[2][1]*ANCHOR[2][1] - d2*d2) / 2.0;
  
  float X = (A * (ANCHOR[2][1] - ANCHOR[1][1]) - B * (ANCHOR[1][1] - ANCHOR[0][1])) /
            (ANCHOR[1][0] * (ANCHOR[2][1] - ANCHOR[1][1]) - ANCHOR[2][0] * (ANCHOR[1][1] - ANCHOR[0][1]));
  float Y = (A - ANCHOR[1][0] * X) / ANCHOR[1][1];
  
  *x = X;
  *y = Y;
  return true;
}

void setup() {
  Serial.begin(115200);
  initUWB();
  
  target.x = 0; target.y = 0;
  target.vx = 0; target.vy = 0;
  target.lastUpdate = millis();
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  unsigned long now = millis();
  
  // 1. TDOA定位
  float rawX, rawY;
  if(tdoaLocate(&rawX, &rawY)) {
    // 2. 速度估计
    float dt_real = (now - target.lastUpdate) / 1000.0;
    if(dt_real > 0.01) {
      target.vx = (rawX - target.x) / dt_real;
      target.vy = (rawY - target.y) / dt_real;
    }
    
    target.x = rawX;
    target.y = rawY;
    target.lastUpdate = now;
    
    // 3. 位置预测(超前)
    target.px = target.x + target.vx * PREDICT_TIME;
    target.py = target.y + target.vy * PREDICT_TIME;
  }
  
  // 4. 跟随预测位置
  float dx = target.px - robotX;
  float dy = target.py - robotY;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx);
  
  // 5. 控制
  float vLin = constrain((dist - 0.8) * 55, 0, MAX_SPEED);
  float vAng = constrain((angle - robotYaw) * 2.0, -1.5, 1.5);
  
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
  
  // 6. 更新里程计
  float speed = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
  robotX += speed * cos(robotYaw) * dt;
  robotY += speed * sin(robotYaw) * dt;
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
TDOA定位精度高于RSSI三边测量
速度估计用于位置预测，实现超前跟随
0.5秒预测时间补偿系统延迟
适用于快速移动目标的跟随

3、UWB+IMU紧耦合融合 + 自适应速度跟随
适用场景：UWB信号不稳定环境下的高可靠跟随。UWB提供绝对位置，IMU提供高频相对位移，融合后实现平滑稳定的自适应速度跟随。

/* ===== Arduino BLDC UWB+IMU紧耦合 + 自适应速度跟随 =====
 * 硬件：2×BLDC差速底盘 + 3个UWB基站 + MPU6050 IMU
 * 核心：UWB低频绝对定位 + IMU高频相对定位 → 扩展卡尔曼滤波融合
 *       自适应速度：根据目标距离和方向自动调整
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>
#include <Math.h>

MPU6050 imu;

BLDCMotor motorL(5), motorR(6);

// UWB数据
float uwbX = 0, uwbY = 0;
bool uwbNew = false;
unsigned long lastUWB = 0;

// IMU数据
float imuYaw = 0;
float imuDx = 0, imuDy = 0;

// 融合状态(扩展卡尔曼滤波)
struct EKFState {
  float x, y, yaw;
  float vx, vy;
  float P[4][4];  // 协方差矩阵(简化4x4)
};
EKFState ekf;

// 自适应速度参数
float currentMaxSpeed = 200;
const float MIN_SPEED = 50;
const float MAX_SPEED_LIMIT = 300;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  imu.initialize();
  
  initUWB();
  
  // 初始化EKF
  ekf.x = 0; ekf.y = 0; ekf.yaw = 0;
  ekf.vx = 0; ekf.vy = 0;
  for(int i=0; i<4; i++)
    for(int j=0; j<4; j++)
      ekf.P[i][j] = (i==j) ? 1.0 : 0.0;
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 读取IMU(高频)
  int16_t ax, ay, az, gx, gy, gz;
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  float gyroZ = gz / 131.0;
  imuYaw += gyroZ * dt;
  
  // 2. 读取UWB(低频)
  if(checkUWBData(&uwbX, &uwbY)) {
    uwbNew = true;
    lastUWB = millis();
  }
  
  // 3. EKF预测(IMU驱动)
  ekfPredict(dt, gyroZ);
  
  // 4. EKF更新(UWB观测)
  if(uwbNew) {
    ekfUpdate(uwbX, uwbY);
    uwbNew = false;
  }
  
  // 5. 自适应速度计算
  float dx = targetX - ekf.x;
  float dy = targetY - ekf.y;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx);
  
  // 根据距离自适应最大速度
  if(dist > 3.0) currentMaxSpeed = MAX_SPEED_LIMIT;
  else if(dist > 1.5) currentMaxSpeed = map(dist, 1.5, 3.0, 150, MAX_SPEED_LIMIT);
  else currentMaxSpeed = map(dist, 0.5, 1.5, 50, 150);
  currentMaxSpeed = constrain(currentMaxSpeed, MIN_SPEED, MAX_SPEED_LIMIT);
  
  // 6. 跟随控制
  float vLin = constrain((dist - 1.0) * 45, 0, currentMaxSpeed);
  float vAng = constrain((angle - ekf.yaw) * 2.0, -1.5, 1.5);
  
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

void ekfPredict(float dt, float gyroZ) {
  // 状态预测
  ekf.x += ekf.vx * dt;
  ekf.y += ekf.vy * dt;
  ekf.yaw += gyroZ * dt;
  
  // 协方差预测(简化)
  float Q[4] = {0.1, 0.1, 0.05, 0.1};
  for(int i=0; i<4; i++) ekf.P[i][i] += Q[i];
}

void ekfUpdate(float zx, float zy) {
  // 测量残差
  float yx = zx - ekf.x;
  float yy = zy - ekf.y;
  
  // 测量噪声
  float R = 0.3;
  
  // 卡尔曼增益(简化对角)
  float Kx = ekf.P[0][0] / (ekf.P[0][0] + R);
  float Ky = ekf.P[1][1] / (ekf.P[1][1] + R);
  
  // 状态更新
  ekf.x += Kx * yx;
  ekf.y += Ky * yy;
  
  // 协方差更新
  ekf.P[0][0] = (1 - Kx) * ekf.P[0][0];
  ekf.P[1][1] = (1 - Ky) * ekf.P[1][1];
}

关键设计点：
扩展卡尔曼滤波融合UWB和IMU数据
IMU提供50Hz高频预测，UWB提供10Hz低频校正
自适应速度根据目标距离动态调整
协方差矩阵反映定位不确定性

要点解读
① UWB定位的精度与基站布局密切相关
基站应呈三角形/四边形分布，避免共线
基站间距建议5~10米
标签应在基站构成的凸多边形内
墙角、金属物体附近会有多径效应
案例一的三基站布局和案例二的四基站布局都是常见的部署方式。
② 三边测量 vs TDOA的选择
案例一使用三边测量，案例二使用TDOA。
③ 卡尔曼滤波是UWB定位的标配
原始UWB数据噪声大（±20cm跳动），直接用于控制会导致机器人抖动。卡尔曼滤波可以：
平滑测量噪声
预测缺失帧（UWB掉帧时）
融合多传感器（案例三）
案例一和案例三分别展示了简单卡尔曼滤波和扩展卡尔曼滤波。
④ 跟随控制的关键是“预测”而非“反应”
由于UWB更新频率低（10~50Hz）且有延迟，纯粹的反应式控制会导致：
跟随滞后
目标转弯时冲出
案例二中的速度预测和案例三中的自适应速度都是为了解决这个问题。
⑤ UWB+IMU融合是性价比最高的定位方案
相比纯UWB：
帧率从10Hz提升到50Hz
抗遮挡能力增强
成本仅增加一个IMU（￥10）
相比视觉定位：
不受光照影响
隐私友好
计算量小

在这里插入图片描述
4、单标签测距 + PID闭环跟随
适用场景：简单跟随任务，如自动跟随购物车、行李机器人，利用UWB模块测量与目标标签（Tag）的实时距离，通过PID算法驱动BLDC电机保持设定距离。

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

// UWB配置
#define TAG_ADDRESS 0xDECA0AD000000001
#define ANCHOR_ADDRESS 0xDECA0AD000000002

// BLDC电机定义（差速驱动）
BLDCMotor motorL(7), motorR(7);
// 需补充对应的Driver和Encoder初始化代码...

// PID参数
double setpoint = 100.0;        // 期望跟随距离（厘米）
double input_dist, output_speed;
double Kp = 1.2, Ki = 0.05, Kd = 0.1;
PID positionPID(&input_dist, &output_speed, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  
  // 初始化UWB
  DW1000.begin(TAG_ADDRESS);
  DW1000.newConfiguration();
  DW1000.setDeviceAddress(TAG_ADDRESS);
  DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
  
  // 初始化BLDC电机
  motorL.init(); motorR.init();
  
  // PID初始化
  positionPID.SetMode(AUTOMATIC);
  positionPID.SetOutputLimits(-255, 255);
}

void loop() {
  // 1. 获取UWB实时测距值（厘米）
  float distance = DW1000.getRange();
  
  // 2. 异常值过滤（防止跳变干扰）
  if (distance > 0 && distance < 1000) {
    input_dist = distance;
  }
  
  // 3. PID计算速度指令
  positionPID.Compute();
  
  // 4. 差速驱动BLDC电机
  int baseSpeed = 128;           // 基础PWM占空比
  motorL.move(baseSpeed + output_speed * 0.3);
  motorR.move(baseSpeed - output_speed * 0.3);
  
  // 5. FOC更新
  motorL.loopFOC();
  motorR.loopFOC();
  
  delay(100);  // 10Hz控制频率
}

核心要点：本例实现“距离→速度”的闭环跟随，UWB单标签提供目标距离信息，PID控制器根据距离偏差输出速度指令，再通过差速分解驱动左右电机。

5、双UWB标签差分定位（获取目标朝向）
适用场景：多机器人编队、协同搬运，双标签方案能够解算目标的绝对朝向角，使跟随机器人可以实现“侧后方跟随”或“队形保持”，而非单纯追着目标中心跑。

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

// BLDC电机定义
BLDCMotor motorL(7), motorR(7);

// 模拟获取到的两个UWB标签坐标
struct Coordinate { float x, y; };
Coordinate tag1 = {2.5, 3.0};
Coordinate tag2 = {2.8, 3.4};

// 自身状态（通过UWB/里程计获取）
float selfX = 0, selfY = 0, selfHeading = 0;

void setup() {
  Serial.begin(115200);
  // 初始化UWB和电机...
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  
  // 1. 实时获取两个UWB标签的绝对坐标
  // tag1 = getUWBTagPosition(TAG_ID_1);
  // tag2 = getUWBTagPosition(TAG_ID_2);
  
  // 2. 【核心】双标签差分定位：计算目标的绝对朝向角
  float dx = tag2.x - tag1.x;
  float dy = tag2.y - tag1.y;
  float targetHeading = atan2(dy, dx);  // 弧度范围 -PI 到 PI
  
  // 3. 计算目标中心坐标
  float targetCenterX = (tag1.x + tag2.x) / 2.0;
  float targetCenterY = (tag1.y + tag2.y) / 2.0;
  
  // 4. 计算期望跟随位置（目标正后方0.8米）
  const float DESIRED_OFFSET = 0.8;
  float desiredX = targetCenterX - DESIRED_OFFSET * cos(targetHeading);
  float desiredY = targetCenterY - DESIRED_OFFSET * sin(targetHeading);
  
  // 5. 计算误差并驱动电机
  float errorX = desiredX - selfX;
  float errorY = desiredY - selfY;
  float distanceError = sqrt(errorX * errorX + errorY * errorY);
  float targetAngle = atan2(errorY, errorX);
  
  // 角度误差归一化
  float angleError = targetAngle - selfHeading;
  if (angleError > PI) angleError -= 2 * PI;
  if (angleError < -PI) angleError += 2 * PI;
  
  // 6. 速度控制：距离误差→线速度，角度误差→角速度
  float linearSpeed = constrain(distanceError * 2.0, 0, 3.0);
  float angularSpeed = constrain(angleError * 3.0, -2.0, 2.0);
  
  // 7. 差速驱动
  motorL.move(linearSpeed - angularSpeed * 0.2);
  motorR.move(linearSpeed + angularSpeed * 0.2);
  
  delay(30);
}

核心要点：双标签差分定位的核心价值在于获取绝对朝向。通过两个间距固定的标签坐标差，利用atan2(dy, dx)解算目标朝向，使跟随机器人具备“预判”能力，实现顺滑的曲线跟随而非机械追逐。

6、多基站三角定位 + 坐标级跟随
适用场景：需要在较大范围内精确定位并跟随的AGV、巡检机器人。通过部署3-4个已知坐标的UWB基站（Anchor），机器人标签解算自身(x, y)绝对坐标，实现“坐标级”精准跟随。

#include <DW1000Ranging.h>
#include <SimpleFOC.h>
#include <math.h>

// 定义3个基站的已知坐标（单位：米）
struct Anchor { float x, y; };
Anchor anchors[3] = {{0.0, 0.0}, {5.0, 0.0}, {2.5, 4.0}};

// 机器人当前坐标（由UWB解算）
float robotX = 0, robotY = 0;
float targetX = 2.5, targetY = 2.0;  // 目标坐标（可从UWB标签获取）

BLDCMotor motorL(7), motorR(7);

void setup() {
  Serial.begin(115200);
  
  // UWB初始化
  DW1000Ranging.initCommunication(9, 10);
  DW1000Ranging.attachNewRange(newRangeCallback);
  DW1000Ranging.startAsTag("7D:00:22:EA:82:60:3B:9C");
  
  motorL.init(); motorR.init();
}

void loop() {
  DW1000Ranging.loop();  // 持续处理UWB通信
  
  // 1. 计算机器人与目标的相对角度和距离
  float dx = targetX - robotX;
  float dy = targetY - robotY;
  float distance = sqrt(dx*dx + dy*dy);
  float targetAngle = atan2(dy, dx) * 180 / PI;
  
  // 2. 运动控制：距离越远速度越快，角度误差驱动转向
  float baseSpeed = constrain(distance * 50, 0, 200);
  float turnRatio = constrain(targetAngle * 2, -100, 100);
  
  motorL.move(baseSpeed - turnRatio);
  motorR.move(baseSpeed + turnRatio);
  motorL.loopFOC();
  motorR.loopFOC();
  
  delay(50);
}

// UWB测距回调函数，通过三边测量法解算坐标
void newRangeCallback() {
  // 实际实现中需收集各基站距离，利用三边测量法(Trilateration)解算robotX/robotY
  // 参考: https://www.th-luebeck.de/fileadmin/media_cosa/Dateien/Veroeffentlichungen/Sammlung/TR-2-2015-least-sqaures-with-ToA.pdf
}