“Arduino BLDC 之多机器人交互 + DWA 速度可达性筛选环”可以理解为：多台 BLDC 差速机器人在共享邻居状态的基础上，各自运行 DWA 局部规划器；在动态窗口内先按动力学、加速度和制动距离筛选出“可达且安全”的速度集合，再引入多机器人碰撞风险、编队/跟随约束和优先级规则，最终选出可执行的速度指令，由 BLDC FOC 平滑落地。
一、主要特点

1. DWA 把“路径规划”转化为“速度空间筛选”
   DWA 不直接在地图上搜索路径，而是在当前速度 (v, ω) 附近采样候选速度对，预测未来短时间轨迹，再评价哪一组最接近目标、最远离障碍、最平滑。
   “速度可达性筛选环”通常包含三层约束：
   硬件极限窗口：最大线速度、最大角速度；
   动力学窗口：当前速度加上加速度约束后，下一周期真正能达到的速度范围；
   安全制动窗口：保证在撞上障碍物前能刹停。
2. 多机器人交互让 DWA 从“避障”升级为“避人/避队友”
   单机 DWA 只把障碍当成静态或动态障碍物；多机场景下，每个邻居都有速度和航向，碰撞风险会随时间变化。常见做法是在评价函数中加入：
   邻居预测轨迹距离；
   相对速度方向；
   编队误差或跟随间距；
   同时到达/协同时间约束；
   优先级让行代价。
   这样机器人不会只躲障碍，还会主动避免与队友形成对称死锁。
3. 可达性筛选环是实时安全闸门
   即使上层给出目标速度、编队速度或跟随速度，也必须先经过可达性筛选：
   是否超过电机/电池能力；
   是否超过加速度和 jerk 限制；
   是否能在制动距离内停下；
   是否与邻居未来位置冲突；
   是否会导致队形撕裂或跟随目标丢失。
   只有全部通过，才会下发给 BLDC 执行。
4. BLDC FOC 保证速度指令可平滑跟踪
   DWA 输出的速度指令通常是连续变化的，尤其在窄通道、交叉会车、编队变形时更明显。BLDC 配合 FOC 电流环、速度环和编码器闭环，可降低低速转矩脉动，使加减速、原地转向、微幅修正更平稳。
   如果只用开环 ESC，速度指令容易抖动，DWA 的平滑优势会被执行层抵消。
5. 适合“上位机协商 + 下位机执行”的分层架构
   多机交互涉及邻居状态同步、优先级协商、队形维护和冲突检测；DWA 需要轨迹预测和代价评估；BLDC FOC 需要稳定高频闭环。工程上更常见的做法是：上位机/ESP32-S3/STM32 负责多机交互和 DWA 筛选，Arduino 或同类 MCU 负责电机闭环、编码器采样和安全限幅。
   二、应用场景
6. 仓储多 AGV 协同搬运
   多台机器人在货架通道、十字路口、装卸点相遇。DWA 负责局部避障和速度筛选，多机交互负责会车优先级和通行权分配，避免窄道死锁。
7. 园区巡逻编队
   多台机器人按拓扑路径巡逻，遇到行人、车辆或临时障碍时保持编队间距。DWA 处理局部绕行，交互层处理队形压缩、侧移让行和重新归队。
8. 餐厅/展厅服务机器人集群
   空间狭窄、人流复杂，机器人之间容易在门口、拐角、餐桌间相互阻挡。通过 DWA 可达性筛选和优先级规则，可实现平滑让行，而不是同时后退造成尴尬停滞。
9. 跟随型多机任务
   例如一台领航机器人携带传感器，若干跟随机器人保持安全间距。DWA 保证跟随过程中不撞障碍，交互层保证跟随间距、队形角度和通信丢失后的降级行为。
10. 科研与竞赛平台
    适合验证 DWA 速度采样、多机优先级、对称死锁破解、编队变形、ESP-NOW/UDP 低延迟通信和 BLDC 差速闭环。
    三、需要注意的事项
11. 可达性窗口必须基于真实标定参数
    最大速度、最大加速度、制动距离、轮径、轮距和最小转弯半径都需要实测。若参数偏乐观，DWA 会选出“理论上可达、实际上执行不了”的速度，导致跟踪误差、打滑或碰撞。
12. 多机状态共享要有时间戳和预测补偿
    邻居位置如果只有坐标没有速度、航向和时间戳，避碰判断会滞后。应根据通信延迟对邻居做短时位置预测，并在通信丢失时降级为保守避障。
13. 优先级规则必须全局一致
    如果 A 机按“先到者优先”，B 机按“任务重者优先”，容易形成对称死锁。应预先约定统一规则，例如任务等级、等待时间、距离冲突点远近、主干道优先等，并加入超时防饥饿机制。
14. DWA 评价函数权重需随环境动态调整
    空旷区域可提高速度权重，提升通行效率；人流密集或窄通道应提高障碍和邻居距离权重，缩短预测窗口；编队任务中还需加入队形保持权重。固定权重容易在复杂场景下出现停滞或贴障行驶。
15. 避免“只避障、不协同”
    如果每台机器人只把队友当成普通动态障碍，可能出现反复让行、路径振荡或编队撕裂。更稳妥的做法是：局部层用 DWA 保证安全，交互层用优先级、避让点或协同到达时间约束保证整体行为一致。
16. BLDC 执行层必须限速、限流、限加速度
    多机交互常触发频繁加减速、原地转向和微幅修正。若不限制 q 轴电流和角速度上限，容易引起电压跌落、编码器噪声、主控复位或底盘抖动。
