Arduino BLDC 之 DWA 局部避障控制器的核心，是把“速度空间采样 + 短时轨迹预测 + 多目标评价”压缩成一个毫秒级滚动闭环，使差速 BLDC 底盘在动态障碍环境中既能避开突发障碍，又能平滑回归全局路径。 它不直接规划几何路径，而是每一周期在可行速度窗口内选出最优 (v, ω)，再交给底层电机闭环执行。

主要特点
. 动态窗口由运动学与动力学约束共同决定
DWA 的采样空间不是固定矩形，而是三类约束的交集：速度边界约束 Vm 来自电机和底盘物理极限；加速度约束 Vd 由当前速度和最大加减速能力决定，形成“动态”窗口；安全约束 Vs 要求机器人在碰到障碍前能刹停。 对 BLDC 差速底盘而言，这意味着线速度和角速度上限必须与编码器反馈、FOC 电流环、电池电压和轮胎附着能力匹配，不能只按理论值设置。
. 轨迹预测采用短时滚动时域
算法在每个控制周期内，对窗口中的每组 (v, ω) 用差速运动学模型前向积分，得到未来 0.5—3 秒内的离散轨迹。 由于预测时间短、模型阶次低，计算量可控，适合嵌入式平台高频运行。但这也决定了 DWA 的前瞻性有限，面对 U 形障碍、窄长通道或高速横向障碍时，可能陷入局部最优。
. 评价函数决定行为偏好
典型评价函数包含：朝向目标得分、障碍物距离得分、速度得分，部分实现还会加入贴合全局路径得分。 权重越高，机器人越倾向该行为：提高目标权重会更快趋近目标，但可能贴障行驶；提高障碍权重更安全，但可能绕远或停滞；提高速度权重更高效，但会降低容错。参数整定需要结合场景反复验证。
. 与 BLDC 执行层天然耦合
DWA 输出的是连续变化的线速度和角速度，差速底盘再将其分解为左右轮目标速度。BLDC 配合编码器和 FOC 电流环，能够低脉动、低延迟地跟踪频繁变化的速度指令，比开环直流电机或步进电机更适合执行 DWA 的平滑避障轨迹。 若底层只有开环 ESC，DWA 输出的精细速度指令会因执行误差而退化，甚至出现避障抖动。
. 通常作为局部规划器嵌入分层导航
完整系统中，全局规划器提供宏观路径，DWA 负责局部避障和路径跟踪，底层电机控制器负责速度闭环。 在 Arduino 生态中，AVR 可承担简化版 DWA 与电机控制；若接入激光雷达、SLAM 或多传感器融合，更推荐 ESP32、STM32 或上位机负责感知与全局规划，Arduino/下位 MCU 专注 DWA 输出解析与 BLDC 伺服。

应用场景
室内服务与仓储 AMR：人员、叉车、托盘会突然出现，DWA 可在保持全局路径的同时平滑减速绕行。
巡检与配送机器人：走廊、电梯厅、仓库通道中存在临时障碍，适合用滚动窗口局部重规划。
扫地机器人与小型移动平台：计算资源有限、环境动态变化频繁，DWA 的短时预测和速度空间采样较为匹配。
校园竞赛与教学平台：可用于验证差速运动学、轨迹评价、参数整定、传感器融合和 BLDC 闭环控制。
野外低速移动底盘：若障碍密度不高、速度较低，可结合超声波/ToF/激光雷达做轻量避障；但复杂非结构化地形仍需配合全局规划或更高级局部规划器。

需要注意的事项
动力学参数必须实测标定
最大线速度、角速度、加减速度不能凭经验随意填写。应通过编码器实测底盘加减速能力，并留余量。参数过大会产生不可执行轨迹，过小会让机器人反应迟钝。
安全约束不能省略
若只保留速度和加速度约束，机器人可能在高速下逼近障碍而无法及时刹停。Vs 安全窗口应基于最近障碍距离和最大减速度实时计算，尤其在窄通道和动态障碍场景中更关键。
评价权重需按场景调优
固定权重在开阔环境表现良好，但在拥挤、狭窄或动态障碍增多时可能失效。可先采用均衡权重，再根据“绕远、贴墙、振荡、停滞”等现象逐项微调；进阶方案可引入自适应权重。
感知噪声会直接放大为运动抖动
DWA 依赖障碍物距离和自身位姿。超声波易受反射角影响，激光雷达在强光、玻璃、黑体吸收面下也可能异常，里程计打滑会使位姿漂移。应加入滤波、异常值剔除和传感器冗余，否则评价函数会频繁误判。
Arduino 资源受限，应控制采样密度与周期
DWA 的计算量随 vx_samples、vtheta_samples 和 sim_time 增长。在 AVR 上需减少采样数、使用整数或定点运算、避免动态内存分配；更复杂场景建议把 DWA 放到 ESP32/STM32 或上位机，Arduino 只接收 (v, ω) 并执行 BLDC 闭环。
从工程落地看，更建议先实现“差速运动学 + 动态窗口采样 + 三目标评价 + BLDC 速度闭环”的最小版本，再逐步加入全局路径引导、传感器滤波、自适应权重和故障降级。这样比直接套用完整 ROS 导航栈更适合 Arduino/ESP32 级别的嵌入式平台。

1、基础 DWA 速度空间搜索 + 超声波避障（简化版）
适用场景：室内平坦环境的差速机器人，使用超声波传感器实时扫描障碍物，在动态窗口内采样多组（线速度 v，角速度 ω），模拟短时轨迹并评分，选出最优速度指令。该案例实现了 DWA 的核心“采样→预测→评分→选优”循环。

