基于Arduino与BLDC（无刷直流电机）构建的室内服务机器人，其“滚动窗口路径规划+动态避让行人”系统是一套典型的分层闭环控制架构。该方案将宏观的全局引导与微观的局部避障相结合，并依赖BLDC的高动态响应能力来执行复杂的运动指令。
以下是该系统的核心技术拆解：

主要特点

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

应用场景

1. 人机协作仓储与物流AMR
   在电商仓库或柔性制造车间，人员和叉车会随时横穿机器人通道。滚动窗口重规划能确保AMR在高速运行中，对突然出现的动态障碍物进行平滑减速绕行，并在安全后迅速恢复原速，保障物流效率与人员安全。
2. 服务与导览机器人
   在商场、医院、酒店等复杂且人流密集的室内环境中，机器人需要频繁应对突然停步的顾客或横穿的宠物。该系统能提供极其平滑的避让体验，避免急刹或剧烈转向带来的乘客不适感或物品倾覆风险。
3. 室外非结构化环境巡检
   在园区安防或农业巡检中，机器人会遇到落叶、小动物或临时施工路障。滚动窗口机制允许机器人根据局部地形实时调整轨迹，而无需每次都触发庞大的全局重算，提升了户外复杂环境下的通行效率。
4. ROS导航算法验证与科研
   该系统常被用作验证ROS（机器人操作系统）导航算法的硬件平台，特别是在测试base_local_planner中的dwa_local_planner插件时，能够直观地观察和调优局部规划参数。

注意事项

1. 传感器选型与融合
   激光雷达（LiDAR）： 是构建高精度局部代价地图的核心，提供360°点云数据，但成本较高。
   超声波/红外： 成本低，适合作为近距离防撞的补充，但易受环境干扰，需进行多传感器数据融合以消除盲区。
2. 动力学参数标定
   DWA算法的效果高度依赖于机器人运动学参数的准确性。必须精确标定BLDC底盘的最大线速度、最大角速度、最大加速度和最大减速度。参数偏差会导致规划出的轨迹在实际执行时发生碰撞或偏离。
3. 计算性能瓶颈
   虽然滚动窗口降低了计算量，但在Arduino（特别是基础款Uno/Nano）上运行复杂的DWA算法仍具挑战。建议优先选用ESP32或Arduino Due等具备更高主频和更大RAM的高性能微控制器，以保证控制周期的实时性。
4. 评价函数的权重调优
   DWA的核心在于多目标评价函数（包含朝向、距离、速度等）。在实际应用中，需要根据具体场景反复调试各项指标的权重系数，以在“追求效率”和“保证安全”之间找到最佳平衡点。

1、滚动窗口局部感知与路径重规划触发
此案例聚焦于滚动窗口的感知与重规划触发机制。机器人维护以自身为中心、半径2米的局部窗口，当超声波检测到窗口内出现新障碍（含行人）时，将障碍位置映射到局部地图并触发重规划。这种“只关注眼前”的策略，使Arduino能以10Hz以上的频率响应环境变化。

#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)
#define CELL_SIZE 0.2              // 栅格分辨率(m)

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;

    // 检测到新障碍 → 估算位置并标记
    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()) {
        float targetX = globalPath[pathIdx].first;
        float targetY = globalPath[pathIdx].second;
        float dist = sqrt(pow(targetX - robotX, 2) + pow(targetY - robotY, 2));

        if (dist < 0.15) pathIdx++;
        dwaControl(targetX, targetY);
    }
    delay(50);
}

关键逻辑：滚动窗口的核心是“以机器人为中心”的局部感知。全局地图在动态环境中会迅速过时，而局部窗口实时更新，内存占用和计算量大幅降低。参考学术研究中“在每一滚动窗口都能找到适当的局部子目标”的思路，这种策略使有限算力下实现动态避障成为可能。

2、动态障碍物差异化代价权重与弹性带变形
此案例引入差异化代价权重与弹性带路径变形。将行人（动态障碍）与墙壁（静态障碍）赋予不同的避让优先级，动态障碍的斥力权重更高。路径以弹性带形式存在，受障碍斥力推开的同时受内部张力拉直，无需全局重算即可实现平滑绕行。

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

std::vector<Obstacle> localObstacles;

// ===== 弹性带路径点 =====
struct Pose { float x, y; };
std::vector<Pose> elasticPath;
const int PATH_LENGTH = 20;

// 弹性带参数
const float TENSION = 0.3;       // 内部张力（拉直路径）
const float REPULSION = 2.0;     // 障碍斥力系数
const float DAMPING = 0.8;       // 阻尼系数
const float OBS_RANGE = 0.5;     // 障碍影响范围(m)

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

    // 静态障碍物
    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});  // 动态障碍半径更大
    }
}