17. 8 位 Arduino 不宜承担完整多机 DWA
    ATmega328P 可以做速度闭环、基础传感器采集和简单 DWA 演示；但多机轨迹预测、邻居通信、优先级仲裁和代价函数评估更适合放在 ESP32、STM32 或上位机。底层 MCU 应保证 FOC 控制周期稳定，不被通信和规划任务阻塞。
18. 必须保留硬件级安全兜底
    当通信中断、定位跳变、编码器异常、电池欠压或邻居状态不可信时，应优先减速停机或进入保守等待，而不是继续执行编队或跟随指令。硬件急停优先级应高于任何算法协商。
    整体来看，这个方案的关键是：DWA 负责“当前这一下能走哪些速度”，多机器人交互负责“这些速度会不会和队友冲突”，可达性筛选环负责“把不可执行、不安全、不协同的速度提前剔除”，BLDC FOC 负责“把最终选出的速度平稳落地”。做原型时，建议先跑通“单机 DWA + BLDC 速度闭环”，再加入“邻居状态广播 → 多机代价项 → 优先级让行 → 编队/跟随约束”。

1、ARVO 速度障碍预筛选 + DWA 运动学可达性验证
适用场景：两台或多台机器人在狭窄通道中相向而行，需要先剔除会导致碰撞的速度组合，再从剩余候选中选择物理可达且朝向目标的最优速度。核心逻辑是“ARVO 做硬筛选，DWA 做软评价”。

#include <SimpleFOC.h>

// 双轮 BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8), driverR(5, 6, 7, 4);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);

// 机器人状态
float posX = 0, posY = 0, theta = 0;
float curV = 0, curW = 0;
float goalX = 5.0, goalY = 0.5;

// 其他机器人状态（模拟或通过通信更新）
struct RobotState { float x, y, vx, vy, radius; bool valid; };
RobotState others[2];

// 运动学约束
const float maxV = 0.6, maxW = 1.2;
const float maxAccV = 0.5, maxAccW = 1.0;
const float dt = 0.1;

// ARVO 自适应半径
float baseR = 0.35, adaptiveR = 0.35;

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();
  Serial.println("ARVO+DWA 速度筛选就绪");
}

// 简化 RVO 判定：若候选速度朝向他机且相对速度指向碰撞锥，则剔除
bool isInRVO(float v, float w) {
  for (int i = 0; i < 2; i++) {
    if (!others[i].valid) continue;
    float dx = others[i].x - posX;
    float dy = others[i].y - posY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist > 3.0) continue; // 远距离不触发 RVO
    // 相对位置方向
    float relAngle = atan2(dy, dx);
    // 自身运动方向
    float moveAngle = theta + w * dt;
    float angleDiff = fabs(moveAngle - relAngle);
    // 若朝向对方且距离接近安全半径，判定为危险
    if (angleDiff < 0.8 && dist < adaptiveR + 0.3) return true;
  }
  return false;
}

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

  // 模拟其他机器人运动（实际应从通信获取）
  others[0] = {3.0f + sin(millis()/2000.0f)*0.8f, 0.4f, -0.15f, 0.0f, 0.2f, true};
  others[1] = {4.5f, -0.3f, -0.1f, 0.0f, 0.2f, true};

  // ===== 第一层：ARVO 硬筛选 =====
  float candidateV[25], candidateW[25];
  int candidateCount = 0;

  for (int i = 0; i < 5; i++) {
    for (int j = 0; j < 5; j++) {
      float v = -maxV + 2 * maxV * i / 4.0f;
      float w = -maxW + 2 * maxW * j / 4.0f;
      if (!isInRVO(v, w)) {
        candidateV[candidateCount] = v;
        candidateW[candidateCount] = w;
        candidateCount++;
      }
    }
  }

  // ===== 第二层：DWA 运动学可达 + 评价 =====
  float bestV = 0, bestW = 0, bestCost = 1e6;

  for (int k = 0; k < candidateCount; k++) {
    float v = candidateV[k], w = candidateW[k];

    // 加速度约束：一个周期内能否达到
    if (fabs(v - curV) > maxAccV * dt) continue;
    if (fabs(w - curW) > maxAccW * dt) continue;

    // 预测位置
    float predX = posX + v * cos(theta) * dt;
    float predY = posY + v * sin(theta) * dt;
    float predTheta = theta + w * dt;

    // 航向代价
    float angleToGoal = atan2(goalY - predY, goalX - predX);
    float headingCost = fabs(predTheta - angleToGoal);

    // 障碍物代价
    float obsCost = 0;
    for (int m = 0; m < 2; m++) {
      if (!others[m].valid) continue;
      float d = sqrt(pow(others[m].x - predX, 2) + pow(others[m].y - predY, 2));
      if (d < adaptiveR) obsCost += (adaptiveR - d) * 80.0f;
    }

    float velocityCost = (maxV - v) * 0.3f;
    float total = headingCost * 1.5f + obsCost * 4.0f + velocityCost;

    if (total < bestCost) { bestCost = total; bestV = v; bestW = w; }
  }

  // ===== 执行 =====
  curV = bestV; curW = bestW;
  posX += curV * cos(theta) * dt;
  posY += curV * sin(theta) * dt;
  theta += curW * dt;

  // 差速解算
  float wheelBase = 0.3;
  float vL = curV - curW * wheelBase / 2;
  float vR = curV + curW * wheelBase / 2;
  motorL.move(vL / 0.05); // 轮半径 0.05m，转为 rad/s
  motorR.move(vR / 0.05);

  delay(100);
}

