“Arduino BLDC 之园区巡逻跟随机器人 UWB+视觉多模态抗遮挡”可以理解为：机器人以 UWB 提供稳定的目标相对定位，以视觉提供身份识别、语义判断和近距离精校正，再以 IMU、里程计和卡尔曼滤波进行短时遮挡下的航位推算；当目标被树木、车辆、行人短暂遮挡时，系统不会立即丢失，而是依靠融合估计维持跟随；最终由 BLDC FOC 把距离、角度和避障约束平滑落地为左右轮速度指令。

一、主要特点

1. UWB 解决“目标在哪里”的底层定位问题
   UWB 通过 TOF/TWR 或 PDoA/AoA 获得目标距离和方位角，厘米级精度、低延迟，且不依赖光照。 在园区夜间、逆光、树荫、地库等视觉不稳定场景下，UWB 能保持较稳定的相对定位。
2. 视觉解决“目标是谁、当前姿态如何”的问题
   视觉可用于人脸/人体检测、Re-ID、AprilTag 识别、深度估计和近距离精校正。它的优势是信息量大，能判断目标身份、朝向、是否跌倒、是否被误跟；缺点是受遮挡、逆光、粉尘、夜间低照度影响较大。
3. 多模态抗遮挡的核心是“互补 + 切换 + 推算”
   典型融合策略是：
   UWB 与视觉一致时，按置信度加权融合；
   视觉丢失但 UWB 仍有效时，以 UWB 为主；
   UWB 跳变但视觉稳定时，以视觉为主；
   两者短时异常时，用 IMU/里程计做航位推算；
   超时仍未恢复，则原地搜索或返回安全点。
4. 遮挡判定不能只看单传感器是否“有输出”
   更稳健的做法是结合多指标：UWB 与视觉位置偏差、UWB 测距质量、视觉置信度、目标检测框大小、IMU 运动一致性、历史轨迹连续性。单一阈值容易把噪声误判为遮挡，或把真实遮挡漏判。
5. 跟随控制通常采用“距离 + 航向 + 安全避障”三层约束
   距离环保证机器人与目标保持期望间距；航向环保证机器人朝向目标方向；避障作为硬优先级，当前方有障碍、行人或台阶时，必须中断跟随。 园区环境还需要加入速度限幅、转弯限幅和跟停缓冲，避免急追急刹。
6. BLDC FOC 让低速跟随更柔顺
   园区巡逻跟随常在 0.2–1.0 m/s 低速区间运行，且需要频繁微调、原地转向和绕障。BLDC 配合 FOC 电流环、速度环和编码器闭环，可降低低速转矩脉动，使跟随轨迹更平滑，也更适合夜间安静场景。
7. 分层架构更适合落地
   感知层负责 UWB/视觉/IMU/编码器数据采集；融合层负责目标位置估计、遮挡判定和航位推算；决策层负责跟随距离、搜索策略和安全降级；执行层负责 BLDC 速度/电流闭环。 ATmega328P 适合做电机执行和传感器采样，但完整多模态融合、视觉推理和卡尔曼滤波更适合放在 ESP32-S3、STM32 或上位机。

二、应用场景

1. 园区保安人机协同巡逻
   保安佩戴 UWB 标签，机器人保持侧后方跟随，夜间利用视觉/红外扫描环境，发现异常时定点记录并上报。
2. 厂区物料随行
   工人搬运物料时，机器人跟随其移动，避免频繁往返取料。UWB 保证在货架、设备间稳定定位，视觉辅助确认目标身份。
3. 园区巡检与设备点检
   机器人跟随巡检人员到达配电箱、消防栓、管廊口等点位，自动拍照、记录位置，并在人员短暂离开时保持等待。
4. 园区导览与接待跟随
   访客佩戴标签或通过视觉绑定身份，机器人跟随讲解。视觉可判断访客是否停留、转向或离开，UWB 保证在人群交叉时不易跟丢。
5. 校园/社区安防巡逻
   夜间光线不足、树荫遮挡、车辆临时停放较多，单一视觉容易失效。UWB+视觉融合可提升连续跟随能力，机器人也可承担照明、告警和远程视频回传任务。
6. 科研与竞赛平台
   适合验证 UWB PDoA/TWR、YOLO/AprilTag 识别、EKF 融合、遮挡恢复、跟随状态机和 BLDC 差速闭环。

三、需要注意的事项

1. UWB 并非完全不怕遮挡
   UWB 对轻遮挡、非视距环境比视觉更鲁棒，但金属墙体、密集货架、混凝土柱、地面反射仍会造成多径和 NLOS 误差。 部署时应避免大面积金属反射，基站/标签安装高度和角度也需要实测。
2. 视觉需要处理身份绑定与 ID 切换
   多人交叉、相似服装、目标转身、短暂出画都会导致视觉 ID 漂移。应结合 UWB 标签 ID、历史轨迹、运动一致性和置信度阈值，防止跟错人。
3. 遮挡判定要有迟滞和超时机制
   不能因为一帧检测失败就判定丢失，也不能长时间维持不可信估计。应设置遮挡进入阈值、恢复阈值、最大推算时间和最大推算距离，超时后进入原地搜索或安全停机。
4. 融合算法要考虑时间同步和坐标系对齐
   UWB、视觉、IMU、编码器采样率不同，时间戳不一致会导致滤波发散；各传感器坐标系也必须通过外参标定对齐。
5. 跟随距离应随环境动态调整
   开阔区域可适当增大跟随距离，提高通行效率；人流密集、拐角、坡道、地库入口应缩短距离并降低速度；夜间或低照度场景应提高避障权重。
