Arduino BLDC 智能园区配送机器人的核心技术在于：增量式重规划负责在动态环境中快速响应行人、车辆等突发障碍，加速度连续平滑负责将路径指令转化为符合 BLDC 运动学约束的连续轨迹，两者协同实现“实时安全避障 + 平稳高效配送”。
一、系统架构与核心原理
该系统由两大核心模块协同构成：
增量式重规划层：园区环境中行人、电动车、临时堆放物等动态障碍频繁出现。系统采用分层规划架构——全局层基于已知地图生成宏观路径（如 A* 或 RRT*），局部层以 D* Lite 或动态窗口法（DWA）为核心，当传感器检测到新障碍侵入安全区域时，仅对受影响区域进行增量更新，而非全局重搜，从而在毫秒级时间内生成绕行子路径。
加速度连续平滑层：重规划输出的离散路径点或速度指令，通过 S 型曲线（S-Curve）轨迹规划转化为加速度连续（Jerk 受限）的参考轨迹，再由 FOC 驱动的 BLDC 电机精确跟踪执行，消除启停冲击和转向抖动。
其本质是：增量重规划解决"障碍来了怎么快速改路"，加速度连续平滑解决"改完的路怎么让 BLDC 走得稳"。
二、主要特点

1. 分层式动态避障架构
   系统采用典型的三层控制架构，各层职责明确、优先级递进：
   全局规划层：基于园区静态地图（通过激光 SLAM 预建），以较低频率（1~10Hz）维护从起点到终点的全局最优路径。
   局部重规划层：以 D* Lite 或 DWA 为核心，以较高频率（10~20Hz）在局部窗口内检测动态障碍并生成绕行子路径。D* Lite 通过 g-rhs 一致性检测机制仅更新受影响节点；DWA 则在速度空间内采样多组候选速度对，模拟短期轨迹后通过评价函数选优。
   反应式避障层：最高优先级，当近距传感器检测到紧急危险时，立即触发制动或瞬时转向，保证绝对安全。
2. 增量更新的计算效率优势
   在园区配送场景中，动态障碍通常是局部的（如一个行人横穿道路），而非全局环境剧变。增量式重规划的核心优势在于只更新受影响的局部区域：
   D* Lite 从目标点反向搜索，起点随机器人移动而变化，每次移动仅需增量更新起点附近节点，无需重算整棵搜索树。
   DWA 在当前运动状态下构建"可行速度窗口"，仅采样当前可达的速度组合，计算量可控，非常适合 Arduino 级别平台。
   实测表明，在园区低速场景（最大速度 15km/h）下，动态障碍物实时重规划延迟可控制在 85ms 以内，满足安全响应需求。
3. S 型加速度连续平滑
   这是区别于传统梯形加减速的关键设计。S 型曲线将运动过程分为七个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。其核心特征是加速度本身连续变化，不存在阶跃突变：
   Jerk（加加速度）受限：加速度不能瞬间建立或消失，而是平滑地增加和减少，有效抑制机械谐振频率，显著降低启停振动和噪音。
   FOC 驱动的天然匹配：FOC 通过独立控制 q 轴电流实现精确转矩输出，正弦波驱动消除了方波换相的转矩脉动，使电机在整个速度范围内都能平稳运行。FOC 的高动态响应特性使其能快速跟随 S 型曲线生成的平滑指令。
   对配送货物的保护：园区配送机器人承载餐食、快递等物品，S 型平滑确保启停过程无冲击，避免因颠簸导致货物损坏或洒落。
4. BLDC 电机的高动态执行能力
   BLDC 电机在该系统中承担"精确执行平滑轨迹"的角色：
   快速启停与差速转向：动态避障要求机器人在 100~500ms 内完成"检测-规划-执行"全链路响应，BLDC 的高扭矩密度和快速响应特性是物理基础。两轮差速驱动通过独立控制左右 BLDC 转速差实现灵活转向。
   闭环精确跟踪：BLDC 配合编码器 + PID/FOC 闭环，可精确跟踪 S 型曲线生成的参考速度和位置指令，确保实际运动轨迹与规划轨迹高度一致。
   能效优势：FOC 驱动的 BLDC 相比传统有刷电机效率提升 15%~30%，对依赖电池供电、需长时间连续运行的配送机器人至关重要。
5. 多传感器融合感知
   园区环境复杂，单一传感器存在盲区。系统通常融合多种传感器：
   激光雷达：中远距离精确测距和轮廓测量，构建局部代价地图。
   超声波传感器：广域覆盖，检测激光雷达易失效的低反射率物体（如玻璃门）。
   视觉传感器：结合 YOLO 等目标检测模型识别行人、车辆等动态障碍的类别和运动趋势。
   IMU + 编码器：提供里程计和姿态信息，辅助定位和轨迹跟踪。
   三、应用场景