/**
 * Arduino BLDC机器人 - 基础DWA速度空间搜索 + 超声波避障
 * 硬件假设：Arduino Mega/ESP32, 2×BLDC(SimpleFOC), 编码器, 超声波×3
 * 核心：动态窗口采样(v, ω) -> 轨迹推演 -> 评价函数选优
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);
Encoder encL(2, 4, 2048);
Encoder encR(3, 5, 2048);

// ===== DWA参数 =====
const float MAX_VEL = 0.5;      // 最大线速度 m/s
const float MIN_VEL = 0.0;      // 最小线速度
const float MAX_W = 1.2;        // 最大角速度 rad/s
const float VEL_STEP = 0.1;     // 线速度采样步长
const float W_STEP = 0.2;       // 角速度采样步长
const float SIM_TIME = 1.5;     // 轨迹推演时长 s
const float SIM_DT = 0.1;       // 推演步长 s
const float MAX_ACCEL = 0.5;    // 最大线加速度 m/s²
const float MAX_W_ACCEL = 1.0;  // 最大角加速度 rad/s²

// ===== 评价函数权重 =====
const float ALPHA_HEADING = 0.4;  // 朝向目标权重
const float BETA_DIST = 0.4;      // 障碍物距离权重
const float GAMMA_VEL = 0.2;      // 速度权重

// ===== 机器人状态 =====
float robotX = 0, robotY = 0, robotTheta = 0;
float currentV = 0, currentW = 0;
float targetX = 3.0, targetY = 2.0;

// ===== 超声波引脚 =====
const int SONAR_TRIG[3] = {22, 24, 26};
const int SONAR_ECHO[3] = {23, 25, 27};

float sonarDist[3] = {999, 999, 999};

float measureDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long dur = pulseIn(echo, HIGH, 20000);
  return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}

// 更新机器人位姿（简化里程计）
void updatePose(float v, float w, float dt) {
  robotTheta += w * dt;
  robotX += v * cos(robotTheta) * dt;
  robotY += v * sin(robotTheta) * dt;
}

// 评价一条候选轨迹
float evaluateTrajectory(float v, float w) {
  // 1. 推演轨迹并检查碰撞
  float simX = robotX, simY = robotY, simTheta = robotTheta;
  float minDist = 999;
  
  for (float t = 0; t < SIM_TIME; t += SIM_DT) {
    simTheta += w * SIM_DT;
    simX += v * cos(simTheta) * SIM_DT;
    simY += v * sin(simTheta) * SIM_DT;
    
    // 简化障碍物检查：使用当前超声波距离
    // 实际项目中应基于局部代价地图查询
    float d = sonarDist[0]; // 前方距离
    if (d < minDist) minDist = d;
  }
  
  // 碰撞检查：模拟轨迹末端距障碍物过近则拒绝
  if (minDist < 0.15) return -9999;
  
  // 2. 计算评价指标
  float dx = targetX - simX;
  float dy = targetY - simY;
  float distToGoal = sqrt(dx*dx + dy*dy);
  
  // heading: 朝向目标的程度
  float targetAngle = atan2(dy, dx);
  float headingDiff = fabs(targetAngle - simTheta);
  if (headingDiff > PI) headingDiff = 2*PI - headingDiff;
  float headingScore = 1.0 - headingDiff / PI;
  
  // dist: 障碍物距离得分
  float distScore = min(minDist / 1.0, 1.0);
  
  // velocity: 速度得分
  float velScore = v / MAX_VEL;
  
  // 综合评分
  return ALPHA_HEADING * headingScore + BETA_DIST * distScore + GAMMA_VEL * velScore;
}

// DWA主函数：返回最优(v, ω)
void dwaPlan(float &bestV, float &bestW) {
  bestV = 0; bestW = 0;
  float bestScore = -9999;
  
  // 动态窗口约束
  float vMin = max(MIN_VEL, currentV - MAX_ACCEL * SIM_TIME);
  float vMax = min(MAX_VEL, currentV + MAX_ACCEL * SIM_TIME);
  float wMin = max(-MAX_W, currentW - MAX_W_ACCEL * SIM_TIME);
  float wMax = min(MAX_W, currentW + MAX_W_ACCEL * SIM_TIME);
  
  // 速度空间采样
  for (float v = vMin; v <= vMax; v += VEL_STEP) {
    for (float w = wMin; w <= wMax; w += W_STEP) {
      float score = evaluateTrajectory(v, w);
      if (score > bestScore) {
        bestScore = score;
        bestV = v;
        bestW = w;
      }
    }
  }
}

void setup() {
  Serial.begin(115200);
  
  for (int i = 0; i < 3; i++) {
    pinMode(SONAR_TRIG[i], OUTPUT);
    pinMode(SONAR_ECHO[i], INPUT);
  }
  
  motorL.linkSensor(&encL);
  motorR.linkSensor(&encR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 1. 感知
  for (int i = 0; i < 3; i++) {
    sonarDist[i] = measureDistance(SONAR_TRIG[i], SONAR_ECHO[i]);
  }
  
  // 2. DWA规划
  float bestV, bestW;
  dwaPlan(bestV, bestW);
  
  // 3. 差速运动学转换
  float vL = bestV - bestW * 0.15;  // 轮距0.15m
  float vR = bestV + bestW * 0.15;
  
  motorL.move(vL / 0.05);  // 转换为rad/s（轮径0.05m）
  motorR.move(vR / 0.05);
  
  // 4. 更新位姿
  updatePose(bestV, bestW, 0.05);
  currentV = bestV;
  currentW = bestW;
  
  Serial.print("V:"); Serial.print(bestV);
  Serial.print(" W:"); Serial.print(bestW);
  Serial.print(" Dist:"); Serial.println(sonarDist[0]);
  
  delay(50);
}

要点：该代码实现了 DWA 的核心四步循环——动态窗口计算（受加速度约束）、速度采样、轨迹推演与碰撞检查、评价函数选优。评价函数综合了朝向目标（heading）、障碍物距离（dist）和速度（velocity）三个指标。简化处理：障碍物检查直接使用超声波当前距离，而非局部代价地图查询，这在传感器布局合理时可行，但精度有限。

2、DWA + 滚动窗口重规划 + 动态障碍物避让
适用场景：存在动态障碍物（行人、移动设备）的环境。机器人维护一个以自身为中心的滚动局部代价地图，DWA 在窗口内进行高频重规划，当检测到动态障碍物时，临时修改局部代价地图并重新搜索最优轨迹。

/**
 * Arduino BLDC机器人 - DWA + 滚动窗口重规划
 * 核心：局部代价地图动态更新 -> DWA在滚动窗口内重规划 -> 动态障碍物绕行
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);

// ===== 滚动窗口代价地图（简化：极坐标栅格） =====
#define WINDOW_RADIUS 1.5   // 窗口半径 m
#define ANGULAR_RES 12      // 角度分辨率 (30° per cell)
#define RADIAL_RES 5        // 径向分辨率
float localCostmap[ANGULAR_RES][RADIAL_RES]; // 代价 0-1

// ===== DWA参数（沿用案例一） =====
// ... (MAX_VEL, VEL_STEP等)

// ===== 动态障碍物追踪 =====
struct DynamicObstacle {
  float x, y;
  float vx, vy;
  unsigned long lastSeen;
  bool active;
};
#define MAX_DYN_OBS 4
DynamicObstacle dynObs[MAX_DYN_OBS];
int dynObsCount = 0;

// 超声波引脚
const int SONAR_FRONT_TRIG = 22;
const int SONAR_FRONT_ECHO = 23;

// 全局路径点（简化：直线路径）
float globalPath[3][2] = {{0,0}, {1.5,0.5}, {3.0,1.0}};
int currentWaypoint = 1;

float measureDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long dur = pulseIn(echo, HIGH, 20000);
  return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}

// 更新滚动窗口代价地图
void updateLocalCostmap(float dist, float angle) {
  // 将障碍物映射到极坐标栅格
  int angIdx = (int)((angle + PI) / (2*PI) * ANGULAR_RES);
  int radIdx = (int)(dist / WINDOW_RADIUS * RADIAL_RES);
  angIdx = constrain(angIdx, 0, ANGULAR_RES-1);
  radIdx = constrain(radIdx, 0, RADIAL_RES-1);
  
  // 膨胀：标记障碍物周围区域
  for (int da = -1; da <= 1; da++) {
    for (int dr = -1; dr <= 1; dr++) {
      int a = constrain(angIdx+da, 0, ANGULAR_RES-1);
      int r = constrain(radIdx+dr, 0, RADIAL_RES-1);
      localCostmap[a][r] = min(1.0, localCostmap[a][r] + 0.5);
    }
  }
}

// 查询代价地图中某位置的危险程度
float queryCostmap(float x, float y, float robotX, float robotY, float robotTheta) {
  float dx = x - robotX;
  float dy = y - robotY;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx) - robotTheta;
  
  if (dist > WINDOW_RADIUS) return 0;
  
  int angIdx = (int)((angle + PI) / (2*PI) * ANGULAR_RES);
  int radIdx = (int)(dist / WINDOW_RADIUS * RADIAL_RES);
  angIdx = constrain(angIdx, 0, ANGULAR_RES-1);
  radIdx = constrain(radIdx, 0, RADIAL_RES-1);
  
  return localCostmap[angIdx][radIdx];
}

// DWA评价函数（修改版：使用滚动窗口代价地图）
float evaluateTrajectory(float v, float w, float rx, float ry, float rtheta) {
  float simX = rx, simY = ry, simTheta = rtheta;
  float maxCost = 0;
  
  for (float t = 0; t < 1.5; t += 0.1) {
    simTheta += w * 0.1;
    simX += v * cos(simTheta) * 0.1;
    simY += v * sin(simTheta) * 0.1;
    
    float cost = queryCostmap(simX, simY, rx, ry, rtheta);
    if (cost > maxCost) maxCost = cost;
  }
  
  if (maxCost > 0.8) return -9999; // 碰撞风险过高
  
  // 评价指标（简化）
  float distToGoal = sqrt(pow(globalPath[currentWaypoint][0]-simX,2) + 
                          pow(globalPath[currentWaypoint][1]-simY,2));
  float headingScore = 1.0 - distToGoal / 5.0;
  float clearanceScore = 1.0 - maxCost;
  
  return 0.5 * headingScore + 0.5 * clearanceScore;
}

void setup() {
  Serial.begin(115200);
  pinMode(SONAR_FRONT_TRIG, OUTPUT);
  pinMode(SONAR_FRONT_ECHO, INPUT);
  
  // 初始化代价地图
  for (int i = 0; i < ANGULAR_RES; i++)
    for (int j = 0; j < RADIAL_RES; j++)
      localCostmap[i][j] = 0;
  
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 1. 感知：更新滚动窗口
  float dist = measureDistance(SONAR_FRONT_TRIG, SONAR_FRONT_ECHO);
  if (dist < WINDOW_RADIUS) {
    updateLocalCostmap(dist, 0); // 前方
  }
  
  // 代价地图衰减（遗忘旧障碍）
  for (int i = 0; i < ANGULAR_RES; i++)
    for (int j = 0; j < RADIAL_RES; j++)
      localCostmap[i][j] *= 0.95;
  
  // 2. DWA规划（简化）
  float bestV = 0.3, bestW = 0;
  float bestScore = -9999;
  
  for (float v = 0.1; v <= 0.5; v += 0.1) {
    for (float w = -1.0; w <= 1.0; w += 0.2) {
      float score = evaluateTrajectory(v, w, 0, 0, 0); // 简化：使用原点
      if (score > bestScore) {
        bestScore = score;
        bestV = v;
        bestW = w;
      }
    }
  }
  
  // 3. 执行
  float vL = bestV - bestW * 0.15;
  float vR = bestV + bestW * 0.15;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);
  
  Serial.print("V:"); Serial.print(bestV);
  Serial.print(" W:"); Serial.print(bestW);
  Serial.print(" FrontDist:"); Serial.println(dist);
  
  delay(50);
}

要点：该案例引入了滚动窗口代价地图的概念。与案例一的“直接用超声波距离”不同，它将障碍物映射到以机器人为中心的极坐标栅格中，DWA 评价时查询轨迹经过的栅格代价。动态障碍物处理：代价地图随时间衰减（*= 0.95），旧障碍物逐渐“遗忘”，使机器人能够适应移动障碍物。专利 CN112631294A 中描述的“以机器人当前位置为起点，全局规划路径上最临近关键节点为临时目标点”的策略与此类似。

3、DWA + 全局路径跟随（分层导航架构）
适用场景：存在全局路径（A* 输出）的长距离导航任务。DWA 不直接朝向最终目标，而是跟随全局路径的“最临近前视点”，实现“全局粗规划 + 局部精避障”的分层协同。

/**
 * Arduino BLDC机器人 - DWA + 全局路径跟随
 * 核心：全局A*路径 -> 提取局部前视点 -> DWA朝向该点规划
 * 参考：分层导航架构（全局A* + 局部DWA）
 */

