4、低通滤波与平滑：移动平均滤波、低通滤波在轨迹平滑中的应用

轨迹规划算出来的速度曲线，直接发给电机，十有八九会抖。为什么？因为规划层算的是理想值，底层执行有延迟、有惯性、有噪声。你想想看，一个阶跃式的加速度变化，电机物理上根本追不上，追不上就会产生冲击，表现在末端就是抖动。

我最早做六轴机器人轨迹时，就吃过这个亏。规划出来的速度曲线明明很光滑，一跑起来末端抖得像筛子。后来才明白，不是规划错了，是缺了一道「滤波」工序。说白了，滤波就是给轨迹穿上一层缓冲衣，让突变变平缓，让高频噪声被滤掉。

这一章，我们就来聊两种最常用的平滑手段：移动平均滤波和一阶低通滤波。它们原理简单，但用好了，效果立竿见影。

4.1 移动平均滤波：最简单的平滑器

移动平均滤波的思路特别直观：取当前点前后 N 个点的平均值，代替当前点的值。公式长这样：

y[k] = (x[k-N] + x[k-N+1] + ... + x[k] + ... + x[k+N]) / (2N+1)

说白了，就是用局部均值去「稀释」突变。N 越大，平滑效果越强，但延迟也越大。

我在项目里常用它来做速度前馈的预处理。比如示教器录出来的轨迹，点位之间经常有毛刺，直接跑会一卡一卡的。用移动平均跑一遍，轨迹就顺了。

4.1.1 窗口大小怎么选？

窗口大小是唯一需要调的参数。我个人的经验是：

• 窗口太小（N=1~2）：基本没效果，噪声还在。
• 窗口适中（N=3~5）：适合大多数工业轨迹，能滤掉高频毛刺，延迟可接受。
• 窗口太大（N>10）：轨迹被过度平滑，拐角被「削圆」，精度下降。

4.1.2 边界处理——容易被忽略的坑

移动平均有个天然问题：轨迹开头和结尾的 N 个点，凑不齐完整的窗口。怎么办？

常见做法有三种：

方法	做法	优缺点
零填充	缺失位置补 0	简单，但边界会向下偏移
边界复制	用第一个/最后一个值填充	边界平滑，但会引入平台区
缩小窗口	边界处用更小的窗口计算	最自然，但实现稍复杂

我个人推荐第三种——缩小窗口。虽然代码多几行，但边界不会出现奇怪的偏移或平台。

def moving_average(data, window_size):
    half = window_size // 2
    smoothed = np.zeros_like(data)
    for i in range(len(data)):
        left = max(0, i - half)
        right = min(len(data), i + half + 1)
        smoothed[i] = np.mean(data[left:right])
    return smoothed

4.2 一阶低通滤波：更「软」的平滑方式

移动平均有个缺点：它对窗口内所有点一视同仁。但实际场景中，离当前点越近的数据，应该越重要。一阶低通滤波就解决了这个问题——它用指数加权的方式，让旧数据的影响逐渐衰减。

公式很简单：

y[k] = α * x[k] + (1 - α) * y[k-1]

其中 α 是滤波系数，范围 0~1。α 越大，当前值权重越高，滤波越弱；α 越小，历史值权重越高，滤波越强。

你想想看，这个公式其实就是一个「信任度分配」：你有多相信当前测量值？如果传感器噪声大，就把 α 设小一点，多相信历史趋势。

4.2.1 α 怎么选？

α 和截止频率 fc 的关系是：

α = 2π * fc * Ts / (1 + 2π * fc * Ts)

其中 Ts 是采样周期。我一般不会直接算这个公式，而是凭经验试：

• α=0.1~0.3：强滤波，适合噪声大的信号，但延迟明显。
• α=0.4~0.6：中等滤波，工业轨迹常用范围。
• α=0.7~0.9：弱滤波，只去除极高频毛刺。

4.2.2 递推实现——零开销的平滑

一阶低通最大的优势是：它只需要记住上一个输出值 y[k-1]，不需要像移动平均那样维护一个窗口数组。这意味着：

• 计算量极低：每次只需一次乘法和一次加法。
• 内存占用恒定：只存一个 float。
• 适合实时系统：可以在中断里跑，不影响主循环。

class LowPassFilter:
    def __init__(self, alpha):
        self.alpha = alpha
        self.y_prev = 0.0
        self.initialized = False

    def update(self, x):
        if not self.initialized:
            self.y_prev = x
            self.initialized = True
            return x
        y = self.alpha * x + (1 - self.alpha) * self.y_prev
        self.y_prev = y
        return y

4.3 移动平均 vs 一阶低通：怎么选？

很多初学者会纠结这个问题。我的看法是：没有绝对的好坏，只有合不合适。

对比维度	移动平均	一阶低通
计算量	O(N)，N 为窗口大小	O(1)，常数时间
内存	需要缓存 N 个点	只需 1 个状态变量
延迟	固定 N/2 个采样周期	与 α 相关，约 1/(2πfc)
平滑效果	均匀平滑，无过冲	指数平滑，有轻微滞后感
适用场景	离线批处理、后处理	实时控制、在线滤波

我个人的选择原则很简单：

• 如果是离线处理示教轨迹或仿真数据，用移动平均，效果更均匀。
• 如果是实时控制，比如在伺服周期里滤波，用一阶低通，计算快、延迟可控。

4.4 实战中的注意事项

最后分享几个我在项目里总结出来的经验：

1. 滤波不是万能药。如果轨迹本身有原理性错误（比如加速度超限），滤波只能掩盖症状，不能根治。先检查规划参数，再考虑滤波。
2. 滤波后的轨迹一定要重新检查约束。平滑后的速度峰值可能被压低，也可能在某些拐点处超出加速度限制。我习惯在滤波后加一段校验逻辑。
3. 不要对位置和速度用同一组滤波参数。位置滤波可以强一点，速度滤波要弱一点，否则速度响应会严重滞后，导致位置跟踪误差变大。
4. 多级滤波要谨慎。我曾经试过位置滤波 + 速度滤波 + 电流滤波三级串联，结果系统响应慢得像蜗牛。后来改成只对速度做一阶低通，反而效果最好。