6. 产业园区末端配送
   在制造工业园区、产业基地中，配送机器人需在厂区道路、装卸区之间自主运输原料、半成品和成品。园区内行人、叉车、电动车混行，增量重规划确保机器人实时避让动态障碍，S 型平滑确保重载工况下货物稳定运输。
7. 校园快递/外卖配送
   大学校园内道路网络复杂，人流密集且时段性强。机器人需在食堂、宿舍楼、快递站之间穿梭，动态避让行人和自行车。增量重规划在行人密集区快速生成绕行路径，S 型平滑确保餐食不洒落。
8. 医院物资配送
   在医院走廊、电梯口等人流密集区域，配送机器人运送药品、餐食或医疗垃圾。增量重规划使机器人能灵活避让医护人员和患者，S 型平滑确保药品和液体样本不受颠簸影响。
9. 仓储物流 AGV
   在电商仓库中，AGV 需在货架通道内穿梭，实时避让行走的拣货员和临时停放的障碍物。增量重规划使 AGV 在不停车的情况下实时调整路径，S 型平滑减少因急停急起造成的货物晃动。
10. 教育科研平台
    作为移动机器人导航与控制课程的实验平台，演示增量搜索算法、S 型轨迹规划和 FOC 电机控制在资源受限平台上的工程实现。
    四、需要注意的事项
11. Arduino 平台资源瓶颈
    标准 Arduino Uno（16MHz, 2KB RAM）难以独立运行完整的增量重规划 + S 型曲线 + FOC 控制链路：
    内存限制：栅格地图 + 优先队列 + S 型曲线轨迹缓冲区对 RAM 需求较大，Uno 的 2KB SRAM 极易耗尽。
    算力限制：FOC 电流环需 10kHz 以上执行频率，S 型曲线涉及分段函数和积分运算，D* Lite/DWA 的优先队列操作也需可观算力。
    推荐方案：采用 ESP32（双核 240MHz，520KB SRAM）、Teensy 4.1（600MHz Cortex-M7）或 STM32 等高性能平台；或采用"上位机（树莓派/Jetson）负责感知规划 + Arduino 负责底层电机控制"的分布式架构。
12. 传感器噪声引发的"伪重规划"
    激光雷达反光、超声波多反射、视觉检测抖动等传感器噪声会被重规划算法误判为"环境变化"，触发大量无效重规划，导致路径频繁微调、机器人"蛇形走位"：
    解决方案：对障碍物位置进行卡尔曼滤波平滑；设置代价地图更新的迟滞阈值（hysteresis），避免微小波动触发重规划；采用懒惰更新（Lazy Update）策略丢弃过期的优先队列节点。
13. S 型曲线参数与 BLDC 动态特性的匹配
    S 型曲线的三个关键参数（最大速度 Vmax、最大加速度 Amax、最大加加速度 Jmax）必须与 BLDC 电机的实际能力匹配：
    Jmax 过小：加速过程过长，配送效率下降。
    Jmax 过大：退化为梯形规划，丧失平滑优势。
    Amax 超限：BLDC 无法提供足够转矩，导致丢转或跟踪误差增大。
    建议：通过实验标定参数，并根据载重状态动态调整——重载时降低 Amax 和 Jmax，轻载时适当提高。
14. 重规划与底盘控制的时序协调
    规划周期（50~100ms）与底盘 FOC 控制硬周期（0.1ms）存在数量级差异：
    时序冲突：重规划期间底盘控制线程若按旧路径执行可能撞上新障碍，若停下来等待则用户感知到"原地发呆"。
    解决方案：采用双缓冲机制——规划线程在后台计算新路径，完成后原子性地切换路径指针；FOC 控制运行在最高优先级定时器中断中，重规划放在主循环或低优先级任务中，确保电机指令不被阻塞。
15. 紧急避障时的速度衔接连续性
    当重规划触发紧急绕行时，S 型曲线需从当前实际速度点重新生成，而非从静止状态重新开始。否则会出现速度跳变，导致 BLDC 电流冲击和机械抖动：
    解决方案：紧急重规划时，以当前实际速度和加速度作为 S 型曲线的初始边界条件，确保新旧轨迹段之间的速度和加速度连续衔接。
16. 多电机 EMI 干扰与电源管理
    多个 BLDC 电机同时工作时，功率器件开关噪声大：
    功率区与信号区 PCB 布局分离，编码器信号线与功率线保持间距。
    母线侧并联高频陶瓷电容与电解电容组合，抑制电压纹波。
    电机动力电源与主控逻辑电源共地但独立供电，避免电机启动电流导致主控重启。

