从“开机全靠猜”到任意位置重定位：我做了一个 ROS 2 3D LiDAR 导航系统

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

已开源至 GitHub，欢迎 Star：

github.com/Ikunio/Lida...

最近折腾了一套基于 ROS 2 Humble 的 3D LiDAR 自主导航系统，项目名叫 Lidar_nav2_ws。

这套系统的重点不是"我又调通了一个 Nav2"，而是想解决两个更实际的问题：

第一，机器人能不能在初始位姿未知的情况下，通过 3D 点云完成全局重定位；

第二，系统能不能把 激光里程计、点云重定位、Nav2 导航和机器人本体 解耦，后续想换算法时不用把整个工程拆了重写。

简单说，我想要的不是一个只能在固定位置启动的 demo，而是一套后续可以继续扩展、继续替换算法、继续上实机折腾的导航框架。

1. 为什么要做这套系统

在移动机器人导航里，Nav2 已经提供了比较完整的导航能力，包括全局规划、局部规划、代价地图、行为树和控制器等模块。

但是 Nav2 本身并不负责告诉机器人"我现在到底在哪里"。

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

3D LiDAR 可以提供更丰富的空间结构信息，但同时也带来一个问题：

如何把当前扫描到的 3D 点云，稳定匹配到已有的全局 PCD 地图里？

尤其是机器人开机时，如果它不在固定位置，系统不知道它在地图中的初始位姿，这时候只靠局部配准算法往往并不可靠。

所以这套系统的核心目标是：

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

其中最关键的就是：

KISS-Matcher + small_gicp 实现全局重定位。

2. 系统整体架构

系统整体数据流大概如下：

arduino 复制代码

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

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

模块	作用
LIO 模块	负责根据 LiDAR 和 IMU 输出里程计结果
TF 桥接模块	负责统一不同 LIO 后端的 TF 关系
点云生成模块	输出 `/registered_scan`，作为重定位输入
重定位模块	将当前局部点云和先验 PCD 地图对齐
Nav2 模块	负责路径规划、局部避障和导航执行
机器人描述与仿真模块	负责 URDF、Gazebo 和传感器仿真

这样设计之后，LIO 不直接和 Nav2 强绑定，重定位也不和某一个具体里程计算法强绑定。

也就是说，后续可以比较方便地切换：

复制代码

FAST-LIO  ↔  Point-LIO

small_gicp  ↔  ICP  ↔  KISS-Matcher + small_gicp

仿真 URDF  ↔  实机 URDF

这也是我最想要的效果：

每个模块都能独立替换，而不是所有东西揉成一锅粥。

机器人系统最怕的不是某个算法不够强，而是所有模块耦合在一起。

一旦出问题，你根本分不清是 LIO 漂了、TF 错了、点云没对齐，还是 Nav2 在发疯。

最后调着调着，甚至会开始怀疑是不是 Ubuntu 在针对你。

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

系统首先支持建图模式。

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

流程可以理解为：

arduino 复制代码

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

这里二维地图主要给 Nav2 使用，三维 PCD 地图主要给后续点云重定位使用。

我没有强行让 Nav2 直接处理完整三维点云，而是把三维点云进行高度切片，转换成类似二维激光的数据，再送给 Nav2 的代价地图模块。

这一步听起来不够"高大上"，但非常实用。

工程里很多时候不是看架构图有多高级，而是看机器人能不能稳定跑起来。

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 地图进行精细配准，并持续维护机器人在全局地图中的位姿。

最终系统会发布：

arduino 复制代码

map -> odom

这个 TF 很关键。

因为 LIO 本身通常提供的是：

rust 复制代码

odom -> base_footprint

它描述的是机器人在里程计坐标系下的相对运动。

而 Nav2 需要知道机器人在全局地图 map 里的位置。

所以重定位模块要做的事情，就是持续维护：

arduino 复制代码

map -> odom

这样完整 TF 链路就变成：

rust 复制代码

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

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

7. 为什么不只用 small_gicp

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

但这种方式对我来说不够理想。

因为它本质上变成了：

复制代码

