Arduino BLDC 之狭窄通道配送机器人“贝塞尔曲线路径 + S 型速度平滑”，本质上是“贝塞尔曲线把折线航点变成可行驶连续路径，S 型速度曲线把阶跃速度指令变成加加速度受限的平滑参考，BLDC 底层再通过 FOC 速度/位置闭环跟踪这条时空轨迹”的运动控制闭环：前者解决“走哪条线”，后者解决“以多快的速度、多柔顺的加速度走”，两者结合后，机器人才能在通道宽度有限、转弯空间小、人流密集或货架密集的配送环境中保持低冲击、低打滑、低洒漏和高定位精度。

主要特点

1. 贝塞尔曲线路径解决“折线航点不可直接行驶”的问题
   全局规划器输出的路径通常是折线航点序列，直接跟踪会出现直角转弯、航向突变和瞬时角速度跳变。贝塞尔曲线通过控制点把相邻航点连接成光滑曲线，使曲率连续变化，从而降低差速底盘或阿克曼底盘在转弯时的侧滑和姿态抖动。 在狭窄通道中，控制点位置还应受通道中心线和安全边距约束，避免曲线外凸后剐蹭墙体或货架。
2. S 型速度平滑抑制启停、转向和加减速冲击
   传统梯形加减速在加速度切换点存在阶跃，容易引发电流冲击、货物晃动和定位超调。S 型速度曲线通过限制加加速度，使加速度连续变化，速度曲线呈 S 形过渡。 对 BLDC 系统而言，这意味着 FOC 电流环不需要应对突然增大的转矩指令，电机发热、齿轮箱冲击和底盘振动都会降低。完整 S 型曲线通常可划分为加加速、匀加速、减加速、匀速、加减速、匀减速、减减速等阶段，但在嵌入式实现中常根据距离约束简化为五段或三段。
3. 路径与速度必须做时间参数化耦合
   贝塞尔曲线只给出几何路径，不能直接决定“何时到达哪里”。工程上需要把曲线按弧长离散化，再结合 S 型速度规划生成随时间变化的参考位置和参考速度。这样底盘控制器跟踪的就不是静态路径点，而是一条带时间戳的参考轨迹。若只规划路径不规划速度，机器人在弯道仍可能超速；若只规划速度不规划路径，机器人仍会在直角航点处出现航向跳变。
4. BLDC FOC 是柔顺执行的关键底层
   普通 ESC 或开环速度控制难以精确跟踪 S 型速度参考，尤其在低速启停和微转向时容易出现抖动。BLDC 配合 SimpleFOC 等 FOC 方案后，电流环可提供低转矩脉动的力矩输出，速度环和位置环再分别跟踪 S 型参考速度和贝塞尔路径误差。 对配送机器人而言，这直接影响货物稳定性、停靠精度和通道内微调能力。
5. 分层架构更适合狭窄通道配送落地
   建议分为四层：
   感知层：激光雷达/深度相机、IMU、轮速编码器、UWB 或视觉里程计。
   规划层：全局航点序列、贝塞尔曲线拟合、通道安全边距约束。
   速度层：S 型速度规划、弯道限速、避障降速、动态重规划。
   执行层：BLDC FOC 速度/位置闭环、差速运动学解算、急停保护。
   Arduino Uno/Nano 适合做单电机速度跟随或简单 S 型验证；完整狭窄通道配送系统更适合 ESP32-S3、Teensy 4.1 或 STM32 作为底层运动控制器，上位机负责 SLAM、全局路径和贝塞尔曲线生成。

应用场景
仓储货架间 AGV/AMR 配送：通道窄、货架密、转弯半径小，贝塞尔路径可减少直角转向，S 型速度可降低货物晃动。
餐厅/酒店送餐机器人：人流密集、地毯与瓷砖混合、需频繁启停，S 型平滑能减少汤汁洒漏和乘客惊吓。
医院药品与餐食配送：走廊狭长、病床和推车多，要求低噪声、低冲击和精准停靠。
工厂车间物料搬运：设备间隙小、地面可能有油渍或线缆，平滑路径和柔顺加减速可降低打滑风险。
科研与教学平台：用于验证贝塞尔路径跟踪、S 型速度规划、BLDC FOC 闭环和狭窄空间运动控制。

