以专业的视角来看，基于 Arduino 生态（通常以 ESP32 等高性能 MCU 为核心）的机器人贝塞尔曲线与速度曲线优化算法，是解决移动机器人运动学约束、提升运行平滑度与动态响应能力的核心底层技术。该技术将几何路径规划与底层 BLDC 电机控制深度结合，使机器人能够执行极其丝滑的复杂运动。

一、 主要特点

1. 几何路径平滑与运动学约束满足
   传统的导航算法（如 A* 算法）生成的路径往往包含大量尖锐的折线或直角转弯，这不符合机器人的运动学约束。贝塞尔曲线（Bézier curves）作为一种参数化曲线，能够以极高的保真度拟合这些离散的路径点，生成具有连续曲率的平滑轨迹。特别是对于具有非完整约束（如纯滚动约束）的轮式机器人，贝塞尔曲线在端点的切线向量能够完美匹配机器人的输入和输出方向，确保机器人无需原地旋转即可顺滑过弯。
2. 运动轨迹的精细化与防谐振
   在速度规划层面，系统摒弃了会导致加速度突变、从而引发机械谐振的梯形速度曲线，转而采用 S 型（S-Curve）速度曲线。S 型曲线通过对加加速度（Jerk）进行限制，使机器人的速度、加速度变化极其平缓。这种优化不仅消除了机械结构的抖动，还大幅降低了 BLDC 电机的瞬时电流冲击，提升了系统的整体寿命与运行静音性。
3. 多目标联合优化与动态避障
   贝塞尔曲线的控制点可以通过数学优化进行动态调整。现代算法通常将物理优化目标（如最小化速度、加速度、加加速度）与几何约束（如安全走廊、避障距离）结合，通过二次规划求解出最优的控制点。这使得机器人能够在保证绝对安全的前提下，规划出耗时最短、最平滑的避障轨迹。
4. 高动态底层执行与精准跟随
   平滑的轨迹需要强大的底层执行器支撑。结合 BLDC 电机与 FOC（磁场定向控制）算法，系统能够实现极低转矩脉动和毫秒级的动态响应。通过双闭环（速度环+位置环）PID 控制，机器人能够以极高的精度跟踪贝塞尔曲线生成的离散目标点，实现从“期望轨迹”到“实际运动”的完美映射。
   二、 应用场景
5. 工业 AGV/AMR 仓储物流
   在自动化仓库中，叉车机器人或搬运机器人需要频繁穿梭于狭窄的货架之间。贝塞尔曲线规划的路径能够确保机器人在高速转弯时不发生侧滑或急刹，S 型速度曲线则保证了满载货物时的行驶平稳性，防止货物倾倒。
6. 医疗与商用服务机器人
   在医院、酒店或餐厅等对静音和平稳性要求极高的场景中，送餐或送药机器人必须避免机械抖动和噪音。平滑的贝塞尔轨迹与 S 型速度规划能显著提升乘坐体验（如载人底盘）或物品安全性，同时降低电机的电磁噪声。
7. 无人机与机械臂轨迹规划
   对于多旋翼无人机或 SCARA 机械臂，贝塞尔曲线被广泛用于生成平滑的三维空间轨迹。通过优化控制点的有限差分或方差，可以生成既满足安全边界又具备极佳动态特性的飞行或抓取路径，避免机械臂关节的剧烈冲击。
8. 自动驾驶与园区巡检
   在室外园区或结构化道路中，无人车需要绕过动态障碍物（如行人、其他车辆）。基于安全走廊的贝塞尔曲线优化算法，能够实时生成平滑的绕行轨迹，配合 FOC 驱动的底盘，实现如同老司机般平顺的避障与恢复行驶。
   三、 需要注意的事项
9. 算力瓶颈与实时性保障
   贝塞尔曲线的实时生成、控制点优化以及 S 型曲线的插值计算，涉及大量的浮点运算。标准的 Arduino Uno 等 8 位或低速 MCU 难以胜任高频（如 >100Hz）的轨迹规划与电机控制。强烈建议采用 ESP32、STM32 等 32 位高性能 MCU，或将轨迹规划放在上位机（如树莓派），Arduino 仅负责底层的电机 FOC 执行。
10. 运动学参数匹配与过冲风险
    贝塞尔曲线的曲率必须与机器人的最大线速度、最大加速度及转弯半径严格匹配。如果规划的曲线曲率过大，而机器人的 BLDC 电机响应跟不上，会导致严重的轨迹跟踪误差甚至失控冲出路径。在代码中必须加入曲率限制与速度前瞻（Look-ahead）算法，根据前方弯道动态降低目标速度。
11. 控制频率与离散化误差
    贝塞尔曲线是连续的，但 MCU 的控制是离散的时间步。如果控制循环的频率过低，会导致轨迹离散化误差变大，机器人运动出现顿挫感。必须确保 motor.loopFOC() 和轨迹插值函数在高频定时器中断中稳定运行，严禁在主循环中使用 delay() 等阻塞函数。
12. 传感器噪声与反馈延迟
    平滑的轨迹控制高度依赖精确的反馈。如果编码器或 IMU 数据存在高频噪声，PID 控制器会产生不必要的频繁修正，破坏 S 型曲线的平滑性。必须在反馈回路中加入滑动平均或卡尔曼滤波；同时，需严格标定系统延迟，确保下发的速度指令与当前的实际位姿在时间戳上对齐。
13. 异常处理与安全降级
    在复杂的动态环境中，如果机器人因打滑或碰撞严重偏离了贝塞尔参考轨迹，强行跟踪可能会导致机械结构受损或电机过流。系统必须设计轨迹偏差监控机制，当误差超过安全阈值时，应立即触发安全降级策略（如减速悬停或重新规划路径），而不是盲目加大 PID 输出来追赶轨迹。

1、贝塞尔曲线路径生成与插值执行（差速底盘点对点移动）
适用场景：机器人需在狭窄或非结构化环境中平滑移动，通过三次贝塞尔曲线规划A点到B点的路径，使运动轨迹连续、无急转。

核心逻辑：定义四个控制点（起点P0、起点方向控制P1、终点方向控制P2、终点P3），通过三次贝塞尔公式实时计算插值点，将插值点转换为速度指令驱动差速底盘。该方案参考了贝塞尔曲线在足式机器人足端轨迹和机械臂控制中的工程实践。

#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; // 总执行时间(秒)
};

BezierPath currentPath;
float pathProgress = 0.0;  // 归一化进度 0~1
bool isExecuting = false;

// ==================== 三次贝塞尔公式 ====================
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;
    float b1 = 3 * u * u * t;
    float b2 = 3 * u * t * t;
    float 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];
}

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;  // 4秒走完
    isExecuting = true;
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    if (isExecuting) {
        // 计算当前进度
        pathProgress += 0.01 / currentPath.duration;  // 假设每帧0.01秒
        if (pathProgress >= 1.0) {
            pathProgress = 1.0;
            isExecuting = false;
        }
        
        // 计算当前位置
        float pos[2];
        bezierCubic(pathProgress, 
                    currentPath.P0, currentPath.P1, 
                    currentPath.P2, currentPath.P3, pos);
        
        // 差速驱动（到达目标点）
        float dx = pos[0] - 0;  // 假设起始位置为原点
        float dy = pos[1] - 0;
        float dist = sqrt(dx*dx + dy*dy);
        
        // 线性速度控制
        float speed = constrain(dist * 0.4, 0.1, 0.6);
        float angle = atan2(dy, dx);
        
        float wheelBase = 0.25;
        motorL.move(speed - angle * wheelBase / 2);
        motorR.move(speed + angle * wheelBase / 2);
        
        Serial.print("Progress:"); Serial.print(pathProgress);
        Serial.print(" X:"); Serial.print(pos[0]);
        Serial.print(" Y:"); Serial.println(pos[1]);
    }
    delay(10);
}