1、弹性带（TEB）局部路径变形避障
此案例采用类似“Timed Elastic Band”的弹性带思想，将路径视为一条可弹性变形的橡皮筋。当超声波检测到障碍物时，路径点受“斥力”推开，同时内部张力保持路径平滑，无需每次重规划，非常适合Arduino平台的轻量级实时避障需求。

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

BLDCMotor motorL = BLDCMotor(7), motorR = BLDCMotor(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 路径点结构（弹性带节点）
struct Pose { float x, y; };
std::vector<Pose> path;
const int PATH_LENGTH = 20;

// TEB参数：张力拉直路径，斥力推开障碍，阻尼防止振荡
const float TENSION = 0.3;
const float REPULSION = 2.0;
const float DAMPING = 0.8;
const float OBS_RANGE = 0.5;  // 障碍影响范围(m)

// 四向超声波
NewPing sonarF(TRIG_F, ECHO_F, MAX_DIST);
NewPing sonarL(TRIG_L, ECHO_L, MAX_DIST);
NewPing sonarR(TRIG_R, ECHO_R, MAX_DIST);

// 初始化路径：从起点到终点直线
void initPath(float sx, float sy, float ex, float ey) {
    path.clear();
    for(int i=0; i<PATH_LENGTH; i++) {
        float t = (float)i / (PATH_LENGTH - 1);
        path.push_back({sx + t*(ex-sx), sy + t*(ey-sy)});
    }
}

// 弹性带变形核心
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 * (path[i-1].x - 2*path[i].x + path[i+1].x);
        fy += TENSION * (path[i-1].y - 2*path[i].y + path[i+1].y);
        
        // 前方障碍斥力
        if(dF < OBS_RANGE && i > PATH_LENGTH/3) {
            float force = REPULSION * (1 - dF/OBS_RANGE);
            // 根据左右空间选择绕行方向
            if(dL > dR) { fx += force * 0.5; fy -= force * 0.3; }
            else        { fx -= force * 0.5; fy -= force * 0.3; }
        }
        // 左右障碍斥力
        if(dL < OBS_RANGE) fx += REPULSION * (1 - dL/OBS_RANGE);
        if(dR < OBS_RANGE) fx -= REPULSION * (1 - dR/OBS_RANGE);
        
        // 阻尼更新
        path[i].x += fx * DAMPING;
        path[i].y += fy * DAMPING;
    }
    
    // 平滑滤波：限制曲率突变
    for(int iter=0; iter<3; iter++) {
        for(int i=2; i<PATH_LENGTH-2; i++) {
            path[i].x = 0.25*path[i-1].x + 0.5*path[i].x + 0.25*path[i+1].x;
            path[i].y = 0.25*path[i-1].y + 0.5*path[i].y + 0.25*path[i+1].y;
        }
    }
}

void loop() {
    deformElasticBand();   // 每帧变形
    // 跟踪路径前点（取前瞻点）
    int lookahead = 3;
    float targetX = path[lookahead].x;
    float targetY = path[lookahead].y;
    // 计算控制量并驱动BLDC...
    delay(50);
}

关键逻辑：弹性带避障的核心是“力导向”。障碍物产生斥力推开路径点，而相邻点间的张力将路径拉直，两者平衡后形成一条平滑的绕行路径。这种方法无需全局重规划，计算量极轻，非常适合Arduino。

2、两层级联规划（顶层A* + 底层弹性带）
此案例采用分层架构：顶层用A*在稀疏栅格地图上规划宏观路径（低频，按需触发），底层用弹性带进行局部微调（高频，每帧执行）。这种架构兼顾了全局最优性与局部实时性。

// 顶层：A*在10×10栅格上规划
#define TOP_MAP_SIZE 10
int topMap[TOP_MAP_SIZE][TOP_MAP_SIZE];
std::vector<Pose> topPath;  // 顶层路径点（粗粒度）

// 底层：在顶层相邻节点间，用弹性带微调
std::vector<Pose> botPath;  // 底层局部路径（精细）

// 顶层规划（仅在起点变化或地图变化时触发）
void planTopLevel(int startX, int startY, int goalX, int goalY) {
    // 在topMap上运行A*，存入topPath
}

// 底层变形（每帧执行）
void planBottomLevel() {
    if(topPath.size() < 2) return;
    
    // 找到机器人最近的顶层路径段
    // 将该段内的几个点作为弹性带初始化
    // 运行deformElasticBand()进行局部避障微调
}

void loop() {
    // 每帧执行底层规划
    planBottomLevel();
    
    // 跟踪底层路径
    if(!botPath.empty()) {
        Pose target = botPath[2]; // 前瞻点
        // 计算控制量并驱动BLDC...
    }
    
    // 每10帧检查是否需要顶层重规划
    if(frameCount % 10 == 0) {
        // 检测路径是否被阻断，若是则触发planTopLevel()
    }
}

