这个系统的核心思想可以概括为：UWB 负责“离目标多远、在哪个方向”，视觉负责“横向偏差是否可信、目标是否被遮挡”，纵向距离控制与横向方位控制分别闭环，再由 BLDC 差速底盘执行。 这样可以把 UWB 测距稳定、视觉方位敏感、FOC 低速平滑三类优势结合起来。

主要特点
纵横向解耦，控制更稳定
纵向用 UWB 距离控制前后跟随，横向用视觉像素偏差或角度估计控制左右修正。UWB 的角度噪声不容易污染纵向距离环，视觉短暂丢失时也不至于让纵向跟随立刻崩溃。
UWB 主导距离，视觉提供侧向约束
UWB 通过 ToF/PDOA 提供厘米级测距和方位信息，适合做跟随的主信源；视觉则在近距离、正前方视野内提供更高精度的横向位置约束，并用于判断目标是否仍在视野中。
抗遮挡能力更强
视觉容易被遮挡、逆光、粉尘影响；UWB 在轻遮挡、低光照环境下更鲁棒。两者互补后，短时遮挡可以用 UWB 维持距离控制，或用视觉最后可信方位做短时外推；长时间丢失则应降速、停车或进入搜索状态。
BLDC + FOC 让跟随更柔顺
跟随场景需要频繁启停、差速转向。BLDC 配合 FOC 可以在低速段保持平稳扭矩输出，再叠加 S 型加减速，能明显减少“顿挫、拉扯、忽快忽慢”的问题。
适合嵌入式分层实现
典型架构是：视觉协处理器输出目标框/方位，UWB 模块输出距离/角度，主控完成融合、状态机和速度规划，FOC 速度环/电流环在底层高频执行。

应用场景
园区/厂区巡逻跟随：机器人跟随安保人员，UWB 保证距离稳定，视觉辅助判断人员是否仍在正前方。
仓储拣选跟随：工人佩戴 UWB 标签，机器人携带物料或周转箱跟随；视觉可用于二次确认目标身份或检测遮挡。
设施农业/大棚运输：行间环境复杂、光照变化大，UWB 提供稳定距离，视觉帮助修正横向偏移。
陪护/随行机器人：在室内走廊、电梯口、转角等场景，UWB 降低视觉丢失带来的跟丢风险。
教育科研平台：适合做多传感器融合、纵横向解耦控制、FOC 运动控制的综合实验。

需要注意的事项
UWB 不是万能主导
UWB 测距通常可靠，但测角受天线布局、多径、NLOS 影响较大。不能把 UWB 角度直接当作高精度横向控制源，必须用视觉或 IMU 做约束。
视觉侧向约束要有置信度门控
视觉输出应带有效标志、目标 ID、框大小、置信度等。只有当目标在视野内、距离合理、ID 匹配时才参与横向控制；否则进入外推、限速或停车。
遮挡恢复策略要保守
短时丢失可以惯性外推或维持上一可信方位；超过设定时间后不应继续盲跟，应降速、原地搜索或等待目标重新出现。
FOC 环与融合环要分层
FOC 电流环/速度环需要高频稳定执行，视觉和 UWB 融合可以放在较低频率。避免在电机控制主循环里做耗时视觉推理。
电源和 EMC 必须隔离
BLDC 启停和差速转向会产生较大电流冲击和高频噪声，控制板、传感器、电机驱动最好独立供电并做好滤波，否则容易出现主控复位、UWB/IMU 数据跳变。
坐标系和时间同步要对齐
UWB、视觉、IMU、轮速计的坐标系原点、朝向必须统一标定；数据频率不同时需要时间戳对齐，否则横向修正会滞后或振荡。
安全优先级要明确
避障优先级应高于跟随。即使 UWB 显示目标仍在前方，若前方有障碍物、台阶、人员突然折返，也应先减速或停车。

1、UWB 纵向 EKF + 视觉横向偏差解耦控制（基础框架）
适用场景：机器人在狭窄走廊或园区道路跟随人员。UWB 提供相对距离，但角度信息噪声较大；视觉检测道路边界或目标横向像素偏差，提供侧向约束。两者不融合为一个复合误差，而是分别驱动纵向线速度和横向角速度，避免 UWB 的角度噪声污染横向控制。

