宇树GO2机器狗SLAM建图实战：点云格式解析与转换指南

拿到宇树机器狗go2，接好电源、连上工控机、启动雷达驱动，最让人上头的不是看它走路，而是让它把周围环境“画”出来。但真上手搞SLAM建图，第一个把我干趴下的不是算法调参，而是点云格式。驱动节点明明在跑，话题也有数据，可SLAM程序就是不吃，rviz里面一片黑，查了半天才发现，问题出在消息类型上：同一颗雷达发了两套点云，一套是私有格式，一套是标准格式，选错了，后面全是白忙。

这篇文章是go2 SLAM建图系列的第一篇，专门把“点云格式”这件事掰开揉碎讲清楚。你要是手里也有一台go2，或者这类搭载Livox雷达的四足机器人，正打算跑通建图流程，那这篇文章能帮你少走很多弯路。我会从数据链路、消息结构、文件格式、实测转换、算法兼容性这几个维度，把点云格式这个“地基”给打牢。

1. 为什么要先从“点云格式”开篇

1.1 SLAM建图的输入侧：一帧点云到底是什么

SLAM这个缩写听着高深，拆开就是定位加建图。激光雷达往四周扫一圈，打出无数个激光点，每个点都有一个三维坐标，这些点攒在一起，就是一帧点云。机器狗往前走，雷达不停扫，SLAM算法把一帧一帧的点云配准、拼接起来，同时估算出机器人在哪里，最后形成一张地图。

但这里有个很多人忽视的细节：激光雷达“扫出来”的东西，和SLAM算法“吃进去”的东西，以及你在可视化软件里“看到”的东西，往往不是同一种数据形态。同样是这一帧点云，在驱动里可能是一段二进制缓冲，在话题上可能是PointCloud2消息，存到文件里可能是PCD，也可能是PLY、LAS。不同形态之间的差异，就是标题里说的“点云格式”。

我见过太多人在这一步栽跟头。有的是录了一整天的bag包，回放时才发现消息类型不对，算法根本不认；有的是好不容易把PointCloud2转成了PCD，结果用CloudCompare打开全是一片黑，才发现强度字段的偏移量写错了。这些问题的根源只有一个：没搞懂点云格式背后的数据结构。

1.2 格式不匹配的典型翻车现场

先描述一个我从身边人那里反复看到的场景。有人拿到go2之后，发现驱动发布了两个很相似的话题：/livox/lidar，消息类型是livox_ros_driver2/msg/CustomMsg；另一个是/livox/lidar/pointcloud2，消息类型是sensor_msgs/msg/PointCloud2。他顺手把前一个话题当作输入丢给了LIO-SAM，结果程序直接报错说消息类型不匹配，然后就开始怀疑人生——明明话题上有数据，为什么SLAM跑不起来？

还有一个场景是在离线处理时。录了个bag包，里面既有/livox/lidar又有/livox/lidar/pointcloud2，还有/tf。有人图省事，只用自定义节点读CustomMsg，然后手动转点云，结果写出来的代码里时间戳用的是雷达的微秒计数，没有转成ROS时间，导致后续所有帧的坐标变换全部错乱，地图直接重影。这两个案例的共同点就是：把“点云格式”当成小事，结果被格式狠狠地教育了一顿。

2. 宇树机器狗go2点云数据链路梳理

2.1 搭载的Livox MID-360雷达特性

go2和很多国产四足机器人一样，采用外接或预装激光雷达的方案，市面上用得非常多的一款是Livox MID-360。这颗雷达有个很特别的地方：它用的是非重复扫描技术，不像传统机械雷达那样一圈圈地转，而是让激光束在一个视场里来回扫描，点云分布更均匀，时间越长覆盖越密。

MID-360几个硬指标你得有数：水平视场角360度，垂直视场角59度（-7度到52度），测距范围最远40米，精度在2厘米左右，每秒最多输出20万个点。整颗雷达通过网口和工控机通讯，在ROS里发布点云。这意味着它的数据流本质上是你电脑上多了一张“网卡”，通过UDP协议接收雷达数据包，再由Livox驱动解析成点云。

