该方案的核心特点是利用双UWB标签差分定位解算目标姿态，结合BLDC电机配合FOC算法实现的毫秒级力矩响应与多轴同步控制，使双机械臂能够像“主从机械手”一样实现高精度的位置同步与协作跟随；主要适用于柔性制造、人机协同装配、科研验证等场景；实际部署需重点解决Arduino算力瓶颈、高频控制环实时性、UWB多径效应及双臂运动学耦合等问题。

一、主要特点
双UWB标签差分定位——从“单点追踪”到“全向姿态感知”
传统单UWB标签仅能获取目标的二维或三维坐标，无法判断目标的朝向（航向角）。双UWB标签通过在目标物体（如工人的安全帽、移动载具）上对称安装两个标签，利用两点间的几何关系实时解算出目标的绝对位置和航向角（Yaw）。
全向姿态感知：这种差分机制彻底消除了单标签在特定几何角度（如与基站共线）下的方向判断盲区，使机器人具备精确的“360°全向姿态感知”能力。
抗遮挡与鲁棒性：双标签互为冗余，当其中一个标签因遮挡导致信号丢失时，系统仍可依靠另一个标签维持基本的跟随功能，极大提升了系统的鲁棒性。
BLDC+FOC多轴同步控制——从“独立驱动”到“协同运动”
双机械臂的协作跟随要求两个机械臂在空间和时间上保持高度一致，任何微小的延迟或误差都可能导致碰撞或任务失败。
FOC磁场定向控制：结合FOC算法，BLDC电机可以实现精细的力矩闭环控制。通过Clark和Park变换，将三相交流电流解耦为励磁电流（d轴）和转矩电流（q轴），实现转矩脉动<1%的平稳输出，确保机械臂在低速和高速下都能平稳运行。
多轴同步架构：采用“一主多从”的分布式控制架构，主控制器（如高性能Arduino或上位机）负责轨迹规划与逆运动学解算，通过CAN或EtherCAT总线向各关节驱动板同步下发目标位置/速度指令，确保各关节“同时启动、同时结束”，轴间同步误差可控制在微秒级。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC的高频电流环（通常>1kHz），能够毫秒级响应目标位置的变化，确保机械臂在目标突然启停或急转弯时“如影随形”。
位置同步与协作跟随——从“被动跟踪”到“主动协同”
相对坐标转换：系统将UWB获取的全局坐标系下的目标位置实时转换为机械臂本体坐标系下的相对距离和方位角，为底层的运动控制提供直接输入。
动态任务分配：在多目标场景下，系统可通过为不同的UWB标签分配独立的网络ID，实现选择性跟随。例如，双机械臂可以分别跟随两名不同的工人，或共同跟随同一个大型工件。
碰撞检测与避障：结合机械臂的动力学模型和实时力矩反馈，系统可以检测潜在的碰撞风险，并主动调整运动轨迹或触发紧急停止，确保人机协作的安全。
分层控制架构
规划层（UWB定位+逆运动学）：负责解算目标姿态，规划机械臂末端执行器的期望轨迹。
控制层（FOC+多轴同步）：负责将轨迹转化为关节力矩指令，并确保双臂的同步运动。
感知层（UWB+编码器+力矩传感器）：负责提供目标位置、关节角度和力矩反馈。

二、典型应用场景
柔性制造与人机协同装配
在汽车装配线、电子元件插件等场景中，工人佩戴UWB标签，双机械臂作为“智能助手”跟随工人移动。
协作跟随：机械臂自动携带工具或零件，跟随工人到指定工位，并在工人操作时提供辅助支撑或递送工具，实现“货到人”或“工具到人”的高效协同。
位置同步：双机械臂可以协同搬运大型或重型工件（如汽车挡风玻璃、电池包），通过位置同步确保工件在搬运过程中保持水平，避免倾斜或掉落。
智能仓储与物流拣选
在大型仓库中，双机械臂安装在移动底盘上，跟随佩戴UWB标签的拣货员。
动态跟随：机械臂自动识别并跟随拣货员，在货架间穿梭时自动调整姿态，避免与货架或行人碰撞。
选择性跟随：通过为不同的UWB标签分配独立的网络ID，机械臂可以只响应其绑定的操作员，避免了多机协同时的“串台”与混乱。
医疗手术辅助与康复训练
在微创手术或康复训练中，双机械臂可以协同操作手术器械或辅助患者肢体运动。
高精度同步：UWB定位结合FOC控制，可以实现亚毫米级的定位精度和微秒级的同步误差，满足手术或康复训练对精度的严苛要求。
柔顺控制：通过力矩反馈，机械臂可以感知患者的肢体阻力，并主动调整输出力矩，实现柔顺的辅助运动，避免对患者造成二次伤害。
科研与教育验证
作为高校机器人学、嵌入式控制课程的实验平台，用于验证UWB定位算法、FOC驱动、多轴同步控制等前沿技术。