#include <SimpleFOC.h>

// 双轮 BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(3, 5, 6, 7);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);

// ===== 解耦感知输入 =====
// UWB：仅用于纵向距离
float uwbDist = 2.0;
bool uwbValid = false;

// 视觉：仅用于横向偏差（像素或归一化值）
float visionLateral = 0.0;  // 负=偏左，正=偏右
bool visionValid = false;

// ===== 纵向 EKF（简化标量）=====
struct KalmanScalar {
  float x, P, Q, R;
  void init(float x0, float q, float r) { x = x0; P = 1.0; Q = q; R = r; }
  float update(float meas) {
    P += Q;
    float K = P / (P + R);
    x += K * (meas - x);
    P = (1 - K) * P;
    return x;
  }
};
KalmanScalar kfDist;

// 纵向状态机（Stop-Slow-Follow 滞回）
enum LongState { STOP, SLOW, FOLLOW };
LongState longState = FOLLOW;

// ===== 控制参数 =====
const float TARGET_DIST = 1.5;
const float MAX_SPEED = 0.7;     // m/s
const float LATERAL_GAIN = 1.5;  // 横向增益
const float DT = 0.05;

void setup() {
  Serial.begin(115200);
  kfDist.init(TARGET_DIST, 0.01, 0.3);

  encL.init(); encR.init();
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

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

  // ===== 1. 纵向：UWB 距离 → EKF 平滑 → 状态机 =====
  float linearCmd = 0;
  if (uwbValid && uwbDist > 0.2 && uwbDist < 8.0) {
    float filteredDist = kfDist.update(uwbDist);

    // 滞回状态机：防止距离在阈值附近抖动导致速度反复切换
    switch (longState) {
      case STOP:
        if (filteredDist > TARGET_DIST + 0.3) longState = SLOW;
        linearCmd = 0;
        break;
      case SLOW:
        if (filteredDist < TARGET_DIST - 0.2) longState = STOP;
        else if (filteredDist > TARGET_DIST + 0.6) longState = FOLLOW;
        else linearCmd = (filteredDist - TARGET_DIST) * 0.3;  // 低速逼近
        break;
      case FOLLOW:
        if (filteredDist < TARGET_DIST + 0.2) longState = SLOW;
        else linearCmd = constrain((filteredDist - TARGET_DIST) * 0.8, 0, MAX_SPEED);
        break;
    }
  } else {
    linearCmd = 0;  // UWB 失效，纵向停车
  }

  // ===== 2. 横向：视觉偏差 → 角速度（独立通道）=====
  float angularCmd = 0;
  if (visionValid) {
    // 视觉侧向偏差映射为角速度，方向：偏右→左转（负角速度）
    angularCmd = constrain(-visionLateral * LATERAL_GAIN, -1.0, 1.0);
  }
  // 视觉失效：横向保持当前航向（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);
}

解读：该案例直接映射了学术论文中的解耦架构：“纵向跟随和横向安全由不同的误差源和时间特性影响，直接耦合异构感知信息会增加控制不确定性”。kfDist 仅平滑 UWB 距离，visionLateral 仅驱动角速度，两者在控制指令层汇合但计算路径完全独立。纵向状态机的滞回设计（STOP→SLOW→FOLLOW 的阈值不对称）对应论文中“EKF + 滞回状态机将波动的距离观测转化为稳定的速度指令”的方法。

2、视觉侧向约束触发 + UWB 纵向速度降级（安全增强）
场景：当视觉检测到机器人即将偏离道路边界或靠近侧方障碍时，横向纠偏可能不够——高速下即使打满方向也可能来不及。需要在视觉侧向偏差过大时，主动降低 UWB 纵向速度上限，为横向纠偏争取时间。这是解耦架构下的跨通道安全联动：横向约束“反压”纵向速度。

#include <SimpleFOC.h>

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

float uwbDist = 2.0;
bool uwbValid = false;
float visionLateral = 0.0;
bool visionValid = false;

