增加两档PID控制，针对流量误差大和小的情况分别控制，在误差大时快速调节，误差小时慢慢调节以减少震荡和稳态误差。

2026-08-21 17:25:30 +08:00
parent c56858ab0f
commit 26d4cbc316
21 changed files with 14662 additions and 14 deletions
@@ -198,7 +198,7 @@ CSV 同时记录 `is_ratio_smaller_than_0.528`：压力比小于临界值 `0.528
 - 流量超出 `FLOW_VALID_MIN_SLM~FLOW_VALID_MAX_SLM` 或压力高于 `MAX_PRESSURE_KPA` 时停止控制并报故障；
 - 短暂的流量/压力读取失败保持上一输出，不下发新位置；连续达到各自阈值后停止控制；
 - 实际控制周期超过 `MAX_CONTROL_DT_S` 时停止控制；
- PID 输出限制在 `0~100%`，`MAX_OPENING_RATE_PCT_S` 限制每秒最大开度变化；
+- PID 输出限制在 `0~100%`；FAR/NEAR 两档分别限制每秒最大开度变化；
 - 电机命令写入异常或返回失败时停止控制；
 - 所有退出路径都会尝试断开设备，但当前闭环代码不会主动下发关阀命令。

@@ -221,9 +221,13 @@ CSV 同时记录 `is_ratio_smaller_than_0.528`：压力比小于临界值 `0.528
 | `TARGET_FLOW_MIN_SLM` / `TARGET_FLOW_MAX_SLM` | `0` / `300` | 允许的目标范围 |
 | `TARGET_SWITCH_KEY` | `s` | 运行中按此键进入"输入新目标流量"模式 |
 | `CONTROL_PERIOD_S` / `MAX_CONTROL_DT_S` | `0.1` / `0.3` | 目标周期 / 超时阈值 |
-| `PID_KP` / `PID_KI` / `PID_KD` | `0.5` / `0.5` / `0.0` | 当前 PI 参数，需按现场继续整定 |
+| `PID_FAR_KP` / `PID_FAR_KI` / `PID_FAR_KD` | `0.5` / `0.5` / `0.0` | 大误差快速模式 PID 参数 |
+| `PID_NEAR_KP` / `PID_NEAR_KI` / `PID_NEAR_KD` | `0.05` / `0.01` / `0.0` | 小误差稳定模式 PID 参数 |
 | `OPENING_MIN_PCT` / `OPENING_MAX_PCT` | `0` / `100` | PID 输出开度范围 |
-| `MAX_OPENING_RATE_PCT_S` | `20` | 每秒最大开度变化百分比 |
+| `MAX_OPENING_RATE_FAR_PCT_S` / `MAX_OPENING_RATE_NEAR_PCT_S` | `20` / `2` | FAR/NEAR 每秒最大开度变化百分比 |
+| `PID_NEAR_ERROR_MIN_SLM` / `PID_NEAR_ERROR_TARGET_RATIO` | `3` / `0.05` | FAR 进入 NEAR 的绝对/相对误差阈值 |
+| `PID_FAR_ERROR_MIN_SLM` / `PID_FAR_ERROR_TARGET_RATIO` | `12` / `0.20` | NEAR 返回 FAR 的绝对/相对误差阈值 |
+| `PID_MODE_SWITCH_CONFIRM_CYCLES` | `3` | 模式切换条件需连续满足的周期数 |
 | `MOTOR_CLOSED_POSITION` / `MOTOR_OPEN_POSITION` | `1000` / `0` | 反向行程端值 |
 | `INITIAL_OPENING_PCT` | `100` | 闭环 PID 内部初始输出 |
 | `FLOW_VALID_MIN_SLM` / `FLOW_VALID_MAX_SLM` | `0` / `300` | 流量安全范围 |
@@ -38,14 +38,27 @@ STATUS_PRINT_PERIOD_S = 1.0
 # PID：输出统一定义为阀门开度百分比
 # ---------------------------------------------------------------------------

-# 第一版建议先按 PI 使用（KD=0），然后通过阶跃实验重新整定。
-PID_KP = 0.5
-PID_KI = 0.5
-PID_KD = 0.0
+# 大误差时使用 FAR 参数快速接近目标；小误差时使用 NEAR 参数抑制振荡。
+# 两档之间采用带滞回的状态切换，不会逐周期连续改变 PID 参数。
+PID_FAR_KP = 0.5
+PID_FAR_KI = 0.5
+PID_FAR_KD = 0.0
+PID_NEAR_KP = 0.05
+PID_NEAR_KI = 0.01
+PID_NEAR_KD = 0.0

 OPENING_MIN_PCT = 0.0
 OPENING_MAX_PCT = 100.0