关键逻辑：分层规划将A*的计算量限制在10×10的栅格上（最多100个节点），而高频控制只运行轻量的弹性带变形。这种架构让Arduino也能“跑得起”带全局规划的导航系统。

3、S型速度曲线 + 加速度连续跟踪
此案例聚焦于执行层的平滑控制。BLDC电机收到路径点后，并非直接阶跃到达，而是通过S型速度曲线规划生成加速度连续的轨迹，实现“软启动-匀速-软停靠”，避免对配送货物的冲击。

#include <SimpleFOC.h>

BLDCMotor motorL = BLDCMotor(7), motorR = BLDCMotor(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// S型速度曲线规划器
class SCurvePlanner {
public:
    float pos, vel, accel, jerk;
    float targetPos;
    float vMax, aMax, jMax;
    
    void setTarget(float target, float v_max, float a_max, float j_max) {
        targetPos = target;
        vMax = v_max; aMax = a_max; jMax = j_max;
    }
    
    // 每帧更新，生成平滑位置指令
    void update(float dt) {
        // 计算到目标的距离
        float distToGoal = targetPos - pos;
        float velSign = (distToGoal > 0) ? 1.0 : -1.0;
        float distAbs = fabs(distToGoal);
        
        // 减速区判断：当前速度需要多大距离才能刹停
        float stopDist = (vel * vel) / (2 * aMax);
        if(distAbs < stopDist) {
            // 进入减速区，施加减速度
            accel -= jMax * dt;
        } else if(vel < vMax) {
            // 加速区
            accel += jMax * dt;
        }
        
        // 限制加速度边界
        accel = constrain(accel, -aMax, aMax);
        // 更新速度与位置
        vel += accel * dt;
        vel = constrain(vel, -vMax, vMax);
        pos += vel * dt;
    }
};

SCurvePlanner plannerL, plannerR;

// 将路径点转换为左右轮目标位置
void followPathPoint(Pose target, float dt) {
    // 差速模型：先算线速度和角速度，再分解到左右轮
    float linearSpeed = 0.5;   // 目标线速度
    float angularSpeed = 0.0;  // 目标角速度
    
    // 用S曲线规划器生成平滑速度指令
    plannerL.setTarget(linearSpeed - angularSpeed*WHEEL_BASE/2, 1.0, 1.5, 0.5);
    plannerR.setTarget(linearSpeed + angularSpeed*WHEEL_BASE/2, 1.0, 1.5, 0.5);
    plannerL.update(dt);
    plannerR.update(dt);
    
    // 输出到BLDC（速度模式）
    motorL.target = plannerL.vel;
    motorR.target = plannerR.vel;
}

void loop() {
    static unsigned long lastTime = micros();
    float dt = (micros() - lastTime) / 1e6;
    lastTime = micros();
    
    // 获取当前路径目标点
    Pose target = getCurrentTarget();
    followPathPoint(target, dt);
    
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
    delay(10);
}

关键逻辑：S型曲线通过限制加加速度（jerk，即加速度的变化率），让速度从零平滑增加到目标值，再平滑减速到零。与传统梯形曲线相比，S曲线消除了启停时的冲击，对配送机器人而言意味着货物不会因急刹而倾倒。

要点解读

1. 增量重规划不等于“全部重算”，局部修正是核心

D* Lite这类增量算法的精髓在于：环境变化时，只更新受影响的局部节点，而非全图重算。案例一中的弹性带变形和案例二的两层规划都体现了这一思想——每次只调整路径的局部形状。对于Arduino有限的内存，这是实现动态避障的必由之路。

2. 路径平滑必须走“位置-速度”双重平滑路线

A*输出的离散折线路径，和BLDC电机的实际执行之间隔着一道鸿沟。前者只给了“去哪儿”，后者需要“怎么去”。位置平滑（贝塞尔插值、弹性带）让路径几何连续，速度平滑（S型曲线）让运动过程连续。两者缺一不可，否则要么转急弯卡住，要么启停剧烈冲击货物。

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

完整的A*在20×20栅格上就要消耗约8KB内存，接近Arduino Mega的上限。通过分层架构——顶层用稀疏地图做全局规划（低频），底层用小窗口做局部调整（高频）——可以把重计算“稀释”到可接受的频率。

4. 控制环频率远高于规划环频率是系统稳定的前提

规划层可能5-10Hz更新一次，但BLDC的FOC电流环需要1kHz以上的响应，速度/位置环也需要100Hz以上。若直接将50Hz的规划指令喂给电机，必然导致振荡。正确做法是：规划层输出路径点，执行层以高频PID“插补”路径点之间的轨迹。

5. MCU选型直接决定方案上限

标准Arduino Uno（ATmega328P）无法同时运行FOC + S曲线轨迹规划 + A*重规划。工程建议：至少使用ESP32（双核，240MHz）或Teensy 4.0（600MHz），让一个核心跑感知与规划，另一个核心专职运行BLDC的FOC控制环。若必须用Uno，则只能运行案例一这种纯局部避障方案。

4、基础DWA动态窗口+滚动窗口增量重规划——室内外混合避障配送
适用场景：园区室内外混合配送场景，需应对突然出现的行人、临时堆放物料等动态障碍，核心需求是高频局部环境感知、实时速度空间重规划、路径平滑过渡，确保机器人在配送过程中快速避障，避免急刹急转。

核心逻辑：采用滚动窗口机制，仅以机器人当前位姿为中心，实时更新局部代价地图，大幅降低算力消耗；融合改进型DWA算法，在速度空间采样多组速度组合，结合运动学约束与障碍距离，通过评价函数筛选最优速度；依托BLDC的FOC闭环控制，精准跟踪速度指令，同时通过S曲线加减速实现加速度连续，避免机械冲击。

/* ===== 园区配送：基础DWA+滚动窗口增量重规划 =====
 * 硬件：ESP32 + 2×BLDC差速底盘 + 三路超声波传感器
 * 核心：滚动窗口局部感知→DWA速度采样→评价选优→BLDC平滑执行
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// --- BLDC差速电机 ---
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化（示例：BLDCDriver3PWM driverL(9,10,11,8)）

// --- 超声波传感器 ---
#define TRIG_F 2 #define ECHO_F 3
#define TRIG_L 4 #define ECHO_L 5
#define TRIG_R 6 #define ECHO_R 7
NewPing sonarF(TRIG_F, ECHO_F, 200);
NewPing sonarL(TRIG_L, ECHO_L, 200);
NewPing sonarR(TRIG_R, ECHO_R, 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 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;   // 当前位置
float robotYaw = 0;             // 当前朝向(rad)
float goalX = 2.0, goalY = 2.0; // 配送目标点

void setup() {
    Serial.begin(115200);
    // BLDC初始化（速度控制模式）
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 超声波引脚初始化
    pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
    pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
    pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    // 核心：滚动窗口内DWA重规划
    dwaControl();
    // 位姿更新（编码器里程计）
    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 readDistance(int trig, int echo) {
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    long dur = pulseIn(echo, HIGH, 30000);
    return dur == 0 ? 3.0 : dur * 0.034 / 2 / 100.0;
}

// --- 障碍距离融合（三路传感器）---
float getObstacleDistance() {
    float dF = readDistance(TRIG_F, ECHO_F);
    float dL = readDistance(TRIG_L, ECHO_L);
    float dR = readDistance(TRIG_R, ECHO_R);
    return min(dF, min(dL, dR));
}

// --- DWA核心控制函数 ---
void dwaControl() {
    // 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, 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);
}

// --- 轨迹评价函数 ---
float evaluateTrajectory(float v, float w) {
    float time = 0, x = robotX, y = robotY, yaw = robotYaw;
    float minObsDist = 999, bestHeading = 0;
    // 轨迹预测与评价
    while (time < 1.0) {
        float dx = v * cos(yaw) * DT;
        float dy = v * sin(yaw) * DT;
        x += dx; y += dy; yaw += w * DT;
        time += DT;
        // 朝向目标评价
        float dxGoal = goalX - x, dyGoal = goalY - y;
        bestHeading += acos(dxGoal * cos(yaw) + dyGoal * sin(yaw) / sqrt(dxGoal*dxGoal + dyGoal*dyGoal));
        // 障碍距离评价
        float obsDist = getObstacleDistance();
        minObsDist = min(minObsDist, obsDist);
    }
    // 综合评分
    float headingScore = bestHeading / time;
    float distScore = minObsDist;
    float velocityScore = sqrt(v*v + w*w);
    return ALPHA_HEADING * headingScore + BETA_DIST * distScore + GAMMA_VELOCITY * velocityScore;
}

5、贝塞尔曲线路径生成+S型速度曲线平滑——狭窄通道配送
适用场景：园区狭窄通道（如货架间、楼宇走廊）的配送场景，需频繁转弯，核心需求是路径几何平滑、速度加速度连续变化、无机械冲击，确保满载货物平稳通过狭窄区域，避免货物倾覆。

核心逻辑：采用三次贝塞尔曲线拟合全局路径，通过控制点保证路径曲率连续，满足机器人运动学约束；结合S型速度曲线替代传统梯形速度曲线，对加加速度进行限制，实现速度、加速度的平滑过渡；依托BLDC双闭环控制，精准跟踪曲线离散点，实现丝滑过弯。

/* ===== 狭窄通道配送：贝塞尔曲线路径+S型速度平滑 =====
 * 硬件：ESP32 + 2×BLDC差速底盘 + 编码器
 * 核心：贝塞尔路径插值→S型速度规划→BLDC双闭环跟踪
 */
#include <SimpleFOC.h>
#include <math.h>

// --- BLDC差速电机 ---
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// --- 贝塞尔曲线参数 ---
struct BezierPath {
    float P0[2];  // 起点
    float P1[2];  // 控制点1（起点切线方向）
    float P2[2];  // 控制点2（终点切线方向）
    float P3[2];  // 终点
    float duration; // 执行时间(s)
};
BezierPath currentPath;
float pathProgress = 0.0;  // 归一化进度（0~1）
bool isExecuting = false;

// --- S型速度曲线参数 ---
const float MAX_SPEED = 0.5;      // 最大线速度(m/s)
const float MAX_ACCEL = 0.2;      // 最大加速度(m/s²)
const float JERK = 0.1;           // 加加速度(m/s³)

void setup() {
    Serial.begin(115200);
    // BLDC初始化（速度+位置双闭环）
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    // 定义贝塞尔路径（狭窄通道转弯场景）
    currentPath.P0[0] = 0.0; currentPath.P0[1] = 0.0;
    currentPath.P1[0] = 1.0; currentPath.P1[1] = 1.5;
    currentPath.P2[0] = 3.0; currentPath.P2[1] = 1.5;
    currentPath.P3[0] = 4.0; currentPath.P3[1] = 0.0;
    currentPath.duration = 4.0;
    isExecuting = true;
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    if (isExecuting) {
        // 1. 贝塞尔曲线插值（计算当前位置）
        pathProgress += 0.01 / currentPath.duration;
        if (pathProgress >= 1.0) {
            pathProgress = 1.0;
            isExecuting = false;
        }
        float pos[2];
        bezierCubic(pathProgress, currentPath.P0, currentPath.P1, currentPath.P2, currentPath.P3, pos);
        // 2. S型速度规划（根据路径曲率动态调整速度）
        float curvature = calculateCurvature(pos);
        float targetSpeed = sCurveSpeedPlan(curvature);
        // 3. 差速跟踪（位置→速度转换）
        float dx = pos[0] - 0, dy = pos[1] - 0;
        float angle = atan2(dy, dx);
        float wheelBase = 0.25;
        motorL.move(targetSpeed - angle * wheelBase / 2);
        motorR.move(targetSpeed + angle * wheelBase / 2);
        // 调试输出
        Serial.print("Progress:"); Serial.print(pathProgress);
        Serial.print(" Speed:"); Serial.print(targetSpeed);
        Serial.println();
    }
}

// --- 三次贝塞尔曲线计算 ---
void bezierCubic(float t, float P0[2], float P1[2], float P2[2], float P3[2], float output[2]) {
    float u = 1 - t;
    float b0 = u*u*u, b1 = 3*u*u*t, b2 = 3*u*t*t, b3 = t*t*t;
    output[0] = b0*P0[0] + b1*P1[0] + b2*P2[0] + b3*P3[0];
    output[1] = b0*P0[1] + b1*P1[1] + b2*P2[1] + b3*P3[1];
}

// --- 路径曲率计算（三点式）---
float calculateCurvature(float pos[2]) {
    // 简化：基于相邻路径点计算曲率，实际需预存路径点
    float dx1 = pos[0] - 0, dy1 = pos[1] - 0;
    float dx2 = 4.0 - pos[0], dy2 = 0.0 - pos[1];
    return fabs(dx1*dy2 - dy1*dx2) / (sqrt(dx1*dx1+dy1*dy1)*sqrt(dx2*dx2+dy2*dy2));
}

// --- S型速度规划（曲率匹配）---
float sCurveSpeedPlan(float curvature) {
    // 曲率越大，速度越低，结合S型加减速限制
    float targetSpeed = MAX_SPEED - curvature * 0.5;
    targetSpeed = constrain(targetSpeed, 0.1, MAX_SPEED);
    // 简化S型速度输出（实际需分加速/匀速/减速阶段）
    return targetSpeed;
}

6、GPS全局路径跟踪+多目标点增量避障——园区跨区配送
适用场景：园区跨区域配送（如从仓库到多个办公楼），需应对室外动态障碍（行人、车辆），核心需求是全局路径精准跟踪、多目标点动态切换、局部增量避障，确保机器人在室外环境中按预设航点配送，遇障时局部绕行后回归全局路径。

核心逻辑：采用GPS+IMU+里程计融合定位，获取全局绝对坐标，维护多目标点队列；当检测到动态障碍时，触发局部增量重规划（如DWA），生成绕行轨迹；避障完成后，自动回归全局路径，依托BLDC差速控制实现精准转向与速度跟踪，同时通过PID控制保证路径跟踪的加速度连续。

/* ===== 跨区配送：GPS全局跟踪+多目标点增量避障 =====
 * 硬件：ESP32 + BLDC差速底盘 + GPS模块 + 激光雷达
 * 核心：GPS定位→多目标点管理→增量避障→BLDC路径跟踪
 */
#include <SoftwareSerial.h>
#include <TinyGPS++.h>
#include <PID_v1.h>
#include <SimpleFOC.h>

// --- 硬件引脚定义 ---
#define GPS_RX 10 #define GPS_TX 11
#define ESC_PWM 9  // BLDC速度PWM
#define STEER_PWM 10 // 差速转向控制
SoftwareSerial gpsSerial(GPS_RX, GPS_TX);
TinyGPSPlus gps;

// --- GPS与目标点变量 ---
double currentLon, currentLat;
double targetLon = 116.404, targetLat = 39.915; // 示例目标点
double distanceThreshold = 5.0; // 到达判定距离（米）

// --- PID控制变量（路径纠偏）---
double currentAngle;    // 当前行驶方向角
double targetAngle;     // 指向目标的方向角
double angleError;      // 角度偏差
double steerOutput;     // 转向输出
double speedOutput = 800;// 基础速度PWM
PID pidSteer(&angleError, &steerOutput, 0, 0.8, 0.05, 0.01);

// --- BLDC电机 ---
BLDCMotor motorL(7), motorR(7);

void setup() {
    Serial.begin(9600);
    gpsSerial.begin(9600);
    // BLDC初始化
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // PID初始化
    pidSteer.SetMode(AUTOMATIC);
    pidSteer.SetOutputLimits(-1000, 1000);
    Serial.println("跨区配送机器人启动！");
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    // 1. GPS定位解析
    while (gpsSerial.available() > 0) {
        if (gps.encode(gpsSerial.read())) {
            if (gps.location.isValid()) {
                currentLon = gps.location.lng().deg();
                currentLat = gps.location.lat().deg();
                if (gps.course.isValid()) currentAngle = gps.course.deg();
            }
        }
    }
    // 2. 计算目标方向角与偏差
    if (gps.location.isValid()) {
        targetAngle = calculateTargetAngle(currentLat, currentLon, targetLat, targetLon);
        angleError = targetAngle - currentAngle;
        // 归一化角度偏差（-180~180）
        while (angleError > 180) angleError -= 360;
        while (angleError < -180) angleError += 360;
        // 3. PID计算转向输出
        pidSteer.Compute();
        // 4. BLDC差速控制（转向+速度）
        float wheelBase = 0.25;
        float leftSpeed = speedOutput - steerOutput * wheelBase / 2;
        float rightSpeed = speedOutput + steerOutput * wheelBase / 2;
        motorL.move(leftSpeed);
        motorR.move(rightSpeed);
        // 5. 到达判定与目标点切换（多目标点管理）
        double distance = haversineDistance(currentLat, currentLon, targetLat, targetLon);
        if (distance < distanceThreshold) {
            Serial.println("到达目标点，切换下一个配送点");
            // 此处补充多目标点队列切换逻辑
        }
    }
    delay(50);
}

// --- 目标方向角计算（简化版）---
double calculateTargetAngle(double lat1, double lon1, double lat2, double lon2) {
    double dLon = lon2 - lon1;
    double y = sin(dLon) * cos(lat2 * PI/180);
    double x = cos(lat1 * PI/180) * sin(lat2 * PI/180) - sin(lat1 * PI/180) * cos(lat2 * PI/180) * cos(dLon);
    return atan2(y, x) * 180 / PI;
}

// --- 球面距离计算（Haversine公式）---
double haversineDistance(double lat1, double lon1, double lat2, double lon2) {
    double dLat = (lat2 - lat1) * PI/180;
    double dLon = (lon2 - lon1) * PI/180;
    double a = sin(dLat/2) * sin(dLat/2) + cos(lat1 * PI/180) * cos(lat2 * PI/180) * sin(dLon/2) * sin(dLon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    return 6371000 * c; // 地球半径6371km，返回米
}

要点解读

1. 滚动窗口增量重规划：聚焦局部变化，平衡实时性与算力
   滚动窗口是动态障碍应对的核心机制，其核心逻辑是仅以机器人当前位姿为中心，维护局部代价地图，而非全局全量更新，从根源上解决算力瓶颈与实时性问题：
   局部感知降低算力：传统全局重规划需遍历全地图，对嵌入式平台算力要求极高，而滚动窗口仅处理局部区域，使Arduino/ESP32等平台能实现10Hz以上的高频重规划，快速响应突发障碍。
   增量更新减少冗余：仅针对障碍变化区域更新代价地图，避免重复计算，确保路径规划始终基于最新环境信息，解决全局地图动态过时的痛点。
   适配动态环境特性：园区配送中障碍多为突然出现的行人、车辆，滚动窗口的局部高频更新特性，完美匹配这类短时、局部的环境变化，保障避障响应速度。

2. 加速度连续平滑：从路径到速度的全链路平滑，消除机械冲击
   加速度连续是提升配送平稳性的关键，需从路径几何与速度规划双维度实现，确保运动过程无突变、无机械磨损：
   路径几何平滑：传统A*、栅格路径存在直角拐点，直接跟踪会导致急转急停。贝塞尔曲线、B样条等算法通过控制点拟合，生成曲率连续的路径，满足机器人运动学约束，避免因路径突变引发的机械冲击。
   速度曲线优化：传统梯形速度曲线存在加速度阶跃，易引发机械谐振。S型速度曲线通过对加加速度限制，实现速度、加速度的平滑过渡，消除启停和转向时的冲击，同时降低BLDC电机的瞬时电流冲击，延长使用寿命。
   执行机构匹配：平滑的路径与速度规划需依托BLDC的高动态响应特性，通过FOC闭环控制与双闭环PID，精准跟踪规划指令，确保从期望轨迹到实际运动的完美映射，实现丝滑的运动效果。

3. 分层闭环架构：全局规划与局部避障的协同，保障导航稳定性
   分层闭环是导航系统的核心架构，通过全局路径引导+局部避障调整+底层精准执行的三层协同，解决单一算法的局限性：
   全局规划提供宏观引导：A*、GPS多目标点规划负责生成从起点到终点的宏观最优路径，为机器人提供明确的行驶方向，避免局部避障陷入盲目性，解决局部极小值问题。
   局部规划实现动态避障：DWA等局部规划算法在滚动窗口内，结合实时传感器数据与全局路径引导，生成避障速度指令，确保机器人在动态环境中安全绕行，同时平滑回归全局路径。
   底层执行闭环反馈：BLDC执行层将规划指令转化为实际运动，同时通过编码器、IMU等传感器反馈位姿信息，形成闭环，确保规划与执行的一致性，提升导航稳定性。

4. 算力与实时性平衡：嵌入式平台的工程核心，架构与优化并重
   智能园区配送机器人的算力与实时性是工程落地的关键瓶颈，需从硬件选型、架构设计、代码优化三方面平衡：
   硬件选型适配算力需求：标准Arduino Uno等8位MCU难以支撑高频浮点运算，需采用ESP32、STM32等32位高性能MCU，或采用“上位机（树莓派/Jetson）+下位机（Arduino）”架构，上位机负责路径规划与避障，下位机专注BLDC执行，实现算力分工。
   算法轻量化与参数调优：对复杂算法进行简化，保留核心逻辑，同时根据底盘运动学参数精细调优DWA的评价函数权重、采样分辨率等参数，避免因参数不当导致的计算冗余或运动异常，保障实时性。
   代码优化保障控制周期：严禁在主循环中使用delay()等阻塞函数，采用定时器中断实现高频控制循环，确保motor.loopFOC()、轨迹规划等核心函数的执行周期稳定，避免控制延迟引发的轨迹振荡。

5. 传感器与执行器协同：数据精准与运动可靠的基础，闭环保障安全
   传感器与执行器的精准协同是系统可靠运行的前提，需从数据质量、延迟补偿、安全保护三方面保障：
   传感器数据滤波与对齐：动态避障对传感器数据实时性、准确性要求极高，需通过滑动平均、卡尔曼滤波消除噪声，同时严格标定传感器安装位置与数据延迟，确保障碍物坐标与机器人位姿的时间戳对齐，避免因数据偏差引发的误避障。
   执行器延迟补偿与限制：BLDC执行存在固有延迟，需在规划算法中引入预瞄点机制，基于预测位置计算控制指令，补偿延迟；同时设置线速度、角速度的S曲线加减速限制，防止阶跃指令导致电机过流、轮胎打滑。
   安全保护机制设计：需加入硬件级急停、过流保护与软件级轨迹偏差监控，当机器人因打滑、碰撞偏离轨迹超过阈值时，触发安全降级策略，如减速悬停或重新规划，避免机械损坏与碰撞事故，保障配送安全。

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