从 LIO 到 Nav2：我把 FAST-LIO改造成了机器人能用的底盘里程计

项目已开源至Github，欢迎Star

github.com/Ikunio/Lida...

做 3D LiDAR 导航时，很多人会遇到一个很微妙但很折磨的问题：

FAST-LIO 能跑，Point-LIO 也能跑，点云建图看着也挺帅，但一接 Nav2，机器人就开始怀疑人生。

RViz 里点云很稳，轨迹也有，甚至 /cloud_registered 看起来像那么回事。

但到了机器人导航这一步，问题就来了：

Nav2 想要的是 odom -> base_footprint
LIO 算法输出的却往往是 camera_init -> body
FAST-LIO 和 Point-LIO 的里程计话题还不一样
点云坐标系、车体坐标系、雷达坐标系、IMU 坐标系混在一起
最后 TF 树一看：像一碗没拌匀的麻酱面

所以我在这个 ROS 2 3D LiDAR 导航工作空间里，专门做了一层 LIO 里程计桥接器。

它的作用很简单，但非常关键：

把 FAST-LIO / Point-LIO 这种偏算法内部使用的 LIO 位姿，转换成机器人导航真正需要的标准里程计：odom -> base_footprint。

换句话说，它不是为了让 LIO "看起来能跑"，而是为了让 LIO 真的能被机器人拿来导航。

1. 问题本质：LIO 的里程计，不等于机器人底盘的里程计

很多 LIO 算法输出的位姿，本质上描述的是：

css 复制代码

camera_init -> body

其中：

camera_init：LIO 初始化时建立的世界坐标系
body：算法内部使用的机体系，通常和 IMU / LiDAR 外参强相关
位姿含义：当前传感器 body 相对于初始化坐标系的运动

这对 LIO 算法本身没问题。

因为算法只关心：

复制代码

我现在相对刚启动时，动到了哪里？

但机器人导航系统，尤其是 Nav2，更关心的是：

复制代码

我的底盘中心在哪里？
我能不能基于这个底盘位姿规划、避障、控制？

这就是区别。

LIO 输出的是"传感器 / 算法 body 的位姿"，

Nav2 需要的是"机器人底盘的位姿"。

一个是算法视角。

一个是机器人视角。

这俩要是不桥接，Nav2 就像拿着体检报告去修车------不是没信息，是信息语义不对。

2. Nav2 真正想要什么？

在移动机器人导航里，常见 TF 树应该长这样：

arduino 复制代码

map
 └── odom
      └── base_footprint
           └── chassis / base_link
                └── livox_frame

其中：

map -> odom：由重定位 / 全局定位模块发布
odom -> base_footprint：由里程计模块发布
base_footprint -> chassis / base_link：机器人底盘静态关系
base_link / chassis -> livox_frame：雷达外参

这里面最关键的是：

rust 复制代码

odom -> base_footprint

这条 TF 是局部连续里程计。

Nav2 的局部代价地图、控制器、轨迹跟踪，都需要它。

所以，如果你直接把 LIO 的 camera_init -> body 原样丢给 Nav2，问题就可能出现：

body 不一定是底盘中心
body 可能带有雷达 / IMU 安装偏移
camera_init 不是标准语义下的 odom
机器人底盘高度、roll、pitch 可能影响 2D 导航
FAST-LIO / Point-LIO 输出接口不统一

结果就是：

css 复制代码

LIO：我没问题，我输出了位姿。
Nav2：你这位姿是谁的？
LIO：body 的。
Nav2：body 又是谁？
LIO：这你别管。
Nav2：那我也别跑了。

3. 桥接器要解决什么？

这个桥接器的目标不是重新写 LIO，也不是重新造 Nav2。

它做的是中间这层最容易被忽略、但工程上非常关键的事情：

markdown 复制代码

FAST-LIO / Point-LIO 输出
        ↓
统一里程计接口
        ↓
坐标系语义转换
        ↓
发布机器人标准 odom
        ↓
Nav2 可直接使用

核心目标有三个：

3.1 统一 FAST-LIO 和 Point-LIO 的输出

FAST-LIO 和 Point-LIO 都是 LIO，但输出接口并不完全一样。

典型情况是：

