从‘一阶惯性环节’到代码实现：C语言写一个电机FOC控制用的低通滤波器（含幅频特性分析）

从理论到实践用C语言实现电机FOC控制中的一阶低通滤波器在电机磁场定向控制FOC系统中电流采样信号往往夹杂着高频噪声和开关谐波这些干扰会严重影响控制环路的性能。一阶低通滤波器作为信号调理的基础模块其实现质量直接决定了电流环的稳定性和动态响应。本文将带你从传递函数出发逐步推导出适合嵌入式MCU实现的高效C代码并通过幅频特性测试验证其实际滤波效果。1. 一阶低通滤波器的数学本质一阶低通滤波器在连续时域可以表示为以下传递函数G(s) 1 / (τs 1)其中τ为时间常数与截止频率fc的关系为τ 1 / (2πfc)在离散化处理时我们需要选择合适的数值积分方法。对于嵌入式系统前向欧拉法因其计算简单成为首选。将s替换为(z-1)/(Ts*z)的近似表达式得到离散传递函数G(z) (Ts/τ) / (z - (1 - Ts/τ))转换为差分方程形式y[n] α * x[n] (1 - α) * y[n-1]其中α Ts/(τ Ts) 2πfcTs/(2πfcTs 1)注意采样周期Ts的选择应满足Nyquist定理通常取控制系统主频的5-10倍2. C语言实现与优化技巧针对STM32等常用MCU我们给出三种不同精度需求的实现方案2.1 浮点型实现适合带FPU的MCUtypedef struct { float alpha; float prev_output; } LPF_Float; void LPF_Float_Init(LPF_Float* filter, float cutoff_freq, float sample_time) { float tau 1.0f / (2.0f * PI * cutoff_freq); filter-alpha sample_time / (tau sample_time); filter-prev_output 0.0f; } float LPF_Float_Update(LPF_Float* filter, float input) { filter-prev_output filter-alpha * input (1.0f - filter-alpha) * filter-prev_output; return filter-prev_output; }2.2 定点数实现适合无FPU的MCUtypedef struct { int32_t alpha; // Q15格式 int32_t prev_output; // Q15格式 } LPF_Fixed; void LPF_Fixed_Init(LPF_Fixed* filter, float cutoff_freq, float sample_time) { float tau 1.0f / (2.0f * PI * cutoff_freq); float alpha sample_time / (tau sample_time); filter-alpha (int32_t)(alpha * 32768); // 转换为Q15 filter-prev_output 0; } int16_t LPF_Fixed_Update(LPF_Fixed* filter, int16_t input) { int32_t input_scaled (int32_t)input 15; // 转为Q15 int32_t term1 (filter-alpha * input_scaled) 15; int32_t term2 ((32768 - filter-alpha) * filter-prev_output) 15; filter-prev_output term1 term2; return (int16_t)(filter-prev_output 15); }2.3 移位运算优化版极致性能typedef struct { uint16_t shift; // α 1/(2^shift) int16_t prev_output; } LPF_Fast; void LPF_Fast_Init(LPF_Fast* filter, float cutoff_freq, float sample_time) { float tau 1.0f / (2.0f * PI * cutoff_freq); float alpha sample_time / (tau sample_time); filter-shift (uint16_t)(log2f(1.0f/alpha) 0.5f); filter-prev_output 0; } int16_t LPF_Fast_Update(LPF_Fast* filter, int16_t input) { filter-prev_output input ((filter-prev_output - input) filter-shift); return filter-prev_output; }三种实现方式的对比实现方式计算复杂度精度适用场景浮点型中高带FPU的Cortex-M4/M7定点数较高中无FPU的MCU移位运算低低8/16位低端MCU3. 离散化误差分析与补偿前向欧拉法离散化会引入频率畸变实际截止频率f_actual与设计值f_design的关系为f_actual tan(πf_designTs) / (πTs)当f_designTs 0.1时误差小于1%。对于高精度要求的场合可采用预畸变补偿float warped_freq tanf(PI * design_freq * sample_time) / (PI * sample_time); // 使用warped_freq作为截止频率重新计算α不同离散化方法的特性对比前向欧拉法计算最简单高频段可能出现不稳定适合低频滤波fc fs/10后向欧拉法无条件稳定高频衰减更强需要解代数方程双线性变换频率畸变最小计算复杂度最高适合宽频带应用4. 在FOC电流环中的实际应用在电机FOC控制中典型的三相电流采样系统需要为每相配置独立的低通滤波器typedef struct { LPF_Float phaseU; LPF_Float phaseV; LPF_Float phaseW; float cutoff_freq; } Current_Filters; void CurrentFilters_Init(Current_Filters* filters, float cutoff) { float Ts 1.0f / CURRENT_SAMPLE_RATE; // 例如100kHz采样率 filters-cutoff_freq cutoff; LPF_Float_Init(filters-phaseU, cutoff, Ts); LPF_Float_Init(filters-phaseV, cutoff, Ts); LPF_Float_Init(filters-phaseW, cutoff, Ts); } void CurrentFilters_Update(Current_Filters* filters, float iu, float iv, float iw, float* iu_filtered, float* iv_filtered, float* iw_filtered) { *iu_filtered LPF_Float_Update(filters-phaseU, iu); *iv_filtered LPF_Float_Update(filters-phaseV, iv); *iw_filtered LPF_Float_Update(filters-phaseW, iw); }关键参数选择经验PWM频率通常20kHz采样率与PWM同步典型值100kHz每个PWM周期采样5次截止频率电流环1-2kHz低于PWM频率1个数量级速度环100-200Hz位置环10-20Hz调试技巧初始设置为目标频带的10倍频程逐步降低截止频率直到噪声可接受观察阶跃响应的超调量变化检查相位裕度是否足够≥45°5. 幅频特性测试验证通过白噪声激励法可以实测滤波器的频率响应// 生成测试信号0-fs/2的白噪声 float test_signal[SAMPLE_COUNT]; for(int i0; iSAMPLE_COUNT; i) { test_signal[i] 2.0f * ((float)rand()/RAND_MAX) - 1.0f; } // 通过滤波器 float output[SAMPLE_COUNT]; LPF_Float filter; LPF_Float_Init(filter, 1000.0f, 1.0f/SAMPLE_RATE); for(int i0; iSAMPLE_COUNT; i) { output[i] LPF_Float_Update(filter, test_signal[i]); } // 计算FFT arm_rfft_fast_instance_f32 fft; arm_rfft_fast_init_f32(fft, SAMPLE_COUNT); float fft_output[SAMPLE_COUNT]; arm_rfft_fast_f32(fft, output, fft_output, 0); // 绘制幅频曲线 for(int bin0; binSAMPLE_COUNT/2; bin) { float freq bin * SAMPLE_RATE / SAMPLE_COUNT; float magnitude sqrtf(fft_output[2*bin]*fft_output[2*bin] fft_output[2*bin1]*fft_output[2*bin1]); printf(%f Hz: %f\n, freq, magnitude); }实测结果分析要点-3dB点是否与设计值吻合过渡带斜率是否接近-20dB/decade高频段是否存在混叠现象相位响应在截止频率处是否为-45°常见问题排查现象可能原因解决方案截止频率偏高α计算错误检查Ts和fc的单位一致性高频振荡数值溢出改用32位累加器阶跃响应过冲离散化误差降低fc或改用双线性变换输出漂移定点数截断误差增加Q格式位数在STM32CubeIDE中可以结合Live Watch功能实时观察滤波效果。例如注入一个幅值1000、频率可调的正弦信号通过改变输入频率验证-3dB点的位置。