三、需要注意的事项
Arduino算力瓶颈与架构分工
双UWB标签解算、逆运动学解算、FOC算法及多轴同步控制同时运行对算力要求极高。
平台选型：经典8位Arduino（如Uno）几乎无法胜任。必须选用ESP32-S3（双核240MHz）、STM32H7或Arduino Portenta H7等高性能平台，或采用“上位机+下位机”架构。
主从架构：建议采用“上位机+下位机”架构。上位机（如树莓派、Jetson或PC）负责UWB数据解析、逆运动学解算和轨迹规划；下位机（如专用FOC驱动板）负责高频电流环控制和电机换相。
控制频率与实时性
双机械臂的协作跟随要求极高的实时性，尤其是多轴同步的精度。
高频控制环：FOC的电流环频率建议≥1kHz，速度环和位置环≥500Hz。多轴同步的通信周期应控制在1ms以内，避免轴间同步误差累积。
通信延迟：上位机与下位机之间的通信（如CAN、EtherCAT）延迟必须控制在毫秒级，否则会导致力矩指令滞后，引发机械臂抖动或碰撞。
UWB多径效应与定位精度
UWB在金属密集或复杂工业环境中容易受多径效应影响，导致测距误差。
多源融合：建议融合IMU（惯性测量单元）与轮式里程计，通过扩展卡尔曼滤波（EKF）进行紧耦合融合，在UWB信号短暂丢失或跳变时依靠惯性航位推算维持定位连续性。
基站部署：UWB基站应避开金属遮挡物，并采用非共线布置（至少3个基站），以优化几何稀释精度（GDOP）。
双臂运动学耦合与碰撞检测
双机械臂在协同运动时，存在复杂的运动学耦合关系，且容易发生自碰撞或与环境碰撞。
逆运动学解算：需精确建立双机械臂的运动学模型，并考虑双臂之间的相对位置约束，避免奇异点。
碰撞检测：需在算法中加入实时碰撞检测机制，结合力矩传感器或电流反馈，当检测到异常阻力时立即触发紧急停止。
电机力矩响应与一致性
协作跟随的效果高度依赖电机的力矩响应性能和一致性。
电机选型：选用低齿槽效应、高扭矩密度的BLDC电机，确保低速下的力矩平稳性。
参数一致性：同一机械臂上的所有电机参数（极对数、内阻、电感）必须一致，否则会导致各关节力矩输出不均，影响同步精度。
电磁兼容（EMC）与电源管理
多电机高频PWM驱动会产生严重的电磁干扰，影响UWB定位精度。
电源隔离：动力电源与逻辑电源必须物理隔离，使用独立DC-DC模块。
滤波设计：电源入口并联大容量电解电容和高频陶瓷电容，吸收电压尖峰。
布线规范：强电与弱电严格分开，UWB天线应远离电机和电源线，避免干扰。

1、双UWB差分定位 + 基础位置跟随
此案例聚焦核心感知层：通过双UWB标签实现主从机械臂的差分定位，并据此驱动从臂进行基础位置跟随。解决了“仅测距不测向”的痛点，让从臂具备跟随主臂朝向的能力。

#include <SimpleFOC.h>
#include <DW1000.h> // 常用UWB库

// 假设已定义主从机械臂各关节BLDC电机 (从臂为例)
BLDCMotor joint1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 10, 11);

// UWB标签ID
const uint8_t MASTER_TAG = 0x01;
const uint8_t SLAVE_TAG = 0x02;

// 位置与跟随参数
float masterPos[2] = {0, 0};  // 主臂位置 (x, y)
float slavePos[2] = {0, 0};   // 从臂当前位置
float followOffset[2] = {0.5, 0}; // 期望协同间距 (x方向偏移0.5m)
float followError[2] = {0, 0};
float minSafeDist = 0.2;      // 最小安全距离 (m)

