基于Arduino与BLDC（无刷直流电机）构建的机器人系统，其“DWA（动态窗口法）基础运动约束算法”是一套将运动学限制、动力学约束与实时避障深度融合的局部路径规划与速度控制核心架构。该方案通过在速度空间内进行约束采样与轨迹评估，为BLDC底盘生成安全、平滑且物理可达的速度指令，是机器人实现动态避障与精准导航的关键技术。

主要特点

1. 三重约束下的动态速度窗口构建
   DWA算法的核心在于“动态窗口”的实时计算，该窗口并非固定范围，而是根据机器人当前状态动态变化的可行速度空间。算法通过三重约束确定采样边界：速度边界约束（Vm）由BLDC硬件极限决定，即最大/最小线速度和角速度；加速度约束（Vd）由电机性能决定，在当前速度基础上考虑最大加减速能力，确保速度指令的物理可达性；安全约束（Vs）确保机器人能在碰到障碍物前刹停，通过公式v ≤ √(2·dist·d_v)剔除会导致碰撞的速度组合。最终采样空间为三者交集：V = Vm ∩ Vd ∩ Vs。
2. 基于运动学模型的短时轨迹预测
   在动态窗口内，算法均匀采样多组（v, ω）速度对（工程中通常线速度采样20个、角速度采样20个，共400条轨迹），并基于差速驱动运动学模型进行短时轨迹推演。假设在预测时间T（通常1-3秒）内速度恒定，通过迭代计算得到未来一段时间内的离散轨迹点序列。每条轨迹都会逐点校验其与障碍物的最小距离，若小于安全阈值则直接淘汰，确保安全性优先。
3. 多目标加权评价函数与最优轨迹选择
   DWA通过多目标评价函数G(v, ω) = α·heading(v, ω) + β·dist(v, ω) + γ·velocity(v, ω)对每条模拟轨迹进行打分。heading（方位角评价）衡量轨迹终点朝向与目标点方向的接近程度，角度差越小得分越高；dist（障碍物距离评价）衡量轨迹上所有点与最近障碍物的最小距离，距离越远得分越高；velocity（速度评价）衡量轨迹的线速度大小，速度越大得分越高以鼓励高效运动。算法选择得分最高的轨迹对应的速度(v_best, ω_best)作为当前控制周期的输出，并在下一周期重新感知、规划，形成“感知-规划-执行”闭环。
4. BLDC高动态响应与FOC平滑执行
   DWA算法输出的速度和角速度指令往往是连续且高频变化的（控制频率通常10-20Hz）。BLDC电机配合FOC（磁场定向控制）驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪DWA输出的平滑速度曲线。在频繁启停和转向的避障过程中，BLDC能保持运动平滑，避免了传统步进电机或直流有刷电机在频繁启停和转向时的机械冲击与轨迹偏差。

应用场景

1. 人机协作仓储与物流AMR
   在电商仓库或柔性制造车间，人员和叉车会随时横穿机器人通道。DWA算法能确保AMR在高速运行中，对突然出现的动态障碍物进行平滑减速绕行，并在安全后迅速恢复原速。BLDC的高动态响应能力使机器人能快速响应避障指令（如紧急制动、原地转向），保障物流效率与人员安全。
2. 室内服务与送餐机器人
   在商场、餐厅或酒店等动态人流环境中，机器人需实时避开行人、桌椅等移动或静态障碍物。DWA的短时预测机制能前瞻性地评估多条轨迹的安全性，结合BLDC的柔顺运动控制，使机器人行为更加自然、舒适，避免“急停急走”的僵硬感。
3. 教育与竞赛机器人平台
   在Robocon、智能车竞赛等场景中，Arduino+BLDC构成低成本高性能底盘，要求在未知赛道中实现高速避障与路径跟踪。DWA算法计算量相对可控，非常适合Arduino级别的平台，是学习局部路径规划与运动控制算法的绝佳实践项目。
4. AGV原型开发与验证
   工厂物流AGV需在狭窄通道中运行，动态避障保障人机共存安全。DWA可作为局部规划器与全局路径规划器（如A*）协同工作：全局规划器提供宏观参考路径，DWA在滚动窗口内根据实时传感器数据进行局部避障与路径微调。Arduino平台可用于验证控制逻辑，再迁移至工业控制器。

注意事项

1. 评价函数权重系数的场景化标定
   DWA的性能高度依赖于评价函数中各指标（heading、dist、velocity）的权重系数。权重并非固定经验值，需根据应用场景系统性标定：仓储AGV强调效率可适当提高velocity权重，服务机器人侧重安全性应提高dist权重，heading权重过高会导致机器人直冲目标忽略障碍物，dist权重过高则会使机器人过度保守、贴着墙根走。建议采用典型权重作为起点（如α=0.75, β=0.05, γ=0.2），再根据实际效果微调。
2. 采样分辨率与计算量的平衡
   速度采样分辨率（v和ω的采样步长）直接影响计算量和路径质量。步长太粗可能漏掉好的速度组合，步长太细则计算量过大。工程上一般v采样10-20个值，ω采样20-40个值，总共200-800条轨迹。在Arduino Uno等8位MCU上，若同时运行FOC控制，计算资源极为紧张，建议简化采样数量或采用定点数运算优化，或优先选用ESP32、STM32等具备硬件FPU的高性能微控制器。
3. 仿真时间与预测精度的权衡
   前向仿真时间（sim_time）太短看不到远处障碍物，太长则计算量大且预测不准。一般取1-3秒：机器人速度快时仿真时间短一些（如1秒），速度慢时可适当延长。仿真时间与控制频率也有关——频率高（如20Hz）时仿真时间可以短一些，因为很快会重新规划。
4. 局部最小值与U型障碍物困境
   DWA是局部规划算法，可能陷入局部最优解。在U型障碍物中，机器人可能在U口来回摆动无法脱困。工程上的解决办法包括：如果DWA连续几个周期输出速度接近零，触发恢复行为（如原地旋转或后退）；引入全局路径引导，让heading函数参考全局路径而非直接指向目标点，避免机器人在局部区域“打转”。