需要注意的事项
贝塞尔曲线不能脱离通道安全边距
控制点若随意放置，曲线可能外凸到通道墙侧或货架侧。应根据机器人半宽、定位误差和传感器盲区设置安全走廊，再对控制点进行约束或投影。
弯道必须限速，不能只靠路径平滑
即使路径光滑，若进入弯道时线速度过高，差速底盘仍会侧滑。可根据曲率计算允许的最大角速度，并叠加到 S 型速度规划的上限中。
S 型参数需要结合负载和地面条件整定
最大速度、最大加速度和最大加加速度不能只按空载调试。满载、空载、地毯、瓷砖、斜坡工况下，相同参数表现差异很大。应预留负载自适应或工况切换机制。
动态重规划时速度不能跳变
遇到行人、临时障碍或航点更新时，新轨迹的初始速度应与当前实际速度连续，否则 BLDC 速度环会出现阶跃误差，引发电流冲击。可采用从当前状态重新生成 S 型曲线的方式衔接。
Arduino 平台要控制浮点运算负载
贝塞尔曲线求点、弧长参数化和 S 型分段计算都涉及浮点运算。若控制周期为 1~10 ms，应避免在主循环中重复计算高阶公式，可采用查表法、增量更新或预离散化轨迹。
编码器分辨率和 IMU 安装位置影响跟踪质量
低速平滑控制依赖准确的轮速反馈；IMU 若安装在振动源附近，航向估计会受电机和减速器振动干扰。编码器建议选用较高分辨率方案，IMU 应远离电机并做好机械固定。
急停优先级必须高于路径与速度规划
狭窄通道中碰撞风险高，一旦激光雷达、碰撞开关或急停按钮触发，应立即中断贝塞尔跟踪和 S 型规划，进入制动或零力矩状态，而不是继续执行原轨迹。
差速底盘需处理左右轮速度耦合
贝塞尔路径跟踪通常输出线速度和角速度，再解算为左右轮目标速度。若左右轮负载不一致或单侧打滑，应加入交叉修正或电流/速度异常检测，避免路径跟踪跑偏。

1、单段贝塞尔 + 正弦S型简化速度（入门级）
场景说明：仓库直通道中需要从货架A平滑绕行至货架B，通道仅比车身宽30cm。用一段三次贝塞尔曲线规划路径，用正弦函数近似S型速度曲线（计算量小，适合Arduino Uno/ESP32）。

#include <SimpleFOC.h>

// ===== 双轮差速 BLDC =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

// ===== 贝塞尔控制点 =====
struct Point2D { float x, y; };
Point2D P0 = {0.0, 0.0};       // 起点
Point2D P1 = {0.5, 0.3};       // 控制点1（决定初始方向）
Point2D P2 = {1.8, 0.3};       // 控制点2（决定终点方向）
Point2D P3 = {2.3, 0.0};       // 终点

// ===== 运动参数 =====
float duration = 4.0f;         // 总时长(s)
float elapsed = 0.0f;
float maxSpeed = 0.35f;        // 最大线速度(m/s)
float wheelBase = 0.25f;       // 轮距(m)
float wheelRadius = 0.05f;     // 轮半径(m)

