Arduino BLDC 之“ARVO/VO + DWA 融合速度筛选（运动学约束）”，本质上是“DWA 提供运动学可行速度窗口与轨迹评价，VO/ARVO 提供动态障碍物的速度空间安全筛选，BLDC FOC 负责低延迟、低抖动地执行最终线速度/角速度指令”的局部规划闭环：DWA 保证机器人“能做到的速度”，VO/ARVO 保证机器人“不会撞上动态障碍物的速度”，两者叠加后，机器人才能在行人、叉车、其他机器人共存的动态环境中既满足底盘运动学约束，又具备前瞻性的动态避障能力。

主要特点

1. DWA 负责“运动学可行速度空间”的生成与轨迹评价
   DWA 在速度空间中采样线速度 v 和角速度 ω，并通过三类约束确定动态窗口：硬件极限约束，即机器人最大/最小线速度和角速度；加速度约束，即当前速度下在一个控制周期内可达的速度范围；安全制动约束，即在碰到障碍物前能够安全减速或停止。 随后对窗口内每组速度进行短时轨迹推演，并按航向偏差、障碍物距离、速度效率等指标打分，选出当前最优速度。 它的优势是天然考虑机器人运动学和加速度能力，输出可直接进入底盘控制。
2. VO/ARVO 负责“动态障碍物速度空间禁入区”的计算
   VO 根据机器人和障碍物的相对位置、相对速度，计算哪些速度会在未来一段时间内导致碰撞，这些速度构成速度障碍区。 ARVO/RVO 在此基础上进一步考虑对方也会避障，避免双方同时向同一侧让行而产生振荡。 融合到 DWA 后，VO/ARVO 通常不直接生成轨迹，而是在 DWA 采样得到候选速度后，剔除落入速度障碍区的速度组合，从而增强对动态障碍物的前瞻性。
3. 运动学约束是两者融合的关键边界
   纯 VO/RVO 可能输出机器人无法瞬时达到的速度，例如超过最大加速度、最小转弯半径或电机力矩能力。 DWA 的动态窗口正好弥补这一点：它只允许在当前速度、加速度和硬件极限范围内选择速度。 因此融合后的流程通常是：先由 DWA 生成运动学可行窗口，再用 VO/ARVO 剔除危险速度，最后用 DWA 评价函数从剩余安全速度中选择最优解。
4. 融合方式通常采用“硬筛选 + 软评价”两层结构
   硬筛选层由 VO/ARVO 负责，直接删除会导致碰撞的速度组合；软评价层由 DWA 负责，在剩余安全速度中比较航向、距离目标、平滑性和速度效率。 这样既避免把动态障碍物当成静态障碍处理，也避免因为 VO 区域过保守而让机器人完全无法前进。工程上还可引入碰撞时间 TTC 或风险度函数，对接近危险但未完全禁入的速度进行降权。
5. BLDC FOC 负责把速度指令柔顺执行出来
   局部规划输出的线速度和角速度最终要分解为左右轮或全向轮目标速度。BLDC 配合 FOC 电流环可以提供低转矩脉动的力矩输出，使加减速更平滑，减少急转、打滑和货物晃动。若底层是开环 ESC，DWA/VO 输出的精细速度指令容易被电机响应滞后和电流冲击破坏，闭环效果会明显下降。
6. 分层架构更适合嵌入式落地
   建议分为四层：
   感知层：激光雷达、深度相机、IMU、轮速编码器。
   状态层：机器人位姿估计、障碍物检测、障碍物速度估计。
   规划层：DWA 动态窗口生成、VO/ARVO 速度障碍计算、速度筛选与评价。
   执行层：BLDC FOC 速度/位置闭环、运动学解算、急停与安全降级。
   Arduino Uno/Nano 适合做单电机速度跟随或简单 DWA 验证；若要同时运行多障碍物 VO 计算、DWA 轨迹评价和 BLDC FOC 闭环，更推荐 ESP32-S3、Teensy 4.1、STM32 或上位机协同。

应用场景
仓储物流 AGV/AMR：通道内有人、叉车和其他 AGV 交叉移动，需要兼顾运动学可行性和动态避障。
餐厅/酒店配送机器人：行人突然出现、路径频繁变化，DWA 可保持平滑，VO/ARVO 可提升对移动目标的反应。
医院走廊机器人：病床、推车和人员移动频繁，要求低冲击、低噪声、高安全裕度。
多机器人协同环境：多个机器人共享通道时，RVO/ARVO 有助于减少相互让行振荡。
科研与教学平台：用于验证 DWA、VO/RVO、速度空间筛选、运动学约束和 BLDC 底盘控制。