void setup() {
  Serial.begin(115200);
  // 初始化UWB
  DW1000.begin();
  // 初始化BLDC电机及FOC
  driver1.init();
  joint1.linkDriver(&driver1);
  joint1.init();
  joint1.initFOC();
}

void loop() {
  // 1. 读取双UWB差分坐标
  if (DW1000.getPosition(MASTER_TAG, masterPos)) {
    // 获取成功
  }
  DW1000.getPosition(SLAVE_TAG, slavePos);

  // 2. 计算跟随目标位置 (主臂位置 + 预设偏移)
  float targetPos[2] = {masterPos[0] + followOffset[0], masterPos[1] + followOffset[1]};

  // 3. 计算跟随误差
  followError[0] = targetPos[0] - slavePos[0];
  followError[1] = targetPos[1] - slavePos[1];
  float distError = sqrt(followError[0]*followError[0] + followError[1]*followError[1]);

  // 4. 安全距离判断：小于阈值则停止跟随
  if (distError < minSafeDist) {
    Serial.println("WARNING: Too close! Stopping.");
    joint1.move(0);
    joint1.loopFOC();
    delay(50);
    return;
  }

  // 5. 速度控制 (简化为比例控制)
  float speedMag = constrain(distError * 0.5, 0, 1.5); // 速度系数
  float targetAngle = atan2(followError[1], followError[0]); // 目标方向角

  // 6. 驱动从臂关节 (为简化，假设单关节机械臂，角度映射到速度)
  //    实际多关节需逆运动学解算
  joint1.move(speedMag * cos(targetAngle)); 
  joint1.loopFOC(); // 刷新FOC闭环

  // 调试输出
  Serial.print("Target: "); Serial.print(targetPos[0]); Serial.print(","); Serial.println(targetPos[1]);
  Serial.print("Error: "); Serial.println(distError);
  delay(50);
}

2、基于CAN总线的位置/速度双环同步
此案例关注执行层的精确同步。采用CAN总线的主从架构，主节点广播同步指令，从节点运行“位置环+速度环”双闭环PID，实现高精度的多轴协同运动。本案例聚焦单一从臂内部多关节的同步，是协同控制的基础。

#include <SimpleFOC.h>
#include <FlexCAN_T4.h> // CAN库 (以Teensy为例)

// 假设从臂有两个关节
BLDCMotor jointA = BLDCMotor(7);
BLDCMotor jointB = BLDCMotor(7);

// 定义PID参数 (位置环和速度环)
float Kp_pos = 10.0, Ki_pos = 1.0, Kd_pos = 0.5;
float Kp_vel = 5.0, Ki_vel = 0.5;

// 目标角度 (由上位机或主臂通过CAN发送)
float targetAngleA = 0, targetAngleB = 0;
bool newTarget = false;

void setup() {
  Serial.begin(115200);
  // 初始化CAN
  Can0.begin();
  Can0.setBaudRate(1000000);
  Can0.onReceive(canReceiveHandler); // 注册CAN接收中断

  // 初始化电机...
}

// CAN接收中断处理函数
void canReceiveHandler(const CAN_message_t &msg) {
  if (msg.id == 0x100) { // 从臂A关节目标
    targetAngleA = msg.buf[0] | (msg.buf[1] << 8); // 解析角度
    newTarget = true;
  } else if (msg.id == 0x101) { // 从臂B关节目标
    targetAngleB = msg.buf[0] | (msg.buf[1] << 8);
    newTarget = true;
  }
}

void loop() {
  static unsigned long lastTime = 0;
  float dt = (millis() - lastTime) / 1000.0;
  lastTime = millis();

  if (newTarget) {
    // 更新位置环目标 (实际项目中可执行更复杂的轨迹插补)
    jointA.target = targetAngleA;
    jointB.target = targetAngleB;
    newTarget = false;
  }

  // 位置环 (外环) 计算速度指令
  float velCmdA = Kp_pos * (jointA.target - jointA.shaft_angle) 
                  - Kd_pos * jointA.shaft_velocity; // 简化PID
  float velCmdB = Kp_pos * (jointB.target - jointB.shaft_angle) 
                  - Kd_pos * jointB.shaft_velocity;

  // 速度环 (内环) 直接驱动电机 (此处为简化，实际应调用FOC速度控制)
  jointA.move(velCmdA);
  jointB.move(velCmdB);

  // 刷新FOC
  jointA.loopFOC();
  jointB.loopFOC();

  delay(2); // 500Hz控制周期，确保快速响应
}

