一个“瞎子“机器人的自救：GridBot 迷宫实验全记录

bu_ai_da_zi

89人浏览 · 2026-10-06 09:20:30

bu_ai_da_zi · 2026-10-06 09:20:30 发布

实验条件：方向传感器故障——机器人对自己的朝向感知永远是"北"。
最终结果：通关率 100%，平均步数从 370 降到 89，自动评测满分 50/50。

具身智能状态估计路径规划BFSPOMDP

1. 先看结果
2. 实验设定：坏掉的传感器
3. 我的解决方案总览：五个机制
4. 机制一：航位推算
5. 机制二：记忆地图
6. 机制三：BFS 最短路 + 三级目标优先级
7. 机制四：探索预算
8. 踩坑记录：六个问题与解决过程
9. 深层算法：这套玩具背后站着什么
10. 复现方式
11. 结语

1. 先看结果

指标	醉汉 baseline	优化后	提升
平均步数	370	89	快 4.2 倍
金币收集率	58%	97%	1.7 倍
通关成功率	8/15 (53%)	15/15 (100%)	1.9 倍

逐图明细（每张图跑 3 次，策略确定性，3 次结果完全相同）：

地图	baseline 步数 / 金币 / 通关	优化后步数 / 金币 / 通关
地图 1 新手村	370 / 67% / 1÷3	81 / 3÷3 / 3÷3
地图 2 小径	319 / 75% / 2÷3	94 / 4÷4 / 3÷3
地图 3 花园	408 / 73% / 2÷3	90 / 5÷5 / 3÷3
地图 4 迷宫入口	333 / 50% / 2÷3	90 / 6÷6 / 3÷3
地图 5 挑战迷宫	419 / 24% / 1÷3	92 / 6÷6 / 3÷3

① 状态估计 —— 我在哪、朝向哪本实验：用坐标位移反推朝向　｜　进阶：扩展卡尔曼滤波、粒子滤波、SLAM② 世界建模 —— 环境长什么样本实验：字典存二值地形（空地/墙/金币）　｜　进阶：占据栅格 + log-odds 概率更新③ 路径规划 —— 怎么走过去本实验：BFS 最短路，每步重规划　｜　进阶：A*、D* Lite、带转向代价的状态栅格④ 目标决策 —— 现在该干什么本实验：金币优先级 > 预算内探索 > 终点　｜　进阶：信息增益/距离启发式、POMDP 策略

图 1　具身智能算法栈的四个层次，以及本实验与工业界做法的对应关系

2. 实验设定：坏掉的传感器

GridBot 是一个 10×10 网格世界的教学实验。机器人每步能拿到：

四周感知 front / left / right / back，取值是"空地 / 墙壁 / 金币 / 终点 / 边界"；
自己的坐标 position，以及终点坐标 target（公开信息）；
记忆空间 memory，跨步持久的列表，可读可写；
每步返回一个动作：前进 / 左转 / 右转 / 后转。

Baseline 是"醉汉策略"——随机走，撞墙就随机转身，平均 350 步通关、成功率 60%。

而我的实验条件更狠：方向传感器坏了，perception["direction"] 永远返回 "北"。偏偏所有感知都是相对自身朝向的——朝向错了，"我看到的东西在地图哪一格"就算不出来，建图、寻路、捡金币全部无从谈起。

一句话概括这个实验的难点：信息缺失 + 信息不可信，只能用"行动—反馈"把丢掉的信息重新算出来。

3. 我的解决方案总览：五个机制

先说全貌。整套方案就是五个机制叠起来，每个机制解决一个具体问题：

#	机制	解决什么问题	关键代码	为什么有效
1	航位推算	不知道自己的朝向	坐标位移反推 + 转向叠加	位移是绝对观测，无法被故障传感器污染
2	开局校准	第一步之前朝向未知	先走一步，走不动就右转	一次移动就能反推出朝向，成本 ≤ 4 步
3	记忆地图	不知道环境长什么样	相对感知 → 绝对坐标	地图只增不减，重复观测互相印证
4	BFS 三级决策	不知道该往哪走	金币 > 探索 > 终点	每步都是最短路，目标单调推进，不会死循环
5	绕开终点格	提前通关导致金币丢失	BFS 的 `avoid_target`	终点是吸收态，必须当障碍而非通路