需要注意的事项
障碍物速度估计必须稳定
VO/ARVO 依赖障碍物位置和速度。如果仅用单帧雷达点云，速度估计容易抖动，导致速度障碍区频繁跳变。应加入卡尔曼滤波、轨迹关联或多帧平滑。
VO 区域不能过于保守
若把每个障碍物都当成高速逼近目标，机器人可能被困住。应结合碰撞时间 TTC、障碍物运动方向和预测不确定性设置安全裕度。
DWA 采样分辨率会影响实时性
采样过粗会漏掉可行速度，采样过细会增加计算负担。嵌入式平台应控制采样数量、预测步长和评价函数复杂度，必要时采用分层采样或查表加速。
运动学参数必须实际标定
最大线速度、最大角速度、最大加速度、最小转弯半径和减速度不能只按理论值填写。BLDC 减速器、电池压降、地面摩擦和负载变化都会影响真实可达速度窗口。
动态窗口与 VO 筛选顺序不能颠倒
更稳妥的做法是先在 DWA 动态窗口内采样，再用 VO/ARVO 筛选。如果先在全速度空间计算 VO，再强行投影回可行窗口，可能丢失最优解或引入不可达速度。
急停优先级必须高于局部规划
当激光雷达检测到近距离碰撞风险、IMU 检测到异常倾角或急停按钮触发时，应立即中断 DWA/VO 规划，进入制动或零力矩状态。
Arduino 平台需控制算力边界
多障碍物 VO 计算、DWA 轨迹推演和 FOC 闭环同时运行对 8 位 MCU 压力较大。建议把感知和高层规划放在上位机或高性能 MCU，Arduino/ESP32 负责底层速度跟踪和电机控制。
评价函数权重需要随场景自适应
空旷区域可提高速度权重，人流密集区应提高障碍物距离和安全裕度权重，狭窄通道应降低角速度权重以减少摆动。 固定权重容易在复杂场景中表现不稳定。

1、ARVO 膨胀半径自适应 + DWA 运动学约束筛选（入门级）
场景说明：面向室内多机器人配送场景，机器人需要与其他机器人或动态障碍物交互。ARVO 根据周围环境复杂度动态调整安全膨胀半径，DWA 在膨胀半径约束下筛选出既安全又可执行的速度。适合 ESP32 或 Teensy 4.x。

#include <SimpleFOC.h>

// ===== 双轮差速 BLDC =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

// ===== 机器人运动学参数 =====
float maxLinearV = 0.5f;      // 最大线速度 m/s
float maxAngularV = 1.5f;     // 最大角速度 rad/s
float maxLinearAcc = 1.0f;    // 最大线加速度 m/s²
float maxAngularAcc = 3.0f;   // 最大角加速度 rad/s²
float wheelBase = 0.25f;
float wheelRadius = 0.05f;
float dt = 0.1f;              // 控制周期 100ms

// ===== 当前状态 =====
float curV = 0, curW = 0;     // 当前速度
float posX = 0, posY = 0, theta = 0;

// ===== ARVO 自适应膨胀半径 =====
float baseRadius = 0.30f;     // 基础安全半径 m
float adaptiveRadius = 0.30f;
float envComplexity = 0.5f;   // 环境复杂度 0~1

// ===== 障碍物信息 =====
struct Obstacle {
  float x, y;        // 位置
  float vx, vy;      // 速度
  float radius;      // 半径
  bool valid;
} obstacles[4];

// ===== 目标点 =====
float goalX = 5.0f, goalY = 0.0f;

// ===== DWA 采样参数 =====
#define V_SAMPLES 9
#define W_SAMPLES 11
float bestV = 0, bestW = 0;
float minCost = 1e6f;

void setup() {
  Serial.begin(115200);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  Serial.println("ARVO+DWA 速度筛选就绪");
}

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

  // ===== 1. 更新环境信息（模拟传感器输入）=====
  updateObstacles();

  // ===== 2. ARVO：自适应膨胀半径 =====
  computeAdaptiveRadius();

  // ===== 3. DWA：运动学约束下筛选最优速度 =====
  dwaSelectVelocity();

  // ===== 4. 执行 =====
  executeVelocity(bestV, bestW);

  // ===== 5. 更新位姿 =====
  updatePose(dt);

  // 监控输出
  static int cnt = 0;
  if (++cnt % 20 == 0) {
    Serial.print("半径="); Serial.print(adaptiveRadius, 2);
    Serial.print(" 最优v="); Serial.print(bestV, 2);
    Serial.print(" w="); Serial.print(bestW, 2);
    Serial.print(" 位姿=("); Serial.print(posX, 1);
    Serial.print(","); Serial.print(posY, 1);
    Serial.println(")");
  }

  delay(100);
}

void updateObstacles() {
  // 模拟动态障碍物（实际通过激光/视觉获取）
  // 障碍物1：从右侧向左移动
  obstacles[0] = {2.0f + sin(millis()/1000.0f)*0.5f, 0.3f, -0.2f, 0.0f, 0.2f, true};
  // 障碍物2：静止
  obstacles[1] = {3.5f, -0.4f, 0.0f, 0.0f, 0.15f, true};
  obstacles[2].valid = false;
  obstacles[3].valid = false;
}