// 侧向安全参数
const float LATERAL_WARN = 0.4;   // 侧向偏差警告阈值
const float LATERAL_DANGER = 0.7; // 危险阈值
const float TARGET_DIST = 1.5;

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

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

  // ===== 纵向基础速度（UWB）=====
  float baseLinear = 0;
  if (uwbValid && uwbDist > 0.2 && uwbDist < 8.0) {
    baseLinear = constrain((uwbDist - TARGET_DIST) * 0.7, 0, 0.7);
  }

  // ===== 横向视觉约束 =====
  float angularCmd = 0;
  float speedScale = 1.0;  // 纵向速度缩放

  if (visionValid) {
    float absLat = fabs(visionLateral);

    // 横向纠偏角速度
    angularCmd = constrain(-visionLateral * 1.8, -1.2, 1.2);

    // ===== 侧向偏差反压纵向速度 =====
    if (absLat > LATERAL_DANGER) {
      speedScale = 0.2;  // 严重偏出：大幅降速
    } else if (absLat > LATERAL_WARN) {
      speedScale = 0.5;  // 警告：中等降速
    }
  }

  float linearCmd = baseLinear * speedScale;

  // ===== 差速执行 =====
  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);
}

解读：该案例在解耦基础上增加了安全联动。纯解耦架构中，横向纠偏和纵向速度互不影响，但在极端侧向偏差下，仅靠角速度纠偏可能来不及。speedScale 根据 absLat 降低纵向速度，给横向纠偏留出时间窗口。学术论文中视觉侧向约束的目标是“监控偏离和碰撞风险，并触发自适应航向校正”，本案例的 LATERAL_WARN/LATERAL_DANGER 阈值正是这一逻辑的工程实现。视觉失效时 speedScale 保持 1.0（不降速），因为无视觉信息时不应假设危险存在。

3、UWB 角度辅助 + 视觉丢失时的横向降级策略
场景：视觉在强光或遮挡下短暂失效，横向通道失去输入。如果直接保持当前角速度，机器人可能持续偏转；如果角速度归零，之前的纠偏努力白费。本案例利用 UWB 的低精度角度信息作为横向降级输入：视觉正常时 UWB 角度不参与控制，视觉失效时 UWB 角度以低增益接管，维持大致朝向而非完全失控。

#include <SimpleFOC.h>

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

// UWB 提供距离和（低精度）角度
float uwbDist = 2.0;
float uwbAngle = 0.0;   // 目标相对机器人的方位角 (rad)
bool uwbValid = false;

// 视觉侧向偏差
float visionLateral = 0.0;
bool visionValid = false;
unsigned long lastVisionMs = 0;
const unsigned long VISION_TIMEOUT = 300;  // 视觉丢失判定

// 状态
bool visualLost = false;

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

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

  // ===== 视觉丢失检测 =====
  if (visionValid) {
    lastVisionMs = millis();
    visualLost = false;
  } else if (millis() - lastVisionMs > VISION_TIMEOUT) {
    visualLost = true;
  }

  // ===== 纵向（UWB，不受视觉状态影响）=====
  float linearCmd = 0;
  if (uwbValid && uwbDist > 0.2 && uwbDist < 8.0) {
    linearCmd = constrain((uwbDist - 1.5) * 0.7, 0, 0.6);
  }

  // ===== 横向：视觉优先，UWB 降级 =====
  float angularCmd = 0;

  if (!visualLost && visionValid) {
    // 视觉正常：高增益侧向纠偏
    angularCmd = constrain(-visionLateral * 1.8, -1.2, 1.2);
  } else if (uwbValid) {
    // 视觉丢失：UWB 角度低增益接管，维持大致朝向
    // UWB 角度噪声大，仅用于防止完全失控，增益极低
    angularCmd = constrain(-uwbAngle * 0.3, -0.4, 0.4);
  }
  // 两者都失效：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);
}

解读：该案例处理了解耦架构的一个实际缺口：视觉侧向约束依赖视觉的连续性，但视觉在真实场景中会短暂失效。VISION_TIMEOUT 判定丢失后，横向通道从“视觉高增益”降级到“UWB 低增益”。UWB 角度精度不足以做精细侧向控制，但足以“维持大致朝向”，避免机器人因角速度归零而保持一个已经偏转的航向继续前进。搜索结果中 DAMO 案例的多模态抗遮挡策略同样采用“视觉失效时切换备用模态”的分级降级逻辑，本案例将其适配到解耦架构的横向通道中。