下面逐条拆开讲，包括代码、为什么这么写、以及它成立的前提。

4. 机制一：航位推算 —— 用动作和坐标把朝向"算"回来

4.1 思路

方向传感器失灵，但坐标传感器是好的。而我自己发的每一条指令我自己都记得，于是：前进之后坐标变化了多少，那个变化量的方向，就是我的真实朝向。

(0, -1) → 北    (1, 0) → 东    (0, 1) → 南    (-1, 0) → 西

四种位移唯一对应四个绝对朝向，没有歧义。这不是猜测，是引擎给出的坐标事实。

转向则更简单：转向时位置不变，但"左转/右转/后转"是我自己发出的指令，必然执行成功，所以在已知朝向上叠加 ±90°/180° 即可；撞墙没动，朝向也不变。

动作 u_t：前进 / 左转 / 右转 / 后转自己发出的指令，必然被执行成功预测：θ̂_t = θ_t−1 ⊕ u_t转向可精确叠加 ±90° / 180°，所以预测无噪声观测：位移 Δx = 位置_t − 位置_t−1坐标传感器每步给出绝对位置，无累积漂移校正：θ_t 由 Δx 唯一确定(0,1)→南　(1,0)→东　(0,−1)→北　(−1,0)→西每步循环

图 2　朝向估计的预测—校正闭环（贝叶斯滤波的退化情形）

4.2 代码

# 航位推算：用"上一步发生了什么"更新真实朝向
last_pos = state[0]
500">if last_pos 500">is 500">not 500">None 500">and pos != last_pos:
    # 上一步"前进"且真的移动了 → 位移方向 = 真实朝向
    moved = (pos[0] - last_pos[0], pos[1] - last_pos[1])
    500">for d, delta 500">in DELTA.items():
        500">if delta == moved:
            state[0] = d
            state[0] = 500">True
            500">break
500">elif state[0] 500">in TURN_DELTA 500">and state[1]:
    # 上一步执行了转向 → 在已知朝向上叠加转角
    state[0] = (state[1] + TURN_DELTA[state[2]]) % 4
state[0] = pos

三条分支覆盖全部情形：动了 → 反推；转了 → 叠加；撞墙没动 → 不变。

4.3 为什么它不会累积漂移

	观测类型	误差行为
轮式里程计（真机器人）	增量观测：这一小步走了多远、转了多少度	微小误差逐步累加，位置误差 O(t)、朝向误差 O(√t) 发散
本实验的 `position`	绝对观测：我在 (x, y)，是真值	误差每步被观测直接清零，不存在累积

所以我这里其实不是"估计"，而是解方程：朝向是 4 个离散值之一，观测（位移方向）能把候选一次性收敛到唯一答案。唯一的代价是：没移动的那些步，朝向无法被观测校正——正好由"转向自己记 + 撞墙不变"补齐。

4.4 开局校准：解决"第一步之前朝向未知"

500">if 500">not state[0]:
    500">if perception[0] 500">not 500">in (1, 2):
        500">return 0      # 能走就走一步，一动就校准成功
    500">return 0          # 走不动就换个方向再试

代价上界：最坏情况原地转 3 次找到通路，第 4 步必然校准成功。换来的是后面所有建图都建立在已证实的朝向上——否则错一步，整张地图的坐标就全歪了。

5. 机制二：记忆地图 —— 把局部感知贴到全局坐标

500">for rel 500">in range(4):
    abs_dir = (heading + rel) % 4          # 相对方向 → 绝对方向
    dx, dy = DELTA[abs_dir]
    cell_pos = (pos[0] + dx, pos[1] + dy)  # 贴到全局坐标
    cell = perception[REL_CELL[rel]]
    worldmap[cell_pos] = 0 500">if cell == 1 500">else cell

三个设计决定，每个都有原因：

