ArduPilot飞控开源代码之滤波设置

1. 源由

对于飞控传感器来来说，振动噪声也是数据。
单纯从数据角度看，和为了控制而必须补偿的干扰（例如风、湍流、油门动力补偿等）类似。
如果这些振动噪声进入系统，那么将会影响姿态控制回路的最佳调谐，导致性能下降。

为了更好的性能输出，获得最有的PID参数，必须从逻辑角度，根据特性对噪声进行识别和去除，调优步骤如下：

1. 从硬件上减小噪声（减震、机架强度、IMU避震海绵等等）
2. 从软件上过滤噪声（数字低通、带通滤波）
3. 调整PID姿态控制回路性能（PID适配）

鉴于硬件选用了DIY的Mark4机架上的整体硬件，且之前在Betaflight上都正常飞行过。其性能参考：BetaFlight Mark4 H7 Dual270 + BN880 + CRSF 配置存档。

在硬件不变更的情况下，滤波设置是后续调整PID性能的基础。

2. 原理

ArduPilot为噪声提供了两种滤波器：

• INS_ACCEL_FILTER控制的加速度计信号低通滤波器，以及INS_GYRO_FILTER控制的陀螺仪信号低通滤波器。
• INS_HNTCH_ENABLE和INS_HNTC2_ENABLE控制的谐波陷波滤波器。

对于多旋翼模型来说，实际上所有的振动都源于电动机的旋转频率。ArduPilot支持两个陷波滤波器，其滤波器频率可以与电机的电机旋转频率相关联，并提供初级频率及其谐波的陷波。

虽然低通滤波器可以有效地减少噪声的影响，但低频设置滤波点会产生大量的相位滞后。从姿态控制的角度来看，会有迟滞感。

启用谐波陷波滤波器中的一个或两个来针对电机产生的噪声，配合低通滤波，从而实现更紧密的调谐。

3. 调优

目前，手头DIY Mark4的模型，其噪声在Betaflight上已经非常明确的给出了图谱，详见参考资料中【BetaFlight Mark4的“妖怪”声音分析】。

在频谱图上给出的峰值点：

1. ~180Hz
2. ~350Hz
3. ~480Hz

从目前默认5寸ArduPilot参数情况看：

3.1 ACC低通滤波 INS_ACCEL_FILTER

维持系统默认参数，保持不变。

3.2 GRYO低通滤波 INS_GYRO_FILTER

默认5寸参数75Hz。

鉴于BF飞行情况，调整未250Hz。

3.3 陷波滤波 INS_HNTCH_ENABLE & INS_HNTC2_ENABLE

低通截止频率250Hz，因此在这之前只有一个180Hz左右的一个谐波噪声需要过滤。

鉴于目前H743的板子上，未看到bDshot协议反馈的RPM转速，ESC四合一电调板也不支持Telemetry。所以采用ArduPilot的FFT，最低中心频率从默认的100开始，3叶桨。

INS_HNTCH_HMNCS = 7 //1st/2nd/3rd harmonic
INS_HNTCH_FREQ = 120 //base center frequency in Hz
INS_HNTCH_MODE = 4 //Dynamic FFT
INS_HNTCH_BW = 60 //Harmonic Notch Filter bandwidth in Hz
INS_HNTCH_OPTS = 1 //Multi-Source

4. 总结

总体来说：

1. 整体还是比较平稳；
2. 控制有点“肉”，没有BF那套参数的相应快速；
3. 猛推油门，有抬头后低头现象；
4. 瞬时零油门，有抬头现象；

ArduPilot Mark4 5寸默认PID+滤波调参

ArduPilot Mark4 猛加油+松油门，抬头现象

后续可能需要继续改善的工作：

1. 分析数据log，看下是否滤波OK；
2. 进一步调整PID rate参数；
3. 抬头现象的调参：ATC_THR_G_BOOST/ATCANG*_P；

5. 参考资料

【1】ArduPilot: imu notch filtering
【2】ArduPilot: vibration damping
【3】ArduPilot: measuring vibration
【4】ArduPilot: throttle based notch
【5】BetaFlight Mark4 H7 Dual270 + BN880 + CRSF 配置存档
【6】BetaFlight Mark4之“妖怪”声音
【7】BetaFlight Mark4之“妖怪”声音之二
【8】ArduPilot开源飞控系统之简单介绍