解读：该案例的核心是两层筛选。ARVO 层先剔除“速度空间中会导致未来碰撞”的候选，DWA 层再从剩余候选中验证加速度可达性并选择最优。搜索结果中的原始代码正是这一结构的直接体现。若跳过 ARVO 直接做 DWA，机器人在狭窄通道中容易陷入“反复计算但所有轨迹都有碰撞风险”的死锁状态；若跳过 DWA 的加速度约束，规划器可能输出电机无法瞬时达到的速度，导致实际运动与预测轨迹脱节。

2、多机器人状态广播 + 分布式 DWA 速度窗口裁剪
适用场景：三台以上机器人共享各自的位置与速度信息，每台机器人根据邻居状态动态收缩自己的 DWA 速度窗口，避免“同时向同一方向避让”造成的振荡。

#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

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

// 本机状态
struct MyState {
  uint8_t id;
  float x, y, theta, v, w;
} self = {1, 0, 0, 0, 0, 0};

// 邻居状态（ESP-NOW 接收）
struct PeerState {
  uint8_t id;
  float x, y, theta, v, w;
  bool valid;
} peers[4];

// DWA 参数
const float maxV = 0.5, maxW = 1.0;
const float safeDist = 0.6;

// ESP-NOW 接收回调
void onDataRecv(const uint8_t* mac, const uint8_t* data, int len) {
  PeerState* p = (PeerState*)data;
  for (int i = 0; i < 4; i++) {
    if (!peers[i].valid || peers[i].id == p->id) {
      peers[i] = *p;
      peers[i].valid = true;
      break;
    }
  }
}

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();

  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_register_recv_cb(onDataRecv);
}

// 根据邻居状态裁剪速度窗口
void clipWindowByPeers(float& vMin, float& vMax, float& wMin, float& wMax) {
  for (int i = 0; i < 4; i++) {
    if (!peers[i].valid) continue;
    float dx = peers[i].x - self.x;
    float dy = peers[i].y - self.y;
    float dist = sqrt(dx*dx + dy*dy);

    if (dist > safeDist * 2) continue;

    // 邻居在前方且同向：限制本机最大速度，避免追尾
    float relAngle = atan2(dy, dx) - self.theta;
    while (relAngle > PI) relAngle -= 2*PI;
    while (relAngle < -PI) relAngle += 2*PI;

    if (fabs(relAngle) < 0.5 && dist < safeDist * 1.5) {
      vMax = min(vMax, peers[i].v * 0.8); // 不超过前车速度的 80%
    }

    // 邻居在侧方：限制转向幅度，避免“甩尾”碰撞
    if (fabs(relAngle) > 0.5 && fabs(relAngle) < 1.8 && dist < safeDist) {
      if (relAngle > 0) wMin = max(wMin, 0.2); // 左侧有车，禁止左转
      else wMax = min(wMax, -0.2);              // 右侧有车，禁止右转
    }
  }
}

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

  // ===== DWA 速度采样（窗口已被邻居裁剪） =====
  float vMin = -maxV, vMax = maxV, wMin = -maxW, wMax = maxW;
  clipWindowByPeers(vMin, vMax, wMin, wMax);

  float bestV = 0, bestW = 0, bestScore = -1e6;
  float goalX = 5.0, goalY = 0.0;

  for (float v = vMin; v <= vMax; v += 0.1) {
    for (float w = wMin; w <= wMax; w += 0.15) {
      // 预测
      float predX = self.x + v * cos(self.theta) * 0.2;
      float predY = self.y + v * sin(self.theta) * 0.2;

      // 航向评分
      float angleToGoal = atan2(goalY - predY, goalX - predX);
      float headingScore = 1.0 - fabs(angleToGoal - self.theta) / PI;

      // 邻居距离评分
      float distScore = 1.0;
      for (int i = 0; i < 4; i++) {
        if (!peers[i].valid) continue;
        float d = sqrt(pow(peers[i].x - predX, 2) + pow(peers[i].y - predY, 2));
        if (d < safeDist) distScore *= d / safeDist;
      }

      float score = headingScore * 0.6 + distScore * 0.4;
      if (score > bestScore) { bestScore = score; bestV = v; bestW = w; }
    }
  }

  // 执行
  self.v = bestV; self.w = bestW;
  float vL = bestV - bestW * 0.15;
  float vR = bestV + bestW * 0.15;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);

  // 更新位姿（简化）
  self.x += bestV * cos(self.theta) * 0.1;
  self.y += bestV * sin(self.theta) * 0.1;
  self.theta += bestW * 0.1;

  // 广播自身状态（ESP-NOW，略）
  // esp_now_send(broadcastAddr, (uint8_t*)&self, sizeof(self));

  delay(100);
}

解读：该案例将多机器人交互的作用定位在DWA 速度窗口的前置裁剪上，而非在 DWA 内部处理所有邻居。这样做的好处是：DWA 的采样空间从源头被约束到“不会与邻居产生紧迫冲突”的区域，计算量显著降低，同时避免了多机同时避让时的对称振荡。搜索结果中关于 ARVO 自适应半径的讨论也指向同一设计原则：多机场景下，安全边界需要根据邻居状态动态调整，而非固定值。

3、DWA 速度可达性兜底 + 通信超时降级
适用场景：多机器人系统中，某台机器人的通信中断或邻居状态过期，DWA 需要从“协同规划”降级为“单车安全优先”模式，速度可达性筛选成为最后的安全防线。

#include <SimpleFOC.h>

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

// 状态
float posX = 0, posY = 0, theta = 0;
float curV = 0, curW = 0;
unsigned long lastPeerUpdate = 0;
bool peerDataValid = false;

