4. 运动规划算法：T型、S型、电子凸轮与插补

运动规划，说白了就是让电机按照我们想要的轨迹跑起来。这可不是简单给个速度指令就完事了。我做了十几年运动控制，见过太多因为规划没做好，导致设备振动、加工精度差、甚至撞机的案例。这一章，我把最核心的四种算法掰开揉碎了讲。

4.1 T型速度规划

T型规划是最基础的加减速方式。名字很形象——速度曲线像个梯形：匀加速、匀速、匀减速三段。

优点很明显：算法简单，计算量小，适合低端MCU。但缺点也致命——加速度在拐点处突变，会产生冲击力。我在做一台高速点胶机时遇到过这个问题：T型规划下，胶阀在加减速切换点抖动，导致胶点大小不均匀。后来换成S型才解决。

4.2 S型速度规划

S型规划在T型基础上增加了加加速度（Jerk）控制。速度曲线变成S形，加速度连续变化，没有突变。

为什么需要加加速度？你想想看，电梯启动时如果直接给最大加速度，人会觉得被推了一下。但如果是慢慢增加加速度，人就感觉不到冲击。工业设备也是这个道理。

参数	T型	S型
加速度连续性	不连续（突变）	连续
加加速度	无穷大	有限值
计算复杂度	O(1)	O(n)
适用场景	低速、低精度	高速、高精度

S型规划有7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。嗯，听着复杂，但实际实现时可以用查表法或者分段多项式。

// S型规划核心代码片段（伪代码）
float jerk = 1000;  // 加加速度
float acc = 0;
float vel = 0;

for (t = 0; t < T_acc; t++) {
    acc += jerk * dt;
    vel += acc * dt;
    pos += vel * dt;
}

4.3 电子凸轮（Electronic CAM）

电子凸轮是用软件模拟机械凸轮的功能。主轴旋转时，从轴按照预设的曲线运动。说白了，就是建立主轴位置和从轴位置之间的映射关系。

我在做包装机项目时用过电子凸轮。传统机械凸轮磨损快、换型麻烦。电子凸轮只要改几个参数就能切换包装规格，效率提升了好几倍。

凸轮表的生成是核心。我习惯用5次多项式插值，保证位置、速度、加速度都连续。如果点数不够，运动曲线会有抖动。

4.4 插补算法

插补就是让多个轴协同运动，走出直线、圆弧或任意曲线。这是运动控制的核心技术。

4.4.1 直线插补

最简单的插补方式。给定起点和终点，各轴按比例运动。我用的是DDA（数字微分分析器）算法，计算量小，适合实时系统。

// DDA直线插补
int steps = max(abs(dx), abs(dy));
int x_acc = 0, y_acc = 0;

for (int i = 0; i < steps; i++) {
    x_acc += dx;
    y_acc += dy;
    if (x_acc >= steps) { x_step++; x_acc -= steps; }
    if (y_acc >= steps) { y_step++; y_acc -= steps; }
}

4.4.2 圆弧插补

圆弧插补比直线复杂。我常用时间分割法：把圆弧分成小段直线逼近。关键在于插补周期的选择——周期太短，计算量大；周期太长，轨迹误差大。

插补周期	轨迹误差	CPU占用
1ms	<0.01mm	高
5ms	~0.05mm	中
10ms	>0.1mm	低

4.4.3 样条插补

样条插补用于复杂曲线。B样条和NURBS是主流。我做过一个五轴激光切割项目，用NURBS插补实现了平滑的曲面切割。

样条插补的难点在于实时性。控制周期通常只有几百微秒，要在这么短的时间内完成样条求值、速度规划、位置输出，对CPU要求很高。我建议用FPGA做硬件加速，或者用预计算+查表的方式。

4.5 算法选择决策树

下面这张图是我自己总结的算法选择逻辑，帮你快速决策。

4.6 实战经验总结

最后，我把自己这些年踩过的坑和积累的经验整理成几条原则：

1. 先仿真再上机。我习惯用MATLAB先把轨迹跑一遍，确认没问题再下载到控制器。省得烧电机。
2. 预留安全余量。理论计算的最大加速度，实际要打7折。机械有间隙、有弹性，理论值往往偏大。
3. 监控实时性。插补周期抖动超过10%，轨迹就会出问题。我用示波器看DAC输出，能直观看到抖动。
4. 善用查表法。三角函数、样条求值这些计算，能查表就别实时算。我见过一个项目，样条插补周期从500μs优化到50μs，就是靠查表。