实现流量控制前馈 + 增益调度（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,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())