传感器融合 — 概率场、距离观测模型与互补滤波

这一章讲什么: 让第 12 章的定位从「 minutes 级收敛」进化到「20 秒收敛」。 三件事: ① 把「墙在哪」从硬边界变成概率场——高斯模糊抹出的缓坡,让猜测能「顺着坡爬向更可能的位置」; ② 给距离传感器建几何模型(8 路光束按 pose 的位姿投射),查概率场给每个 pose 打分,与边界模型相乘融合; ③ 同样的融合逻辑用到运动模型:编码器的里程与 IMU 的 yaw 各管一段,按 7:3 互补。 读完你会明白「融合」不是玄学,是「多个独立证据源按权重相乘/相加」的工程操作。

1. 先看现象:定位从「漫长」到「一进场就知道」

第 12 章的痛点在本章开头被精准复盘: 边界模型是二值的(非内即外),机器人若在场子中央,所有 pose 权重一样, 系统毫无分辨力;收敛全靠「哪天开到墙边」——书里实测要几分钟⁷。

本章完工后的对照实验:同一套粒子滤波, 加上距离观测模型后,「我的测试里 20 秒就能形成一个主导位置簇, 而此前边界-only 系统要几分钟」⁸。而且在场地任何一个角落启动都有效—— 因为距离传感器看的是「离墙多远」,中场一样有读数。

这就是融合的回报公式:独立证据源越多,每个猜测被区分得越细,收敛越快。

2. 顶层全景:三件套

 ① 概率场    场地墙(折线)→ 画成 1mm/像素 的图 → 高斯模糊 → 每格一个「像墙」的概率
 ② 几何投射  每个 pose:8 路光束 = 单位向量 × 读数,按 (x, y, θ) 旋转平移 → 命中点
 ③ 双重融合  观测层:边界权重 ^0.5 × 距离权重 ^0.5(影响系数调节)
             运动层:theta = 0.3×编码器 + 0.7×IMU(互补滤波)

 图说:① 是地图,② 是「假如我在 pose P,传感器该看到什么」,③ 是「两边的话都听,按信任度加权」。

3. 核心原理之一:概率场——把墙从刀锋变成缓坡

概率场(probability field,也叫似然场 likelihood field): 空间里每个位置一个数,表示「某事件在此发生的可能性」—— 这里是「传感器在此测到墙的概率」⁹。

概率随位置怎么变?书里给了四种形态,是一条认知链¹⁰:

形态	样子	用在哪
阶跃(step)	非黑即白	第 12 章的边界模型(内 1.0 / 外 0.1)
渐变	无单一阈值	边界模型的 crosshair 升级版
双阶跃(边界对)	一上一下	「测距值正好对上墙」的严格判定——不容噪
高斯脊(Gaussian ridge)	峰在墙上、两侧成坡	本章选择:噪声也是高斯的,给猜测留「爬坡」的余地

标准差(SD) 是这条脊的胖瘦旋钮:大多数正确猜测落在 ±1 SD 之间, SD 不定边界(超出只是概率更低)——它是「分散程度」而不是「围墙」¹⁰。 为什么选高斯:假设传感器的误差本身呈高斯分布(中间高、两边低的钟形曲线)(测量值大概率挤在真值附近, 越偏越少)——模型和噪声同形,匹配才是统计上诚实的¹⁰。

实现意外地简单:在 Jupyter 里用 OpenCV 把墙的折线画成图,再一行高斯模糊¹¹:

walls_map = np.zeros((map_frame.height, map_frame.width), dtype=np.float32)
cv2.polylines(walls_map, map_frame.world_to_map(np.array([arena.walls])),
              isClosed=True, color=1.0, thickness=1)     # 画墙:概率 1 的细线
distance_map = cv2.GaussianBlur(walls_map, (0, 0), 45)    # 抹成脊:SD = 45 mm

