滤波器：低通、高通、陷波，以及它们在回路里的代价

1. 滤波器就是一个传递函数

滤波器是一个传递函数 \(H(s)\)，输出的频谱等于输入的频谱乘以 \(H(j\omega)\)（基础3 第4节）。看一个滤波器，就是看它的波特图，只是工程上横轴习惯用频率 \(f\)（Hz），\(\omega=2\pi f\)。几个术语：

• 通带：增益接近 1（0 dB）、信号基本原样通过的频段；阻带：增益很小、信号被压下去的频段；
• 截止频率 \(f_c\)：增益降到 −3 dB（0.707 倍）的频率，是通带和阻带的分界；
• 滚降：阻带里频率每升高 10 倍，增益下降多少 dB。一阶 −20 dB/dec，二阶 −40 dB/dec。

2. 一阶低通与一阶高通

一阶低通

它就是第2章的惯性环节、第5章波特图里的 \(\frac{1}{Ts+1}\)，也是 RC 电路：\(\tau=RC\)。例如 \(R=10\) kΩ、\(C=100\) nF，\(\tau=1\) ms，\(f_c=159\) Hz（RC 电路的传递函数见第2章 2.3 节）。

\(f_c=100\) Hz（\(\tau=1.59\) ms）时：

一阶高通

直流增益为 0，高频增益为 1；\(f_c\) 处 −3 dB、相位 +45°（超前）。\(f_c=100\) Hz 时，10 Hz 处 −20.04 dB、+84.3°，1000 Hz 处 −0.04 dB、+5.7°，正好和低通对称。

用途：去掉直流偏置和缓慢的漂移（交流耦合），只留下变化的部分。比如只关心振动、不关心静态位置的时候。

低通 + 高通 = 1：\(\dfrac{1}{\tau s+1}+\dfrac{\tau s}{\tau s+1}=1\)。同一个 \(\tau\) 的低通和高通，把信号分成「慢的部分」和「快的部分」，加起来正好是原信号。互补滤波（第11章 11.8 节）就是这个原理：加速度计算出的角度准但噪声大，只取它的低频部分；陀螺仪积分出的角度平滑但会漂，只取它的高频部分；两者相加，各取所长。

拖一拖截止频率，用「看频率 f」读出任意频率的增益、相位和等效延迟：

3. 二阶低通

就是第3章的标准二阶系统（振荡环节）。高频以 −40 dB/dec 下降，\(f_c\) 处相位 −90°。阻尼比 \(\zeta\) 决定 \(f_c\) 附近的形状（\(f_c=100\) Hz）：

• \(\zeta=0.707\) 叫巴特沃斯滤波器：通带最平坦，没有峰，\(f_c\) 正好是 −3 dB 点，最常用；
• \(\zeta\) 太小会在 \(f_c\) 附近放大信号，反而把那里的噪声变大了；
• 和一阶相比：同样的 \(f_c\)，1000 Hz 处多衰减了 20 dB（10 倍）。代价是延迟大一些：在低频处，二阶低通相当于延迟 \(2\zeta/\omega_n\)，巴特沃斯 100 Hz 是 2.25 ms，一阶 100 Hz 是 1.59 ms。

两个一阶 RC 电路直接串起来，并不是 \(\zeta=1\) 的二阶低通，因为后一级会从前一级取电流（第2章的负载效应）。中间加一个运放跟随器隔开才是。

4. 陷波滤波器与带通

陷波

只压掉某一个频率附近的一小段，其他频率基本不动：

• 离 \(f_0\) 远的地方，分子分母差不多，增益约为 1；
• 在 \(f_0\) 处，\(s=j\omega_0\) 让 \(s^2+\omega_0^2=0\)，只剩一次项：\(H(j\omega_0)=\dfrac{\zeta_z}{\zeta_p}\)，这就是深度。\(\zeta_z=0.1\zeta_p\) 是 −20 dB，\(\zeta_z=0\) 是无穷深；
• 宽度由 \(\zeta_p\) 决定：−3 dB 带宽约为 \(2\zeta_pf_0\)。

