refactor: 重构架构并整理仓库
This commit is contained in:
Horldsence
@@ -0,0 +1,16 @@
|
||||
[package]
|
||||
name = "pid"
|
||||
description = "PID controller"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,272 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 通用 PID 控制器。
|
||||
|
||||
/// 并联式 PID 控制器（f32，显式时间步）。
|
||||
///
|
||||
/// 特性：
|
||||
/// - 输出限幅 `[out_min, out_max]`；
|
||||
/// - 积分抗饱和（clamping：输出已饱和且误差继续同向时暂停积分）；
|
||||
/// - 微分作用于**测量值**（避免设定值阶跃的微分冲击），可选一阶低通滤波。
|
||||
///
|
||||
/// 时间步 `dt` 由调用方显式传入（秒）。`dt <= 0` 或非有限值时本次调用
|
||||
/// 直接返回上次输出且不更新内部状态（防御跳帧 / 首帧场景）。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Pid {
|
||||
kp: f32,
|
||||
ki: f32,
|
||||
kd: f32,
|
||||
out_min: f32,
|
||||
out_max: f32,
|
||||
/// 微分低通时间常数（秒）；`0` 关闭滤波。
|
||||
derivative_tau: f32,
|
||||
integral: f32,
|
||||
prev_measurement: Option<f32>,
|
||||
derivative_filtered: f32,
|
||||
last_output: f32,
|
||||
}
|
||||
|
||||
impl Pid {
|
||||
/// 创建 PID。`out_min` 必须小于 `out_max`。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// 参数非有限或 `out_min >= out_max` 时 panic。
|
||||
#[must_use]
|
||||
pub fn new(kp: f32, ki: f32, kd: f32, out_min: f32, out_max: f32) -> Self {
|
||||
assert!(
|
||||
out_min < out_max && [kp, ki, kd, out_min, out_max].iter().all(|v| v.is_finite()),
|
||||
"invalid PID parameters"
|
||||
);
|
||||
Self {
|
||||
kp,
|
||||
ki,
|
||||
kd,
|
||||
out_min,
|
||||
out_max,
|
||||
derivative_tau: 0.0,
|
||||
integral: 0.0,
|
||||
prev_measurement: None,
|
||||
derivative_filtered: 0.0,
|
||||
last_output: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 设置微分低通时间常数（秒），`0` 关闭滤波。链式调用。
|
||||
#[must_use]
|
||||
pub fn with_derivative_tau(mut self, tau: f32) -> Self {
|
||||
self.derivative_tau = tau.max(0.0);
|
||||
self
|
||||
}
|
||||
|
||||
/// 单步更新，返回限幅后的输出。
|
||||
pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 {
|
||||
// NaN 与 +inf 的比较结果均为 false，故 `dt > 0` 已隐含有限性。
|
||||
let input_valid = dt > 0.0 && setpoint.is_finite() && measurement.is_finite();
|
||||
if !input_valid {
|
||||
return self.last_output;
|
||||
}
|
||||
let error = setpoint - measurement;
|
||||
|
||||
// 微分作用于测量值（反向），一阶低通滤波。
|
||||
let d_raw = match self.prev_measurement {
|
||||
Some(prev) => -(measurement - prev) / dt,
|
||||
None => 0.0,
|
||||
};
|
||||
if self.derivative_tau > 0.0 {
|
||||
let alpha = dt / (self.derivative_tau + dt);
|
||||
self.derivative_filtered += alpha * (d_raw - self.derivative_filtered);
|
||||
} else {
|
||||
self.derivative_filtered = d_raw;
|
||||
}
|
||||
let d_term = self.kd * self.derivative_filtered;
|
||||
|
||||
// 积分抗饱和（clamping）：仅在不会把输出进一步推入饱和时积分。
|
||||
let i_candidate = self.integral + self.ki * error * dt;
|
||||
let unsat = self.kp * error + i_candidate + d_term;
|
||||
let saturating_up = unsat > self.out_max && error > 0.0;
|
||||
let saturating_down = unsat < self.out_min && error < 0.0;
|
||||
if !saturating_up && !saturating_down {
|
||||
self.integral = i_candidate;
|
||||
}
|
||||
|
||||
let output = (self.kp * error + self.integral + d_term).clamp(self.out_min, self.out_max);
|
||||
self.prev_measurement = Some(measurement);
|
||||
self.last_output = output;
|
||||
output
|
||||
}
|
||||
|
||||
/// 清空积分、微分记忆与上次输出。
|
||||
pub fn reset(&mut self) {
|
||||
self.integral = 0.0;
|
||||
self.prev_measurement = None;
|
||||
self.derivative_filtered = 0.0;
|
||||
self.last_output = 0.0;
|
||||
}
|
||||
|
||||
/// 上一次 `update` 的输出（诊断 / 串级外环取速度目标用）。
|
||||
#[must_use]
|
||||
pub fn last_output(&self) -> f32 {
|
||||
self.last_output
|
||||
}
|
||||
|
||||
/// 当前积分累加项。
|
||||
#[must_use]
|
||||
pub fn integral(&self) -> f32 {
|
||||
self.integral
|
||||
}
|
||||
|
||||
/// 比例增益。
|
||||
#[must_use]
|
||||
pub fn kp(&self) -> f32 {
|
||||
self.kp
|
||||
}
|
||||
|
||||
/// 积分增益。
|
||||
#[must_use]
|
||||
pub fn ki(&self) -> f32 {
|
||||
self.ki
|
||||
}
|
||||
|
||||
/// 微分增益。
|
||||
#[must_use]
|
||||
pub fn kd(&self) -> f32 {
|
||||
self.kd
|
||||
}
|
||||
|
||||
/// 输出下限。
|
||||
#[must_use]
|
||||
pub fn output_min(&self) -> f32 {
|
||||
self.out_min
|
||||
}
|
||||
|
||||
/// 输出上限。
|
||||
#[must_use]
|
||||
pub fn output_max(&self) -> f32 {
|
||||
self.out_max
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const DT: f32 = 0.001;
|
||||
|
||||
#[test]
|
||||
fn pure_proportional() {
|
||||
let mut pid = Pid::new(2.0, 0.0, 0.0, -100.0, 100.0);
|
||||
let out = pid.update(10.0, 4.0, DT);
|
||||
assert!((out - 12.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn output_clamps_both_ends() {
|
||||
let mut pid = Pid::new(100.0, 0.0, 0.0, -10.0, 10.0);
|
||||
assert!((pid.update(1.0, 0.0, DT) - 10.0).abs() < 1e-5);
|
||||
assert!((pid.update(-1.0, 0.0, DT) - (-10.0)).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn integral_accumulates() {
|
||||
let mut pid = Pid::new(0.0, 10.0, 0.0, -1e6, 1e6);
|
||||
pid.update(1.0, 0.0, DT); // integral += 10 * 1 * 0.001
|
||||
pid.update(1.0, 0.0, DT);
|
||||
let out = pid.update(1.0, 0.0, DT);
|
||||
let expected = 10.0 * 1.0 * DT * 3.0;
|
||||
assert!((out - expected).abs() < 1e-4, "out = {out}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn anti_windup_bounds_integral_when_saturated() {
|
||||
// 大误差持续饱和 200 步：kp = 0 时输出即积分项。clamping 抗饱和要求
|
||||
// 积分不超过 out_max，且进入保持区后输出不再增长。
|
||||
let ki = 5.0;
|
||||
let mut pid = Pid::new(0.0, ki, 0.0, -16384.0, 16384.0);
|
||||
for _ in 0..200 {
|
||||
pid.update(1e6, 0.0, DT);
|
||||
}
|
||||
assert!(pid.integral() <= 16384.0, "integral = {}", pid.integral());
|
||||
let frozen = pid.update(1e6, 0.0, DT);
|
||||
let still = pid.update(1e6, 0.0, DT);
|
||||
assert!(
|
||||
(frozen - still).abs() < 1e-3,
|
||||
"saturated output must stop growing: {frozen} vs {still}"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn anti_windup_recovers_quickly_on_reverse() {
|
||||
// 饱和后目标反向，输出应在一个反向误差步内脱离正饱和。
|
||||
let mut pid = Pid::new(1.0, 10.0, 0.0, -100.0, 100.0);
|
||||
for _ in 0..500 {
|
||||
pid.update(1000.0, 0.0, DT);
|
||||
}
|
||||
assert!((pid.update(1000.0, 0.0, DT) - 100.0).abs() < 1e-3);
|
||||
// 一步反向大误差（kp=1, e=-1000）足以击穿下限，说明无深度饱和。
|
||||
let out = pid.update(-1000.0, 0.0, DT);
|
||||
assert!(out <= 0.0, "expected non-positive output, got {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn derivative_acts_on_measurement() {
|
||||
// 设定值恒 0，测量值阶跃：输出应为负向微分冲击（-kd*Δ/dt）。
|
||||
let mut pid = Pid::new(0.0, 0.0, 0.01, -1e6, 1e6);
|
||||
pid.update(0.0, 0.0, DT); // 建立初值，首帧微分为 0
|
||||
let out = pid.update(0.0, 0.5, DT);
|
||||
let expected = -0.01 * (0.5 / DT);
|
||||
assert!((out - expected).abs() < 1.0, "out = {out}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn derivative_filter_softens_spike() {
|
||||
let step = |tau: f32| {
|
||||
let mut pid = Pid::new(0.0, 0.0, 10.0, -1e9, 1e9).with_derivative_tau(tau);
|
||||
pid.update(0.0, 0.0, DT);
|
||||
pid.update(0.0, 1.0, DT).abs()
|
||||
};
|
||||
let unfiltered = step(0.0);
|
||||
let filtered = step(0.05);
|
||||
assert!(
|
||||
filtered < unfiltered * 0.05,
|
||||
"filtered {filtered} should be far below unfiltered {unfiltered}"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nonpositive_dt_returns_last_output_and_keeps_state() {
|
||||
let mut pid = Pid::new(1.0, 1.0, 0.0, -100.0, 100.0);
|
||||
let first = pid.update(10.0, 0.0, DT);
|
||||
let integral_before = pid.integral().to_bits();
|
||||
// 逐位相等：无效输入必须原样返回上次输出。
|
||||
assert_eq!(pid.update(99.0, 0.0, 0.0).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.update(99.0, 0.0, f32::NAN).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.integral().to_bits(), integral_before);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nan_input_returns_last_output() {
|
||||
let mut pid = Pid::new(1.0, 0.0, 0.0, -100.0, 100.0);
|
||||
let first = pid.update(1.0, 0.0, DT);
|
||||
assert_eq!(pid.update(f32::NAN, 0.0, DT).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.update(1.0, f32::INFINITY, DT).to_bits(), first.to_bits());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_history() {
|
||||
let mut pid = Pid::new(1.0, 10.0, 1.0, -1e6, 1e6);
|
||||
pid.update(5.0, 1.0, DT);
|
||||
pid.update(5.0, 1.0, DT);
|
||||
pid.reset();
|
||||
assert_eq!(pid.integral().to_bits(), 0.0f32.to_bits());
|
||||
// 重置后首帧微分为 0，输出 = P 项 + 本步积分（ki*e*dt = 0.04）。
|
||||
let out = pid.update(5.0, 1.0, DT);
|
||||
assert!((out - 4.04).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic = "invalid PID parameters"]
|
||||
fn new_rejects_inverted_limits() {
|
||||
let _pid = Pid::new(1.0, 0.0, 0.0, 1.0, -1.0);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user