45 这个数有出处:VL53L5CX 数据手册的连续模式精度, 简化版是「15 mm 与距离的 8% 取大」,最坏约 120 mm——45 落在两者之间; 书还提醒 SD 必须为奇数、不超过图边距的一半,否则坡会互相淹没¹¹。

两个坐标系的区分是本节最容易踩的坑: 世界坐标(pose 的世界,0–1500)与地图坐标(概率场的图,带 100 mm 边距)。 边距不是装饰——高斯坡要伸出墙外,坡外的读数可能是带噪的真实测量, 必须给它们留格存概率;MapFrame 一对函数做互转(world_to_map 加边距, map_to_world 减),NumPy 数组整体加减(广播),全场 pose 一次转完¹²。

4. 核心原理之二:传感器几何——「假如我在 P,该看到什么」

观测模型回答的问题:假如机器人真的在 pose P、头朝 θ,8 路光束会打在哪?

每路光束 = 传感器位置 + 方向单位向量 × 该路距离读数。两个常数:

• 传感器相对机器人原点的位置(书里前移 50 mm——你的支架若不同,这个数必须改, 排错第一条就是它)¹³;
• 8 路光束的朝向角——按传感器阵列的排布定(第 148 节的几何建模)。

然后是对第 12 章变换公式的复用:每个 pose 的 (x, y, θ) 把 8 个局部命中点 旋转 + 平移到世界系。NumPy 的广播再一次吃下全场 pose × 8 束 = 16 万点。

Jupyter 里的验证画面很有说服力:场地上撒若干 pose,每个 pose 画出 1 条前伸线 + 8 条光束,大多数光束戳在空处,少数戳在墙上——戳中率就是分数的雏形¹⁴。

5. 核心原理之三:查表打分与模型融合——均值 inside,相乘 outside

打分两步:

endpoints = self.sensor_endpoints(poses)        # (n_pose, 8, 2) 命中点
flat_probs = self.get_probabilities(flat)        # 逐点查概率场
probs = flat_probs.reshape(len(poses), 8)
return np.mean(probs, axis=1)                    # 8 路平均 → 每 pose 一个权重

8 路为什么取平均而不是相乘? 书里用 Tip 框单独解释(这段是本章最值得背的话):

同一模型内部,各读数强相关,取均值——一次失手不至于把权重压扁; 跨模型组合时,要的是「每份独立证据都成支持或反对」—— 比如边界模型抓到出界,就得在权重里反映出来,不能被平均稀释。¹⁵

融合进主循环:

boundary_weights = self.boundary_model.calculate_weights(self.poses) ** 0.5
distance_weights = self.distance_model.calculate_weights(self.poses) ** 0.5
return boundary_weights * distance_weights

** 影响系数 是个小机关:指数 < 1 软化该模型的发言权(降噪),

1 放大(信它)——两个 0.5 = 平权¹⁵。 相乘 = 贝叶斯意义上的「联合概率」:两个独立证据都对,权重才高; 一个否决,权重就下来。这正是第 12 章那句「低概率别给 0」的用途兑现。

效果对比书里给得具体:边界模型分不清 6 号和 14 号 pose(都在界内); 距离模型下,6 号的光束戳在墙上、14 号的戳在空处,一眼分高低—— 反过来距离模型不分场内场外,所以两者必须搭着用¹⁶。

6. 核心原理之四:运动模型融合——互补滤波

观测融合完了,运动模型也有两个可融合的来源:

• 编码器:短程准、被打滑骗、轮径/轮距的微小误差被放大;
• IMU yaw:不怕打滑、独立测量,但要校准、怕磁扰、陀螺漂移¹⁷。

代码重构先走一步:把编码器相关逻辑抽成 EncoderMotionData 类, 收数据与用数据解耦——传感器按自己的节奏来(MQTT 回调里只往 pending_distance/pending_theta 里累加),主循环按自己的节奏取 (consume_data() 取走并清零)¹⁸。这一步顺带治好一个老毛病: 编码器数据来得比处理快时,不再排队爆炸,而是攒成增量一次消费。