5. 运动学参数标定与模型准确性
   DWA算法的准确性高度依赖于机器人运动学参数的标定。必须精确测量BLDC底盘的最大线速度、最大角速度、最大加速度和最大减速度，以及轮径R和轮距D等参数。参数偏差会导致规划出的轨迹在实际执行时发生碰撞或偏离。建议在部署前进行系统辨识实验，确保模型参数与实际硬件一致。
6. 传感器数据质量与噪声抑制
   DWA算法依赖实时传感器数据构建局部环境地图。必须采用卡尔曼滤波或中值滤波等算法，对激光雷达或超声波传感器的数据进行预处理，剔除因环境噪声或动态干扰产生的野值。注意不同传感器的有效测距范围与更新频率匹配——超声波更新慢，不宜用于高速避障场景。
7. 电源隔离与电磁兼容性
   BLDC电机的大电流PWM驱动信号极易干扰传感器信号和主控板。在硬件设计上，必须将电机动力线与传感器信号线严格分开布线，并做好屏蔽处理；建议采用隔离电源（如DC-DC模块）为控制电路供电，防止电机启停时的电压波动导致主控板复位或传感器数据失真。

1、DWA基础速度空间搜索与BLDC差速执行
此案例实现DWA的最小可行框架。核心是在由BLDC最大线速度、角速度和加速度构成的速度窗口内采样，推演每条轨迹的终点到目标的距离，选择最近的一条作为执行指令。

#include <SimpleFOC.h>
#include <math.h>

// ===== BLDC 差速底盘 =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);
Encoder encL(2,3), encR(4,5);

// ===== DWA 速度采样参数 =====
const float MAX_V = 0.5;       // 最大线速度(m/s)
const float MAX_W = 1.0;       // 最大角速度(rad/s)
const float V_STEP = 0.1;      // 线速度采样步长
const float W_STEP = 0.2;      // 角速度采样步长
const float DT = 0.1;          // 控制周期(s)
const float SIM_TIME = 1.0;    // 轨迹推演时间(s)

// ===== 机器人位姿 =====
float robotX = 0, robotY = 0, robotYaw = 0;

// ===== DWA 速度搜索核心 =====
void dwaSearch(float& bestV, float& bestW) {
    bestV = 0; bestW = 0;
    float bestCost = 1e9;

    // 在速度空间内均匀采样
    for (float v = 0; v <= MAX_V; v += V_STEP) {
        for (float w = -MAX_W; w <= MAX_W; w += W_STEP) {
            // 推演该速度对应的轨迹终点
            float simX = robotX, simY = robotY, simYaw = robotYaw;
            for (float t = 0; t < SIM_TIME; t += DT) {
                simX += v * cos(simYaw) * DT;
                simY += v * sin(simYaw) * DT;
                simYaw += w * DT;
            }

            // 评价：到目标点的距离越小越好
            float distToGoal = sqrt(pow(5.0 - simX, 2) + pow(3.0 - simY, 2));
            if (distToGoal < bestCost) {
                bestCost = distToGoal;
                bestV = v;
                bestW = w;
            }
        }
    }
}

// 编码器中断
void doA_L() { encL.handleA(); }  void doB_L() { encL.handleB(); }
void doA_R() { encR.handleA(); }  void doB_R() { encR.handleB(); }