理解硬件特性有什么用？直接关系到格式处理。非重复扫描的点云在时间上是连续积聚的，一帧200毫秒里点云数量不是固定的，每帧点数会上下浮动。这和机械雷达的“每帧点数固定”完全不同。后面做SLAM时，如果算法对点数或分辨率很敏感，就得对点云做降采样或者规整化，这些操作的基础就是先把格式吃透。

2.2 从驱动到话题：点云的两种“长相”

用Livox官方ROS2驱动启动后，你会看到好几个话题，最核心的是这两个：

• /livox/lidar：类型为livox_ros_driver2/msg/CustomMsg，这是Livox私有消息格式。
• /livox/lidar/pointcloud2：类型为sensor_msgs/msg/PointCloud2，这是ROS标准点云消息。

这两者之间是“同一份点云，两种表达”。CustomMsg是Livox为了在带宽和时间同步上做到极致而设计的私有结构，里面包含了每个点的x、y、z、reflectivity、tag、line、offset_time等字段，以及一帧点云的起始时间戳。而PointCloud2是ROS社区通用的标准消息，所有主流SLAM工具和可视化软件都认它。

实际数据链路通常是这样的：雷达固件通过网口把原始UDP包发给驱动，驱动解析后生成CustomMsg，然后驱动内部再做一次转换，发布成PointCloud2。你可以在自己的节点里订阅CustomMsg做自定义处理，也可以直接用现成的PointCloud2喂给SLAM算法。需要注意的是，两个话题默认开启与否由驱动配置决定，所以要先确认你的驱动launch文件里有没有打开pointcloud2输出。我第一次就是没开这个开关，找半天找不到pointcloud2话题，最后看了一眼launch文件里一个布尔参数，才发现是默认关闭的。

2.3 拿到机器狗后怎么自检点云是否正常

拿到go2、装好驱动之后，不要急着跑SLAM，先把点云自检做一遍，确认数据链路是通的。这一步能帮你把后续的很多问题扼杀在摇篮里。

打开终端，依次执行：

ros2 topic list

先看看有没有/livox/lidar和/livox/lidar/pointcloud2。如果没有，把launch文件里的enable_pointcloud2或者类似参数改成true重新启动。接着查看话题发布频率：

ros2 topic hz /livox/lidar/pointcloud2

如果频率稳定在10Hz或者20Hz（取决于你设置的扫描频率），说明驱动正常。然后打印一条消息看看结构：

ros2 topic echo /livox/lidar/pointcloud2 --once

你会看到一大串字段。重点看三个信息：height和width是不是正常（对于这种无序点云，height通常是1，width就是点数）；fields里有没有x、y、z和intensity；data数组的长度是不是和width * point_step一致。如果这几个都对得上，恭喜，数据链路没有问题。

这里有一个实战小技巧：point_step代表一个点占多少字节。比如一个点包含x、y、z、intensity四个float32字段，每个字段4字节，point_step就是16。解析的时候，第i个点的x坐标就存放在data[i * 16 : i * 16 + 4]这个区间里，偏移量完全由fields里的offset定义。这个逻辑后面写Python解析脚本时会反复用到。

3. 点云格式全谱系拆解

3.1 一帧点云的“原子”结构：坐标、强度、时间戳

点云格式再怎么变，底层“原子”就那几样。最基本的当然是三维坐标x、y、z，这决定了一个点在空间里的位置。然后是强度intensity，表示激光回波的强弱，不同材质反光率不一样，所以强度值可以用来区分地面、墙面、植被等物体。再往上，还有timestamp，记录这个点是什么时候被扫到的。