要点解读

1. 解耦的核心动机是“纵向和横向的误差源与时间特性不同”，而非“架构好看”
   学术论文明确指出：纵向跟随的误差源是人员间歇性运动导致的 UWB 距离波动，需要 EKF 平滑和状态机滞回来稳定速度；横向安全的误差源是道路边界或障碍物的视觉特征，需要实时响应和航向校正。如果像传统融合方案那样把 UWB 距离和视觉角度加权成一个“综合误差”，UWB 的角度噪声会污染纵向速度指令，视觉的帧率波动会干扰横向纠偏。案例一的 kfDist 和 visionLateral 在代码路径上完全分离，汇合点仅在差速解算，这是解耦的工程含义。

2. UWB 在解耦架构中的角色是“纵向测距仪”，不是“定位系统”
   案例中 UWB 仅用于获取 uwbDist，uwbAngle 在视觉正常时完全不参与控制（案例三仅在视觉丢失时以极低增益接管）。这与 DAMO 案例中将 UWB 的距离和角度同时用于融合控制的做法有本质区别。学术论文采用的单锚点单标签 UWB 配置同样以纵向距离为主要输出，角度估计仅作为辅助。如果强行用 UWB 角度做横向控制，其分米级精度和安装角度偏差会导致机器人持续蛇形。

3. 纵向状态机的滞回阈值必须不对称，否则会在目标距离附近反复切换
   案例一的 STOP → SLOW 阈值是 TARGET_DIST + 0.3，SLOW → FOLLOW 阈值是 TARGET_DIST + 0.6，FOLLOW → SLOW 阈值是 TARGET_DIST + 0.2。这种不对称设计使进入低速和退出低速的“门槛”不同，防止人员停下时机器人在目标距离 ±10cm 范围内反复启停。学术论文中的“Stop-Slow-Follow 有限状态机”正是为了解决“间歇性运动导致的速度切换不稳定”。如果使用单阈值，UWB 的测量噪声（RMSE 在 2-4cm 量级）就足以触发抖动。

4. 视觉侧向约束需要在“纠偏”之外增加“降速”联动，纯角速度纠偏在高速下不够
   案例二的 speedScale 是解耦架构的安全冗余。当侧向偏差超过 LATERAL_WARN 时，纵向速度降为 50%；超过 LATERAL_DANGER 时降为 20%。这背后的物理直觉是：横向纠偏的角速度有上限（受轮胎附着力和电机力矩限制），如果机器人以 0.7m/s 前进而侧向偏差正在快速增大，即使打满方向也可能在纠偏完成前越过边界。降速为纠偏争取时间。学术论文的视觉侧向约束方法同样以“触发航向校正”为核心，但未明确提及降速联动；案例二的 speedScale 是对这一缺口的工程补充。

5. 视觉失效时的横向降级必须有明确的“备用源”或“安全保持策略”
   案例三的 visualLost 判定和 uwbAngle * 0.3 降级逻辑对应搜索结果中“多模态抗遮挡”的通用原则：视觉连续丢帧时，不能把 0 值当作有效横向偏差（那等于告诉机器人“你已经在正中间了”），也不能保持上一帧的角速度指令（那会让偏转持续累积）。UWB 角度作为低精度备用源，虽然不能做精细纠偏，但能防止航向漂移。如果 UWB 也不可用，angularCmd = 0 是最后的安全保持——保持当前航向而非盲目转向，让操作者有介入时间。DAMO 案例的状态机在双模态失效时最终进入 ST_LOST 停车搜索，本案例的 angularCmd=0 是更保守的“保持航向”策略，适用于有操作员在附近可随时接管的情境。

4、UWB 距离环主导的纵向跟随
UWB 输出"距离 + 粗方位"，本案例只用距离做纵向环，方位先缓存备用；重点处理 TOF 跳变与丢帧。

/* ==========================================================
   平台：ESP32-S3 + DWM1000(UWB) + 2×BLDC(SimpleFOC) 差速
   纵向环：UWB 距离 → 速度指令；20Hz 有效量测 / 100Hz 外推
   ========================================================== */
#include <Arduino.h>
#include <SimpleFOC.h>
#include <SPI.h>