-MAX_OPENING_RATE_PCT_S = 20.0    # 每秒最多改变 20% 开度
+MAX_OPENING_RATE_FAR_PCT_S = 20.0
+MAX_OPENING_RATE_NEAR_PCT_S = 2.0
+
+# FAR -> NEAR：绝对误差连续满足 near 阈值指定周期数后切换。
+# NEAR -> FAR：绝对误差连续达到 far 阈值指定周期数后切换。
+PID_NEAR_ERROR_MIN_SLM = 3.0
+PID_NEAR_ERROR_TARGET_RATIO = 0.05
+PID_FAR_ERROR_MIN_SLM = 12.0
+PID_FAR_ERROR_TARGET_RATIO = 0.20
+PID_MODE_SWITCH_CONFIRM_CYCLES = 3

 # ---------------------------------------------------------------------------
 # 稳态判定（用于控制性能报告）
@@ -116,8 +129,35 @@ def validate_config():
        raise ValueError("本系统行程越大开度越小，因此打开端行程必须小于关闭端行程")
    if MOTOR_OPEN_POSITION < 0 or MOTOR_CLOSED_POSITION > 1000:
        raise ValueError("电机行程必须位于 MT2AM8Client 当前采用的 0~1000 范围")
-    if MAX_OPENING_RATE_PCT_S <= 0:
-        raise ValueError("MAX_OPENING_RATE_PCT_S 必须大于 0")
+    pid_gains = (
+        PID_FAR_KP,
+        PID_FAR_KI,
+        PID_FAR_KD,
+        PID_NEAR_KP,
+        PID_NEAR_KI,
+        PID_NEAR_KD,
+    )
+    if any(value < 0 for value in pid_gains):
+        raise ValueError("FAR/NEAR PID 参数不得为负数")
+    if (
+        MAX_OPENING_RATE_FAR_PCT_S <= 0
+        or MAX_OPENING_RATE_NEAR_PCT_S <= 0
+    ):
+        raise ValueError("FAR/NEAR 最大开度变化速度必须大于 0")
+    if PID_NEAR_ERROR_MIN_SLM < 0 or PID_FAR_ERROR_MIN_SLM < 0:
+        raise ValueError("PID 模式切换误差下限不得为负数")
+    if (
+        PID_NEAR_ERROR_TARGET_RATIO < 0
+        or PID_FAR_ERROR_TARGET_RATIO < 0
+    ):
+        raise ValueError("PID 模式切换目标比例不得为负数")
+    if (
+        PID_NEAR_ERROR_MIN_SLM >= PID_FAR_ERROR_MIN_SLM
+        or PID_NEAR_ERROR_TARGET_RATIO >= PID_FAR_ERROR_TARGET_RATIO
+    ):
+        raise ValueError("PID NEAR 阈值必须小于 FAR 阈值")
+    if PID_MODE_SWITCH_CONFIRM_CYCLES < 1:
+        raise ValueError("PID_MODE_SWITCH_CONFIRM_CYCLES 必须至少为 1")
    if MAX_CONSECUTIVE_FLOW_FAILURES < 1:
        raise ValueError("MAX_CONSECUTIVE_FLOW_FAILURES 必须至少为 1")
    if PRESSURE_INPUT_CHANNEL is not None and MAX_PRESSURE_KPA is None:
@@ -89,6 +89,15 @@ class FlowControlLoop:
        max_control_dt_s=None,
        max_consecutive_flow_failures=3,
        max_consecutive_pressure_failures=3,
+        pid_far_gains=(0.5, 0.5, 0.0),
+        pid_near_gains=(0.05, 0.01, 0.0),
+        opening_rate_far_pct_s=20.0,
+        opening_rate_near_pct_s=2.0,
+        pid_near_error_min_slm=3.0,
+        pid_near_error_target_ratio=0.05,
+        pid_far_error_min_slm=12.0,
+        pid_far_error_target_ratio=0.20,
+        pid_mode_switch_confirm_cycles=3,
        logger=None,
    ):
        if period_s <= 0:
@@ -100,6 +109,46 @@ class FlowControlLoop:
        if pressure_channel is not None and max_pressure_kpa is None:
            raise ValueError("启用压力监测时必须设置 max_pressure_kpa")

