3、运动控制核心算法：梯形加减速、S形加减速与速度规划
各位好，我是老周。今天咱们聊点硬核的——运动控制卡里最核心的那几段算法。说白了，就是怎么让电机听话地跑起来，又稳又准。
你想想看，如果让电机直接以最高速度启动，那冲击力有多大？轻则丢步，重则把机械结构震散架。所以，加减速算法就是给电机一个“缓冲”，让它平滑地加速、匀速、减速。我入行那会儿，第一个项目就是调这个，印象太深了。
3.1 梯形加减速算法
梯形加减速，名字很形象。速度曲线像个梯形——先匀加速，再匀速，最后匀减速。这是最基础、最常用的算法。
核心思想：加速度恒定，速度线性变化。计算量小，实现简单。
3.1.1 算法原理
假设我们要从起点走到终点，总距离为 S，最大速度为 Vmax，加速度为 A，减速度也为 A（对称情况）。
整个过程分三段：
- 加速段：速度从 0 线性增加到 Vmax，时间 t1 = Vmax / A，距离 S1 = 0.5 * A * t1²
- 匀速段：速度保持 Vmax，时间 t2 = (S - S1 - S3) / Vmax，距离 S2 = Vmax * t2
- 减速段：速度从 Vmax 线性减到 0，时间 t3 = Vmax / A，距离 S3 = 0.5 * A * t3²
这里有个坑：如果总距离 S 太小，可能还没加速到 Vmax 就得开始减速了。这种情况叫“三角波”或“未达到最大速度”。
我曾经踩过的坑：有一次做点胶机，行程只有 5mm，我按梯形算，结果加速段还没跑完就开始减速了，实际速度曲线是个三角形。后来我加了个判断：如果 S1 + S3 > S，就按三角波重新计算最大速度。
3.1.2 梯形算法的优缺点
| 优点 | 缺点 |
|---|---|
| 计算量极小，适合低端 MCU | 加速度突变，存在柔性冲击 |
| 实现简单，调试方便 | 启停瞬间有抖动，不适合精密定位 |
| 速度规划直观，容易理解 | 高速时容易过冲 |
我个人习惯：在要求不高的场合，比如传送带、简单搬运，梯形算法完全够用。但如果你做的是高精度设备，比如数控机床、芯片封装，那就要上 S 形了。
3.2 S形加减速算法
S 形加减速，说白了就是让加速度也平滑变化。速度曲线像个 S 形，没有突变。这样冲击力小，定位更准。
我的经验：S 形算法虽然复杂，但效果立竿见影。有一次做激光切割机，用梯形算法时，拐角处总有振纹。换成 S 形后，振纹消失了，切割面光滑得像镜子。
3.2.1 算法原理
S 形加减速通常用 7 段式：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着绕口？其实就三个字——加加速度（Jerk）可控。
加加速度 Jerk 是加速度的变化率。Jerk 越小，运动越平滑。
7 段式的速度曲线如下：
- 加加速段：加速度从 0 线性增加到 Amax，速度呈二次曲线上升
- 匀加速段：加速度保持 Amax，速度线性上升
- 减加速段：加速度从 Amax 线性减到 0，速度呈二次曲线上升
- 匀速段：速度保持 Vmax
- 加减速段：加速度从 0 线性减到 -Amax，速度呈二次曲线下降
- 匀减速段：加速度保持 -Amax，速度线性下降
- 减减速段：加速度从 -Amax 线性增到 0，速度呈二次曲线下降
嗯，这里要注意：不是所有运动都需要 7 段。如果距离短，可能只有 4 段甚至 3 段。算法里要动态判断。
3.2.2 梯形 vs S形 对比
| 对比项 | 梯形加减速 | S形加减速 |
|---|---|---|
| 加速度变化 | 突变（阶跃） | 连续变化 |
| 加加速度 Jerk | 无穷大 | 有限值（可设定） |
| 计算复杂度 | O(1) | O(n) 或更高 |
| 定位精度 | 一般 | 高 |
| 适用场景 | 低精度、低成本 | 高精度、高速 |
3.3 速度规划与插补原理
速度规划，就是根据运动轨迹，算出每个时刻该跑多快。插补，就是把连续轨迹拆成一个个离散的脉冲或位置点。
这两者密不可分。你想想看，如果只规划速度，不插补，电机怎么知道具体往哪走？反过来，只插补不规划速度，那运动就是生硬的。
3.3.1 速度规划流程
我一般按这个步骤来：
- 输入参数：起点、终点、最大速度、加速度、加加速度（S形时）
- 判断运动类型：梯形还是 S 形？是否达到最大速度？
- 计算关键时间点：加速结束时间、匀速开始/结束时间、减速开始时间
- 生成速度曲线：按时间分段，计算每个时刻的目标速度
- 输出给插补器：把速度曲线转成位置指令
避坑指南：我曾经在速度规划时忽略了“加减速距离”的计算。结果电机跑到终点前还没减速，直接冲过去了。后来我加了个“预减速”判断：当剩余距离小于减速所需距离时，立即进入减速段。
3.3.2 插补原理
插补分两种：
- 直线插补：两点之间走直线，各轴按比例分配速度
- 圆弧插补：走圆弧，需要实时计算圆心角、半径
插补的核心是“时间分割法”。把运动时间分成一个个小周期（比如 1ms），每个周期算一次位置增量。周期越短，轨迹越平滑，但对 CPU 要求越高。
举个例子，直线插补：
// 伪代码：直线插补
dx = x_end - x_start
dy = y_end - y_start
total_steps = max(|dx|, |dy|) // 步数
x_inc = dx / total_steps
y_inc = dy / total_steps
for i = 0 to total_steps:
x_current = x_start + i * x_inc
y_current = y_start + i * y_inc
output_pulse(x_current, y_current)
delay(周期时间)
你看，代码很简单。但实际工程里要考虑很多：脉冲频率上限、加减速耦合、多轴同步……
3.4 核心算法关系图
下面这张图，是我自己总结的。它把梯形、S 形、速度规划、插补之间的关系串起来了。你一看就明白。
3.5 实际工程中的选择建议
说了这么多，到底怎么选？我个人的经验是：
- 梯形算法：适合步进电机、低负载、低成本场景。比如 3D 打印机、小型传送带。
- S形算法：适合伺服电机、高负载、高精度场景。比如数控机床、机器人关节。
- 速度规划：一定要考虑“加减速距离”和“剩余距离”的动态关系，否则会撞限位。
- 插补：多轴运动时，各轴必须同步。我习惯用“主从同步”方式，以行程最长的轴为主轴。
一个小技巧：如果你刚开始调运动控制，先用梯形算法跑通流程，再换成 S 形优化。这样出问题容易定位。我当年就是太自信，直接上 S 形，结果折腾了两天才发现是参数设错了。
好了，这一章的内容就到这儿。梯形和 S 形是基础，速度规划和插补是灵魂。把这些吃透了，后面讲多轴联动、前瞻规划就轻松多了。