梯形速度规划（T型曲线）：原理与数学描述
梯形速度规划，圈里人常叫它T型曲线。说白了，就是让机器人先匀加速，再匀速，最后匀减速。你想想看，这像不像一个梯形？名字就是这么来的。
我刚开始做运动控制那会儿，觉得这东西太简单了。后来踩了不少坑才发现，越是基础的东西，越要搞透彻。今天咱们就把T型曲线掰开了揉碎了讲清楚。
为什么需要T型曲线？
直接给电机发目标位置不行吗？当然不行。你试试让机器人从静止瞬间跳到1m/s，结果就是——机械结构哐当一声，搞不好直接散架。加速度突变会产生无穷大的加加速度（Jerk），这在物理世界是不存在的。
T型曲线解决了两个核心问题：
- 限制最大速度——不让机器人跑太快
- 限制最大加速度——不让机器人启动/停止太猛
嗯，就这么简单。但实现起来，细节可不少。
数学描述：三段式结构
T型曲线把整个运动过程切成三段：
| 阶段 | 时间区间 | 加速度 | 速度变化 |
|---|---|---|---|
| 加速段 | 0 ≤ t < t₁ | a = a_max | 0 → v_max |
| 匀速段 | t₁ ≤ t < t₂ | a = 0 | v = v_max |
| 减速段 | t₂ ≤ t ≤ t₃ | a = -a_max | v_max → 0 |
这里有个关键点：加速段和减速段的时间通常是对称的。除非你刻意设置不对称的加减速，否则默认就是对称的。我个人习惯用对称的，好算，也好调。
核心公式推导
假设总位移为S，最大速度为v_max，最大加速度为a_max。那么：
加速段时间： t_a = v_max / a_max
加速段位移： S_a = 0.5 × a_max × t_a² = v_max² / (2 × a_max)
减速段位移： S_d = S_a（对称情况下）
匀速段位移： S_c = S - S_a - S_d
匀速段时间： t_c = S_c / v_max
总时间： T = 2 × t_a + t_c
重要提醒： 如果S太小，连加速段和减速段都塞不下，那就没有匀速段了。这种情况叫「三角形速度规划」，是T型曲线的一个特例。我在项目里遇到过好几次，新手容易忽略。
加速段/匀速段/减速段计算
咱们分阶段来看位置和速度的计算公式。假设起始位置为0，起始速度为0。
加速段（0 ≤ t < t_a）
a(t) = a_max
v(t) = a_max × t
p(t) = 0.5 × a_max × t²匀速段（t_a ≤ t < t_a + t_c）
a(t) = 0
v(t) = v_max
p(t) = S_a + v_max × (t - t_a)减速段（t_a + t_c ≤ t ≤ T）
a(t) = -a_max
v(t) = v_max - a_max × (t - t_a - t_c)
p(t) = S_a + S_c + v_max × (t - t_a - t_c) - 0.5 × a_max × (t - t_a - t_c)²我的小技巧： 实际写代码时，别直接用这些分段公式去算每个时刻的位置。更好的做法是先算好三个关键时间点t₁、t₂、t₃，然后根据当前时间判断在哪个段，再套对应的公式。这样代码结构清晰，不容易出错。
代码实现与仿真
下面我给出一份完整的Python实现。这份代码我用了好几年，从六轴机器人到AGV小车都跑过，算是经过实战检验的。
import numpy as np
import matplotlib.pyplot as plt
class TrapezoidalProfile:
"""梯形速度规划器"""
def __init__(self, v_max, a_max):
self.v_max = v_max # 最大速度
self.a_max = a_max # 最大加速度
def plan(self, S):
"""规划从0到S的运动"""
# 计算加速段参数
t_a = self.v_max / self.a_max
S_a = 0.5 * self.a_max * t_a ** 2
# 检查是否有匀速段
if S < 2 * S_a:
# 三角形规划：没有匀速段
t_a = np.sqrt(S / self.a_max)
t_c = 0
S_a = 0.5 * self.a_max * t_a ** 2
v_peak = self.a_max * t_a
else:
# 梯形规划：有匀速段
t_c = (S - 2 * S_a) / self.v_max
v_peak = self.v_max
# 关键时间点
t1 = t_a
t2 = t_a + t_c
t3 = 2 * t_a + t_c
return t1, t2, t3, v_peak, S_a
def get_state(self, t, S):
"""获取t时刻的位置、速度、加速度"""
t1, t2, t3, v_peak, S_a = self.plan(S)
if t < 0:
return 0.0, 0.0, 0.0
elif t < t1:
# 加速段
p = 0.5 * self.a_max * t ** 2
v = self.a_max * t
a = self.a_max
elif t < t2:
# 匀速段
p = S_a + v_peak * (t - t1)
v = v_peak
a = 0.0
elif t < t3:
# 减速段
t_dec = t - t2
p = S_a + v_peak * t_c + v_peak * t_dec - 0.5 * self.a_max * t_dec ** 2
v = v_peak - self.a_max * t_dec
a = -self.a_max
else:
# 运动结束
p = S
v = 0.0
a = 0.0
return p, v, a
# 仿真示例
if __name__ == "__main__":
# 参数设置
v_max = 1.0 # m/s
a_max = 0.5 # m/s²
S = 5.0 # m
# 创建规划器
planner = TrapezoidalProfile(v_max, a_max)
# 获取关键时间
t1, t2, t3, v_peak, S_a = planner.plan(S)
print(f"加速时间: {t1:.2f}s")
print(f"匀速时间: {t2-t1:.2f}s")
print(f"减速时间: {t3-t2:.2f}s")
print(f"总时间: {t3:.2f}s")
print(f"峰值速度: {v_peak:.2f}m/s")
# 生成轨迹点
dt = 0.01
t_span = np.arange(0, t3 + dt, dt)
positions = []
velocities = []
accelerations = []
for t in t_span:
p, v, a = planner.get_state(t, S)
positions.append(p)
velocities.append(v)
accelerations.append(a)
# 绘制结果
fig, axes = plt.subplots(3, 1, figsize=(10, 8))
axes[0].plot(t_span, positions)
axes[0].set_ylabel('位置 (m)')
axes[0].grid(True)
axes[1].plot(t_span, velocities)
axes[1].set_ylabel('速度 (m/s)')
axes[1].grid(True)
axes[2].plot(t_span, accelerations)
axes[2].set_ylabel('加速度 (m/s²)')
axes[2].set_xlabel('时间 (s)')
axes[2].grid(True)
plt.tight_layout()
plt.show()我曾经踩过的坑： 在嵌入式平台上跑这个代码时，浮点数精度问题导致时间判断出错。比如t刚好等于t1时，由于浮点误差可能被误判到匀速段。我的解决办法是加一个很小的epsilon（比如1e-6），判断时用t + epsilon来做区间比较。
T型曲线知识体系
下面这张图帮你理清T型曲线的核心逻辑：
实际应用中的注意事项
代码写完了，但离真正能用还差一步。我总结几个实战中容易出问题的地方：
- 速度单位要统一——我见过有人把mm/s和m/s混着用，结果机器人直接飞出去了。建议所有内部计算都用标准单位（m, m/s, m/s²）。
- 加减速不对称的情况——有些场景需要加速慢、减速快（比如抓取易碎品）。这时候要分别计算t_a和t_d，公式会复杂一些，但原理是一样的。
- 实时性要求——在实时系统里，别在中断里做大量浮点运算。我一般会提前算好轨迹表，运行时直接查表插值。
- 起点终点速度不为零——比如连续轨迹的衔接点。这个后面章节会详细讲，这里先提一嘴。
我的调试习惯： 每次写完轨迹规划代码，第一件事就是画速度曲线图。如果速度曲线不是完美的梯形或三角形，那肯定有bug。肉眼检查比看数据快多了。
好了，T型曲线就讲到这里。代码你可以直接拿去用，但建议先跑一遍仿真，看看曲线形状对不对。下一节咱们聊聊S型曲线——那个更平滑，但也更复杂。
公众号:蓝海资料掘金营，微信deep3321