当前位置：首页 > news >正文

程序代码篇---卡尔曼滤波与PID的组合应用

news 2025/7/26 15:21:50

一、小车速度控制（卡尔曼滤波 + PID）

场景说明：

小车通过编码器测量速度（含噪声），用卡尔曼滤波得到准确速度，再通过 PID 控制电机输出，使小车稳定在目标速度。

1. Python 仿真版本（原理演示）

import numpy as np
import matplotlib.pyplot as plt# 1. 卡尔曼滤波器（处理编码器噪声）
class KalmanFilter:def __init__(self, initial_speed, Q=0.1, R=0.5):self.x = initial_speed  # 估计速度self.P = 1.0            # 估计误差协方差self.Q = Q              # 过程噪声（系统本身的不确定性）self.R = R              # 测量噪声（编码器噪声）self.F = 1.0            # 状态转移矩阵（速度不变假设）self.H = 1.0            # 测量矩阵def predict(self):# 预测：根据上一时刻状态估计当前状态self.x = self.F * self.xself.P = self.F * self.F * self.P + self.Qdef update(self, measurement):# 更新：用测量值修正预测K = self.P * self.H / (self.H * self.P * self.H + self.R)  # 卡尔曼增益self.x = self.x + K * (measurement - self.H * self.x)      # 修正估计self.P = (1 - K * self.H) * self.P                         # 更新误差# 2. PID控制器（控制电机输出）
class PIDController:def __init__(self, Kp=5.0, Ki=0.1, Kd=0.5):self.Kp = Kp  # 比例项（误差越大，输出越强）self.Ki = Ki  # 积分项（消除长期误差）self.Kd = Kd  # 微分项（抑制震荡）self.error = 0.0self.integral = 0.0self.derivative = 0.0self.prev_error = 0.0def compute(self, target, current):self.error = target - currentself.integral += self.error  # 累积误差self.derivative = self.error - self.prev_error  # 误差变化率self.prev_error = self.error# PID输出 = 比例项 + 积分项 + 微分项output = self.Kp * self.error + self.Ki * self.integral + self.Kd * self.derivativereturn np.clip(output, 0, 100)  # 限制输出在0-100（模拟PWM范围）# 3. 仿真小车运动
def simulate_car():target_speed = 10.0  # 目标速度：10m/skf = KalmanFilter(initial_speed=0.0)pid = PIDController()# 模拟数据true_speeds = []  # 真实速度（用于对比）measurements = []  # 带噪声的编码器测量值filtered_speeds = []  # 卡尔曼滤波后的速度pid_outputs = []  # PID输出（电机功率）current_true_speed = 0.0for t in range(100):# 模拟真实速度变化（受电机输出影响）if t > 0:current_true_speed += (pid_outputs[-1] - 50) * 0.01  # 简化模型current_true_speed = max(0, current_true_speed)  # 速度不为负# 模拟编码器测量（真实速度+噪声）measurement = current_true_speed + np.random.normal(0, 1.0)  # 噪声measurements.append(measurement)# 卡尔曼滤波kf.predict()kf.update(measurement)filtered_speeds.append(kf.x)true_speeds.append(current_true_speed)# PID控制pid_output = pid.compute(target_speed, kf.x)pid_outputs.append(pid_output)# 绘图对比plt.figure(figsize=(12, 6))plt.plot(true_speeds, label='真实速度', linestyle='--')plt.plot(measurements, label='编码器测量（带噪声）', alpha=0.5)plt.plot(filtered_speeds, label='卡尔曼滤波后速度')plt.axhline(target_speed, color='r', label='目标速度')plt.xlabel('时间（步）')plt.ylabel('速度（m/s）')plt.legend()plt.show()simulate_car()

代码解释：

卡尔曼滤波：过滤编码器的随机噪声，让速度曲线更平滑（对比 “测量值” 和 “滤波后值”）。
PID 控制：根据 “目标速度” 和 “滤波后速度” 的误差，调整电机输出（如 PWM 占空比），使实际速度逐渐接近目标。

2. ESP32 硬件版本（实际控制）

硬件需求：ESP32 + 带编码器的直流电机 + L298N 电机驱动
接线说明：

编码器 A/B 相 → ESP32 GPIO（如 D2、D3）
电机驱动 IN1/IN2 → ESP32 GPIO（如 D4、D5）
电机电源 → 外部电池

