自动驾驶系统滤波（1）：自动驾驶滤波算法的基础理论与多传感器融合架构

一、引言：自动驾驶为何是滤波算法的“终极考场”

如果通信领域的滤波是“在嘈杂的酒吧里听清朋友说话”，那么自动驾驶领域的滤波就是“在飓风、暴雨且视线受阻的夜晚，仅凭指尖的触觉驾驶一辆时速百公里的卡车穿过狭窄的胡同”。前者处理的是一维的时间序列信号，而后者处理的是四维时空（三维空间+时间）中多模态、高冲突、强非线性的海量数据。

自动驾驶系统（ADS）的核心决策依赖于对自车状态（位置、速度、航向角）和环境状态（障碍物位置、轨迹、道路拓扑）的精准估计。然而，现实世界充满了不确定性：

• GPS在隧道里会漂移；

• 摄像头在强光下会致盲；

• 激光雷达在雾霾中会“致幻”；

• 毫米波雷达无法分辨静止的金属井盖和行驶的汽车。

滤波算法在自动驾驶中的角色，不仅仅是“去噪”，更是“数据炼金术”——它必须从这些不可靠、不完整、甚至相互矛盾的传感器读数（Observations）中，提炼出唯一可靠的“真相”（Truth）。本部分将深入解析支撑自动驾驶感知与定位的滤波理论基石，以及多传感器融合的系统架构。

二、自动驾驶中的状态空间模型：从物理量到概率分布

滤波的本质是对动态系统的状态进行估计。在自动驾驶中，我们首先需要定义什么是“状态”。

2.1 自车状态向量（Ego Vehicle State）

这是滤波算法最直接的目标。一个典型的自动驾驶车辆状态向量 X包含：

• 位置：(x,y,z)—— 全球坐标系或局部坐标系下的三维坐标。

• 姿态：(roll,pitch,yaw)—— 横滚、俯仰、偏航角（欧拉角）或四元数（Quaternion）。

• 速度：(vx​,vy​,vz​)—— 三轴速度。

• 角速度：(ωx​,ωy​,ωz​)—— 绕三轴的旋转速率（通常由陀螺仪提供）。

• 加速度：(ax​,ay​,az​)—— 三轴线加速度（通常由加速度计提供）。

2.2 环境状态向量（Environmental State）

除了自车，我们还需要估计周围世界的状态。这通常是一个高维向量：

• 动态障碍物：每个障碍物（车辆、行人）的位置、速度、加速度、尺寸、朝向。

• 静态地图：车道线曲率、交通标志牌位置、红绿灯状态。

2.3 运动学模型（Kinematic Models）与动力学模型（Dynamic Models）

为了预测下一时刻的状态，我们需要建立系统模型。

• 自行车模型（Bicycle Model）：自动驾驶最常用的简化模型。假设车辆是刚体，左右轮合并，忽略悬架和轮胎变形。适用于低速场景（泊车、城市巡航）。

• 阿克曼转向模型（Ackerman Steering Model）：考虑转向几何，适用于高速场景。

• 粒子滤波专用模型：对于复杂的非线性运动（如行人随意走动），无法用固定公式描述，只能通过采样（Sampling）来模拟运动。

三、传感器噪声模型：滤波算法的“敌情侦察”

不理解噪声，就无法设计滤波器。自动驾驶传感器的噪声特性千差万别，这决定了融合策略的选择。

3.1 惯性测量单元（IMU）：高频但有偏

IMU（加速度计+陀螺仪）是自动驾驶的“内耳”，提供高频（100Hz-1000Hz）的状态更新。

• 噪声类型：

  • 白噪声（White Noise）：随机抖动，随时间平均会消失。

  • 随机游走（Random Walk）：零偏（Bias）的不稳定性。这是IMU最大的敌人。即使车辆静止，积分出来的速度和位置也会随时间发散（Drift）。

• 滤波策略：IMU不能作为唯一的定位源，必须与绝对定位源（如GPS、视觉）进行紧耦合（Tight Coupling）。

3.2 全球导航卫星系统（GNSS）：绝对但跳动

GPS/北斗提供全球绝对坐标，但存在多径效应（信号反射）和遮挡问题。

• 噪声特性：非高斯、非平稳。在城市峡谷（Urban Canyon）中，噪声分布呈现多峰特性（Multimodal）——信号可能来自卫星直射，也可能来自高楼反射。

• 滤波策略：不能使用简单的卡尔曼滤波（假设高斯），需要使用鲁棒滤波或因子图优化（Factor Graph Optimization）。

3.3 激光雷达（LiDAR）：精准但稀疏

LiDAR通过发射激光脉冲测量距离，生成点云（Point Cloud）。