3、任务级协同 + 动态避障
此案例上升到任务调度层。在双UWB定位和基础同步之上，引入任务优先级、路径占用时间窗和避障逻辑，实现两台机械臂在共享空间中的任务协同与动态避碰。

// 核心结构体：任务和路径段
struct Task { int id; float pickup[2]; float dropoff[2]; int priority; };
struct Segment { float start[2]; float end[2]; float tIn; float tOut; int owner; };

Task currentTasks[2]; // 任务队列
Segment lockedSegments[10]; // 被占用的路径段

void loop() {
  // 1. 主控节点 (上位机) 执行任务分配
  if (isMaster) {
    // 根据优先级分配任务 (高优先级先规划)
    for (auto &task : currentTasks) {
      // 使用A*或类似算法规划路径，避开lockedSegments
      // 将规划出的路径段加入lockedSegments，并广播给从节点
      broadcastPath(task);
    }
  }

  // 2. 从节点执行 (与案例一、二结合)
  if (!isMaster) {
    // 获取自身UWB位置
    getMyPosition();

    // 检测与主臂的距离
    float distToMaster = getDistanceToMaster();
    if (distToMaster < 0.3) { 
      // 避障优先级最高：停止或避让
      performEmergencyStop();
      return;
    }

    // 检查前方路径段是否被占用 (根据时间窗)
    if (isPathOccupied(myPath, lockedSegments)) {
      // 选择等待或重规划
      waitForClearance();
    } else {
      // 正常执行跟随 (调用案例一或二的运动控制函数)
      followMaster();
    }
  }
  delay(50);
}

// 碰撞检测函数 (示例)
bool isPathOccupied(Segment mySeg, Segment locked[], int len) {
  for (int i=0; i<len; i++) {
    // 检查路径段是否相交且时间窗重叠
    if (segmentsIntersect(mySeg, locked[i]) && timeOverlap(mySeg, locked[i])) {
      return true;
    }
  }
  return false;
}

要点解读
双UWB差分定位解决“测距不测向”的短板：单个UWB标签只能得到距离，无法判断目标朝向。通过在两个机械臂上各安装两个UWB标签（或采用双天线方案），计算标签连线的向量，可以直接解算出目标的航向角。这使得跟随机器人具备了“预判”能力，转向时能提前切入跟随曲线，避免机械式追尾。

CAN总线是实现微秒级同步的关键：对于多轴协同，CAN总线的广播特性至关重要。采用“数据预存 + SYNC触发”模式：主节点先广播各轴目标数据，从节点接收后暂存；主节点再广播一帧无数据的SYNC同步帧，所有从节点同时执行预存指令。由于CAN的广播延迟仅在一个位时间（bit time）以内，同步精度可达微秒级。

位置/速度双环级联是保证柔顺跟随的核心：执行层的控制架构直接影响跟随品质。位置环（外环）根据目标与当前角度差，输出速度指令给速度环（内环），由速度环驱动电机。这种级联结构能让电机在接近目标时自动减速，实现“柔顺停止”。关键在于内环（速度环）的响应带宽必须远高于外环（位置环），否则系统会振荡。

避障逻辑拥有硬优先级，独立于任务调度：在双机械臂协同作业的狭窄空间中，安全是第一位的。当超声波或激光雷达检测到碰撞风险时，必须无条件中断当前跟随或任务，执行急停或主动避让。这一判断应在底层实时响应，不依赖上层的任务调度，以确保反应速度。

分布式架构是应对算力瓶颈的工程标准：完整的路径规划（如A*算法）、任务分配和逆运动学解算对Arduino（尤其是8位平台）负担很重。工程上推荐采用分层架构：由树莓派或Jetson Nano等高性能平台作为上位机，负责任务决策、路径规划；Arduino（尤其是ESP32或Teensy等32位平台）作为下位机，专职负责FOC控制、CAN通信和底层安全监控。

4、仓储双AGV主从协同搬运——双UWB差分定位+虚拟弹簧编队
适用场景：工业仓储中，主AGV搬运长货架，从AGV在侧后方辅助支撑，需保持恒定编队间距，同时规避近场障碍，实现“软连接”式协同跟随。