// ===== 弹性带变形（每步执行）=====
void deformElasticBand() {
    float dF = sonarF.ping_cm() / 100.0;
    float dL = sonarL.ping_cm() / 100.0;
    float dR = sonarR.ping_cm() / 100.0;

    for(int i=1; i<PATH_LENGTH-1; i++) {
        float fx = 0, fy = 0;

        // 内部张力：拉直路径
        fx += TENSION * (elasticPath[i-1].x - 2*elasticPath[i].x + elasticPath[i+1].x);
        fy += TENSION * (elasticPath[i-1].y - 2*elasticPath[i].y + elasticPath[i+1].y);

        // 前方障碍斥力（动态障碍权重更高）
        if(dF < OBS_RANGE && i > PATH_LENGTH/3) {
            float weight = 1.5;  // 行人权重
            float repForce = REPULSION * weight * (1 - dF/OBS_RANGE);
            if(dL > dR) fx += repForce * 0.5;
            else        fx -= repForce * 0.5;
            fy -= repForce * 0.3;
        }

        // 左右障碍斥力
        if(dL < OBS_RANGE) fx += REPULSION * (1 - dL/OBS_RANGE);
        if(dR < OBS_RANGE) fx -= REPULSION * (1 - dR/OBS_RANGE);

        // 带阻尼更新
        elasticPath[i].x += fx * DAMPING;
        elasticPath[i].y += fy * DAMPING;
    }

    // 平滑滤波：限制曲率突变
    for(int iter=0; iter<3; iter++) {
        for(int i=2; i<PATH_LENGTH-2; i++) {
            elasticPath[i].x = 0.25*elasticPath[i-1].x + 0.5*elasticPath[i].x + 0.25*elasticPath[i+1].x;
            elasticPath[i].y = 0.25*elasticPath[i-1].y + 0.5*elasticPath[i].y + 0.25*elasticPath[i+1].y;
        }
    }
}

关键逻辑：弹性带变形是“力导向”的避障方式——障碍物产生斥力推开路径点，相邻点间的张力将路径拉直。行人（动态障碍）的斥力权重高于墙壁（静态障碍），实现“优先避人”的智能策略。这种方法无需全局重规划，计算量轻，非常适合Arduino平台。动态障碍物与静态障碍物的差异化处理，使机器人能够“感知”到行人的特殊性和避让优先级。

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评价函数（动态障碍权重更高）=====
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主控制循环 =====
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;
}

关键逻辑：改进DWA在传统DWA基础上引入了动态障碍代价权重和滚动窗口运动趋势约束。动态窗口根据当前速度和加速度限制采样范围，避免机器人运动突变。在室内服务场景中，DWA的短时轨迹推演能力使其能够对行人运动做出预判——通过评估未来1.5秒内与行人的距离变化，选择远离行人的速度轨迹。BLDC配合FOC的毫秒级响应，确保DWA输出的连续速度指令能被精准执行。

要点解读

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

全局地图在动态环境中会迅速过时，而滚动窗口机制仅维护机器人周围一定半径内的局部环境模型，大幅降低内存占用和计算负荷，使Arduino/ESP32等嵌入式平台能以高频（10Hz以上）处理局部环境变化。这种“只关注眼前”的策略，是有限算力下实现动态避障的工程标准。参考学术研究，规划出的路径不仅要求短，还要远离障碍物，因此在滚动窗口内需兼顾路径长度与安全距离。

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

行人（动态障碍）与墙壁（静态障碍）的避让紧迫性不同。在DWA评价函数或弹性带斥力计算中，动态障碍应赋予更高的权重（如2倍），引导规划器优先选择远离移动物体的轨迹。这种差异化处理让机器人实现“优先避人、其次避墙”的智能行为。在室内服务场景中，行人的运动具有不确定性，提高其避让权重是安全性的基本保障。

3. 弹性带变形是轻量级动态避障的有效手段

弹性带将路径视为可弹性变形的橡皮筋：障碍物产生斥力推开路径点，相邻点间的张力将路径拉直。这种方法无需全局重规划，每帧仅更新局部路径点的位置，计算量极轻。动态障碍物的斥力权重高于静态障碍物，实现差异化避让。对于Arduino平台，弹性带是比完整DWA更轻量的替代方案，尤其在障碍物稀疏的走廊环境中表现良好。

4. BLDC+FOC是DWA连续速度指令精准执行的保障

DWA输出的是连续的速度向量（v, w）和角速度，要求执行机构具备高动态响应能力。BLDC配合FOC算法可实现扭矩和转速的毫秒级精确调节，低转速下转矩平滑，完美契合DWA的连续速度指令。SimpleFOC库的MotionControlType::velocity模式是实现速度闭环的标准方法。若使用开环驱动，地面摩擦或负载变化会直接导致速度波动，使DWA的规划轨迹与实际运动产生偏差。

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

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

4、商场服务机器人（滚动窗口跟随+人群静态避让）
适用场景：商场引导、咨询机器人，需沿预设的“点到点路径”（如入口→服务台→出口）行驶，遇静止/慢速移动的人群（如顾客驻足、排队）时，在滚动窗口内动态调整路径，避免全路径重算。

核心逻辑：
滚动窗口触发：每2秒生成以机器人为中心、半径3m的局部规划窗口，仅在窗口内叠加全局路径片段；
行人感知简化：通过激光雷达检测窗口内的静态/慢速行人，标记为障碍物；
局部路径避让：若障碍物与全局路径重叠，在窗口内用“侧移绕行”策略生成新路径，优先保证路径连续性。

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

// 硬件配置
#define LIDAR_RX_PIN 3  // 雷达数据接收引脚
#define MOTOR_L_PIN 5   // 左轮BLDC驱动引脚
#define MOTOR_R_PIN 6   // 右轮BLDC驱动引脚
#define WINDOW_RADIUS 3.0f  // 滚动窗口半径（m）
#define PLAN_INTERVAL 2000   // 重规划周期（ms）