bash 复制代码

FAST-LIO  → /Odometry
Point-LIO → /aft_mapped_to_init

如果每次切换 LIO 后端，都要改 Nav2、改重定位、改 TF、改参数，那这个系统就很难维护。

所以桥接器第一步就是统一输入：

bash 复制代码

FAST-LIO /Odometry
Point-LIO /aft_mapped_to_init
        ↓
统一转成标准里程计数据

这样后面的 Nav2、重定位、点云切片模块，都不需要关心你前面用的是 FAST-LIO 还是 Point-LIO。

前端可以换，后端不用崩。

这就叫解耦。

说人话就是：

你 LIO 在前面卷你的算法，我 Nav2 在后面开我的车，大家通过标准接口说话，别互相折磨。

3.2 把 `camera_init -> body` 转成 `odom -> base_footprint`

这是桥接器最核心的部分。

LIO 内部常见位姿可以理解为：

复制代码

T_camera_init_body

但机器人导航需要的是：

复制代码

T_odom_base_footprint

所以我们要完成语义转换：

css 复制代码

camera_init  ≈ odom
body         → base_footprint

但注意，这里不是简单改名字。

不是把 frame_id 从 camera_init 改成 odom，

再把 child_frame_id 从 body 改成 base_footprint 就完事。

那叫"TF 化妆"，不叫"TF 转换"。

真正要考虑的是：

ini 复制代码

T_odom_base_footprint = T_camera_init_body × T_body_base_footprint

其中：

T_camera_init_body 来自 LIO
T_body_base_footprint 是 body 到底盘中心的外参关系
输出结果才是机器人底盘在 odom 下的位置

如果 body 本身和底盘中心完全重合，那这一步可以简化。

但在真实机器人上，LiDAR / IMU 往往安装在车体上方或前方，不一定在底盘旋转中心。

如果不处理这层外参，机器人在 Nav2 里就可能出现：

车体中心偏移
旋转时轨迹绕错点
局部代价地图和真实底盘不重合
雷达点云看似正常，但底盘 footprint 位置不对
导航时"明明没撞，地图上已经撞了；明明撞了，地图上还说没事"

这类问题最阴间，因为它不是代码直接报错，而是机器人用行为告诉你：

我能跑，但我不想正常跑。

3.3 让 3D LIO 更适合 2D 移动机器人导航

FAST-LIO / Point-LIO 都是 3D LiDAR-Inertial Odometry。

它们可以估计 6DoF 位姿：

复制代码

x, y, z, roll, pitch, yaw

但大多数室内移动机器人，尤其是四轮滑移底盘，Nav2 实际上更关心平面运动：

复制代码

x, y, yaw

也就是说，机器人可以在 3D 世界里感知，但底盘控制主要还是 2D 运动。

所以桥接器需要把 LIO 的 3D 位姿整理成适合底盘导航的形式：

markdown 复制代码

LIO 6DoF Pose
      ↓
提取 x / y / yaw
      ↓
构造 base_footprint 平面位姿
      ↓
发布 odom -> base_footprint

这样做的好处是：

保留 LIO 的高精度位移估计
避免 roll / pitch 直接污染 2D 导航
让 Nav2 使用更稳定的底盘平面坐标
让代价地图、规划器、控制器的坐标语义更清晰

如果不做这层处理，机器人在坡度、IMU 抖动、雷达安装倾角变化时，Nav2 可能会出现奇怪的位姿抖动。

3D LIO 很强，但 Nav2 不需要你把所有 3D 姿态都塞给它。

就像你问一个人今天吃什么，他给你背了一篇《舌尖上的中国》------信息量很大，但不一定能下单。

4. 桥接后的系统结构

整个系统可以理解成下面这条链路：

bash 复制代码

Livox MID-360 + IMU
        ↓
FAST-LIO / Point-LIO
        ↓
LIO Interface
        ↓
sensor_scan_generation
        ↓
/odom
/tf: odom -> base_footprint
/registered_scan
        ↓
3D 重定位 / 2D 激光切片 / Nav2

其中桥接层主要承担两个任务。

第一层：`lio_interface`

它负责把不同 LIO 后端的输出统一起来。

makefile 复制代码

