lidar_localization_ros2:ROS 2 先验地图 3D 激光定位,冷启动自己找位姿

一句话定位

lidar_localization_ros2 是一个 ROS 2 包:给它一张事先建好的点云地图(.pcd 或 .ply)和一路 LiDAR 数据流,它就把机器人的位姿持续发布在地图坐标系里。它不是 SLAM——不建图、不优化轨迹,只回答"我现在在地图哪个位置"这一个问题。可以独立运行(发布 map -> base_link),也可以作为 Nav2 的定位源(发布 map -> odom)。仓库 rsasaki0109/lidar_localization_ros2,作者 ryohei sasaki(个人开发者),580★ / 132 fork,BSD-2-Clause,2020-04-18 建仓,最近一次 push 是 2026-10-17——六年间没有断维护,当前包版本 1.3.0,主目标 ROS 2 Jazzy、兼容 Humble。

它与作者另一个仓库 lidar_slam_ros2 是配套关系:后者负责建图和里程计(RKO-LIO),前者负责在建好的地图上定位。README 里的标准用法就是先跑 RKO-LIO 里程计,再跑本包的 quickstart.py --map map.pcd。所以读这个包,等于读"机器人有了地图之后怎么用"这一层的参照实现。

数据面

项	值
仓库 / 作者	rsasaki0109/lidar_localization_ros2(个人 User,ryohei sasaki)
Stars / Forks / Watchers	580 / 132 / 6(2026-10-18)
语言 / 协议	GitHub 标记 Python;定位核心是 C++ lifecycle 组件 + Python 工具链 / BSD-2-Clause
建仓 / 最近 push	2020-04-18 / 2026-10-17
包版本 / ROS 版本	1.3.0 / Jazzy 主目标,Humble 兼容
注册后端	NDT、GICP、NDT_OMP、GICP_OMP、SMALL_GICP、SMALL_VGICP
地图格式	.pcd / .ply(运行时推荐二进制 little-endian float32 PLY)
硬依赖	ndt_omp_ros2(同作者的 OpenMP 加速 NDT/GICP);small_gicp 可选,CMake 开关保护
测试	test/ 下 81 个文件:C++ 策略单测 + Python 契约/CLI 测试
运行形态	lifecycle 节点 + 多线程执行器(3 线程);standalone / Nav2 / MID-360 腿式机器人三种 launch
输出话题	/pcl_pose、/path、/alignment_status、/reinitialization_requested

它站在 SLAM 的哪一边:先验地图上的定位,不是建图

移动机器人定位有两种问法。一种是"我从零开始,一边建图一边知道自己在哪"(SLAM);另一种是"地图已经有了,可能是上周建的,可能来自另一台机器人,我每次开机都要在这张图上找到自己"(基于地图的定位)。后者是 AMCL 在 2D 激光上回答的问题,lidar_localization_ros2 把同样的问答搬到了 3D 点云地图和 3D LiDAR 上,并且把匹配引擎换成了 NDT / GICP 家族的 3D 配准。

这个分工带来一组 SLAM 不拥有的问题,包的设计基本围绕它们展开:

• 冷启动没有初始位姿。第一次开机、被拾起换房间、跟踪丢失之后,机器人都需要重新回答"我在哪"。包的做法是分支定界(BBS_2D)在 2D 占据栅格上做全图搜索,再用 3D NDT 给候选打分,最后由定位器自己验证。
• 自相似地图有别名解。长廊、对称房间、重复结构会让多个候选都"完美贴合"。包的回应是一串显式安全闸:分数阈值、与第二名的分差、候选年龄、多次扫描共识、验证期——宁可搜索失败后要求操作者在 RViz 里给 2D Pose Estimate,也绝不猜,更没有隐式回退到地图原点。
• 定位不能比里程计还抖。纯 NDT 跟踪在扫描失配时会跳变,包的回应是分层预测:twist 预测 → twist EKF → IMU 预积分平滑 → 外部里程计 TF 桥接,每层都有安全闸,任何一层不同意就回退到 LiDAR 测量本身。