// BLDC电机对象
BLDCMotor motorL, motorR;
BLDCDriver3PWM driverL(9, 10, 11, MOTOR_L_PIN);
BLDCDriver3PWM driverR(12, 13, 14, MOTOR_R_PIN);

// 全局路径（预设：入口→服务台→出口，直线+折线）
std::vector<std::pair<float, float>> globalPath = {
  {0.0f, 0.0f},   // 入口
  {5.0f, 0.0f},   // 服务台（沿x轴直行5m）
  {5.0f, 3.0f},   // 出口（y轴直行3m）
  {10.0f, 3.0f}
};

// 滚动窗口规划器
class RollingWindowPlanner {
public:
  RollingWindowPlanner(float radius) : windowRadius(radius), robotX(0), robotY(0) {}

  // 获取窗口内的全局路径片段（核心：裁剪全局路径到窗口）
  std::vector<std::pair<float, float>> getWindowPath() {
    std::vector<std::pair<float, float>> localPath;
    for (size_t i = 0; i < globalPath.size(); i++) {
      float dx = globalPath[i].first - robotX;
      float dy = globalPath[i].second - robotY;
      if (sqrt(dx*dx + dy*dy) <= windowRadius) {
        localPath.push_back(globalPath[i]);
      }
    }
    return localPath;
  }

  // 动态避让行人（静态避让策略）
  std::vector<std::pair<float, float>> replan(std::vector<std::pair<float, float>> obstacles) {
    std::vector<std::pair<float, float>> localPath = getWindowPath();
    if (localPath.empty()) return localPath;

    // 1. 判断障碍物是否与路径冲突（障碍物到路径的距离<0.5m）
    bool needDetour = false;
    for (auto& obs : obstacles) {
      if (isConflict(obs, localPath)) {
        needDetour = true;
        break;
      }
    }

    // 2. 冲突时生成侧移绕行路径（窗口内避让）
    if (needDetour) {
      // 计算路径方向，左侧绕行
      float dx = localPath[1].first - robotX;
      float dy = localPath[1].second - robotY;
      float angle = atan2(dy, dx);
      float detourX = robotX + cos(angle) * 0.5f;
      float detourY = robotY + sin(angle) * 0.5f + 0.8f; // 侧移0.8m
      localPath.insert(localPath.begin() + 1, {detourX, detourY});
    }

    return localPath;
  }

  void updateRobotPos(float x, float y) {
    robotX = x;
    robotY = y;
  }

private:
  float windowRadius;
  float robotX, robotY;

  // 点到线段的距离（判断障碍物是否与路径冲突）
  float pointToLineDist(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 sqrt(dx*dx + dy*dy);
  }

  bool isConflict(std::pair<float, float> obs, std::vector<std::pair<float, float>> path) {
    if (path.size() < 2) return false;
    float dist = pointToLineDist(robotX, robotY, path[1].first, path[1].second, obs.first, obs.second);
    return dist < 0.5f; // 安全距离阈值0.5m
  }
};

// 激光雷达行人检测（简化：解析180度范围内障碍物）
class LidarDetector {
public:
  std::vector<std::pair<float, float>> detect() {
    std::vector<std::pair<float, float>> obstacles;
    // 模拟雷达数据（实际需解析RPLiDAR协议：如串口输出角度+距离）
    // 此处模拟：每30度一个障碍物，若距离<3m则视为行人
    for (int angle = 0; angle < 180; angle += 30) {
      float rad = angle * M_PI / 180.0f;
      float dist = 2.5f; // 模拟行人距离
      if (dist < WINDOW_RADIUS) {
        obstacles.push_back({
          robotX + dist * cos(rad),
          robotY + dist * sin(rad)
        });
      }
    }
    return obstacles;
  }

  void updateRobotPos(float x, float y) {
    robotX = x;
    robotY = y;
  }

private:
  float robotX = 0, robotY = 0;
};

// 全局对象
RollingWindowPlanner planner(WINDOW_RADIUS);
LidarDetector detector;
uint32_t lastPlanTime = 0;
float robotX = 0, robotY = 0;

void setup() {
  Serial.begin(115200);
  // 初始化电机
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  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();
  Serial.println("电机与雷达初始化完成");
}

void loop() {
  // 1. 电机控制循环（必须高频执行，保障电机闭环）
  motorL.loopFOC();
  motorR.loopFOC();

  // 2. 周期性执行滚动窗口重规划
  if (millis() - lastPlanTime > PLAN_INTERVAL) {
    // 3. 更新机器人位置（编码器推算：假设100脉冲/m，累计脉冲计算位置）
    static long pulseCount = 0;
    pulseCount += 10; // 模拟前进0.1m（简化编码器读取）
    robotX = pulseCount / 1000.0f; // 每1000脉冲=1m
    planner.updateRobotPos(robotX, robotY);
    detector.updateRobotPos(robotX, robotY);

    // 4. 检测窗口内的行人障碍物
    std::vector<std::pair<float, float>> obstacles = detector.detect();

    // 5. 滚动窗口内重规划（避让行人）
    std::vector<std::pair<float, float>> localPath = planner.replan(obstacles);

    // 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);
      float speed = 0.3f; // 基础速度0.3m/s

      // 差速转向（角度>5度时转向）
      if (abs(angle) > 5 * M_PI / 180) {
        float turnSpeed = speed * 0.2f;
        if (angle > 0) {
          motorL.move(speed + turnSpeed);
          motorR.move(speed - turnSpeed);
        } else {
          motorL.move(speed - turnSpeed);
          motorR.move(speed + turnSpeed);
        }
      } else {
        motorL.move(speed);
        motorR.move(speed);
      }

      // 更新位置（模拟编码器计数）
      robotX += 0.05f * cos(angle);
      robotY += 0.05f * sin(angle);
    }

    lastPlanTime = millis();
  } else {
    // 非规划周期：沿当前路径直行
    motorL.move(0.3f);
    motorR.move(0.3f);
  }

  delay(10);
}

