4、梯形速度曲线详解：加速段、匀速段、减速段计算，边界条件处理，代码实现
梯形速度曲线，说白了就是让机器人先加速、再匀速、最后减速。这是轨迹规划里最基础、也最实用的模型之一。我刚开始做运动控制时，第一个上手的项目就是调这个曲线。别看它结构简单，里面坑不少，尤其是边界条件处理，稍不注意就会让机器人“抖”一下。
4.1 梯形曲线的三段结构
梯形曲线由三段组成：加速段、匀速段、减速段。为什么叫梯形？你想想看，速度随时间变化的形状，就像一个梯形——先爬坡、再平走、最后下坡。
- 加速段：从初始速度 v₀ 加速到目标速度 v_max，加速度为 a（恒定）
- 匀速段：以 v_max 匀速运动，加速度为 0
- 减速段：从 v_max 减速到终止速度 v_end，减速度为 -a（恒定）
这里有个关键点：加速和减速的加速度大小通常是对称的，但也可以不对称。我个人习惯用对称的，因为好算、好调。不过我在项目中遇到过非对称的情况——比如负载变化大时，加速慢一点、减速快一点，效果反而更好。
4.2 各段的时间与位移计算
我们先定义几个参数：
| 符号 | 含义 |
|---|---|
| v₀ | 初始速度 |
| v_max | 最大速度（目标速度） |
| v_end | 终止速度 |
| a | 加速度（正值） |
| d | 总位移 |
加速段时间：
t_acc = (v_max - v₀) / a减速段时间：
t_dec = (v_max - v_end) / a加速段位移：
s_acc = v₀ * t_acc + 0.5 * a * t_acc²减速段位移：
s_dec = v_max * t_dec - 0.5 * a * t_dec²匀速段位移：
s_cruise = d - s_acc - s_dec匀速段时间：
t_cruise = s_cruise / v_max总时间：
T_total = t_acc + t_cruise + t_dec嗯，这里要注意：如果 s_cruise 算出来是负数，说明位移太短，根本跑不到 v_max。这就是边界条件问题，我们下面细说。
4.3 边界条件处理：三角形曲线
为什么会算出来 s_cruise 为负？说白了就是距离不够，还没加速到 v_max 就得开始减速了。这时候梯形退化成三角形——只有加速和减速，没有匀速段。
处理方式很简单：先假设能跑到 v_max，算一下 s_acc + s_dec 是否大于总位移 d。如果大于，那就得重新算一个实际能达到的最大速度 v_actual。
实际最大速度的计算公式：
v_actual = sqrt( (2 * a * d + v₀² + v_end²) / 2 )这个公式怎么来的？其实就是匀加速和匀减速的位移公式联立求解。我在项目中遇到过这种情况——有一次调试AGV小车，总位移只有20厘米，但参数设的v_max是1m/s，结果算出来匀速段是负的，小车直接“飞”过去了。后来加了边界判断，才稳下来。
4.4 代码实现
下面是我常用的梯形速度曲线生成函数。用Python写的，逻辑清晰，方便移植到C++或MATLAB。
def trapezoidal_profile(v0, v_max, v_end, a, d):
"""
生成梯形速度曲线参数
返回：各段时间和实际最大速度
"""
# 先算加速和减速所需位移
t_acc = (v_max - v0) / a
t_dec = (v_max - v_end) / a
s_acc = v0 * t_acc + 0.5 * a * t_acc**2
s_dec = v_max * t_dec - 0.5 * a * t_dec**2
# 检查是否能达到 v_max
if s_acc + s_dec > d:
# 三角形曲线：重新计算实际最大速度
v_actual = math.sqrt((2 * a * d + v0**2 + v_end**2) / 2)
t_acc = (v_actual - v0) / a
t_dec = (v_actual - v_end) / a
s_acc = v0 * t_acc + 0.5 * a * t_acc**2
s_dec = v_actual * t_dec - 0.5 * a * t_dec**2
t_cruise = 0
v_max_actual = v_actual
else:
# 梯形曲线：有匀速段
s_cruise = d - s_acc - s_dec
t_cruise = s_cruise / v_max
v_max_actual = v_max
return {
't_acc': t_acc,
't_cruise': t_cruise,
't_dec': t_dec,
'v_max_actual': v_max_actual,
's_acc': s_acc,
's_cruise': s_cruise if 's_cruise' in locals() else 0,
's_dec': s_dec
}4.5 梯形曲线的核心逻辑图
下面这张图展示了梯形速度曲线的完整决策流程。从输入参数开始，判断是否能达到 v_max，然后分两条路走——梯形或三角形。你一看就明白。
4.6 实际应用中的注意事项
梯形曲线虽然简单，但用起来有几个坑，我帮你列出来：
- 加速度不能太大：否则电机会过流，或者机器人会抖动。我一般限制在电机额定扭矩的80%以内。
- 速度不能太小：如果 v_max 设得太低，匀速段会很长，效率低。你想想看，明明可以跑快，为什么要慢悠悠的？
- 起始和终止速度要匹配：如果 v₀ 和 v_end 不相等，加速和减速就不对称了。这时候要分别算，不能偷懒用同一个公式。
- 浮点数精度问题：在嵌入式系统里，float 精度有限，算出来的时间可能差几个微秒。我建议用 double，或者加一个很小的 epsilon 做容差。
好了，梯形速度曲线就讲到这里。代码可以直接拿去用，但建议你根据实际硬件调一下加速度和速度的限幅。嗯，这部分内容够你消化一阵子了。