6. 避障优先级必须高于跟随
   无论目标位置多么明确，前方出现行人、车辆、台阶、玻璃门或狭窄通道时，都应先保证安全。避障触发时应挂起跟随，恢复后再重新锁定目标。
7. BLDC 执行层需限速、限流、限加速度
   跟随过程中目标可能突然加速、急停或转向。若不限制线速度、角速度、加速度和 q 轴电流，容易造成电压跌落、底盘抖动、目标丢失甚至碰撞。
8. 8 位 Arduino 不宜承担完整融合与视觉推理
   ATmega328P 可用于 FOC 执行、编码器采样、IMU 读取和简单跟随控制；但 UWB 滤波、视觉检测、EKF 融合、状态机管理和通信调度更适合放在 ESP32-S3、STM32 或上位机。 底层 MCU 应保证电机控制周期稳定，不被视觉推理或串口通信阻塞。
9. 必须保留失联、低电、急停等安全兜底
   当 UWB 长时间失联、视觉持续丢失、IMU 异常、编码器故障、电池欠压或通信中断时，应减速停机或进入保守等待。算法层面的抗遮挡不能替代硬件级安全保护。

整体来看，这个架构的关键是：UWB 提供稳定相对定位，视觉提供身份与语义校正，融合层处理遮挡与推算，决策层管理跟随距离和安全优先级，BLDC FOC 把指令柔顺执行。做原型时，建议先跑通“UWB 单目标跟随 + BLDC 速度闭环”，再加入“视觉身份绑定 → 多模态融合 → 遮挡判定 → 航位推算 → 避障硬优先级 → 失联安全降级”。

1、UWB/视觉置信度加权融合 + 遮挡降级跟随
适用场景：巡逻人员佩戴 UWB 标签，机器人前方摄像头同时检测人员视觉特征。当视觉因人群遮挡或光照突变失效时，系统自动降低视觉权重、提升 UWB 权重，保持跟随连续性而非急停。

#include <SimpleFOC.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// ===== 多模态感知输入（由外部模块通过串口/ROS 更新） =====
float uwbDist = 2.0, uwbAngle = 0.0;       // UWB 测距与测角
bool uwbValid = false;
float visionDist = 2.0, visionAngle = 0.0; // 视觉估算距离与角度
bool visionValid = false;

// 融合输出
float fusedDist = 2.0, fusedAngle = 0.0, fusedConfidence = 0.0;

// 跟随参数
const float TARGET_DIST = 1.5;   // 目标跟随距离 (m)
const float MAX_SPEED = 0.8;     // m/s

// ===== 多模态融合 =====
void fuseSensors() {
  float uwbWeight = 0.0, visionWeight = 0.0;

  // UWB 置信度：有效且距离在合理范围
  if (uwbValid && uwbDist > 0.3 && uwbDist < 10.0) {
    uwbWeight = 0.6;  // UWB 基础权重
    // 距离越远，UWB 相对视觉更可靠（视觉远距离精度下降）
    if (uwbDist > 3.0) uwbWeight = 0.8;
  }

  // 视觉置信度：有效且目标在视野中央区域（边缘精度低）
  if (visionValid && abs(visionAngle) < 0.6) {
    visionWeight = 0.8;  // 视觉在近距离、正前方时精度高
    if (visionDist > 3.0) visionWeight = 0.4;  // 远距离视觉降权
  }

  // 归一化融合
  float totalWeight = uwbWeight + visionWeight;
  if (totalWeight > 0.01) {
    fusedDist = (uwbDist * uwbWeight + visionDist * visionWeight) / totalWeight;
    fusedAngle = (uwbAngle * uwbWeight + visionAngle * visionWeight) / totalWeight;
    fusedConfidence = totalWeight / 1.4;  // 归一化到 0~1
  } else {
    fusedConfidence = 0.0;
  }
}

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // 模拟传感器更新（实际从串口/ROS 读取）
  // uwbValid = ...; visionValid = ...;
  
  fuseSensors();

  // ===== 跟随控制 =====
  float linearCmd = 0, angularCmd = 0;

  if (fusedConfidence > 0.15) {
    // 距离误差 → 线速度（PID 简化）
    float distError = fusedDist - TARGET_DIST;
    linearCmd = constrain(distError * 0.8, -MAX_SPEED, MAX_SPEED);

    // 角度误差 → 角速度
    angularCmd = constrain(fusedAngle * 1.2, -1.5, 1.5);

    // 死区
    if (abs(linearCmd) < 0.05) linearCmd = 0;
    if (abs(angularCmd) < 0.1) angularCmd = 0;
  } else {
    // 双模态均失效：安全停车
    linearCmd = 0;
    angularCmd = 0;
  }

  // 差速解算
  float wheelBase = 0.3;
  float vL = linearCmd - angularCmd * wheelBase / 2;
  float vR = linearCmd + angularCmd * wheelBase / 2;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);

  delay(50);
}

解读：该案例的核心是置信度动态加权而非硬切换。搜索结果中的原始代码采用“偏差超过 1m 判定遮挡 → 切换惯性推算”的硬逻辑，但硬切换在临界点附近容易产生振荡。本案例改为连续加权：视觉在近距离正前方权重高，UWB 在远距离和视觉失效时权重自然上升。当两者都失效时，fusedConfidence 降至阈值以下，机器人安全停车而非盲目惯性推算——园区场景中，盲目推算比停车更危险。IEEE 的研究同样指出，UWB 在视觉失效时接管是提升鲁棒性的关键。

2、UWB 主导 + 视觉侧向约束的纵横向解耦跟随
适用场景：园区道路有明确边界（路沿、绿化带），UWB 负责纵向距离保持，视觉负责横向纠偏。两者解耦处理，避免融合后的控制量相互干扰。

