ROS2 3D LiDAR导航系统任意位置重定位实现

基于 ROS 2 的 3D LiDAR 自主导航系统：重点聊聊全局重定位

最近搭建了一套基于 ROS 2 Humble 的 3D LiDAR 自主导航系统，项目命名为 Lidar_na v2_ws。

这套系统的核心并非“再次调通一个 Na v2”，而是要解决两个更实际的问题。

第一，机器人能否在初始位姿完全未知的条件下，借助 3D 点云完成全局重定位？
第二，系统能否将激光里程计、点云重定位、Na v2 导航以及机器人本体这几个模块彻底解耦？以便日后更换算法时，无需将整个推倒重写。

简单概括一下目标：它并非那种只能在固定位置启动的演示 demo，而是一套可持续扩展、算法可替换、能在真机上不断迭代的导航框架。

1. 为什么要做这套系统

在移动机器人导航领域，Na v2 已经提供了相当完整的能力，例如全局规划、局部规划、代价地图、行为树以及控制器。但 Na v2 本身并不负责回答一个更基础的问题：机器人当前到底在哪里？

对于普通的 2D 激光雷达，常见的做法是使用 AMCL。但一旦传感器换成 Livox MID-360 这类 3D LiDAR，情况就复杂得多。

3D LiDAR 能够提供更丰富的空间结构信息，但同时也带来了一个新挑战：如何将当前扫描到的 3D 点云，稳定地匹配到已有的全局 PCD 地图中？

尤其是机器人刚开机时，如果它不在预设的固定位置，系统完全不清楚它在全局地图中的初始位姿。此时若只依赖局部配准算法，结果往往非常不可靠。

因此，这套系统的核心目标可以拆解如下：

• 使用 3D LiDAR 和 IMU 运行 LIO；
• 构建二维栅格地图供 Na v2 使用；
• 保存三维 PCD 地图用于点云重定位；
• 在初始位姿不确定时，通过全局点云匹配完成重定位；
• 持续发布 map -> odom，为 Na v2 提供稳定的全局定位输入；
• 将 LIO、重定位、Na v2 和机器人描述这几个模块彻底解耦，方便后续算法替换。

而整个链条中最关键的一环就是：
KISS-Matcher + small_gicp 实现全局重定位。

2. 系统整体架构

系统整体的数据流大致如下：

LiDAR / IMU ↓ LIO 里程计 ↓ TF 桥接 ↓ /registered_scan ↓ 3D 点云重定位 ↓ 发布 map -> odom ↓ Na v2 导航

我将整个系统拆成了几个相对独立的模块：

这样设计的好处是，LIO 不直接与 Na v2 强绑定，重定位也不与某个特定的里程计算法强绑定。

换言之，后续可以方便地切换：

FAST-LIO ↔ Point-LIO
small_gicp ↔ ICP ↔ KISS-Matcher + small_gicp
仿真 URDF ↔ 实机 URDF

这才是最理想的状态：
每个模块都能独立替换，而不是所有东西耦合在一起。

机器人系统最怕的不是某个算法不够强，而是模块之间耦合得太紧密。一旦出问题，你根本分不清是 LIO 漂移了、TF 配错了、点云没对齐，还是 Na v2 在异常运行。最后调试到怀疑人生，甚至开始琢磨是不是 Ubuntu 在故意跟你作对。

3. 建图模式：先生成 2D 地图和 3D PCD 地图

系统首先支持建图模式。

建图阶段，机器人在仿真环境中运动，通过 LiDAR 和 IMU 运行 LIO，同时生成二维栅格地图和三维点云地图。

流程大致如下：

机器人运动 → LiDAR 扫描环境 → LIO 估计运动 → 生成局部点云和二维 LaserScan → SLAM Toolbox 构建 2D 地图 → 保存 2D map + 3D PCD

二维地图主要供 Na v2 使用，三维 PCD 地图则留给后续的点云重定位环节。

这里有一个非常实用的设计思路：我并没有强行让 Na v2 直接处理完整的 3D 点云，而是先将三维点云进行高度切片，转换成类似二维激光的数据，再喂给 Na v2 的代价地图模块。这一步听起来或许不够“高大上”，但胜在实用。工程中很多时候不是看架构图有多漂亮，而是看机器人能否真正稳定地跑起来。

4. 重定位模式：KISS-Matcher + small_gicp