核心逻辑：主AGV按预设路径行驶，从AGV通过双UWB差分定位获取主机相对位置，采用虚拟弹簧模型动态调整跟随速度——间距过大时加速追赶，过近时减速避让，结合BLDC差速驱动实现精准轨迹跟踪，同时嵌入超声波近场避障作为安全冗余。

#include <SimpleFOC.h>
#include <Wire.h>
// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// ==================== 双UWB定位参数 ====================
#define ROBOT_ID 2  // 1=主机, 2=从机
float targetPos[2] = {0, 0};   // 主机位置（通过UWB获取）
float currentPos[2] = {0, 0};  // 从机自身位置
float deltaPos[2] = {0, 0};    // 相对位置
// ==================== 虚拟弹簧跟随参数 ====================
const float DESIRED_DIST_X = -0.8;   // 期望相对X偏移（主车后0.8m）
const float DESIRED_DIST_Y = 0.0;    // 期望相对Y偏移（同车道）
const float SPRING_K = 1.5;          // 弹簧刚度
const float DAMPING_K = 0.3;         // 阻尼系数
const float MAX_SPEED = 1.2;         // 最大跟随速度
// ==================== 超声波近场避障 ====================
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarFront(TRIG_F, ECHO_F, 200);
const float SAFE_DIST = 0.4;  // 前向安全距离(m)
void setup() {
    Serial.begin(115200);
    
    // 初始化BLDC电机
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    
    // 初始化UWB模块（DW1000）
    // uwb_init(ROBOT_ID);
}

// ==================== 主机位置获取（UWB差分定位）====================
void updateUWB() {
    // 实际通过UWB模块读取，此处模拟
    // uwb_get_position(targetPos);
    // uwb_get_self_position(currentPos);
    deltaPos[0] = targetPos[0] - currentPos[0];
    deltaPos[1] = targetPos[1] - currentPos[1];
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. UWB定位更新
    updateUWB();
    
    // 2. 虚拟弹簧力计算
    float dx = deltaPos[0] - DESIRED_DIST_X;
    float dy = deltaPos[1] - DESIRED_DIST_Y;
    float fx = SPRING_K * dx + DAMPING_K * (dx - lastDx) / 0.05;
    float fy = SPRING_K * dy + DAMPING_K * (dy - lastDy) / 0.05;
    lastDx = dx; lastDy = dy;
    
    // 3. 前向超声波避障（硬优先级）
    float frontDist = sonarFront.ping_cm() / 100.0;
    if (frontDist > 0 && frontDist < SAFE_DIST) {
        motorL.move(-0.4); motorR.move(-0.4);
        delay(300);
        return;
    }
    
    // 4. 差速驱动
    float vLin = constrain(sqrt(fx*fx + fy*fy) * 1.2, 0, MAX_SPEED);
    float vAng = constrain(atan2(fy, fx) * 1.2, -0.6, 0.6);
    float wheelBase = 0.3;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);
    
    delay(50);
}

5、多目标动态跟随与任务分配——双UWB差分+ESP-NOW状态共享
适用场景：柔性制造车间中，多台机械臂需动态跟随不同工位的移动物料车，同时根据任务优先级、自身负载状态，自动分配跟随目标，避免资源冲突。

核心逻辑：通过双UWB差分定位获取各移动目标的位姿（位置+航向），借助ESP-NOW低延迟通信实现机械臂状态共享；主控端基于距离、任务优先级、机械臂空闲状态，动态分配跟随目标，机械臂根据目标航向计算期望跟随位置，通过BLDC差速驱动实现精准跟踪。

/* ===== 双机动态跟随 + 协同任务分配 =====
 * 硬件：ESP32 + BLDC电机 + 双UWB模块 + ESP-NOW通信
 * 核心：ESP-NOW共享状态，双UWB获取相对位姿，任务协商分配
 */
#include <SimpleFOC.h>
#include <esp_now.h>

BLDCMotor motorL(7), motorR(7);
// ==================== 机器人状态 ====================
struct RobotState {
    float x, y, heading;      // 位姿
    float vx, vy;             // 当前速度
    bool isBraking;           // 是否刹车
    bool isBusy;              // 是否忙碌
    uint8_t macAddr[6];       // MAC地址
};
RobotState selfState, peerState;
// ==================== 双UWB定位参数 ====================
struct Coordinate { float x, y; };
Coordinate targetTag1 = {3.0, 2.5}, targetTag2 = {3.2, 2.8}; // 目标双标签坐标
float selfX = 0, selfY = 0, selfHeading = 0;
// ==================== 任务分配参数 ====================
const float DESIRED_OFFSET = 0.6;  // 期望跟随距离
bool assignedTask = false;
uint8_t targetId = 0;