// 运动学极限（DWA 可达性的硬边界）
const float maxV = 0.5, maxW = 1.0;
const float maxAccV = 0.4, maxAccW = 0.8;
const float dt = 0.1;

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 updatePeerData(float px, float py) {
  // 邻居位置可用，更新时间戳
  lastPeerUpdate = millis();
  peerDataValid = true;
  // 实际应将 px/py 存入邻居列表
}

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

  // 通信超时检测：超过 500ms 未收到邻居更新，降级
  if (millis() - lastPeerUpdate > 500) {
    peerDataValid = false;
  }

  // ===== DWA 速度采样 =====
  float bestV = 0, bestW = 0, bestScore = -1e6;

  for (float v = -maxV; v <= maxV; v += 0.1) {
    for (float w = -maxW; w <= maxW; w += 0.2) {

      // 【核心】DWA 可达性硬约束：加速度限制
      // 无论通信状态如何，这一步永远执行
      if (fabs(v - curV) > maxAccV * dt) continue;
      if (fabs(w - curW) > maxAccW * dt) continue;

      // 预测
      float predX = posX + v * cos(theta) * 0.3;
      float predY = posY + v * sin(theta) * 0.3;

      // 评分
      float goalX = 4.0, goalY = 0.0;
      float angleToGoal = atan2(goalY - predY, goalX - predX);
      float headingScore = 1.0 - fabs(angleToGoal - theta) / PI;

      // 若有邻居数据，加入避让评分；若无，仅依赖航向
      float avoidScore = 1.0;
      if (peerDataValid) {
        // 简化：假设邻居在 (2.0, 0.2)
        float d = sqrt(pow(2.0 - predX, 2) + pow(0.2 - predY, 2));
        if (d < 0.5) avoidScore = d / 0.5;
      }

      float score = headingScore * 0.5 + avoidScore * 0.5;
      if (score > bestScore) { bestScore = score; bestV = v; bestW = w; }
    }
  }

  // 若无有效速度（极端情况），执行安全制动
  if (bestScore < -1e5) {
    bestV = 0; bestW = 0;
  }

  // 执行
  curV = bestV; curW = bestW;
  posX += curV * cos(theta) * dt;
  posY += curV * sin(theta) * dt;
  theta += curW * dt;

  float vL = curV - curW * 0.15;
  float vR = curV + curW * 0.15;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);

  delay(100);
}

解读：该案例强调 DWA 速度可达性筛选的安全兜底价值。当多机器人通信正常时，邻居信息参与避让评分；当通信中断或数据过期时，系统降级为仅依赖自身运动学约束和航向目标的模式，但 maxAccV/maxAccW 的硬约束始终生效。这意味着即使上层协同逻辑完全失效，机器人也不会输出超出物理能力的速度指令，避免了因“规划器失控”导致的急加速或急转。搜索结果中关于 BLDC 高动态响应与 DWA 约束配合的讨论，也隐含了这一安全分层的思想：算法层的智能程度可以降级，但执行层的物理约束不可绕过。

要点解读

1. ARVO 与 DWA 是“筛选”与“评价”的分工，不是替代关系
   DWA 的核心产出是“运动学可行且评价最优的速度”，但它本身对动态障碍物的处理能力有限——它把障碍物当作静态点来算代价。ARVO 的介入方式是在 DWA 采样之后、评价之前，直接删除那些落入速度障碍区的候选。搜索结果中的代码结构清晰地体现了这一点：先做 RVO 判定生成候选列表，再做 DWA 加速度检查和代价评价。如果把 ARVO 的碰撞判定塞进 DWA 的评价函数里作为软代价，机器人可能会为了追求“低代价”而选择擦边球速度，在多机场景下这是危险的。

2. 多机器人交互的关键信息是“速度”而非仅“位置”
   仅共享位置信息的多机系统，在 DWA 层面只能做“距离避让”，效果等同于把其他机器人当作移动的静态障碍。真正有效的协同需要共享速度向量，因为 ARVO 的速度障碍计算依赖于相对速度。案例二的 PeerState 结构中包含 v 和 w 字段，正是为了在接收端能够计算相对速度并据此裁剪窗口。搜索结果中关于 ARVO 自适应膨胀半径的讨论也指出，固定半径在多机场景下会导致死锁或过度保守，而自适应调整的依据正是邻居的运动状态。

3. Arduino 的算力决定了 DWA 采样必须“少而准”
   标准 DWA 在 PC 上可以采样数百组速度对，但在 Arduino 上，每组候选都需要做三角函数、平方根和邻居遍历。案例一将采样限制在 5×5=25 组，案例三限制在约 50 组，这是嵌入式实现的现实妥协。提高效率的手段包括：先用 ARVO 做硬筛选减少候选数量（案例一），用通信窗口裁剪缩小采样范围（案例二），以及简化评价函数为加权线性组合而非复杂模型。搜索结果中明确建议，若需完整 DWA 加多障碍物 VO 计算，应使用 ESP32-S3 或 Teensy 4.x 而非 Arduino Uno。

4. BLDC FOC 的价值在于“执行规划器输出的连续速度”
   DWA 输出的 bestV 和 bestW 在连续两个控制周期之间是平滑变化的（因为加速度约束限制了跳变幅度）。如果底层使用开环 ESC 或有刷电机，电机响应滞后和死区会导致实际速度与规划速度脱节，DWA 的预测轨迹失去意义。SimpleFOC 的速度环能够以毫秒级响应跟踪 motor.move(targetVelocity) 的指令，使得“规划速度 ≈ 实际速度”，这是 DWA 评价函数中运动学预测模型能够成立的前提。搜索结果同样指出，BLDC 配合 FOC 的平滑执行是 ARVO+DWA 方案在物理层落地的基础。