BLDCMotor      M_L(7), M_R(7);
BLDCDriver3PWM D_L(32,33,25), D_R(26,27,14);
Encoder        E_L(34,35,2048), E_R(36,39,2048);

const float R_W = 0.065f, TRACK = 0.28f;

/* ---------- UWB 量测 ---------- */
typedef struct {
  float    dist;      // m  本车→目标的水平距离
  float    bearing;   // rad 目标相对本车航向的方位（双基站解算）
  uint32_t t_ms;      // 采集时刻，用于时延补偿
  bool     valid;
} UwbMsg_t;
UwbMsg_t uwb = {0};
volatile uint32_t uwbLastOk = 0;      // 最近一次有效帧时刻

/* ---------- 距离滤波：3 点中值去跳变 + 一阶低通 ---------- */
class RangeFilter {
public:
  float buf[3] = {0, 0, 0};
  int   n = 0;
  float y = -1.0f;
  float push(float x) {
    buf[0] = buf[1]; buf[1] = buf[2]; buf[2] = x;
    if (n < 3) n++;
    float m;
    if (n < 3) m = x;
    else {
      float a = buf[0], b = buf[1], c = buf[2];
      m = fmaxf(fminf(a, b), fminf(fmaxf(a, b), c));   // 中值
    }
    y = (y < 0) ? m : y + 0.25f * (m - y);
    return y;
  }
};
static RangeFilter rf;

/* ---------- 纵向环参数（须台架标定） ---------- */
const float D_DES    = 1.20f;   // 期望跟随距离 m
const float D_DEAD   = 0.06f;   // 距离死区 m，抑制微抖
const float V_MAX    = 1.50f;
const float KP_D     = 0.90f;   // 距离 P
const float KD_D     = 0.18f;   // 距离 D，抑制接近时的超调
const uint32_t LINK_TIMEOUT = 300;   // ms，超过即判定失联

static float ePrev = 0.0f;

/* UWB 中断/任务里应调用：把原始帧喂进队列，这里给解析结果 */
void onUwbFrame(float d, float b, uint32_t t) {
  const float D_MIN = 0.15f, D_MAX = 8.0f;
  if (d < D_MIN || d > D_MAX) return;      // 明显异常，丢弃
  uwb.dist    = d;
  uwb.bearing = b;
  uwb.t_ms    = t;
  uwb.valid   = true;
  uwbLastOk   = millis();
}

/* ---------- 纵向速度指令 ---------- */
float longitudinalCmd(float dt) {
  uint32_t now = millis();

  /* 失联降级：先斜坡收油，再停 */
  if (now - uwbLastOk > LINK_TIMEOUT) {
    static float vSafe = 0.0f;
    vSafe += (0.0f - vSafe) * fminf(1.0f, 3.0f * dt);   // 时间常数 ~0.33s
    ePrev = 0.0f;
    return vSafe;
  }

  float d  = rf.push(uwb.dist);
  float e  = d - D_DES;

  /* 死区：目标静止且距离合理时不主动蠕动 */
  if (fabsf(e) < D_DEAD) { ePrev = e; return 0.0f; }

  float de = (e - ePrev) / fmaxf(dt, 1e-3f);
  ePrev = e;

  float v = KP_D * e + KD_D * de;
  return constrain(v, -V_MAX * 0.6f, V_MAX);   // 后退限速更严
}

void setup() {
  Serial.begin(115200);
  M_L.linkDriver(&D_L); M_R.linkDriver(&D_R);
  M_L.linkSensor(&E_L); M_R.linkSensor(&E_R);
  D_L.voltage_power_supply = D_R.voltage_power_supply = 12.0f;
  D_L.init(); D_R.init();
  M_L.init(); M_R.init(); M_L.initFOC(); M_R.initFOC();
  M_L.torque_controller = M_R.torque_controller = TorqueControlType::voltage;
}