2、S型速度曲线优化器（面向对象封装 + 差速底盘集成）
适用场景：机器人启动/停止或速度切换时，通过S型速度规划消除加速度突变，避免货物倾倒、轮胎打滑和机械冲击。此方案常用于工业级机器人保护机械结构和负载。

核心逻辑：构建S型速度规划器类，通过限制加加速度（Jerk）实现加速度连续变化，使速度从当前值平滑过渡到目标值。将规划器集成到差速底盘类中，左右轮速度指令独立平滑。

#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);

// ==================== S型速度规划器类 ====================
class SCurvePlanner {
private:
    float current_vel;
    float target_vel;
    float max_accel;
    float max_jerk;
    float dt;
    float current_accel;
    
public:
    SCurvePlanner(float accel, float jerk, float dt_step) {
        max_accel = accel;
        max_jerk = jerk;
        dt = dt_step;
        current_vel = 0;
        target_vel = 0;
        current_accel = 0;
    }
    
    void setTarget(float vel) {
        target_vel = vel;
    }
    
    float update() {
        float error = target_vel - current_vel;
        
        // 基于误差计算期望加速度（P控制）
        float desired_accel = error * 2.0;
        
        // 限制加加速度（Jerk），实现S型平滑
        float jerk = (desired_accel - current_accel) / dt;
        if (jerk > max_jerk) jerk = max_jerk;
        if (jerk < -max_jerk) jerk = -max_jerk;
        
        current_accel += jerk * dt;
        
        // 限制加速度
        if (current_accel > max_accel) current_accel = max_accel;
        if (current_accel < -max_accel) current_accel = -max_accel;
        
        // 更新速度
        current_vel += current_accel * dt;
        
        // 到达目标值的死区处理
        if (abs(error) < 0.005) {
            current_vel = target_vel;
            current_accel = 0;
        }
        
        return current_vel;
    }
    
    float getCurrent() { return current_vel; }
};

// ==================== 差速底盘类（集成S型规划）====================
class DifferentialDrive {
private:
    float wheel_base;
    float wheel_radius;
    SCurvePlanner plannerLeft;
    SCurvePlanner plannerRight;
    
public:
    DifferentialDrive(float base, float radius) 
        : wheel_base(base), wheel_radius(radius),
          plannerLeft(1.0, 2.0, 0.01),
          plannerRight(1.0, 2.0, 0.01) {}
    
    void setTargetVelocity(float v, float w) {
        // 差速解算
        float v_left = v - (w * wheel_base / 2.0);
        float v_right = v + (w * wheel_base / 2.0);
        
        plannerLeft.setTarget(v_left);
        plannerRight.setTarget(v_right);
    }
    
    void update() {
        // 获取平滑后的速度
        float smoothLeft = plannerLeft.update();
        float smoothRight = plannerRight.update();
        
        // 转换为电机转速指令
        float rpm_left = (smoothLeft / (2 * PI * wheel_radius)) * 60.0;
        float rpm_right = (smoothRight / (2 * PI * wheel_radius)) * 60.0;
        
        // 发送给BLDC（此处已转速度，实际需结合编码器闭环）
        motorL.move(rpm_left / 100.0);   // 假设最大100 RPM
        motorR.move(rpm_right / 100.0);
    }
};

DifferentialDrive robot(0.30, 0.05);

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;
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 模拟速度指令变化（前进+转向）
    static float phase = 0;
    phase += 0.02;
    float v = 0.4 + 0.2 * sin(phase);
    float w = 0.3 * cos(phase * 0.5);
    
    robot.setTargetVelocity(v, w);
    robot.update();
    
    delay(10);
}

3、贝塞尔路径 + S型速度规划联合控制（完整运动框架）
适用场景：机器人在动态环境中执行精确的曲线路径跟踪，同时需要S型速度曲线保证启停平稳。该框架兼顾路径平滑性与速度连续性，适合机械臂末端轨迹控制和移动机器人路径跟随。

核心逻辑：将贝塞尔曲线路径点与S型速度规划器结合——S型规划器输出时间维度上的平滑速度/位置指令，贝塞尔曲线将位置指令映射到笛卡尔空间坐标。两者协同工作，实现“速度平滑 + 路径平滑”的双重优化。

#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 BezierCurve {
    float P0[2], P1[2], P2[2], P3[2];
    float totalLength;
};

// S型速度规划器（同案例二，省略重复代码）
class SCurvePlanner { /* ... */ };

// ==================== 联合控制类 ====================
class BezierWithSmoothSpeed {
private:
    BezierCurve curve;
    SCurvePlanner speedPlanner;
    float totalTime;      // 规划总时长
    float elapsedTime;
    bool active;
    
public:
    BezierWithSmoothSpeed(float accel, float jerk) 
        : speedPlanner(accel, jerk, 0.01), elapsedTime(0), active(false) {}
    
    void setPath(BezierCurve path, float duration) {
        curve = path;
        totalTime = duration;
        elapsedTime = 0;
        active = true;
        speedPlanner.setTarget(1.0);  // 目标归一化进度为1
    }
    
    void update() {
        if (!active) return;
        
        elapsedTime += 0.01;
        float progress = elapsedTime / totalTime;
        
        if (progress >= 1.0) {
            progress = 1.0;
            active = false;
        }
        
        // 使用S型规划器平滑归一化进度（等效于速度平滑）
        float smoothProgress = speedPlanner.update();
        // 对进度进行约束：S型规划器输出应确保终点对齐
        float t = constrain(smoothProgress, 0, 1.0);
        
        // 计算贝塞尔路径点
        float pos[2];
        bezierCubic(t, curve.P0, curve.P1, curve.P2, curve.P3, pos);
        
        // 差速驱动
        float dx = pos[0] - 0;
        float dy = pos[1] - 0;
        float dist = sqrt(dx*dx + dy*dy);
        
        // 速度由S型规划器直接给出平滑速度值
        float speed = speedPlanner.getCurrent() * 0.6;  // 最大速度缩放
        if (dist < 0.1) speed = 0;
        
        float angle = atan2(dy, dx);
        float wheelBase = 0.25;
        motorL.move(speed - angle * wheelBase / 2);
        motorR.move(speed + angle * wheelBase / 2);
    }
};

BezierWithSmoothSpeed controller(1.5, 3.0, 0.01);

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;
    
    // 定义一条S型贝塞尔路径
    BezierCurve path = {
        {0.0, 0.0},
        {1.5, 2.0},
        {3.0, 2.0},
        {4.5, 0.0}
    };
    controller.setPath(path, 5.0);  // 5秒完成
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    controller.update();
    delay(10);
}

要点解读
贝塞尔曲线解决“路径连续性”，S型曲线解决“速度连续性”：贝塞尔曲线的凸包性和端点插值特性使路径本身C²连续（加速度连续），而S型速度规划通过限制加加速度（Jerk）解决速度指令突变问题。两者配合可同时避免路径急转和机械冲击，是工业级运动控制的标准组合方案。

