拿到宇树机器狗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通常是1width就是点数fields里有没有x、y、z和intensitydata数组的长度是不是和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 常见点云存储格式对比点云数据落盘存储时格式更是五花八门。这里我把最常见的几种列出来并给出它们的适用场景。格式扩展名内容特点典型场景XYZ.xyz纯文本每行一个点只有xyz简单调试、教学演示XYZI.xyzi纯文本每行xyz加intensity带强度的手工处理PCD.pcdPCL原生格式支持ASCII和二进制可扩展字段SLAM算法处理、PCL库生态PLY.ply支持顶点加面片常用于三维重建网格重建、模型交换LAS/LAZ.las/.laz测绘行业标准支持分类、回波数等属性GIS、测绘、点云分类ROSBag.bag/.db3ROS消息序列化存储可回放所有话题数据采集、算法调试离线回放不要小看格式选择。同样一帧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为1width为点数。fields点字段列表每个字段有name比如x、y、z、offset字段在单个点内的字节偏移、datatypeUINT8、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、zfloat32、reflectivityuint8、taguint8、lineuint8和offset_timeuint32相对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(uribag_path, storage_idsqlite3) converter_options rosbag2_py.ConverterOptions( input_serialization_formatcdr, output_serialization_formatcdr ) 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 fVERSION .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, dtypenp.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算法对输入格式的偏好不一样我是这么区分的算法点云输入格式是否需要IMU特点FAST-LIO / FAST-LIO2支持Livox CustomMsg也支持PointCloud2是紧耦合对运动畸变补偿好适合go2这类动态平台LIO-SAMPointCloud2是基于因子图雷达加IMU加GPS融合CartographerPointCloud2可选Google出品2D/3D都能做工程复杂度较高Point-LIOCustomMsg优先是香港大学开源极高速场景擅长我在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/lidarLivox原始消息给支持CustomMsg的算法模块用。/livox/imuIMU数据提供角速度和加速度用于运动畸变补偿和状态预测。/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、时间戳这四件套打印出来看一眼八成问题就浮出水面了。