宇树GO2建图必读：点云格式与SLAM数据链路全解析

去年拿到宇树机器狗GO2之后，我干的第一个正经活就是把它的SLAM建图功能跑通。入手之前看官方介绍，感觉这玩意儿很省心，自带激光雷达、深度相机，好像开箱就能建图。真上手才发现，建图的底层是点云数据，而点云格式这块就会拦住很多人——不夸张地说，GO2的传感器输出格式、坐标定义和ROS2消息类型之间有一堆绕来绕去的坑，弄不清楚，SLAM根本无从谈起。

这篇就聚焦“点云格式”这个话题，把宇树GO2上做SLAM建图前必须搞明白的点云数据链路完整梳理一遍，包括GO2用到的传感器分别输出什么格式、各自的特点是什么、怎么在ROS2环境里读取和转换、如何选建图方案。我自己踩过的坑会尽可能标出来，尽量帮大家少走弯路。

1. 先搞清楚GO2身上有哪些传感器在产生点云

1.1 主传感器：Livox MID-360激光雷达

GO2配置里最核心的建图传感器是Livox MID-360，这是一款非重复扫描机制的固态激光雷达，出门左转右转就能在市面上买到同款。它的点云输出格式对新手来说非常反直觉，和传统机械雷达完全不是一回事。

传统像Velodyne VLP-16这种机械雷达，扫描线数是固定的，数据按激光线束组织，每个点带激光ID，有明确的水平角度和垂直角度。而MID-360走的是“非重复扫描”路线，采用棱镜加旋转电机的设计，视场角覆盖360°水平、-7°到52°垂直，但同一时刻的点云分布并不均匀。所谓“非重复”指的是：你把这帧点云和上一帧点云对比，点的位置不会完全重合，随着时间积累，视场内的覆盖率会越来越高。

这对建图来说是礼拜一和礼拜天的区别。固定重复扫描的雷达，视场里总有些“永远扫不到”的暗区；非重复扫描会让暗区随积分时间增加而快速减少。有论文专门测过，MID-360积分到0.1秒，覆盖率已经能做到相当高；到0.5秒左右，中心区域的覆盖率就非常可观了。

所以我给GO2做建图的第一条建议就是：不要单帧看点云，要用多帧累积的视角理解它。很多人第一次在rviz2里看到MID-360的点云，会觉得稀疏得像毛发一样，一条走廊扫过去感觉墙面全是洞。这太正常了，单帧非重复扫描的数据就是要靠时间累积才能体现优势。

1.2 辅助传感器：双目深度相机

GO2的头部还有一组双目相机，具体型号是Intel RealSense D435i的相容版本。这组相机输出的深度图可以反投影成彩色点云或深度点云，分辨率比激光雷达高得多，但精度和有效距离跟激光雷达不在一个档次。

D435i这类双目主动立体相机，有效量程大概在0.3米到3米之间，再远误差就飙上去了。室外强光下，主动红外投影基本被太阳光淹没，深度会变得非常差。室内近距离补盲还行，在桌面级物体识别和抓取场景里有价值，但如果指望这组点云参与大范围SLAM，基本很难达到预期。

我在GO2上实测，近距离（3米内）融合D435i点云，确实能让建图出来的近距离物体轮廓细腻很多，比如桌面上的充电线接口、工具箱里的扳手轮廓，激光雷达往往一扫而过，深度相机能补充不少细节。但代价是要做时间同步和坐标变换，处理不好就是豆腐渣工程。

1.3 其他数据源：轮式里程计和IMU

严格来说，里程计和IMU并不直接输出点云，但它们输出的是点云配准、畸变去除和建图位姿的基准。

GO2机身自带高精度编码器，底盘可以输出轮速里程计，频率能达到100Hz以上，短时间短距离内精度靠得住。IMU负责提供高频姿态参考，加速度和角速度数据对去除激光雷达运动畸变特别重要。GO2四足行走时，机身俯仰和翻滚的幅度比轮式机器人明显大得多，尤其快步态或小跑时，每走一步机身都会抖三抖。如果不用IMU做运动补偿，激光点云会在行走过程中产生肉眼可见的拖影，建图根本没法看。

2. 点云格式：从Livox私有格式到ROS标准格式的转换

2.1 为什么不能直接用Livox原始数据

这部分应该算是整篇文章的核心。Livox MID-360上手的第一个难题，就是它的驱动不会直接输出ROS标准格式的sensor_msgs/msg/PointCloud2。