void setup() {
  Serial.begin(115200);
  motorL.linkDriver(&driverL);
  motorR.linkDriver(&driverR);
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  Serial.println("狭窄通道配送启动");
}

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

  elapsed += 0.01f;
  if (elapsed > duration) {
    motorL.move(0); motorR.move(0);
    motorL.loopFOC(); motorR.loopFOC();
    return;
  }

  // ===== 1. 归一化进度 =====
  float t = elapsed / duration;

  // ===== 2. S型速度平滑（正弦近似）=====
  // 速度曲线：v = maxSpeed * sin(π * t)，两端为0，中间最大
  float speed = maxSpeed * sin(PI * t);

  // ===== 3. 贝塞尔曲线位置 =====
  float u = 1.0f - t;
  float b0 = u*u*u, b1 = 3*u*u*t, b2 = 3*u*t*t, b3 = t*t*t;
  float curX = b0*P0.x + b1*P1.x + b2*P2.x + b3*P3.x;
  float curY = b0*P0.y + b1*P1.y + b2*P2.y + b3*P3.y;

  // ===== 4. 计算航向角（切线方向）=====
  // 贝塞尔导数：3(1-t)²(P1-P0) + 6(1-t)t(P2-P1) + 3t²(P3-P2)
  float du = 3*u*u*(P1.x-P0.x) + 6*u*t*(P2.x-P1.x) + 3*t*t*(P3.x-P2.x);
  float dv = 3*u*u*(P1.y-P0.y) + 6*u*t*(P2.y-P1.y) + 3*t*t*(P3.y-P2.y);
  float heading = atan2(dv, du);

  // ===== 5. 差速解算 =====
  // 航向变化率 → 角速度
  static float lastHeading = 0;
  float angularVel = (heading - lastHeading) / 0.01f;
  lastHeading = heading;

  float vL = speed - angularVel * wheelBase / 2.0f;
  float vR = speed + angularVel * wheelBase / 2.0f;

  // 线速度 → 轮转速(rad/s) = v / r
  motorL.move(vL / wheelRadius);
  motorR.move(vR / wheelRadius);

  // 调试输出
  static int cnt = 0;
  if (++cnt % 20 == 0) {
    Serial.print("t="); Serial.print(t*100); Serial.print("% ");
    Serial.print("pos=("); Serial.print(curX); Serial.print(","); Serial.print(curY); Serial.print(") ");
    Serial.print("v="); Serial.println(speed);
  }
}

代码要点：sin(PI * t) 实现两端速度为0的平滑启停，等效于简化S曲线，计算量极低。贝塞尔导数计算切线方向，差速解算将线速度与角速度分配到左右轮。适合ESP32及以上主控。

2、分段贝塞尔 + 曲率极值点减速（进阶）
场景说明：狭窄通道中存在多个拐角（如货架之间的S形通道），需要多段贝塞尔拼接且保证连接点处切线连续。参考B-spline在曲率极值点处减速的策略，在每段贝塞尔的高曲率区域自动降速。

#include <SimpleFOC.h>

// ===== 电机 =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

struct Point2D { float x, y; };

// ===== 多段贝塞尔路径（S形通道）=====
#define SEG_CNT 3
struct BezierSeg {
  Point2D p0, p1, p2, p3;
  float estLen;      // 估算长度
  float maxCurv;     // 最大曲率
};

BezierSeg segments[SEG_CNT];

// 初始化：使相邻段在连接点处切线连续（C1连续）
void buildPath() {
  // 段0: (0,0) → (1,0)，上偏
  segments[0].p0 = {0.0, 0.0};
  segments[0].p1 = {0.3, 0.25};
  segments[0].p2 = {0.7, 0.25};
  segments[0].p3 = {1.0, 0.0};

  // 段1: (1,0) → (2,0)，下偏（与段0切线连续）
  segments[1].p0 = {1.0, 0.0};
  // 控制点方向：段0终点切线方向 = P3-P2 = (0.3, -0.25)
  segments[1].p1 = {1.3, -0.25};
  segments[1].p2 = {1.7, -0.25};
  segments[1].p3 = {2.0, 0.0};

  // 段2: (2,0) → (3,0)，上偏
  segments[2].p0 = {2.0, 0.0};
  segments[2].p1 = {2.3, 0.25};
  segments[2].p2 = {2.7, 0.25};
  segments[2].p3 = {3.0, 0.0};

  // 估算每段长度和最大曲率
  for (int i = 0; i < SEG_CNT; i++) {
    segments[i].estLen = 0;
    segments[i].maxCurv = 0;
    Point2D prev = segments[i].p0;
    for (int k = 1; k <= 20; k++) {
      float t = k / 20.0f;
      Point2D cur = bezier3(&segments[i], t);
      segments[i].estLen += sqrt(pow(cur.x-prev.x,2) + pow(cur.y-prev.y,2));
      prev = cur;
    }
  }
}

Point2D bezier3(BezierSeg* s, float t) {
  float u = 1-t;
  return {
    u*u*u*s->p0.x + 3*u*u*t*s->p1.x + 3*u*t*t*s->p2.x + t*t*t*s->p3.x,
    u*u*u*s->p0.y + 3*u*u*t*s->p1.y + 3*u*t*t*s->p2.y + t*t*t*s->p3.y
  };
}

void setup() {
  Serial.begin(115200);
  buildPath();
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  Serial.println("分段贝塞尔路径就绪");
}

