4. 梯形速度规划：T型速度曲线原理、加速/匀速/减速阶段计算、代码实现（Python）
梯形速度规划，圈里人常叫它T型曲线。说白了，就是让电机先匀加速、再匀速、最后匀减速。你想想看，速度-时间图像画出来就是个梯形，名字就这么来的。
我刚开始做运动控制那会儿，总觉得这算法太简单，没啥技术含量。直到有一次在产线上调试一个高速拾放机构，才发现——越是基础的算法，越容易在细节上翻车。嗯，今天咱们就把T型曲线掰开揉碎了讲清楚。
核心思想：将整个运动过程划分为三个阶段——加速段、匀速段、减速段。每个阶段内加速度恒定，速度线性变化。
4.1 T型速度曲线的数学模型
先看三个基本参数：
- 总位移 \( S \)（你要走多远）
- 最大速度 \( V_{max} \)（你允许跑多快）
- 加速度 \( a \)（加、减速的力度，通常对称）
这三个参数一确定，整个曲线就定了。但有个前提——你得先判断能不能跑到最大速度。
4.1.1 能不能跑到最大速度？
我见过不少新手，上来就按最大速度算时间，结果发现位移不够，根本加不到那个速度就得开始减速。这就是所谓的三角形曲线——只有加速和减速，没有匀速段。
判断条件很简单：
- 如果 \( S > \frac{V_{max}^2}{a} \)，能跑到最大速度，有匀速段 → 梯形曲线
- 如果 \( S \leq \frac{V_{max}^2}{a} \)，跑不到最大速度，没有匀速段 → 三角形曲线
这个临界值怎么来的？其实就是加速段和减速段各需要 \( \frac{V_{max}}{a} \) 时间，位移各为 \( \frac{V_{max}^2}{2a} \)，加起来就是 \( \frac{V_{max}^2}{a} \)。
我的习惯：写代码时先算这个临界位移，再决定走梯形还是三角形。否则后面算出来的时间全是负数，排查起来很头疼。
4.2 各阶段计算公式
假设能跑到最大速度，咱们分三段算。
4.2.1 加速阶段
- 时间：\( t_1 = \frac{V_{max}}{a} \)
- 位移：\( S_1 = \frac{1}{2} a t_1^2 = \frac{V_{max}^2}{2a} \)
- 速度：\( v(t) = a \cdot t \)，其中 \( 0 \leq t \leq t_1 \)
4.2.2 匀速阶段
- 时间：\( t_2 = \frac{S - 2S_1}{V_{max}} \)
- 位移：\( S_2 = V_{max} \cdot t_2 \)
- 速度：\( v(t) = V_{max} \)，其中 \( t_1 \leq t \leq t_1 + t_2 \)
4.2.3 减速阶段
- 时间：\( t_3 = \frac{V_{max}}{a} \)（与加速对称）
- 位移：\( S_3 = \frac{V_{max}^2}{2a} \)
- 速度：\( v(t) = V_{max} - a \cdot (t - t_1 - t_2) \)，其中 \( t_1 + t_2 \leq t \leq t_1 + t_2 + t_3 \)
总时间 \( T = t_1 + t_2 + t_3 \)。
我曾经踩过的坑：减速阶段的速度公式，很多人写成 \( v(t) = V_{max} - a \cdot t \)。这不对！因为减速开始的时间不是0，而是 \( t_1 + t_2 \)。一定要减去这个偏移量，否则速度曲线会断掉。
4.3 知识体系与核心逻辑
下面这张图，把T型曲线的整个计算流程串起来了。我建议你多看两眼，写代码时心里就有谱了。
4.4 Python代码实现
下面是我自己项目里一直在用的实现。我习惯把规划器和插补器分开，这样调试时方便单独测。
import numpy as np
def trapezoidal_profile(S, Vmax, a, dt=0.001):
"""
T型速度规划
参数:
S: 总位移 (正数)
Vmax: 最大速度
a: 加速度 (正数)
dt: 插补周期 (秒)
返回:
t_seq: 时间序列
v_seq: 速度序列
s_seq: 位置序列
"""
# 1. 判断能否达到最大速度
S_critical = Vmax**2 / a
if S > S_critical:
# 梯形曲线
t1 = Vmax / a
S1 = 0.5 * a * t1**2
t2 = (S - 2 * S1) / Vmax
t3 = t1
T = t1 + t2 + t3
print(f"梯形曲线: t1={t1:.3f}s, t2={t2:.3f}s, t3={t3:.3f}s, 总时间={T:.3f}s")
else:
# 三角形曲线
Vp = np.sqrt(a * S) # 实际能达到的峰值速度
t1 = Vp / a
t3 = t1
t2 = 0.0
T = t1 + t3
Vmax = Vp # 更新实际最大速度
print(f"三角形曲线: t1={t1:.3f}s, t2=0s, t3={t3:.3f}s, 总时间={T:.3f}s")
# 2. 生成时间序列
N = int(T / dt) + 1
t_seq = np.linspace(0, T, N)
v_seq = np.zeros(N)
s_seq = np.zeros(N)
# 3. 逐点计算速度和位置
for i, t in enumerate(t_seq):
if t <= t1:
# 加速段
v = a * t
s = 0.5 * a * t**2
elif t <= t1 + t2:
# 匀速段
v = Vmax
s = 0.5 * a * t1**2 + Vmax * (t - t1)
else:
# 减速段
t_dec = t - t1 - t2
v = Vmax - a * t_dec
s = (0.5 * a * t1**2 + Vmax * t2 +
Vmax * t_dec - 0.5 * a * t_dec**2)
v_seq[i] = v
s_seq[i] = s
return t_seq, v_seq, s_seq
# 使用示例
if __name__ == "__main__":
# 参数：走100mm，最大速度50mm/s，加速度200mm/s²
t, v, s = trapezoidal_profile(S=100.0, Vmax=50.0, a=200.0, dt=0.001)
print(f"最终位置: {s[-1]:.3f} mm (目标: 100 mm)")我个人习惯：dt 取 0.001 秒（1ms）是大多数伺服驱动器的标准插补周期。如果控制器性能差一些，可以放宽到 0.005 秒，但精度会下降。
4.5 常见问题与避坑
代码写完了，但实际跑起来可能还有坑。我挑几个最常见的说说。
| 问题 | 现象 | 原因 | 解决办法 |
|---|---|---|---|
| 最终位置偏差 | 走不到目标位置 | 浮点累积误差 | 最后一步强制修正位置 |
| 速度跳变 | 加减速衔接处抖动 | 时间偏移量没算对 | 检查减速段公式中的 t_dec |
| 加速度不对称 | 加、减速时间不同 | 加、减速度值设了不同值 | 统一用同一个 a |
| 位移太小 | 计算报错或时间极短 | S 小于最小可行位移 | 加最小位移保护判断 |
我曾经吃过一次亏：在减速段的位置计算里，我忘了加上匀速段已经走过的位移 \( V_{max} \cdot t_2 \)。结果位置曲线在减速段突然往回跳，电机直接过冲报警。排查了整整一个下午才发现是公式少了一项。
4.6 梯形 vs 三角形：怎么选？
其实不是"选"的问题，而是物理约束决定的。你想想看，位移就那么长，加速度就那么大，能不能跑到最大速度是算出来的，不是拍脑袋定的。
但有一点我可以分享：在允许的情况下，尽量让运动进入匀速段。为什么？因为匀速段的速度最平稳，对机械结构的冲击最小。我见过一些设备，为了赶节拍硬把加速度设得很大，结果机器抖得跟筛子似的，得不偿失。
梯形速度规划，说白了就是"稳"字当头。它不花哨，但可靠。你把它吃透了，后面学S型曲线、多项式规划都会轻松很多。