ArduPilot飞控开源代码之滤波设置
- 1. 源由
- 2. 原理
- 3. 调优
- 3.1 ACC低通滤波 INS_ACCEL_FILTER
- 3.2 GRYO低通滤波 INS_GYRO_FILTER
- 3.3 陷波滤波 INS_HNTCH_ENABLE & INS_HNTC2_ENABLE
- 4. 总结
- 5. 参考资料
1. 源由
对于飞控传感器来来说，振动噪声也是数据。
单纯从数据角度看，和为了控制而必须补偿的干扰（例如风、湍流、控制链路斜率等）类似。
如果这些振动噪声进入系统，那么将会影响姿态控制回路的最佳调谐，导致性能下降。
为了更好的性能输出，获得最有的PID参数，必须从逻辑角度，根据特性对噪声进行识别和去除，调优步骤如下：
- 从硬件上减小噪声（减震、机架强度、IMU避震海绵等等）
- 从软件上过滤噪声（数字低通、带通滤波）
- 调整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的“妖怪”声音分析】。
在频谱图上给出的峰值点：
- ~180Hz
- ~350Hz
- ~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. 总结
总体来说：
- 整体还是比较平稳；
- 控制有点“肉”，没有BF那套参数的相应快速；
- 猛推油门，有抬头后低头现象；
- 瞬时零油门，有抬头现象；
ArduPilot Mark4 5寸默认PID+滤波调参
ArduPilot Mark4 猛加油+松油门，抬头现象
后续可能需要继续改善的工作：
- 分析数据log，看下是否滤波OK；
- 进一步调整PID rate参数；
- 抬头现象的调参：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之“妖怪”声音之二