void setup() {
    Serial.begin(115200);
    encL.init(); encL.enableInterrupts(doA_L, doB_L);
    encR.init(); encR.enableInterrupts(doA_R, doB_R);
    
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.linkSensor(&encL); motorR.linkSensor(&encR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

void loop() {
    // 1. DWA 速度搜索
    float bestV, bestW;
    dwaSearch(bestV, bestW);

    // 2. 差速解算
    float wheelBase = 0.25;
    motorL.target = bestV - bestW * wheelBase / 2.0;
    motorR.target = bestV + bestW * wheelBase / 2.0;

    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC(); motorR.loopFOC();

    // 3. 简化位姿更新（实际应使用编码器）
    robotYaw += bestW * DT;
    robotX += bestV * cos(robotYaw) * DT;
    robotY += bestV * sin(robotYaw) * DT;

    delay(DT * 1000);
}

核心逻辑：DWA在速度空间（线速度×角速度）中均匀采样，每组速度推演一条未来轨迹，选择距离目标最近的轨迹作为执行指令。这一基础框架的核心是“推演—评价—选择”三步循环。在实际工程中，采样步长和推演时间需要权衡——步长过粗会漏掉最优解，推演时间过长会增加计算量。

2、运动学约束下的动态窗口裁剪
此案例在基础DWA上引入运动学约束。速度采样不再从零开始，而是限制在当前速度的加速度可达范围内（动态窗口），同时纳入BLDC的实际最大速度限制。评价函数中加入避障项，超声波近距时主动降低速度。

// ===== 动态窗口计算 =====
const float ACC_V = 2.0;       // 最大线加速度(m/s²)
const float ACC_W = 3.0;       // 最大角加速度(rad/s²)

// 当前速度（由编码器反馈）
float currentV = 0, currentW = 0;

void computeDynamicWindow(float& vMin, float& vMax, float& wMin, float& wMax) {
    // 基于加速度限制的动态窗口
    vMin = max(0.0f, currentV - ACC_V * DT);
    vMax = min(MAX_V, currentV + ACC_V * DT);
    wMin = max(-MAX_W, currentW - ACC_W * DT);
    wMax = min(MAX_W, currentW + ACC_W * DT);
}

// ===== 带避障评价的DWA =====
void dwaSearchWithConstraints(float& bestV, float& bestW) {
    float vMin, vMax, wMin, wMax;
    computeDynamicWindow(vMin, vMax, wMin, wMax);
    
    bestV = 0; bestW = 0;
    float bestScore = -1e9;

    for (float v = vMin; v <= vMax; v += V_STEP) {
        for (float w = wMin; w <= wMax; w += W_STEP) {
            float simX = robotX, simY = robotY, simYaw = robotYaw;
            
            for (float t = 0; t < SIM_TIME; t += DT) {
                simX += v * cos(simYaw) * DT;
                simY += v * sin(simYaw) * DT;
                simYaw += w * DT;
            }

            // 评价项1：朝向目标
            float distToGoal = sqrt(pow(5.0 - simX, 2) + pow(3.0 - simY, 2));
            float headingScore = 1.0 / (distToGoal + 0.1);
            
            // 评价项2：避障（前方超声波距离）
            int obsDist = sonarF.ping_cm();
            float obsScore = (obsDist > 0 && obsDist < 50) ? 0.1 : 1.0;
            
            // 评价项3：速度偏好
            float velScore = v / MAX_V;
            
            // 加权评分（权重需调优）
            float score = 0.5 * headingScore + 0.3 * obsScore + 0.2 * velScore;
            
            if (score > bestScore) {
                bestScore = score;
                bestV = v; bestW = w;
            }
        }
    }
}

核心逻辑：动态窗口的引入使DWA从“全局搜索”变为“局部搜索”——机器人只在当前速度附近的可达范围内采样，确保速度变化不超过BLDC的物理加速度极限。评价函数的三项（朝向、避障、速度）通过加权组合，权重决定了机器人“更倾向于绕路还是更倾向于减速”。避障权重高则安全但效率低，速度权重高则高效但风险增加。

3、评价函数权重调优与轨迹平滑执行
此案例聚焦于评价函数权重的工程调优。通过滑动窗口记录历史速度指令，对输出速度做低通滤波，避免DWA在相邻周期输出的速度跳变导致BLDC频繁加减速。

// ===== 评价函数权重（可调参数）=====
float W_HEADING = 0.6;   // 朝向目标权重
float W_OBS = 0.3;       // 避障权重
float W_VEL = 0.1;       // 速度权重

// ===== 速度平滑滤波 =====
const float SMOOTH_ALPHA = 0.3;  // 平滑系数（0~1，越小越平滑）
float smoothV = 0, smoothW = 0;

void applyVelocitySmoothing(float& v, float& w) {
    // 一阶低通滤波：新指令 = α × 旧值 + (1-α) × 新值
    smoothV = SMOOTH_ALPHA * smoothV + (1 - SMOOTH_ALPHA) * v;
    smoothW = SMOOTH_ALPHA * smoothW + (1 - SMOOTH_ALPHA) * w;
    v = smoothV;
    w = smoothW;
}

// ===== 带平滑的完整DWA循环 =====
void loop() {
    float bestV, bestW;
    dwaSearchWithConstraints(bestV, bestW);
    
    // 速度平滑：避免BLDC频繁加减速
    applyVelocitySmoothing(bestV, bestW);
    
    // 差速解算与执行
    float wheelBase = 0.25;
    motorL.target = bestV - bestW * wheelBase / 2.0;
    motorR.target = bestV + bestW * wheelBase / 2.0;
    
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC(); motorR.loopFOC();
    
    // 更新当前速度（用于下一周期动态窗口计算）
    currentV = bestV;
    currentW = bestW;
    
    delay(DT * 1000);
}

核心逻辑：DWA输出的是每周期最优速度，但相邻周期的“最优”可能跳变。BLDC虽然响应快，频繁的加减速指令仍会导致机械冲击和能耗增加。速度平滑滤波通过一阶低通滤波将跳变的速度指令“磨平”，让BLDC在平滑的速度曲线上运行。权重调优的原则是：狭窄通道提高避障权重（W_OBS），长直通道提高速度权重（W_VEL）。

要点解读

1. DWA的核心是“速度采样 + 轨迹推演 + 评价选择”三步循环

DWA在速度空间（线速度v×角速度w）中采样多组速度对，对每组速度推演未来一段时间内的运动轨迹，通过评价函数给每条轨迹打分，选择得分最高的速度对作为本周期执行指令。评价函数通常包含三个维度：朝向目标的程度、与障碍物的安全距离、速度大小。这种“推演未来”的机制使DWA具备对动态障碍物的预判能力，而非仅对当前状态做出反应。

2. 动态窗口将采样限制在BLDC物理可达范围内

直接从全速度空间采样是低效且危险的——可能采样到BLDC无法瞬间达到的速度。动态窗口根据当前速度和最大加速度，计算出本周期“物理上可达”的速度范围，只在这个范围内采样。这确保输出速度指令不会超出电机响应极限，避免“规划出来但执行不了”的问题。对于BLDC差速底盘，动态窗口的边界由最大线速度、最大角速度和对应的加速度共同决定。

3. 评价函数权重是DWA性能的“调优旋钮”

朝向、避障、速度三项的权重组合决定了DWA的“性格”。避障权重过高，机器人在开阔区域也会过度谨慎，效率低下；速度权重过高，机器人可能在障碍物前反应不及。工程调优的经验是：在狭窄通道和密集区域提高避障权重，在长直通道降低避障权重、提高速度权重。权重应作为可动态调整的参数，而非固定值。

4. BLDC的加速度约束是动态窗口的物理基础

动态窗口的边界由“当前速度 ± 加速度 × 控制周期”决定。如果BLDC的最大加速度设置过小，机器人响应迟钝；设置过大，DWA可能输出超出实际能力的速度指令，导致电机堵转或失步。加速度参数必须通过实验标定，与BLDC的实际扭矩-转速特性匹配。SimpleFOC的电流环带宽决定了BLDC的实际加速度响应能力。

5. 速度平滑是DWA与BLDC协同的“润滑剂”

DWA每周期独立搜索最优速度，相邻周期的结果可能跳变。直接将跳变的速度指令送给BLDC，会导致电机频繁加减速，产生机械冲击和噪音。对DWA输出做低通滤波，用平滑后的速度驱动BLDC，可以让机器人运动更平顺。平滑系数需要权衡——系数过小则响应迟钝，过大则滤波效果不足。BLDC配合FOC的平滑转矩输出，与速度平滑滤波结合，是实现“丝滑”运动的关键。

4、避障场景DWA速度控制程序（核心：速度边界与障碍约束）
该案例聚焦DWA最基础的速度控制场景，实现机器人在复杂环境中的避障移动，通过对速度可行窗口的动态限制与障碍约束，输出最优速度，是DWA速度控制的基础实现，适用于仓储、展厅等有不规则障碍物的场景。

核心逻辑
约束生成：基于机器人动力学与障碍物位置，动态生成速度与角速度的可行窗口；
速度采样：在可行窗口内随机采样多组速度-角速度组合；
评价最优：通过避障距离、方向偏差等指标筛选最优速度；
执行输出：将最优速度转化为BLDC电机的PWM控制信号，实现运动执行。

// DWA避障场景速度控制程序（基础版）
#include <Adafruit_NeoPixel.h> // 可选：状态指示

// ===== 硬件引脚定义 =====
// 电机控制引脚
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
// 感知引脚
#define LEFT_SONAR_TRIG 18
#define LEFT_SONAR_ECHO 19
#define RIGHT_SONAR_TRIG 20
#define RIGHT_SONAR_ECHO 21
#define FRONT_SONAR_TRIG 22
#define FRONT_SONAR_ECHO 23
// 调试外设
#define LED_PIN 8
Adafruit_NeoPixel statusLed(1, LED_PIN, NEO_GRB + NEO_KHZ800);

// ===== DWA核心参数 =====
// 速度边界约束（cm/s、rad/s）
const float V_MAX = 30.0f;      // 最大线速度
const float V_MIN = 0.0f;       // 最小线速度（静止）
const float OMEGA_MAX = 1.0f;   // 最大角速度
const float OMEGA_MIN = -1.0f;  // 最小角速度
// 安全约束
const float SAFE_DISTANCE = 15.0f; // 安全距离阈值（cm）
const int SAMPLE_COUNT = 100; // 速度采样次数
// 评价函数权重
const float W_AVOID = 2.0f;   // 避障权重
const float W_GOAL = 1.0f;    // 目标方向权重

// ===== 状态变量 =====
float front_distance = 0;
float left_distance = 0;
float right_distance = 0;
float best_v = 0;
float best_w = 0;
float target_angle = 0; // 可选：目标方向（简化为0，即向前）

// ===== 函数声明 =====
float measureDistance(int trig, int echo);
void generateVOperatingSpace();
float evaluateScore(float v, float w);
void controlMotor(float v, float w);
void updateStatusLed(bool safe);

void setup() {
  Serial.begin(115200);
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  // 超声波引脚初始化
  pinMode(LEFT_SONAR_TRIG, OUTPUT);
  pinMode(LEFT_SONAR_ECHO, INPUT);
  pinMode(RIGHT_SONAR_TRIG, OUTPUT);
  pinMode(RIGHT_SONAR_ECHO, INPUT);
  pinMode(FRONT_SONAR_TRIG, OUTPUT);
  pinMode(FRONT_SONAR_ECHO, INPUT);
  // 状态灯初始化
  statusLed.begin();
  statusLed.show();
  Serial.println("DWA避障速度控制初始化完成");
}

void loop() {
  // 1. 感知：读取周围障碍物距离
  front_distance = measureDistance(FRONT_SONAR_TRIG, FRONT_SONAR_ECHO);
  left_distance = measureDistance(LEFT_SONAR_TRIG, LEFT_SONAR_ECHO);
  right_distance = measureDistance(RIGHT_SONAR_TRIG, RIGHT_SONAR_ECHO);
  
  // 2. 生成速度可行窗口（动态约束）
  generateVOperatingSpace();
  
  //3. 速度采样与评价，优化最优速度
  best_v = V_MIN;
  best_w = OMEGA_MIN;
  float max_score = -1000.0f;
  for (int i = 0; i < SAMPLE_COUNT; i++) {
    // 在可行范围内随机采样速度与角速度
    float v = random(V_MIN * 100, V_MAX * 100) / 100.0f;
    float w = random(OMEGA_MIN * 100, OMEGA_MAX * 100) / 100.0f;
    // 障碍约束：过滤掉会碰撞的速度
    if (v > 0 && front_distance < SAFE_DISTANCE && w == 0) {
      continue; // 前方有障碍且直行时，过滤该速度
    }
    float score = evaluateScore(v, w);
    if (score > max_score) {
      max_score = score;
      best_v = v;
      best_w = w;
    }
  }
  
  // 4. 执行：将最优速度转化为电机控制
  controlMotor(best_v, best_w);
  
  // 5. 调试与状态指示
  Serial.print("前方:"); Serial.print(front_distance);
  Serial.print("cm 左侧:"); Serial.print(left_distance);
  Serial.print("cm 右侧:"); Serial.print(right_distance);
  Serial.print("cm 速度:"); Serial.print(best_v);
  Serial.print("cm/s 角速度:"); Serial.println(best_w);
  
  updateStatusLed(front_distance > SAFE_DISTANCE);
  delay(50);
}

// 测量距离
float measureDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH);
  return (duration * 0.0343f) / 2.0f;
}

