1. 为什么需要滤波与PID结合
在四旋翼或云台系统中，角度传感器（如MPU6050）输出的原始数据包含高频噪声和机械振动。直接使用这些数据驱动PID，会导致控制量剧烈波动，舵机或电机发热、抖动，甚至失控。因此，必须对传感器数据进行低通滤波，去除高频分量，再送入PID控制器。
一阶低通滤波（LPF）因其计算量小、实时性强，成为嵌入式控制中最常用的滤波方法。其本质是：
output = alpha * input + (1 - alpha) * output_prev;
其中 alpha 为滤波系数，取值范围0~1。alpha 越小，滤波越强，但响应越慢；alpha 越大，滤波越弱，响应越快。
2. 一阶低通滤波原理与参数选择
2.1 数学原理
一阶低通滤波的传递函数为：
H(s) = 1 / (τs + 1)
离散化后得到差分方程：
y[k] = a * x[k] + (1 - a) * y[k-1]
其中 a = T / (τ + T)，T 为采样周期，τ 为时间常数。
2.2 参数选择
- 采样周期
T：通常取控制周期，如1ms（1000Hz）。 - 时间常数
τ：根据系统带宽需求。若云台带宽要求20Hz，则τ ≈ 1/(2π*20) ≈ 8ms，则a ≈ 0.111。 - 实际调试中，可先设置
a=0.2，观察响应，再微调。
3. PID控制器设计
3.1 位置式PID
对于角度环，常用位置式PID：
float pid_update(PID_t *pid, float setpoint, float measurement) {
float error = setpoint - measurement;
pid->integral += error;
// 抗积分饱和
if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;
float derivative = error - pid->prev_error;
pid->prev_error = error;
return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}
3.2 微分先行与低通滤波
纯微分项会放大噪声，因此通常对微分项进行低通滤波，或采用“微分先行”结构（只对测量值微分）。推荐使用：
float derivative = (measurement - pid->prev_measurement) / dt;
pid->prev_measurement = measurement;
// 对derivative再低通滤波
pid->deriv_lpf = alpha_d * derivative + (1 - alpha_d) * pid->deriv_lpf;
4. 完整代码示例（STM32平台）
以下代码在STM32F103上实现，使用定时器1ms中断调用控制函数。
// pid.h
#ifndef __PID_H
#define __PID_H
typedef struct {
float Kp, Ki, Kd;
float integral;
float prev_error;
float prev_measurement;
float deriv_lpf;
float integral_max;
float deriv_alpha; // 微分低通系数
} PID_t;
void pid_init(PID_t *pid, float kp, float ki, float kd, float integral_max, float deriv_alpha);
float pid_update(PID_t *pid, float setpoint, float measurement, float dt);
#endif
// pid.c
#include "pid.h"
void pid_init(PID_t *pid, float kp, float ki, float kd, float integral_max, float deriv_alpha) {
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
pid->integral = 0;
pid->prev_error = 0;
pid->prev_measurement = 0;
pid->deriv_lpf = 0;
pid->integral_max = integral_max;
pid->deriv_alpha = deriv_alpha;
}
float pid_update(PID_t *pid, float setpoint, float measurement, float dt) {
float error = setpoint - measurement;
// 积分项（带限幅）
pid->integral += error * dt;
if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;
// 微分项（对测量值微分，并低通滤波）
float derivative = (measurement - pid->prev_measurement) / dt;
pid->deriv_lpf = pid->deriv_alpha * derivative + (1 - pid->deriv_alpha) * pid->deriv_lpf;
pid->prev_measurement = measurement;
// 输出
float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * pid->deriv_lpf;
return output;
}
// main.c 示例
#include "pid.h"
#include "mpu6050.h" // 假设有MPU6050驱动
PID_t pid_roll, pid_pitch;
float target_angle = 0.0f;
float measured_angle;
void TIM1_UP_IRQHandler(void) {
if (TIM_GetITStatus(TIM1, TIM_IT_Update)) {
// 读取传感器（已滤波）
measured_angle = mpu6050_get_angle();
// 一阶低通滤波（alpha=0.15）
static float filtered_angle = 0;
filtered_angle = 0.15f * measured_angle + 0.85f * filtered_angle;
// PID计算
float output = pid_update(&pid_roll, target_angle, filtered_angle, 0.001f);
// 输出到舵机或电机（PWM）
set_servo_pwm(output);
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
}
}
int main(void) {
// 初始化硬件...
pid_init(&pid_roll, 2.0f, 0.1f, 0.05f, 100.0f, 0.2f);
// 启动定时器...
while(1) {}
}
5. 调试步骤与注意事项
5.1 调试步骤
- 先调滤波：单独观察滤波后的角度曲线，确保噪声明显减小，且延迟可接受。
- 整定P：只加P，从小到大，直到系统出现等幅振荡，然后取当前值的60%作为P。
- 加I：缓慢增加I，消除稳态误差，注意积分限幅防止超调。
- 加D：增加D，抑制超调，但D过大会引入噪声，需配合微分滤波。
- 联合调试：在真实负载下微调，观察阶跃响应。
5.2 注意事项
-
采样时间一致性：PID和滤波的
dt必须与实际中断周期一致，否则参数失效。 - 抗积分饱和：积分限幅要合理，否则系统会长时间饱和。
-
滤波系数调整：
alpha过小会导致响应迟钝，过大则滤波效果差。建议从0.1~0.3开始。 - 舵机死区：如果输出变化很小，舵机可能不响应，可添加死区处理。
- 测试环境：在固定台架上测试，避免飞行器振动影响判断。
6. 总结
本文详细介绍了四旋翼/云台角度控制中PID与一阶低通滤波的结合方法。通过合理选择滤波系数和PID参数，可以有效抑制噪声，提高控制稳定性。实际工程中，还需根据具体硬件特性反复调试，才能达到最佳性能。希望本文能为你的嵌入式控制开发提供实用参考。