4. 感知融合
在自动驾驶与机器人系统中，单一传感器很难同时满足精度、鲁棒性与环境适应性要求。摄像头擅长识别语义与纹理，但在测距和恶劣光照下表现不稳定；激光雷达提供高精度三维几何信息，却受雨雾影响且成本较高；毫米波雷达具备全天候测速能力，但角分辨率有限。感知融合的核心任务，就是把来自不同传感器、不同时刻、不同坐标系的数据统一到同一状态估计框架中，输出比任何单一传感器都更可信的环境描述。
一、多传感器融合的层次与核心问题
多传感器融合通常分为三个层次：数据级融合直接对原始点云或像素做配准与叠加，信息损失最小，但对时间同步和标定精度要求极高；特征级融合先提取边缘、目标框、点云聚类等特征再关联，兼顾信息量与计算量；决策级融合对各传感器独立输出进行置信度加权或投票，鲁棒性最好，但精度上限较低。工程中常采用特征级与决策级混合架构。
融合要解决四个基本问题：时空对齐（时间戳同步、外参标定）、数据关联（判断两个观测是否来自同一目标）、状态估计（从带噪观测中递推真实状态）、一致性检验（识别异常观测并剔除）。其中状态估计是融合的数学心脏，卡尔曼滤波与粒子滤波正是两类最常用的递推估计方法。
二、卡尔曼滤波：高斯假设下的最优递推
卡尔曼滤波建立在两个假设之上：系统状态转移和观测模型近似线性，且过程噪声与观测噪声均服从高斯分布。它用均值表示状态估计，用协方差表示不确定性，通过“预测—更新”两步循环不断融合新观测。
预测步根据运动模型外推状态与协方差；更新步利用观测残差和卡尔曼增益修正估计。增益的本质是权衡“相信模型”还是“相信观测”：观测噪声越小，增益越大，估计越向观测靠拢。
# 卡尔曼滤波核心递推（一维匀速模型示例）
# 预测
x_pred = F @ x_est
P_pred = F @ P_est @ F.T + Q
# 更新
y = z - H @ x_pred # 新息（残差）
S = H @ P_pred @ H.T + R # 新息协方差
K = P_pred @ H.T @ inv(S) # 卡尔曼增益
x_est = x_pred + K @ y
P_est = (I - K @ H) @ P_pred
# 参数含义
# F 状态转移矩阵 Q 过程噪声协方差
# H 观测矩阵 R 观测噪声协方差
# z 当前观测 K 卡尔曼增益当系统非线性时，扩展卡尔曼滤波（EKF）通过一阶泰勒展开在工作点附近线性化；无迹卡尔曼滤波（UKF）则用确定性采样点传播均值和协方差，避免雅可比矩阵推导，在强非线性下精度更高。工程中EKF因计算量小、易于实现，仍是组合导航与目标跟踪的主流选择。
三、粒子滤波：用样本逼近任意分布
当后验分布呈现多峰、强非线性，或观测模型不可微时，高斯假设不再成立。粒子滤波用一组带权重的随机样本（粒子）近似后验概率分布，每个粒子代表一个可能状态假设，权重反映该假设与观测的吻合程度。
其流程为：采样——从建议分布中生成粒子；预测——按运动模型传播粒子；加权——根据观测似然更新权重；重采样——淘汰低权重粒子、复制高权重粒子，抑制权重退化。重采样会带来样本贫化问题，常通过增加扰动或采用自适应重采样缓解。
四、典型场景：前车跟踪中的融合实践
以高速公路前车跟踪为例：毫米波雷达提供纵向距离与相对速度，摄像头提供横向位置与目标类别，激光雷达提供精确轮廓。融合流程如下：
- 以雷达和IMU数据驱动EKF预测前车状态（位置、速度、加速度）。
- 将摄像头检测框反投影到车辆坐标系，与预测位置做马氏距离关联。
- 关联成功后用视觉观测更新横向状态，用雷达观测更新纵向状态。
- 计算归一化新息平方（NIS）做一致性检验，超阈值则判定为异常观测并降权。
- 当目标被遮挡导致观测中断时，仅靠预测维持航迹，并逐步扩大协方差。
| 对比维度 | 卡尔曼滤波（KF/EKF/UKF） | 粒子滤波（PF） |
|---|---|---|
| 分布假设 | 高斯分布 | 任意分布 |
| 系统模型 | 线性或可线性化 | 强非线性亦可 |
| 计算复杂度 | 低，与状态维度立方相关 | 高，与粒子数成正比 |
| 多峰处理 | 弱 | 强 |
| 典型应用 | 组合导航、目标跟踪、传感器融合 | 全局定位、多目标跟踪、SLAM |
| 主要风险 | 线性化误差、发散 | 粒子退化、样本贫化、实时性不足 |
五、常见误区与复盘要点
- 忽视时间同步：不同传感器时间戳偏差几十毫秒，在高速场景下会引入米级位置误差。应统一到同一时间基准并做运动补偿。
- 标定误差未建模：外参偏差会被滤波当作真实观测，导致估计系统性偏移。需将标定不确定度纳入观测噪声。
- 噪声参数拍脑袋：Q和R设置不合理会直接导致滤波发散或过度平滑。应通过实际数据统计或自适应估计确定。
- 关联错误未处理：错误关联比不关联更危险，会污染状态估计。必须设置门限并保留航迹管理机制。
- 盲目追求复杂算法：在嵌入式平台上，稳定可靠的EKF往往优于调参困难的PF。算法选型要匹配算力与实时性约束。
多传感器融合不是简单的数据叠加，而是在不确定性中做最优决策。卡尔曼滤波与粒子滤波提供了两套互补的数学工具：前者以高斯假设换取效率与稳定性，后者以采样代价换取对复杂分布的建模能力。理解它们的假设边界与适用条件，才能在感知规划控制安全监控工程中做出合理的架构选择。