feat(q3): 贝叶斯网格粒子滤波闭环(PF负观测+信息增益选点+2-opt调度)

- particles.py: 网格幸存集滤波,Voronoi 裕量保证真源保留(缺席证明/网格清除证书/跳测规则)
- planner/bayes_loop.py: 统一任务循环(站点/定位/清除滚动 2-opt 排序)、期望后验半径选点
- planner/seven.py: 七点覆盖策略族提升为库代码
- cli: mcm26b run --strategy {grid,seven,bayes}
- BayesConfig 参数组;tests +24 项(真源保留/证书/端到端),129 全绿
This commit is contained in:
Horldsence
2026-09-11 09:02:28 +08:00
parent 7930eb65bf
commit e0665c73fb
9 changed files with 1335 additions and 67 deletions
@@ -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 裕量定理 + 随机化真源保留性测试兜底
+6 -66
View File
@@ -14,80 +14,20 @@ from pathlib import Path
from statistics import mean, median from statistics import mean, median
from time import perf_counter 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.config import ProblemConfig
from mcm26b.coverage import CoverageLedger from mcm26b.coverage import CoverageLedger # noqa: F401
from mcm26b.planner import OmnidirectionalLoop, clear_certificate 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.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 import LocalSimulator, LocalSimTransport, Source
from mcm26b.sim.scenes import random_omni_scene 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] 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, VARIANTS = {"grid49_immediate": OmnidirectionalLoop, "seven_immediate": SevenImmediate,
"seven_batch": SevenBatch, "seven_sweep": SevenSweep} "seven_batch": SevenBatch, "seven_sweep": SevenSweep}
+12 -1
View File
@@ -17,12 +17,16 @@ from pathlib import Path
from .config import ProblemConfig from .config import ProblemConfig
from .geometry.__main__ import main as q1_main from .geometry.__main__ import main as q1_main
from .planner import DirectionalExtension, OmnidirectionalLoop from .planner import DirectionalExtension, OmnidirectionalLoop
from .planner.bayes_loop import BayesLoop
from .planner.seven import SevenBatch
from .protocol.client import SimulatorClient from .protocol.client import SimulatorClient
from .protocol.log import SessionLog from .protocol.log import SessionLog
from .protocol.rules import EXIT_USER from .protocol.rules import EXIT_USER
from .sim import LocalSimTransport, LocalSimulator, Source from .sim import LocalSimTransport, LocalSimulator, Source
from .sim.scenes import mixed_scene, random_omni_scene from .sim.scenes import mixed_scene, random_omni_scene
Q3_STRATEGIES = {"grid": OmnidirectionalLoop, "seven": SevenBatch, "bayes": BayesLoop}
def _sim_check(args) -> int: def _sim_check(args) -> int:
"""附件1表2/附件2第10节计时示例:期望 105/111/194/199 秒。""" """附件1表2/附件2第10节计时示例:期望 105/111/194/199 秒。"""
@@ -83,9 +87,13 @@ def _run(args) -> int:
"/enter 返回的 remaining_real_duration_s 为准。", file=sys.stderr) "/enter 返回的 remaining_real_duration_s 为准。", file=sys.stderr)
log = SessionLog(path=Path(args.log)) if args.log else SessionLog() log = SessionLog(path=Path(args.log)) if args.log else SessionLog()
client = SimulatorClient(args.robot_id, config=config, transport=transport, log=log) 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 = loop_cls(client, config).run()
summary["robot_id"] = args.robot_id summary["robot_id"] = args.robot_id
summary["strategy"] = ("directional" if args.problem == 4 else args.strategy)
if real_budget_s is not None: if real_budget_s is not None:
summary["self_test_real_budget_s"] = real_budget_s summary["self_test_real_budget_s"] = real_budget_s
if args.summary_steps is not None and len(summary.get("steps", [])) > args.summary_steps: 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("--log", default=None, help="会话日志 JSONL 输出路径")
run.add_argument("--problem", type=int, choices=(3, 4), default=3, run.add_argument("--problem", type=int, choices=(3, 4), default=3,
help="问题3=全向源,问题4=混合定向源(默认 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, run.add_argument("--bearing-margin-deg", type=float, default=0.005,
help="示向度舍入裕量(服务器保留两位小数;默认 0.005°)") help="示向度舍入裕量(服务器保留两位小数;默认 0.005°)")
run.add_argument("--real-budget-s", type=float, default=None, run.add_argument("--real-budget-s", type=float, default=None,
+33
View File
@@ -99,6 +99,38 @@ class SearchConfig:
# 避免触及 100 小时上限被模拟器强制结束 # 避免触及 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) @dataclass(frozen=True)
class ProblemConfig: class ProblemConfig:
"""聚合配置。各模块函数的第一个默认参数,便于单测替换。""" """聚合配置。各模块函数的第一个默认参数,便于单测替换。"""
@@ -107,6 +139,7 @@ class ProblemConfig:
cost: CostConfig = CostConfig() cost: CostConfig = CostConfig()
session: SessionConfig = SessionConfig() session: SessionConfig = SessionConfig()
search: SearchConfig = SearchConfig() search: SearchConfig = SearchConfig()
bayes: BayesConfig = BayesConfig()
DEFAULT_CONFIG = ProblemConfig() DEFAULT_CONFIG = ProblemConfig()
+356
View File
@@ -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)
+429
View File
@@ -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
+93
View File
@@ -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
+154
View File
@@ -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()
+172
View File
@@ -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()