#include <SimpleFOC.h>

BLDCMotor motorL(7);
BLDCMotor motorR(7);

// ===== 全局路径（简化：预定义路径点） =====
#define PATH_LEN 6
float globalPath[PATH_LEN][2] = {
  {0.0, 0.0}, {1.0, 0.2}, {2.0, 0.5}, 
  {2.5, 1.0}, {3.0, 1.8}, {3.5, 2.5}
};

// ===== 前视距离 =====
const float LOOKAHEAD_DIST = 0.8; // m

// ===== DWA参数（沿用案例一） =====
const float MAX_VEL = 0.5;
const float VEL_STEP = 0.1;
const float W_STEP = 0.2;

// 机器人状态
float robotX = 0, robotY = 0, robotTheta = 0;
float currentV = 0, currentW = 0;

// 超声波
const int SONAR_TRIG = 22;
const int SONAR_ECHO = 23;
float frontDist = 999;

float measureDistance() {
  digitalWrite(SONAR_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(SONAR_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(SONAR_TRIG, LOW);
  long dur = pulseIn(SONAR_ECHO, HIGH, 20000);
  return (dur == 0) ? 999.0 : dur * 0.0343 / 2.0;
}

// 从全局路径提取前视点
// 策略：找到距离机器人最近且超过前视距离的路径点
void getLookaheadPoint(float &laX, float &laY) {
  int closestIdx = 0;
  float closestDist = 9999;
  
  // 找到最近路径点
  for (int i = 0; i < PATH_LEN; i++) {
    float d = sqrt(pow(globalPath[i][0]-robotX,2) + pow(globalPath[i][1]-robotY,2));
    if (d < closestDist) {
      closestDist = d;
      closestIdx = i;
    }
  }
  
  // 从最近点向后找，直到超过前视距离
  laX = globalPath[PATH_LEN-1][0];
  laY = globalPath[PATH_LEN-1][1];
  
  for (int i = closestIdx; i < PATH_LEN; i++) {
    float d = sqrt(pow(globalPath[i][0]-robotX,2) + pow(globalPath[i][1]-robotY,2));
    if (d >= LOOKAHEAD_DIST) {
      laX = globalPath[i][0];
      laY = globalPath[i][1];
      break;
    }
  }
}

// DWA评价（朝向局部前视点）
float evaluateTrajectory(float v, float w, float laX, float laY) {
  float simX = robotX, simY = robotY, simTheta = robotTheta;
  
  for (float t = 0; t < 1.5; t += 0.1) {
    simTheta += w * 0.1;
    simX += v * cos(simTheta) * 0.1;
    simY += v * sin(simTheta) * 0.1;
  }
  
  // 碰撞检查
  if (frontDist < 0.15 && v > 0.1) return -9999;
  
  // 朝向局部前视点
  float dx = laX - simX;
  float dy = laY - simY;
  float targetAngle = atan2(dy, dx);
  float headingDiff = fabs(targetAngle - simTheta);
  if (headingDiff > PI) headingDiff = 2*PI - headingDiff;
  float headingScore = 1.0 - headingDiff / PI;
  
  float distScore = min(frontDist / 1.0, 1.0);
  float velScore = v / MAX_VEL;
  
  return 0.4 * headingScore + 0.4 * distScore + 0.2 * velScore;
}

void setup() {
  Serial.begin(115200);
  pinMode(SONAR_TRIG, OUTPUT);
  pinMode(SONAR_ECHO, INPUT);
  
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 1. 感知
  frontDist = measureDistance();
  
  // 2. 获取局部前视点
  float laX, laY;
  getLookaheadPoint(laX, laY);
  
  // 3. DWA规划（朝向局部前视点）
  float bestV = 0.3, bestW = 0;
  float bestScore = -9999;
  
  for (float v = 0.1; v <= 0.5; v += VEL_STEP) {
    for (float w = -1.0; w <= 1.0; w += W_STEP) {
      float score = evaluateTrajectory(v, w, laX, laY);
      if (score > bestScore) {
        bestScore = score;
        bestV = v;
        bestW = w;
      }
    }
  }
  
  // 4. 执行
  float vL = bestV - bestW * 0.15;
  float vR = bestV + bestW * 0.15;
  motorL.move(vL / 0.05);
  motorR.move(vR / 0.05);
  
  // 5. 更新位姿（简化）
  robotTheta += bestW * 0.05;
  robotX += bestV * cos(robotTheta) * 0.05;
  robotY += bestV * sin(robotTheta) * 0.05;
  currentV = bestV;
  currentW = bestW;
  
  Serial.print("V:"); Serial.print(bestV);
  Serial.print(" W:"); Serial.print(bestW);
  Serial.print(" LookAhead:"); Serial.print(laX); Serial.print(","); Serial.println(laY);
  
  delay(50);
}

要点：该案例体现了分层导航的核心思想——全局路径提供“宏观引导”，DWA 的朝向评分不再指向最终目标，而是指向全局路径上的局部前视点。搜索结果指出，分层架构将“慢思考”（全局规划，3-5Hz）与“快反应”（局部DWA，10-20Hz）分离，全局规划负责从宏观上找“最优路”，局部规划负责在微观上“安全走”。前视点的选取策略是关键：距离过近会导致机器人“贴线行驶”，过远则会“抄近道”偏离路径。

要点解读

1. DWA 的动态窗口本质是“三重约束的交集”，而非简单的速度范围。 动态窗口由三个集合的交集构成：硬件速度约束（电机物理极限）、加速度约束（一个控制周期内可达的速度范围，窗口“动态”变化的核心）、安全制动约束（保证在碰撞前能刹停）。很多初学者只考虑第一个约束，导致轨迹推演时选出的速度“物理上可达但安全上不可行”。案例一中的 vMin = max(MIN_VEL, currentV - MAX_ACCEL * SIM_TIME) 体现了加速度约束，而安全制动约束需要结合障碍物距离和减速度计算。

2. 评价函数的三项权重（heading/dist/velocity）是 DWA 调优的核心战场，不存在通用最优值。 搜索结果中的实验参数（α=0.4, β=0.4, γ=0.2）是一个合理的起点，但实际值需根据场景调整。heading 权重过高会导致机器人为对准目标而“忽视”障碍物；dist 权重过高会导致机器人“畏缩不前”，在狭窄通道中原地打转；velocity 权重过高则会让机器人“贪快”而频繁触发避障。调优策略：先在无障碍环境中调 heading 和 velocity，再加入障碍物调 dist，逐步增加 dist 权重直到轨迹平滑且不碰撞。

3. Arduino 的算力瓶颈决定了 DWA 必须“简化采样”或“分层部署”。 DWA 的标准实现需要采样数十组速度、每组推演数十个时间步、每步查询局部代价地图，计算量在 Arduino Uno 上难以实时完成。务实方案：减少采样数量（如线速度 5 个采样、角速度 7 个采样，共 35 条轨迹），缩短推演时间（1.0~1.5s），使用简单的极坐标代价地图而非完整栅格地图。如果算力仍不足，应将 DWA 部署在 ESP32/STM32 或上位机，Arduino 仅作为速度执行器。

4. 滚动窗口代价地图的“遗忘机制”是动态避障的关键，但遗忘速率需谨慎设定。 案例二中的 localCostmap[i][j] *= 0.95 使旧障碍物逐渐消失。遗忘过快会导致机器人“忘记”刚刚避开的静态障碍物，重新撞上；遗忘过慢则会让动态障碍物的“残影”持续影响规划，导致机器人绕远路。经验值：代价衰减系数在 0.9~0.98 之间（对应半衰期约 0.3~1.5 秒），需根据传感器更新频率和机器人速度调整。

5. 全局路径跟随的“前视距离”是平衡“路径精度”与“避障灵活性”的杠杆。 前视距离过短（<0.5m），DWA 会频繁“贴线”修正，轨迹抖动明显；前视距离过长（>1.5m），机器人会“抄近道”偏离全局路径，在密集障碍物中可能陷入死角。推荐策略：前视距离与机器人速度成正比（如 lookahead = 0.6 + 0.5 * v），速度越快前视越远，保证高速时的轨迹平滑性；同时在检测到近距障碍物时临时缩短前视距离，优先避障。

4、室内平整地面DWA局部避障导航程序（核心：轻量DWA+基础平稳避障）
适用场景：车间物流、室内巡检等平整地面场景，环境简单、障碍稀疏，要求机器人避障稳定、行驶流畅。

核心逻辑
以激光测距实现局部环境采样，结合里程计获取实时位姿
在速度窗口内采样多组运动指令，推算短期前进轨迹
根据障碍距离、轨迹偏移、速度三大指标评分，筛选最优指令驱动电机，遇障平滑转向。

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte LIDAR_PIN = A0;  // 简化激光测距
const byte PWM_LEFT = 9, DIR_LEFT = 8;
const byte PWM_RIGHT = 10, DIR_RIGHT = 11;
const byte ENC_LEFT_A = 2, ENC_RIGHT_A = 4;

// ========== DWA参数 ==========
// 速度窗口
const float V_MIN = 0.2, V_MAX = 0.6, W_MIN = -0.5, W_MAX = 0.5;
// 采样精度
const int CMD_NUM = 10;
// 评分权重
const float ALPHA_OBS = 1.0, ALPHA_HEAD = 0.5, ALBA_VEL = 0.3;
const float DIST_SAFE = 0.5;  // 安全距离，单位米

// ========== 变量 ==========
long encLeft = 0, encRight = 0;
float robotX = 0, robotY = 0, robotTheta = 0;

// ========== 函数声明 ==========
uint16_t getLidarDistance();
void odometry(unsigned long dt);
std::vector<std::pair<float,float>> predictTrajectory(float v, float w, float dt);
float scoreTrajectory(std::vector<std::pair<float,float>> traj, float targetX, float targetY);
void driveMotor(int leftPWM, int rightPWM);

// ========== 简化向量结构（避免依赖库） ==========
struct Point {
  float x, y;
  Point(float x=0, float y=0):x(x),y(y){}
};

void setup() {
  Serial.begin(115200);
  Serial.println("室内DWA局部避障启动");

  pinMode(LIDAR_PIN, INPUT);
  pinMode(PWM_LEFT, OUTPUT); pinMode(DIR_LEFT, OUTPUT);
  pinMode(PWM_RIGHT, OUTPUT); pinMode(DIR_RIGHT, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_LEFT_A), [](){encLeft++;}, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_RIGHT_A), [](){encRight++;}, RISING);
}

