“Arduino BLDC之机器人改进DWA + 滚动窗口动态重规划”代表了移动机器人局部路径规划与底层高性能驱动深度融合的前沿工程实践。该系统旨在解决传统动态窗口法（DWA）在复杂动态环境中容易陷入局部极小值、全局跟踪精度低，以及全局地图在动态变化中迅速过时等痛点。通过融合滚动窗口机制、多目标评价函数优化以及BLDC电机的精准执行，系统能够实现极其平滑、安全且高效的自主导航。

一、 主要特点

1. 滚动窗口（Rolling Window）局部感知与高频重规划
   全局地图在动态环境中会迅速过时，且全图规划对算力要求极高。系统采用滚动窗口机制，仅以机器人当前位姿为中心，实时更新并维护一个局部范围内的代价地图（Costmap）。这大幅降低了主控的内存占用与计算负荷，使Arduino（或ESP32等嵌入式平台）能够以高频（如10Hz以上）处理局部环境变化，确保对突发障碍物的快速响应。
2. 融合多维约束的改进型DWA算法
   传统的DWA在接近目标或绕过障碍时易出现轨迹大幅偏折甚至原地打转（Twirling）。改进型DWA引入了更丰富的约束与评价机制：
   运动学与动力学约束： 严格纳入BLDC底盘的最大线速度、角速度及加速度限制，确保生成的轨迹在物理上绝对可行。
   时空轨迹预测： 引入模型预测动态障碍物的未来轨迹并计算碰撞风险，增强“前瞻性”避障能力。
   分层动态权重调整： 根据目标距离与障碍物邻近度，实时优化评价函数（如安全权重、速度权重）的分配，在安全与效率之间取得最佳平衡。
3. 严密的“全局-局部”分层闭环架构
   系统形成严密的分层控制：全局规划器（如A*）提供一条通往目标的宏观参考路径；局部规划器（改进DWA）在滚动窗口内根据实时传感器数据和全局路径的引导，计算出最优的局部避障速度；BLDC底层控制器精准执行该速度。当机器人绕过动态障碍后，又能平滑地回归全局路径。
4. BLDC的高动态响应与平滑执行
   动态避障算法输出的速度和角速度指令往往是连续且高频变化的。BLDC电机配合FOC（磁场定向控制）或高性能闭环驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪DWA输出的平滑轨迹，避免了传统步进电机或直流有刷电机在频繁启停和转向时的机械冲击与轨迹偏差。
   二、 典型应用场景
5. 人机协作仓储与物流AMR
   在电商仓库或柔性制造车间，人员和叉车会随时横穿机器人通道。滚动窗口重规划能确保AMR在高速运行中，对突然出现的动态障碍物进行平滑减速绕行，并在安全后迅速恢复原速，保障物流效率与人员安全。
6. 服务与导览机器人
   在商场、医院、酒店等复杂且人流密集的室内环境中，机器人需要频繁应对突然停步的顾客或横穿的宠物。改进DWA能提供极其平滑的避让体验，避免急刹或剧烈转向带来的乘客不适感或物品倾覆风险。
7. 室外非结构化环境巡检
   在园区安防或农业巡检中，机器人会遇到落叶、小动物或临时施工路障。滚动窗口机制允许机器人根据局部地形实时调整轨迹，而无需每次都触发庞大的全局重算，提升了户外复杂环境下的通行效率。
8. ROS导航算法验证与科研
   作为高校与科研机构验证局部规划算法与底层电机控制耦合特性的理想平台。通过调整评价函数的权重，可以直观研究机器人在不同场景下的避障策略与运动平滑度。
   三、 需要注意的关键事项
9. 主控算力瓶颈与实时性保障
   在滚动窗口内进行DWA的多轨迹推演和评价计算，对浮点运算能力要求较高。标准的Arduino Uno难以胜任高频的DWA解算。强烈建议采用ESP32、STM32等高性能MCU，或使用ROS系统配合上位机（如树莓派/Jetson）运行局部规划器，Arduino仅作为底层BLDC速度执行器。同时，严禁在主循环中使用delay()等阻塞函数，必须保证控制周期的绝对稳定。
10. DWA参数调优与轨迹平滑性
    改进DWA算法的性能高度依赖参数配置。模拟时间（Sim Time）设置过短会导致机器人“目光短浅”错过远处障碍，过长则会增加计算延迟；评价函数的权重若设置不当，极易导致机器人在接近目标或绕过障碍时出现轨迹大幅偏折、减速停顿甚至原地打转。必须结合实际BLDC底盘的运动学参数进行精细调优。
11. 传感器数据的噪声处理与延迟
    动态避障对传感器数据的实时性和准确性极其敏感。超声波或激光雷达的噪声可能导致代价地图中出现“伪障碍”，引发机器人不必要的急刹。需在软件层加入滑动平均或卡尔曼滤波；同时，必须严格标定传感器安装位置与数据发布延迟，确保滚动窗口内的障碍物坐标与机器人当前位姿在时间戳上严格对齐。
12. BLDC底层的安全与运动学限制
    在将上层DWA的速度指令下发给BLDC驱动器时，必须加入严格的软件保护机制。包括：对线速度和角速度进行S曲线加减速平滑处理，防止阶跃指令导致电机过流或轮胎打滑；设置最大加速度限制，确保底盘的物理运动能力能够跟上规划器的期望；并保留硬件级急停与过流保护，防止因规划器死锁或传感器失效导致的碰撞事故。

1、基础DWA速度空间采样 + 固定目标点跟踪
适用场景：室内服务机器人在已知环境中沿预设目标点行驶，DWA每控制周期在动态窗口内采样最优速度。

