📚 课程目录第4章梯形速度规划

4. 梯形速度规划：梯形速度曲线的数学模型、加速/匀速/减速三段式、参数计算

梯形速度规划，说白了就是让电机先匀加速、再匀速、最后匀减速。你想想看，这就像开车——红灯变绿，你踩油门加速，到了限速就稳住，看到前面红灯再踩刹车。简单、直观、好算。

我刚开始做运动控制那会儿，第一个上手的规划器就是梯形。后来在数控系统里也经常用它做快速定位。虽然现在有更平滑的S形曲线，但梯形速度规划在计算量、实现难度和响应速度上，依然有它的位置。

4.1 梯形速度曲线的数学模型

梯形速度曲线，顾名思义，速度随时间的变化像一个梯形。它由三段组成：

加速段：速度从0线性增加到目标速度 \( v_{max} \)
匀速段：保持 \( v_{max} \) 运行
减速段：速度从 \( v_{max} \) 线性减到0

对应的加速度曲线是方波：加速段为正加速度 \( a \)，匀速段为0，减速段为负加速度 \( -a \)。

数学上，我们通常用分段函数来描述速度 \( v(t) \)：

v(t) = 
  a·t,                    0 ≤ t < t₁
  v_max,                  t₁ ≤ t < t₂
  v_max - a·(t - t₂),     t₂ ≤ t ≤ t₃

其中 \( t₁ \) 是加速结束时间，\( t₂ \) 是匀速结束时间，\( t₃ \) 是总运动时间。

核心参数关系：

加速时间 \( t_a = t₁ = v_{max} / a \)
减速时间 \( t_d = t₃ - t₂ = v_{max} / a \)（加减速对称时）
匀速时间 \( t_c = t₂ - t₁ \)

4.2 加速/匀速/减速三段式详解

咱们一段一段拆开看。我习惯用“位移-速度-加速度”三层关系来理解。

4.2.1 加速段

加速度恒定，速度线性上升。位移是时间的二次函数：

s(t) = ½·a·t²,    0 ≤ t < t₁
v(t) = a·t
a(t) = a

加速段结束时，速度达到 \( v_{max} \)，走过的位移是：

s_a = ½·a·t₁² = v_{max}² / (2·a)

嗯，这里要注意：如果总位移不够大，可能根本到不了 \( v_{max} \) 就得开始减速。这种情况我们叫“三角形速度曲线”，后面会讲。

4.2.2 匀速段

速度不变，位移线性增长：

s(t) = s_a + v_{max}·(t - t₁),    t₁ ≤ t < t₂
v(t) = v_{max}
a(t) = 0

匀速段走过的位移：

s_c = v_{max}·t_c

4.2.3 减速段

加速度为负，速度线性下降：

s(t) = s_a + s_c + v_{max}·(t - t₂) - ½·a·(t - t₂)²,    t₂ ≤ t ≤ t₃
v(t) = v_{max} - a·(t - t₂)
a(t) = -a

减速段走过的位移与加速段对称：

s_d = v_{max}² / (2·a) = s_a

我的经验：在大多数工业应用中，加减速采用相同的加速度值。这样计算简单，电机正反转特性也一致。但如果你遇到垂直轴或负载变化大的场景，加减速分开设会更安全。

4.3 参数计算：加速度、速度、位移

实际项目中，我们通常已知总位移 \( S \)、最大速度 \( v_{max} \) 和加速度 \( a \)，需要计算各段时间。或者反过来，已知位移和时间约束，反推加速度和速度。

4.3.1 已知 S、v_max、a，求各段时间

先算加减速段位移：

s_a = s_d = v_max² / (2·a)

如果 \( s_a + s_d \ge S \)，说明到不了最大速度，属于三角形规划。否则：

s_c = S - s_a - s_d
t_c = s_c / v_max
t_a = t_d = v_max / a
总时间 T = t_a + t_c + t_d

