简介面向自动驾驶、机器人导航与环境感知中的点云地面过滤需求这份资源提供了一套基于Ray Filter算法的ROS节点实现。核心代码以PCL库为支撑涵盖雷达点云订阅、格式转换、地面点滤除与过滤结果发布等关键环节开发者可参照节点文件、CMake配置与launch启动写法将算法迁移到自有LiDAR数据流程中。压缩包共8个文件以2个cpp源文件核心算法与节点入口、1个头文件为主配合launch启动文件、package.xml及json工程配置整体仅4KB轻量而便于逐行研读。目前已有7706人学习下载。资源虽紧凑但完整覆盖了Ray Filter原理理解、PCL点云处理接口调用、ROS节点参数调整与运行验证的典型开发路径对从事三维环境感知或机器人系统开发的读者有直接参考价值。 做机器人导航的朋友应该都遇到过这种问题激光雷达点云一上来地面点就占了半壁江山不先做地面过滤后面的聚类、目标检测和局部路径规划根本没法看。我常用的方案就是基于ray filter的雷达点云地面过滤ROS节点它能把原始点云实时分割成地面点和障碍物点简单、快还抗地形起伏。这篇文章不聊纯论文只说我在实际项目里怎么从头搭这个节点、参数怎么调、踩了哪些坑。如果你正在做无人车、配送机器人或者室外巡检可以直接把这套实现拿过去改。1. 为什么选ray filter做地面过滤很多人在做点云地面过滤时第一反应都是RANSAC平面拟合我也不例外。但实际跑了一圈之后我才发现ray filter这种看起来不那么“高级”的算法反而最贴合工程需求。1.1 RANSAC的局限和ray filter的思路RANSAC的强项是平直路面一个平面模型直接把地面带走。但路面一旦有坡度、减速带、路沿或者雷达本身有轻微晃动平面拟合就会把凸起削平把低洼处漏掉。越野场景更明显一个土坡在RANSAC眼里就是“非地面”于是一整片坡道变成障碍物机器人还没上坡就停车。ray filter换了个思路把3D点云按方位角切成一圈扇区每个扇区相当于一条射线方向上的点列。在这条射线上地面是由近到远连续延伸的相邻点的高度变化应该落在一个合理的坡度范围内。只要沿射线逐个点扫描维护一个“局部地面高度”就能把连续的地面点挑出来。这个思路对非平整地面更友好而且本质上只是做分组、排序和斜率判断计算量非常小。1.2 和几种常见地面分割方案的横向对比我在项目里对比过RANSAC、高度网格、深度学习和ray filter。各有各的适用场景但如果让我选一套在普通工控机上稳定跑的方案大概率还是ray filter。方案实时性非平坦地面实现难度适用场景RANSAC平面拟合高一般低平整园区、仓库Ray Filter高好中室外机器人、车载高度图/网格中高中中低速园区依赖分辨率深度学习方法中好高有GPU的研发样机深度学习方案效果确实好但部署成本高还要考虑训练数据能不能覆盖自己车上的雷达型号。ray filter参数直观、行为可解释出了问题能快速定位这是我最看重的一点。1.3 ROS节点整体消息流和话题设计节点内部不维护复杂状态每一帧点云独立处理天然适合实时系统。我做的节点输入话题是/velodyne_points类型为sensor_msgs/PointCloud2经过ray filter处理后输出两个话题/ground_filter/ground地面点云/ground_filter/obstacle非地面点云两个话题都用PointCloud2类型rviz里可以直接加载显示下游节点也能无缝对接。实际部署时我还会额外输出一个/ground_filter/debug话题把分扇区的边界和当前地面高度标记画出来调参阶段特别有用。输入话题通过remap配置换雷达或者改bag文件时不用改代码。2. ray filter核心算法与参数设计这一节是整个节点最核心的地方。理解了算法到底在干什么后面写代码、调参数才不会乱。2.1 极坐标分扇区点云怎么变成射线算法第一步是把点云从XYZ坐标转成极坐标形式。每个点计算水平角theta atan2(y, x)水平距离r sqrt(x^2 y^2)高度z保持不变然后把theta按角度分辨率分组。比如分辨率设成0.2度那么一圈360度会被分成1800个扇区每个扇区就是一个“射线方向上的点列”。这里有个容易踩的坑很多人以为ray filter是逐线束处理也就是按雷达的垂直扫描线来分。实际上不对。以16线雷达为例同一水平角下会有16个不同垂直角度的点它们都在同一个扇区里。如果按线束处理一条线扫到远距离后会非常稀疏无法连续判断地面。按扇区处理则能把不同线束的近处和远处点组合在一起地面连续性明显更好。2.2 地面判定逻辑高度差、坡度与状态更新算法核心是沿着每条射线从近到远逐个点扫描。我维护两个状态变量last_ground_z最近一个被判为地面的点的高度last_ground_r最近一个被判为地面的点的水平距离对于新来的点d_diff p.r - last_ground_r; h_diff p.z - last_ground_z; slope h_diff / d_diff;如果slope小于设定的max_slope_并且高度差h_diff满足约束就判定为地面点同时更新last_ground_z和last_ground_r。否则判定为障碍物点。这里最关键的地方是不能直接用全局高度阈值。因为在传感器坐标系下远处的地面点高度会随着距离缓慢变化上坡时尤其明显。如果拿固定高度去卡整个上坡全部会被划进障碍物。为了避免下坡时地面断档我加了一个drop_height_diff_参数。当h_diff小于0也就是当前点比上一个地面点低时只要下降量不超过drop_height_diff_仍然认为是地面。这个逻辑对过减速带、下台阶特别重要。2.3 参数含义与标定经验节点里最核心的参数就是下面这几个我把它们全放在yaml文件里试车时改起来方便。参数名含义推荐值调参经验sensor_height雷达离地高度1.0~2.0m必须实际测量误差超过5cm就会影响近处地面判断angle_resolution扇区角度分辨率0.2°越小越精细但稀疏雷达建议放宽到0.3°max_slope地面最大坡度正切值0.2~0.5越野路面可以到0.8但要防墙根被当地面max_height_diff相邻地面点最大高度差0.2~0.5m主要防噪声点不能太大drop_height_diff下坡时最大地面下降量0.3~0.6m控制下坡断档太大容易把台阶底部当地面min_distance近场盲区距离0.5~1.0m过滤车体点云和安装支架调参时不要只看数字最好的办法是准备一段包含平路、坡道、路沿和墙面的bag在rviz里同时打开地面点和障碍物点用不同颜色反复比对。先调max_slope再调max_height_diff最后调近场距离。我一般从宽松阈值开始慢慢收紧看到障碍物边缘出现毛刺再退回来。2.4 边界情况处理雷达外参安装倾斜是最大的坑。如果雷达没有标定好地面点在射线上的高度变化规律会被破坏ray filter再怎么调参数都白搭所以先做外参标定再谈地面过滤。点云里的NaN值也需要处理进入算法前先做isnan检查或者用PCL的removeNaNFromPointCloud。另外低线束雷达到了远处会非常稀疏一个扇区里可能只有一两个点这时候做连续性判断没有意义。我在代码里加了min_points_per_sector_扇区点太少就跳过避免产生异常分割。3. 实操搭建从package到节点运行这一节直接给可以抄作业的工程结构。我基于Ubuntu 20.04 ROS Noetic pcl_ros实现ROS 2的改动也不大。3.1 环境准备与package骨架ROS环境如果还没装好可以用社区的鱼香ROS一键安装脚本比手动配环境省事很多。我自己的习惯是装好基础ROS后再创建一个catkin_ws工作空间然后新建包cd ~/catkin_ws/src catkin_create_pkg ray_ground_filter roscpp sensor_msgs pcl_ros pcl_conversions std_msgs包目录结构如下catkin_ws/src/ray_ground_filter/ ├── CMakeLists.txt ├── package.xml ├── launch/ray_ground_filter.launch ├── config/ray_ground_filter.yaml ├── include/ray_ground_filter/ray_ground_filter.h └── src/ ├── ray_ground_filter.cpp └── ray_ground_filter_node.cpppackage.xml里要确保声明了roscpp、sensor_msgs、pcl_ros和pcl_conversions否则编译会报找不到依赖。3.2 核心代码讲解头文件里我主要定义了一个RayGroundFilter类持有ROS句柄、订阅者、发布者和所有参数。核心成员如下class RayGroundFilter { public: RayGroundFilter(ros::NodeHandle nh, ros::NodeHandle pnh); private: void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr msg); void filterSector(std::vectorPolarPoint sector, pcl::PointCloudpcl::PointXYZI::Ptr ground, pcl::PointCloudpcl::PointXYZI::Ptr obstacle); ros::Subscriber sub_; ros::Publisher ground_pub_; ros::Publisher obstacle_pub_; double sensor_height_; double angle_resolution_; int num_sectors_; double max_slope_; double max_height_diff_; double drop_height_diff_; double min_distance_; int min_points_per_sector_; };PolarPoint是一个简单的结构体保存XYZI点、水平距离r和角度theta。回调函数里先做坐标转换和分扇区void RayGroundFilter::pointCloudCallback( const sensor_msgs::PointCloud2::ConstPtr msg) { pcl::PointCloudpcl::PointXYZI::Ptr cloud( new pcl::PointCloudpcl::PointXYZI()); pcl::fromROSMsg(*msg, *cloud); std::vectorstd::vectorPolarPoint sectors(num_sectors_); for (const auto pt : cloud-points) { if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) continue; double r std::hypot(pt.x, pt.y); if (r min_distance_) continue; double theta std::atan2(pt.y, pt.x) * 180.0 / M_PI; int idx static_castint((theta 180.0) / angle_resolution_); idx std::max(0, std::min(num_sectors_ - 1, idx)); sectors[idx].push_back({pt, r, theta}); } pcl::PointCloudpcl::PointXYZI::Ptr ground_cloud( new pcl::PointCloudpcl::PointXYZI()); pcl::PointCloudpcl::PointXYZI::Ptr obstacle_cloud( new pcl::PointCloudpcl::PointXYZI()); for (int i 0; i num_sectors_; i) { if (static_castint(sectors[i].size()) min_points_per_sector_) continue; filterSector(sectors[i], ground_cloud, obstacle_cloud); } sensor_msgs::PointCloud2 out_msg; pcl::toROSMsg(*ground_cloud, out_msg); out_msg.header msg-header; ground_pub_.publish(out_msg); pcl::toROSMsg(*obstacle_cloud, out_msg); out_msg.header msg-header; obstacle_pub_.publish(out_msg); }扇区处理函数是核心逻辑void RayGroundFilter::filterSector( std::vectorPolarPoint sector, pcl::PointCloudpcl::PointXYZI::Ptr ground, pcl::PointCloudpcl::PointXYZI::Ptr obstacle) { std::sort(sector.begin(), sector.end(), [](const PolarPoint a, const PolarPoint b) { return a.r b.r; }); double last_ground_z -sensor_height_; double last_ground_r 0.0; for (const auto p : sector) { double d_diff p.r - last_ground_r; if (d_diff 1e-3) continue; double h_diff p.z - last_ground_z; double slope h_diff / d_diff; bool is_ground false; if (slope max_slope_) { if (h_diff max_height_diff_ || slope 0.0) { if (h_diff -drop_height_diff_) { is_ground true; } } } if (is_ground) { ground-points.push_back(p.pt); last_ground_z p.z; last_ground_r p.r; } else { obstacle-points.push_back(p.pt); } } }这套代码在16线雷达上单帧处理时间大约10ms以内配合后续的聚类节点完全够用。3.3 launch与yaml配置launch文件我写得很简单主要用remap让节点适配不同雷达话题launch node nameray_ground_filter pkgray_ground_filter typeray_ground_filter_node outputscreen remap frompoints_in to/velodyne_points/ rosparam file$(find ray_ground_filter)/config/ray_ground_filter.yaml/ /node /launchyaml参数文件sensor_height: 1.5 angle_resolution: 0.2 max_slope: 0.3 max_height_diff: 0.3 drop_height_diff: 0.4 min_distance: 0.6 min_points_per_sector: 3启动时先跑roscore再roslaunch ray_ground_filter ray_ground_filter.launch。如果想看话题是否发布正常用rostopic hz /ground_filter/ground就能看到输出频率。3.4 用rosbag和rviz离线验证没有实车时我习惯先录一段rosbag来验证。比如播放已经录好的点云数据rosbag play --loop your_data.bag另外开一个终端运行节点再开rviz添加两个PointCloud2显示分别订阅/ground_filter/ground和/ground_filter/obstacle颜色调成绿色和红色。这样一眼就能看出有没有误分割。如果某个区域总是出错暂停播放在rviz里用测量工具看看实际点的高度再回头调参数。我调试时通常不开动态重配置而是改yaml后重启节点这样最简单。如果参数需要频繁调可以加上dynamic_reconfigure支持不过代码量会多一些对大多数人来说没必要。4. 常见问题与排查技巧实录下面这些问题都是我在实际项目中真实遇到过的整理成速查表形式方便你直接对照。4.1 地面点滤不干净地面点滤不干净通常表现为远处地面点残留或者地面点里混着草、路沿。排查顺序是先看sensor_height是否准确再看max_slope是不是太大最后看max_height_diff。我有一次在草地上测试草的高度不断变化地面点云里全是毛刺。把max_height_diff从0.3降到0.12之后草点少了大半但远处一些低矮土坎也被删了。草和低障碍物本来就很难完全区分这时候要在“地面干净”和“不漏障碍物”之间做取舍没有一劳永逸的参数。4.2 障碍物被误删最常见的误删对象是车侧面的墙和护栏。原因是一个扇区里墙体底部和地面连接处的坡度连续高度差也在阈值内于是墙根被当地面整面墙跟着被带走。解决办法是加一个“近处突变检测”当某个点距离上一个地面点很近但高度差明显偏大时直接标记为障碍物并且停止更新这个扇区的地面高度。我在代码里加了一个near_height_threshold水平距离小于0.3m时高度差超过0.25m就当作墙面处理。城区场景建议把max_slope放到0.15到0.2之间比较安全。4.3 上下坡和颠簸路面上坡时地面高度在射线方向上持续上升如果max_slope设小了坡面会被提前当成障碍物。我的做法是把sensor_height作为初值但实际处理时允许last_ground_z跟着地面点一起累计变化不要用全局高度阈值去卡。下坡时容易出现地面断档所以drop_height_diff_不能设成0否则下坡的第一批点全会被划成障碍物。颠簸路面会让雷达上下晃动地面点在z方向来回跳。我在部分项目里会给last_ground_z做平滑更新而不是直接赋当前点高度last_ground_z 0.7 * p.z 0.3 * last_ground_z;这样能明显减少抖动带来的误判但这个系数需要现场试太大反而会引入滞后。4.4 性能和长期运行ray filter本身很快但点云很密时分扇区和排序还是会吃CPU。我试过几个提速方法先做直通滤波把距离超过50m、高度高于3m的点提前丢掉再把扇区处理用OpenMP并行化每个线程处理一段角度范围最后把节点改成nodelet模式减少PointCloud2的序列化和拷贝开销。还有一个长期运行的坑PCL在每次回调里分配新点云对象长时间跑会有内存碎片。我的处理是复用点云对象并用定时器每5分钟清一次空容器。这个坑我第一次连续跑12个小时才遇到当时节点内存一路涨到好几个G排查了很久才发现是频繁分配内存的问题。希望看到这里的人别再等节点炸了才回头查。本文还有配套的精品资源点击获取