融合本身朴素得漂亮——互补滤波(complementary filter):

theta = (1 - imu_rotation_influence) * encoder_theta + imu_rotation_influence * imu_theta
# imu_rotation_influence = 0.7 —— IMU 管七成,编码器管三成

「互补」的含义:两个源的噪声和误差源不重叠(编码器怕打滑,IMU 怕磁扰和漂移), 按信任度加权相加,各自的短板被对方的优势覆盖;权重可 trust 度调¹⁹。 IMU 侧同样做了「收用分离」:on_imu_data 记上次 yaw、这次取差值(经 ±π 归一) 累进 pending_theta;主循环到点才消费²⁰。

两个工程细节收尾:主循环改用手动 client.loop(0.1) + self.update(client) (计算移出 MQTT 回调线程,避免和传感器回调抢数据); 整个循环包 try/finally——行为退出时保证停掉它拉起的 imu_service¹⁸²⁰。

7. 作者的判断与证据

• 判断: 模型融合的权重(0.5/0.5)和运动融合的权重(0.7/0.3)都是信任声明, 不是最优解;书里明说「可按对每个源的信任程度调」¹⁵¹⁹。
• 判断: 「加模型有代价」开篇即说:处理时间涨、复杂度涨、调试要隔离单模型—— 融合不是免费的午餐,但值得²¹。
• 事实层: SD 45 的数据手册依据、20 秒 vs 几分钟的对照、 传感器前移 50 mm 的几何要求、轮径轮距要精确到接地点——可复核⁸¹¹¹³。
• 排错哲学: 五条排错全部指向「先怀疑几何与标定,再怀疑算法」—— 概率模型不会原谅错的长度¹³。

8. 边界与局限

• 没有正式的 SLAM。 地图是手工建模的(arena.py 里的顶点), 机器人不建图——概率场是「已知地图上的定位」,不是同步建图与定位。
• 互补滤波是手作的。 标准做法是卡尔曼滤波家族(扩展卡尔曼滤波 EKF); 加权相加是它的极简版,书里明说这就是「互补滤波」,没有假装更高级¹⁹。
• 计算量随模型数线性涨。 两个观测模型已让 Pi 吃力; 加更多模型(如视觉路标)需要书中未提的优化(降频、多线程、减粒子)。
• 对场地假设严格。 墙必须横平竖直、尺寸到毫米、无动态障碍; 一块挪动的椅子就是全场猜测的噪声源。

9. 可带走的

1. 概率场 = 每格一个概率的图;高斯脊让「像墙」从刀锋变缓坡,猜测可爬坡⁹¹⁰。
2. SD 跟着传感器噪声定(数据手册),模型与噪声同形才是诚实建模¹¹。
3. 地图坐标要留边距——坡外的世界也是世界¹²。
4. 观测模型 = 「假如我在 P,该看到什么」+ 查场¹⁴。
5. 模型内取均值(相关),模型间取相乘(独立)——这是最常被写反的一条¹⁵。
6. 影响系数(** 0.5)是模型的音量旋钮:小软化、大放大¹⁵。
7. 多证据融合的回报:收敛时间从分钟级到秒级,且不再依赖「开到墙边」⁸。
8. 收用分离(pending/consume)让多速传感器共存;计算别住在回调线程里¹⁸。
9. 互补滤波:噪声不重叠的两源按信任度加权;编码器 + IMU = 0.3 + 0.7 是起点¹⁹。

补遗:单位向量、几何建模与融合上机的细节

单位向量——几何的预制件。 8 路光束的本质是 8 个方向。 书里的做法是把「方向」预先做成单位向量:在半径 1 的单位圆上, 角度 θ 对应的点坐标就是 (cos θ, sin θ)——长度永远是 1, 想射多远就乘多远:乘 50 就是「沿 θ 方向 50 毫米处的坐标」¹。 8 个角度不变,8 个单位向量预先算好存起来,运行时只做一次乘法—— 这是「查表换速度」的老手法。

