该方案的核心特点是利用分布式多智能体架构实现全局最优的动态任务分配，结合BLDC电机配合FOC算法的高动态响应与双通道自适应阻抗控制，使双机器人能够在通道约束下实现柔顺协同与自适应跟随；主要适用于狭窄空间物流搬运、人机协同装配、科研验证等场景；实际部署需重点解决Arduino算力瓶颈、高频控制环实时性、通道几何约束建模及双臂力矩耦合等问题。

一、主要特点
动态跟随与协同任务分配——从“静态指派”到“全局最优调度”
传统多机器人系统常采用静态任务分配，一旦环境或任务发生变化，系统难以快速响应。动态跟随与协同任务分配引入了分布式多智能体控制架构。
全局最优调度：系统通过分布式消息传递机制，实时获取各机器人的状态（如位置、剩余电量、当前负载）。当新的跟随任务出现时，算法会综合评估距离、负载等维度，动态指派最优的机器人执行任务，实现全局效率最大化。
激活扩散机制：在复杂的层级任务中，机器人通过局部信息交互，利用激活扩散机制自主识别并认领下一步子任务，无需中心节点干预，极大提升了系统的鲁棒性和可扩展性。
多源异构融合：在跟随过程中，系统融合UWB、IMU与轮式里程计数据，通过扩展卡尔曼滤波（EKF）进行紧耦合，确保在信号短暂丢失时依靠惯性航位推算维持跟随连续性。
通道约束自适应——从“盲目避障”到“几何空间顺应”
在狭窄通道或复杂管道中移动时，机器人不仅要避开障碍物，还要顺应通道的几何形状。
几何约束建模：系统通过激光雷达或深度相机实时构建通道的局部几何模型，将通道约束转化为机器人运动学模型中的不等式约束。
自适应轨迹规划：在满足通道约束的前提下，规划层实时调整机器人的期望轨迹，确保机器人在通过狭窄区域时，机身姿态与通道走向保持一致，避免碰撞或卡死。
动态拓扑重构：当通道形状发生突变（如急转弯、变径）时，控制算法能自动更新邻接关系，重构控制策略，实现弹性自适应。
BLDC+FOC双通道自适应阻抗控制——从“刚性执行”到“柔顺协同”
双机器人协同搬运大型工件时，工件与机器人之间、机器人与环境之间存在复杂的力交互。
双通道阻抗架构：系统引入双通道自适应阻抗控制算法，独立调节内部力（工件与机器人之间的夹持力）和外部力（机器人与环境之间的交互力）。通过在线自适应更新虚拟刚度和阻尼增益，确保在工件刚度变化或接触条件改变时，系统仍能保持稳定的力跟踪性能。
力矩解耦与精准输出：结合FOC（磁场定向控制）算法，BLDC电机通过Clark和Park变换，将三相电流解耦为励磁电流（d轴）和转矩电流（q轴），实现转矩脉动<1%的平稳输出。这使得机器人能够在零速状态下输出额定扭矩，且响应速度极快，为上层柔顺控制提供了完美的执行基础。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC的高频电流环（通常>1kHz），能够毫秒级响应协同任务中的力矩变化，确保双机器人在搬运过程中“步调一致”。
分层控制架构
规划层（任务分配+轨迹规划）：负责全局任务调度与通道约束下的轨迹生成。
控制层（双通道阻抗+FOC）：负责将轨迹转化为关节力矩指令，并处理力交互与协同同步。
感知层（UWB+LiDAR+力矩传感器）：负责提供目标位置、环境几何信息与力矩反馈。

二、典型应用场景
狭窄空间物流搬运
在仓库货架间、飞机机翼内部或船舶舱室等狭窄通道中，双机器人协同搬运超长或超宽货物。
通道自适应：机器人能根据通道宽度自动调整机身姿态和跟随间距，确保货物平稳通过。
协同搬运：双机器人通过位置同步与力矩协同，确保货物在搬运过程中保持水平，避免倾斜或掉落。
人机协同装配
在汽车装配线或电子元件插件场景中，双机器人作为“智能助手”跟随工人移动。
动态任务分配：系统根据工人的操作位置和任务需求，动态指派最近的机器人递送工具或零件。
柔顺交互：双通道阻抗控制使机器人在与工人或工件接触时具备柔顺性，避免刚性碰撞，保障人员安全。
管道巡检与维护
在石油管道、城市下水道等封闭通道中，双机器人协同携带检测设备进行巡检。
几何顺应：机器人能自适应管道的弯曲和变径，保持稳定的运动姿态。
协同作业：一台机器人负责照明与探测，另一台负责清理或修复，通过协同任务分配实现高效作业。
科研与教育验证
作为高校机器人学、多智能体系统课程的实验平台，用于验证分布式任务分配、阻抗控制、通道约束规划等前沿技术。