5、医院配送机器人（滚动窗口动态避让+行人轨迹预测）
适用场景：医院病房配送药品/餐食，需在走廊等狭窄空间行驶，遇快速移动的行人（如医护人员、推车家属）时，需预测行人轨迹，提前在滚动窗口内调整路径，避免碰撞。

核心逻辑：
行人轨迹预测：通过激光雷达获取行人的连续位置，计算速度（位置差/时间差），用线性模型预测行人未来1秒的轨迹；
滚动窗口冲突预判：在窗口内判断预测轨迹与机器人路径的冲突，若有冲突，生成“提前避让+等待”策略；
动态速度调整：根据行人距离动态调整机器人速度，距离近时减速，冲突时停车等待。

#include <SimpleFOC.h>
#include <vector>
#include <cmath>
#include <map>
#include <Arduino.h>

// 硬件配置
#define LIDAR_RX_PIN 3
#define ENCODER_L_A 2  // 左轮编码器A相
#define ENCODER_L_B 4  // 左轮编码器B相
#define ENCODER_R_A 7  // 右轮编码器A相
#define ENCODER_R_B 8  // 右轮编码器B相
#define WINDOW_RADIUS 4.0f
#define PLAN_INTERVAL 1000
#define SAFE_DIST 0.8f  // 行人安全距离阈值

// BLDC电机对象（带编码器）
BLDCMotor motorL, motorR;
BLDCDriver3PWM driverL(9, 10, 11, 5);
BLDCDriver3PWM driverR(12, 13, 14, 6);
// 编码器对象（简化，实际需使用InterruptIn）
volatile long encoderL = 0, encoderR = 0;

// 行人追踪结构
struct Pedestrian {
  float x, y;       // 当前位置
  float vx, vy;     // 速度（m/s）
  uint32_t lastTime; // 最后更新时间
  bool isMoving;    // 是否移动
};

// 滚动窗口规划器（含行人轨迹预测）
class DynamicRollingPlanner {
public:
  DynamicRollingPlanner(float radius) : windowRadius(radius), robotX(0), robotY(0) {}

  // 更新行人状态（轨迹预测）
  std::vector<Pedestrian> updatePedestrians(std::vector<Pedestrian> pedestrians, std::vector<std::pair<float, float>> newObs) {
    for (auto& obs : newObs) {
      bool found = false;
      for (auto& p : pedestrians) {
        float dx = p.x - obs.first, dy = p.y - obs.second;
        if (sqrt(dx*dx + dy*dy) < 0.3f) { // 匹配已有行人
          float dt = (millis() - p.lastTime) / 1000.0f;
          if (dt > 0) {
            p.vx = (obs.first - p.x) / dt;
            p.vy = (obs.second - p.y) / dt;
          }
          p.x = obs.first; p.y = obs.second;
          p.lastTime = millis();
          p.isMoving = sqrt(p.vx*p.vx + p.vy*p.vy) > 0.1f;
          found = true;
          break;
        }
      }
      if (!found) {
        Pedestrian newP = {obs.first, obs.second, 0, 0, millis(), false};
        pedestrians.push_back(newP);
      }
    }
    return pedestrians;
  }

  // 预测行人未来位置（线性模型）
  std::pair<float, float> predictPosition(Pedestrian p, uint32_t dt_ms) {
    float dt = dt_ms / 1000.0f;
    return {p.x + p.vx * dt, p.y + p.vy * dt};
  }

  // 滚动窗口重规划（动态避让）
  std::vector<std::pair<float, float>> replan(std::vector<Pedestrian> pedestrians, std::vector<std::pair<float, float>> globalPath) {
    std::vector<std::pair<float, float>> localPath;
    if (globalPath.empty()) return localPath;

    // 1. 裁剪全局路径到窗口
    for (auto& p : globalPath) {
      float dx = p.first - robotX, dy = p.second - robotY;
      if (sqrt(dx*dx + dy*dy) <= windowRadius) {
        localPath.push_back(p);
      }
    }
    if (localPath.empty()) return localPath;

    // 2. 预测行人轨迹，判断是否与路径冲突
    bool needWait = false;
    std::pair<float, float> collidePos = {0, 0};
    for (auto& p : pedestrians) {
      if (!p.isMoving) continue;
      // 预测未来1秒的位置
      auto predPos = predictPosition(p, 1000);
      // 判断预测位置是否在路径上
      if (isOnPath(predPos, localPath)) {
        float dist = sqrt(pow(predPos.first - robotX, 2) + pow(predPos.second - robotY, 2));
        if (dist < SAFE_DIST * 2) {
          needWait = true;
          collidePos = predPos;
          break;
        }
      }
    }

    // 3. 冲突时生成等待路径（停车避让）
    if (needWait) {
      localPath.clear();
      localPath.push_back({robotX, robotY});
      // 待避让后重新加入全局路径
      localPath.insert(localPath.end(), globalPath.begin(), globalPath.end());
    }

    return localPath;
  }

