机器人基础知识 · 第 5 篇
阅读准备：了解平均值与二维坐标。本文从一次距离测量出发，逐步解释偏差、协方差、误差传播和融合。全部数据由明确的教学模型生成，不能作为八界传感器精度或实机性能结论。

一个距离传感器连续测量静止目标，输出围绕 1.03 m 波动。取 200 次平均后，结果稳定在 1.0304 m。

现在问：这个结果准确吗？

只看“波动变小”无法回答。如果目标实际在 1.0000 m，平均结果仍偏了约 30 mm。软件输出越来越稳定，机器人却可能每次都伸到同一个错误的位置。

机器人不仅需要一个估计值，还需要知道这个值有多可信、哪些方向更不确定，以及不同测量是否共享同一种误差。协方差就是表达这些关系的一种工具，但它不会替我们发现没有写进模型的错误。

图 1：基于八界 URDF 与作者提供的外观参考制作的概念示意，非实机拍摄。模型底图约束头部、机身和机械臂结构；散点与椭圆是视觉隐喻，不是设备采样或标定结果。

1. 误差、噪声和不确定性，分别在说什么

先建立一个简单测量模型：

zᵢ = x_true + b + εᵢ

其中 zᵢ 是第 i 次读数，x_true 是被测量的真值，b 是在这组测量条件下近似固定的偏差，εᵢ 是随机起伏。以下假设 εᵢ 均值为零、标准差为 σ，且不同次测量相互独立。

在这个模型里，三件事需要分开：

• 误差是读数与真值之差。教学实验知道真值，可以直接算；真实系统通常无法逐次知道它。
• 噪声描述读数中的随机起伏。重复测量可以帮助估计它，但环境变化、温漂和运动会破坏“同一条件”的假设。
• 测量不确定性描述根据现有信息，对测量结果仍有多大疑问。它可以包含随机测量、标定参数、分辨率、修正量等多个来源，不能简单等同于重复测量的标准差。[1]

例如用标定结果减去一个估计偏差，并不意味着偏差相关的不确定性消失了。用于修正的偏差估计本身也不精确，需要继续计入结果。

因此，“误差很小”和“报告的不确定性很小”不是同一句话。模型遗漏了一个 30 mm 的安装偏差时，算法完全可能自信地报告一个错误的位置。

2. 平均 200 次，究竟改善了什么

设真值为 1 m，固定偏差为 +30 mm，随机噪声标准差为 10 mm。固定随机种子生成 200 次独立测量，得到：

样本均值             1.030359 m
样本标准差           0.010199 m
均值的标准误差估计   0.000721 m
均值相对真值的误差   0.030359 m

均值的标准误差只有约 0.72 mm，为什么实际误差还有 30.36 mm？因为它估计的是均值在重复实验中的随机起伏，固定偏差没有被减掉。

对独立、同分布、方差有限的噪声，有：

z̄ = (z₁ + ... + z_N) / N
E[z̄] = x_true + b
Var(z̄) = σ² / N
标准差(z̄) = σ / √N

样本方差通常使用 Σ(zᵢ−z̄)²/(N−1) 估计总体方差；NumPy 中对应 np.std(z, ddof=1)。但均值仍围绕 x_true+b，而不是 x_true 收敛。

图 2：上图是单次测量误差，下图是累计均值误差。阴影为模型给出的均值期望 ±1 个标准误差，仅描述随机部分；它不包含对固定偏差的修正。

这也解释了两个常见工程现象。

第一，静止场景下加大滑动平均窗口，画面会更稳，但坐标可能仍不准。应该检查标定、尺度或系统性偏差，而不能持续增加窗口。

第二，目标运动时，更大的窗口还会混入更早的状态。此时“所有读数测的是同一个 x_true”已经不成立；平均得到的稳定值可能只是一个落后的状态。时间造成的问题将在下一篇展开。

3. 一个点有两个方向，为什么不能只存一个精度

