蒙特卡洛定位 — 一千个猜测如何收敛成一个位置

这一章讲什么: 全书第一个真正的「AI」——蒙特卡洛定位(粒子滤波)。 思路一反直觉:机器人不去算自己在哪,而是同时养一大堆「自己可能在的位置」 (poses,位姿),让每个猜测都按编码器数据走一遍、让场地边界给每个猜测打分, 然后按分数重新抽签——分数高的猜测被复制,低的被淘汰。几轮下来, guesses 聚集的地方 就是机器人最可能在的地方。 机器从「反应式」跨到「假设—检验」式,靠的是一条漂亮的概率循环。

1. 先看现象:页面上那朵渐渐聚拢的「猜测云」

本章的上机画面是全书的转折点。打开 localisation 页面,按 Start, 你会看到测试场的轮廓线(蓝框 + 右下角缺口)和满场随机撒的几十个点—— 每一个点都是一次「机器人可能在这」的猜测,带着一个朝向小箭头。

用手动驾驶(本章新做的键盘 WASD 控制)开动车: - 不动时,点云均匀铺开——系统没有任何偏好; - 往墙开一段,贴墙的猜测纷纷消失(它们「撞墙」了,被判不可能); - 来回几趟,点云收缩成几簇,最后聚成一小团,跟着车走⁴。

这一团就是答案:系统从未「知道」坐标,它只是让不可能的猜测死掉了。 本章的全部机制,就是给这句直觉配上数学和代码。

2. 顶层全景:概率循环的四拍

 ① 养猜测   全场随机撒 population_size 个 pose(x, y, θ)
 ② 推着走   编码器每来一帧:每个 pose 按 (平移, 转角) 全体移动 + 注入噪声
 ③ 打分数   观测模型给每个 pose 一个权重(出界的 0.1,在界内的 1.0;后升级为渐变)
 ④ 重抽签   低方差重采样:按权重复制好猜测、淘汰坏猜测;但只在「信息量够了」时做(ESS)

 图说:四拍循环 = 预测(①②)与更新(③④)交替——粒子滤波的标准心跳。

3. 核心原理之一:位姿(pose)——一个猜测的三元组

位姿(pose)= 一组能代表机器人位置和朝向、可以被代码操纵和画出来的数⁵。 地面机器人活在二维平面,用笛卡尔坐标:x、y 定位置,第三个数 θ(theta, 也叫 yaw 或 heading)定朝向——机器人「正前方」沿自己的 x 轴⁵。

 世界坐标:(0,0) 在角上,x 沿一边、y 沿另一边;毫米为单位
 pose A:(500, 500, 0)      → 场中央,头朝 +x
 pose B:(150, 100, π/6)    → 左下角,头偏 30°

 图说:位姿就是「站在哪 + 脸朝哪」;theta 用弧度,因为三角函数都用弧度。

书里顺手给了一个坐标系约定,叫 ENU:East = X、North = Y、Up = Z; 附带一句实话——「你的墙多半不对着正北」,所以实际简化成「x、y 对齐场地的墙 (假设墙互相垂直)」⁵。毫米对齐编码器读数,弧度对齐三角函数——单位纪律从第 11 章延续下来。

代码上 pose 用 NumPy 的结构数组表示(x、y、theta 三个字段(带名字的格子)),一批 pose 叫 Poses——所有操作(旋转、平移)都是对整个数组一次性做的,这是后面两万个 猜测也能实时跑的根基。

4. 核心原理之二:两种基本动作——旋转与平移

位姿的变换只有两种,照原书接住:

• 旋转(rotate):θ 加一个数。一行 NumPy:poses['theta'] += rotation—— 所有猜测原地掉头,位置不动⁶。
• 平移(translate):沿自己的 θ 方向走一段 L。需要三角函数: x 加 cos(θ) × L,y 加 sin(θ) × L⁶。

 平移的三角学:θ=30° 走 100 mm
   x 增加 cos(30°)×100 ≈ 87 mm
   y 增加 sin(30°)×100 ≈ 50 mm
 图说:斜着走 = 横纵各分一点;cos 管横、sin 管纵。

