在基于 Arduino 的 BLDC（无刷直流电机）机器人控制系统中，引入自适应陷波滤波器（Adaptive Notch Filter, ANF）是解决机械谐振与变频干扰的高级手段。传统的固定参数滤波器在面对负载变化、电池电压波动或复杂工况导致的共振频率偏移时往往失效，而自适应陷波滤波器能实时跟踪并抑制这些时变的干扰频率。

一、主要特点

1. 动态频率跟踪与实时抑制
   自适应陷波滤波器的核心能力在于其“自适应”性。
   在线辨识：不同于固定中心频率的陷波器，ANF 能够通过归一化估计算法（如 Lattice 结构算法或 Steiglitz-McBride 方法）实时分析系统反馈信号（如电流、转速或加速度），自动辨识出当前的干扰频率。
   参数自整定：一旦检测到干扰频率发生变化（例如因负载变动导致的机械共振点漂移），滤波器能自动调整其内部的零极点位置，即动态改变中心频率、带宽和衰减深度，确保始终对准干扰源进行抑制。
2. 抑制机械谐振与振动
   在机器人关节或高动态移动底盘中，传动机构（如减速箱、皮带、连杆）不可避免地存在弹性。
   双惯量系统问题：电机转子与负载之间通过弹性体连接，形成“双惯量系统”。在快速加减速时，这种弹性会导致负载端与电机端产生相对振荡（扭转振动）。
   作用机制：ANF 通过在控制环路（通常是速度环或电流环）中插入一个深度衰减的“凹口”，精准吸收特定频率的振动能量，从而阻尼系统的谐振峰，提升相位裕度，防止系统因谐振而失稳或产生噪声。
3. 提升信噪比与控制带宽
   信噪比优化：在存在周期性干扰（如齿轮啮合噪声、PWM 载波干扰）的环境中，ANF 能有效滤除这些窄带干扰，保留信号的有效成分，为后续的 PID 或更高级控制算法提供“干净”的反馈数据。
   带宽拓展：由于 ANF 有效压制了谐振峰，原本为了系统稳定而被迫降低的 PID 增益（从而限制了带宽）现在可以适当提高。这意味着机器人能获得更快的响应速度和更高的控制精度。
   二、应用场景
   高精度机器人关节控制
   在协作机器人、机械臂关节或力控灵巧手等应用中。
   场景：关节常面临末端负载变化（如更换夹具、抓取不同重量物体）的情况，这会导致传动系统的谐振频率发生显著偏移。
   应用：自适应陷波滤波器能实时跟踪这种频率漂移，持续抑制机械谐振，确保关节在不同负载下都能保持高刚度和低振动的平稳运行，避免因共振导致的定位抖动或异响。
   高速高精移动机器人底盘
   在需要频繁启停、高速运行的 AGV、AMR 或全向移动底盘中。
   场景：由于车体结构、轮胎变形或地面不平整，底盘在特定速度区间容易产生剧烈抖动（颤振）。
   应用：通过在速度环引入 ANF，实时滤除因车体模态振动引起的干扰频率，提升底盘的高速稳定性，改善轨迹跟踪精度，同时降低运行噪声，提升乘坐或运输的舒适性。
   振动环境下的传感器信号处理
   在搭载精密传感器（如激光雷达、相机）的机器人平台上。
   场景：电机和机械结构的振动会传导至传感器，导致激光雷达点云扩散、相机画面模糊。
   应用：利用 ANF 处理 IMU 或振动传感器的信号，提取振动特征，通过前馈或反馈方式驱动主动减振机构，或直接在软件层面修正传感器数据，提升感知系统的可靠性。
   三、需要注意的事项
   算法复杂度与硬件算力限制
   挑战：自适应陷波滤波器的频率辨识算法（如梯度下降、相关性分析）涉及较多的浮点乘除运算和迭代计算，计算量远大于传统 PID。
   对策：标准的 8 位 Arduino Uno/Nano 极难胜任实时控制。必须使用带有硬件 FPU（浮点运算单元）的 32 位微控制器，如 Arduino Due、Teensy 4.x 或 STM32H7/N7 系列，以确保控制周期的确定性和算法的收敛速度。
   收敛速度与稳态精度的权衡
   参数整定：ANF 算法中通常有一个关键的收敛因子（步长）。步长过大，跟踪速度快，但稳态时参数波动大，可能引入额外噪声；步长过小，跟踪平稳，但当干扰频率突变时，滤波器可能“掉队”，导致抑制失效。
   对策：需根据具体系统的动态特性进行精细标定。对于频率变化剧烈的工况，可采用变步长策略，或引入加速度/速度前馈来辅助频率预测。
   多模态谐振与陷波器数量
   局限性：物理系统往往存在多个谐振点（如一阶模态、二阶模态）。单个陷波滤波器只能处理一个频率点。
   对策：若系统存在明显的双共振点（如电机端共振和负载端共振），需要在控制环路中串联多个自适应陷波滤波器。这将进一步增加计算负担，且多个滤波器之间可能存在耦合干扰，需谨慎设计参数，避免相互影响导致系统不稳定。
   低信噪比下的频率丢失风险
   挑战：在干扰信号极弱或被强噪声淹没的工况下，ANF 的频率辨识算法可能无法收敛，甚至锁定到噪声频率上（误判）。
   对策：设计频率跟踪精度评估因子，实时监控滤波器的工作状态。若检测到输出信号与输入信号的相关性过低，应暂停自适应过程或重置参数，防止错误的频率估计破坏系统稳定性。