面向对象封装是工程化落地的关键：将贝塞尔计算、S型规划器、差速底盘分别封装为类，可显著提升代码可复用性和可维护性。案例二的SCurvePlanner与DifferentialDrive解耦设计，使速度优化算法可独立移植到不同底盘模型。

非阻塞计时是实时控制的基石：所有速度规划和路径插值必须使用millis()或micros()进行基于时间的进度计算，严禁使用delay()阻塞主循环。BLDC的FOC控制需要持续高频运行（建议1kHz以上），阻塞会导致电机失控或振荡。

BLDC FOC闭环是“理论曲线落地”的执行保障：S型规划器输出的是“速度指令”，而非“PWM占空比”。SimpleFOC库通过FOC算法和编码器反馈实现速度闭环，确保实际转速精确跟踪指令，避免负载变化导致实际曲线变形。开环控制会因负载波动使速度曲线偏离规划值。

计算优化策略应对Arduino资源瓶颈：贝塞尔公式中的pow()和立方运算在8位Arduino上开销较大。优化手段包括：预计算查找表（将t离散化为256个点）、定点数替代浮点数、以及采用ESP32/Teensy等高性能平台。控制周期建议≥50Hz，过高的插值频率可能耗尽CPU资源。

4、仓储AGV仓储货架取货路径平滑控制（3次贝塞尔轨迹+S曲线速度规划）
适用场景：仓储AGV在密集货架间取货，需频繁转弯、启停，传统直线轨迹易产生机械冲击，导致货架晃动、定位偏差，且影响电机与机械结构寿命。本案例通过3次贝塞尔曲线规划取货路径，搭配S型速度曲线控制加减速，实现AGV的柔顺启动、平稳转弯、精准停靠，适配货架通道狭窄、取货路径多变的仓储环境。

核心逻辑：以货架取货点、AGV起点为核心控制点，构建3次贝塞尔曲线作为运动轨迹；通过等参数间隔采样生成离散轨迹点，结合S曲线速度规划算法，确定每个轨迹点的最优速度，实现速度曲线与轨迹的耦合；BLDC电机依据规划的轨迹点与速度，通过位置闭环控制实现精准跟踪，保障启动、转弯、停靠的平顺性。

// 仓储AGV：贝塞尔曲线+S曲线速度规划路径控制程序（Arduino + SimpleFOC）
#include <Arduino.h>
#include <SimpleFOC.h>
#include <vector>
#include <cmath>

// 贝塞尔曲线核心配置
#define BEZIER_DEGREE 3                 // 贝塞尔曲线阶数（3次曲线满足平滑拐弯需求）
#define PARAM_STEP 0.02f                // 曲线参数采样步长（越小轨迹越细腻，计算量越大）
#define MAX_TRAJECTORY_POINTS 500       // 最大轨迹点存储量（适配仓储AGV中长距离路径）

// S曲线速度规划配置
#define MAX_VELOCITY 0.5f               // 最大运行速度（m/s，适配AGV仓储低速场景）
#define MAX_ACCELERATION 0.3f           // 最大加速度（m/s²，根据载重、电机扭矩设置）
#define MAX_JERK 1.0f                   // 最大加加速度（m/s³，控制启动冲击）

// 仓储场景坐标系定义（单位：米，适配货架通道布局）
struct Point2D {
  float x;
  float y;
};

// 轨迹规划节点
struct TrajectoryNode {
  float x;
  float y;
  float velocity;   // 该点目标速度（m/s）
  float timestamp;  // 规划时间戳（ms）
};

// AGV运动状态
struct AGVStatus {
  Point2D currentPos;  // 当前坐标
  float currentVelocity; // 当前实际速度
  bool isTaskRunning;   // 任务执行状态
  BLDCMotor motor;      // BLDC电机对象
} agvStatus;

// 全局变量
Point2D controlPoints[BEZIER_DEGREE + 1]; // 贝塞尔控制点
std::vector<TrajectoryNode> trajectoryPoints;

// 计算3次贝塞尔曲线坐标（t∈[0,1]）
Point2D calculateBezier3(float t) {
  float t1 = 1 - t;
  float t2 = t1 * t1;
  float t3 = t2 * t1;
  float t4 = t * t * t;
  
  Point2D result;
  result.x = t3 * controlPoints[0].x + 3 * t2 * t * controlPoints[1].x + 3 * t1 * t * t * controlPoints[2].x + t4 * controlPoints[3].x;
  result.y = t3 * controlPoints[0].y + 3 * t2 * t * controlPoints[1].y + 3 * t1 * t * t * controlPoints[2].y + t4 * controlPoints[3].y;
  return result;
}

// S曲线速度规划（输入起点速度、终点速度、总位移，输出速度曲线）
std::vector<float> sCurveVelocityPlan(float v0, float vf, float totalDisplacement) {
  std::vector<float> velocityProfile;
  float t = 0.0f;
  float currentDisplacement = 0.0f;
  float currentVelocity = v0;
  float currentAcceleration = 0.0f;
  float jerkDirection = 1.0f; // 加加速度方向：1=加速阶段，-1=减速阶段
  
  while (currentDisplacement < totalDisplacement) {
    // 加加速度阶段（S曲线核心：避免加速度突变）
    if (currentVelocity < MAX_VELOCITY && currentAcceleration < MAX_ACCELERATION) {
      currentAcceleration += MAX_JERK * jerkDirection * 0.01f; // 时间步长0.01s
      if (currentAcceleration >= MAX_ACCELERATION) {
        currentAcceleration = MAX_ACCELERATION;
        jerkDirection = 0; // 进入匀加速阶段
      }
    } else if (currentVelocity >= MAX_VELOCITY || currentAcceleration >= MAX_ACCELERATION) {
      jerkDirection = 0; // 匀速或匀加速阶段
    }
    
    // 减速阶段预判（到达终点前启动减速）
    float remainingDisplacement = totalDisplacement - currentDisplacement;
    float requiredVelocity = sqrt(2 * MAX_ACCELERATION * remainingDisplacement);
    if (remainingDisplacement < (MAX_VELOCITY * MAX_VELOCITY) / (2 * MAX_ACCELERATION)) {
      jerkDirection = -1; // 切换至减速阶段
      if (currentAcceleration > -MAX_ACCELERATION) {
        currentAcceleration -= MAX_JERK * 0.01f;
        if (currentAcceleration < -MAX_ACCELERATION) {
          currentAcceleration = -MAX_ACCELERATION;
        }
      }
      if (currentVelocity + currentAcceleration * 0.01f < requiredVelocity) {
        currentVelocity = requiredVelocity;
      }
    }
    
    // 速度与位移更新
    currentVelocity += currentAcceleration * 0.01f;
    currentVelocity = constrain(currentVelocity, 0, MAX_VELOCITY);
    currentDisplacement += currentVelocity * 0.01f;
    velocityProfile.push_back(currentVelocity);
    t += 0.01f;
  }
  
  // 确保终点速度准确
  if (!velocityProfile.empty()) {
    velocityProfile.back() = vf;
  }
  return velocityProfile;
}