#include <SimpleFOC.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// UWB 纵向
float uwbDist = 2.0;
bool uwbValid = false;

// 视觉横向（车道线/路沿检测，输出横向偏差）
float visionLateralOffset = 0.0;  // 正=偏右，负=偏左 (m)
bool visionValid = false;

// 控制参数
const float TARGET_DIST = 1.8;
float Kp_long = 0.6;   // 纵向 P
float Kp_lat = 1.5;    // 横向 P

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();

  // ===== 纵向：UWB 主导 =====
  float linearCmd = 0;
  if (uwbValid && uwbDist > 0.3 && uwbDist < 8.0) {
    float distError = uwbDist - TARGET_DIST;
    linearCmd = constrain(distError * Kp_long, -0.6, 0.6);
  } else {
    linearCmd = 0;  // UWB 失效，停止纵向运动
  }

  // ===== 横向：视觉主导 =====
  float angularCmd = 0;
  if (visionValid && abs(visionLateralOffset) < 1.5) {
    // 横向偏差 → 角速度修正（负反馈）
    angularCmd = constrain(-visionLateralOffset * Kp_lat, -1.2, 1.2);
  } else {
    // 视觉失效：保持当前航向，不额外转向
    angularCmd = 0;
  }

  // 前进时横向修正才有意义；原地转向会放大噪声
  if (abs(linearCmd) < 0.1) {
    angularCmd *= 0.3;
  }

  // 差速
  float wheelBase = 0.3;
  float vL = linearCmd - angularCmd * wheelBase / 2;
  float vR = linearCmd + angularCmd * wheelBase / 2;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);

  delay(50);
}

解读：该案例借鉴了学术研究中“UWB 纵向 + 视觉横向解耦”的思路。搜索结果中的融合方案将距离和角度加权平均后统一控制，但在园区结构化道路场景中，纵向跟随距离和横向道路约束的物理意义不同，加权融合会引入耦合误差。解耦后，UWB 失效时纵向停车但横向保持，视觉失效时横向保持当前航向但纵向继续——两者的降级策略各自独立，更符合实际失效模式。需要注意：横向视觉检测需要稳定的道路边界特征，若园区路面无标线，可替换为视觉检测巡逻人员相对机器人的横向像素偏差。

3、抗遮挡状态机 + S 型加减速平滑执行
适用场景：巡逻人员频繁穿梭于人群或设备之间，UWB 和视觉交替失效。状态机管理“正常跟随 / 单模态降级 / 惯性维持 / 安全停车”四种模式，BLDC 执行层用 S 型曲线柔化频繁启停的冲击。

#include <SimpleFOC.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 传感器
float uwbDist = 2.0, uwbAngle = 0.0;
bool uwbValid = false;
float visionDist = 2.0, visionAngle = 0.0;
bool visionValid = false;

// 状态机
enum FollowState { NORMAL, UWB_ONLY, VISION_ONLY, DEAD_RECKON, SAFE_STOP };
FollowState state = NORMAL;
unsigned long stateTimer = 0;
const unsigned long DEAD_RECKON_TIMEOUT = 800;  // 惯性推算最长 800ms

// 惯性推算
float lastValidDist = 2.0, lastValidAngle = 0.0;
float imuYawRate = 0.0;

// S 型加减速
float currentSpeed = 0;
float targetSpeed = 0;

float sCurveRamp(float current, float target, float dt) {
  float maxAccel = 1.5;  // m/s²
  float diff = target - current;
  if (abs(diff) < 0.02) return target;
  float step = maxAccel * dt;
  if (diff > 0) return min(current + step, target);
  else return max(current - step, target);
}

void updateState() {
  bool uwbOk = uwbValid && uwbDist > 0.3 && uwbDist < 8.0;
  bool visionOk = visionValid && abs(visionAngle) < 0.8;

  if (uwbOk && visionOk) {
    state = NORMAL;
    lastValidDist = (uwbDist + visionDist) / 2;
    lastValidAngle = (uwbAngle + visionAngle) / 2;
  } else if (uwbOk && !visionOk) {
    state = UWB_ONLY;
    lastValidDist = uwbDist;
    lastValidAngle = uwbAngle;
  } else if (!uwbOk && visionOk) {
    state = VISION_ONLY;
    lastValidDist = visionDist;
    lastValidAngle = visionAngle;
  } else {
    // 双模态失效
    if (state != DEAD_RECKON && state != SAFE_STOP) {
      state = DEAD_RECKON;
      stateTimer = millis();
    }
    if (state == DEAD_RECKON && millis() - stateTimer > DEAD_RECKON_TIMEOUT) {
      state = SAFE_STOP;
    }
  }
}

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  driverL.voltage_power_supply = 24; driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  updateState();

  float dist, angle;
  float targetLinear = 0, targetAngular = 0;

  switch (state) {
    case NORMAL:
      dist = (uwbDist + visionDist) / 2;
      angle = (uwbAngle + visionAngle) / 2;
      targetLinear = constrain((dist - 1.5) * 0.7, -0.7, 0.7);
      targetAngular = constrain(angle * 1.0, -1.2, 1.2);
      break;
    case UWB_ONLY:
      targetLinear = constrain((uwbDist - 1.5) * 0.7, -0.7, 0.7);
      targetAngular = constrain(uwbAngle * 0.8, -0.8, 0.8);  // UWB 角度精度有限，降增益
      break;
    case VISION_ONLY:
      targetLinear = constrain((visionDist - 1.5) * 0.7, -0.7, 0.7);
      targetAngular = constrain(visionAngle * 1.2, -1.2, 1.2);
      break;
    case DEAD_RECKON:
      // 基于最后有效角度和 IMU 航向变化推算
      targetLinear = 0.3;  // 低速维持
      targetAngular = lastValidAngle * 0.5 + imuYawRate * 0.1;
      break;
    case SAFE_STOP:
      targetLinear = 0;
      targetAngular = 0;
      break;
  }

  // S 型平滑
  currentSpeed = sCurveRamp(currentSpeed, targetLinear, 0.05);

  float wheelBase = 0.3;
  float vL = currentSpeed - targetAngular * wheelBase / 2;
  float vR = currentSpeed + targetAngular * wheelBase / 2;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);

  delay(50);
}