+        far_gains = tuple(float(value) for value in pid_far_gains)
+        near_gains = tuple(float(value) for value in pid_near_gains)
+        if len(far_gains) != 3 or len(near_gains) != 3:
+            raise ValueError("FAR/NEAR PID 参数必须各包含 Kp、Ki、Kd")
+        if any(
+            not math.isfinite(value) or value < 0
+            for value in far_gains + near_gains
+        ):
+            raise ValueError("FAR/NEAR PID 参数必须是非负有限数值")
+        far_rate = float(opening_rate_far_pct_s)
+        near_rate = float(opening_rate_near_pct_s)
+        if (
+            not math.isfinite(far_rate)
+            or not math.isfinite(near_rate)
+            or far_rate <= 0
+            or near_rate <= 0
+        ):
+            raise ValueError("FAR/NEAR 最大开度变化速度必须大于 0")
+        near_error_min = float(pid_near_error_min_slm)
+        far_error_min = float(pid_far_error_min_slm)
+        near_error_ratio = float(pid_near_error_target_ratio)
+        far_error_ratio = float(pid_far_error_target_ratio)
+        if any(
+            not math.isfinite(value) or value < 0
+            for value in (
+                near_error_min,
+                far_error_min,
+                near_error_ratio,
+                far_error_ratio,
+            )
+        ):
+            raise ValueError("PID 模式切换阈值必须是非负有限数值")
+        if near_error_min >= far_error_min:
+            raise ValueError("PID NEAR 绝对误差阈值必须小于 FAR 阈值")
+        if near_error_ratio >= far_error_ratio:
+            raise ValueError("PID NEAR 相对误差阈值必须小于 FAR 阈值")
+        confirm_cycles = int(pid_mode_switch_confirm_cycles)
+        if confirm_cycles != pid_mode_switch_confirm_cycles or confirm_cycles < 1:
+            raise ValueError("PID 模式切换确认周期数必须至少为 1")
+
        self.hardware = hardware
        self.pid = pid
        self.period_s = float(period_s)
@@ -125,6 +174,15 @@ class FlowControlLoop:
        self.max_consecutive_pressure_failures = int(
            max_consecutive_pressure_failures
        )
+        self.pid_far_gains = far_gains
+        self.pid_near_gains = near_gains
+        self.opening_rate_far_pct_s = far_rate
+        self.opening_rate_near_pct_s = near_rate
+        self.pid_near_error_min_slm = near_error_min
+        self.pid_near_error_target_ratio = near_error_ratio
+        self.pid_far_error_min_slm = far_error_min
+        self.pid_far_error_target_ratio = far_error_ratio
+        self.pid_mode_switch_confirm_cycles = confirm_cycles
        self.logger = logger

        self.target_flow_slm = 0.0
@@ -137,6 +195,9 @@ class FlowControlLoop:
        self.pressure_failure_count = 0
        self.running = False
        self.faulted = False
+        self.pid_mode = "FAR"
+        self._pid_mode_confirm_count = 0
+        self._apply_pid_mode("FAR", force=True, log_change=False)

    def set_target_flow(self, target_flow_slm):
        """设置目标流量；超出允许范围时拒绝，而不是静默截断。"""
@@ -148,11 +209,15 @@ class FlowControlLoop:
                f"目标流量 {target} SLM 超出 "
                f"{self.target_min_slm}~{self.target_max_slm} SLM"
            )
+        target_changed = target != self.target_flow_slm
        self.target_flow_slm = target
+        if target_changed:
+            self._apply_pid_mode("FAR", reason="目标流量切换")

    def start(self, initial_opening=100.0):
        """复位状态并启动控制；默认从全开开度开始。"""
        initial_opening = self._bounded_opening(initial_opening)