1、关节振动抑制（单轴）

#include <Encoder.h>
#include <Arduino_PID_Library.h>

// 硬件配置
#define MOTOR_PWM_PIN 9
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
Encoder encoder(ENCODER_A_PIN, ENCODER_B_PIN);

// 自适应陷波滤波器参数
float notchFreq = 50.0;  // 初始陷波频率(Hz)（需匹配机械共振频率）
float bandwidth = 5.0;   // 带宽(Hz)
float learningRate = 0.01; // 自适应学习率
float mu = 0.99;         // 遗忘因子
float notchOutput = 0.0;
float x_prev = 0.0, y_prev = 0.0; // 滤波器状态

// PID控制器
double Kp = 1.2, Ki = 0.5, Kd = 0.1;
double targetAngle = 90.0, currentAngle = 0.0, output = 0.0;
PID anglePID(&currentAngle, &output, &targetAngle, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  anglePID.SetMode(AUTOMATIC);
  anglePID.SetOutputLimits(-255, 255);
  anglePID.SetSampleTime(10);
}

void loop() {
  // 读取编码器角度
  long encValue = encoder.read();
  currentAngle = encValue * (360.0 / (1000.0 * 10.0));
  
  // 自适应陷波滤波器（抑制50Hz振动）
  float x = currentAngle;
  float omega = 2 * PI * notchFreq / 1000.0; // 转换为角频率(10ms采样)
  float b = (1 - mu) / (1 + mu);
  
  // 自适应调整陷波频率
  float error = x - y_prev;
  notchFreq += learningRate * error * x_prev;
  
  // 陷波滤波计算
  float a0 = 1 + b*b - 2*b*cos(omega);
  float a1 = 2*(b - cos(omega));
  float a2 = 1 + b*b - 2*b*cos(omega);
  float b0 = 1;
  float b1 = -2*cos(omega);
  float b2 = 1;
  
  notchOutput = (b0*x + b1*x_prev + b2*x_prev2 - a1*y_prev - a2*y_prev2) / a0;
  
  // 更新状态
  float x_prev2 = x_prev;
  x_prev = x;
  float y_prev2 = y_prev;
  y_prev = notchOutput;
  
  // 使用滤波后角度进行PID控制
  anglePID.Compute();
  analogWrite(MOTOR_PWM_PIN, (int)output);
  
  // 调试输出
  Serial.print("Raw: "); Serial.print(currentAngle);
  Serial.print(" Filtered: "); Serial.print(notchOutput);
  Serial.print(" Freq: "); Serial.println(notchFreq);
  
  delay(10);
}

2、多轴振动协同抑制（双关节机械臂）

#include <Encoder.h>

// 关节1配置
#define MOTOR1_PWM_PIN 9
#define ENCODER1_A_PIN 2
#define ENCODER1_B_PIN 3
Encoder encoder1(ENCODER1_A_PIN, ENCODER1_B_PIN);
float notchFreq1 = 45.0, notchOutput1 = 0.0;
float x_prev1 = 0.0, y_prev1 = 0.0;