// ===== 全局进度 =====
float globalT = 0;           // 0~1 对应整条路径
float segT = 0;              // 当前段内进度
int curSeg = 0;
float totalLen = 0;

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

  // 计算总长度（仅首次）
  static bool lenDone = false;
  if (!lenDone) {
    totalLen = 0;
    for (int i = 0; i < SEG_CNT; i++) totalLen += segments[i].estLen;
    lenDone = true;
  }

  // ===== 1. 沿路径的弧长进度 =====
  float arcSpeed = 0.3f;  // 基础弧长速度(m/s)
  float dt = 0.01f;
  float ds = arcSpeed * dt;
  globalT += ds / totalLen;
  if (globalT >= 1.0f) { globalT = 1.0f; motorL.move(0); motorR.move(0); return; }

  // ===== 2. 定位当前段和段内t =====
  float accum = 0;
  curSeg = 0;
  for (int i = 0; i < SEG_CNT; i++) {
    float segFrac = segments[i].estLen / totalLen;
    if (globalT < accum + segFrac || i == SEG_CNT-1) {
      curSeg = i;
      segT = (globalT - accum) / segFrac;
      break;
    }
    accum += segFrac;
  }

  // ===== 3. 曲率感知减速 =====
  // 曲率 κ ≈ |x'y'' - y'x''| / (x'²+y'²)^1.5
  float u = 1-segT;
  BezierSeg* s = &segments[curSeg];
  // 一阶导
  float dx = 3*u*u*(s->p1.x-s->p0.x) + 6*u*segT*(s->p2.x-s->p1.x) + 3*segT*segT*(s->p3.x-s->p2.x);
  float dy = 3*u*u*(s->p1.y-s->p0.y) + 6*u*segT*(s->p2.y-s->p1.y) + 3*segT*segT*(s->p3.y-s->p2.y);
  // 二阶导
  float ddx = 6*u*(s->p2.x-2*s->p1.x+s->p0.x) + 6*segT*(s->p3.x-2*s->p2.x+s->p1.x);
  float ddy = 6*u*(s->p2.y-2*s->p1.y+s->p0.y) + 6*segT*(s->p3.y-2*s->p2.y+s->p1.y);
  float denom = pow(dx*dx+dy*dy, 1.5f);
  float curv = denom > 0.001f ? abs(dx*ddy - dy*ddx) / denom : 0;

  // 曲率越大，速度越低：v = vBase / (1 + K*curv)
  float speedFactor = 1.0f / (1.0f + 5.0f * curv);
  float speed = arcSpeed * speedFactor;

  // ===== 4. 位置与差速 =====
  Point2D pos = bezier3(s, segT);
  float heading = atan2(dy, dx);
  static float lastHeading = 0;
  float angVel = (heading - lastHeading) / dt;
  lastHeading = heading;

  float vL = speed - angVel * 0.25f / 2.0f;
  float vR = speed + angVel * 0.25f / 2.0f;
  motorL.move(vL / 0.05f);
  motorR.move(vR / 0.05f);

  Serial.print("段"); Serial.print(curSeg);
  Serial.print(" t="); Serial.print(segT*100,1);
  Serial.print("% curv="); Serial.print(curv,3);
  Serial.print(" v="); Serial.println(speed);
}

代码要点：多段贝塞尔通过控制点设计保证C1连续（相邻段连接点切线方向一致）。曲率计算公式为 κ = |x’y’’ - y’x’'| / (x’²+y’²)^1.5，高曲率区域自动降速，模拟“过弯减速”的驾驶行为。这是狭窄通道多拐角场景的核心策略。

3、贝塞尔 + 完整七段S曲线规划器（工业级）
场景说明：狭窄通道中需要精确控制加减速过程，避免任何加速度突变。采用完整的七段S曲线（加加速、匀加速、减加速、匀速、加减速、匀减速、减减速），与贝塞尔路径进度解耦，速度规划器独立工作。

#include <SimpleFOC.h>

// ===== 电机 =====
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);

struct Point2D { float x, y; };

// ===== S曲线规划器 =====
class SCurvePlanner {
public:
  float vmax;      // 最大速度
  float amax;      // 最大加速度
  float jmax;      // 最大加加速度(Jerk)

  float t_total;
  float t1, t2, t3, t4, t5, t6, t7;
  float T;