例　\(f_0=300\) Hz，\(\zeta_p=0.3\)，\(\zeta_z=0.03\)：300 Hz 处 −20 dB，−3 dB 带宽 224～402 Hz，宽 178 Hz（估算 \(2\times0.3\times300=180\) Hz）。

陷波在 \(f_0\) 以下带来相位滞后，以上带来相位超前。越宽，离 \(f_0\) 很远的地方也越受影响：

带通

只让 \(f_0\) 附近的频率通过：中心处 0 dB、0°，−3 dB 带宽 \(2\zeta f_0\)。理想陷波（\(\zeta_z=0\)）加上同样宽度的带通正好等于 1，两者是互补的。用途：从信号里挑出某一个频率，比如只看共振频率附近的振动，或者提取和转速同步的某次谐波。

5. 滤波器放进回路：相位的代价

来看一个电机速度环。PI 控制器的零点抵消掉电机的极点以后，开环传递函数是 \(L(s)=\dfrac{200}{s}\)（第13章案例一的设计）：截止频率 \(\omega_c=200\) rad/s（31.8 Hz），相位裕度 90°。编码器差分得到的速度很「毛糙」，想在速度反馈上加一个一阶低通。截止频率选多少？

两个结论：

• 滤波器离回路截止频率越近，代价越大。估算：相位损失 ≈ \(\arctan\dfrac{\text{回路截止频率}}{\text{滤波器截止频率}}\)。100 Hz 的低通在 31.8 Hz 处滞后 \(\arctan(31.8/100)=17.7^\circ\)，实际裕度从 90° 降到 73.1°，掉了 16.9°（截止频率也略微变小了，所以比估算少一点）；
• 看滤波后的信号会低估超调。\(f_c=30\) Hz 时，控制器看到的超调是 17.5%，实际转速冲了 32.5%：滤波器把超调也「滤」掉了一半。调试时要看未滤波的信号。

在 里勾选不同的滤波器、拖动截止频率，看绿线（开环）的相位裕度怎么变：

用 python-control 验证本节开头那张表（测速低通的 \(f_c\) 分别取 300、100、50、30 Hz）：

import numpy as np
import control as ct

s = ct.tf("s")
L0 = 200 / s                                   # 速度环的开环（PI 零极点对消后，见第13章案例一）
for fc in (300, 100, 50, 30):
    F = 1 / (s / (2 * np.pi * fc) + 1)         # 测速一阶低通，截止频率 fc（Hz）
    gm, pm, wg, wc = ct.margin(L0 * F)
    print(f"fc = {fc:3d} Hz：ωc = {wc:.0f} rad/s，相位裕度 {pm:.1f}°")
# fc = 300 Hz：ωc = 199 rad/s，相位裕度 84.0°
# fc = 100 Hz：ωc = 191 rad/s，相位裕度 73.1°
# fc =  50 Hz：ωc = 175 rad/s，相位裕度 60.9°
# fc =  30 Hz：ωc = 155 rad/s，相位裕度 50.6°

6. 用陷波压住机械共振

电机通过联轴器、皮带或丝杠带动负载，传动部件有弹性，会在某个频率上共振。简化模型：开环再乘上一个共振环节

机械共振的阻尼通常很小，共振峰高 \(1/(2\zeta_r)\approx16.7\) 倍（+24.4 dB）。速度环在 300 Hz 处本来只有 \(200/1885=0.106\)（−19.5 dB），叠上共振峰后变成 1.77 倍（约 +5 dB），而这里的相位正好是 −180°：幅值裕度 −5 dB，闭环不稳定。实际表现是电机在接近 300 Hz 的频率上尖叫、振动。

两种办法：