// ESP-NOW数据接收回调
void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    RobotState *recvState = (RobotState*)data;
    memcpy(&peerState, recvState, sizeof(RobotState));
}

void setup() {
    Serial.begin(115200);
    // BLDC电机初始化
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    // ESP-NOW初始化
    esp_now_init();
    esp_now_register_recv_cb(OnDataRecv);
    // 配置自身MAC地址与通信参数
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 双UWB差分解算目标位姿
    float targetHeading = atan2(targetTag2.y - targetTag1.y, targetTag2.x - targetTag1.x);
    float targetCenterX = (targetTag1.x + targetTag2.x) / 2.0;
    float targetCenterY = (targetTag1.y + targetTag2.y) / 2.0;
    
    // 2. 任务分配逻辑：若自身空闲且目标未被占用，则分配任务
    if (!selfState.isBusy && !peerState.isBusy) {
        float selfDist = sqrt(pow(targetCenterX - selfX, 2) + pow(targetCenterY - selfY, 2));
        float peerDist = sqrt(pow(targetCenterX - peerState.x, 2) + pow(targetCenterY - peerState.y, 2));
        if (selfDist < peerDist) {
            assignedTask = true;
            selfState.isBusy = true;
        }
    }
    
    // 3. 若分配任务，执行跟随
    if (assignedTask) {
        float desiredX = targetCenterX - DESIRED_OFFSET * cos(targetHeading);
        float desiredY = targetCenterY - DESIRED_OFFSET * sin(targetHeading);
        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;
        
        // 速度控制
        float linearSpeed = constrain(distanceError * 1.2, 0, 1.0);
        float angularSpeed = constrain(angleError * 2.0, -0.6, 0.6);
        float wheelBase = 0.25;
        motorL.move(linearSpeed - angularSpeed * wheelBase / 2);
        motorR.move(linearSpeed + angularSpeed * wheelBase / 2);
        
        // 任务完成判定
        if (distanceError < 0.1) {
            assignedTask = false;
            selfState.isBusy = false;
            motorL.move(0); motorR.move(0);
        }
    }
    
    // 4. 状态广播：发送自身状态给其他机器人
    uint8_t* macAddr = (uint8_t*)malloc(6);
    esp_now_peer_info_t* peer = malloc(sizeof(esp_now_peer_info_t));
    esp_now_get_peer_mac(peer, macAddr);
    esp_now_send(macAddr, (uint8_t*)&selfState, sizeof(RobotState));
    free(macAddr); free(peer);
    
    delay(50);
}

6、刚性双机械臂协同装配——线控转向虚拟链接+位置同步
适用场景：工业装配线上，双机械臂刚性连接同一工件，需实现高精度位置同步，避免因机械装配误差导致“互搏”（一推一拉），确保协同装配的精度与稳定性。

核心逻辑：采用“虚拟弹簧”模型连接主从机械臂，主从电机均设为扭矩控制模式，通过编码器实时采集角度差，根据角度差施加反向纠正力矩，实现柔性同步；同时通过编码器反馈实现位置闭环，确保双机械臂角度严格同步，适配刚性连接的装配场景。

#include <SimpleFOC.h>
#include <PciManager.h>
#include <PciListenerImp.h>
// ==================== 主电机（带编码器）====================
BLDCMotor motorMaster(7);
BLDCDriver3PWM driverMaster(9, 10, 11, 8);
Encoder encoderMaster(18, 19, 2048, 20); // A,B,PPR,Index
void doMA(){ encoderMaster.handleA(); }
void doMB(){ encoderMaster.handleB(); }
void doMI(){ encoderMaster.handleIndex(); }

