4. 梯形速度规划：三段式设计的数学原理与参数配置

梯形速度曲线，说白了就是让机器人先匀加速、再匀速、最后匀减速。听起来很简单对吧？但就是这看似简单的三段式，在实际工程中埋了不少坑。我最早做AGV运动控制时，觉得梯形规划太“低级”，一上来就搞S形曲线，结果调试了两个月都没跑顺。后来老老实实把梯形规划吃透，才发现——基础打不牢，上层全是空中楼阁。

4.1 梯形速度曲线的数学原理

梯形速度曲线由三段组成：加速段、匀速段、减速段。每一段的速度随时间线性变化，或者保持不变。

先看加速段。加速度为常数 \(a\)，速度从 \(v_0\) 增加到 \(v_{max}\)：

v(t) = v₀ + a·t          (0 ≤ t ≤ t₁)
s(t) = v₀·t + ½·a·t²

匀速段就简单了，速度不变：

v(t) = v_max             (t₁ ≤ t ≤ t₂)
s(t) = s₁ + v_max·(t - t₁)

减速段，加速度为 \(-a\)，速度降到目标速度 \(v_{end}\)：

v(t) = v_max - a·(t - t₂)  (t₂ ≤ t ≤ t₃)
s(t) = s₂ + v_max·(t - t₂) - ½·a·(t - t₂)²

嗯，这里要注意：加速和减速的加速度大小可以不同。我在项目中遇到过，空载和满载时电机响应差异很大，所以通常会分别配置加速加速度和减速加速度。

4.2 三段式设计：什么时候能跑出梯形？

你想想看，梯形曲线不是什么时候都能完整跑出来的。实际工程中，最常见的情况有两种：

• 完整梯形：位移足够大，加速段和减速段都能跑满，中间有一段完整的匀速。
• 三角形曲线：位移太小，刚加速完就要减速，匀速段长度为0。

判断条件很简单：

// 计算理论最大速度下需要的加速+减速位移
S_acc_dec = (v_max² - v₀²) / (2·a_acc) + (v_max² - v_end²) / (2·a_dec)

if (S_total >= S_acc_dec) {
    // 完整梯形，有匀速段
} else {
    // 三角形，需要降低实际最大速度
    v_actual = sqrt((2·a_acc·a_dec·S_total + a_dec·v₀² + a_acc·v_end²) / (a_acc + a_dec))
}

我个人习惯把这段逻辑封装成一个函数，叫 compute_trapezoid_profile()。输入起点速度、终点速度、最大速度、加速度、减速度、总位移，输出三段的时间和位移。这样上层调用时完全不用关心内部是梯形还是三角形。

4.3 参数配置：最大加速度与Jerk

梯形规划的参数配置，核心就两个：最大加速度和Jerk（加加速度）。等等，梯形规划不是没有Jerk吗？没错，纯梯形规划的速度曲线是分段线性的，加速度在切换点处有跳变。但实际工程中，我们通常会给梯形曲线加上Jerk限制，让加速度平滑过渡。

参数	物理含义	典型取值范围	对性能的影响
最大加速度	速度变化率的极限	0.5 ~ 5 m/s²	决定加减速时间，影响整体效率
最大Jerk	加速度变化率的极限	5 ~ 50 m/s³	决定加速度切换的平滑度，影响机械冲击
最大速度	匀速段的速度上限	0.5 ~ 10 m/s	决定最高运行效率，受限于电机和安全性

我曾经在一个码垛机器人项目上吃过亏。当时只配置了最大加速度，没限制Jerk。结果加速度从0瞬间跳到最大值，机械臂末端抖得像筛子一样，码垛精度直接超差。后来加上Jerk限制，把加速度变化率控制在20 m/s³以内，问题才解决。

4.4 梯形规划的完整实现

下面是一个完整的梯形速度规划实现，包含Jerk限制的平滑处理：

typedef struct {
    float v0;        // 起始速度
    float v_end;     // 终点速度
    float v_max;     // 最大速度
    float a_max;     // 最大加速度
    float j_max;     // 最大Jerk
    float s_total;   // 总位移
} TrapezoidConfig;

typedef struct {
    float t_acc;     // 加速段时间
    float t_const;   // 匀速段时间
    float t_dec;     // 减速段时间
    float s_acc;     // 加速段位移
    float s_const;   // 匀速段位移
    float s_dec;     // 减速段位移
    float v_actual;  // 实际达到的最大速度
} TrapezoidProfile;

TrapezoidProfile compute_trapezoid(TrapezoidConfig cfg) {
    TrapezoidProfile prof = {0};
    
    // 1. 计算理论加速和减速所需位移
    float s_acc_min = (cfg.v_max * cfg.v_max - cfg.v0 * cfg.v0) / (2 * cfg.a_max);
    float s_dec_min = (cfg.v_max * cfg.v_max - cfg.v_end * cfg.v_end) / (2 * cfg.a_max);
    float s_min_total = s_acc_min + s_dec_min;
    
    if (cfg.s_total >= s_min_total) {
        // 完整梯形
        prof.v_actual = cfg.v_max;
        prof.t_acc = (cfg.v_max - cfg.v0) / cfg.a_max;
        prof.t_dec = (cfg.v_max - cfg.v_end) / cfg.a_max;
        prof.s_acc = s_acc_min;
        prof.s_dec = s_dec_min;
        prof.s_const = cfg.s_total - s_acc_min - s_dec_min;
        prof.t_const = prof.s_const / cfg.v_max;
    } else {
        // 三角形，需要计算实际能达到的最大速度
        float a = cfg.a_max;
        prof.v_actual = sqrt((2 * a * cfg.s_total + cfg.v0 * cfg.v0 + cfg.v_end * cfg.v_end) / 2);
        prof.t_acc = (prof.v_actual - cfg.v0) / a;
        prof.t_dec = (prof.v_actual - cfg.v_end) / a;
        prof.s_acc = (cfg.v0 + prof.v_actual) * prof.t_acc / 2;
        prof.s_dec = (prof.v_actual + cfg.v_end) * prof.t_dec / 2;
        prof.s_const = 0;
        prof.t_const = 0;
    }
    
    return prof;
}

4.5 梯形规划的知识体系

下面这张图总结了梯形速度规划的核心知识点和它们之间的关系：

4.6 梯形规划的局限性

梯形规划虽然简单高效，但有两个明显的短板：

1. 加速度跳变：在加速段结束和减速段开始的时刻，加速度瞬间变化，会产生冲击力。对于高精度定位或重载场景，这个冲击可能造成位置超调或机械损伤。
2. 速度曲线不光滑：速度的导数（加速度）不连续，导致电机扭矩指令有阶跃。这会激发机械结构的谐振频率，产生振动。

怎么解决？我一般会在梯形规划的输出端加一个速度前馈+加速度前馈的补偿。具体做法是：在梯形规划的每个时间点，不仅输出目标速度，还输出目标加速度。伺服驱动器收到加速度前馈后，可以提前补偿惯性力，从而减小实际加速度的跳变幅度。