#include <Encoder.h>// 1. 卡尔曼滤波器
class KalmanFilter {
private:float x;  // 估计速度float P;  // 估计误差协方差float Q;  // 过程噪声float R;  // 测量噪声float F;  // 状态转移矩阵float H;  // 测量矩阵public:KalmanFilter(float initialSpeed, float Q_val, float R_val) {x = initialSpeed;P = 1.0;Q = Q_val;R = R_val;F = 1.0;H = 1.0;}void predict() {x = F * x;P = F * F * P + Q;}void update(float measurement) {float K = P * H / (H * P * H + R);x = x + K * (measurement - H * x);P = (1 - K * H) * P;}float getSpeed() { return x; }
};// 2. PID控制器
class PIDController {
private:float Kp, Ki, Kd;float error, integral, derivative, prevError;public:PIDController(float p, float i, float d) {Kp = p;Ki = i;Kd = d;error = 0;integral = 0;derivative = 0;prevError = 0;}float compute(float target, float current) {error = target - current;integral += error * 0.01;  // 0.01s采样周期derivative = (error - prevError) / 0.01;prevError = error;float output = Kp * error + Ki * integral + Kd * derivative;return constrain(output, 0, 255);  // 限制PWM范围（0-255）}
};// 3. 硬件初始化
Encoder motorEncoder(2, 3);  // 编码器A相→D2，B相→D3
const int in1 = 4, in2 = 5;  // 电机驱动引脚
KalmanFilter kf(0.0, 0.1, 0.5);  // Q=0.1, R=0.5
PIDController pid(10.0, 0.5, 2.0);  // Kp=10, Ki=0.5, Kd=2
float targetSpeed = 50.0;  // 目标速度（编码器脉冲/秒）void setup() {Serial.begin(115200);pinMode(in1, OUTPUT);pinMode(in2, OUTPUT);
}void loop() {static unsigned long lastTime = 0;unsigned long now = millis();if (now - lastTime >= 10) {  // 10ms采样一次（100Hz）lastTime = now;// 读取编码器速度（脉冲/秒）long encoderPos = motorEncoder.read();float rawSpeed = encoderPos * 100;  // 10ms→1秒换算motorEncoder.write(0);  // 重置计数// 卡尔曼滤波kf.predict();kf.update(rawSpeed);float filteredSpeed = kf.getSpeed();// PID控制电机float pwm = pid.compute(targetSpeed, filteredSpeed);analogWrite(in1, pwm);  // 正转（IN2接GND）digitalWrite(in2, LOW);// 串口输出调试Serial.print("原始速度: ");Serial.print(rawSpeed);Serial.print("  滤波后: ");Serial.print(filteredSpeed);Serial.print("  PWM: ");Serial.println(pwm);}
}

代码解释：

编码器读取：通过Encoder库计算脉冲数，换算为每秒速度。
卡尔曼滤波：处理编码器的机械抖动噪声，得到稳定的速度值。
PID 输出：根据目标速度（如 50 脉冲 / 秒）和滤波后速度的误差，调整 PWM 占空比，控制电机转速。

二、室温控制（PID 单控，无需卡尔曼）

场景说明：

通过温度传感器（如 DHT11）读取室温，用 PID 控制加热片（或空调），使温度稳定在目标值（如 25℃）。温度传感器噪声小，通常无需卡尔曼滤波。

1. Python 仿真版本

import numpy as np
import matplotlib.pyplot as plt# PID控制器
class PIDController:def __init__(self, Kp=20, Ki=0.5, Kd=5):self.Kp = Kpself.Ki = Kiself.Kd = Kdself.error = 0self.integral = 0self.derivative = 0self.prev_error = 0def compute(self, target, current, dt=1):self.error = target - currentself.integral += self.error * dt  # 积分=误差×时间self.derivative = (self.error - self.prev_error) / dt  # 导数=误差变化/时间self.prev_error = self.erroroutput = self.Kp * self.error + self.Ki * self.integral + self.Kd * self.derivativereturn np.clip(output, 0, 100)  # 加热功率0-100%# 仿真室温控制
def simulate_room_temp():target_temp = 25.0  # 目标温度25℃pid = PIDController()current_temp = 20.0  # 初始温度20℃temps = [current_temp]powers = []for t in range(100):# 计算加热功率power = pid.compute(target_temp, current_temp)powers.append(power)# 模拟温度变化（加热+散热）current_temp += power * 0.05  # 加热贡献current_temp -= (current_temp - 20) * 0.1  # 散热（室温20℃）current_temp += np.random.normal(0, 0.1)  # 小噪声temps.append(current_temp)# 绘图plt.figure(figsize=(12, 6))plt.plot(temps, label='室温')plt.axhline(target_temp, color='r', label='目标温度')plt.plot(powers, label='加热功率', linestyle='--')plt.xlabel('时间（秒）')plt.ylabel('温度（℃）/功率（%）')plt.legend()plt.show()simulate_room_temp()

代码解释：

温度低于目标时，PID 输出正功率（加热）；接近目标时，功率逐渐减小，避免超调。
积分项（Ki）会累积小误差（如长期差 1℃），确保最终温度稳定在目标。

2. ESP32 硬件版本

硬件需求：ESP32 + DHT11 温湿度传感器 + 继电器（控制加热片）
接线说明：

DHT11 数据脚 → ESP32 GPIO14
继电器控制脚 → ESP32 GPIO15

#include <DHT.h>// 1. PID控制器
class PIDController {
private:float Kp, Ki, Kd;float error, integral, derivative, prevError;public:PIDController(float p, float i, float d) {Kp = p;Ki = i;Kd = d;error = 0;integral = 0;derivative = 0;prevError = 0;}float compute(float target, float current, float dt) {error = target - current;integral += error * dt;derivative = (error - prevError) / dt;prevError = error;// 限制积分项，防止饱和integral = constrain(integral, -100, 100);return Kp * error + Ki * integral + Kd * derivative;}
};// 2. 硬件初始化
#define DHTPIN 14
#define DHTTYPE DHT11
DHT dht(DHTPIN, DHTTYPE);
const int relayPin = 15;
PIDController pid(30.0, 0.5, 5.0);  // Kp=30, Ki=0.5, Kd=5
float targetTemp = 25.0;  // 目标温度25℃void setup() {Serial.begin(115200);dht.begin();pinMode(relayPin, OUTPUT);digitalWrite(relayPin, LOW);  // 初始关闭加热
}void loop() {static unsigned long lastTime = 0;unsigned long now = millis();float dt = (now - lastTime) / 1000.0;  // 时间差（秒）if (dt >= 1.0) {  // 1秒采样一次lastTime = now;// 读取温度float currentTemp = dht.readTemperature();if (isnan(currentTemp)) {Serial.println("读取DHT11失败");return;}// PID计算float output = pid.compute(targetTemp, currentTemp, dt);// 控制继电器（输出>0时加热）if (output > 0) {digitalWrite(relayPin, HIGH);  // 打开加热} else {digitalWrite(relayPin, LOW);   // 关闭加热}// 串口输出Serial.print("当前温度: ");Serial.print(currentTemp);Serial.print("℃  目标: ");Serial.print(targetTemp);Serial.print("℃  输出: ");Serial.println(output);}
}

代码解释：

DHT11 读取室温，若温度低于目标，PID 输出正值，继电器吸合（加热）；温度达标后，输出为 0，停止加热。
微分项（Kd）可抑制温度快速波动（如加热过度后快速降温）。

总结

技术	小车控制作用	室温控制作用
卡尔曼滤波	处理编码器噪声，得到真实速度	通常无需（温度传感器噪声小）
PID 控制	调整电机输出，稳定速度	控制加热设备，稳定温度

实际使用时，需根据硬件特性调整参数（如 PID 的 Kp/Ki/Kd，卡尔曼的 Q/R），可通过 “先调 P，再调 I，最后调 D” 的方式优化效果。

额外小知识：

在自动化控制和状态估计领域，PID、卡尔曼滤波的使用场景可根据 “是否需要控制”“是否需要处理噪声” 来划分，总结如下：

一、单用 PID：只需要 “稳定控制”，且状态测量准确

核心场景：系统状态能被直接、准确测量（噪声小），重点是让系统稳定在目标值。

适用条件：
- 传感器数据可靠（如高精度温度传感器、无噪声的转速计），不需要复杂的状态估计；
- 核心需求是 “通过控制消除误差”（如让温度、速度、位置稳定在目标）。
例子：
- 家用热水器控温：温度传感器读数稳定（噪声小），PID 直接根据 “目标 50℃” 和 “当前 48℃” 的误差调节加热功率；
- 简易电机调速：电机转速通过编码器直接读取（无明显噪声），PID 控制电压让转速稳定在 1000 转 / 分。

二、单用卡尔曼滤波：只需要 “准确测量”，不需要控制

核心场景：系统状态测量受噪声干扰（数据不准），但不需要控制，只需得到真实状态。

适用条件：
- 传感器数据噪声大（如 GPS 跳变、加速度计抖动），需要过滤噪声或融合多源数据；
- 核心需求是 “从噪声中提取真实状态”（如定位、测速、测角度），不涉及控制决策。
例子：
- 手机计步：加速度计数据因走路颠簸抖动，卡尔曼滤波过滤噪声，得到真实的步伐加速度；
- 无人机姿态显示：陀螺仪数据会漂移，卡尔曼滤波融合加速度计数据，输出稳定的角度（仅用于显示，不用于控制）。

三、结合使用 PID 和卡尔曼滤波：既需要 “准确测量”，又需要 “稳定控制”

核心场景：系统状态测量不准（噪声大），且需要基于状态进行控制（消除误差）。此时卡尔曼滤波为 PID 提供 “干净的状态数据”，PID 用这些数据实现精准控制。

适用条件：
- 传感器数据噪声大或存在累积误差（如 GPS、陀螺仪、低成本传感器）；
- 控制效果严重依赖状态测量的准确性（状态不准会导致控制失效或震荡）。
例子：
- 无人机悬停：
  - 气压计测高度时数据波动大（噪声），陀螺仪测角度时存在漂移，卡尔曼滤波融合两者得到真实高度和角度；
  - PID 基于 “真实高度” 和 “目标高度 10 米” 的误差，控制电机转速，实现稳定悬停。
- 自动驾驶车辆速度控制：
  - GPS 测速跳变（噪声），轮速传感器受打滑影响（误差），卡尔曼滤波估计真实车速；
  - PID 根据 “真实车速” 和 “目标 60km/h” 的误差，调节油门 / 刹车，避免因测速不准导致的急加速 / 急减速。