第一步：人先猜一个差不多的位置
第二步：机器人证明人猜得还行

这不叫智能，这叫人类辅助智能。

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

所以这套系统采用：

arduino 复制代码

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

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

当然，这句话不能吹得太满。

如果机器人开机位置附近全是空旷区域，点云结构太少，或者当前环境和先验地图差异太大，那再好的算法也救不了。

算法不是玄学，不能指望它凭空气定位。

更准确地说，这套系统实现的是：

在当前点云和先验地图具有足够结构与重叠的情况下，支持无准确初值的全局重定位。

这比"每次靠人手动给初始位姿"要实用很多。

8. 解耦设计带来的价值

除了重定位本身，这套系统另一个重点是解耦。

我希望它不是只能跑某一个固定配置，而是能作为一个实验框架继续扩展。

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

arduino 复制代码

/registered_scan

/Odometry

TF: map -> odom -> base_footprint

PCD map

2D map

这样后续要替换算法时，不需要推翻整个系统。

例如：

替换内容	说明
FAST-LIO / Point-LIO	切换不同激光里程计后端
small_gicp / ICP	切换不同局部点云配准方法
KISS-Matcher + small_gicp	使用全局粗配准 + 局部精配准
仿真机器人 / 实机机器人	切换不同 URDF 和传感器配置

这种结构对机器人调试很重要。

因为实际调试时，经常遇到的问题不是"某个算法一定不行"，而是你不知道到底是哪一层出了问题。

如果模块边界清楚，就可以分别排查：

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

系统不一定优雅，但至少出问题时还能定位。

对于机器人开发来说，这已经很重要了。

9. 当前实现效果

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

9.1 建图模式

建图模式下，机器人在 Gazebo 仿真环境中运动，通过 LiDAR 和 IMU 数据运行 LIO，同时构建二维占用栅格地图，并保存三维 PCD 地图。

这个模式主要用于生成后续导航和重定位所需的先验地图。

9.2 重定位导航模式

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

如果采用 KISS-Matcher + small_gicp 方案，流程如下：

arduino 复制代码

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

在 RViz 中，可以看到机器人通过点云匹配回到全局地图坐标系下。

当 map -> odom 稳定发布后，Nav2 就可以在全局地图中进行路径规划和导航控制。

10. 一些调试经验

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

这是非常关键的一点。

如果 small_gicp 和其他重定位节点同时发布 map -> odom，机器人位姿可能会跳变，Nav2 也会跟着抽风。

这种问题看起来像算法不稳定，实际上是 TF 打架。

两个节点都觉得自己是对的，最后错的是你。

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

KISS-Matcher 虽然可以做全局粗配准，但它不是魔法。

如果机器人刚开机时扫描到的结构太少，或者当前位置和先验地图重叠不够，初始化就可能失败。

比较实用的做法是，在初始化阶段让机器人原地旋转或缓慢移动一小段距离，累计更多局部点云。

点云结构越完整，全局初始化越容易成功。

10.3 坐标系必须统一

常见 TF 链路是：

rust 复制代码

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 自主导航系统，核心不是单纯跑通 Nav2，而是实现两个目标：

第一，使用 KISS-Matcher + small_gicp 完成全局重定位。

KISS-Matcher 负责在无准确初值时进行全局粗配准，small_gicp 负责局部精配准和连续跟踪，最终持续发布：

arduino 复制代码

map -> odom

让 Nav2 获得稳定的全局定位输入。

第二，系统整体采用解耦设计。

LIO、重定位、Nav2、机器人描述和仿真模块之间尽量通过标准 ROS 2 接口连接，方便后续切换：

复制代码

FAST-LIO / Point-LIO

small_gicp / ICP / KISS-Matcher + small_gicp

仿真环境 / 实机环境

目前它还不是完美工程，但已经解决了我最想解决的问题：

机器人不必每次都在固定位置开机，也不必完全依赖人工给初始位姿，而是可以通过 3D 点云尝试完成全局重定位。

如果用一句话总结：

这是一套以全局重定位为核心、以模块解耦为设计目标的 ROS 2 3D LiDAR 自主导航系统。

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

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