★ FEATURED ARTICLE 讲实话这个项目刚开始我并不觉得有什么难度。两个 Livox Mid-360 而已ROS2 驱动装上点云话题出来写个节点把两路点云合并发布不就完了但真正动手之后才发现从 Python 原型到 C PCL 方案几乎每一步都有坑等着你。这篇文章把我在 Livox Mid-360 双雷达 ROS2 融合项目中踩过的所有坑、做过的重要决策、以及最后跑通的完整方案都记录下来给后面要做双雷达或者多雷达融合的朋友一个参考。先说结论如果你只是验证一下双雷达能不能出数据用 Python 写写原型没问题但如果你要拿它做实时融合、建图、导航的输入直接上 C PCL别犹豫。至于为什么看完下面的性能对比和踩坑过程你就明白了。1. 双雷达方案是怎么来的一块盲区逼出来的改造1.1 单 Mid-360 为什么不够用我们项目里的机器人是一台室内巡检小车需要在走廊、货架区、机房这些环境里自主导航。最开始用的是单个 Livox Mid-360装在小车顶部水平朝前。Mid-360 的参数大家应该都清楚水平 360° FOV垂直方向只有 59°覆盖范围是 -7° 到 52°10Hz 帧率下每帧大约 2 万个点。问题就出在这个垂直视场角上。机器人前方 1 米到 3 米这一片近地区域正好在 -7° 以下单雷达完全扫不到。障碍物检测全靠激光雷达的话碰到低矮的障碍物比如地面上凸起的角铁、落在地上的托盘、甚至比较大的石块导航根本反应不过来。我也试过把单个 Mid-360 前倾安装让激光能扫到近处地面但代价是远处的视场角又被抬高了走廊尽头宽度 1 米多的门框都经常扫不到导航的小车经常在门口来回转圈。1.2 双雷达的安装方式与初始外参后来就定了一个很朴素的方案两个 Mid-360 装在同一块铝合金底座上一个水平安装负责中远距离环境感知另一个前倾 45° 专门覆盖近处盲区。两个雷达之间有一个固定的机械外参先按 CAD 图纸量出来一个初始值后续再用点云配准精修。硬件拓扑比较简单两个 Mid-360 都走千兆网线接到一台工控机通过一个千兆交换机扩展网口。注意Livox 雷达的 IP 是要手动配置的两个雷达不能冲突。我们把水平安装的雷达设为 192.168.1.2前倾的设为 192.168.1.3工控机网口 IP 设成 192.168.1.50。这个 IP 后面在驱动配置文件里要一一对应千万不能搞反不然 rviz2 里看到的两路点云就跟两拨人各画各的一样。下面是两个雷达的分工和基本参数项目雷达 1水平雷达 2前倾 45°安装角度水平0°前倾俯仰 -45°IP 地址192.168.1.2192.168.1.3主要职责中远距离环境感知建图近处盲区补充障碍物检测帧率10Hz10Hz每帧点数约 2 万约 2 万光是把两路点云同时显示在 rviz2 里就有不少人会被卡住。我刚开始也以为装好驱动就行结果两个雷达的话题都能出数据但是交替闪烁速度还贼慢。这个问题的根子不在雷达而在下面的驱动配置和节点通信上。2. Python 原型阶段四天踩坑四个典型现场为什么一开始选了 Python原因很实在当时需要快速验证双雷达融合之后的点云能不能用来做障碍物检测Python 写起来快numpy、open3d 这些库现成的而且 livox_ros_driver2 本身对 Python 节点订阅点云是友好的不用自己处理底层协议的解析。于是我就在 ROS2 Humble 环境下搭了一个 Python 原型节点打算把两路 PointCloud2 消息接进来做坐标变换、合并、发布。结果这一版原型让我在四天里踩遍了 ROS2 Python 开发的典型坑有些坑单独拎出来不算难但它们叠加在一起足以让人怀疑人生。2.1 第一坑QoS 不匹配节点“收不到”点云现象是livox_ros_driver2 的两个点云话题在ros2 topic echo下都能看到数据但我的 Python 节点订阅之后回调函数一次都没被触发。反复检查话题名、命名空间、消息类型全都没问题最后终于想到是不是 QoS 的问题。ROS2 的 DDS 通信机制和 ROS1 有本质区别发布端和订阅端必须 QoS 兼容才能建立连接。livox_ros_driver2 发布 PointCloud2 时用的是传感器数据 QoSbest_effort 策略深度只有 5而我用rclpy创建订阅时图省事用了默认 QoSreliable深度 10两者不兼容连接根本建立不起来。修复方式是在 Python 订阅器里显式指定传感器数据 QoSfrom rclpy.qos import qos_profile_sensor_data self.sub_left self.create_subscription( PointCloud2, /livox/lidar_1/pointcloud2, self.cloud_callback_left, qos_profileqos_profile_sensor_data )这个坑几乎每个从 ROS1 转 ROS2 的人都会踩一遍。养成一个习惯订阅点云、图像这类高频传感器话题时一律用qos_profile_sensor_data别用默认 QoS。2.2 第二坑rclpy 单线程执行器被点云处理卡死QoS 问题解决之后点云进来了但新的问题立刻浮出水面两个雷达的回调函数处理不过来点云在 rviz2 里表现成一跳一跳的而且节点 CPU 占用率直接顶满。原因也很典型。我最初的节点没有配置回调组默认走SingleThreadedExecutor所有回调都在同一个线程里按顺序执行。我一个回调里要干这些事把 PointCloud2 消息转成 numpy 数组、做 4x4 外参矩阵变换、可能还要做一次简单的降采样、再合并发布。Mid-360 一帧大约 2 万个点看起来不多吧但在 Python 里走一遍这些操作单帧处理时间实测要 150 到 200 毫秒。而雷达 10Hz 发布等于 100 毫秒来一帧回调永远追不上数据流入的速度。我当时还天真的试过用MultiThreadedExecutor加ReentrantCallbackGroup让两路点云回调并行处理。确实有改善但改善有限因为 GIL 锁的存在Python 多线程在处理 CPU 密集型的点云计算时基本上还是在串行执行。另外如果以后几个雷达同时处理这个方案无论如何也撑不住。2.3 第三坑时间戳对不齐拼接处出现重影这是 Python 阶段最隐蔽的坑。点云能接进来了外参矩阵也应用了但站在机器人旁边晃动一下或者让小车走两步融合后的点云里雷达 2 的点就会“拖尾”雷达 1 和雷达 2 的扫描线像两张错开的照片叠在一起。一开始我以为是外参标定得不够准反复调了好几轮 ICP静态场景下对齐效果明明还行。后来才发现问题出在时间戳上两个雷达的点云话题是独立发布的时间戳之间没有对齐如果你是各自取“最新一帧”来做变换合并两帧之间实际采集时刻可能相差几十毫秒甚至更多。机器人一动起来这几厘米的移动误差就会让拼接结果出现重影。实际上 ROS2 里处理多传感器时间同步标准方案是用message_filters的ApproximateTimeSynchronizer但它默认是针对 C 接口设计的Python 里虽然message_filters提供了ApproximateTimeSynchronizer的绑定但由于点云消息回调处理本身太重同步之后的积压问题更加突出。也就是说时间同步器勉强能对齐时间戳但处理链路依旧卡顿治标不治本。2.4 第四坑Python 手动保存 PCD埋下后续大雷为了离线调参我还写了一段 Python 脚本把融合后的点云保存成 PCD 文件方便用 CloudCompare 查看和对齐效果做量化。结果这个脚本保存出来的 PCD 文件在我后面转到 C 后引发了一次极其痛苦的排查具体细节后面会专门写一节。先埋个伏笔用 Python 手工构造 PCD 文件头时字段顺序和空点云处理稍不注意就会生成一个 PCL 完全无法加载的文件而且报错信息极其不友好。说实话Python 原型阶段让我明白了两个道理。第一ROS2 的消息同步、QoS、执行器这些底层机制不管用什么语言开发都得吃透否则报错来了你都不知道往哪个方向查。第二点云数据量大且计算密集的场景Python 做原型可以进不了实时链路。这不是说 Python 不优秀而是这个场景天然不适合 Python。3. 转向 C PCL不是矫情是性能账算明白了3.1 Python 与 C 处理同一帧点云的实测对比促使我下决心转 C 的是一组很平淡但很扎心的性能对比数据。我在同样的工控机上分别用 Python 和 C 写了同样的点云处理流程接收一帧 PointCloud2 → 转成点云 → 应用外参变换 → 体素滤波 → 合并。实测单帧处理耗时如下处理阶段Pythonnumpy/open3dCPCLPointCloud2 反序列化约 20ms小于 2ms坐标变换约 2 万点约 45ms约 3ms体素滤波leaf 0.05m约 80ms约 5ms合并与发布约 20ms约 1ms总计超过 150ms约 10ms10Hz 的雷达帧间隔是 100msPython 方案处理一帧的时间比帧间隔还长必然是持续积压、持续丢帧。C 方案绰绰有余留下了大量余量给后续的降噪、目标聚类、甚至导航模块。这笔账一算C 方案几乎不需要犹豫。3.2 livox_ros_driver2 多雷达驱动配置在进入 C 节点之前先把多雷达驱动配置说清楚因为这一步做不对后面所有代码都是白搭。livox_ros_driver2 的源码里自带双雷达示例但配置项比较多我直接在驱动的中文注释基础上整理了一份精简版配置启动两个雷达节点。核心思路是每个雷达对应一个驱动节点实例通过lidar_ip区分设备发布的话题由multi_topic参数控制是否会带上命名空间。launch 文件大意如下launch node pkglivox_ros_driver2 execlivox_ros_driver2_node namelivox_lidar_1 outputscreen param namexfer_format value1/ !-- 1 表示输出 PointCloud2 -- param namemulti_topic valuetrue/ param namedata_src value1/ param namelidar_ip value192.168.1.2/ param namepublish_freq value10.0/ /node node pkglivox_ros_driver2 execlivox_ros_driver2_node namelivox_lidar_2 outputscreen param namexfer_format value1/ param namemulti_topic valuetrue/ param namedata_src value1/ param namelidar_ip value192.168.1.3/ param namepublish_freq value10.0/ /node /launch启动之后两路点云分别发布在/livox/lidar_1/pointcloud2和/livox/lidar_2/pointcloud2。注意data_src1表示使用网络接口连接雷达如果你的雷达是通过 USB 转接的这个值要改成相应配置。3.3 融合节点的整体设计融合节点我命名为lidar_fusion_node整体思路是把雷达 1水平雷达的坐标系作为融合后的主坐标系雷达 2前倾雷达的点云通过外参矩阵变换到雷达 1 坐标系下然后做体素滤波降采样再合并发布。节点内部用message_filters做时间同步再转入回调处理。回调里这几步是必不可少的把两路PointCloud2消息转成pcl::PointCloudpcl::PointXYZI。如果点云为空直接返回不处理。对雷达 2 的点云应用外参矩阵T_12变换到雷达 1 坐标系。合并两片点云。体素滤波降采样去除重叠区域的冗余点。把点云转回PointCloud2发布出去。3.4 核心代码时间同步、坐标变换、降采样与合并C 节点核心代码结构如下你可以直接参考#include rclcpp/rclcpp.hpp #include sensor_msgs/msg/point_cloud2.hpp #include message_filters/subscriber.h #include message_filters/synchronizer.h #include message_filters/sync_policies/approximate_time.h #include pcl/point_cloud.h #include pcl/point_types.h #include pcl/conversions.h #include pcl/common/transforms.h #include pcl/filters/voxel_grid.h #include pcl_conversions/pcl_conversions.h class LidarFusionNode : public rclcpp::Node { public: LidarFusionNode() : Node(lidar_fusion_node) { sub_left_.subscribe(this, /livox/lidar_1/pointcloud2); sub_right_.subscribe(this, /livox/lidar_2/pointcloud2); sync_ std::make_sharedmessage_filters::SynchronizerSyncPolicy( SyncPolicy(10), sub_left_, sub_right_); sync_-registerCallback(LidarFusionNode::cloudCallback, this); fused_pub_ this-create_publishersensor_msgs::msg::PointCloud2( /livox/fused/pointcloud2, rclcpp::SensorDataQoS()); // 外参矩阵把雷达2的点云变换到雷达1坐标系 // 下面是 CAD 初始值实际用的值是在 ICP 精化之后写入的 T_12_ Eigen::Matrix4f::Identity(); // ... 从文件或参数加载 } private: typedef message_filters::sync_policies::ApproximateTime sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2 SyncPolicy; void cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr left_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr right_msg) { pcl::PointCloudpcl::PointXYZI::Ptr cloud_left(new pcl::PointCloudpcl::PointXYZI()); pcl::PointCloudpcl::PointXYZI::Ptr cloud_right(new pcl::PointCloudpcl::PointXYZI()); pcl::fromROSMsg(*left_msg, *cloud_left); pcl::fromROSMsg(*right_msg, *cloud_right); if (cloud_left-empty() || cloud_right-empty()) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, Received empty cloud, skip.); return; } // 1. 把雷达2点云变换到雷达1坐标系 pcl::PointCloudpcl::PointXYZI::Ptr cloud_right_trans( new pcl::PointCloudpcl::PointXYZI()); pcl::transformPointCloud(*cloud_right, *cloud_right_trans, T_12_); // 2. 合并 pcl::PointCloudpcl::PointXYZI::Ptr cloud_fused( new pcl::PointCloudpcl::PointXYZI()); *cloud_fused *cloud_left; *cloud_fused *cloud_right_trans; // 3. 体素滤波降采样 pcl::PointCloudpcl::PointXYZI::Ptr cloud_downsampled( new pcl::PointCloudpcl::PointXYZI()); pcl::VoxelGridpcl::PointXYZI voxel; voxel.setInputCloud(cloud_fused); voxel.setLeafSize(0.05f, 0.05f, 0.05f); voxel.filter(*cloud_downsampled); // 4. 发布融合后的点云 sensor_msgs::msg::PointCloud2 fused_msg; pcl::toROSMsg(*cloud_downsampled, fused_msg); fused_msg.header.frame_id lidar_1_link; fused_msg.header.stamp left_msg-header.stamp; fused_pub_-publish(fused_msg); } message_filters::Subscribersensor_msgs::msg::PointCloud2 sub_left_; message_filters::Subscribersensor_msgs::msg::PointCloud2 sub_right_; std::shared_ptrmessage_filters::SynchronizerSyncPolicy sync_; rclcpp::Publishersensor_msgs::msg::PointCloud2::SharedPtr fused_pub_; Eigen::Matrix4f T_12_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_sharedLidarFusionNode()); rclcpp::shutdown(); return 0; }编译时记得在 CMakeLists.txt 里添加 PCL 和 message_filters 的依赖find_package(PCL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pcl_ros REQUIRED) find_package(message_filters REQUIRED) find_package(sensor_msgs REQUIRED)package.xml 里也对应加上depend标签。用colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPERelease编译Release 模式比 Debug 模式在点云处理上快很多这个细节别忽略。这段代码本身不复杂但有两个点值得展开说。第一ApproximateTimeSynchronizer的队列长度和 slop 参数要合理设置我实际用的是队列长度 10、slop 50ms。如果机器人运动速度很快可以适当调大 slop但不要太大否则时间对齐就失去了意义。第二体素滤波的 leaf size 很关键。我用 0.05m因为这对导航来说精度足够同时能显著降低点云数量。如果你做的是高精度建图需求可以缩小到 0.02m但代价是后续处理压力增大。4. PCD 加载报错的完整排查链路height given (0) but no width4.1 报错复现与第一轮排查文件头C 融合节点稳定跑起来之后我开始做离线建图验证打算把融合点云保存成 PCD 地图文件再加载回来做精对齐。这时候文章开头提到的那个诡异报错出现了[pcl::PCDReader::readHeader] height given (0) but no width! [pcl::PCDReader::readHeader] width given (0) but no height!这个报错是在pcl::io::loadPCDFile()加载我用 Python 脚本保存的map.pcd时出现的。第一反应是 PCD 文件头写错了于是用head -n 15 map.pcd查看文件内容# .PCD v0.7 - Point Cloud Data file format VERSION 0.7 FIELDS x y z intensity SIZE 4 4 4 4 TYPE F F F F COUNT 1 1 1 1 WIDTH 0 HEIGHT 0 VIEWPOINT 0 0 0 1 0 0 0 POINTS 0 DATA ascii果然WIDTH 0、HEIGHT 0、POINTS 0这是一个空点云文件。问题在于PCL 的 PCDReader 在读取文件头时对于WIDTH0且HEIGHT0的 header 会直接判定为非法宁可报错也不给你一个空点云对象。而如果文件头写成WIDTH 0、HEIGHT 1PCL 就能正常加载成一个空点云。这个细节非常容易踩。4.2 第二轮排查定位到 PointCloud2 到 PCL 的转换知道文件头长什么样之后下一步是搞清楚为什么保存出来的是WIDTH 0 HEIGHT 0。我检查了 Python 保存脚本里面明明判断了点云不为空才写入的理论上不会出现空文件。于是把目光转向了保存前的那一步从PointCloud2消息转成 numpy 数组再写入 PCD 的过程。具体来说我用numpy从sensor_msgs/PointCloud2里手动解析点云数据然后判断如果是空点云就跳过保存。麻烦之处在于ROS2 里的一帧“空点云消息”有两种形态一种是PointCloud2.width 0另一种是PointCloud2.width ! 0但data为空。我的判断逻辑只检查了data是否为空没有检查width。于是在某些帧里width本身已经是 0程序还是照常执行了 PCD 写入逻辑最终生成了WIDTH 0 HEIGHT 0的文件。实际上如果你用 PCL 的 C API 保存点云pcl::io::savePCDFile()会根据cloud-width和cloud-height自动生成文件头。如果点云对象本身是从一个空PointCloud2消息通过pcl::fromROSMsg()转过来的cloud-width和cloud-height就会是 0保存出来就是上面的样子。C 代码里也会遇到同样的问题不是 Python 的锅。4.3 根因与修复方案根因总结下来就一句话PCL 对于WIDTH0, HEIGHT0的 PCD 文件头是直接拒绝加载的而在保存点云时如果源点云对象的 width/height 字段没有被正确填充就会生成这种非法的文件头。修复方案其实很简单保存前加一个判空逻辑if (cloud-empty()) { RCLCPP_WARN(get_logger(), Point cloud is empty, skip saving.); return; } pcl::io::savePCDFileBinary(map.pcd, *cloud);如果是 Python 保存写文件头的时候也要显式处理空点云情况至少保证WIDTH 0 HEIGHT 1而不是HEIGHT 0。但这个坑更深一层的教训是保存 PCD 这种中间产物时一定要用库自带的 API 去写入不要自己手工拼文件头。手工拼文件头看着简单但字段顺序、类型字节、换行符、DATA 模式这些细节任何一个不对都可能让下游工具或 PCL 库无法加载。4.4 这类 header 报错的通用排查清单结合 PCL 社区里常见的问题我整理了一份针对 PCD 加载报 header 错误的排查清单供大家参考原因分类具体表现排查方向空点云导致 WIDTH/HEIGHT 为 0文件头 WIDTH 0 HEIGHT 0 POINTS 0保存前判空手工拼接文件头字段顺序错误字段顺序和 DATA 区域不一致用库的 API 保存Windows 下 CRLF 换行符每行结尾有\r用dos2unix转换文件被截断POINTS 与实际数据行数不匹配检查文件大小和写入完整性编码问题带 BOMVERSION 行前有不可见字符用file命令检查编码这个报错本身只是一个 PCD 加载问题但它在整个项目里的意义不小它提醒我凡是涉及到数据“落盘”和“读盘”的环节都要用成熟库来处理格式并且要对空数据场景做特殊处理。编译器不会帮你挡掉这些运行时数据问题只有靠对数据流的敬畏。5. 融合效果验证与后续扩展5.1 rviz2 里的对齐验证C 融合节点跑通之后第一件事是在 rviz2 里验证融合结果。添加一个 PointCloud2 display话题选/livox/fused/pointcloud2Fixed Frame 设成lidar_1_link。同时把两个原始雷达的话题也添加进来用不同颜色区分比如水平雷达用白色前倾雷达用红色融合结果用绿色。静态场景下墙上、柱子上、门框边缘应该看不到红色和白色点云分层两种颜色的点云像叠在同一张照片里。小车行进过程中近处地面和低矮障碍物由前倾雷达补出来的点云应该清晰可见远处走廊结构由水平雷达维持。如果发现哪里错位明显优先怀疑外参而不是代码逻辑。5.2 量化验证重叠区域残差rviz2 里肉眼看是对齐了但“看起来对”和“真的对”是两码事。我做了两个量化验证第一个是静态场景下的重叠区域残差计算。找一面平整的墙面让两个雷达都能扫到采集 N 帧融合后的点云提取出墙面附近一定厚度范围内的点用 PCL 拟合平面计算点到平面距离的标准差。如果外参准确这个标准差应该在 2cm 以内。如果发现误差偏大并且偏向某个方向基本可以确定是外参的旋转分量有问题。第二个是移动场景下的“重影检测”。让小车匀速直线行驶把连续几帧融合点云叠加显示观察边缘轮廓是否清晰。如果时间同步没有做好边缘会出现明显的拖影。这一步我最初在 Python 原型里就验证过当时直接暴露了时间戳不同步的问题。5.3 后续扩展八叉树地图、点云降噪与导航融合点云稳定输出之后我把它接到了后续的建图和导航链路里。八叉树地图octomap_server可以直接订阅/livox/fused/pointcloud2生成占据栅格地图机器人导航的全局代价地图也有了稳定的输入。需要注意的是双雷达合并后点云密度在近处很高直接用原始融合点云喂给八叉树地图地图会显得很“脏”我建议在融合节点里把体素滤波 leaf size 适当调大建图用 0.1m导航用 0.05m。另外如果融合点云要送入 SLAM 算法做定位前端特征提取前最好先做一轮降噪。PCL 的StatisticalOutlierRemoval滤波器邻域点数 K 设 20标准差阈值设 1.0对去除雷达非重复扫描产生的孤立噪点很有效。但这步开销也不小实时性要求高的场景建议只在建图链路里启用实时导航链路保持轻量化。最后再分享一个小技巧ApproximateTimeSynchronizer的 slop 参数不要一上来就给很大先给 30ms 跑一版再看融合点云在动态环境下的重影程度按需增大到 50ms 或 80ms。因为 slop 越大时间上失配的可能性越大但太小又可能导致同步率低。这个参数在 Python 和 C 里都存在建议在实车上实测调参不要凭感觉拍脑袋。我自己最终用的是 50ms在实测中同步率大约在 95% 以上足够用了。
阅读完成 · 觉得有帮助？
