3. 梯形速度规划：原理、实现、优缺点分析
梯形速度规划，这个名字听起来挺唬人。其实说白了，就是让机器人手臂的速度像爬楼梯一样——先匀加速，再匀速，最后匀减速。我在刚入行那会儿，第一次看到这个名词，还以为是什么高深的数学变换。后来发现，它就是最朴素、最实用的运动控制方法之一。
3.1 核心原理：三段式运动
为什么叫梯形？你想想看，把速度随时间的变化画出来，形状就像一个梯形。整个过程分三段：
- 加速段：从零开始，以恒定加速度加速到目标速度
- 匀速段：保持目标速度运行一段时间
- 减速段：以恒定减速度减速到零
嗯，这里要注意：加速和减速的加速度大小可以不一样。我在项目中遇到过，有时候为了减少冲击，会把减速度设得比加速度小一些，让末端执行器更平稳地停下来。
核心公式：
加速段时间：t₁ = v_max / a
减速段时间：t₃ = v_max / d
匀速段时间：t₂ = (总位移 - 加速段位移 - 减速段位移) / v_max
3.2 流程图：梯形速度规划的核心逻辑
下面这张图，是我自己画的一个决策流程。每次做梯形规划，脑子里都会过一遍这个逻辑：
3.3 代码实现：一个可用的梯形规划器
下面是我在实际项目中用过的梯形规划代码。我习惯用C语言写底层控制，这里给你一个精简版：
typedef struct {
float q_start; // 起始位置
float q_end; // 终点位置
float v_max; // 最大速度
float a; // 加速度
float d; // 减速度
float t1, t2, t3; // 三段时间
float total_time; // 总时间
float s1, s2, s3; // 三段位移
} TrapezoidProfile;
void Trapezoid_Init(TrapezoidProfile *prof) {
float s_total = prof->q_end - prof->q_start;
float s_acc = 0.5f * prof->v_max * prof->v_max / prof->a;
float s_dec = 0.5f * prof->v_max * prof->v_max / prof->d;
if (s_acc + s_dec <= fabs(s_total)) {
// 三段式：能达到最大速度
prof->t1 = prof->v_max / prof->a;
prof->t3 = prof->v_max / prof->d;
prof->s1 = 0.5f * prof->a * prof->t1 * prof->t1;
prof->s3 = 0.5f * prof->d * prof->t3 * prof->t3;
prof->s2 = fabs(s_total) - prof->s1 - prof->s3;
prof->t2 = prof->s2 / prof->v_max;
} else {
// 三角形：达不到最大速度
float v_peak = sqrt(2.0f * fabs(s_total) *
(prof->a * prof->d) / (prof->a + prof->d));
prof->t1 = v_peak / prof->a;
prof->t3 = v_peak / prof->d;
prof->t2 = 0.0f;
prof->s1 = 0.5f * prof->a * prof->t1 * prof->t1;
prof->s3 = 0.5f * prof->d * prof->t3 * prof->t3;
prof->s2 = 0.0f;
}
prof->total_time = prof->t1 + prof->t2 + prof->t3;
}
float Trapezoid_GetPosition(TrapezoidProfile *prof, float t) {
float s = 0.0f;
if (t <= prof->t1) {
// 加速段
s = 0.5f * prof->a * t * t;
} else if (t <= prof->t1 + prof->t2) {
// 匀速段
s = prof->s1 + prof->v_max * (t - prof->t1);
} else if (t <= prof->total_time) {
// 减速段
float t_dec = t - prof->t1 - prof->t2;
s = prof->s1 + prof->s2 + prof->v_max * t_dec - 0.5f * prof->d * t_dec * t_dec;
} else {
s = prof->s1 + prof->s2 + prof->s3;
}
return prof->q_start + (prof->q_end > prof->q_start ? s : -s);
}我的经验：这段代码我用了好几年。有一个坑——s_total 的正负号一定要处理好。我曾经因为忘了取绝对值，导致机器人反向运动时规划出错，差点撞到夹具。从那以后，我所有位置计算都先取绝对值，最后再根据方向恢复符号。
3.4 优缺点分析
梯形规划不是万能的。我把它优缺点列出来，你心里有个数：
| 维度 | 优点 | 缺点 |
|---|---|---|
| 计算量 | 极低。只有几个乘除和判断，单片机都能跑 | 无 |
| 实现难度 | 简单。半小时就能写出来 | 边界条件（如距离太短）需要额外处理 |
| 运动平滑性 | 速度连续，没有突变 | 加速度在拐点处突变，产生冲击力 |
| 适用场景 | 点对点运动、传送带抓取、码垛 | 高精度轨迹跟踪、高速加工不适合 |
| 实时性 | 优秀。每个周期只需一次函数调用 | 无 |
注意：梯形规划最大的问题在于加速度不连续。在加速段结束的瞬间，加速度从 a 跳变到 0。这个跳变会引起机械振动。如果你的机器人负载比较大，或者对振动敏感，建议考虑 S 形规划。
3.5 什么时候用梯形？什么时候换别的？
我个人习惯这样判断：
- 用梯形：负载小、速度慢、对精度要求不高的场合。比如小型桌面机械臂、教育机器人。我做过一个捡球机器人，用的就是梯形规划，跑得很稳。
- 换S形：负载大、速度快、或者末端有精密操作。比如焊接、涂胶、高速分拣。梯形规划在这些场合容易抖，S形虽然计算量大一点，但值得。
你想想看，如果你的机器人手臂上装了一个装满水的杯子，用梯形规划急停，水肯定会洒出来。这就是加速度突变带来的问题。
3.6 避坑指南
最后，分享几个我踩过的坑：
- 速度上限设得太高：我曾经把 v_max 设得比电机能跑到的速度还大，结果规划出来的时间全是负的。一定要做限幅。
- 加速度和减速度不匹配：如果 a 和 d 相差太大，梯形会变成不对称形状。有时候这不是坏事，但你要心里有数。
- 忘记处理零位移：如果起点和终点一样，梯形规划会出问题。我习惯在规划前加一个判断，位移为零就直接返回。
梯形速度规划，说白了就是运动控制里的"Hello World"。它简单、可靠、容易调试。虽然现在有更高级的规划方法，但我建议每个做机器人控制的人，都亲手写一遍梯形规划。这个过程会让你对运动控制有最直观的理解。