三、需要注意的事项
Arduino算力瓶颈与架构分工
动态任务分配、通道约束规划、双通道阻抗控制及FOC算法同时运行对算力要求极高。
平台选型：经典8位Arduino（如Uno）几乎无法胜任。必须选用ESP32-S3（双核240MHz）、STM32H7或Arduino Portenta H7等高性能平台，或采用“上位机+下位机”架构。
主从架构：建议采用“上位机+下位机”架构。上位机（如树莓派、Jetson或PC）负责任务分配、轨迹规划和通道约束解算；下位机（如专用FOC驱动板）负责高频电流环控制和电机换相。
控制频率与实时性
双机器人协同要求极高的实时性，尤其是力矩同步与通道约束响应。
高频控制环：FOC的电流环频率建议≥1kHz，速度环和位置环≥500Hz。双通道阻抗控制的更新频率应与控制环匹配（如100Hz~500Hz），避免力矩指令滞后。
通信延迟：上位机与下位机之间的通信（如CAN、EtherCAT）延迟必须控制在毫秒级，否则会导致力矩指令滞后，引发机身抖动或碰撞。
通道几何约束建模与避障
通道约束自适应的准确性高度依赖环境感知的精度。
多源融合：建议融合激光雷达、深度相机与超声波传感器，构建高精度的局部几何模型，提高通道约束建模的鲁棒性。
延迟补偿：传感器采样、滤波、通信都会引入延迟。需在算法中加入延迟补偿机制，预测通道变化趋势，提前调整运动轨迹。
双臂力矩耦合与协同同步
双机器人在协同搬运时，存在复杂的力矩耦合关系，且容易发生自碰撞或与环境碰撞。
力矩解耦：需在控制算法中引入力矩解耦机制，消除双臂之间的相互干扰，确保各臂力矩输出独立可控。
碰撞检测：需在算法中加入实时碰撞检测机制，结合力矩传感器或电流反馈，当检测到异常阻力时立即触发紧急停止。
电机力矩响应与一致性
协同任务的效果高度依赖电机的力矩响应性能和一致性。
电机选型：选用低齿槽效应、高扭矩密度的BLDC电机，确保低速下的力矩平稳性。
参数一致性：同一机器人上的所有电机参数（极对数、内阻、电感）必须一致，否则会导致各关节力矩输出不均，影响协同精度。
电磁兼容（EMC）与电源管理
多电机高频PWM驱动会产生严重的电磁干扰，影响传感器精度。
电源隔离：动力电源与逻辑电源必须物理隔离，使用独立DC-DC模块。
滤波设计：电源入口并联大容量电解电容和高频陶瓷电容，吸收电压尖峰。
布线规范：强电与弱电严格分开，传感器信号线使用屏蔽线。

1、双UWB差分定位 + 基础动态跟随
此案例聚焦感知层与执行层的闭环。通过双UWB标签差分定位获取主机器人的位置与航向，驱动从机器人进行顺滑的动态跟随，解决了单标签“只能测距、无法定向”的核心痛点。

#include <SimpleFOC.h>
#include <DW1000.h> // UWB库，需适配具体模块

// 假设已定义主从机器人BLDC差速底盘电机
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 8);

// UWB标签ID定义
const uint8_t TAG_LEADER_1 = 0x01; // 主机器人前置标签
const uint8_t TAG_LEADER_2 = 0x02; // 主机器人后置标签
const uint8_t TAG_FOLLOWER = 0x03; // 从机器人标签

// 双标签差分定位与跟随参数
float leaderPos[2] = {0, 0};    // 主机器人位置 (x, y)
float leaderYaw = 0;            // 主机器人航向角 (rad)
float followerPos[2] = {0, 0};  // 从机器人当前位置
float followOffset[2] = {-0.5, 0.3}; // 期望跟随偏移 (左后方0.5m, 侧方0.3m)
float followError[2] = {0, 0};
float maxSpeed = 1.5; // 最大线速度 (m/s)

void setup() {
  Serial.begin(115200);
  // 初始化UWB模块 (具体初始化代码依库而定)
  DW1000.begin();
  
  // 初始化BLDC电机及FOC
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  // 1. 双标签差分定位：获取主机器人位置与航向
  float pos1[2], pos2[2];
  if (DW1000.getPosition(TAG_LEADER_1, pos1) && DW1000.getPosition(TAG_LEADER_2, pos2)) {
    // 主机器人位置取两标签中点
    leaderPos[0] = (pos1[0] + pos2[0]) / 2.0;
    leaderPos[1] = (pos1[1] + pos2[1]) / 2.0;
    // 解算航向角 (两标签连线与X轴夹角)
    leaderYaw = atan2(pos2[1] - pos1[1], pos2[0] - pos1[0]);
  }
  
  // 2. 获取从机器人自身位置
  DW1000.getPosition(TAG_FOLLOWER, followerPos);

  // 3. 计算带航向补偿的跟随目标 (纯追踪算法思想)
  // 目标位置 = 主机器人位置 + 航向旋转后的跟随偏移
  float targetPos[2];
  targetPos[0] = leaderPos[0] + followOffset[0] * cos(leaderYaw) - followOffset[1] * sin(leaderYaw);
  targetPos[1] = leaderPos[1] + followOffset[0] * sin(leaderYaw) + followOffset[1] * cos(leaderYaw);

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

  // 5. 速度控制 (比例控制 + 航向修正)
  float speedMag = constrain(distError * 0.8, 0, maxSpeed);
  float targetAngle = atan2(followError[1], followError[0]);
  
  // 差速底盘运动学分解 (v线速度, w角速度)
  float v = speedMag * cos(targetAngle - leaderYaw); // 沿主机器人航向分解
  float w = 0.5 * atan2(sin(targetAngle - leaderYaw), cos(targetAngle - leaderYaw)); // 转向
  
  // 6. 驱动BLDC电机 (速度模式)
  float leftV = (v - w * 0.3); // 0.3为轮距一半
  float rightV = (v + w * 0.3);
  motorL.target = constrain(leftV, -maxSpeed, maxSpeed);
  motorR.target = constrain(rightV, -maxSpeed, maxSpeed);
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();

  delay(50); // 20Hz控制周期
}