void loop() {
  unsigned long dt = millis();
  // 1. 里程计算位姿
  odometry(dt);

  // 2. 采集最近的局部环境
  uint16_t dist = getLidarDistance();

  // 3. 遍历速度窗口，筛选最优指令
  float bestV = V_MIN, bestW = W_MIN;
  float bestScore = -1000;

  for(int i=0; i<CMD_NUM; i++) {
    float v = V_MIN + (V_MAX - V_MIN) * i / (CMD_NUM-1);
    float w = W_MIN + (W_MAX - W_MIN) * i / (CMD_NUM-1);

    // 推算短期轨迹
    auto traj = predictTrajectory(v, w, 10);
    // 轨迹评分
    float score = scoreTrajectory(traj, 2.0, 2.0); // 假设目标点(2,2)
    if(score > bestScore) {
      bestScore = score;
      bestV = v; bestW = w;
    }
  }

  // 4. 速度转轮速，驱动电机
  int leftPWM = map(bestV - bestW*0.3, 0, 1, 50, 180);
  int rightPWM = map(bestV + bestW*0.3, 0, 1, 50, 180);
  driveMotor(leftPWM, rightPWM);

  Serial.print("最优速度：");Serial.print(bestV,2);
  Serial.print(" 角速度：");Serial.print(bestW,2);
  delay(30);
}

