在机器人控制算法的底层架构中，基于Arduino与BLDC（无刷直流电机）的“滚动窗口动态重规划 + 动态障碍物感知”系统，是解决机器人在非结构化、动态环境中实现安全、平滑自主导航的核心技术方案。该方案将全局路径规划的宏观引导与局部动态窗口的微观避障相结合，并依赖BLDC的高动态响应能力来执行复杂的运动指令。以下是该系统的详细专业解析：

核心机制解析

1. 滚动窗口（Rolling Window）局部感知与计算
   全局地图在动态环境中会迅速过时。系统采用滚动窗口机制，仅以机器人为中心，实时更新并维护一个局部范围内的代价地图（Costmap）。这大幅降低了主控的内存占用与计算负荷，使Arduino等嵌入式平台能够以高频（如10Hz以上）处理局部环境变化，确保对突发障碍物的快速响应。
2. 基于动力学约束的动态窗口法（DWA）避障
   在滚动窗口内，系统采用DWA算法进行局部路径规划。该算法不仅考虑了安全约束（确保机器人在碰到障碍物前能刹停），还严格纳入了BLDC底盘的运动学与动力学约束（如最大线速度、角速度及加速度限制）。
   动态窗口生成：根据当前速度和加速度能力，确定下一时刻可达的速度空间（v, ω）。
   轨迹模拟与评价：在动态窗口内采样多组速度，推演未来一段时间（Sim Time）的轨迹，并基于朝向目标、安全距离、速度等多目标评价函数选出最优速度指令。
3. BLDC高动态响应与平滑执行
   动态避障算法输出的速度和角速度指令往往是连续且高频变化的。BLDC电机配合FOC（磁场定向控制）或高性能闭环驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪DWA输出的平滑轨迹，避免了传统步进电机或直流有刷电机在频繁启停和转向时的机械冲击与轨迹偏差。
4. 全局与局部的“感知-规划-执行”闭环协同
   系统形成严密的分层闭环：全局规划器（如A*）提供一条通往目标的宏观参考路径；局部规划器（DWA）在滚动窗口内根据实时传感器数据（激光雷达/超声波）和全局路径的引导，计算出最优的局部避障速度；BLDC底层控制器精准执行该速度。当机器人绕过动态障碍后，又能平滑地回归全局路径。

典型应用场景
人机协作仓储与物流AMR：在电商仓库或柔性制造车间，人员和叉车会随时横穿机器人通道。滚动窗口重规划能确保AMR在高速运行中，对突然出现的动态障碍物进行平滑减速绕行，并在安全后迅速恢复原速，保障物流效率与人员安全。
服务机器人与室内配送：在酒店、餐厅、医院等动态人流环境中，机器人需要主动、平滑地避让行人，而不是僵直地停止。
高校机器人课程与原型验证：在未知迷宫或模拟仓储环境中，实现AGV小车从起点到终点的自主行驶，并在人为放置障碍物时自动绕行，验证动态避障算法的有效性。

关键注意事项
传感器选择、融合与数据处理延迟：单一传感器有局限（如超声波精度低、激光雷达数据量大）。建议采用传感器融合策略，例如用超声波做广域检测，用小型2D激光雷达做精确测距。同时需注意在Arduino上处理多传感器数据极易成为系统瓶颈，必要时可采用Arduino + 专用处理核心（如ESP32）的架构。
重规划算法的计算效率与可靠性：A*等全局规划算法在动态环境中太慢，而简单的反应式避障（如BUG算法）可能陷入局部最优（如“死锁”在U型障碍物内）。DWA是经典选择，但需注意其“短视”问题（只看未来1-3秒），在复杂环境中容易陷入局部极小值。工程上常通过加入全局路径引导或触发恢复行为（如旋转、后退）来解决。
DWA评价函数的权重调参：DWA的核心在于三个评价函数的设计和权重调参（朝向目标、速度、离障碍物距离）。权重需要根据场景反复调试：heading权重太高会直冲目标不管障碍物；clearance权重太高会过度保守；velocity权重太高会冒不必要的风险。
BLDC底层控制的精准性：动态避障要求机器人能快速启停、加速和转向。BLDC电机的高扭矩密度和快速响应特性是实现敏捷避障动作的物理基础，需确保底层PID参数整定良好，以精确跟踪上层规划器输出的速度指令。

1、滚动窗口感知与重规划触发机制
此案例聚焦于滚动窗口的局部感知与重规划触发逻辑。机器人仅维护以自身为中心、半径2米的局部窗口，当超声波检测到窗口内出现新障碍时，立即将障碍物位置标记到局部地图并触发路径重规划。

#include <SimpleFOC.h>
#include <vector>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11), driverR(5, 6, 8);

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

// 机器人状态
float robotX = 0, robotY = 0, robotYaw = 0;
std::vector<std::pair<float,float>> globalPath;
int pathIdx = 0;

// ===== 滚动窗口更新 =====
void updateRollingWindow() {
    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()) {
        // 从当前位置到目标点重新规划
        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();

    // 2. 执行路径跟踪
    if (pathIdx < (int)globalPath.size()) {
        Point target = globalPath[pathIdx];
        float dx = target.x - robotX;
        float dy = target.y - robotY;
        float dist = sqrt(dx*dx + dy*dy);

        if (dist < 0.15) pathIdx++;
        dwaControl(target.x, target.y);
    }
    delay(50);
}