假设测得目标的二维位置 (x, y)。如果 x 偏大的时候，y 也倾向于偏大，散点会沿右上方向拉长。即使两个轴的标准差都为 20 mm，也不能用一个圆完整表达它。

令随机向量 p = [x, y]ᵀ，均值为 μ，协方差为：

Σ = E[(p−μ)(p−μ)ᵀ]

  = [ Var(x)     Cov(x,y) ]
    [ Cov(x,y)   Var(y)   ]

对角线记录各方向的方差；非对角线记录两个方向是否共同变化。位置用米时，上述所有元素的单位都是平方米。

考虑这组教学参数：

Σ = [ 0.00040   0.00024 ] m²
    [ 0.00024   0.00040 ]

σx = σy = 0.020 m
相关系数 = Cov(x,y)/(σxσy) = 0.6

正的协方差意味着两个分量常常同向偏离均值。负协方差也完全可能，例如 x 偏大时 y 倾向于偏小；不能因为看见负数就把非对角线清零。

合法协方差必须对称且半正定。后者意味着任何方向上的方差都不能为负。任意填写一个很大的非对角线值，可能破坏这个条件，让后续分解或滤波出现问题。

椭圆的方向与大小从哪里来

对协方差做特征分解：

Σ = Q Λ Qᵀ

Q 的列给出散布的主方向，特征值的平方根给出这些方向上的标准差。本例特征值为 0.00064 与 0.00016 m²，主方向标准差约为 25.30 mm 与 12.65 mm，长轴沿 45° 方向。

这不是“x 轴标准差忽然改变”，而是在旋转坐标轴后看同一组数据。相关性原本藏在非对角线里，换到主方向后就表现为不同的轴向尺度。

图 3：同一组二维高斯分布，橙色区域满足 d²≤1，理论概率约为 39.3%；绿色区域满足 d²≤5.991，理论概率为 95%。散点是有限次模拟，不要求恰好按理论比例落入区域。

4. 为什么二维的“1σ 椭圆”不是 68% 区域

一维正态分布里，均值 ±1 个标准差覆盖约 68.3% 的概率。把这个数字直接贴到二维椭圆上，是很常见的错误。

对正定协方差，定义马氏距离的平方：

d² = (p−μ)ᵀ Σ⁻¹ (p−μ)

它先按不确定性的方向和尺度归一化，再衡量偏离程度。因此，沿长轴偏离 20 mm，与沿短轴偏离 20 mm，不具有同样的统计含义。

当 p 服从均值、协方差已知的二维高斯分布时，d² 服从自由度为 2 的卡方分布。它的累积分布为 1−exp(−d²/2)，因此：[3]

P(d² ≤ 1) ≈ 39.35%
P(d² ≤ 5.991) = 95%

95% 椭圆的半轴是 √(5.991λ₁) 和 √(5.991λ₂)，本例约为 61.92 mm 与 30.96 mm。

这里描述的是已知高斯分布中随机位置的概率区域。如果均值和协方差也是从很少的样本估出来，或者读数含有遮挡、误匹配和多峰分布，就不能把这个椭圆直接解释为严格的有限样本置信区域。

同理，目标检测网络输出的 confidence=0.95，并不自动等于“位置落在这个椭圆内的概率为 95%”。分类置信度、位置误差分布和几何协方差，需要分别建立与验证模型。

5. 坐标变了，协方差也必须一起变

第二篇讲过，点从 B 系变到 A 系：

p_A = R · p_B + t

如果 R 和 t 是确定的，那么：

μ_A = R · μ_B + t
Σ_A = R · Σ_B · Rᵀ

确定的平移只移动均值，不改变散布；旋转则让整个误差椭圆一起旋转。

只旋转目标坐标、不旋转协方差，会出现奇怪的结果：目标转到了另一个方向，算法却仍把原来那个方向当成“不确定性最大”的方向。融合与规划使用的权重因此会错。

