XYT·星野图
首页项目归档照片墙音乐灵境说说杂谈友链关于
封面
返回上一级

TD25A 填充清扫:四色层、五张网与执行层

写作时间:2026-08-26 03:40
# 机器人
# ROS2
# 覆盖规划
# TD25A
# Fields2Cover
# 待实车

状态:本文描述的是当前生产链路(OpenNav/Fields2Cover 官方 B 路线),四色层已收敛为唯一实现、 缓存判据已重建、禁区语义已统一。本轮为架构梳理与离线实测,图片全部来自真实数据, 但过滤网阈值与分组件网连通性的改动建议尚未实施,也未做新的实车运动验收。

这篇是给自己和协作者的一份说明书:填充清扫(弓字形铺满地面的那一半, 沿边是另一条独立的链)从地图到路径中间到底发生了什么,每一层的原理是什么, 会怎么坏。它取代 2026-08-03 那篇《从地图图层到矩形分区》里描述的矩形分区/分簇 方案——那套黄色矩形已经退役,现在的分解由 Fields2Cover 做。

每层一句话

赶时间就看这张表,后面每一节是它的展开。

层 一句话 输入 → 输出
四色层(底座) 这台车在这张图上能去哪、能在哪转身 占据栅格 + 实测位姿 + 标定足迹 → 青/橙/紫/红
① 分区网 把能去的地方切成「区」,区决定转场门户 青 ∧ 任务域 → 区
② 分组件网 区里再按「能转身」切成「组件」,一个组件一次 F2C 请求 橙 ∧ 区 → 组件
③ 过滤网 决定什么不清。生成前、生成后各挂一批 组件/区 → 留下来的那些
④ 生成网 整个组件交给 F2C:BCD 分解 → 定角出带 → TSP → Dubins 转弯 组件多边形 → 扫带 + BCD cell
⑤ 装配网 定序、接驳、圆化、插硬停,拼成一条车能开的线 一堆带 → path + hard_stop_indices
执行层 两棵行为树把这条线跑完:入口树 + 块循环树 path → 车真的动

层级是包含关系,不是并列:区 ⊃ 组件 ⊃ BCD cell ⊃ 扫带。装配网连的是 扫带↔扫带、组件↔组件、区↔区,没有「cell 入口/出口」这一级。


先说结论:规划层是五张网

以前描述故障要绕一大圈说"第几层",层号我自己都编错过。现在统一按职责命名, 不按顺序编号:

【四色层】底座 → ①分区网 → ②分组件网 → ③过滤网 → ④生成网 → ⑤装配网 → 【执行层】

四色层沿用旧名,它是五张网共同的底座,不算网。

有一个必须一起记住的不对称:过滤网不是流水线上的一段,它有生成前和生成后 两个挂载点(组件级门在 F2C 之前,直线度与覆盖率闸门在 F2C 之后)。所以不能说 "过滤网在生成网上面"。


底座 · 四色层

规划的第一件事不是画路线,是先搞清楚这台车在这张图上到底能去哪、能在哪转身。

它先按车体尺寸做三张掩膜,严格嵌套:

raw_free ⊃ centre_feasible ⊃ rotation_safe
地图标空      车中心放得下      车能原地转满 360°

然后从车的实测位姿出发做一次 SE(2) 前向可达搜索——只准前进、按 16 个朝向 展开——把车真正开得到的地方染成青色。

真实活动地图上的四色层

图 1:当前活动地图上的四色层,来自缓存的真实提取结果。橙色是能转身的主干, 紫色是它外面一圈"开得进转不了"的边缘,右侧那些从房间伸出来的红色条是拓扑死区。

  • 青 = 前向可达。能开过去。
  • 橙 = 青 ∧ 能原地转。只有这里允许生成清扫带。
  • 紫 = 青 − 橙。开得进去但转不了身,走廊多半是紫的。
  • 红 = 紫里面的拓扑死区:拿掉它唯一的门户就与外界断开。

