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
+12 -1
View File
@@ -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,
+33
View File
@@ -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()
+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