  void updateRobotPos(float x, float y) {
    robotX = x;
    robotY = y;
  }

private:
  float windowRadius;
  float robotX, robotY;

  // 判断点是否在路径线段上
  bool isOnPath(std::pair<float, float> pos, std::vector<std::pair<float, float>> path) {
    if (path.size() < 2) return false;
    for (size_t i = 0; i < path.size() - 1; i++) {
      float x1 = path[i].first, y1 = path[i].second;
      float x2 = path[i+1].first, y2 = path[i+1].second;
      float dx = x2 - x1, dy = y2 - y1;
      float len = sqrt(dx*dx + dy*dy);
      if (len == 0) continue;
      float t = ((pos.first - x1)*dx + (pos.second - y1)*dy) / (len*len);
      if (t >= 0 && t <= 1) {
        float projX = x1 + t*dx, projY = y1 + t*dy;
        float dist = sqrt(pow(pos.first - projX, 2) + pow(pos.second - projY, 2));
        if (dist < 0.3f) return true;
      }
    }
    return false;
  }
};

// 激光雷达行人检测（多目标跟踪）
class LidarTracker {
public:
  std::vector<std::pair<float, float>> track() {
    std::vector<std::pair<float, float>> obstacles;
    // 实际解析：RPLiDAR输出角度+距离（如0-360度，每0.5度一个点）
    // 简化：提取快速移动的目标（速度>0.5m/s视为行人）
    for (int angle = 0; angle < 180; angle += 10) {
      float rad = angle * M_PI / 180.0f;
      float dist = 2.0f + sin(millis()/1000.0f) * 0.5f; // 模拟移动行人
      if (dist < WINDOW_RADIUS) {
        obstacles.push_back({
          robotX + dist * cos(rad),
          robotY + dist * sin(rad)
        });
      }
    }
    return obstacles;
  }

  void updateRobotPos(float x, float y) {
    robotX = x;
    robotY = y;
  }

private:
  float robotX = 0, robotY = 0;
};

// 全局对象
DynamicRollingPlanner planner(WINDOW_RADIUS);
LidarTracker tracker;
std::vector<Pedestrian> pedestrians;
uint32_t lastPlanTime = 0;
float robotX = 0, robotY = 0;

// 编码器中断（简化示例，实际需配置外部中断）
void IRAM_ATTR encodeLISR() {
  encoderL++;
}
void IRAM_ATTR encodeRISR() {
  encoderR++;
}

void setup() {
  Serial.begin(115200);
  // 初始化电机
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  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(ENCODER_L_A, INPUT_PULLUP);
  pinMode(ENCODER_L_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), encodeLISR, RISING);
  // 同理配置右轮编码器（实际需AB相正交解码）
  Serial.println("初始化完成");
}

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

  // 1. 从编码器读取位置（100脉冲/m）
  float moveDist = (encoderL + encoderR) / 2 / 100.0f;
  robotX += moveDist * cos(0); // 假设沿x轴直行（实际需航向推算）
  encoderL = 0; encoderR = 0;

  // 2. 周期性重规划（快速响应，1秒一次）
  if (millis() - lastPlanTime > PLAN_INTERVAL) {
    planner.updateRobotPos(robotX, robotY);
    tracker.updateRobotPos(robotX, robotY);

    // 3. 检测行人
    auto obstacles = tracker.track();

    // 4. 更新行人轨迹
    pedestrians = planner.updatePedestrians(pedestrians, obstacles);

    // 5. 全局路径（预设配送路径）
    std::vector<std::pair<float, float>> globalPath = {
      {0.0f, 0.0f}, {10.0f, 0.0f}
    };

    // 6. 滚动窗口重规划
    auto localPath = planner.replan(pedestrians, globalPath);

    // 7. 控制逻辑：冲突时停车，否则跟踪路径
    if (localPath.empty() || localPath.size() < 2) {
      motorL.move(0);
      motorR.move(0);
    } else {
      float targetX = localPath[1].first, targetY = localPath[1].second;
      float dx = targetX - robotX, dy = targetY - robotY;
      float angle = atan2(dy, dx);
      float speed = 0.2f;

      // 动态避让行人：若预测行人距离<1m，减速至0.1m/s
      bool nearPedestrian = false;
      for (auto& p : pedestrians) {
        auto predPos = planner.predictPosition(p, 1000);
        float dist = sqrt(pow(predPos.first - robotX, 2) + pow(predPos.second - robotY, 2));
        if (dist < SAFE_DIST) {
          nearPedestrian = true;
          break;
        }
      }
      speed = nearPedestrian ? 0.1f : 0.2f;

      // 差速转向
      if (abs(angle) > 3 * M_PI / 180) {
        float turnSpeed = speed * 0.15f;
        if (angle > 0) {
          motorL.move(speed + turnSpeed);
          motorR.move(speed - turnSpeed);
        } else {
          motorL.move(speed - turnSpeed);
          motorR.move(speed + turnSpeed);
        }
      } else {
        motorL.move(speed);
        motorR.move(speed);
      }
    }

    lastPlanTime = millis();
  }

  delay(10);
}