为什么它不算网。 网是"筛",四色层是"底"。而且它可以缓存——缓存键里含一个 se2_start_signature(吸附后的起点格 + 16 向里最近的朝向下标),两者相同则结果 逐位相同。这是可证的复用,不是估计。


网 ① 分区网:把能去的地方切成"区"

拿青色和任务范围求交,取连通分量。每一块就是一个区。

区的用途不是清扫,是转场:两个区之间要找门户(把 A 的区膨胀一下去碰 B 的区), 车从一个区走到另一个区必须走青色。

这里刻意用青色而不是橙色——区与区之间的走廊,正是那种"开得过去但转不了身"的 地方,换成橙色门户会直接消失,转场整条断掉。

活动地图上的区

图 2:当前活动地图上只有一个区,整片青色是连通的。区多不多完全取决于地图: 门被家具挡住、走廊被压窄,都会让青色裂开。

输入 青色掩膜 ∧ 任务域
做什么 四连通分量标记
输出 CoverageRegion.travel_mask,供区间转场找门户
会怎么坏 本该连通的被判成两个区 → 平白多一次转场;本该分开的连成一片 → 顺序绕远

网 ② 分组件网:区里再切出"组件"

在每个区内部,拿橙色和这个区求交,再取一次连通分量。每一块叫一个组件—— 它是后面发给 Fields2Cover 的基本单位,一个组件一次请求。

为什么要两层?因为语义不同、不能合并:区是"能开到"(青),组件是"能清扫"(橙)。 一个区里可以有好几个组件,因为走廊往往把橙色切断——走廊本身开得过去,但窄到 车转不了身。

活动地图上的 5 个组件

图 3:同一张图切出 5 个组件,浅青底是那个唯一的区。最大的 55.2 m², 最小的只有 0.008 m²——3 个格子。

输入 橙色掩膜 ∧ 单个区
做什么 四连通分量标记
输出 若干组件,每个单独发一次 F2C 请求
会怎么坏 用的是四连通,见下

这里有个已定位未修的问题。 _connected_components_fast 的 docstring 第一行就是 4-connected,dilate_binary 也是。走廊里橙色会退化成 1 格宽的线,这种线上 一个对角台阶就足以把它切断,凭空多出碎片。

换 8 连通在 4 张真实地图上重算:当前活动地图 6 块 → 4 块,被过滤网打中的数量 1 → 0;四张图合计 40 个丢弃项里有 6 个纯属 4 连通判据的产物。

而对这张掩膜,4 连通可能本就过严:两个对角相邻的 turn_safe 格之间,机器人 要过去只需要平移(平移走青色,不需要 turn_safe 连续),判成两个分量在物理上 没有依据。


网 ③ 过滤网:决定什么不清

所有"决定什么被排除"的判据都归这一张网。生成前后各挂一批。

生成前最主要的是组件级门 ①:

面积 < 0.80 m²  【且】  max(最长带, 包围盒跨度) < 1.30 m   →   丢

注意是且:两条都不合格才丢。一条 0.5 m 宽 3 m 长的窄走廊面积只有 0.6 m², 但跨度 3 m,留下;一个 3 格的雷达噪点两条都不过,丢掉。

丢掉的只是"清扫任务",不是"通行权"——车照样能从那片开过去,只是不在那里 排清扫带。同时它会从覆盖率分母里扣除,也会从转接器的"允许转向"掩膜里扣除。

被丢掉的 3 格碎片

图 4:当前活动地图上唯一被丢的东西。走廊里橙色退化成 1 格宽的细线(浅橙那条 竖线),细线断了一下,掉出 3 个格子成了孤岛。它不是房间角落,是掩膜自己的毛刺。

另一张图上的完整判决

图 5:红=丢,橙=面积不合格但被跨度救回,绿=正常留。注意被救回的那三个 swaths 0——老栅格生成器一条带都画不出来,正是靠 F2C 去救。

