第1章：运动控制核心算法——轨迹规划与插补

各位同学，大家好。我是你们这门课的老朋友。今天咱们聊点硬核的——轨迹规划和插补算法。

说实话，我刚入行那会儿，觉得运动控制嘛，不就是让电机转起来？后来被现实狠狠教育了一顿。你想想看，一个机械臂从A点到B点，如果直接硬冲过去，那震动、冲击、过冲……设备没几天就废了。所以，轨迹规划和插补，说白了就是解决两个问题：怎么走和走多准。

1.1 轨迹规划：让运动“优雅”起来

轨迹规划的核心，是生成一条时间-位置曲线。这条曲线要满足：速度连续、加速度可控、不超出电机能力。

我个人习惯把轨迹规划分成三类：梯形、S形、多项式。咱们一个一个说。

1.1.1 梯形速度规划

梯形规划是最简单的。速度曲线像个梯形——匀加速、匀速、匀减速。代码实现也直接：

// 梯形速度规划核心参数
float v_max = 100;   // 最大速度
float a_max = 500;   // 最大加速度
float total_dist = 1000; // 总位移

// 计算加速段距离
float acc_dist = v_max * v_max / (2 * a_max);
// 如果总距离不够加速到最大速度，需要三角型规划
if (total_dist < 2 * acc_dist) {
    // 三角型规划：加速到某速度后立即减速
    v_max = sqrt(a_max * total_dist);
    acc_dist = total_dist / 2;
}

嗯，这里要注意：梯形规划的加速度在拐点处突变。我在项目中遇到过，用梯形规划驱动一个重型龙门架，每次加减速切换时，整个架子都在抖。后来不得不换成S形。

1.1.2 S形速度规划

S形规划比梯形多了加加速度（Jerk）约束。速度曲线不再是直线，而是平滑的S形。说白了，就是让加速度也连续变化。

S形规划有7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但核心公式就一个：

// S形规划中的加加速度段位置计算
float jerk = 2000;   // 加加速度
float t = 0.1;       // 当前时间
float pos = (1.0/6.0) * jerk * t * t * t;  // 加加速段位置

你想想看，为什么工业机器人几乎都用S形？因为冲击小、寿命长。我调试过一台六轴机器人，用S形规划后，末端抖动从±0.5mm降到了±0.05mm。

1.1.3 多项式轨迹规划

当你要让运动轨迹通过多个指定点，并且起点和终点的速度、加速度都约束时，多项式规划就派上用场了。

最常用的是五次多项式：

// 五次多项式：位置 = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
// 已知起点位置、速度、加速度，终点位置、速度、加速度
// 解6个方程求6个系数
float a0 = q0;                    // 起点位置
float a1 = v0;                    // 起点速度
float a2 = 0.5 * acc0;            // 起点加速度
// ... 后面三个系数通过终点约束求解

说实话，多项式规划在数控加工中特别常见。我做过一个五轴联动项目，刀尖轨迹必须平滑通过几十个点，用五次多项式拟合后，加工表面粗糙度从Ra3.2降到了Ra0.8。

1.2 插补算法：让轨迹“精准”起来

轨迹规划算出了时间-位置曲线，但实际运动是离散的——每个控制周期（比如1ms）要给电机发一个位置指令。插补就是干这个的：在规划好的轨迹上，等间距地取点。

1.2.1 直线插补

直线插补最简单。从起点到终点，按时间比例线性分配位置：

// 直线插补：每个周期计算当前位置
float t_ratio = current_time / total_time;  // 0~1
float pos_x = start_x + (end_x - start_x) * t_ratio;
float pos_y = start_y + (end_y - start_y) * t_ratio;

但要注意：直线插补的速度控制要和轨迹规划配合。如果直接用时间比例，速度会不均匀。我一般会先算好速度曲线，再反推时间。

1.2.2 圆弧插补

圆弧插补稍微复杂点。核心是圆心角的等分：

// 圆弧插补：按角度增量计算
float angle_step = total_angle / total_steps;
float current_angle = start_angle + step * angle_step;
float pos_x = center_x + radius * cos(current_angle);
float pos_y = center_y + radius * sin(current_angle);

我在项目中遇到过一个问题：圆弧插补时，如果半径很小，角度步长太大，轨迹会变成多边形。后来我改用弦高误差控制——根据半径动态调整步长。

1.2.3 样条插补

样条插补是高级玩法。当轨迹是任意曲线时，用三次样条或B样条拟合。每个插补周期，根据参数t计算曲线上的点：

// 三次样条插补：给定4个控制点P0,P1,P2,P3
// 参数t从0到1
float t2 = t * t;
float t3 = t2 * t;
float pos_x = 0.5 * ( (2*P1_x) + 
    (-P0_x + P2_x)*t + 
    (2*P0_x - 5*P1_x + 4*P2_x - P3_x)*t2 + 
    (-P0_x + 3*P1_x - 3*P2_x + P3_x)*t3 );

说实话，样条插补的难点不在算法，而在实时性。我做过一个高速激光切割项目，控制周期只有200μs，样条计算必须优化到微秒级。最后用了查表法——提前算好系数，运行时只做乘加。

1.3 知识体系总览

为了让大家有个整体印象，我画了张图。这张图把轨迹规划和插补算法的关系理清楚了：

1.4 实际项目中的选择策略

讲了这么多，到底怎么选？我给大家一个参考表：

应用场景	推荐轨迹规划	推荐插补算法	关键考量
点位运动（如点胶机）	S形	直线插补	减少冲击，防止滴胶
数控加工（如雕刻机）	多项式	样条插补	表面光洁度
机器人焊接	S形	圆弧插补	焊缝均匀性
高速拾放	梯形（轻载）	直线插补	节拍时间

好了，这一章的内容就到这儿。轨迹规划和插补是运动控制的灵魂，后面所有章节都建立在这个基础上。大家回去把代码敲一遍，尤其是S形规划的7段逻辑，自己推导一遍，印象会深很多。

有什么问题，咱们下节课再聊。

---