6、餐厅送餐机器人（滚动窗口拥挤避让+多行人协同规划）
适用场景：餐厅送餐场景，需穿越拥挤的餐桌与行人区域，遇多行人交叉、排队、突然转身等复杂动态场景，需在滚动窗口内协调避让多个行人，保证送餐效率。

核心逻辑：
滚动窗口拥挤区域建模：将窗口划分为网格，统计每个网格的行人密度，标记为“拥挤区域”；
多行人避让优先级：根据行人速度（速度快的优先级高）、距离（距离近的优先级高）确定避让顺序；
窗口内最优避让路径：用轻量化的A*算法在网格内搜索，优先避开拥挤区域，选择路径短、转向少的路线。

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

// 硬件配置
#define CAM_TX_PIN 3  // 深度相机串口发送
#define CAM_RX_PIN 4  // 深度相机串口接收
#define WINDOW_RADIUS 5.0f
#define GRID_SIZE 10  // 窗口网格数（每格0.5m，总窗口5m）
#define PLAN_INTERVAL 800

// BLDC电机
BLDCMotor motorL, motorR;
BLDCDriver3PWM driverL(9, 10, 11, 5);
BLDCDriver3PWM driverR(12, 13, 14, 6);

// 网格与障碍物结构
struct GridNode {
  int x, y;
  bool obstacle;
  int gCost, hCost, fCost; // A*算法代价
  GridNode* parent;
  GridNode(int _x, int _y) : x(_x), y(_y), obstacle(false), gCost(0), hCost(0), fCost(0), parent(nullptr) {}
};

// 滚动窗口拥挤避让规划器（A*网格搜索）
class CrowdAwarePlanner {
public:
  CrowdAwarePlanner(float radius) : windowRadius(radius), robotX(0), robotY(0) {
    // 初始化网格（10x10，每格0.5m，对应5m窗口）
    for (int x = 0; x < GRID_SIZE; x++) {
      for (int y = 0; y < GRID_SIZE; y++) {
        grid[x][y] = new GridNode(x, y);
      }
    }
  }

  // 更新网格障碍物（基于行人密度）
  void updateGrid(std::vector<std::pair<float, float>> pedestrians) {
    // 清空网格障碍物
    for (int x = 0; x < GRID_SIZE; x++) {
      for (int y = 0; y < GRID_SIZE; y++) {
        grid[x][y]->obstacle = false;
        grid[x][y]->gCost = grid[x][y]->hCost = grid[x][y]->fCost = 0;
      }
    }

    // 标记行人所在网格为障碍物
    for (auto& p : pedestrians) {
      int gx = (p.first - robotX + windowRadius) / (windowRadius * 2 / GRID_SIZE);
      int gy = (p.second - robotY + windowRadius) / (windowRadius * 2 / GRID_SIZE);
      if (gx >=0 && gx < GRID_SIZE && gy >=0 && gy < GRID_SIZE) {
        grid[gx][gy]->obstacle = true;
      }
    }

    // 标记拥挤区域（行人密度>2的网格）
    updateCrowdedArea();
  }

  // A*算法搜索避让路径
  std::vector<std::pair<float, float>> aStarPath(std::pair<float, float> start, std::pair<float, float> target) {
    std::vector<std::pair<float, float>> path;
    // 起点网格
    int startGx = (start.first - robotX + windowRadius) / (windowRadius * 2 / GRID_SIZE);
    int startGy = (start.second - robotY + windowRadius) / (windowRadius * 2 / GRID_SIZE);
    // 终点网格
    int endGx = (target.first - robotX + windowRadius) / (windowRadius * 2 / GRID_SIZE);
    int endGy = (target.second - robotY + windowRadius) / (windowRadius * 2 / GRID_SIZE);

    if (startGx < 0 || startGx >= GRID_SIZE || endGx < 0 || endGx >= GRID_SIZE) return path;

    // 优先队列（按fCost排序）
    std::priority_queue<GridNode*, std::vector<GridNode*>, std::function<bool(GridNode*, GridNode*)>> openList(
      [](GridNode* a, GridNode* b) { return a->fCost > b->fCost; }
    );
    bool closedList[GRID_SIZE][GRID_SIZE] = {false};

    GridNode* startNode = grid[startGx][startGy];
    startNode->gCost = 0;
    startNode->hCost = heuristic(startNode, grid[endGx][endGy]);
    startNode->fCost = startNode->gCost + startNode->hCost;
    openList.push(startNode);

    while (!openList.empty()) {
      GridNode* curr = openList.top();
      openList.pop();
      int x = curr->x, y = curr->y;

      if (x == endGx && y == endGy) {
        // 回溯路径
        while (curr != nullptr) {
          path.push_back({
            robotX - windowRadius + curr->x * (windowRadius * 2 / GRID_SIZE) + (windowRadius * 2 / GRID_SIZE)/2,
            robotY - windowRadius + curr->y * (windowRadius * 2 / GRID_SIZE) + (windowRadius * 2 / GRID_SIZE)/2
          });
          curr = curr->parent;
        }
        std::reverse(path.begin(), path.end());
        return path;
      }

      closedList[x][y] = true;

      // 检查相邻节点（8方向）
      int dx[] = {-1, 1, 0, 0, -1, -1, 1, 1};
      int dy[] = {0, 0, -1, 1, -1, 1, -1, 1};
      for (int i = 0; i < 8; i++) {
        int nx = x + dx[i], ny = y + dy[i];
        if (nx <0 || nx >= GRID_SIZE || ny <0 || ny >= GRID_SIZE) continue;
        if (closedList[nx][ny] || grid[nx][ny]->obstacle) continue;

        GridNode* neighbor = grid[nx][ny];
        float newG = curr->gCost + (i < 4 ? 1.0f : 1.414f); // 4方向1，对角线1.414
        if (newG < neighbor->gCost || neighbor->parent == nullptr) {
          neighbor->gCost = newG;
          neighbor->hCost = heuristic(neighbor, grid[endGx][endGy]);
          neighbor->fCost = neighbor->gCost + neighbor->hCost;
          neighbor->parent = curr;
          openList.push(neighbor);
        }
      }
    }
    return path; // 无路径
  }