• 噪声特性：测距误差通常服从高斯分布（厘米级），但点云密度随距离平方衰减。此外，运动畸变（Motion Distortion）是主要误差源——在扫描一帧（100ms）的过程中，车辆移动导致点云扭曲。

• 滤波策略：需要对点云进行运动补偿（Motion Compensation），通常使用IMU数据进行去畸变。

3.4 摄像头（Camera）：丰富但脆弱

视觉提供纹理和语义信息（车道线、交通灯），但对光照极度敏感。

• 噪声特性：像素噪声、镜头畸变、运动模糊。更重要的是，视觉定位依赖于重投影误差（Reprojection Error），这是一个高度非线性的函数。

• 滤波策略：必须使用扩展卡尔曼滤波（EKF）或更先进的滑窗优化（Sliding Window Optimization）。

四、自动驾驶滤波的核心挑战

相比于通信或机器人领域，自动驾驶对滤波算法提出了近乎苛刻的要求：

挑战维度

具体要求

算法应对

五、滤波算法的分类体系在自动驾驶中的应用

针对不同的场景，自动驾驶系统会构建一个滤波器层级架构（Hierarchical Filtering Architecture）。

5.1 底层：IMU预积分滤波（高频）

• 算法：误差状态卡尔曼滤波（ES-EKF, Error State EKF）。

• 作用：融合IMU的高频数据，预测车辆的短时运动状态。它不直接估计位置，而是估计相对于上一时刻的误差状态（Error State），这样可以避免大角度旋转带来的奇异性问题。

5.2 中层：多传感器松耦合（中频）

• 算法：标准卡尔曼滤波（KF）或无迹卡尔曼滤波（UKF）。

• 作用：将GPS的位置、轮速计（Wheel Odometry）的速度、视觉的相对位移，作为独立的观测值，更新ES-EKF的状态。

• 特点：各个传感器独立工作，互不干扰，容错性好，但精度略低。

5.3 顶层：多传感器紧耦合（低频）

• 算法：因子图优化（Factor Graph Optimization）或UKF。

• 作用：将原始传感器数据（如视觉特征点、LiDAR点云、GPS伪距）直接作为观测，与状态变量建立几何约束。

• 特点：精度最高，但计算量巨大，且一旦一个传感器失效，可能导致整个系统崩溃。

5.4 对象级：目标跟踪滤波

• 算法：交互多模型（IMM, Interacting Multiple Models）。

• 作用：估计周围车辆和行人的轨迹。由于目标运动模式不确定（直行、转弯、停车），IMM会并行运行多个模型（匀速、匀加速、转弯），并根据匹配度加权融合。

六、时间同步与空间标定：滤波的物理基础

滤波算法生效的前提是数据的一致性。

• 时间同步（Time Synchronization）：GPS秒脉冲（PPS）信号必须同步所有传感器的时钟。如果LiDAR比IMU慢了10ms，融合出来的位置就会偏差几十厘米。

• 空间标定（Spatial Calibration）：必须精确测量传感器之间的外参（Extrinsic Parameters）——即摄像头相对于LiDAR旋转了多少度，平移了多少米。标定的误差会直接导致融合算法的发散。

七、案例研究：特斯拉的“纯视觉”滤波架构

特斯拉（Tesla）抛弃了激光雷达和毫米波雷达，仅依靠摄像头进行自动驾驶。这对其滤波算法提出了极致挑战。

八、安全与冗余：双脑架构

为了通过ISO 26262功能安全认证，自动驾驶系统通常采用双冗余架构：

• 主域控制器：高性能SoC（如NVIDIA Orin/Xavier），运行复杂的紧耦合滤波和优化算法。

• 安全微控制器（MCU）：低功耗单片机（如Infineon Aurix），运行简化的、经过形式化验证（Formal Verification）的卡尔曼滤波算法。一旦主域控制器死机，MCU能接管车辆并执行最小风险策略（MRM，如靠边停车）。

九、总结与展望

自动驾驶滤波算法是一个多学科交叉的复杂系统，它融合了经典控制理论、概率论、计算机视觉和实时操作系统。在这一部分中，我们建立了自动驾驶滤波的理论框架，明确了传感器噪声模型，并概述了分层融合架构。

然而，理论框架只是第一步。在实际应用中，最基础、最核心、也是最早被使用的滤波工具是什么？毫无疑问是卡尔曼滤波（Kalman Filter）及其变种。虽然它诞生于上世纪60年代，但在今天的自动驾驶系统中，它依然是支撑定位与感知的脊梁。

在下一部分中，我们将深入卡尔曼滤波家族的演进。从标准卡尔曼滤波（KF）到扩展卡尔曼滤波（EKF），再到无迹卡尔曼滤波（UKF）和误差状态卡尔曼滤波（ES-EKF）。我们将揭示为什么EKF是自动驾驶定位的“标配”，以及UKF在处理强非线性时的独特优势。