// 生成速度可行窗口（基于障碍动态调整）
void generateVOperatingSpace() {
  // 若前方障碍过近，缩小线速度上限
  if (front_distance < SAFE_DISTANCE * 2.0f) {
    V_MAX = front_distance / 2.0f;
  } else {
    V_MAX = 30.0f;
  }
  // 左侧障碍近，限制右转角速度
  if (left_distance < SAFE_DISTANCE) {
    OMEGA_MAX = -0.5f; // 限制右转（负角速度对应右转）
  }
  // 右侧障碍近，限制左转角速度
  if (right_distance < SAFE_DISTANCE) {
    OMEGA_MIN = 0.5f; // 限制左转（正角速度对应左转）
  }
}

// 评价函数：计算速度组合的得分，得分越高越优
float evaluateScore(float v, float w) {
  // 避障得分：距离障碍物越近，得分越低
  float avoid_score = 0;
  if (v > 0) { // 向前运动时，主要看前方距离
    avoid_score = (v > 0) ? (front_distance - SAFE_DISTANCE) / SAFE_DISTANCE : 0;
  }
  if (w < 0) { // 右转时，看右侧距离
    avoid_score = (right_distance - SAFE_DISTANCE) / SAFE_DISTANCE;
  }
  if (w > 0) { // 左转时，看左侧距离
    avoid_score = (left_distance - SAFE_DISTANCE) / SAFE_DISTANCE;
  }
  
  // 目标方向得分：越接近目标方向，得分越高
  float goal_score = map(abs(target_angle - atan2(w, v)), 0, PI, 1.0f, 0.0f);
  avoid_score = max(avoid_score, -1.0f); // 避免得分过低
  
  return W_AVOID * avoid_score + W_GOAL * goal_score;
}