FAST-LIO:
  /Odometry

Point-LIO:
  /aft_mapped_to_init

统一后:
  标准 LIO 位姿输出

它解决的是"不同算法接口不一致"的问题。

第二层：`sensor_scan_generation`

它负责面向机器人导航发布标准数据：

bash 复制代码

/tf: odom -> base_footprint
/odom
/registered_scan

它解决的是"算法输出不能直接给 Nav2 用"的问题。

这两层加起来，就把系统从：

复制代码

算法能跑

提升到了：

复制代码

机器人能用

这两句话看起来差不多，但工程上差了一个通宵。

5. 为什么这个桥接器很重要？

因为机器人系统不是单个算法 Demo。

一个完整的 ROS 2 导航系统里面，至少有这些模块：

LIO 里程计
TF 树
3D 点云重定位
2D 激光切片
Nav2 代价地图
局部规划器
全局规划器
底盘控制
仿真 / 实机切换

每个模块都对坐标系有预期。

如果坐标系语义不统一，就会出现经典问题：

复制代码

LIO 说自己没问题
Nav2 说 TF 不对
RViz 说看起来还行
机器人说我选择撞墙

这时候你去调参数，往往越调越乱。

真正的问题不一定是：

复制代码

Nav2 参数没调好

而可能是：

复制代码

你喂给 Nav2 的里程计，本来就不是机器人底盘里程计

所以桥接器的价值在于：

它把 LIO 的"算法位姿"，翻译成 Nav2 能理解的"机器人底盘位姿"。

这一步做好了，后面的重定位、导航、代价地图、控制器才有稳定基础。

6. FAST-LIO 和 Point-LIO 可以自由切换，后端不用大改

这个工作空间里同时支持 FAST-LIO 和 Point-LIO。

两者各有特点：

FAST-LIO：成熟、稳定、工程使用广泛
Point-LIO：高频、响应快，对部分激烈运动场景更友好

如果没有桥接器，切换 LIO 后端时，可能要连带修改：

订阅话题
TF frame
odom 发布逻辑
点云输入话题
重定位输入
Nav2 参数

但加入桥接层之后，系统结构就变成：

markdown 复制代码

FAST-LIO  ┐
          ├── lio_interface ── 标准 odom / TF / registered_scan ── Nav2
Point-LIO ┘

后端看到的是统一接口。

所以你可以更专注地比较：

复制代码

当前场景更适合 FAST-LIO，还是 Point-LIO？

而不是每换一次算法，就把整个 ROS 2 工作空间重新做一次"开颅手术"。

7. 和重定位模块的关系：`odom` 稳了，`map -> odom` 才有意义

在这个系统里，重定位模块会发布：

arduino 复制代码

map -> odom

而 LIO 桥接器发布：

rust 复制代码

odom -> base_footprint

最终机器人在地图中的位姿就是：

rust 复制代码

map -> odom -> base_footprint

这个结构非常关键。

LIO 负责局部连续运动。

重定位负责把局部里程计对齐到全局地图。

Nav2 负责基于全局地图规划和局部避障。

如果 odom -> base_footprint 本身就不稳定，

那么 map -> odom 再准也救不回来。

这就像你用高精度 GPS 给一个轮子歪的车导航。

地图是准的，路线是对的，车是斜着跑的。

所以桥接器是重定位和 Nav2 的地基。

地基不稳，别说 KISS-Matcher，亲嘴都 Matcher 不上。

8. 工程实现思路

桥接器的核心实现思路可以概括为：

ini 复制代码

// 1. 订阅 LIO 输出
subscribe(lio_odom_topic);

// 2. 读取 LIO 位姿
T_camera_init_body = odom_msg.pose;

// 3. 查询或配置 body 到 base_footprint 的外参
T_body_base = getStaticExtrinsic();

// 4. 计算机器人底盘位姿
T_odom_base = T_camera_init_body * T_body_base;

// 5. 根据移动机器人需求处理平面位姿
x = T_odom_base.x;
y = T_odom_base.y;
yaw = extractYaw(T_odom_base.rotation);

// 6. 发布标准 odom 消息
publish(nav_msgs::msg::Odometry);

// 7. 广播 TF
broadcastTransform("odom", "base_footprint");

