From e0665c73fbff0dd07fb13ae5efbbe40dbbde3346 Mon Sep 17 00:00:00 2001 From: Horldsence Date: Fri, 11 Sep 2026 09:02:28 +0800 Subject: [PATCH] =?UTF-8?q?feat(q3):=20=E8=B4=9D=E5=8F=B6=E6=96=AF?= =?UTF-8?q?=E7=BD=91=E6=A0=BC=E7=B2=92=E5=AD=90=E6=BB=A4=E6=B3=A2=E9=97=AD?= =?UTF-8?q?=E7=8E=AF=EF=BC=88PF=E8=B4=9F=E8=A7=82=E6=B5=8B+=E4=BF=A1?= =?UTF-8?q?=E6=81=AF=E5=A2=9E=E7=9B=8A=E9=80=89=E7=82=B9+2-opt=E8=B0=83?= =?UTF-8?q?=E5=BA=A6=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - particles.py: 网格幸存集滤波，Voronoi 裕量保证真源保留（缺席证明/网格清除证书/跳测规则） - planner/bayes_loop.py: 统一任务循环（站点/定位/清除滚动 2-opt 排序）、期望后验半径选点 - planner/seven.py: 七点覆盖策略族提升为库代码 - cli: mcm26b run --strategy {grid,seven,bayes} - BayesConfig 参数组；tests +24 项（真源保留/证书/端到端），129 全绿 --- ...ss_f2eec9cb-6a04-41a3-9d72-ce6ab4455a52.md | 80 ++++ scripts/benchmark_q3_search.py | 72 +-- src/mcm26b/cli.py | 13 +- src/mcm26b/config.py | 33 ++ src/mcm26b/particles.py | 356 +++++++++++++++ src/mcm26b/planner/bayes_loop.py | 429 ++++++++++++++++++ src/mcm26b/planner/seven.py | 93 ++++ tests/test_bayes_loop_q3.py | 154 +++++++ tests/test_particles.py | 172 +++++++ 9 files changed, 1335 insertions(+), 67 deletions(-) create mode 100644 .zcode/plans/plan-sess_f2eec9cb-6a04-41a3-9d72-ce6ab4455a52.md create mode 100644 src/mcm26b/particles.py create mode 100644 src/mcm26b/planner/bayes_loop.py create mode 100644 src/mcm26b/planner/seven.py create mode 100644 tests/test_bayes_loop_q3.py create mode 100644 tests/test_particles.py diff --git a/.zcode/plans/plan-sess_f2eec9cb-6a04-41a3-9d72-ce6ab4455a52.md b/.zcode/plans/plan-sess_f2eec9cb-6a04-41a3-9d72-ce6ab4455a52.md new file mode 100644 index 0000000..9387df9 --- /dev/null +++ b/.zcode/plans/plan-sess_f2eec9cb-6a04-41a3-9d72-ce6ab4455a52.md @@ -0,0 +1,80 @@ +# 目标 + +在新分支 **`q3-bayes`**（基于当前 `gpt`）实现第三问新算法：**网格化贝叶斯粒子滤波（含负观测更新）+ 信息增益选点 + 开放路径 2-opt 任务排序 + 七点覆盖兜底 + Optuna 离线调参**。评分口径 = 虚拟总时间（A2 基线：官方演练 12 源 5232s、109 次检测）；硬性要求全清率 100%、缺席证明正确、零异常。 + +# 关键设计 + +## 1. 粒子滤波 `src/mcm26b/particles.py`（新建） + +与用户方案的差异点：本项目要求清除证书/缺席证明**严格**（A2 靠保守几何证书做到 100% 清除率），软核概率 PF 有删掉真源的风险。因此采用**网格幸存集滤波**（= 硬似然 PF + Voronoi 裕量，与用户"|θ̂−θ|>1° 权重置零"的设定一致但更严格）： + +- 每频道一组 Ω 上间距 h≈30m 的网格粒子（约 1.1 万格，alive 用整数索引集合；空间哈希加速邻域查询） +- 保守 kill 判定（margin = h·√2/2，保证真源所在格必活）： + - `no_signal`@s：kill dist(g,s) ≤ 1000 − margin（题面 r_eff ≥ 1000 ⟹ 可严格排除该盘） + - `direction`@s 读数 θ：kill 角向偏差超出 δ_eff=1.005°（含余量）或 dist > 1500 + margin 的格 + - `near`@s：kill dist(g,s) > 5 + margin +- 证书全部由此推导：清除证书 = MEC(alive).R + margin ≤ 20 − clear_margin；**缺席证明 = alive 为空集**（无需额外像素覆盖检查）；跳测规则 = min dist(alive, s) > 1500 + margin ⟹ 必 no_signal，跳过省 6s/次 +- 信息增益：对候选点用子采样（≤500 粒子）× K 个代表假设做 MC 期望熵减估计；复用 `smallest_enclosing_circle`(Welzl) 求 MEC + +## 2. 新策略 `BayesLoop(OmnidirectionalLoop)`（`src/mcm26b/planner/bayes_loop.py`） + +复用基类 run() 骨架、异常收尾、summary、anomaly、预算机制，重写阶段逻辑： +- **发现阶段**：原点起扫（频道排序省换频）→ 站点 = 七点环 ∪ PF 高价值点，按"期望发现数 − λ·移动时间"贪心选站；每站批量扫频时跳过"PF 判定必 no_signal"的 (站,频道) 对与已证缺席频道；每站扫完即服务已发现频道（SevenBatch 风格） +- **定位服务**：选点 = 保证接收盘内候选环采样 + 信息增益评分（ΔH − λ·移动 − μ·检测），有证书即清除；步数上限沿用 `max_localize_steps` +- **清除调度**：待服务任务（定位+清除 ≤20 节点）开放路径最近邻 + 2-opt，纯标准库，每个决策点滚动重排 +- **收尾**：全部 CLEARED/ABSENT → all_resolved；清满 16 提前停；persistent_search 沿用现有配置语义（实现时对照 `run_q3_seven_practice.py` 保持官方演练停止行为一致） + +## 3. 配置 `config.py` + +新增 `BayesConfig` frozen dataclass（[工程]）：particle_spacing_m、λ/μ 权重、MC 代表数/子采样、skip_margin、站点环半径、2-opt 开关/迭代数等；挂到 `ProblemConfig.bayes`。默认值不改变现有策略行为。 + +## 4. 七点覆盖提升为库代码 `src/mcm26b/planner/seven.py` + +把 `scripts/benchmark_q3_search.py` 的 `seven_points()/SevenImmediate/SevenBatch` 移入 src（旧脚本改 import，消融结果可复现），供 CLI 与新基准复用。 + +## 5. CLI `cli.py` + +`mcm26b run --strategy {grid,seven,bayes}`，默认 grid；**若基准通过则默认改为 bayes**（用户已确认）。 + +## 6. 基准 `scripts/benchmark_q3_bayes.py` + +配对消融 {seven_batch, bayes} ×（seeds 0..5 × 10/16 源 + 边界源 + 0 源纯缺席场景），复用现有 run_case/LocalSimulator 模式；输出 `output/q3/bayes-ablation/`（benchmark.json + report.md）：全清率、correct_absence、异常、accounted/total 虚拟时间、配对降幅、实机 compute_wall_s。 + +## 7. Optuna 调参 `scripts/tune_q3_bayes.py` + +- `uv add --group dev optuna`（仅调参脚本用，生产零新增依赖） +- TPE，目标 = 场景子集平均虚拟时间 + 10⁶×失败惩罚；场景与 error_seed 多样化防过拟合 +- 搜索空间：h、λ/μ、skip margin、候选环、2-opt 开关等 +- optuna 不可用时自动退化为随机搜索；最优参数写 JSON，复跑基准确认后合入 config.py 默认值 + +## 8. 测试 + +- `tests/test_particles.py`：真源保留性（随机场景 × LocalSimulator 误差场，任意更新序列后真源邻域格必活）；证书正确性（alive=∅ ⟺ 真缺席；MEC 证书清除必成功）；跳测规则永不漏检 +- `tests/test_bayes_loop_q3.py`：端到端 LocalSimulator（10/16/0 源、边界源、预算耗尽、故障注入）：全清 + 缺席正确 + 无异常 + 正常退出 +- 现有 105 项测试保持绿 + +## 9. 文档 + +`docs/algo-3-bayes.md`（Voronoi 裕量严格性论证、信息增益与调度设计、与 A2 对比数据）；README 策略表加行；`docs/algo-3.md` 加交叉引用。 + +# 执行顺序 + +1. `git checkout -b q3-bayes` +2. BayesConfig → particles.py（含单测）→ seven.py 提升 +3. bayes_loop.py + CLI 接入 → pytest 全绿 +4. 跑配对消融基准 +5. Optuna 调参 → 回填参数复跑基准 +6. 若达标（全清 100% 且配对均值更优）：`mcm26b run` 默认切 bayes + 文档更新 +7. 分支上分阶段 commit（不 push） + +# 验收标准 + +- 基准全部场景：all_cleared 100%、correct_absence 100%、anomalies 空、normal_exit、单局实机计算 < 60s +- 配对平均虚拟时间优于 seven_batch（目标降幅 ≥5%；参考量级 A2=5232s/12 源） +- `uv run pytest` 全绿；`mcm26b run --self-test --problem 3` 通过 + +# 风险与对策 + +- PF 实机计算耗时 → 空间哈希 + MC 子采样，基准记录 compute_wall_s，超预算先降 MC 规模（实机预算 1200s，A2 仅用 3.4s，余量巨大） +- LocalSimulator 误差场确定性 → 调参与基准用多 error_seed +- 负观测删点的正确性 → Voronoi 裕量定理 + 随机化真源保留性测试兜底 \ No newline at end of file diff --git a/scripts/benchmark_q3_search.py b/scripts/benchmark_q3_search.py index f059b52..7981c1c 100644 --- a/scripts/benchmark_q3_search.py +++ b/scripts/benchmark_q3_search.py @@ -14,80 +14,20 @@ from pathlib import Path from statistics import mean, median from time import perf_counter -from mcm26b.belief import ChannelStatus +from mcm26b.belief import ChannelStatus # noqa: F401 (兼容旧 import 路径) from mcm26b.config import ProblemConfig -from mcm26b.coverage import CoverageLedger -from mcm26b.planner import OmnidirectionalLoop, clear_certificate +from mcm26b.coverage import CoverageLedger # noqa: F401 +from mcm26b.planner import OmnidirectionalLoop, clear_certificate # noqa: F401 +from mcm26b.planner.seven import (SevenBatch, SevenImmediate, SevenSweep, + RING_RADIUS_M, seven_points) from mcm26b.protocol.client import SimulatorClient -from mcm26b.protocol.rules import NEAR +from mcm26b.protocol.rules import NEAR # noqa: F401 from mcm26b.sim import LocalSimulator, LocalSimTransport, Source from mcm26b.sim.scenes import random_omni_scene -RING_RADIUS_M = 1200.0 # Engineering choice; analytic proof in algo-3 section 5. ROOT = Path(__file__).resolve().parents[1] -def seven_points(): - return [(0.0, 0.0)] + [(RING_RADIUS_M * math.cos(k * math.pi / 3), - RING_RADIUS_M * math.sin(k * math.pi / 3)) for k in range(6)] - - -class SevenImmediate(OmnidirectionalLoop): - def __init__(self, client, config): - super().__init__(client, config) - self.ledger = CoverageLedger(config=config, points=seven_points()) - - -class SevenBatch(SevenImmediate): - """Finish scanning a station, then serve detected sources in nearest order.""" - defer_all = False - - def _serve_queue(self): - # Each detected channel gets at most one localization call in this pass. - queue = self.belief.channels_by_status(ChannelStatus.DETECTED) - while queue: - c = min(queue, key=lambda c: (math.dist(self.client.state.position, - self.belief.candidate_set(c).mec()[0]), c)) - queue.remove(c) - self._localize_and_clear(c) - if self._count_limit_reached(): - break - - def _coverage_phase(self): - while not self._count_limit_reached(): - index = self._next_point_index() - if index is None: - return - point = self.ledger.points[index] - pending = self._pending_channels_at(index) - # Keeping the current measuring channel first saves a switch when possible. - pending.sort(key=lambda c: (c != self.client.state.channel, c)) - for c in pending: - outcome = self.client.measure(point, c) - self.ledger.mark_measured(c, index) - self.belief.update_measure(c, point, outcome.result, outcome.svd_deg) - if outcome.result == NEAR: - cert = clear_certificate(self.belief.candidate_set(c), c, self.config) - # Same-station near clear; do not displace the station scan. - if cert.certified: - self._attempt_clear(c, point, cert) - if self._count_limit_reached(): - return - if not self.defer_all: - self._serve_queue() - - def _resolve_detected(self): - self._serve_queue() - unresolved = [c for c, b in self.belief.channels.items() - if b.status not in (ChannelStatus.CLEARED, ChannelStatus.ABSENT)] - return "all_resolved" if not unresolved else "unresolved_channels" - - -class SevenSweep(SevenBatch): - """Finish discovery at all necessary stations before servicing the queue.""" - defer_all = True - - VARIANTS = {"grid49_immediate": OmnidirectionalLoop, "seven_immediate": SevenImmediate, "seven_batch": SevenBatch, "seven_sweep": SevenSweep} diff --git a/src/mcm26b/cli.py b/src/mcm26b/cli.py index 966d04a..e0bc745 100644 --- a/src/mcm26b/cli.py +++ b/src/mcm26b/cli.py @@ -17,12 +17,16 @@ from pathlib import Path from .config import ProblemConfig from .geometry.__main__ import main as q1_main from .planner import DirectionalExtension, OmnidirectionalLoop +from .planner.bayes_loop import BayesLoop +from .planner.seven import SevenBatch from .protocol.client import SimulatorClient from .protocol.log import SessionLog from .protocol.rules import EXIT_USER from .sim import LocalSimTransport, LocalSimulator, Source from .sim.scenes import mixed_scene, random_omni_scene +Q3_STRATEGIES = {"grid": OmnidirectionalLoop, "seven": SevenBatch, "bayes": BayesLoop} + def _sim_check(args) -> int: """附件1表2/附件2第10节计时示例：期望 105/111/194/199 秒。""" @@ -83,9 +87,13 @@ def _run(args) -> int: "/enter 返回的 remaining_real_duration_s 为准。", file=sys.stderr) log = SessionLog(path=Path(args.log)) if args.log else SessionLog() client = SimulatorClient(args.robot_id, config=config, transport=transport, log=log) - loop_cls = DirectionalExtension if args.problem == 4 else OmnidirectionalLoop + if args.problem == 4: + loop_cls = DirectionalExtension + else: + loop_cls = Q3_STRATEGIES[args.strategy] summary = loop_cls(client, config).run() summary["robot_id"] = args.robot_id + summary["strategy"] = ("directional" if args.problem == 4 else args.strategy) if real_budget_s is not None: summary["self_test_real_budget_s"] = real_budget_s if args.summary_steps is not None and len(summary.get("steps", [])) > args.summary_steps: @@ -125,6 +133,9 @@ def main(argv=None) -> int: run.add_argument("--log", default=None, help="会话日志 JSONL 输出路径") run.add_argument("--problem", type=int, choices=(3, 4), default=3, help="问题3=全向源，问题4=混合定向源（默认 3）") + run.add_argument("--strategy", choices=tuple(Q3_STRATEGIES), default="grid", + help="问题3策略：grid=49点网格基线，seven=七点批量，" + "bayes=粒子滤波+信息增益（默认 grid；仅 --problem 3 生效）") run.add_argument("--bearing-margin-deg", type=float, default=0.005, help="示向度舍入裕量（服务器保留两位小数；默认 0.005°）") run.add_argument("--real-budget-s", type=float, default=None, diff --git a/src/mcm26b/config.py b/src/mcm26b/config.py index 226c6e3..01fb2b4 100644 --- a/src/mcm26b/config.py +++ b/src/mcm26b/config.py @@ -99,6 +99,38 @@ class SearchConfig: # 避免触及 100 小时上限被模拟器强制结束 +@dataclass(frozen=True) +class BayesConfig: + """P4b 贝叶斯网格滤波策略参数（问题3新策略 bayes）。[工程] + + 网格粒子滤波的严格性依赖 Voronoi 裕量：真实源所在格心与源的距离 + <= sqrt(2)/2 * particle_spacing_m，所有 kill 判定都内缩该裕量， + 因此幸存集永远包含真源（定理见 docs/algo-3-bayes.md §2）。 + """ + + particle_spacing_m: float = 25.0 # 网格间距；Voronoi 裕量 ≈ 17.68m； + # 全域约 1.6 万格/频道，20 频道内存与耗时可控 + # ---- 信息增益评分 [工程] ---- + score_mode: str = "radius" # radius=期望后验半径（直指证书）| + # entropy=期望 log 幸存占比（信息增益） + score_subsample: int = 800 # 期望幸存熵估计的粒子子采样规模 + mc_representatives: int = 6 # 假设真值代表点数（均匀抽取幸存格） + lambda_move_s: float = 0.1 # 选点评分中移动时间的折算系数（分值/秒）； + # 与后验半径同量纲，量级须小否则终局拒走（欠收敛） + wide_rings: tuple = (1.0, 1.5, 2.0) # 无保证接收盘时的探索环（× PF MEC 半径） + # ---- 站点选择 [工程] ---- + station_ring_radius_m: float = 1200.0 # 七点环半径；覆盖半径 968.9m 的解析证明 + inner_mass_weight: float = 0.5 # 站点评分中 1000m 内幸存质量的权重 + resolve_bonus: float = 10.0 # "就地可证毕"（no_signal 即缺席/有信号即收敛） + # 频道的站点评分加成 + station_move_penalty_s: float = 40.0 # 站点评分中移动时间（秒）的折算分母 + # ---- 任务排序 [工程] ---- + two_opt_enabled: bool = True # 开放路径 2-opt 改进（最近邻打底） + two_opt_passes: int = 3 # 2-opt 最大改进轮数 + # ---- PF 清除证书 [工程] ---- + grid_cert_enabled: bool = True # 允许用幸存集 MEC 颁发清除证书（凸证书之外） + + @dataclass(frozen=True) class ProblemConfig: """聚合配置。各模块函数的第一个默认参数，便于单测替换。""" @@ -107,6 +139,7 @@ class ProblemConfig: cost: CostConfig = CostConfig() session: SessionConfig = SessionConfig() search: SearchConfig = SearchConfig() + bayes: BayesConfig = BayesConfig() DEFAULT_CONFIG = ProblemConfig() diff --git a/src/mcm26b/particles.py b/src/mcm26b/particles.py new file mode 100644 index 0000000..f73d6ae --- /dev/null +++ b/src/mcm26b/particles.py @@ -0,0 +1,356 @@ +"""P4b 贝叶斯网格粒子滤波：逐频道幸存集维护与负观测更新。 + +与 belief.CandidateRegion（保守凸外包，无法表达非凸"挖洞"）互补： +网格幸存集精确表达"角域 ∩ 距离盘 ∩ 全部 no_signal 排除盘"的非凸可行集， +从而支持三件凸表示做不到的事—— + +1. 缺席证明：幸存集为空 ⟹ 可行集为空 ⟹ 该频道无源（严格，无需覆盖账本）； +2. 跳测规则：幸存格到测点的最小距离 > r_max + 裕量 ⟹ 该次测量必 no_signal， + 且不产生任何新排除，可直接跳过（省 5s 检测 + 1s 换频）； +3. 负观测更新：no_signal 排除 B(s, 1000)，凸表示不删点，网格直接删格。 + +严格性（真源保留定理，docs/algo-3-bayes.md §2）：格距 h 的 Voronoi 裕量 +m = sqrt(2)/2·h 是格心到格内任一点的最大距离。所有 kill 判定均内缩 m， +即"仅当整格与可行集不相交时才删格"。真源所在格与可行集之交含真源本身， +故该格永不被删——任意观测序列后幸存集恒包含真源，且： +- 清除证书：MEC(幸存).R + m <= 20 − 工程余量 ⟹ 真源距清除点 <= 20； +- 缺席证明：幸存集为空 ⟹ 可行集为空。 + +观测语义与 sim.Simulator 一致 [题面]： +- no_signal：距离 > r_eff >= 1000 ⟹ 可行集 ∩ B(s,1000) = ∅； +- direction：距离 <= 1500，示向度误差 |读数−真值| <= δ_eff（含舍入裕量）； +- near：距离 <= 5。 + +性能设计：kill 与查询全部走 O(1) 集合运算；最近距离查询用粗粒度占用 +索引（每格聚合 CF² 个细格）螺旋扫描 + 早退，服务跳测规则的高频调用。 +""" + +from __future__ import annotations + +import math + +from .config import ProblemConfig, Vec2 +from .geometry.localization import smallest_enclosing_circle +from .protocol.rules import DIRECTION, NEAR, NO_SIGNAL + +_ANG_EPS_DEG = 1e-9 # 角向 kill 比较的浮点余量 +_DIST_EPS_M = 1e-9 +_COARSE_FACTOR = 4 # 粗格每边聚合的细格数 + + +def angular_gap(a_deg: float, b_deg: float) -> float: + """两个方位角（度）的最小夹角。""" + return abs((a_deg - b_deg + 180.0) % 360.0 - 180.0) + + +class ChannelParticles: + """单个频道的网格幸存集。 + + 网格为覆盖 Ω 外扩一格的轴对齐方格（边长 h），索引 ``i*W + j``； + ``alive`` 是幸存格索引集合。初始集 = 格心距原点 <= Ω 半径 + 裕量的 + 全部格子，因此真源（|p| <= 1800 [题面]）所在格必在初始集内。 + ``version`` 随每次删除自增，调用方可据此缓存派生量（MEC、跳测判定）。 + """ + + def __init__(self, config: ProblemConfig = ProblemConfig(), + spacing_m: float | None = None): + field = config.field + self.config = config + self.spacing_m = float(spacing_m if spacing_m is not None + else config.bayes.particle_spacing_m) + if not 1.0 <= self.spacing_m <= 200.0: + raise ValueError("网格间距须在 [1, 200] 米") + self.margin_m = self.spacing_m * math.sqrt(2) / 2 # Voronoi 裕量 + self.r_eff_min_m = field.r_eff_min_m + self.r_eff_max_m = field.r_eff_max_m + self.near_radius_m = field.near_radius_m + self.half_angle_deg = (field.bearing_error_half_deg + + config.search.bearing_rounding_margin_deg) + # 覆盖 [-R-m, R+m]² 的格点阵列；格 (i,j) 的中心在 + # (x0 + (i+0.5)h, x0 + (j+0.5)h)，中心到格内任一点 <= h·√2/2 = 裕量 + span = field.omega_radius_m + self.margin_m + self.x0 = -span + self.width = int(math.ceil(2 * span / self.spacing_m)) + 1 + limit_sq = (span + 1e-9) ** 2 + self.alive: set[int] = set() + self._coarse: dict[int, int] = {} # 粗格索引 -> 幸存细格数 + coarse_w = self._coarse_width() + for i in range(self.width): + x = self.x0 + (i + 0.5) * self.spacing_m + for j in range(self.width): + if x * x + (self.x0 + (j + 0.5) * self.spacing_m) ** 2 <= limit_sq: + index = i * self.width + j + self.alive.add(index) + key = (i // _COARSE_FACTOR) * coarse_w + j // _COARSE_FACTOR + self._coarse[key] = self._coarse.get(key, 0) + 1 + self.version = 0 + self._mec: tuple[Vec2, float] | None = None + + # ------------------------------------------------------------ 索引与坐标 + def _coarse_width(self) -> int: + return (self.width + _COARSE_FACTOR - 1) // _COARSE_FACTOR + + def cell_center(self, index: int) -> Vec2: + i, j = divmod(index, self.width) + return (self.x0 + (i + 0.5) * self.spacing_m, + self.x0 + (j + 0.5) * self.spacing_m) + + def _discard(self, indices: list[int]) -> None: + if not indices: + return + coarse_w = self._coarse_width() + for index in indices: + if index not in self.alive: + continue + self.alive.discard(index) + key = (index // self.width // _COARSE_FACTOR) * coarse_w \ + + (index % self.width) // _COARSE_FACTOR + count = self._coarse.get(key, 0) - 1 + if count <= 0: + self._coarse.pop(key, None) + else: + self._coarse[key] = count + self.version += 1 + self._mec = None + + # ------------------------------------------------------------ 观测更新 + def _kill_disk(self, center: Vec2, radius_m: float) -> None: + """删除格心距 center <= radius_m 的全部幸存格（整格判据见模块注释）。""" + if not self.alive: + return + reach = radius_m + _DIST_EPS_M + ilo = max(0, int(math.floor((center[0] - reach - self.x0) / self.spacing_m))) + ihi = min(self.width - 1, + int(math.ceil((center[0] + reach - self.x0) / self.spacing_m))) + jlo = max(0, int(math.floor((center[1] - reach - self.x0) / self.spacing_m))) + jhi = min(self.width - 1, + int(math.ceil((center[1] + reach - self.x0) / self.spacing_m))) + limit_sq = reach * reach + doomed = [] + for i in range(ilo, ihi + 1): + dx = self.x0 + (i + 0.5) * self.spacing_m - center[0] + base = i * self.width + for j in range(jlo, jhi + 1): + index = base + j + if index not in self.alive: + continue + dy = self.x0 + (j + 0.5) * self.spacing_m - center[1] + if dx * dx + dy * dy <= limit_sq: + doomed.append(index) + self._discard(doomed) + + def _kill_outside_disk(self, center: Vec2, radius_m: float) -> None: + """删除格心距 center > radius_m 的全部幸存格（保留盘内）。""" + if not self.alive: + return + reach = radius_m + _DIST_EPS_M + doomed = [index for index in self.alive + if math.dist(self.cell_center(index), center) > reach] + self._discard(doomed) + + def _kill_outside_wedge(self, station: Vec2, reading_deg: float) -> None: + """删除角向偏离读数超出 δ_eff + 角向格裕量的幸存格。 + + 角向格裕量 = asin(m / max(dist−m, 1))：格内任一点相对格心的方位 + 偏差不超过它（点距测点还可再近 m，故分母再内缩 m）。 + """ + if not self.alive: + return + sx, sy = float(station[0]), float(station[1]) + doomed = [] + for index in self.alive: + gx, gy = self.cell_center(index) + dist = math.hypot(gx - sx, gy - sy) + if dist <= self.margin_m + 1.0: + continue # 格贴测点：格内方位可指向任意方向，不可删 + angular_margin = math.degrees( + math.asin(min(0.9999, self.margin_m / (dist - self.margin_m)))) + gap = angular_gap(math.degrees(math.atan2(gy - sy, gx - sx)), reading_deg) + if gap > self.half_angle_deg + angular_margin + _ANG_EPS_DEG: + doomed.append(index) + self._discard(doomed) + + def apply_measure(self, station: Vec2, result: str, + svd_deg: float | None = None) -> None: + """按读数更新幸存集。真源保留性由 Voronoi 裕量保证（模块注释）。""" + if result == NO_SIGNAL: + # no_signal ⟹ dist > r_eff >= 1000：整格落入 B(s, 1000−m) 的删。 + self._kill_disk(station, self.r_eff_min_m - self.margin_m) + elif result == NEAR: + self._kill_outside_disk(station, self.near_radius_m + self.margin_m) + elif result == DIRECTION: + if svd_deg is None: + raise ValueError("direction 读数必须携带 svd_deg") + # direction 隐含 dist <= r_max [题面] + self._kill_outside_disk(station, self.r_eff_max_m + self.margin_m) + self._kill_outside_wedge(station, svd_deg) + else: + raise ValueError(f"未知测向结果: {result!r}") + + # ------------------------------------------------------------ 查询 + def __len__(self) -> int: + return len(self.alive) + + def alive_points(self, limit: int | None = None) -> list[Vec2]: + sorted_alive = sorted(self.alive) + if limit is not None and len(sorted_alive) > limit: + step = len(sorted_alive) / limit + indices = {sorted_alive[int(k * step) % len(sorted_alive)] + for k in range(limit)} + indices = sorted(indices) + else: + indices = sorted_alive + return [self.cell_center(index) for index in indices] + + def nearest_alive_distance(self, point: Vec2, limit_m: float) -> float | None: + """最近幸存格心距离；> limit_m 时返回 None（粗格环扫 + 下界剪枝）。 + + 返回值语义：恰为最近幸存格心距离（若 <= limit_m）；None 表示所有 + 幸存格心都超出 limit_m。粗格聚合半径已计入扫描范围，不漏判。 + """ + if not self.alive: + return None + coarse_cell_m = self.spacing_m * _COARSE_FACTOR + coarse_reach = coarse_cell_m * math.sqrt(2) / 2 + cw = self._coarse_width() + ci_base = int(math.floor((point[0] - self.x0) / coarse_cell_m)) + cj_base = int(math.floor((point[1] - self.x0) / coarse_cell_m)) + ring_max = int(math.ceil((limit_m + coarse_reach) / coarse_cell_m)) + 1 + best: float | None = None + for ring in range(ring_max + 1): + lower_bound = max(0.0, ring - 1) * coarse_cell_m - coarse_reach + if best is not None and lower_bound >= best: + break # 更外环的所有格心都 >= 下界 >= 当前最优 + for ci in range(max(0, ci_base - ring), min(cw - 1, ci_base + ring) + 1): + for cj in range(max(0, cj_base - ring), min(cw - 1, cj_base + ring) + 1): + if max(abs(ci - ci_base), abs(cj - cj_base)) != ring: + continue # 只扫当前环（Chebyshev 环） + if (ci * cw + cj) not in self._coarse: + continue + ccx = self.x0 + (ci + 0.5) * coarse_cell_m + ccy = self.x0 + (cj + 0.5) * coarse_cell_m + if math.hypot(ccx - point[0], ccy - point[1]) \ + > limit_m + coarse_reach: + continue + i0, j0 = ci * _COARSE_FACTOR, cj * _COARSE_FACTOR + for i in range(i0, min(i0 + _COARSE_FACTOR, self.width)): + base = i * self.width + for j in range(j0, min(j0 + _COARSE_FACTOR, self.width)): + index = base + j + if index in self.alive: + dist = math.dist(self.cell_center(index), point) + if dist <= limit_m and (best is None or dist < best): + best = dist + return best + + def guaranteed_no_signal(self, station: Vec2) -> bool: + """真源与该测点距离必 > r_max（>= r_eff）⟹ 必 no_signal，测了也无信息。""" + return self.nearest_alive_distance( + station, self.r_eff_max_m + self.margin_m) is None + + def mec(self) -> tuple[Vec2, float]: + """幸存集最小包围圆（圆心, 半径）；按 version 缓存。""" + if self._mec is None: + if not self.alive: + raise ValueError("幸存集为空（该频道已可证明缺席）") + points = [self.cell_center(index) for index in self.alive] + self._mec = smallest_enclosing_circle(points) + return self._mec + + # ------------------------------------------------------------ 信息增益 + def subsample(self, size: int) -> list[Vec2]: + """按索引序均匀抽取的子采样（索引序≈空间序，代表点空间铺开）。""" + return self.alive_points(limit=size) + + def expected_survival(self, point: Vec2, subsample: list[Vec2], + representatives: list[Vec2]) -> float: + """E[log 幸存占比]：越小该测点的期望信息增益越大。 + + r_eff 不确定性解析积分：dist∈(1000,1500] 时按 r_eff~U(1000,1500) + 取 p_ns=(dist−1000)/500 为 no_signal 分支概率。 + - no_signal 分支：删 B(point, 1000−m)，幸存 = 子采样中盘外比例； + - direction 分支：以代表点方位为期望读数，角域 ∩ B(point,1500+m) 内幸存； + - near 分支（dist <= 5+m）：可行集收缩到 5m 盘，幸存占比趋近 0。 + """ + if not self.alive or not subsample: + return 0.0 + eps = 1.0 / len(subsample) + keep_ns = max(1.0 - self.fraction_within(subsample, point, + self.r_eff_min_m - self.margin_m), + eps) + log_keep_ns = math.log(keep_ns) + total = 0.0 + for rep in representatives: + dist = math.hypot(rep[0] - point[0], rep[1] - point[1]) + if dist <= self.near_radius_m + self.margin_m: + total += math.log(eps) + continue + p_ns = min(1.0, max(0.0, (dist - self.r_eff_min_m) + / (self.r_eff_max_m - self.r_eff_min_m))) + reading = math.atan2(rep[1] - point[1], rep[0] - point[0]) + survivors = 0 + for p in subsample: + if math.dist(p, point) > self.r_eff_max_m + self.margin_m: + continue + gap = angular_gap(math.degrees( + math.atan2(p[1] - point[1], p[0] - point[0])), + math.degrees(reading)) + if gap <= self.half_angle_deg + _ANG_EPS_DEG: + survivors += 1 + keep_dir = max(survivors / len(subsample), eps) + total += p_ns * log_keep_ns + (1.0 - p_ns) * math.log(keep_dir) + return total / max(len(representatives), 1) + + def fraction_within(self, samples: list[Vec2], point: Vec2, + radius_m: float) -> float: + if not samples: + return 0.0 + reach = radius_m + _DIST_EPS_M + count = sum(1 for p in samples if math.dist(p, point) <= reach) + return count / len(samples) + + def _branch_radius(self, members: list[Vec2]) -> float: + """分支后验的半径代理：幸存子采样到其质心的最大距离（O(n)，免 Welzl）。""" + if not members: + return 0.0 + mx = sum(p[0] for p in members) / len(members) + my = sum(p[1] for p in members) / len(members) + return max(math.hypot(p[0] - mx, p[1] - my) for p in members) + + def expected_posterior_radius(self, point: Vec2, subsample: list[Vec2], + representatives: list[Vec2]) -> float: + """一次测量后期望后验半径（米）：选点主指标，直指清除证书阈值。 + + 对每个代表假设（真值候选）按 r_eff~U(1000,1500) 解析积分 + no_signal/direction 分支，near 分支（dist <= 5+m）后验为 5m 盘； + 各分支半径用幸存子采样的质心最大距离近似（偏差 << 格距，排序用）。 + 与几何版 worst-posterior 同族，但作用于含负观测的非凸幸存集， + 且以期望代替最坏（代表点覆盖幸存集主轴）。 + """ + if not self.alive or not subsample: + return 0.0 + eps_radius = self.spacing_m / 2.0 + total = 0.0 + for rep in representatives: + dist = math.hypot(rep[0] - point[0], rep[1] - point[1]) + if dist <= self.near_radius_m + self.margin_m: + total += self.near_radius_m + self.margin_m + continue + p_ns = min(1.0, max(0.0, (dist - self.r_eff_min_m) + / (self.r_eff_max_m - self.r_eff_min_m))) + members_ns = [p for p in subsample + if math.dist(p, point) > self.r_eff_min_m - self.margin_m] + reading = math.atan2(rep[1] - point[1], rep[0] - point[0]) + members_dir = [] + for p in subsample: + if math.dist(p, point) > self.r_eff_max_m + self.margin_m: + continue + gap = angular_gap(math.degrees( + math.atan2(p[1] - point[1], p[0] - point[0])), + math.degrees(reading)) + if gap <= self.half_angle_deg + _ANG_EPS_DEG: + members_dir.append(p) + r_ns = self._branch_radius(members_ns) if members_ns else eps_radius + r_dir = self._branch_radius(members_dir) if members_dir else eps_radius + total += p_ns * r_ns + (1.0 - p_ns) * r_dir + return total / max(len(representatives), 1) diff --git a/src/mcm26b/planner/bayes_loop.py b/src/mcm26b/planner/bayes_loop.py new file mode 100644 index 0000000..8e566be --- /dev/null +++ b/src/mcm26b/planner/bayes_loop.py @@ -0,0 +1,429 @@ +"""P4b 贝叶斯网格滤波闭环（问题3新策略 ``bayes``）。 + +在 :class:`OmnidirectionalLoop` 骨架（run/异常收尾/summary/预算/异常留痕） +上的三层重构，对应 docs/algo-3-bayes.md 的架构： + +1. 感知层：每频道一个网格幸存集（particles.ChannelParticles），全部读数 + （含 no_signal 负观测）即时更新；幸存集为空即缺席证明，无需等覆盖账本； +2. 决策层：统一任务循环——站点扫频、定位测量、清除三类任务合成任务表， + 开放路径最近邻 + 2-opt 排序，每执行一个任务滚动重排（动态 TSP）； +3. 选点层：定位测量点在保证接收盘（凸证书）或 PF MEC 探索环（宽候选集） + 内按"期望 log 幸存占比 + λ·移动时间"评分（信息增益贪心）。 + +与基线 SevenBatch 的差异：负观测跳站/跳测（必 no_signal 的 (站,频道) 对 +不消耗检测与换频）、PF 缺席证明提前证毕、全局任务排序替代"站内最近邻"。 +清除证书仍以凸证书为主（PF 网格证书仅在负观测挖洞使幸存集更紧时提前 +触发），两者均满足 Voronoi/保守裕量，不牺牲全清率。 +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass + +from ..belief import ChannelStatus +from ..config import ProblemConfig, Vec2 +from ..coverage import CoverageLedger +from ..particles import ChannelParticles +from ..protocol.client import SimulatorClient +from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL +from ..protocol.timing import move_time +from . import (ClearCertificate, OmnidirectionalLoop, clear_certificate, + candidate_points, safe_disk_radius, select_measurement_point) +from .seven import seven_points + + +@dataclass(frozen=True) +class Task: + """统一任务表的一项。service_s 为不含移动的固有耗时（检测+换频/清除）。""" + + kind: str # "station" | "locate" | "clear" + position: Vec2 + service_s: float + channel: int | None = None + station_index: int | None = None + + +def _point_key(point: Vec2) -> tuple[int, int]: + """候选点去重键：0.5m 网格取整（同一物理点不重复测量）。""" + return (int(round(point[0] * 2)), int(round(point[1] * 2))) + + +class BayesLoop(OmnidirectionalLoop): + """问题3贝叶斯闭环：负观测感知 + 信息增益选点 + 滚动任务排序。""" + + def __init__(self, client: SimulatorClient, + config: ProblemConfig = ProblemConfig()): + super().__init__(client, config) + self.ledger = CoverageLedger( + config, points=seven_points(config.bayes.station_ring_radius_m)) + self.particles = {channel: ChannelParticles(config) + for channel in self.belief.channels} + self._measured_points: dict[int, set[tuple[int, int]]] = {} + self._clear_failed: set[int] = set() # 证书清除失败后须先补测再重试 + self._stall: dict[int, int] = {} # 连续无收缩计数（异常留痕用） + self._skip_cache: dict[tuple[int, int], tuple[int, bool]] = {} + self.skipped_measures = 0 # 跳测规则省下的检测次数 + self.no_shrink_measures = 0 # 未收缩幸存集的定位检测（诊断用） + self.pf_absent_channels: list[int] = [] # 由幸存集为空证明缺席的频道 + + # ------------------------------------------------------------ 主循环 + def _coverage_phase(self) -> None: + """统一任务循环：发现 + 定位 + 清除滚动调度，直到任务表为空。""" + while self.stop_reason is None: + if self._count_limit_reached() or self._virtual_cap_reached(): + return + if not self._step_once(): + return + + def _step_once(self) -> bool: + """构建任务表、排序并执行首任务；无可执行任务返回 False。""" + tasks = self._build_tasks() + if not tasks: + return False + order = self._order_tasks(tasks) + self._execute_task(order[0]) + return True + + # ------------------------------------------------------------ 任务构建 + def _station_pending(self, index: int) -> list[int]: + """站点待测频道：UNKNOWN、未测过、非"必 no_signal"且未证缺席。""" + point = self.ledger.points[index] + pending = [] + for channel, belief in self.belief.channels.items(): + if belief.status is not ChannelStatus.UNKNOWN: + continue + if self.ledger.measured_at(channel, index): + continue + if self._skip_station_channel(channel, index, point): + continue + pending.append(channel) + return pending + + def _skip_station_channel(self, channel: int, index: int, point: Vec2) -> bool: + """跳测规则（按 pf.version 缓存；幸存集单调收缩，判定单调）。""" + pf = self.particles[channel] + if not len(pf): + return True + cached = self._skip_cache.get((channel, index)) + if cached is not None and cached[0] == pf.version: + return cached[1] + value = pf.guaranteed_no_signal(point) + self._skip_cache[(channel, index)] = (pf.version, value) + return value + + def _build_tasks(self) -> list[Task]: + tasks: list[Task] = [] + for index, point in enumerate(self.ledger.points): + pending = self._station_pending(index) + if not pending: + continue + switches = len(pending) - (1 if self.client.state.channel in pending else 0) + tasks.append(Task("station", tuple(point), + self.config.cost.measure_s * len(pending) + + self.config.cost.switch_s * max(switches, 0), + station_index=index)) + for channel in self.belief.channels_by_status(ChannelStatus.DETECTED): + cert = self._best_certificate(channel) + if cert is not None and cert.certified \ + and channel not in self._clear_failed: + tasks.append(Task("clear", cert.center, + self.config.cost.clear_success_s, + channel=channel)) + continue + point = self._select_point(channel) + if point is None: + continue + tasks.append(Task("locate", point, + self.config.cost.measure_s + self.config.cost.switch_s, + channel=channel)) + return tasks + + # ------------------------------------------------------------ 任务排序 + def _order_tasks(self, tasks: list[Task]) -> list[Task]: + """开放路径排序：最近邻打底，2-opt 改进（终点自由，不回起点）。""" + if len(tasks) <= 2: + return sorted(tasks, key=lambda t: ( + move_time(self.client.state.position, t.position, self.config.cost), + t.kind, t.channel or 0)) + order = self._nearest_order(tasks) + if not self.config.bayes.two_opt_enabled: + return order + return self._two_opt(order) + + def _nearest_order(self, tasks: list[Task]) -> list[Task]: + remaining = list(tasks) + order = [] + position = self.client.state.position + while remaining: + best = min(remaining, key=lambda t: ( + move_time(position, t.position, self.config.cost), + t.kind, t.channel or 0)) + remaining.remove(best) + order.append(best) + position = best.position + return order + + def _path_cost(self, order: list[Task]) -> float: + total = 0.0 + position = self.client.state.position + for task in order: + total += move_time(position, task.position, self.config.cost) + total += task.service_s + position = task.position + return total + + def _two_opt(self, order: list[Task]) -> list[Task]: + best = list(order) + best_cost = self._path_cost(best) + for _ in range(self.config.bayes.two_opt_passes): + improved = False + for i in range(len(best) - 1): + for j in range(i + 1, len(best)): + candidate = best[:i] + best[i:j + 1][::-1] + best[j + 1:] + cost = self._path_cost(candidate) + if cost < best_cost - 1e-6: + best, best_cost, improved = candidate, cost, True + if not improved: + break + return best + + # ------------------------------------------------------------ 任务执行 + def _execute_task(self, task: Task) -> None: + if task.kind == "station": + self._execute_station(task.station_index) + elif task.kind == "clear": + cert = self._best_certificate(task.channel) + if cert is not None and cert.certified: + if not self._attempt_clear(task.channel, cert.center, cert): + self._clear_failed.add(task.channel) + self._may_clear.discard(task.channel) + else: + self._clear_failed.discard(task.channel) + elif task.kind == "locate": + self._execute_locate(task.channel, task.position) + + def _execute_station(self, index: int) -> None: + """单站批量扫频：只测待测频道（跳测规则已过滤），扫完即返回重排。""" + point = self.ledger.points[index] + pending = self._station_pending(index) + unknown = [c for c, b in self.belief.channels.items() + if b.status is ChannelStatus.UNKNOWN] + self.skipped_measures += len(unknown) - len(pending) + # 当前频道优先，减少换频；同站 near 直接清除不打断扫频 + pending.sort(key=lambda c: (c != self.client.state.channel, c)) + for channel in pending: + if self.stop_reason is not None or self._count_limit_reached() \ + or self._virtual_cap_reached(): + return + outcome = self.client.measure(point, channel) + self.ledger.mark_measured(channel, index) + self._record_measure(channel, point, outcome) + if outcome.result in (DIRECTION, NEAR): + self._may_clear.add(channel) + if outcome.result == NEAR: + region = self.belief.candidate_set(channel) + cert = clear_certificate(region, channel, self.config) + if cert.certified: + if self._attempt_clear(channel, point, cert): + self._clear_failed.discard(channel) + else: + self._clear_failed.add(channel) + self._may_clear.discard(channel) + else: + self._anomaly("near_without_certificate", channel, position=point) + + def _execute_locate(self, channel: int, point: Vec2) -> None: + """单步定位测量：执行后立即重排（滚动时域），收敛则顺路清除。""" + pf = self.particles[channel] + before = len(pf) + outcome = self.client.measure(point, channel) + self._record_measure(channel, point, outcome) + self._may_clear.add(channel) + if outcome.result == NO_SIGNAL: + region = self.belief.candidate_set(channel) + if region is not None and safe_disk_radius(region, self.config) >= 0 \ + and math.dist(point, region.mec()[0]) \ + <= safe_disk_radius(region, self.config): + self._anomaly("no_signal_in_safe_disk", channel, position=point) + after = len(pf) + cert = self._best_certificate(channel) + if cert is not None and cert.certified: + if self._attempt_clear(channel, cert.center, cert): + self._clear_failed.discard(channel) + self._stall.pop(channel, None) + return + self._clear_failed.add(channel) + self._may_clear.discard(channel) + if after == before: + self._stall[channel] = self._stall.get(channel, 0) + 1 + self.no_shrink_measures += 1 # 只是浪费不破坏守恒性，不进异常表 + else: + self._stall[channel] = 0 + + # ------------------------------------------------------------ 感知与证书 + def _record_measure(self, channel: int, position: Vec2, outcome) -> None: + """belief（凸证书）与 PF（幸存集）同步更新；PF 空即证缺席。""" + self.belief.update_measure(channel, position, outcome.result, + outcome.svd_deg) + self._measured_points.setdefault(channel, set()).add(_point_key(position)) + self._clear_failed.discard(channel) # 新证据落地：允许再次尝试清除 + belief = self.belief.channels[channel] + if belief.status in (ChannelStatus.CLEARED, ChannelStatus.ABSENT): + return # 已证毕：清除/缺席后的 no_signal 与距离无关，不进 PF + pf = self.particles[channel] + pf.apply_measure(position, outcome.result, outcome.svd_deg) + if not len(pf): + if belief.status is ChannelStatus.UNKNOWN: + self.belief.mark_absent(channel) + if channel not in self.pf_absent_channels: + self.pf_absent_channels.append(channel) + else: + self._anomaly("pf_empty_while_detected", channel, position=position) + + def _best_certificate(self, channel: int) -> ClearCertificate | None: + """凸证书优先；负观测挖洞使 PF 幸存集更紧时用网格证书补位。""" + region = self.belief.candidate_set(channel) + cert = clear_certificate(region, channel, self.config) \ + if region is not None else None + if cert is not None and cert.certified: + return cert + if self.config.bayes.grid_cert_enabled: + pf = self.particles[channel] + if len(pf): + center, radius = pf.mec() + limit = (self.config.field.clear_radius_m - pf.margin_m + - self.config.search.clear_margin_m) + if radius <= limit: + return ClearCertificate(channel, center, radius, limit, True) + return cert + + # ------------------------------------------------------------ 信息增益选点 + def _select_point(self, channel: int) -> Vec2 | None: + """定位测量点：期望 log 幸存占比 + λ·移动时间 最小的候选。""" + bayes, search = self.config.bayes, self.config.search + pf = self.particles[channel] + if not len(pf): + return None + region = self.belief.candidate_set(channel) + center, radius = pf.mec() + measured = self._measured_points.get(channel, set()) + candidates: list[Vec2] = [] + safe = safe_disk_radius(region, self.config) if region is not None else -1.0 + if region is not None and safe >= 0: + # 保证接收盘（凸证书口径）：任意候选必产生 direction/near + candidates = candidate_points(tuple(region.mec()[0]), safe, + search.candidate_rings, + search.candidate_angles) + else: + # 宽候选集：PF MEC 探索环（负观测挖洞后幸存集的包围圆） + for fraction in bayes.wide_rings: + ring = max(radius * fraction, 2.0 * pf.margin_m) + candidates.extend(candidate_points(center, ring, (1.0,), + search.candidate_angles)) + candidates.append(tuple(center)) + candidates = [c for c in candidates if _point_key(c) not in measured] + if not candidates: + return None + subsample = pf.subsample(bayes.score_subsample) + stride = max(len(subsample) // bayes.mc_representatives, 1) + representatives = [subsample[k] + for k in range(0, len(subsample), stride)] + position = self.client.state.position + + def score(q: Vec2) -> float: + move = bayes.lambda_move_s * move_time(position, q, self.config.cost) + if bayes.score_mode == "entropy": + return (pf.expected_survival(q, subsample, representatives) + + move) + return (pf.expected_posterior_radius(q, subsample, representatives) + + move) + + return min(candidates, key=lambda q: (score(q), _point_key(q))) + + # ------------------------------------------------------------ 安全网与复扫 + def _localize_and_clear(self, channel: int) -> bool: + """阻塞式定位子循环（resolve 阶段安全网）：PF 选点，失败退凸选点。""" + search = self.config.search + self._may_clear.add(channel) + for _ in range(search.max_localize_steps): + if self._virtual_cap_reached(): + return False + cert = self._best_certificate(channel) + if cert is not None and cert.certified and channel in self._may_clear: + if self._attempt_clear(channel, cert.center, cert): + self._clear_failed.discard(channel) + return True + self._clear_failed.add(channel) + self._may_clear.discard(channel) + continue + point = self._select_point(channel) + if point is None: + region = self.belief.candidate_set(channel) + if region is None: + return False + point = select_measurement_point( + region, self.client.state.position, self.config).position + outcome = self.client.measure(point, channel) + self._record_measure(channel, point, outcome) + self._may_clear.add(channel) + if outcome.result == NO_SIGNAL: + region = self.belief.candidate_set(channel) + if region is not None and safe_disk_radius(region, self.config) >= 0 \ + and math.dist(point, region.mec()[0]) \ + <= safe_disk_radius(region, self.config): + self._anomaly("no_signal_in_safe_disk", channel, position=point) + cert = self._best_certificate(channel) + if cert is not None and cert.certified: + if self._attempt_clear(channel, cert.center, cert): + self._clear_failed.discard(channel) + return True + self._clear_failed.add(channel) + self._may_clear.discard(channel) + return self.belief.channels[channel].status is ChannelStatus.CLEARED + + def _persistent_round(self) -> None: + """一轮复扫：与基线一致的全站复测，但读数同步进 PF（防假设偏差）。""" + measured: set[tuple[int, int]] = set() + while self.stop_reason is None: + index = self._next_resweep_point(measured) + if index is None: + return + point = self.ledger.points[index] + for channel in self._resweep_channels_at(index, measured): + if self._virtual_cap_reached(): + return + measured.add((channel, index)) + was_absent = (self.belief.channels[channel].status + is ChannelStatus.ABSENT) + outcome = self.client.measure(point, channel) + self._record_measure(channel, point, outcome) + if was_absent and outcome.result in (DIRECTION, NEAR): + # 复测推翻了缺席证明：守恒性留痕，按新证据定位清除。 + self._anomaly("absent_detected_on_resweep", channel, + position=point, result=outcome.result) + if outcome.result == DIRECTION: + self._localize_and_clear(channel) + elif outcome.result == NEAR: + cert = self._best_certificate(channel) + if cert is not None and cert.certified: + if not self._attempt_clear(channel, cert.center, cert): + self._clear_failed.add(channel) + else: + self._anomaly("near_without_certificate", channel, + position=point) + if self.stop_reason is not None: + return + + # ------------------------------------------------------------ 收尾 + def _summary(self, info) -> dict: + report = super()._summary(info) + report["strategy"] = "bayes" + report["skipped_measures"] = self.skipped_measures + report["no_shrink_measures"] = self.no_shrink_measures + report["pf_absent_channels"] = sorted(self.pf_absent_channels) + report["pf_alive_final"] = { + str(channel): len(self.particles[channel]) + for channel in sorted(self.particles) + if self.belief.channels[channel].status is ChannelStatus.DETECTED} + return report diff --git a/src/mcm26b/planner/seven.py b/src/mcm26b/planner/seven.py new file mode 100644 index 0000000..2c69aed --- /dev/null +++ b/src/mcm26b/planner/seven.py @@ -0,0 +1,93 @@ +"""P4 七点覆盖策略族：解析证明的六边形发现站 + 同站批量扫频。 + +从 scripts/benchmark_q3_search.py 提升为库代码（A2 基线，官方演练 +12/12 全清、虚拟 5232s）。覆盖几何：原点 + 半径 RING_RADIUS_M 环上 +6 等分点；切换最近站的半径 a/sqrt(3)，域内任意点到最近站距离的解析 +上界 max(R/sqrt(3), sqrt(Rom³/(sqrt(3)·Rring)+...)) < 1000m [题面 +r_eff 下界]，故任意合法源必被至少一站检出——缺席证明只需该频道 +7 站全 no_signal（CoverageLedger.coverage_complete）。 + +调度变体： +- SevenImmediate：站内即时进入定位子循环（A1）； +- SevenBatch：站内只扫频，扫完按"距候选集 MEC 圆心最近"服务队列（A2）； +- SevenSweep：全域扫完再统一服务（A3）。 +""" + +from __future__ import annotations + +import math + +from ..belief import ChannelStatus +from ..config import ProblemConfig +from ..coverage import CoverageLedger +from ..protocol.rules import NEAR +from . import OmnidirectionalLoop, clear_certificate + +RING_RADIUS_M = 1200.0 # [工程] 解析证明见 docs/algo-3.md §5 与 geometry_check + + +def seven_points(ring_radius_m: float = RING_RADIUS_M) -> list[tuple[float, float]]: + """原点 + 环上 6 等分点的七站覆盖集。""" + return [(0.0, 0.0)] + [(ring_radius_m * math.cos(k * math.pi / 3), + ring_radius_m * math.sin(k * math.pi / 3)) for k in range(6)] + + +class SevenImmediate(OmnidirectionalLoop): + """七点覆盖 + 站内即时定位（基类覆盖阶段逻辑不变，仅换站集）。""" + + def __init__(self, client, config: ProblemConfig = ProblemConfig(), + ring_radius_m: float = RING_RADIUS_M): + super().__init__(client, config) + self.ledger = CoverageLedger(config=config, points=seven_points(ring_radius_m)) + + +class SevenBatch(SevenImmediate): + """站内只扫频，扫完按最近候选集圆心顺序服务检测队列（A2 主策略）。""" + + defer_all = False + + def _serve_queue(self): + # Each detected channel gets at most one localization call in this pass. + queue = self.belief.channels_by_status(ChannelStatus.DETECTED) + while queue: + c = min(queue, key=lambda c: (math.dist(self.client.state.position, + self.belief.candidate_set(c).mec()[0]), c)) + queue.remove(c) + self._localize_and_clear(c) + if self._count_limit_reached(): + break + + def _coverage_phase(self): + while not self._count_limit_reached(): + index = self._next_point_index() + if index is None: + return + point = self.ledger.points[index] + pending = self._pending_channels_at(index) + # Keeping the current measuring channel first saves a switch when possible. + pending.sort(key=lambda c: (c != self.client.state.channel, c)) + for c in pending: + outcome = self.client.measure(point, c) + self.ledger.mark_measured(c, index) + self.belief.update_measure(c, point, outcome.result, outcome.svd_deg) + if outcome.result == NEAR: + cert = clear_certificate(self.belief.candidate_set(c), c, self.config) + # Same-station near clear; do not displace the station scan. + if cert.certified: + self._attempt_clear(c, point, cert) + if self._count_limit_reached(): + return + if not self.defer_all: + self._serve_queue() + + def _resolve_detected(self): + self._serve_queue() + unresolved = [c for c, b in self.belief.channels.items() + if b.status not in (ChannelStatus.CLEARED, ChannelStatus.ABSENT)] + return "all_resolved" if not unresolved else "unresolved_channels" + + +class SevenSweep(SevenBatch): + """Finish discovery at all necessary stations before servicing the queue.""" + + defer_all = True diff --git a/tests/test_bayes_loop_q3.py b/tests/test_bayes_loop_q3.py new file mode 100644 index 0000000..73f0f0a --- /dev/null +++ b/tests/test_bayes_loop_q3.py @@ -0,0 +1,154 @@ +"""P4b 贝叶斯闭环端到端验收：全清、PF 缺席证明、预算与故障。 + +对齐 test_loop_q3.py 的验收口径（docs/algo-3-bayes.md）： +- 10/16 源场景全部清除且守恒性零异常（凸证书 + 网格证书均不得失效）； +- PF 缺席证明（幸存集为空）与覆盖账本口径一致，不误判有源频道； +- 无源场景全部频道证明缺席后正常退出（persistent 关闭时 all_resolved）； +- 近圆域边界源（r_eff=1000）不漏搜； +- 持续复扫（persistent 默认开）：找全后复扫到虚拟安全余量， + accounted_virtual_time_s 记录首次证明找全时刻； +- 网络故障幂等重试不污染闭环结果； +- 虚拟时刻逐步复算一致。 +""" + +import math +import unittest +from dataclasses import replace + +from mcm26b.config import ProblemConfig +from mcm26b.evaluation import recompute_virtual_time +from mcm26b.planner.bayes_loop import BayesLoop +from mcm26b.protocol.client import SimulatorClient +from mcm26b.sim import (LocalSimTransport, LocalSimulator, + ScriptedFaultTransport, Source) +from mcm26b.sim.scenes import random_omni_scene + +CONFIG = replace(ProblemConfig(), + search=replace(ProblemConfig().search, + bearing_rounding_margin_deg=0.005)) +SMALL_CAP = replace(CONFIG, + field=replace(CONFIG.field, max_virtual_duration_s=60_000.0)) +NO_PERSIST = replace(CONFIG, search=replace(CONFIG.search, + persistent_search=False)) + + +def _run_scene(sources, fault_script=None, config=SMALL_CAP): + simulator = LocalSimulator(sources, robot_id="T001", config=config) + transport = LocalSimTransport(simulator) + if fault_script is not None: + transport = ScriptedFaultTransport(transport, fault_script) + client = SimulatorClient("T001", config=config, transport=transport) + loop = BayesLoop(client, config) + summary = loop.run() + report = recompute_virtual_time(client.log.action_events()) + return summary, report, simulator + + +class BayesLoopAcceptanceTests(unittest.TestCase): + def test_ten_sources_all_cleared(self): + sources = random_omni_scene(seed=5, count=10, config=CONFIG) + summary, report, simulator = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(summary["unresolved_channels"], []) + self.assertIsNotNone(summary["accounted_virtual_time_s"]) + self.assertEqual(summary["anomalies"], [], "守恒性或证书不应被打破") + self.assertEqual(report["mismatches"], []) + self.assertEqual(sorted(simulator.cleared), truth) + self.assertEqual(summary["exit_reason"], "user_exit") + self.assertEqual(summary["strategy"], "bayes") + + def test_sixteen_sources_stop_by_count_bound(self): + sources = random_omni_scene(seed=6, count=16, config=CONFIG) + summary, report, simulator = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "sixteen_cleared") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(report["mismatches"], []) + self.assertEqual(summary["anomalies"], []) + + def test_empty_arena_all_absent(self): + """0 源：每个频道都必须由 PF 排除盘证明缺席后正常退出。""" + summary, report, simulator = _run_scene([], config=NO_PERSIST) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "all_resolved") + self.assertEqual(sorted(summary["absent_channels"]), + list(range(1, 21))) + self.assertEqual(summary["pf_absent_channels"], list(range(1, 21))) + self.assertEqual(summary["clears"], 0) + self.assertEqual(summary["anomalies"], []) + self.assertEqual(report["mismatches"], []) + + def test_boundary_sources_detected_and_cleared(self): + """近圆域边界源（r_eff=1000）：七站覆盖半径 968.9m 保证发现。""" + sources = [Source(c + 1, 1800.0 * math.cos(math.pi / 6 + 2 * math.pi * c / 4), + 1800.0 * math.sin(math.pi / 6 + 2 * math.pi * c / 4), + 1000.0) for c in range(4)] + summary, report, simulator = _run_scene(sources, config=NO_PERSIST) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(summary["anomalies"], []) + self.assertEqual(report["mismatches"], []) + + def test_persistent_phase_resweeps_until_cap(self): + """找全后持续复扫到虚拟安全余量 [用户决策 2026-09-11]。""" + sources = random_omni_scene(seed=5, count=10, config=CONFIG) + summary, _report, _simulator = _run_scene(sources, config=SMALL_CAP) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "virtual_time_cap") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertGreater(summary["measures"], 7 * 20, + "找全后应继续复扫未清除频道") + + def test_transport_fault_retries_keep_session_clean(self): + sources = random_omni_scene(seed=5, count=10, config=CONFIG) + script = [("drop_before", 1)] # 首个请求丢弃一次：幂等重试路径 + summary, report, simulator = _run_scene(sources, fault_script=script) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(report["mismatches"], []) + + def test_skip_rule_never_skips_detectable_channel(self): + """跳测规则只允许跳过"必 no_signal"的 (站,频道) 对。""" + sources = random_omni_scene(seed=5, count=10, config=CONFIG) + simulator = LocalSimulator(sources, robot_id="T001", config=CONFIG) + client = SimulatorClient("T001", config=CONFIG, + transport=LocalSimTransport(simulator)) + loop = BayesLoop(client, CONFIG) + loop.run() + from mcm26b.belief import ChannelStatus + for index, point in enumerate(loop.ledger.points): + for channel, belief in loop.belief.channels.items(): + if belief.status is not ChannelStatus.UNKNOWN: + continue + skipped = loop._skip_station_channel(channel, index, point) + if skipped and len(loop.particles[channel]): + nearest = loop.particles[channel].nearest_alive_distance( + point, loop.particles[channel].r_eff_max_m + + loop.particles[channel].margin_m) + self.assertIsNone( + nearest, f"频道 {channel} 站点 {index} 被跳过但可能检出") + + def test_cli_strategy_registry(self): + """CLI --strategy 注册表：bayes/seven 用七站，grid 用 49 点网格。""" + import mcm26b.cli as cli + from mcm26b.coverage import coverage_points + from mcm26b.planner.seven import SevenBatch, seven_points + self.assertIs(cli.Q3_STRATEGIES["bayes"], BayesLoop) + for name, loop_cls in cli.Q3_STRATEGIES.items(): + simulator = LocalSimulator([], robot_id="T001", config=CONFIG) + client = SimulatorClient("T001", config=CONFIG, + transport=LocalSimTransport(simulator)) + loop = loop_cls(client, CONFIG) + expected = seven_points() if name in ("bayes", "seven") \ + else coverage_points(CONFIG) + self.assertEqual(loop.ledger.points, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_particles.py b/tests/test_particles.py new file mode 100644 index 0000000..e3bea1b --- /dev/null +++ b/tests/test_particles.py @@ -0,0 +1,172 @@ +"""P4b 网格粒子滤波验收：真源保留定理、证书正确性与查询原语。 + +核心不变式（docs/algo-3-bayes.md §2）： +- 真源保留：任意观测序列（|误差| <= 1°、r_eff ∈ [1000,1500]、题面语义） + 后，真源所在格必在幸存集中——由此 MEC 证书与缺席证明才严格成立； +- 缺席证明：无源频道在排除盘覆盖 Ω 后幸存集必须清空； +- 查询：nearest_alive_distance 与暴力扫描一致；version 单调递增。 +""" + +import math +import unittest +from dataclasses import replace +from random import Random + +from mcm26b.config import ProblemConfig +from mcm26b.geometry.localization import smallest_enclosing_circle +from mcm26b.particles import ChannelParticles, angular_gap +from mcm26b.protocol.rules import DIRECTION, NEAR, NO_SIGNAL +from mcm26b.planner.seven import seven_points + +CONFIG = ProblemConfig() +rng_shared = Random(99) + + +def _make_particle(spacing_m=25.0): + return ChannelParticles(CONFIG, spacing_m=spacing_m) + + +def _cell_index(particle, point): + i = int(math.floor((point[0] - particle.x0) / particle.spacing_m)) + j = int(math.floor((point[1] - particle.x0) / particle.spacing_m)) + return i * particle.width + j + + +def _observe(source, station, rng, cleared=False): + """按题面语义产生一次读数（误差在 [-1°,1°] 内随机，含两位小数舍入）。""" + if cleared: + return NO_SIGNAL, None + dist = math.dist(station, (source[0], source[1])) + if dist > source[2]: + return NO_SIGNAL, None + if dist <= CONFIG.field.near_radius_m: + return NEAR, None + true_bearing = math.degrees(math.atan2(source[1] - station[1], + source[0] - station[0])) + error = (2.0 * rng.random() - 1.0) * CONFIG.field.bearing_error_half_deg + return DIRECTION, round(true_bearing + error, 2) % 360.0 + + +class TruthPreservationTests(unittest.TestCase): + def test_truth_cell_survives_arbitrary_sequences(self): + """随机源 × 随机测点 × 随机合法误差：真源所在格永不被删。""" + rng = Random(20260911) + for trial in range(8): + particle = _make_particle() + radius = CONFIG.field.omega_radius_m * math.sqrt(rng.random()) + angle = rng.uniform(0.0, 2.0 * math.pi) + source = (radius * math.cos(angle), radius * math.sin(angle), + rng.uniform(1000.0, 1500.0)) + truth_cell = _cell_index(particle, source) + self.assertIn(truth_cell, particle.alive) + stations = [(rng.uniform(-1800, 1800), rng.uniform(-1800, 1800)) + for _ in range(25)] + seven_points() + for station in stations: + result, svd = _observe(source, station, rng) + particle.apply_measure(station, result, svd) + self.assertIn(truth_cell, particle.alive, + f"trial {trial} station {station}: 真源格被删") + self.assertTrue(particle.alive, + "含源频道幸存集不允许为空") + # 真源距幸存集 <= Voronoi 裕量（清除证书的几何依据） + nearest = min(math.dist(p, source[:2]) for p in + (particle.cell_center(i) for i in particle.alive)) + self.assertLessEqual(nearest, particle.margin_m + 1e-6) + + def test_absence_proof_when_exclusions_cover_domain(self): + """无源频道：七站 no_signal 的排除盘覆盖 Ω，幸存集必须清空。""" + particle = _make_particle() + for station in seven_points(): + particle.apply_measure(station, NO_SIGNAL, None) + self.assertEqual(len(particle), 0, + "七站覆盖半径 968.9m < 1000−m，缺席必须可证") + + def test_direction_result_never_empties_feasible_set(self): + """direction 分支（角域 ∩ B(s,1500)）不允许把含源频道剪空。""" + rng = Random(7) + particle = _make_particle() + source = (900.0, -700.0, 1200.0) + for _ in range(40): + station = (rng.uniform(-1700, 1700), rng.uniform(-1700, 1700)) + result, svd = _observe(source, station, rng) + particle.apply_measure(station, result, svd) + if result == DIRECTION: + self.assertTrue(particle.alive) + + +class CertificateTests(unittest.TestCase): + def test_clear_certificate_implies_clear_success(self): + """证书蕴含关系：MEC(幸存)+裕量 <= 20−0.5 ⟹ 真源距清除点 <= 20。 + + 用小测试场（omega=200m）+ 细网格（h=5m）使证书可达：两次方向 + 交会后 near 读数把可行集收缩到 5m 盘，网格证书触发，然后按 + /clear 语义验证真源到证书圆心的距离不超过 20m。 + """ + small = replace(CONFIG, field=replace(CONFIG.field, + omega_radius_m=200.0)) + particle = ChannelParticles(small, spacing_m=5.0) + limit = (CONFIG.field.clear_radius_m - particle.margin_m + - CONFIG.search.clear_margin_m) + source = (60.0, 40.0, 1500.0) # r_eff 取题面上界；小场内必收得到信号 + # 两个远站方向交会（读数带 ±1° 合法误差），可行集收缩为小菱形 + for station in ((-150.0, -40.0), (40.0, -150.0)): + result, svd = _observe(source, station, rng_shared) + particle.apply_measure(station, result, svd) + self.assertTrue(particle.alive) + # near：源距站 3m（<=5m），可行集收缩到 B(station, 5+裕量) + station = (source[0] + 3.0, source[1]) + particle.apply_measure(station, NEAR, None) + center, radius = particle.mec() + self.assertLessEqual(radius, limit, + "5m 盘收缩后网格证书必须可触发") + self.assertLessEqual(radius + particle.margin_m + + CONFIG.search.clear_margin_m, + CONFIG.field.clear_radius_m) + self.assertLessEqual(math.dist(center, source[:2]), + CONFIG.field.clear_radius_m, + "证书触发时真源到清除点必须 <= 20m") + + def test_mec_contains_all_alive_cells(self): + particle = _make_particle() + particle.apply_measure((400.0, 300.0), DIRECTION, 25.0) + center, radius = particle.mec() + for point in particle.alive_points(): + self.assertLessEqual(math.dist(point, center), radius + 1e-6) + + +class QueryPrimitiveTests(unittest.TestCase): + def test_nearest_alive_distance_matches_bruteforce(self): + rng = Random(3) + particle = _make_particle() + for _ in range(6): + station = (rng.uniform(-1500, 1500), rng.uniform(-1500, 1500)) + particle.apply_measure(station, NO_SIGNAL, None) + for _ in range(30): + probe = (rng.uniform(-1900, 1900), rng.uniform(-1900, 1900)) + limit = rng.uniform(100.0, 2500.0) + got = particle.nearest_alive_distance(probe, limit) + brute = min((math.dist(p, probe) for p in particle.alive_points()), + default=None) + if brute is not None and brute <= limit: + self.assertIsNotNone(got) + self.assertAlmostEqual(got, brute, delta=1e-6) + else: + self.assertIsNone(got, + f"limit={limit} brute={brute} 应返回 None") + + def test_version_and_mec_cache(self): + particle = _make_particle() + before = particle.version + particle.apply_measure((0.0, 0.0), NO_SIGNAL, None) + self.assertGreater(particle.version, before) + mec_once = particle.mec() + self.assertIs(particle.mec(), mec_once, "version 不变时 MEC 应命中缓存") + + def test_angular_gap(self): + self.assertAlmostEqual(angular_gap(350.0, 10.0), 20.0) + self.assertAlmostEqual(angular_gap(10.0, 350.0), 20.0) + self.assertAlmostEqual(angular_gap(45.0, 45.0), 0.0) + + +if __name__ == "__main__": + unittest.main()