3、梯形速度规划：原理、实现、优缺点分析。
梯形速度规划，这个名字听起来挺唬人。其实说白了，就是让电机先匀加速，再匀速，最后匀减速。你想想看，速度曲线画出来像个梯形，所以就叫这个名。这是最基础、最直观的运动规划方式，也是我入行时接触的第一个算法。
我记得刚做机器人那会儿，领导丢给我一个任务：让机械臂从A点走到B点，别抖，别过冲。我当时啥也不懂，翻了几篇论文，头都大了。后来一个老工程师跟我说，你先试试梯形速度，简单粗暴，够用。嗯，这一试，就是好多年。
3.1 梯形速度规划的原理
核心思想就三个字：加-匀-减。整个运动过程被分成三个阶段：
- 加速阶段：电机从零速度开始，以恒定加速度
a加速到目标速度v_max。 - 匀速阶段：以
v_max匀速运动，保持速度不变。 - 减速阶段：以恒定减速度
-a减速到零，精准到达目标位置。
为什么会这样设计？因为电机有物理极限。加速度太大，电机会堵转；速度太快，机械结构受不了。梯形规划就是在这两个约束下，找到一条可行的路径。
这里有个关键点：不是所有运动都有匀速段。如果位移太短，还没加速到最大速度就得开始减速了。这时候梯形就变成了三角形。我在项目中遇到过这种情况，当时没注意，结果电机刚加速完就急刹车，末端抖得厉害。
核心公式：
- 加速段位移：
s_acc = 0.5 * v_max² / a - 减速段位移：
s_dec = 0.5 * v_max² / a - 匀速段位移：
s_const = s_total - s_acc - s_dec - 总时间：
t_total = v_max / a + s_const / v_max + v_max / a
3.2 代码实现：一个简单的梯形规划器
我习惯用C语言写底层规划，因为嵌入式里跑得快。下面这个函数，输入起始位置、目标位置、最大速度、加速度，输出每个时间点的位置和速度。
typedef struct {
float pos; // 当前位置
float vel; // 当前速度
float acc; // 当前加速度
int phase; // 0:加速 1:匀速 2:减速 3:完成
} TrapezoidState;
TrapezoidState trapezoid_plan(float start, float end, float v_max, float a, float t) {
TrapezoidState state = {0};
float s_total = end - start;
float s_acc = 0.5f * v_max * v_max / a;
float s_dec = s_acc;
// 检查是否有匀速段
if (s_acc + s_dec >= s_total) {
// 三角形规划，没有匀速段
float v_peak = sqrtf(a * s_total);
s_acc = 0.5f * v_peak * v_peak / a;
s_dec = s_acc;
float t_acc = v_peak / a;
float t_dec = t_acc;
if (t <= t_acc) {
// 加速阶段
state.pos = start + 0.5f * a * t * t;
state.vel = a * t;
state.acc = a;
state.phase = 0;
} else if (t <= t_acc + t_dec) {
// 减速阶段
float t_local = t - t_acc;
state.pos = start + s_acc + v_peak * t_local - 0.5f * a * t_local * t_local;
state.vel = v_peak - a * t_local;
state.acc = -a;
state.phase = 2;
} else {
// 完成
state.pos = end;
state.vel = 0;
state.acc = 0;
state.phase = 3;
}
} else {
// 标准梯形规划
float t_acc = v_max / a;
float t_const = (s_total - s_acc - s_dec) / v_max;
float t_dec = t_acc;
if (t <= t_acc) {
// 加速阶段
state.pos = start + 0.5f * a * t * t;
state.vel = a * t;
state.acc = a;
state.phase = 0;
} else if (t <= t_acc + t_const) {
// 匀速阶段
float t_local = t - t_acc;
state.pos = start + s_acc + v_max * t_local;
state.vel = v_max;
state.acc = 0;
state.phase = 1;
} else if (t <= t_acc + t_const + t_dec) {
// 减速阶段
float t_local = t - t_acc - t_const;
state.pos = start + s_acc + v_max * t_const + v_max * t_local - 0.5f * a * t_local * t_local;
state.vel = v_max - a * t_local;
state.acc = -a;
state.phase = 2;
} else {
// 完成
state.pos = end;
state.vel = 0;
state.acc = 0;
state.phase = 3;
}
}
return state;
}
个人经验：实际项目中，我一般会在加速和减速的衔接处加一个很小的平滑过渡。比如用S曲线去修一下拐角，虽然梯形规划本身是刚性的，但稍微处理一下，末端抖动能减少不少。
3.3 优缺点分析
梯形规划用了这么多年，自然有它的道理。但也不是万能的，我踩过不少坑。
| 优点 | 缺点 |
|---|---|
| 实现简单，计算量小，适合嵌入式实时系统 | 加速度突变，在加速-匀速、匀速-减速的切换点有冲击 |
| 参数直观，v_max和a物理意义明确，好调试 | 速度曲线不光滑，容易激发机械共振 |
| 时间最优（给定v_max和a，运动时间最短） | 能耗不是最优，急加速急减速浪费能量 |
| 容易预测轨迹，适合点位运动 | 对柔性负载不友好，末端容易振荡 |
避坑指南：我曾经在一个高精度点胶机上用梯形规划，结果胶水轨迹末端总有一滴多余的胶。查了半天，发现是减速段结束时加速度突变，导致机械臂微小过冲。后来换成S形规划才解决。所以，如果你的应用对末端精度要求极高，梯形规划要慎用。
3.4 适用场景
梯形规划不是万能的，但也不是一无是处。我个人觉得，下面这些场景用它最合适：
- 粗定位运动：比如传送带上的抓取，精度要求不高，速度越快越好。
- 低负载系统：小电机带小负载，惯性小，冲击不明显。
- 原型验证：快速搭一个demo，先跑起来再说，后面再优化。
- 计算资源受限：8位单片机、低端DSP，跑不了太复杂的算法。
你想想看，如果只是让一个风扇摇头，或者让一个抽屉推拉，梯形规划完全够用。没必要杀鸡用牛刀。
3.5 知识体系：梯形速度规划的核心逻辑
下面这张图，是我自己总结的梯形规划知识框架。从输入参数到输出轨迹，再到应用场景，一目了然。
嗯，梯形规划就讲这么多。它就像工具箱里的一把螺丝刀，简单但常用。别因为它简单就瞧不起它，很多复杂场景下，它反而是最稳的选择。