7. 速度规划与加减速：梯形加减速、S形加减速、速度曲线生成
速度规划，说白了就是让电机“怎么跑”的问题。你想想看，如果一上来就给电机发最高速的脉冲，它肯定“哐”一下冲出去，然后要么丢步，要么机械结构直接震散架。我早年做一台三轴点胶机时就吃过这个亏——电机直接啸叫，胶水甩得到处都是。从那以后，我每次做运动控制，第一件事就是先把加减速曲线画出来。
这一章，咱们就聊聊两种最经典的加减速策略：梯形和S形。我会结合我在RTOS任务里的实际写法，把速度曲线生成这件事讲透。
7.1 梯形加减速：简单粗暴，但够用
梯形加减速，名字很形象。速度曲线像个梯形——匀加速、匀速、匀减速三段。这是最基础的算法，也是我入行时第一个手写的速度规划函数。
核心思想： 加速度恒定，速度线性变化。
公式：v(t) = v₀ + a·t
其中 v₀ 是初速度，a 是加速度，t 是时间。
梯形加减速的优点是计算量极小，适合在RTOS的中断服务函数里实时计算。缺点嘛，也很明显——加速度突变。在加速开始和结束的瞬间，加速度从0跳变到a，或者从a跳变到0，这会导致机械冲击。我做过一个测试，用梯形加减速驱动一个500g的负载，在高速启停时，平台末端会有肉眼可见的抖动。
不过，对于大多数对启停平稳性要求不高的场景，比如传送带、简单的XY平台，梯形加减速完全够用。我个人的习惯是：如果负载惯量比小于5，我就用梯形；超过5，我会考虑S形。
7.1.1 梯形加减速的RTOS任务实现
在RTOS里，我通常把速度规划放在一个周期任务里，比如每1ms执行一次。任务里维护一个状态机：加速、匀速、减速、停止。
// 梯形加减速状态机示例
typedef enum {
ACCEL,
CRUISE,
DECEL,
STOP
} MotionState;
void SpeedProfileTask(void *param) {
MotionState state = ACCEL;
float current_speed = 0.0f;
float target_speed = 1000.0f; // 目标速度，单位：脉冲/秒
float accel = 500.0f; // 加速度
float decel = 500.0f; // 减速度
float total_distance = 5000; // 总距离，单位：脉冲
float accel_distance, decel_distance, cruise_distance;
// 预先计算三段距离
accel_distance = (target_speed * target_speed) / (2 * accel);
decel_distance = (target_speed * target_speed) / (2 * decel);
cruise_distance = total_distance - accel_distance - decel_distance;
float traveled = 0.0f;
while (1) {
switch (state) {
case ACCEL:
current_speed += accel * 0.001f; // 每1ms加速
if (current_speed >= target_speed) {
current_speed = target_speed;
state = CRUISE;
}
break;
case CRUISE:
// 保持匀速
if (traveled >= accel_distance + cruise_distance) {
state = DECEL;
}
break;
case DECEL:
current_speed -= decel * 0.001f;
if (current_speed <= 0.0f) {
current_speed = 0.0f;
state = STOP;
}
break;
case STOP:
// 停止，等待下一个指令
break;
}
// 根据current_speed计算脉冲间隔，发送给电机
traveled += current_speed * 0.001f;
vTaskDelay(pdMS_TO_TICKS(1));
}
}
我的经验： 梯形加减速的加速度值不要设得太激进。我曾经为了赶节拍，把加速度设到电机额定值的90%，结果跑了半小时电机就过热保护了。建议留20%的余量。
7.2 S形加减速：丝滑，但计算量大
S形加减速，顾名思义，速度曲线像个拉长的S。它的核心是让加速度也平滑变化，而不是突变。说白了，就是加加速度（Jerk）是有限的。
为什么要用S形？我举个例子。有一次我做一台高精度点胶机，要求点胶头在转弯时不能有丝毫抖动，否则胶水会拉丝。梯形加减速根本不行，一减速一加速，胶水就断断续续。换成S形加减速后，问题立刻解决。嗯，这里要注意：S形加减速不是万能的，它主要解决的是“加速度突变”带来的冲击问题。
7.2.1 S形加减速的数学原理
S形加减速通常用三段式或七段式。七段式最常用：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。你看，光是名字就比梯形复杂多了。
| 阶段 | 加速度变化 | 速度变化 |
|---|---|---|
| 加加速 | 从0增加到最大加速度 | 缓慢增加 |
| 匀加速 | 保持最大加速度 | 线性增加 |
| 减加速 | 从最大加速度减到0 | 缓慢增加至匀速 |
| 匀速 | 0 | 恒定 |
| 加减速 | 从0增加到最大减速度 | 缓慢减少 |
| 匀减速 | 保持最大减速度 | 线性减少 |
| 减减速 | 从最大减速度减到0 | 缓慢减少至0 |
公式上，S形加减速需要引入加加速度J（Jerk）。速度是加速度的积分，加速度是加加速度的积分。所以：
v(t) = v₀ + ∫a(t)dt = v₀ + ∫(∫J dt)dt
看着有点绕？没关系，实际写代码时，我们通常用查表法或者分段多项式来算，不会真的去解积分方程。
7.2.2 S形加减速的RTOS实现
在RTOS里实现S形加减速，我建议用查表法。预先算好一个周期的速度曲线表，运行时直接查表插值。这样既保证了实时性，又避免了在中断里做浮点运算。
// S形加减速查表法示例
#define TABLE_SIZE 1000
float speed_table[TABLE_SIZE]; // 预先计算好的速度曲线表
void InitSShapeTable(float max_speed, float accel, float jerk) {
// 这里用简化的S形曲线：正弦型S形
for (int i = 0; i < TABLE_SIZE; i++) {
float t = (float)i / TABLE_SIZE;
// 使用正弦函数模拟S形
speed_table[i] = max_speed * (0.5f - 0.5f * cosf(M_PI * t));
}
}
float GetSShapeSpeed(float progress) {
// progress: 0.0 ~ 1.0
int index = (int)(progress * (TABLE_SIZE - 1));
return speed_table[index];
}
注意： 查表法虽然快，但精度受限于表的大小。我建议表的大小至少1024点，否则在低速段会出现明显的阶梯感。另外，如果总距离很短，可能还没加速到最高速就要减速了，这时候需要做“三角型”S形处理——也就是没有匀速段。
7.3 速度曲线生成：从理论到代码
速度曲线生成，说白了就是把上面讲的加减速算法，变成一个可以在RTOS里跑起来的“速度发生器”。我个人习惯把它封装成一个模块，对外只提供三个接口：初始化、设置参数、获取当前速度。
为什么要封装？因为在实际项目中，速度规划往往不是孤立的。它要和位置环、电流环配合。我见过很多新手把速度规划代码和电机驱动代码混在一起写，最后改一个参数要改三个地方，非常痛苦。
7.3.1 速度曲线生成的通用流程
- 参数计算阶段： 根据目标速度、加速度、加加速度、总距离，计算出各段的长度和时间。
- 实时查表阶段： 在RTOS周期任务中，根据当前已走距离或时间，查表得到当前速度。
- 速度输出阶段： 将速度值转换为脉冲频率或模拟电压，发送给电机驱动器。
这里有一个坑，我曾经踩过：在参数计算阶段，一定要判断“能不能跑到目标速度”。如果总距离太短，可能刚加速就要减速，这时候要自动降速。我管这个叫“三角型速度规划”。
避坑指南： 我曾经在一个项目中，没有做距离判断，直接按梯形加减速算。结果总距离只有100个脉冲，目标速度设了1000脉冲/秒。加速到一半发现距离不够了，电机直接冲过终点，造成了机械碰撞。从那以后，我每次做速度规划，第一件事就是算“最短加速距离”。
7.3.2 梯形 vs S形：怎么选？
我个人的选择标准是这样的：
- 梯形加减速： 适合启停不频繁、对冲击不敏感的场景。比如传送带、风扇、水泵。计算量小，代码简单。
- S形加减速： 适合高精度、高平稳性要求的场景。比如数控机床、机器人、点胶机。虽然计算量大，但效果确实好。
你想想看，如果做一台玩具小车，梯形加减速完全够用。但如果做一台医疗注射泵，你敢用梯形吗？针头一抖，病人就遭罪了。
7.4 知识体系与核心逻辑
下面这张SVG图，是我自己总结的速度规划知识体系。你看一眼就能明白整个逻辑链条。
这张图把梯形和S形的核心特点、优缺点、以及RTOS实现方式都串起来了。你写代码前，先对着这张图理清思路，会少走很多弯路。
我的建议： 刚开始做速度规划时，先用梯形加减速把整个流程跑通。等你对RTOS任务调度、脉冲生成这些底层机制熟悉了，再换成S形。一口吃不成胖子，运动控制尤其如此。