📌 概述
定位（Localization）是自动驾驶系统中连接感知与规划的桥梁。没有准确的定位，车辆无从知道自己"在哪里"，也就无法执行路径规划与控制——这也是为什么业界常将定位称为自动驾驶的"骨架"。
本讲覆盖多传感器融合定位的核心技术栈：从 GNSS/RTK 的厘米级定位原理、IMU 与车轮里程计的航位推算、卡尔曼滤波的预测-更新闭环，到松耦合/紧耦合融合方案，以及基于 HD 地图的定位（车道匹配、点云匹配、NDT / ICP）。最后，我们将讨论端到端 VLA（Vision-Language-Action） 方案中为什么可以绕过传统定位模块，以及这给系统带来的取舍。
🎯 核心概念
GNSS 与 RTK
GNSS（Global Navigation Satellite System）：包括 GPS（美国）、BeiDou（中国）、GLONASS（俄罗斯）、Galileo（欧盟）。基本原理是通过测量卫星信号到达接收机的时间计算伪距（pseudorange），解算四颗以上卫星即可得到三维位置。
误差来源：
- 电离层/对流层延迟
- 卫星轨道与钟差
- 多径效应（城市峡谷中最严重）
- 接收机噪声
RTK（Real-Time Kinematic）：通过地面基准站（Base Station）将自身已知位置的观测值与接收机的观测值做差分，消除电离层、卫星钟差等共同误差。RTK 可将定位精度从米级提升到厘米级（2-5cm），但依赖基站与流动站之间的通信链路（通常为 4G/5G）。
局限：城市峡谷、隧道、地库中 GNSS 信号衰减或丢失，单靠 GNSS 无法满足连续性要求。
IMU（Inertial Measurement Unit）
IMU 包含三轴加速度计与三轴陀螺仪，测量比力（specific force）与角速度。
航位推算（Dead Reckoning）：通过对角速度积分得到姿态，去除重力后对加速度双重积分得到位移。
$$ v_{t+1} = v_t + a_t \cdot \Delta t $$$$ p_{t+1} = p_t + v_{t+1} \cdot \Delta t + \frac{1}{2} a_t \cdot \Delta t^2 $$误差特性：
- 高频响应好（~100 Hz）
- 短时精度高
- 存在积分漂移（bias drift），长时间误差累积严重
IMU 按精度分为消费级（BMI088）、工业级（ADIS）、导航级（LN-200）、战略级（光纤陀螺），价格从几十元到数十万元不等。
车轮里程计（Wheel Odometry）
利用轮速传感器测量各轮旋转角度，结合车辆运动学模型推算位置增量：
- 差速模型：两轮速度差计算转角
- 阿克曼模型：前轮转向角 + 后轮速度推算轨迹
- 优点：不依赖外部信号，短时稳定
- 缺点：受轮胎打滑、路面坡度影响大
🔧 技术详解
卡尔曼滤波（Kalman Filter, KF）
卡尔曼滤波是多传感器融合定位的最核心数学工具，融合预测模型（先验）与观测模型（后验），以递归方式估计系统状态。
预测步骤（Predict）
$$ \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k $$$$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$- $F_k$：状态转移矩阵（如运动学模型）
- $B_k$：控制输入矩阵
- $Q_k$：过程噪声协方差
更新步骤（Update）
$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) $$$$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$- $H_k$：观测矩阵
- $R_k$：观测噪声协方差
- $K_k$：卡尔曼增益，控制模型预测与传感器观测之间的信任权重
扩展：EKF 与 UKF
当系统为非线性时（如 IMU 的姿态更新），使用 EKF（Extended Kalman Filter） 通过一阶泰勒展开线性化；UKF（Unscented Kalman Filter） 使用 sigma 点传播，精度更高但计算稍大。
Error-State KF（ESKF）
在实际 IMU 融合中，更常用的是 ESKF，将状态分解为名义状态（nominal state，由 IMU 积分直接得到）和误差状态（error state，用小量表示），KF 只估计误差状态。优点是误差状态接近线性，且小量表示避免了欧拉角的万向锁问题。
多传感器融合方案
松耦合（Loosely Coupled）
各传感器独立解算位姿，然后在状态估计层融合。例如 GNSS 输出位置和速度作为观测 $z_k$，IMU 做预测。
优点：模块化好，任一个传感器故障不影响其他 缺点：丢弃了传感器原始观测中的丰富信息，融合精度有限
紧耦合（Tightly Coupled）
直接使用传感器原始观测值。GNSS 直接使用伪距、伪距率；IMU 与 GNSS 原始观测量在滤波器中联合处理。
优点：
- 卫星少于 4 颗时仍可利用部分观测约束
- 精度优于松耦合 30-50%（尤其在城市峡谷场景）
- 更有利于 detect 和处理 GNSS 异常（多径、周跳）
缺点：计算复杂度高，系统复杂度大，调试困难
HD 地图定位
车道匹配
利用视觉或 LiDAR 检测到的车道线/路沿特征，与 HD 地图中的车道模型匹配，修正横向位置。典型方法包括：
- 粒子滤波：在横向方向采样粒子，根据匹配似然度加权
- ICP 匹配：将提取的特征点与地图车道中心线 ICP 配准
点云匹配定位（LiDAR Localization）
使用 LiDAR 的实时点云与预建的点云地图进行配准：
ICP（Iterative Closest Point）：
- 寻找最近邻对应点对
- 求解刚体变换最小化点对距离
- 迭代至收敛
- 缺点：需要初始位姿接近，对噪声敏感
NDT（Normal Distributions Transform）：
- 将参考点云划分为体素网格，每个体素用高斯分布 $N(\mu, \Sigma)$ 表示
- 评价函数为源点云落在目标体素中的概率密度之和
- 比 ICP 更鲁棒，收敛域更大，是工业界的主流选择
NDT 扫描匹配流程：
- 加载预建点云地图，构建 NDT 体素网格
- 实时扫描点云作为源点云
- 以待匹配位姿（IMU/里程计预测）为初始值
- 使用 Newton 优化法最大化配准得分
- 输出修正后的位姿
📊 方法对比：融合方案
| 方案 | 精度（RMS） | 计算量 | 鲁棒性 | GNSS 依赖 |
|---|---|---|---|---|
| GNSS + 航位推算（松耦合） | 0.3-1.0 m | 低 | 中 | 强 |
| GNSS-RTK + IMU（紧耦合） | 0.05-0.15 m | 中 | 高 | 中 |
| LiDAR + NDT 定位 | 0.05-0.20 m | 中 | 极高 | 无 |
| HD 地图 + 车道匹配 | 0.10-0.30 m | 低 | 中 | 无 |
| 视觉定位（VIO） | 0.5-2% 轨迹 | 中 | 中 | 无 |
| 多源紧耦合融合 | 0.03-0.10 m | 高 | 极高 | 弱 |
🔗 与自动驾驶的关联
定位在传统自动驾驶架构中的角色
定位为预测模块提供自车轨迹的参考系，为规划模块提供目标位置的基准。没有准确定位，行为预测（motion prediction）和路径规划的结果都将失去物理意义。
VLA/端到端方案中的定位
近年来以 UniAD、VAD、DriveMLM 等为代表的端到端方案中，部分工作选择跳过显式定位模块，以图像序列 + 车辆位姿（直接来自 CAN 总线）作为输入，网络隐式学习时空对应关系。
为什么可以跳过？
- 端到端网络直接将感知特征映射到 action space，不需要"定位-规划"的显式接口
- 大模型（VLM/LLM）的"场景理解"能力可以在上下文层面替代部分定位需求
- 训练数据中的位姿标注来自 ground truth，不存在累积漂移
取舍：
- 优点：系统简化无需维护定位模块，减少了级联误差
- 缺点：缺乏可解释性和可干预性；遇到地图未覆盖区域时，纯端到端模型可能产生灾难性行为
- 收敛：定位仍将是量产 L3/L4 系统的必备组件，端到端方案更多是作为决策层补充而非完全替代传感器融合定位
📚 延伸阅读
- Welch, G. & Bishop, G. “An Introduction to the Kalman Filter.” SIGGRAPH Course, 2001.
- Soloviev, A. “Tight Coupling of GPS and INS for Urban Navigation.” IEEE Trans. on AES, 2010.
- Biber, P. & Straßer, W. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” IROS, 2003.
- Levinson, J. & Thrun, S. “Robust Vehicle Localization in Urban Environments Using Probabilistic Maps.” ICRA, 2010.
- Qin, T. et al. “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.” IEEE Trans. on Robotics, 2018.
- Hu, Y. et al. “Planning-oriented Autonomous Driving.” (UniAD) CVPR, 2023.