/* ===== 基础DWA速度采样 + 固定目标跟踪 =====
 * 硬件：ESP32 + 2×BLDC差速电机 + 超声波/激光雷达传感器
 * 核心：每周期计算动态窗口、采样速度组合、评价选优
 * 参考：DWA速度空间搜索基础案例
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化...

// ==================== 超声波传感器 ====================
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 200);

// ==================== DWA参数 ====================
const float MAX_V = 0.5;           // 最大线速度(m/s)
const float MAX_W = 1.5;           // 最大角速度(rad/s)
const float ACC_V = 0.3;           // 线加速度(m/s²)
const float ACC_W = 1.0;           // 角加速度(rad/s²)
const float DT = 0.1;              // 模拟步长(s)
const float PREDICT_TIME = 1.0;    // 预测时间(s)
const int SAMPLES_V = 8;           // 线速度采样数
const int SAMPLES_W = 10;          // 角速度采样数

// ==================== 评价函数权重 ====================
const float ALPHA_HEADING = 0.5;   // 朝向目标权重
const float BETA_DIST = 0.3;       // 障碍物距离权重
const float GAMMA_VELOCITY = 0.2;  // 速度效率权重

// ==================== 状态变量 ====================
float vx = 0, vw = 0;
float robotX = 0, robotY = 0, robotYaw = 0;
float goalX = 2.0, goalY = 2.0;

void setup() {
    Serial.begin(115200);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

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

    // ==================== 【核心】DWA控制 ====================
    // 1. 计算动态窗口
    float minV = max(0.0, vx - ACC_V * DT);
    float maxV = min(MAX_V, vx + ACC_V * DT);
    float minW = max(-MAX_W, vw - ACC_W * DT);
    float maxW = min(MAX_W, vw + ACC_W * DT);

    // 2. 速度空间采样
    float bestScore = -999;
    float bestV = 0, bestW = 0;

    for (int i = 0; i < SAMPLES_V; i++) {
        float v = minV + i * (maxV - minV) / (SAMPLES_V - 1);
        for (int j = 0; j < SAMPLES_W; j++) {
            float w = minW + j * (maxW - minW) / (SAMPLES_W - 1);

            // 轨迹评价
            float score = evaluateTrajectory(v, w);
            if (score > bestScore) {
                bestScore = score;
                bestV = v;
                bestW = w;
            }
        }
    }

    // 3. 执行最优速度
    vx = bestV;
    vw = bestW;
    float wheelBase = 0.25;
    motorL.move(vx - vw * wheelBase / 2);
    motorR.move(vx + vw * wheelBase / 2);

    // 4. 更新位姿
    float vAct = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
    robotX += vAct * cos(robotYaw) * 0.02;
    robotY += vAct * sin(robotYaw) * 0.02;

    delay(20);
}

// ==================== 轨迹评价函数 ====================
float evaluateTrajectory(float v, float w) {
    float time = 0;
    float x = robotX, y = robotY, yaw = robotYaw;
    float minObsDist = 999;

    // 模拟轨迹
    while (time < PREDICT_TIME) {
        yaw += w * DT;
        x += v * cos(yaw) * DT;
        y += v * sin(yaw) * DT;
        time += DT;

        // 获取障碍物距离（简化）
        float obsDist = getObstacleDistance(x, y);
        if (obsDist < minObsDist) minObsDist = obsDist;
        if (obsDist < 0.2) return -1;  // 碰撞检测
    }

    // 三项评价指标
    float targetAngle = atan2(goalY - y, goalX - x);
    float headingScore = 1.0 - fabs(targetAngle - yaw) / PI;
    float distScore = minObsDist / 2.0;
    if (distScore > 1.0) distScore = 1.0;
    float velScore = v / MAX_V;

    return ALPHA_HEADING * headingScore 
         + BETA_DIST * distScore 
         + GAMMA_VELOCITY * velScore;
}

核心要点：
动态窗口：速度采样范围受当前速度和加速度限制，保证速度可达
三项评价函数：朝向目标（导航性）+ 安全距离（避障）+ 速度效率（流畅性）
滚动窗口：每控制周期重新规划，适应动态环境

2、改进DWA（自适应速度 + 路径偏差评估）+ A全局引导
适用场景：动态迷宫或仓库环境中，A提供全局路径节点作为DWA的引导目标，改进DWA增加自适应速度和路径偏差评估项。

/* ===== 改进DWA + A*全局路径引导 =====
 * 核心：A*生成全局路径节点，改进DWA增加自适应权重和偏差评估
 * 参考：改进DWA融合算法研究
 */
#include <SimpleFOC.h>
#include <vector>
#include <algorithm>

BLDCMotor motorL(7), motorR(7);

// ==================== A*路径节点 ====================
struct Point { float x, y; };
std::vector<Point> globalPath;
int pathIdx = 0;

// ==================== DWA改进参数 ====================
const float MAX_V = 0.5, MAX_W = 1.5;
const float ACC_V = 0.3, ACC_W = 1.0;
const float DT = 0.1, PREDICT_TIME = 1.0;
const int SAMPLES_V = 8, SAMPLES_W = 10;

// ==================== 改进评价函数权重 ====================
float w_heading = 0.35;
float w_dist = 0.25;
float w_velocity = 0.15;
float w_deviation = 0.25;  // 【新增】路径偏差项

// ==================== 状态变量 ====================
float vx = 0, vw = 0;
float robotX = 0, robotY = 0, robotYaw = 0;
float minObsDist = 999;

// ==================== 改进DWA评价函数 ====================
float evaluateWithDeviation(float v, float w, float tx, float ty) {
    float time = 0;
    float x = robotX, y = robotY, yaw = robotYaw;
    float minDist = 999;

    // 模拟轨迹
    while (time < PREDICT_TIME) {
        yaw += w * DT;
        x += v * cos(yaw) * DT;
        y += v * sin(yaw) * DT;
        time += DT;

        float obsDist = getObstacleDistance(x, y);
        if (obsDist < minDist) minDist = obsDist;
        if (obsDist < 0.15) return -1;
    }

    // 1. 航向评价
    float targetAngle = atan2(ty - y, tx - x);
    float headingScore = 1.0 - fabs(targetAngle - yaw) / PI;

    // 2. 安全距离评价
    float distScore = minDist / 1.5;
    if (distScore > 1.0) distScore = 1.0;

    // 3. 速度评价
    float velScore = v / MAX_V;

    // 4. 【新增】路径偏差评价：轨迹终点偏离全局路径的距离
    float deviation = 999;
    for (auto& p : globalPath) {
        float dx = p.x - x;
        float dy = p.y - y;
        float d = sqrt(dx*dx + dy*dy);
        if (d < deviation) deviation = d;
    }
    float deviationScore = 1.0 / (1.0 + deviation * 2.0);

    return w_heading * headingScore 
         + w_dist * distScore 
         + w_velocity * velScore 
         + w_deviation * deviationScore;
}

// ==================== 自适应速度控制 ====================
float computeAdaptiveSpeed(float minObsDist) {
    // 靠近障碍物时主动降速
    if (minObsDist < 0.3) {
        return 0.15;
    } else if (minObsDist < 0.6) {
        return 0.3;
    } else {
        return MAX_V;
    }
}

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

    // 获取全局路径目标点
    if (pathIdx >= (int)globalPath.size()) {
        motorL.move(0); motorR.move(0);
        return;
    }

    Point target = globalPath[pathIdx];
    float targetX = target.x;
    float targetY = target.y;

    // 到达当前节点检查
    float dx = targetX - robotX;
    float dy = targetY - robotY;
    if (sqrt(dx*dx + dy*dy) < 0.15) {
        pathIdx++;
        return;
    }

    // 【核心】自适应速度调整
    float maxV = computeAdaptiveSpeed(minObsDist);

    // 动态窗口（使用自适应最大速度）
    float minV = max(0.0, vx - ACC_V * DT);
    float maxV_adj = min(maxV, vx + ACC_V * DT);
    float minW = max(-MAX_W, vw - ACC_W * DT);
    float maxW = min(MAX_W, vw + ACC_W * DT);

    // 速度采样与评价
    float bestScore = -999;
    float bestV = 0, bestW = 0;

    for (int i = 0; i < SAMPLES_V; i++) {
        float v = minV + i * (maxV_adj - minV) / (SAMPLES_V - 1);
        for (int j = 0; j < SAMPLES_W; j++) {
            float w = minW + j * (maxW - minW) / (SAMPLES_W - 1);
            float score = evaluateWithDeviation(v, w, targetX, targetY);
            if (score > bestScore) {
                bestScore = score;
                bestV = v;
                bestW = w;
            }
        }
    }

    vx = bestV; vw = bestW;
    float wheelBase = 0.25;
    motorL.move(vx - vw * wheelBase / 2);
    motorR.move(vx + vw * wheelBase / 2);

    delay(50);
}