5. 通信超时必须有明确的降级策略，而非“保持上一帧”
   多机器人系统中，某台机器人的通信中断是常态而非异常。案例三的处理方式是：超过 500ms 未收到邻居更新，将 peerDataValid 置为 false，DWA 评价中不再使用邻居位置，但运动学约束和航向目标继续生效。降级后的行为是“单车自主导航”，而非“继续按照过期邻居位置避让”。后者比前者更危险，因为过期位置可能指向一个已经空出来的区域，导致机器人做出不必要的急转；或者指向一个邻居已经离开的方向，导致漏避让。通信超时阈值的选择需要权衡：太短会导致频繁降级，太长会导致对已失效信息的过度信任。

4、DWA 动态窗口 + 可停性可达筛选（单机基础版）

硬件：ESP32 + SimpleFOC 差速底盘（2×BLDC）+ 编码器 + 三路超声（左/前/右）。

核心思路：在速度空间 (v, ω) 里采样，先用两道筛选把不可行速度剔除——① 动态窗口 Vd（受当前速度和加速度限制，本周期物理上能到达的速度）；② 可停性 Va（按该速度走，刹车距离内不能撞到障碍）。剩下的可行速度逐条前向仿真打分，取最优。

/*
 * 案例4：DWA 动态窗口 + 可停性筛选
 * 速度空间离散采样 -> Vd(可达) ∩ Va(可停) -> 前向仿真打分 -> 最优(v,w)
 */
#include <SimpleFOC.h>
#include <math.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 WHEEL_R = 0.065f, TRACK = 0.32f;
const float V_MAX = 0.80f, W_MAX = 1.60f;     // 最大线速度/角速度
const float A_V = 0.60f, A_W = 2.00f;          // 加速度上限
const float DT_CTRL = 0.05f;                   // DWA 周期 20Hz
const float SIM_DT = 0.10f, HORIZON = 1.50f;   // 仿真步长/时长
const float R_ROBOT = 0.18f;                   // 车体半径(含余量)

// ---- 位姿与目标 ----
float px = 0, py = 0, pth = 0;
float gx = 3.0f, gy = 0.0f;
float vCur = 0, wCur = 0;

// ---- 障碍点（三路超声转车体系坐标点）----
const int TRIG[3] = {6, 8, 10}, ECHO[3] = {7, 9, 11};
float obsX[3], obsY[3]; float obsD[3];         // 车体系障碍点
float readUS(int i) {
  digitalWrite(TRIG[i], LOW); delayMicroseconds(2);
  digitalWrite(TRIG[i], HIGH); delayMicroseconds(10); digitalWrite(TRIG[i], LOW);
  long d = pulseIn(ECHO[i], HIGH);
  return (d > 0) ? d * 0.034f / 2.0f / 100.0f : 4.0f;   // m
}
void updateObstacles() {
  const float ang[3] = {0.45f, 0.0f, -0.45f};           // 左/前/右 安装角
  for (int i = 0; i < 3; i++) {
    obsD[i] = readUS(i);
    obsX[i] = obsD[i] * cosf(ang[i]);
    obsY[i] = obsD[i] * sinf(ang[i]);
  }
}

struct Vel { float v, w; };

// ---- 筛选1：可停性（Va）——刹车距离必须短于障碍距离 ----
bool admissible(float v, float dObs) {
  float stopDist = (v * v) / (2.0f * A_V);   // 以 A_V 减速的制动距离
  return (stopDist + R_ROBOT) < dObs;        // 留车体半径余量
}

// ---- 前向仿真 + 打分 ----
float evalTraj(float v, float w, float &clearMin) {
  float x = px, y = py, th = pth;
  clearMin = 9.0f;
  for (float t = 0; t < HORIZON; t += SIM_DT) {
    x += v * cosf(th) * SIM_DT;
    y += v * sinf(th) * SIM_DT;
    th += w * SIM_DT;
    // 障碍点转到世界系做距离检查
    for (int i = 0; i < 3; i++) {
      float wx = px + obsX[i]*cosf(pth) - obsY[i]*sinf(pth);
      float wy = py + obsX[i]*sinf(pth) + obsY[i]*cosf(pth);
      float d = hypotf(wx - x, wy - y) - R_ROBOT;
      if (d < clearMin) clearMin = d;
    }
    if (clearMin < 0) return -1e9f;          // 仿真已撞：直接淘汰
  }
  // 评价三项：朝向目标 / 离障余量 / 速度
  float headErr = fabsf(atan2f(gy - y, gx - x) - th);
  float sHead = 1.0f - headErr / PI;                 // 0..1
  float sClear = constrain(clearMin / 1.0f, 0.0f, 1.0f);
  float sVel = v / V_MAX;
  return 2.0f*sHead + 1.5f*sClear + 0.8f*sVel;       // 权重可调
}