要点：通过双标签的基线向量解算航向，从机器人可以“预判”主机器人转向趋势，实现顺滑曲线跟随而非机械追尾。

2、动态跟随 + 协同任务分配
此案例在跟随基础上引入决策层。通过UWB识别多个目标并进行动态任务分配，当新任务产生时，系统根据距离与负载状态，智能指派最优机器人执行。

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

// ... (BLDC电机及UWB初始化同案例一) ...

// 任务结构体
struct Task {
  uint8_t targetID;   // UWB标签ID (代表目标点或工人)
  float pos[2];       // 目标位置 (可由UWB实时获取)
  int priority;       // 任务优先级 (0最高)
  bool assigned;      // 是否已分配
};

// 机器人状态结构体
struct RobotState {
  uint8_t id;
  float pos[2];
  float battery;      // 电量 (0~1)
  bool isBusy;        // 是否忙碌
  Task currentTask;
};

RobotState robot1, robot2; // 双机器人状态
Task taskQueue[5];    // 任务队列 (最多5个)

void setup() {
  // ... (同上) ...
  // 初始化各机器人状态
  robot1.id = 1; robot1.isBusy = false;
  robot2.id = 2; robot2.isBusy = false;
}

void loop() {
  // 1. 更新双机器人位置 (UWB)
  DW1000.getPosition(TAG_ROBOT1, robot1.pos);
  DW1000.getPosition(TAG_ROBOT2, robot2.pos);

  // 2. 扫描任务队列，进行动态分配
  for (int i = 0; i < 5; i++) {
    if (!taskQueue[i].assigned) {
      // 获取目标位置 (假设目标也佩戴UWB标签)
      DW1000.getPosition(taskQueue[i].targetID, taskQueue[i].pos);
      
      // 计算各机器人与目标距离
      float dist1 = calcDistance(robot1.pos, taskQueue[i].pos);
      float dist2 = calcDistance(robot2.pos, taskQueue[i].pos);
      
      // 评分函数：综合考虑距离与电量 (距离近、电量高者优先)
      float score1 = 0.6 * (1 - dist1/10.0) + 0.4 * robot1.battery; // 假设最大距离10m
      float score2 = 0.6 * (1 - dist2/10.0) + 0.4 * robot2.battery;
      
      // 选择得分更高且空闲的机器人分配任务
      if (score1 > score2 && !robot1.isBusy) {
        assignTask(&robot1, &taskQueue[i]);
      } else if (!robot2.isBusy) {
        assignTask(&robot2, &taskQueue[i]);
      }
    }
  }

  // 3. 执行各自任务 (跟随或前往目标)
  if (robot1.isBusy) {
    followTarget(robot1.pos, robot1.currentTask.pos); // 调用案例一的跟随逻辑
  }
  if (robot2.isBusy) {
    followTarget(robot2.pos, robot2.currentTask.pos);
  }

  delay(50);
}

// 任务分配函数
void assignTask(RobotState* robot, Task* task) {
  robot->currentTask = *task;
  robot->isBusy = true;
  task->assigned = true;
  Serial.print("Robot "); Serial.print(robot->id);
  Serial.println(" assigned to task.");
}

要点：任务分配的关键是设计合理的评分函数，综合距离、电量和优先级等维度动态决策，实现“人尽其才”的柔性调度。

3、通道约束自适应编队压缩
此案例引入环境感知层，解决在狭窄通道中保持队形的自适应问题。当检测到通道变窄，自动压缩双机器人编队间距并微调航向，确保协同通过。

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

// ... (BLDC电机及UWB初始化同案例一) ...

// 通道与编队参数
#define NORMAL_SPACING 1.0    // 正常编队间距 (m)
#define MIN_SPACING 0.4       // 最小压缩间距 (m)
#define CHANNEL_WIDTH 1.5     // 通道宽度阈值 (m)
#define SAFE_SIDE_DIST 0.2    // 侧方安全距离 (m)

// 红外传感器引脚 (用于检测通道宽度)
#define IR_LEFT_PIN A0
#define IR_RIGHT_PIN A1

float leaderYaw = 0;
float currentSpacing = NORMAL_SPACING;
bool inNarrowChannel = false;

void setup() {
  // ... (同上) ...
  pinMode(IR_LEFT_PIN, INPUT);
  pinMode(IR_RIGHT_PIN, INPUT);
}