4.3.2 已知 S 和 T，求 v_max 和 a

这种情况更常见——客户说“2秒内走100mm”，你得反算出速度和加速度。

假设加减速对称，总位移公式：

S = v_max·(T - v_max / a)

这里有两个未知数，需要额外约束。我通常的做法是：

先根据电机能力设定加速度 \( a \)
再解出 \( v_{max} \)

解方程：

v_max² / a - v_max·T + S = 0
v_max = [a·T - sqrt(a²·T² - 4·a·S)] / 2

注意判别式必须 ≥ 0，否则给定的时间和位移不可能同时满足。

我曾经踩过的坑：有一次客户要求“0.5秒走10mm”，我按公式算出来加速度需要 80 m/s²，电机根本扛不住。后来才意识到，参数计算前一定要先校核电机峰值扭矩和最大加速度限制。公式算得通，不代表硬件做得到。

4.3.3 三角形速度曲线（无匀速段）

当位移较小时，加速完就得立刻减速，没有匀速段。此时：

v_peak = sqrt(a·S)    // 实际能达到的最大速度
t_a = t_d = v_peak / a
总时间 T = 2·v_peak / a = 2·sqrt(S / a)

判断条件：\( S < v_{max}² / a \)

4.4 梯形速度规划的知识结构

下面这张图帮你理清整个知识体系：

4.5 梯形规划的优缺点

维度	评价	说明
计算量	极低	只有加减乘除，没有三角函数或迭代
实现难度	简单	几行C代码就能跑起来
响应速度	快	实时计算开销小，适合高刷新率
平滑性	差	加速度突变，会产生冲击（Jerk无限大）
适用场景	定位、点对点	对舒适度要求不高的工业场合

我的建议：如果你做的是码垛机、传送带定位、或者简单的XY平台，梯形规划完全够用。但如果是做CNC加工或机器人轨迹，建议至少用S形曲线。我早期在一个贴片机项目里用了梯形规划，结果末端抖动太大，后来换成S形才解决。

4.6 梯形规划的代码骨架

下面是一个梯形速度规划器的核心计算函数。我习惯把参数校验和分段计算分开写：

typedef struct {
    float v_max;    // 最大速度
    float a;        // 加速度
    float S;        // 总位移
    float t_a;      // 加速时间
    float t_c;      // 匀速时间
    float t_d;      // 减速时间
    float T;        // 总时间
    int   is_triangle; // 是否为三角形规划
} TrapezoidProfile;

int TrapezoidPlan(TrapezoidProfile *p) {
    float s_acc = p->v_max * p->v_max / (2.0f * p->a);
    
    // 判断是否三角形
    if (2.0f * s_acc >= p->S) {
        p->is_triangle = 1;
        float v_peak = sqrtf(p->a * p->S);
        p->t_a = v_peak / p->a;
        p->t_d = p->t_a;
        p->t_c = 0.0f;
        p->T = p->t_a + p->t_d;
    } else {
        p->is_triangle = 0;
        p->t_a = p->v_max / p->a;
        p->t_d = p->t_a;
        p->t_c = (p->S - 2.0f * s_acc) / p->v_max;
        p->T = p->t_a + p->t_c + p->t_d;
    }
    return 0;
}

实际运行时，你还需要一个位置插值函数，根据当前时间 t 计算目标位置。这个我们后面讲插补时会详细展开。

注意：上面的代码假设加减速对称。如果你的系统加减速能力不同，需要分别传入 a_acc 和 a_dec，计算逻辑会复杂一些。我曾经在一个大负载龙门架上吃过亏——加速时电机够力，减速时惯性太大刹不住，后来改成非对称加减速才搞定。

梯形速度规划是运动控制里最基础的规划方式。它简单、可靠、容易调试。虽然平滑性不如S形曲线，但在很多工业场景下，它依然是首选。理解透梯形规划，后面学S形、多项式规划就会轻松很多。