3. 传感器与观测模型：让机器人“看见”世界
做目标跟踪，第一步不是算法，而是传感器。你想想看，如果传感器数据都是错的，再牛的滤波算法也白搭。我在自动驾驶项目里踩过最大的坑，就是低估了传感器噪声的影响。今天咱们就聊聊三种主流传感器——激光雷达、摄像头、毫米波雷达，它们各自怎么工作，噪声从哪来，以及拿到原始数据后该怎么处理。
3.1 激光雷达：精度之王，但也有脾气
激光雷达的原理很简单——发射激光束，测量反射时间。光速是恒定的，所以距离 = 光速 × 时间 / 2。听起来完美，对吧？
但实际用起来，问题不少。我遇到过最头疼的是多径效应。激光打到玻璃上，一部分反射，一部分穿透，再打到后面的物体上反射回来。这时候雷达会收到两个回波，距离值就乱了。
激光雷达观测模型：
zt = h(xt) + vt
其中 vt ~ N(0, R)，R 通常是对角矩阵，但实际中不同角度、不同距离的噪声方差差别很大。
我个人习惯把激光雷达的噪声模型分成三块：
- 距离噪声： 一般 ±2cm，但远距离（>100m）会扩大到 ±10cm
- 角度噪声： 水平角分辨率 0.1°~0.4°，垂直方向更粗
- 反射率噪声： 低反射率物体（黑色车漆）容易丢点
避坑指南： 我曾经在雨雪天做实验，激光雷达的点云直接炸了。雨滴会反射激光，产生大量噪点。后来我加了基于强度的滤波，把反射率低于某个阈值的点直接扔掉，效果好了很多。
3.2 摄像头：信息最丰富，但最不靠谱
摄像头提供的是稠密的像素信息。每个像素代表一个方向的光强。但问题是——它没有深度信息。
你想想看，一个 2D 图像，怎么知道物体离你多远？只能靠几何关系推算。比如已知车道宽度，或者用双目视差。但这些都是间接测量，误差很大。
摄像头的观测模型可以写成：
// 针孔相机模型
[u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T
其中：
K = [fx, 0, cx;
0, fy, cy;
0, 0, 1]
这里的噪声来源主要有：
- 量化噪声： 像素坐标是离散的，±0.5 像素的误差很正常
- 镜头畸变： 广角镜头边缘的畸变，不校正的话误差能到几十个像素
- 光照变化： 逆光、阴影、夜间，特征点提取会大量丢失
注意： 摄像头对动态光照极其敏感。我做过一个测试，同一场景，正午和傍晚提取的特征点数量能差 5 倍。所以做视觉跟踪时，一定要加光照补偿或者自适应阈值。
3.3 毫米波雷达：全天候选手，但分辨率低
毫米波雷达用 77GHz 左右的电磁波。它不怕雨雪雾，能直接测距离和速度（多普勒效应）。但它的角度分辨率很差，一般只有几度。
说白了，毫米波雷达知道「前方 50 米有个东西以 10m/s 的速度靠近我」，但不知道这东西在车道左边还是右边。这就很尴尬了。
它的观测模型：
| 测量量 | 精度 | 噪声特性 |
|---|---|---|
| 距离 | ±0.5m | 高斯分布，远距离方差增大 |
| 速度 | ±0.1m/s | 比较准，多普勒效应直接测量 |
| 角度 | ±3°~±5° | 最不准，容易产生假目标 |
我的经验： 毫米波雷达的虚警率很高。路边的金属护栏、井盖，甚至地上的易拉罐，都可能被检测成目标。我一般会加一个「置信度阈值」，只有连续几帧都出现的点才认为是真实目标。
3.4 数据预处理：去噪与滤波
拿到原始数据后，不能直接用。噪声太多，会搞死跟踪算法。我一般做三步预处理：
- 时间戳对齐： 不同传感器的采样频率不同（激光 10Hz，摄像头 30Hz，雷达 20Hz），必须插值到同一时间戳
- 空间坐标变换： 所有传感器数据转到车体坐标系下，需要标定外参
- 离群点剔除： 用统计方法或基于距离的方法去掉明显异常的点
举个去噪的例子——激光雷达的离群点剔除：
// 基于邻域统计的离群点剔除
for each point p in point_cloud:
neighbors = kd_tree.radius_search(p, radius=0.5m)
if len(neighbors) < 3:
mark_as_outlier(p)
else:
mean_dist = mean(neighbors.distances)
if p.distance > mean_dist * 2.0:
mark_as_outlier(p)
嗯，这里要注意。阈值不能设太死。我在高速场景下，点云稀疏，3 个邻居可能都找不到。这时候我会动态调整半径，或者改用基于密度的聚类方法。
3.5 知识体系总览
下面这张图总结了传感器与观测模型的核心逻辑。从原始数据到可用的观测值，中间要经过噪声建模、预处理、坐标变换三个关键步骤。
这张图把整个流程串起来了。三种传感器各有优劣，噪声模型也各不相同。但最终都要经过预处理，变成统一的观测值格式，才能喂给后面的跟踪算法。
核心要点：
- 激光雷达：精度高，但受天气影响大，需要做离群点剔除
- 摄像头：信息最丰富，但深度估计不准，需要校正畸变
- 毫米波雷达：全天候，但角度分辨率低，虚警多
- 预处理三步走：时间对齐 → 空间变换 → 去噪滤波
好了，传感器这块就聊到这儿。说白了，没有完美的传感器，只有合适的预处理和融合策略。我在实际项目中，一般会先用激光雷达做主要跟踪，摄像头辅助识别类别，毫米波雷达做冗余备份。这样即使某个传感器挂了，系统还能跑。