// ---- DWA 主流程 ----
void planDWA(Vel &best) {
  // 动态窗口 Vd：加速度可达区间 ∩ 全局限幅
  float vLo = max(-V_MAX, vCur - A_V * DT_CTRL);
  float vHi = min( V_MAX, vCur + A_V * DT_CTRL);
  float wLo = max(-W_MAX, wCur - A_W * DT_CTRL);
  float wHi = min( W_MAX, wCur + A_W * DT_CTRL);

  float bestScore = -1e9f; best = {0, 0};
  for (float v = vLo; v <= vHi + 1e-6f; v += 0.05f) {
    // 前方最近障碍距离（用于可停性判断）
    float dFront = min(obsD[1], 9.0f);
    if (!admissible(v, dFront)) continue;          // Va 筛选
    for (float w = wLo; w <= wHi + 1e-6f; w += 0.10f) {
      float clear; float s = evalTraj(v, w, clear);
      if (s > bestScore) { bestScore = s; best = {v, w}; }
    }
  }
  // 全被筛掉（贴障/紧急）：退回刹停
  if (bestScore < -1e8f) { best = {0, 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.3f; wheelL.PID_velocity.I = 3.0f;
  wheelR.PID_velocity.P = 0.3f; wheelR.PID_velocity.I = 3.0f;
  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);
  for (int i = 0; i < 3; i++) { pinMode(TRIG[i], OUTPUT); pinMode(ECHO[i], INPUT); }
  Serial.begin(115200);
}

void loop() {
  // 1. 里程计（差速运动学积分）
  float vl = wheelL.shaft_velocity * WHEEL_R;
  float vr = wheelR.shaft_velocity * WHEEL_R;
  vCur = 0.5f * (vl + vr);
  wCur = (vr - vl) / TRACK;
  px += vCur * cosf(pth) * DT_CTRL;
  py += vCur * sinf(pth) * DT_CTRL;
  pth += wCur * DT_CTRL;

  // 2. 感知
  updateObstacles();

  // 3. 规划
  Vel cmd; planDWA(cmd);

  // 4. 执行：差速解算 -> 速度环
  float vL = cmd.v - cmd.w * TRACK * 0.5f;
  float vR = cmd.v + cmd.w * TRACK * 0.5f;
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(vL / WHEEL_R); wheelR.move(vR / WHEEL_R);

  Serial.print("v="); Serial.print(cmd.v, 2);
  Serial.print(" w="); Serial.println(cmd.w, 2);
  delay(50);   // 20Hz；量产把两轮速度环移入 1kHz 定时器
}

提示：串口打印和 pulseIn 都在主循环里，量产必须把轮速环挂 1kHz 定时器中断，DWA 保持 20Hz。admissible() 只用前方距离是简化，工程上应取“沿该速度方向射线上最近障碍”。

5、多机交互 —— 共享预测轨迹 + 互惠避让 + 优先级仲裁

硬件：ESP32 + 差速 BLDC + 超声 + ESP-NOW（机间广播位姿与意图）。

核心思路：多机交互的关键，是先共享“我打算怎么走”（速度意图或预测轨迹），仅靠交换位置远远不够。每台车在评价函数里加一项“时空冲突代价”：把自己的仿真点在对应时刻与邻居的预测位置比距离；并按互惠系数各承担一半避让责任，避免双方同时大幅让开或同时不让。优先级只在互惠失效时兜底，即空车让重载车、低编号让高编号。

/*
 * 案例5：多机 DWA —— 共享预测 + 互惠避让 + 优先级兜底
 * 广播内容：位姿 + 当前(v,w) + 优先级；接收方按匀速模型外推邻居轨迹
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

struct PeerMsg {
  uint8_t id; float x, y, th; float v, w; uint8_t prio;
};
PeerMsg gPeers[4]; int gNPeer = 0;
uint8_t MY_ID = 1, MY_PRIO = 1;

// ---- 本机状态 ----
float px = 0, py = 0, pth = 0, vCur = 0, wCur = 0;
const float R_ROBOT = 0.18f, R_SAFE = 0.45f;     // 车半径 / 期望间距
const float RECIP = 0.5f;                        // 互惠系数：各担一半
const float SIM_DT = 0.10f, HORIZON = 1.20f;

// ---- 邻居轨迹外推（匀速模型，最省通信）----
void peerAt(const PeerMsg &p, float t, float &x, float &y) {
  x = p.x + p.v * cosf(p.th) * t;
  y = p.y + p.v * sinf(p.th) * t;
}

// ---- 交互代价：时空最近距离 -> 罚分 ----
float peerCost(float x0, float y0, float th0, float v, float w) {
  float cost = 0;
  for (int k = 0; k < gNPeer; k++) {
    const PeerMsg &p = gPeers[k];
    float x = x0, y = y0, th = th0;
    for (float t = SIM_DT; t <= HORIZON; t += SIM_DT) {
      x += v * cosf(th) * SIM_DT; y += v * sinf(th) * SIM_DT; th += w * SIM_DT;
      float ex, ey; peerAt(p, t, ex, ey);
      float d = hypotf(ex - x, ey - y);
      // 时空冲突：距离越近罚越重；按互惠系数缩放到各自承担的部分
      float need = R_SAFE * (1.0f + (p.prio > MY_PRIO ? 0.4f : 0.0f));  // 让高优先级
      if (d < need) cost += RECIP * (need - d) * (need - d) * 40.0f;
      if (d < 2 * R_ROBOT) cost += 500.0f;      // 实际碰撞：重罚
    }
  }
  return cost;
}

// ---- 同向冲突时的“右侧通行”偏好，打破对称僵局 ----
float sideBias(float w) {
  // 与最近邻居接近对峙时，偏向 w<0（向右让），避免同侧互撞
  for (int k = 0; k < gNPeer; k++) {
    float dx = gPeers[k].x - px, dy = gPeers[k].y - py;
    float d = hypotf(dx, dy);
    if (d < 1.0f) {
      float angToPeer = atan2f(dy, dx);
      float rel = angToPeer - pth;
      if (fabsf(rel) < 1.2f) return -1.2f * w;   // 对峙：鼓励右转
    }
  }
  return 0;
}

struct Vel { float v, w; };

void planDWA(Vel &best) {
  float vLo = max(-0.8f, vCur - 0.6f * 0.05f), vHi = min(0.8f, vCur + 0.6f * 0.05f);
  float wLo = max(-1.6f, wCur - 2.0f * 0.05f), wHi = min(1.6f, wCur + 2.0f * 0.05f);
  float bestScore = -1e9f; best = {0, 0};

  for (float v = vLo; v <= vHi + 1e-6f; v += 0.05f) {
    for (float w = wLo; w <= wHi + 1e-6f; w += 0.10f) {
      float clear; float s = evalTrajStatic(v, w, clear);   // 静态障碍打分(同案例一)
      if (s < -1e8f) continue;                              // 静态不可达/会撞
      s -= peerCost(px, py, pth, v, w);                     // 多机交互代价
      s -= sideBias(w);                                     // 对称僵局偏好
      // 优先级兜底：低优先级要更保守（速度上限压低）
      if (MY_PRIO < 2 && v > 0.5f) s -= (v - 0.5f) * 3.0f;
      if (s > bestScore) { bestScore = s; best = {v, w}; }
    }
  }
  if (bestScore < -1e8f) best = {0, 0};
}

// ---- ESP-NOW 广播/接收 ----
void onRecv(const uint8_t *mac, const uint8_t *data, int len) {
  if (len != sizeof(PeerMsg)) return;
  PeerMsg m; memcpy(&m, data, sizeof(m));
  if (m.id == MY_ID) return;
  for (int i = 0; i < gNPeer; i++) if (gPeers[i].id == m.id) { gPeers[i] = m; return; }
  if (gNPeer < 4) gPeers[gNPeer++] = m;
}

void broadcast() {
  PeerMsg m = {MY_ID, px, py, pth, vCur, wCur, MY_PRIO};
  esp_now_send(NULL, (uint8_t*)&m, sizeof(m));   // 广播
}

void setup() {
  // 电机/编码器初始化同案例一（略）
  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_register_recv_cb(onRecv);
  esp_now_peer_info_t pi = {};
  memset(pi.peer_addr, 0xFF, 6);                 // 广播地址
  pi.channel = 1; pi.encrypt = false;
  esp_now_add_peer(&pi);
  Serial.begin(115200);
}

void loop() {
  // 1. 位姿更新（差速积分）
  px += vCur * cosf(pth) * 0.05f;
  py += vCur * sinf(pth) * 0.05f;
  pth += wCur * 0.05f;

  broadcast();                                    // 2. 共享意图
  Vel cmd; planDWA(cmd);                          // 3. 交互式 DWA

  float vL = cmd.v - cmd.w * 0.16f, vR = cmd.v + cmd.w * 0.16f;
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(vL / 0.065f); wheelR.move(vR / 0.065f);

  vCur = cmd.v; wCur = cmd.w;
  Serial.print("peers="); Serial.print(gNPeer);
  Serial.print(" v="); Serial.print(cmd.v, 2);
  Serial.print(" w="); Serial.println(cmd.w, 2);
  delay(50);
}

提示：RECIP=0.5 是互惠避让的关键——双方都只承担一半避让量，合起来正好让开；若取 1.0，两车会同时大幅转向，形成“过冲—回摆”振荡。优先级只在互惠无法解决时生效，即窄道、死锁，不参与日常避让，否则会让行为不可预测。

6、FreeRTOS 三层 —— 协调层预约 + DWA 速度筛选 + 1kHz 执行
硬件：ESP32 双核 + 差速 BLDC + 超声 + ESP-NOW。
核心思路：DWA 的“筛选”要跑得快，而多机协商（预约路口、死锁解除）要跑得慢。三层分工：协调层 10Hz 做路口预约与优先级仲裁，DWA 层 20Hz 只做速度空间筛选与打分，执行层 1kHz 纯跟踪；另设独立的死锁检测（两车都接近零速超过 2s）和倒退让行兜底。

/*
 * 案例6：协调(10Hz) - DWA筛选(20Hz) - 执行(1kHz) 三层分离
 */
#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wheelL(7), wheelR(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;

// ---- 协调层共享 ----
volatile uint8_t gResvZone = 0;        // 已预约的路口/窄道区号（0=无）
volatile uint8_t gYieldTo   = 0;       // 需让行的机器人 ID（0=无）
volatile bool    gDeadlock  = false;
// ---- DWA 输出 ----
volatile float c_v = 0, c_w = 0;
volatile bool  c_estop = false;
// ---- 感知 ----
volatile float s_vCur = 0, s_wCur = 0;
volatile uint8_t s_zone = 0;           // 当前所处区号
volatile bool s_zoneFree = true;

const uint8_t MY_ID = 1;
unsigned long zeroSince = 0;

// ---- 协调层：10Hz，预约 + 让行 + 死锁解除 ----
void taskCoord(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    uint8_t zone = s_zone;
    float v = s_vCur;

    // 1) 死锁检测：连续 2s 近乎静止且前方有邻居
    if (fabsf(v) < 0.03f) {
      if (zeroSince == 0) zeroSince = millis();
      else if (millis() - zeroSince > 2000) gDeadlock = true;
    } else zeroSince = 0;

    // 2) 死锁解除：低 ID 车倒退让行，高 ID 车先行（确定性，不协商）
    if (gDeadlock) {
      gYieldTo = MY_ID;                    // 自己退
      c_v = -0.25f; c_w = 0.0f;            // 倒退 0.25m/s（执行层限幅）
      if (millis() - zeroSince > 4000) gDeadlock = false;   // 超时放弃，交上层
    } else {
      // 3) 路口预约：进入窄区前先申请，被占则等待
      if (zone != 0 && !s_zoneFree) { c_v = 0; c_w = 0; }
      else gResvZone = zone;
    }
    vTaskDelayUntil(&last, pdMS_TO_TICKS(100));   // 10Hz
  }
}