实际系统中的外参本身也可能不确定。这时不能只套上面这一行公式，而需要把外参误差作为输入变量一起传播；若外参与观测相关，还要保留相应的交叉协方差。

距离与角度误差，怎样变成位置误差

用二维极坐标做一个可以算清楚的例子。传感器给出距离 r 与角度 θ：

x = r cosθ
y = r sinθ

在当前估计附近，对它做一阶展开。令输入微小误差为 [δr, δθ]ᵀ，则：

[δx; δy] ≈ G · [δr; δθ]

G = [ cosθ   −r sinθ ]
    [ sinθ    r cosθ ]

Σ_xy ≈ G · Σ_rθ · Gᵀ

G 是这个坐标映射对输入的雅可比。公式背后的机制很直观：输入每偏一点，输出沿哪个方向变化、放大多少，由 G 决定。一般的协方差传播同样来自这种局部线性化。[2]

设 r=1 m、θ=30°，距离标准差为 20 mm，角度标准差为 2°，两者独立。计算时必须先把 2° 转成约 0.03491 rad。

import numpy as np

r, theta = 1.0, np.deg2rad(30)
sigma_r, sigma_theta = 0.02, np.deg2rad(2)
G = np.array([[np.cos(theta), -r*np.sin(theta)],
              [np.sin(theta),  r*np.cos(theta)]])
C_in = np.diag([sigma_r**2, sigma_theta**2])
C_xy = G @ C_in @ G.T
print(C_xy)

结果约为：

Σ_xy = [ 0.0006046   −0.0003544 ] m²
       [−0.0003544    0.0010139 ]

负的非对角线很合理：在这个方向上，角度偏大，会使 x 变小、y 变大。

图 4：距离标准差保持 20 mm，角度标准差保持 2°。径向标准差不变，切向标准差近似从 34.9 mm 增长到 69.8 mm。图中的椭圆均为一阶传播得到的 95% 区域。

这说明“角度只差一点”也可能造成明显的位置偏差：距离越远，横向偏移越大。随文代码还进行了 20 万次蒙特卡洛采样；本次样本协方差与一阶结果的相对 Frobenius 范数差约为 0.145%。这是对本组小噪声教学参数的数值核对，不意味着一阶传播在任何情况下都这么准确。

如果角度不确定性很大，映射后的分布可能更像弯曲的带状区域；如果结果有两个候选目标，一个椭圆甚至会把“两个都不在的位置”当作均值。这时应考虑采样传播或多假设表达，而不是盲目扩大一个高斯椭圆。

6. 两个传感器如何融合，为什么不能默认取平均

现在让两个来源测量同一时刻、同一坐标语义下的同一个标量。假设它们无偏、误差独立且服从高斯分布：

z₁ = 1.02 m，σ₁ = 0.02 m
z₂ = 0.98 m，σ₂ = 0.04 m

第二个来源的标准差是第一个的两倍，方差则是四倍。它们不应自动获得相同权重。

求加权残差平方和的最小值：

L(x) = (z₁−x)²/σ₁² + (z₂−x)²/σ₂²

对 x 求导并令其为零，得到：

wᵢ = (1/σᵢ²) / Σⱼ(1/σⱼ²)
x̂ = Σᵢ wᵢzᵢ
σ_fused² = 1 / Σᵢ(1/σᵢ²)

权重是 0.8 与 0.2，融合结果为 1.012 m，标准差约 0.01789 m。

import numpy as np

z = np.array([1.02, 0.98])
sigma = np.array([0.02, 0.04])
precision = 1 / sigma**2
weights = precision / precision.sum()
mean = weights @ z
std = np.sqrt(1 / precision.sum())
print("权重:", weights)
print("融合结果:", mean, "m")
print("模型标准差:", std, "m")

这里的“更准”指在假设成立时，估计量的方差更小，并不保证每一次融合都比每一个原始读数更接近真值。也不能因为公式简洁，就忽略它的前提。