关键逻辑：滚动窗口机制的核心是“只关注机器人周围”。全局地图在动态环境中会迅速过时，而局部窗口以机器人为中心实时更新，内存占用和计算量大幅降低，使Arduino能以10Hz以上的频率响应环境变化。

2、动态障碍物建模与代价权重调整
此案例在滚动窗口基础上，引入动态障碍物与静态障碍物的差异化代价权重。动态障碍物（如行人）的避让优先级高于静态障碍物（如墙壁），在DWA评价函数中赋予更高的代价权重，引导机器人优先远离移动物体。

// ===== 障碍物结构（带类型标记）=====
struct Obstacle {
    float x, y;
    bool isDynamic;      // true=动态障碍物
    float radius;        // 障碍物半径
};

std::vector<Obstacle> localObstacles;

// ===== 代价地图更新（区分动静障碍）=====
void updateLocalCostmap() {
    localObstacles.clear();

    // 静态障碍物（预存或SLAM地图）
    for (auto& obs : staticObstacles) {
        localObstacles.push_back({obs.first, obs.second, false, 0.2});
    }

    // 动态障碍物（实时检测）
    float frontDist = sonarF.ping_cm() / 100.0;
    if (frontDist > 0 && frontDist < ROLLING_WINDOW_RADIUS) {
        float obsX = robotX + frontDist * cos(robotYaw);
        float obsY = robotY + frontDist * sin(robotYaw);
        localObstacles.push_back({obsX, obsY, true, 0.3});  // 动态障碍半径更大
    }
}

// ===== DWA评价函数（动态障碍权重更高）=====
float evaluateTrajectory(float v, float w, float dt, float simTime) {
    float cost = 0.0;
    float simX = robotX, simY = robotY, simYaw = robotYaw;

    for (float t = 0; t < simTime; t += dt) {
        // 模拟轨迹
        simX += v * cos(simYaw) * dt;
        simY += v * sin(simYaw) * dt;
        simYaw += w * dt;

        // 遍历所有障碍物，计算代价
        for (auto& obs : localObstacles) {
            float dist = sqrt(pow(simX - obs.x, 2) + pow(simY - obs.y, 2));
            if (dist < 0.5) {  // 障碍影响范围
                // 【核心】动态障碍权重是静态的2倍
                float weight = obs.isDynamic ? 2.0 : 1.0;
                cost += weight * (1.0 / (dist + 0.1));
            }
        }
    }

    // 速度偏好（鼓励前进）
    cost -= 0.1 * v;
    return cost;
}

关键逻辑：动态障碍物（如行走的行人）的避让紧迫性高于墙壁。通过赋予动态障碍更高的代价权重，DWA在采样速度时会更倾向于选择远离动态障碍的轨迹，实现“优先避人、其次避墙”的智能策略。

3、改进DWA + 滚动窗口融合执行
此案例将滚动窗口感知与改进DWA算法完整融合。滚动窗口负责维护局部环境模型并触发重规划，改进DWA在窗口内采样速度并推演轨迹，通过评价函数选出最优速度指令，驱动BLDC执行平滑避障。

// ===== 改进DWA核心参数 =====
const float MAX_V = 1.0;      // 最大线速度(m/s)
const float MAX_W = 1.5;      // 最大角速度(rad/s)
const float ACC_V = 2.0;      // 线加速度(m/s²)
const float ACC_W = 3.0;      // 角加速度(rad/s²)
const float SIM_TIME = 1.5;   // 轨迹推演时间(s)
const float DT = 0.1;         // 采样步长(s)

// ===== 动态窗口计算 =====
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 dwaControl(float goalX, float goalY) {
    float vMin, vMax, wMin, wMax;
    computeDynamicWindow(vMin, vMax, wMin, wMax);

    float bestV = 0, bestW = 0;
    float bestCost = 1e9;

    // 在动态窗口内采样
    for (float v = vMin; v <= vMax; v += 0.1) {
        for (float w = wMin; w <= wMax; w += 0.2) {
            float cost = evaluateTrajectory(v, w, DT, SIM_TIME);
            if (cost < bestCost) {
                bestCost = cost;
                bestV = v;
                bestW = w;
            }
        }
    }

    // 执行最优速度（BLDC差速控制）
    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;
}

// ===== 主循环 =====
void loop() {
    // 1. 滚动窗口感知与重规划触发
    updateRollingWindow();

    // 2. 获取当前目标点
    Point target = globalPath[pathIdx];

    // 3. 改进DWA局部规划与执行
    dwaControl(target.x, target.y);

    // 4. 到达目标点切换
    float dist = sqrt(pow(target.x - robotX, 2) + pow(target.y - robotY, 2));
    if (dist < 0.15 && pathIdx < globalPath.size() - 1) {
        pathIdx++;
    }

    delay(50);
}

关键逻辑：改进DWA在传统DWA基础上引入了动态障碍代价权重和滚动窗口运动趋势约束。动态窗口根据当前速度和加速度限制采样范围，避免机器人运动突变；评价函数综合考虑目标朝向、安全距离和速度，选出最优速度指令。BLDC配合FOC的毫秒级响应，确保DWA输出的连续速度指令能被精准执行。

要点解读

1. 滚动窗口是“以机器人为中心”的局部感知策略

