谐波陷波滤波器的参数体系、动态跟踪、复合陷波与工程调参指南
一、引言：为什么需要谐波陷波
在多旋翼无人机的飞行控制中，电机和螺旋桨产生的振动是陀螺仪噪声的最主要来源。与随机白噪声不同，电机振动具有鲜明的周期性特征：它由一个基频（与电机转速直接相关）和一系列谐波分量（2 倍频、3 倍频、4 倍频......）组成。这些谐波分量能量集中、频率明确，如果不加处理地进入姿态控制环，会导致飞行抖动、PID 难以调高，甚至引发共振炸机。
普通的低通滤波器虽然能衰减高频噪声，但它是"一刀切"的------在滤除电机谐波的同时，也会衰减掉姿态控制需要的有效带宽，引入较大的相位延迟。而陷波滤波器（Notch Filter）则能精准地在特定频率处产生深度衰减，对通带内其他频率几乎没有影响。
HarmonicNotchFilter（谐波陷波滤波器）正是在单频陷波的基础上更进一步：它能在基频的多个谐波位置同时部署陷波，并且支持动态跟踪电机转速变化，是 ArduPilot 中抑制电机振动噪声最核心、最有效的武器。本文将系统解析它的参数体系、工作原理、复合陷波机制，并给出完整的工程调参指南。
二、类结构与核心成员
HarmonicNotchFilter 采用模板类设计，内部维护一个 NotchFilter 数组，通过组合而非继承的方式管理多个陷波：
┌─────────────────────────────────────────────────────────┐
│ HarmonicNotchFilter<T> │
├─────────────────────────────────────────────────────────┤
│ 成员变量: │
│ - _filters: NotchFilter<T>\[\] 陷波滤波器数组 │
│ - _sample_freq_hz: float 采样频率 (Hz) │
│ - _A: float 衰减因子 │
│ - _Q: float 品质因子 │
│ - _harmonics: uint32_t 谐波位掩码 │
│ - _composite_notches: uint8_t 复合陷波数量 (1/2/3)│
│ - _num_filters: uint16_t 滤波器总数 │
└─────────────────────────────────────────────────────────┘
这种设计的核心思想是：HarmonicNotchFilter 本身不实现滤波算法，而是负责"管理"一组 NotchFilter。它根据谐波配置、复合陷波配置和动态跟踪模式，计算出每个陷波的中心频率，然后将输入信号依次通过所有陷波，输出最终结果。这种组合模式使得单个 NotchFilter 的算法可以被复用，而 HarmonicNotchFilter 专注于频率调度和多滤波器管理。
三、核心参数详解
HarmonicNotchFilter 的参数通过 HarmonicNotchFilterParams 结构体管理，对应飞控参数中的 INS_HNTCH_* 系列。每个参数的含义如下：
|--------|--------|-----------------------|----------|
| 参数 | 名称 | 说明 | 典型取值 |
| FREQ | 基频 | 陷波滤波器的基准中心频率 (Hz) | 60~120 |
| BW | 带宽 | 每个陷波的 3dB 带宽 (Hz) | 20~60 |
| ATT | 衰减 | 中心频率处的最大衰减量 (dB) | 20~60 |
| HMNCS | 谐波 | 位掩码，指定启用哪些谐波（最多 16 次） | 1~15 |
| REF | 参考值 | 动态跟踪模式下的参考值（如油门参考点） | 0.5~1.0 |
| MODE | 跟踪模式 | 动态频率跟踪方式（0~5） | 1/3/4 |
| OPTS | 选项 | 双陷波、三陷波、全 IMU 启用等位选项 | 0/1/16 |
| FM_RAT | 最小频率比 | 油门跟踪时的最低频率比例（防止低频异常） | 0.1~0.3 |
3.1 基频（FREQ）
基频是整个谐波陷波体系的"锚点"。所有谐波陷波的中心频率都是基频的整数倍：第 1 谐波 = 基频 × 1，第 2 谐波 = 基频 × 2，以此类推。基频的确定是调参的第一步，通常通过 FFT 分析陀螺仪日志，找到电机振动的主峰频率。
对于四轴无人机，电机基频通常在悬停油门下约为 60~120 Hz（取决于电机 KV、螺旋桨尺寸和飞行重量）。需要注意的是，不同电机的转速可能略有差异，基频应取主要噪声峰的中心值。
3.2 带宽（BW）
带宽决定了每个陷波的"宽度"。带宽越大，陷波覆盖的频率范围越广，但对通带的影响也越大；带宽越小，陷波越"尖锐"，但如果电机转速波动较大，噪声峰可能移出陷波范围。
经验上，带宽通常设为基频的 30%~50%。例如基频 80 Hz 时，带宽取 30~40 Hz 较为合理。如果使用动态跟踪模式（转速实时更新），带宽可以适当取小一些，因为陷波中心会跟随噪声移动。
3.3 衰减量（ATT）
衰减量指定了中心频率处的最大衰减深度，单位为 dB。40 dB 意味着中心频率处的信号幅度被衰减到原来的 1%（20×log10(0.01) = -40 dB）。衰减量越大，噪声抑制越强，但也意味着滤波器的"Q 值"越高，对参数精度和稳定性的要求也越高。
通常取 40 dB 作为平衡点。对于噪声特别强的机型，可以取 50~60 dB；对于噪声较弱、追求最小相位影响的机型，可以取 20~30 dB。
四、谐波位掩码机制（HMNCS）
HMNCS 参数使用位掩码（Bitmask）来指定启用哪些谐波，最多支持 16 次谐波。每一位对应一个谐波阶数，置 1 表示启用该谐波，置 0 表示禁用：
Bit 0: 1st harmonic (基频 ×1) ──┐
Bit 1: 2nd harmonic (基频 ×2) ──┤
Bit 2: 3rd harmonic (基频 ×3) ──┤ 最多 16 次谐波
Bit 3: 4th harmonic (基频 ×4) ──┤
... ──┤
Bit 15: 16th harmonic (基频 ×16) ──┘
4.1 常见取值对照
|-------------|---------|------------|---------------|
| HMNCS 值 | 二进制 | 启用的谐波 | 陷波数量（单陷波） |
| 1 | 0b0001 | 第 1 谐波（基频） | 1 |
| 3 | 0b0011 | 第 1、2 谐波 | 2 |
| 7 | 0b0111 | 第 1、2、3 谐波 | 3 |
| 15 | 0b1111 | 第 1~4 谐波 | 4 |
| 31 | 0b11111 | 第 1~5 谐波 | 5 |
需要注意的是，上表中的"陷波数量"是每个电机的陷波数。对于四轴无人机，如果有 4 个电机各自独立跟踪转速，实际陷波总数 = 谐波数 × 电机数 × 复合陷波数。例如 HMNCS=3（2 个谐波）× 4 电机 × 双陷波 = 16 个陷波滤波器。
4.2 如何选择谐波数量
电机噪声的谐波能量通常随着阶数升高而递减，但高阶谐波可能与机体结构共振频率重合，反而产生较强的噪声峰。选择谐波数量的原则是：
- 通过 FFT 分析确认哪些谐波位置有明显的噪声峰，只启用有噪声的谐波。
- 通常前 2~4 次谐波能量最强，是主要抑制目标。
- 高阶谐波（5 次以上）如果 FFT 中没有明显峰值，不建议启用------浪费 CPU 且可能引入不必要的相位影响。
- 注意奈奎斯特限制：高阶谐波频率可能超过采样率的 48%，会被自动禁用（详见第十一章）。
五、动态跟踪模式（MODE）
电机转速在飞行过程中是不断变化的------悬停、爬升、机动时油门不同，转速也不同，对应的噪声基频自然也在变化。如果陷波中心频率固定不变，当电机转速偏离设定值时，噪声峰就会移出陷波范围，滤波效果大打折扣。动态跟踪模式正是为了解决这个问题。
|---------|----------------|-----------------------|--------|-----------------|--------------------|
| 模式值 | 名称 | 频率来源 | 精度 | 硬件依赖 | 推荐场景 |
| 0 | Fixed | 参数 FREQ 固定值 | --- | 无 | 转速恒定或调试阶段 |
| 1 | UpdateThrottle | 油门位置映射 | 低 | 无 | 无 RPM/ESC 遥测时的折中方案 |
| 2 | UpdateRPM | RPM 传感器 | 高 | 需 RPM 传感器 | 有转速传感器的机型 |
| 3 | UpdateBLHeli | ESC 遥测 (BLHeli/DShot) | 高 | 需支持遥测的 ESC | 主流推荐方案 |
| 4 | UpdateGyroFFT | 陀螺仪 FFT 实时检测 | 中 | 需启用 FFT（占用 CPU） | 无硬件遥测时的智能方案 |
| 5 | UpdateRPM2 | 第二个 RPM 传感器 | 高 | 需第二路 RPM | 多组独立转速的机型 |
5.1 各模式详解与选型建议
Fixed（模式 0）：
频率完全由 FREQ 参数固定，不随飞行状态变化。适用于电机转速基本恒定的场景（如固定翼巡航），或在调参初期用于验证陷波效果。缺点是飞行中油门变化时滤波效果会波动。
UpdateThrottle（模式 1）：
根据油门位置估算电机转速。需要设置 REF 参数作为油门参考点（通常为悬停油门对应的归一化值），频率 = FREQ × (油门 / REF)。这是一种"无硬件依赖"的折中方案，但精度有限------电机转速与油门并非严格线性关系，且受电池电压、飞行姿态等因素影响。建议在没有 ESC 遥测和 RPM 传感器时使用。
UpdateRPM（模式 2）/ UpdateRPM2（模式 5）：
通过外接 RPM 传感器（如霍尔传感器、光学转速计）获取精确的电机转速数据。精度最高，但需要额外硬件和接线。模式 5 用于第二组独立转速（如直升机的主旋翼和尾旋翼）。
UpdateBLHeli（模式 3）：
通过支持 DShot 遥测的电调（ESC）获取每个电机的实时转速。这是目前多旋翼最主流、最推荐的方案------无需额外硬件，只要电调支持 BLHeli_32 / BLHeli_S DShot 遥测即可。每个电机独立跟踪转速，精度高，且能反映不同电机间的转速差异。
UpdateGyroFFT（模式 4）：
对陀螺仪数据进行实时 FFT 分析，自动检测噪声主峰频率并动态更新陷波中心。优点是完全无需硬件支持，且能"自适应"任何噪声源；缺点是 FFT 计算会占用一定 CPU 资源，且在噪声峰不明显或存在多个峰时可能跟踪不稳定。适合没有 ESC 遥测的机型作为智能替代方案。
选型优先级：UpdateBLHeli（模式 3）> UpdateRPM（模式 2）> UpdateGyroFFT（模式 4）> UpdateThrottle（模式 1）> Fixed（模式 0）。如果电调支持 DShot 遥测，强烈建议使用模式 3，这是效果最好、成本最低的方案。
六、滤波器选项（OPTS）
OPTS 参数使用位掩码控制 HarmonicNotchFilter 的各种行为选项：
|-----------------|-------|-------|-------------------------------|
| 选项 | 位 | 值 | 说明 |
| DoubleNotch | 0 | 1 | 使用双陷波，在中心频率两侧各放一个陷波，获得更深更宽的衰减 |
| DynamicHarmonic | 1 | 2 | 动态谐波数量，根据频率自动启用/禁用高阶谐波 |
| LoopRateUpdate | 2 | 4 | 在调度器循环速率更新频率（而非默认的 200 Hz） |
| EnableOnAllIMUs | 3 | 8 | 在所有 IMU 上启用谐波陷波（默认仅主 IMU） |
| TripleNotch | 4 | 16 | 使用三陷波，在双陷波基础上增加中心陷波，衰减最深 |
| TreatLowAsMin | 5 | 32 | 将低于最小值的频率视为最小频率（防止低频异常） |
多个选项可以叠加。例如 OPTS = 17（二进制 10001）表示同时启用 DoubleNotch（位 0）和 TripleNotch（位 4）------不过实际上 TripleNotch 已经包含了双陷波的效果，通常不需要同时设置。OPTS = 9（1001）表示双陷波 + 全 IMU 启用。
七、工作原理
7.1 初始化流程
初始化时，HarmonicNotchFilter 根据参数计算滤波器系数并分配陷波数组：
void HarmonicNotchFilter::init(float sample_freq_hz,
HarmonicNotchFilterParams ¶ms)
{
// 1. 根据中心频率、带宽、衰减量计算 A 和 Q 参数
NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz,
attenuation_dB, _A, _Q);
// 2. 计算双/三陷波的频率扩展比例
_notch_spread = bandwidth_hz / (32 * center_freq_hz);
// 3. 根据谐波数、复合陷波数分配陷波滤波器组
allocate_filters(num_notches, harmonics, composite_notches);
}
7.2 频率更新
在动态跟踪模式下，频率更新函数会根据当前基频，为每个启用的谐波设置对应的陷波中心频率：
void HarmonicNotchFilter::update(float center_freq_hz)
{
// 对每个启用的谐波，设置对应的陷波中心频率
// 谐波频率 = 基频 × 谐波倍数 (1, 2, 3, 4, ...)
for (each harmonic_n) {
notch_center = center_freq_hz * (harmonic_n + 1);
set_center_frequency(idx, notch_center, spread_mul, harmonic_mul);
}
}
频率更新并非每次 apply() 都执行，而是以较低的速率（默认 200 Hz，或通过 LoopRateUpdate 选项设为调度器循环速率）定期执行。这是因为电机转速变化相对较慢，不需要在每个采样点都更新频率，而且频繁更新 Biquad 系数可能导致滤波器输出瞬态。
7.3 信号处理
实际的信号处理非常直接------将输入依次通过所有已启用的陷波滤波器：
T HarmonicNotchFilter::apply(const T &sample)
{
T output = sample;
// 依次通过所有陷波滤波器（级联）
for (uint16_t i = 0; i < _num_enabled_filters; i++) {
output = _filtersi.apply(output);
}
return output;
}
多个陷波采用级联（Cascade）方式：前一个陷波的输出作为后一个陷波的输入。级联的总幅频响应是各陷波幅频响应的乘积，因此在重叠区域衰减会叠加。这也是双陷波/三陷波能获得更深衰减的原理------多个陷波在中心频率附近重叠，衰减相乘。
八、复合陷波深度解析
单个陷波滤波器在中心频率处的衰减量由 ATT 参数决定（如 40 dB），但在偏离中心频率时衰减会迅速减小。为了获得更深的衰减和更宽的有效带宽，HarmonicNotchFilter 支持将多个陷波组合在一起，形成"复合陷波"。
8.1 单陷波、双陷波、三陷波对比
|--------|----------------------------------|-------------------|----------|------------|-------------|
| 类型 | 陷波位置 | 中心衰减 | 有效带宽 | CPU 开销 | 适用场景 |
| 单陷波 | 中心频率 f0 | ATT（如 40 dB） | 较窄 | 1× | 噪声峰尖锐且稳定 |
| 双陷波 | f0×(1-spread), f0×(1+spread) | 约 2×ATT（如 80 dB） | 较宽 | 2× | 噪声峰较宽或波动较大 |
| 三陷波 | f0, f0×(1-spread), f0×(1+spread) | 约 3×ATT（如 120 dB） | 最宽 | 3× | 噪声极强或高 Q 共振 |
8.2 频率扩展计算
双陷波和三陷波中，两侧陷波相对于中心频率的偏移量由 _notch_spread 决定：
// 扩展比例计算
_notch_spread = bandwidth_hz / (32 * center_freq_hz);
// 双陷波：两个陷波分别位于
// f_center × (1 - spread) （略低）
// f_center × (1 + spread) （略高）
// 三陷波：在双陷波基础上增加中心陷波
// f_center （中心）
// f_center × (1 - spread) （略低）
// f_center × (1 + spread) （略高）
以基频 80 Hz、带宽 40 Hz 为例：spread = 40 / (32 × 80) = 0.015625。双陷波的两个中心频率分别为 80 × (1-0.015625) ≈ 78.75 Hz 和 80 × (1+0.015625) ≈ 81.25 Hz。两个陷波相距仅 2.5 Hz，在中心区域高度重叠，衰减相乘后可达到约 80 dB 的深度。
8.3 如何选择复合陷波
- 默认使用单陷波即可满足大多数场景，CPU 开销最小。
- 如果 FFT 显示噪声峰较宽（如电机转速波动大、多电机频率分散），启用双陷波（OPTS=1）。
- 如果噪声极强（如大载重机型、高振动环境），或存在高 Q 结构共振，启用三陷波（OPTS=16）。
- 注意 CPU 资源：陷波总数 = 谐波数 × 电机数 × 复合陷波数。四轴 + 4 谐波 + 三陷波 = 48 个陷波，每个陷波是一个 Biquad（5 次乘法 + 4 次加法），在 1 kHz 采样率下约需 48 × 9 × 1000 = 432k 次运算/秒，需确认飞控 CPU 有余量。
九、典型配置示例
9.1 四轴无人机（油门跟踪模式）
INS_HNTCH_ENABLE = 1 // 启用谐波陷波
INS_HNTCH_FREQ = 80 // 基频 80 Hz（悬停时电机转速）
INS_HNTCH_BW = 40 // 带宽 40 Hz
INS_HNTCH_ATT = 40 // 衰减 40 dB
INS_HNTCH_HMNCS = 3 // 启用第 1、2 谐波（80 Hz, 160 Hz）
INS_HNTCH_MODE = 1 // 油门跟踪模式
INS_HNTCH_REF = 0.5 // 悬停油门参考值 0.5
此配置创建 2 个谐波 × 4 电机 = 8 个单陷波滤波器，适用于没有 ESC 遥测的入门级四轴。
9.2 四轴无人机（ESC 遥测 + 双陷波）
INS_HNTCH_ENABLE = 1 // 启用
INS_HNTCH_FREQ = 90 // 基频 90 Hz
INS_HNTCH_BW = 35 // 带宽 35 Hz
INS_HNTCH_ATT = 45 // 衰减 45 dB
INS_HNTCH_HMNCS = 7 // 启用第 1~3 谐波（90, 180, 270 Hz）
INS_HNTCH_MODE = 3 // ESC 遥测模式（BLHeli DShot）
INS_HNTCH_OPTS = 1 // 双陷波
此配置创建 3 谐波 × 4 电机 × 2（双陷波）= 24 个陷波滤波器，是中高端四轴的推荐配置。每个电机通过 ESC 遥测独立跟踪转速，精度高，双陷波提供更深的衰减。
9.3 大型无人机（FFT 跟踪 + 三陷波 + 全 IMU）
INS_HNTCH_ENABLE = 1 // 启用
INS_HNTCH_FREQ = 60 // 基频 60 Hz（大桨低转速）
INS_HNTCH_BW = 50 // 带宽 50 Hz
INS_HNTCH_ATT = 50 // 衰减 50 dB
INS_HNTCH_HMNCS = 15 // 启用第 1~4 谐波
INS_HNTCH_MODE = 4 // 陀螺仪 FFT 实时跟踪
INS_HNTCH_OPTS = 24 // 三陷波(16) + 全 IMU 启用(8)
此配置创建 4 谐波 × 4 电机 × 3（三陷波）= 48 个陷波滤波器，且在所有 IMU 上启用。适用于振动强烈的大型多旋翼或高载重机型，CPU 开销较大，需使用高性能飞控（如 H7 级以上）。
十、与普通 NotchFilter 对比
|--------|------------------------------|-------------------------------|
| 特性 | NotchFilter（单频陷波） | HarmonicNotchFilter（谐波陷波） |
| 陷波数量 | 单个 | 多个（谐波数 × 电机数 × 复合陷波数） |
| 谐波支持 | 不支持，需手动创建多个 | 内置谐波位掩码，自动部署 |
| 动态跟踪 | 需手动调用 set_center_frequency() | 内置 6 种跟踪模式，自动更新 |
| 复合陷波 | 不支持 | 支持双陷波/三陷波 |
| 参数管理 | 每个陷波独立设置 | 统一参数（FREQ/BW/ATT），谐波自动计算 |
| CPU 开销 | 极低（1 个 Biquad） | 中等（多个 Biquad 级联） |
| 适用场景 | 单一固定频率噪声 | 电机/螺旋桨谐波噪声（随转速变化） |
简单来说，NotchFilter 是"砖块"，HarmonicNotchFilter 是用这些砖块搭建的"建筑"。HarmonicNotchFilter 内部的每一个陷波都是一个标准的 NotchFilter，它的价值在于自动化的谐波部署、动态频率跟踪和复合陷波管理------这些如果手动用 NotchFilter 实现，需要大量重复代码且容易出错。
十一、频率限制与安全边界
HarmonicNotchFilter 在设计时设置了若干安全边界，防止极端参数导致滤波器不稳定或产生不良影响：
#define HARMONIC_NYQUIST_CUTOFF 0.48f // 陷波频率上限：采样率的 48%
#define NOTCHFILTER_ATTENUATION_CUTOFF 0.25 // 低于最小频率 25% 时禁用陷波
11.1 奈奎斯特上限（48% 采样率）
根据奈奎斯特采样定理，数字系统只能处理低于采样率一半（50%）的频率。HarmonicNotchFilter 将陷波频率上限定为采样率的 48%，留了 2% 的安全裕度。任何超过此上限的陷波（通常是高阶谐波）会被自动禁用。
例如采样率为 1000 Hz 时，陷波频率上限为 480 Hz。如果基频为 80 Hz，第 6 谐波为 480 Hz（刚好达到上限），第 7 谐波 560 Hz 则会被禁用。这也是为什么不建议盲目启用过多谐波------高阶谐波可能因超过奈奎斯特上限而被自动禁用，白白占用配置位却不产生实际效果。
11.2 低频下限（最小频率的 25%）
当动态跟踪的频率低于设定最小频率的 25% 时，陷波会被临时禁用。这是因为在极低频率下，陷波滤波器的 Q 值会变得极高，可能导致数值不稳定或产生不期望的振铃。这个保护机制在电机停转或怠速时尤为重要------防止低频陷波影响姿态控制的低频响应。
十二、工程调参实践指南
12.1 调参步骤
第一步：采集 FFT 数据
在飞控中启用 FFT 日志功能（INS_LOG_BAT_MASK 或等效参数），进行一次包含悬停、爬升、机动的飞行日志。用日志分析工具（如 Mission Planner 的 FFT 分析、或 ArduPilot Web Tools）查看陀螺仪信号的频谱，找到电机振动的主峰频率和明显的谐波峰。
第二步：确定基频和谐波
从 FFT 图中读取最强峰的频率作为基频（FREQ）。检查 2 倍、3 倍、4 倍频处是否有明显峰值，只启用有明显噪声的谐波（设置 HMNCS）。不要盲目启用所有谐波------没有噪声的位置放陷波只会增加 CPU 开销和相位影响。
第三步：选择跟踪模式
优先使用 ESC 遥测模式（MODE=3），如果电调不支持 DShot 遥测，考虑 FFT 模式（MODE=4）或油门模式（MODE=1）。设置 REF 参数为悬停时的油门值（油门模式）或转速参考值。
第四步：设置带宽和衰减
带宽（BW）通常取基频的 30%~50%。衰减（ATT）默认 40 dB，噪声特别强时可增至 50~60 dB。如果使用动态跟踪，带宽可适当取小（因为中心会跟随）；如果使用固定频率，带宽应取大一些以覆盖转速变化范围。
第五步：飞行验证与微调
配置完成后进行试飞，对比启用前后的陀螺仪噪声水平和飞行手感。如果仍有明显振动峰，检查是否谐波遗漏或带宽不足；如果出现飞行迟钝或相位滞后感，检查是否衰减过大或谐波过多。反复迭代直到 FFT 中电机谐波峰被有效抑制且飞行手感正常。
12.2 常见问题排查
|-------------|----------------------|------------------------------------------|
| 问题现象 | 可能原因 | 解决方法 |
| FFT 中噪声峰仍明显 | 基频设置不准 / 谐波遗漏 / 带宽不足 | 重新用 FFT 确认基频；检查 HMNCS 是否覆盖所有噪声峰；增大 BW |
| 飞行迟钝、响应变慢 | 衰减过大 / 谐波过多 / 带宽过宽 | 降低 ATT；减少谐波数量；缩小 BW |
| 特定油门段振动大 | 动态跟踪 REF 不准 / 油门非线性 | 校准 REF 为实际悬停油门；改用 ESC 遥测模式 |
| CPU 占用过高 | 陷波数量过多 / 三陷波 + 多谐波 | 减少谐波数；改用单陷波或双陷波；关闭非必要 IMU |
| 低频姿态振荡 | 低频陷波影响了控制带宽 | 检查 FM_RAT 设置；确认 TreatLowAsMin 选项；降低低频段衰减 |
| FFT 模式跟踪不稳定 | 噪声峰不明显 / 多峰干扰 | 改用 ESC 遥测或油门模式；调整 FFT 参数确保峰检测稳定 |
12.3 CPU 资源估算
每个陷波滤波器是一个 Biquad 二阶 IIR，每次 apply() 需要约 5 次乘法 + 4 次加法。总运算量估算公式：
运算量(次/秒) = 陷波总数 × 9(次运算) × 采样率(Hz)
以四轴、3 谐波、双陷波、1 kHz 采样率为例：陷波总数 = 4 × 3 × 2 = 24，运算量 = 24 × 9 × 1000 = 216,000 次/秒。对于 F4 级飞控（约 100~200 MIPS）这占比约 0.1%~0.2%，完全可以接受。但如果是 4 谐波 + 三陷波 + 全 IMU（双 IMU），陷波总数可达 96，运算量约 864k 次/秒，在 F4 上可能需要关注。建议在 H7 及以上飞控使用激进配置。
十三、应用场景
- 多旋翼无人机：最主要的应用场景，抑制电机/螺旋桨产生的基频和谐波振动，是提升飞行品质的关键手段。
- 直升机：抑制主旋翼（低频、高能量）和尾旋翼（高频）的旋转噪声，通常需要两组独立的谐波陷波（使用 RPM2 模式）。
- 固定翼（电动）：抑制电机和螺旋桨噪声，尤其在爬升和大油门时效果明显。
- 垂直起降（VTOL）：多旋翼模式下抑制电机噪声，固定翼模式下可降低或关闭陷波以减少相位影响。
- 任何有旋转机械的系统：只要存在周期性振动且频率可测或可估，都可以用谐波陷波进行抑制。
十四、结语
HarmonicNotchFilter 是 ArduPilot 滤波体系中最具"工程智慧"的设计之一。它没有发明新的滤波算法，而是将最基础的 Biquad 陷波通过谐波部署、动态跟踪和复合级联组合成一个强大的电机噪声抑制系统。理解它的参数体系（FREQ/BW/ATT/HMNCS/MODE/OPTS）、六种动态跟踪模式的取舍、双/三陷波的衰减机制，以及奈奎斯特和低频安全边界，是每一位飞控调参工程师的必修课。
调参的核心原则是"用 FFT 说话"------不要凭感觉设置参数，先采集频谱数据，确认噪声在哪里，再有针对性地部署陷波。最少的谐波数、最低的衰减量、最合适的跟踪模式，往往就是最好的配置。过度滤波带来的相位延迟和 CPU 开销，其危害可能不亚于噪声本身。
随着 ESC 遥测（DShot）的普及和 FFT 分析工具的成熟，谐波陷波的配置越来越简单。但无论工具如何进化，理解背后的原理始终是做出正确决策的基础。希望这篇文章能成为你调参路上的实用参考。