  void begin(float _vmax, float _amax, float _jmax, float distance) {
    vmax = _vmax; amax = _amax; jmax = _jmax;

    // 判断能否达到vmax（简化：先按七段计算）
    float tj = amax / jmax;           // 加加速时间
    float ta = vmax / amax - tj;      // 匀加速时间
    if (ta < 0) { ta = 0; tj = sqrt(vmax / jmax); }

    float s_acc = vmax * tj + 0.5f * amax * ta; // 简化估算
    float s_total = 2 * s_acc;       // 加+减
    float s_const = distance - s_total;

    if (s_const < 0) {
      // 达不到vmax，重新计算（缩短匀速段）
      // 简化：按三角形profile
      vmax = sqrt(distance * amax);
      tj = amax / jmax;
      ta = vmax / amax - tj;
      if (ta < 0) { ta = 0; tj = sqrt(vmax / jmax); }
      s_const = 0;
    }

    t1 = tj; t2 = ta; t3 = tj;
    t4 = (s_const > 0) ? s_const / vmax : 0;
    t5 = t3; t6 = t2; t7 = t1;
    T = t1+t2+t3+t4+t5+t6+t7;
  }

  // 输入时间，输出速度
  float getVelocity(float t) {
    if (t <= 0) return 0;
    if (t >= T) return 0;

    float v = 0;
    float tt = t;

    // 加加速段
    if (tt < t1) return 0.5f * jmax * tt * tt;
    tt -= t1;
    v = 0.5f * jmax * t1 * t1;

    // 匀加速段
    if (tt < t2) return v + amax * tt;
    tt -= t2;
    v += amax * t2;

    // 减加速段
    if (tt < t3) return v + amax * tt - 0.5f * jmax * tt * tt;
    tt -= t3;
    v = vmax;

    // 匀速段
    if (tt < t4) return vmax;
    tt -= t4;

    // 加减速段（对称减速）
    if (tt < t5) return vmax - 0.5f * jmax * tt * tt;
    tt -= t5;
    v = vmax - 0.5f * jmax * t5 * t5;

    // 匀减速段
    if (tt < t6) return v - amax * tt;
    tt -= t6;
    v -= amax * t6;

    // 减减速段
    return v - amax * tt + 0.5f * jmax * tt * tt;
  }
};

SCurvePlanner speedPlanner;

// ===== 贝塞尔路径 =====
Point2D P0 = {0.0, 0.0};
Point2D P1 = {0.8, 0.5};
Point2D P2 = {2.2, 0.5};
Point2D P3 = {3.0, 0.0};

float bezierLen = 2.8f;  // 预估算

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

  speedPlanner.begin(0.4f, 0.6f, 2.0f, bezierLen);
  Serial.println("七段S曲线规划器就绪");
}

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

  static float elapsed = 0;
  static float lastHeading = 0;
  elapsed += 0.01f;

  if (elapsed > speedPlanner.T) {
    motorL.move(0); motorR.move(0);
    motorL.loopFOC(); motorR.loopFOC();
    return;
  }

  // ===== 1. S曲线给出当前速度 =====
  float speed = speedPlanner.getVelocity(elapsed);

  // ===== 2. 弧长积分得到路径进度 =====
  // ds = v * dt，累计弧长 → 贝塞尔t
  static float arcLen = 0;
  arcLen += speed * 0.01f;
  float t = arcLen / bezierLen;
  t = constrain(t, 0, 1);

  // ===== 3. 贝塞尔位置与切线 =====
  float u = 1-t;
  float curX = u*u*u*P0.x + 3*u*u*t*P1.x + 3*u*t*t*P2.x + t*t*t*P3.x;
  float curY = u*u*u*P0.y + 3*u*u*t*P1.y + 3*u*t*t*P2.y + t*t*t*P3.y;

  float dx = 3*u*u*(P1.x-P0.x) + 6*u*t*(P2.x-P1.x) + 3*t*t*(P3.x-P2.x);
  float dy = 3*u*u*(P1.y-P0.y) + 6*u*t*(P2.y-P1.y) + 3*t*t*(P3.y-P2.y);
  float heading = atan2(dy, dx);

  // ===== 4. 差速解算 =====
  float angVel = (heading - lastHeading) / 0.01f;
  lastHeading = heading;

  float vL = speed - angVel * 0.25f / 2.0f;
  float vR = speed + angVel * 0.25f / 2.0f;
  motorL.move(vL / 0.05f);
  motorR.move(vR / 0.05f);

  // 监控
  static int cnt = 0;
  if (++cnt % 50 == 0) {
    Serial.print("t="); Serial.print(elapsed,2);
    Serial.print(" T="); Serial.print(speedPlanner.T,2);
    Serial.print(" v="); Serial.print(speed,3);
    Serial.print(" pos=("); Serial.print(curX,2); Serial.print(","); Serial.print(curY,2); Serial.println(")");
  }
}