注意一个语义:平移方向是「自己面朝哪」而不是世界坐标轴——所以同一个 L, 不同 θ 的 pose 会走向不同地方⁶。这正是「每个猜测独立演化」的基础。

5. 核心原理之三:场地建模——一列顶点就是一面墙

测试场 C 被翻译成顶点环:一圈 (x, y) 坐标,首尾相接即墙⁷:

walls = [(0,1500), (1500,1500), (1500,500), (1000,500), (1000,0), (0,0)]

这就是第 09 章「内尺寸取公制倍数」的回扣——建模时这些整数直接进代码。 形状有讲究:带缺口的 L 形、全是直角;全对称的形状(纯方框)对机器人「长得都一样」, 定位无从收敛——第 09 章选 C 布局的那句「无旋转对称,后文定位用得上」,在这里兑现⁷。

地图经 localisation/map topic 发给网页,网页用 Chart.js 插件在散点图下层画蓝色多边形 (canvas 的 path:moveTo → 逐顶点 lineTo → closePath → stroke)——模型一份, 机器人与浏览器共用⁸。

6. 核心原理之四:从编码器到运动——每个猜测走同样的路

现在把第 11 章的运动学反转:那里是「给定弧线,算两轮各走多少」; 这里是「量到两轮各走了多少,反推走了什么弧」⁹。

每个编码器数据帧,先取增量(deltas):本次读数 − 上次读数。然后两个公式:

 mid_distance = (左增量 + 右增量) / 2      ← 车心中点走的距离
 theta        = (右增量 − 左增量) / 轮距    ← 转过的角度

 图说:两轮读数相同 → 直线,theta = 0(特判);
      不等 → 右多左少 = 逆时针;一轮正一轮负 = 原地转。

把这段弧线拆成三步作用到每个 pose 上:rotate(θ/2) → translate(mid_distance) → rotate(θ/2)——先转到弧的切线方向、沿弦走到终点、再转到出射方向。 书里坦白:这是小弧近似,把弧当弦(直线捷径)处理⁹。

两个实现细节值得记住:

• 轮距要量「接地中点」之间的距离(作者 136 mm),比第 11 章的轮对轮 159 mm 小—— 轮子略外八字的话,两个值就是不一样⁹;
• 每帧注入随机噪声(randomise_motion):同样指令下真实机器人的滑动、颠簸是随机的, 猜测群必须散开一点才能覆盖真相——粒子滤波的「粒子」二字指的就是这个扩散¹⁰。

7. 核心原理之五:观测模型——给每个猜测打分

移动之后,该问「这个猜测说得通吗?」。观测模型(observation model)做的事: 拿预测和观测比,输出「这个位姿有多可能」¹¹。

第一个模型最朴素——边界模型:pose 在场内 → 权重 1.0;出界(包括掉进缺口)→ 0.1¹²。

def calculate_weights(self, poses):
    return np.where(self.in_boundary(poses), 1.0, low_probability)

这里有个精细的设计决策,书里专门解释:低概率取 0.1 而不是 0—— 因为多个观测模型要相乘组合,权重是 0 的猜测会被永远判死、别的模型救不回来; 给个「很小但非零」的数,保留翻案的可能¹²。这个「留活口」原则是概率推断(贝叶斯方法:拿新证据不断更新相信程度)工程里的常见手感。

后来边界模型升级成渐变(coat of paint / crosshair)模型:不再非黑即白, 而是对每个 pose 前后左右取 5 个采样点,按离墙的远近给连续权重—— 贴墙的猜测降权、离墙一两百毫米的最受欢迎,系统开始表现出「偏好」而非「审判」¹³。

而书里点破了这一整套循环的名字:

「大量猜测 → 更新 → 观测 → 再猜测,这个循环就是蒙特卡洛模拟的核心。」¹¹