生成后还有两道真正生效的:直线度(provider 返回的带必须是一条直线段, 不许把接驳或曲线塞进这个类型)和覆盖率闸门。

一次离线体检

在 4 张真实地图 75 个橙色岛上,用生产代码本身的函数重算了一遍:

  1. 面积那一半阈值从未生效过。 四象限里"面积 ≥ 0.80 且跨度 < 1.30"这一格是 空的;把 A 从 0.6 扫到 5.0,丢弃数恒为 40。门实际上是单条件 max(最长带, 跨度) < 1.30 → 丢。
  2. 跨度阈值正好压着一个岛。 有一个 crit 恰好 = 1.30 被留下,5 cm 栅格抖一格 就翻面。
  3. 门量的是代理量。 门量"中心掩膜面积",真正的收益是"刷过面积"(中心掩膜 按清扫半宽 0.4909 m 膨胀),两者最高差 349 倍。而生产算 KPI 分母时用的 正是刷过面积——门和 KPI 用了两把尺。

判决散点

图 6:横轴是门量的东西,纵轴是实际刷到的面积。那条红竖线是唯一真正生效的判据, 它完全无视纵轴,于是出现 4 对逆序:一个刷过 2.96 m² 的岛被丢,而 2.60~2.75 m² 的四个被留。规模要说实话——75 个里仅此 1 个逆序,两量相关性其实很好, 现行判据结果大体对,但理由不对。

还有一条耦合值得盯:被丢的会从覆盖率分母里扣除,所以门丢得越狠 KPI 越好看。 实测静默移出分母的比例:某张破图 11.2%、9f 3.4%、当前活动图 0%。 图越破扣得越多——恰恰在最该报警的图上,KPI 最不敏感。


网 ④ 生成网:交给 Fields2Cover 出带

TD25A 自己不生成扫带、不做 BCD、不排序。它把一整个组件转成多边形,一次交给 F2C(经 OpenNav Coverage action)。F2C 内部做四件事:

  1. 分解 — BOUSTROPHEDON(牛耕式)BCD,把组件切成子格;
  2. 定角 + 生成带 — 暴力法每 1° 试一个方向;覆盖优先,但带数超过稳定值 1.25× 的角度直接拒,覆盖率相差半个百分点以内时带少者胜;
  3. 排序 — TSP,2 秒时限;
  4. 转弯 — Dubins 曲线,只准前进;倒车只留给死区专用原语。

一次真实规划返回的 4 条牛耕带

图 7:一次真实规划里 F2C 为一个组件返回的结果,4 条牛耕带(绿), 蓝色是带与带之间的缺口。

调用其实分两趟:第一趟只要带、不排序(防止空子格进 TSP,并拿到精确直带 几何);第二趟才要顺序和原生转弯。两趟的 TSP 可能选相反方向,所以要把第二趟的 转弯端点匹配回第一趟的带——不能事后把路线整条反转,那会让 Dubins 曲线倒着 执行。

如果 TD25A 后来改了进入顺序(比如上一个组件的出口在窄门另一侧),还有第三趟 reconnect_turns:把新顺序发回去重新生成转弯。规矩是改了顺序就不许复用原来的 曲线。这条有实测来历——某张生产图上有两个组件带子全部合格,仅仅因为原始入口 端点在窄门的错误一侧就被整个丢弃。

返回的不只是带:还有每条带所属的子格 id、子格多边形本身(就是 BCD cell)、 以及原生转弯位姿序列。

2026-08-27 更正。 这里原先写「子格多边形存进 CoverageSegment.recovery_cells, 作为执行期的恢复单元」——不成立。查全 src/ 后确认它唯一的消费者是一套 「把分区环画在填充层上」的旧显示函数,而那套显示早已不在任何生产路径上 (map_view 用的是另一个 partitioned_plan_display_primitives)。执行期从不读它。 该字段与那两个显示函数已一并删除;BCD cell 现在走 plan.planned_components[].bcd_cells, 只供显示。同时保留了缠在里面的一道 fail-closed 校验——排序后每条扫带必须 能一一对回 provider 返回的源扫带——那条不变式管的是 provider 适配层有没有把 扫带弄丢,与显示无关。

