机器人 / 系统课时

观测模型如何评价位姿假设

观测模型如何评价位姿假设

机器人在地图上可能存在多个候选位姿,但不是每个位置都能解释当前传感器读数。观测模型通过预测传感器在候选位姿下应该看到什么,再比较真实观测与预测值,计算各位姿的观测似然。本篇用二维激光雷达和一面墙建立最小模型,并用 Python 验证评分结果。

学习目标

  • 理解候选位姿、地图、预测观测和真实观测之间的关系。
  • 使用观测残差与高斯噪声模型计算位姿的观测似然。
  • 通过可运行示例比较位姿假设,并识别多束激光评分中的常见问题。

从地图和位姿预测传感器读数

二维激光 SLAM 中,机器人位姿通常表示为 x=(x,y,θ)\mathbf{x}=(x,y,\theta),其中 x,yx,y 是地图坐标系下的位置,θ\theta 是朝向。已知地图 mm 和候选位姿 x\mathbf{x} 后,可以通过射线投射预测激光雷达的距离读数 z^\hat z。

预测值和真实读数有明确区别:

  • 真实观测 zz:激光雷达实际返回的距离,保持不变。
  • 预测观测 z^\hat z:假设机器人处于某个候选位姿,射线从该位置出发与地图中的障碍物相交,计算得到的距离。
  • 观测残差 rr:真实观测减去预测观测,即 r=z−z^r=z-\hat z。

考虑一张只有一面垂直墙壁的简化地图,墙位于 x=5.0x=5.0 m。激光雷达与机器人位姿原点重合,机器人面向 +x+x 方向,只使用正前方的一束激光。真实观测距离为 z=3.9z=3.9 m。

现在比较两个候选位姿:

候选位姿 位置与朝向 地图预测距离 观测残差
A (1.0,0,0°)(1.0,0,0°) 4.0 m -0.1 m
B (1.4,0,0°)(1.4,0,0°) 3.6 m +0.3 m

候选 A 的预测值更接近真实读数,因此它更符合这一次观测。但要判断两者可信度相差多少,还需要考虑激光测量误差的大小。

候选位姿对墙面距离的预测与观测残差对比

在真实 ROS 2 系统中,距离通常来自 sensor_msgs/msg/LaserScan 消息的 ranges 字段。每条射线的方向由 angle_min 和 angle_increment 确定;预测时还必须通过 TF 将雷达坐标系中的射线变换到地图坐标系,不能直接把机器人中心当作所有传感器的实际安装位置。

用高斯噪声计算观测似然

同样是 0.1 m 的残差,对精度约为厘米级的传感器可能值得怀疑,对噪声较大的传感器则可能完全正常。因此,残差不能单独当作位姿得分。

假设测量误差服从零均值高斯分布,标准差为 σ\sigma,单束激光的观测似然密度可以写成:

p(z∣x,m)=12πσexp⁡(−r22σ2)p(z\mid\mathbf{x},m)=\frac{1}{\sqrt{2\pi}\sigma}\exp\left(-\frac{r^2}{2\sigma^2}\right)

其中 r=z−z^(x,m)r=z-\hat z(\mathbf{x},m)。该公式表示:在给定地图和位姿假设的条件下,出现当前距离读数的概率密度有多大。

为演示计算,设 σ=0.10\sigma=0.10 m,得到:

指标 候选 A 候选 B
预测距离 4.0 m 3.6 m
残差 -0.1 m +0.3 m
标准化残差 r/σr/\sigma -1 +3
似然密度 2.420 0.044

两个候选的似然密度之比约为 54.6:154.6:1。在当前高斯模型下,观测对 A 的支持明显强于 B。

噪声参数直接影响评分的区分能力。如果把 σ\sigma 从 0.10 m 调大到 0.30 m,A 与 B 的似然比会从约 54.6 降到 1.56。噪声越大,相同残差差异通常越难区分。

实际系统应依据传感器标定、测距距离和现场条件确定噪声模型,而不是将演示中的 0.10 m 直接作为所有雷达的固定参数。

用 Python 比较两个候选位姿