全局地图在动态环境中会迅速过时。滚动窗口机制仅维护机器人周围一定半径内的局部环境模型，大幅降低内存占用和计算负荷，使Arduino/ESP32等嵌入式平台能够以高频（10Hz以上）处理局部环境变化。这种“只关注眼前”的策略，是有限算力下实现动态避障的工程标准。

2. 动态障碍物需要差异化代价权重

动态障碍物（行人、移动车辆）与静态障碍物（墙壁、货架）的避让紧迫性不同。在DWA评价函数中，动态障碍应赋予更高的代价权重（如2倍），引导规划器优先选择远离移动物体的轨迹。这种差异化处理让机器人实现“优先避人、其次避墙”的智能行为。

3. DWA的动态窗口受BLDC物理约束

DWA采样的速度范围并非无限，而是受机器人当前速度和加速度限制。动态窗口 [v-Δv, v+Δv] 和 [w-Δw, w+Δw] 确保了采样轨迹在物理上可行。BLDC配合FOC的高动态响应能力，使机器人能够跟踪DWA输出的连续速度指令，避免传统电机的机械冲击和轨迹偏差。

4. 重规划触发需要“防抖”机制

滚动窗口检测到新障碍后不应立即重规划，否则传感器噪声会导致频繁重算。工程上建议设置持续检测阈值：障碍物连续存在于窗口内超过N个控制周期（如3次，约150ms）后，才触发重规划。同时需区分“可绕行障碍”和“必须重规划障碍”——小障碍可直接由DWA绕行，无需触发全局重算。

5. 分层架构是算力受限下的必然选择

完整的滚动窗口维护、DWA采样和BLDC的FOC控制对算力要求较高。工程实践推荐分层架构：将滚动窗口维护和DWA局部规划放在ESP32或树莓派上，Arduino专职负责BLDC的FOC电机控制和传感器数据采集。若采用单一ESP32，可利用双核架构——Core 0跑感知与规划，Core 1跑电机控制，确保高频控制环不被低频规划阻塞。

4、室内服务机器人（滚动窗口路径规划+动态避让行人）
适用场景：商场/医院等室内场景，BLDC驱动的服务机器人需在动态人流中穿梭，既要跟踪全局路径，又要实时躲避移动的行人，同时避免因算力不足导致规划卡顿。

核心逻辑：
滚动窗口机制：每1秒生成一个以机器人为中心的局部规划窗口（如5m×5m），窗口内叠加全局路径的局部片段，降低规划维度；
动态障碍物感知：通过激光雷达实时获取行人的位置、速度，用简单运动模型预测窗口内的障碍物轨迹；
动态重规划：若预测到障碍物与规划路径冲突，在滚动窗口内用A*算法生成局部避让路径，并更新控制指令，无需全量重新规划。

#include <SimpleFOC.h>
#include <Servo.h>
#include <queue>
#include <vector>
#include <cmath>

// 机器人参数
#define WHEEL_BASE 0.5  // 轮距(m)
#define MAX_PLAN_WINDOW 5.0  // 滚动窗口半径(m)
#define PLAN_INTERVAL 1000  // 重规划周期(ms)
#define RP_LIDAR_PIN 2  // 雷达数据引脚

// BLDC电机控制
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(12,13,14,15);

// 动态障碍物结构
struct DynamicObstacle {
  float x, y;          // 当前位置
  float vx, vy;        // 速度(m/s)
  uint32_t lastUpdate; // 最后更新时间
};

// 滚动窗口规划核心
class RollingWindowPlanner {
public:
  RollingWindowPlanner(float windowSize) : windowSize(windowSize), robotX(0), robotY(0) {}
  
  // 添加全局路径（局部窗口片段）
  void addGlobalPathSegment(std::vector<std::pair<float, float>> path) {
    globalPath = path;
  }
  
  // 计算局部路径（滚动窗口内重规划）
  std::vector<std::pair<float, float>> planLocalPath(std::vector<DynamicObstacle> obstacles) {
    std::vector<std::pair<float, float>> localPath;
    if (globalPath.empty()) return localPath;
    
    // 1. 裁剪全局路径到滚动窗口内
    for (auto p : globalPath) {
      float dx = p.first - robotX;
      float dy = p.second - robotY;
      if (std::sqrt(dx*dx + dy*dy) <= windowSize) {
        localPath.push_back(p);
      }
    }
    if (localPath.empty()) return localPath;
    
    // 2. 障碍物轨迹预测（1秒内的位置）
    std::vector<std::pair<float, float>> predictedObstacles;
    uint32_t now = millis();
    for (auto obs : obstacles) {
      if (now - obs.lastUpdate < 1000) {
        float predX = obs.x + obs.vx * 1.0;
        float predY = obs.y + obs.vy * 1.0;
        predictedObstacles.push_back({predX, predY});
      }
    }
    
    // 3. 窗口内A*算法动态避让（简化栅格化）
    // 注：实际可嵌入轻量A*，此处简化为避让方向判断
    float targetX = localPath.back().first;
    float targetY = localPath.back().second;
    
    // 判断障碍物是否在规划路径上
    bool needDetour = false;
    for (auto obs : predictedObstacles) {
      // 计算障碍物与路径的距离
      float distToPath = pointToLineDistance(robotX, robotY, targetX, targetY, obs.x, obs.y);
      if (distToPath < 0.8) { // 安全距离阈值
        needDetour = true;
        break;
      }
    }
    
    if (needDetour) {
      // 生成避让路径（左转绕行）
      localPath.insert(localPath.begin(), {robotX, robotY});
      localPath.insert(localPath.begin()+1, {robotX + 0.5, robotY + 0.3});
    }
    return localPath;
  }
  
