传感器融合与车辆定位

引言

自动驾驶的规划模块需要两个输入：周围有什么（感知），以及我在哪（定位）。前者由 ADAS 感知与传感器融合 讲透——各传感器的物理原理、BEV 表征、检测与跟踪网络；后者是本文的重点。感知与定位在工程上常常被放在同一个模块里实现，但它们的数学工具与失败模式完全不同：感知关心「检测率与误检率」，定位关心「误差协方差与完好性」。

定位精度的要求随自动化等级陡增。L2 的横向控制只需要车道级精度（约 1 米），因为它靠车道线闭环；L3 以上要判断「我在哪条车道、离路口多远」，需要分米级；L4 的城区自动驾驶与自动泊车要求 10 到 30 厘米，且必须有「定位失效检测」——因为定位错了 1 米，车可能就压上了路缘或冲进对向车道。

定位的难点在于「没有任何单一传感器能同时满足精度、频率、可用性与成本」。GNSS 提供绝对位置但频率低（10 Hz）且城市峡谷里会被遮挡和多路径污染；IMU 频率高（100 到 1000 Hz）但会漂移；摄像头能识别车道线但受光照影响；激光雷达能匹配高精地图但雨天衰减。所以定位本质上是多源融合的状态估计问题：用高频但漂移的惯性递推，用低频但绝对的观测校正。

本文按「传感器互补 → 融合层级 → 状态估计 → 数据关联 → 同步标定 → GNSS → 组合导航 → 地图匹配」的顺序展开，聚焦估计理论与工程实现，与感知篇形成互补。

目录

1. 定位在自动驾驶中的位置与精度要求
2. 传感器特性与互补性
3. 融合的三个层级
4. 状态估计基础：KF、EKF、UKF
5. 数据关联
6. 时间同步与空间标定
7. GNSS 定位：从单点到 RTK
8. IMU 组合导航与失锁递推
9. 高精地图匹配与定位完好性

1. 定位在自动驾驶中的位置与精度要求

不同功能对定位精度的要求差一个数量级，这决定了传感器配置：

精度要求分级：
  L2 高速巡航（车道保持）
    横向精度：≤ 1 m
    依据：车道线闭环，绝对定位只用于地图匹配与导航
    配置：GNSS 单点 + 轮速 + 摄像头

  L3 高速领航（自动变道、匝道）
    横向精度：≤ 0.3 m
    纵向精度：≤ 1 m（判断是否在匝道口）
    配置：GNSS RTK + IMU + 摄像头 + 高精地图

  L4 城区 / 泊车
    横向精度：≤ 0.1 m
    纵向精度：≤ 0.2 m
    附加要求：完好性（必须能检测定位失效）
    配置：RTK + 高精度 IMU + 激光雷达 + 地图匹配

两个关键概念贯穿全文。**精度（accuracy）**是误差的大小，**完好性（integrity）**是「误差超限时能否被检测到」。航空领域早就认识到：一个「误差 5 米但报告误差 0.2 米」的定位系统比「误差 1 米且诚实报告 1 米」的系统危险得多，因为前者会让下游模块做出错误决策。自动驾驶的定位模块必须同时输出「位置估计」和「该估计的可信度（协方差 + 保护级）」。

2. 传感器特性与互补性

从状态估计的视角看，每个传感器观测的是车辆状态向量的不同分量：

互补性可以概括成一句话：惯性传感器给高频但漂移的递推，绝对观测给低频但不漂移的校正。融合的本质就是在两者之间做最优加权——卡尔曼滤波正是这个「最优加权」的数学形式，权重由各自的协方差决定。

一个常被忽略的观测源是车辆自身的信号：方向盘转角、挡位、制动状态、雨刷状态。方向盘转角与轮速结合可以估计横摆角速度（自行车模型），在 IMU 短暂异常时作为冗余；雨刷开启信号则可用于降低摄像头观测的权重（雨天视觉退化）。这类「车辆总线信号」通过 CAN 就能拿到，是低成本高价值的融合输入。

3. 融合的三个层级

融合按「在哪一层合并」分为三类，这是选型的第一决策点：

