机器人 / 系统课时
如何用协方差表示位姿不确定性
如何用协方差表示位姿不确定性
机器人报告的位姿即使是“地图坐标 x=2 米、y=1 米、朝向 30°”,也不代表它准确处于这个点。本篇用高斯分布的均值和协方差表示位置与航向误差,算出二维位置的 95% 概率椭圆,并判断变量相关性会怎样改变椭圆方向。
学习目标
- 识别二维位姿均值和 协方差矩阵的含义。
- 计算位置、航向标准差与位置误差相关系数。
- 解释二维 95% 概率椭圆,并定位 ROS 2 消息中的相关字段。
二维位姿的概率表示
平面机器人位姿写成 ,其中 用米,航向角 在计算时用弧度。在 的局部高斯近似中,均值 表示估计中心,用协方差 表示误差大小和相互关系。
假设均值是 m、 m、,协方差为:
对角线分别是 、 和航向角的方差,单位依次为 m²、m²、rad²;非对角线是协方差,位置与角度间的交叉项单位为 m·rad。示例把航向与位置的交叉协方差设为零,只为便于演算,真实 SLAM 不一定如此。
读出误差尺度和相关性
把方差开平方就得到标准差: m, m, rad,即约 。这表示各变量的边际误差尺度,不是最大误差界限。
位置交叉项 m²,标准化后得到:
说明 估高时, 的估计误差也更倾向于估高;它不表示机器人正在向右上方运动。协方差矩阵必须对称、半正定,本例 ,符合二维子矩阵的基本条件。
把二维位置误差画成椭圆
只抽取 的 子矩阵。对于二维高斯,包含约 95% 位置概率的区域满足:
这里 是自由度为 2 的卡方分布 95% 分位点。对子矩阵求特征值并开平方,再乘 ,得到椭圆长、短半轴分别约 0.515 m 和 0.186 m,长轴从地图 正方向逆时针旋转约 19.3°。机器人朝向仍为 30°,两种角度不能混淆。

用 Python 验算椭圆参数
以下程序仅用 Python 3 标准库,保存为 covariance_demo.py 并运行 python3 covariance_demo.py。它按照对称 协方差矩阵的特征值公式计算半轴,避免依赖绘图库。
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}°")预期输出:
相关系数: 0.60
95%半轴: 0.515 m, 0.186 m
长轴角度: 19.3°
航向1σ: 5.0°对照 ROS 2 的协方差字段
nav_msgs/msg/Odometry 的 pose.covariance 不是直接的 矩阵,而是按行优先展开的 36 个数,对应状态顺序 x, y, z, roll, pitch, yaw。检查二维定位结果时,covariance[0] 是 方差、covariance[7] 是 方差、covariance[1] 和 covariance[6] 是 交叉协方差、covariance[35] 是 yaw 方差。
均值朝向由 pose.pose.orientation 四元数换算,不能把 covariance[35] 当作朝向角。具体字段顺序可核对 ROS 2 官方消息定义。读取数据后先检查矩阵对称性、单位与数值是否合理,再画椭圆。特别是不能把未估计的自由度填成零后就宣称它“没有误差”,也不能把概率椭圆当作机器人实体外形或避障安全边界。