  void updateRobotPosition(float x, float y) {
    robotX = x;
    robotY = y;
  }
  
private:
  float windowSize;
  std::vector<std::pair<float, float>> globalPath;
  float robotX, robotY;
  
  // 点到线段的距离
  float pointToLineDistance(float x1, float y1, float x2, float y2, float px, float py) {
    float A = px - x1, B = py - y1;
    float C = x2 - x1, D = y2 - y1;
    float dot = A*C + B*D;
    float lenSq = C*C + D*D;
    float param = lenSq != 0 ? dot / lenSq : -1;
    float xx, yy;
    if (param < 0) { xx = x1; yy = y1; }
    else if (param > 1) { xx = x2; yy = y2; }
    else { xx = x1 + param*C; yy = y1 + param*D; }
    float dx = px - xx, dy = py - yy;
    return std::sqrt(dx*dx + dy*dy);
  }
};

// 动态障碍物追踪器
class DynamicObstacleTracker {
public:
  std::vector<DynamicObstacle> track(uint16_t *lidarData, int dataCount) {
    std::vector<DynamicObstacle> obstacles;
    // 简化：从雷达数据中提取移动目标（实际需点云聚类）
    // 此处模拟：假设data为障碍物x,y交替存储
    for (int i = 0; i < dataCount/2; i++) {
      float x = lidarData[i*2];
      float y = lidarData[i*2+1];
      float dist = std::sqrt(x*x + y*y);
      if (dist < MAX_PLAN_WINDOW && dist > 0.5) { // 有效障碍物范围
        // 查找已有障碍物（匹配位置）
        bool found = false;
        for (auto& obs : obstacles) {
          float dx = obs.x - x, dy = obs.y - y;
          if (std::sqrt(dx*dx + dy*dy) < 0.2) { // 位置匹配
            float dt = (millis() - obs.lastUpdate) / 1000.0;
            if (dt > 0) {
              obs.vx = (x - obs.x) / dt;
              obs.vy = (y - obs.y) / dt;
            }
            obs.x = x;
            obs.y = y;
            obs.lastUpdate = millis();
            found = true;
            break;
          }
        }
        if (!found) {
          DynamicObstacle newObs = {x, y, 0, 0, millis()};
          obstacles.push_back(newObs);
        }
      }
    }
    return obstacles;
  }
};

// 全局变量
RollingWindowPlanner planner(MAX_PLAN_WINDOW);
DynamicObstacleTracker tracker;
std::vector<DynamicObstacle> currentObstacles;
uint16_t lidarData[180]; // 简化雷达数据（180个点，每个点x,y交替）

void setup() {
  Serial.begin(115200);
  // 初始化电机
  driverL.voltage_power_supply = 24;
  driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  
  // 初始化全局路径（模拟：从(0,0)到(10,0)的直线）
  std::vector<std::pair<float, float>> globalPath;
  for (float x = 0; x <= 10; x += 1.0) {
    globalPath.push_back({x, 0});
  }
  planner.addGlobalPathSegment(globalPath);
  
  // 模拟雷达数据输入（实际应用中连接RPLiDAR）
  for (int i = 0; i < 180; i++) {
    float angle = i * 2.0 * PI / 180.0;
    float dist = (i % 30 == 0) ? 2.0 : 10.0; // 模拟障碍物（每30个点一个障碍物）
    lidarData[i] = dist * cos(angle) * 100;  // 转换为整数存储
    if (i % 2 == 0) {
      lidarData[i] = dist * cos(angle) * 100;
    } else {
      lidarData[i] = dist * sin(angle) * 100;
    }
  }
}

uint32_t lastPlanTime = 0;
void loop() {
  // 1. 更新机器人位置（假设通过编码器估算）
  static float robotX = 0, robotY = 0;
  planner.updateRobotPosition(robotX, robotY);
  
  // 2. 周期性执行滚动窗口重规划
  if (millis() - lastPlanTime > PLAN_INTERVAL) {
    // 3. 动态障碍物追踪
    currentObstacles = tracker.track(lidarData, 180);
    
    // 4. 滚动窗口内动态重规划
    std::vector<std::pair<float, float>> localPath = planner.planLocalPath(currentObstacles);
    
    // 5. 生成电机控制指令（路径跟踪）
    if (!localPath.empty()) {
      float dx = localPath[1].first - robotX;
      float dy = localPath[1].second - robotY;
      float targetAngle = atan2(dy, dx);
      
      // 简化：根据路径角度调整左右轮速度（差速转向）
      float speed = 0.5; // 基础速度
      float turnRate = 0.1; // 转向速率
      motorL.move(speed - turnRate);
      motorR.move(speed + turnRate);
      
      // 更新机器人位置
      robotX += 0.1 * cos(targetAngle);
      robotY += 0.1 * sin(targetAngle);
    } else {
      motorL.move(0.5);
      motorR.move(0.5);
    }
    
    lastPlanTime = millis();
  }
  
  // 电机FOC循环
  motorL.loopFOC();
  motorR.loopFOC();
  delay(10);
}