熔断:尝试 ≥ 64 次或累计 ≥ 60 秒即断路;生产禁止静默退回 NumPy,熔断即整单失败。


网 ⑤ 装配网:拼成一条车能开的路

到这一步手上是一堆分散的带。装配网做四件事:

  1. 定序 — 决定区与区、组件与组件的访问顺序;
  2. 接驳 — 三级降级:能共线就走直线,否则试两条单拐角曼哈顿路径,都不行才上 四邻域 A*。每个候选都用精确的车体矩形验证过才接受;
  3. 圆化 — A* 在栅格上每改一次向就是一个 45° 拐角,等效半径 0.05–0.15 m, 低于底盘 0.25 m 的最小转弯半径。要先 Floyd 抽稀(它会让拐角更尖)再把拐角 圆化;
  4. 硬停 — 圆化不动的地方,插入"停车—原地转—再走"。

装配后的可执行路径

图 8:同一份计划装配完成后,从真实规划落盘文件里画出来。绿=清扫带,蓝=接驳, 红点=硬停。

硬停有四条触发规则:段内的折返尖点;段末语义不连续;段交界处局部转弯半径 < 0.20 m(FOLLOWABLE_MIN_RADIUS_M,注意这和底盘的 min_turning_radius = 0.25 不是一回事);段内拐角既转不过来又过不了车体扫掠校验。

拐角漏判是有实车代价的:一次没圆化的 A* 拐角让 MPPI 横向鼓出 0.556 m。

一个缺口的三种走法

三种走法

图 9:几何用的是真实参数(间距 0.7363 m、最小转弯半径 0.25 m)。

  • A 原生 Dubins:端点对得上、整足迹在 raw_free 上重新验过 → 用它,车不停。
  • B 备用 A*:原生被拒时落到这条,要圆化,圆化不动就给硬停。
  • C 半圆掉头:半径 0.368 m 几何上装得下,但生产 require_f2c_native_turns=True 把它关掉了——不许拿 NumPy 时代的备用件冒充原生转弯。

三种情况下刷子都是开着的。 转场和掉头都算清扫动作,不是空驶—— coverage_motion_profile 的原话是 Transfers, native U-turns and shared lane endpoints are coverage motion, not dead travel,只有安全 HOLD/取消才关输出。所以原生转弯 被拒的代价是时间(多一个约 1.2 秒的硬停 + 绕路),不是覆盖。

还有一条硬账:原生转弯使用数 + 安全接驳替换数 == 带数 − 1。每个缺口必须被记 到其中一边,账对不上才整单失败,单纯"原生转弯被拒"本身不炸单。


执行层:两棵行为树

规划层交出一条路径后,执行层用两个独立的 NavigateToPose 目标把它跑完。

为什么是两个而不是一棵树?因为扫头状态不同:入口段扫头抬起、不染色、 不推进度,落下扫头之后才进第二棵树。合成一棵就只能全程抬着或全程落着, 会把没扫过的地面记成已清扫。

① 入口树 navigate_to_coverage_entry.xml —— 当前车位 → 第一条清扫带起点

RecoveryNode(retries=2)          起点或终点落在膨胀层里时 Theta* 会直接拒绝规划
└── FollowPath                   controller = CoverageEntryFollowPath(短腿专用整定)
    goal_checker                 coverage_entry_goal_checker · 25 cm

入口目标是"沿带向后退 ≤ 0.4 m 的虚拟点",真起点由第一块 FollowPath 带动量碾过。

② 块循环树 coverage_execute.xml —— 逐块跑完整条路径