对于Livox这类固态雷达，时间戳尤其重要。它每个点都带一个offset_time，表示这个点距离帧起始时刻的偏移量，单位是纳秒。为什么要做这么细？因为机器狗在运动，雷达在扫描，一帧点云内部每个点其实是在不同时刻、不同位姿下扫到的。SLAM算法在做点云配准时，如果要考虑运动畸变，就需要知道每个点的精确时间戳，从而用IMU数据去补偿。这也是CustomMsg和标准PointCloud2最核心的差距之一：标准消息通常只给整帧一个时间戳，而CustomMsg能精细到每个点。

3.2 常见点云存储格式对比

点云数据落盘存储时，格式更是五花八门。这里我把最常见的几种列出来，并给出它们的适用场景。

不要小看格式选择。同样一帧10万个点的点云，ASCII的PCD可能有6MB，二进制的PCD只有1.2MB，而LAS可能还能带压缩变成几百KB。数据量大了之后，落盘速度和回放效率都会被格式影响。

3.3 核心消息类型PointCloud2：字段、偏移量与内存布局

在所有格式当中，sensor_msgs/msg/PointCloud2是ROS生态里最核心的点云消息类型，没有之一。几乎所有SLAM算法、可视化工具、数据预处理节点都认它。搞懂它的内存布局，你就能自己写解析工具，也能在格式转换时不出错。

看它的结构体定义，核心字段如下：

• header：标准消息头，包含时间戳stamp和坐标系frame_id。
• height、width：对于有序点云，height是扫描线数，比如64线雷达是64；对于无序点云，height为1，width为点数。
• fields：点字段列表，每个字段有name（比如x、y、z）、offset（字段在单个点内的字节偏移）、datatype（UINT8、FLOAT32等）、count。
• point_step：单个点占用的字节数。
• row_step：一行点云占用的字节数，无序点云下等于point_step * width。
• data：原始字节缓冲区，所有点的数据都平铺在这里。
• is_dense：是否有无效点（NaN或inf）。

你可以把data理解成一个大数组，里面按顺序放着所有点。要把第i个点提取出来，就从i * point_step开始切，切出point_step个字节，再按fields里的offset和datatype去解出各个字段。这个逻辑听着枯燥，但很重要，因为很多转换脚本出错，就是offset算错了。

3.4 Livox的CustomMsg：工业雷达的私有协议

CustomMsg是Livox在ROS驱动里定义的消息类型。为什么放着标准PointCloud2不用，非要搞一套私有的？因为标准消息里没有天然的“每点时间戳”字段，也没有line（扫描线号）这种细分信息，而Livox的非重复扫描需要精确到每个点的采集时刻来做去畸变和建图。

CustomMsg的几个关键字段值得记一下：

• timebase：这一帧点云的起始时间戳，单位是纳秒。
• point_num：这一帧的点数。
• points：点数组，每个点包含x、y、z（float32）、reflectivity（uint8）、tag（uint8）、line（uint8）和offset_time（uint32，相对timebase的纳秒偏移）。

所以，如果你要处理运动畸变，用CustomMsg是更方便的；如果只是常规建图、可视化，直接用PointCloud2就够了。很多开源算法比如FAST-LIO，其实同时支持这两种输入，只要在配置文件里切换input source类型。选择的关键是看你的下游算法默认读哪种格式。

4. 实测：从go2抓取点云并完成格式转换

4.1 录制第一份点云bag包

理论讲再多，不如动手录一包数据。我建议你把机器狗放到一个有桌椅、墙角、绿植这类特征的室内环境，保持机器人不动或者慢慢前行，然后录制60秒左右的数据。录制时除了点云话题，一定要把/tf和/tf_static一并录进去，否则后续回放时坐标变换全断，SLAM根本跑不起来。

命令如下：

ros2 bag record /livox/lidar/pointcloud2 /tf /tf_static -o go2_slam_01

录制完成后，可以用ros2 bag info go2_slam_01查看包信息。重点确认三件事：PointCloud2消息数量是否正常（60秒乘10Hz，应该有600帧左右）；/tf的消息数量是不是足够；frame_id是不是设置成了激光雷达的坐标系，比如livox_frame或者lidar_link。如果frame_id不对，后面做坐标变换时会直接报错。