1. 降低增益：把 200 降到 56，共振峰压到 0 dB 以下 6 dB，稳定了，但回路慢了约 3.6 倍；
2. 加陷波：中心 300 Hz、\(\zeta_p=0.3\)、深 −20 dB。幅值裕度变成 15.0 dB，而相位裕度只从 90° 降到 86.3°（陷波在 31.8 Hz 处只带来 3.3° 滞后），回路速度不变。

但共振频率不是固定的：负载变了、丝杠走到不同位置、温度变了，它都会漂。用宽、窄两种陷波试一下：

宽度要盖住共振频率可能漂移的范围，这比相位上省的那几度重要得多。在下面的 里拖动「共振频率」，看陷波在什么时候压不住；取消勾选「开环里加入这个滤波器」，看原来的共振峰：

def resonance(fr, zr=0.03):
    wr = 2 * np.pi * fr
    return wr**2 / (s**2 + 2 * zr * wr * s + wr**2)


def notch(f0, zp, depth):
    w0 = 2 * np.pi * f0
    return (s**2 + 2 * zp * depth * w0 * s + w0**2) / (s**2 + 2 * zp * w0 * s + w0**2)


cases = [("没有陷波", L0 * resonance(300)),
         ("陷波 300 Hz", L0 * resonance(300) * notch(300, 0.3, 0.1)),
         ("共振漂到 330 Hz", L0 * resonance(330) * notch(300, 0.3, 0.1)),
         ("窄陷波，共振 330 Hz", L0 * resonance(330) * notch(300, 0.1, 0.1)),
         ("不用陷波，增益降到 56", 56 / s * resonance(300))]
for name, L in cases:
    gm, pm, wg, wc = ct.margin(L)
    ok = all(p.real < 0 for p in ct.poles(ct.feedback(L, 1)))
    print(f"{name}：幅值裕度 {20 * np.log10(gm):.1f} dB，{'稳定' if ok else '不稳定'}")
# 没有陷波：幅值裕度 -5.0 dB，不稳定
# 陷波 300 Hz：幅值裕度 15.0 dB，稳定
# 共振漂到 330 Hz：幅值裕度 8.5 dB，稳定
# 窄陷波，共振 330 Hz：幅值裕度 0.4 dB，稳定
# 不用陷波，增益降到 56：幅值裕度 6.1 dB，稳定

这里的共振模型是最简化的一种，真实的传动系统往往还有一个「反共振」（幅频曲线先凹下去再凸起来），也可能有好几个共振。但处理思路一样：找到峰、压下去、留够裕度。

7. 滑动平均：最简单的 FIR 滤波器

取最近 \(N\) 个采样的平均值：

它只用输入、不用过去的输出，这类滤波器叫 FIR（有限脉冲响应）；第 2～6 节那些用到过去输出的（一阶、二阶低通，陷波，带通）叫 IIR。滑动平均有两个特点：

• 在 \(f_s/N\) 的整数倍上增益正好为 0。\(f_s=1\) kHz、\(N=20\)（平均 20 ms）时，50 Hz、100 Hz、150 Hz……全部为 0，正好把 50 Hz 工频和它的谐波一起去掉。对一个 PWM 周期取平均，也能把 PWM 纹波完全去掉；
• 相位是线性的，相当于一个固定的延迟 \((N-1)/2\) 个采样，上一条里 \(f_s=1\) kHz、\(N=20\) 的例子就是 9.5 ms。这个延迟很大：

把它放进第 5 节那个截止频率 31.8 Hz 的速度环 \(L(s)=200/s\)，那里一下子多出 109° 的滞后：相位裕度从 90° 掉到 13.5°，幅值裕度只剩 2.7 dB，几乎要振荡。滑动平均适合显示、记录和慢速的监测量（温度、电压的显示值），以及要去掉某个已知频率的场合，不适合直接放进快速回路。

/* 滑动平均：整数累加，不会有浮点误差的积累 */
#define MA_N 20
typedef struct { int32_t buf[MA_N]; int32_t sum; int idx; } MovAvg;

int32_t ma_update(MovAvg *m, int32_t x)
{
    m->sum += x - m->buf[m->idx];
    m->buf[m->idx] = x;
    m->idx = (m->idx + 1) % MA_N;
    return m->sum / MA_N;
}