解读：该案例将搜索结果中的“遮挡检测 → 惯性推算”硬逻辑扩展为带超时的状态机。关键设计是 DEAD_RECKON_TIMEOUT = 800ms：惯性推算超过此时间仍未恢复任一传感器，强制进入 SAFE_STOP。园区巡逻场景中，巡逻人员可能进入建筑物或转角，UWB 和视觉同时丢失超过 1 秒是正常的，此时继续惯性推算是危险的——机器人可能撞上巡逻人员刚刚绕过的障碍物。S 型加减速函数 sCurveRamp 解决了频繁启停的顿挫问题，这是 BLDC + FOC 相对有刷电机的核心优势，搜索结果中也强调了 S 型算法在园区跟随场景中的必要性。

要点解读

1. 抗遮挡的核心不是“预测目标在哪”，而是“知道何时该放弃预测”
   搜索结果中原始代码的逻辑是“偏差超过 1m 判定遮挡，切换惯性推算”。但惯性推算的误差随时间累积，800ms 后推算位置可能偏离实际目标数米。案例三设置超时强制停车，原则是：短时遮挡（<800ms）用推算维持平滑，长时丢失（>800ms）停车等待是更安全的选择。园区环境中巡逻人员可能进入设备间或转角，机器人不应盲目追入未知区域。IEEE 研究也指出，UWB 接管的目的是“维持跟随直到视觉恢复”，而非无限期替代视觉。

2. UWB 与视觉的权重应随距离和角度动态调整，而非固定比例
   UWB 在远距离（>3m）的测距精度相对稳定，但测角精度有限；视觉在近距离正前方精度高，但远距离和边缘视野误差增大。案例一采用 uwbDist > 3.0 时 UWB 权重升至 0.8、abs(visionAngle) < 0.6 时视觉权重升的规则，使融合输出在两种传感器的“各自优势区间”内偏向更可靠的一方。固定 50:50 加权在远距离会拉低 UWB 精度，在近距离会拉低视觉精度。

3. 园区巡逻场景的跟随是“纵横向解耦”问题，不是单一距离控制
   案例二借鉴了农业机器人研究中的解耦思路：纵向（前后距离）由 UWB 负责，横向（左右偏移）由视觉负责。这种分工的合理性在于：UWB 测距的物理基础是飞行时间，对纵向距离敏感；视觉的横向像素偏差对左右位置敏感。将两者加权融合为一个“距离+角度”的复合值会引入耦合误差——UWB 的角度噪声会污染纵向距离控制。解耦后，任一传感器失效时只需降级其对应的控制通道，另一通道不受影响。

4. BLDC 的 FOC 平滑执行是“人走车随”体感的关键，而非单纯动力选择
   园区巡逻跟随中，巡逻人员的步速频繁变化，机器人需要频繁加减速。有刷电机配合简单 PWM 在低速段存在死区和齿槽效应，加减速时会产生顿挫，让被跟随者感到“车在拽我”或“车跟不上”。FOC 速度环的毫秒级响应配合 S 型加减速曲线（案例三的 sCurveRamp），使速度指令的变化被柔化执行。搜索结果中明确将“BLDC + S 型加减速”列为园区跟随场景的核心配置，原因正在于此。

5. Arduino 主控的算力分配决定了视觉处理必须外置
   案例中的视觉输入（visionDist、visionAngle）是作为外部变量存在的，实际系统中这些值来自树莓派、K210 或 ESP32-S3 等协处理器。Arduino/ESP32 主控负责的是传感器融合、状态机决策和 FOC 执行。若试图在 Arduino 上直接运行视觉算法（如 AprilTag 检测或 YOLO 推理），FOC 环的定时抖动会严重恶化。合理的分工是：视觉协处理器以 10-30Hz 输出目标位置，主控以 50-100Hz 运行融合与状态机，FOC 环以 10kHz+ 运行。搜索结果中关于 UWB 精度的讨论也隐含了这一架构：UWB 模块本身通常通过串口输出测距数据，主控只需解析而非处理原始射频信号。

4、UWB + 视觉置信度门控切换（抗遮挡最小闭环）
适用硬件：ESP32 + SimpleFOC 差速底盘（2×BLDC）+ 编码器 + DW1000/DWM1000 UWB（目标佩戴标签）+ HuskyLens/ESP32-CAM 视觉模块（串口输出目标框）。
核心思路：两个传感器不做像素级融合，而是按信任域切换——视觉可信时用视觉方位 + UWB 距离；视觉丢失时用“最后方位 + 指数外推”滑行，并立即限速；超过 2s 不再信任方位，转原地搜索。

/*
 * 案例4：UWB(距离/身份) + 视觉(方位/确认) 门控切换跟随
 * 信任域：视觉有效 -> 视觉方位；视觉丢失 -> 方位外推(≤2s) -> 停止转向
 */
#include <SimpleFOC.h>

BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onL(){ encL.handleA(); } void onR(){ encR.handleA(); }

