【轨迹规划】有界加加速度（Jerk-Bounded）轨迹规划：从 IEEE 论文到 STM32 落地（附完整代码）

📚 本文是一篇"论文精读 + 工程落地"学习笔记，用大白话把 2003 年 IEEE 经典论文讲清楚，再给出一套可以直接在 STM32 上运行的 C++ 实现。目标读者：运动控制、机器人、伺服 / 步进电机插补方向的嵌入式工程师。

论文信息

一句话读懂这篇论文

用"正弦波模板 + 五次多项式"逼近理想轨迹，实现在线生成平滑、有界加加速度（jerk）、C² 连续的运动轨迹。计算量有硬上界（每个路径段最多 8 个控制点），嵌入式实时系统完全跑得动。

📑 目录

• 一、这篇论文解决什么问题？
• 二、第 1 步：为什么要限制加加速度？
• 三、第 2 步：LSPB 到底哪里不好？
• 四、第 3 步：核心思想——正弦波模板
• 五、第 4 步：为什么用五次多项式逼近正弦波？
• 六、第 5 步：SAP 算法——轨迹段是怎么拼出来的
• 七、第 6 步：多路径点怎么拼接？
• 八、第 7 步：STM32 完整代码实现
• 九、实验结果：到底好不好用？
• 十、总结与工程启示

一、这篇论文解决什么问题？

工业机器人运行时的轨迹质量直接决定三个指标：跟踪精度、机械磨损、节拍时间。

• 轨迹太"硬"（加速度突变）→ 机械臂振动、磨损大、跟随误差大；
• 轨迹太"软"（过度平滑）→ 运动变慢，影响生产效率；
• 算法太复杂 → 实时系统（MCU）算不过来。

论文要同时满足四个要求：

二、第 1 步：为什么要限制加加速度？

加加速度（jerk） 是加速度的变化率：

$$jerk=\frac{da}{dt}=\frac{d^{3}x}{d{t}^3}$$

简单理解：加速度描述"速度变化快不快"，jerk 描述"加速度变化快不快"。加速度突变 = jerk 无穷大，机械臂就会"猛冲猛停"，产生振动和冲击。

限制 jerk 的四个核心理由：

三、第 2 步：LSPB 到底哪里不好？

3.1 什么是 LSPB？

LSPB（Linear Segment with Parabolic Blend，线性段 + 抛物线混合）是经典的轨迹规划方法：中间匀速（线性段），两头用抛物线过渡（匀加速 / 匀减速）。它实现简单，是很多教材的入门方案。

3.2 问题：加速度是"方波"

LSPB 的加速度在三个值之间切换：最大加速度 → 0 → 最小加速度，轮廓是一个方波（Square Wave）：

a(t)
A_MAX  ┌─────────────────────┐
       │                     │
  0    ┼─────────────────────┼──────────► t
       │                     │
-A_MAX └─────────────────────┘

⚠️ 致命伤：方波在切换点加速度瞬间跳变，jerk 无穷大。伺服系统根本追不上这种突变，实际执行时间反而更长，还会引起振动。

3.3 前人方法的全家福对比

核心差距：缺一个"计算量小 + 适合在线 + 可指定 jerk 界限"的方法。这就是本文要补的空白。

四、第 3 步：核心思想——正弦波模板

4.1 关键洞察

既然方波太"硬"，那就把方波的直角"磨圆"。论文的绝妙想法是：

💡 用正弦波模板（Sine Wave Template）替代加速度方波，再用五次多项式高效逼近正弦波。

这是一个双层近似：

理想方波（无限 jerk，不可实现）
   │  第 1 层近似：磨圆直角，保证平滑
   ▼
正弦波模板（jerk 连续，但 sin() 计算贵）
   │  第 2 层近似：保证计算效率
   ▼
五次多项式（段内 5 次乘加，实时可控）

4.2 正弦波模板的数学形式

加速度从 0 上升到 A_MAX，走半正弦波：

$$a(t)=\frac{A_{MAX}}{2}\left[1-\cos (\omega t)\right],t\in [0,t_{ramp}]$$

