【自动驾驶】从贝叶斯到卡尔曼：线性滤波的数学之美与实践之路

1. 贝叶斯概率理解不确定性的语言想象你正在雾天开车前方隐约有个模糊的影子。你的大脑会快速判断那可能是一个行人60%概率也可能只是路标40%概率。这种在不确定环境中做判断的能力正是贝叶斯概率的核心思想——用数学语言描述我们对世界的认知程度。在自动驾驶系统中贝叶斯公式就像一位永不疲倦的概率会计师。它持续追踪三个关键变量先验概率根据历史数据得到的初始判断比如某路段行人出现概率似然概率当前传感器观测到特定信号的可能性后验概率融合新旧信息后的最优判断用数学表示就是P(状态|观测) [P(观测|状态) × P(状态)] / P(观测)这个看似简单的公式却是处理传感器噪声的利器。去年我们在测试中发现当激光雷达在雨天的误报率上升时系统会自动降低激光雷达数据的权重转而更依赖毫米波雷达的稳定输出——这正是贝叶斯更新的实际体现。2. 卡尔曼滤波动态世界的预测大师2.1 预测-更新的双人舞卡尔曼滤波的精妙之处在于它的两步循环机制。就像玩抛接球游戏预测步根据物体当前速度和位置预测0.1秒后球的落点更新步当实际观察到球的位置后修正预测误差在代码实现中这个循环通常不超过20行while True: # 预测阶段 x_hat F x_prev P_hat F P_prev F.T Q # 更新阶段 K P_hat H.T np.linalg.inv(H P_hat H.T R) x_new x_hat K (z - H x_hat) P_new (I - K H) P_hat2.2 噪声协方差的调参艺术Q过程噪声和R观测噪声这两个矩阵是算法的灵敏度旋钮。我们在实际项目中总结出这些经验Q调大系统更信任新观测值响应更快但容易受噪声影响R调大系统更依赖预测模型平滑但响应延迟典型初始值Q np.diag([0.1, 0.1, 0.3, 0.3]) # 位置噪声小速度噪声大 R np.diag([0.5, 0.5]) # 观测噪声中等3. 多传感器融合实战3.1 传感器特性对比传感器类型优点缺点适用场景毫米波雷达测速准、不受天气影响分辨率低车辆检测激光雷达高精度3D点云雨雪天性能下降行人识别摄像头丰富语义信息依赖光照条件交通标志识别3.2 融合架构设计我们采用的分层融合方案在实践中表现优异底层融合同类型传感器数据对齐如多个雷达中层融合跨模态数据关联雷达点云视觉检测框高层融合基于卡尔曼滤波的状态估计一个典型的激光雷达与视觉融合流程def fuse_lidar_camera(lidar_points, camera_detections): # 坐标转换 points_in_cam lidar_to_camera(lidar_points) # 数据关联 matches hungarian_algorithm(points_in_cam, camera_detections) # 状态更新 for lidar_idx, cam_idx in matches: z measurement_matrix np.vstack([points_in_cam[lidar_idx], camera_detections[cam_idx]]) kf.update(z)4. 行人跟踪的工程挑战4.1 运动模型选择恒定速度模型在大多数场景表现良好但当行人突然转向时会出现滞后。我们开发了多模型自适应滤波器常规运动时使用CV模型检测到急转弯切换至CTRV恒定转率和速度模型完全静止启用零速度模型4.2 数据关联难题当多个行人近距离行走时容易发生ID切换。我们的解决方案是使用马氏距离代替欧式距离引入外观特征当有摄像头时设置置信度衰减机制实测表明这套方案将MOTA指标提升了37%Before: ID switches15, FP32, FN28 After: ID switches6, FP18, FN17在部署过程中最耗时的不是算法本身而是参数调试。我们建立了自动化测试平台用历史数据批量评估不同参数组合最终将调试周期从2周缩短到3天。