void loop() {
  static uint32_t tPrev = micros();
  uint32_t now = micros();
  float dt = (now - tPrev) * 1e-6f;
  if (dt <= 0 || dt > 0.05f) dt = 2e-3f;
  tPrev = now;

  float vCmd = longitudinalCmd(dt);      // 差速底盘：本例侧向环输出 0
  float wCmd = 0.0f;

  float vL = vCmd - wCmd * TRACK * 0.5f;
  float vR = vCmd + wCmd * TRACK * 0.5f;
  const float Kp = 0.35f;
  M_L.target = Kp * (vL - E_L.getVelocity() * R_W);
  M_R.target = Kp * (vR - E_R.getVelocity() * R_W);
  M_L.loopFOC(); M_L.move();
  M_R.loopFOC(); M_R.move();
}

5、视觉侧向约束
侧向约束不直接输出横向速度——差速底盘没有横向自由度，视觉偏差最终必须映射为角速度 ω，再通过"前进 + 转向"合成侧向位移。

/* ==========================================================
   侧向环：OpenMV 目标框水平偏差 → 方位角 → 侧向偏差 → ω
   视觉 30Hz；不参与纵向控制，仅做侧向约束与距离交叉校验
   ========================================================== */
#include <Arduino.h>

/* ---------- 视觉帧（OpenMV 串口定长上报） ---------- */
typedef struct {
  float    u_err;    // (u_obj - u_cx)/f_pix，归一化水平偏差（rad 近似）
  float    box_w;    // 目标框像素宽，用于针孔测距
  uint32_t t_ms;
  bool     valid;
} VisMsg_t;
VisMsg_t vis = {0};
volatile uint32_t visLastOk = 0;

const float F_PIX     = 240.0f;   // 焦距（像素），需棋盘格标定
const float TARGET_W  = 0.22f;    // 目标实际宽度 m
const uint32_t VIS_TIMEOUT = 250; // ms

/* 视觉内参：光心相对车体中心的横向偏移（m），安装误差要补偿 */
const float CAM_OFFSET_Y = 0.0f;

/* ---------- 侧向环参数 ---------- */
const float KP_Y   = 1.60f;    // 侧向 P
const float KD_Y   = 0.22f;    // 侧向 D
const float W_MAX  = 1.80f;    // rad/s
static float eyPrev = 0.0f;
static float wFilt  = 0.0f;

/* 视觉距离估计：仅作交叉校验，不进入控制 */
static float visionRange(void) {
  if (vis.box_w < 4.0f) return -1.0f;      // 框太小，测距无效
  return F_PIX * TARGET_W / vis.box_w;
}

/* ---------- 侧向速度（角速度）指令 ---------- */
float lateralCmd(float dt, float dUwb) {
  uint32_t now = millis();

  /* 目标出框 / 遮挡 / 串口超时：侧向增益必须在 ~0.3s 内衰减到 0，
     绝不能用几十毫秒前的旧偏差继续打方向 */
  if (now - visLastOk > VIS_TIMEOUT || !vis.valid) {
    wFilt += (0.0f - wFilt) * fminf(1.0f, 3.5f * dt);
    eyPrev = 0.0f;
    return wFilt;
  }

  /* 归一化偏差 → 方位角 → 侧向偏差（小角近似） */
  float alpha = atanf(vis.u_err);
  float ey    = dUwb * tanf(alpha) - CAM_OFFSET_Y;

  float dey = (ey - eyPrev) / fmaxf(dt, 1e-3f);
  eyPrev = ey;

  /* 符号约定：图像 u 轴向右为正，目标在画面右侧 → 需右转 → ω 取负 */
  float w = -(KP_Y * ey + KD_Y * dey);

  /* 一阶低通：视觉量测噪声比 UWB 大一个量级 */
  wFilt += 0.35f * (w - wFilt);
  return constrain(wFilt, -W_MAX, W_MAX);
}

/* ---------- 在 loop 中与纵向环合并 ---------- */
/*
void loop() {
  ... dt 计算、vCmd = longitudinalCmd(dt) ...
  float wCmd = lateralCmd(dt, uwb.dist);

  // 距离交叉校验：视觉测距与 UWB 差值过大时，怀疑目标被部分遮挡
  float dVis = visionRange();
  bool visTrust = (dVis > 0) && (fabsf(dVis - uwb.dist) < 0.35f);

  if (!visTrust) wCmd *= 0.4f;      // 降权而非直接切断，避免抖动

  float vL = vCmd - wCmd * TRACK * 0.5f;
  float vR = vCmd + wCmd * TRACK * 0.5f;
  ...
}
*/