这里说一个我踩过的坑：第一次录制时漏掉了/tf_static，只录了/tf，结果离线跑LIO-SAM时，程序一直报找不到base_link到lidar_link的静态变换。后来才发现静态变换是在驱动启动时发布的，属于/tf_static话题，必须和普通变换分开记录。

4.2 用Python解析PointCloud2并导出PCD

录完数据，我们来写一个Python脚本，读取bag包里的PointCloud2消息，把它解析成numpy数组，再导出成PCD文件。

ROS2环境下，推荐用sensor_msgs_py这个官方Python库，比手写解析省事得多。

import numpy as np import rosbag2_py from sensor_msgs_py import point_cloud2 from sensor_msgs.msg import PointCloud2 from rclpy.serialization import deserialize_message bag_path = "go2_slam_01" reader = rosbag2_py.SequentialReader() storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( input_serialization_format="cdr", output_serialization_format="cdr" ) reader.open(storage_options, converter_options) topic_types = {} for topic, type_name in reader.get_all_topics_and_types(): topic_types[topic.name] = topic.type for message in reader.read_messages(): topic = message.topic_metadata.name if topic == "/livox/lidar/pointcloud2": msg = deserialize_message(message.serialized_data, PointCloud2) points = point_cloud2.read_points_numpy(msg) print(f"帧点数: {len(points)}, 字段: {points.dtype.names}") # 这里只取第一帧用于演示 break

运行这个脚本，你会看到输出类似于：

帧点数: 14253, 字段: ('x', 'y', 'z', 'intensity')

说明这一帧点云有14253个点，每个点包含xyz和intensity。read_points_numpy返回的是一个结构化数组，可以直接通过points["x"]拿到所有x坐标，非常方便。

接下来把它写成一个二进制PCD文件。PCD文件头格式不长，关键是指定FIELDS、SIZE、TYPE、WIDTH、POINTS和DATA ascii/binary。这里用二进制模式，体积更小、读写更快。

with open("frame_0001.pcd", "wb") as f: header = f"""VERSION .7 FIELDS x y z intensity SIZE 4 4 4 4 TYPE F F F F COUNT 1 1 1 1 WIDTH {len(points)} HEIGHT 1 VIEWPOINT 0 0 0 1 0 0 0 POINTS {len(points)} DATA binary """ f.write(header.encode()) f.write(np.ascontiguousarray(points, dtype=np.float32).tobytes())

注意，read_points_numpy得到的结构化数组顺序可能和我们预期的字段顺序不完全一致，所以写入前用ascontiguousarray重排成连续内存的float32数组。这里有个细节：PCD文件头的字段类型和顺序必须与数据区严格对应，如果FIELDS写的是x y z intensity，数据区就必须按这个顺序排列，否则打开文件会出现菱形飞点。

4.3 在CloudCompare和rviz2中完成可视化验证

PCD文件写好后，打开CloudCompare，直接拖进去或者用File > Open打开，就能看到点云。如果一切正常，你会看到三维空间里稀疏分布的点，墙、地面、桌椅轮廓清晰可辨。如果打开后只有几条线或者说点云是乱的，大概率是字段顺序、二进制对齐或者坐标系方向出了问题。

除了CloudCompare，你也可以直接在rviz2里看实时点云。启动驱动后用rviz2添加PointCloud2显示，话题选择/livox/lidar/pointcloud2，固定坐标系设成livox_frame即可。rviz2的好处是能和TF树联动，你可以实时看到机器狗本体、雷达坐标系和点云三者之间的关系，对排查坐标变换问题特别有用。

在CloudCompare里，我还习惯给点云上一上强度伪彩。选中点云的Scalar Field为intensity，然后用渐变色彩显示。这一步不是为了好看，而是通过强度值可以快速分辨不同材质：墙面反光强，强度值高；黑色织物、树木吸收激光，强度值低。这对于后续做点云分割、地图过滤都有参考价值。