void loop() {
  // 1. 检测通道宽度 (红外测距)
  float leftDist = analogRead(IR_LEFT_PIN) * 0.5;  // 简化映射
  float rightDist = analogRead(IR_RIGHT_PIN) * 0.5;
  float actualWidth = leftDist + rightDist;

  // 2. 通道约束自适应逻辑
  if (actualWidth < CHANNEL_WIDTH && actualWidth > 0) {
    inNarrowChannel = true;
    // 编队间距自适应压缩 (线性映射)
    float compressRatio = actualWidth / CHANNEL_WIDTH;
    currentSpacing = NORMAL_SPACING * compressRatio;
    currentSpacing = constrain(currentSpacing, MIN_SPACING, NORMAL_SPACING);
    
    // 微调航向角，使机器人居中通过通道
    float centerBias = (leftDist - rightDist) / actualWidth;
    float yawAdjust = constrain(centerBias * 15.0, -10.0, 10.0) * DEG_TO_RAD;
    leaderYaw += yawAdjust * 0.1; // 平滑调整
    
    // 压缩状态下减速通过
    float speedScale = 0.5 + 0.5 * compressRatio;
    // ... 将speedScale应用到电机的目标速度上 ...
    
    Serial.println("Narrow channel: spacing=" + String(currentSpacing) + ", yawAdjust=" + String(yawAdjust));
  } else {
    inNarrowChannel = false;
    currentSpacing = NORMAL_SPACING;
  }

  // 3. 将压缩后的编队间距应用到跟随算法 (参考案例一)
  // followOffset[0] 应根据 currentSpacing 动态调整
  followOffset[0] = -currentSpacing; // 跟随距离

  // 4. 执行跟随 (同案例一)
  // ... 调用followTarget() ...

  delay(50);
}

要点：通道约束自适应的本质是将环境感知（通道宽度）引入编队参数（间距、航向）的动态调节，使双机在非结构化环境中保持协同能力。

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

任务分配是“柔性与效率”的博弈：动态任务分配的核心是设计合理的评分函数（如距离权重+电量权重+任务优先级）。在双机协同场景中，可配合“任务窃取”（Work Stealing）机制——当一台机器人任务受阻时，另一台可主动接管，确保任务链条不中断。

通道约束自适应是编队生存的关键：在狭小通道中，机器人无法保持理想编队间距。通过红外或激光雷达检测通道宽度，动态压缩间距并微调航向，可使双机“挤过去”而非“卡住”。这是从仿真走向真实场景必须解决的问题。

避障拥有硬优先级，独立于跟随与任务：无论处于跟随还是任务执行状态，当传感器检测到前方障碍物时，必须无条件中断当前动作，执行急停或避让。这一判断应在底层实时响应，不依赖上层决策，确保安全底线。

分布式架构是应对算力瓶颈的工程标准：双UWB差分定位的非线性解算、多任务动态分配等对算力要求较高。标准Arduino Uno极易因内存溢出或浮点运算过载而崩溃。强烈建议采用ESP32、Teensy 4.1等高算力主控，或采用“上位机解算与调度 + Arduino底层BLDC控制”的分布式架构。

4、狭窄通道双AGV协同跟随——UWB差分定位+通道边界约束自适应
适用场景：工业仓储窄通道（宽度1.5m）中，双AGV需沿通道中心线跟随主AGV，同时通过UWB差分定位感知通道边界，动态调整间距与侧向位置，避免因通道狭窄导致的碰撞或跟随失效。

核心逻辑：从AGV通过双UWB标签获取主AGV的相对位置，结合双UWB基站实现通道边界差分定位；采用“虚拟弹簧+通道边界约束”模型，主AGV正常行驶，从AGV根据通道宽度自适应调整侧向位置，同时保持安全跟随间距，在通道变窄时自动减速并拉大间距，确保通行安全。

/* ===== 狭窄通道双AGV协同跟随 + 通道约束自适应 =====
 * 硬件：ESP32 + BLDC差速驱动 + 双UWB（主机+从机标签） + 3个通道边界UWB基站
 * 核心：UWB差分定位（相对位置+边界位置）+ 虚拟弹簧+通道宽度约束
 */
#include <SimpleFOC.h>

// ==================== 硬件初始化 ====================
BLDCMotor motorL(7), motorR(7);          // 左/右BLDC电机
BLDCDriver3PWM driverL(9,10,11,8);       // 左驱动（PWM口+使能口）
BLDCDriver3PWM driverR(3,5,6,7);         // 右驱动
Encoder encoderL(18,19,2048), encoderR(20,21,2048);  // 编码器

// ==================== 双UWB与通道参数 ====================
float masterPos[2] = {0,0};          // 主机坐标（x,y，单位m）
float slavePos[2] = {0,0};           // 从机自身坐标
float channelLeft = 0, channelRight = 0; // 通道左右边界（单位m）
float masterVx = 0, masterVy = 0;    // 主机速度（m/s）
const float DESIRED_DIST = 0.6;      // 期望跟随距离（m）
const float CHANNEL_MIN = 1.5;       // 通道最小宽度（m）
const float SPRING_K = 1.2;          // 跟随弹簧刚度
const float BOUND_K = 0.8;           // 边界约束力系数
const float SAFE_MARGIN = 0.15;      // 通道边界安全余量（m）
float lastTime = 0;                  // 时间戳（用于速度计算）