LockCoverageBlockTable           先取回块数,锁住这一版计划
Repeat num_cycles = block_count
├── GetCoverageBlock             拉一块:路径、下一次转角、用哪个控制器、超时
├── Timeout → FollowPath         控制器逐块切换;超时=取消目标,整树失败,不跳段
└── CoverageSpin                 硬停转角

两个细节值得单独记:

  • 不用 stock Spin。 Humble 的 SpinAction 只在构造时读一次 spin_dist, 那时黑板还没写,实车五次硬停全是 Turning 0.00。CoverageSpin 每次 tick 读黑板,读不到就抛异常让整棵树失败。
  • 循环终止语义是这里唯一真正的设计点。 块数先取回、跑固定次数:正常跑完 = 连续 n 次成功,任何一块失败 = 立即失败。绝不能改成"取不到下一块就退出"—— 那会让一次 FollowPath 失败被当成正常结束、静默判成功。有合同测试钉着。

2026-08-27 复核:填充侧的三处变化

状态:已部署、离线全绿(1112 passed / 2 failed,两条既有红灯与本轮无关); 四色层复用已在实车 UI 日志里观测到命中,未做新的实车运动验收。

一、填充规划不再自己重算四色层

四色层实测占一次规划耗时的 93%,而它只取决于「地图 + 足迹合同 + 清扫宽度 + 禁区 + SE(2) 起点签名」——这些都不是填充独有的。「提取地图四色层」按钮和沿边 早就把同一份结果写进磁盘缓存,只有填充一直自己重算。现在填充也直接吃那份。

实车日志(同一次会话):

14:57:25  ▶ 提取地图四色层
14:59:41  ✓ 四色层提取完成 … 134 s  → 已写入缓存
15:23:06    SE(2)通行域: … domains=reused_from_extraction   ← 全图填充规划命中

框选区域也复用。 一开始只放行全图口径,理由是「缓存那份是全图、裁窗那份 窗口外恒空,两者不等价」。实测推翻了:裁窗只是省算力的手段,它带连通性证书、 证书不成立就整域退回全图,所以裁窗结果是全图结果的保守子集。20 m × 35 m 双房地图上窗口外青色差 379.7 m²,两份计划仍逐点相同(3064 点、98 处硬停)。

顺带定罪并修掉三个既有缺陷(都不是这次引入的):

  1. 「提取四色层」按钮写缓存时漏了 SE(2) 起点签名,写成默认的 (-1,-1,-2),而 命中判据严格比对真实签名 —— 按钮写出来的条目永远命不中,填充复用、沿边 复用、定位就绪自动加载全部落空。
  2. 缓存键只摘要 mode|half_width|turn_clearance 三个标量,而这两个标量都是从 轮廓导出的属性:同轮廓、同 turn_margin、只把 tracking_margin_m 从 0 改成 0. 10,三个标量一字不差而可达域不同 → 两份足迹合同共用同一条缓存条目,fail-open。 改为摘要整条合同。
  3. rotation_safe 从没被写进缓存,落盘时被橙色顶替(橙 = rotation_safe ∩ 青)。 以前没人读它;填充一开始吃缓存它就成了转向认证用的域,实测夹具上少 3593 格。 CACHE_VERSION 3 → 4 让旧条目整批失效。

未命中时不写缓存:规划器算的四色层没有红色,写进去会让沿边拿到一份 「没有死区」的假条目。

二、区 / 组件 / BCD 三层现在看得见

图层勾选整块搬进独立弹窗(入口在状态栏 MPPI调参 右边),并新增两层:

  • 组件:橙 ∧ 区 的连通分量,分色填充。此前 UI 只画到「区」,看不出真正送给 F2C 的那一层,「为什么这块和那块分开生成扫带」在图上无从判断。只画进了计划 的组件——被过滤网丢掉的走另一条账,语义不同。
  • BCD cell:F2C 在每个组件内部做的分解。每 cell 一个独立颜色的实线框 + C1/N 编号标签,不填色(填了会盖住组件层)。NumPy 回退没有 F2C 分解, 这层恒空,不拿组件轮廓冒充。