$$\omega =\frac{\pi }{t_{ramp}}$$

各符号含义：

验证边界条件：

• t = 0：a(0) = 0（起始加速度为零 ✅）
• t = t_ramp：a(t_ramp) = A_MAX（到达最大加速度 ✅）

4.3 核心公式：t_ramp 怎么定？

对 a(t) 求导，得到 jerk：

$$j(t)=\frac{A_{MAX}}{2}\omega \sin (\omega t)=\frac{A_{MAX}\pi }{2t_{ramp}}\sin \left(\frac{\pi t}{t_{ramp}}\right)$$

正弦函数最大值是 1，所以 jerk 峰值出现在 t = t_ramp / 2 处：

$$j_{max}=\frac{A_{MAX}\pi }{2t_{ramp}}$$

令 j_max 等于用户指定的上限 J_MAX，反解出 ramp 时间：

$$t_{ramp}=\frac{\pi A_{MAX}}{2J_{MAX}}$$

📌 这是全文最重要的公式：给定加速度上限和 jerk 上限，爬升时间就唯一确定了。

4.4 对比：正弦波 vs 线性坡道

正弦波 ramp 时间比线性坡道长约 57%（π/2 ≈ 1.57 倍），但换来 jerk 全程连续。这是"多花一点时间，换更稳的运动"，值得。

五、第 4 步：为什么用五次多项式逼近正弦波？

5.1 sin() 在 MCU 上太贵

sin() 函数在嵌入式微控制器上的计算开销大约是多项式求值的 30~50 倍。实时插补每 1ms 就要算一次，用 sin() 会挤占大量 CPU 时间。

5.2 五次多项式的好处

5.3 关于 jerk 连续性的"坦白"

论文原文明确说明了两点：

“The jerk profile imposed by the template during the acceleration ramp is parabolic, and starts and ends at 0.”
“The jerk profile between ramps is discontinuous at the quintic control points. This discontinuity is, at most, 10% of the jerk limit.”

翻译成人话：

• ✅ 段内：jerk 呈抛物线形，从 0 开始、回到 0，完全连续；
• ⚠️ 段间接头（五次控制点）：jerk 有小跳变，但 ≤ 10%·J_MAX。

这是为了计算效率做的有意折衷，10% 的跳变在实际系统中可接受。

六、第 5 步：SAP 算法——轨迹段是怎么拼出来的

6.1 先认识三个基本段

6.2 SAP 是什么？

SAP = Sustained Acceleration Pulse（持续加速度脉冲）。它是论文附录 A 给出的算法，负责把一次完整的变速（从 v_cur 到 v_tar）切成若干段五次多项式。

一个完整的变速过程最多 4 段（见论文 Fig.3）：

段1: 加速度 ramp-up    a: 0 → A_MAX     正弦半波上升
段2: 加速度 cruise     a = A_MAX        恒加速
段3: 加速度 ramp-down  a: A_MAX → 0     正弦半波下降
段4: 速度 cruise       a = 0, v = v_tar 恒速

6.3 SAP 算法流程

对应文字版步骤：

输入: v_cur, v_tar, dist, J_MAX, A_MAX

第 1 步: t_ramp = π·A_MAX / (2·J_MAX)      ← 正弦模板决定的 ramp 时间
第 2 步: Δv_ramp = (A_MAX / 2) · t_ramp      ← 单次 ramp 能改变的速度
第 3 步: Δv = v_tar - v_cur

第 4 步: 判断能否达到 A_MAX
  若 |Δv| > 2·Δv_ramp:
     → 三段: 正弦上升 + 直线加速巡航 + 正弦下降 (可达 A_MAX)
  否则:
     → 两段: 正弦上升 + 立即正弦下降 (达不到 A_MAX, 解三次方程求 a_peak)

第 5 步: 计算各段位移, 检查是否超限
  超限 → 降低峰值速度, 递归重算

第 6 步: 有剩余距离 → 插入恒速巡航段

第 7 步: 输出控制点

6.4 两种"达不到 A_MAX"的情况（附录 B）

实际运动中，不一定每次都能把加速度拉到 A_MAX，有两种限制：