核心要点：
路径偏差评估项：确保局部避障后回归全局路径，避免绕远路
自适应速度控制：靠近障碍物时主动降速，保障安全
A*全局引导：解决DWA易陷入局部最优的问题

3、滚动窗口动态重规划 + 动态障碍物感知
适用场景：动态环境中障碍物频繁出现，需在滚动窗口内实时感知并触发重规划。

/* ===== 滚动窗口动态重规划 =====
 * 核心：滚动窗口感知新障碍，触发局部路径重规划
 * 参考：动态障碍物局部重规划 + 改进RRT-Connect与DWA融合
 */
#include <SimpleFOC.h>
#include <vector>
#include <algorithm>

BLDCMotor motorL(7), motorR(7);

// ==================== 地图与窗口参数 ====================
#define ROLLING_WINDOW_RADIUS 2.0  // 滚动窗口半径(m)
std::vector<std::pair<float,float>> staticObstacles;
std::vector<std::pair<float,float>> dynamicObstacles;

// ==================== 滚动窗口更新 ====================
void updateRollingWindow(float robotX, float robotY) {
    // 清除动态障碍物
    dynamicObstacles.clear();

    // 在当前窗口半径内检测新障碍物
    float frontDist = sonarF.ping_cm() / 100.0;
    float leftDist = sonarL.ping_cm() / 100.0;
    float rightDist = sonarR.ping_cm() / 100.0;

    // 检测到新障碍 → 加入动态障碍物列表
    if (frontDist > 0 && frontDist < ROLLING_WINDOW_RADIUS) {
        // 根据传感器方向估算障碍物位置
        float obsX = robotX + frontDist * cos(robotYaw);
        float obsY = robotY + frontDist * sin(robotYaw);
        dynamicObstacles.push_back({obsX, obsY});
    }

    // 【核心】检测到新障碍 → 触发重规划
    if (!dynamicObstacles.empty()) {
        // 标记新障碍到地图
        markNewObstacles();

        // 执行局部重规划
        triggerReplan();
    }
}

// ==================== 重规划触发器 ====================
void triggerReplan() {
    // 从当前位置到目标点重新规划
    if (pathIdx < (int)globalPath.size()) {
        Point goal = globalPath.back();
        // 更新地图后重新调用A*
        globalPath = aStar((int)(robotX/CELL_SIZE), (int)(robotY/CELL_SIZE),
                           (int)(goal.x/CELL_SIZE), (int)(goal.y/CELL_SIZE));
        pathIdx = 0;
        Serial.println("🔄 滚动窗口重规划触发");
    }
}

// ==================== 主控制循环 ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();

    // 1. 更新滚动窗口
    updateRollingWindow(robotX, robotY);

    // 2. 执行路径跟踪
    if (pathIdx < (int)globalPath.size()) {
        Point target = globalPath[pathIdx];
        float targetX = target.x;
        float targetY = target.y;

        float dx = targetX - robotX;
        float dy = targetY - robotY;
        float dist = sqrt(dx*dx + dy*dy);

        if (dist < 0.15) {
            pathIdx++;
            return;
        }

        // DWA控制（使用改进评价函数）
        dwaControl(targetX, targetY);
    }

    delay(50);
}

核心要点：
滚动窗口机制：仅以机器人为中心的局部窗口内感知和规划，降低计算复杂度
动态障碍物感知：实时检测并标记新障碍，触发重规划
重规划集成：检测到新障碍时更新地图并重新执行A*搜索

要点解读

1. 改进DWA的核心在于评价函数的“三加一”策略
   传统DWA的三项评价函数（朝向目标、安全距离、速度效率）在复杂环境中容易顾此失彼。改进DWA增加第四项——路径偏差评估，确保机器人避障后回归全局路径。此外，自适应速度控制根据障碍物距离动态调节速度上限，在保障安全的同时提升效率。

2. A全局路径 + DWA局部避障的分层架构是工业应用的标准范式
   A保证全局路径最优性，DWA负责局部避障和实时响应。两者融合方案在复杂动态环境中的成功率可达80%，单帧计算时间控制在25ms内。研究指出，改进融合算法相比传统方案，运行时间可下降31.46%-89.33%，路径长度缩短9.21%-17.61%。

3. 动态窗口的“滚动性”决定DWA的实时响应能力
   DWA的“动态”体现在每控制周期重新计算速度窗口：线速度窗口范围受当前速度和加速度约束；角速度窗口同样受限于当前角速度和角加速度。这种滚动机制确保DWA能快速响应环境变化，同时保证速度指令在机器人执行能力范围内。

4. 重规划触发条件是系统可靠性的关键
   重规划不能“频繁触发”，否则计算负载过高；也不能“疏于触发”，否则撞上新增障碍。工程实践中需结合置信度滤波：连续多次检测到障碍才触发重规划，避免单次噪声导致频繁重规划。检测距离阈值应与机器人制动距离匹配——检测距离过短来不及制动，过长则易误触发。

5. 实时性与计算资源的平衡决定系统架构
   标准Arduino Uno（AVR架构）浮点运算能力有限，难以承载DWA的轨迹推演和评价计算。需采用ESP32或STM32等高性能MCU，或使用分层架构：上位机（树莓派/Jetson）负责DWA重规划，下位机（ESP32/STM32）负责BLDC实时电机控制。控制频率建议≥20Hz，以保证对突发障碍物的快速响应。

4、动态障碍规避的改进DWA实时重规划系统
适用场景：机器人在已知静态路径基础上，实时应对突发动态障碍（如移动物体、临时遮挡），通过改进DWA的窗口搜索与动态权重调整，实现路径平滑调整与避障。

// 核心库引入（BLDC控制+数学运算+地图模拟）
#include <SimpleFOC.h>
#include <math.h>
#include <vector>

// 硬件引脚定义（BLDC电机+编码器）
#define LEFT_BLDC_PWM 9
#define LEFT_BLDC_IN1 10
#define LEFT_BLDC_IN2 11
#define RIGHT_BLDC_PWM 5
#define RIGHT_BLDC_IN1 6
#define RIGHT_BLDC_IN2 7
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 12

// 改进DWA核心参数（动态环境适配）
const float MAP_SIZE = 1000.0;  // 地图尺寸（mm）
const float ROBOT_RADIUS = 150.0; // 机器人半径（mm）
const float OBSTACLE_RADIUS = 200.0; // 障碍物半径（mm）
const float DT = 0.1;  // 采样周期（s）
const float V_MAX = 200.0; // 最大线速度（mm/s）
const float W_MAX = 60.0;  // 最大角速度（°/s）
const float V_MIN = 50.0;  // 最小线速度（mm/s）
const float W_MIN = 10.0;  // 最小角速度（°/s）
const int WINDOW_SIZE = 5;  // 滚动窗口采样点数
const float DYNAMIC_WEIGHT = 0.7; // 动态避障权重

// BLDC电机与编码器对象
BLDCMotor leftMotor = BLDCMotor(11);
BLDCMotor rightMotor = BLDCMotor(11);
BLDCDriver2PWM leftDriver = BLDCDriver2PWM(LEFT_BLDC_PWM, LEFT_BLDC_IN1, LEFT_BLDC_IN2);
BLDCDriver2PWM rightDriver = BLDCDriver2PWM(RIGHT_BLDC_PWM, RIGHT_BLDC_IN1, RIGHT_BLDC_IN2);
Encoder leftEnc(LEFT_ENC_A, LEFT_ENC_B);
Encoder rightEnc(RIGHT_ENC_A, RIGHT_ENC_B);

