简介

在自动驾驶系统中，定位（ Localization ）是核心基础能力之一。它的任务非常明确：

确定车辆在高精度地图中的精确位置——精确到厘米级。

这不仅是“导航”，更是安全行驶的前提。如果车辆不知道自己确切的位置，就无法正确规划路径，也无法判断是否偏离车道，甚至可能导致严重事故。

为什么普通GPS不够用？

我们日常使用手机导航时，依赖的是 GPS（全球定位系统），它能提供大约 1~3米 的定位精度。但在自动驾驶场景下，这一精度远远不够。

GPS的局限性：

• 精度不足：3米误差意味着可能跨过车道线，对自动驾驶来说不可接受；
• 信号遮挡：在城市高楼间（“城市峡谷”）、隧道、地下停车场或山区，GPS信号会减弱甚至丢失；
• 多路径效应：信号经建筑物反射后产生延迟，导致定位漂移。

结论：仅靠GPS无法满足L3及以上自动驾驶的定位需求。

如何实现高精度定位？核心思路

既然GPS不够准，那就需要结合其他传感器与高精度地图，通过“感知 + 匹配 + 融合”的方式，实现厘米级定位。

核心方法：

将车辆传感器所“看到”的环境特征（如电线杆、树木、建筑边缘）与高精度地图中已知的地标位置进行比对，从而反推出车辆当前的精确坐标。

这个过程就像：

“我在一个熟悉的街区，看到前方是某家咖啡馆、左边是邮局——那我现在应该站在街角第三棵树的位置。”

坐标系的理解：车辆坐标系 vs 地图坐标系

要完成上述匹配，必须理解不同坐标系之间的关系。

1. 车辆坐标系（Body Frame）

• 原点在车辆某参考点（如后轴中心）
• X轴指向车辆前进方向，Y轴向左，Z轴向上
• 随车辆移动和转向而实时变化
• 所有传感器（如激光雷达、摄像头）最初都在此坐标系下输出数据

2. 地图坐标系（Map Frame / World Frame）

• 固定不变的全局坐标系（如UTM或ECEF）
• 高精度地图基于此坐标系构建
• 所有车辆最终都需要将自己的位置转换到该坐标系中

关键挑战：坐标变换

由于车辆在运动中不断转弯、加速、颠簸，其自身坐标系始终在变化。因此，必须将传感器观测到的地标位置，从车辆坐标系转换到地图坐标系，才能与地图数据匹配。

这一过程称为 坐标变换（Transformation），是定位算法的核心步骤之一。常用数学工具包括：旋转矩阵、四元数、齐次变换等。

常见的自动驾驶定位技术

为了实现高精度定位，通常采用多源融合策略，结合多种技术优势互补.

GNSS RTK（实时动态差分定位）

• 在普通GPS基础上引入地面基站校正信号
• 精度可达 5~10厘米
• 优点：全球可用；缺点：仍受遮挡影响，需配合其他传感器

全球定位系统（GPS）是自动驾驶中实现定位的基础技术之一。它由三个核心部分组成，共同协作以确定地球上任意位置的坐标。

一、GPS 的三大组成部分

1. 空间段：GPS 卫星

• 在地球中轨道（约20,200公里高空）运行着约 30颗GPS卫星，确保地球上任何地点、任何时间都能接收到至少4颗卫星信号。
• 每颗卫星均配备高精度原子钟，用于生成极其精确的时间戳信号，这是定位精度的关键。

2. 控制段：地面控制站

• 分布在全球的多个地面监控站负责：
• 跟踪卫星运行状态
• 校正卫星轨道参数
• 监测并验证广播信号的准确性
• 控制中心会定期向卫星发送更新数据，确保整个系统稳定可靠运行。

3. 用户段：GPS 接收器

• 安装在车辆上的 GPS 接收器，需同时接收到至少四颗卫星的信号才能完成三维定位（经度、纬度、高度）和时间同步。
• 接收器并不直接测量距离，而是通过计算信号从卫星到接收器的传播时间，再乘以光速，得到“伪距”（Pseudorange）：

二、GPS 的局限性

尽管 GPS 广泛应用，但在自动驾驶场景下存在明显不足：

三、提升精度：RTK（实时动态差分定位）

为了克服普通 GPS 的精度瓶颈，自动驾驶广泛采用 RTK（Real-Time Kinematic） 技术。

RTK 的工作原理：