一帧扫描的完整流水线

核心是 PCLLocalization,一个 rclcpp_lifecycle::LifecycleNode,跑在三线程多线程执行器上(IMU 预积分和定时发布有独立回调组,不被长的配准回调饿死)。实现按关注点拆成 src/component_*.cpp,而所有决策逻辑被抽成 include/lidar_localization/ 下 60 多个无 ROS 依赖的 policy 头文件(measurement_gate_policy.hpp、registration_seed_policy.hpp、imu_preintegration_guard_policy.hpp……),每个都有对应单测。这个分层是这个包最值得学的工程决定:策略是纯函数,可以离开 ROS 图单测和回归。

一帧 sensor_msgs/PointCloud2 从进来到发布位姿,走的是这条路:

1. 扫描准入(scan_admission_policy):时间戳、最小间隔、队列深度检查。
2. 预处理(scan_preprocessing_policy):距离过滤 + 可选体素下采样;顺带从 time/timestamp/offset_time/t 字段提取逐点相对时间(Livox FLOAT64 纳秒单位有自动判别)。
3. 连续时间去畸变(默认开):用运动估计把一帧扫描内的点统一到同一参考时刻;数据没就绪时原样放行,不硬扭。
4. 种子选择(registration_seed_policy):当前位姿 / 上一帧增量 / twist 预测 / IMU 预积分 / 外部里程计 TF 预测,按优先级和可用性选。
5. 配准:NDT / GICP 等后端,局部地图裁剪(默认 150 m 半径)控制大图开销。
6. 测量门(measurement_gate_policy):fitness、修正量、种子偏差超限就拒收,拒收不插值乱猜。
7. 位姿后端:原始配准位姿 / twist EKF / GTSAM 滑窗平滑 / IMU 预积分平滑四选一,结果带上协方差发布。
8. 发布:/pcl_pose(带协方差)、/path(默认保留最近 2000 个位姿)、TF、以及一组诊断键值。

全局初始化与自动恢复:G1 / G2 / G3 三件套

这是整个包里证据链最重的部分,文档 docs/global_localization.md 专门讲它,分三个阶段:

阶段	做什么	入口
G1	地图级候选生成:在 2D 占据栅格上跑分支定界(BBS_2D),artifact-first、可离线	make_bbs_relocalization_attempts.py
G2	按需查询服务:来一帧扫描,返回排好序的候选位姿(JSON + PoseArray)	global_localization_node.py
G3	带安全闸的自动重播种:监视丢失信号,查询 G2,过闸才发 /initialpose	reinitialization_supervisor_node.py

G1/G2 需要先用 generate_occupancy_map_from_pcd 从点云地图生成占据栅格(0.4–2.0 m 高度带内的点算障碍,天花板和树冠不算)。G3 的决策逻辑在 reinitialization_supervisor_policy.py 这个不依赖 ROS 的状态机里,回归测试覆盖。它的安全闸清单读起来像一份失败复盘笔记:候选分数下限、两次重置最小间隔、请求去抖、恢复 fitness 阈值、连续恢复证据样本数、超时判定、尝试次数硬上限、查询超时、以及与定位器位姿的交叉校验——任何一闸不过就不发。文档里写得很直白:闭环恢复回路如果允许一个"自信但错误"的候选被反复发布,会灾难性发散,这是 Phase 3 的教训。