用累加和的写法，每个采样只要一次加减法。累加和如果用 float，舍入误差会慢慢积累，所以 ADC 读数这类整数最好用整数累加。结构体使用前要清零。

8. 在单片机里实现

一阶低通

一行代码：\(y(k)=y(k-1)+a\,[x(k)-y(k-1)]\)。系数 \(a\) 有两种常见算法（\(f_c=100\) Hz）：

\(f_c\) 远小于 \(f_s\)（比如 \(f_c<f_s/20\)）时两种都行，\(a\approx2\pi f_c/f_s\)；\(f_c\) 接近 \(f_s/10\) 时，后向欧拉的截止频率偏低很多，用极点匹配的公式。

一阶高通

后向欧拉离散化：\(y(k)=a\,[y(k-1)+x(k)-x(k-1)]\)，\(a=\dfrac{\tau}{\tau+T_s}\)。

二阶节（biquad）

所有二阶的滤波器（二阶低通、带通、陷波……）离散化以后都是同一个形式：

这叫二阶节（biquad）。更高阶的滤波器一般拆成几个二阶节串联，数值上更稳。系数用双线性变换（Tustin）算：把 \(s=K\dfrac{1-z^{-1}}{1+z^{-1}}\) 代进连续的传递函数。

预畸变：双线性变换会把频率轴「压缩」，连续滤波器在 \(f_0\) 的特征，离散化以后跑到

永远比 \(f_0\) 低。300 Hz 的陷波，不预畸变时的实际位置：

办法是取 \(K=\dfrac{\omega_0}{\tan(\omega_0T_s/2)}\)，而不是 \(\dfrac{2}{T_s}\)，让 \(f_0\) 这一点准确地落在 \(f_0\) 上，这叫预畸变。陷波的中心频率最要紧，一定要预畸变。记 \(c=\dfrac{1}{\tan(\pi f_0/f_s)}\)，代进去整理得到：

勾选「叠加离散实现」，对比预畸变前后（\(f_s=1\) kHz）：

C 代码（转置直接 II 型，每个采样 5 次乘法）：

#include <math.h>
#include <stdint.h>

#define PI 3.14159265f

/* 一阶低通：y += a·(x − y) */
typedef struct { float a, y; } LowPass1;

void lp1_init(LowPass1 *f, float fs, float fc)
{
    f->a = 1.0f - expf(-2.0f * PI * fc / fs);   /* 极点和连续滤波器完全对应 */
    f->y = 0.0f;
}

float lp1_update(LowPass1 *f, float x)
{
    f->y += f->a * (x - f->y);
    return f->y;
}

/* 二阶节（biquad），转置直接 II 型：每个采样 5 次乘法，2 个状态 */
typedef struct { float b0, b1, b2, a1, a2, s1, s2; } Biquad;

float biquad_update(Biquad *f, float x)
{
    float y = f->b0 * x + f->s1;
    f->s1 = f->b1 * x - f->a1 * y + f->s2;
    f->s2 = f->b2 * x - f->a2 * y;
    return y;
}

/* 二阶低通：截止频率 fc，阻尼比 zeta（0.707 为巴特沃斯） */
void lp2_init(Biquad *f, float fs, float fc, float zeta)
{
    float c = 1.0f / tanf(PI * fc / fs);       /* 双线性变换，在 fc 处预畸变 */
    float a0 = c * c + 2.0f * zeta * c + 1.0f;
    f->b0 = 1.0f / a0;
    f->b1 = 2.0f / a0;
    f->b2 = 1.0f / a0;
    f->a1 = 2.0f * (1.0f - c * c) / a0;
    f->a2 = (c * c - 2.0f * zeta * c + 1.0f) / a0;
    f->s1 = f->s2 = 0.0f;
}

