梯形速度规划：原理与数学模型

梯形速度规划，这个名字听着挺唬人，其实说白了就是「加速-匀速-减速」三段式。我最早接触这个算法是在做一款桌面级机械臂的时候，那时候对速度规划没什么概念，直接给电机发阶跃速度指令，结果机械臂启动时抖得像筛子一样。后来才明白，你需要给速度一个「爬坡」的过程。

梯形规划的核心思想很简单：速度不能突变。你想想看，物理世界里的物体都有惯性，速度从0直接跳到目标值，加速度理论上就是无穷大，这谁受得了？所以我们要给速度一个平滑的过渡。

数学模型

先看加速度的表达式。梯形规划里，加速度是分段常数：

a(t) = 
  a_max,    0 ≤ t < t1    // 加速段
  0,        t1 ≤ t < t2   // 匀速段
  -a_max,   t2 ≤ t < t3   // 减速段

速度就是加速度的积分。从0开始加速，到匀速，再减速到0：

v(t) =
  a_max * t,              0 ≤ t < t1
  v_max,                  t1 ≤ t < t2
  v_max - a_max * (t - t2), t2 ≤ t < t3

位置呢？再对速度积分一次。这个我就不写完整公式了，你记住一个关键点：位移是速度曲线下的面积。梯形面积怎么算？上底加下底乘高除以2，初中数学就够了。

三段式结构详解

我习惯把梯形规划分成三种情况来讨论。为什么？因为实际项目中，你给的位移可能不够长，速度根本跑不到最大值。

情况一：完整梯形（能跑到最大速度）

这种情况最理想。加速段、匀速段、减速段都有。加速时间和减速时间相等：

t_acc = v_max / a_max
t_dec = v_max / a_max
t_const = (S - v_max * t_acc) / v_max

总时间就是三段相加。我在做AGV小车导航时，大部分路径规划都能跑到完整梯形，因为场地够大。

情况二：三角形（跑不到最大速度）

位移不够大，刚加速到一半就得减速。这时候没有匀速段，像个三角形。我记得有一次调试一个短行程的直线模组，位移只有10毫米，按完整梯形算出来匀速段时间是负数，当时还以为是代码写错了。后来才意识到，这种情况只能用三角形规划。

v_peak = sqrt(a_max * S)   // 实际能达到的峰值速度
t_acc = v_peak / a_max
t_dec = t_acc

情况三：临界状态

刚好加速到最大速度就得减速，匀速段长度为0。这个在实际中很少遇到，但作为边界条件一定要处理。我曾经在写单元测试时漏掉了这个case，结果线上出了个bug，电机在临界点附近反复启停，声音特别刺耳。

优缺点分析

梯形规划用了这么多年，自然有它的道理。但也不是万能的，我跟你聊聊我的真实感受。

优点	缺点
实现简单，代码量少	加速度突变，有冲击
计算量小，适合嵌入式	加加速度（jerk）无穷大
参数直观，容易调试	高速时振动明显
时间最优（给定约束下）	不适合精密加工场景

优点方面，我最看重的是它的简单性。在STM32这类单片机上，梯形规划只需要几行C代码就能跑起来，实时性完全没问题。而且参数物理意义明确，现场调试时调个v_max和a_max，效果立竿见影。

缺点方面，最大的问题就是加速度突变。在加速段结束的瞬间，加速度从a_max跳变到0，加加速度（jerk）理论上无穷大。这会导致什么？机械结构会「咯噔」一下。我做过一个高速分拣机器人，用梯形规划时，每次加减速切换点都能听到明显的撞击声，后来换成S形规划才解决。

什么时候该用梯形规划？

我个人建议：

• 低成本的简单运动：比如家用扫地机、玩具机器人，梯形规划完全够用
• 对时间要求苛刻：梯形规划是时间最优的，能跑多快跑多快
• 计算资源有限：8位单片机、低端DSP，跑梯形规划毫无压力
• 调试阶段先用着：我习惯先用梯形规划把功能跑通，再优化成更平滑的曲线

嗯，这里要注意一点：梯形规划虽然简单，但参数设置不合理也会出问题。比如加速度设得太大，电机可能丢步；设得太小，运动又太慢。我一般会先根据电机手册算一个理论值，再在实际负载下调试验证。

核心逻辑流程图

下面这张图是我自己画的，把梯形规划的决策逻辑串了一遍。你跟着箭头走一遍，基本就明白整个流程了。

这张图把整个决策过程串起来了。你从输入开始，先判断位移够不够跑完整梯形，然后分两条路走。实际写代码时，我建议把这两种情况封装成两个函数，逻辑清晰，也方便后续扩展。

梯形规划虽然基础，但它是理解更复杂规划算法的基石。你把这个吃透了，后面学S形规划、多项式规划都会轻松很多。说白了，梯形规划就是速度规划里的「Hello World」，简单但经典。