四旋翼/舵机云台角度控制：PID 与一阶低通滤波的工程实践
👁 6 阅读 · 2026-08-21 · 嵌入式 控制
在四旋翼飞行器或舵机云台的角度控制中，PID控制器是核心，但传感器噪声和机械振动常导致输出抖动。本文从工程角度出发，详细讲解PID参数整定与一阶低通滤波器的原理、配置步骤及代码实现，帮助开发者构建稳定、平滑的角度环。通过实际代码示例，展示如何在STM32平台上融合滤波与PID，提升控制品质。
## 1. 为什么需要滤波与PID结合
在四旋翼或云台系统中，角度传感器（如MPU6050）输出的原始数据包含高频噪声和机械振动。直接使用这些数据驱动PID，会导致控制量剧烈波动，舵机或电机发热、抖动，甚至失控。因此，必须对传感器数据进行低通滤波，去除高频分量，再送入PID控制器。
一阶低通滤波（LPF）因其计算量小、实时性强，成为嵌入式控制中最常用的滤波方法。其本质是：
```c
output = alpha * input + (1 - alpha) * output_prev;
```
其中 `alpha` 为滤波系数，取值范围0~1。`alpha` 越小，滤波越强，但响应越慢；`alpha` 越大，滤波越弱，响应越快。
## 2. 一阶低通滤波原理与参数选择
### 2.1 数学原理
一阶低通滤波的传递函数为：
```
H(s) = 1 / (τs + 1)
```
离散化后得到差分方程：
```c
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：
```c
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 微分先行与低通滤波
纯微分项会放大噪声，因此通常对微分项进行低通滤波，或采用“微分先行”结构（只对测量值微分）。推荐使用：
```c
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中断调用控制函数。
```c
// 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
```
```c
// 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;
}
```
```c
// 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 调试步骤
1. **先调滤波**：单独观察滤波后的角度曲线，确保噪声明显减小，且延迟可接受。
2. **整定P**：只加P，从小到大，直到系统出现等幅振荡，然后取当前值的60%作为P。
3. **加I**：缓慢增加I，消除稳态误差，注意积分限幅防止超调。
4. **加D**：增加D，抑制超调，但D过大会引入噪声，需配合微分滤波。
5. **联合调试**：在真实负载下微调，观察阶跃响应。
### 5.2 注意事项
- **采样时间一致性**：PID和滤波的`dt`必须与实际中断周期一致，否则参数失效。
- **抗积分饱和**：积分限幅要合理，否则系统会长时间饱和。
- **滤波系数调整**：`alpha` 过小会导致响应迟钝，过大则滤波效果差。建议从0.1~0.3开始。
- **舵机死区**：如果输出变化很小，舵机可能不响应，可添加死区处理。
- **测试环境**：在固定台架上测试，避免飞行器振动影响判断。
## 6. 总结
本文详细介绍了四旋翼/云台角度控制中PID与一阶低通滤波的结合方法。通过合理选择滤波系数和PID参数，可以有效抑制噪声，提高控制稳定性。实际工程中，还需根据具体硬件特性反复调试，才能达到最佳性能。希望本文能为你的嵌入式控制开发提供实用参考。