This commit is contained in:
Horldsence
2026-09-11 01:19:38 +08:00
parent 83d2e24448
commit a6308fd40c
12 changed files with 1631 additions and 87 deletions
+245 -18
View File
@@ -1,24 +1,38 @@
"""P3 逐频道信念状态:候选集合 K_c 维护与频道处理状态。
现状:骨架(P3 起填充)。接口与不变式先行固定,防止后续实现漂移。
核心不变式(验收标准):
- 保守性:真源永远属于维护中的候选集合;任何更新不得因误差、
圆弧离散或浮点容差把真点排除(自建真值案例中真源始终保留);
- direction 读数:K_c ⊆ Ω ∩ W(s, θ±δ) ∩ B(s, 1500)(首次测向构造见
geometry.planning.first_bearing_candidate_disk 的解析证书);
- no_signal 读数(P4,全向源):仅可排除 B(s, 1000);用于定向源时
是"位置×朝向"联合约束,单独维护空间候选会误删真源(P5 才启用);
- 保守性:真源永远属于维护中的候选集合。候选集用"圆盘外切多边形 ∩
测向角域半平面"的凸多边形表示——外切多边形严格包含圆盘,半平面裁剪
带正容差,因此每步更新只会放大而不会缩小真实候选集;
- direction 读数:K_c ⊆ Ω ∩ W(s, θ±δ_eff) ∩ B(s, 1500),后续读数再交
同样的约束;δ_eff = 误差半角 + 示向度舍入裕量(服务器保留两位小数,
舍入本身引入 ≤0.005° 偏差);
- no_signal 读数(P4,全向源):基线不据此删空间候选——对定向源它是
"位置×朝向"联合约束,单独维护空间候选会误删真源(P5 才启用);
"该频道不存在源"的证明由覆盖账本(coverage.CoverageLedger)给出;
- near 读数:源在 5m 内,候选集收缩为 B(s, 5),可直接产生清除证书;
- /clear 成功:状态转 CLEARED;失败只说明"该频道 20m 内无可清除源",
不得直接删除超出 20m 的候选。
不得直接删除超出 20m 的候选,必须先补测再重试。
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from enum import Enum
from ..config import ProblemConfig
from ..config import ProblemConfig, Vec2
from ..geometry.localization import smallest_enclosing_circle
from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL
# 半平面裁剪的正容差(米)。坐标量级 ~2100,浮点误差远小于它;
# 容差只向"保留更多候选"的方向倾斜,不破坏保守性。
_CLIP_TOL_M = 1e-6
_DEDUPE_TOL_M = 1e-7
class InconsistentBelief(RuntimeError):
"""读数把候选集剪空等违背保守性的情形;须停机核查而非继续跑。"""
class ChannelStatus(Enum):
@@ -29,11 +43,185 @@ class ChannelStatus(Enum):
ABSENT = "absent" # 覆盖证明不存在(P4)
def circumscribed_polygon(center: Vec2, radius_m: float, sides: int) -> list[Vec2]:
"""圆的外切正多边形顶点(外接半径 r/cos(π/n)),严格包含圆盘。
近似误差(外接圆超出圆盘的部分)为 r(1/cos(π/n) - 1) ≤ r(1-cos(π/n))·sec²,
调用方用 (1-cos(π/n)) 口径记账即可(略保守偏大)。
"""
if sides < 8:
raise ValueError("外切多边形边数过少,近似误差不可控")
circumradius = radius_m / math.cos(math.pi / sides)
step = 2 * math.pi / sides
return [(center[0] + circumradius * math.cos(step * i + step / 2),
center[1] + circumradius * math.sin(step * i + step / 2))
for i in range(sides)]
def polygon_halfplanes(polygon: list[Vec2]) -> list[tuple[float, float, float]]:
"""CCW 多边形 -> 外法线半平面列表 (a, b, c),含义 a·x + b·y <= c。"""
planes = []
count = len(polygon)
for index in range(count):
p, q = polygon[index], polygon[(index + 1) % count]
dx, dy = q[0] - p[0], q[1] - p[1]
norm = math.hypot(dx, dy)
if norm <= 1e-12:
continue
a, b = dy / norm, -dx / norm # CCW 的右法线朝外
planes.append((a, b, a * p[0] + b * p[1]))
return planes
def clip_halfplane(polygon: list[Vec2], a: float, b: float, c: float,
tol: float = _CLIP_TOL_M) -> list[Vec2]:
"""Sutherland–Hodgman 凸多边形半平面裁剪,保留 a·x + b·y <= c + tol。
边界交点取在无容差直线上;进入判定带正容差,保证"贴边"的真点不丢。
"""
if not polygon:
return []
output: list[Vec2] = []
count = len(polygon)
for index in range(count):
p, q = polygon[index], polygon[(index + 1) % count]
fp = a * p[0] + b * p[1] - c
fq = a * q[0] + b * q[1] - c
p_in, q_in = fp <= tol, fq <= tol
if p_in:
output.append(p)
if p_in != q_in:
ratio = fp / (fp - fq)
output.append((p[0] + ratio * (q[0] - p[0]),
p[1] + ratio * (q[1] - p[1])))
return _dedupe(output)
def _dedupe(points: list[Vec2], tol: float = _DEDUPE_TOL_M) -> list[Vec2]:
unique: list[Vec2] = []
for point in points:
if not unique or math.dist(point, unique[-1]) > tol:
unique.append(point)
if len(unique) > 1 and math.dist(unique[0], unique[-1]) <= tol:
unique.pop()
return unique
class CandidateRegion:
"""某频道候选集合的保守凸外包多边形(顶点按 CCW;允许退化为 1-2 点)。"""
def __init__(self, polygon: list[Vec2], slack_m: float = 0.0):
self.polygon = list(polygon)
self.slack_m = float(slack_m) # 外包近似引入的半径向误差记账
self._mec: tuple[Vec2, float] | None = None
# ------------------------------------------------------------ 构造
@classmethod
def disk(cls, center: Vec2, radius_m: float, sides: int,
slack_m: float | None = None) -> "CandidateRegion":
if slack_m is None:
slack_m = radius_m * (1 - math.cos(math.pi / sides))
return cls(circumscribed_polygon(center, radius_m, sides), slack_m)
@classmethod
def initial(cls, station: Vec2, config: ProblemConfig) -> "CandidateRegion":
"""首个 direction 后的基础集:Ω ∩ B(station, r_max) 的保守外包。"""
field, sides = config.field, config.search.polygon_sides
slack = field.omega_radius_m * (1 - math.cos(math.pi / sides))
region = cls.disk((0.0, 0.0), field.omega_radius_m, sides, slack)
region.clip_disk(station, field.r_eff_max_m, sides)
return region
# ------------------------------------------------------------ 收缩
def _clip(self, a: float, b: float, c: float) -> None:
polygon = clip_halfplane(self.polygon, a, b, c)
if not polygon:
raise InconsistentBelief(
f"候选区域被半平面 ({a:.6f},{b:.6f},{c:.6f}) 剪空:读数与保守集矛盾")
self.polygon = polygon
self._mec = None
def clip_disk(self, center: Vec2, radius_m: float, sides: int) -> None:
"""与圆盘 B(center, radius) 的外切多边形求交(近似误差入账)。"""
for a, b, c in polygon_halfplanes(circumscribed_polygon(center, radius_m, sides)):
self._clip(a, b, c)
self.slack_m += radius_m * (1 - math.cos(math.pi / sides))
def clip_wedge(self, station: Vec2, bearing_deg: float,
half_angle_deg: float) -> None:
"""与测向角域 W(station, bearing±half_angle) 求交(半角须 <90°)。
两个边界半平面与 localization.halfplanes 同口径,0°/360° 跨界由
sin/cos 直接处理,无需分支。
"""
if not 0 < half_angle_deg < 90:
raise ValueError("角域半角须在 (0, 90) 度")
lower = math.radians(bearing_deg - half_angle_deg)
upper = math.radians(bearing_deg + half_angle_deg)
sx, sy = float(station[0]), float(station[1])
self._clip(math.sin(lower), -math.cos(lower),
math.sin(lower) * sx - math.cos(lower) * sy)
self._clip(-math.sin(upper), math.cos(upper),
-math.sin(upper) * sx + math.cos(upper) * sy)
# ------------------------------------------------------------ 查询
def mec(self) -> tuple[Vec2, float]:
"""最小包围圆(圆心, 半径)。半径是真实候选集 ρ 的上界。"""
if self._mec is None:
if not self.polygon:
raise InconsistentBelief("候选区域为空")
self._mec = smallest_enclosing_circle(self.polygon)
return self._mec
def witnesses(self, spacing_m: float) -> list[Vec2]:
"""顶点 + 边界按间距细分的采样点(最坏后验评估用)。"""
if len(self.polygon) < 3:
return list(self.polygon)
points = list(self.polygon)
count = len(self.polygon)
for index in range(count):
p, q = self.polygon[index], self.polygon[(index + 1) % count]
length = math.dist(p, q)
steps = int(length // spacing_m)
for k in range(1, steps + 1):
ratio = k / (steps + 1)
points.append((p[0] + ratio * (q[0] - p[0]),
p[1] + ratio * (q[1] - p[1])))
return _dedupe(points, tol=1e-9)
def contains(self, point: Vec2, tol_m: float = 0.05) -> bool:
"""点是否在候选集内(带容差;用于真源保留性检验,不做删点依据)。"""
if not self.polygon:
return False
if len(self.polygon) == 1:
return math.dist(point, self.polygon[0]) <= tol_m + self.slack_m
if len(self.polygon) == 2:
p, q = self.polygon
return _point_segment_distance(point, p, q) <= tol_m + self.slack_m
for index in range(len(self.polygon)):
p = self.polygon[index]
q = self.polygon[(index + 1) % len(self.polygon)]
if (q[0] - p[0]) * (point[1] - p[1]) - (q[1] - p[1]) * (point[0] - p[0]) \
< -tol_m - self.slack_m:
return False
return True
def _point_segment_distance(point: Vec2, p: Vec2, q: Vec2) -> float:
vx, vy = q[0] - p[0], q[1] - p[1]
length_sq = vx * vx + vy * vy
if length_sq <= 1e-18:
return math.dist(point, p)
t = max(0.0, min(1.0, ((point[0] - p[0]) * vx + (point[1] - p[1]) * vy) / length_sq))
return math.dist(point, (p[0] + t * vx, p[1] + t * vy))
@dataclass
class ChannelBelief:
channel: int
status: ChannelStatus = ChannelStatus.UNKNOWN
measurements: list = field(default_factory=list) # (位置, 结果, svd_deg) 记录
region: CandidateRegion | None = None
measurements: list = field(default_factory=list) # 逐步观测记录(审计用)
class BeliefState:
@@ -45,14 +233,53 @@ class BeliefState:
for channel in range(config.field.channel_min,
config.field.channel_max + 1)}
def update_measure(self, channel: int, position, result: str,
svd_deg: float | None) -> None:
"""按读数收敛候选集合。未实现:需要几何模块的在线圆盘角域交。"""
raise NotImplementedError("P3:接入 geometry 的圆盘约束角域交后实现")
@property
def effective_half_angle_deg(self) -> float:
"""示向度误差半角 + 舍入裕量:服务器保留两位小数引入 ≤0.005° 偏差。"""
return (self.config.field.bearing_error_half_deg
+ self.config.search.bearing_rounding_margin_deg)
def candidate_set(self, channel: int):
"""返回该频道当前候选集合(保守外包形式)。未实现。"""
raise NotImplementedError("P3")
def update_measure(self, channel: int, position: Vec2, result: str,
svd_deg: float | None) -> None:
"""按读数收敛候选集合;真源保留性由保守表示保证。"""
belief = self.channels[channel]
belief.measurements.append({"position": tuple(position), "result": result,
"svd_deg": svd_deg})
if result == DIRECTION:
if svd_deg is None:
raise ValueError("direction 读数必须携带 svd_deg")
sides = self.config.search.polygon_sides
if belief.region is None:
belief.region = CandidateRegion.initial(position, self.config)
else:
# 后续 direction 同样隐含距离 <= r_max [题面]
belief.region.clip_disk(position, self.config.field.r_eff_max_m, sides)
belief.region.clip_wedge(position, svd_deg, self.effective_half_angle_deg)
belief.status = ChannelStatus.DETECTED
elif result == NEAR:
sides = self.config.search.polygon_sides
if belief.region is None:
belief.region = CandidateRegion.disk(
tuple(position), self.config.field.near_radius_m, sides)
else:
belief.region.clip_disk(position, self.config.field.near_radius_m, sides)
belief.status = ChannelStatus.DETECTED
elif result == NO_SIGNAL:
# P3/P4 基线:不据此删空间候选(缺席证明由覆盖账本给出)。
pass
else:
raise ValueError(f"未知测向结果: {result!r}")
def candidate_set(self, channel: int) -> CandidateRegion | None:
"""该频道当前候选集合(保守外包形式);尚无 direction/near 时为 None。"""
return self.channels[channel].region
def mark_cleared(self, channel: int) -> None:
self.channels[channel].status = ChannelStatus.CLEARED
def mark_absent(self, channel: int) -> None:
self.channels[channel].status = ChannelStatus.ABSENT
def channels_by_status(self, *statuses: ChannelStatus) -> list[int]:
return sorted(channel for channel, belief in self.channels.items()
if belief.status in statuses)
+52 -5
View File
@@ -2,7 +2,8 @@
- q1 问题一角域交会求解(P2 已完工)
- sim-check 用自建模拟器复现附件计时示例(P0 自检)
- run 全向闭环(P4 骨架,未实现)
- run 闭环:发现—定位—清除—退出(--problem 3 全向 / 4 混合源;
--self-test 走本地模拟器)
"""
from __future__ import annotations
@@ -10,12 +11,17 @@ from __future__ import annotations
import argparse
import json
import sys
from dataclasses import replace
from pathlib import Path
from .config import ProblemConfig
from .geometry.__main__ import main as q1_main
from .planner import DirectionalExtension, OmnidirectionalLoop
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
def _sim_check(args) -> int:
@@ -40,8 +46,36 @@ def _sim_check(args) -> int:
def _run(args) -> int:
raise NotImplementedError(
"P4 全向闭环尚未实现;当前可用子命令:q1(问题一求解)、sim-check(协议自检)")
"""问题3/4闭环。--self-test 时真值只存在于测试端入口,策略端仅见响应。"""
config = ProblemConfig()
session = replace(config.session,
base_url=args.base_url or config.session.base_url)
config = replace(config,
session=session,
search=replace(config.search,
bearing_rounding_margin_deg=args.bearing_margin_deg))
truth = None
transport = None
if args.self_test:
if args.problem == 4:
sources = mixed_scene(seed=args.seed, count=args.sources,
directional=args.directional)
else:
sources = random_omni_scene(seed=args.seed, count=args.sources)
truth = sorted(source.channel for source in sources)
transport = LocalSimTransport(LocalSimulator(sources, robot_id=args.robot_id,
config=config))
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
summary = loop_cls(client, config).run()
summary["robot_id"] = args.robot_id
if truth is not None:
# 仅测试端核对:清除集合是否与真值一致。
summary["truth"] = {"source_channels": truth,
"all_cleared": sorted(summary["cleared_channels"]) == truth}
print(json.dumps(summary, ensure_ascii=False, indent=2))
return 0 if summary["completed"] and (truth is None or summary["truth"]["all_cleared"]) else 1
def main(argv=None) -> int:
@@ -62,8 +96,21 @@ def main(argv=None) -> int:
check.add_argument("--robot-id", default="2026001")
check.set_defaults(func=_sim_check)
run = subparsers.add_parser("run", help="P4 全向闭环(骨架)")
run.add_argument("--robot-id", required=True)
run = subparsers.add_parser("run", help="闭环:发现—定位—清除—退出(问题3/4)")
run.add_argument("--robot-id", required=True, help="登录参赛队号,逐字节一致")
run.add_argument("--base-url", default=None,
help="模拟器接口地址(默认取配置 http://127.0.0.1:2026)")
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("--bearing-margin-deg", type=float, default=0.005,
help="示向度舍入裕量(服务器保留两位小数;默认 0.005°)")
run.add_argument("--self-test", action="store_true",
help="走本地模拟器与随机场景(测试端自检,不连官方)")
run.add_argument("--seed", type=int, default=20260910, help="自检场景种子")
run.add_argument("--sources", type=int, default=10, help="自检场景源数量")
run.add_argument("--directional", type=int, default=3,
help="问题4自检场景中定向源数量(--problem 4 生效)")
run.set_defaults(func=_run)
args = parser.parse_args(argv)
+11
View File
@@ -73,6 +73,17 @@ class SearchConfig:
grid_half_cells: int = 3 # i,j ∈ [-3,3],49 点覆盖 [-2100,2100]^2 ⊇ Ω
fallback_clear_grid_m: float = 20.0 # P5 后备清除网格;10*sqrt(2)<=20
bearing_rounding_margin_deg: float = 0.0 # 默认不加舍入裕量;敏感性分析时显式设 0.005
# ---- P3/P4 在线定位与闭环参数 [工程] ----
polygon_sides: int = 192 # 圆盘外切多边形边数;Ω 近似误差 1800(1-cos(π/192))≈0.24m
witness_spacing_m: float = 60.0 # 最坏后验评估的边界采样间距;选定后再按 1/4 加密复核
candidate_rings: tuple = (0.0, 0.4, 0.75, 1.0) # 安全接收盘内候选点的半径比例
candidate_angles: int = 12 # 每环候选方向数
clear_margin_m: float = 0.5 # 清除证书的额外工程余量(多边形近似误差之外)
max_localize_steps: int = 8 # 单频道定位阶段允许的追加测量上限
max_anomalies_per_channel: int = 3 # 单频道异常记录上限,防止异常刷屏
# ---- P5 定向扩展参数 [工程] ----
max_blocked_probes: int = 4 # 定位单轮内被朝向遮挡的探点预算(对跖复核+环探)
fallback_max_cells: int = 400 # 后备清除格数硬顶;超过则不盲扫(宽候选集继续定位)
@dataclass(frozen=True)
+508 -49
View File
@@ -1,80 +1,539 @@
"""P3/P4/P5 策略层:主动选点、全向闭环与定向扩展。
现状:骨架。各入口先固定签名与验收标准,逐阶段填充实现。
P3 第二检测点(验收:保证收到信号;Q 空时退化;near 分支):
- K1 = Ω ∩ W(s1, θ1±δ) ∩ B(s1, 1500);安全候选盘证书见
geometry.planning.first_bearing_candidate_disk(δ<=1.005° 时非空);
- 在证书圆盘内围绕测向轴两侧采样候选点,以"最坏后验包围圆半径"
为主指标、移动+检测代价为次指标;
- 在保证接收盘 B(mec_center, 1000-ρ) 内环形采样候选点,以
"最坏后验包围圆半径"为主指标、移动代价为次指标;
- 采样是启发式(目标边界点 × 读数区间端点),不是严格最坏情形
最优解;选定后按 1/4 间距加密复核并如实上报数值;
- 返回 near 时:候选源在 5m 内,直接对当前频道尝试 /clear。
P4 全向闭环(验收:10/16 源正确退出;边界朝外不漏;停止条件正确):
发现(coverage.CoverageLedger)→ 定位(belief 收敛)→ 清除 → 停止判定
(清除数达 16,或每频道均有清除成功/不存在证明)→ /exit。
发现 10 源、连续无信号、候选采样为空都不是停止条件。
发现 10 源、连续无信号、候选采样为空都不是停止条件;
现实预算耗尽退出记为未证明完成(completed=false),不与成功混记。
P5 定向扩展(验收:朝外边界源能发现;失去信号能清除;不误删真源):
no_signal 是"位置×朝向"联合约束,基线不据此删空间候选;
失去信号后对有界候选做 <=20m 有限圆盘覆盖逐点 /clear,
并按预算上界(格数×请求耗时)决定是否执行。
P5 定向扩展(问题4混合源闭环):
- 49 点覆盖的发现保证对定向源同样成立(源在单元凸包内,任意发射半平面
含至少一个顶点),覆盖阶段不区分源类型;
- 保证接收盘内的 no_signal 只能由朝向遮挡解释 [观测推理]:频道记为
"观测到定向",对跖探点复核一次;联合约束基线不删空间候选(真源不误删);
- 定位停滞时对有界候选集构造 <=20m 有限圆盘覆盖(fallback_clear_cells),
预算核对后逐格 /clear(清除与朝向无关);格数超硬顶或预算不足即跳过,
绝不盲扫。
"""
from __future__ import annotations
import math
from dataclasses import dataclass
from ..config import ProblemConfig
from ..protocol.client import SimulatorClient
from ..belief import (BeliefState, CandidateRegion, ChannelStatus,
clip_halfplane)
from ..config import ProblemConfig, Vec2
from ..coverage import CoverageLedger
from ..evaluation import recompute_virtual_time, summarize
from ..protocol.client import DeadlineExceeded, SimulatorClient
from ..protocol.rules import CLEAR_SUCCESS, DIRECTION, NEAR, NO_SIGNAL
from ..protocol.timing import move_time
@dataclass
def bearing_deg(a: Vec2, b: Vec2) -> float:
"""从 a 指向 b 的方位角(0-360,0 为 x 正方向,逆时针)。"""
return math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) % 360.0
@dataclass(frozen=True)
class SecondPointPlan:
"""第二检测点的候选与依据(P3 输出形态预览)。"""
"""下一检测点的候选与依据(P3 输出)。"""
position: tuple[float, float]
position: Vec2
worst_posterior_radius_m: float
move_cost_s: float
reason: str
def select_second_point(client: SimulatorClient,
first_position, first_bearing_deg: float,
config: ProblemConfig = ProblemConfig()):
"""在安全候选盘内选择第二检测点。未实现:P3。"""
raise NotImplementedError("P3:先实现 belief 候选集,再实现采样评估")
@dataclass(frozen=True)
class ClearCertificate:
"""清除证书:候选集外包半径(含近似误差)不超过 20m 阈值。"""
channel: int
center: Vec2
radius_m: float
limit_m: float
certified: bool
def clear_certificate(region: CandidateRegion, channel: int,
config: ProblemConfig = ProblemConfig()) -> ClearCertificate:
"""ρ_poly <= 20 - 外包近似误差 - 工程余量 才允许发起清除。"""
center, radius = region.mec()
limit = (config.field.clear_radius_m - region.slack_m
- config.search.clear_margin_m)
return ClearCertificate(channel, center, radius, limit, radius <= limit)
def candidate_points(center: Vec2, safe_radius_m: float, rings, angles: int) -> list[Vec2]:
"""保证接收盘内的候选点:盘心 + 各比例半径 × 均匀方向。"""
points = [tuple(center)]
for fraction in rings:
radius = fraction * safe_radius_m
if radius <= 1e-9:
continue
step = 2 * math.pi / angles
for k in range(angles):
angle = step * k + step / 2
points.append((center[0] + radius * math.cos(angle),
center[1] + radius * math.sin(angle)))
return points
def _worst_posterior_radius(region: CandidateRegion, q: Vec2,
witnesses: list[Vec2], half_angle_deg: float) -> float:
"""对候选点 q,遍历"目标边界点 × 读数区间端点"的最坏后验包围半径。
读数 θ' 与目标 g 一致当且仅当 |θ' - bearing(q→g)| <= δ,因此端点
φ(g)±δ 是每个 g 的最不利读数。这是启发式近似,不构成最优性证明 [工程]。
"""
posterior_radii = []
for g in witnesses:
phi = bearing_deg(q, g)
for reading in (phi - half_angle_deg, phi + half_angle_deg):
probe = CandidateRegion(list(region.polygon), region.slack_m)
probe.clip_wedge(q, reading, half_angle_deg)
if probe.polygon:
posterior_radii.append(probe.mec()[1])
if not posterior_radii:
return region.mec()[1] # 无有效采样点时退回当前半径
return max(posterior_radii)
def safe_disk_radius(region: CandidateRegion,
config: ProblemConfig = ProblemConfig()) -> float:
"""保证接收盘 B(mec, safe) 的半径;<0 表示无保证(ρ 超过最小接收半径)。
盘内任意点到候选集每点距离 <= safe + ρ + 1e-3 < 1000 <= 最小接收半径,
故必然收到 direction/near。1mm 数值余量避免浮点贴边被 r_eff=1000 的源
判为 no_signal。
"""
return config.field.r_eff_min_m - region.mec()[1] - 1e-3
def select_measurement_point(region: CandidateRegion, from_position: Vec2,
config: ProblemConfig = ProblemConfig()) -> SecondPointPlan:
"""在保证接收盘内选择下一检测点(P3 核心;对任意候选集通用)。
保证接收盘 B(mec_center, 1000-ρ):盘内任意点到候选集每点距离
<= (1000-ρ) + ρ = 1000 <= 最小接收半径,故必然收到 direction/near,
不会浪费一次 no_signal。ρ>1000 时退化为探索 MEC 圆心(可能 no_signal)。
"""
field, search = config.field, config.search
center, radius = region.mec()
half_angle = field.bearing_error_half_deg + search.bearing_rounding_margin_deg
def move_cost(point: Vec2) -> float:
return move_time(from_position, point, config.cost)
# 1mm 数值余量在 safe_disk_radius 内记账。
safe_radius = safe_disk_radius(region, config)
if safe_radius < 0:
return SecondPointPlan(tuple(center), radius, move_cost(center),
"无保证接收盘(ρ>1000):探索 MEC 圆心,可能 no_signal")
coarse = region.witnesses(search.witness_spacing_m)
samples = candidate_points(center, safe_radius,
search.candidate_rings, search.candidate_angles)
best_key, best_point, best_worst = None, None, None
for point in samples:
worst = _worst_posterior_radius(region, point, coarse, half_angle)
key = (round(worst, 6), round(move_cost(point), 6))
if best_key is None or key < best_key:
best_key, best_point, best_worst = key, point, worst
dense = _worst_posterior_radius(region, best_point,
region.witnesses(search.witness_spacing_m / 4),
half_angle)
reason = (f"安全盘 B(centre,{safe_radius:.0f}m) 内 {len(samples)} 候选;"
f"粗采样最坏后验 {best_worst:.1f}m,1/4 间距加密复核 {dense:.1f}m")
return SecondPointPlan(tuple(best_point), dense, move_cost(best_point), reason)
def select_second_point(client: SimulatorClient, first_position: Vec2,
first_bearing_deg: float,
config: ProblemConfig = ProblemConfig()) -> SecondPointPlan:
"""P3 入口:由首次 direction 构造 K1,返回保证接收的第二检测点方案。"""
region = CandidateRegion.initial(first_position, config)
half_angle = config.field.bearing_error_half_deg \
+ config.search.bearing_rounding_margin_deg
region.clip_wedge(first_position, first_bearing_deg, half_angle)
return select_measurement_point(region, client.state.position, config)
class OmnidirectionalLoop:
"""问题3全向"发现—定位—清除—停止"闭环。未实现:P4。"""
"""问题3全向"发现—定位—清除—停止"闭环(P4 基线)。
def __init__(self, client: SimulatorClient,
config: ProblemConfig = ProblemConfig()):
self.client = client
self.config = config
def run(self) -> dict:
raise NotImplementedError("P4:依赖 coverage + belief + planner 就位")
class DirectionalExtension:
"""问题4定向源扩展:朝向不确定下的覆盖与后备清除。未实现:P5。"""
def __init__(self, client: SimulatorClient,
config: ProblemConfig = ProblemConfig()):
self.client = client
self.config = config
def run(self) -> dict:
raise NotImplementedError("P5:依赖 P4 闭环稳定后扩展")
def fallback_clear_cells(center, width_m: float, height_m: float,
config: ProblemConfig = ProblemConfig()):
"""失去信号后的后备清除格心列表(<=20m 圆盘覆盖)。
半开区间统一索引,源在格线上时至少归入一格;相交判定数值
不确定时保留格子(宁可多测)。格数即 /clear 请求预算上界。
未实现:P5(先由演练实测请求预算决定启用规模)。
串行驱动 SimulatorClient:覆盖阶段在 49 点网格上对每个 UNKNOWN 频道
检测;发现 direction 即进入该频道的定位子循环(选点→测量→收敛→
证书清除),near 直接清除。停止条件只有三种:清除数达 16(数量上界
证明)、每频道均 CLEARED/ABSENT、现实预算耗尽(记为未完成)。
"""
raise NotImplementedError("P5")
def __init__(self, client: SimulatorClient,
config: ProblemConfig = ProblemConfig()):
self.client = client
self.config = config
self.belief = BeliefState(config)
self.ledger = CoverageLedger(config)
self.cleared: list[int] = []
self.anomalies: list[dict] = []
self.stop_reason: str | None = None
# 证书有效但 /clear 失败后必须先补测再重试,避免在同一证书上
# 反复消耗 3 秒/次的失败清除。
self._may_clear: set[int] = set()
# ------------------------------------------------------------ 主流程
def run(self) -> dict:
self.client.enter()
try:
self._coverage_phase()
if self.stop_reason is None:
self._mark_absent_after_coverage()
self.stop_reason = self._resolve_detected()
except DeadlineExceeded:
self.stop_reason = "real_budget_exhausted"
info = self.client.exit()
return self._summary(info)
# ------------------------------------------------------------ 覆盖阶段
def _pending_channels_at(self, index: int) -> list[int]:
return [channel for channel, belief in self.belief.channels.items()
if belief.status is ChannelStatus.UNKNOWN
and not self.ledger.measured_at(channel, index)]
def _next_point_index(self) -> int | None:
"""尚有 UNKNOWN 频道待测的最近网格点(贪心最近邻 [工程])。"""
position = self.client.state.position
best_index, best_distance = None, 0.0
for index, point in enumerate(self.ledger.points):
if not self._pending_channels_at(index):
continue
distance = math.dist(position, point)
if best_index is None or distance < best_distance:
best_index, best_distance = index, distance
return best_index
def _coverage_phase(self) -> None:
while True:
if self._count_limit_reached():
return
index = self._next_point_index()
if index is None:
return
point = self.ledger.points[index]
for channel in self._pending_channels_at(index):
outcome = self.client.measure(point, channel)
self.ledger.mark_measured(channel, index)
self.belief.update_measure(channel, point, outcome.result,
outcome.svd_deg)
if outcome.result == DIRECTION:
self._localize_and_clear(channel)
elif outcome.result == NEAR:
region = self.belief.candidate_set(channel)
cert = clear_certificate(region, channel, self.config)
if cert.certified:
self._attempt_clear(channel, cert.center, cert)
else: # 理论上不发生(near 候选半径 5m);保守记录
self._anomaly("near_without_certificate", channel,
position=point)
if self._count_limit_reached():
return
def _count_limit_reached(self) -> bool:
if len(self.cleared) >= self.config.field.source_count_max:
self.stop_reason = "sixteen_cleared"
return True
return False
# ------------------------------------------------------------ 定位与清除
def _localize_and_clear(self, channel: int) -> bool:
"""定位子循环:选点测量收敛候选集,拿到清除证书后清除。
返回是否已清除;步数耗尽仍未收敛则保持 DETECTED 留待二轮。
进入本函数前该频道刚有一次 direction 读数,允许立即清除。
"""
search = self.config.search
self._may_clear.add(channel)
for _ in range(search.max_localize_steps):
region = self.belief.candidate_set(channel)
if region is None:
return False
cert = clear_certificate(region, channel, self.config)
if cert.certified and channel in self._may_clear:
if self._attempt_clear(channel, cert.center, cert):
return True
self._may_clear.discard(channel)
continue
plan = select_measurement_point(region, self.client.state.position,
self.config)
outcome = self.client.measure(plan.position, channel)
self.belief.update_measure(channel, plan.position, outcome.result,
outcome.svd_deg)
self._may_clear.add(channel)
if outcome.result == NO_SIGNAL:
self._anomaly("no_signal_in_safe_disk", channel,
position=plan.position, detail=plan.reason)
return self.belief.channels[channel].status is ChannelStatus.CLEARED
def _attempt_clear(self, channel: int, target: Vec2,
cert: ClearCertificate) -> bool:
outcome = self.client.clear(target, channel)
if outcome.result == CLEAR_SUCCESS:
self.belief.mark_cleared(channel)
self.cleared.append(channel)
return True
if cert.certified:
self._anomaly("clear_failed_with_certificate", channel, position=target,
radius_m=round(cert.radius_m, 3), limit_m=cert.limit_m)
return False
def _anomaly(self, kind: str, channel: int, **detail) -> None:
"""异常只在守恒性被打破或证书失效时出现;每频道限量记录防刷屏。"""
recorded = sum(1 for item in self.anomalies if item["channel"] == channel)
if recorded >= self.config.search.max_anomalies_per_channel:
return
self.anomalies.append({"kind": kind, "channel": channel, **detail})
# ------------------------------------------------------------ 收尾
def _mark_absent_after_coverage(self) -> None:
"""全向源缺席证明:全网格无非 no_signal 读数的频道不存在源 [题面]。"""
for channel, belief in self.belief.channels.items():
if belief.status is ChannelStatus.UNKNOWN \
and self.ledger.coverage_complete(channel):
self.belief.mark_absent(channel)
def _resolve_detected(self) -> str:
"""覆盖完成后,对仍未清除的 DETECTED 频道做第二轮定位。"""
for channel in self.belief.channels_by_status(ChannelStatus.DETECTED):
self._localize_and_clear(channel)
unresolved = [channel for channel, belief in self.belief.channels.items()
if belief.status not in (ChannelStatus.CLEARED,
ChannelStatus.ABSENT)]
return "all_resolved" if not unresolved else "unresolved_channels"
def _summary(self, info) -> dict:
events = self.client.log.action_events()
stats = summarize(events)
report = recompute_virtual_time(events)
cleared = self.belief.channels_by_status(ChannelStatus.CLEARED)
absent = self.belief.channels_by_status(ChannelStatus.ABSENT)
resolved = set(cleared) | set(absent)
unresolved = [channel for channel in sorted(self.belief.channels)
if channel not in resolved]
completed = (self.stop_reason == "sixteen_cleared"
or (self.stop_reason == "all_resolved" and not unresolved))
return {
"completed": completed,
"stop_reason": self.stop_reason,
"exit_reason": info.reason,
"virtual_time_s": round(info.virtual_time_s, 3),
"cleared_channels": cleared,
"absent_channels": absent,
"unresolved_channels": unresolved,
"measures": stats["measures"],
"clears": stats["clears"],
"clear_success": stats["cleared"],
"move_distance_m": round(stats["distance_m"], 1),
"anomalies": self.anomalies,
"virtual_recompute_mismatches": len(report["mismatches"]),
}
class DirectionalExtension(OmnidirectionalLoop):
"""问题4混合源闭环(P5):全向基线 + 定向观测处理 + 后备清除。
与 P4 的三点差异(协议衔接见 docs/协议规则表与状态机.md §7):
- 49 点覆盖的发现保证对定向源同样成立,覆盖阶段不区分源类型;
- 保证接收盘内的 no_signal 只能由朝向遮挡解释 [观测推理]:该频道
记为"观测到定向",随后对 MEC 圆心取对跖探点复核;联合约束基线
不据此删除空间候选(真源不误删);
- 定位停滞(候选集有界但未达清除证书)时构造 <=20m 有限圆盘覆盖,
预算核对后逐格 /clear:清除与朝向无关 [题面],是定向源不依赖
可见性的完备后备。格数超硬顶或现实预算不足则跳过并留痕,绝不盲扫。
"""
def __init__(self, client: SimulatorClient,
config: ProblemConfig = ProblemConfig()):
super().__init__(client, config)
self.directional_observed: list[int] = [] # 观测到定向性的频道
self.blocked_probes: dict[int, Vec2] = {} # channel -> 被遮挡的探点
self.fallback_log: list[dict] = [] # 后备清除逐次留痕
# ------------------------------------------------------------ 定位与清除
def _localize_and_clear(self, channel: int) -> bool:
"""定位子循环:选点测量收敛候选集,拿到清除证书后清除。
与 P4 的差异:保证接收盘内的 no_signal 说明探点被朝向遮挡
[观测推理],走"对跖复核 -> 环探"升级梯子(不计入定位步数,
由 max_blocked_probes 限流),之后仍未收敛再转后备清除。
"""
search = self.config.search
self._may_clear.add(channel)
steps = 0
while steps < search.max_localize_steps:
region = self.belief.candidate_set(channel)
if region is None:
return False
cert = clear_certificate(region, channel, self.config)
if cert.certified and channel in self._may_clear:
if self._attempt_clear(channel, cert.center, cert):
return True
self._may_clear.discard(channel) # 证书失败:补测后才可重试
continue
if len(self.blocked_probes.get(channel, [])) > search.max_blocked_probes:
break # 梯子用尽仍被遮挡:定位停滞,转后备清除
point = self._next_probe(region, channel)
outcome = self.client.measure(point, channel)
self.belief.update_measure(channel, point, outcome.result,
outcome.svd_deg)
self._may_clear.add(channel)
if outcome.result == NO_SIGNAL \
and safe_disk_radius(region, self.config) >= 0:
if channel not in self.directional_observed:
self.directional_observed.append(channel)
self.blocked_probes.setdefault(channel, []).append(point)
continue # 被遮挡探点不消耗定位步数,由遮挡预算限流
if outcome.result == NO_SIGNAL:
self._anomaly("no_signal_outside_safe_disk", channel,
position=point)
else:
self.blocked_probes.pop(channel, None)
steps += 1
if self.belief.channels[channel].status is ChannelStatus.CLEARED:
return True
region = self.belief.candidate_set(channel)
if region is None:
return False
cert = clear_certificate(region, channel, self.config)
if cert.certified and channel in self._may_clear:
if self._attempt_clear(channel, cert.center, cert):
return True
self._may_clear.discard(channel)
return self._fallback_clear(channel, region)
def _next_probe(self, region: CandidateRegion, channel: int) -> Vec2:
"""定向遮挡下的探点升级梯子 [工程]。
- 未被遮挡过:worst-posterior 选点器(保证接收盘内);
- 第 1 次被遮挡:对 MEC 圆心取对跖点,仍在保证接收盘内;
- 之后:绕 MEC 圆心、半径 = ρ 的环探,方位自首个被遮挡点起
每次 +45°。环包围任意候选源(|g-c| <= ρ),发射半平面在环上
截出 >=180° 弧,等距采样大概率落入;环点距离不构成接收保证,
失败由后备清除兜底。
"""
blocked = self.blocked_probes.get(channel, [])
center, radius = region.mec()
if not blocked:
return select_measurement_point(region, self.client.state.position,
self.config).position
safe = safe_disk_radius(region, self.config)
first = blocked[0]
if len(blocked) == 1 and safe >= 0:
dx, dy = center[0] - first[0], center[1] - first[1]
norm = math.hypot(dx, dy)
if norm <= 1e-9:
probe = min(safe, 1.0)
return (center[0] + probe, center[1])
scale = min(1.0, safe / norm)
return (center[0] + dx / norm * scale, center[1] + dy / norm * scale)
base = math.atan2(first[1] - center[1], first[0] - center[0])
azimuth = base + math.radians(45.0 * (len(blocked) - 1))
return (center[0] + radius * math.cos(azimuth),
center[1] + radius * math.sin(azimuth))
def _fallback_clear(self, channel: int, region: CandidateRegion) -> bool:
"""定位停滞的后备:候选集有限圆盘覆盖逐格清除(格数即请求上界)。"""
search, session = self.config.search, self.config.session
width = max(p[0] for p in region.polygon) - min(p[0] for p in region.polygon)
height = max(p[1] for p in region.polygon) - min(p[1] for p in region.polygon)
cells = fallback_clear_cells(region.polygon, self.config)
entry = {"channel": channel, "cells": len(cells),
"bound": fallback_cell_bound(width, height, self.config),
"posterior_radius_m": round(region.mec()[1], 3)}
self.fallback_log.append(entry)
if len(cells) > search.fallback_max_cells:
# 宽候选集盲扫违背预算纪律:留待定位收敛,不得硬扫 [计划§4]。
entry["outcome"] = "skipped_cell_cap"
self._anomaly("fallback_cell_cap_exceeded", channel,
cells=len(cells), cap=search.fallback_max_cells)
return False
# 现实预算核对:N 次请求 × 单请求高分位估计 + 退出余量,不足不启动。
remaining = self.client.real_remaining_s
if remaining is None or len(cells) * session.real_rtt_p95_s \
+ session.reserve_exit_s > remaining:
entry["outcome"] = "skipped_budget"
return False
position = self.client.state.position
attempts = 0
for cell in sorted(cells, key=lambda c: math.dist(position, c)):
attempts += 1
outcome = self.client.clear(cell, channel)
if outcome.result == CLEAR_SUCCESS:
self.belief.mark_cleared(channel)
self.cleared.append(channel)
entry["outcome"] = "cleared"
entry["attempts"] = attempts
return True
# 候选集保守包含真源,格心距格内任一点 <= 10*sqrt(2) < 20m:全部
# 失败说明守恒性被打破,按异常留痕,不得据此删除候选。
entry["outcome"] = "exhausted"
entry["attempts"] = attempts
self._anomaly("fallback_exhausted_without_success", channel,
cells=len(cells))
return False
# ------------------------------------------------------------ 收尾
def _summary(self, info) -> dict:
report = super()._summary(info)
report["directional_observed_channels"] = list(self.directional_observed)
report["fallback_clears"] = self.fallback_log
return report
def fallback_cell_bound(width_m: float, height_m: float,
config: ProblemConfig = ProblemConfig()) -> int:
"""外包框枚举的格数上界 (⌈w/cell⌉+2)(⌈h/cell⌉+2)(计划§4 规模检查)。"""
cell = config.search.fallback_clear_grid_m
return (math.ceil(width_m / cell) + 2) * (math.ceil(height_m / cell) + 2)
def fallback_clear_cells(polygon: list[Vec2],
config: ProblemConfig = ProblemConfig()) -> list[Vec2]:
"""候选外包多边形的 <=20m 有限圆盘覆盖格心列表(P5 后备清除)。
全局半开 20m 网格索引;多边形与格子的相交判定带正容差(宁可多测),
多边形边界恰在格线上时两侧格子都保留,因此格内任意点距某格心
<= 10*sqrt(2) < 20m,逐格 /clear 至少一次命中。列表长度即 /clear
请求预算上界;先核对 fallback_cell_bound,超预算不得逐格盲扫。
"""
if not polygon:
return []
cell = config.search.fallback_clear_grid_m
xs = [p[0] for p in polygon]
ys = [p[1] for p in polygon]
i0, i1 = math.floor(min(xs) / cell), math.floor(max(xs) / cell)
j0, j1 = math.floor(min(ys) / cell), math.floor(max(ys) / cell)
eps = 1e-9
centers: list[Vec2] = []
for i in range(i0, i1 + 1):
for j in range(j0, j1 + 1):
clipped = list(polygon)
for a, b, c in ((1.0, 0.0, (i + 1) * cell + eps),
(-1.0, 0.0, -(i * cell - eps)),
(0.0, 1.0, (j + 1) * cell + eps),
(0.0, -1.0, -(j * cell - eps))):
clipped = clip_halfplane(clipped, a, b, c, tol=eps)
if not clipped:
break
if clipped:
centers.append(((i + 0.5) * cell, (j + 0.5) * cell))
return centers
+60
View File
@@ -0,0 +1,60 @@
"""自检场景生成器(测试端专用)。
真值只进入测试与 CLI ``--self-test`` 入口,策略端仅见模拟器响应;
场景覆盖题面强调的边界情形:近圆域边界源、最小/最大接收半径两档,
以及问题4的定向源(含朝向背离圆心的朝外边界源)。
"""
from __future__ import annotations
import math
from random import Random
from .simulator import Source
def random_omni_scene(seed: int = 20260910, count: int = 10) -> list[Source]:
"""全向源场景:频道互异,含近圆域边界点,接收半径 1000/1500 交替。"""
if not 0 <= count <= 20:
raise ValueError("源数量须在 0..20")
rng = Random(seed)
channels = rng.sample(range(1, 21), count)
sources = []
for index, channel in enumerate(channels):
if index == 0:
radius = rng.uniform(1780.0, 1799.0) # 近圆域边界
elif index == 1 and count > 1:
radius = rng.uniform(1780.0, 1799.0)
else:
radius = rng.uniform(0.0, 1795.0)
angle = rng.uniform(0.0, 2 * math.pi)
r_eff = 1000.0 if index % 2 == 0 else 1500.0
sources.append(Source(channel=channel,
x=radius * math.cos(angle),
y=radius * math.sin(angle),
r_eff_m=r_eff))
return sources
def mixed_scene(seed: int = 20260910, count: int = 10,
directional: int = 3) -> list[Source]:
"""问题4混合场景:count 个源中 directional 个为定向源。
改造 random_omni_scene 的前 directional 个源:近圆域边界的前两个定向源
朝向取"背离圆心"(发射半平面基本朝向域外,覆盖网格仍保证可发现,
即"朝外边界源不漏"验收情形),其余定向源朝向随机。
"""
if not 0 <= directional <= count:
raise ValueError("定向源数量须在 0..count")
sources = list(random_omni_scene(seed=seed, count=count))
rng = Random(seed + 1)
for index in range(directional):
source = sources[index]
if index < 2 and count > 1 and math.hypot(source.x, source.y) > 1700.0:
heading = math.degrees(math.atan2(source.y, source.x)) % 360.0
else:
heading = rng.uniform(0.0, 360.0)
sources[index] = source.__class__(channel=source.channel, x=source.x,
y=source.y, r_eff_m=source.r_eff_m,
directional=True, heading_deg=heading)
return sources