// 将速度与角速度转化为BLDC电机PWM控制
void controlMotor(float v, float w) {
  // 差速计算：v = (左轮速度 + 右轮速度)/2，w = (右轮速度 - 左轮速度)/轮距
  const float WHEEL_BASE = 20.0f; // 轮距（cm，根据机器人底盘调整）
  float left_v = v - w * WHEEL_BASE / 2.0f;
  float right_v = v + w * WHEEL_BASE / 2.0f;
  
  // 速度上限限制（避免超速）
  left_v = constrain(left_v, V_MIN, V_MAX);
  right_v = constrain(right_v, V_MIN, V_MAX);
  
  // 速度转PWM（PWM范围0~255，需根据电机特性校准）
  const float FORWARD_RATIO = 8.5; // PWM与速度的转换系数（1cm/s对应8.5PWM，需校准）
  int left_pwm = left_v * FORWARD_RATIO;
  int right_pwm = right_v * FORWARD_RATIO;
  
  // 方向控制
  if (left_pwm > 0) {
    digitalWrite(LEFT_MOTOR_DIR, HIGH); // 正转
  } else {
    digitalWrite(LEFT_MOTOR_DIR, LOW); // 反转
    left_pwm = abs(left_pwm);
  }
  if (right_pwm > 0) {
    digitalWrite(RIGHT_MOTOR_DIR, HIGH); // 正转
  } else {
    digitalWrite(RIGHT_MOTOR_DIR, LOW); // 反转
    right_pwm = abs(right_pwm);
  }
  
  analogWrite(LEFT_MOTOR_PWM, left_pwm);
  analogWrite(RIGHT_MOTOR_PWM, right_pwm);
}

// 状态灯指示：安全绿色，危险红色
void updateStatusLed(bool safe) {
  if (safe) {
    statusLed.setPixelColor(0, 0, 255, 0); // 绿色
  } else {
    statusLed.setPixelColor(0, 255, 0, 0); // 红色
  }
  statusLed.show();
}

程序优化方向
引入编码器实现速度闭环，通过PID调节电机实际速度，消除速度误差；
加入加速度约束，对速度变化进行平滑处理，避免速度突变导致的冲击；
优化评价函数权重，可支持目标位置导航，适配有明确目标的场景。

5、动态跟随场景DWA速度控制程序（核心：速度矢量耦合约束）
该案例聚焦人机互动或动态跟随场景，机器人需跟随移动目标（如行人、移动小车），核心是实现线速度与角速度的耦合约束，避免速度矢量与目标运动不匹配，适用于导览跟随、物流跟随等场景。

核心逻辑
目标动态感知：通过超声波或红外模块获取目标的位置与运动方向；
速度矢量约束：基于目标的运动状态，约束机器人的速度矢量，使其与目标运动方向一致；
距离约束：根据机器人与目标的距离，动态调整速度边界，保持合理跟随距离；
最优速度输出：通过评价函数筛选出兼顾跟随精度与运动平滑的速度。

// DWA动态跟随场景速度控制程序
#include <Adafruit_NeoPixel.h>

// ===== 硬件引脚定义 =====
// 电机控制
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
// 目标感知（简化：前方单点超声波，可扩展为多点）
#define TARGET_SONAR_TRIG 22
#define TARGET_SONAR_ECHO 23
#define TARGET_ANGLE_PIN A0hiddenContent// 目标角度检测（模拟量，可选）
// 调试外设
#define LED_PIN 8
Adafruit_NeoPixel statusLed(1, LED_PIN, NEO_GRB + NEO_KHZ800);

// ===== DWA跟随核心参数 =====
const float FOLLOW_DISTANCE = 50.0f;  // 目标跟随距离（cm）
const float V_MAX = 35.0f;            // 最大跟随速度
const float V_MIN = 5.0f;             // 最小跟随速度（避免完全静止）
const float OMEGA_MAX = 1.2f;         // 最大角速度
const float OMEGA_MIN = -1.2f;        // 最小角速度
const int SAMPLE_COUNT = 80;          // 速度采样次数
// 评价函数权重
const float W_DISTANCE = 2.5f;        // 距离权重（保持跟随距离）
const float W_DIRECTION = 2.0f;       // 方向权重（匹配目标方向）
const float W_SMOOTH = 0.5f;          // 平滑权重（速度变化平滑）

// ===== 状态变量 =====
float target_distance = 0;
float target_angle = 0;  // 目标相对于机器人的角度（rad）
float last_v = 0;       // 上一周期速度，用于平滑
float best_v = 0;
float best_w = 0;

// ===== 函数声明 =====
float getTargetInfo();
float evaluateScore(float v, float w);
void controlMotor(float v, float w);