// 激光测距，模拟返回距离（cm）
uint16_t getLidarDistance() {
  return map(analogRead(LIDAR_PIN),0,1023,5,100);
}

// 简易里程计，更新机器人位姿
void odometry(unsigned long dt) {
  // 简化：根据编码器增量推算位移与航向
  long deltaL = 0, deltaR = 0; // 实际获取编码器差值
  float deltaS = (deltaL + deltaR) * 0.001;
  float deltaTheta = (deltaR - deltaL) * 0.0005;

  robotTheta += deltaTheta;
  robotX += deltaS * cos(robotTheta);
  robotY += deltaS * sin(robotTheta);
}

// 推算预测轨迹
std::vector<std::pair<float,float>> predictTrajectory(float v, float w, float dt) {
  std::vector<std::pair<float,float>> traj;
  float x = robotX, y = robotY, theta = robotTheta;
  for(int i=0; i<10; i++) {
    x += v * cos(theta) * dt/100;
    y += v * sin(theta) * dt/100;
    theta += w * dt/100;
    traj.push_back({x,y});
  }
  return traj;
}

// 轨迹评分：障碍距离+航向对齐+速度
float scoreTrajectory(std::vector<std::pair<float,float>> traj, float tx, float ty) {
  // 障碍距离得分
  uint16_t dist = getLidarDistance();
  float obsScore = dist > DIST_SAFE*100 ? 100 : (dist / (DIST_SAFE*100)) * 100;

  // 航向对齐得分（轨迹朝向目标）
  float lastX = traj.back().first, lastY = traj.back().second;
  float dx = lastX - robotX, dy = lastY - robotY;
  float targetTheta = atan2(dy, dx);
  float headScore = targetTheta == robotTheta ? 100 : 50;

  // 速度得分
  float vParam = traj.size() * 0.1;
  float velScore = vParam * ALBA_VEL;

  return ALPHA_OBS*obsScore + ALPHA_HEAD*headScore + velScore;
}