情况 1：位移限制

• 位移太短，加速到 A_MAX 再减速会"冲过"目标位置；
• 需要解三次方程（论文式 9）求一个更低的峰值加速度 a_peak。

情况 2：速度变化限制

• 速度变化太小，没必要也不允许升到 A_MAX；
• 同样降低 a_peak。

对于限制情况，只需要 2~3 段五次：ramp-up + ramp-down（+ 可选恒速巡航）。

论文式 (9) 的求解思路：

$$a_{peak}=\sqrt{\frac{2Jdv}{\pi }}$$

• 该三次方程在 (0,1) 区间有唯一正实根；
• 可用泰勒级数近似，平均 1.5 次迭代收敛，实时性有保障。

6.5 三种情况的总结表（论文 §II-E）

📌 硬上界：完整点对点运动 = 加速（≤4 段）+ 恒速巡航（0~2 段）+ 减速（≤4 段）= 最多 8 段。这就是计算时间的硬上界，也是"适合实时"的底气。

七、第 6 步：多路径点怎么拼接？

机器人运动往往是"一串路径点"，不是单个点对点。论文 §II-E 给出了拼接规则：

1. 每个路径点处加速度为零（a = 0）——这是前后段连续的前提；
2. 方向改变 → 先减速到零，再朝新方向加速；
3. 相邻段共享路径点的 (q, v)，保证 C² 连续（位置、速度、加速度都连续）。

一句话：每个路径点都是一个"干净的停靠点"，轨迹在这里平滑过渡。

八、第 7 步：STM32 完整代码实现

8.0 设计目标

• 纯 C++，无动态分配（不 new、不 malloc）；
• 1ms 定时器中断里调用，实时性强；
• STM32F103 / F405 等主流 MCU 均可运行；
• 算法参考：Macfarlane & Croft, IEEE TRA 2003。

8.1 配置参数

先定义运动限制与插补周期，改参数即可适配不同设备：

// ============================================================
// Sine-SAP 有界加加速度轨迹规划器 — 完整实现 (v3)
// 适用: STM32F103/F405，无动态分配，纯 C++
// 参考: Macfarlane & Croft, "Jerk-Bounded Manipulator Trajectory
//        Planning Design for Real-Time Applications", IEEE TRA 2003
// ============================================================
#include <cmath>
#include <cstdint>

// ======================== 配置参数 ============================

#define J_MAX  (2000.0f)    // 最大加加速度 (单位/s³)
#define A_MAX  (100.0f)     // 最大加速度   (单位/s²)
#define V_MAX  (50.0f)      // 最大速度     (单位/s)
#define DT     (0.001f)     // 插补周期     (s)，即 1ms
#define EPS    (1e-4f)      // 浮点容差
#define PI     (3.14159265f)

8.2 数据结构

三种核心类型，看懂它们就理解了整个框架：

• Segment：一段五次多项式（6 个系数 + 边界条件 + 时长 + 位移）；
• Trajectory：整条轨迹，最多 8 段（论文证明的硬上界）；
• TrajectoryState：执行状态，在定时器中断里维护。

// 五次多项式段: p(τ)=Σ c[i]·τ^i, τ=t/T ∈ [0,1]
// 系数通过高斯消去法直接求解 3×3 线性系统得到
struct Segment {
    float c[6];   // c0..c5: 五次多项式系数 (τ域)
    float v0, v1; // 段始/末速度
    float a0, a1; // 段始/末加速度
    float T;      // 段持续时间 (s)
    float dist;   // 段内位移 (m 或 pulse)
};

// 轨迹: 论文附录 A/B 证明最多 8 段即为硬上界
struct Trajectory {
    Segment segs[8];
    int count;         // 实际段数 (0..8)
    float total_dist;  // 总位移
};

// 路点定义 (用于 planViaWaypoints)
struct Waypoint {
    float q; // 位置
    float v; // 通过速度 (0 = 停靠)
};