quickstart 把这一整套包装成一条命令:ros2 run lidar_localization_ros2 quickstart.py --map /path/map.pcd。它自动发现点云话题、IMU 话题、/clock、odom -> base TF 和 LiDAR 帧,打印 Discovery: 行;首次启动全图搜索,之后按地图内容 SHA-256 恢复上次验证过的位姿;搜索不到明确匹配就要求在 RViz 里指一下,没有回退到 (0,0,0) 的选项。针对腿式机器人上倒装 MID-360 只离地 0.45 m、默认高度带里只剩约 100 个点导致 2D 搜索饱和的问题,还有稀疏带补救搜索(带内点少于 512 个时从传感器平面向上重搜,1024 点、32 候选、NDT 精化)——这一条把 Go2 采集房 12 次回放的错误启动从 3–5 次降到 0。

IMU 预积分与连续时间去畸变

IMU 支持逐层开放、默认全关(纯 NDT/GICP 跟踪),不用 IMU 的用户完全不受影响:

层	参数	作用
Twist 预测	use_twist_prediction	帧间匀速模型,用 IMU/里程计角速度旋转种子
Twist EKF	use_twist_ekf	twist 上的小匀速 EKF,同时喂预测种子和发布协方差
IMU 预积分 + 平滑	use_imu + use_imu_preintegration	Forster et al. 2017 的流形预积分,滑窗 Gauss-Newton(6-DoF NDT 先验 + 在线陀螺/加计零偏估计)
Twist GTSAM 平滑	use_gtsam_smoother	不用原始 IMU 的里程计+NDT 滑窗平滑

预积分数学有数值回归测试钉死:纯旋转复现 delta_R = Exp(ω·t)、纯平移复现 delta_v = a·t、残差为零、五个零偏雅可比对得上有限差分——纯 Eigen 实现,可以脱离 ROS 单独编译运行。安全侧是一组 guard:一致性门(连续多次 IMU 预测与已接受 LiDAR 位姿比对通过才允许当种子)、修正守卫(IMU 与 NDT 修正相差超 2.0 m 或 10° 就重置平滑器并以 LiDAR 测量为准)、reset 后 5 次预热接受。

连续时间去畸变(默认开)有三种运动源:relative_motion、imu_pose_history、lidar_constant_velocity。这里必须引用文档自己的负面结论:2026-07-13 在 Koide outdoor_hard_01a 85–112 s 角落窗口三连跑,三种模式都没过共享的精度/覆盖/拒绝连击/延迟门,默认因此保留"没就绪就不改扫描"的保守兜底。这种把负面结果写进文档的做法,在这个仓库里是常态而不是例外。

Nav2 集成与话题/帧契约

帧契约固定为 map / odom / base_link,话题全部可改名。standalone 模式发布 map -> base_link;Nav2 模式发布 map -> odom,要求外部提供 odom -> base_link(比如 RKO-LIO 或轮式/腿式里程计)。use_odom: true 只读 /odom,不发布里程计 TF。

Nav2 模式下有个 AMCL 式的设计值得注意:冻结的 map -> odom 桥。map -> odom 只从"被接受"的扫描匹配更新,扫描被拒期间原样重 stamp 广播,于是即使 NDT 暂时失配,经 TF 组合出的 map -> base_link 依然连续可用——下游不会因为定位器短暂怀疑人生就看到位姿跳断。相关话题 /odom_bridge_pose 用 transient_local QoS 发布,G3 supervisor 订阅它而不是自己查 TF(文档记录:/tf 的 volatile durability 不可靠地到达第三个进程,这是实测踩出来的)。

launch 文件四个:nav2_lidar_localization.launch.py(仅定位)、nav2_navigation.launch.py(定位 + Nav2 全栈)、mid360_legged_localization.launch.py(MID-360 腿式机器人)、lidar_localization.launch.py(全部参数暴露的基础 launch)。参数以 YAML(localization_param_dir)为准,launch 参数只在显式传入时覆盖。lifecycle 激活走 GetState/ChangeState 服务,不依赖转换事件。

基准:公开数据集上的硬数字