代码要点：七段S曲线将运动分为加加速、匀加速、减加速、匀速、加减速、匀减速、减减速七个阶段，Jerk（加加速度）有界，从根本上消除加速度突变。速度规划器与贝塞尔路径解耦：S曲线只负责“走多快”，贝塞尔只负责“走哪条路”，弧长积分将两者连接。这是工业级AGV/AMR的标准架构。

要点解读

1. 贝塞尔曲线解决“路径连续性”，S型曲线解决“速度连续性”——两者必须同时满足

狭窄通道的物理约束要求机器人转弯时路径不能有直角拐点，否则必然碰撞或需要原地旋转。三次贝塞尔曲线天然具有C²连续性（位置、速度、加速度连续），端点切线方向可控，能精确匹配通道入口和出口方向。但仅有路径连续不够——如果沿路径的速度指令发生突变（如梯形速度曲线的加速度阶跃），电机仍会产生冲击电流和机械振动。S型曲线通过限制Jerk（加加速度）使加速度平滑变化，两者配合才能实现“轨迹丝滑”。

2. 曲率感知降速是狭窄通道的核心策略

狭窄通道的拐角处曲率极大，如果维持恒定速度通过，离心力会导致机器人侧滑或偏离路径。案例二中的 κ = |x’y’’ - y’x’'| / (x’²+y’²)^1.5 实时计算曲率，并用 speedFactor = 1/(1+K*curv) 自动降速。学术研究表明，在B-spline/Bézier曲线的曲率极值点处独立进行S型速度规划，可以显著提升机器人运动的平滑性和跟踪精度。

3. 路径与速度必须解耦设计

三个案例的共同架构是：贝塞尔曲线只管几何路径（“去哪儿”），速度规划器只管时间-速度剖面（“多快去”）。案例一用 sin(PI*t) 直接耦合两者（简单但不灵活），案例二用弧长进度连接，案例三用独立的SCurvePlanner类实现完全解耦。解耦的好处是：更换速度规划策略（梯形→S型→自适应）时无需修改路径规划代码，反之亦然。这是工程化落地的关键设计原则。

4. 多段贝塞尔的C1连续性是“不原地转向”的前提

狭窄通道往往需要多段曲线拼接（如S形绕行）。如果相邻段的连接点处切线方向不连续，机器人必须在连接点停顿并旋转，这在狭窄通道中极易碰撞。案例二中，段1的起始控制点方向被刻意设置为段0终点切线方向（P3-P2），保证连接点C1连续。构建多段贝塞尔时，每段的起始控制点应沿上一段终点的切线方向延伸，延伸距离可取段长的1/3左右。

5. 算力与实时性是Arduino平台的首要约束

完整的七段S曲线规划涉及多个浮点乘除和条件判断，贝塞尔曲线的导数计算也需要浮点运算。8位Arduino Uno在100Hz控制频率下几乎无法同时完成轨迹规划和FOC控制。建议使用ESP32（240MHz双核）或Teensy 4.x（600MHz）作为主控。如果必须用低端MCU，可以采用案例一的正弦近似方案降低计算量，或采用“上位机规划 + 下位机执行”的分层架构：上位机（如树莓派）计算贝塞尔路径点和S曲线速度表，下位机Arduino只做插值和电机控制。

4、狭窄直通道贝塞尔路径规划+S型速度平滑
适用场景：走廊、管道等固定狭窄直通道，无需避障，仅需规划平滑路径避免碰撞，S型速度减少冲击。

#include <SimpleFOC.h>
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1); // 左轮
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3); // 右轮
float bezier_path[200]; // 贝塞尔路径点数组
float target_speed = 0;
int path_idx = 0;