+        self._apply_pid_mode("FAR", reason="控制启动")
        self.pid.reset(initial_output=initial_opening)
        self.last_opening_pct = initial_opening
        self.last_motor_position = opening_to_motor_position(
@@ -252,6 +317,7 @@ class FlowControlLoop:
            self.pid.reset(initial_output=0.0)
            opening = 0.0
        else:
+            self._update_pid_mode(flow)
            opening = self.pid.update(
                measurement=flow,
                setpoint=self.target_flow_slm,
@@ -287,6 +353,86 @@ class FlowControlLoop:
            actual_dt_s=actual_dt,
        )

+    def _update_pid_mode(self, flow):
+        """按误差滞回切换 FAR/NEAR；参数只在模式变化时更新一次。"""
+        error_abs = abs(self.target_flow_slm - flow)
+        near_threshold = max(
+            self.pid_near_error_min_slm,
+            self.pid_near_error_target_ratio * self.target_flow_slm,
+        )
+        far_threshold = max(
+            self.pid_far_error_min_slm,
+            self.pid_far_error_target_ratio * self.target_flow_slm,
+        )
+
+        if self.pid_mode == "FAR":
+            condition_met = error_abs <= near_threshold
+            next_mode = "NEAR"
+            threshold = near_threshold
+        else:
+            condition_met = error_abs >= far_threshold
+            next_mode = "FAR"
+            threshold = far_threshold
+
+        if not condition_met:
+            self._pid_mode_confirm_count = 0
+            return
+
+        self._pid_mode_confirm_count += 1
+        if self._pid_mode_confirm_count < self.pid_mode_switch_confirm_cycles:
+            return
+
+        comparison = "<=" if next_mode == "NEAR" else ">="
+        self._apply_pid_mode(
+            next_mode,
+            reason=(
+                f"|误差|={error_abs:.3f} SLM {comparison} "
+                f"阈值={threshold:.3f} SLM，连续 "
+                f"{self.pid_mode_switch_confirm_cycles} 个周期"
+            ),
+        )
+
+    def _apply_pid_mode(
+        self,
+        mode,
+        *,
+        reason=None,
+        force=False,
+        log_change=True,
+    ):
+        if mode not in ("FAR", "NEAR"):
+            raise ValueError(f"未知 PID 模式: {mode!r}")
+        self._pid_mode_confirm_count = 0
+        if not force and mode == self.pid_mode:
+            return False
+
+        previous_mode = self.pid_mode
+        if mode == "FAR":
+            gains = self.pid_far_gains
+            opening_rate = self.opening_rate_far_pct_s
+        else:
+            gains = self.pid_near_gains
+            opening_rate = self.opening_rate_near_pct_s
+
+        self.pid.update_parameters(*gains)
+        self.pid.set_output_rate_limit(opening_rate)
+        self.pid_mode = mode
+        if log_change:
+            reason_text = "" if reason is None else f"；原因={reason}"
+            self._log(
+                "info",
+                "PID 模式 %s -> %s：Kp=%.3f Ki=%.3f Kd=%.3f "
+                "最大开度速度=%.3f%%/s%s",
+                previous_mode,
+                mode,
+                gains[0],
+                gains[1],
+                gains[2],
+                opening_rate,
+                reason_text,
+            )
+        return True
+
    def _read_flow(self):
        try:
            value = self.hardware.get_flow(self.flow_channel)
@@ -400,6 +546,7 @@ class FlowControlLoop:
            "device_connected": bool(getattr(self.hardware, "connected", False)),
            "flow_failure_count": self.flow_failure_count,
            "pressure_failure_count": self.pressure_failure_count,
+            "pid_mode": self.pid_mode,
        }

    def _bounded_opening(self, opening):
@@ -63,13 +63,13 @@ def build_logger():

 def build_control_loop(hardware, logger):
    pid = IncrementalPID(
-        kp=config.PID_KP,
-        ki=config.PID_KI,
-        kd=config.PID_KD,
+        kp=config.PID_FAR_KP,
+        ki=config.PID_FAR_KI,
+        kd=config.PID_FAR_KD,
        dt=config.CONTROL_PERIOD_S,
        out_min=config.OPENING_MIN_PCT,
        out_max=config.OPENING_MAX_PCT,
-        output_rate_limit=config.MAX_OPENING_RATE_PCT_S,
+        output_rate_limit=config.MAX_OPENING_RATE_FAR_PCT_S,
    )
    return FlowControlLoop(
        hardware=hardware,
@@ -91,6 +91,27 @@ def build_control_loop(hardware, logger):
        max_consecutive_pressure_failures=(
            config.MAX_CONSECUTIVE_PRESSURE_FAILURES
        ),
+        pid_far_gains=(
+            config.PID_FAR_KP,
+            config.PID_FAR_KI,
+            config.PID_FAR_KD,
+        ),
+        pid_near_gains=(
+            config.PID_NEAR_KP,
+            config.PID_NEAR_KI,
+            config.PID_NEAR_KD,
+        ),
+        opening_rate_far_pct_s=config.MAX_OPENING_RATE_FAR_PCT_S,
+        opening_rate_near_pct_s=config.MAX_OPENING_RATE_NEAR_PCT_S,
+        pid_near_error_min_slm=config.PID_NEAR_ERROR_MIN_SLM,
+        pid_near_error_target_ratio=(
+            config.PID_NEAR_ERROR_TARGET_RATIO
+        ),
+        pid_far_error_min_slm=config.PID_FAR_ERROR_MIN_SLM,
+        pid_far_error_target_ratio=config.PID_FAR_ERROR_TARGET_RATIO,
+        pid_mode_switch_confirm_cycles=(
+            config.PID_MODE_SWITCH_CONFIRM_CYCLES
+        ),
        logger=logger,
    )