void computeAdaptiveRadius() {
  // 统计附近障碍物数量与距离
  int nearCount = 0;
  float minDist = 10.0f;
  for (int i = 0; i < 4; i++) {
    if (!obstacles[i].valid) continue;
    float dx = obstacles[i].x - posX;
    float dy = obstacles[i].y - posY;
    float d = sqrt(dx*dx + dy*dy) - obstacles[i].radius;
    if (d < 1.0f) nearCount++;
    if (d < minDist) minDist = d;
  }

  // 复杂度 = 障碍物数量因子 + 距离因子
  envComplexity = constrain(nearCount / 3.0f + (1.0f - minDist / 2.0f) * 0.5f, 0, 1);

  // 自适应半径：环境越复杂，半径越大（更保守）
  adaptiveRadius = baseRadius * (0.8f + 0.6f * envComplexity);
  adaptiveRadius = constrain(adaptiveRadius, 0.20f, 0.50f);
}

void dwaSelectVelocity() {
  // ===== DWA 动态窗口 =====
  // 速度限制
  float vMin = max(-maxLinearV, curV - maxLinearAcc * dt);
  float vMax = min(maxLinearV, curV + maxLinearAcc * dt);
  float wMin = max(-maxAngularV, curW - maxAngularAcc * dt);
  float wMax = min(maxAngularV, curW + maxAngularAcc * dt);

  minCost = 1e6f;

  // ===== 速度采样 =====
  for (int i = 0; i < V_SAMPLES; i++) {
    float v = vMin + (vMax - vMin) * i / (V_SAMPLES - 1);
    for (int j = 0; j < W_SAMPLES; j++) {
      float w = wMin + (wMax - wMin) * j / (W_SAMPLES - 1);

      // ===== 模拟轨迹（简化：仅检查终点）=====
      float predX = posX + v * cos(theta) * dt;
      float predY = posY + v * sin(theta) * dt;
      float predTheta = theta + w * dt;

      // ===== 评价函数 =====
      // 1. 朝向目标代价
      float dxGoal = goalX - predX;
      float dyGoal = goalY - predY;
      float angleToGoal = atan2(dyGoal, dxGoal);
      float headingCost = abs(predTheta - angleToGoal);

      // 2. 障碍物代价（使用 ARVO 自适应半径）
      float obstacleCost = 0;
      for (int k = 0; k < 4; k++) {
        if (!obstacles[k].valid) continue;
        float dx = obstacles[k].x - predX;
        float dy = obstacles[k].y - predY;
        float d = sqrt(dx*dx + dy*dy);
        if (d < adaptiveRadius) {
          // 距离小于自适应半径，代价急剧上升
          obstacleCost += (adaptiveRadius - d) * 100.0f;
        }
      }

      // 3. 速度代价（鼓励快速移动）
      float velocityCost = (maxLinearV - v) * 0.5f;

      // 总代价
      float totalCost = headingCost * 2.0f + obstacleCost * 5.0f + velocityCost;

      if (totalCost < minCost) {
        minCost = totalCost;
        bestV = v;
        bestW = w;
      }
    }
  }
}

void executeVelocity(float v, float w) {
  float vL = v - w * wheelBase / 2.0f;
  float vR = v + w * wheelBase / 2.0f;

  // 线速度 → 轮转速 rad/s
  motorL.move(vL / wheelRadius);
  motorR.move(vR / wheelRadius);

  curV = v;
  curW = w;
}

void updatePose(float dt) {
  posX += curV * cos(theta) * dt;
  posY += curV * sin(theta) * dt;
  theta += curW * dt;
}

代码要点：ARVO 的膨胀半径根据“附近障碍物数量 + 最近距离”动态调整，环境越复杂半径越大。DWA 的动态窗口由当前速度和加速度限制确定（vMin/vMax/wMin/wMax），确保筛选出的速度在物理上可执行 。障碍物代价用自适应半径作为阈值，超出半径的障碍物不影响代价。

2、多机器人交互 + DWA 速度可达性筛选（进阶）
场景说明：面向多机器人共用的狭窄通道场景。RVO 的核心问题是“理论可选但实际不可达”——RVO 给出的避障速度可能超出机器人当前加速能力。本案例将 RVO 的避障速度集合作为“候选集”，DWA 从中筛选出加速度约束下可达的最优速度 。

#include <SimpleFOC.h>

// ===== 双轮差速 BLDC =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

// ===== 运动学参数 =====
float maxV = 0.6f, maxW = 2.0f;
float maxAccV = 1.2f, maxAccW = 4.0f;
float wheelBase = 0.25f, wheelRadius = 0.05f;
float dt = 0.1f;

// ===== 状态 =====
float curV = 0, curW = 0;
float posX = 0, posY = 0, theta = 0;

// ===== 其他机器人（RVO 交互对象）=====
struct Robot {
  float x, y;
  float vx, vy;
  float radius;
  bool valid;
} others[3];

// ===== 目标 =====
float goalX = 6.0f, goalY = 0.0f;

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