void setup() {
    Serial.begin(115200);
    // 初始化电机与编码器
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;  // 速度控制模式
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    // 初始化UWB模块（实际调用硬件驱动）
    // uwb_init(MASTER_TAG, SLAVE_TAG);  // 初始化双UWB标签
    // uwb_anchors_init(3);               // 初始化3个通道边界基站
}

// ==================== UWB差分定位更新 ====================
void updateUWB() {
    // 实际调用UWB驱动，此处模拟主机和边界数据
    // masterPos = uwb_get_target_position();  // 主机位置
    // slavePos = uwb_get_self_position();     // 自身位置
    // channelLeft = uwb_get_anchor_x(0) - CHANNEL_MIN/2;  // 左边界
    // channelRight = uwb_get_anchor_x(1) + CHANNEL_MIN/2; // 右边界
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 1. UWB定位更新
    updateUWB();
    
    // 2. 计算跟随误差与通道约束力
    float dx = masterPos[0] - slavePos[0];  // X向误差（纵向）
    float dy = masterPos[1] - slavePos[1];  // Y向误差（侧向）
    float dist = sqrt(dx*dx + dy*dy);
    
    // 纵向跟随力（虚拟弹簧）
    float followForceX = SPRING_K * (dist - DESIRED_DIST) * (dx/dist);
    // 侧向对齐力
    float alignForceY = SPRING_K * dy * 0.5;  // 侧向力减半，优先保证纵向跟随
    
    // 通道边界约束（自适应调整侧向位置）
    float boundForceY = 0;
    float safeLeft = channelLeft + SAFE_MARGIN;
    float safeRight = channelRight - SAFE_MARGIN;
    
    if (slavePos[1] < safeLeft) {
        boundForceY = BOUND_K * (safeLeft - slavePos[1]);  // 推回左边界内
    } else if (slavePos[1] > safeRight) {
        boundForceY = -BOUND_K * (slavePos[1] - safeRight); // 推回右边界内
    }
    
    // 3. 通道宽度自适应（宽度不足时减速）
    float channelWidth = channelRight - channelLeft;
    float speedFactor = map(channelWidth, CHANNEL_MIN, 2.0, 0.5, 1.0);
    speedFactor = constrain(speedFactor, 0.3, 1.0);  // 限制减速幅度
    
    // 4. 运动学解算（差速驱动转换）
    float vLin = followForceX * speedFactor;  // 线速度（受通道宽度约束）
    float vAng = (alignForceY + boundForceY) * 0.8;  // 角速度（侧向力控制转向）
    float wheelBase = 0.25;  // 轮距（m）
    float leftVel = vLin - vAng * wheelBase / 2;
    float rightVel = vLin + vAng * wheelBase / 2;
    
    // 限制最大速度，避免通道内急加速
    leftVel = constrain(leftVel, -0.8, 0.8);
    rightVel = constrain(rightVel, -0.8, 0.8);
    
    // 5. 电机执行
    motorL.move(leftVel);
    motorR.move(rightVel);
    
    // 6. 状态输出（调试）
    Serial.print("Dist:"); Serial.print(dist);
    Serial.print(" Channel:"); Serial.print(channelWidth);
    Serial.print(" Speed:"); Serial.print(vLin);
    Serial.println();
    
    delay(50);
}

5、移动目标动态跟随+任务优先级分配——UWB+ESP-NOW状态共享+任务冲突消解
适用场景：车间移动物料车（携带双UWB标签）需双机器人跟随搬运，且物料车会携带不同优先级任务（紧急/普通）。双机器人需实时获取物料车状态，根据任务优先级和自身负载自动分配跟随目标，避免任务冲突，同时动态跟随目标移动。

核心逻辑：物料车通过双UWB标签广播自身位置与任务优先级，双机器人通过ESP-NOW低延迟通信共享自身状态（位置、负载、剩余电量）；主控算法基于“任务优先级>距离>电量”的规则动态分配跟随目标，机器人接收目标后，通过UWB差分定位实现精准跟随，同时通过“任务队列”处理优先级突变（如紧急任务插入时，空闲机器人立即切换目标）。

/* ===== 双机器人动态跟随 + 协同任务分配 =====
 * 硬件：2台ESP32（双机器人）+ BLDC驱动 + 双UWB标签（物料车）+ ESP-NOW通信
 * 核心：ESP-NOW共享状态 + 任务优先级动态分配 + UWB精准跟随
 */
#include <SimpleFOC.h>
#include <esp_now.h>

// ==================== 任务与状态结构体 ====================
struct RobotState {
    uint8_t id;               // 机器人ID（1/2）
    float x, y;               // 当前位置（m）
    bool isIdle;              // 是否空闲
    float battery;            // 剩余电量（%）
    uint8_t macAddr[6];       // MAC地址（用于ESP-NOW）
};