void setup() {
  Serial.begin(115200);
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  // 目标检测引脚初始化
  pinMode(TARGET_SONAR_TRIG, OUTPUT);
  pinMode(TARGET_SONAR_ECHO, INPUT);
  pinMode(TARGET_ANGLE_PIN, INPUT);
  // 状态灯初始化
  statusLed.begin();
  statusLed.show();
  Serial.println("DWA动态跟随速度控制初始化完成");
}

void loop() {
  // 1. 感知：获取目标距离与角度
  float targetInfo = getTargetInfo();
  if (targetInfo > 0) { // 检测到目标
    // 2. 速度可行窗口约束（基于跟随距离）
    float v_max = (target_distance > FOLLOW_DISTANCE) ? V_MAX : (V_MAX * 0.6f);
    float v_min = (target_distance < FOLLOW_DISTANCE * 0.8f) ? V_MAX : V_MIN;
    
    //3. 速度采样与评价
    best_v = v_min;
    best_w = OMEGA_MIN;
    float max_score = -1000.0f;
    for (int i = 0; i < SAMPLE_COUNT; i++) {
      float v = random(v_min * 100, v_max * 100) / 100.0f;
      float w = random(OMEGA_MIN * 100, OMEGA_MAX * 100) / 100.0f;
      
      // 耦合约束：角速度方向需指向目标，避免偏离
      if (target_angle > 0.2f && w < 0.1f) continue; // 目标在左侧，需左转
      if (target_angle < -0.2f && w > -0.1f) continue; // 目标在右侧，需右转
      
      float score = evaluateScore(v, w);
      if (score > max_score) {
        max_score = score;
        best_v = v;
        best_w = w;
      }
    }
  } else {
    // 无目标：原地待命
    best_v = 0;
    best_w = 0;
  }
  
  // 4. 执行：电机控制
  controlMotor(best_v, best_w);
  
  // 5. 调试与状态指示
  Serial.print("目标距离:"); Serial.print(target_distance);
  Serial.print("cm 目标角度:"); Serial.print(target_angle);
  Serial.print("rad 速度:"); Serial.print(best_v);
  Serial.print("cm/s 角速度:"); Serial.println(best_w);
  
  if (target_distance > 0 && target_distance < FOLLOW_DISTANCE * 1.5) {
    statusLed.setPixelColor(0, 0, 255, 0); // 绿色：跟随中
  } else if (target_distance > 0) {
    statusLed.setPixelColor(0, 255, 255, 0); // 黄色：距离过远
  } else {
    statusLed.setPixelColor(0, 255, 0, 0); // 红色：无目标
  }
  statusLed.show();
  
  last_v = best_v;
  delay(50);
}

// 获取目标距离与角度（简化版：角度通过模拟量映射，实际可搭配视觉/多超声波）
float getTargetInfo() {
  // 测量目标距离
  digitalWrite(TARGET_SONAR_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TARGET_SONAR_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TARGET_SONAR_TRIG, LOW);
  long duration = pulseIn(TARGET_SONAR_ECHO, HIGH);
  float distance = (duration * 0.0343f) / 2.0f;
  
  if (distance > 100.0f || distance < 10.0f) {
    return -1; // 无有效目标
  }
  
  // 目标角度检测（模拟量映射到-1.0~1.0 rad，范围较粗略）
  int angle_val = analogRead(TARGET_ANGLE_PIN);
  target_angle = map(angle_val, 0, 1023, -1.0f, 1.0f);
  target_distance = distance;
  
  return distance;
}

// 评价函数：跟随场景的打分逻辑
float evaluateScore(float v, float w) {
  // 距离得分：距离越接近期望距离，得分越高
  float distance_diff = abs(target_distance - FOLLOW_DISTANCE);
  float distance_score = 1.0f / (1.0f + distance_diff);
  
  // 方向得分：速度方向越匹配目标角度，得分越高
  float current_angle = atan2(w, v);
  float direction_diff = abs(target_angle - current_angle);
  // 角度差超过pi时，取最小差值
  if (direction_diff > PI) direction_diff = 2.0f * PI - direction_diff;
  float direction_score = 1.0f - direction_diff / PI;
  
  // 平滑得分：速度变化越平滑，得分越高
  float smooth_score = 1.0f - abs(v - last_v) / V_MAX;
  
  return W_DISTANCE * distance_score + W_DIRECTION * direction_score + W_SMOOTH * smooth_score;
}

// 电机控制（差速转换、速度上限与平滑处理）
void controlMotor(float v, float w) {
  const float WHEEL_BASE = 20.0f; // 轮距
  float left_v = v - w * WHEEL_BASE / 2.0f;
  float right_v = v + w * WHEEL_BASE / 2.0f;
  
  // 速度平滑：限制速度变化率，避免突变
  const float MAX_V_CHANGE = 5.0f; // 最大速度变化率（cm/s/周期）
  left_v = constrain(v + (left_v - v) * 0.5f, V_MIN, V_MAX);
  right_v = constrain(v + (right_v - v) * 0.5f, V_MIN, V_MAX);
  
  // 速度转PWM
  const float FORWARD_RATIO = 8.5;
  int left_pwm = constrain(left_v * FORWARD_RATIO, 0, 255);
  int right_pwm = constrain(right_v * FORWARD_RATIO, 0, 255);
  
  // 方向控制（跟随场景默认正转）
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  
  analogWrite(LEFT_MOTOR_PWM, left_pwm);
  analogWrite(RIGHT_MOTOR_PWM, right_pwm);
}

程序优化方向
引入多传感器融合（如多超声波、红外阵列）实现目标的精准定位，解决单点检测的目标丢失问题；
加入目标运动趋势预测，通过目标距离的连续变化预判目标运动方向，提前调整速度矢量；
引入PID控制优化跟随距离的稳定性，避免出现距离忽远忽近的情况。