void setup() {
  Serial.begin(115200);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  Serial.println("多机器人 ARVO+DWA 就绪");
}

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

  // 更新其他机器人状态（模拟）
  updateOtherRobots();

  // ARVO 自适应半径
  computeAdaptiveRadius();

  // ===== RVO 速度集合生成 =====
  // 采样一组候选速度，过滤掉 RVO 判定为碰撞的速度
  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;

      // 检查该速度是否在 RVO 避障区域内
      if (!isInRVO(v, w)) {
        candidateV[candidateCount] = v;
        candidateW[candidateCount] = w;
        candidateCount++;
      }
    }
  }

  // ===== DWA 从 RVO 候选中筛选可达速度 =====
  float bestV = 0, bestW = 0;
  float bestCost = 1e6f;

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

    // 运动学约束：加速度限制内可达
    if (abs(v - curV) > maxAccV * dt) continue;
    if (abs(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 = abs(predTheta - angleToGoal);

    float obstacleCost = 0;
    for (int m = 0; m < 3; m++) {
      if (!others[m].valid) continue;
      float dx = others[m].x - predX;
      float dy = others[m].y - predY;
      float d = sqrt(dx*dx + dy*dy);
      if (d < adaptiveR) obstacleCost += (adaptiveR - d) * 80.0f;
    }

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

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

  // 执行
  executeVelocity(bestV, bestW);
  updatePose(dt);

  delay(100);
}

void updateOtherRobots() {
  // 模拟两个机器人在狭窄通道中相向而行
  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};
  others[2].valid = false;
}

void computeAdaptiveRadius() {
  int near = 0;
  float minD = 10.0f;
  for (int i = 0; i < 3; i++) {
    if (!others[i].valid) continue;
    float dx = others[i].x - posX;
    float dy = others[i].y - posY;
    float d = sqrt(dx*dx + dy*dy) - others[i].radius;
    if (d < 1.2f) near++;
    if (d < minD) minD = d;
  }
  float complexity = constrain(near / 2.0f + (1.0f - minD / 2.0f) * 0.3f, 0, 1);
  adaptiveR = baseR * (0.9f + 0.5f * complexity);
}

bool isInRVO(float v, float w) {
  // 简化 RVO 判定：检查该速度是否会导致与任一机器人碰撞
  // 实际 RVO 需要计算速度障碍锥，此处用预测位置简化
  float predX = posX + v * cos(theta) * dt;
  float predY = posY + v * sin(theta) * dt;

  for (int i = 0; i < 3; i++) {
    if (!others[i].valid) continue;
    // 预测对方位置
    float oX = others[i].x + others[i].vx * dt;
    float oY = others[i].y + others[i].vy * dt;
    // 距离检查
    float dx = oX - predX;
    float dy = oY - predY;
    float d = sqrt(dx*dx + dy*dy);
    if (d < adaptiveR) return true;  // 在 RVO 区域内
  }
  return false;
}

void executeVelocity(float v, float w) {
  float vL = v - w * wheelBase / 2.0f;
  float vR = v + w * wheelBase / 2.0f;
  motorL.move(vL / wheelRadius);
  motorR.move(vR / wheelRadius);
  curV = v; curW = w;
}

void updatePose(float dt) {
  posX += curV * cos(theta) * dt;
  posY += curV * sin(theta) * dt;
  theta += curW * dt;
}

代码要点：核心逻辑是“RVO 先过滤，DWA 再筛选”——isInRVO() 剔除会导致碰撞的速度，得到候选集；DWA 的加速度约束 abs(v - curV) > maxAccV * dt 进一步剔除当前电机无法达到的速度 。双层过滤确保最终输出的速度既安全又可执行。

3、ARVO 膨胀半径动态联动 DWA 最大速度（工业级）
场景说明：面向复杂动态环境中的配送机器人。ARVO 的膨胀半径不仅用于障碍物代价，还直接联动 DWA 的最大线速度——半径越大（环境越危险），最大速度越低，实现“风险感知→动态约束→速度筛选”的完整闭环 。

#include <SimpleFOC.h>

// ===== 双轮 BLDC =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

// ===== 基础运动学 =====
float baseMaxV = 0.8f;        // 基础最大线速度
float baseMaxAccV = 1.5f;     // 基础最大加速度
float maxW = 2.0f, maxAccW = 5.0f;
float wheelBase = 0.25f, wheelRadius = 0.05f;
float dt = 0.08f;

float curV = 0, curW = 0;
float posX = 0, posY = 0, theta = 0;

// ===== 动态障碍物 =====
struct DynamicObstacle {
  float x, y;
  float vx, vy;
  float radius;
  float timeToCollision;  // 预计碰撞时间
  bool valid;
} dynObs[5];

// ===== ARVO 自适应参数 =====
float baseRadius = 0.35f;
float adaptiveRadius = 0.35f;
float riskLevel = 0;          // 风险等级 0~1

// ===== 目标 =====
float goalX = 8.0f, goalY = 0.0f;

