第四节：传感器模型与数据融合

各位同学，今天我们来聊聊自动驾驶的“眼睛”和“大脑”。

传感器是车的眼睛，数据融合就是大脑。没有融合，每个传感器看到的都是片面的世界。我做了这么多年系统，见过太多因为融合没做好而出事的案例。嗯，咱们一步步来。

4.1 摄像头模型

摄像头，说白了就是模仿人眼。但它有个致命弱点——怕黑、怕雨、怕逆光。

摄像头模型的核心是针孔模型。它把三维世界投影到二维图像上。公式很简单：

// 针孔相机投影模型
// (X, Y, Z) 是世界坐标，(u, v) 是像素坐标
// fx, fy 是焦距，cx, cy 是主点偏移

u = fx * X / Z + cx
v = fy * Y / Z + cy

我在项目中遇到过一个问题：摄像头标定没做好，导致车道线检测偏差了10厘米。10厘米啊，在高速上就是生与死的距离。

4.2 激光雷达模型

激光雷达，我习惯叫它LiDAR。它用激光束扫描周围环境，直接给你一个点云。

LiDAR的模型核心是光束传播模型。每个点包含距离、角度、反射强度。公式如下：

// 激光雷达点云生成模型
// θ 是水平角，φ 是垂直角，r 是距离

x = r * cos(φ) * cos(θ)
y = r * cos(φ) * sin(θ)
z = r * sin(φ)

你想想看，LiDAR的好处是什么？它直接给你三维坐标，不需要像摄像头那样去“猜”深度。但坏处也很明显——贵，而且怕雨雪。

4.3 毫米波雷达模型

毫米波雷达，这是我最喜欢的传感器。为什么？因为它稳定。雨雪雾天照样工作。

它的模型基于多普勒效应。不仅能测距离，还能测速度。核心公式：

// 毫米波雷达测量模型
// f_d 是多普勒频移，v_r 是径向速度，λ 是波长

f_d = 2 * v_r / λ
r = c * Δt / 2   // 距离测量

但毫米波雷达有个问题——分辨率低。它只能告诉你“那里有个东西”，但说不清“那是什么”。所以它必须和摄像头配合使用。

4.4 IMU/GNSS模型

IMU（惯性测量单元）和GNSS（全球导航卫星系统）是定位的黄金搭档。

IMU的模型是积分模型。它测量加速度和角速度，然后积分得到位置和姿态。但积分会累积误差，这就是为什么需要GNSS来修正。

// IMU积分模型（简化版）
// a 是加速度，ω 是角速度，dt 是时间步长

v_new = v_old + a * dt
p_new = p_old + v_new * dt
θ_new = θ_old + ω * dt

GNSS的模型就简单了——它直接给你经纬度和高度。但精度受环境影响很大。高楼遮挡、多径效应，都会让定位漂移。

4.5 卡尔曼滤波融合

好了，现在我们有四个传感器。每个都有优点和缺点。怎么把它们结合起来？

卡尔曼滤波，说白了就是加权平均。但它不是简单的平均，而是根据每个传感器的置信度来动态调整权重。

核心公式分两步：预测和更新。

// 卡尔曼滤波核心公式
// 预测步骤
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q

// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_new = x_pred + K * (z - H * x_pred)
P_new = (I - K * H) * P_pred

这里面的关键参数：

• Q：过程噪声协方差。代表你对模型的信任程度。
• R：测量噪声协方差。代表你对传感器的信任程度。
• K：卡尔曼增益。它决定了你更相信预测还是测量。

4.6 融合策略实战

在实际项目中，我通常用松耦合策略。什么意思？就是每个传感器先自己做目标检测，然后我把它们的结果融合起来。

举个例子：

1. 摄像头检测到前方有车，距离50米，置信度0.8
2. 毫米波雷达检测到前方有目标，距离48米，速度10m/s，置信度0.9
3. LiDAR检测到前方有车，距离49米，置信度0.95

卡尔曼滤波会怎么做？它会根据置信度加权平均。LiDAR的置信度最高，所以它的权重最大。最终融合结果是距离49.2米。

4.7 总结与避坑

好了，这一节的内容就这些。我最后给你几个实用建议：

• 别过度依赖单一传感器。每个传感器都有盲区，多传感器融合是必须的。
• 卡尔曼滤波不是万能的。在非线性系统中，要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
• 调试时先看数据。我见过太多人一上来就调参数，结果发现是传感器坏了。先可视化数据，再动手调参。

下一节我们会讲控制算法。到时候你会看到，没有好的感知融合，控制算法再牛也没用。嗯，今天就到这里。