// 轨迹执行状态 (定时器中断中维护)
struct TrajectoryState {
    Trajectory traj;    // 当前轨迹
    int seg_idx;        // 当前段索引
    float t_local;      // 当前段内时间 (s)
    float base_pos;     // 已执行段累积的绝对位置
    float base_time;    // 已执行段累积的时间 (s)
    bool done;          // 轨迹是否完成
};

8.3 正弦模板辅助函数

把第 4 节推导的公式直接翻译成代码：

// 正弦波模板 ramp 时间
// 论文推导: a(t) = (A/2)·[1 - cos(π·t/t_ramp)]
//          j(t) = (A·π/(2·t_ramp))·sin(π·t/t_ramp)
//          j_max = A·π/(2·t_ramp) = J_MAX
//       →  t_ramp = π·A/(2·J)
float calcRampTime(float A, float J) {
    return PI * A / (2.0f * J);
}

// 单次正弦 ramp 内的速度变化
// 推导: ∫₀^{T} (A/2)·[1 - cos(π·t/T)] dt = A·T/2
float calcDeltaV(float A, float T) {
    return 0.5f * A * T;
}

// 正弦 ramp-up (a: 0→A) 位移
// a(t) = (A/2)·[1 - cos(π·t/T)], A 可正可负 (正=加速,负=减速)
// ∬ a dt² = v₀·T + A·T²·(1/4 - 1/π²)
float calcDistSineRampUp(float v0, float A, float T) {
    return v0 * T + A * T * T * (0.25f - 1.0f / (PI * PI));
}

// 正弦 ramp-down (a: A→0) 位移
// a(t) = (A/2)·[1 + cos(π·t/T)], A 可正可负
// ∬ a dt² = v₀·T + A·T²·(1/4 + 1/π²)
float calcDistSineRampDown(float v0, float A, float T) {
    return v0 * T + A * T * T * (0.25f + 1.0f / (PI * PI));
}

// 三次方程求解 a_peak (距离/速度受限时)
// 论文式(9): dv = 2·Δv_ramp = π·a_peak²/(2·J)
// → a_peak = sqrt(2·J·dv/π)
float solvePeakAccel(float dv, float J) {
    return sqrtf(2.0f * J * dv / PI);
}

8.4 五次多项式：系数求解

给定两端的位置、速度、加速度（6 个边界条件），用高斯消去解 3×3 线性系统，得到 6 个系数：

// p(τ) = c0 + c1·τ + ... + c5·τ⁵, τ ∈ [0,1]
// 边界条件:
//   τ=0: p=q0, dp/dτ=v0·T, d²p/dτ²=a0·T²
//   τ=1: p=q1, dp/dτ=v1·T, d²p/dτ²=a1·T²
//
// 高斯消去法求解 3×3 线性系统 [1 1 1;3 4 5;6 12 20]·[c3;c4;c5]=[d1;d2;d3]
// 系数用于 τ-域求值，无需再除以 T³/T⁴/T⁵
void calcQuinticCoef(float q0, float v0, float a0,
                     float q1, float v1, float a1,
                     float T, float* c) {
    float T2 = T * T;
    float dq = q1 - q0;

    // 缩放到 τ 域
    float v0t = v0 * T;
    float v1t = v1 * T;
    float a0t = a0 * T2;
    float a1t = a1 * T2;

    // 线性系统右端项
    float d1 = dq - v0t - 0.5f * a0t;
    float d2 = v1t - v0t - a0t;
    float d3 = a1t - a0t;

    c[0] = q0;
    c[1] = v0t;
    c[2] = 0.5f * a0t;

    // 高斯消去法: c5 = (d3 + 12d1 - 6d2)/2, c4 = d2 - 3d1 - 2c5, c3 = d1 - c4 - c5
    c[5] = (d3 + 12.0f * d1 - 6.0f * d2) / 2.0f;
    c[4] = d2 - 3.0f * d1 - 2.0f * c[5];
    c[3] = d1 - c[4] - c[5];
}

8.5 五次多项式：求值

位置、速度、加速度三个域分别求值，都用霍纳法则（乘加），效率最高：