6、双环融合与失效状态机
纵向来自 UWB、横向来自视觉，两条通路各自会断，因此必须有一个显式的降级状态机，而不是散落的 if 判断。本案例额外加入 UWB 方位与视觉方位的一致性校验，用于识别 UWB 多径或视觉误检。

/* ==========================================================
   融合层：纵横向解耦 + 量测一致性校验 + 四级降级
   FULL → UWB_ONLY → VIS_ONLY → SAFE_STOP
   ========================================================== */
#include <Arduino.h>

enum FollowState { ST_FULL, ST_UWB_ONLY, ST_VIS_ONLY, ST_SAFE };
static FollowState st = ST_FULL;

/* ---------- 一致性校验：UWB 方位 vs 视觉方位 ---------- */
const float CONSIST_TH   = 0.28f;   // rad，约 16°
const int   CONSIST_N    = 6;       // 连续帧数阈值
static int  inconsCnt = 0;

/* ---------- 状态机 ---------- */
FollowState updateState(bool uwbOk, bool visOk, float dUwb,
                        float bUwb, float alphaVis) {
  uint32_t now = millis();
  uwbOk = uwbOk && (now - uwbLastOk < LINK_TIMEOUT);
  visOk = visOk && (now - visLastOk < VIS_TIMEOUT);

  /* 一致性校验只在两者都有效时才做 */
  if (uwbOk && visOk) {
    float dAng = fabsf(normalizeAngle(bUwb - alphaVis));
    /* 远距离时方位差的角度分辨率本身会放大，阈值按距离放松 */
    float th = CONSIST_TH + 0.03f * dUwb;
    if (dAng > th) inconsCnt++;
    else           inconsCnt = 0;
  } else {
    inconsCnt = 0;
  }

  if (!uwbOk && !visOk)                 return ST_SAFE;
  if (inconsCnt >= CONSIST_N)           return ST_VIS_ONLY;  // 判 UWB 多径
  if (!uwbOk)                           return ST_VIS_ONLY;
  if (!visOk)                           return ST_UWB_ONLY;
  return ST_FULL;
}

/* ---------- 按状态给出 (v, ω) ---------- */
void fusedCmd(float dt, float &vOut, float &wOut) {
  FollowState s = updateState(uwb.valid, vis.valid,
                              uwb.dist, uwb.bearing, atanf(vis.u_err));

  if (s != st) {
    /* 状态切换时清零微分项，防止 D 项在切换瞬间产生脉冲 */
    eyPrev = 0.0f;
    ePrev  = 0.0f;
    wFilt  = 0.0f;
    st = s;
  }

  switch (st) {
  case ST_FULL:
    vOut = longitudinalCmd(dt);
    wOut = lateralCmd(dt, uwb.dist);
    break;

  case ST_UWB_ONLY:
    /* 视觉丢失：纵向照旧，侧向退化为 UWB 粗方位环，带宽减半 */
    vOut = longitudinalCmd(dt) * 0.7f;
    wOut = -0.8f * normalizeAngle(uwb.bearing) * 0.5f;
    break;

  case ST_VIS_ONLY:
    /* UWB 丢失：无绝对距离基准，改为"视觉测距 + 恒定间距"保守跟随 */
    {
      float dVis = visionRange();
      if (dVis > 0.3f) {
        float e = dVis - D_DES;
        vOut = constrain(0.7f * e, -0.35f, 0.70f);   // 全局限速
      } else {
        vOut = 0.0f;
      }
      wOut = lateralCmd(dt, (dVis > 0.3f ? dVis : D_DES));
    }
    break;

  case ST_SAFE:
  default:
    vOut = 0.0f;
    wOut = 0.0f;
    /* 此处应上报故障；若装有距离传感器，可在此触发急停 */
    break;
  }
}

/* ---------- 底层：解耦结果 → 差速轮速 ---------- */
void applyWheel(float v, float w) {
  float vL = v - w * TRACK * 0.5f;
  float vR = v + w * TRACK * 0.5f;
  const float Kp = 0.35f;
  M_L.target = Kp * (vL - E_L.getVelocity() * R_W);
  M_R.target = Kp * (vR - E_R.getVelocity() * R_W);
}

