激光点云定位

  • Description：先验地图下的激光点云定位 (Apollo 风格) — 点云定位流程、航向角优化 (LK 图像对齐)、直方图滤波器 (x,y)、姿态对准 (调平/罗经/GNSS 航向)、双滤波器解决量测时序乱序、车辆运动约束
  • My Notion Note ID：K2E-A-S5
  • Created：2020-06-08
  • Updated：2026-05-26
  • License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

Table of Contents

1. 点云定位 vs SLAM

这里讲的是有先验地图的定位 (localization)，不是 SLAM (无先验、边走边建图)。

  • SLAM — 同时建图 + 定位，无先验
  • 点云定位 — 已有高精地图 (见 高精地图 那篇)，把实时激光点云匹配到地图上求位姿

自动驾驶量产方案常用"HD Map + 多传感器融合定位" — 这是 Apollo / 百度等的主流做法。本笔记内容来自这类工程实践。

2. 点云定位流程

输入：融合定位提供的预测位姿 + 激光雷达点云。 输出：x,y,z,yawx, y, z, \text{yaw} (zz 通过定位地图获取)。

两个步骤：

  1. 航向角优化 (§3) — 对应图像对齐
  2. (x, y) 计算 (§4) — 对应 SSD 直方图滤波器

3. 航向角优化

为什么先优化 yaw：融合定位容易产生角度误差，低端 IMU 尤其严重 (尤其初始时刻误差极大)。航向角哪怕 1.5° 误差，直接做直方图分布会明显偏 (位置可能偏一米多)。

方法：用 Lucas-Kanade (LK) 图像对齐算法 (见 光流 那篇) 做点云投影图的对齐，先把 yaw 校准，再做位置估计。

4. 直方图滤波器定位 (x, y)

yaw 校准后，用 SSD-HF (Sum of Squared Differences - Histogram Filter) 估计 (x,y)(x, y)：

  • 把实时点云投影与地图做 SSD 匹配
  • 直方图滤波器维护 (x,y)(x, y) 的概率分布
  • (直方图滤波是离散状态的 Bayes 滤波，见 Probabilistic Robotics 系列 ch02)

zz 不靠点云算，直接从定位地图查询。

5. 姿态对准

姿态对准 = 得到 IMU 的 roll / pitch / yaw。

调平 (roll, pitch)

车静止时，加速度计测量的比力仅由重力导致，可通过 f=C⋅gf = C \cdot g 求解 roll / pitch。这就是"调平"。

罗经对准 (yaw, 高精度 IMU)

对非常高精度的 IMU：车静止时通过测量载体系中的地球自转来确定方位 (yaw)。消费级 IMU 测不到地球自转 (见 IMU 系列 ch01 性能指标)，做不了罗经对准。

yaw 对准 (车载低端 IMU)

车上 IMU 等级不够，用两种方式：

  1. 车直线跑起来 — 用 GNSS 获取的速度方向估计航向
  2. 双天线 GNSS — 两个天线位置连线计算航向

6. 双滤波器解决量测乱序

问题：GNSS 定位很快 (有原始消息就能快速解算)，但点云定位慢 (几十到上百 ms)。先收到的消息解算时间长才出结果，造成量测更新乱序 (out-of-order measurement)。

解法：用两个滤波器

  • Filter1 — 主要做时间更新 (预测)，实时对外提供服务
  • Filter2 — 做量测更新。把 Filter1 的状态拷过来做量测更新，补齐到最新时刻 (用时间更新)，最后用 Filter2 替换 Filter1

这样解决了时间戳乱序问题。

(这是处理延时量测的工程技巧，与 VIO 系列 里 MSCKF 保留多帧状态的思路异曲同工)

7. 车辆运动约束

车辆运动有先验约束可用：

  • 横向约束 — 车行驶中一般不会大幅横向偏离，假设横向偏离很小 → 加约束 (类似 NHC 非完整约束)
  • 整车运动模型 — 更复杂的横向运动模型，需在数学上推导兼容滤波模型后才能加入

(车辆约束 + 轮速 + VIO 见 K2E-B-W 系列 Wheel Odometry 论文)

8. 工程经验

工程实践问答：

  • 大车遮挡 → 有时定位失败；用自适应算法明显改善
  • 激光反射值不稳定 (随距离/角度变) → 可做反射值标定 (按事先采集场景 + 准 pose 投影，每根线标定 0-255 反射值)；实务中配合高度值融合后影响不大
  • 隧道 / 桥洞退化 (前后方向特征不变，纵向不可观) → 靠地面车道线反射值给纵向约束；即使没有，惯导平滑也能撑过

References

  • Wan, G., et al. (2018). Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse City Scenes. ICRA. arXiv:1711.05805 — Apollo 多传感器融合定位 (主参考)
  • Groves, P. D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (2nd ed.). — GNSS/INS 组合导航权威
  • 《GNSS 与惯性及多传感器组合导航系统原理》. 国防工业出版社. — 中文版
  • Thrun, Burgard, Fox. Probabilistic Robotics. — Markov 定位 / 直方图滤波 / 粒子滤波理论基础