const float R_WHEEL = 0.065f, TRACK = 0.34f;
const float D_FOLLOW = 1.20f;      // 期望跟随距离
const float D_STOP   = 0.80f;      // 最小安全距离
const float V_MAX    = 0.90f;
const uint8_t TARGET_ID = 7;       // 目标 UWB 标签 / 视觉 ID

// ---------- 视觉输入（串口帧: id,cx,cy,w,h）----------
struct Vision { bool valid=false; uint8_t id=0; float cx=0; float wRel=0; };
Vision vis;
unsigned long lastVisMs = 0;
int visGood = 0, visBad = 0;
bool visTrusted = false;

void readVision() {
  if (Serial2.available()) {
    String s = Serial2.readStringUntil('\n');
    int id, cx, cy, w, h;
    if (sscanf(s.c_str(), "%d,%d,%d,%d,%d", &id, &cx, &cy, &w, &h) == 5) {
      vis.id = id;
      vis.cx = (cx - 160) / 160.0f;        // 归一化 -1..1（320 宽视野）
      vis.wRel = w / 320.0f;               // 相对宽度（距离代理）
      vis.valid = true;
      lastVisMs = millis();
    }
  }
  if (millis() - lastVisMs > 250) vis.valid = false;   // 250ms 无帧=丢
}

// 迟滞：连续 3 帧有效才可信，连续 4 帧丢失才失信（防抖）
void updateTrust() {
  bool ok = vis.valid && (vis.id == TARGET_ID) && (vis.wRel > 0.04f);
  if (ok) { visGood++; visBad = 0; } else { visBad++; visGood = 0; }
  if (visGood >= 3) visTrusted = true;
  if (visBad  >= 4) visTrusted = false;
}

// ---------- UWB ----------
struct Uwb { float d=0; uint8_t id=0; float fpRatio=0; bool valid=false; };
Uwb uwb;
float readUWB();          // 库封装：返回距离(m)
uint8_t readUWBid();
float readUWBquality();   // 首径功率/总功率比，低=NLOS 多径

// ---------- 方位状态 ----------
float lastBearing = 0, bearingCmd = 0;
unsigned long lastGoodMs = 0;

void setup() {
  encL.init(); encL.enableInterrupts(onL);
  encR.init(); encR.enableInterrupts(onR);
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 3.5f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 3.5f;
  wheelL.voltage_limit = wheelR.voltage_limit = 12.0f;
  wheelL.current_limit = wheelR.current_limit = 4.0f;
  wheelL.init(); wheelR.init();
  drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  Serial2.begin(115200);
  Serial.begin(115200);
}

void loop() {
  // 1) 感知
  uwb.d = readUWB(); uwb.id = readUWBid(); uwb.fpRatio = readUWBquality();
  uwb.valid = (uwb.d > 0.05f) && (uwb.d < 30.0f) && (uwb.id == TARGET_ID);
  readVision();
  updateTrust();

  // 2) 方位来源仲裁（抗遮挡核心）
  bool bearingTrusted;
  if (visTrusted) {
    bearingCmd = vis.cx * 0.62f;             // 视觉方位（按 FOV 标定系数）
    lastBearing = bearingCmd; lastGoodMs = millis();
    bearingTrusted = true;
  } else {
    float t = (millis() - lastGoodMs) / 1000.0f;
    bearingCmd = lastBearing * expf(-t / 1.5f);   // 指数外推，逐渐归零
    bearingTrusted = (t < 2.0f);
  }

  // 3) 距离环（UWB 主导）
  float vCmd = 0;
  if (uwb.valid) {
    float e = uwb.d - D_FOLLOW;
    vCmd = constrain(1.6f * e, -0.25f, V_MAX);
    if (uwb.d < D_STOP) vCmd = -0.15f;             // 过近：主动后退
    if (uwb.fpRatio < 0.60f) vCmd = min(vCmd, 0.45f);  // NLOS：限速
  } else {
    vCmd = 0;                                      // 无距离：不盲目前进
  }

  // 4) 方位环 -> 角速度，遮挡时禁止大转向
  float wCmd = bearingTrusted ? constrain(1.8f * bearingCmd, -0.9f, 0.9f) : 0.0f;
  if (!visTrusted) vCmd = min(vCmd, 0.45f);        // 盲跟限速

  // 5) 执行
  float vL = vCmd - wCmd * TRACK * 0.5f;
  float vR = vCmd + wCmd * TRACK * 0.5f;
  wheelL.move(vL / R_WHEEL); wheelR.move(vR / R_WHEEL);

  Serial.print("uwb="); Serial.print(uwb.d, 2);
  Serial.print(" fp="); Serial.print(uwb.fpRatio, 2);
  Serial.print(" vis="); Serial.print(visTrusted);
  Serial.print(" v="); Serial.println(vCmd, 2);
  delay(20);
}

镜头遮挡、强逆光、目标背对镜头都表现为“视觉连续丢帧”，这套迟滞+外推能在 4 帧（约 100ms）内做出降级反应，而不是用 0 值参与运算（那是“把遮挡当成了目标消失”）。

5、FreeRTOS 四任务 + 遮挡状态机（跟踪/滑行/搜索/丢失）+ α-β 预测
适用硬件：ESP32 双核 + 差速 BLDC + UWB + 视觉 + 前向 TOF。
核心思路：抗遮挡的本质是不确定性管理。用 α-β 滤波器维护目标相对位置与速度估计，遮挡时按预测继续滑行（速度限幅、时间限时），并主动执行“原地慢转扫描”搜索；重捕获需连续帧确认，不允许单帧跳变。

/*
 * 案例5：UWB+视觉四任务 + α-β 预测 + 遮挡状态机
 * 状态：TRACK(双源可信) / COAST(视觉丢，按预测滑行) / SEARCH(限时后搜索) / LOST(停车上报)
 */
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;
const float R_WHEEL = 0.065f, TRACK = 0.34f;

