086、梯形速度曲线生成

086、梯形速度曲线生成

从一次电机“起飞”事故说起

去年调试一台三轴机械臂，Z轴在空载时上升速度设定为300mm/s，加速度500mm/s²。上位机发了个100mm的绝对定位指令，电机直接“弹射起步”——速度曲线像被刀切过一样，从0瞬间跳到300mm/s，然后到终点前又硬生生刹停。机械臂抖得像帕金森患者，末端执行器上的吸盘直接把工件甩飞了。

查了三天，发现是速度规划模块里梯形曲线生成有个边界条件没处理。当时用的开源库，默认参数下加速度时间计算溢出了，导致加速段被跳过。从那以后，我写速度曲线生成代码，第一件事就是画个梯形，把三个阶段的边界条件用数学公式钉死在代码里。

梯形速度曲线的数学骨架

梯形速度曲线由三段组成：匀加速段、匀速段、匀减速段。核心参数就四个：起始速度Vs、目标速度Vt、加速度a、减速度d（通常a=d，但实际工程中往往不等，比如重力轴上升时减速度要小于加速度）。

关键公式就三个：

加速段时间：ta = (Vt - Vs) / a
减速段时间：td = (Vt - Ve) / d
匀速段时间：tc = (总位移 - 加速段位移 - 减速段位移) / Vt

这里有个坑：当目标位移太短时，可能根本跑不到目标速度，梯形会退化成三角形。很多新手在这里直接套公式，算出来tc为负值，然后程序就崩了。

代码实现：从“能跑”到“靠谱”

先看一个典型的梯形曲线生成函数，我习惯用C写，因为嵌入式里跑得多：