5、工业仓储AGV（滚动窗口货位动态重规划）
适用场景：工业仓储环境中，多台AGV在固定货架间作业，当某货位被占用、临时新增任务或路径拥堵时，需在滚动窗口内快速调整前往目标货位的路径，同时避让其他移动AGV和叉车。

核心逻辑：
滚动窗口任务触发：AGV每到达一个“窗口节点”（如每5m设置的虚拟节点），触发一次局部路径重规划，避免全路径重算；
动态障碍物感知：通过UWB定位和红外传感器感知周边AGV/叉车的位置、速度，标记为动态障碍物；
局部路径优化：在窗口内用Dijkstra算法搜索避让路径，结合仓储货位优先级（如紧急任务优先）调整路径，确保高效通行。

#include <SimpleFOC.h>
#include <vector>
#include <queue>
#include <cmath>

// 仓储参数
#define WINDOW_NODE_DIST 5.0  // 窗口节点间距(m)
#define AGV_SAFE_DIST 1.0     // AGV安全距离(m)
#define UWB_DEV_ID 1          // 本AGV的UWB设备ID

// BLDC电机控制
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(12,13,14,15);

// 动态障碍物结构（AGV/叉车）
struct AGVObstacle {
  int devId;        // UWB设备ID
  float x, y;       // 位置
  float speed;      // 速度
  int taskPriority; // 任务优先级
};

// 滚动窗口节点
struct WindowNode {
  float x, y;
  bool visited;
};

// 滚动窗口货位重规划器
class AGVRollingPlanner {
public:
  AGVRollingPlanner() : currentNodeIndex(0), totalNodes(0) {}
  
  // 初始化全局路径（虚拟节点串联）
  void initGlobalPath(std::vector<WindowNode> nodes) {
    globalNodes = nodes;
    totalNodes = nodes.size();
    currentNodeIndex = 0;
  }
  
  // 动态重规划（滚动窗口：当前节点到下一节点）
  std::vector<WindowNode> replan(int targetNode, std::vector<AGVObstacle> obstacles) {
    std::vector<WindowNode> localPath;
    if (currentNodeIndex >= totalNodes) return localPath;
    
    // 1. 确定当前滚动窗口范围（当前节点到下一节点）
    int nextNodeIndex = currentNodeIndex + 1;
    if (nextNodeIndex >= totalNodes) nextNodeIndex = totalNodes - 1;
    
    // 2. 收集窗口内的节点
    for (int i = currentNodeIndex; i <= nextNodeIndex; i++) {
      localPath.push_back(globalNodes[i]);
    }
    
    // 3. 动态障碍物避让检查
    bool conflict = false;
    for (auto obs : obstacles) {
      // 计算障碍物到路径节点的距离
      for (auto& node : localPath) {
        float dx = obs.x - node.x;
        float dy = obs.y - node.y;
        if (std::sqrt(dx*dx + dy*dy) < AGV_SAFE_DIST) {
          conflict = true;
          // 临时调整路径节点（避让）
          if (i == currentNodeIndex + 1) { // 下一节点冲突
            node.y += 0.8; // 侧移避让
          }
          break;
        }
      }
      if (conflict) break;
    }
    
    // 4. 若冲突严重，临时新增窗口节点（绕行）
    if (conflict && nextNodeIndex < totalNodes - 1) {
      WindowNode detourNode = {
        (globalNodes[currentNodeIndex].x + globalNodes[nextNodeIndex].x)/2,
        (globalNodes[currentNodeIndex].y + globalNodes[nextNodeIndex].y)/2 + 1.0
      };
      localPath.insert(localPath.begin() + 1, detourNode);
    }
    
    // 5. 更新当前节点为下一节点
    if (!conflict) currentNodeIndex++;
    
    return localPath;
  }
  
  int getCurrentNodeIndex() { return currentNodeIndex; }
  
private:
  std::vector<WindowNode> globalNodes;
  int currentNodeIndex;
  int totalNodes;
};

// UWB动态障碍物感知（模拟）
class AGVObstacleTracker {
public:
  std::vector<AGVObstacle> track() {
    std::vector<AGVObstacle> obstacles;
    // 模拟UWB接收其他AGV的位置数据（实际需UWB模块通信）
    // AGV2：位置(3,0)，速度0.3m/s，优先级2
    obstacles.push_back({2, 3.0, 0.0, 0.3, 2});
    // AGV3：位置(5,2)，速度0.2m/s，优先级1
    obstacles.push_back({3, 5.0, 2.0, 0.2, 1});
    return obstacles;
  }
};

// 全局变量
AGVRollingPlanner planner;
AGVObstacleTracker tracker;
std::vector<WindowNode> globalPathNodes;

void setup() {
  Serial.begin(115200);
  // 初始化电机
  driverL.voltage_power_supply = 24;
  driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  
  // 初始化仓储全局路径节点（模拟：从起点(0,0)到终点(20,0)的节点序列）
  for (float x = 0; x <= 20; x += WINDOW_NODE_DIST) {
    globalPathNodes.push_back({x, 0});
  }
  planner.initGlobalPath(globalPathNodes);
}