// 状态变量（DWA核心状态+滚动窗口）
struct RobotState {
  float x, y;        // 当前位置（mm）
  float theta;       // 航向角（°）
  float vx, vy;      // 线速度分量（mm/s）
  float omega;       // 角速度（°/s）
};
struct Obstacle {
  float x, y;        // 障碍物位置
  bool isDynamic;    // 是否动态障碍
  float vx, vy;      // 动态障碍运动速度
};
std::vector<Obstacle> dynamicObstacles; // 动态障碍列表
RobotState currentState = {0, 0, 0, 0, 0, 0};
std::vector<RobotState> windowHistory; // 滚动窗口历史状态
float globalPath[3][2] = {{0,0}, {500, 500}, {1000, 1000}}; // 全局路径（简化）
int globalPathIndex = 0; // 全局路径当前目标点索引

// 辅助函数：角度归一化（0~360°）
float normalizeAngle(float angle) {
  while (angle < 0) angle += 360.0;
  while (angle >= 360.0) angle -= 360.0;
  return angle;
}

// 辅助函数：计算障碍物与机器人距离
float getDistance(float x1, float y1, float x2, float y2) {
  return sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
}

// DWA代价函数：融合静态路径代价与动态避障代价
float dwaCostFunction(float v, float w, RobotState state, Obstacle* obsArray, int obsCount) {
  float pathCost = 0.0;  // 路径跟踪代价
  float obstacleCost = 0.0; // 障碍避让代价
  float dynamicCost = 0.0;  // 动态障碍代价

  // 1. 路径跟踪代价：与全局路径目标点的距离
  float targetX = globalPath[globalPathIndex][0];
  float targetY = globalPath[globalPathIndex][1];
  float distToTarget = getDistance(state.x, state.y, targetX, targetY);
  pathCost = distToTarget * 0.001; // 距离越小，代价越小

  // 2. 静态障碍避让代价
  for (int i=0; i<obsCount; i++) {
    if (!obsArray[i].isDynamic) {
      float dist = getDistance(state.x, state.y, obsArray[i].x, obsArray[i].y);
      if (dist < ROBOT_RADIUS + OBSTACLE_RADIUS) {
        obstacleCost += 1.0 / dist; // 距离越近，代价越大
      }
    }
  }

  // 3. 动态障碍避让代价（改进权重）
  for (int i=0; i<obsCount; i++) {
    if (obsArray[i].isDynamic) {
      // 预测未来0.5s的位置（滚动窗口简化）
      float obsFutureX = obsArray[i].x + obsArray[i].vx * 0.5;
      float obsFutureY = obsArray[i].y + obsArray[i].vy * 0.5;
      float dist = getDistance(state.x, state.y, obsFutureX, obsFutureY);
      if (dist < ROBOT_RADIUS + OBSTACLE_RADIUS) {
        dynamicCost += DYNAMIC_WEIGHT * (1.0 / dist); // 动态障碍权重更高
      }
    }
  }

  return pathCost + obstacleCost + dynamicCost;
}

// 改进DWA动态窗口搜索：结合滚动窗口状态预测
void improvedDwaPlanner(RobotState* state, float* targetV, float* targetW) {
  float minCost = 1e9;
  float bestV = 0, bestW = 0;

  // 动态窗口采样（结合滚动窗口的运动趋势）
  float vStep = (V_MAX - V_MIN) / 10;
  float wStep = (W_MAX - W_MIN) / 10;

  for (float v = V_MIN; v <= V_MAX; v += vStep) {
    for (float w = W_MIN; w <= W_MAX; w += wStep) {
      // 滚动窗口：基于历史状态预测当前窗口的可行速度范围
      bool feasible = true;
      if (windowHistory.size() >= 2) {
        // 检查速度是否在运动趋势窗口内（避免突变）
        float prevV = sqrt(windowHistory.back().vx*windowHistory.back().vx + 
                          windowHistory.back().vy*windowHistory.back().vy);
        float prevW = windowHistory.back().omega;
        float dv = abs(v - prevV);
        float dw = abs(w - prevW);
        if (dv > 50 || dw > 10) feasible = false; // 速度突变不加入采样
      }

      if (!feasible) continue;

      // 预测下一步状态
      float nextTheta = state->theta + w * DT;
      nextTheta = normalizeAngle(nextTheta);
      float nextX = state->x + v * cos(nextTheta * M_PI / 180.0) * DT;
      float nextY = state->y + v * sin(nextTheta * M_PI / 180.0) * DT;
      RobotState nextState = {nextX, nextY, nextTheta, v*cos(nextTheta*M_PI/180), v*sin(nextTheta*M_PI/180), w};

      // 计算代价
      float cost = dwaCostFunction(v, w, nextState, &dynamicObstacles[0], dynamicObstacles.size());
      if (cost < minCost) {
        minCost = cost;
        bestV = v;
        bestW = w;
      }
    }
  }

  *targetV = bestV;
  *targetW = bestW;
}

// 电机控制：根据规划速度输出BLDC控制信号
void motorControl(float vLeft, float vRight) {
  float wheelRadius = 50.0; // 轮半径（mm）
  // 线速度转转速（rpm）：v = ω*r → ω = v/r；ω(rad/s) → rpm = ω*60/(2π)
  float leftRpm = (vLeft / wheelRadius) * 60.0 / (2.0 * M_PI);
  float rightRpm = (vRight / wheelRadius) * 60.0 / (2.0 * M_PI);

  // 设置电机目标转速（SimpleFOC速度控制模式）
  leftMotor.target = leftRpm;
  rightMotor.target = rightRpm;
}

void setup() {
  Serial.begin(115200);
  Serial.println("改进DWA+动态障碍规避系统初始化中...");

  // 初始化BLDC电机
  leftDriver.init();
  rightDriver.init();
  leftMotor.linkDriver(&leftDriver);
  rightMotor.linkDriver(&rightDriver);
  leftMotor.init();
  rightMotor.init();
  leftMotor.initFOC();
  rightMotor.initFOC();
  leftMotor.controller = MotionControlType::velocity;
  rightMotor.controller = MotionControlType::velocity;
  leftMotor.velocity_PI_gains = {1.0, 0.1};
  rightMotor.velocity_PI_gains = {1.0, 0.1};

  // 初始化编码器
  leftEnc.init();
  rightEnc.init();
  leftMotor.linkSensor(&leftEnc);
  rightMotor.linkSensor(&rightEnc);

  // 初始化动态障碍（模拟场景）
  dynamicObstacles.push_back({300, 300, true, 50, 0}); // 向右移动的动态障碍
  dynamicObstacles.push_back({700, 700, false, 0, 0}); // 静态障碍

  // 初始化滚动窗口
  windowHistory.reserve(WINDOW_SIZE);

  Serial.println("初始化完成！机器人启动动态避障模式");
}