Mid-360驱动默认输出的点云类型是livox_ros_driver2/msg/CustomMsg。这个Msg是Livox公司为了适配自家产品特殊的数据结构——非重复扫描的运动补偿偏移、回波数量、点云ID、时间戳等——专门设计的。拜这个自定义格式所赐，很多标准SLAM算法（比如GMapping、Cartographer原版、LIO-SAM的Livox适配版之前的版本）在读取阶段就崩了，因为它们默认输入是标准PointCloud2。

想直接用开源SLAM算法，必须先把这个CustomMsg转成标准PointCloud2，再喂给SLAM算法。宇树官方SDK底层其实已经在Ubuntu 20.04 + ROS1的环境里做过这层转换，但从这份SDK移植到ROS2时，这个转换环节经常被忽略。不少人在ROS2里订阅/goal2/livox/lidar这个Topic，看到的要么是空数据，要么一订阅就类型报错，往往就是没做CustomMsg到PointCloud2的转换。

2.2 CustomMsg和PointCloud2的结构差异对比

简单列个对比表帮助理解：

这个表的含义是：即便CustomMsg里头的空间坐标本身没有错，点云在rviz2里看起来也是“能显示”，但它和标准算法之间就隔着一道墙。尤其注意每个点的时间戳差异——MID-360的一个扫描周期内，不同点是在不同时间被击中的，而点云被当做一个整体去订阅时，时间基准是乱的。四足行走时的抖动一叠加，点云畸变非常明显。

2.3 转换工具链：livox_ros_driver2自带的转换功能

Livox官方在livox_ros_driver2仓库里其实提供了CustomMsg到PointCloud2的转换节点。需要好好看它的launch文件，很多使用者压根没注意，默认launch里已经带了这个转换。

在ROS2下使用这个驱动的典型步骤是这样：

1. 把livox_ros_driver2源码clone到你的ROS2工作空间。
2. 编译时确保依赖项（比如ROS2 humble或foxy）已装好。
3. 运行ros2 launch livox_ros_driver2 rviz_MID360.launch.py。
4. 这个launch里会启动livox driver，并在同一个文件里带一个pointcloud_to_laserscan或custom_msg_to_pointcloud2的转换节点（取决于版本）。

需要注意，不同版本仓库的launch配置不完全一样，有的把转换节点注释掉，有的默认开启。最简单的验证方法是启动后ros2 topic list，看是否出现类似/livox/lidar/pointcloud2的topic。如果只有/livox/lidar这种CustomMsg，就要自己手动启动一个转换节点。

如果驱动版本里没带现成转换节点，用ROS2的launch文件自己补一个也不难，核心代码思路就是把CustomMsg里的点按字段映射到PointCloud2的PointField，偏移量、数据类型、点数这三个要素对齐就行。网上有现成的实现，搜livox_custom_msg_to_pointcloud2能找到大量参考。

2.4 时间戳与坐标系的统一问题

转成PointCloud2只是第一步。发到SLAM算法里之前，还有两件事要做：时间同步和坐标系变换。

GO2的机器人和底层SDK之间，传感器数据的时间戳基准并非天然统一。主控板可能给激光雷达的时间戳是雷达上电后的相对时间，IMU则是系统启动后的时间，相机又是另一个时间线。做SLAM时，算法会把激光帧和IMU数据按时间戳找最近邻匹配。如果时间基准不一致，匹配就全错。

我的做法比较笨但稳定：先跑一遍录包，把每个传感器topic的header.stamp打印出来对比，肉眼确认它们的时间范围是否在同一区间。如果不同，就在驱动层面把时间戳改成统一时钟源，或者用message_filters做近似时间同步（允许20ms以内的偏差）。GO2上实测，20ms同步窗口在步行状态下基本够用，但小跑或快跑时还是要减小到5ms内才靠谱。

坐标系方面，Livox雷达输出默认是雷达本体坐标系，IMU是IMU坐标系，轮速里程计是底盘坐标系。SLAM前必须在URDF或TF树里把雷达、IMU、底盘的相对位姿关系描述清楚。宇树官方SDK导出了一个包含机身link和传感器link的URDF，但坐标系间的外参标定精度并不高，尤其是激光雷达相对于IMU的旋转和平移。对建图结果要求高的话，建议做一个手眼标定或基于建图反向优化外参。这个坑我踩过一次，外参大概偏了2度，建图出来的墙面转角全是弧形的，找了好久才发现是外参问题。

3. GO2上建图方案的选型与点云格式的适配

3.1 方案一：Fast-LIO2（基于Livox点云）

Fast-LIO2是目前在GO2上跑得最顺的方案之一，因为它本身就是为Livox系列雷达设计的，能直接吃CustomMsg格式，不需要先转PointCloud2。代码仓库里对MID-360的支持也比较完善，直接提供MID-360的配置文件。