// ==================== 从电机（磁传感器I2C）====================
BLDCMotor motorSlave(7);
BLDCDriver3PWM driverSlave(3, 5, 6, 7);
MagneticSensorI2C sensorSlave(0x36, 12, 0x0E, 4); // AS5600
// ==================== 同步参数 ====================
const float SYNC_GAIN = 5.0;  // 虚拟链接刚度，越大越“刚性”
void setup() {
    Serial.begin(115200);
    
    // ---- 主电机初始化 ----
    encoderMaster.init();
    PciManager.registerListener(&PciListenerImp(encoderMaster.pinA, doMA));
    PciManager.registerListener(&PciListenerImp(encoderMaster.pinB, doMB));
    PciManager.registerListener(&PciListenerImp(encoderMaster.index_pin, doMI));
    motorMaster.linkSensor(&encoderMaster);
    motorMaster.linkDriver(&driverMaster);
    motorMaster.controller = MotionControlType::torque;  // 关键：扭矩模式
    motorMaster.init();
    motorMaster.initFOC();
    
    // ---- 从电机初始化 ----
    sensorSlave.init();
    motorSlave.linkSensor(&sensorSlave);
    motorSlave.linkDriver(&driverSlave);
    motorSlave.controller = MotionControlType::torque;
    motorSlave.init();
    motorSlave.initFOC();
}

void loop() {
    // 1. FOC更新
    motorMaster.loopFOC();
    motorSlave.loopFOC();
    
    // 2. 核心：虚拟链接同步控制
    // 两电机根据角度差施加相反的纠正力矩，维持位置同步
    float angleDiff = motorSlave.shaft_angle - motorMaster.shaft_angle;
    
    motorMaster.move( SYNC_GAIN * angleDiff);  // 主电机追赶从机
    motorSlave.move( -SYNC_GAIN * angleDiff);  // 从电机追赶主机
    
    // 3. 调试输出
    Serial.print("Master:"); Serial.print(motorMaster.shaft_angle);
    Serial.print(" Slave:"); Serial.print(motorSlave.shaft_angle);
    Serial.print(" Diff:"); Serial.println(angleDiff);
    
    delay(10);
}

要点解读

1. 双UWB差分定位：从“位置感知”到“位姿感知”的核心突破
   双UWB的核心价值并非仅获取目标坐标，而是通过两个标签的几何关系解算目标航向，实现从单一位置感知到“位置+姿态”的全维度位姿感知。案例1和案例2均通过atan2(tag2.y-tag1.y, tag2.x-tag1.x)直接计算目标航向，使跟随机械臂能预判目标运动趋势，避免机械跟随的滞后性，这是单标签定位无法实现的，也是双机械臂精准协同的基础。

2. 控制算法与运动学约束的深度耦合：避免“理想算法落地失效”
   工业机械臂多为差速驱动，存在非完整约束，直接套用理想控制算法会导致轨迹畸变。案例中均引入运动学解算：将全局目标位置误差通过atan2转换为角度误差，再结合差速公式分配左右BLDC电机转速，同时在控制参数中嵌入轮距、最大速度等机械约束，确保算法输出符合机械臂物理运动规律，避免过冲、打滑或原地打转。

3. 多源数据融合与抗干扰设计：保障复杂工业环境的稳定性
   工业场景存在电磁干扰、信号遮挡、传感器噪声等干扰，需通过多源融合与抗干扰设计保障系统稳定。一是电源隔离，UWB模块与BLDC驱动电源完全独立，避免电机PWM噪声干扰UWB射频信号；二是数据滤波，对UWB定位数据采用卡尔曼滤波或滑动平均，抑制跳点；三是多源互补，案例1引入超声波避障作为近场冗余，案例2通过ESP-NOW实现状态共享，弥补单一定位的不足，确保复杂环境下系统不失效。

4. 主从控制架构的柔性设计：解决刚性协同的“互搏”痛点
   双机械臂刚性连接时，若均采用速度闭环控制，易因机械装配误差导致“互搏”。案例3采用“主从扭矩控制+虚拟弹簧”架构，主从电机均设为扭矩模式，通过角度差施加反向纠正力矩，本质是构建柔性虚拟连接，既消除位置误差，又避免刚性耦合下的电流激增，同时通过编码器实时反馈实现高精度位置同步，适配工业装配的刚性协同需求。

5. 算力与实时性的工程化平衡：适配Arduino平台的落地关键
   双UWB解算、协同算法、FOC控制对算力要求高，标准Arduino Uno难以胜任，需合理平衡算力与实时性。一是硬件选型，优先采用ESP32、Teensy等高性能MCU，支持浮点运算与高频控制；二是任务分层，将UWB解算、任务分配等复杂逻辑放在上位机或高性能MCU，Arduino仅负责BLDC底层FOC执行；三是控制周期稳定，严禁在主循环使用阻塞函数，确保控制周期固定，避免因算力不足导致控制滞后或死机，保障系统实时性与稳定性。

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