void loop() {
  // 1. 更新机器人状态（从编码器获取实际位置/速度，简化为模拟）
  currentState.x += currentState.vx * DT;
  currentState.y += currentState.vy * DT;
  currentState.theta = normalizeAngle(currentState.theta + currentState.omega * DT);

  // 更新滚动窗口（维护最近WINDOW_SIZE个状态）
  windowHistory.push_back(currentState);
  if (windowHistory.size() > WINDOW_SIZE) {
    windowHistory.erase(windowHistory.begin());
  }

  // 2. 更新动态障碍位置
  for (int i=0; i<dynamicObstacles.size(); i++) {
    if (dynamicObstacles[i].isDynamic) {
      dynamicObstacles[i].x += dynamicObstacles[i].vx * DT;
      dynamicObstacles[i].y += dynamicObstacles[i].vy * DT;
      // 超出地图范围则重置
      if (dynamicObstacles[i].x > MAP_SIZE || dynamicObstacles[i].x < 0) {
        dynamicObstacles[i].x = 500.0;
      }
    }
  }

  // 3. 改进DWA规划
  float targetV = 0, targetW = 0;
  improvedDwaPlanner(&currentState, &targetV, &targetW);

  // 4. 速度转换（差速模型：vLeft = v + w*L/2；vRight = v - w*L/2；L为轮距）
  float L = 200.0; // 轮距（mm）
  float vLeft = targetV + (targetW * L / 2.0) * M_PI / 180.0; // 角速度转线速度
  float vRight = targetV - (targetW * L / 2.0) * M_PI / 180.0;

  // 5. 电机控制
  motorControl(vLeft, vRight);
  leftMotor.move(50);
  rightMotor.move(50);

  // 6. 输出状态信息
  Serial.print("位置：(");Serial.print(currentState.x);Serial.print(",");Serial.print(currentState.y);
  Serial.print(") | 规划速度：v=");Serial.print(targetV);Serial.print(", w=");Serial.print(targetW);
  Serial.print(" | 动态障碍：");Serial.print(dynamicObstacles[0].x);Serial.print(",");Serial.print(dynamicObstacles[0].y);
  Serial.println();

  // 7. 检查是否到达全局路径目标点（切换下一个目标）
  float targetDist = getDistance(currentState.x, currentState.y, 
                                globalPath[globalPathIndex][0], globalPath[globalPathIndex][1]);
  if (targetDist < 100.0 && globalPathIndex < 2) {
    globalPathIndex++;
    Serial.println("到达目标点，切换下一个全局路径点");
  }

  // 循环周期控制
  delay(DT * 1000);
}

代码逻辑说明：
核心改进点：在传统DWA基础上，引入动态障碍代价权重（高于静态障碍），结合滚动窗口的运动趋势约束速度采样，避免机器人运动突变；
核心逻辑：通过模拟编码器更新机器人状态，维护滚动窗口记录运动趋势，实时更新动态障碍位置，通过改进的代价函数筛选最优速度，驱动BLDC电机实现动态避障；
扩展性：预留全局路径数组与目标点切换逻辑，可对接真实全局规划器，动态障碍数据可替换为激光雷达等传感器的实时输入。

5、滚动窗口路径预测的动态重规划系统
适用场景：机器人沿固定路径巡检时，通过滚动窗口预测未来窗口内的路径状态，提前识别潜在障碍与路径偏差，实现提前重规划，提升动态响应速度。

// 核心库引入
#include <SimpleFOC.h>
#include <math.h>
#include <vector>

// 硬件引脚（与案例1一致，可复用）
#define LEFT_BLDC_PWM 9
#define LEFT_BLDC_IN1 10
#define LEFT_BLDC_IN2 11
#define RIGHT_BLDC_PWM 5
#define RIGHT_BLDC_IN1 6
#define RIGHT_BLDC_IN2 7
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 12

// 滚动窗口重规划核心参数
const int ROLLING_WINDOW_STEPS = 10; // 滚动窗口步数（预测未来10个周期的状态）
const float PREDICT_DT = 0.2;       // 预测时间步长（s）
const float PATH_REPLAN_THRESHOLD = 150.0; // 路径偏差重规划阈值（mm）
const float MIN_SAFE_DISTANCE = 250.0;     // 最小安全距离（mm）

// 路径结构
struct WindowPath {
  std::vector<float> x;
  std::vector<float> y;
  std::vector<float> theta;
};

// 全局变量
BLDCMotor leftMotor = BLDCMotor(11);
BLDCMotor rightMotor = BLDCMotor(11);
BLDCDriver2PWM leftDriver = BLDCDriver2PWM(LEFT_BLDC_PWM, LEFT_BLDC_IN1, LEFT_BLDC_IN2);
BLDCDriver2PWM rightDriver = BLDCDriver2PWM(RIGHT_BLDC_PWM, RIGHT_BLDC_IN1, RIGHT_BLDC_IN2);
Encoder leftEnc(LEFT_ENC_A, LEFT_ENC_B);
Encoder rightEnc(RIGHT_ENC_A, RIGHT_ENC_B);

RobotState currentState = {0, 0, 0, 0, 0, 0};
WindowPath originalPath; // 原始路径
WindowPath replannedPath; // 重规划路径
std::vector<Obstacle> obstacles;
bool needReplan = false;
int currentWindowStep = 0;

// 辅助函数：计算路径曲率
float calculatePathCurvature(float x1, float y1, float theta1, float x2, float y2, float theta2) {
  float dx = x2 - x1;
  float dy = y2 - y1;
  float ds = sqrt(dx*dx + dy*dy);
  if (ds < 1.0) return 0.0;
  float dTheta = normalizeAngle(theta2 - theta1);
  return dTheta / ds; // 曲率 = 角度变化/弧长
}

// 滚动窗口路径预测：生成未来窗口内的路径状态
WindowPath predictWindowPath(RobotState startState, WindowPath basePath, int stepIndex) {
  WindowPath predictedPath;
  predictedPath.x.reserve(ROLLING_WINDOW_STEPS);
  predictedPath.y.reserve(ROLLING_WINDOW_STEPS);
  predictedPath.theta.reserve(ROLLING_WINDOW_STEPS);

  // 基于基础路径生成预测路径（结合当前状态的运动趋势）
  for (int i=0; i<ROLLING_WINDOW_STEPS; i++) {
    int pathIdx = stepIndex + i;
    if (pathIdx >= basePath.x.size()) {
      // 超出基础路径范围，沿原航向延伸
      predictedPath.x.push_back(startState.x + (startState.theta > 180 ? -1 : 1) * PREDICT_DT * i * 50);
      predictedPath.y.push_back(startState.y + (startState.theta > 90 && startState.theta < 270 ? -1 : 1) * PREDICT_DT * i * 50);
      predictedPath.theta.push_back(startState.theta);
    } else {
      // 调整基础路径，结合当前运动趋势（滚动窗口校正）
      float offsetX = (startState.vx / 100.0) * PREDICT_DT * i; // 速度校正偏移
      float offsetY = (startState.vy / 100.0) * PREDICT_DT * i;
      predictedPath.x.push_back(basePath.x[pathIdx] + offsetX);
      predictedPath.y.push_back(basePath.y[pathIdx] + offsetY);
      predictedPath.theta.push_back(basePath.theta[pathIdx]);
    }
  }
  return predictedPath;
}