// 关节2配置
#define MOTOR2_PWM_PIN 10
#define ENCODER2_A_PIN 4
#define ENCODER2_B_PIN 5
Encoder encoder2(ENCODER2_A_PIN, ENCODER2_B_PIN);
float notchFreq2 = 55.0, notchOutput2 = 0.0;
float x_prev2 = 0.0, y_prev2 = 0.0;

// 共享参数
float learningRate = 0.005;
float mu = 0.98;

void setup() {
  Serial.begin(115200);
}

void adaptiveNotchFilter(float &x, float &y, float &notchFreq, 
                        float &x_prev, float &y_prev, 
                        float &x_prev2, float &y_prev2) {
  float omega = 2 * PI * notchFreq / 1000.0;
  float b = (1 - mu) / (1 + mu);
  
  // 自适应调整
  float error = x - y_prev;
  notchFreq += learningRate * error * x_prev;
  
  // 陷波计算
  float a0 = 1 + b*b - 2*b*cos(omega);
  float a1 = 2*(b - cos(omega));
  float a2 = 1 + b*b - 2*b*cos(omega);
  
  y = (x + b*x_prev - x_prev2 - a1*y_prev - a2*y_prev2) / a0;
  
  // 更新状态
  x_prev2 = x_prev;
  x_prev = x;
  y_prev2 = y_prev;
  y_prev = y;
}

void loop() {
  // 关节1处理
  long encValue1 = encoder1.read();
  float angle1 = encValue1 * (360.0 / (1000.0 * 10.0));
  adaptiveNotchFilter(angle1, notchOutput1, notchFreq1, x_prev1, y_prev1, x_prev1_2, y_prev1_2);
  analogWrite(MOTOR1_PWM_PIN, (int)(notchOutput1 * 5.0)); // 简单控制示例
  
  // 关节2处理
  long encValue2 = encoder2.read();
  float angle2 = encValue2 * (360.0 / (1000.0 * 10.0));
  adaptiveNotchFilter(angle2, notchOutput2, notchFreq2, x_prev2, y_prev2, x_prev2_2, y_prev2_2);
  analogWrite(MOTOR2_PWM_PIN, (int)(notchOutput2 * 5.0));
  
  // 调试输出
  Serial.print("J1 Freq: "); Serial.print(notchFreq1);
  Serial.print(" J2 Freq: "); Serial.println(notchFreq2);
  
  delay(10);
}

3、带FFT分析的自适应陷波滤波器（进阶版）

#include <arduinoFFT.h>
#include <Encoder.h>

#define MOTOR_PWM_PIN 9
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
Encoder encoder(ENCODER_A_PIN, ENCODER_B_PIN);

// FFT参数
#define SAMPLES 128
#define SAMPLING_FREQ 1000 // 1kHz采样率
double vReal[SAMPLES], vImag[SAMPLES];
arduinoFFT FFT = arduinoFFT();

// 自适应陷波滤波器
float notchFreq = 0.0;
float notchOutput = 0.0;
float x_prev = 0.0, y_prev = 0.0;

void setup() {
  Serial.begin(115200);
}

void updateNotchFrequency() {
  // 采集FFT样本
  for (int i = 0; i < SAMPLES; i++) {
    long encValue = encoder.read();
    vReal[i] = encValue * (360.0 / (1000.0 * 10.0));
    vImag[i] = 0;
    delayMicroseconds(1000000 / SAMPLING_FREQ); // 精确采样间隔
  }
  
  // 执行FFT
  FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(vReal, vImag, SAMPLES, FFT_FORWARD);
  FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);
  
  // 寻找主频峰（忽略直流分量）
  float maxMagnitude = 0;
  int peakIndex = 1;
  for (int i = 1; i < SAMPLES/2; i++) {
    if (vReal[i] > maxMagnitude) {
      maxMagnitude = vReal[i];
      peakIndex = i;
    }
  }
  
  // 更新陷波频率
  notchFreq = peakIndex * SAMPLING_FREQ / (float)SAMPLES;
}

void adaptiveNotchFilter(float x) {
  static float notchFreqPrev = 0.0;
  if (abs(notchFreq - notchFreqPrev) > 1.0) { // 频率变化超过1Hz时更新
    updateNotchFrequency();
    notchFreqPrev = notchFreq;
  }
  
  // 简化陷波滤波（实际需实现动态系数计算）
  float omega = 2 * PI * notchFreq / 1000.0;
  static float b = 0.95; // 固定带宽参数
  
  // 二阶IIR陷波滤波
  float a0 = 1 + b*b - 2*b*cos(omega);
  notchOutput = (x + b*x_prev - x_prev2 - 2*(b - cos(omega))*y_prev 
                + (1 + b*b - 2*b*cos(omega))*y_prev2) / a0;
  
  // 更新状态
  static float x_prev2 = 0.0, y_prev2 = 0.0;
  x_prev2 = x_prev;
  x_prev = x;
  y_prev2 = y_prev;
  y_prev = notchOutput;
}