  void updateRobotPos(float x, float y) {
    robotX = x;
    robotY = y;
  }

private:
  float windowRadius;
  float robotX, robotY;
  GridNode* grid[GRID_SIZE][GRID_SIZE];

  // 启发式代价（曼哈顿距离）
  float heuristic(GridNode* a, GridNode* b) {
    return abs(a->x - b->x) + abs(a->y - b->y);
  }

  // 更新拥挤区域（标记高密度网格为障碍物）
  void updateCrowdedArea() {
    int density[GRID_SIZE][GRID_SIZE] = {0};
    // 统计每个网格的行人数量（简化，实际需行人检测）
    for (int x = 0; x < GRID_SIZE; x++) {
      for (int y = 0; y < GRID_SIZE; y++) {
        if (grid[x][y]->obstacle) density[x][y]++;
      }
    }
    // 密度>2标记为拥挤区域
    for (int x = 0; x < GRID_SIZE; x++) {
      for (int y = 0; y < GRID_SIZE; y++) {
        if (density[x][y] > 2) {
          grid[x][y]->obstacle = true;
        }
      }
    }
  }
};

// 深度相机行人检测（简化：串口接收位置数据）
class DepthCameraDetector {
public:
  std::vector<std::pair<float, float>> detect() {
    std::vector<std::pair<float, float>> pedestrians;
    // 模拟串口接收数据（格式：x,y;x,y;...）
    String data = "1.2,0.5;2.0,1.0;0.8,-0.5;";
    if (Serial.available()) {
      data = Serial.readStringUntil('\n');
    }
    // 解析数据
    int start = 0, end = data.indexOf(';');
    while (end != -1) {
      String point = data.substring(start, end);
      int comma = point.indexOf(',');
      if (comma != -1) {
        float x = point.substring(0, comma).toFloat();
        float y = point.substring(comma+1).toFloat();
        if (sqrt(x*x + y*y) < WINDOW_RADIUS) {
          pedestrians.push_back({x, y});
        }
      }
      start = end + 1;
      end = data.indexOf(';', start);
    }
    return pedestrians;
  }
};

// 全局对象
CrowdAwarePlanner planner(WINDOW_RADIUS);
DepthCameraDetector detector;
uint32_t lastPlanTime = 0;
float robotX = 0, robotY = 0;

void setup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, CAM_RX_PIN, CAM_TX_PIN); // 深度相机串口

  // 初始化电机
  driverL.voltage_power_supply = 12;
  driverR.voltage_power_supply = 12;
  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();

  Serial.println("送餐机器人初始化完成");
}

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

  // 1. 位置更新（假设沿x轴行驶）
  robotX += 0.04f;

  // 2. 周期性重规划（800ms/次，适应拥挤场景）
  if (millis() - lastPlanTime > PLAN_INTERVAL) {
    planner.updateRobotPos(robotX, robotY);

    // 3. 检测多行人
    auto pedestrians = detector.detect();

    // 4. 更新网格障碍物
    planner.updateGrid(pedestrians);

    // 5. 全局路径目标（送餐点）
    std::pair<float, float> target = {10.0f, 0.0f};
    // 窗口内起点（机器人当前位置）
    std::pair<float, float> start = {robotX, robotY};

    // 6. A*算法滚动窗口重规划
    auto localPath = planner.aStarPath(start, target);

    // 7. 控制逻辑：沿A*路径行驶
    if (!localPath.empty() && localPath.size() >= 2) {
      float targetX = localPath[1].first, targetY = localPath[1].second;
      float dx = targetX - robotX, dy = targetY - robotY;
      float angle = atan2(dy, dx);
      float speed = 0.25f;

      // 转向控制（减少急转）
      if (abs(angle) > 10 * M_PI / 180) {
        float turnSpeed = speed * 0.3f;
        if (angle > 0) {
          motorL.move(speed + turnSpeed);
          motorR.move(speed - turnSpeed);
        } else {
          motorL.move(speed - turnSpeed);
          motorR.move(speed + turnSpeed);
        }
      } else if (abs(angle) > 3 * M_PI / 180) {
        float turnSpeed = speed * 0.1f;
        if (angle > 0) {
          motorL.move(speed + turnSpeed);
          motorR.move(speed - turnSpeed);
        } else {
          motorL.move(speed - turnSpeed);
          motorR.move(speed + turnSpeed);
        }
      } else {
        motorL.move(speed);
        motorR.move(speed);
      }
    } else {
      // 无路径时停车
      motorL.move(0);
      motorR.move(0);
    }

    lastPlanTime = millis();
  }

  delay(10);
}