基准方法本身值得先说:回放公开数据集,跑的是和用户完全相同的两条命令(里程计 + quickstart.py --map),每次运行都没有初始位姿,误差在 map 系下直接对地面真值、不做轨迹对齐——因为对齐会把"初始化错了但一路跟得很稳"这种最差失败模式掩盖掉(文档举例:Go2 上有次差 1.8 m,对齐后看着像 0.2 m)。

数据集	运行	正确初始化	中位误差	p95 误差
Koide outdoor,手持 MID-360(4 序列)	12	12	0.08–0.12 m	0.21–0.91 m
Koide outdoor kidnap(传感器被拿到别处)	6	6	0.04–0.12 m	15.9–60.5 m
JEPLO,Unitree Go2,lidar_slam_ros2 地图(4 序列)	12	12	0.03 m	0.09–0.13 m
JEPLO,Go2 大地图 + 低传感器设置(7 序列)	18	18	0.02–0.03 m	-

测量时间 2026-10-06 / 10-11,Jazzy、1x 回放。 kidnap 序列的 p95 来自"传感器被抱着走、还没恢复"那一段,不是稳定跟踪误差。v1.3.0 标签验证(2026-10-09,Humble)另有一次完整回放:map 系位置 RMSE 0.2796 m,2203/2209 个 GT 匹配位姿戳(99.73%)。

同样写进文档的失败:一次 outdoor_hard_02b 运行里,RKO-LIO 丢掉 ICP 关键点不足的帧,恢复监督者在里程计中断的豁免窗口内凭单个未确认答案把位姿重置到了 128 m 外;/alignment_status 在错误重置后大部分位姿报告 OK(自信的错配),又在 20–41% 的好位姿上误报;Go2 大图的默认搜索在 2026-10-06 设置下会饱和歧义(后来的稀疏带补救修了这个)。没有跨数小时的运行,没有真机验证。

上手:安装、quickstart、无机器人复现

安装走仓库自带脚本,约 30 分钟(8 核):

mkdir -p ~/lidarloc_ws/src && cd ~/lidarloc_ws/src
git clone https://github.com/rsasaki0109/lidar_localization_ros2.git
cd lidar_localization_ros2
scripts/bootstrap_colcon_workspace.sh --build
source ~/lidarloc_ws/install/setup.bash

它会连带构建同作者的 ndt_omp_ros2;无 sudo / 手动构建路径见 docs/local_build.md。有机器人就 quickstart.py --map 起步;Unitree Go2 + MID-360 有五分钟指南(docs/go2_mid360_quickstart.md);没有机器人可以跑公开数据集:ros2 run lidar_localization_ros2 run_koide_public_bag.py --download 会下载 Koide 数据、跑里程计+定位、对真值报误差,回放约五分钟。bringup 有问题先跑 check_lidar_localization_bringup.py --profile standalone,排查路径 docs/troubleshooting.md。

边界:它不承诺什么

docs/v1_status.md 用"Safe claims / Do not claim"两栏把边界划得很清楚,这里直接转述:可以说的是"提供 ROS 2 基于地图的 3D LiDAR 定位和 Nav2 launch 助手、推荐预设在文档边界内有公开回放和 smoke 验证";不可以说的是:长时程城市级鲁棒性、生产级全局重定位、默认基准路径下的自动重置发布、未经当前公开对比的 SMALL_GICP 默认可用、Jetson+MID-360 真机验证。协方差语义还不足以作为下游融合的头条卖点;占据栅格生成是 route-crop 导向的,不是通用建图工具。

适合谁:手里已经有点云地图(自建或来自 lidar_slam_ros2 这类 SLAM 包)、需要 3D 精度定位的机器人团队——腿式机器人 + 倒装 MID-360 是文档里证据最全的形态;需要 Nav2 定位源替换 AMCL 的;以及要一个"策略与 ROS 解耦、可单测"的定位流水线作参照实现的人。不适合:没有先验地图的场景(那是 SLAM 的活),或需要生产级长时程无人值守恢复的场景——那正是文档里Known Limits 明说的未决问题。