后融合是量产的主流，原因很实际：摄像头、雷达、激光雷达往往来自不同供应商，各自输出目标列表，主机厂在应用层做关联与融合，供应商的模块可以独立替换与认证。后融合的代价是「关联」成了瓶颈——两个传感器对同一个物体的位置估计不同，如何判断它们是同一个目标，是第 5 节要解决的问题。

前融合（BEV）是前沿方向，它把多路摄像头与激光雷达投影到统一的鸟瞰图空间，用神经网络直接输出融合后的目标与可行驶区域。它的优势是避免了后融合的信息损失（例如摄像头知道「这是辆卡车」，雷达知道「它在以 80 km/h 接近」，后融合只能取其一或简单拼接，前融合可以让网络自己学如何利用）。代价是时空对齐要求极高——10 ms 的时间偏差在 120 km/h 下就是 33 cm 的位置偏差。

工程上的常见做法是分层混合：底层的时序融合用滤波器（估计自车运动），顶层的多传感器融合用 BEV 网络，两者通过坐标变换衔接。

4. 状态估计基础：KF、EKF、UKF

所有贝叶斯滤波器都是同一个两步循环：预测（用运动模型外推） 与 更新（用观测校正）。

卡尔曼滤波（线性高斯系统）：
  预测：
    x̂⁻ = F·x̂           状态外推（运动模型）
    P⁻ = F·P·Fᵀ + Q     协方差外推（Q 为过程噪声）
  更新：
    K  = P⁻·Hᵀ·(H·P⁻·Hᵀ + R)⁻¹   卡尔曼增益
    x̂  = x̂⁻ + K·(z - H·x̂⁻)      状态校正（z 为观测）
    P  = (I - K·H)·P⁻            协方差收缩

  Q 大 → 更信任观测；R 大 → 更信任预测
  卡尔曼增益 K 就是「最优加权」的体现

现实系统是非线性的（车辆运动、雷达极坐标观测），于是有三条路：

• EKF（扩展卡尔曼滤波）：在工作点做一阶泰勒展开，用雅可比矩阵代替 F 与 H。计算量小、工程成熟，是车载定位的默认选择。缺点是线性化误差在强非线性场景（大角度机动、极坐标观测）下会导致发散。
• UKF（无迹卡尔曼滤波）：不做线性化，而是取一组 sigma 点，通过非线性函数传播后重建均值与协方差。对非线性系统精度更高（二阶），代价是计算量约 2n+1 次函数求值。
• 粒子滤波（PF）：用一组带权样本表示任意分布，能处理多峰与非高斯，但计算量大，多用于全局定位初始化（在候选位置中收敛）。

定位场景的状态向量通常是 15 维或 16 维：

# 组合导航的状态向量（15 维，误差状态形式）
# 位置误差(3) 速度误差(3) 姿态误差(3) 陀螺零偏(3) 加计零偏(3)
import numpy as np

class Eskf:
    def __init__(self):
        self.x = np.zeros(15)          # 误差状态
        self.P = np.eye(15) * 1.0      # 初始协方差

    def predict(self, imu, dt):
        # F: 误差状态转移（含地球自转与哥氏项，简化省略）
        F = np.eye(15) + self._F_cont(imu) * dt
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + self.Q(dt)   # Q 与 IMU 噪声密度相关

    def update_gnss(self, z_pos, R_pos):
        H = np.zeros((3, 15)); H[0:3, 0:3] = np.eye(3)   # 直接观测位置
        y = z_pos - self._predicted_pos()                # 新息
        S = H @ self.P @ H.T + R_pos                     # 新息协方差
        K = self.P @ H.T @ np.linalg.inv(S)              # 卡尔曼增益
        self.x = self.x + K @ y
        self.P = (np.eye(15) - K @ H) @ self.P

实践中最关键的不是滤波器的形式，而是 Q 与 R 的标定。Q 反映「运动模型有多不可信」（由 IMU 噪声密度与运动模型误差决定），R 反映「观测有多不可信」（由 GNSS 精度因子与多路径程度决定）。R 设得过于乐观会让定位被错误观测带偏，设得过于保守则无法及时纠正漂移。工程上常用新息序列（innovation）的一致性检验来调参：如果新息均值不为零或方差与理论不符，说明 Q 或 R 与实际不符。