void setup() {
  Serial.begin(115200);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  Serial.println("ARVO 动态约束 DWA 就绪");
}

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

  // ===== 1. 更新障碍物与碰撞时间预测 =====
  updateDynamicObstacles();

  // ===== 2. ARVO：计算风险等级与自适应半径 =====
  computeRiskAndRadius();

  // ===== 3. ARVO 联动：动态调整 DWA 最大速度 =====
  float dynamicMaxV = baseMaxV * (1.0f - riskLevel * 0.6f);
  float dynamicMaxAcc = baseMaxAccV * (1.0f - riskLevel * 0.4f);

  // ===== 4. DWA 在动态约束下筛选 =====
  float bestV = 0, bestW = 0;
  float bestCost = 1e6f;

  float vMin = max(-dynamicMaxV, curV - dynamicMaxAcc * dt);
  float vMax = min(dynamicMaxV, curV + dynamicMaxAcc * dt);
  float wMin = max(-maxW, curW - maxAccW * dt);
  float wMax = min(maxW, curW + maxAccW * dt);

  for (int i = 0; i < 7; i++) {
    float v = vMin + (vMax - vMin) * i / 6.0f;
    for (int j = 0; j < 9; j++) {
      float w = wMin + (wMax - wMin) * j / 8.0f;

      float predX = posX + v * cos(theta) * dt;
      float predY = posY + v * sin(theta) * dt;
      float predTheta = theta + w * dt;

      // 朝向代价
      float angleGoal = atan2(goalY - predY, goalX - predX);
      float headingCost = abs(predTheta - angleGoal);

      // 障碍物代价（使用自适应半径）
      float obsCost = 0;
      for (int k = 0; k < 5; k++) {
        if (!dynObs[k].valid) continue;
        float dx = dynObs[k].x - predX;
        float dy = dynObs[k].y - predY;
        float d = sqrt(dx*dx + dy*dy);
        if (d < adaptiveRadius) {
          obsCost += (adaptiveRadius - d) * 120.0f;
        }
        // 动态障碍物额外惩罚：考虑碰撞时间
        if (dynObs[k].timeToCollision < 2.0f && d < 1.0f) {
          obsCost += 200.0f;
        }
      }

      // 速度代价（鼓励向目标快速移动，但受动态约束）
      float velCost = (dynamicMaxV - v) * 0.4f;

      float total = headingCost * 1.8f + obsCost * 5.0f + velCost;

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

  // 执行
  executeVelocity(bestV, bestW);
  updatePose(dt);

  Serial.print("风险="); Serial.print(riskLevel, 2);
  Serial.print(" 动态maxV="); Serial.print(dynamicMaxV, 2);
  Serial.print(" 最优v="); Serial.print(bestV, 2);
  Serial.print(" 半径="); Serial.println(adaptiveRadius, 2);

  delay(80);
}

void updateDynamicObstacles() {
  // 模拟动态障碍物
  // 障碍物1：向机器人移动
  dynObs[0] = {4.0f - millis()/3000.0f, 0.3f, -0.2f, 0.0f, 0.2f, 0, true};
  // 障碍物2：横穿
  dynObs[1] = {5.0f, sin(millis()/1500.0f)*0.8f, 0.0f, 0.15f, 0.18f, 0, true};
  dynObs[2].valid = false;
  dynObs[3].valid = false;
  dynObs[4].valid = false;

  // 计算碰撞时间
  for (int i = 0; i < 5; i++) {
    if (!dynObs[i].valid) continue;
    float dx = dynObs[i].x - posX;
    float dy = dynObs[i].y - posY;
    float dist = sqrt(dx*dx + dy*dy) - dynObs[i].radius - 0.15f;
    // 简化 TTC：相对速度方向上的距离/相对速度
    float relVx = dynObs[i].vx - curV * cos(theta);
    float relVy = dynObs[i].vy - curV * sin(theta);
    float relSpeed = sqrt(relVx*relVx + relVy*relVy);
    dynObs[i].timeToCollision = (relSpeed > 0.05f) ? dist / relSpeed : 10.0f;
  }
}

void computeRiskAndRadius() {
  // 风险 = 最近障碍物的接近程度 + 碰撞时间因子
  float minDist = 10.0f;
  float minTTC = 10.0f;

  for (int i = 0; i < 5; i++) {
    if (!dynObs[i].valid) continue;
    float dx = dynObs[i].x - posX;
    float dy = dynObs[i].y - posY;
    float d = sqrt(dx*dx + dy*dy) - dynObs[i].radius;
    if (d < minDist) minDist = d;
    if (dynObs[i].timeToCollision < minTTC) minTTC = dynObs[i].timeToCollision;
  }

  // 风险等级：距离越近、碰撞时间越短，风险越高
  float distRisk = constrain(1.0f - minDist / 2.0f, 0, 1);
  float ttcRisk = constrain(1.0f - minTTC / 3.0f, 0, 1);
  riskLevel = max(distRisk, ttcRisk);

  // 自适应半径：风险越高，半径越大
  adaptiveRadius = baseRadius * (1.0f + riskLevel * 0.8f);
  adaptiveRadius = constrain(adaptiveRadius, 0.25f, 0.60f);
}

void executeVelocity(float v, float w) {
  float vL = v - w * wheelBase / 2.0f;
  float vR = v + w * wheelBase / 2.0f;
  motorL.move(vL / wheelRadius);
  motorR.move(vR / wheelRadius);
  curV = v; curW = w;
}

void updatePose(float dt) {
  posX += curV * cos(theta) * dt;
  posY += curV * sin(theta) * dt;
  theta += curW * dt;
}

代码要点：这是最完整的工业级架构。ARVO 不仅输出自适应半径，还输出风险等级 riskLevel，该风险等级直接联动 DWA 的动态窗口——dynamicMaxV = baseMaxV * (1.0f - riskLevel * 0.6f) 。这意味着环境越危险，DWA 可选的最大速度越低，从源头限制激进决策。碰撞时间预测 timeToCollision 作为额外惩罚项，进一步抑制高风险速度。

要点解读

1. RVO 的核心缺陷是“速度理论可选但实际不可达”，DWA 的运动学约束是唯一解药

RVO/ARVO 计算出的避障速度集合，是基于几何和运动学理想假设的——它只考虑“这个速度能否避免碰撞”，不考虑“电机能否在当前周期内加速到这个速度” 。DWA 的动态窗口 v ∈ [curV - a_maxdt, curV + a_maxdt] 恰好补上了这个缺口。案例二中 abs(v - curV) > maxAccV * dt 的筛选条件，就是这条原则的直接实现。

2. ARVO 的“自适应膨胀半径”解决的是固定半径的“过度保守”问题

传统 RVO 使用固定膨胀半径。在空旷环境中，大半径会让机器人无谓地绕远；在拥挤环境中，小半径又可能导致避障失败 。ARVO 根据环境复杂度动态调整：空旷时缩小半径（扩大可选速度范围），拥挤时扩大半径（保障安全距离）。案例一的 envComplexity 和案例三的 riskLevel 是两种不同的量化方式——前者基于“障碍物数量+距离”，后者基于“碰撞时间预测”。

3. DWA 的评价函数必须与 ARVO 的安全半径“联动”

如果 ARVO 计算出的自适应半径 adaptiveRadius 只用于路径规划，而 DWA 的障碍物代价使用固定的安全距离，两者就会脱节。案例三的处理方式是最彻底的：adaptiveRadius 直接作为 DWA 障碍物代价的阈值，同时 riskLevel 联动 dynamicMaxV 。这样 ARVO 的风险感知结果会贯穿整个速度筛选过程，而不是仅仅停留在路径层面。

4. Arduino 平台无法同时运行 ARVO 和 DWA 的完整计算，必须做架构选择

ARVO 需要计算速度障碍锥，DWA 需要遍历速度空间并模拟轨迹。在 8 位 Arduino 上，100ms 的控制周期内完成这些浮点运算几乎不可能 。务实的方案有三种：一是用 ESP32/Teensy 4.x 的高主频 MCU；二是采用“上位机（树莓派/Jetson）运行 ARVO+DWA + Arduino 作为 BLDC 速度执行器”的分层架构；三是大幅简化算法——如案例一用“预测位置距离”替代严格的 RVO 速度障碍锥计算。

5. 动态障碍物的“碰撞时间预测”比单纯的“距离”更能指导避障决策

案例三中 timeToCollision 的计算是关键增值点。一个正在快速靠近的障碍物，即使当前距离较远，其威胁也可能大于一个距离较近但静止的障碍物。在 DWA 评价函数中加入 timeToCollision < 2.0f 的惩罚项，能让机器人在“距离安全但即将碰撞”的情况下提前减速或转向 。对于户外配送机器人而言，这种预见性避障比纯粹的“反应式避障”安全得多。

4、室内静态障碍场景ARVO+DWA融合速度筛选
适用场景：办公室、仓库等室内固定障碍环境，基于激光雷达识别静态障碍，融合两种算法筛选安全速度。

#include <SimpleFOC.h>
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1); // 左轮
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3); // 右轮