// 滚动窗口重规划判定：检测路径冲突与偏差
void windowReplanCheck(WindowPath predictedPath, float* replanV, float* replanW) {
  float minDist = 1e9;
  bool pathConflict = false;
  float conflictX = 0, conflictY = 0;

  // 1. 检查预测路径与障碍物的冲突
  for (int i=0; i<predictedPath.x.size(); i++) {
    for (int j=0; j<obstacles.size(); j++) {
      float dist = getDistance(predictedPath.x[i], predictedPath.y[i], 
                              obstacles[j].x, obstacles[j].y);
      if (dist < MIN_SAFE_DISTANCE) {
        if (dist < minDist) {
          minDist = dist;
          pathConflict = true;
          conflictX = obstacles[j].x;
          conflictY = obstacles[j].y;
        }
      }
    }
  }

  // 2. 检查路径偏差（与原始路径的偏差）
  float pathDeviation = 0.0;
  for (int i=0; i<predictedPath.x.size(); i++) {
    int pathIdx = currentWindowStep + i;
    if (pathIdx < originalPath.x.size()) {
      float dev = getDistance(predictedPath.x[i], predictedPath.y[i], 
                             originalPath.x[pathIdx], originalPath.y[pathIdx]);
      pathDeviation += dev;
    }
  }
  pathDeviation /= predictedPath.x.size();

  // 3. 判定是否需要重规划
  if (pathConflict || pathDeviation > PATH_REPLAN_THRESHOLD) {
    needReplan = true;
    // 重规划策略：调整速度与航向避开冲突点
    if (pathConflict) {
      // 计算避开冲突点的航向
      float conflictTheta = atan2(conflictY - currentState.y, conflictX - currentState.x) * 180.0 / M_PI;
      float avoidTheta = normalizeAngle(conflictTheta + 90.0); // 垂直于障碍方向
      float dTheta = normalizeAngle(avoidTheta - currentState.theta);
      if (dTheta > 180.0) dTheta -= 360.0;
      *replanV = 150.0; // 重规划线速度
      *replanW = dTheta > 0 ? 30.0 : -30.0; // 调整航向
    } else {
      // 路径偏差：减速调整位置
      *replanV = 100.0;
      *replanW = pathDeviation > 0 ? 20.0 : -20.0;
    }
  } else {
    needReplan = false;
    // 无重规划，按原路径跟踪
    *replanV = 180.0;
    *replanW = 0.0;
  }
}

// 电机控制（与案例1一致）
void motorControl(float vLeft, float vRight) {
  float wheelRadius = 50.0;
  float leftRpm = (vLeft / wheelRadius) * 60.0 / (2.0 * M_PI);
  float rightRpm = (vRight / wheelRadius) * 60.0 / (2.0 * M_PI);
  leftMotor.target = leftRpm;
  rightMotor.target = rightRpm;
}

void setup() {
  Serial.begin(115200);
  Serial.println("滚动窗口路径预测重规划系统初始化...");

  // 初始化电机与编码器
  leftDriver.init();
  rightDriver.init();
  leftMotor.linkDriver(&leftDriver);
  rightMotor.linkDriver(&rightDriver);
  leftMotor.init();
  rightMotor.init();
  leftMotor.initFOC();
  rightMotor.initFOC();
  leftMotor.controller = MotionControlType::velocity;
  rightMotor.controller = MotionControlType::velocity;
  leftMotor.velocity_PI_gains = {1.0, 0.1};
  rightMotor.velocity_PI_gains = {1.0, 0.1};
  leftEnc.init();
  rightEnc.init();
  leftMotor.linkSensor(&leftEnc);
  rightMotor.linkSensor(&rightEnc);

  // 初始化原始路径（直线+曲线路径）
  originalPath.x = {0, 200, 400, 600, 800, 1000};
  originalPath.y = {0, 200, 400, 400, 600, 600};
  for (int i=0; i<originalPath.x.size(); i++) {
    float theta = i < 3 ? 45.0 : 0.0; // 前3段45°，后3段0°
    originalPath.theta.push_back(theta);
  }

  // 初始化障碍物
  obstacles.push_back({500, 400, false, 0, 0}); // 静态障碍，位于路径曲线处

  Serial.println("初始化完成，启动滚动窗口重规划模式");
}

void loop() {
  // 1. 更新当前状态
  currentState.x += currentState.vx * PREDICT_DT;
  currentState.y += currentState.vy * PREDICT_DT;
  currentState.theta = normalizeAngle(currentState.theta + currentState.omega * PREDICT_DT);

  // 2. 生成滚动窗口预测路径
  WindowPath predictedPath = predictWindowPath(currentState, originalPath, currentWindowStep);

  // 3. 重规划判定与速度输出
  float replanV = 0, replanW = 0;
  windowReplanCheck(predictedPath, &replanV, &replanW);

  // 4. 输出判定结果
  if (needReplan) {
    Serial.println("触发滚动窗口重规划！调整速度避开障碍/偏差");
  } else {
    Serial.println("路径正常，继续跟踪原始路径");
  }

  // 5. 速度转换与电机控制
  float L = 200.0;
  float vLeft = replanV + (replanW * L / 2.0) * M_PI / 180.0;
  float vRight = replanV - (replanW * L / 2.0) * M_PI / 180.0;
  motorControl(vLeft, vRight);
  leftMotor.move(50);
  rightMotor.move(50);

  // 6. 更新窗口步数
  currentWindowStep++;
  if (currentWindowStep >= originalPath.x.size() - ROLLING_WINDOW_STEPS) {
    currentWindowStep = 0; // 循环跟踪路径
  }

  // 输出路径状态
  Serial.print("当前位置：(");Serial.print(currentState.x);Serial.print(",");Serial.print(currentState.y);
  Serial.print(") | 窗口步数：");Serial.print(currentWindowStep);
  Serial.print(" | 重规划状态：");Serial.println(needReplan ? "是" : "否");

  delay(PREDICT_DT * 1000);
}

代码逻辑说明：

核心改进点：引入滚动窗口路径预测机制，基于原始路径与当前运动趋势，预测未来窗口内的路径状态，提前识别潜在障碍与路径偏差；
核心逻辑：通过滚动窗口预测路径，检测预测路径与障碍的冲突、与原始路径的偏差，判定是否需要重规划，重规划时结合冲突点位置与偏差方向调整速度和航向，BLDC电机执行调整后的运动指令；
适用场景：适用于需要严格跟踪预设路径的巡检场景，如仓库货架巡检、管道巡检，提前预判路径变化，避免临时避障导致的路径偏离。

6、动态环境协同的改进DWA+滚动窗口综合重规划系统
适用场景：多机器人协同或复杂动态环境（如工厂车间，人员、AGV、设备混行），通过改进DWA与滚动窗口的协同，实现全局路径与局部避障的统一，兼顾路径最优与动态响应。

// 核心库引入
#include <SimpleFOC.h>
#include <math.h>
#include <vector>

// 硬件引脚（同前，可复用）
#define LEFT_BLDC_PWM 9
#define LEFT_BLDC_IN1 10
#define LEFT_BLDC_IN2 11
#define RIGHT_BLDC_PWM 5
#define RIGHT_BLDC_IN1 6
#define RIGHT_BLDC_IN2 7
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 12