5. 数据关联

数据关联解决「这帧检测到的目标，对应上一帧的哪条轨迹 / 另一个传感器的哪个目标」。它是后融合与目标跟踪的核心，也是最容易在密集场景翻车的环节：

关联流程：
  1. 门限（Gating）：用马氏距离筛掉不可能的配对
     d² = (z - ẑ)ᵀ · S⁻¹ · (z - ẑ)   （S 为新息协方差）
     d² > χ² 阈值（如 3 自由度 95% 对应 7.81）→ 排除
  2. 代价矩阵：每对「观测 - 轨迹」计算代价（马氏距离、IoU、外观）
  3. 分配：求使总代价最小的匹配

  常见算法：
    最近邻（NN）    ：贪心，最快，密集场景易错
    全局最近邻（GNN）：匈牙利算法求全局最优，常用
    JPDA            ：按概率加权所有可能配对，抗杂波
    MHT             ：维护多假设树，延迟决策，最强但最重

关联的三个典型难题：密集目标（相邻车道的两辆车距离小于门限，会互相抢检测）、遮挡（目标被遮挡几帧后重现，ID 可能跳变，需要轨迹预测撑过遮挡期）、传感器时间不同步（不同传感器的观测时刻不同，关联前必须插值到同一时刻）。

一个实用技巧是用运动一致性而非纯几何距离做二次校验：如果两个目标的位置接近但速度差异很大（如一辆静止车与一辆 60 km/h 的车），它们不应被关联。把速度、航向、类别都放进代价函数，能显著降低 ID 跳变。

6. 时间同步与空间标定

融合的精度上限由时空对齐的精度决定，这一步做不好，后面的滤波再精细也没用。

时间同步三层：
  1. 硬件触发
     一个同步信号同时触发所有摄像头曝光
     精度：微秒级，消除曝光抖动
  2. 时间戳（gPTP，IEEE 802.1AS）
     每个传感器数据包打上全局时基的时间戳
     精度：< 1 us，依赖全车 TSN 配置
  3. 软件插值
     把不同时刻的观测投影到统一时刻
     用 IMU 积分做运动补偿

  误差量级（120 km/h = 33 m/s）：
    1 ms 偏差 → 3.3 cm
    10 ms 偏差 → 33 cm   ← 已超过城区定位的精度要求

时间同步的底层协议与配置见 CAN FD 与车载以太网 里的 TSN 与 gPTP 部分——融合对时基的精度要求（微秒级）比大多数车载应用都高。

空间标定分内参与外参。内参（焦距、主点、畸变系数）在产线用标定板确定，一般不变；外参（传感器之间的相对位姿）会随振动、温度、装配应力漂移，所以需要在线标定：行驶中根据道路特征（车道线平行性、地面平面性、路面纹理）估计外参的漂移量并持续修正。

外参误差对定位的影响随距离放大：0.1° 的角度误差在 50 米外就是 8.7 cm 的位置误差，在 200 米外是 35 cm。所以远距离目标的融合对标定精度极其敏感，这也是为什么高精地图匹配要强调「近处特征定横向、远处特征定纵向」。

7. GNSS 定位：从单点到 RTK

GNSS 的定位原理是「用多颗卫星的测距交会」，精度取决于测距误差：

定位模式与精度：
  单点定位（SPP）
    仅用伪距，误差源：电离层、对流层、卫星钟差、多路径
    精度：水平 1~3 m（多星座可到 1 m 内）

  差分定位（DGPS）
    用已知位置的基准站广播修正量
    精度：亚米级（0.5~1 m）

  RTK（实时动态差分）
    用载波相位（波长 19 cm 的 L1）做双差
    精度：水平 1~3 cm（固定解 Fixed）
    需要：基准站 + 通信链路（4G / 电台）
    收敛：几秒到几十秒；固定解丢失后要重新收敛

  PPP / PPP-RTK
    用精密星历与钟差产品，单站即可
    PPP 收敛慢（分钟级），PPP-RTK 借助区域改正数快得多
    精度：厘米级，适合无基准站覆盖的区域