6、窄空间通行场景DWA速度控制程序（核心：角速度与线速度耦合约束）
该案例聚焦窄通道、狭窄空间的通行场景，机器人需在狭窄环境中安全通行，核心是对角速度与线速度的耦合约束，避免因角速度过大导致碰撞，同时保证通行效率，适用于仓储货架间、狭窄楼道、设备间等场景。

核心逻辑
空间感知：通过多超声波检测通道宽度、两侧距离，判断空间是否允许通行；
速度耦合约束：根据通道宽度动态约束线速度与角速度的关系，窄空间降低线速度、限制角速度；
居中约束：优先保证机器人居中通行，减少左右偏移导致的碰撞风险；
最优速度输出：筛选出兼顾通行安全与效率的速度，实现窄空间的平稳通行。

// DWA窄空间通行速度控制程序
// ===== 硬件引脚定义 =====
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
// 多超声波引脚（左、中、右三路）
#define LEFT_SENSOR_TRIG 18
#define LEFT_SENSOR_ECHO 19
#define CENTER_SENSOR_TRIG 22
#define CENTER_SENSOR_ECHO 23
#define RIGHT_SENSOR_TRIG 20
#define RIGHT_SENSOR_ECHO 21

// ===== DWA窄空间核心参数 =====
const float MAX_WIDE_SPEED = 30.0f;      // 宽空间最大线速度
const float MIN_NARROW_SPEED = 10.0f;     // 窄空间最小线速度
const float MAX_OMEGA_WIDE = 1.0f;        // 宽空间最大角速度
const float MAX_OMEGA_NARROW = 0.3f;      // 窄空间最大角速度（限制转向）
const float CENTER_DISTANCE = 8.0f;       // 期望居中距离（cm，小于该值视为偏右）
const float CHANNEL_MIN = 25.0f;          // 通道最小允许宽度（cm）
const int SAMPLE_COUNT = 60;              // 采样次数（窄空间减少采样，提高实时性）
// 评价函数权重
const float W_SAFE = 3.0f;   // 安全权重
const float W_CENTER = 2.0f; // 居中权重
const float W_EFFICIENCY = 1.0f; // 效率权重

// ===== 状态变量 =====
float left_distance = 0;
float center_distance = 0;
float right_distance = 0;
float channel_width = 0;
bool passable = true; // 通道是否可通行
float best_v = 0;
float best_w = 0;

// ===== 函数声明 =====
float measureDistance(int trig, int echo);
void analyzeSpace();
float evaluateScore(float v, float w);
void controlMotor(float v, float w);

void setup() {
  Serial.begin(115200);
  // 电机初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  // 传感器初始化
  pinMode(LEFT_SENSOR_TRIG, OUTPUT);
  pinMode(LEFT_SENSOR_ECHO, INPUT);
  pinMode(CENTER_SENSOR_TRIG, OUTPUT);
  pinMode(CENTER_SENSOR_ECHO, INPUT);
  pinMode(RIGHT_SENSOR_TRIG, OUTPUT);
  pinMode(RIGHT_SENSOR_ECHO, INPUT);
  Serial.println("DWA窄空间通行速度控制初始化完成");
}

void loop() {
  // 1. 感知：读取多超声波数据
  left_distance = measureDistance(LEFT_SENSOR_TRIG, LEFT_SENSOR_ECHO);
  center_distance = measureDistance(CENTER_SENSOR_TRIG, CENTER_SENSOR_ECHO);
  right_distance = measureDistance(RIGHT_SENSOR_TRIG, RIGHT_SENSOR_ECHO);
  
  // 2. 空间分析：判断通道宽度与可通行性
  analyzeSpace();
  
  // 3. 速度约束与采样
  float v_max;
  float omega_max;
  if (!passable) {
    best_v = 0;
    best_w = Omega_MIN;
  } else if (channel_width < 40.0f) { // 窄空间
    v_max = MIN_NARROW_SPEED + (channel_width - CHANNEL_MIN) / (40.0f - CHANNEL_MIN) * (MAX_WIDE_SPEED - MIN_NARROW_SPEED);
    omega_max = MAX_OMEGA_NARROW;
  } else { // 宽空间
    v_max = MAX_WIDE_SPEED;
    omega_max = MAX_OMEGA_WIDE;
  }
  
  best_v = 0;
  best_w = 0;
  float max_score = -1000.0f;
  if (passable) {
    for (int i = 0; i < SAMPLE_COUNT; i++) {
      float v = random(0, v_max * 100) / 100.0f;
      float w = random(-omega_max * 100, omega_max * 100) / 100.0f;
      
      // 耦合约束：窄空间禁止原地转向，必须直行或小角度转向
      if (channel_width < CHANNEL_MIN + 5.0f && abs(w) > 0.1f) continue;
      
      float score = evaluateScore(v, w);
      if (score > max_score) {
        max_score = score;
        best_v = v;
        best_w = w;
      }
    }
  }
  
  // 4. 执行电机控制
  controlMotor(best_v, best_w);
  
  // 5. 调试输出
  Serial.print("左:"); Serial.print(left_distance);
  Serial.print("cm 中:"); Serial.print(center_distance);
  Serial.print("cm 右:"); Serial.print(right_distance);
  Serial.print("cm 通道宽:"); Serial.print(channel_width);
  Serial.print("cm 可通行:"); Serial.print(passable?"是":"否");
  Serial.print(" 速度:"); Serial.print(best_v);
  Serial.print("cm/s 角速度:"); Serial.println(best_w);
  
  delay(50);
}

// 距离测量
float measureDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH);
  return (duration * 0.0343f) / 2.0f;
}

// 空间分析：计算通道宽度、居中情况、可通行性
void analyzeSpace() {
  // 通道宽度：取左右传感器的最小值（实际为左右传感器距离之和，需校准）
  channel_width = left_distance + right_distance;
  // 特殊场景：若中心传感器检测到前方障碍，视为窄通道变窄
  if (center_distance < 30.0f) {
    channel_width = min(channel_width, center_distance * 2.0f);
  }
  // 可通行性判断：通道宽度需大于最小阈值，且左右均有足够余量
  passable = (channel_width > CHANNEL_MIN) &&
             (left_distance > 5.0f) && (right_distance >5.0f);
}