// 驱动电机
void driveMotor(int leftPWM, int rightPWM) {
  leftPWM = constrain(leftPWM, 0, 200);
  rightPWM = constrain(rightPWM, 0, 200);
  analogWrite(PWM_LEFT, leftPWM);
  analogWrite(PWM_RIGHT, rightPWM);
}

适用场景优化
可替换为真实激光雷达，实现360°环境采样，提升避障精度
增加目标点动态更新，适配巡检路径的实时调整
优化评分权重，根据室内环境调整障碍与航向的优先级

5、户外越野DWA动态避障程序（核心：多方向采样+复杂障碍避让）
适用场景：户外巡检、野外勘察等场景，障碍不规则、地形起伏，要求机器人灵活避障、避免侧翻。

核心逻辑
多通道测距实现多方向环境感知，识别不规则障碍
扩大速度窗口与轨迹采样数量，适配户外复杂工况
加入坡度与机身姿态修正，避障同时保证行驶稳定，防止侧翻。

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte SENSOR[4] = {A0,A1,A2,A3}; // 前后左右四向测距
const byte PWM[2] = {9,10}, DIR[2]={8,11};
const byte ENC[2]={2,4};
const byte IMU_PIN=A4;

// ========== DWA参数 ==========
const float V_MIN=0.3, V_MAX=0.7, W_MIN=-0.6, W_MAX=0.6;
const int CMD_NUM=15; // 户外采样数增多
const float ALPHA_OBS=1.2, ALPHA_HEAD=0.6, ALPHA_VEL=0.4;
const float SAFE_DIST=0.4;
const int MAX_PITCH=15; // 允许最大坡度