工程上的关键是理解解状态（solution status）。RTK 输出会标注「固定解（Fixed）/ 浮点解（Float）/ 单点解（Single）」，精度依次劣化。融合模块必须把解状态映射成观测噪声 R：固定解 R 小（厘米级）、浮点解 R 大（分米级）、单点解更大（米级）。把浮点解当固定解用，会让定位在不知不觉中偏离几米。

城市峡谷是 GNSS 的噩梦：高楼反射造成多路径（信号走了更长路径，测距偏大）、遮挡导致可见卫星不足（几何精度因子 GDOP 变差）、非视距（NLOS）信号被当作直射信号。缓解手段包括多频（L1 + L5 可部分消除电离层与多路径）、双天线（测航向）、以及与其他传感器的紧耦合。多星座（GPS + 北斗 + Galileo + GLONASS）能把可见卫星数翻倍，是低成本的改善手段。

V2X 场景对定位有额外要求（消息必须携带精确位置，否则碰撞判断会误判），V2X 车路协同与车联网通信 里提到协同驾驶要求亚米级精度，也是靠 RTK 实现的。

8. IMU 组合导航与失锁递推

IMU 是组合导航的「骨架」，它提供高频的运动信息，让定位在 GNSS 失锁期间仍能递推：

IMU 分级：
  MEMS（消费级）：零偏稳定性 1~10 °/h，价格几十元
  MEMS（车规）  ：零偏稳定性 0.5~2 °/h，价格几百元
  战术级        ：零偏稳定性 0.01~0.1 °/h，价格上万
  导航级        ：< 0.001 °/h，航空用

  漂移估算（GNSS 失锁后纯惯性递推）：
    陀螺零偏 1 °/h → 60 秒后航向误差 1°，100 米外横向误差约 1.7 m
    陀螺零偏 10 °/h → 60 秒后航向误差 10°，横向误差 17 m（不可用）

组合导航按耦合深度分三级：

松耦合是量产主流（GNSS 模块与 IMU 模块各自独立，EKF 在应用层融合），紧耦合在卫星可见数经常不足的场景（城市峡谷、林荫道）能多撑几颗星，深耦合则是军用级方案。

失锁期间的辅助手段决定可用性：

• 零速修正（ZUPT）：检测到车辆静止（轮速为零且加速度平稳）时，把速度观测量设为零，直接抑制 IMU 的速度漂移。等红灯、堵车时非常有效。
• 非完整性约束（NHC）：车辆正常行驶时侧向与垂向速度近似为零（不漂移、不弹跳），可作为一个虚拟观测。这个约束在 GNSS 失锁期间能显著约束横向漂移。
• 轮速里程计：轮速计提供纵向位移，与 IMU 的航向结合可做航位推算（DR）。打滑时轮速失真，要靠 IMU 的加速度交叉校验。

一次典型的隧道场景：进入隧道 GNSS 失锁，IMU + 轮速 + NHC 递推，几十秒内位置漂移控制在米级；出隧道后 GNSS 重新捕获，EKF 用新观测校正，若漂移过大（超过门限）需要重新初始化而不是硬拉回来——强行校正会导致滤波器发散。

9. 高精地图匹配与定位完好性

高精地图（HD Map）是 L3+ 定位的「绝对参照物」，它把定位从「相对运动估计」变成「已知环境中的定位」：

高精地图的定位价值：
  1. 提供车道级几何（车道中心线、边界、曲率）
  2. 提供语义先验（停止线、路牌、杆状物、坡道）
  3. 提供约束（"车辆应在车道内" 作为虚拟观测）

匹配方法：
  点到线匹配（ICP 变体）
    把激光雷达点云或视觉特征与地图车道线/路缘匹配
    最小化点到线的距离和
  正态分布变换（NDT）
    把点云与地图都表示为概率分布，匹配分布
    对初始误差容忍度高于 ICP
  特征匹配
    用杆状物、路牌等稳定地标的相对位置反算自车位姿
    纵向定位主要靠它（车道线只能约束横向）

