4. 梯形速度规划：从数学模型到代码实现
梯形速度曲线，说白了就是让电机先匀加速、再匀速、最后匀减速。你想想看，这就像开车——红灯变绿时一脚油门踩到限速，然后匀速巡航，看到下一个红灯再平稳刹停。简单、直观、好算，工业现场90%的单轴运动都在用这个。
我个人习惯把梯形规划当作所有复杂规划的“基准测试”。不管后面学S形还是多项式，先跑通梯形再说。今天我们就把它彻底吃透。
4.1 梯形速度曲线的数学模型
先看一个完整的梯形长什么样：
数学上，梯形曲线用分段函数描述：
v(t) =
a · t, 0 ≤ t < t₁ // 加速段
v_max, t₁ ≤ t < t₂ // 匀速段
v_max - a · (t - t₂), t₂ ≤ t ≤ t₃ // 减速段这里有个关键前提：加速和减速的加速度大小相等。如果不等，那就变成“非对称梯形”，后面我们会单独讲。
4.2 三个阶段的参数计算
实际项目中，我们通常已知三个量：总位移 S、最大速度 v_max、加速度 a。需要算出三个时间点 t₁、t₂、t₃。
先算加速/减速时间：
t_acc = v_max / a
t_dec = v_max / a // 对称情况下相等再算加速段和减速段走过的位移：
S_acc = 0.5 · a · t_acc² = v_max² / (2a)
S_dec = S_acc然后判断能不能达到最大速度：
核心判断逻辑：
如果 S ≥ S_acc + S_dec，说明有匀速段，梯形完整。
如果 S < S_acc + S_dec，说明还没加速到 v_max 就得减速了——这就是“三角形速度曲线”。
有匀速段时：
S_const = S - S_acc - S_dec
t_const = S_const / v_max
t₁ = t_acc
t₂ = t_acc + t_const
t₃ = t_acc + t_const + t_dec没有匀速段时（三角形）：
// 实际能达到的最高速度 v_peak < v_max
v_peak = sqrt(a · S)
t₁ = v_peak / a
t₂ = t₁
t₃ = 2 · t₁我曾经在调试一台高速贴片机时，忽略了三角形情况，结果规划器算出了负的匀速时间。嗯，从那以后我写代码第一件事就是判断“能不能到 v_max”。
4.3 参数计算对照表
| 参数 | 完整梯形（有匀速段） | 三角形（无匀速段） |
|---|---|---|
| 加速时间 t₁ | v_max / a | sqrt(S / a) |
| 匀速开始 t₂ | t₁ + (S - v_max²/a) / v_max | t₁（无匀速） |
| 总时间 t₃ | t₂ + v_max / a | 2 · sqrt(S / a) |
| 实际峰值速度 | v_max | sqrt(a · S) |
4.4 代码实现：C语言版本
下面是我在嵌入式控制器上实际跑过的代码。结构体清晰，注释到位，直接可以移植。
typedef struct {
float v_max; // 最大速度
float a; // 加速度（绝对值）
float S; // 总位移
float t1, t2, t3; // 三个时间节点
float v_peak; // 实际峰值速度
int has_const;// 是否有匀速段
} TrapezoidProfile;
void Trapezoid_Plan(TrapezoidProfile *p) {
float t_acc = p->v_max / p->a;
float S_acc = 0.5f * p->a * t_acc * t_acc;
float S_dec = S_acc;
if (p->S >= S_acc + S_dec) {
// 有匀速段
p->has_const = 1;
p->v_peak = p->v_max;
float S_const = p->S - S_acc - S_dec;
float t_const = S_const / p->v_max;
p->t1 = t_acc;
p->t2 = t_acc + t_const;
p->t3 = t_acc + t_const + t_acc;
} else {
// 三角形，到不了 v_max
p->has_const = 0;
p->v_peak = sqrtf(p->a * p->S);
float t_peak = p->v_peak / p->a;
p->t1 = t_peak;
p->t2 = t_peak; // 没有匀速段
p->t3 = 2.0f * t_peak;
}
}
// 根据时间 t 获取当前速度
float Trapezoid_GetVelocity(TrapezoidProfile *p, float t) {
if (t < 0.0f) return 0.0f;
if (t < p->t1) {
// 加速段
return p->a * t;
} else if (t < p->t2) {
// 匀速段
return p->v_peak;
} else if (t < p->t3) {
// 减速段
float dt = t - p->t2;
return p->v_peak - p->a * dt;
} else {
return 0.0f;
}
}个人经验：实际项目中，我习惯在结构体里额外加一个 float t_remain 字段，用于实时插补时判断“还剩多少时间”。这样在中断服务函数里调用 Trapezoid_GetVelocity 时，直接传 t_remain 就行，不用每次都算绝对时间。
4.5 仿真验证：Python快速原型
写C代码之前，我通常先用Python跑一遍仿真，确认参数没问题。下面是一个完整的仿真脚本：
import numpy as np
import matplotlib.pyplot as plt
def trapezoid_plan(S, v_max, a, dt=0.001):
t_acc = v_max / a
S_acc = 0.5 * a * t_acc**2
S_dec = S_acc
if S >= S_acc + S_dec:
# 完整梯形
S_const = S - S_acc - S_dec
t_const = S_const / v_max
t1, t2, t3 = t_acc, t_acc + t_const, t_acc + t_const + t_acc
v_peak = v_max
else:
# 三角形
v_peak = np.sqrt(a * S)
t_peak = v_peak / a
t1, t2, t3 = t_peak, t_peak, 2 * t_peak
# 生成时间序列
t = np.arange(0, t3, dt)
v = np.zeros_like(t)
acc_mask = t < t1
const_mask = (t >= t1) & (t < t2)
dec_mask = (t >= t2) & (t < t3)
v[acc_mask] = a * t[acc_mask]
v[const_mask] = v_peak
v[dec_mask] = v_peak - a * (t[dec_mask] - t2)
# 积分验证位移
S_actual = np.trapz(v, t)
print(f"规划位移: {S:.4f}, 实际位移: {S_actual:.4f}")
return t, v, t1, t2, t3
# 测试用例
t, v, t1, t2, t3 = trapezoid_plan(S=10.0, v_max=2.0, a=1.0)
plt.figure(figsize=(10, 4))
plt.plot(t, v, 'b-', linewidth=2)
plt.axvline(t1, color='gray', linestyle='--', alpha=0.5)
plt.axvline(t2, color='gray', linestyle='--', alpha=0.5)
plt.axvline(t3, color='gray', linestyle='--', alpha=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Velocity')
plt.title('Trapezoidal Velocity Profile')
plt.grid(True, alpha=0.3)
plt.show()避坑提醒：我曾经在仿真时发现位移积分总是差一点点。排查了半天，发现是 np.arange 的步长 dt 导致最后一个时间点没包含 t3。解决办法是用 np.linspace(0, t3, int(t3/dt)) 或者手动补一个点。这种边界问题在实时系统里尤其要小心。
4.6 梯形规划的优缺点
| 维度 | 评价 | 说明 |
|---|---|---|
| 计算量 | 极低 | 只有加减乘除和一次开方（三角形情况），适合低算力MCU |
| 加速度突变 | 有冲击 | 在 t₁、t₂、t₃ 三个点加速度跳变，机械上会有“咯噔”一下 |
| 适用场景 | 低速/大负载 | 传送带、升降台、点胶机等对冲击不敏感的场合 |
| 不适用场景 | 高速高精度 | 数控机床、机器人关节——加速度突变会引起振动和轨迹误差 |
梯形规划虽然简单，但它是理解所有速度规划的基础。你想想看，S形曲线无非就是把梯形的“直角拐弯”变成了圆角。下一章我们就会看到，S形曲线就是在梯形的基础上，给加速度也加上了斜坡。
不过在那之前，我建议你把今天的代码亲手敲一遍，跑通仿真。尤其是三角形和梯形两种情况的切换条件——这个判断逻辑，几乎所有的轨迹规划器里都会用到。