struct TargetState {
    uint8_t taskId;           // 任务ID
    uint8_t priority;         // 优先级（1=紧急，2=普通）
    float x, y;               // 目标位置（m）
    float vx, vy;             // 目标速度（m/s）
};

RobotState selfState;          // 自身状态
RobotState peerState;          // 对端机器人状态
TargetState currentTarget;     // 当前分配的任务目标

// ==================== 硬件初始化 ====================
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);

// ==================== 参数配置 ====================
const float DESIRED_OFFSET = 0.5;  // 跟随偏移量（m）
const float MAX_FOLLOW_SPEED = 0.6; // 最大跟随速度（m/s）
const uint8_t PRIORITY_EMERGENCY = 1;
const uint8_t PRIORITY_NORMAL = 2;

// ==================== ESP-NOW回调：接收对端状态 ====================
void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    RobotState *recv = (RobotState*)data;
    if (recv->id != selfState.id) {
        memcpy(&peerState, recv, sizeof(RobotState));
    }
}

// ==================== 任务分配逻辑 ====================
void taskAllocation(TargetState *newTarget) {
    // 优先级规则：紧急任务>普通任务；空闲优先；距离近优先；电量高优先
    if (newTarget->priority == PRIORITY_EMERGENCY) {
        // 紧急任务：若自身空闲，立即分配
        if (selfState.isIdle) {
            currentTarget = *newTarget;
            selfState.isIdle = false;
            return;
        }
        // 自身忙碌，尝试抢占对端（对端空闲时）
        if (peerState.isIdle && selfState.battery > peerState.battery) {
            currentTarget = *newTarget;
            peerState.isIdle = false;  // 通知对端让出任务
            // ESP-NOW发送对端状态更新
        }
    } else {
        // 普通任务：空闲机器人中，距离近的优先
        float selfDist = sqrt(pow(newTarget->x - selfState.x,2) + pow(newTarget->y - selfState.y,2));
        float peerDist = sqrt(pow(newTarget->x - peerState.x,2) + pow(newTarget->y - peerState.y,2));
        // 自身空闲且距离更近，分配
        if (selfState.isIdle && selfDist < peerDist) {
            currentTarget = *newTarget;
            selfState.isIdle = false;
        }
    }
}

void setup() {
    Serial.begin(115200);
    // 初始化电机
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    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地址（实际需获取硬件MAC）
    esp_now_peer_info_t peer;
    memcpy(peer.peer_addr, peerState.macAddr, 6);
    esp_now_add_peer(&peer);
    
    // 初始化自身状态
    selfState.id = 1;  // 假设机器人ID为1
    selfState.isIdle = true;
    selfState.battery = 90.0;
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 1. 接收目标状态（UWB读取物料车信息，此处模拟）
    TargetState newTarget;
    newTarget.taskId = 1;
    newTarget.priority = PRIORITY_NORMAL;  // 示例：普通任务
    newTarget.x = 3.5; newTarget.y = 2.0;
    newTarget.vx = 0.3; newTarget.vy = 0.1;
    
    // 2. 任务分配
    taskAllocation(&newTarget);
    
    // 3. 若已分配目标，执行跟随
    if (!selfState.isIdle) {
        // 计算期望跟随位置（目标后方偏移）
        float targetAngle = atan2(currentTarget.vy, currentTarget.vx);  // 目标航向角
        float desiredX = currentTarget.x - DESIRED_OFFSET * cos(targetAngle);
        float desiredY = currentTarget.y - DESIRED_OFFSET * sin(targetAngle);
        
        // 跟随误差计算
        float errorX = desiredX - selfState.x;
        float errorY = desiredY - selfState.y;
        float distError = sqrt(errorX*errorX + errorY*errorY);
        
        // 速度控制：误差越小，速度越慢（接近目标时减速）
        float linearSpeed = map(distError, 0, 2.0, 0, MAX_FOLLOW_SPEED);
        linearSpeed = constrain(linearSpeed, 0, MAX_FOLLOW_SPEED);
        
        // 航向对齐（调整机器人朝向）
        float selfAngle = atan2(errorY, errorX);  // 期望航向
        float angleError = selfAngle - targetAngle;  // 与目标航向的误差
        if (angleError > PI) angleError -= 2*PI;
        if (angleError < -PI) angleError += 2*PI;
        float angularSpeed = angleError * 1.5;  // 航向调整角速度
        
        // 差速驱动解算
        float wheelBase = 0.25;
        float leftVel = linearSpeed - angularSpeed * wheelBase/2;
        float rightVel = linearSpeed + angularSpeed * wheelBase/2;
        
        motorL.move(leftVel);
        motorR.move(rightVel);
        
        // 4. 任务完成判定：距离目标足够近
        if (distError < 0.1) {
            selfState.isIdle = true;
            motorL.move(0); motorR.move(0);
            Serial.println("Task Completed!");
        }
    }
    
    // 5. 发送自身状态给对端（ESP-NOW广播）
    esp_now_send(NULL, (uint8_t*)&selfState, sizeof(RobotState));
    
    delay(50);
}