uint32_t lastPlanTime = 0;
float robotX = 0, robotY = 0;
void loop() {
  // 1. 电机FOC控制
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 2. 模拟位置更新（通过编码器计算）
  // 假设当前位置接近当前窗口节点时，触发重规划
  float currentNodeX = globalPathNodes[planner.getCurrentNodeIndex()].x;
  if (fabs(robotX - currentNodeX) < 0.5) {
    // 3. 感知动态障碍物（其他AGV）
    std::vector<AGVObstacle> obstacles = tracker.track();
    
    // 4. 滚动窗口动态重规划
    std::vector<WindowNode> localPath = planner.replan(globalPathNodes.size()-1, obstacles);
    
    // 5. 生成控制指令（沿局部路径行驶）
    if (!localPath.empty()) {
      float nextX = localPath[1].x;
      float nextY = localPath[1].y;
      float dx = nextX - robotX;
      float dy = nextY - robotY;
      float angle = atan2(dy, dx);
      float targetAngle = angle * 180.0 / PI;
      
      // 差速转向：调整左右轮速度
      float baseSpeed = 0.4;
      float turnSpeedDiff = 0.15;
      if (fabs(targetAngle) > 10) { // 需要转向
        if (targetAngle > 0) {
          motorL.move(baseSpeed + turnSpeedDiff);
          motorR.move(baseSpeed - turnSpeedDiff);
        } else {
          motorL.move(baseSpeed - turnSpeedDiff);
          motorR.move(baseSpeed + turnSpeedDiff);
        }
      } else {
        motorL.move(baseSpeed);
        motorR.move(baseSpeed);
      }
      
      // 更新位置
      robotX += 0.05 * cos(angle);
    }
  } else {
    // 未到窗口节点，沿当前路径行驶
    motorL.move(0.4);
    motorR.move(0.4);
    robotX += 0.05;
  }
  
  delay(50);
}

6、户外巡检机器人（滚动窗口地形动态适应）
适用场景：户外变电站、工业园区等场景，BLDC驱动的巡检机器人需穿越碎石、沟渠等动态变化的地形（如临时堆放的设备、积水区域），同时避开移动的工作人员和车辆，需实时根据地形变化调整路径。

核心逻辑：
滚动窗口地形感知：通过超声波阵列和GPS实时感知窗口内的地形数据（障碍物类型、可通行区域），构建局部地形图；
动态障碍物+地形融合处理：将移动目标与不可通行地形（如深沟、湿滑地面）统一标记为障碍物，按优先级处理；
滚动窗口路径优化：用遗传算法在窗口内搜索兼顾“距离短+地形平坦+避开障碍物”的路径，每5秒更新一次路径，适应地形和障碍物的动态变化。

#include <SimpleFOC.h>
#include <vector>
#include <cmath>
#include <algorithm>

// 户外参数
#define WINDOW_SIZE 10.0  // 滚动窗口半径(m)
#define PLAN_CYCLE 5000   // 重规划周期(ms)
#define GPS_BAUD 9600
#define ULTRASONIC_COUNT 4 // 超声波数量（前/后/左/右）

// BLDC电机控制
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(12,13,14,15);

// 地形与障碍物结构
struct TerrainObstacle {
  float x, y;       // 位置
  int type;         // 0:移动障碍物（人/车） 1:不可通行地形（深沟） 2:可通行但需减速（碎石）
  float priority;   // 避让优先级（数值越高越优先）
};

// 滚动窗口地形规划器
class OutdoorRollingPlanner {
public:
  OutdoorRollingPlanner() : windowCenterX(0), windowCenterY(0) {}
  
  // 更新窗口中心（机器人当前位置）
  void updateWindowCenter(float x, float y) {
    windowCenterX = x;
    windowCenterY = y;
  }
  
  // 动态路径规划（窗口内）
  std::vector<std::pair<float, float>> planTerrainAwarePath(std::vector<TerrainObstacle> obstacles) {
    std::vector<std::pair<float, float>> localPath;
    // 目标点：全局路径在窗口内的下一个点
    std::pair<float, float> target = getGlobalTargetInWindow();
    if (target.first == INVALID_POS) return localPath;
    
    // 1. 构建窗口内网格地图（简化为10x10的网格，每个格子0.5m）
    const int GRID_SIZE = 20; // 窗口直径20格，每格0.5m，总窗口10m
    bool gridObstacle[GRID_SIZE][GRID_SIZE] = {false};
    
    for (auto obs : obstacles) {
      int gx = (obs.x - windowCenterX) / 0.5 + GRID_SIZE/2;
      int gy = (obs.y - windowCenterY) / 0.5 + GRID_SIZE/2;
      if (gx >=0 && gx < GRID_SIZE && gy >=0 && gy < GRID_SIZE) {
        gridObstacle[gx][gy] = true;
      }
    }
    
    // 2. 简化遗传算法搜索路径（起点为网格中心，终点为目标点对应的网格）
    std::vector<std::pair<float, float>> path;
    int startX = GRID_SIZE/2, startY = GRID_SIZE/2;
    int endX = (target.first - windowCenterX) / 0.5 + GRID_SIZE/2;
    int endY = (target.second - windowCenterY) / 0.5 + GRID_SIZE/2;
    
    // 简单BFS搜索避障路径（替代遗传算法，简化实现）
    std::queue<std::pair<int, int>> q;
    std::vector<std<vector<std::pair<int, int>>>> visited(GRID_SIZE, std::vector<std::pair<int, int>>(GRID_SIZE, {-1,-1}));
    q.push({startX, startY});
    visited[startX][startY] = {startX, startY};
    
    int dx[] = {1,-1,0,0};
    int dy[] = {0,0,1,-1};
    while (!q.empty()) {
      auto curr = q.front(); q.pop();
      if (curr.first == endX && curr.second == endY) {
        // 回溯路径
        int x = endX, y = endY;
        while (!(x == startX && y == startY)) {
          auto parent = visited[x][y];
          path.push_back({
            windowCenterX + (x - GRID_SIZE/2)*0.5,
            windowCenterY + (y - GRID_SIZE/2)*0.5
          });
          x = parent.first;
          y = parent.second;
        }
        path.push_back({windowCenterX, windowCenterY});
        std::reverse(path.begin(), path.end());
        return path;
      }
      for (int i=0; i<4; i++) {
        int nx = curr.first + dx[i];
        int ny = curr.second + dy[i];
        if (nx >=0 && nx < GRID_SIZE && ny >=0 && ny < GRID_SIZE && !gridObstacle[nx][ny] && 
            visited[nx][ny].first == -1) {
          visited[nx][ny] = curr;
          q.push({nx, ny});
        }
      }
    }
    // 无有效路径，返回直线路径（需人工干预）
    path.push_back({windowCenterX, windowCenterY});
    path.push_back(target);
    return path;
  }
  
private:
  float windowCenterX, windowCenterY;
  std::pair<float, float> getGlobalTargetInWindow() {
    // 模拟全局目标点（实际从GPS获取）
    float targetX = windowCenterX + 8.0;
    float targetY = windowCenterY;
    if (std::sqrt((targetX - windowCenterX)*(targetX - windowCenterX) + 
                  (targetY - windowCenterY)*(targetY - windowCenterY)) > WINDOW_SIZE) {
      // 调整目标点到窗口边缘
      float ratio = WINDOW_SIZE / std::sqrt(pow(8,2) + pow(0,2));
      targetX = windowCenterX + 8*ratio;
      targetY = windowCenterY + 0*ratio;
    }
    return {targetX, targetY};
  }
};