几何常数的三个来源。 ① 视场 45°(π/4)——取自 VL53L5CX 数据手册, 8 路光束均匀铺满:np.linspace(-fov/2, +fov/2, 8) 一次生成; ② 传感器前移 50 mm——传感器不在机器人中心,挂在车头, 每个命中点都要先加这个偏移;③ 每路读数取该列上方的最小值—— 和避障章一样砍掉脚下几行²。三个数任何一个失真, 概率场打分就会系统性偏移,排错清单第一条永远是「核几何」。

概率场先单独验证再上机。 Jupyter 里四格自检:加载 distance_map.npy → 拿四个坐标查概率——(0,0) 场角、(1705,1500) 图外(保证是 0.1)、 (1500,1200) 墙上(≈0.84)、(400,400) 场中(0.1)—— 输出 array([0.84, 0.1, 0.84, 0.1]) 说明查表正确³。

融合上机的第一个画面。 部署后只按 Start(不动车),距离传感器一开, 满场 pose 立刻收敛到「与眼前距离读数对得上」的那些位置—— 中场不再茫然,这就是第二个观测模型的即时回报⁴。

归一化:高斯模糊之后必须再抬一次底。 模糊把墙的概率摊到了整条坡上, 峰值只剩 0.032、谷底是 0.0——直接拿去打分,所有 pose 的分数都挤在 零附近,「戳中墙」和「戳在空处」的差别被压扁。所以先 cv2.normalize(distance_map, None, alpha=0.1, beta=1.0, NORM_MINMAX) 把谷底抬到 low_probability(0.1)、峰顶放大到 1.0, 再 np.save 存成 distance_map.npy 上机。查表侧配一个越界护栏: 命中点落在图外的,用一张「在图内为真」的布尔掩码统一填 low_probability, 而不是让数组下标越界崩掉⁵。

低通滤波:想过,然后决定不加。 收用分离之后自然会问: pending_theta 攒的是原始 yaw 差值,要不要先过低通滤波(只让变化慢的 成分通过、把突刺滤掉)?书给了三条不加之理:BNO055 板载融合算法本来就 对噪声大的传感器做了低通;差值在主循环的较低频率上才被消费, 高频噪声根本采不到;接下来还要和编码器融合,融合本身就是降噪⁶。

10. 原文地图

主题	原书位置
为什么二值边界不够	text/147-fm-probability-fields.txt:1(搜「binary」)
归一化与存盘	text/147-fm-probability-fields.txt:308(搜「0.032」) · text/147-fm-probability-fields.txt:320(搜「cv2.normalize」)
不加低通的理由	text/150-fm-fusing-imu-yaw-data.txt:234(搜「low-pass filter」)
概率场定义与四种变化	text/147-fm-probability-fields.txt:26(搜「probability field or likelihood field」) · text/147-fm-probability-fields.txt:45(搜「Step change」)
高斯脊与 SD	text/147-fm-probability-fields.txt:85(搜「Gaussian ridge」) · text/147-fm-probability-fields.txt:60(搜「Standard Deviation, or SD」)
MapFrame 与边距	text/147-fm-probability-fields.txt:120(搜「MapFrame」)
高斯模糊与 45 的出处	text/147-fm-probability-fields.txt:249(搜「GaussianBlur」) · text/147-fm-probability-fields.txt:260(搜「15 mm and 8%」)
光束投射验证图	text/149-fm-fusing-distance-sensor-data.txt:8(搜「radiating from them」)
查表打分与均值/相乘	text/149-fm-fusing-distance-sensor-data.txt:41(搜「Calling np.mean calculates」) · text/149-fm-fusing-distance-sensor-data.txt:174(搜「Why use multiplication instead of mean」)
影响系数融合	text/149-fm-fusing-distance-sensor-data.txt:117(搜「boundary_model_influence」)
20 秒对照	text/150-fm-fusing-imu-yaw-data.txt:15(搜「dominant location group in 20 seconds」)
收用分离重构	text/150-fm-fusing-imu-yaw-data.txt:54(搜「pending_theta」)
互补滤波	text/150-fm-fusing-imu-yaw-data.txt:285(搜「complementary filter」) · text/150-fm-fusing-imu-yaw-data.txt:253(搜「imu_rotation_influence = 0.7」)
章节总结	text/151-fm-summary.txt:33(搜「climb to better weighted options」)