// 协同重规划核心参数
const int ROBOT_COUNT = 2;          // 协同机器人数量（模拟）
const float COLLAB_SAFE_DISTANCE = 300.0; // 协同机器人安全距离
const float GLOBAL_WEIGHT = 0.6;     // 全局路径权重
const float LOCAL_WEIGHT = 0.4;      // 局部避障权重
const float COLLAB_WEIGHT = 0.3;     // 协同避障权重
const int REPLAN_CYCLE = 500;        // 重规划周期（ms）

// 协同机器人结构
struct CollabRobot {
  float x, y, theta;
  float vx, vy, omega;
  int id;
};

// 全局变量
BLDCMotor leftMotor = BLDCMotor(11);
BLDCMotor rightMotor = BLDCMotor(11);
BLDCDriver2PWM leftDriver = BLDCDriver2PWM(LEFT_BLDC_PWM, LEFT_BLDC_IN1, LEFT_BLDC_IN2);
BLDCDriver2PWM rightDriver = BLDCDriver2PWM(RIGHT_BLDC_PWM, RIGHT_BLDC_IN1, RIGHT_BLDC_IN2);
Encoder leftEnc(LEFT_ENC_A, LEFT_ENC_B);
Encoder rightEnc(RIGHT_ENC_A, RIGHT_ENC_B);

RobotState selfState = {0, 0, 0, 0, 0, 0};
std::vector<CollabRobot> collabRobots; // 协同机器人列表
std::vector<Obstacle> envObstacles;    // 环境障碍
float globalPath[4][2] = {{0,0}, {300,300}, {600,300}, {900,0}}; // 全局路径
int currentPathIdx = 0;
unsigned long lastReplanTime = 0;

// 辅助函数：协同机器人间距离
float collabDistance(CollabRobot a, CollabRobot b) {
  return getDistance(a.x, a.y, b.x, b.y);
}

// 综合代价函数：融合全局路径、局部避障、协同避障
float comprehensiveCost(float v, float w, RobotState state, CollabRobot* collabs, int collabCount, Obstacle* obs, int obsCount) {
  float globalCost = 0.0;
  float localCost = 0.0;
  float collabCost = 0.0;

  // 1. 全局路径代价：与目标点的距离
  float targetX = globalPath[currentPathIdx][0];
  float targetY = globalPath[currentPathIdx][1];
  float distToTarget = getDistance(state.x, state.y, targetX, targetY);
  globalCost = GLOBAL_WEIGHT * distToTarget * 0.001;

  // 2. 局部避障代价：与环境障碍的距离
  for (int i=0; i<obsCount; i++) {
    float dist = getDistance(state.x, state.y, obs[i].x, obs[i].y);
    if (dist < MIN_SAFE_DISTANCE) {
      localCost += LOCAL_WEIGHT * (1.0 / dist);
    }
  }

  // 3. 协同避障代价：与协同机器人的距离
  for (int i=0; i<collabCount; i++) {
    float dist = collabDistance({state.x, state.y, state.theta, v, vy, w}, collabs[i]);
    if (dist < COLLAB_SAFE_DISTANCE) {
      collabCost += COLLAB_WEIGHT * (1.0 / dist);
    }
  }

  return globalCost + localCost + collabCost;
}

// 滚动窗口协同重规划核心逻辑
void collabReplan() {
  float bestV = 0, bestW = 0;
  float minCost = 1e9;

  // 动态窗口采样（结合滚动窗口的速度约束）
  float vStep = (V_MAX - V_MIN) / 8;
  float wStep = (W_MAX - W_MIN) / 8;

  for (float v = V_MIN; v <= V_MAX; v += vStep) {
    for (float w = W_MIN; w <= W_MAX; w += wStep) {
      // 预测下一步状态
      float nextTheta = normalizeAngle(selfState.theta + w * DT);
      float nextX = selfState.x + v * cos(nextTheta * M_PI / 180.0) * DT;
      float nextY = selfState.y + v * sin(nextTheta * M_PI / 180.0) * DT;
      RobotState nextState = {nextX, nextY, nextTheta, v*cos(nextTheta*M_PI/180), v*sin(nextTheta*M_PI/180), w};

      // 计算综合代价
      float cost = comprehensiveCost(v, w, nextState, &collabRobots[0], collabRobots.size(), &envObstacles[0], envObstacles.size());
      if (cost < minCost) {
        minCost = cost;
        bestV = v;
        bestW = w;
      }
    }
  }

  // 输出最优速度
  Serial.print("协同重规划结果：v=");Serial.print(bestV);Serial.print(", w=");Serial.print(bestW);Serial.println();

  // 速度转换与电机控制
  float L = 200.0;
  float vLeft = bestV + (bestW * L / 2.0) * M_PI / 180.0;
  float vRight = bestV - (bestW * L / 2.0) * M_PI / 180.0;
  float wheelRadius = 50.0;
  float leftRpm = (vLeft / wheelRadius) * 60.0 / (2.0 * M_PI);
  float rightRpm = (vRight / wheelRadius) * 60.0 / (2.0 * M_PI);
  leftMotor.target = leftRpm;
  rightMotor.target = rightRpm;
  leftMotor.move(50);
  rightMotor.move(50);
}

// 协同机器人状态更新（模拟多机器人通信）
void updateCollabRobots() {
  // 模拟协同机器人的运动（实际可替换为无线通信获取状态）
  for (int i=0; i<collabRobots.size(); i++) {
    collabRobots[i].x += collabRobots[i].vx * DT;
    collabRobots[i].y += collabRobots[i].vy * DT;
    collabRobots[i].theta = normalizeAngle(collabRobots[i].theta + collabRobots[i].omega * DT);
    // 边界检查
    if (collabRobots[i].x > 1000) collabRobots[i].x = 0;
    if (collabRobots[i].y > 1000) collabRobots[i].y = 0;
  }
}