enum Occl { ST_TRACK, ST_COAST, ST_SEARCH, ST_LOST };
volatile Occl g_st = ST_TRACK;

// ---- α-β 滤波：跟踪目标相对位置/速度（机体坐标系）----
struct AB { float x=0, y=0, vx=0, vy=0; };
AB est;
const float ALPHA = 0.55f, BETA = 0.18f;

void abPredict(float dt) { est.x += est.vx*dt; est.y += est.vy*dt; }
void abUpdate(float zx, float zy, float dt) {
  float ex = zx - est.x, ey = zy - est.y;
  est.x += ALPHA*ex; est.y += ALPHA*ey;
  est.vx += BETA*ex/dt; est.vy += BETA*ey/dt;
}

// ---- 共享量 ----
volatile float s_uwbD=9, s_uwbQ=0, s_visCx=0; 
volatile bool  s_uwbOK=false, s_visOK=false;
volatile float s_ex=0, s_ey=0;          // 预测相对位置
volatile float c_v=0, c_w=0;
volatile bool  c_estop=false;

volatile unsigned long s_lastVisMs=0, s_visOkMs=0;
volatile int s_visCnt=0;

// ---- 任务1：视觉（100Hz），重捕获连续帧计数 ----
void taskVision(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    bool got = visionParse();         // 解析一帧
    xSemaphoreTake(mux, portMAX_DELAY);
    if (got) {
      s_visCx = visionCx(); s_visOK = true;
      s_lastVisMs = millis(); s_visCnt++;
    } else {
      s_visOK = false;
      if (millis() - s_lastVisMs > 300) s_visCnt = 0;
    }
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
  }
}

// ---- 任务2：UWB（20Hz）----
void taskUwb(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float d = readUWB(), q = readUWBquality();
    xSemaphoreTake(mux, portMAX_DELAY);
    s_uwbD = d; s_uwbQ = q;
    s_uwbOK = (d > 0.05f && d < 30.0f) && (readUWBid() == 7);
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));
  }
}

// ---- 任务3：融合与状态机（50Hz）----
void taskFusion(void*) {
  TickType_t last = xTaskGetTickCount();
  float dt = 0.02f;
  unsigned long coastT0 = 0;

  for (;;) {
    float d, q, cx; bool uok, vok; unsigned long lastVis; int vcnt;
    xSemaphoreTake(mux, portMAX_DELAY);
    d=s_uwbD; q=s_uwbQ; cx=s_visCx; uok=s_uwbOK; vok=s_visOK;
    lastVis=s_lastVisMs; vcnt=s_visCnt;
    xSemaphoreGive(mux);

    // ---- 观测构造：UWB 给距离，视觉给方位 ----
    bool measOK = uok && (q > 0.5f);
    float zx=0, zy=0;
    if (measOK) {
      float bear = vok ? (cx * 0.62f) : est.y / (fabsf(est.x)+0.2f); // 视觉方位或估计方位
      zx = d * cosf(bear); zy = d * sinf(bear);
    }

    // ---- 预测 ----
    abPredict(dt);
    if (measOK && (vok || g_st != ST_TRACK)) abUpdate(zx, zy, dt);

    // ---- 状态机：遮挡时长驱动分级降级 ----
    unsigned long gone = millis() - lastVis;
    switch (g_st) {
      case ST_TRACK:
        if (!vok) { g_st = ST_COAST; coastT0 = millis(); }
        break;
      case ST_COAST:
        // 滑行：按预测走，限速限时
        if (vok && vcnt >= 3) { g_st = ST_TRACK; }
        else if (millis() - coastT0 > 2000) { g_st = ST_SEARCH; coastT0 = millis(); }
        break;
      case ST_SEARCH:
        // 搜索：原地慢转扫描（不前进），扩大视野
        if (vok && vcnt >= 3) { g_st = ST_TRACK; }
        else if (millis() - coastT0 > 8000) { g_st = ST_LOST; }
        break;
      case ST_LOST:
        if (vok && vcnt >= 3) { g_st = ST_TRACK; coastT0 = millis(); }
        break;
    }

    // ---- 输出 ----
    float v = 0, w = 0;
    float dist = sqrtf(est.x*est.x + est.y*est.y);
    float bear = atan2f(est.y, est.x);

    switch (g_st) {
      case ST_TRACK:
        v = constrain(1.6f*(dist - 1.2f), -0.2f, 0.9f);
        w = constrain(1.8f*bear, -0.9f, 0.9f);
        if (dist < 0.8f) v = -0.12f;
        if (q < 0.6f) v = min(v, 0.45f);        // NLOS 限速
        break;
      case ST_COAST:
        v = constrain(1.2f*(dist - 1.2f), -0.15f, 0.45f);   // 限速滑行
        w = constrain(1.4f*bear, -0.6f, 0.6f);
        break;
      case ST_SEARCH:
        v = 0;                                   // 原地不前进
        w = 0.55f;                               // 慢速扫描（一个方向）
        break;
      case ST_LOST:
        v = 0; w = 0;
        break;
    }

    xSemaphoreTake(mux, portMAX_DELAY);
    s_ex = est.x; s_ey = est.y;
    c_v = v; c_w = w;
    c_estop = (g_st == ST_LOST);
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));
  }
}