地图存在 memory[0] 里（字典 {(x, y): 地形名}）。 memory 是引擎给的跨步持久空间，memory[0] 放状态字典，后续每步取出复用。
地图外一律记成"墙壁"。后面的搜索天然不会跑出地图，不用在每个算法里重复做边界判断。
站上金币格要把它从地图里消掉。金币被捡走后若还当目标，就会陷入"到那儿没东西 → 再去别处 → 又回来"的死循环。地图必须与真实世界同步。

6. 机制三：BFS 最短路 + 三级目标优先级

500">def bfs(goal_test, avoid_target=500">False):
    queue = deque([pos]); prev = {pos: 500">None}
    500">while queue:
        cur = queue.popleft()
        500">if cur != pos 500">and goal_test(cur):
            path = []
            500">while cur != pos:
                path.append(cur); cur = prev[cur]
            500">return path[::-1]
        500">for dx, dy 500">in DELTA.values():
            nxt = (cur[0] + dx, cur[1] + dy)
            500">if nxt 500">in prev 500">or 500">not in_grid(nxt):
                500">continue
            500">if avoid_target 500">and nxt == target:      # 别从终点身上踩过去
                500">continue
            500">if worldmap.get(nxt) == 0:          # 已知墙不撞
                500">continue
            prev[nxt] = cur; queue.append(nxt)      # 未知格子也允许走 = 探索能力
    500">return 500">None

优先级：① 有已知金币 → 去最近的那个（路上不许穿过终点）； ② 没有已知金币且步数 < 75 → 去最近的"探索前沿"（已知通路旁还有未知格子的位置），并在最短的若干条候选里优先挑第一步不用转身的（转身也算一步）； ③ 预算用完或没有前沿可探 → 直奔终点。

为什么绝不撞墙：BFS 只把"非墙壁"的格子入队，而相邻格子必然已被感知记录过（每步记录四周 4 格），所以路径第一步永远是已知可通行格。
为什么不会死循环：每步都在缩短到当前目标的最短路距离，而目标集合（金币 → 前沿 → 终点）是单调消耗的。
为什么要 avoid_target：踩上终点是吸收态，游戏立即结束。"不把终点当目标"和"不从终点经过"是两件事——后者我一开始漏了（见第 8 节坑 4）。

7. 机制四：探索预算 —— 一个真实的取舍问题

探索越久金币越多，但步数也越多；评测规则要求"步数 < 100"才拿满分 20 分。所以这本质上是带约束的最优化问题：在步数 < 100 的约束下最大化金币收集率。

探索预算	平均步数	金币率	总分
70	83	89%	50/50
75	89	97%	50/50
85	97	97%	50/50
95 以上	≥100	97%	45/50

75 步是"金币已经吃满"里步数最少的一档，离 100 步红线还有 11 步余量。再往上加，金币已经涨不动（原因见坑 6），步数却会突破 100 掉分。

8. 踩坑记录：六个问题与解决过程

整个实验我一共栽了六次，按"现象 → 排查 → 根因 → 修复"记下来。先看总览：

#	问题	根因	修复
1	完全不知道自己的朝向	`direction` 永远返回"北"，是废数据	航位推算 + 开局校准
2	机器人卡在角落来回抖动	前沿判定把地图外的格子也算成未知	加地图范围检查
3	探索到一半就提前通关	终点格也被当成了"探索目标"	探索目标判定中排除终点
4	修完坑 3，还是提前通关	路径从终点格上穿过去（路过也算踩上）	BFS 加 `avoid_target` 参数
5	金币与步数此消彼长	探索预算固定，两项评分互相拉扯	扫参定在 75 步
6	金币率怎么调都卡在 97%	地图 5 声明 7 个金币，实际只有 6 个格	确认是地图数据的天花板

坑 1：朝向完全不可知 —— 整个实验的出发点

现象：perception["direction"] 永远返回"北"。四个方向的感知都是相对自身朝向的，朝向错了就意味着"我看到的东西在地图哪一格"完全算不出来。

排查：确认这是实验条件强制的传感器故障。盘一遍还有哪些信息可信：四周格子（可信）、自己的坐标 position（可信）、终点坐标（公开）、记忆空间（可写）。

根因：传感器坏了，但"我做过什么动作"这件事我永远知道——因为动作就是我发出的。

修复：航位推算（位移反推 + 转向叠加）+ 开局校准（见第 4 节）。

坑 2：机器人卡在地图角落来回抖动

现象：步数暴涨，但地图格子数停在 37 不再增长，机器人在 (9,0) 和 (9,1) 之间反复横跳。

排查：打印每步的"可探索前沿"列表，发现右上角靠边的格子永远在列表里—— 它旁边的"未知格子"是地图外面的坐标。

根因：前沿判定把地图外的格子也算成了未知区域，靠边的格子永远满足条件，BFS 一直把它当最近目标。

修复：

500">def frontier_goal(c):
    cx, cy = c
    500">return any(
        0 <= cx + dx < GRID_SIZE 500">and 0 <= cy + dy < GRID_SIZE
        500">and (cx + dx, cy + dy) 500">not 500">in worldmap
        500">for dx, dy 500">in DELTA.values()
    )