Fast-LIO2的特点是把IMU和LiDAR做紧耦合，用IMU预测运动、激光去畸变、更新位姿。对GO2这种四足机器人来说，IMU的角速度频繁变化，运动畸变严重，Fast-LIO2的紧耦合优势就非常明显。我在GO2上测试，匀速慢走时Fast-LIO2的轨迹偏差大概能控制在厘米级；跑起来之后会稍微差一些，但整体仍然可用。

跑Fast-LIO2时点云格式这块基本不用操心，就是要注意配置里的lid_topic名称要指向CustomMsg那个topic。如果偶然买到的是已经转好的PointCloud2版本，也可以改配置让Fast-LIO2接收PointCloud2，但那样反而会丢掉一些Livox特有的时间偏移信息，不是最优解。

3.2 方案二：LIO-SAM（先转标准格式）

LIO-SAM的原始仓库主要是针对机械雷达设计的，但网上有一堆适配Livox和GO2的fork版本。跑LIO-SAM前，点云必须转换成PointCloud2，因为它内部用PCL处理点云，PCL不认识CustomMsg。

适配LIO-SAM的典型做法是：

1. 用livox_ros_driver2的转换节点把/CustomMsg转成PointCloud2。
2. 把LIO-SAM的lidar topic指向转换后的PointCloud2。
3. 配置里确保N_SCAN和Horizon_SCAN参数和MID-360的点云分布匹配。很多人在这一步摔跟头——如果用MID-360的非重复扫描点云当普通旋转雷达点云用，LIO-SAM的特征提取逻辑会非常别扭，因为它默认按固定扫描线找edge和surface point。非重复扫描下没有明确的扫描线，匹配质量会退化。

我会推荐先用Fast-LIO2把点云状态打底，再用LIO-SAM做优化，两者输出对比着看，这样建图结果更可靠。

3.3 方案三：Cartographer（适合回环密集型环境）

Cartographer对输入点云的要求是标准PointCloud2，但它不要求激光雷达必须是机械式，它更依赖子图（submap）匹配和回环检测来压误差。GO2如果在室内走廊、实验室这类特征重复、但距离不算特别长的环境下跑，Cartographer的表现反而很稳。

Cartographer跑起来有个麻烦：GO2行走时姿态变化大，Cartographer默认假设机器人在平面上运动，这就容易出问题。解决办法是开启3D SLAM模式，或者把IMU数据接入Cartographer的位姿估计器。GO2的IMU可以直接给Cartographer提供重力对齐的位姿先验，这样在斜坡或起伏地形下也能跑。

点云格式适配Cartographer同样要先转成PointCloud2，如果用3D SLAM模式，对点云畸变还比较敏感，所以运动补偿这步不能省。

3.4 方案四：其他方案与可视化工具

除了上面三个主流方案，现在也有不少基于深度学习的端到端SLAM，但对硬件要求高，GO2的机载算力跑起来比较勉强。不建议在GO2机载端做实时学习型SLAM，更适合后台离线处理。

可视化建图效果推荐用rviz2。前提还是那个：所有点云topic尽量统一成PointCloud2，不然rviz2里虽然有CustomMsg的插件能显示，但很多特效和测量工具不可用。另外可以安装pointcloud_to_laserscan包，把点云转成2D LaserScan，这样在rviz2里就能用2D代价地图直接看建图结果，调试更直观。

4. 实操：从驱动安装到点云Topic正常输出

4.1 在GO2上安装Livox驱动

GO2出厂时的系统环境一般已经是Ubuntu 20.04或22.04，具体看批次。给MID-360装驱动，直接编译livox_ros_driver2：

mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd ~/ros2_ws colcon build --packages-select livox_ros_driver2 source install/setup.bash

编译前确认环境变量ROS_DISTRO正确，比如export ROS_DISTRO=humble。如果编译过程中报缺少livox_interfaces依赖，先把源码里自带的接口包单独编译一遍。我遇到过一次CMake找不到livox_interfaces的情况，就是因为先编译了驱动包，没先把接口包编好。

4.2 ROS2下读取点云Topic并验证格式

驱动启动之后，用如下命令查看所有topic：

ros2 topic list

正常应该有/livox/lidar（CustomMsg）和/livox/lidar/pointcloud2（PointCloud2，如果默认转换节点已启动）。

查看消息类型：

ros2 topic info /livox/lidar ros2 topic info /livox/lidar/pointcloud2

前者应该是livox_ros_driver2/msg/CustomMsg，后者是sensor_msgs/msg/PointCloud2。如果发现只有CustomMsg，没有PointCloud2，说明转换节点没启动，去launch文件里把转换节点取消注释，或手动启动一个转换节点。