// 生成贝塞尔轨迹+速度曲线（核心规划函数）
void generateTrajectory() {
  trajectoryPoints.clear();
  float totalDisplacement = 0.0f;
  Point2D prevPoint = controlPoints[0];
  
  // 计算轨迹总长度（用于速度规划）
  for (float t = PARAM_STEP; t <= 1.0f; t += PARAM_STEP) {
    Point2D currentPoint = calculateBezier3(t);
    totalDisplacement += sqrt(pow(currentPoint.x - prevPoint.x, 2) + pow(currentPoint.y - prevPoint.y, 2));
    prevPoint = currentPoint;
  }
  
  // 生成带速度的轨迹点
  std::vector<float> velocityProfile = sCurveVelocityPlan(0.0f, 0.0f, totalDisplacement);
  int velocityIndex = 0;
  float accumulatedDisplacement = 0.0f;
  prevPoint = controlPoints[0];
  
  for (float t = 0.0f; t <= 1.0f; t += PARAM_STEP) {
    Point2D currentPoint = calculateBezier3(t);
    float segmentDisplacement = sqrt(pow(currentPoint.x - prevPoint.x, 2) + pow(currentPoint.y - prevPoint.y, 2));
    accumulatedDisplacement += segmentDisplacement;
    
    // 匹配速度曲线
    while (velocityIndex < velocityProfile.size() && accumulatedDisplacement >= accumulatedDisplacement) {
      if (velocityIndex >= velocityProfile.size()) break;
      velocityIndex++;
    }
    
    float targetVelocity = 0.0f;
    if (velocityIndex < velocityProfile.size()) {
      targetVelocity = velocityProfile[velocityIndex];
    } else {
      targetVelocity = 0.0f;
    }
    
    trajectoryPoints.push_back({currentPoint.x, currentPoint.y, targetVelocity, millis()});
    prevPoint = currentPoint;
  }
}

// BLDC电机位置闭环控制（将轨迹点映射为电机脉冲）
void motorPositionTrack(const TrajectoryNode& targetNode) {
  // 简化映射：将x坐标映射为电机位置（实际需结合编码器、减速比、机械传动换算）
  // 假设电机每脉冲对应0.001m的位移，目标位置为targetNode.x对应的脉冲数
  float targetPosition = targetNode.x * 1000; // 米转脉冲（假设1m=1000脉冲）
  float targetVelocity = targetNode.velocity * 100; // 米/秒转脉冲/秒（假设1m/s=100脉冲/秒）
  
  // 设置电机目标位置与速度
  agvStatus.motor.target = targetVelocity; // 速度模式（实际可切换位置模式）
  agvStatus.motor.loopFOC();
  
  // 实时监测位置偏差（简化逻辑，实际需结合编码器反馈闭环控制）
  float currentPosition = agvStatus.motor.shaft_angle * 100; // 角度转脉冲（假设360°=36000脉冲）
  float positionError = targetPosition - currentPosition;
  
  // 位置PID调节（示例简化，实际需完善PID参数）
  if (abs(positionError) > 5) { // 允许5脉冲的误差带
    agvStatus.motor.target = targetVelocity * (positionError / 50.0f); // 偏差越大，速度调节越明显
  } else {
    agvStatus.motor.target = 0; // 到达目标位置，停止电机
    agvStatus.isTaskRunning = false;
  }
}

// 执行取货路径任务
void executePickupTask() {
  if (trajectoryPoints.empty()) {
    Serial.println("轨迹未规划，无法执行任务");
    return;
  }
  
  agvStatus.isTaskRunning = true;
  for (size_t i = 0; i < trajectoryPoints.size(); i++) {
    if (!agvStatus.isTaskRunning) break;
    
    Serial.print("执行轨迹点：(");
    Serial.print(trajectoryPoints[i].x);
    Serial.print(", ");
    Serial.print(trajectoryPoints[i].y);
    Serial.print(")，目标速度：");
    Serial.print(trajectoryPoints[i].velocity);
    Serial.println("m/s");
    
    motorPositionTrack(trajectoryPoints[i]);
    delay(10); // 短延时保证电机响应，实际需根据电机响应速度调整
  }
  
  // 任务完成确认
  if (agvStatus.isTaskRunning) {
    agvStatus.isTaskRunning = false;
    agvStatus.motor.target = 0;
    Serial.println("取货路径执行完成");
  }
}

// BLDC电机初始化
void initBLDCMotor() {
  BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8); // PWM引脚+使能引脚
  EncoderAnalog encoder = EncoderAnalog(A1, A2, 1000, 2.1f); // 模拟编码器（按需替换为增量式编码器）
  
  agvStatus.motor.linkDriver(&driver);
  agvStatus.motor.linkSensor(&encoder);
  agvStatus.motor.voltage_limit = 12.0f; // 工作电压，按机器人供电调整
  agvStatus.motor.PID_velocity.P = 0.01f;
  agvStatus.motor.PID_velocity.I = 0.1f;
  agvStatus.motor.PID_velocity.D = 0.0f;
  agvStatus.motor.LPF_velocity.Tf = 0.01f;
  
  agvStatus.motor.init();
  agvStatus.motor.initFOC();
  Serial.println("BLDC电机初始化完成");
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // 初始化AGV状态
  agvStatus.currentPos = {0.0f, 0.0f};
  agvStatus.currentVelocity = 0.0f;
  agvStatus.isTaskRunning = false;
  
  // 初始化BLDC电机
  initBLDCMotor();
  
  // 配置仓储取货路径的贝塞尔控制点（示例：从(0,0)到(10,5)，中间设置2个控制点形成平滑拐弯）
  controlPoints[0] = {0.0f, 0.0f};   // 起点：AGV初始位置
  controlPoints[1] = {2.0f, 1.0f};   // 控制点1：引导轨迹平滑转向
  controlPoints[2] = {8.0f, 4.0f};   // 控制点2：维持平滑曲线走向
  controlPoints[3] = {10.0f, 5.0f};  // 终点：货架取货点
  
  // 生成轨迹与速度曲线
  generateTrajectory();
  Serial.println("仓储取货轨迹规划完成");
}

void loop() {
  if (!agvStatus.isTaskRunning) {
    executePickupTask();
  }
  delay(100);
}

代码逻辑说明：
轨迹规划核心：采用3次贝塞尔曲线贴合货架通道的转弯需求，通过参数均匀采样生成细腻轨迹，避免直线拐弯的尖锐棱角，从根源降低机械冲击；
速度曲线融合：S型速度规划解决传统梯形速度的加速度突变问题，启动阶段通过加加速度平滑过渡，靠近终点预判减速，实现“软启动-匀速-软停靠”，与贝塞尔轨迹形成“位置-速度”双重平滑；
闭环控制适配：将轨迹点的坐标映射为BLDC电机的位置目标，通过闭环控制实现轨迹跟踪，兼顾AGV的定位精度与运动平顺性，适配仓储AGV高频启停的核心需求。

5、流水线协作机器人多段轨迹连续搬运（分段贝塞尔拼接+速度平滑衔接）
适用场景：流水线协作机器人需在多个工位间连续搬运物料，轨迹需衔接多个点位，传统多段直线拼接存在速度突变，导致机器人抖动、物料滑落，且影响机械臂使用寿命。本案例采用分段贝塞尔曲线拼接多段轨迹，搭配速度平滑衔接算法，实现工位间无冲击过渡，保障物料搬运的稳定性与连续性，适配流水线多工位连续作业场景。

核心逻辑：将流水线多个搬运工位（起点、中间工位、终点）作为控制点，通过分段贝塞尔曲线拼接形成完整轨迹，确保各曲线段在连接点处一阶、二阶连续；同时在分段点设置速度衔接策略，通过速度过渡函数实现相邻曲线段的速度平滑匹配，避免速度突变；BLDC电机依据拼接后的完整轨迹与速度曲线，实现多段连续运动的无冲击切换，保障物料搬运的连贯性。

