实现流量控制前馈 + 增益调度（MFC 结构）
按 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:
@@ -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")
|
||||
|
||||
@@ -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)
|
||||
|
||||
+243
-15
@@ -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,
|
||||
}
|
||||
|
||||
|
||||
@@ -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())
|
||||
@@ -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):
|
||||
|
||||
@@ -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("全部通过")
|
||||
+195
@@ -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]
|
||||
Reference in New Issue
Block a user