这套系统最核心的部分就是重定位。

具体要解决的问题是：
机器人手上已经有一张先验 PCD 地图，但它在某个未知位置开机，系统需要根据当前扫描到的局部点云，自动匹配到全局地图中，并估算出机器人当前的位姿。

如果只靠 small_gicp 来干这件事，问题会很明显。

small_gicp 更适合做局部精配准。当机器人的初始位姿已经比较接近真实位置时，它收敛很快，精度也不错。可一旦初始位姿差得很远，GICP 很容易陷入局部最优，或者干脆匹配失败。

这就好比一个人被空投到一个陌生城市，手里只有附近几栋楼的照片，让他判断自己身在何处。如果先告诉他“大概在某条街附近”，他或许还能找到。如果什么都不说，那就有些玄学了。

所以重定位被设计成两阶段：

第一阶段：KISS-Matcher 全局粗配准
第二阶段：small_gicp 局部精配准与连续跟踪

5. KISS-Matcher 负责全局粗配准

KISS-Matcher 在系统里的职责是进行全局初始化。

它不依赖一个非常准确的初始位姿，而是利用当前累积的局部点云和先验 PCD 地图进行全局匹配，先估算出一个大致的位姿变换。

这里的重点是“粗配准”。它不需要一步到位给出特别精确的位置，只要能把机器人从“完全不知道自己在哪”的状态，拉回到“大概在地图中的这个区域”，后面的 small_gicp 就可以接上了。

因此，KISS-Matcher 在这里不是最终答案，而是给 small_gicp 提供一个靠谱的初值。换句话说，KISS-Matcher 负责把问题从地狱难度降到正常难度。

6. small_gicp 负责精配准和连续跟踪

只要 KISS-Matcher 完成全局初始化，系统就会切换到 small_gicp。

small_gicp 会利用当前的 /registered_scan 和先验 PCD 地图进行精细配准，并持续维护机器人在全局地图中的位姿。

最终系统会发布 map -> odom 这个 TF。这个 TF 非常关键。

因为 LIO 本身通常提供的是 odom -> base_footprint，描述的是机器人在里程计坐标系下的相对运动。而 Na v2 需要知道机器人在全局地图 map 里的位置。

所以重定位模块要做的，就是持续维护好 map -> odom。这样完整的 TF 链路就变成了：

map -> odom -> base_footprint -> chassis -> livox_frame

只要这条链路稳定不出错，Na v2 就可以正常规划路径、更新代价地图，并在 RViz 中准确显示机器人在全局地图中的位置。

7. 为什么不只用 small_gicp

如果机器人每次都在固定位置开机，或者每次都能在 RViz 中手动给一个比较准的 2D Pose Estimate，那么只用 small_gicp 确实是可行的。

但这种工作方式对我来说不够理想。因为它本质上变成了：第一步，人先猜一个差不多的位置；第二步，机器人证明人猜得还行。这不叫智能，这叫人类辅助智能。

我更希望系统能在没有准确初始位姿的情况下，自己先完成全局粗定位，然后再进入连续精配准。所以这套系统采用了：

KISS-Matcher 粗配准 → small_gicp 精配准 → 持续发布 map -> odom

这样机器人就可以尝试实现任意位置开机重定位。

当然，这句话不能说得太满。如果机器人开机位置附近全是空旷区域，点云结构太少，或者当前环境与先验地图差异太大，那再好的算法也救不了。算法不是玄学，不能指望它凭空气定位。

更准确地说，这套系统实现的是：在当前点云和先验地图具有足够结构信息和重叠区域的前提下，支持无需准确初值的全局重定位。这比“每次靠人手动给初始位姿”要实用得多。

8. 解耦设计带来的价值

除了重定位本身，这套系统的另一个重点是解耦。我希望它不是那种只能跑一个固定配置的项目，而是能作为一个实验框架持续扩展。

目前系统中，LIO、重定位和 Na v2 之间主要通过标准的 ROS 2 接口连接：

/registered_scan
/Odometry
TF: map -> odom -> base_footprint
PCD map
2D map

这种结构让后续的算法替换变得非常灵活：

这种设计对机器人调试来说非常关键。因为实际调试时，经常遇到的问题不是“某个算法一定不行”，而是你根本不知道到底是哪一层出了问题。如果模块边界清楚，就可以逐一排查：