6、通道约束自适应避障与协同跟随——UWB+激光测距+动态约束调整
适用场景：工业产线中，双机器人需沿狭窄通道（带移动障碍物，如工人、临时物料架）跟随主机器人，需实时感知障碍物位置，动态调整通道约束边界，同时保持协同跟随，避免碰撞。

核心逻辑：从机器人通过双UWB定位主机器人，通过激光测距模块实时感知通道内移动障碍物，动态调整约束边界；采用“约束边界+动态避障”算法，当障碍物靠近时，缩小约束安全区域，同时通过路径规划调整跟随轨迹，实现“跟随+避障+通道约束”的三重目标。

/* ===== 通道约束自适应避障 + 协同跟随 =====
 * 硬件：ESP32 + BLDC + 双UWB + 激光测距模块（如TF-Luna）
 * 核心：UWB定位 + 激光动态避障 + 通道约束自适应调整
 */
#include <SimpleFOC.h>
#include <TFLIRArduino.h>  // 激光测距库（示例）

// ==================== 硬件初始化 ====================
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);

// 激光测距（以TF-Luna为例，I2C接口）
TFLIRArduino tfl;

// ==================== 状态与参数 ====================
float masterPos[2] = {0,0};          // 主机器人位置
float selfPos[2] = {0,0};            // 自身位置
float channelBoundLeft = 0, channelBoundRight = 0; // 静态通道边界
float obstacleDist = 1.0;             // 激光测距的障碍物距离（m）
float dynamicBoundLeft, dynamicBoundRight; // 动态约束边界
const float SAFE_MARGIN = 0.2;        // 静态边界安全余量
const float OBSTACLE_MARGIN = 0.15;   // 障碍物安全余量
const float MAX_AVOID_SPEED = 0.4;    // 避障时最大速度（m/s）

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    // 激光初始化
    tfl.init();
    
    // 静态通道边界（初始值，实际由UWB基站获取）
    channelBoundLeft = 0.25;
    channelBoundRight = 1.25;
    dynamicBoundLeft = channelBoundLeft + SAFE_MARGIN;
    dynamicBoundRight = channelBoundRight - SAFE_MARGIN;
}

// ==================== UWB定位更新 ====================
void updateUWB() {
    // 模拟UWB数据获取
    // masterPos = uwb_get_target();
    // selfPos = uwb_get_self();
}

// ==================== 激光动态避障与约束调整 ====================
void updateDynamicBound() {
    // 读取障碍物距离（假设激光安装在正前方，距离单位m）
    obstacleDist = tfl.readDistance() / 100.0;  // 转换为m
    
    // 障碍物靠近时，动态调整约束边界（向中间收缩）
    if (obstacleDist < 0.8) {
        // 障碍物靠近，收缩边界（收缩量与障碍物距离成反比）
        float shrink = map(obstacleDist, 0.8, 0.3, 0.0, 0.1);
        shrink = constrain(shrink, 0, 0.1);
        dynamicBoundLeft = channelBoundLeft + SAFE_MARGIN + shrink;
        dynamicBoundRight = channelBoundRight - SAFE_MARGIN - shrink;
    } else {
        // 无障碍物，恢复静态安全边界
        dynamicBoundLeft = channelBoundLeft + SAFE_MARGIN;
        dynamicBoundRight = channelBoundRight - SAFE_MARGIN;
    }
    
    // 确保边界不越界
    dynamicBoundLeft = max(dynamicBoundLeft, channelBoundLeft);
    dynamicBoundRight = min(dynamicBoundRight, channelBoundRight);
}

// ==================== 跟随与避障控制 ====================
void followAndAvoid() {
    // 1. UWB定位更新
    updateUWB();
    
    // 2. 动态边界更新
    updateDynamicBound();
    
    // 3. 计算跟随误差
    float dx = masterPos[0] - selfPos[0];
    float dy = masterPos[1] - selfPos[1];
    float distError = sqrt(dx*dx + dy*dy);
    float desiredDist = 0.5;
    
    // 4. 通道约束力（限制在动态边界内）
    float boundForce = 0;
    if (selfPos[1] < dynamicBoundLeft) {
        boundForce = 0.5 * (dynamicBoundLeft - selfPos[1]);
    } else if (selfPos[1] > dynamicBoundRight) {
        boundForce = -0.5 * (selfPos[1] - dynamicBoundRight);
    }
    
    // 5. 避障优先级：障碍物近时，优先避障，其次跟随
    float followSpeed = 0;
    if (obstacleDist < 0.6) {
        // 障碍物较近，减速并调整侧向位置（向通道中央偏移）
        followSpeed = map(obstacleDist, 0.6, 0.3, 0.2, 0);
        followSpeed = constrain(followSpeed, 0, MAX_AVOID_SPEED);
        // 侧向力：推向通道中央
        float mid = (dynamicBoundLeft + dynamicBoundRight)/2;
        boundForce += 0.3 * (mid - selfPos[1]);
    } else {
        // 无障碍物，正常跟随
        followSpeed = map(distError, 0, 1.0, 0, 0.6);
        followSpeed = constrain(followSpeed, 0, 0.6);
    }
    
    // 6. 运动学解算
    float targetAngle = atan2(dy, dx);  // 期望航向
    float selfAngle = atan2(selfPos[1] - masterPos[1], selfPos[0] - masterPos[0]) + PI;
    float angleError = targetAngle - selfAngle;
    if (angleError > PI) angleError -= 2*PI;
    if (angleError < -PI) angleError += 2*PI;
    
    float leftVel = followSpeed - angleError * 0.8;
    float rightVel = followSpeed + angleError * 0.8;
    
    // 7. 边界与速度约束
    leftVel = constrain(leftVel, -0.8, 0.8);
    rightVel = constrain(rightVel, -0.8, 0.8);
    
    // 8. 电机执行
    motorL.move(leftVel);
    motorR.move(rightVel);
    
    // 调试输出
    Serial.print("Obstacle:"); Serial.print(obstacleDist);
    Serial.print(" DynamicBound:"); Serial.print(dynamicBoundLeft);
    Serial.print("-"); Serial.print(dynamicBoundRight);
    Serial.print(" Speed:"); Serial.println(followSpeed);
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    followAndAvoid();
    delay(50);
}