// ========== 变量 ==========
long enc[2]={0,0};
float robotX=0, robotY=0, robotTheta=0;
int pitchAngle=0;
// ========== 函数声明 ==========
uint16_t getFrontDist();
uint16_t getLeftDist(), getRightDist();
int getPitch();
std::vector<std::pair<float,float>> predictTraj(float v, float w);
float evalTraj(std::vector<std::pair<float,float>> traj);
void driveMotor(uint8_t idx, int pwm);

void setup() {
  Serial.begin(115200);
  Serial.println("户外越野DWA避障启动");

  for(uint8_t i=0;i<4;i++) pinMode(SENSOR[i], INPUT);
  for(uint8_t i=0;i<2;i++) {
pinMode(PWM[i],OUTPUT); pinMode(DIR[i],OUTPUT);
attachInterrupt(digitalPinToInterrupt(ENC[i]),[i](){enc[i]++;},RISING);
}
}

void loop() {
  unsigned long dt=millis();
  // 采集环境与姿态数据
  uint16_t front = getFrontDist();
  uint16_t left = getLeftDist();
  uint16_t right = getRightDist();
  pitchAngle = getPitch();
  // 坡度超限，降低速度窗口
  bool slopeRestrict = pitchAngle > MAX_PITCH;
  float vMax = slopeRestrict ? V_MAX*0.5 : V_MAX;
  // DWA轨迹搜索
  float bestV=V_MIN, bestW=W_MIN, bestScore=-1000;
  for(int i=0;i<CMD_NUM;i++) {
    float v = V_MIN + (vMax - V_MIN)*i/(CMD_NUM-1);
    float w = W_MIN + (W_MAX - W_MIN)*i/(CMD_NUM-1);
    auto traj = predictTraj(v,w);
    float score = evalTraj(traj);
    if(score>bestScore) {
      bestScore=score; bestV=v; bestW=w;
    }
  }
  // 差速驱动
  int leftPWM = bestV + bestW*0.3;
  int rightPWM = bestV - bestW*0.3;
  driveMotor(0, constrain(leftPWM*150,0,200));
  driveMotor(1, constrain(rightPWM*150,0,200));

  Serial.print("前距：");Serial.print(front);
  Serial.print(" 坡度：");Serial.println(pitchAngle);
  delay(30);
}

// 前方测距（cm）
uint16_t getFrontDist() {
  return map(analogRead(SENSOR[0]),0,1023,10,80);
} uint16_t getLeftDist() { return map(analogRead(SENSOR[1]),0,1023,10,80); }
uint16_t getRightDist() { return map(analogRead(SENSOR[2]),0,1023,10,80); }

// 获取坡度
int getPitch() {
  return analogRead(IMU_PIN)%30; // 简化模拟
}

// 生成预测轨迹
std::vector<std::pair<float,float>> predictTraj(float v, float w) {
  std::vector<std::pair<float,float>> traj;
  float x=robotX, y=robotY, theta=robotTheta;
  for(int i=0;i<12;i++) {
    x += v*cos(theta)*0.1;
    y += v*sin(theta)*0.1;
    theta += w*0.1;
    traj.push_back({x,y});
  }
  return traj;
}

// 轨迹评分：前方障碍+侧方安全+坡度影响
float evalTraj(std::vector<std::pair<float,float>> traj) {
  uint16_t front=getFrontDist();
  uint16_t left=getLeftDist();
  uint16_t right=getRightDist();

  // 前方障碍得分
  float frontScore = front>SAFE_DIST*100?80:(front/(SAFE_DIST*100))*80;
  // 侧向安全得分（转弯时规避侧障）
  float sideScore=100;
  // 坡度影响得分，越陡得分越低
  float slopeScore = pitchAngle>MAX_PITCH ?30:100;

  return ALPHA_OBS*frontScore + ALPHA_HEAD*sideScore + ALPHA_VEL*slopeScore;
}

// 驱动电机
void driveMotor(uint8_t idx, int pwm) {
  digitalWrite(DIR[idx],HIGH);
  analogWrite(PWM[idx],pwm);
}

适用场景优化
加入多角度激光测距，实现更密集的环境点云，提升障碍识别精度
引入地形识别算法，提前预判障碍类型，自适应调整避障策略
优化轨迹评分逻辑，加入避障优先级，应对突发动态障碍物

6、狭窄空间DWA精准避障作业程序（核心：高精度轨迹+限制动范围）
适用场景：管道检修、室内设备维护等狭窄场景，空间受限、障碍密集，要求机器人精准避障、不碰壁。

核心逻辑
缩小速度窗口与轨迹预测范围，适配狭窄空间的灵活调整
高精度采样与轨迹计算，确保微小转向的精准性
多维度轨迹评分，优先避障，结合作业联动，实现避障与作业协同。

#include <Arduino.h>

// ========== 硬件定义 ==========
const byte LIDAR_PIN=A0;
const byte PWM[2]={9,10}, DIR[2]={8,11};
const byte ENC[2]={2,4};
const byte WORK_PIN=12; // 作业设备（如机械臂）
// ========== DWA参数（狭窄空间缩小窗口） ==========
const float V_MIN=0.1, V_MAX=0.35, W_MIN=-0.4, W_MAX=0.4;
const int CMD_NUM=20; // 高精度采样
const float ALPHA_OBS=1.5, ALPHA_HEAD=0.3, ALPHA_VEL=0.2;
const float SAFE_DIST=0.15;

// ========== 变量 ==========
long enc[2]={0,0};
float robotX=0, robotY=0, robotTheta=0;
// ========== 函数声明 ==========
uint16_t getLidarDist();
std::vector<std::pair<float,float>> getTraj(float v, float w);
float scoreTraj(std::vector<std::pair<float,float>> traj);
void driveMotor(uint8_t i, int pwm);