// 运动学约束参数
float max_speed = 0.8f;      // 最大线速度 m/s
float max_rot_speed = 1.2f;  // 最大角速度 rad/s
float wheelbase = 0.5f;      // 轮距 m
float min_safe_dist = 0.3f;  // 最小安全距离 m

// 激光雷达点云（模拟）
float lidar_dists[8] = {1.2f, 1.0f, 0.8f, 0.4f, 0.5f, 0.9f, 1.1f, 1.3f};

// ARVO模块：计算障碍风险值（0-1，越大风险越高）
float arvo_calc_risk(float v_sample, float w_sample, float cur_angle) {
  float min_dist = 10.0f, risk = 0.0f;
  // 计算雷达各点对应轨迹上的最近距离
  for (int i = 0; i < 8; i++) {
    float dx = lidar_dists[i] * cosf(i * 0.7853f - cur_angle);
    float dy = lidar_dists[i] * sinf(i * 0.7853f - cur_angle);
    // 速度越快，轨迹上的接近距离越短，风险越高
    float v_effect = (v_sample + 0.5f) * 0.5f;
    float dist_th = min_safe_dist + v_effect;
    if (dx < dist_th || dy < dist_th) {
      risk += (dist_th - min(dx, dy)) / dist_th;
    }
  }
  // 角速度越快，转向风险越高
  risk += w_sample * 0.2f;
  return constrain(risk, 0.0f, 1.0f);
}