要点解读

1. 双UWB+多源感知融合：实现“精准跟随+动态约束”的基础
   双机器人动态跟随与通道约束的核心，是多源感知数据的精准融合：
   双UWB：不仅获取主从机器人的相对位置，还可结合UWB基站获取通道静态边界，同时通过双标签差分定位获取目标航向，为跟随提供方向基准；
   多源补充：案例3引入激光测距补充障碍物感知，案例5引入ESP-NOW补充机器人状态共享，形成“位置+姿态+障碍物+状态”的全维度感知体系；
   融合逻辑：静态边界用于约束机器人活动范围，动态障碍物感知用于调整约束边界，机器人状态用于任务分配，三者结合为“跟随+约束+分配”提供数据支撑。
2. 任务分配的动态冲突消解：匹配工业场景的实时性需求
   工业场景中任务具有动态性（如优先级突变、目标位置变化），静态任务分配易导致资源浪费或冲突，核心是基于优先级的动态冲突消解机制：
   优先级优先原则：紧急任务具有最高优先级，打破“空闲优先”的基础规则，避免关键任务延误；
   多维度决策依据：在优先级相同的情况下，综合距离、电量、忙碌状态等维度，案例5通过“距离近优先+电量高优先”，兼顾任务执行效率与机器人续航平衡；
   状态实时同步：通过ESP-NOW低延迟通信实时共享机器人状态，确保分配逻辑基于最新的全局信息，避免因状态滞后导致的冲突。
3. 通道约束的自适应调整：平衡跟随效率与安全边界
   狭窄通道的核心矛盾是“空间有限”与“跟随需求”的冲突，单纯固定约束边界会导致效率低下或安全隐患，核心是约束边界的动态自适应：
   静态约束+动态收缩：初始通道边界由场景固有属性确定，当检测到障碍物时，根据障碍物距离动态收缩约束边界，扩大安全余量，案例6中障碍物越近，边界收缩幅度越大，速度越低；
   约束与避障联动：将约束调整与避障逻辑深度耦合，当障碍物进入临界距离时，不仅收缩边界，还调整机器人侧向位置（向通道中央偏移），避免机器人陷入边界与障碍物的夹缝；
   速度自适应匹配：通道宽度与障碍物距离直接影响速度，案例4和案例6均通过映射函数将通道宽度/障碍物距离转换为速度系数，狭窄区域自动减速，确保安全。
4. 运动学约束与控制算法的耦合：避免“算法理想化落地失效”
   双机器人协同采用差速驱动底盘，存在非完整约束，直接套用理想控制算法会导致轨迹偏差，核心是控制算法与差速运动学约束的深度耦合：
   位姿误差转换：将全局位置误差转换为机器人的线速度与角速度，再通过差速公式分配左右轮速度，案例4-6均通过atan2计算航向角，将位置误差映射为航向调整量，适配差速底盘的运动特性；
   执行器约束限制：对计算出的速度进行上下限约束，避免电机过载或超速，案例中通过constrain函数限制最大线速度与角速度，匹配BLDC电机的物理性能；
   控制模式适配：跟随场景采用速度控制模式，保证响应速度；通道约束避障时切换为速度-位置混合控制，确保安全边界内的精准调整。
5. 低延迟通信与算力平衡：适配Arduino平台的工程化落地关键
   Arduino平台算力有限，而双机器人协同涉及UWB解算、任务分配、控制闭环等复杂逻辑，核心是低延迟通信与算力的平衡：
   通信协议选型：采用ESP-NOW（WiFi直连，延迟<1ms）替代蓝牙，保证状态共享的实时性，满足任务分配与协同避障的通信需求；
   算力分层优化：UWB定位解算、复杂任务分配逻辑放在ESP32主控中，Arduino仅负责BLDC电机的FOC控制闭环，避免算力不足导致控制周期波动；
   控制周期稳定：通过固定循环延迟（如50ms）确保控制周期稳定，避免因复杂逻辑导致控制周期飘移，同时将非关键逻辑（如状态打印）与控制循环分离，减少对控制实时性的影响。

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