void setup() {
Serial.begin(115200);
  Serial.println("狭窄空间DWA避障启动");

  pinMode(LIDAR_PIN, INPUT);
  for(uint8_t i=0;i<2;i++) {
pinMode(PWM[i],OUTPUT); pinMode(DIR[i],OUTPUT);
attachInterrupt(digitalPinToInterrupt(ENC[i]), [i](){enc[i]++;}, RISING);
}
pinMode(WORK_PIN, OUTPUT);
} void loop() {
  unsigned long dt=millis();
  // 激光测距
  uint16_t dist=getLidarDist();
  // 搜索最优轨迹
  float bestV=V_MIN, bestW=W_MIN, bestScore=-1000;
  for(int i=0;i<CMD_NUM;i++) {
    float v=V_MIN+(V_MAX-V_MIN)*i/(CMD_NUM-1);
    float w=W_MIN+(W_MAX-W_MIN)*i/(CMD_NUM-1);
    auto traj=getTraj(v,w);
float score=scoreTraj(traj);
    if(score>bestScore) {
      bestScore=score;
      bestV=v;  bestW=w;
    }
  }
  // 驱动
  int leftPWM=(bestV + bestW*0.2)*180;
  int rightPWM=(bestV - bestW*0.2)*180;
  driveMotor(0, constrain(leftPWM,0,200));
  driveMotor(1, constrain(rightPWM,0,200));

  // 作业联动：避障安全时开启作业
  digitalWrite(WORK_PIN, dist>SAFE_DIST*100 ? HIGH:LOW);

  Serial.print("距离：");Serial.print(dist);
  Serial.print(" 角速度：");Serial.println(bestW,2);
  delay(20);
}

// 高精度激光测距
uint16_t getLidarDist() {
  return map(analogRead(LIDAR_PIN),0,1023,5,30);
}

// 生成精准预测轨迹
std::vector<std::pair<float,float>> getTraj(float v, float w) {
  std::vector<std::pair<float,float>> traj;
  float x=robotX, y=robotY, theta=robotTheta;
  for(int i=0;i<15;i++) { // 增加采样点
    x += v*cos(theta)*0.05;
    y += v*sin(theta)*0.05;
    theta += w*0.05;
    traj.push_back({x,y});
  }
  return traj;
}

// 轨迹评分：精度优先，避障第一
float scoreTraj(std::vector<std::pair<float,float>> traj) {
  uint16_t dist=getLidarDist();
  // 障碍距离得分，安全距离内严格扣分
  float obsScore;
  if(dist > SAFE_DIST*100) {
    obsScore=100;
  } else {
    obsScore=50;
  }
  // 航向得分，贴合通道走向
  float headingAlign = abs(traj.back().x) < 0.1 ? 80:40;
  // 速度得分，狭窄空间低速优先
  float vScore = traj.size()*0.05;
  return ALPHA_OBS*obsScore + ALPHA_HEAD*headingAlign + ALPHA_VEL*vScore;
}

// 驱动电机
void driveMotor(uint8_t idx, int pwm) {
  digitalWrite(DIR[idx],HIGH);
  analogWrite(PWM[idx],pwm);
}

适用场景优化
加入视觉识别，识别管道内壁、设备边缘，实现更精准的边界避让
引入路径记忆功能，重复作业时复用安全轨迹，提升效率
优化速度窗口动态调整，根据空间宽度实时缩放，适配更复杂的狭窄环境

要点解读
要点1：速度窗口合理设定，平衡灵活度与稳定性
速度窗口是DWA的核心参数，决定了机器人的避障灵活度与行驶稳定性。
按场景定范围：室内平整环境可设较宽窗口，提升行驶速度；狭窄、越野环境缩小窗口，保证可控性
动态调整窗口：遇到复杂障碍、狭窄区域时，自动收窄速度与角速度上限，避免失控
窗口与控制周期匹配：采样频率越高，窗口可适当缩小，确保轨迹推算精度，避免预测误差
要点2：环境感知精准度决定避障可靠性
DWA依赖局部环境数据，感知精度直接影响避障效果与安全性。
多传感器融合：激光、超声、视觉组合感知，弥补单一传感器的盲区与误差，提升障碍识别能力
高精度采样：障碍密集场景需提高采样频率，确保捕捉到细微的障碍边界，避免擦碰
数据预处理：对采集的环境数据进行滤波、去噪，减少干扰导致的误判，提升数据可信度
要点3：轨迹评分权重合理设计，匹配作业需求
轨迹评分是筛选最优指令的核心，权重分配需贴合具体作业目标。
避障优先级最高：多数场景下，障碍距离权重占比最大，确保避障安全，再兼顾其他目标
按需调整权重：巡检场景侧重航向对齐，作业场景侧重行驶稳定，动态分配三大评分权重
加入个性化指标：可结合能耗、效率、机身姿态等，丰富评分体系，适配特殊需求
要点4：预测轨迹与实际运动匹配，减少偏差
DWA的轨迹推算需与机器人实际运动特性高度匹配，否则会出现避障失效。
匹配运动学模型：采用机器人实际的运动学模型推算轨迹，避免理想化模型导致的预测误差
控制执行偏差：电机响应延迟、转速误差会影响实际轨迹，加入闭环反馈，实时修正指令
仿真与标定：提前标定机器人运动参数，通过仿真验证轨迹精度，确保实际执行贴合预期
要点5：实时性与算力适配，保障系统流畅运行
DWA需要实时计算大量轨迹，需平衡算法复杂度与硬件算力，确保实时响应。
算法轻量化：简化轨迹计算步骤，减少采样点数量，适配Arduino等低算力平台
优化计算逻辑：采用高效数据结构，减少冗余计算，提升轨迹评分与筛选的速度
多线程与中断优化：将传感器采集、电机控制与DWA计算分离，避免阻塞，保障实时响应突发障碍

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