void loop() {
  static uint32_t tPrev = micros();
  uint32_t now = micros();
  float dt = (now - tPrev) * 1e-6f;
  if (dt <= 0 || dt > 0.05f) dt = 2e-3f;
  tPrev = now;

  float v = 0, w = 0;
  fusedCmd(dt, v, w);

  /* 轮速饱和预判：差速底盘上 ω 会吃掉纵向速度余量，
     超限时优先保 ω（姿态）还是保 v（跟住距离）需按场景定，
     跟人场景下优先保 ω，避免转弯跑到目标侧后方 */
  float wNeed = fabsf(w) * TRACK * 0.5f;
  float vAvail = fmaxf(0.0f, V_MAX - wNeed);
  if (v > vAvail) v = vAvail;

  applyWheel(v, w);
  M_L.loopFOC(); M_L.move();
  M_R.loopFOC(); M_R.move();
}

要点解读
解耦的本质是量测责任划分，不是控制器分离。 UWB 的测距精度（厘米级，与距离弱相关）和视觉的方位精度（像素级，与距离强相关）性质完全相反，把两者塞进同一个位姿误差向量做 PI，会出现"目标做圆周运动时纵向环把方位误差当距离误差修正"的串扰。正确做法是纵向环只接收 d，横向环只接收 α，中间量 e_y = d·tan(α) 是唯一的乘积耦合点，它随距离变化会改变侧向环的开环增益，因此 KP_Y 必须对距离做归一化（或直接用方位角做被控量）才能让整段距离范围内响应一致。

时延差必须显式补偿，不能靠调增益硬扛。 UWB 从测距到解算输出通常有 50~100 ms 延迟，视觉 30 fps 意味着 33 ms 采样加处理时延；而底层速度环在 500 Hz~1 kHz。用 millis() 给每帧打时间戳、再按当前车速外推更新，比盲目降低 KP_D 更有效——后者会让跟随在目标急停时出现明显的冲过头。案例中的 t_ms 字段不是装饰，量测外推用 d_pred = d_meas + v·Δt 即可，误差远小于不做补偿。

两条环的带宽要错开两倍以上。 纵向环 0.8~1.5 Hz、横向环 2~3 Hz 是实测比较稳的配置。原因在于差速底盘的 yaw 变化会立即改变车体前向方向，从而耦合进距离量测（转向时轮速差导致的里程误差、UWB 天线相位中心旋转），两者带宽接近时会出现"越调越晃"的极限环。横向环取更高带宽还有个实际理由：侧向偏差在视觉上有直接、低延迟的观测，而 UWB 距离受多径影响更新质量更差。

失效降级必须是有状态、有斜坡的，不能是瞬时切换。 视觉丢帧在真实场景里非常频繁（遮挡、逆光、目标转身），如果直接切到 UWB 侧向环，等于在几十毫秒内把 KP 从 1.6 换成 0.4 并改变符号约定，车会明显抖一下。案例二、三都用了 ~0.3 s 时间常数的增益衰减，且状态切换时强制清零 D 项——微分项是最容易在切换瞬间产生尖峰的地方。另外 UWB 与视觉的全失效不应只停车，还应考虑"最后已知方位 + 主动搜索"或上报，否则目标绕到车后时会永久失联。

安装标定决定这套方案的精度上限。 四个必须标定项：UWB 天线的取整偏差与天线延迟（用已知距离的多次静态测量拟合，不做这步会引入 10~20 cm 恒定偏置，直接变成跟随距离的稳态误差）；双基站构型的几何精度衰减（基线太短会让方位解算在近处剧烈抖动，建议基线不小于 0.3 m 并避开金属车身反射）；相机焦距 F_PIX 与光心位置（棋盘格标定，光心与车体中心的横向偏移 CAM_OFFSET_Y 必须补偿，否则直线跟随时车会偏向一侧持续偏航）；两者坐标系外参（UWB 方位零度与相机光轴零度需对齐，否则一致性校验的阈值无法设定）。调参顺序建议是：先单环开环验证符号与量级，再分别闭环调好纵向与横向，最后才打开降级状态机与一致性校验。

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