两个来源若没有转换到同一坐标系、对应的不是同一个物体，或者一个读数来自 200 ms 前，融合公式本身再正确，也是在融合不同的东西。

7. 最危险的过度自信：把相关信息当成独立证据

假设每次测量都包含同一个共享项 b：

zᵢ = x_true + b + εᵢ
Var(b) = (0.03 m)²
Var(εᵢ) = (0.01 m)²

在这个模型里，b 被视为一个零均值、标准差 30 mm 的随机项；它在同一批观测中共享。每个 εᵢ 则相互独立，也与 b 独立。共享项可以代表一份共同标定误差或共同参考状态的不确定性。

对 N 次观测平均后：

Var(z̄) = Var(b) + Var(ε)/N

因为 b+b+...+b 再除以 N，仍然是 b。它不会缩小 N 倍。

若误把各次观测完全独立，则会计算成：

错误模型：Var(z̄) = [Var(b)+Var(ε)]/N

N=100 时，正确模型给出约 30.02 mm 的标准差，错误模型却只给出约 3.16 mm。

图 5：橙色曲线逐渐接近共享项的 30 mm 下限；蓝色曲线由于遗漏相关性，持续高估重复测量带来的收益。

这类问题不只发生在同一传感器的连续帧。两个模块可能使用同一份地图、同一个里程计先验或同一次标定；模块名字不同，不代表信息来源独立。把已经融合过的结果又当作独立原始观测反馈回来，也可能重复计算同一份信息。

还要与第一节的固定偏差区分：本节主动把共享项的不确定性写进了模型，因此能算出它的影响。若真实世界存在某个偏差，而模型里根本没有它，再漂亮的协方差也不会自动把它补出来。

8. 在机器人项目中，让不确定性真正有用

对八界这样的机器人，感知点、相机外参、底盘定位和关节状态可能共同影响工具目标的位置。不确定性应跟着数据的坐标与时间语义流动，而不只是接口里附带的一个矩阵。

实际检查时，可以从下面几件具体事情开始：

• 固定目标重复采样，分别检查均值偏移与随机散布；不要只看图像是否稳定。
• 将点和协方差一起变换，核对位置、角度的单位及接口分量排列。[4]
• 记录哪些误差来自共享标定、地图或先验，避免把重复信息当作新证据。
• 用留出的数据检查误差区域的覆盖情况，并按距离、视角、材质或运动状态分组，观察模型是否只在某些条件下有效。

验证覆盖情况也依赖参考测量质量。如果用于对比的“真值”本身有不确定性，应把这一层计入判断，不能假设参考设备绝对准确。

当前本地八界模型中的部分外参配置仍是占位值。配置写成零，不表示安装误差或不确定性为零；不能拿这些占位值声称已经完成实机标定。

本文的 examples/05_uncertainty.py 可复现样本统计、概率椭圆、坐标旋转、极坐标传播、加权融合与相关性反例。运行方式为：

python3 examples/05_uncertainty.py

05_uncertainty.py
欢迎关注科沃斯八界 AtomGit [5]，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。本文示例为独立教学代码，不代表某个已公开 SDK 的实现。

理解了不确定性之后，还剩下一种非常容易漏掉的误差来源：数据本身没有坏，只是已经旧了。下一篇将从采集时间、处理队列和坐标变换的时刻讲起，把时间放回机器人的数据模型中。

参考链接

[1] NIST TN 1297：Appendix D1 — Terminology，误差、偏差和不确定性的术语边界。
[2] NIST TN 1297：Appendix A — Law of Propagation of Uncertainty，一阶传播与协方差项。
[3] NIST/SEMATECH：Chi-Square Distribution，卡方分布定义。文中二维概率与数值由该定义推导并计算。
[4] ROS REP-103 官方源文档：Standard Units of Measure and Coordinate Conventions，单位、坐标及协方差排列约定。
[5] 科沃斯八界 AtomGit，项目代码关注入口，相关代码逐步释放。