8. 核心原理之六:重采样——按分数重新抽签

有了权重,就能做重采样(resampling):从旧猜测群里按权重抽一批新猜测。 权重高的 pose 被抽中多次(等于自我复制),权重低的逐渐绝迹—— 种群朝着「说得通」的方向进化¹⁴。

书里选的算法是低方差(结果不散、稳定)重采样(low-variance resampling),机制非常几何:

 1. 把所有权重复制成数轴:每段宽度 = 该 pose 的权重
 2. 数轴总长 ÷ 样本数 = 间隔 interval
 3. 在 [0, interval) 里随机取一个起点
 4. 从起点起,每隔 interval 落一刀;每刀落在谁的段里,谁就入选

 图说:权重最大的 c 段最宽、挨刀最多,被复制好几次;
      等间隔 + 随机起点,保证抽样的均匀性,计算还快。

代码四行(NumPy):cumsum 累加权重 → 算间隔 → arange 生成等距点 → searchsorted 查每个点落在哪个权重段。书里补了一刀:NumPy 有现成的 rng.choice(按概率随机选),但更慢也更不稳——低方差法是标准答案¹⁴。

接入主循环后还有个人性化设计:显示用的样本(200 个)在重采样前抽取, 保证你看到的是「带旧权重的分布」;种群本身则重采样回满额(先 2,000 调通,再 20,000 提精度)¹⁵。

9. 核心原理之七:ESS——什么时候才值得重采样

系统立起来了,但书里马上演示了它的自伤:每帧都重采样的话,车没动时权重几乎没变, 重采样只是在噪声里复制、误杀好猜测;更糟的是,一旦「错误的簇」偶然壮大,正确的就被清洗掉。 控制手段是一个量:有效样本量(Effective Sample Size, ESS)¹⁶。

直觉:权重均匀(「人人都是 1/N,没有证据偏袒谁」)时 ESS ≈ 种群数——重采样没有意义, 只会放大噪声;权重开始分化(「几个明显的赢家」)时 ESS 骤降——这时重采样才有信息量。 作者给的例子:100 个粒子,ESS 23 ≈ 只有 23 个在真正贡献;ESS 99.7 ≈ 一片散沙¹⁶。

计算照原书的四步:归一化 → 平方 → 求和 → 取倒数:

 ESS = 1 / Σ(每个归一化权重)²
 图说:平方把小权重压得更小,求和看「集中度」;越集中,倒数越小,ESS 越低。

接入:ess_threshold = population_size // 10(初始取种群数的十分之一); ESS 高于阈值 → 跳过这帧不重采样;低于 → 重采样,ESS 回弹。 上机效果是 ESS 曲线呈锯齿:移动 → 证据累积 → ESS 下降 → 重采样 → 弹回¹⁷。

一句必须记住的诚实注脚书里给了:

「ESS 告诉你的是权重有多集中,而不是滤波对不对——它回答不了『我们选的方向对不对』。」¹⁶

10. 调试工具:先减小种群,验证运动模型

第 108 节(localisation debugging tools)的方法论值得单独带走的: 出问题先怀疑运动模型,而验证它的办法是把种群缩到 2 个—— 一个直行、一个原地转,肉眼核对「机器人动了多少、点动了多少」。 若直行时两点连线发散、转弯时角度不符,先查轮距与轮径(左右轮径可能不同, 需要分别的 left_wheel_radius/right_wheel_radius 补偿), 把系统误差调干净,再把种群放回 200¹⁸。 先修系统偏差,再谈概率——噪声模型盖不住错的运动学。

11. 作者的判断与证据