// DWA模块：速度空间采样+运动学筛选
void dwa_sample_speed(float v_p, float w_p, float *best_v, float *best_w) {
  float v_min = 0.1f, v_max = max_speed;
  float w_min = -max_rot_speed, w_max = max_rot_speed;
  float best_score = -1; float best_v_tmp=0, best_w_tmp=0;
  // 采样速度空间（步长0.05）
  for (float v = v_min; v <= v_max; v += 0.05f) {
    for (float w = w_min; w <= w_max; w += 0.1f) {
      // 运动学约束：差速机器人运动学，线速度与角速度匹配
      if (abs(v) < 0.1f && abs(w) > 0.5f) continue; // 低速时禁止大转角
      // 计算ARVO风险
      float risk = arvo_calc_risk(v, w, 0.0f);
      if (risk > 0.6f) continue; // 高风险速度直接淘汰
      // 评分：跟踪目标速度优先，风险次之
      float score = 1.0f - 0.7f*risk - 0.3f*sqrtf((v-v_p)*(v-v_p) + (w-w_p)*(w-w_p));
      if (score > best_score) {
        best_score = score; best_v_tmp = v; best_w_tmp = w;
      }
    }
  }
  *best_v = best_v_tmp; *best_w = best_w_tmp;
}

void setup() {
  Serial.begin(9600);
  wheelL.init(); wheelR.init();
  wheelL.controller = FOC_VELOCITY; wheelR.controller = FOC_VELOCITY;
}

void loop() {
  float v_target = 0.5f, w_target = 0.0f;
  float best_v, best_w;
  dwa_sample_speed(v_target, w_target, &best_v, &best_w);
  // 差速运动学转换：线速度+角速度→左右轮速度
  float v_l = best_v - best_w * wheelbase / 2.0f;
  float v_r = best_v + best_w * wheelbase / 2.0f;
  wheelL.setVelocity(constrain(v_l * 100, -100, 100));
  wheelR.setVelocity(constrain(v_r * 100, -100, 100));
  delay(10);
}

5、户外动态障碍场景ARVO+DWA融合速度筛选
适用场景：园区、道路等动态障碍环境，结合动态障碍预测，调整速度筛选权重。

#include <SimpleFOC.h>
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1);
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3);

float max_speed = 1.0f, max_rot_speed = 1.5f, wheelbase = 0.6f;
float min_safe_dist = 0.5f;
float dynamic_obs_x = 0, dynamic_obs_y = 0; // 动态障碍坐标
bool has_dynamic = false;

// ARVO：动态障碍风险加权
float arvo_dynamic_risk(float v_sample, float w_sample, float cur_angle) {
  float risk = 0.0f;
  if (has_dynamic) {
    // 计算与动态障碍的相对速度，速度越大，风险越高
    float rel_v = v_sample;
    float dist = sqrtf(dynamic_obs_x*dynamic_obs_x + dynamic_obs_y*dynamic_obs_y);
    if (dist < 2.0f) {
      risk += (2.0f - dist) * rel_v * 0.3f; // 距离越近、速度越快，风险越高
    }
  }
  // 静态障碍风险同案例1，此处简化为固定基线
  risk += 0.15f;
  return constrain(risk, 0.0f, 1.0f);
}

// DWA：动态权重调整
void dwa_dynamic_sample(float v_p, float w_p, float *best_v, float *best_w) {
  float v_min = 0.1f, v_max = max_speed, w_min = -max_rot_speed, w_max = max_rot_speed;
  float best_score = -1, best_v_tmp = 0, best_w_tmp = 0;
  for (float v = v_min; v <= v_max; v += 0.05f) {
    for (float w = w_min; w <= w_max; w += 0.1f) {
      // 运动学约束
      if (abs(v) < 0.2f && abs(w) > 0.8f) continue;
      float risk = arvo_dynamic_risk(v, w, 0.0f);
      if (risk > 0.5f) continue;
      // 动态场景：风险权重提升至0.8
float score = 1.0f - 0.8f*risk - 0.2f*sqrtf((v-v_p)*(v-v_p) + (w-w_p)*(w-w_p));
      if (score > best_score) {
        best_score = score; best_v_tmp = v; best_w_tmp = w;
      }
    }
  }
  *best_v = best_v_tmp; *best_w = best_w_tmp;
}