坑 3：机器人"探索"到一半就提前通关了

现象：地图 3 只走 43 步就结束，5 个金币只捡到 2 个。疑点：预算才用 43/75，地图还有一大片没探。

排查：日志显示第 40 步机器人在 (9,6)，地图已有 68 格、还有 10 个前沿没去，但下一步就朝 (9,7) → (9,8) → (9,9) 走过去了——那是终点。

根因：终点格 (9,9) 在还没探索过它周围时，也满足"旁边还有未知格子"，被当成普通探索目标，而它恰好最近。踩终点不可逆。

修复：探索目标判定里排除终点，把"探索"和"去终点"彻底分开：

500">def frontier_goal(c):
    500">if c == target:      # 终点不算探索点
        500">return 500">False

修完地图 3 从 43 步 / 2 金币，变成 90 步 / 5 金币全捡。

坑 4：修完坑 3，还是提前通关

现象：地图 2 仍然只跑 38 步、4 个金币只捡 2 个。

排查：第 35 步机器人在 (9,7)，还有 14 个探索点没去、预算才用一半，但它连着三步 (9,7) → (9,8) → (9,9) 又踩上了终点。

根因：它的目标是探索点 (9,8)（合法），但最短路径正好从终点格上穿过去。 "不把终点当目标"和"不从终点身上路过"是两件事。

修复：

500">if avoid_target 500">and nxt == target:
    500">continue        # 别从终点身上踩过去

修完地图 2 从 2/4 金币变成 4/4 全捡。

坑 5：金币与步数此消彼长，怎么调都有短板

现象：一开始拍了个 80 步（金币 87%、步数 81）；修完绕行问题后，同样 80 步，步数涨到 93，余量只剩 7 步。

根因：两项指标存在真实冲突——金币靠探索换来，探索必然消耗步数；且金币率有上限（坑 6），触顶后再加预算就是纯亏步数。

修复：扫参后定在 75 步（见第 7 节）。

坑 6：金币率怎么调都卡在 97%

现象：预算加到 120 步，地图 5 依然只有 6/7。

排查：日志显示地图 5 已无可探索前沿（整张图探完）。于是去数地图数据本身：