• 判断: 定位的本质是假设管理,不是坐标计算。「机器人从对传感器做反应, 升级为对『自己在哪』提出假设并检验」——本章总结原话⁴。
• 判断: 0.1 而非 0 的低概率、显示样本重采样前抽取、ESS 门控—— 三处都是「给系统留余地」的工程哲学,书里都给了理由¹²¹⁵¹⁶。
• 事实层: 公式(mid/theta 反解、三步弧线、ESS)可直接推导验证; 轮距 136 vs 159 的区分、population 2,000→20,000 的梯度,都是可复现的实验点⁹¹⁵。
• 边界声明: ESS 不管对错,只管集中度;观测模型此刻只有边界一个—— 单一模型的局限明写,多模型融合是第 17 章¹⁶。

12. 边界与局限

• 只用一个观测模型(边界)。 场地中央的猜测永远权重 1.0,系统对「场子中间在哪」 毫无分辨力——收敛要靠机器人靠近边界才发生。距离传感器、IMU 的融合在第 17 章。
• 弧当弦的近似 + 轮子打滑。 运动模型短程够用,长程仍漂;没有显式的打滑模型。
• 种群大小的工程权衡没展开。 20,000 是经验值;计算量、收敛速度、粒子耗竭 (particle depletion)的权衡只露了一面。
• 没有理论保证。 粒子滤波收敛性的数学(几乎必然收敛等)不在本书范围; 这是动手实现,不是定理课。

13. 可带走的

1. pose = (x, y, θ):位置两个数、朝向一个数;毫米 + 弧度;坐标轴对齐场地的墙⁵。
2. 位姿变换只两种:旋转(θ 相加)、平移(x += cos θ·L、y += sin θ·L,沿自己朝向)⁶。
3. 场地 = 顶点环;不对称形状才能定位;模型经 MQTT 与浏览器共用一份⁷⁸。
4. 编码器反解运动:中距 = 均值,转角 = 差 ÷ 轮距;弧拆成「半转-弦-半转」三步⁹。
5. 粒子滤波四拍:预测(带噪声)→ 观测打分 → (ESS 允许时)重采样 → 再预测¹¹。
6. 权重永远留非零活口(0.1 而非 0)——多模型相乘时 0 是死刑,小数是保释¹²。
7. 低方差重采样:权重铺成数轴、等间隔落刀;比 rng.choice 快且稳¹⁴。
8. ESS = 1/Σw²:量「集中度」不量「对错」;设阈值门控重采样,ESS 曲线呈锯齿¹⁶¹⁷。
9. 调试先缩种群到 2 个、验直行与转弯,先修系统偏差再谈噪声¹⁸。

补遗:调试工具与显示样本的工程细节

最小种群调试法(full detail)。 「Localisation debugging tools」一节的处方比正文 §10 引用的更狠:终极手段是把种群缩到 1 个 pose,开着车在场里转, 肉眼核对「机器人动了多少、点动了多少」——系统误差(轮距、轮径、左右不对称) 在这个阶段必须全部修掉,否则后面两万个粒子只是在精确地传播一个错误的运动模型¹。 作者还提醒:左右轮径可能不同,要分别引入 left_wheel_radius 和 right_wheel_radius 补偿;调干净后再把种群放回 200。

观测模型代码的骨架。 边界模型看着数学,实现只有两层: in_boundary() 用 NumPy 的逻辑与/或,先判「在四条墙之内」,再判「不在右下缺口」, 返回一串 True/False;calculate_weights() 用 np.where 把 True 映射成 1.0、 False 映射成 0.1——打印出来就是 [0.1 1.0 1.0 0.1 1.0 ...] 这样的权重数组²。 全场的 pose 一次算完,这是粒子滤波能在 Pi 上实时跑的根基。

显示样本为什么在重采样前抽取。 正文提过一句,这里补全理由: 重采样会复制高权重 pose、淘汰低权重 pose——采样后的分布代表「新的信念」, 而你想在屏幕上看到的是「旧信念经本次观测修正后的样子」。 所以网页拿到的 200 个显示样本,总是在替换种群之前抽好; 种群的更新换代是系统自己的私事,不必给人看³。

14. 原文地图