void detect_dynamic_obstacle() {
  // 模拟动态障碍检测，实际接视觉/雷达跟踪接口
  if (openmv.track_object()) {
    dynamic_obs_x = openmv.get_obj_x(); dynamic_obs_y = openmv.get_obj_y();
    has_dynamic = true;
  } else {
    has_dynamic = false;
  }
}

void loop() {
  detect_dynamic_obstacle();
  float v_target = 0.6f, w_target = 0.0f;
  float best_v, best_w;
  dwa_dynamic_sample(v_target, w_target, &best_v, &best_w);
  float v_l = best_v - best_w * wheelbase / 2.0f;
  float v_r = best_v + best_w * wheelbase / 2.0f;
  wheelL.setVelocity(best_v * 100);
  wheelR.setVelocity(best_w * 100);
  delay(10);
}

6、狭窄通道场景ARVO+DWA融合速度筛选
适用场景：狭窄走廊/管道，强化通道几何约束，筛选适配通道宽度的速度。

#include <SimpleFOC.h>
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1);
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3);

float max_speed = 0.6f, max_rot_speed = 0.8f, wheelbase = 0.4m;
float channel_width = 0.6f; // 通道宽度
float min_safe_dist = 0.1f; // 狭窄通道安全距离更小

// ARVO：通道边界风险计算
float arvo_channel_risk(float v_sample, float w_sample, float cur_angle, float cur_offset) {
  // cur_offset：机器人当前通道横向偏移
  float offset_risk = abs(cur_offset) * 2.0f; // 偏向边界越近，风险越高
  float vel_risk = v_sample * 0.5f; // 速度越快，姿态控制越难，风险越高
  float w_risk = abs(w_sample) * 0.8f; // 转角越大，越容易触碰通道壁
  float total_risk = 0.4f*offset_risk + 0.4f*vel_risk + 0.2f*w_risk;
  return constrain(total_risk, 0.0f, 1.0f);
}

// DWA：狭窄通道约束强化
void dwa_channel_sample(float v_p, float w_p, float *best_v, float *best_w) {
  float v_min = 0.1f, v_max = max_speed, w_min = -max_rot_speed, w_max = max_rot_speed;
  float best_score = -1, best_v_tmp = 0, best_w_tmp = 0;
  // 狭窄通道：缩小速度空间，低速优先
  v_max = min(v_max, 0.4f);
  w_max = min(w_max, 0.4f);
  for (float v = v_min; v <= v_max; v += 0.03f) {
    for (float w = w_min; w <= w_max; w += 0.05f) {
      // 运动学约束：角速度上限收紧
      if (abs(w) > 0.3f) continue;
      float risk = arvo_channel_risk(v, w, 0.0f, 0.05f);
      if (risk > 0.5f) continue;
      float score = 1.0f - 0.7f*risk - 0.3f*abs(v-v_p) - 0.2f*abs(w-w_p);
      if (score > best_score) {
        best_score = score; best_v_tmp = v; best_w_tmp = w;
      }
    }
  }
  *best_v = best_v_tmp; *best_w = best_w_tmp;
}

void loop() {
  float v_target = 0.3f, w_target = 0.0f;
  float best_v, best_w;
  dwa_channel_sample(v_target, w_target, &best_v, &best_w);
  float v_l = best_v - best_w * wheelbase / 2.0f;
  float v_r = best_v + best_w * wheelbase / 2.0f;
  wheelL.setVelocity(best_v * 100);
  wheelR.setVelocity(best_w * 100);
  delay(10);
}

要点解读
运动学约束是速度筛选的核心边界，需优先前置
必须结合机器人实际机械参数设置硬约束：差速机器人需满足线速度与角速度的匹配关系，避免低速大转角导致打滑；足式机器人需限制关节角速度、足端受力范围，从根源上排除不可行的速度组合，减少无效采样。

ARVO负责风险量化，动态权重适配场景
ARVO的核心是将障碍、路况转化为0-1的风险值，风险阈值需根据场景动态调整：静态障碍设0.6、动态障碍0.5、狭窄通道0.5，低于风险的速度才可进入筛选流程，从源头规避高风险速度。

DWA采样需计算量与精度平衡，适配Arduino算力
Arduino算力有限，需优化采样步长：线速度步长0.05m/s、角速度0.1rad/s较为合适，可减少计算量同时保证精度；同时可剔除明显超出运动学约束的速度组合，降低采样维度，保证实时性。

融合逻辑需协同互补，避免双重冗余或冲突
ARVO负责“筛掉不安全速度”，DWA负责“选出最优速度”，两者配合：先通过ARVO过滤高风险速度，再用DWA在可行域内选匹配目标速度的最优解；需设置统一评分体系，避免两者逻辑冲突，确保筛选后的速度同时满足安全与效率要求。

需加入反馈校准机制，修正筛选偏差
实际运动中需结合编码器、IMU反馈修正速度误差，若执行速度与筛选速度偏差超过10%，需重新进入筛选流程；同时定期校准激光雷达、视觉参数，修正ARVO的风险计算误差，保证筛选结果的稳定性。

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