// 全局路径切换检查
void checkGlobalPath() {
  float targetX = globalPath[currentPathIdx][0];
  float targetY = globalPath[currentPathIdx][1];
  float dist = getDistance(selfState.x, selfState.y, targetX, targetY);
  if (dist < 100.0 && currentPathIdx < 3) {
    currentPathIdx++;
    Serial.println("到达全局路径目标点，切换至下一段路径");
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("动态环境协同重规划系统初始化...");

  // 初始化电机与编码器
  leftDriver.init();
  rightDriver.init();
  leftMotor.linkDriver(&leftDriver);
  rightMotor.linkDriver(&rightDriver);
  leftMotor.init();
  rightMotor.init();
  leftMotor.initFOC();
  rightMotor.initFOC();
  leftMotor.controller = MotionControlType::velocity;
  rightMotor.controller = MotionControlType::velocity;
  leftMotor.velocity_PI_gains = {1.0, 0.1};
  rightMotor.velocity_PI_gains = {1.0, 0.1};
  leftEnc.init();
  rightEnc.init();
  leftMotor.linkSensor(&leftEnc);
  rightMotor.linkSensor(&rightEnc);

  // 初始化协同机器人（模拟2台）
  collabRobots.push_back({200, 200, 45, 100, 100, 15, 1});
  collabRobots.push_back({800, 200, 315, 80, -80, -10, 2});

  // 初始化环境障碍
  envObstacles.push_back({400, 300, false, 0, 0});
  envObstacles.push_back({600, 600, true, 0, 30});

  Serial.println("初始化完成，启动协同重规划模式");
}

void loop() {
  // 1. 更新自身状态（模拟编码器反馈）
  selfState.x += selfState.vx * DT;
  selfState.y += selfState.vy * DT;
  selfState.theta = normalizeAngle(selfState.theta + selfState.omega * DT);

  // 2. 定期触发协同重规划（按周期执行）
  if (millis() - lastReplanTime >= REPLAN_CYCLE) {
    lastReplanTime = millis();
    updateCollabRobots(); // 更新协同机器人状态
    collabReplan();        // 执行协同重规划
    checkGlobalPath();     // 检查全局路径切换
  }

  // 3. 输出状态信息
  Serial.print("自身位置：(");Serial.print(selfState.x);Serial.print(",");Serial.print(selfState.y);
  Serial.print(") | 协同机器人1：(");Serial.print(collabRobots[0].x);Serial.print(",");Serial.print(collabRobots[0].y);
  Serial.print(") | 协同机器人2：(");Serial.print(collabRobots[1].x);Serial.print(",");Serial.print(collabRobots[1].y);
  Serial.println(")");

  delay(DT * 1000);
}

代码逻辑说明：
核心改进点：融合改进DWA与滚动窗口，引入协同避障代价，实现全局路径、局部避障、多机器人协同的综合重规划，通过固定周期触发重规划平衡计算负载；
核心逻辑：维护协同机器人状态列表与环境障碍，定期更新状态并触发综合代价函数计算，筛选兼顾全局路径、局部避障、协同避障的最优速度，驱动BLDC电机执行，同时检查全局路径切换；
适用场景：适用于多机器人协同作业的复杂动态环境，如工厂车间AGV协同、仓储多机器人分拣，实现全局路径一致与局部动态避障的平衡。

要点解读

1. 改进DWA核心算法：滚动窗口约束与动态代价融合
   改进DWA的核心是解决传统DWA在动态环境中的实时性不足与避障灵活性问题，关键在于滚动窗口对运动趋势的约束与动态代价函数的设计：
   滚动窗口的运动趋势约束：传统DWA仅基于当前状态采样速度，改进后通过维护滚动窗口记录近期运动状态，约束速度采样范围——避免机器人因速度突变导致的运动震荡，确保速度调整符合运动惯性，提升路径跟踪的平稳性；
   动态代价函数的多维度融合：在传统路径跟踪、静态避障代价基础上，加入动态障碍代价、协同机器人代价等动态维度，通过可配置的权重平衡全局路径跟踪与局部动态避障的优先级；
   滚动窗口的动态环境适配：窗口内的障碍与环境数据实时更新，确保代价函数基于最新的环境信息计算，使机器人能及时响应突发的动态障碍，解决传统DWA对动态环境响应滞后的问题。
2. 滚动窗口动态重规划的：窗口设计与实时性平衡
   滚动窗口的核心是实现动态重规划，需解决窗口尺寸与实时性的平衡，同时保证路径连续性：
   窗口尺寸的动态适配：窗口尺寸需结合机器人的运动速度与环境复杂度调整——窗口过小无法覆盖潜在风险，过大则增加计算负载；案例中窗口尺寸为5~ 10个采样点，覆盖未来0.5~2秒的运动状态，兼顾风险覆盖与计算效率；
   窗口状态的动态更新机制：采用循环队列或滑动窗口策略，实时替换旧数据、新增新数据，保证窗口始终存储最新的运动状态；更新频率与采样周期同步，确保数据时效性，避免因数据滞后导致重规划决策失误；
   重规划触发条件与周期控制：重规划不能过于频繁，否则增加系统计算负担，也不能过于稀疏，否则无法及时响应动态变化；案例采用固定周期触发与突发条件触发（障碍接近阈值）结合的方式，平衡实时性与系统负载，确保动态响应的及时性与系统稳定性。
3. BLDC电机控制的精准性：与规划算法的深度协同
   BLDC电机是执行规划结果的核心，其控制精准性直接影响规划效果，关键在于与规划算法的速度跟踪、动态响应协同：
   速度跟踪的实时闭环控制：采用SimpleFOC库的速度闭环控制，实时采集编码器反馈的实际转速，与规划的目标转速对比，通过PID控制器调整PWM占空比，确保电机实际转速紧跟规划速度，避免因速度跟踪偏差导致路径偏离；
   动态响应的参数适配：规划算法输出的速度指令存在突变（如避障时的急转），需通过BLDC电机的速度环PID参数优化，提升电机的动态响应速度——P值增大提升响应速度，I值消除稳态误差，D值抑制震荡；同时针对突发速度变化，预留电机扭矩余量，确保电机能快速响应速度突变；
   差速模型的速度解算适配：差速底盘机器人需将规划输出的线速度与角速度转换为左右轮转速，需准确匹配轮半径、轮距等底盘参数，确保转换后的速度与规划结果一致；参数不匹配会导致底盘实际运动与规划不符，需通过实际测试校准。
4. 多源环境信息融合：动态感知与预测的结合
   改进DWA与滚动窗口的动态性依赖准确的环境信息，核心是实现多源环境信息的融合，同时预测动态环境的变化：
   静态与动态障碍的分层感知：通过传感器（模拟或实际）分别采集静态障碍（如固定设备）与动态障碍（如移动人员、协同机器人），在代价函数中对两者赋予不同权重——动态障碍权重更高，确保机器人优先规避动态风险；
   动态环境的预测建模：针对动态障碍（如移动物体），建立简单的运动预测模型，基于当前速度与航向，预测未来窗口内的位置，提前识别潜在碰撞风险；案例采用线性预测模型，复杂度低、计算速度快，适配Arduino的算力限制；
   多源信息的滤波与去噪：传感器采集的原始数据存在噪声，需通过滤波算法（如卡尔曼滤波、滑动平均）处理，提升障碍位置与自身状态的精度；滤波后的稳定数据是代价函数准确计算的基础，避免因噪声导致的误避障或路径偏差。
5. 系统鲁棒性保障：容错机制与可靠性设计
   动态环境下系统易受传感器故障、电机异常、通信中断影响，需通过容错机制与可靠性设计保障稳定运行：
   传感器与电机的故障容错：传感器出现短暂失效时，采用历史数据补偿——基于滚动窗口的历史状态预测当前状态，维持短期规划；电机响应异常时，启动安全停止机制，触发最小安全速度，避免机器人失控；同时预留硬件监测接口（如电流检测、温度检测），实现电机过载、过热保护；
   动态环境的鲁棒性设计：面对环境突变（如突发障碍遮挡），通过滚动窗口的缓冲机制，允许重规划后的路径存在一定平滑过渡，避免急转急停；同时将全局路径作为备份，当局部规划完全失效时，切换至全局路径的简化跟踪，确保机器人不会完全失去方向；
   计算资源的优化与负载平衡：Arduino算力有限，需优化计算复杂度——减少速度采样数量、简化代价函数计算、采用低复杂度的预测模型；同时合理设置重规划周期，避免频繁重规划占用过多算力，保证系统实时性，避免因计算过载导致规划延迟或卡顿。

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