void loop() {
  long encValue = encoder.read();
  float currentAngle = encValue * (360.0 / (1000.0 * 10.0));
  
  adaptiveNotchFilter(currentAngle);
  analogWrite(MOTOR_PWM_PIN, (int)(notchOutput * 5.0));
  
  // 调试输出
  Serial.print("Dominant Freq: "); Serial.print(notchFreq);
  Serial.print(" Filtered: "); Serial.println(notchOutput);
  
  delay(10);
}

要点解读
陷波频率自适应机制
动态调整：通过误差反馈（案例一）或FFT频谱分析（案例三）实时更新陷波频率，适应机械结构变化。
学习率选择：learningRate需权衡收敛速度与稳定性（典型值0.001~0.1），过大导致振荡，过小响应慢。
数字滤波器实现要点
二阶IIR结构：陷波滤波器通常采用二阶IIR形式，需注意系数计算时的数值稳定性（案例一中a0分母接近零时的保护）。
状态变量管理：必须保存前两次输入（x_prev, x_prev2）和输出（y_prev, y_prev2）以实现递归计算。
多轴协同处理
参数隔离：每个关节需独立滤波器实例（案例二），避免不同机械模态间的耦合干扰。
计算负载：Arduino Uno难以实时处理多轴FFT，建议：
使用ESP32等双核MCU分担计算
采用简化自适应算法（如案例一的LMS变体）
与控制环路的集成
滤波位置选择：
前馈路径：在PID计算前滤波（如案例一），直接抑制振动成分
反馈路径：滤波测量值（需注意相位延迟补偿）
采样同步：滤波器采样周期必须与控制周期严格一致（如10ms），避免混叠。
实际工程优化建议
硬件加速：使用STM32的硬件FPU或ESP32的DSP指令集加速浮点运算。
抗饱和处理：在输出级增加限幅（如constrain(output, -255, 255)）防止积分饱和。
故障检测：当自适应频率超出合理范围（如>200Hz）时触发保护机制。
参数持久化：将优化后的陷波频率存入EEPROM，避免每次上电重新学习。

4、机械臂共振频率跟踪系统

#include <AdaptiveNotchFilter.h>
#include <Encoder.h>
#include <PID_v1.h>

// 自适应陷波滤波器配置
AdaptiveNotchFilter anf;              // 默认采样率1kHz
float freqEstimation = 50.0;          // 初始估计频率(Hz)
float bandwidth = 5.0;                // 带宽系数(越大收敛越快但稳定性下降)

// 硬件接口
Encoder jointEnc(2,3);                // 编码器读取关节角度
BLDCMotor motor(9,10,8);             // 驱动电机控制引脚
PID positionPID(&currentAngle, &motorPower, targetAngle, 1.2, 0.05, 0.1);

void setup() {
  anf.begin(1000);                    // 设置采样频率1kHz
  anf.setFrequency(freqEstimation);   // 初始化中心频率
  anf.setBandwidth(bandwidth);        // 设置带宽参数
  positionPID.SetMode(AUTOMATIC);
}

void loop() {
  static unsigned long lastTime = millis();
  if (millis() - lastTime > 1) {      // 1ms控制周期
    // 获取当前位置信号
    float angle = map(jointEnc.read(), 0, ENC_CPR, 0, 360);
    float errorSignal = targetAngle - angle; // 误差信号作为滤波器输入
    
    // 自适应滤波处理
    float filteredError = anf.update(errorSignal);
    
    // PID控制输出
    positionPID.Compute();
    motor.setSpeed(motorPower + filteredError * K_ADAPTIVE_GAIN);
    
    // 在线更新频率估计值（基于FFT结果）
    if (newFreqAvailable()) {
      anf.setFrequency(latestFreq);  // 动态调整中心频率
    }
    
    lastTime = millis();
  }
}

要点解读：