下面使用 Python 3.9 或更新版本运行一个仅依赖标准库的示例。地图被简化为 x=5.0x=5.0 m 的墙面,因此预测距离就是墙面横坐标减去候选位姿的横坐标。这里固定 y=0y=0、θ=0\theta=0,不涉及旋转或复杂地图射线追踪。

python
import math

wall_x = 5.0
z = 3.9
sigma = 0.1
candidates = {"A": 1.0, "B": 1.4}
scores = {}

for name, x in candidates.items():
    # 仅适用于雷达朝+x且射线首先碰到x=5墙面的简化场景
    predicted = wall_x - x
    residual = z - predicted

    # 高斯观测似然密度,sigma表示测距误差标准差
    score = math.exp(-0.5 * (residual / sigma) ** 2) / (
        math.sqrt(2 * math.pi) * sigma
    )
    scores[name] = score
    print(f"{name}: zhat={predicted:.1f}m, r={residual:+.1f}m, pdf={score:.3f}")

print(f"A/B likelihood ratio={scores['A'] / scores['B']:.1f}")

预期输出:

text
A: zhat=4.0m, r=-0.1m, pdf=2.420
B: zhat=3.6m, r=+0.3m, pdf=0.044
A/B likelihood ratio=54.6

可以把 sigma 改成 0.3 再运行。预期结果是 A/B 似然比约为 1.6,而不是 54.6。这验证了测量噪声决定评分对残差的敏感程度。

程序只计算观测似然,没有加入运动模型或先验位姿概率。完整的贝叶斯定位还会结合先验或预测分布:

p(x∣z,m)∝p(z∣x,m)p(x∣m)p(\mathbf{x}\mid z,m)\propto p(z\mid\mathbf{x},m)p(\mathbf{x}\mid m)

也就是说,某个位姿既要能够解释当前观测,也要结合已有定位信息判断其是否合理。

多束激光和异常观测如何处理

真实激光雷达通常一次返回数百甚至数千束距离。给定候选位姿和地图,需要分别预测各束距离,并将它们的观测似然合成为候选位姿的评分。

如果暂时假设各束测量误差在给定位姿和地图后条件独立,就可以将各束似然相乘。由于连续相乘容易产生浮点下溢,实际实现通常累加对数似然:

log⁡L(x)=∑i=1Nlog⁡p(zi∣x,m)\log L(\mathbf{x})=\sum_{i=1}^{N}\log p(z_i\mid\mathbf{x},m)

其中 NN 是参与评分的有效射线数量。该条件独立假设只是计算近似,相邻激光束受同一障碍物和环境因素影响时可能存在相关性。

例如,一帧扫描有 360 束,可以先均匀抽取 36 束进行初步评分,再根据运算预算提高采样密度。这样能减少射线投射开销,但需要检查采样是否遗漏狭窄障碍物或重要几何特征。

处理 ROS 2 LaserScan 数据时,建议依次检查:

  1. 确认 header.frame_id 对应的雷达坐标系能够通过有效时间戳的 TF 变换连接到地图坐标系。
  2. 读取 range_min、range_max,按传感器消息约定识别无效读数;对 NaN、无穷值和超出有效范围的数值分别处理。
  3. 对有效测距射线执行地图射线投射,取得预测距离;无回波、最大量程和地图未知区域应按所选模型单独处理。
  4. 计算每束残差及对数似然,再合成候选位姿分数;比较候选时保持射线选择规则一致。

还必须处理地图与现实不一致的情况。例如地图中的走廊为空,但行人刚好挡住激光,真实测距会比地图预测距离短。此时直接使用单一且很窄的高斯分布,可能把正确位姿错误地判为低可信度。

一种简单的改进是建立混合观测模型:让 95% 的权重来自正常命中模型,其余 5% 来自表示异常测量的随机分布。更完整的激光束模型还可区分意外遮挡、最大量程返回和随机测量,相关权重需要基于真实数据验证。

观测模型评价的是候选位姿能在多大程度上解释传感器读数,并不直接给出唯一真实位置。只有将地图预测、合理的测量噪声、历史位姿信息和异常观测处理结合起来,才能得到可用于 SLAM 定位更新的可信评分。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

观测模型如何评价位姿假设

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通