// 流水线协作机器人：分段贝塞尔拼接+速度平滑衔接程序（Arduino + SimpleFOC）
#include <Arduino.h>
#include <SimpleFOC.h>
#include <vector>
#include <cmath>

// 分段贝塞尔与速度衔接配置
#define SEGMENT_COUNT 3                 // 轨迹段数（适配3个工位：起点→工位1→工位2→终点）
#define SMOOTH_TRANSITION_DURATION 0.5f // 速度过渡时长（秒，控制衔接平顺性）
#define VELOCITY_SMOOTH_FACTOR 0.1f     // 速度平滑系数（越小衔接越平缓，过小影响效率）

// 工位点定义
struct Workstation {
  Point2D pos;
  float entryVelocity;  // 进入该工位的目标速度
  float exitVelocity;   // 离开该工位的目标速度
};

// 轨迹段结构（分段贝塞尔曲线）
struct TrajectorySegment {
  Point2D controlPoints[4]; // 3次贝塞尔控制点（起点、2个中间控制点、终点）
  std::vector<TrajectoryNode> nodes; // 该段的轨迹点集合
  float startVelocity;
  float endVelocity;
};

// 流水线机器人状态
struct AssemblyRobotStatus {
  int currentSegment; // 当前执行的轨迹段
  float currentVelocity;
  bool isRunning;
  BLDCMotor motor;
} robotStatus;

// 全局变量
Workstation workstations[SEGMENT_COUNT + 1];
std::vector<TrajectorySegment> trajectorySegments;

// 3次贝塞尔曲线计算（同案例1，复用核心公式）
Point2D calculateBezier3(const Point2D cp[4], float t) {
  float t1 = 1 - t;
  float t2 = t1 * t1;
  float t3 = t2 * t1;
  float t4 = t * t * t;
  
  Point2D result;
  result.x = t3 * cp[0].x + 3 * t2 * t * cp[1].x + 3 * t1 * t * t * cp[2].x + t4 * cp[3].x;
  result.y = t3 * cp[0].y + 3 * t2 * t * cp[1].y + 3 * t1 * t * t * cp[2].y + t4 * cp[3].y;
  return result;
}

// 计算轨迹段长度
float calculateSegmentLength(const TrajectorySegment& segment) {
  float length = 0.0f;
  Point2D prev = segment.controlPoints[0];
  for (float t = 0.01f; t <= 1.0f; t += 0.01f) {
    Point2D current = calculateBezier3(segment.controlPoints, t);
    length += sqrt(pow(current.x - prev.x, 2) + pow(current.y - prev.y, 2));
    prev = current;
  }
  return length;
}

// 速度平滑衔接函数（相邻轨迹段速度过渡）
float smoothVelocityTransition(float prevVelocity, float nextVelocity, float progress) {
  // 采用线性插值实现平滑过渡，progress∈[0,1]为过渡进度
  return prevVelocity + (nextVelocity - prevVelocity) * progress * VELOCITY_SMOOTH_FACTOR;
}

// 生成分段贝塞尔轨迹（核心规划函数）
void generateSegmentedTrajectory() {
  trajectorySegments.clear();
  
  // 构建轨迹段：每段连接相邻两个工位，控制点根据工位位置与流水线布局调整
  for (int i = 0; i < SEGMENT_COUNT; i++) {
    TrajectorySegment segment;
    // 起点为当前工位，终点为下一工位
    segment.controlPoints[0] = workstations[i].pos;
    segment.controlPoints[3] = workstations[i + 1].pos;
    
    // 计算中间控制点（确保曲线平滑，与流水线方向对齐）
    Point2D dir = {workstations[i + 1].pos.x - workstations[i].pos.x, 
                   workstations[i + 1].pos.y - workstations[i].pos.y};
    float length = sqrt(dir.x * dir.x + dir.y * dir.y);
    float nx = dir.x / length;
    float ny = dir.y / length;
    
    // 中间控制点：位于起点与终点之间，偏移垂直方向（形成平滑曲线）
    float midDist = length / 2;
    float offset = length * 0.1f; // 垂直偏移量，控制曲线弯曲程度
    segment.controlPoints[1] = {
      workstations[i].pos.x + midDist * 0.3f * nx - offset * ny,
      workstations[i].pos.y + midDist * 0.3f * ny + offset * nx
    };
    segment.controlPoints[2] = {
      workstations[i].pos.x + midDist * 0.7f * nx + offset * ny,
      workstations[i].pos.y + midDist * 0.7f * ny - offset * nx
    };
    
    // 设置速度边界（进入速度为当前工位的出口速度，离开速度为下一工位的入口速度）
    segment.startVelocity = workstations[i].exitVelocity;
    segment.endVelocity = workstations[i + 1].entryVelocity;
    
    // 生成该段的轨迹点（带速度）
    float segmentLength = calculateSegmentLength(segment);
    float progressStep = 1.0f / (segmentLength * 10); // 按长度步进，保证速度曲线均匀
    float progress = 0.0f;
    
    while (progress < 1.0f) {
      Point2D pos = calculateBezier3(segment.controlPoints, progress);
      // 速度从startVelocity平滑过渡到endVelocity
      float velocity = smoothVelocityTransition(segment.startVelocity, segment.endVelocity, progress);
      segment.nodes.push_back({pos.x, pos.y, velocity, millis()});
      progress += progressStep;
      if (progress > 1.0f) progress = 1.0f;
    }
    
    trajectorySegments.push_back(segment);
  }
}

// 分段轨迹连续执行
void executeSegmentedTrajectory() {
  if (robotStatus.currentSegment >= SEGMENT_COUNT) {
    robotStatus.isRunning = false;
    Serial.println("所有轨迹段执行完成");
    robotStatus.motor.target = 0;
    return;
  }
  
  TrajectorySegment& currentSegment = trajectorySegments[robotStatus.currentSegment];
  if (currentSegment.nodes.empty()) {
    robotStatus.currentSegment++;
    executeSegmentedTrajectory();
    return;
  }
  
  // 执行当前段的轨迹点
  for (size_t i = 0; i < currentSegment.nodes.size(); i++) {
    if (robotStatus.currentSegment >= SEGMENT_COUNT) break;
    
    const TrajectoryNode& node = currentSegment.nodes[i];
    // 速度映射为电机目标速度
    robotStatus.motor.target = node.velocity * 100; // 米/秒转脉冲/秒（适配案例1的映射关系）
    robotStatus.currentVelocity = node.velocity;
    
    Serial.print("段");
    Serial.print(robotStatus.currentSegment + 1);
    Serial.print(" 执行点：(");
    Serial.print(node.x);
    Serial.print(", ");
    Serial.print(node.y);
    Serial.print(")，速度：");
    Serial.print(node.velocity);
    Serial.println("m/s");
    
    robotStatus.motor.loopFOC();
    delay(10);
    
    // 判断是否到达当前段终点
    if (i >= currentSegment.nodes.size() - 1) {
      robotStatus.currentSegment++;
      executeSegmentedTrajectory();
      break;
    }
  }
}