500">from maps 500">import ALL_MAPS
500">for m 500">in ALL_MAPS:
    actual = sum(1 500">for row 500">in m[0] 500">for c 500">in row 500">if c == 2)
    print(m[0], 1, m[2], 3, actual)

地图 1 — 新手村:   声明金币=3 实际=3  差异=0
地图 2 — 小径:     声明金币=4 实际=4  差异=0
地图 3 — 花园:     声明金币=5 实际=5  差异=0
地图 4 — 迷宫入口: 声明金币=6 实际=6  差异=0
地图 5 — 挑战迷宫: 声明金币=7 实际=6  差异=1   ← 问题在这

根因：MAP_5 的元数据 "coins": 7 与网格里真实金币格数量不一致。地图 5 上限就是 6/7，全局天花板 = (3+4+5+6+6) / (3+4+5+6+7) = 96% （评测按每图比率取平均，显示 97%）。

修复：无需改代码，但要确认它不是策略缺陷—— 地图上真实存在的金币，我的机器人 100% 全部捡到了。教训：分数上不去时，先分清"我的算法不行"还是"数据本身有问题"。

9. 深层算法：这套玩具背后站着什么

9.1 状态估计：我写了一个退化版的贝叶斯滤波

预测：bel⁻(θ_t) = ∫ p(θ_t | u_t, θ_{t-1}) · bel(θ_{t-1}) dθ_{t-1}
校正：bel(θ_t) ∝ p(x_t | θ_t) · bel⁻(θ_t)

我的场景是它的退化情形：状态只有 4 个离散值、运动确定性、观测无噪声，后验概率直接塌缩成一个确定值——不是"估计"，是"解方程"。真实机器人有三代方法：

① 扩展卡尔曼滤波（EKF）

状态：x = (x, y, θ)ᵀ，信念 N(μ, Σ)
预测：μ⁻ = f(μ, u)              Σ⁻ = F Σ Fᵀ + Q      F = ∂f/∂x |μ
校正：K  = Σ⁻ Hᵀ (H Σ⁻ Hᵀ + R)⁻¹                     H = ∂h/∂x |μ⁻
      μ  = μ⁻ + K (z − h(μ⁻))   Σ = (I − K H) Σ⁻

直观理解：Q 是"我对自己动作有多不信"，R 是"我对传感器有多不信"，卡尔曼增益 K 就是在两个不信任度之间做加权平均。

② 粒子滤波（Particle Filter）

初始化：采样 N 个粒子 {x_i}，权重 1/N
循环：
  1. 预测：每个粒子按运动模型采样   x_i ← f(x_i, u) + 噪声
  2. 加权：w_i ∝ p(z | x_i)        （谁0观测，谁就更重）
  3. 重采样：按权重有放回地重抽 N 个粒子（权重大的被复制，小的被淘汰）

杀手级场景是"全局定位"：信念可能同时有几十个峰（几条长得一样的走廊），高斯分布表示不了，粒子群可以。

③ SLAM

当位姿和地图互相依赖（不知道自己在哪 → 地图画歪 → 定位更不准），必须联合估计位姿与地图。 EKF-SLAM 把地图特征点拼进状态向量（协方差随特征数平方增长），现代方案是图优化：把位姿与观测建成图，用最小二乘整体求解。本实验若换成"坐标定位故障"条件，问题立刻退化到这个难度的下限版本。

9.2 建图：从二值字典到占据栅格

L(m_i) ← L(m_i) + log[ p(m_i | z_t) / (1 − p(m_i | z_t)) ] − L_0
其中 L = log[ p / (1 − p) ]

数值稳定性：概率连乘会下溢到 0，log-odds 把乘法变加法；
可累积：同一格被反复观测，每次加一个证据项，置信度自然增长；
需要逆观测模型 p(m_i | z_t)：传感器只说"这条激光被挡住了"，要从"射线打到了东西"反推"沿途格子大概是空的、终点那格大概有障碍"。

9.3 路径规划：BFS 只是这条链的起点

算法	关键思想	复杂度	适用场景
BFS（我的实现）	无权图逐层扩散	O(V + E)	格子等权、小地图
Dijkstra	带权图，按已用代价出队	O((V+E) log V)	地形代价不同
A*	`f = g + h`，启发式引导	通常远快于 Dijkstra	h 可采纳时才保证最优
*D Lite**	环境变化后增量修复路径	比重算便宜一个量级	边走边发现障碍
状态栅格 / Hybrid A*	状态扩成 `(x, y, θ)`	状态数 ×4 起步	真车不能原地转，必须走圆弧

关于 A* 的 h：四邻接网格里取曼哈顿距离是可采纳的（实际最短步数不可能小于它），所以 A* 既搜得快、又保证最优。BFS 其实就是"h ≡ 0 的 A*"，Dijkstra 是"h ≡ 0 的带权版"。

一个诚实的瑕疵：我的 BFS 只数格子、不数转身，而转身在规则里也算一步。我靠"等长候选里优先不转身"打了个补丁，正确做法是把状态扩成 (x, y, θ) 或给换方向的边加权重：

# 改造示意：把 (位置, 朝向) 作为搜索状态，边权 = 动作步数
500">for action 500">in (0, 1, 2, 3):
    cost = 1                                  # 每个动作都消耗 1 步
    nxt_state = apply(pos, heading, action)    # 前进会移动，转向只改朝向
    push(nxt_state, g + cost)                  # 4 倍状态空间的 Dijkstra/A*

9.4 金币收集顺序：一个 TSP 的在线版本

"按什么顺序把金币全捡了、总步数最短"就是旅行商问题（TSP）——NP 难。我用的"每次奔最近的金币"是最邻近贪心：O(n²) 很快，但近似质量不保证。进阶做法：

2-opt 局部搜索：反复尝试交换路径里两条边的连接方式，能变短就接受；
Christofides 算法：对度量 TSP 给出 1.5 倍近似保证（最小生成树 + 最小权完美匹配 + 欧拉回路短接）。

# 2-opt 伪代码
improved = 500">True
500">while improved:
    improved = 500">False
    500">for i 500">in range(len(route) - 1):
        500">for j 500">in range(i + 2, len(route)):
            500">if dist(route) > dist(reverse_segment(route, i + 1, j)):
                route = reverse_segment(route, i + 1, j)
                improved = 500">True

9.5 目标决策：POMDP 与"探索—利用"权衡

我真正的困难不是排序，而是金币位置未知——典型的探索与利用权衡。形式化模型是 POMDP（部分可观测马尔可夫决策过程）：

POMDP = ⟨ S, A, T, R, Ω, O ⟩
  S  状态（我在哪、地图长什么样、金币在哪）
  A  动作（前进/左转/右转/后转）
  T  转移概率 p(s' | s, a)
  R  奖励（捡到金币 +，撞墙 −，到达终点 +）
  Ω  观测（四周格子 + 坐标）
  O  观测概率 p(o | s', a)
策略 π(a | b) 作用在信念 b(s) = p(s | 历史) 上
信念更新：b0) ∝ O(o | s1 | s, a) b(s)

要点：状态看不见，所以策略不能依赖状态，只能依赖信念。工程上常用简单启发式——给每个探索点打分：

score(frontier) = 信息增益(frontier) / 路径代价(frontier)

即"能新看到多少格子 ÷ 跑过去要多少步"。我那 75 步预算是这个打分函数的极简替代品：先做高分动作，预算耗尽就收工。

9.6 一个被复用的工程思想：滚动时域

循环：
  1. 基于当前信息求解未来一段的最优计划
  2. 只执行计划的第一步
  3. 世界变化 / 拿到新观测 → 丢掉剩余计划，回到第 1 步

为什么不用一次性完整计划？因为信息不断更新（新发现的墙会让原计划作废），计划越长越容易失效。短视 + 勤重算，在动态环境里比"一次算到底"更鲁棒。

9.7 如果继续升级这个实验，我会做三件事

升级	具体改动	预期收益
*A + 转向代价**	搜索状态从 `(x, y)` 扩成 `(x, y, θ)`，动作边权统一为 1 步	让"少转身"成为算法内生的解，而不是补丁
2-opt 金币顺序	对已发现的金币做局部搜索排序，替换"每次奔最近"	多金币地图上减少绕路
粒子滤波实验	人为给 `position` 加噪声，对比航位推算漂移与粒子滤波恢复	亲眼看懂"为什么真机器人要 EKF / SLAM"

10. 复现方式

cd gridbot
python run.py --evaluate        # 跑完整评测，输出分数报告
python run.py --map 5 --fast    # 可视化跑地图 5
python run.py --baseline        # 跑醉汉 baseline 做对比

代码要点集中在 my_bot.py 的 decide() 函数里： 航位推算 → 写记忆地图 → BFS 三级目标规划 → 动作换算。