// 位置: p(τ) = c0 + c1·τ + c2·τ² + c3·τ³ + c4·τ⁴ + c5·τ⁵
float evalQuintic(float tau, const float* c) {
    return c[0] + tau * (c[1] + tau * (c[2] + tau * (c[3]
                + tau * (c[4] + c[5] * tau))));
}

// 速度: v(t) = dp/dt = (1/T) · dp/dτ
float evalQuinticVel(float tau, const float* c, float T) {
    float dp = c[1] + tau * (2.0f * c[2] + tau * (3.0f * c[3]
                        + tau * (4.0f * c[4] + 5.0f * c[5] * tau)));
    return dp / T;
}

// 加速度: a(t) = d²p/dt² = (1/T²) · d²p/dτ²
float evalQuinticAcc(float tau, const float* c, float T) {
    float d2p = 2.0f * c[2] + tau * (6.0f * c[3] + tau * (12.0f * c[4]
                              + 20.0f * c[5] * tau));
    return d2p / (T * T);
}

8.6 构造单个段

把系数 + 边界条件打包成 Segment：

Segment makeSegment(float v0, float v1, float a0, float a1,
                    float T, float dist) {
    Segment seg;
    // q0=0, q1=dist → 段内相对位移
    calcQuinticCoef(0.0f, v0, a0, dist, v1, a1, T, seg.c);
    seg.v0 = v0;
    seg.v1 = v1;
    seg.a0 = a0;
    seg.a1 = a1;
    seg.T = T;
    seg.dist = dist;
    return seg;
}

8.7 SAP 速度坡道：genSpeedRamp

这是论文 SAP 算法的代码实现，核心中的核心，务必对照第 6 节看：

// 生成从 v_cur 到 v_tar 的变速子轨迹，位移限制 max_dist
// 可达 A_MAX 时: ramp-up + cruise + ramp-down  (3 段)
// 受限时:       ramp-up + ramp-down            (2 段)
// a(t) 用正弦模板, p(t) 用五次多项式逼近
Trajectory genSpeedRamp(float v_cur, float v_tar, float max_dist) {
    Trajectory traj = {{0}, 0, 0.0f};

    float dv = v_tar - v_cur;
    if (fabsf(dv) < EPS) return traj;  // 无需变速

    float sign = (dv > 0.0f) ? 1.0f : -1.0f;
    dv = fabsf(dv);

    float tr   = calcRampTime(A_MAX, J_MAX);     // t_ramp
    float dv_r = calcDeltaV(A_MAX, tr);           // 单次 ramp 的速度变化量

    if (dv > 2.0f * dv_r) {
        // ====== 可达 A_MAX: ramp-up + cruise + ramp-down ======

        float tc = (dv - 2.0f * dv_r) / A_MAX;   // 恒加速段时间

        // 段1: 加速度 ramp-up (a: 0 → sign·A_MAX)
        // a(t)=(A_MAX/2)·[1-cos(π·t/tr)], 有向加速度 = sign·a(t)
        float d1 = calcDistSineRampUp(v_cur, sign * A_MAX, tr);
        traj.segs[0] = makeSegment(v_cur, v_cur + sign * dv_r,
                                   0.0f, sign * A_MAX, tr, d1);

        // 段2: 加速度 cruise (a = sign·A_MAX 恒定)
        float v_mid = v_cur + sign * dv_r;
        float d2 = v_mid * tc + 0.5f * sign * A_MAX * tc * tc;
        traj.segs[1] = makeSegment(v_mid, v_mid + sign * A_MAX * tc,
                                   sign * A_MAX, sign * A_MAX, tc, d2);

        // 段3: 加速度 ramp-down (a: sign·A_MAX → 0)
        // a(t)=(A_MAX/2)·[1+cos(π·t/tr)], 有向加速度 = sign·a(t)
        float v_before = v_mid + sign * A_MAX * tc;
        float d3 = calcDistSineRampDown(v_before, sign * A_MAX, tr);
        traj.segs[2] = makeSegment(v_before, v_tar,
                                   sign * A_MAX, 0.0f, tr, d3);

        traj.count = 3;
        traj.total_dist = d1 + d2 + d3;

    } else {
        // ====== 达不到 A_MAX: ramp-up + ramp-down (脉冲) ======
        // dv = 2·Δv_ramp = π·a_peak²/(2·J)
        // → a_peak = sqrt(2·J·dv/π)

        float a_peak = solvePeakAccel(dv, J_MAX);
        float tr2   = calcRampTime(a_peak, J_MAX);
        float dv_r2 = calcDeltaV(a_peak, tr2);

        // 段1: 加速度 ramp-up (a: 0 → sign·a_peak)
        float v_mid = v_cur + sign * dv_r2;
        float d1 = calcDistSineRampUp(v_cur, sign * a_peak, tr2);
        traj.segs[0] = makeSegment(v_cur, v_mid,
                                   0.0f, sign * a_peak, tr2, d1);

        // 段2: 加速度 ramp-down (a: sign·a_peak → 0)
        float d2 = calcDistSineRampDown(v_mid, sign * a_peak, tr2);
        traj.segs[1] = makeSegment(v_mid, v_tar,
                                   sign * a_peak, 0.0f, tr2, d2);

        traj.count = 2;
        traj.total_dist = d1 + d2;
    }

    // 检查位移超限 → 降低峰值速度，递归
    if (traj.total_dist > max_dist && max_dist > 0.0f) {
        // 注: 论文 §II-D 使用三次方程(9)精确求解受限 a_peak
        // 此处用平方根比例近似，对嵌入式场景可接受
        // 精确做法: 牛顿迭代 1.5 次求解论文式(9)三次方程
        float scale = sqrtf(max_dist / traj.total_dist);
        float v_reduced = v_cur + sign * scale * dv;
        return genSpeedRamp(v_cur, v_reduced, max_dist);
    }

    return traj;
}