// BLDC电机初始化（适配流水线协作机器人高响应需求）
void initAssemblyMotor() {
  BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
  EncoderAnalog encoder = EncoderAnalog(A1, A2, 1000, 2.1f);
  
  robotStatus.motor.linkDriver(&driver);
  robotStatus.motor.linkSensor(&encoder);
  robotStatus.motor.voltage_limit = 12.0f;
  // 流水线协作需更快的响应速度，调整PID参数
  robotStatus.motor.PID_velocity.P = 0.02f;
  robotStatus.motor.PID_velocity.I = 0.15f;
  robotStatus.motor.PID_velocity.D = 0.0f;
  robotStatus.motor.LPF_velocity.Tf = 0.005f;
  
  robotStatus.motor.init();
  robotStatus.motor.initFOC();
  Serial.println("流水线协作电机初始化完成");
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // 初始化流水线工位（示例：3个搬运工位，坐标与速度适配流水线布局）
  workstations[0] = {{0.0f, 0.0f}, 0.0f, 0.3f};  // 起点：速度从0启动至0.3m/s
  workstations[1] = {{5.0f, 3.0f}, 0.3f, 0.5f};  // 工位1：进入0.3m/s，离开加速至0.5m/s
  workstations[2] = {{10.0f, 6.0f}, 0.5f, 0.3f}; // 工位2：进入0.5m/s，离开减速至0.3m/s
  workstations[3] = {{15.0f, 5.0f}, 0.3f, 0.0f}; // 终点：进入0.3m/s，停靠减速至0
  
  // 初始化机器人状态
  robotStatus.currentSegment = 0;
  robotStatus.currentVelocity = 0.0f;
  robotStatus.isRunning = false;
  
  // 初始化电机
  initAssemblyMotor();
  
  // 生成分段轨迹
  generateSegmentedTrajectory();
  Serial.println("流水线分段轨迹规划完成");
}

void loop() {
  if (!robotStatus.isRunning) {
    robotStatus.isRunning = true;
    robotStatus.currentSegment = 0;
    executeSegmentedTrajectory();
  }
  delay(100);
}

代码逻辑说明：
分段贝塞尔拼接：以流水线工位为控制点，通过中间控制点的合理布局，确保相邻贝塞尔曲线段在连接点处的一阶导数连续（速度方向一致），避免曲线拼接的突变，形成连续平滑的多段轨迹；
速度平滑衔接：针对相邻工位的速度差异，设计基于进度的速度过渡函数，在轨迹段内实现速度的线性平滑过渡，消除分段点的速度突变，解决传统多段轨迹的衔接抖动问题；
连续执行逻辑：通过轨迹段索引实现多段轨迹的顺序执行，自动衔接相邻段的速度与位置，无需额外暂停切换，保障流水线物料搬运的连续性，适配流水线“连续运转、无间断搬运”的核心需求。

6、园区物料转运机器人动态避障轨迹规划（动态贝塞尔修正+变速度避障）
适用场景：园区物料转运机器人在室外场景作业时，面临行人、车辆等动态障碍物，传统静态轨迹无法实时调整，易发生碰撞；且突发避障时速度突变，导致物料倾斜、机器人失衡。本案例通过动态贝塞尔曲线实时修正轨迹，结合变速度避障策略，实现障碍物的精准规避与速度的动态适配，保障园区复杂动态环境下的运输安全与物料稳定。
核心逻辑：通过传感器实时监测环境障碍物，基于障碍物位置与运动趋势，在原有贝塞尔轨迹基础上动态插入或调整控制点，生成避开障碍物的修正轨迹；同时根据障碍物距离、运动状态，实时调整速度曲线，在靠近障碍物时减速，避障后平滑恢复速度，避免速度突变；BLDC电机依据修正后的轨迹与速度，实现动态避障的精准执行，适配园区室外动态环境。

// 园区物料转运：动态贝塞尔修正+变速度避障程序（Arduino + SimpleFOC）
#include <Arduino.h>
#include <SimpleFOC.h>
#include <vector>
#include <cmath>

// 动态避障核心配置
#define SAFE_DISTANCE 1.5f             // 安全避障距离（米，根据机器人尺寸、障碍物类型调整）
#define OBSTACLE_DETECTION_RATE 50     // 障碍物检测频率（ms，实时性越高，响应越快）
#define VELOCITY_DECEL_FACTOR 0.8f     // 靠近障碍物时的减速系数（越小减速越急，避障响应越快）
#define MAX_TRAJECTORY_ADJUST_DEGREE 3 // 轨迹修正的最大阶数（保持曲线平滑）

// 动态障碍物结构
struct DynamicObstacle {
  Point2D position;
  Point2D velocity; // 障碍物运动速度（m/s）
  float radius;     // 障碍物等效半径（米）
  unsigned long lastDetectTime;
};

// 动态轨迹规划结构
struct DynamicTrajectory {
  Point2D originalControlPoints[MAX_TRAJECTORY_ADJUST_DEGREE + 1]; // 原始控制点
  Point2D adjustedControlPoints[MAX_TRAJECTORY_ADJUST_DEGREE + 1]; // 修正后控制点
  std::vector<TrajectoryNode> currentNodes;
  bool isAdjusted;
};

// 园区转运机器人状态
struct TransportRobotStatus {
  Point2D currentPos;
  Point2D targetPos;
  float currentVelocity;
  bool isObstacleDetected;
  DynamicTrajectory trajectory;
  BLDCMotor motor;
} transportStatus;

// 全局变量
std::vector<DynamicObstacle> obstacles;

// 模拟传感器检测障碍物（实际需接入激光雷达、超声波传感器，此处为模拟逻辑）
void detectDynamicObstacles() {
  // 模拟检测：每50ms更新一次障碍物信息，模拟行人、车辆的运动
  unsigned long currentTime = millis();
  if (obstacles.empty()) {
    // 初始化模拟障碍物：行人从右侧向轨迹方向移动
    obstacles.push_back({
      {8.0f, 2.5f},    // 初始位置
      {-0.1f, 0.05f},  // 运动速度（向左移动）
      0.3f,            // 行人等效半径
      currentTime
    });
    // 初始化模拟障碍物：车辆静止在轨迹旁
    obstacles.push_back({
      {5.0f, 4.0f},
      {0.0f, 0.0f},
      0.8f,
      currentTime
    });
  }
  
  // 更新障碍物位置（模拟运动）
  for (auto& obs : obstacles) {
    float deltaTime = (currentTime - obs.lastDetectTime) / 1000.0f;
    obs.position.x += obs.velocity.x * deltaTime;
    obs.position.y += obs.velocity.y * deltaTime;
    obs.lastDetectTime = currentTime;
    
    // 判断是否进入避障区域
    float robotToObstacleDist = sqrt(
      pow(transportStatus.currentPos.x - obs.position.x, 2) + 
      pow(transportStatus.currentPos.y - obs.position.y, 2)
    );
    if (robotToObstacleDist < SAFE_DISTANCE + obs.radius) {
      transportStatus.isObstacleDetected = true;
      Serial.print("检测到障碍物，距离：");
      Serial.print(robotToObstacleDist);
      Serial.println("m，启动避障");
    }
  }
}

