第二章 传感器模型与数据预处理：雷达、摄像头、激光雷达的数学模型、坐标变换、时间同步、数据对齐
各位同学，欢迎来到实战教程的第二讲。
上一章我们聊了融合跟踪的整体框架。今天要啃的这块骨头，说实话，是整个系统里最“脏”最“累”的活——传感器模型与数据预处理。
你想想看，雷达、摄像头、激光雷达，这三个家伙“看”世界的方式完全不同。雷达告诉你“前方20米有个金属物体”，摄像头告诉你“那个红色的方块看起来像一辆车”，激光雷达则给你一堆密密麻麻的点云。怎么让它们“说同一种语言”？这就是本章要解决的问题。
核心观点：融合跟踪的精度，80%取决于数据预处理的质量。模型再漂亮，输入的是垃圾，输出依然是垃圾。
2.1 雷达传感器模型
雷达，说白了就是靠发射电磁波并接收回波来测距。我个人习惯把雷达模型拆成两部分看：测量模型和噪声模型。
2.1.1 测量模型
对于毫米波雷达，常见的测量输出是：
- 距离 r：目标到雷达的径向距离
- 方位角 θ：目标相对于雷达正前方的水平角度
- 径向速度 v_r：目标沿雷达径向方向的速度分量
数学上可以写成：
r = sqrt(x² + y²)
θ = atan2(y, x)
v_r = (x * vx + y * vy) / r嗯，这里要注意，雷达的测量是在极坐标系下完成的。而我们的跟踪算法通常工作在笛卡尔坐标系。这个转换，坑很多。
个人经验：我在项目中遇到过，直接把雷达的极坐标测量转换到笛卡尔坐标，然后用标准卡尔曼滤波。结果发现滤波发散。为什么？因为转换后的噪声分布不再是高斯分布了。后来我改用扩展卡尔曼滤波（EKF）或者无迹卡尔曼滤波（UKF），才解决了这个问题。
2.1.2 噪声模型
雷达的噪声通常假设为高斯白噪声，但参数跟距离有关：
σ_r = 0.15 * r + 0.5 (距离标准差，单位米)
σ_θ = 0.5° (角度标准差)
σ_vr = 0.1 (速度标准差，单位m/s)你看，距离越远，测距误差越大。这是雷达的物理特性决定的。
2.2 摄像头传感器模型
摄像头嘛，它给我们的是图像。但图像里没有直接的“距离”信息。我们需要通过几何模型来反推。
2.2.1 针孔相机模型
这是最经典的模型。说白了就是小孔成像原理：
u = fx * X / Z + cx
v = fy * Y / Z + cy其中 (u, v) 是图像像素坐标，(X, Y, Z) 是相机坐标系下的3D点。fx, fy 是焦距参数，cx, cy 是主点偏移。
这里有个关键问题：单目摄像头无法直接测量深度 Z。一个像素点可能对应远处的大物体，也可能对应近处的小物体。这就是所谓的“尺度不确定性”。
避坑指南：我曾经天真地以为，用单目摄像头加一个地面平面假设就能算出距离。结果在坡道上直接翻车。地面不是平的，你的假设就全错了。后来我改用双目或者雷达+摄像头融合，才稳住了。
2.2.2 畸变校正
实际摄像头都有镜头畸变，主要是径向畸变和切向畸变。不校正的话，图像边缘的物体位置会偏：
x_corrected = x * (1 + k1 * r² + k2 * r⁴ + k3 * r⁶)
y_corrected = y * (1 + k1 * r² + k2 * r⁴ + k3 * r⁶)k1, k2, k3 是畸变参数，需要标定得到。我建议你每次换摄像头或者调整焦距后，都重新做一次标定。
2.3 激光雷达传感器模型
激光雷达，我更喜欢叫它LiDAR。它发射激光束，测量飞行时间（ToF）来得到距离。精度高，但受天气影响大。
2.3.1 测量模型
对于机械旋转式LiDAR，每个激光束的测量可以表示为：
x = r * cos(ω) * sin(α)
y = r * cos(ω) * cos(α)
z = r * sin(ω)其中 r 是距离，ω 是垂直俯仰角，α 是水平旋转角。
LiDAR的输出是一帧点云，通常包含数万到数百万个点。每个点有 (x, y, z) 坐标，有些还带有反射强度信息。
2.3.2 点云预处理
拿到原始点云后，不能直接用。我一般会做三步：
- 地面滤除：用RANSAC或平面拟合，把地面点去掉。不然地面点会干扰目标检测。
- 聚类：用DBSCAN或欧式聚类，把属于同一个物体的点聚在一起。
- 下采样：用体素滤波器，降低点云密度。不然数据量太大，实时性扛不住。
我的习惯：体素滤波的格子大小我通常设0.1m到0.2m。太小了降不下去，太大了会丢失细节。你可以根据你的场景调一调。
2.4 坐标变换
好了，现在三个传感器都有自己的坐标系。雷达坐标系、相机坐标系、LiDAR坐标系，还有车体坐标系。怎么统一？
说白了就是刚体变换：旋转 + 平移。
P_body = R * P_sensor + T其中 R 是3x3旋转矩阵，T 是3x1平移向量。这个 (R, T) 就是传感器到车体的外参。
外参标定是个大活。我见过有人用手动测量，结果误差好几厘米。我建议用标定板或者基于自然特征的方法，精度能到毫米级。
重要提醒：坐标变换时，注意旋转顺序和方向。不同的库（比如Eigen、OpenCV、ROS）对旋转的定义可能不同。我曾经因为搞混了“先绕X轴还是先绕Z轴”，排查了整整两天。
2.5 时间同步
时间同步，说白了就是让所有传感器的数据“对齐”到同一个时间戳上。
为什么需要？你想想看，雷达以10Hz工作，摄像头以30Hz工作，LiDAR以20Hz工作。它们的数据到达时间不一样。如果不做同步，你拿到的“同一时刻”的数据，实际上可能差了100ms。对于高速运动的车辆，100ms意味着好几米的位移。
2.5.1 硬件同步 vs 软件同步
| 同步方式 | 精度 | 实现难度 | 适用场景 |
|---|---|---|---|
| 硬件同步（GPS PPS + 触发线） | 微秒级 | 高 | 自动驾驶、高精度融合 |
| 软件同步（时间戳对齐） | 毫秒级 | 低 | 低速、低精度场景 |
我个人建议，能上硬件同步就上硬件同步。软件同步虽然简单，但遇到传感器延迟抖动大的时候，效果很差。
2.5.2 时间戳对齐算法
软件同步的常用方法是线性插值：
假设雷达在 t1 时刻有数据，摄像头在 t2 时刻有数据
我们要得到 t 时刻的雷达数据（t1 < t < t2）
P(t) = P(t1) + (P(t2) - P(t1)) * (t - t1) / (t2 - t1)嗯，这里要注意，插值假设物体在短时间内做匀速运动。如果物体急加速或急转弯，插值误差会变大。
2.6 数据对齐
数据对齐，是最后一步，也是最容易出问题的一步。
对齐包括：
- 空间对齐：通过坐标变换，把所有数据统一到车体坐标系
- 时间对齐：通过时间同步，保证数据对应同一时刻
- 语义对齐：比如雷达检测到的目标，和摄像头检测到的目标，是不是同一个物体？这需要关联匹配
语义对齐通常用最近邻匹配或者匈牙利算法。我习惯的做法是：先用马氏距离计算两个目标之间的相似度，然后用匈牙利算法做最优匹配。
避坑指南：我曾经在数据对齐时，忘了考虑传感器之间的遮挡关系。结果雷达检测到的一个目标，在摄像头视角里被另一个物体挡住了。强行关联导致跟踪出错。后来我加了一个可视性检查，才解决了这个问题。
2.7 本章知识体系
下面这张图，是我自己画的本章知识体系结构。你可以把它当作一个“地图”，随时回来查阅。
这张图清晰地展示了本章的脉络：从三个传感器的原始数据出发，经过坐标变换、时间同步，最终完成数据对齐。每一步都有坑，每一步都需要细心处理。
最后说一句：数据预处理没有银弹。不同的传感器配置、不同的应用场景，处理方式都不一样。我的建议是：先跑通一个简单的pipeline，然后根据实际数据不断迭代优化。别想着一步到位。
好了，这一章的内容就到这里。下一章我们会聊目标检测与特征提取，到时候见。