建立基准站（Base Station）

• 在地面部署一个或多个已知精确坐标的固定基站；
• 基站同时使用 GPS 测量自身位置。

计算误差

• 将 GPS 测量值与已知的“地面实况”位置进行比较；
• 二者之间的偏差即为当前 GPS 系统的综合误差（包括大气延迟、卫星钟差、轨道误差等）

误差校正

• 基站将该误差信息通过无线网络（如4G/5G）实时发送给附近的移动接收器（如自动驾驶车辆）；
• 车辆接收器利用此校正量，修正自身的 GPS 测量结果。

RTK 的优势：

• 定位精度可提升至 5~10厘米级；
• 显著改善城市环境下的定位稳定性；
• 是 L3/L4 自动驾驶中 GNSS 定位的核心增强技术。

四、RTK 的局限性

尽管 RTK 极大提升了精度，但仍面临挑战：

• 依赖基站覆盖：必须在基站有效通信范围内（通常10~20公里）；
• 信号遮挡问题依然存在：高楼、隧道等仍会导致信号丢失；
• 初始化时间较长：需要几秒到几十秒完成“模糊度解算”才能输出高精度结果；
• 成本较高：需部署和维护基站网络。

惯性导航系统（INS）

在自动驾驶定位系统中，除了依赖卫星信号的GPS，我们还需要一种能够在短时间内连续提供位置和速度信息的技术——这就是惯性导航，其核心传感器是 IMU（Inertial Measurement Unit，惯性测量单元）。

• 使用IMU（惯性测量单元）测量加速度和角速度
• 可在GPS丢失时短时“推算”位置（航位推算）
• 优点：不依赖外部信号；缺点：误差随时间累积（漂移）

✅ GNSS + INS 融合是主流方案，称为 GNSS/INS组合导航系统。

如何通过运动学计算位置与速度？

根据经典运动学公式只要知道：

• 初始位置
• 初始速度
• 实时加速度

就可以推算出车辆在任意时刻的速度和位置。

但问题来了：我们如何测量加速度？

答案是使用 三轴加速度计（Triaxial Accelerometer），它可以测量车辆在三个方向（前后、左右、上下）上的加速度。

然而，加速度计有一个关键限制：

它测量的是车辆自身坐标系（Body Frame）下的加速度，而不是地图所使用的全局坐标系（World Frame）。

因此，若车辆正在转弯或倾斜，必须将加速度数据从“车体坐标系”转换到“全局坐标系”，才能正确积分计算位移。

为了完成这一坐标变换，我们需要另一个传感器——陀螺仪（ Gyroscope ）。

陀螺仪用于测量车辆绕三个轴的角速度（即旋转速率），通过积分可得到车辆的姿态变化（偏航角、俯仰角、翻滚角）。

结合陀螺仪的姿态信息，我们就能实时计算出加速度计数据在全局坐标系中的投影，从而正确进行位移和速度的积分运算。

IMU 的优势与局限

优势：高更新频率，实时性强

• IMU 的采样频率通常高达 100Hz 甚至 1000Hz，远高于 GPS 的 1~10Hz；
• 可在极短时间内提供连续的位置、速度和姿态更新；
• 特别适合在 GPS 信号短暂丢失时“填补空白”，被称为定位系统的“时间桥梁”。

局限：误差随时间累积（漂移）

• 加速度需要两次积分才能得到位置，任何微小的测量误差都会被不断放大；
• 例如，加速度偏差仅 0.01 m/s²，经过 10 秒积分后，位置误差可达 0.5 米；时间越长，误差越大；
• 因此，IMU 仅适用于短时定位，无法长期独立使用。

GPS + IMU 融合：取长补短的黄金组合

为克服各自的缺点，自动驾驶系统普遍采用 GPS 与 IMU 融合定位：

通过滤波算法（如卡尔曼滤波），系统可以：

• 用 GPS 定期修正 IMU 的位置和速度；
• 用 IMU 在 GPS 信号丢失期间维持定位输出。

现实挑战：长时间 GPS 丢失

尽管 GPS + IMU 是主流方案，但在某些场景下仍面临挑战：

• 隧道、地下停车场、城市峡谷、山区道路等环境中，GPS 可能长时间完全丢失；
• 此时仅靠 IMU 无法维持高精度定位，误差会迅速扩大。

解决方案：引入其他传感器（如激光雷达、摄像头）与高精度地图匹配，形成多源融合定位系统。

