3、时间最优轨迹规划：梯形速度曲线原理与实现，S形速度曲线原理与实现

做机器人运动规划这些年，我越来越觉得速度曲线这东西，就像炒菜的火候。火小了菜不熟，火大了容易糊。梯形和S形速度曲线，就是两种最经典的“火候控制”方式。

说白了，时间最优轨迹规划，就是让机器人跑得又快又稳。梯形速度曲线追求“快”，S形速度曲线追求“稳”。今天咱们就把这两个家伙彻底聊透。

3.1 梯形速度曲线：简单粗暴的效率之王

梯形速度曲线，我习惯叫它“T型曲线”。为什么叫梯形？你想想看，速度随时间的变化图，就像一个梯形——先匀加速，再匀速，最后匀减速。

它的核心逻辑就三个字：加、恒、减。

核心公式（单段运动）：

加速段：v(t) = v₀ + a·t，位移 s₁ = v₀·t₁ + ½·a·t₁²
匀速段：v(t) = v_max，位移 s₂ = v_max·t₂
减速段：v(t) = v_max - a·t，位移 s₃ = v_max·t₃ - ½·a·t₃²

其中，v₀是初速度，v_max是最大速度，a是加速度（假设加减速对称）。

我在项目中遇到过一个问题：梯形曲线在加减速切换的瞬间，加速度会突变。说白了就是“急刹车”和“猛起步”。这对电机冲击很大，尤其是重载机器人，搞不好会抖得跟筛子似的。

梯形曲线的优缺点：

优点	缺点
计算简单，实时性好	加速度突变，冲击大
时间最优（给定限速下）	容易引起机械振动
参数少，容易调试	不适合高精度场景

我的小技巧：如果项目对时间要求极苛刻，比如高速分拣机器人，梯形曲线是首选。但记得在加减速切换点加一个低通滤波，能有效抑制抖动。

3.2 梯形速度曲线的代码实现

嗯，这里要注意。实现梯形曲线，关键是要判断运动能不能达到最大速度。如果位移太短，可能刚加速就要减速了，根本跑不到v_max。

// 梯形速度曲线规划（C语言伪代码）
typedef struct {
    float v0;      // 初速度
    float v_max;   // 最大速度
    float a;       // 加速度
    float s_total; // 总位移
    float t1, t2, t3; // 三段时间
} TrapezoidProfile;

void planTrapezoid(TrapezoidProfile *p) {
    // 计算理论加速/减速所需位移
    float s_acc = (p->v_max * p->v_max - p->v0 * p->v0) / (2 * p->a);
    float s_dec = p->v_max * p->v_max / (2 * p->a);
    
    if (s_acc + s_dec <= p->s_total) {
        // 能达到最大速度
        p->t1 = (p->v_max - p->v0) / p->a;
        p->t3 = p->v_max / p->a;
        p->t2 = (p->s_total - s_acc - s_dec) / p->v_max;
    } else {
        // 达不到最大速度，只有加速和减速
        float v_peak = sqrt((2 * p->a * p->s_total + p->v0 * p->v0) / 2);
        p->t1 = (v_peak - p->v0) / p->a;
        p->t3 = v_peak / p->a;
        p->t2 = 0;
    }
}

我曾经踩过的坑：梯形曲线在减速段结束时，如果速度没降到0，会直接跳变到目标速度。这会导致末端执行器“点头”现象。解决办法是在减速段末尾加一个微小的匀速段做缓冲。

3.3 S形速度曲线：丝般顺滑的进阶方案

S形速度曲线，说白了就是给梯形曲线“圆角”。它把加速度的变化也考虑进去了，引入了“加加速度”（Jerk）的概念。

为什么会这样？因为梯形曲线的加速度突变，本质上是加加速度无穷大。S形曲线通过限制加加速度，让速度变化更平滑。

S形曲线的七段式结构：

加加速度段（Jerk为正，加速度从0增加到a_max）
匀加速段（加速度保持a_max）
减加速度段（Jerk为负，加速度从a_max降到0）
匀速段（速度保持v_max）
加减速度段（Jerk为负，加速度从0降到-a_max）
匀减速段（加速度保持-a_max）
减减速度段（Jerk为正，加速度从-a_max升到0）

你想想看，七段式听起来复杂，但核心就一句话：加速度不能突变，要像爬楼梯一样一级一级来。

S形曲线的关键参数：

v_max：最大速度
a_max：最大加速度
j_max：最大加加速度（Jerk）

j_max越大，曲线越接近梯形；j_max越小，曲线越“圆润”。

3.4 S形速度曲线的代码实现

我个人习惯把S形曲线分成两种场景来处理：完整七段和缩短版。完整七段用于长距离运动，缩短版用于短距离。

// S形速度曲线规划（简化版）
typedef struct {
    float v0, v_max, v1;  // 初速、最大速、末速
    float a_max, j_max;   // 最大加速度、加加速度
    float s_total;        // 总位移
    float t[7];           // 七段时间
} SCurveProfile;

void planSCurve(SCurveProfile *p) {
    // 先假设能跑完整七段
    float t_a = p->a_max / p->j_max;  // 加/减加速度段时间
    float t_v = (p->v_max - p->v0) / p->a_max - t_a; // 匀加速段时间
    
    // 计算加速段总位移
    float s_acc = p->v0 * (2*t_a + t_v) + 
                  0.5 * p->j_max * t_a * t_a * (t_a + t_v) +
                  0.5 * p->a_max * t_v * t_v;
    
    // 减速段对称计算...
    // 如果s_acc + s_dec > s_total，需要缩短匀加速/匀减速段
    
    // 实际项目中，我建议用查表法或迭代法求解
    // 这里给出的是简化逻辑
}

我的经验之谈：S形曲线虽然平滑，但计算量比梯形大3-5倍。在嵌入式系统上，我通常预先计算好轨迹表，运行时直接查表插值。这样既保证了平滑性，又不占用实时计算资源。

3.5 梯形 vs S形：怎么选？

这个问题我经常被问到。我的回答是：看场景，别迷信。

场景	推荐曲线	原因
高速搬运（如码垛）	梯形	时间优先，抖动可接受
精密装配（如芯片贴装）	S形	末端抖动必须最小化
人机协作	S形	平滑运动给人安全感
低算力MCU	梯形	计算简单，实时性好

注意：不要以为S形曲线就一定比梯形慢。在短距离运动中，S形曲线因为加速度变化平缓，反而可能比梯形更快到达目标位置。我做过测试，某些场景下S形比梯形快5%-10%。

3.6 知识体系总览

下面这张图，是我梳理的本章核心逻辑。你可以把它当作一个速查地图。

这张图把梯形和S形曲线的核心差异、适用场景、实现要点都串起来了。我个人建议你把它打印出来贴在工位上，做规划时瞄一眼，思路会清晰很多。

好了，关于梯形和S形速度曲线，咱们就聊到这儿。这两种曲线是轨迹规划的基石，搞懂了它们，后面再学更复杂的算法（比如多项式规划、B样条曲线）就会轻松很多。

可可精品资料库

📚 课程目录

3、时间最优轨迹规划：梯形速度曲线原理与实现，S形速度曲线原理与实现

3.1 梯形速度曲线：简单粗暴的效率之王

3.2 梯形速度曲线的代码实现

3.3 S形速度曲线：丝般顺滑的进阶方案

3.4 S形速度曲线的代码实现

3.5 梯形 vs S形：怎么选？

3.6 知识体系总览