验证点云内容的一种快速方式是用rviz2直接Add一个PointCloud2显示，主题选/livox/lidar/pointcloud2，坐标系选livox_frame或雷达link，此时应该能看到点云。如果画面里什么都没有，先检查TF树：

ros2 run tf2_ros tf2_echo map livox_frame

没有TF的话，rviz2完全找不到点云位置，就算有数据也显示不出来。

4.3 把点云话题接入建图程序

以Fast-LIO2为例，修改配置文件里的lid_topic和imu_topic：

common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu"

启动后观察控制台输出。如果一直打印“wait for point cloud data”，说明话题没对上或时间戳不匹配。常见原因是IMU数据没发布，或者发布频率不够。Fast-LIO2对IMU很敏感，IMU频率至少要达到100Hz以上才能稳定跑起来，GO2的IMU通常没问题，但要确认驱动里IMU数据发布确实打开了。

5. 常见点云格式相关故障与排查实录

5.1 rviz2里看不到点云，但topic有数据

这类问题九成出在TF上。有数据、有时间戳，但rviz2不知道点云在哪个坐标系下，没有TF树就会放弃显示。解决方法是先把点云Topic的Fixed Frame换成点云消息Header里写的坐标系名称，能显示出来就说明是TF问题，然后去检查URDF或静态变换发布。

另外文件夹权限也会坑人。GO2机载系统有时候用户名不是sudo组，导致某些配置写不进去，驱动编译出的launch文件无法创建log目录，间接导致节点没起来。

5.2 点云出现严重的拖影或扭曲

GO2小跑时最容易出现这个问题。原因基本是运动畸变没被补偿。Fast-LIO2这种方案里，去畸变依赖IMU的高频姿态。如果IMU数据延迟比较大，拖影就会很明显。

我的排查顺序是：

1. 先看IMU数据频率是否稳定在200Hz左右。
2. 再看IMU和雷达时间戳之间的延迟，允许微调配置里的时间偏移。
3. 最后看外参是否正确，外参错了点云会整体倾斜或扭曲而非单纯拖影。

如果三者都正常但拖影依旧，可能是GO2的步频超过了算法处理帧率，适当降低行走速度或调低建图分辨率能缓解。

5.3 CustomMsg转PointCloud2后丢失了时间信息

有些SLAM算法需要点云内部的时间信息做去畸变，比如LIO-SAM。如果直接转成标准PointCloud2，各点独立时间戳就丢了，只剩整帧时间戳，去畸变能力大打折扣。这时要么改用支持CustomMsg的Fast-LIO2，要么在转换时把时间偏移存到自定义字段里，再改算法读取。

从我的实践看，在GO2上做纯建图，Fast-LIO2是省心之选；如果要做地图后处理、多传感器融合，再考虑LIO-SAM的生态里的工具。但无论选哪个，对点云格式链路有个清醒的认识，都能少花很多冤枉时间。

5.4 建图飘移严重，回环闭环无效

GO2在长走廊或特征稀疏区域，飘移几乎没办法避免。点云格式对回环的影响主要体现在：特征点不够时，匹配的位姿解算不唯一，回环检测又因为视野重复度低而找不到闭环。解决办法是让机器人二次经过同一区域，营造更多闭环机会；同时降低建图速度，保证足够的点云密度。GO2虽然能跑很快，但SLAM建图时建议慢走，效果差很多。

6. 一点实操心得：点云格式这件事值得花时间吃透

回头看我第一次折腾GO2建图的过程，最大弯路就是没搞清楚点云格式的层级关系，以为所有数据都是PointCloud2，结果在rviz2里折腾半天，建图程序一启动就各种报错。后来老老实实把Livox CustomMsg、PointCloud2、LaserScan这三层的数据流贯通了，后面不管是换算法、换传感器还是换环境，都能很快适应。

如果你现在也卡在GO2建图第一步，我的建议是先别急跑算法，把“数据链路打通+可视化能看”作为第一个里程碑。激光雷达有数据、IMU有数据、TF树正确、点云能稳定显示，到这一步，基本就已经成功了七成。剩下的，无非是选一个合适的算法，把参数稍微调一调而已。

最后再说一个小技巧：调试时可以做一个录包习惯，把所有topic都录成rosbag2，关键时候回放比现场重现Problem要高效得多。我在GO2上测地形适应时，跑一遍包下来，再反复调整参数重放，能省掉大量机器人来回走路的时间。这也是我后来建议所有玩GO2建图的朋友最先养成的好习惯。