规划日志同时报每个组件切了几个 cell,紧挨着「原生掉头诊断」放——两行对着看: cell 多 → 缝多 → 接头多 → 原生掉头退成蓝色转场的就多。

三、红色层不参与规划判据,但必须带出来显示

现象:全图覆盖清扫跑完,刚提取的红色层消失了。

根因:规划传 classify_dead_zones=False(红色是四色里最贵的一步,实测 +64 s, 而规划从不读它),拿回来的红色是全零;计划因此不带红色;UI 规划成功后用 计划刷新四色层显示,对缺失的红色代入一张空掩膜 —— 红色就这么被顶掉。

修法不是让规划去算红色,而是:复用已提取的四色层时,红色是按钮算过、且过了 出口数校验的权威结果,带进计划(∩ 任务选区)。规划自算那次保持空值,并在 日志里说明「是没算,不是没有」。

「红色不参与任何规划判据」这句话有证据:复用那次带着红色、自算那次没有, 两条路径的三张掩膜与整条路径逐点相同。


故障往哪张网上定位

症状 哪张网 先看什么
该去的地方是灰的 / 车位一变就全变 四色层 SE(2) 起点签名、足迹标定、禁区
平白多一次转场;顺序绕远 分区网 青色是不是被走廊压断
一整块地被拆成好几份 分组件网 四连通把 1 格宽的橙线切断了没有
该清的没清 / 不该清的清了 过滤网 组件面积与跨度、覆盖率闸门
带的方向、条数、形状不对 生成网 F2C 定角目标、熔断计数
路径绕、接驳断、硬停缺或过多 装配网 接驳降到第几级、局部转弯半径
规划好好的,车跑不下来 执行层 控制器整定、Timeout、Spin 角度

口径与遗留

图片口径。 四色层来自缓存的真实提取结果;组件与过滤判决由生产代码本身的函数 在真实地图上重算;路径来自规划落盘文件。只有"一个缺口三种走法"是示意图, 几何用的是真实参数。跨图统计的 75 个橙色岛是扩样本(全图 rotation_safe 的岛, 未与青色和区求交),生产只看其中可达的子集;但"面积阈值不生效"和"判决逆序" 是判据本身的性质,与样本无关。

已记账未实施:

  1. 过滤网门 ① 改为单条件 刷过面积 < T。T = 2.5 m² 与现行只差 1 个判决(最保守的 等价替换);T = 2.75~3.0 与经济学更一致——进一个组件的固定开销 ≈ spin 180° / 0. 4 rad·s⁻¹ = 7.9 s + 2 个硬停 2.4 s + 接驳 ≈ 10 s 起,按长带实测均速 0.28 m/s × 0. 9818 m 宽 = 0.275 m²/s,打平需 2.75 m²。
  2. 分组件网 4 连通 → 8 连通。与上一条改的是同一件事的两头:一个决定"什么算一块", 一个决定"这块值不值得清"。
  3. 把 native_turn_candidate_used_count / native_turn_replaced_by_safe_transfer_count 写进规划落盘文件。现在这些计数只在内存里参与合同校验,规划成功时全部丢弃, 而"多降级了几次接驳"恰恰是成功但变慢的情形。

这三条都属于规划层语义变更,会改变计划结果,需要单独一轮实车验收, 因此本轮一律不动。

avatar

XYT

以文字为星,以思考为野,绘一幅属于自己的星野图。

RECOMMENDED

DrivoR 怎么处理相机:4 张图到 64 个 token

2026-08-25 18:10

想法博客

2026-08-27 23:50

机器人姿态输入对比:DrivoR 的 11 维与 TD25A 的 4 维

2026-08-26 15:30

Table of Contents