8.8 恒速巡航段

Segment makeCruiseSeg(float v, float dist) {
    if (fabsf(v) < EPS) {
        Segment seg = {{0}, 0, 0, 0, 0, 0, 0};
        return seg;
    }
    return makeSegment(v, v, 0.0f, 0.0f, dist / v, dist);
}

8.9 点对点运动：planPointToPoint

静止 → 加速 → 巡航 → 减速 → 静止，整条轨迹一次生成：

// 论文附录 A: 两次方波 → 加速+减速 ≤ 8 段
Trajectory planPointToPoint(float total_dist) {
    // 加速曲线: 0 → V_MAX (ramp-up + cruise + ramp-down)
    Trajectory accel = genSpeedRamp(0.0f, V_MAX, total_dist * 0.5f);
    // 减速曲线: V_MAX → 0 (ramp-up + cruise + ramp-down)
    Trajectory decel = genSpeedRamp(V_MAX, 0.0f, total_dist * 0.5f);

    float sum_dist = accel.total_dist + decel.total_dist;

    // 若距离不够 → 降低峰值速度
    if (sum_dist > total_dist) {
        float ratio = total_dist / sum_dist;
        float v_peak = V_MAX * ratio;  // 一阶近似 (距离 ≈ 速度·时间)
        accel = genSpeedRamp(0.0f, v_peak, total_dist * 0.5f);
        decel = genSpeedRamp(v_peak, 0.0f, total_dist * 0.5f);
        sum_dist = accel.total_dist + decel.total_dist;
    }

    Trajectory traj = {{0}, 0, 0.0f};

    // 合并加速段
    for (int i = 0; i < accel.count; i++)
        traj.segs[traj.count++] = accel.segs[i];

    // 插入恒速巡航段 (利用剩余距离)
    float remain = total_dist - sum_dist;
    if (remain > EPS) {
        float v_peak_actual = (decel.count > 0) ? decel.segs[0].v0 : V_MAX;
        Segment cruise = makeCruiseSeg(v_peak_actual, remain);
        traj.segs[traj.count++] = cruise;
    }

    // 合并减速段
    for (int i = 0; i < decel.count; i++)
        traj.segs[traj.count++] = decel.segs[i];

    traj.total_dist = total_dist;
    return traj;
}

8.10 多路点：planViaWaypoints

论文 §II-E 的多路径点拼接，内部用二分查找自动适配巡航速度：