// ---- DWA 层：20Hz，速度筛选 + 打分（不含协商）----
void taskDWA(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float vCur = s_vCur, wCur = s_wCur;
    float vLo = max(-0.8f, vCur - 0.6f * 0.05f), vHi = min(0.8f, vCur + 0.6f * 0.05f);
    float wLo = max(-1.6f, wCur - 2.0f * 0.05f), wHi = min(1.6f, wCur + 2.0f * 0.05f);

    float bestV = 0, bestW = 0, bestS = -1e9f;
    for (float v = vLo; v <= vHi + 1e-6f; v += 0.05f) {
      float dObs = readFrontUS();                 // 前方距离
      if ((v * v) / (2 * 0.6f) + 0.18f >= dObs) continue;   // 可停性筛选
      for (float w = wLo; w <= wHi + 1e-6f; w += 0.10f) {
        float clear; float s = evalTrajStatic(v, w, clear);
        if (s < -1e8f) continue;
        if (s > bestS) { bestS = s; bestV = v; bestW = w; }
      }
    }
    xSemaphoreTake(mux, portMAX_DELAY);
    c_v = bestV; c_w = bestW;
    c_estop = (bestS < -1e8f);                    // 无可行解 -> 刹停
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(50));    // 20Hz
  }
}

// ---- 执行层：1kHz，纯速度环，含斜率限幅与急停 ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  float vCmd = 0, wCmd = 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; }

    // 加减速限幅：加速度不超过 0.6 m/s²，角加速度 2 rad/s²
    vCmd += constrain(v - vCmd, -0.6f * 0.001f, 0.6f * 0.001f);
    wCmd += constrain(w - wCmd, -2.0f * 0.001f, 2.0f * 0.001f);

    float vL = vCmd - wCmd * 0.16f, vR = vCmd + wCmd * 0.16f;
    wheelL.loopFOC(); wheelR.loopFOC();
    wheelL.move(vL / 0.065f); wheelR.move(vR / 0.065f);

    // 回写感知量（供 DWA 与协调层使用）
    s_vCur = vCmd; s_wCur = wCmd;
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 电机初始化同案例一（略）
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskCoord, "coord", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskDWA,   "dwa",   4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,  "exec",  4096, NULL, 3, NULL, 1);
}
void loop() { vTaskDelay(portMAX_DELAY); }