逻辑不复杂，但语义非常重要。

这里最容易犯的错误是：

ini 复制代码

child_frame_id = "base_footprint";

然后直接把 LIO 的 body 位姿塞进去。

这相当于把身份证名字改成"底盘中心"，但本人还是雷达。

Nav2 不一定立刻报警，但机器人迟早用行为艺术表达不满。

9. 这个设计带来的收益

9.1 Nav2 接入更自然

Nav2 不需要理解 FAST-LIO / Point-LIO 的内部坐标系。

它只需要标准 TF：

rust 复制代码

odom -> base_footprint

和标准里程计话题：

bash 复制代码

/odom

这让系统更符合 ROS 2 移动机器人导航的通用范式。

9.2 仿真和实机更容易统一

仿真和实机最大的区别通常在传感器驱动、URDF、时间源、点云格式。

但导航后端最好保持一致。

桥接器把前端 LIO 的差异消化掉，让后端统一使用：

bash 复制代码

/odom
/registered_scan
odom -> base_footprint

这样就可以实现：

复制代码

仿真调通 → 实机少改 → 直接迁移

不是那种"仿真里猛如虎，实机上原地杵"的系统。

9.3 后续模块更容易复用

只要桥接器输出稳定，后面模块都可以复用：

pointcloud_to_laserscan
small_gicp_relocalization
global_relocalization_kiss_matcher
Nav2
RViz 可视化
底盘控制接口

LIO 可以换，导航不用换。

雷达可以换，TF 语义不乱。

这就是工程系统里面最值钱的东西：模块边界清楚。

10. 总结：这个桥接器不是配角，它是 LIO 走向机器人导航的翻译官

很多 3D LiDAR 项目卡住，不是因为 LIO 不够强，也不是因为 Nav2 不好用。

而是中间缺了一层：

复制代码

把算法位姿转换成机器人位姿的工程桥接层

FAST-LIO / Point-LIO 输出的是 LIO 世界里的位姿。

Nav2 需要的是移动机器人世界里的里程计。

所以这个桥接器做的事情，本质上是：

rust 复制代码

camera_init -> body
        ↓
odom -> base_footprint

它把"算法能跑"变成"机器人能用"。

如果说 LIO 是机器人的空间感知能力，

Nav2 是机器人的行动决策能力，

那这个桥接器就是中间的翻译官。

没有它，LIO 在讲高等数学，Nav2 在等普通话。

有了它，机器人终于听懂了：

你现在在哪，车头朝哪，可以往哪走。

项目地址：

arduino 复制代码

https://github.com/Ikunio/Lidar_nav2_ws

如果你也在做 ROS 2、Livox MID-360、FAST-LIO、Point-LIO、3D LiDAR 重定位或者 Nav2 导航，可以看看这个工作空间。

别再让 Nav2 硬吃 LIO 原始坐标系了。

机器人不是不能跑，它只是需要一个靠谱的翻译。

从 LIO 到 Nav2：我把 FAST-LIO改造成了机器人能用的底盘里程计

1. 问题本质：LIO 的里程计，不等于机器人底盘的里程计

2. Nav2 真正想要什么？

3. 桥接器要解决什么？

3.1 统一 FAST-LIO 和 Point-LIO 的输出

3.2 把 camera_init -> body 转成 odom -> base_footprint

3.3 让 3D LIO 更适合 2D 移动机器人导航

4. 桥接后的系统结构

第一层：lio_interface

第二层：sensor_scan_generation

5. 为什么这个桥接器很重要？

6. FAST-LIO 和 Point-LIO 可以自由切换，后端不用大改

7. 和重定位模块的关系：odom 稳了，map -> odom 才有意义

8. 工程实现思路

9. 这个设计带来的收益

9.1 Nav2 接入更自然

9.2 仿真和实机更容易统一

9.3 后续模块更容易复用

10. 总结：这个桥接器不是配角，它是 LIO 走向机器人导航的翻译官

3.2 把 `camera_init -> body` 转成 `odom -> base_footprint`

第一层：`lio_interface`

第二层：`sensor_scan_generation`

7. 和重定位模块的关系：`odom` 稳了，`map -> odom` 才有意义