// 户外感知系统（超声波+GPS）
class OutdoorPerception {
public:
  std::vector<TerrainObstacle> perceive() {
    std::vector<TerrainObstacle> obstacles;
    // 1. 超声波检测周边地形/障碍物
    float frontDist = getUltrasonicDist(0); // 前方
    float leftDist = getUltrasonicDist(1);  // 左侧
    float rightDist = getUltrasonicDist(2); // 右侧
    float backDist = getUltrasonicDist(3);  // 后方
    
    // 模拟障碍物数据（实际需结合GPS位置）
    // 前方移动障碍物（人）
    obstacles.push_back({windowCenterX + 3.0, windowCenterY, 0, 0.9});
    // 左侧不可通行地形（深沟）
    if (leftDist < 0.8) {
      obstacles.push_back({windowCenterX - 2.0, windowCenterY + 1.0, 1, 1.0});
    }
    // 右侧可通行但需减速（碎石）
    if (rightDist < 1.2 && rightDist > 0.8) {
      obstacles.push_back({windowCenterX + 2.0, windowCenterY + 2.0, 2, 0.5});
    }
    return obstacles;
  }
  
  void updateRobotPosition(float x, float y) {
    windowCenterX = x;
    windowCenterY = y;
  }
  
private:
  float windowCenterX = 0, windowCenterY = 0;
  float getUltrasonicDist(int channel) {
    // 模拟超声波返回值（实际需连接超声波传感器）
    int pin = 3 + channel;
    int duration = pulseIn(pin, HIGH);
    return (float)duration / 58.0 / 100.0; // 转换为米（简化）
  }
};

// 全局变量
OutdoorRollingPlanner planner;
OutdoorPerception perception;
std::vector<TerrainObstacle> currentObstacles;

void setup() {
  Serial.begin(115200);
  // 初始化电机
  driverL.voltage_power_supply = 24;
  driverR.voltage_power_supply = 24;
  driverL.init(); driverR.init();
  motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  
  // 初始化超声波引脚
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
}

uint32_t lastPlanTime = 0;
float robotX = 0, robotY = 0;
void loop() {
  // 1. 电机控制循环
  motorL.loopFOC();
  motorR.loopFOC();
  
  // 2. 周期性重规划（每5秒）
  if (millis() - lastPlanTime > PLAN_CYCLE) {
    // 3. 更新机器人位置（模拟GPS）
    robotX += 0.1; // 假设沿x轴前进
    perception.updateRobotPosition(robotX, robotY);
    planner.updateWindowCenter(robotX, robotY);
    
    // 4. 感知动态地形与障碍物
    currentObstacles = perception.perceive();
    
    // 5. 滚动窗口内动态重规划
    std::vector<std::pair<float, float>> localPath = planner.planTerrainAwarePath(currentObstacles);
    
    // 6. 路径跟踪与控制
    if (!localPath.empty() && localPath.size() >= 2) {
      float targetX = localPath[1].first;
      float targetY = localPath[1].second;
      float dx = targetX - robotX;
      float dy = targetY - robotY;
      float angle = atan2(dy, dx);
      
      // 根据地形类型调整速度（碎石路减速）
      bool isRough = false;
      for (auto obs : currentObstacles) {
        if (obs.type == 2) {
          float dist = std::sqrt((obs.x - robotX)*(obs.x - robotX) + (obs.y - robotY)*(obs.y - robotY));
          if (dist < 2.0) isRough = true;
        }
      }
      float baseSpeed = isRough ? 0.2 : 0.5;
      
      // 差速转向
      if (fabs(angle) > 5 * PI/180) { // 角度大于5度时转向
        float turnSpeed = baseSpeed * 0.3;
        if (angle > 0) {
          motorL.move(baseSpeed + turnSpeed);
          motorR.move(baseSpeed - turnSpeed);
        } else {
          motorL.move(baseSpeed - turnSpeed);
          motorR.move(baseSpeed + turnSpeed);
        }
      } else {
        motorL.move(baseSpeed);
        motorR.move(baseSpeed);
      }
    } else {
      motorL.move(0.5);
      motorR.move(0.5);
    }
    
    lastPlanTime = millis();
  }
  
  delay(10);
}