LMS算法实现：采用最小均方误差准则实时更新权系数追踪工频干扰。
Q因子调节：带宽参数需根据机械系统惯性进行折衷选择（典型值2-10）。
非周期性扰动抑制：结合滑动平均窗提高对随机振动的鲁棒性。
相位补偿机制：在反馈回路中加入超前校正环节减少相位滞后。
多模态识别：通过聚类分析区分不同阶次的机械共振模式。

5、电源纹波噪声消除系统

#include <AnalogInput.h>
#include <DigitalFilter.h>
#include <WirelessComm.h>

// 电池管理系统监测
AnalogInput batteryVoltage(A0);      // 电池电压检测
AdaptiveNotchFilter anfDClink;       // DC母线纹波滤波器
float lineFrequency = 60.0;           // 电网基波频率(可改为测量值)

void setup() {
  anfDClink.begin(2000);             // 高速采样率捕捉纹波成分
  anfDClink.setFrequency(lineFrequency);
  anfDClink.enableAdaptation(true);  // 开启自适应模式
}

void loop() {
  float rawVdc = batteryVoltage.read(); // 读取含纹波的直流电压
  float cleanVdc = anfDClink.update(rawVdc); // 滤除纹波得到纯净值
  
  // 过压/欠压保护逻辑
  if (cleanVdc > OVERVOLTAGE_THRESHOLD) {
    triggerProtectionCircuit();
  } else if (cleanVdc < UNDERVOLTAGE_THRESHOLD) {
    initiateSoftStartRoutine();
  }
  
  // 无线传输监测数据
  sendPacketWireless({timestamp, cleanVdc, temperature});
  delay(100); // 降低功耗
}

void validateLineFrequency() {
  // 每周波采样点数=采样率/频率=>建议取整数值保证整数倍关系
  uint16_t samplesPerCycle = round(SAMPLE_RATE / lineFrequency);
  if (samplesPerCycle < MIN_SAMPLES || samplesPerCycle > MAX_SAMPLES) {
    adjustSamplingRate();           // 自动调整采样率适应宽范围变化
  }
}

要点解读：

同步采样优化：使ADC采样时刻与干扰源相位锁定以提高信噪比。
直流偏移校正：加入高通滤波预处理防止饱和现象发生。
非线性负载适配：针对变频器类负载扩展至三次谐波抑制。
能量回馈处理：再生制动时的特殊工况需单独建模。
冗余设计原则：重要场合可采用双通道互为备用方案。

6、麦克风阵列声学回声消除

#include <AudioProcessing.h>
#include <I2SCommunicator.h>
#include <FarEndMonitor.h>

// 语音通信系统架构
MicrophoneArray mics[4];              // 四通道麦克风阵列
AdaptiveNotchFilter echoCanceller;    // 回声消除滤波器
FarEndSpeaker speaker;               // 远端扬声器设备

void setup() {
  for(int i=0; i<4; i++) mics[i].begin();
  echoCanceller.begin(8000);          // 电话音质采样率8kHz
  i2sInit();                         // 初始化音频编解码器
}

void loop() {
  static bool isSpeaking = false;
  int16_t buffer[FRAME_SIZE];         // 音频帧缓冲区
  
  // 近端说话人检测
  float energy = calculateShortTermEnergy(buffer);
  if (energy > SPEECH_THRESHOLD) {
    isSpeaking = true;
    echoCanceller.resetCoefficients(); // 清空历史数据避免误判
  }
  
  // 自适应滤波处理
  float nearEndSignal = getMicData();
  float farEndSignal = getSpeakerOutput();
  float canceledSignal = echoCanceller.process(nearEndSignal, farEndSignal);
  
  // 网络传输准备
  prepareForTransmission(canceledSignal);
  transmitAudioFrame();
  
  // 自适应步长控制
  updateStepSize(echoCanceller.getErrorPower());
}

void updateStepSize(float errorVariance) {
  // 根据误差功率动态调整收敛速度
  float mu = MU_INIT / (1 + KAPPA * errorVariance);
  echoCanceller.setStepSize(mu);
}

要点解读：

归一化最小二乘法：改进传统LMS算法提高收敛速度和解调质量。
双向通话检测：DTMF音检测防止双边对话时的啸叫问题。
子带分解策略：将全频带划分为多个子带来改善计算效率。
非线性失真补偿：针对扬声器谐波失真进行预畸变处理。
环境噪声容忍：加入谱减法增强主瓣方向的信噪比。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。