// 贝塞尔曲线规划：两点+控制点生成平滑路径
void generate_bezier_path(){
  float P0[2] = {0,0}, P1[2] = {50,100}, P2[2] = {150,200}, P3[2] = {200,200}; // 起点、控制点1、控制点2、终点
  for(int t=0; t<=1; t+=0.005f){
    // 三次贝塞尔公式
    bezier_path[path_idx++] = (1-t)*(1-t)*(1-t)*P0[0] + 3*(1-t)*(1-t)*t*P1[0] + 3*(1-t)*t*t*P2[0] + t*t*t*P3[0];
    bezier_path[path_idx++] = (1-t)*(1-t)*(1-t)*P0[1] + 3*(1-t)*(1-t)*t*P1[1] + 3*(1-t)*t*t*P2[1] + t*t*t*P3[1];
  }
}

// S型速度平滑：加速-匀速-减速，避免刚性冲击
void plan_s_speed(float total_dist, float accel, float max_speed){
  float accel_dist = max_speed * max_speed / (2 * accel); // 加速段距离
  if(total_dist < 2 * accel_dist){ // 长距离不足，采用三角速度曲线
    target_speed = sqrtf(total_dist * accel / 2);
  } else {
    target_speed = max_speed; // 中距离采用S型
  }
}

// 路径跟踪：根据贝塞尔路径点调整左右轮速度
void track_path(){
  if(path_idx >= 200) return;
  float cur_x = bezier_path[path_idx], cur_y = bezier_path[path_idx+1];
  float diff = cur_y - wheelL.shaft_angle; // 简化高差，关联速度
  float left = target_speed - diff * 0.5f;
  float right = target_speed + diff * 0.5f;
  wheelL.setVelocity(constrain(left, -150, 150));
  wheelR.setVelocity(constrain(right, -150, 150));
  path_idx +=2;
}

void setup(){
  Serial.begin(9600);
  wheelL.init(); wheelR.init();
  wheelL.controller = FOC_VELOCITY; wheelR.controller = FOC_VELOCITY;
  generate_bezier_path();
  plan_s_speed(200, 50, 100);
}

void loop(){
  track_path();
  delay(10);
}

5、狭窄通道避障贝塞尔路径+动态S型速度
适用场景：狭窄通道内有静态障碍物，需动态调整贝塞尔路径避障，并匹配速度平滑。

#include <SimpleFOC.h>
#include <RPLIDAR.h>
RPLIDAR lidar;
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1);
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3);
float obstacle_x = -1, obstacle_y = -1;
float target_speed = 0;

// 激光雷达检测狭窄通道内障碍物
void detect_obstacle(){
  if(lidar.scan()){
    for(int i=0;i<lidar.n;i++){
      float dx = lidar.points[i].x, dy = lidar.points[i].y;
      if(abs(dx) < 5 && dy > 0){ // 通道左右壁间距<10cm，判定狭窄
        obstacle_x = dx; obstacle_y = dy; return;
      }
    }
  }
  obstacle_x = -1; // 无障碍
}

// 动态贝塞尔路径规划：避障时调整控制点
void generate_adapt_bezier(){
  float P0[2] = {0,0}, P3[2] = {200,200};
  float P1x = 50, P2x = 150, P1y = 50, P2y = 150;
  if(obstacle_x != -1){ // 检测到障碍，偏移控制点避障
    P1x += obstacle_x * 0.8f; P2x += obstacle_x * 0.8f;
  }
  // 生成调整后的贝塞尔路径，此处简化
AnalogWrite(1); // 占位
}

// 动态S型速度：障碍物近时减速
void adjust_s_speed(float dist_to_obstacle){
  float accel = 40, max_speed = 100;
  if(dist_to_obstacle < 50){
    max_speed = 30; // 近障碍物低速行驶
    accel = 20;
  } else if(dist_to_obstacle < 100){
    max_speed = 60;
  }
  target_speed = max_speed;
}

void loop(){
  detect_obstacle();
  generate_adapt_bezier();
  float dist = sqrt(obstacle_x*obstacle_x + obstacle_y*obstacle_y);
  adjust_s_speed(dist);
  wheelL.setVelocity(target_speed - 5);
  wheelR.setVelocity(target_speed + 5);
  delay(10);
}

6、多狭窄通道配送全局贝塞尔路径+分段S型速度
适用场景：多狭窄通道的全流程配送，规划全局贝塞尔路径，按通道分段调整速度平滑，提升配送效率。

