From a03735387cc5607f523815bca775ff9379e126d2 Mon Sep 17 00:00:00 2001 From: louissun Date: Thu, 27 Aug 2026 16:00:51 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E6=B5=81=E9=87=8F=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=89=8D=E9=A6=88=20+=20=E5=A2=9E=E7=9B=8A=E8=B0=83?= =?UTF-8?q?=E5=BA=A6=EF=BC=88MFC=20=E7=BB=93=E6=9E=84=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 按 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 完全一致。 --- config.py | 36 ++++++ data_logger.py | 25 ++++ flow_control.py | 258 +++++++++++++++++++++++++++++++++++++--- identify_valve.py | 284 ++++++++++++++++++++++++++++++++++++++++++++ main.py | 56 ++++++++- test_valve_model.py | 124 +++++++++++++++++++ valve_model.py | 195 ++++++++++++++++++++++++++++++ 7 files changed, 958 insertions(+), 20 deletions(-) create mode 100644 identify_valve.py create mode 100644 test_valve_model.py create mode 100644 valve_model.py diff --git a/config.py b/config.py index 4c082e5..ff53674 100644 --- a/config.py +++ b/config.py @@ -83,6 +83,27 @@ MOTOR_OPEN_POSITION = 800.0 # 控制启动时的阀门初始开度；增量 PID 从该开度开始累加 Δu。 INITIAL_OPENING_PCT = 100.0 +# --------------------------------------------------------------------------- +# 前馈 + 增益调度（默认关闭，便于与纯 PID 做 A/B 对比；接线确认后再开启） +# --------------------------------------------------------------------------- + +# 阀前压力 P1（前馈缩放 / 判阻塞 / 增益调度用），按实际接线确认。 +PRESSURE_BEFORE_CHANNEL = 2 + +# 阀特性表 JSON（identify_valve.py 产出）。空字符串表示关闭前馈。 +VALVE_MODEL_PATH = "" + +# 前馈反解打底 + PI 修残差（市面 MFC 结构）。 +FEEDFORWARD_ENABLED = False +# 前馈开启时，PI 输出是“围绕前馈的修正量”，限幅为对称小范围（±%）。 +FEEDFORWARD_CORRECTION_BAND_PCT = 20.0 + +# PI 增益按阀前压力 P1 缩放（摊平 K_x ∝ P1 的非线性）。 +GAIN_SCHEDULE_ENABLED = False +GAIN_SCHEDULE_REF_ABS_KPA = 501.325 # 400 + 101.325，满压时调好的基准增益对应压力 +GAIN_SCHEDULE_FLOOR_ABS_KPA = 60.0 # 绝对压力下限，防止 P1 很低时增益爆炸 +GAIN_SCHEDULE_SCALE_CHANGE_THRESHOLD = 0.05 # scale 相对变化超过此值才重算增益 + # --------------------------------------------------------------------------- # 安全阈值 # --------------------------------------------------------------------------- @@ -162,3 +183,18 @@ def validate_config(): raise ValueError("MAX_CONSECUTIVE_FLOW_FAILURES 必须至少为 1") if PRESSURE_INPUT_CHANNEL is not None and MAX_PRESSURE_KPA is None: raise ValueError("启用压力通道时必须配置 MAX_PRESSURE_KPA") + if PRESSURE_BEFORE_CHANNEL is not None: + if not isinstance(PRESSURE_BEFORE_CHANNEL, int): + raise ValueError("PRESSURE_BEFORE_CHANNEL 必须为整数通道号或 None") + if PRESSURE_BEFORE_CHANNEL < 0: + raise ValueError("PRESSURE_BEFORE_CHANNEL 不得为负数") + if MAX_PRESSURE_KPA is None: + raise ValueError("启用阀前压力通道时必须配置 MAX_PRESSURE_KPA") + if GAIN_SCHEDULE_REF_ABS_KPA <= 0: + raise ValueError("GAIN_SCHEDULE_REF_ABS_KPA 必须大于 0") + if GAIN_SCHEDULE_FLOOR_ABS_KPA <= 0: + raise ValueError("GAIN_SCHEDULE_FLOOR_ABS_KPA 必须大于 0") + if not 0 < FEEDFORWARD_CORRECTION_BAND_PCT <= 100: + raise ValueError("FEEDFORWARD_CORRECTION_BAND_PCT 必须位于 0~100") + if GAIN_SCHEDULE_SCALE_CHANGE_THRESHOLD <= 0: + raise ValueError("GAIN_SCHEDULE_SCALE_CHANGE_THRESHOLD 必须大于 0") diff --git a/data_logger.py b/data_logger.py index ab6897e..36cc395 100644 --- a/data_logger.py +++ b/data_logger.py @@ -13,8 +13,12 @@ CSV_FIELDS = ( "measured_flow_slm", "opening_pct", "pressure_kpa", + "pressure_before_kpa", "error_slm", "motor_position", + "feedforward_pct", + "correction_pct", + "gain_scale", "actual_dt_s", "status", ) @@ -59,8 +63,14 @@ class FlowRunRecorder: ), "opening_pct": self._format_value(result.opening_pct), "pressure_kpa": self._format_value(result.pressure_kpa), + "pressure_before_kpa": self._format_value( + result.pressure_before_kpa + ), "error_slm": self._format_value(result.error_slm), "motor_position": self._format_value(result.motor_position), + "feedforward_pct": self._format_value(result.feedforward_pct), + "correction_pct": self._format_value(result.correction_pct), + "gain_scale": self._format_value(result.gain_scale), "actual_dt_s": self._format_value(result.actual_dt_s), "status": result.status, } @@ -93,6 +103,7 @@ class FlowRunRecorder: target_flow = [] measured_flow = [] opening = [] + feedforward = [] pressure = [] with self.csv_path.open("r", newline="", encoding="utf-8-sig") as file: @@ -103,6 +114,9 @@ class FlowRunRecorder: self._float_or_nan(row["measured_flow_slm"]) ) opening.append(self._float_or_nan(row["opening_pct"])) + feedforward.append( + self._float_or_nan(row.get("feedforward_pct")) + ) pressure.append(self._float_or_nan(row["pressure_kpa"])) figure, axes = plt.subplots( @@ -138,7 +152,18 @@ class FlowRunRecorder: opening, color="#2E7D32", linewidth=1.4, + label="Opening", ) + if any(math.isfinite(value) for value in feedforward): + axes[1].plot( + time_s, + feedforward, + color="#EF6C00", + linewidth=1.2, + linestyle="--", + label="Feedforward", + ) + axes[1].legend(loc="best") axes[1].set_ylabel("Opening (%)") axes[1].set_title("Valve opening") axes[1].set_ylim(-2, 102) diff --git a/flow_control.py b/flow_control.py index 087adbb..e445140 100644 --- a/flow_control.py +++ b/flow_control.py @@ -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, } diff --git a/identify_valve.py b/identify_valve.py new file mode 100644 index 0000000..21926ee --- /dev/null +++ b/identify_valve.py @@ -0,0 +1,284 @@ +"""从开环扫点 CSV 拟合 A_eff(x) 表，输出 valve_model.json。 + +用法： + python identify_valve.py --csv open_loop_data/open_loop_xxx.csv \\ + --out valve_model.json [--tail 20] [--plot] + +流程： +1. 按 step_index 分组，每组取末尾 --tail 个采样点平均，得到稳态 (x, Q, P1, P2)； +2. 对每个稳态点反解 A_eff = Q / (P1_abs * F(r))； +3. 按行程升序构表，做单调性检查； +4. 用 ValveModel 存成 JSON，并打印摘要（可选画诊断图）。 +""" + +import argparse +import csv +import math +from pathlib import Path + +import config +from valve_model import CRITICAL_RATIO, ValveModel, abs_pressure, f_ratio + +DEFAULT_TAIL = 20 + + +def _to_float(text): + """把 CSV 单元格解析成有限浮点数；空/非法/非有限返回 None。""" + if text is None: + return None + text = str(text).strip() + if text == "": + return None + try: + value = float(text) + except (TypeError, ValueError): + return None + return value if math.isfinite(value) else None + + +def _average(values): + """返回非 None 数值的平均；没有有效值时返回 None。""" + valid = [value for value in values if value is not None] + return None if not valid else sum(valid) / len(valid) + + +def load_steady_points(csv_path, tail=DEFAULT_TAIL): + """提取每个 step_index 的稳态均值，返回按 step 升序的 (x, q, p1, p2) 列表。""" + rows_by_step = {} + with open(csv_path, "r", newline="", encoding="utf-8-sig") as file: + for row in csv.DictReader(file): + step_text = (row.get("step_index") or "").strip() + if step_text == "": + continue + try: + step = int(float(step_text)) + except (TypeError, ValueError): + continue + rows_by_step.setdefault(step, []).append(row) + + points = [] + for step in sorted(rows_by_step): + rows = rows_by_step[step] + rows.sort(key=lambda r: _to_float(r.get("time_s")) or 0.0) + tail_rows = rows[-tail:] if tail > 0 else rows + + x = _average(_to_float(r.get("motor_position")) for r in tail_rows) + q = _average(_to_float(r.get("flow_after_slm")) for r in tail_rows) + p1 = _average(_to_float(r.get("pressure_before_kpa")) for r in tail_rows) + p2 = _average(_to_float(r.get("pressure_after_kpa")) for r in tail_rows) + + if None in (x, q, p1, p2): + continue + points.append((x, q, p1, p2)) + return points + + +def compute_area_table(steady_points): + """把稳态点换算成 (x, A_eff) 单调表，并返回丢弃点与阻塞/亚声速统计。 + + 返回 ``(table, dropped, stats)``。``table`` 为按 x 升序的 (x, A_eff) 列表， + 只保留最长的单调连续段；``dropped`` 为因非单调被丢弃的点。 + """ + area_points = [] + choked = subsonic = 0 + for x, q, p1, p2 in steady_points: + p1_abs = abs_pressure(p1) + p2_abs = abs_pressure(p2) + if p1_abs <= 0.0: + continue + r = p2_abs / p1_abs + f = f_ratio(r) + denom = p1_abs * f + if denom <= 0.0 or q < 0.0: + continue + if r <= CRITICAL_RATIO: + choked += 1 + else: + subsonic += 1 + area_points.append((x, q / denom)) + + area_points.sort(key=lambda item: item[0]) + ys = [a for _, a in area_points] + + if _is_monotonic(ys): + table = area_points + dropped = [] + else: + start, end = _longest_monotonic_run(ys) + table = area_points[start:end + 1] + dropped = area_points[:start] + area_points[end + 1:] + + stats = {"choked": choked, "subsonic": subsonic, "total": choked + subsonic} + return table, dropped, stats + + +def _is_monotonic(ys): + """序列是否单调（允许相等，但不允许中途反向）。""" + direction = 0 + for i in range(1, len(ys)): + delta = ys[i] - ys[i - 1] + if delta == 0: + continue + sign = 1 if delta > 0 else -1 + if direction == 0: + direction = sign + elif direction != sign: + return False + return True + + +def _longest_monotonic_run(ys): + """返回最长单调连续段的闭区间下标 ``(start, end)``。""" + best_start = best_end = 0 + for start in range(len(ys)): + direction = 0 + end = start + for j in range(start + 1, len(ys)): + delta = ys[j] - ys[j - 1] + if delta == 0: + end = j + continue + sign = 1 if delta > 0 else -1 + if direction == 0: + direction = sign + end = j + elif direction == sign: + end = j + else: + break + if end - start > best_end - best_start: + best_start, best_end = start, end + return best_start, best_end + + +def identify(csv_path, tail=DEFAULT_TAIL, out_path=None): + """端到端辨识；返回 ``(model, steady_points, table, dropped, stats)``。""" + steady = load_steady_points(csv_path, tail) + table, dropped, stats = compute_area_table(steady) + if len(table) < 2: + raise ValueError( + f"有效稳态点不足（{len(table)} 个），无法构表。请确认 CSV 的 " + "pressure_before_kpa / pressure_after_kpa / flow_after_slm 列读数合理。" + ) + model = ValveModel( + table, + config.MOTOR_OPEN_POSITION, + config.MOTOR_CLOSED_POSITION, + ) + if out_path: + model.save(out_path) + return model, steady, table, dropped, stats + + +def create_diagnostic_plot(steady_points, table, dropped, image_path): + """画 x vs A_eff（散点+插值线）与 x vs Q_ss（散点）两张子图。""" + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + xs = [x for x, _ in table] + areas = [a for _, a in table] + + figure, axes = plt.subplots(2, 1, figsize=(10, 8), constrained_layout=True) + figure.suptitle("Identified valve characteristic") + + axes[0].scatter(xs, areas, color="#1565C0", label="measured A_eff") + axes[0].plot(xs, areas, color="#1565C0", linewidth=1.2) + if dropped: + axes[0].scatter( + [x for x, _ in dropped], + [a for _, a in dropped], + color="#D84315", + marker="x", + label="dropped (non-monotonic)", + ) + axes[0].set_ylabel("A_eff (slm/kPa)") + axes[0].set_title("Effective area vs stroke") + axes[0].legend(loc="best") + axes[0].grid(True, alpha=0.3, linestyle="--") + + qxs = [x for x, _, _, _ in steady_points] + qs = [q for _, q, _, _ in steady_points] + axes[1].scatter(qxs, qs, color="#2E7D32", label="measured Q_ss") + axes[1].set_ylabel("Flow (SLM)") + axes[1].set_xlabel("Motor stroke x") + axes[1].set_title("Steady flow vs stroke") + axes[1].legend(loc="best") + axes[1].grid(True, alpha=0.3, linestyle="--") + + figure.savefig(image_path, dpi=config.PLOT_DPI, bbox_inches="tight") + plt.close(figure) + + +def parse_args(argv=None): + parser = argparse.ArgumentParser( + description="从开环扫点 CSV 拟合 A_eff(x) 阀特性表" + ) + parser.add_argument("--csv", required=True, help="开环扫点 CSV 路径") + parser.add_argument("--out", default="valve_model.json", help="输出 JSON 路径") + parser.add_argument( + "--tail", type=int, default=DEFAULT_TAIL, + help=f"每组末尾用于平均的采样点数（默认 {DEFAULT_TAIL}）", + ) + parser.add_argument("--plot", action="store_true", help="生成诊断图 PNG") + args = parser.parse_args(argv) + if args.tail < 1: + parser.error("--tail 必须 >= 1") + return args + + +def main(argv=None): + args = parse_args(argv) + csv_path = Path(args.csv) + if not csv_path.exists(): + print(f"CSV 不存在：{csv_path}") + return 2 + + try: + model, steady, table, dropped, stats = identify( + csv_path, tail=args.tail, out_path=args.out + ) + except (ValueError, OSError) as exc: + print(f"辨识失败：{exc}") + return 2 + + print( + f"读取稳态点 {len(steady)} 个；阻塞 {stats['choked']} 个，" + f"亚声速 {stats['subsonic']} 个，合计 {stats['total']} 个。" + ) + print( + f"有效行程范围 {table[0][0]:.1f} ~ {table[-1][0]:.1f}" + f"（{len(table)} 点）。" + ) + + if dropped: + print( + "警告：A_eff(x) 非单调，以下点被丢弃" + "（建议缩小扫点范围到单调段重扫）。" + ) + print("行程 x -> A_eff：") + for x, a in table: + print(f" x={x:8.3f} A_eff={a:.6f}") + if dropped: + print("被丢弃的点：") + for x, a in dropped: + print(f" x={x:8.3f} A_eff={a:.6f} [DROPPED]") + + print(f"已保存阀特性模型：{args.out}") + + if args.plot: + image_path = Path(args.out).with_suffix(".png") + try: + create_diagnostic_plot(steady, table, dropped, image_path) + print(f"诊断图已保存：{image_path}") + except Exception as exc: + print(f"生成诊断图失败（不影响 JSON）：{exc}") + + return 0 + + +if __name__ == "__main__": + import sys + + sys.exit(main()) diff --git a/main.py b/main.py index e82aac2..7e32c6a 100644 --- a/main.py +++ b/main.py @@ -23,6 +23,7 @@ from controllers import IncrementalPID from data_logger import FlowRunRecorder from flow_control import FlowControlFault, FlowControlLoop from performance_reporter import PerformanceReporter +from valve_model import ValveModel class ConsoleGate(logging.Filter): @@ -61,7 +62,27 @@ def build_logger(): return logger, console_gate +def _load_valve_model(logger): + """按配置加载阀特性模型；配置为空或文件缺失/损坏时返回 None（纯 PID）。""" + if not config.VALVE_MODEL_PATH: + return None + model_path = Path(__file__).resolve().parent / config.VALVE_MODEL_PATH + if not model_path.exists(): + logger.warning("阀特性模型文件不存在：%s，退化为纯 PID", model_path) + return None + try: + return ValveModel.load( + str(model_path), + config.MOTOR_OPEN_POSITION, + config.MOTOR_CLOSED_POSITION, + ) + except (ValueError, OSError) as exc: + logger.warning("加载阀特性模型失败（%s）：%s，退化为纯 PID", model_path, exc) + return None + + def build_control_loop(hardware, logger): + valve_model = _load_valve_model(logger) pid = IncrementalPID( kp=config.PID_FAR_KP, ki=config.PID_FAR_KI, @@ -77,6 +98,7 @@ def build_control_loop(hardware, logger): period_s=config.CONTROL_PERIOD_S, flow_channel=config.FLOW_INPUT_CHANNEL, pressure_channel=config.PRESSURE_INPUT_CHANNEL, + pressure_before_channel=config.PRESSURE_BEFORE_CHANNEL, motor_channel=config.MOTOR_OUTPUT_CHANNEL, motor_open_position=config.MOTOR_OPEN_POSITION, motor_closed_position=config.MOTOR_CLOSED_POSITION, @@ -112,6 +134,15 @@ def build_control_loop(hardware, logger): pid_mode_switch_confirm_cycles=( config.PID_MODE_SWITCH_CONFIRM_CYCLES ), + valve_model=valve_model, + feedforward_enabled=config.FEEDFORWARD_ENABLED, + feedforward_correction_band_pct=config.FEEDFORWARD_CORRECTION_BAND_PCT, + gain_schedule_enabled=config.GAIN_SCHEDULE_ENABLED, + gain_schedule_ref_abs_kpa=config.GAIN_SCHEDULE_REF_ABS_KPA, + gain_schedule_floor_abs_kpa=config.GAIN_SCHEDULE_FLOOR_ABS_KPA, + gain_schedule_scale_change_threshold=( + config.GAIN_SCHEDULE_SCALE_CHANGE_THRESHOLD + ), logger=logger, ) @@ -139,12 +170,27 @@ def format_status(result): "--" if result.pressure_kpa is None else f"{result.pressure_kpa:.2f}" ) - return ( - f"状态={result.status} 目标={result.target_flow_slm:.2f} SLM " - f"实际={flow_text} SLM 压力={pressure_text} kPa " - f"开度={result.opening_pct:.2f}% " - f"行程={result.motor_position:.1f} dt={result.actual_dt_s:.3f}s" + pressure_before_text = ( + "--" if result.pressure_before_kpa is None + else f"{result.pressure_before_kpa:.2f}" ) + parts = [ + f"状态={result.status}", + f"目标={result.target_flow_slm:.2f} SLM", + f"实际={flow_text} SLM", + f"压力={pressure_text} kPa", + f"阀前压={pressure_before_text} kPa", + f"开度={result.opening_pct:.2f}%", + ] + if result.feedforward_pct is not None: + parts.append(f"前馈={result.feedforward_pct:.2f}%") + if result.correction_pct is not None: + parts.append(f"修正={result.correction_pct:+.2f}%") + if result.gain_scale is not None and abs(result.gain_scale - 1.0) > 1e-9: + parts.append(f"scale={result.gain_scale:.3f}") + parts.append(f"行程={result.motor_position:.1f}") + parts.append(f"dt={result.actual_dt_s:.3f}s") + return " ".join(parts) def run(target_flow_slm): diff --git a/test_valve_model.py b/test_valve_model.py new file mode 100644 index 0000000..bcc9e9d --- /dev/null +++ b/test_valve_model.py @@ -0,0 +1,124 @@ +"""valve_model / identify_valve 离线自测（不碰硬件）。 + +运行： + python test_valve_model.py +""" + +import csv +import math +import tempfile +from pathlib import Path + +from valve_model import ( + P_ATM, + CRITICAL_RATIO, + ValveModel, + abs_pressure, + f_ratio, +) + + +def _close(a, b, rel=1e-6): + return abs(a - b) <= rel * max(1.0, abs(a), abs(b)) + + +def test_f_ratio(): + assert f_ratio(0.4) == 1.0 + assert f_ratio(CRITICAL_RATIO) == 1.0 + assert f_ratio(1.0) == 0.0 + assert f_ratio(1.1) == 0.0 + expected = math.sqrt( + 1.0 - ((0.7 - CRITICAL_RATIO) / (1.0 - CRITICAL_RATIO)) ** 2 + ) + assert _close(f_ratio(0.7), expected) + + +def test_abs_pressure(): + assert _close(abs_pressure(0.0), P_ATM) + assert _close(abs_pressure(300.0), 300.0 + P_ATM) + + +def test_valve_model_roundtrip(): + # A_eff 随行程线性递减（x 越大开度越小、面积越小）。 + table = [(800.0, 0.5), (900.0, 0.25), (1000.0, 0.0)] + model = ValveModel(table, motor_open=800.0, motor_closed=1000.0) + + assert _close(model.area_from_stroke(900.0), 0.25) + assert _close(model.area_from_stroke(850.0), 0.375) + assert _close(model.stroke_from_area(0.25), 900.0) + + # 阻塞流（P2=0 表压 → r≈0.2 < 0.528），flow_ss 与 feedforward_stroke 互逆。 + for x in (800.0, 850.0, 900.0, 1000.0): + q = model.flow_ss(x, 400.0, 0.0) + x_back = model.feedforward_stroke(q, 400.0, 0.0) + assert _close(x, x_back, rel=0.01), (x, q, x_back) + + # 亚声速（P2=300 表压 → r≈0.8）。 + q = model.flow_ss(900.0, 400.0, 300.0) + assert _close(model.feedforward_stroke(q, 400.0, 300.0), 900.0, rel=0.01) + + +def test_non_monotonic_raises(): + table = [(800.0, 0.5), (900.0, 0.1), (1000.0, 0.3)] + try: + ValveModel(table, motor_open=800.0, motor_closed=1000.0) + except ValueError: + return + raise AssertionError("非单调表应抛 ValueError") + + +def test_identify_end_to_end(): + import identify_valve + + # 构造合成扫点 CSV：每个 step 30 个采样点，全程稳态。 + rows = [] + for step, x in enumerate((800.0, 900.0, 1000.0)): + a_eff = (1000.0 - x) / 400.0 # 单调递减的真值 + q_ss = a_eff * (400.0 + P_ATM) # 阻塞流，F=1 + for i in range(30): + rows.append({ + "time_s": f"{step * 10 + i * 0.1:.6f}", + "step_index": step, + "opening_pct": 100.0 - step * 50.0, + "motor_position": x, + "flow_before_slm": "", + "flow_after_slm": q_ss, + "pressure_before_kpa": 400.0, + "pressure_after_kpa": 0.0, + "P_abs_ratio": "", + "is_ratio_smaller_than_0.528": "Y", + }) + + with tempfile.TemporaryDirectory() as tmp: + csv_path = Path(tmp) / "sweep.csv" + out_path = Path(tmp) / "model.json" + with csv_path.open("w", newline="", encoding="utf-8-sig") as f: + writer = csv.DictWriter(f, fieldnames=list(rows[0].keys())) + writer.writeheader() + writer.writerows(rows) + + model, steady, table, dropped, stats = identify_valve.identify( + csv_path, tail=20, out_path=str(out_path) + ) + assert len(table) == 3, table + assert stats["choked"] == 3 + assert not dropped + for x, a in table: + assert _close(a, (1000.0 - x) / 400.0, rel=0.01), (x, a) + + loaded = ValveModel.load(str(out_path), 800.0, 1000.0) + assert _close(loaded.area_from_stroke(900.0), 0.25, rel=0.01) + + +if __name__ == "__main__": + tests = [ + test_f_ratio, + test_abs_pressure, + test_valve_model_roundtrip, + test_non_monotonic_raises, + test_identify_end_to_end, + ] + for test in tests: + test() + print(f"PASS {test.__name__}") + print("全部通过") diff --git a/valve_model.py b/valve_model.py new file mode 100644 index 0000000..b6424b5 --- /dev/null +++ b/valve_model.py @@ -0,0 +1,195 @@ +"""阀特性模型 + 查找表 + 前馈反解（纯计算，不依赖硬件）。 + +把静态阀特性 ``Q_ss = A_eff(x) * P1_abs * F(r)`` 落成一个可离线测试的模型： +给定目标流量 Q_set 与当前阀前/阀后压力，反解出“应该给多大行程 x”。控制器在 +运行时只调用 :meth:`ValveModel.flow_ss` 与 :meth:`ValveModel.feedforward_stroke`， +不跑任何动态过程。 +""" + +import json +import math + +P_ATM = 101.325 # 标准大气压（kPa） +CRITICAL_RATIO = 0.528 # 空气（γ=1.4）的临界压力比 + + +def abs_pressure(kpa_gauge): + """把表压读数换算成绝对压力（kPa）。""" + return float(kpa_gauge) + P_ATM + + +def f_ratio(r): + """压比函数 F(r)（ISO 6358 椭圆平滑，r 为绝对压比 P2_abs/P1_abs）。 + + 阻塞流（r <= 0.528）时 F=1；亚声速（0.528 < r < 1）时按椭圆衰减； + r >= 1 时无正向流，F=0。 + """ + r = float(r) + if not math.isfinite(r): + return math.nan + if r <= CRITICAL_RATIO: + return 1.0 + if r >= 1.0: + return 0.0 + width = 1.0 - CRITICAL_RATIO + return math.sqrt(1.0 - ((r - CRITICAL_RATIO) / width) ** 2) + + +class ValveModel: + """静态阀特性模型：A_eff(x) 查找表 + 前馈反解。 + + 表以电机行程 x 为键（物理真值），控制器里再用线性反算换回开度。 + ``area_table`` 是按 x 升序的 ``(x, A_eff)`` 列表。 + """ + + def __init__(self, area_table, motor_open, motor_closed): + motor_open = float(motor_open) + motor_closed = float(motor_closed) + if motor_open >= motor_closed: + raise ValueError("打开端行程必须小于关闭端行程") + + table = [(float(x), float(a)) for (x, a) in area_table] + if not table: + raise ValueError("A_eff 表不能为空") + table.sort(key=lambda item: item[0]) + # 去掉完全重复的 x（保留最后一个），避免插值除以零。 + deduped = [] + for x, a in table: + if deduped and deduped[-1][0] == x: + deduped[-1] = (x, a) + else: + deduped.append((x, a)) + table = deduped + if len(table) < 2: + raise ValueError("A_eff 表至少需要两个不同行程的点") + + for x, a in table: + if not (math.isfinite(x) and math.isfinite(a)): + raise ValueError(f"A_eff 表包含非有限值: x={x}, A_eff={a}") + if a < 0.0: + raise ValueError(f"A_eff 不得为负: x={x}, A_eff={a}") + + ys = [a for (_, a) in table] + if not _is_monotonic(ys): + raise ValueError( + "A_eff(x) 非单调：反解无法唯一。请检查扫点数据，" + "缩小范围到单调段后重扫。" + ) + + self.area_table = table + self.motor_open = motor_open + self.motor_closed = motor_closed + self._xs = [x for (x, _) in table] + self._ys = ys + + # ------------------------------------------------------------------ 查询 + + def area_from_stroke(self, x): + """线性插值查 A_eff(x)；越界钳位到表端点值。""" + return _interp(float(x), self._xs, self._ys) + + def stroke_from_area(self, a): + """反查 x(A_eff)；表单调，越界钳位到端点行程。""" + a = float(a) + xs = self._xs + ys = self._ys + increasing = ys[0] <= ys[-1] + + if increasing: + y_min, y_min_x = ys[0], xs[0] + y_max, y_max_x = ys[-1], xs[-1] + else: + y_min, y_min_x = ys[-1], xs[-1] + y_max, y_max_x = ys[0], xs[0] + + if a <= y_min: + return y_min_x + if a >= y_max: + return y_max_x + + for i in range(len(ys) - 1): + y0, y1 = ys[i], ys[i + 1] + if y0 == y1: + continue + if (y0 <= a <= y1) or (y1 <= a <= y0): + t = (a - y0) / (y1 - y0) + return xs[i] + t * (xs[i + 1] - xs[i]) + # 理论走不到这里（a 严格落在 y_min/y_max 之间且表单调）。 + return xs[0] + + # ------------------------------------------------------------ 静态特性 + + def flow_ss(self, x, p1_kpa, p2_kpa): + """给定行程 x 与阀前/阀后表压，返回稳态流量 Q_ss（slm）。""" + p1_abs = abs_pressure(p1_kpa) + if p1_abs <= 0.0: + return 0.0 + p2_abs = abs_pressure(p2_kpa) + r = p2_abs / p1_abs + return self.area_from_stroke(x) * p1_abs * f_ratio(r) + + def feedforward_stroke(self, q_set, p1_kpa, p2_kpa): + """给定目标流量与阀前/阀后表压，反解行程 x（前馈）。""" + q_set = float(q_set) + p1_abs = abs_pressure(p1_kpa) + if p1_abs <= 0.0: + raise ValueError("阀前绝对压力必须大于 0") + p2_abs = abs_pressure(p2_kpa) + r = p2_abs / p1_abs + f = f_ratio(r) + denom = p1_abs * f + if denom <= 0.0: + raise ValueError( + f"压比 r={r:.4f} 下无正向流量（F={f:.4f}），无法前馈反解" + ) + a_req = q_set / denom + return self.stroke_from_area(a_req) + + # ------------------------------------------------------------------ 存取 + + def save(self, path): + data = { + "motor_open": self.motor_open, + "motor_closed": self.motor_closed, + "area_table": [[x, a] for (x, a) in self.area_table], + } + with open(path, "w", encoding="utf-8") as file: + json.dump(data, file, indent=2, ensure_ascii=False) + + @classmethod + def load(cls, path, motor_open, motor_closed): + with open(path, "r", encoding="utf-8") as file: + data = json.load(file) + area_table = [tuple(item) for item in data["area_table"]] + return cls(area_table, motor_open, motor_closed) + + +def _is_monotonic(ys): + """序列是否单调（允许相等，但不允许中途反向）。""" + direction = 0 + for i in range(1, len(ys)): + delta = ys[i] - ys[i - 1] + if delta == 0: + continue + sign = 1 if delta > 0 else -1 + if direction == 0: + direction = sign + elif direction != sign: + return False + return True + + +def _interp(x, xs, ys): + """在按 x 升序的表中线性插值；越界钳位到端点。""" + if x <= xs[0]: + return ys[0] + if x >= xs[-1]: + return ys[-1] + for i in range(len(xs) - 1): + x0, x1 = xs[i], xs[i + 1] + if x0 <= x <= x1: + if x1 == x0: + return ys[i] + t = (x - x0) / (x1 - x0) + return ys[i] + t * (ys[i + 1] - ys[i]) + return ys[-1]