5. 点云格式选型对SLAM算法的影响

5.1 FAST-LIO、LIO-SAM、Cartographer到底要什么格式

点云格式的选型最终要落到算法上。不同SLAM算法对输入格式的偏好不一样，我是这么区分的：

我在go2上用得最顺的是FAST-LIO2。它的launch文件里有一个参数，可以选择订阅livox_ros_driver2/msg/CustomMsg还是sensor_msgs/msg/PointCloud2。如果你把驱动输出的PointCloud2话题直接接进去，就选PointCloud2模式；如果你想让算法拿到每点时间戳做更精细的运动补偿，就选CustomMsg模式。

这里有个实操建议：如果你的机器狗上还有IMU（比如内置的IMU或单独安装的），尽量把IMU话题和点云话题一起接进算法，因为去畸变效果会明显好很多；如果没有IMU，也可以用纯雷达模式，但建图质量在快速转弯时会差一些。

5.2 离线建图的最优数据组织方式

做SLAM建图，我推荐“离线录包-回放跑算法”这个模式。原因很简单：机器狗在室外跑，真机调参不方便，而bag包可以反复回放，参数随便调，不影响设备。

离线建图时，最优的数据组织方式是：把雷达点云、IMU话题、TF统一录在一个bag包里，回放时让算法订阅这些话题。具体结构可以参考下面的组合：

• /livox/lidar/pointcloud2：标准点云，给算法的前端配准模块或rxviz可视化用。
• /livox/lidar：Livox原始消息，给支持CustomMsg的算法模块用。
• /livox/imu：IMU数据，提供角速度和加速度，用于运动畸变补偿和状态预测。
• /tf、/tf_static：坐标变换，保证点云、IMU和机器狗本体之间的坐标关系正确。

回放时用如下命令：

ros2 bag play go2_slam_01

同时在另一个终端启动FAST-LIO2节点。注意回放速度最好控制在1.0倍速以内，如果bag发布频率太快导致算法丢帧，可以用-r 0.5把回放速度放慢，算法处理起来更从容。

5.3 点云预处理：滤波与降采样对后续建图的影响

点云格式搞定了，不等于建图就一帆风顺。我通常会在点云进入SLAM算法前，对原始点云做一波预处理，这往往能显著提高建图稳定性。主要做三件事：

第一，直通滤波，把过远、过近或者不需要的垂直范围切掉。MID-360最远测40米，但太远的点噪声大、精度差，我只保留0.3米到25米范围内的点。在go2室内建图时，我还习惯把天花板附近的点滤掉，因为它们大量是斜向扫描产生的稀疏杂点。

第二，体素降采样。用pcl::VoxelGrid或者PCL的Python绑定，把空间划分成一个个小立方体，每个立方体只保留一个重心点。体素尺寸我常用0.05米到0.1米。这样一来，非重复扫描雷达在视野重叠区打出的密集点就不会让算法算力爆炸，配准速度提升明显，精度损失却微乎其微。

第三，运动畸变补偿。如果你用的是CustomMsg，记得把每点时间戳传给算法。比如FAST-LIO2会用它来把一帧内的不同时刻点云对齐到统一位姿，消除运动模糊。这个处理对低速移动的go2影响不明显，但当机器狗快速转弯或跑动时，运动畸变会让地图出现“彗星尾”一样的拖影，补偿前和补偿后的地图质量差别很大。

我在实测中的体会是：点云格式不是SLAM建图里的“高光时刻”，但所有坑基本都是集中在格式和数据结构这一层。你花一个小时把PointCloud2的内存布局、CustomMsg与标准消息的差异、PCD文件头这些基础啃下来，后面跑通FAST-LIO、LIO-SAM都是一马平川的事。如果遇到奇怪的问题，建议第一反应不要是去调算法参数，而是先把话题、消息类型、frame_id、时间戳这四件套打印出来看一眼，八成问题就浮出水面了。