// ---- 任务4：执行（1kHz，纯跟踪 + 斜率限幅）----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float vC = 0, wC = 0;
  for (;;) {
    float v, w; bool es;
    xSemaphoreTake(mux, portMAX_DELAY);
    v = c_v; w = c_w; es = c_estop;
    xSemaphoreGive(mux);
    if (es) { v = 0; w = 0; }
    vC += constrain(v - vC, -0.8f*0.001f, 0.8f*0.001f);
    wC += constrain(w - wC, -3.0f*0.001f, 3.0f*0.001f);
    float vL = vC - wC*TRACK*0.5f, vR = vC + wC*TRACK*0.5f;
    wheelL.loopFOC(); wheelR.loopFOC();
    wheelL.move(vL/R_WHEEL); wheelR.move(vR/R_WHEEL);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（略）
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskVision, "vis",   4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskUwb,    "uwb",   4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskFusion, "fus",   4096, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(taskExec,   "exec",  4096, NULL, 3, NULL, 1);
}
void loop() { vTaskDelay(portMAX_DELAY); }

关键设计：ST_COAST 限时 2s、限速 0.45 m/s，超时后转 ST_SEARCH（原地慢扫，不前进）；ST_LOST 停车并上报。三个阈值（300ms 判丢、2s 滑行、8s 搜索）就是抗遮挡的“时间预算”，绝不能无限期盲追。

案例6、：多假设跟踪 + 主动避遮挡（peek maneuvers）+ 衣着重识别
适用硬件：ESP32 + 差速 BLDC + UWB + 云台视觉（或用舵机偏转摄像头）。
核心思路：遮挡后目标可能“直走 / 左绕 / 右绕”，用三个假设并行预测，靠 UWB 距离序列给每个假设打分（最一致的胜出）；若假设仍不确定，机器人主动横向移动到能重新看到目标的位置（脱离自身遮挡），并用衣着颜色直方图做重识别，防止跟错人。

/*
 * 案例6：多假设 + 主动 peek + 颜色重识别
 */
#include <SimpleFOC.h>

BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
const float R_WHEEL=0.065f, TRACK=0.34f;

// ---- 假设：位置 + 航向 + 权重 ----
struct Hyp { float x, y, th; float w; };
Hyp H[3];              // 0=直行 1=左偏 2=右偏
float dUwb = 9; bool uwbOK = false;
float px=0, py=0, pth=0;    // 自身位姿（里程计）

// ---- 颜色直方图（重识别）----
#define BINS 8
float histRef[BINS];        // 首次确认时建立
unsigned long lastVisMs = 0;
bool visOK = false, reidOK = false;

void buildHistogram(const uint8_t* rgb, int n, float* h) {
  for (int i=0;i<BINS;i++) h[i]=0;
  for (int i=0;i<n;i++) { h[rgb[i*3] * BINS / 256] += 1; }
  float s=0; for(int i=0;i<BINS;i++) s+=h[i];
  if (s>0) for(int i=0;i<BINS;i++) h[i]/=s;
}
float histDist(const float* a, const float* b) {   // Bhattacharyya 距离
  float bc=0; for(int i=0;i<BINS;i++) bc += sqrtf(a[i]*b[i]);
  return 1.0f - bc;
}

// ---- 初始化假设 ----
void initHyps(float x0, float y0, float th0, float v) {
  H[0] = {x0 + v*cosf(th0)*0.5f, y0 + v*sinf(th0)*0.5f, th0,        1.0f/3};
  H[1] = {x0 + v*cosf(th0+0.6f)*0.5f, y0 + v*sinf(th0+0.6f)*0.5f, th0+0.6f, 1.0f/3};
  H[2] = {x0 + v*cosf(th0-0.6f)*0.5f, y0 + v*sinf(th0-0.6f)*0.5f, th0-0.6f, 1.0f/3};
}

void predictHyps(float dt, float v) {
  for (int i=0;i<3;i++) {
    H[i].x += v*cosf(H[i].th)*dt;
    H[i].y += v*sinf(H[i].th)*dt;
  }
}

// ---- UWB 距离给假设打分 ----
void scoreHyps() {
  if (!uwbOK) return;
  float sum = 0;
  for (int i=0;i<3;i++) {
    float dPred = hypotf(H[i].x - px, H[i].y - py);
    float e = fabsf(dPred - dUwb);
    H[i].w *= expf(-e*e/(2*0.25f*0.25f));     // 高斯似然，σ=0.25m
    H[i].w += 1e-4f;                          // 防归零
    sum += H[i].w;
  }
  for (int i=0;i<3;i++) H[i].w /= sum;        // 归一化
}

int bestHyp() {
  int b=0; for(int i=1;i<3;i++) if (H[i].w>H[b].w) b=i;
  return b;
}

// ---- 主动 peek：横向移动以获得视线 ----
enum Act { A_FOLLOW, A_PEEK, A_SEARCH };
Act act = A_FOLLOW;
unsigned long peekT0 = 0;
float peekDir = 1;

float usLeft(), usRight();     // 侧向测距，用于判断哪侧更空

void setup() {
  // 电机初始化同案例一（略）
  Serial.begin(115200);
}