主题	原书位置
pose 定义与 ENU	text/105-fm-technical-requirements.txt:17(搜「a set of numbers that represent」) · text/105-fm-technical-requirements.txt:24(搜「theta, or」)
旋转与平移公式	text/105-fm-technical-requirements.txt:384(搜「XT = xA」)
场地建模(顶点环)	text/106-fm-modeling-a-test-area.txt:15(搜「walls or boundaries」)
编码器反解运动	text/107-fm-moving-poses-with-encoders.txt:150(搜「mid_distance」) · text/107-fm-moving-poses-with-encoders.txt:167(搜「chord」)
轮距接地点	text/107-fm-moving-poses-with-encoders.txt:158(搜「wheel_distance」)
观测模型与蒙特卡洛循环	text/109-fm-refining-pose-probabilities.txt:25(搜「heart of Monte Carlo」) · text/109-fm-refining-pose-probabilities.txt:42(搜「low_probability」)
低方差重采样	text/109-fm-refining-pose-probabilities.txt:120(搜「low-variance resampling」) · text/109-fm-refining-pose-probabilities.txt:181(搜「slower and less stable」)
ESS	text/110-fm-controlling-the-rate-of-resampling.txt:39(搜「Effective Sample Size」) · text/110-fm-controlling-the-rate-of-resampling.txt:89(搜「not whether the filter is correct」)
ESS 锯齿实测	text/111-fm-summary.txt:9(搜「sawtooth」)
章节总结	text/111-fm-summary.txt:16(搜「dynamic probabilistic」)

---

---

1. 出处:「Localisation debugging tools」(text/108-fm-localisation-debugging-tools.txt:111,搜「population size to 1」)。原文:极限做法——种群设 1,开车转,肉眼核对运动;左右轮径不同时分别补偿;系统偏差修完再恢复 200。 ↩

2. 出处:「Refining pose probabilities」第 55 段(text/109-fm-refining-pose-probabilities.txt:55,搜「logical_and」))。原文:in_boundary 用 np.logical_and 判墙内、判缺口外;calculate_weights 用 np.where(场内 1.0,场外 low_probability)。 ↩

3. 出处:「Refining pose probabilities」第 208 段(text/109-fm-refining-pose-probabilities.txt:208,搜「publish_sample」)。原文:publish_sample 在替换 poses 之前抽取,代表重采样前的权重;种群再重采样回满额。 ↩

4. 出处:「Summary」(text/111-fm-summary.txt:16,搜「dynamic probabilistic」)。原文:用编码器数据 + pose 模型 + 场地模型,搭出机器人位置的动态概率数学模型;机器人从对传感器反应,升级为做出动态预测、检验自己在哪的假设。 ↩↩

5. 出处:「Technical requirements」第 17 段(text/105-fm-technical-requirements.txt:17,搜「a set of numbers that represent」)与第 40 段(搜「theta, or」)。原文:pose 是一组代表位置与朝向、可被代码操纵/检查/可视化的数;二维平面用笛卡尔 x、y + θ(theta/yaw/heading);机器人正前方 = 自己的 +x;ENU(East/North/Up)约定,但实操简化成轴对齐场地墙;毫米、弧度。 ↩↩↩↩

6. 出处:「Technical requirements」第 384 段(text/105-fm-technical-requirements.txt:384,搜「XT = xA」)。原文:旋转 = θ 相加(全部 pose 原地转);平移沿 theta 方向:XT = xA + cos(θ)×L,YT = yA + sin(θ)×L;每个 pose 不论多少都整体算;测试:两 pose 转 90°、各平移 100 mm,沿各自朝向散开。 ↩↩↩↩

