3、传感器数据融合基础：数据融合层级、卡尔曼滤波原理、坐标系对齐
各位同学，欢迎来到第三章。
前两章我们把ADAS系统的整体架构和传感器特性捋了一遍。从这章开始，要进入一个核心话题——数据融合。
说白了，就是让摄像头、毫米波雷达、激光雷达这些“各说各话”的传感器，能协同工作，输出一个统一、可靠的环境模型。我做了这么多年ADAS，可以负责任地告诉你：融合做不好，后面所有的决策、控制都是空中楼阁。
3.1 数据融合的三个层级
数据融合不是一股脑把所有原始数据倒在一起。根据处理阶段的不同，业界通常分为三个层级。我习惯用一个比喻来理解：“拼图”。
- 数据级融合：相当于把几幅拼图的碎片直接混在一起，再拼。信息最完整，但计算量巨大，而且对传感器的时间同步、分辨率一致性要求极高。
- 特征级融合：先各自拼出局部图案（提取特征），再把局部图案拼成完整画面。这是目前ADAS的主流做法。
- 决策级融合：各自拼完整幅图，然后投票决定最终结果。容错性好，但会丢失底层细节。
我在项目中遇到过最典型的案例：某Tier1的方案用了数据级融合，把64线激光雷达的点云和2M摄像头的像素直接对齐。效果确实好，但算力消耗直接翻了三倍，最后量产时不得不降级到特征级融合。所以，选哪个层级，本质是精度与算力的博弈。
| 融合层级 | 处理对象 | 优点 | 缺点 | 典型应用 |
|---|---|---|---|---|
| 数据级 | 原始像素/点云 | 信息损失最小 | 计算量大、带宽高、同步难 | 高精度建图 |
| 特征级 | 目标边界框/点云簇 | 平衡精度与算力 | 特征提取可能丢失细节 | 目标检测与跟踪 |
| 决策级 | 目标类别/置信度 | 系统鲁棒性强 | 无法利用底层互补信息 | 多传感器投票 |
3.2 卡尔曼滤波原理：从“猜”到“准”
融合的核心，是状态估计。你想想看，传感器都有噪声，雷达测距可能跳变，摄像头检测框会抖动。怎么从这些不完美的观测中，得到目标最真实的位置和速度？
卡尔曼滤波就是干这个的。它不复杂，本质上是一个“预测-更新”的循环。
- 预测：根据上一帧的状态（位置、速度），用运动模型猜一下这一帧目标在哪。
- 更新：拿到传感器这一帧的观测值，结合“猜的结果”和“观测结果”，各自带一个权重（卡尔曼增益），算出最优估计。
嗯，这里要注意：卡尔曼增益是动态计算的。如果传感器噪声小，我就更相信观测；如果模型预测得准，我就更相信预测。我曾经调试过一个项目，毫米波雷达的噪声方差标定错了，导致卡尔曼滤波完全跟着噪声跑，目标轨迹像喝醉了酒。后来重新标定了噪声协方差矩阵，效果立竿见影。
下面是一个简化的卡尔曼滤波预测-更新流程，我用Python写了个核心骨架，方便你理解。
import numpy as np
# 状态向量: [x, y, vx, vy]
# 观测向量: [x, y]
def kalman_filter(z, F, H, Q, R, x_est, P):
"""
z: 当前观测值
F: 状态转移矩阵
H: 观测矩阵
Q: 过程噪声协方差
R: 观测噪声协方差
x_est: 上一帧最优估计
P: 上一帧误差协方差
"""
# ---- 预测 ----
x_pred = F @ x_est
P_pred = F @ P @ F.T + Q
# ---- 更新 ----
y = z - H @ x_pred # 残差
S = H @ P_pred @ H.T + R # 残差协方差
K = P_pred @ H.T @ np.linalg.inv(S) # 卡尔曼增益
x_est = x_pred + K @ y
P = (np.eye(len(x_est)) - K @ H) @ P_pred
return x_est, P3.3 坐标系对齐：让传感器“说同一种语言”
这是融合里最基础、也最容易出bug的一步。每个传感器都有自己的坐标系：
- 摄像头：像素坐标系 (u, v)
- 毫米波雷达：极坐标系 (距离, 角度, 速度)
- 激光雷达：笛卡尔坐标系 (x, y, z)
- 车辆本体：以后轴中心为原点的车身坐标系
不把它们对齐到同一个坐标系下，融合就是鸡同鸭讲。我见过一个案例，因为雷达安装支架的俯仰角标定差了0.5度，导致高速上目标位置偏差了2米，AEB系统差点误触发。
坐标系对齐通常分两步：
- 内参标定：传感器自身的参数（如相机焦距、畸变系数）。
- 外参标定：传感器之间的相对位置和姿态（旋转矩阵R + 平移向量t）。
下面这张图展示了从传感器原始数据到统一车身坐标系的核心流程。
3.4 本章小结
这一章我们聊了三件事：
- 融合层级：数据级、特征级、决策级。量产首选特征级。
- 卡尔曼滤波：预测+更新，核心是卡尔曼增益的动态计算。
- 坐标系对齐：内参+外参，把不同传感器的数据统一到车身坐标系。