LiDAR 定位：点云匹配与高精地图协同

在自动驾驶系统中，当 GPS 信号受限（如隧道、城市峡谷）时，激光雷达（LiDAR）定位成为实现厘米级精度的关键手段。其核心原理是：

将实时采集的点云数据与预先构建的高精度地图进行匹配，从而确定车辆在全球坐标系中的精确位置和姿态。

一、点云匹配：定位的核心思想

LiDAR 定位的本质是 “感知-地图比对”：

1. 车辆上的激光雷达持续扫描周围环境，生成三维点云；
2. 将这些实时点云与已知的高精度三维点云地图进行比对；
3. 通过算法寻找最佳匹配位置，反推出车辆当前的位姿（位置 + 方向）。

这就像你走进一个熟悉的房间，仅凭看到的家具布局就能判断自己站在哪里。

二、常用点云匹配算法

1. ICP（Iterative Closest Point，迭代最近点）

ICP 是最经典的点云配准算法之一，适用于两帧点云之间的对齐。

工作流程：

• 对于当前扫描的每个点，在参考点云（地图）中找到最近的对应点，形成点对；
• 计算所有点对之间的平均距离误差；
• 通过不断调整旋转和平移参数，最小化该误差；
• 重复迭代，直到误差收敛到最小值。

输出结果：

• 得到从当前点云到地图点云的最优变换矩阵；
• 即可推算出车辆在地图中的精确位姿。

缺点：对初始位置敏感，计算量大，需较接近的真实初值才能收敛。

2. 直方图滤波（Histogram Filter） / SSD 匹配法

Apollo 系统中采用的一种高效定位方法，也称为 误差平方和（Sum of Squared Differences, SSD） 算法。

核心思想：

• 将当前传感器扫描的点云“滑动”遍历地图上的每一个可能位置；
• 在每个候选位置，计算扫描点与地图对应点之间的距离误差平方和；
• 误差越小，表示匹配度越高；
• 最终选择误差最小的位置作为车辆的估计位姿。

特点：

• 可在全局范围内搜索，适合初值未知的情况；
• 匹配效果可通过可视化呈现——例如，红色点表示对齐良好的点，直观反映匹配质量；
• 计算复杂度较高，通常用于离线或局部精匹配。

3. 卡尔曼滤波（Kalman Filter）在LiDAR定位中的应用

虽然卡尔曼滤波本身不是点云匹配算法，但它常用于融合 LiDAR 匹配结果与其他传感器数据，提升定位稳定性。

工作机制：预测-更新循环

预测阶段：

• 基于上一时刻的状态（位置、速度、方向）和车辆运动模型（如IMU、轮速计），预测当前时刻的位姿

更新阶段：

• 使用 LiDAR 匹配得到的新观测值（位置）；
• 结合预测值与观测值，利用概率加权（卡尔曼增益）得到更精确的估计；
• 更新状态，并进入下一周期。

这个“预测 → 观测 → 修正”的闭环持续运行，实现稳定、平滑的定位输出。

三、LiDAR定位的优势

在 Apollo 系统中，LiDAR 定位模块（localization）正是通过 ICP 或 NDT（正态分布变换）算法，结合高精地图实现稳定输出。

四、LiDAR定位的挑战与局限

尽管技术成熟，LiDAR 定位仍面临现实难题：

1. 高度依赖高精度地图

• 必须提前构建高质量的三维点云地图；
• 地图一旦过时，匹配精度将大幅下降。

2. 地图更新困难

• 高精地图包含大量静态元素（如道路轮廓、电线杆、交通标志）；
• 但现实中存在大量动态干扰：
• 停放的车辆、行人、施工围挡、垃圾、落叶等；
• 这些“临时物体”在地图中不存在，却会出现在实时点云中，造成误匹配。

例如：今天路边停着一辆车，地图中没有；明天车开走了，点云突然“少了一块”——系统可能误判为定位偏移。 

3. 环境变化频繁

• 道路改造、植被生长、积雪覆盖等都会改变点云结构；
• 几乎不可能让地图“完全实时更新”。

视觉定位

在自动驾驶的多传感器体系中，摄像头是最常见、成本最低、信息最丰富的传感器之一。它能够捕捉丰富的纹理、颜色和语义信息，是环境感知的核心工具。然而，仅靠图像实现高精度定位非常困难。因此，视觉定位通常不单独使用，而是与高精度地图、GPS 和其他传感器数据融合，共同提升定位精度。