横向与纵向的约束来源不同，这是高精地图定位的实用认知：车道线匹配给出很强的横向约束（车离车道中心多远），但对纵向位置几乎无约束（沿车道方向移动，车道线看起来一样）；纵向位置要靠路标、杆状物、坡度变化、甚至路面纹理来锚定。很多「定位在纵向上漂移」的问题，根因就是只做了车道线匹配。

多源融合的两种实现范式：

• 滤波（EKF/ESKF）：递推更新，适合实时、增量式，但只保留当前状态与协方差，无法回环优化。量产主流。
• 因子图优化（Factor Graph）：把各传感器的观测建模为因子，用非线性最小二乘批量求解。能处理回环、延迟观测、全局一致性，是建图与离线高精定位的主流。近年随着算力提升，也用于在线定位（滑动窗口优化）。

最后是定位完好性，它把定位与功能安全连起来：

完好性概念：
  保护级（Protection Level, PL）
    在当前卫星几何与观测质量下，位置误差不会超过的界（给定置信度）
    HPL（水平）/ VPL（垂直）
  告警限（Alert Limit, AL）
    应用能容忍的最大误差（如车道保持要求横向 < 1 m）
  判据：PL < AL 才认为定位「可用」

  故障检测与排除（FDE）：
    RAIM（接收机自主完好性监测）
      用冗余卫星做一致性检验（残差卡方检验）
      检测到故障卫星后尝试排除
    多传感器交叉检验
      GNSS 与地图匹配结果差异过大 → 触发降级

定位模块的输出不应只是「位置」，而应是「位置 + 协方差 + 保护级 + 可用性标志」。规划模块据此决定是否允许自动变道、是否降级到人工接管。这正是 SOTIF 在定位上的落地方式——不是「不出错」，而是「出错时知道自己可能出错」。相关的方法论见 功能安全 ISO 26262 工程实践 中关于 SOTIF 与场景边界的部分。

权衡取舍

常见坑清单

1. 把浮点解当固定解：RTK 解状态未映射到观测噪声，定位悄悄偏离几米，必须按解状态分级设 R。
2. 时间戳不同源：各传感器用各自时钟打戳，10 ms 偏差即 33 cm 误差，必须统一到 gPTP 时基。
3. 外参不标定漂移：振动与温度导致外参变化，远距离目标偏差放大，需在线标定。
4. 只做车道线匹配：纵向位置无约束导致沿路漂移，必须引入地标做纵向锚定。
5. 关联门限写死：密集场景下门限过大门限过小都出错，需按协方差自适应。
6. Q/R 拍脑袋：过程噪声与观测噪声与实际不符导致滤波器发散或迟钝，需用新息一致性检验。
7. IMU 零偏未估计：把零偏当噪声，姿态缓慢漂移，必须把零偏纳入状态向量。
8. 失锁后强行拉回：隧道出来用大偏差观测硬校正导致发散，应做重新初始化。
9. 无完好性输出：只给位置不给协方差与保护级，下游无法判断可用性。
10. 忽略车辆信号冗余：方向盘转角、轮速等免费观测未利用，浪费了低成本冗余。

小结

传感器融合与车辆定位的本质是「用多源观测估计一个不可直接测量的状态」。三条主线贯穿全文：观测互补（惯性的高频递推 + 绝对源的低频校正）、时空对齐（时间同步到微秒、外参在线标定）、以及可信度管理（协方差、保护级、完好性）。

工程上的优先级建议是：先把时间同步与标定做扎实（这是精度上限），再调滤波器的 Q/R（这决定实际表现），然后处理数据关联（这决定密集场景的鲁棒性），最后补完好性（这决定能否通过安全论证）。很多团队反过来做——先钻研算法，最后才发现瓶颈在标定与同步。

继续深入：感知侧的传感器原理与 BEV 表征见 ADAS 感知与传感器融合 ；时基与网络同步见 CAN FD 与车载以太网 ；V2X 场景下的定位要求见 V2X 车路协同与车联网通信 ；完好性与 SOTIF 的方法论见 功能安全 ISO 26262 工程实践 。