• LIO 是否正常输出里程计？
• /registered_scan 是否正常发布？
• PCD 地图的坐标系是否正确？
• map -> odom 是否只有一个节点在发布？
• Na v2 的代价地图是否正常更新？
• base_footprint -> livox_frame 的外参是否正确？

这套系统不一定多优雅，但至少出问题时还能精准定位。对于机器人开发来说，这已经是很重要的进步了。

9. 当前实现效果

目前系统主要支持两种模式。

9.1 建图模式

建图模式下，机器人在 Gazebo 仿真环境中运动，通过 LiDAR 和 IMU 数据运行 LIO，同时构建二维占用栅格地图，并保存三维 PCD 地图。这个模式主要用于生成后续导航和重定位所需的先验地图。

9.2 重定位导航模式

重定位模式下，系统会加载已有的 PCD 地图，并使用当前的 /registered_scan 和先验地图进行匹配。

采用 KISS-Matcher + small_gicp 方案时，完整流程如下：

累计当前局部点云 → KISS-Matcher 全局粗配准 → 初始化成功 → small_gicp 连续精配准 → 持续发布 map -> odom → Na v2 正常导航

在 RViz 中，可以看到机器人通过点云匹配，重新回到全局地图坐标系下。当 map -> odom 稳定发布后，Na v2 就可以在全局地图中进行路径规划和导航控制。

10. 一些调试经验

10.1 同一时间只能有一个节点发布 map -> odom

这一点非常关键。

如果 small_gicp 和其他重定位节点同时发布 map -> odom，机器人位姿就会跳变，Na v2 也会跟着出错。这种问题看起来像是算法不稳定，实际上就是 TF 在冲突。两个节点都觉得自己是对的，最后受伤的只有你。

10.2 当前点云和先验地图必须有足够重叠

KISS-Matcher 虽然能做全局粗配准，但它不是魔法。如果机器人刚开机时扫描到的结构太少，或者当前位置与先验地图的重叠区域不够，初始化就可能失败。

一个比较实用的做法是：在初始化阶段让机器人原地旋转或缓慢移动一小段距离，以累计更多局部点云。点云结构越完整，全局初始化就越容易成功。

10.3 坐标系必须统一

常见的 TF 链路是：map -> odom -> base_footprint -> chassis -> livox_frame

如果 base_footprint -> livox_frame 的外参不对，或者 frame 名字不一致，重定位结果就会偏。很多时候你以为是 GICP 没收敛，其实根源是 TF 从一开始就错了。这种问题非常折磨人，因为它不一定有明显报错，只会稳定地错下去。

10.4 降采样参数需要结合地图规模调整

KISS-Matcher 和 small_gicp 都涉及点云降采样。常见的参数包括 voxel_resolution、global_leaf_size、registered_leaf_size 等。

体素太小，计算量大，内存压力高；体素太大，几何细节丢失严重，匹配质量下降。这些参数不能无脑照抄，需要根据地图规模、点云密度和机器性能反复调试。

调参没有银弹，只有反复试。这也是机器人开发最朴素的真理。

11. 总结

这套 ROS 2 3D LiDAR 自主导航系统的核心，不是单纯跑通 Na v2，而是实现了两个目标。

第一，使用 KISS-Matcher + small_gicp 完成全局重定位。KISS-Matcher 负责在无准确初值时进行全局粗配准，small_gicp 负责局部精配准和连续跟踪，最终持续发布 map -> odom，让 Na v2 获得稳定的全局定位输入。

第二，系统整体采用了解耦设计。LIO、重定位、Na v2、机器人描述和仿真模块之间，尽量通过标准 ROS 2 接口连接，方便后续灵活切换 FAST-LIO / Point-LIO、small_gicp / ICP / KISS-Matcher + small_gicp，以及仿真环境 / 实机环境。

目前它还不算一个完美的工程，但已经解决了最想解决的问题：
机器人不必每次都在固定位置开机，也不必完全依赖人工去手动设置初始位姿，而是可以通过 3D 点云尝试完成全局重定位。

如果用一句话来总结：
这是一套以全局重定位为核心、以模块解耦为设计目标的 ROS 2 3D LiDAR 自主导航系统。

它还不完美，但至少已经从“能跑别动”进化到了“知道哪里能动，动了哪里会炸”。

对于任何一个 ROS 2 机器人导航项目来说，这已经算是阶段性的胜利了。