要点解读

1. 滚动窗口的“空间-时间”双约束设计：适配室内服务场景
   滚动窗口是解决Arduino算力瓶颈的核心，需严格匹配室内服务场景的空间范围与时间周期，避免过度规划或规划滞后：
   空间约束：窗口半径需结合室内空间尺度与机器人制动距离，案例中商场/餐厅场景取3-5m（覆盖服务机器人典型制动距离2-3m，预留1-2m安全余量），若窗口过小，无法覆盖全障碍物；过大则算力溢出。
   时间约束：重规划周期需匹配行人移动速度，慢速人群（如商场顾客）取2秒/次，快速人群（如医院医护）取1秒/次，拥挤场景（餐厅）取0.8秒/次，确保在行人移动距离小于安全距离前完成路径调整。
   路径衔接：窗口内路径必须与全局路径局部衔接，案例4-6均通过“裁剪全局路径片段”或“目标点引导”，避免路径脱节，确保机器人始终向目标点行驶。
2. 动态行人的“感知-预测-决策”闭环：从被动避障到主动预判
   室内服务机器人的核心挑战是行人运动的不确定性，需构建从感知到预测再到决策的闭环，而非被动避障：
   感知层：优先选择轻量传感器（激光雷达优于深度相机，深度相机优于双目），激光雷达可实现360°覆盖，适合开阔场景；深度相机适合狭窄拥挤区域，案例3通过深度相机实现行人位置检测，避免激光雷达在密集人群中的遮挡问题。
   预测层：线性轨迹预测是最轻量且有效的方法，通过连续位置差分计算行人速度，预测未来1-2秒的轨迹，案例2通过预测判断行人是否会进入机器人路径，提前做出停车或避让决策，避免仅依赖当前位置导致的碰撞。
   决策层：区分行人状态（静止/慢速/快速），制定差异化策略：静止人群可侧移绕行，快速人群需提前停车避让，拥挤人群需全局避让（案例6），确保避让行为符合人体直觉，提升通行效率。
3. 轻量化算法选型：适配Arduino算力瓶颈
   Arduino（典型16MHz主频）无法运行复杂算法，需围绕“简化、降维、增量”三个原则选型：
   路径规划算法简化：避免D* Lite、RRT等复杂算法，优先选择A（案例3）、BFS（网格搜索）、侧移绕行（案例4），通过网格化将连续空间转化为离散空间，大幅降低计算量；A*算法在10x10网格下，Arduino可在500ms内完成一次搜索，满足实时性要求。
   障碍物处理降维：将3D感知（深度相机）转化为2D位置，将多目标追踪简化为目标匹配（案例2通过距离匹配跟踪行人），避免复杂的点云处理或深度学习模型，降低内存与算力消耗。
   规划结果增量更新：非全量重规划，而是基于上一次路径局部调整，案例1仅在障碍物冲突时插入绕行点，案例5仅在预测冲突时切换停车策略，避免重复计算非冲突区域的路径，大幅提升规划速度。
4. 电机控制与规划周期的同步：保障控制稳定性
   BLDC电机的闭环控制需高频执行，而路径规划需低频执行，二者需通过中断与任务分离实现同步，避免相互阻塞：
   高频控制与低频规划分离：电机FOC控制需每1-2ms执行一次（通过硬件定时器或中断实现），路径规划每0.8-2秒执行一次，案例中通过loop()循环调用motor.loopFOC()保障电机控制频率，同时用millis()定时器触发规划任务，确保电机控制不受规划算力影响。
   控制指令平滑过渡：规划结果切换时，避免速度突变，案例5-6中通过限制转向角度变化率、速度变化幅度（如速度从0.2m/s渐变到0.1m/s），防止因路径突变导致电机过载或机器人侧翻，提升乘坐/配送体验。
   位置反馈与规划的闭环：编码器实时反馈机器人位置，作为规划的输入，案例5通过编码器计算机器人位置，确保窗口中心与机器人实际位置一致，避免因位置估算误差导致规划窗口偏移，保障规划有效性。
5. 室内复杂场景的鲁棒性设计：应对不确定性
   室内服务场景存在行人突然转身、传感器遮挡、光线变化等不确定性，需通过鲁棒性设计避免系统失效：
   安全距离动态预留：根据行人速度调整安全距离，快速行人预留1m以上安全距离，慢速行人预留0.5m，案例2中安全距离与行人速度联动，避免因距离判断不足导致碰撞。
   传感器冗余与补偿：激光雷达遮挡时，切换为深度相机或编码器推算，案例3通过深度相机弥补激光雷达在拥挤区域的遮挡问题，同时通过编码器估算机器人位置，避免GPS在室内失效的问题。
   故障降级与人工干预：当无有效路径或传感器失效时，触发降级策略：停车鸣笛（案例5）、低速直行（案例4）、向服务器发送求助信号，案例6中A*搜索失败时停车，等待人工干预，避免机器人失控，符合室内服务机器人的安全规范。
   动态场景的鲁棒测试：在实际部署前，需测试多类场景：人群交叉、突然转身、密集排队，案例中通过模拟传感器数据测试算法鲁棒性，确保算法在不同场景下均能稳定避让。

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