void loop() {
  dUwb = readUWB(); uwbOK = (dUwb > 0.05f && dUwb < 30.0f) && (readUWBid()==7);
  bool visNow = visionParse();
  if (visNow) { lastVisMs = millis(); visOK = true; }
  else if (millis() - lastVisMs > 300) visOK = false;

  // 视觉新鲜时更新目标绝对位置并重设假设
  if (visOK) {
    float bear = visionCx() * 0.62f;
    float tx = px + dUwb*cosf(pth + bear);
    float ty = py + dUwb*sinf(pth + bear);
    // 首次建立衣着模板
    if (!reidOK) { buildHistogram(visionPersonRgb(), visionPersonPixels(), histRef); reidOK = true; }
    else if (histDist(histRef, visionCurrentHist()) > 0.25f) {
      // 跟错人：丢弃视觉，仅用 UWB，并降低置信
      visOK = false;
    }
    initHyps(tx, ty, pth + bear, 0.6f);
  }

  predictHyps(0.02f, 0.6f);
  scoreHyps();

  // ---- 行为选择 ----
  float v=0, w=0;
  if (visOK && reidOK) {
    act = A_FOLLOW;
    int b = bestHyp();
    float dx = H[b].x - px, dy = H[b].y - py;
    float dist = hypotf(dx, dy), bear = atan2f(dy, dx) - pth;
    while (bear > PI) bear -= 2*PI; while (bear < -PI) bear += 2*PI;
    v = constrain(1.5f*(dist - 1.2f), -0.2f, 0.9f);
    w = constrain(1.8f*bear, -0.9f, 0.9f);
  } else {
    // 遮挡：先尝试主动 peek（横向挪一步，换视角）
    if (act == A_FOLLOW) {
      act = A_PEEK; peekT0 = millis();
      peekDir = (usLeft() > usRight()) ? 1.0f : -1.0f;   // 往更空一侧让
    }
    if (act == A_PEEK) {
      v = 0.10f; w = 0.45f * peekDir;                    // 小角度斜移
      if (millis() - peekT0 > 2500) act = A_SEARCH;
    } else {
      v = 0; w = 0.55f;                                   // 原地扫描
      if (visOK) act = A_FOLLOW;
      if (millis() - lastVisMs > 8000) { v = 0; w = 0; }   // 彻底丢失：停车
    }
    if (visOK) act = A_FOLLOW;
  }

  // 执行
  float vL = v - w*TRACK*0.5f, vR = v + w*TRACK*0.5f;
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(vL/R_WHEEL); wheelR.move(vR/R_WHEEL);

  Serial.print("hyp="); Serial.print(bestHyp());
  Serial.print(" w0="); Serial.print(H[0].w,2);
  Serial.print(" w1="); Serial.print(H[1].w,2);
  Serial.print(" w2="); Serial.print(H[2].w,2);
  Serial.print(" act="); Serial.println(act);
  delay(20);
}

peek 的价值在于：很多遮挡不是目标消失了，而是机器人自己站的位置看不到（比如柱子正好挡住视线）。横向挪 20~40cm 往往立刻恢复视线，比原地转圈搜索快得多。重识别阈值 0.25 要按服装样式标定，纯色衣服易混淆时，需加入身高/步态或 UWB 标签 ID 双重约束。

要点解读
一、抗遮挡不是加权平均，而是“信任域切换 + 不确定性显式管理”
把 UWB 距离和视觉方位丢进一个加权公式，在遮挡时会得到“半个目标”，机器人也就跟着“半个目标”走；这种做法比只用 UWB 更危险，因为视觉失效后置信度并没有真正降下来。正确结构是案例一的信任域开关 + 迟滞：视觉可信就用视觉方位，不可信就切换源，并把“当前不确定性”作为一个显式状态（visTrusted、coast 时长、假设权重）传递给控制层，让速度随之降下来。不确定性必须是一个可被决策的数，而不是一个被平均掉的误差。

二、UWB 与视觉的分工要按能力互补划，不是“谁更准用谁”
UWB 给的是距离 + 身份锚定（标签 ID 唯一，不怕认错人），缺点是方位精度差、NLOS 下测距会跳；视觉给的是方位 + 身份确认（衣着/人脸），缺点是怕遮挡和光照。所以正确的融合是“UWB 定距离与身份、视觉定方位与确认”（案例一），而不是互相取长补短地平均。这也解释了为什么遮挡期必须靠 UWB 兜住距离：方位可以暂时不知道，但距离绝不能失——距离是安全边界，方位只是航向修正。

三、遮挡必须被“检测”出来，且要多判据、带迟滞
“读不到”有三种完全不同的原因：真遮挡、检测器漏检、目标离开视野；处理方式各不相同。判据要叠三层：① 帧连续性与 bbox 面积（案例一 visGood/visBad 计数 + wRel > 0.04）；② UWB 首径功率比 fpRatio（NLOS 多径会让测距突然偏大，案例一用它限速）；③ 距离序列的突变检测（一跳半米以上大概率是 NLOS 而非目标移动）。每一条都要带迟滞（连续 N 帧才切换），否则单帧噪声就会触发降级，行为变得神经质。

四、遮挡期必须“预测—滑行—搜索”三态限时，重捕获要连续帧确认
遮挡后的正确行为不是原地等，也不是加速追，而是分三级：COAST（按预测滑行，限速、限时 2s）→ SEARCH（原地慢扫，不前进，限时 8s）→ LOST（停车上报）。案例二的三个时间预算就是这套逻辑。滑行期必须限速，因为此时位置估计的方差在单调增长；重捕获必须连续 3 帧 + ID 一致才恢复 TRACK，否则目标从柱子后一闪而过就会造成“追一下—停一下”的抽动。允许短时不知道目标在哪，但不允许在不知道的情况下高速移动。

五、主动感知与身份绑定，是园区场景区别于“跟着走”的关键
园区里有柱子、灌木、其他人，被动抗遮挡迟早会跟丢或跟错。两个主动手段：① peek 机动（案例三）——遮挡常是自身视角问题，横向挪一步比原地转圈更有效；② 身份绑定——UWB 标签 ID 做硬绑定，视觉衣着直方图做软确认（案例三 histDist > 0.25 就丢弃视觉），两者任一不符都不跟随。执行层依旧是 1kHz 纯跟踪、只消费一个 (v, ω)，失效即停。跟对人比跟得紧重要得多——园区巡逻跟错人的后果，远比跟丢严重。

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