实现流量控制前馈 + 增益调度（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 完全一致。
2026-08-27 16:00:51 +08:00
parent d7e712ede7
commit a03735387c
7 changed files with 958 additions and 20 deletions
@@ -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]