去年拿到宇树机器狗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的结构差异对比简单列个对比表帮助理解项目Livox CustomMsgsensor_msgs/PointCloud2数据组织按点流式排列带点个数计数按字段列组织可含多字段x,y,z,intensity,ring等时间戳每个点带offset_time精度nsHeader时间戳为整帧统一各点不带独立时间坐标雷达局部坐标系遵循ROS REP-103约定通常是ENU或FLU扩展字段有tag回波方向/类型、line扫描线计数自定义字段名标准viewer才能正确解析兼容性仅Livox系工具链ROS生态通用rviz2/PCL/各类SLAM直接吃这个表的含义是即便CustomMsg里头的空间坐标本身没有错点云在rviz2里看起来也是“能显示”但它和标准算法之间就隔着一道墙。尤其注意每个点的时间戳差异——MID-360的一个扫描周期内不同点是在不同时间被击中的而点云被当做一个整体去订阅时时间基准是乱的。四足行走时的抖动一叠加点云畸变非常明显。2.3 转换工具链livox_ros_driver2自带的转换功能Livox官方在livox_ros_driver2仓库里其实提供了CustomMsg到PointCloud2的转换节点。需要好好看它的launch文件很多使用者压根没注意默认launch里已经带了这个转换。在ROS2下使用这个驱动的典型步骤是这样把livox_ros_driver2源码clone到你的ROS2工作空间。编译时确保依赖项比如ROS2 humble或foxy已装好。运行ros2 launch livox_ros_driver2 rviz_MID360.launch.py。这个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的典型做法是用livox_ros_driver2的转换节点把/CustomMsg转成PointCloud2。把LIO-SAM的lidar topic指向转换后的PointCloud2。配置里确保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_driver2mkdir -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_DISTROhumble。如果编译过程中报缺少livox_interfaces依赖先把源码里自带的接口包单独编译一遍。我遇到过一次CMake找不到livox_interfaces的情况就是因为先编译了驱动包没先把接口包编好。4.2 ROS2下读取点云Topic并验证格式驱动启动之后用如下命令查看所有topicros2 topic list正常应该有/livox/lidarCustomMsg和/livox/lidar/pointcloud2PointCloud2如果默认转换节点已启动。查看消息类型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_topiccommon: 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数据延迟比较大拖影就会很明显。我的排查顺序是先看IMU数据频率是否稳定在200Hz左右。再看IMU和雷达时间戳之间的延迟允许微调配置里的时间偏移。最后看外参是否正确外参错了点云会整体倾斜或扭曲而非单纯拖影。如果三者都正常但拖影依旧可能是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建图的朋友最先养成的好习惯。