4. 抖动根源分析（算法篇）：插补算法误差、轨迹规划不连续、加减速策略缺陷、滤波滞后
各位做机器人控制的朋友，大家好。我是你们的老朋友。
上一章我们聊了机械和电气层面的抖动来源。今天咱们把目光转向算法层。说实话，算法层面的抖动往往更隐蔽，也更让人头疼。机械问题你还能用示波器、加速度计去抓，算法问题很多时候只能靠推演和代码审查。
我个人习惯把算法抖动分为四大类：插补误差、轨迹不连续、加减速策略缺陷、滤波滞后。这四兄弟经常结伴作案，但根源各不相同。咱们一个一个来拆。
核心观点：算法抖动的本质是「期望轨迹」与「实际执行轨迹」之间的偏差。这个偏差要么来自计算精度，要么来自规划逻辑，要么来自信号处理。
4.1 插补算法误差
插补，说白了就是「用离散点逼近连续曲线」。你给机器人一个圆弧指令，控制器不可能真的画一个完美的圆，它只能每隔一个插补周期算出一个点，然后让电机走过去。
误差从哪里来？两个主要来源：
- 泰勒展开截断误差：很多插补算法用泰勒级数近似曲线。阶数取低了，误差就大。我记得有一次调试六轴机器人画圆，半径10mm的圆，用一阶泰勒展开，实测轨迹偏差达到0.3mm。换成三阶展开后，偏差降到0.02mm。
- 弦高误差：用直线段逼近曲线时，直线与曲线之间的最大距离。这个误差跟插补步长直接相关。步长越大，弦高误差越大。
我的经验：插补周期固定时，弦高误差与曲率半径成反比。曲率越大（比如小半径圆弧），误差越明显。我一般会在曲率大的区域动态加密插补点，而不是全局用同一个步长。
来看一个简单的弦高误差计算示例：
// 弦高误差估算（圆弧插补）
double chordError(double radius, double stepAngle) {
// stepAngle 是相邻插补点之间的角度增量（弧度）
return radius * (1 - cos(stepAngle / 2));
}
// 实际项目中，我习惯这样用
double maxChordError = 0.001; // 允许最大1mm误差
double stepAngle = 2 * acos(1 - maxChordError / radius);
4.2 轨迹规划不连续
这个问题在「直线-圆弧」或「直线-直线」拼接处特别常见。你想想看，两段轨迹在连接点处，如果只保证了位置连续，但速度或加速度不连续，那电机就得瞬间改变速度或加速度——这不抖才怪。
我把它分成三个层级：
| 连续层级 | 数学含义 | 抖动表现 |
|---|---|---|
| C0 连续 | 位置连续 | 基本不抖，但速度突变 |
| C1 连续 | 速度连续 | 加速度突变，产生冲击 |
| C2 连续 | 加速度连续 | jerk 突变，高频抖动 |
我曾经接手过一个项目，机器人在走矩形路径时，四个角总是「咯噔」一下。查了半天，发现轨迹规划只做了 C0 连续——位置是接上了，但速度从 200mm/s 瞬间降到 0 再升到 200mm/s。后来在拐角处插入了一段过渡圆弧，保证 C2 连续，抖动立刻消失。
避坑指南：不要以为「速度连续」就够了。对于高速高精度应用，必须保证加速度连续。我曾经在 500mm/s 的送料机器人上吃过亏——C1 连续导致末端抖动 0.5mm，工件每次都放偏。
4.3 加减速策略缺陷
加减速策略，说白了就是「怎么让机器人从 A 点走到 B 点，同时不把机械结构震散」。常见的策略有梯形加减速、S 形加减速、以及更高级的 jerk 受限规划。
每种策略的抖动特性完全不同：
- 梯形加减速：加速度在起点和终点处阶跃变化。jerk 无穷大。说白了就是「急加速、急减速」。低速场景还能忍，高速场景必抖。
- S 形加减速：加速度连续变化，但 jerk 在拐点处跳变。比梯形好很多，但 jerk 跳变仍然会激发机械谐振。
- jerk 受限规划：jerk 本身也连续变化。这是目前工业机器人最常用的方案。但计算量大，对控制器性能有要求。
我个人的习惯是：能用 S 形就别用梯形，能用 jerk 受限就别用 S 形。但也要看场景。有一次做低速码垛机器人，梯形加减速完全够用，非要用 jerk 受限反而增加了计算延迟。
// 梯形 vs S形 加速度对比（伪代码）
// 梯形：加速度阶跃
if (t < t1) acc = maxAcc;
else if (t < t2) acc = 0;
else acc = -maxAcc;
// S形：加速度线性变化
if (t < t1) acc = (maxAcc / t1) * t;
else if (t < t2) acc = maxAcc;
else if (t < t3) acc = maxAcc - (maxAcc / (t3 - t2)) * (t - t2);
4.4 滤波滞后
最后聊滤波。很多工程师喜欢在位置环或速度环前面加低通滤波器，用来滤掉高频噪声。这个思路本身没错，但滤波会引入相位滞后。
相位滞后意味着什么？意味着你给控制器的指令信号，实际执行时「慢了一拍」。这一拍在低速时无所谓，但在高速高动态场景下，滞后会导致轨迹跟踪误差，进而表现为抖动。
举个例子：一阶低通滤波器，截止频率 10Hz，在 5Hz 的信号上就会产生约 45 度的相位滞后。如果这个信号是轨迹指令中的高频成分，那实际轨迹就会「跟不上」期望轨迹，形成一种类似「拖尾」的抖动。
我的建议：不要盲目加滤波。先分析抖动频率成分。如果是 50Hz 以上的高频抖动，用滤波没问题。如果是 5-20Hz 的中频抖动，滤波反而可能让问题更严重。我一般先用 FFT 分析抖动频谱，再决定滤波策略。
另外，滤波器类型也很关键。巴特沃斯滤波器通带平坦，但相位滞后大。贝塞尔滤波器相位线性，适合需要保形的场景。切比雪夫滤波器过渡带陡峭，但通带有纹波。选型时要权衡。
| 滤波器类型 | 相位滞后 | 适用场景 |
|---|---|---|
| 一阶低通 | 大（-45° @ fc） | 简单噪声抑制 |
| 巴特沃斯 | 中等 | 通用滤波 |
| 贝塞尔 | 小（线性相位） | 轨迹保形 |
| 切比雪夫 | 大（通带纹波） | 陡峭截止 |