// 评价函数：窄空间优先安全与居中，兼顾效率
float evaluateScore(float v, float w) {
  // 安全得分：距离两侧障碍物越远，得分越高，取左右距离的最小值
  float min_side_distance = min(left_distance, right_distance);
  float safe_score = min_side_distance / 15.0f; // 障碍物距离阈值15cm
  safe_score = constrain(safe_score, 0.0f, 1.0f);
  
  // 居中得分：机器人越接近通道中心，得分越高
  float center_diff = abs(center_distance - CENTER_DISTANCE);
  float center_score = 1.0f / (1.0f + center_diff);
  
  // 效率得分：速度越快，得分越高（窄空间效率权重较低）
  float efficiency_score = v / v_max;
  
  return w_SAFE * safe_score + W_CENTER * center_score + W_EFFICIENCY * efficiency_score;
}

// 电机控制：窄空间采用低速度、限制转向
void controlMotor(float v, float w) {
  const float WHEEL_BASE = 20.0f;
  float left_v = v - w * WHEEL_BASE / 2.0f;
  float right_v = v + w * WHEEL_BASE / 2.0f;
  
  // 速度与角速度上限（窄空间约束）
  float v_limit = (channel_width < 40.0f) ? 15.0f : MAX_WIDE_SPEED;
  left_v = constrain(left_v, 0, v_limit);
  right_v = constrain(right_v, 0, v_limit);
  
  // 速度转PWM
  const float FORWARD_RATIO = 8.5;
  int left_pwm = left_v * FORWARD_RATIO;
  int right_pwm = right_v * FORWARD_RATIO;
  
  // 方向控制
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  
  analogWrite(LEFT_MOTOR_PWM, constrain(left_pwm, 0, 255));
  analogWrite(RIGHT_MOTOR_PWM, constrain(right_pwm, 0, 255));
}

程序优化方向
增加多组超声波（如左前、左后、右前、右后），更精准地检测通道形态与弯道；
引入航位推算与地图匹配，提升窄空间通行的方向精度，避免偏离通道；
加入通道类型自适应，区分直道、弯道、交叉口等不同窄空间形态，调整约束规则与评价权重。

要点解读
要点1：DWA速度约束需紧扣动力学边界，是速度控制的基础前提
DWA的速度可行窗口并非固定值，必须基于机器人的BLDC电机性能、底盘结构等动力学特性设定，核心包括三个层级：
绝对边界：线速度v与角速度ω的最大/最小阈值，由电机的最高转速、减速比、机器人机械结构决定，不可盲目设定过高值，避免超出电机负载能力导致损坏；
动态边界：根据距离障碍物、通道宽度等环境数据，实时调整速度边界（如窄空间降低速度上限、前方有障碍时缩小速度范围），保证运动安全；
加速度边界：速度变化需受电机加速/减速性能限制，通过速度平滑处理避免速度突变，减少对BLDC电机的冲击，延长设备寿命。
要点2：速度与角速度的耦合约束是DWA速度控制的核心逻辑
DWA的速度控制本质是对“线速度+角速度”矢量的联合控制，二者并非独立变量，而是通过机器人运动模型耦合，要点包括：
运动学关联：基于差速机器人运动模型，将速度矢量(v,ω)转化为左右轮的分速度，需严格遵循差速公式，保证速度转换的准确性；
场景耦合：不同场景下二者的约束关系不同，如窄空间场景需大幅限制角速度（避免转向碰撞），动态跟随场景则需角速度与目标位置匹配（保证跟随精度）；
耦合优化：评价函数需同时考虑线速度与角速度的协同性，避免出现“线速度大但角速度偏离目标”“角速度大但线速度为零”等无效速度组合，提升速度利用效率。
要点3：评价函数的权重需适配场景需求，决定速度控制的优先级
DWA的核心是通过评价函数筛选最优速度，权重设定直接决定机器人的运动倾向，需遵循“安全优先、场景适配、动态可调”原则：
安全权重优先：所有场景下，避障/安全相关权重需最高，确保机器人优先规避碰撞，避免因追求效率或跟随而引发危险；
场景权重匹配：根据场景核心需求调整权重，如避障场景提高避障权重，跟随场景提高距离与方向权重，窄空间场景提高居中与安全权重；
权重动态调整：支持根据环境状态动态调节，如当前方障碍物距离过近时，临时调高避障权重、调低效率权重，实现灵活的优先级切换。
要点4：速度控制需实现闭环反馈，保障速度精度与稳定性
开环速度控制易受电机特性、负载变化、地面摩擦力等因素影响，速度精度低、稳定性差，需搭配闭环逻辑：
速度反馈：通过BLDC电机的霍尔编码器测量实际轮速，转化为机器人的实际线速度与角速度，与DWA输出的目标速度形成闭环对比；
误差修正：引入PID调节，对目标速度与实际速度的误差进行实时修正，逐步缩小偏差，保证实际速度与最优速度一致；
限速与平滑：在闭环基础上加入速度限制与平滑处理，避免修正过度导致的速度震荡，实现平稳的速度控制。
要点5：DWA速度控制的模块化设计支撑场景扩展与参数校准
DWA程序需采用模块化结构，便于后续优化、移植与场景扩展，核心模块包括：
感知模块：独立封装传感器数据读取函数，后续新增传感器（如激光雷达、IMU）时，只需修改该模块即可，无需重构整体逻辑；
约束模块：单独封装速度边界与加速度约束的生成逻辑，可快速适配不同动力学特性的底盘，调整参数即可适配不同电机；
评价模块：独立实现评价函数，后续新增评价指标（如能耗、舒适度）时，只需在评价函数中添加对应项与权重，不改变核心架构，降低维护成本。

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