// 动态修正贝塞尔控制点（基于障碍物位置调整轨迹）
void adjustBezierControlPoints() {
  if (!transportStatus.isObstacleDetected) {
    // 无障碍物，使用原始控制点
    memcpy(transportStatus.trajectory.adjustedControlPoints,
           transportStatus.trajectory.originalControlPoints,
           sizeof(transportStatus.trajectory.originalControlPoints));
    transportStatus.trajectory.isAdjusted = false;
    return;
  }
  
  // 有障碍物，调整控制点避障：将靠近障碍物的控制点向远离障碍物的方向偏移
  for (int i = 0; i <= MAX_TRAJECTORY_ADJUST_DEGREE; i++) {
    transportStatus.trajectory.adjustedControlPoints[i] = 
      transportStatus.trajectory.originalControlPoints[i];
  }
  
  // 遍历所有障碍物，对靠近的控制点进行偏移
  for (const auto& obs : obstacles) {
    for (int i = 1; i <= MAX_TRAJECTORY_ADJUST_DEGREE - 1; i++) {
      Point2D cp = transportStatus.trajectory.originalControlPoints[i];
      float dist = sqrt(pow(cp.x - obs.position.x, 2) + pow(cp.y - obs.position.y, 2));
      if (dist < SAFE_DISTANCE + obs.radius) {
        // 计算偏移方向：从障碍物指向控制点
        float dx = cp.x - obs.position.x;
        float dy = cp.y - obs.position.y;
        float offsetDist = SAFE_DISTANCE + obs.radius - dist + 0.2f; // 额外偏移0.2m，确保安全
        float length = sqrt(dx * dx + dy * dy);
        if (length > 0) {
          transportStatus.trajectory.adjustedControlPoints[i].x = cp.x + (dx / length) * offsetDist;
          transportStatus.trajectory.adjustedControlPoints[i].y = cp.y + (dy / length) * offsetDist;
        }
      }
    }
  }
  
  transportStatus.trajectory.isAdjusted = true;
  Serial.println("贝塞尔控制点修正完成，生成避障轨迹");
}

// 变速度避障规划（根据障碍物距离调整速度）
float calculateAdjustedVelocity(const TrajectoryNode& node, const DynamicObstacle& obs) {
  float baseVelocity = node.velocity;
  float robotToObstacleDist = sqrt(
    pow(node.x - obs.position.x, 2) + pow(node.y - obs.position.y, 2)
  );
  
  if (robotToObstacleDist < SAFE_DISTANCE + obs.radius) {
    // 靠近障碍物，按距离比例减速，最小速度不低于0.1m/s
    float decelRatio = robotToObstacleDist / (SAFE_DISTANCE + obs.radius);
    return baseVelocity * decelRatio * VELOCITY_DECEL_FACTOR;
  } else if (robotToObstacleDist < 2 * SAFE_DISTANCE + obs.radius) {
    // 接近障碍物，平稳减速
    float decelRatio = (robotToObstacleDist - SAFE_DISTANCE - obs.radius) / (SAFE_DISTANCE);
    return baseVelocity * (0.8f + 0.2f * decelRatio);
  } else {
    // 安全距离外，恢复基础速度
    return baseVelocity;
  }
}

// 生成动态修正轨迹
void generateDynamicTrajectory() {
  transportStatus.trajectory.currentNodes.clear();
  Point2D* controlPoints = transportStatus.trajectory.isAdjusted ?
    transportStatus.trajectory.adjustedControlPoints :
    transportStatus.trajectory.originalControlPoints;
  
  // 生成轨迹点
  for (float t = 0.0f; t <= 1.0f; t += 0.02f) {
    Point2D pos = calculateBezier3(controlPoints, t);
    float baseVelocity = 0.5f; // 基础速度（m/s，适配园区转运场景）
    float adjustedVelocity = baseVelocity;
    
    // 根据障碍物调整速度
    for (const auto& obs : obstacles) {
      adjustedVelocity = calculateAdjustedVelocity({pos.x, pos.y, baseVelocity, millis()}, obs);
    }
    
    transportStatus.trajectory.currentNodes.push_back({pos.x, pos.y, adjustedVelocity, millis()});
  }
}

// 执行动态避障轨迹
void executeDynamicAvoidanceTrajectory() {
  if (transportStatus.trajectory.currentNodes.empty()) {
    Serial.println("动态轨迹未生成，无法执行");
    return;
  }
  
  for (size_t i = 0; i < transportStatus.trajectory.currentNodes.size(); i++) {
    const TrajectoryNode& node = transportStatus.trajectory.currentNodes[i];
    
    // 实时检测障碍物，随时准备修正轨迹
    detectDynamicObstacles();
    if (transportStatus.isObstacleDetected) {
      adjustBezierControlPoints();
      generateDynamicTrajectory();
      i = 0; // 重新执行修正后的轨迹
      continue;
    }
    
    // 更新机器人当前位置
    transportStatus.currentPos = {node.x, node.y};
    transportStatus.currentVelocity = node.velocity;
    
    // 设置电机目标速度
    transportStatus.motor.target = node.velocity * 100;
    transportStatus.motor.loopFOC();
    
    Serial.print("避障轨迹点：(");
    Serial.print(node.x);
    Serial.print(", ");
    Serial.print(node.y);
    Serial.print(")，速度：");
    Serial.print(node.velocity);
    Serial.println("m/s");
    
    delay(20);
  }
  
  // 轨迹执行完成
  transportStatus.isObstacleDetected = false;
  transportStatus.motor.target = 0;
  Serial.println("避障轨迹执行完成，恢复原轨迹");
}

// BLDC电机初始化
void initTransportMotor() {
  BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
  EncoderAnalog encoder = EncoderAnalog(A1, A2, 1000, 2.1f);
  
  transportStatus.motor.linkDriver(&driver);
  transportStatus.motor.linkSensor(&encoder);
  transportStatus.motor.voltage_limit = 12.0f;
  // 避障场景需快速响应，优化PID参数提升动态性能
  transportStatus.motor.PID_velocity.P = 0.015f;
  transportStatus.motor.PID_velocity.I = 0.12f;
  transportStatus.motor.PID_velocity.D = 0.0f;
  transportStatus.motor.LPF_velocity.Tf = 0.008f;
  
  transportStatus.motor.init();
  transportStatus.motor.initFOC();
  Serial.println("园区转运电机初始化完成");
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // 初始化转运任务：起点(0,0)到终点(15,5)的原始贝塞尔控制点
  transportStatus.currentPos = {0.0f, 0.0f};
  transportStatus.targetPos = {15.0f, 5.0f};
  transportStatus.currentVelocity = 0.0f;
  transportStatus.isObstacleDetected = false;
  
  // 原始贝塞尔控制点（适配园区开阔场景）
  transportStatus.trajectory.originalControlPoints[0] = {0.0f, 0.0f};
  transportStatus.trajectory.originalControlPoints[1] = {3.0f, 4.0f};
  transportStatus.trajectory.originalControlPoints[2] = {12.0f, 6.0f};
  transportStatus.trajectory.originalControlPoints[3] = {15.0f, 5.0f};
  
  // 初始化电机
  initTransportMotor();
  
  // 生成初始轨迹
  generateDynamicTrajectory();
  Serial.println("园区转运动态轨迹规划完成");
}

void loop() {
  // 实时障碍物检测
  detectDynamicObstacles();
  
  // 执行动态避障轨迹
  if (!transportStatus.isObstacleDetected || transportStatus.trajectory.isAdjusted) {
    executeDynamicAvoidanceTrajectory();
  }
  
  delay(OBSTACLE_DETECTION_RATE);
}