// 拼接规则:
//   1. 每段在路点处 a=0 (保证段间加速度连续, C²)
//   2. 方向改变 → 先减速到零再加速
//   3. 相邻段共享路点的 (q, v) 保证 C² 连续
Trajectory planViaWaypoints(const Waypoint* wps, int n) {
    Trajectory full = {{0}, 0, 0.0f};
    if (n < 2) return full;

    for (int i = 0; i < n - 1; i++) {
        float dq = wps[i+1].q - wps[i].q;
        float v0 = wps[i].v;
        float v1 = wps[i+1].v;
        float abs_dq = fabsf(dq);
        float sign = (dq >= 0) ? 1.0f : -1.0f;

        // 确定巡航方向速度 (不超过 V_MAX，不低于两端速度)
        float v0_signed = sign * v0;
        float v1_signed = sign * v1;
        float vc_signed = sign * V_MAX;
        if (v0_signed > vc_signed) vc_signed = v0_signed;
        if (v1_signed > vc_signed) vc_signed = v1_signed;

        // 加速段 + 减速段
        Trajectory accel = genSpeedRamp(v0_signed, vc_signed, abs_dq * 0.5f);
        Trajectory decel = genSpeedRamp(vc_signed, v1_signed, abs_dq * 0.5f);
        float sum_dist = accel.total_dist + decel.total_dist;

        // 距离受限 → 二分查找适配的巡航速度
        if (sum_dist > abs_dq) {
            float lo = (v0_signed < v1_signed) ? v0_signed : v1_signed;
            float hi = vc_signed;
            for (int iter = 0; iter < 8; iter++) {
                float mid = 0.5f * (lo + hi);
                Trajectory ta = genSpeedRamp(v0_signed, mid, abs_dq * 0.5f);
                Trajectory td = genSpeedRamp(mid, v1_signed, abs_dq * 0.5f);
                float sd = ta.total_dist + td.total_dist;
                if (sd > abs_dq) hi = mid;
                else            lo = mid;
            }
            accel = genSpeedRamp(v0_signed, lo, abs_dq * 0.5f);
            decel = genSpeedRamp(lo, v1_signed, abs_dq * 0.5f);
            sum_dist = accel.total_dist + decel.total_dist;
        }

        // 合并: 加速段 + 巡航 + 减速段
        for (int j = 0; j < accel.count; j++)
            full.segs[full.count++] = accel.segs[j];

        float remain = abs_dq - sum_dist;
        if (remain > EPS) {
            float v_actual = (decel.count > 0) ? decel.segs[0].v0 : vc_signed;
            Segment cruise = makeCruiseSeg(v_actual, remain);
            full.segs[full.count++] = cruise;
        }

        for (int j = 0; j < decel.count; j++)
            full.segs[full.count++] = decel.segs[j];

        full.total_dist += abs_dq;
    }

    return full;
}

8.11 定时器插补：trajectoryStep

每 1ms 调用一次，输出当前位置 / 速度 / 加速度目标值：

// 每 DT 秒调用一次 (定时器中断)
// 输出绝对位置、速度、加速度目标值
void trajectoryStep(TrajectoryState* state,
                    float* pos_out, float* vel_out, float* acc_out) {
    // 已完成 → 保持
    if (state->done) return;

    // 空轨迹保护
    if (state->traj.count == 0) {
        state->done = true;
        return;
    }

    // 段索引保护
    if (state->seg_idx >= state->traj.count) {
        state->done = true;
        return;
    }

    Segment* seg = &state->traj.segs[state->seg_idx];
    state->t_local += DT;

    // 段结束 → 切换到下一段
    if (state->t_local >= seg->T) {
        state->base_pos += seg->dist;   // 累加绝对位移
        state->base_time += seg->T;
        state->seg_idx++;
        state->t_local = 0.0f;

        if (state->seg_idx >= state->traj.count) {
            state->done = true;
            // 停在最终绝对位置
            if (pos_out) *pos_out = state->base_pos;
            if (vel_out) *vel_out = 0.0f;
            if (acc_out) *acc_out = 0.0f;
            return;
        }
        seg = &state->traj.segs[state->seg_idx];
    }

    // 归一化时间 τ ∈ [0,1]
    float tau = state->t_local / seg->T;
    if (tau < 0.0f)       tau = 0.0f;
    else if (tau > 1.0f)  tau = 1.0f;

    // 求值: 段内相对位置 + 已累积绝对位置
    if (pos_out) *pos_out = state->base_pos + evalQuintic(tau, seg->c);
    if (vel_out) *vel_out = evalQuinticVel(tau, seg->c, seg->T);
    if (acc_out) *acc_out = evalQuinticAcc(tau, seg->c, seg->T);
}

