机器人 / 系统课时

如何用协方差表示位姿不确定性

如何用协方差表示位姿不确定性

机器人报告的位姿即使是“地图坐标 x=2 米、y=1 米、朝向 30°”,也不代表它准确处于这个点。本篇用高斯分布的均值和协方差表示位置与航向误差,算出二维位置的 95% 概率椭圆,并判断变量相关性会怎样改变椭圆方向。

学习目标

  • 识别二维位姿均值和 3×33\times3 协方差矩阵的含义。
  • 计算位置、航向标准差与位置误差相关系数。
  • 解释二维 95% 概率椭圆,并定位 ROS 2 消息中的相关字段。

二维位姿的概率表示

平面机器人位姿写成 p=[x,y,θ]T\mathbf p=[x,y,\theta]^\mathsf T,其中 x,yx,y 用米,航向角 θ\theta 在计算时用弧度。在 p∼N(μ,Σ)\mathbf p\sim\mathcal N(\boldsymbol\mu,\Sigma) 的局部高斯近似中,均值 μ\boldsymbol\mu 表示估计中心,用协方差 Σ\Sigma 表示误差大小和相互关系。

假设均值是 x=2.0x=2.0 m、y=1.0y=1.0 m、θ=30∘\theta=30^\circ,协方差为:

Σ=[0.04000.012000.01200.01000000.007615]\Sigma=\begin{bmatrix}0.0400&0.0120&0\\0.0120&0.0100&0\\0&0&0.007615\end{bmatrix}

对角线分别是 xx、yy 和航向角的方差,单位依次为 m²、m²、rad²;非对角线是协方差,位置与角度间的交叉项单位为 m·rad。示例把航向与位置的交叉协方差设为零,只为便于演算,真实 SLAM 不一定如此。

读出误差尺度和相关性

把方差开平方就得到标准差:σx=0.04=0.20\sigma_x=\sqrt{0.04}=0.20 m,σy=0.01=0.10\sigma_y=\sqrt{0.01}=0.10 m,σθ=0.007615≈0.0873\sigma_\theta=\sqrt{0.007615}\approx0.0873 rad,即约 5∘5^\circ。这表示各变量的边际误差尺度,不是最大误差界限。

位置交叉项 Σxy=0.012\Sigma_{xy}=0.012 m²,标准化后得到:

ρxy=0.0120.20×0.10=0.60\rho_{xy}=\frac{0.012}{0.20\times0.10}=0.60

ρxy>0\rho_{xy}>0 说明 xx 估高时,yy 的估计误差也更倾向于估高;它不表示机器人正在向右上方运动。协方差矩阵必须对称、半正定,本例 ∣ρxy∣≤1|\rho_{xy}|\leq1,符合二维子矩阵的基本条件。

把二维位置误差画成椭圆

只抽取 x,yx,y 的 2×22\times2 子矩阵。对于二维高斯,包含约 95% 位置概率的区域满足:

(q−μxy)TΣxy−1(q−μxy)≤5.991(\mathbf q-\boldsymbol\mu_{xy})^\mathsf T\Sigma_{xy}^{-1}(\mathbf q-\boldsymbol\mu_{xy})\leq5.991

这里 5.9915.991 是自由度为 2 的卡方分布 95% 分位点。对子矩阵求特征值并开平方,再乘 5.991≈2.448\sqrt{5.991}\approx2.448,得到椭圆长、短半轴分别约 0.515 m 和 0.186 m,长轴从地图 xx 正方向逆时针旋转约 19.3°。机器人朝向仍为 30°,两种角度不能混淆。

二维高斯位置协方差椭圆与航向不确定性

用 Python 验算椭圆参数

以下程序仅用 Python 3 标准库,保存为 covariance_demo.py 并运行 python3 covariance_demo.py。它按照对称 2×22\times2 协方差矩阵的特征值公式计算半轴,避免依赖绘图库。

python
from math import atan2, degrees, hypot, radians, sqrt

var_x, var_y, cov_xy = 0.04, 0.01, 0.012
sigma_yaw = radians(5.0)

# 对称二维协方差矩阵的两个特征值
gap = hypot(var_x - var_y, 2 * cov_xy)
eig_max = (var_x + var_y + gap) / 2
eig_min = (var_x + var_y - gap) / 2

# 二维高斯95%概率椭圆对应的卡方阈值
scale = sqrt(5.991464547)
major, minor = scale * sqrt(eig_max), scale * sqrt(eig_min)
angle = degrees(0.5 * atan2(2 * cov_xy, var_x - var_y))
rho = cov_xy / sqrt(var_x * var_y)

print(f"相关系数: {rho:.2f}")
print(f"95%半轴: {major:.3f} m, {minor:.3f} m")
print(f"长轴角度: {angle:.1f}°")
print(f"航向1σ: {degrees(sigma_yaw):.1f}°")

预期输出:

text
相关系数: 0.60
95%半轴: 0.515 m, 0.186 m
长轴角度: 19.3°
航向1σ: 5.0°

对照 ROS 2 的协方差字段

nav_msgs/msg/Odometry 的 pose.covariance 不是直接的 3×33\times3 矩阵,而是按行优先展开的 36 个数,对应状态顺序 x, y, z, roll, pitch, yaw。检查二维定位结果时,covariance[0] 是 xx 方差、covariance[7] 是 yy 方差、covariance[1] 和 covariance[6] 是 xyxy 交叉协方差、covariance[35] 是 yaw 方差。

均值朝向由 pose.pose.orientation 四元数换算,不能把 covariance[35] 当作朝向角。具体字段顺序可核对 ROS 2 官方消息定义。读取数据后先检查矩阵对称性、单位与数值是否合理,再画椭圆。特别是不能把未估计的自由度填成零后就宣称它“没有误差”,也不能把概率椭圆当作机器人实体外形或避障安全边界。

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

目录与阅读设置

阅读设置

17

记录一个想法

如何用协方差表示位姿不确定性

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

登录哪都通