---

---

1. 出处:「Modeling distance sensor array geometry」第 53 段(text/148-fm-modeling-distance-sensor-array-geometry.txt:53,搜「store pre-calculated」)。原文:单位向量 = 距离 1 的方向表示;乘长度即得该方向任意距离的坐标;预存省时间。 ↩

2. 出处:「Modeling distance sensor array geometry」第 74 段(text/148-fm-modeling-distance-sensor-array-geometry.txt:74,搜「spread of 45 degrees」)。原文:FoV 45°(数据手册),8 束均分;sensor_forward_offset = 50 mm;读数砍脚下行后取每列最小。 ↩

3. 出处:「Modeling distance sensor array geometry」第 17 段(text/148-fm-modeling-distance-sensor-array-geometry.txt:17,搜「guaranteed to be outside」)。原文:四点查表自检——场角、图外(必 0.1)、墙上(≈0.84)、场中(0.1)。 ↩

4. 出处:「Fusing distance sensor data」第 187 段(text/149-fm-fusing-distance-sensor-data.txt:187,搜「resolve to those matching distances」)。原文:只按 Start,pose 场立刻收敛到与前方距离匹配的猜测。 ↩

5. 出处:「Generating a distance probability field」第 308 段(text/147-fm-probability-fields.txt:308,搜「0.032」)与第 320 段(搜「cv2.normalize」)、第 334 段(搜「np.save」)、第 390 段(搜「np.full」)。原文:模糊后峰值仅 0.032、谷底 0.0,太低不可用;normalize 到 [0.1, 1.0] 再存 distance_map.npy;查表用布尔掩码,图外坐标填 low_probability,避免越界。 ↩

6. 出处:「Creating the IMU motion data system」第 234 段(text/150-fm-fusing-imu-yaw-data.txt:234,搜「low-pass filter」)。原文:可考虑加低通只接收更新的一部分,但不必——BNO055 板载融合已带低通;delta 在更新循环的较低频率被读取;且随后与另一数据源融合,也降低噪声影响。 ↩

7. 出处:「Probability fields」第 1 段(text/147-fm-probability-fields.txt:1,搜「binary」)。原文:边界模型是二值的(内/外),建不了「测距读数与真实墙对得多齐」的模;但加观测模型有代价——处理时间与复杂度齐涨,调试要隔离单模型。 ↩

8. 出处:「Fusing IMU yaw data」(text/150-fm-fusing-imu-yaw-data.txt:15,搜「dominant location group in 20 seconds」)。原文:实测加入距离观测模型后 20 秒形成主导位置簇,此前边界-only 要几分钟;缺口的形状消掉了大量坏猜测。 ↩↩↩

9. 出处:「Probability fields」第 26 段(text/147-fm-probability-fields.txt:26,搜「probability field or likelihood field」)。原文:概率场/似然场 = 空间里每个位置的某事件概率/权重——可能是「机器人在此」的似然,也可能是「传感器在此测到墙」的概率。 ↩↩

10. 出处:「Probability fields」第 45 段(text/147-fm-probability-fields.txt:45,搜「Step change」)与第 80 段(搜「Gaussian ridge」)。原文:四种变化——阶跃(第 12 章边界)、渐变、双阶跃(不容噪)、高斯脊(给真实噪声留坡,可「爬到更可能的位置」);SD 是分散度,不定边界;假设传感器误差亦呈高斯。 ↩↩↩↩

