louis/flow_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

实现流量控制前馈 + 增益调度（MFC 结构）

Browse Source 
按 plan.md 新增三个交付物，全部默认关闭以保持与纯 PID 的 A/B 兼容：

- valve_model.py：阀特性模型 Q_ss = A_eff(x)·P1_abs·F(r)，含 ISO 6358
  椭圆 F(r)、行程/面积互逆插值、单调性校验、JSON 存取。
- identify_valve.py：从 open_loop.py 扫点 CSV 拟合 A_eff 表并输出 JSON。
- 控制器集成：config/flow_control/main/data_logger 新增阀前压读取
  （channel 2）、前馈打底 + PI 修残差、按 P1 增益调度，并记录/绘制前馈项。

新增配置 FEEDFORWARD_ENABLED / GAIN_SCHEDULE_ENABLED 默认 False，
VALVE_MODEL_PATH 默认空，未加载模型时行为与原先纯 PID 完全一致。
This commit is contained in:
louis
2026-08-27 16:00:51 +08:00
parent d7e712ede7
commit a03735387c
7 changed files with 958 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
flow_control.py
+243 -15
View File
@@ -10,6 +10,10 @@ import time
from typing import Optional
# 标准大气压（kPa）。增益调度把表压换算成绝对压力时使用。
_P_ATM_KPA = 101.325
@dataclass
class ControlStepResult:
"""一个控制周期的可记录结果。"""
@@ -23,6 +27,10 @@ class ControlStepResult:
motor_position: float
actual_dt_s: float
status: str = "OK"
pressure_before_kpa: Optional[float] = None
feedforward_pct: Optional[float] = None
correction_pct: Optional[float] = None
gain_scale: Optional[float] = None
def to_dict(self):
return asdict(self)
@@ -89,6 +97,14 @@ class FlowControlLoop:
max_control_dt_s=None,
max_consecutive_flow_failures=3,
max_consecutive_pressure_failures=3,
pressure_before_channel=None,
valve_model=None,
feedforward_enabled=False,
feedforward_correction_band_pct=20.0,
gain_schedule_enabled=False,
gain_schedule_ref_abs_kpa=501.325,
gain_schedule_floor_abs_kpa=60.0,
gain_schedule_scale_change_threshold=0.05,
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,
@@ -174,6 +190,53 @@ class FlowControlLoop:
self.max_consecutive_pressure_failures = int(
max_consecutive_pressure_failures
)
self.pressure_before_channel = (
None if pressure_before_channel is None else int(pressure_before_channel)
)
if self.pressure_before_channel is not None and self.max_pressure_kpa is None:
raise ValueError("启用阀前压力监测时必须设置 max_pressure_kpa")
self.valve_model = valve_model
self.feedforward_enabled = bool(feedforward_enabled)
feedforward_band = float(feedforward_correction_band_pct)
if not math.isfinite(feedforward_band) or not 0.0 < feedforward_band <= 100.0:
raise ValueError("feedforward_correction_band_pct 必须位于 0~100")
self.feedforward_correction_band_pct = feedforward_band
self.gain_schedule_enabled = bool(gain_schedule_enabled)
self.gain_schedule_ref_abs_kpa = float(gain_schedule_ref_abs_kpa)
self.gain_schedule_floor_abs_kpa = float(gain_schedule_floor_abs_kpa)
if not math.isfinite(self.gain_schedule_ref_abs_kpa) or self.gain_schedule_ref_abs_kpa <= 0.0:
raise ValueError("gain_schedule_ref_abs_kpa 必须大于 0")
if not math.isfinite(self.gain_schedule_floor_abs_kpa) or self.gain_schedule_floor_abs_kpa <= 0.0:
raise ValueError("gain_schedule_floor_abs_kpa 必须大于 0")
self.gain_schedule_scale_change_threshold = float(
gain_schedule_scale_change_threshold
)
if (
not math.isfinite(self.gain_schedule_scale_change_threshold)
or self.gain_schedule_scale_change_threshold <= 0.0
):
raise ValueError("gain_schedule_scale_change_threshold 必须大于 0")
# 前馈需要阀特性表 + 阀前压力 P1 + 阀后压力 P2 三样齐全才生效。
self._feedforward_active = bool(
self.feedforward_enabled
and self.valve_model is not None
and self.pressure_before_channel is not None
and self.pressure_channel is not None
)
# 前馈开启时 PID 输出是“围绕前馈的修正量”，限幅改为对称小范围。
if self._feedforward_active:
self.pid.out_min = -self.feedforward_correction_band_pct
self.pid.out_max = self.feedforward_correction_band_pct
# 增益调度当前 scale 与当前档位基础增益，供 _apply_effective_gains 使用。
self._current_scale = 1.0
self._current_base_gains = far_gains
self._last_feedforward_opening = None
self.pid_far_gains = far_gains
self.pid_near_gains = near_gains
self.opening_rate_far_pct_s = far_rate
@@ -188,11 +251,13 @@ class FlowControlLoop:
self.target_flow_slm = 0.0
self.last_flow_slm = None
self.last_pressure_kpa = None
self.last_pressure_before_kpa = None
self.last_opening_pct = 0.0
self.last_motor_position = self.motor_closed_position
self.last_step_time = None
self.flow_failure_count = 0
self.pressure_failure_count = 0
self.pressure_before_failure_count = 0
self.running = False
self.faulted = False
self.pid_mode = "FAR"
@@ -243,7 +308,12 @@ class FlowControlLoop:
"""复位状态并启动控制；默认从全开开度开始。"""
initial_opening = self._bounded_opening(initial_opening)
self._apply_pid_mode("FAR", reason="控制启动")
self.pid.reset(initial_output=initial_opening)
if self._feedforward_active:
# 前馈打底，PID 从零修正量开始。
self.pid.reset(initial_output=0.0)
self._last_feedforward_opening = None
else:
self.pid.reset(initial_output=initial_opening)
self.last_opening_pct = initial_opening
self.last_motor_position = opening_to_motor_position(
initial_opening,
@@ -253,6 +323,7 @@ class FlowControlLoop:
self.last_step_time = None
self.flow_failure_count = 0
self.pressure_failure_count = 0
self.pressure_before_failure_count = 0
self.faulted = False
self.running = True
@@ -304,19 +375,29 @@ class FlowControlLoop:
flow = self._read_flow()
pressure = self._read_pressure()
pressure_before = self._read_pressure_before()
if flow is None:
return self._held_result(
actual_dt,
pressure,
pressure_before,
f"FLOW_READ_RETRY_{self.flow_failure_count}",
)
if self.pressure_channel is not None and pressure is None:
return self._held_result(
actual_dt,
pressure,
pressure_before,
f"PRESSURE_READ_RETRY_{self.pressure_failure_count}",
)
if self.pressure_before_channel is not None and pressure_before is None:
return self._held_result(
actual_dt,
pressure,
pressure_before,
f"PRESSURE_BEFORE_READ_RETRY_{self.pressure_before_failure_count}",
)
if not self.flow_valid_min_slm <= flow <= self.flow_valid_max_slm:
self._trip(
@@ -337,24 +418,70 @@ class FlowControlLoop:
f"{self.max_pressure_kpa:.3f} kPa",
{"pressure_kpa": pressure},
)
if (
pressure_before is not None
and self.max_pressure_kpa is not None
and pressure_before > self.max_pressure_kpa
):
self._trip(
"PRESSURE_OVER_LIMIT",
"PRESSURE_SAFETY_CHECK",
f"阀前压力 {pressure_before:.3f} kPa 超过上限 "
f"{self.max_pressure_kpa:.3f} kPa",
{"pressure_before_kpa": pressure_before},
)
if self.target_flow_slm <= self.zero_flow_threshold_slm:
self.pid.reset(initial_output=0.0)
opening = 0.0
opening_ff = None
correction = None
scale = self._current_scale
else:
scale = self._apply_gain_schedule(pressure_before)
self._maybe_update_gain_schedule(scale)
self._update_pid_mode(flow)
opening = self.pid.update(
measurement=flow,
setpoint=self.target_flow_slm,
dt=actual_dt,
)
if not self._is_finite_number(opening):
self._trip(
"PID_OUTPUT_INVALID",
"PID_UPDATE",
f"PID 输出不是有限数值: {opening!r}",
opening_ff = None
correction = None
if self._feedforward_active:
opening_ff = self._compute_feedforward_opening(
self.target_flow_slm, pressure_before, pressure
)
opening = self._bounded_opening(opening)
if opening_ff is not None:
self._last_feedforward_opening = opening_ff
base = (
opening_ff
if opening_ff is not None
else self._last_feedforward_opening
)
if base is None:
base = 0.0
correction = self.pid.update(
measurement=flow,
setpoint=self.target_flow_slm,
dt=actual_dt,
)
if not self._is_finite_number(correction):
self._trip(
"PID_OUTPUT_INVALID",
"PID_UPDATE",
f"PID 修正量不是有限数值: {correction!r}",
)
opening = self._bounded_opening(base + correction)
else:
opening = self.pid.update(
measurement=flow,
setpoint=self.target_flow_slm,
dt=actual_dt,
)
if not self._is_finite_number(opening):
self._trip(
"PID_OUTPUT_INVALID",
"PID_UPDATE",
f"PID 输出不是有限数值: {opening!r}",
)
opening = self._bounded_opening(opening)
position = opening_to_motor_position(
opening,
@@ -365,6 +492,7 @@ class FlowControlLoop:
self.last_flow_slm = flow
self.last_pressure_kpa = pressure
self.last_pressure_before_kpa = pressure_before
self.last_opening_pct = opening
self.last_motor_position = position
return ControlStepResult(
@@ -376,6 +504,10 @@ class FlowControlLoop:
opening_pct=opening,
motor_position=position,
actual_dt_s=actual_dt,
pressure_before_kpa=pressure_before,
feedforward_pct=opening_ff,
correction_pct=correction,
gain_scale=scale,
)
def _update_pid_mode(self, flow):
@@ -445,20 +577,25 @@ class FlowControlLoop:
gains = self.pid_near_gains
opening_rate = self.opening_rate_near_pct_s
self.pid.update_parameters(*gains)
self._current_base_gains = gains
self._apply_effective_gains(self._current_scale)
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 "
"PID 模式 %s -> %s：基础 Kp=%.3f Ki=%.3f Kd=%.3f，"
"有效 Kp=%.3f Ki=%.3f（scale=%.3f），"
"最大开度速度=%.3f%%/s%s",
previous_mode,
mode,
gains[0],
gains[1],
gains[2],
self.pid.kp,
self.pid.ki,
self._current_scale,
opening_rate,
reason_text,
)
@@ -490,11 +627,99 @@ class FlowControlLoop:
self.pressure_failure_count = 0
return float(value)
def _read_pressure_before(self):
if self.pressure_before_channel is None:
return None
try:
value = self.hardware.get_pressure(self.pressure_before_channel)
except Exception as exc:
self._register_read_failure("PRESSURE_BEFORE", exc)
return None
if value is None or not self._is_finite_number(value):
self._register_read_failure(
"PRESSURE_BEFORE", f"invalid value: {value!r}"
)
return None
self.pressure_before_failure_count = 0
return float(value)
def _compute_feedforward_opening(self, q_set, p1, p2):
"""计算前馈开度（0~100%）；无法计算时返回 None（退化纯 PID）。"""
if self.valve_model is None or not self.feedforward_enabled:
return None
if not self._is_finite_number(q_set):
return None
if p1 is None or p2 is None:
return None
if not self._is_finite_number(p1) or not self._is_finite_number(p2):
return None
# 近零流量：直接全关，不依赖反解（阀门可能不严）。
if q_set <= self.zero_flow_threshold_slm:
return 0.0
try:
x_ff = self.valve_model.feedforward_stroke(
float(q_set), float(p1), float(p2)
)
except (ValueError, ZeroDivisionError, TypeError):
return None
if not self._is_finite_number(x_ff):
return None
span = self.motor_closed_position - self.motor_open_position
if span <= 0.0:
return None
opening_ff = 100.0 * (self.motor_closed_position - x_ff) / span
return self._bounded_opening(opening_ff)
def _apply_gain_schedule(self, p1):
"""按阀前压力计算增益缩放系数；关闭或 P1 无效时返回 1.0。"""
if not self.gain_schedule_enabled:
return 1.0
if p1 is None or not self._is_finite_number(p1):
return 1.0
p1_abs = float(p1) + _P_ATM_KPA
denominator = max(p1_abs, self.gain_schedule_floor_abs_kpa)
if denominator <= 0.0 or not math.isfinite(denominator):
return 1.0
scale = self.gain_schedule_ref_abs_kpa / denominator
if not math.isfinite(scale) or scale <= 0.0:
return 1.0
return scale
def _maybe_update_gain_schedule(self, scale):
"""scale 相对变化超过阈值时，重算并写入有效增益。"""
if scale is None or not math.isfinite(scale):
return
old_scale = self._current_scale
if old_scale is None or old_scale <= 0.0:
self._current_scale = scale
self._apply_effective_gains(scale)
return
relative_change = abs(scale - old_scale) / old_scale
if relative_change > self.gain_schedule_scale_change_threshold:
self._current_scale = scale
self._apply_effective_gains(scale)
self._log(
"info",
"增益调度 scale %.3f -> %.3f（相对变化 %.3f）",
old_scale,
scale,
relative_change,
)
def _apply_effective_gains(self, scale):
"""把当前档位基础增益按 scale 缩放后写入 PID（Kd 不调度）。"""
base = self._current_base_gains
self.pid.update_parameters(base[0] * scale, base[1] * scale, base[2])
def _register_read_failure(self, sensor, detail):
if sensor == "FLOW":
self.flow_failure_count += 1
count = self.flow_failure_count
limit = self.max_consecutive_flow_failures
elif sensor == "PRESSURE_BEFORE":
self.pressure_before_failure_count += 1
count = self.pressure_before_failure_count
limit = self.max_consecutive_pressure_failures
else:
self.pressure_failure_count += 1
count = self.pressure_failure_count
@@ -554,7 +779,7 @@ class FlowControlLoop:
self._log("critical", "%s", fault)
raise fault
def _held_result(self, actual_dt, pressure, status):
def _held_result(self, actual_dt, pressure, pressure_before, status):
return ControlStepResult(
timestamp=time.time(),
target_flow_slm=self.target_flow_slm,
@@ -565,6 +790,7 @@ class FlowControlLoop:
motor_position=self.last_motor_position,
actual_dt_s=actual_dt,
status=status,
pressure_before_kpa=pressure_before,
)
def _context(self):
@@ -572,11 +798,13 @@ class FlowControlLoop:
"target_flow_slm": self.target_flow_slm,
"last_valid_flow_slm": self.last_flow_slm,
"last_pressure_kpa": self.last_pressure_kpa,
"last_pressure_before_kpa": self.last_pressure_before_kpa,
"opening_pct": self.last_opening_pct,
"motor_position": self.last_motor_position,
"device_connected": bool(getattr(self.hardware, "connected", False)),
"flow_failure_count": self.flow_failure_count,
"pressure_failure_count": self.pressure_failure_count,
"pressure_before_failure_count": self.pressure_before_failure_count,
"pid_mode": self.pid_mode,
}