7. 出处:「Modeling a test area」第 40 段(text/106-fm-modeling-a-test-area.txt:40,搜「walls = [」)。原文:墙用顶点环表示(left/bottom/right/top + cutout 缺口);Tip:空间不够可用小尺寸或相似 L 形,但「非直角会让事情复杂得多,没有缺口特征则所有边对机器人看起来一样」;量好各角毫米数填进 walls。 ↩↩↩

8. 出处:「Modeling a test area」第 72 段(text/106-fm-modeling-a-test-area.txt:72,搜「mapPlotter」)。原文:Chart.js 插件 beforeDraw 里用 canvas path 画墙;getPixelForValue 把模型坐标换算成像素;坐标轴建议范围 0–1500。 ↩↩

9. 出处:「Moving poses with encoders」第 150 段(text/107-fm-moving-poses-with-encoders.txt:150,搜「mid_distance」)与第 180 段(搜「chord」)。原文:mid_distance = 均值;theta = (右 − 左)/轮距,逆时针为正;三步:rotate(θ/2)、translate(mid)、rotate(θ/2);小弧近似,把弧当弦;轮距取接地点距离(作者 136 mm,小于第 11 章的 159)。 ↩↩↩↩↩

10. 出处:「Refining pose probabilities」第 199 段(text/109-fm-refining-pose-probabilities.txt:199,搜「randomise_motion」)。原文:on_encoders_data 里先 randomise_motion 给平移与旋转注入随机扰动,再 move;随后算权重、显示样本重采样前抽取、种群重采样回满额。 ↩

11. 出处:「Refining pose probabilities」第 25 段(text/109-fm-refining-pose-probabilities.txt:25,搜「heart of Monte Carlo」)。原文:观测模型 = 拿预测与观测(如传感器)比,给出 pose 的可能性;「做出大量猜测、更新、观测、再做新猜测——这是蒙特卡洛模拟的核心」;本节先做边界模型,权重会随后续传感器数据变化。 ↩↩↩

12. 出处:「Refining pose probabilities」第 42 段(text/109-fm-refining-pose-probabilities.txt:42,搜「low_probability」)。原文:low_probability = 0.1——很小但非零;多模型按相乘组合,0 会丢弃太多预测且其他模型救不回来;np.where(场内, 1.0, 0.1);显示可先用 weights > 0.5 过滤。 ↩↩↩↩

13. 出处:「Controlling the rate of resampling」第 5 段(text/110-fm-controlling-the-rate-of-resampling.txt:5,搜「crosshair」)。原文:升级版边界模型对每个 pose 取 5 个十字采样点的权重再平均;效果:pose 不再紧贴边线,系统偏好「离边稍远」的猜测。 ↩

14. 出处:「Refining pose probabilities」第 120 段(text/109-fm-refining-pose-probabilities.txt:120,搜「low-variance resampling」)与第 210 段(搜「slower and less stable」)。原文:权重铺成数轴,总长 ÷ 样本数 = 间隔;随机起点 + 等距采样;searchsorted 查索引;Note:rng.choice 更慢且不如低方差法稳定。 ↩↩↩

15. 出处:「Refining pose probabilities」第 235 段(text/109-fm-refining-pose-probabilities.txt:235,搜「resample」两次)。原文:publish_sample 在替换 poses 前抽取(代表重采样前的权重);种群建议 2,000 调通、20,000 提精度。 ↩↩↩

16. 出处:「Controlling the rate of resampling」第 39 段(text/110-fm-controlling-the-rate-of-resampling.txt:39,搜「Effective Sample Size」)与第 120 段(搜「not whether the filter is correct」)。原文:ESS = 1/Σ(归一化权重²);均匀时 ≈ 种群数(散沙,别重采样),分化时骤降(有结构,该重采样);Note:ESS 量集中度、不量对错。 ↩↩↩↩↩↩

17. 出处:「Summary」第 9 段(text/111-fm-summary.txt:9,搜「sawtooth」)。原文:实测 ESS 曲线:移动时下降、重采样后弹回,呈锯齿;阈值初始取种群数的十分之一(// 整除)。 ↩↩

18. 出处:「Localisation debugging tools」(text/108-fm-localisation-debugging-tools.txt:106,搜「population size can make it far easier」)。原文:调试时把种群缩到 2,直行与原地转各验一遍;轮径不同要分左右轮径补偿;系统偏差修完再恢复 200。 ↩↩