#include <SimpleFOC.h>
SimpleFOCMotor2 wheelL = SimpleFOCMotor2(2,3,4,A0,A1);
SimpleFOCMotor2 wheelR = SimpleFOCMotor2(5,6,7,A2,A3);
float global_bezier[400]; // 全局路径点
int seg_idx = 0; // 分段索引
int seg_count = 2; // 通道分段数

// 全局贝塞尔路径规划：多段贝塞尔拼接
void generate_global_bezier(){
  // 分段控制点：通道1+通道2
  float seg1[4][2] = {{0,0},{50,50},{100,100},{150,150}};
  float seg2[4][2] = {{150,150},{200,180},{250,220},{300,300}};
  int idx =0;
  for(int s=0;s<seg_count;s++){
    float *P = (s==0)?seg1[0]:seg2[0];
    for(float t=0; t<=1; t+=0.005f){
      global_bezier[idx++] = (1-t)*(1-t)*(1-t)*seg1[s][0].x + 3*(1-t)*(1-t)*t*seg1[s][1].x + 3*(1-t)*t*t*seg1[s][2].x + t*t*t*seg1[s][3].x;
      global_bezier[idx++] = (1-t)*(1-t)*(1-t)*seg1[s][0].y + 3*(1-t)*(1-t)*t*seg1[s][1].y + 3*(1-t)*t*t*seg1[s][2].y + t*t*t*seg1[s][3].y;
    }
  }
}

// 分段S型速度：狭窄段低速，开阔段高速
void plan_segment_speed(int seg_id){float accel = 40, max_speed = 80;
  if(seg_id == 0){ max_speed = 40; // 窄通道低速 accel = 20;
  } else { max_speed = 100; // 开阔段高速  accel = 60;
  }
  target_speed = max_speed;
}

// 分段路径跟踪
void track_segment_path(){
  if(seg_idx >= seg_count) return;
  float cur_x = global_bezier[seg_idx], cur_y = global_bezier[seg_idx+1];
  float left = target_speed - (cur_y - wheelL.shaft_angle) * 0.3f;
  float right = target_speed + (cur_y - wheelR.shaft_angle) * 0.3f;
  wheelL.setVelocity(constrain(left, -150, 150));
  wheelR.setVelocity(constrain(right, -150, 150));
  if(seg_idx % 80 ==0) seg_idx++; // 按段切换
  seg_idx +=2;
  if(seg_idx > 400) seg_idx=0;
}

void setup(){
  Serial.begin(9600);
  wheelL.init(); wheelR.init();
  generate_global_bezier();
}

void loop(){
  plan_segment_speed(seg_idx / 80);
  track_segment_path();
  delay(10);
}

要点解读
贝塞尔曲线需分段规划，保证路径二阶连续
狭窄通道内路径需避免尖角，采用三次贝塞尔曲线，控制点需根据通道宽度调整，确保路径曲率半径大于机器人最小转弯半径；通过多段贝塞尔拼接时，需满足相邻段终点与起点、切线方向一致，避免路径跳变。

S型速度需匹配路径曲率，实现速度-曲率协同
速度平滑不能仅按距离规划，需结合贝塞尔路径的曲率：曲率大的狭窄段自动降低速度，曲率小的段提速；采用加速度限制（jerk限制），确保速度变化平滑，避免机器人急停急转导致货物掉落。

避障时需局部动态调整路径，保持全局路径连贯
狭窄通道内遇障碍时，仅微调局部贝塞尔控制点实现绕障，尽量不改变全局路径趋势；同时增加避障安全距离（10-20cm），确保机器人与通道壁、障碍物无碰撞，路径调整需经过动态可行性验证。

路径跟踪需加入反馈修正，抵消累计误差
实际运动中需要实时比对当前位置与贝塞尔路径点，通过PID反馈修正左右轮速度差；同时引入激光雷达/编码器的里程计数据，消除轮胎打滑、机械误差导致的累计路径偏移，保证跟踪精度在5cm以内。

需设置安全与应急机制，适配狭窄空间特性
狭窄通道内冗余空间小，需设置路径偏差阈值，一旦实际路径与规划路径偏差超过10cm，立刻减速甚至停机；同时加入紧急避障逻辑，遇到突发动态障碍时快速插入局部二次避障路径，保证配送安全。

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