一、为什么视觉定位难以独立完成？

尽管图像数据易于获取、种类多样，但其用于定位存在天然局限：

• 缺乏深度信息：单目摄像头无法直接获取距离，难以判断物体远近；
• 受光照影响大：夜间、逆光、雨雪天气会显著降低图像质量；
• 视角依赖性强：同一场景从不同角度拍摄，图像差异巨大；
• 动态干扰多：行人、车辆等移动物体会影响匹配稳定性。

二、视觉 + 地图 + 概率：协同定位的核心思路

为了提升视觉定位的准确性，系统采用“感知 + 先验地图 + 概率推理”的方法。其核心思想是：

通过摄像头观察到的特征（如车道线、树木、标志），结合高精地图中的已知信息，利用概率模型推断车辆最可能的位置。

举个例子：

假设车辆摄像头检测到“道路右侧有一棵树”。

但地图显示道路右侧有多棵树，分布在不同位置——我们如何判断看到的是哪一棵？

这时，系统不会直接“猜”，而是使用概率方法进行推理。

三、粒子滤波：视觉定位中的“概率搜索”引擎

解决这类定位问题的经典算法是 粒子滤波（ Particle Filter ）。

1. 什么是粒子？

• “粒子”代表车辆可能所处的一个候选位姿（位置 + 方向）；
• 系统在道路上生成大量粒子，覆盖所有可能的位置。

2. 工作流程：观测 → 匹配 → 权重更新 → 重采样

步骤1：初始分布

• 在道路范围内随机或均匀分布成千上万个粒子，每个粒子代表一个假设位置。

步骤2：第一次观测（如“右侧有一棵树”）

• 对每个粒子，查询其在地图中“右侧是否能看到一棵树”；
• 能看到的粒子获得高权重，看不到的权重降低；
• 实际上“看不到树”的位置被逐步排除。

步骤3：继续行驶并更新

• 车辆前行，摄像头再次观测：“右侧又出现一棵树”；
• 系统检查哪些粒子所在位置能看到“一排两棵树”；
• 只有少数位置满足该条件 → 这些粒子权重大幅提升；
• 其他位置的粒子被进一步淘汰。

步骤4：收敛到最可能位置

• 随着持续观测，粒子逐渐聚集在最符合观测结果的区域；
• 最终，系统输出这些高权重粒子的平均位姿，作为当前车辆的估计位置。

这个过程不断循环，实现动态、实时的定位更新。

四、车道线匹配：视觉定位的典型应用

由于树木等自然特征在城市道路中较少且不规则，而车道线几乎存在于所有道路上，因此成为视觉定位中最常用的匹配特征。

视觉车道线匹配流程：

1. 摄像头实时拍摄前方道路，提取车道线像素；
2. 将这些车道线与高精地图中预存的车道线几何形状进行比对；
3. 计算图像中车道线与地图中不同位置车道线的匹配程度（如距离误差、方向一致性）；
4. 匹配度越高，说明车辆越可能位于该地图位置。

示例说明：

• 蓝色线：地图中两个不同位置的车道线形状；
• 红色线：摄像头实际检测到的车道线；
• 若红色线与右侧蓝色线高度重合，而与左侧差异较大 → 车辆更可能位于右侧对应的地图位置。

这种“图像-地图”匹配方式，显著提升了定位的准确性和鲁棒性。

Apollo 如何解决定位问题？

百度 Apollo 采用多传感器融合 + 高精地图匹配的综合定位方案：

• 输入：GNSS、IMU、LiDAR、轮速计、高精地图
• 核心算法：融合滤波（如EKF、Particle Filter）、点云匹配（NDT/ICP）
• 输出：车辆在地图坐标系下的六自由度位姿（x, y, z, roll, pitch, yaw）
• 实现：通过 localization 模块，在 Cyber RT 框架下实时发布定位结果

 Apollo 的定位系统可在 GPS 失效时（如隧道），仅靠 LiDAR + 地图实现连续稳定定位，误差小于20厘米。

总结：定位的本质

定位 = 传感器感知 + 地图先验 + 坐标变换 + 多源融合

它不是单一技术的胜利，而是系统工程的结晶。高精度地图提供了“已知世界”，传感器提供了“实时观察”，而坐标变换与融合算法则完成了“从看到到知道”的跨越。