梯形速度规划：从数学模型到工程实现
梯形速度曲线，说白了就是让机器人先匀加速、再匀速、最后匀减速。听起来简单吧？但真正把它写进代码，你会发现坑不少。我最早做AGV调度系统时，就因为梯形规划没处理好边界条件，车子在终点前急停，直接把货架上的零件晃下来了……嗯，从那以后我对这玩意儿就特别较真。
梯形速度曲线的数学模型
先看理想情况。我们假设最大速度是 v_max，加速度是 a，减速度也是 a（对称梯形）。整个运动分成三段：
- 加速段：速度从0线性增加到
v_max，用时t_a - 匀速段：保持
v_max运行，用时t_c - 减速段：速度从
v_max线性减到0，用时t_d
你想想看，如果总位移是 S，那这三段走过的路程加起来必须等于 S。公式长这样：
加速段位移：S_a = 0.5 * a * t_a²
匀速段位移：S_c = v_max * t_c
减速段位移：S_d = 0.5 * a * t_d²
总位移约束：S_a + S_c + S_d = S这里有个隐藏条件——加速和减速时间通常相等（对称规划），所以 t_a = t_d = v_max / a。代入后就能解出匀速段时间：
t_c = (S - v_max² / a) / v_max核心结论：梯形规划的本质就是解这个一元方程。如果 t_c < 0，说明路程太短，根本跑不到最大速度——这时候就得退化成三角形速度曲线。
三段参数计算：手算一遍就懂了
我习惯把参数分成两类：规划参数（用户给的）和计算参数（推导出来的）。
| 参数类型 | 符号 | 说明 |
|---|---|---|
| 规划参数 | v_max, a, S |
用户设定的最大速度、加速度、总位移 |
| 计算参数 | t_a, t_c, t_d |
三段各自的时间 |
| 中间量 | S_min |
能跑到v_max的最短位移，S_min = v_max² / a |
具体计算流程是这样的：
- 先算
S_min = v_max² / a - 如果
S >= S_min，恭喜，标准梯形：t_a = v_max / a，t_c = (S - S_min) / v_max，t_d = t_a - 如果
S < S_min，只能跑三角形：实际最大速度v_peak = sqrt(a * S)，t_a = t_d = v_peak / a，t_c = 0
我的经验：很多新手直接套梯形公式，结果 t_c 算出负数也不检查。我曾经在一个码垛项目里吃过这个亏——机器人还没加速完就开始减速，轨迹直接扭曲。所以第一步判断 S >= S_min 绝对不能省。
代码实现：C语言版本
下面是我常用的梯形规划函数。注意边界条件处理——我加了三个断言，实际工程里可以换成异常处理或日志输出。
typedef struct {
double t_a; // 加速段时间
double t_c; // 匀速段时间
double t_d; // 减速段时间
double v_max; // 实际最大速度
double a; // 加速度
} TrapezoidProfile;
TrapezoidProfile trapezoid_plan(double S, double v_max, double a) {
TrapezoidProfile profile = {0};
// 边界检查
if (S <= 0 || v_max <= 0 || a <= 0) {
// 无效输入，返回零轨迹
return profile;
}
double S_min = (v_max * v_max) / a;
if (S >= S_min) {
// 标准梯形
profile.t_a = v_max / a;
profile.t_c = (S - S_min) / v_max;
profile.t_d = profile.t_a;
profile.v_max = v_max;
profile.a = a;
} else {
// 退化为三角形
double v_peak = sqrt(a * S);
profile.t_a = v_peak / a;
profile.t_c = 0.0;
profile.t_d = profile.t_a;
profile.v_max = v_peak;
profile.a = a;
}
return profile;
}注意：浮点数比较时别直接用 ==。比如判断 S >= S_min，建议加一个 1e-6 的容差，否则边界情况容易出问题。我曾经因为 S 和 S_min 刚好相等时浮点误差导致误判，机器人原地抖了一下。
边界条件处理：最容易翻车的地方
梯形规划有四个边界条件必须处理：
- 零位移：
S = 0，直接返回全零轨迹，不执行任何运动 - 负参数：
v_max <= 0或a <= 0，属于非法输入，必须拦截 - 路程太短：
S < S_min，退化为三角形 - 速度/加速度极限：实际规划出的
v_peak不能超过电机物理极限，否则会丢步
我个人习惯在规划前加一个预处理函数，专门做参数校验：
int validate_params(double S, double v_max, double a) {
if (S < 0) return -1; // 位移不能为负
if (v_max <= 0) return -2; // 速度必须为正
if (a <= 0) return -3; // 加速度必须为正
return 0; // 参数合法
}你想想看，如果用户误传了一个负位移，程序直接崩了，那多尴尬。所以参数校验是梯形规划的第一道防线。
知识体系总览
下面这张图把梯形速度规划的完整流程串起来了——从输入参数到三段计算，再到边界条件分支，一目了然。
梯形速度规划是运动控制的入门必修课。别看它结构简单，边界条件处理得好不好，直接决定了机器人跑起来是丝滑还是抽搐。我建议你拿到代码后，先跑几个极端测试：零位移、超短位移、超大加速度——把这些边界情况都验证一遍，后面做S形曲线规划时才能心里有底。