/* 陷波：中心频率 f0，宽度 zp（−3 dB 带宽约 2·zp·f0），depth 是中心处的增益（0.1 即 −20 dB） */
void notch_init(Biquad *f, float fs, float f0, float zp, float depth)
{
    float c = 1.0f / tanf(PI * f0 / fs);       /* 双线性变换，在 f0 处预畸变 */
    float zz = zp * depth;
    float a0 = c * c + 2.0f * zp * c + 1.0f;
    f->b0 = (c * c + 2.0f * zz * c + 1.0f) / a0;
    f->b1 = 2.0f * (1.0f - c * c) / a0;
    f->b2 = (c * c - 2.0f * zz * c + 1.0f) / a0;
    f->a1 = f->b1;
    f->a2 = (c * c - 2.0f * zp * c + 1.0f) / a0;
    f->s1 = f->s2 = 0.0f;
}

用法：初始化时调用一次 notch_init(&nf, 4000, 300, 0.3f, 0.1f)，之后每个采样调用 y = biquad_update(&nf, x)。这段代码编译运行过，和 scipy 的结果相比误差在 \(10^{-6}\) 量级（float 的精度）；300 Hz 的正弦进去，出来只剩 0.1 倍。

在电脑上先算出系数、检查频率特性，再搬到单片机上：

from scipy import signal


def notch_coef(fs, f0, zp, depth):
    c = 1 / np.tan(np.pi * f0 / fs)            # 双线性变换，在 f0 处预畸变
    zz = zp * depth
    a0 = c * c + 2 * zp * c + 1
    b = np.array([c * c + 2 * zz * c + 1, 2 * (1 - c * c), c * c - 2 * zz * c + 1]) / a0
    a = np.array([a0, 2 * (1 - c * c), c * c - 2 * zp * c + 1]) / a0
    return b, a


b, a = notch_coef(4000, 300, 0.3, 0.1)
print(b.round(6), a.round(6))                  # [ 0.892116 -1.568401  0.868142] [ 1.       -1.568401  0.760258]
f, h = signal.freqz(b, a, worN=[250, 300, 350], fs=4000)
print((20 * np.log10(abs(h))).round(2))        # [ -5.35 -20.    -6.33]

这组系数和 ct.c2d(..., method="tustin", prewarp_frequency=2*np.pi*300) 算出来的完全一样；二阶低通的公式在 \(\zeta=0.707\) 时和 scipy.signal.butter(2, fc, fs=fs) 一样。

• 系数在初始化时算一次，tanf、expf 不要每个采样都算；改了采样频率要重新算；
• \(f_0\) 必须小于 \(f_s/2\)，而且最好离 \(f_s/2\) 远一点；
• float 一般够用：20 kHz 采样、10 Hz 陷波（\(f_0\) 只有 \(f_s\) 的 1/2000）时，用 float 系数算出的中心在 10.02 Hz，误差很小。

9. 实战流程：给伺服加一个陷波

1. 找共振
   • 回路已经在振（尖叫、嗡嗡响）：采集速度误差或电流，做 FFT（基础3 第6节），最高的那个峰就是振荡频率，它接近共振频率；
   • 回路还没闭上：开环扫频（第10章 10.5 节），看幅频曲线上的尖峰；或者用小锤敲一下结构，采振动数据做 FFT。
2. 定参数：\(f_0\) = 共振频率。在不同负载、不同位置各测一次，看共振频率在多大范围内变化，−3 dB 带宽 \(2\zeta_pf_0\) 要盖住这个范围；深度取「共振峰高出原来多少」再多留 6 dB 左右。
3. 检查代价：看陷波在回路截止频率处带来多少相位滞后，或者直接看开环波特图上的相位裕度。共振离截止频率越近，陷波的代价越大；太近了就只能降低回路带宽。
4. 离散化：用预畸变的 biquad，确认 \(f_0\) 离 \(f_s/2\) 足够远。
5. 实测验证：再做一次 FFT 或扫频，确认峰被压下去了；换负载、换位置再测；看阶跃响应有没有变差。

本章小结

练习