2. 传感器误差模型：理解你的“眼睛”和“耳朵”到底有多准
做多传感器融合，说白了就是让机器人用不同的“感官”去感知世界。但问题是，这些感官本身就不完美。我经常跟团队里的新人说：“你连传感器的脾气都没摸透，融合出来的结果能靠谱吗？”
这一节，我们就来聊聊三种核心传感器的误差模型。IMU、相机、激光雷达，它们各自的“毛病”是什么，怎么建模，怎么在融合中补偿。嗯，这部分内容很基础，但也是后面所有精度评估的根基。
核心观点：误差模型不是数学游戏，它是你算法性能的天花板。不懂误差，你连融合结果的好坏都说不清楚。
2.1 IMU 误差模型：那个“飘”得让你头疼的家伙
IMU 这东西，我做了这么多年项目，对它真是又爱又恨。爱它是因为它高频、不受环境干扰；恨它是因为——它真的会飘。你想想看，一个静止放在桌上的 IMU，积分出来的位置可能已经跑到隔壁房间去了。
IMU 的误差，主要分三类：
2.1.1 零偏（Bias）
零偏是什么？说白了就是传感器在零输入时，输出却不归零。比如陀螺仪，你明明没转它，它却告诉你“我在转”。
- 常值零偏：每次上电后固定的偏移量。我习惯在系统启动时做静态初始化，采集前几秒的数据取平均，把这个偏移扣掉。
- 零偏不稳定性（Bias Instability）：这个比较烦人。它随时间缓慢变化，像个“慢漂移”。我在一个无人机项目里遇到过，飞了10分钟后位置误差直接飙到米级，查了半天才发现是零偏在作祟。
我的经验：零偏不稳定性通常用 Allan 方差来辨识。如果你手头有 IMU 的静态数据，画个 Allan 方差曲线，一眼就能看出它的“底噪”水平。
2.1.2 白噪声（White Noise）
白噪声是高频随机分量。你可以把它想象成传感器读数上的“毛刺”。它服从高斯分布，均值为零，方差为 σ²。
在融合中，白噪声通常被建模为：
// 离散时间下的白噪声模型
// 假设采样间隔为 dt
// 角度随机游走系数 ARW = σ_gyro * sqrt(dt)
// 速度随机游走系数 VRW = σ_accel * sqrt(dt)
// 实际使用时，我一般这样初始化噪声矩阵：
Matrix3d Q_gyro = pow(ARW, 2) * Matrix3d::Identity();
Matrix3d Q_accel = pow(VRW, 2) * Matrix3d::Identity();2.1.3 随机游走（Random Walk）
这个其实是由白噪声积分产生的。你想想看，白噪声每次积分都会累积误差，时间越长，位置和姿态的漂移就越大。
我记得有一次做车载组合导航，IMU 的随机游走系数没标定准，结果车辆转弯时姿态估计直接偏了 5 度。后来老老实实做了 Allan 方差分析，才把问题搞定。
⚠️ 避坑指南：我曾经在低成本的 MEMS IMU 上吃过亏。这类 IMU 的随机游走系数可能比 datasheet 上标的大 2-3 倍。建议你拿到传感器后，先自己跑一遍 Allan 方差标定，别信厂商给的参数。
2.2 相机误差模型：视觉的“近视”与“变形”
相机是个好东西，能提供丰富的纹理信息。但它也有自己的“视力问题”。
2.2.1 重投影误差（Reprojection Error）
重投影误差，是视觉 SLAM 里最核心的误差项。它的定义很简单：3D 点投影到图像上的像素位置，与实际观测到的像素位置之间的差值。
数学上长这样：
// 重投影误差公式
// e = p_obs - project(T * P_world)
// 其中：
// p_obs: 实际观测到的像素坐标 (u, v)
// T: 相机位姿 (旋转 + 平移)
// P_world: 3D 点的世界坐标
// project(): 相机投影函数（包含内参和畸变）
// 在优化中，我们最小化所有匹配点的重投影误差之和
// 这就是 Bundle Adjustment 的核心思想我习惯把重投影误差的单位控制在亚像素级别。如果误差超过 1 个像素，我就会怀疑特征点匹配是不是出了问题。
2.2.2 畸变（Distortion）
畸变是镜头带来的“变形”。主要有两种：
- 径向畸变：图像边缘的直线会变弯。桶形畸变和枕形畸变就是典型的例子。
- 切向畸变：镜头和成像平面不平行导致的。这个在手机相机里比较常见。
畸变模型通常用多项式来描述：
// 径向畸变模型（常用的是 Brown-Conrady 模型）
// r^2 = x^2 + y^2
// x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
// y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y
// 其中 k1, k2, k3 是径向畸变系数
// p1, p2 是切向畸变系数我的建议：畸变标定一定要做。我曾经在一个项目里偷懒用了厂商给的畸变参数，结果视觉里程计在图像边缘区域误差大了 30%。后来用 OpenCV 的棋盘格重新标定了一遍，效果立竿见影。
2.3 激光雷达误差模型：点云里的“毛刺”从哪来？
激光雷达的误差相对直观，主要就两个来源：测距和测角。
2.3.1 测距噪声（Range Noise）
激光雷达测距的原理是飞行时间（ToF）或调频连续波（FMCW）。噪声来源包括：
- 高斯噪声：主要由电子器件热噪声引起。通常建模为均值为零的高斯分布，标准差在 1-3 cm 左右（视传感器型号而定）。
- 多路径效应：激光打到玻璃或镜面反射物上，会产生虚假回波。这个在室内场景比较常见。
我一般这样建模测距噪声：
// 激光雷达测距噪声模型
// z = r + n_r
// 其中 r 是真实距离，n_r ~ N(0, σ_r²)
// σ_r 通常与距离有关：σ_r = σ_0 + k * r
// σ_0: 基础噪声（比如 0.01m）
// k: 距离相关因子（比如 0.001）
// 实际代码中：
double range_noise_sigma = 0.01 + 0.001 * measured_range;2.3.2 角度噪声（Angular Noise）
角度噪声主要来自激光雷达的旋转机构或 MEMS 振镜。它会导致点云中的点“歪掉”。
- 水平角噪声：旋转电机的不稳定性造成的。
- 垂直角噪声：多线激光雷达中，不同线束之间的角度偏差。
我记得有一次调试 16 线激光雷达，发现地面点云有明显的“波浪形”。查了半天，原来是垂直角标定参数偏了 0.1 度。别小看这 0.1 度，在 50 米外，位置误差能到 8-9 厘米。
⚠️ 注意：激光雷达的噪声模型不是各向同性的。水平角噪声通常比垂直角噪声小一个数量级。你在设计融合滤波器时，噪声矩阵一定要区分对待。
2.4 小结：误差模型是融合的“地基”
这一节我们聊了三种传感器的误差模型。IMU 的零偏和随机游走、相机的重投影误差和畸变、激光雷达的测距和角度噪声——每个都有它的“脾气”。
我个人觉得，做多传感器融合，70% 的精力应该花在理解传感器上。你只有把每个传感器的误差特性摸透了，才能在融合时给它们分配合理的权重。否则，再高级的卡尔曼滤波也救不了你。
嗯，下一节我们会聊怎么把这些误差模型用到融合框架里。不过在那之前，我建议你先把手上传感器的 Allan 方差跑一遍，看看它的真实水平。
公众号:蓝海资料掘金营，微信deep3321