// ==================== 轨迹初始化 ==============================

void trajectoryInit(TrajectoryState* state, const Trajectory* traj) {
    state->traj = *traj;
    state->seg_idx = 0;
    state->t_local = 0.0f;
    state->base_pos = 0.0f;
    state->base_time = 0.0f;
    state->done = false;
}

8.12 使用示例

单目标点 + 多路点 + 定时器中断，三步走：

//  // 全局状态
//  TrajectoryState g_state;
//
//  // 下发单次目标 (静止→静止)
//  void setTarget(float target) {
//      Trajectory t = planPointToPoint(target);
//      trajectoryInit(&g_state, &t);
//  }
//
//  // 下发多路点 (非零通过速度, 论文 §II-E)
//  void setWaypoints() {
//      Waypoint wps[] = {{0, 0}, {100, 20}, {200, 10}, {300, 0}};
//      Trajectory t = planViaWaypoints(wps, 4);
//      trajectoryInit(&g_state, &t);
//  }
//
//  // 定时器中断 (1ms)
//  void timerISR() {
//      float pos, vel, acc;
//      trajectoryStep(&g_state, &pos, &vel, &acc);
//      setMotorPosition((int32_t)pos);      // 位置环
//      setMotorVelocity((int32_t)vel);      // 速度前馈 (可选)
//  }

九、实验结果：到底好不好用？

论文在工业机器人上做了仿真 + 实验验证，主要结论：

⚠️ 最反直觉的结论：LSPB 理论时间最短，但实际中由于无限 jerk 导致跟踪误差，实际执行时间反而比有界 jerk 的五次轨迹更长。“看起来快的方案，实际更慢”。

十、总结与工程启示

10.1 论文贡献

1. 正弦波模板 + 五次多项式逼近：正弦波约束加速度变化（数学优美），五次多项式保证在线计算（工程高效）；
2. SAP 算法：自动处理"可达 / 不可达 A_MAX"两种情况，附录 A/B 给出可直接套用的公式；
3. 计算时间硬上限：最多 8 个控制点，插补仅 5 次乘加；
4. C² 连续：位置、速度、加速度连续（jerk 段内有界、段间断 ≤ 10%）；
5. 实验验证：跟踪精度优于 LSPB 和纯五次方法。

10.2 适用场景

• 工业机器人运动控制；
• 实时在线轨迹生成（STM32F103 等 MCU 完全可跑）；
• 对平滑度要求高的应用：涂胶、喷漆、液体搬运等。

10.3 局限性

• 相比理论时间最优（LSPB），执行时间略长（约 10~20%）；
• 段间接头 jerk 有 ≤ 10% 跳跃；
• 需要提前指定 jerk / 加速度 / 速度限制。

10.4 工程启示：完整的码垛控制链

结合前几篇笔记，码垛任务的完整控制链是：

Lazy PRM ──→ 路径点序列 ──→ 五次 + SAP 有界轨迹 ──→ CR20A 执行
    ↑                          ↑
 避什么路                   怎么走好、走快、走稳

一句话收尾：路径规划决定"走哪条路"，轨迹规划决定"怎么走得又快又稳"。这篇论文给出的正弦模板 + 五次多项式方案，就是"又快又稳"的标准答案，而且计算量小到嵌入式都能跑。

📝 本文为学习笔记。代码思路已在 STM32 平台验证，J_MAX / A_MAX / V_MAX 等参数请按实际机构调整。
如果对你有帮助，欢迎收藏点赞，也欢迎在评论区交流讨论！