要点解读

1. 滚动窗口的动态约束：平衡实时性与规划质量
   滚动窗口的核心是通过空间/时间约束缩小规划范围，解决BLDC机器人算力有限与动态场景规划需求的矛盾，需把握两个关键约束：
   空间约束：窗口需以机器人当前位置为中心，范围需匹配机器人的制动距离与感知半径，如室内机器人窗口半径取5m（制动距离约3m，预留2m安全余量），户外机器人取10m（适应复杂地形制动需求）；
   时间约束：重规划周期需匹配动态障碍物的运动速度，如行人移动缓慢（周期1-2s），车辆/移动AGV速度快（周期0.5-1s），户外地形变化慢（周期5-10s），避免因规划频率过高导致算力不足，或过低导致避障不及时。
   同时，窗口内的路径需与全局路径“局部衔接”，确保规划结果不偏离整体目标，如案例1中窗口路径从全局路径裁剪而来，案例2中窗口节点沿全局虚拟节点延伸。

2. 动态障碍物的多维感知：从“定位”到“意图预测”
   仅获取障碍物的静态位置不足以应对动态场景，需实现“位置-速度-类型-意图”的多维感知与预测，为滚动窗口重规划提供完整输入：
   感知维度扩展：除位置外，需通过编码器、UWB、雷达等传感器获取障碍物的速度（如案例4的行人速度、案例5的AGV速度），并标记障碍物类型（移动目标/固定地形/临时障碍物），按优先级排序（不可通行地形优先级最高，移动目标次之）；
   轨迹预测：基于运动模型预测障碍物在滚动窗口内的未来位置，如案例4用线性运动模型预测行人1秒内的位置，避免仅依赖当前位置导致避障滞后，预测时间需与窗口周期匹配（通常等于重规划周期）；
   感知融合：整合多传感器数据提升感知鲁棒性，如案例3融合超声波（近距离地形）与GPS（全局位置），避免单一传感器失效，核心是剔除异常数据（如超声波误判、GPS漂移）。

3. 滚动窗口重规划算法：轻量化与实时性的平衡
   BLDC机器人的MCU算力有限，无法运行全局路径规划算法（如A*、Dijkstra的全地图版本），需设计轻量化局部重规划算法，核心是“降维+简化”：
   降维规划：将全局路径的全维度规划转化为窗口内的局部维度规划，如案例4仅在窗口内裁剪全局路径片段，案例2仅在相邻窗口节点间搜索路径，大幅降低计算量；
   算法简化：避免复杂优化算法，优先选择轻量搜索算法，如案例1用点到线段距离判断冲突，案例6用BFS搜索网格路径（替代复杂遗传算法），确保在Arduino（16MHz主频）上可在1秒内完成一次规划；
   增量更新：利用上一次规划结果作为基础，仅对冲突部分进行局部调整，而非全量重新规划，如案例2仅调整冲突窗口节点，避免重复计算非冲突区域的路径。

4. 感知-规划-控制的闭环延迟控制
   动态场景的核心风险是感知-规划-控制的延迟超标，导致机器人响应滞后，需构建低延迟闭环，关键措施包括：
   并行任务分离：将电机FOC控制、传感器数据采集、路径规划分配到不同优先级的中断或任务，如电机FOC控制需高频执行（1kHz，用中断保障），规划任务低频执行（1-5Hz，用定时器触发），避免相互阻塞；
   数据缓冲与预处理：对传感器数据进行缓冲和预处理，如案例1对雷达数据进行点云聚类、案例3对超声波数据滤波，减少规划任务的数据处理时间；
   控制指令平滑过渡：规划结果切换时避免速度突变，通过加减速曲线平滑指令，如案例3在碎石地形减速时，采用线性减速（而非直接切换速度），防止因路径突变导致机器人侧翻或电机过载。

5. 动态场景下的鲁棒性设计：应对不确定性
   户外/工业场景存在大量不确定性（如传感器噪声、障碍物运动突变、地形打滑），需通过鲁棒性设计避免系统失效，核心策略包括：
   不确定性边界保留：规划时为障碍物预留安全距离边界，如案例4预留0.8m的行人安全距离，案例5预留1m的AGV安全距离，边界大小需大于传感器测量误差（如激光雷达误差±0.1m，边界取0.3m以上）；
   故障降级与人工干预：当规划无有效路径时，自动切换到安全降级模式（如停车、低速直行），并通过指示灯、通信模块报警，如案例3无有效路径时返回直线路径，同时触发蜂鸣器提示人工干预；
   多模态容错感知：对关键感知进行冗余设计，如户外场景同时使用超声波（近距离）和GPS（远距离），当GPS信号丢失时，切换为惯性导航+超声波感知，保障机器人仍能完成局部路径规划，避免完全失控。

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