提示：死锁解除采用“低 ID 倒退”这种确定性规则，而不是让两台车互相协商——协商在窄道里极易死循环，确定性规则至少保证 4s 内必定有一方动。协调层 10Hz 足够（路口预约是秒级事件），DWA 20Hz 是算力与响应的平衡点，执行层 1kHz 不受上层影响。

要点解读

一、DWA 的本质是“先筛可行、再选最优”，筛选比打分更重要
速度空间里有大量采样点根本不可行：超过加速度上限到不了（Vd 之外）、走上去刹不住会撞（Va 之外）。案例一先做两道筛选（vLo/vHi 窗口 + admissible()），再做仿真打分，这既是算力优化（可行点通常只占 10~20%），也是安全保证——打分函数再好也救不回一个本来就刹不住的速度。所以“速度可达性筛选”不是 DWA 的前置优化，而是它的安全内核：筛选条件错了，优化目标再好也没用。

二、多机交互要共享“意图”，不是共享“位置”
只交换坐标的多机系统只能做反应式避让（看到近就退），必然振荡、僵持。案例二广播的是位姿 + 当前 (v,ω) + 优先级，接收方按匀速模型外推出邻居未来 1.2s 的轨迹，冲突检测发生在“时空”里（同时刻同位置附近才算冲突），而不是“此刻距离小于阈值”。这一个转变让避让从“应激反应”变成“提前规划”：距离远但正对撞会提前减速，距离近但同向并行则完全不干预。

三、避让责任必须互惠分摊，否则对称系统必然震荡
两车对峙时，若每台都按“我全责避让”的逻辑算，双方会同时大幅转向、又同时回摆，形成可见的“点头舞”；若都认为“对方该让”，就会对撞。案例二的 RECIP = 0.5 是标准互惠做法——每台只承担一半避让量，合起来恰好让开，且双方行为对称、可预测。互惠之外还要一条确定性兜底（案例二的右侧通行偏好、案例三的低 ID 倒退），专门处理互惠也无法收敛的窄道和死锁。

四、评价函数的权重就是机器人的“性格”，必须可在线下调
2.0·朝向 + 1.5·余量 + 0.8·速度 这组权重决定它是“激进抢时间”还是“保守绕远”。权重不能写死：空旷大厅可以偏速度，人流密集处必须偏余量；让行时（gYieldTo 非零）应临时把余量权重拉高、速度权重压低。工程上把这组权重做成可在线切换的 profile（如“巡航、会车、让行、窄道”四套），比现场调参可靠得多。

五、分层与失效兜底：筛选要快、协商要慢、执行要纯
DWA 20Hz（50ms 内必须算完所有采样点）、协调 10Hz（秒级事件）、执行 1kHz（纯跟踪、只消费一个 (v,ω)）——三层节奏差两个数量级，混在一起必然互相拖累。更要紧的是“无解”时的行为必须预先定义：窗口内所有速度都被筛掉（贴障、被围）时，正确动作是 c_estop 刹停而不是随便选一个（案例一、三），再由协调层用倒退或等待脱困。能安全地“承认无路可走”，比能优雅地规划路径更接近工业级。

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