11. 出处:「Generating a distance probability field」(text/147-fm-probability-fields.txt:249,搜「GaussianBlur」)与第 260 段(搜「15 mm and 8%」)。原文:polylines 画墙(1 mm/像素),GaussianBlur(0,0,45) 抹成脊;SD 必须奇数、≤边距一半;45 介于数据手册精度(15 mm 与 8% 取大、最坏 120 mm)之间;Jupyter 里边做边看。 ↩↩↩↩

12. 出处:「Map coordinates」第 120 段(text/147-fm-probability-fields.txt:120,搜「MapFrame」)。原文:世界坐标 vs 地图坐标两套;边距让墙的坡有地方伸展;坡外的命中可能是带噪真值,该留概率;world_to_map 加边距、map_to_world 减;广播一次转完全场;w_left/right/top/bottom 带前缀防混。 ↩↩

13. 出处:「Fusing IMU yaw data」排错(text/150-fm-fusing-imu-yaw-data.txt:5,搜「offset」)。原文排错:先 journalctl;距离传感器偏移(作者 50 mm)必须反映真实几何;轮径按接地点、轮距、编码器配置都要准;场地建模到毫米;用距离画图页看传感器实际看到什么。 ↩↩↩

14. 出处:「Fusing distance sensor data」第 8 段(text/149-fm-fusing-distance-sensor-data.txt:8,搜「radiating from them」)。原文:Jupyter 验证图——多个 pose 各画前伸线与 8 条光束,有的触墙多数落空;数组下标层层嵌套,括号要格外小心。 ↩↩

15. 出处:「Fusing distance sensor data」第 41 段(text/149-fm-fusing-distance-sensor-data.txt:41,搜「Calling np.mean calculates」)与第 150 段(搜「Why use multiplication instead of mean」)。原文:8 路取均值——失手不压扁权重、也抑噪;跨模型相乘——独立证据各表支持/反对,出界这类强反对必须显现;** 影响系数调发言权,可降噪或偏信。 ↩↩↩↩↩

16. 出处:「Fusing distance sensor data」第 83 段(text/149-fm-fusing-distance-sensor-data.txt:83,搜「pose 6」)。原文:边界模型不分 6/14 号(皆界内),距离模型下 6 号光束触墙而 14 号落空;距离模型不分场内场外——故须组合。 ↩

17. 出处:「Fusing IMU yaw data」第 30 段(text/150-fm-fusing-imu-yaw-data.txt:30,搜「over- or understeer」)。原文:编码器运动模型会过/欠转向、受电机轮差与打滑影响;轮距轮径的微小不准被放大;IMU 的 yaw 独立但需校准、怕磁(含电机)、陀螺漂移;两源互补短板。 ↩

18. 出处:「Preparation for fusing the motion data」(text/150-fm-fusing-imu-yaw-data.txt:54,搜「pending_theta」)。原文:EncoderMotionData 收用分离——回调累加 pending_*、主循环 consume 清零;主循环改 client.loop(0.1) + update,计算移出 MQTT 线程;数据来得快时攒成增量不排队。 ↩↩↩

19. 出处:「Fusing IMU yaw data」(text/150-fm-fusing-imu-yaw-data.txt:285,搜「complementary filter」)与第 240 段(搜「imu_rotation_influence = 0.7」)。原文:theta = (1−影响)×编码器 + 影响×IMU;0.7 = IMU 管七成;名为互补滤波——假设两源噪声与误差互补;可按信任度调。 ↩↩↩↩

20. 出处:「Fusing IMU yaw data」(text/150-fm-fusing-imu-yaw-data.txt:204,搜「normalize_angle」)。原文:IMU 侧记 previous_yaw、取差值经 ±π 归一后累进 pending_theta;sys 到 3 才开始消费;try/finally 保证退出时停 imu_service。 ↩↩

21. 出处:「Probability fields」第 7 段(text/147-fm-probability-fields.txt:7,搜「trade-offs」)。原文:加模型的两个代价——处理时间(可优化/多线程)与复杂度(调试需隔离单模型,但组合才有最好结果)。 ↩