代码逻辑说明：
动态轨迹修正：通过传感器实时感知障碍物，动态调整贝塞尔曲线的控制点位置，使修正后的轨迹绕过障碍物，且保持曲线的平滑性，避免避障轨迹出现尖锐拐角，保障机器人运动稳定性；
变速度避障适配：根据障碍物与机器人的距离实时调整速度，靠近障碍物时减速，远离时恢复，形成“减速-避障-加速”的平滑速度曲线，避免突发减速或加速导致的物料滑落，兼顾避障安全与运输效率；
实时闭环响应：将障碍物检测与轨迹修正、速度调整形成闭环，每50ms更新一次环境信息，一旦检测到障碍物立即修正轨迹，电机同步调整运动状态，实现对动态环境的快速响应，适配园区室外动态场景的避障需求。

要点解读

1. 贝塞尔曲线的“柔顺性”与机器人运动特性的精准适配
   贝塞尔曲线的核心优势在于高阶光滑性与控制灵活性，通过控制点的调整可实现轨迹的平滑过渡，这与机器人对运动柔顺性的核心需求高度契合，是实现低振动、低冲击运动的基础。
   阶数选择与场景匹配：低阶（2次）曲线适合小角度转弯、轻载场景，计算量小响应快；3次曲线兼顾大角度平滑转弯与计算效率，适配案例4仓储AGV、案例3园区转运等通用场景；高阶曲线虽更细腻，但计算复杂度高，需结合Arduino算力选择，避免实时性不足导致轨迹跟踪滞后。
   控制点与轨迹形态的关联：控制点决定了曲线的弯曲程度与走向，案例4通过控制点的合理布局引导AGV平滑转弯，案例6通过动态调整控制点实现避障，充分体现“控制点驱动轨迹”的优势——无需修改底层机械结构，仅通过控制点调整即可实现轨迹的灵活适配，大幅降低机器人运动控制的开发成本。
   机械应力与振动抑制：贝塞尔曲线的一阶、二阶导数连续，避免了直线轨迹拐弯时的加速度突变，从根源减少启动冲击、转弯振动，降低机械臂、电机、轴承等关键部件的应力，延长机器人使用寿命，尤其适配对振动敏感的精密物料搬运场景。
2. 速度曲线与轨迹曲线的“双耦合”设计：从源头规避运动冲击
   速度曲线决定运动的节奏，轨迹曲线决定运动的路径，二者独立规划易导致“轨迹光滑但速度突变”“速度平缓但轨迹尖锐”的问题，双耦合设计实现位置与速度的同步平滑，是消除运动冲击的关键。
   参数同步：轨迹采样与速度匹配：案例4中，轨迹点的生成与速度曲线的采样保持同步，每个轨迹点对应唯一的速度值，确保机器人在跟踪轨迹时，速度随位置平滑变化，避免在拐点处出现“位置到了但速度没跟上”的抖动，实现“位置-速度”的双重平滑。
   边界约束：速度与轨迹的联动控制：速度曲线的边界需与轨迹的几何特性联动，案例5中，在轨迹分段点设置速度衔接策略，确保相邻轨迹段的速度平滑过渡，避免分段点的速度突变；案例6中，根据障碍物位置调整速度，轨迹绕行的同时速度同步减速，避免避障时的速度突变导致物料散落，实现速度与轨迹的动态适配。
   冲击抑制：加加速度的约束设计：S曲线速度规划的核心是控制加加速度，避免加速度突变，将其与贝塞尔轨迹结合，可从源头抑制启动、停止、转弯时的冲击；案例4通过限制加加速度，使启动过程的加速度缓慢提升，搭配光滑轨迹，实现“软启动-平稳运动-软停止”，彻底消除传统运动控制的冲击痛点。
3. BLDC电机的“闭环控制+动态响应”与算法的实时协同
   BLDC电机是机器人运动的执行核心，其闭环控制精度与动态响应速度直接决定轨迹跟踪效果，需与贝塞尔+速度规划算法深度协同，实现“规划-执行-反馈”的闭环联动，保障算法效果的落地。
   闭环控制模式的匹配：针对不同场景需求，需灵活选择电机控制模式——案例4的仓储取货需精准定位，可采用位置闭环模式，跟踪轨迹点的位置目标；案例3的动态避障需快速响应，可采用速度闭环模式，实时跟踪速度曲线的变化；通过SimpleFOC库的多模式支持，实现模式的灵活切换，适配不同场景的控制需求。
   动态响应速度的匹配：算法的规划周期需与电机的响应速度协同，Arduino的算力有限，需通过简化轨迹采样步长、优化速度规划算法（如案例6的简化避障速度逻辑）减少计算量，同时优化BLDC电机的PID参数，提升动态响应速度，确保电机能快速跟踪规划的轨迹与速度，避免规划与执行脱节。
   负载自适应的动力适配：机器人在运动过程中的负载（如载重、避障阻力）会动态变化，需结合负载实时调整BLDC电机的输出扭矩——案例4的AGV取货根据载重调整电机扭矩，避免重载启动时动力不足；案例6的避障遇阻力时自动提升扭矩，确保轨迹跟踪的稳定性，实现电机动力与负载、运动状态的动态适配。
4. 算法轻量化与Arduino算力的适配：保障实时性与落地性
   Arduino作为资源受限的嵌入式平台，算力、内存有限，算法轻量化是实现实时控制的关键，需在算法复杂度与控制效果之间找到平衡，保障算法在Arduino上的落地可行性。
   贝塞尔算法的简化优化：提前计算贝塞尔曲线的系数（如3次曲线的多项式系数），避免在循环中重复计算，减少计算量；采用均匀参数采样替代复杂的弧长参数化，在保证轨迹精度的前提下降低计算复杂度，适配Arduino的算力水平。
   速度规划的查表法优化：S曲线速度规划的迭代计算复杂度高，可采用查表法提前生成常用场景的速度曲线，运行时直接根据总位移、边界速度调取速度值，大幅减少实时计算量；案例6的变速度避障通过简化距离-速度的映射关系，避免复杂的迭代计算，提升响应速度。
   内存资源的合理分配：限制轨迹点的存储数量，通过动态数组替代静态数组，避免内存溢出；对非核心数据采用简化存储（如仅保留关键轨迹点的速度与坐标），合理分配Arduino的内存资源，保障系统长时间稳定运行。
5. 多场景适配的参数化设计：构建可复用的通用方案
   不同应用场景（仓储、流水线、园区室外）的核心需求差异显著，通过参数化设计，将场景差异转化为可配置的参数，实现一套算法框架适配多场景，提升方案的通用性与复用性。
   核心参数的模块化配置：将贝塞尔曲线的阶数、采样步长，速度曲线的最大速度、加速度、加加速度，避障的安全距离、减速系数等核心参数，集中封装为配置结构体，不同场景只需修改配置参数即可快速适配——仓储场景降低最大速度与加速度，适配密集货架；流水线场景提高速度衔接的平滑系数，保障连续搬运；园区室外场景增大避障安全距离，应对动态障碍物。
   场景-参数的映射规则：建立场景特性与参数的映射关系，例如载重越大，加速度、加加速度越小；障碍物速度越快，避障安全距离越大，减速系数越小；通过这种映射规则，可根据实际场景快速推导最优参数，降低参数调试的难度，缩短开发周期。
   模块化的代码架构：将贝塞尔轨迹生成、速度规划、电机控制等核心功能封装为独立函数，不同场景的程序只需调用对应的模块，无需修改底层算法逻辑，便于代码的维护与二次开发；例如案例2的流水线程序可复用案例1的贝塞尔与速度规划模块，仅需调整分段逻辑与衔接策略，大幅提升开发效率。

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