简介面向ROS与PCL点云开发的Ray Filter地面过滤节点适合自动驾驶、机器人导航与环境感知方向的学习者和开发者用于从LiDAR点云中剔除平坦地面、保留建筑与树木等非地面特征。压缩包共8个文件、约4KB包含2个cpp源文件、1个头文件以及launch启动文件、xml与json配置、txt说明等便于在ROS工作空间中组织节点、调整参数并快速编译运行。目前已有7714人学习说明其在点云地面分割入门与工程实践中具有一定参考价值。读者可据此了解射线滤波的核心实现、节点订阅与发布流程、PCL格式转换及射线长度、地面检测阈值等参数的配置思路并借助测试入口验证不同场景下的过滤效果为后续三维环境理解与感知算法开发提供可复用的代码骨架。1. 需求切入一个高度阈值管不住减速带和坡道园区里跑的低速配送车雷达装在前保险杠上方 1.8 m 左右10 Hz 出一帧一帧十来万个点。要做障碍物聚类第一步要先剥掉地面否则欧式聚类会把整片地面连成一个巨型簇。多数人第一版都会写一个高度阈值z 低于 -1.6 m 就算地面。这套逻辑在地下车库勉强能用一上坡就崩减速带、路沿、车身俯仰都会让固定阈值失准。这套基于射线ray filter的雷达点云地面过滤 ROS 节点解决的就是这个问题它不依赖单一高度而是按方位角把点云切成一条条射线在每条射线内部用坡度和高度递推判断把地面和障碍物分开。它适合正在搭 ROS 感知链路、需要一套能跑起来、能调参、能验收的地面分割节点的人PCL 与 ROS 的接口都留好了参数全部走 yaml换场景不用改代码。2. 总体设计极坐标射线分箱与三段判定流水线2.1 节点在感知链路里的位置与话题契约这个节点在整条链路里只做一件事减法。它不负责聚类、不负责跟踪、不负责可行驶区域决策输入一帧点云输出两帧点云——地面点和非地面点。把职责收窄到这个程度有两个好处下游的聚类、检测、栅格建图都能直接复用出问题也容易定位到底是过滤漏了还是聚类参数不对一看 /points_ground 就知道。话题契约建议一开始就定死别中途改名称类型方向说明/points_rawsensor_msgs/PointCloud2订阅原始雷达点云至少含 x/y/z带 intensity 更好/points_no_groundsensor_msgs/PointCloud2发布非地面点直接喂给欧式聚类或检测网络/points_groundsensor_msgs/PointCloud2发布地面点用于复核过滤效果、估可行驶区域~target_framestring参数统一坐标系一般填 base_link~reload_paramsstd_srvs/Trigger服务改完 yaml 不重启进程重载参数坐标系保持一致是最容易被忽略的一条。滤波在哪个坐标系下做sensor_height就必须是那个坐标系原点到地面的垂直距离。在雷达自身坐标系下做这个值是 0先变换到 base_link 再做才是 1.8 左右。两种写法都对混着用就一定翻车第五章会细说。输出点云沿用输入的时间戳和 frame_id不要用ros::Time::now()重新打时间戳。下游如果要做多传感器融合时间戳错开几十毫秒就够让 TF 抛 extrapolation 异常了。2.2 极坐标分箱把三维问题压成一维序列笛卡尔栅格是很多人的第一直觉把 XY 平面切成 0.2 m 的格子每个格子取最低点当地面。这个做法在近处没问题到 30 m 外就废了——同样一个 0.2 m 的格子近处能装几十个点远处可能一个点都没有地面高度估不出来出现一圈一圈的空洞。射线法换了个切法按方位角分箱。以雷达为原点把 360° 切成 N 条射线每条射线是一个扇形窄条条内的点按水平距离radius sqrt(x² y²)升序排。这样一维化之后物理意义非常干净——同一方位角上地面点沿半径方向必然是连续、缓慢变化的只要相邻两点的连线坡度不陡它们就同属地面。坡道、起伏路面在这种表示下都是坡度略大而不是高度不对鲁棒性直接上一个台阶。分箱数量由radial_divider_angle决定N ceil(2π / radial_divider_angle)。这个角度要和雷达的水平角分辨率对齐机械式 16 线一般在 0.2° 到 0.4° 之间取 0.18° 到 0.2° 比较合适。取得太小每条射线里的点太少远处的射线甚至是空的坡度算不出来取得太大一条射线里塞进好几个方位角的点横向的墙体会被误当成地面。排序是这个方案里唯一有量级开销的地方。一帧 10 万点按方位角分箱是 O(N)每条射线内部排序整体是 O(N log(N/k))k 是射线数实测下来单帧在 10 ms 到 20 ms 之间10 Hz 完全够用。真要抠性能射线内部的点本来就是按雷达扫描顺序排列的近似有序插入排序在这种数据上比快排更快这是后话。还有一个细节同心圆分段。同一方位角上两个点半径差得特别远比如中间有一片被遮挡的空白它们之间其实没有相邻关系硬算坡度会得到荒谬的结果。做法是给每条射线再加一层切分相邻点半径跳变超过concentric_divider_distance典型值 0.03 m 到 0.1 m就断开另起一段段与段之间不共享地面基准。2.3 三段判定全局坡度、局部坡度、回收判定的核心是一条规则一个点是不是地面取决于它相对当前地面基准的坡度。围绕这条规则我拆成三段每段解决一类误判。第一段是全局坡度粗筛。对每个点算它相对雷达原点的仰角atan2(z sensor_height, radius)绝对值超过general_max_slope就直接判为非地面连递推都不进。这一段的意义在于防污染树冠、广告牌、货车厢体这些明显高处的点如果不先剔掉它们会混进射线序列把后面的地面基准带偏。这一段只用一个点自身的信息代价极低。第二段是局部坡度递推也是算法的主体。沿射线从近到远走维护一个地面基准(gr, gz)对每个新点算slope atan2(z - gz, radius - gr)。坡度超过local_max_slope说明遇到了向上的立面——车轮、路沿、花坛边缘——判为非地面并且基准不更新这样障碍物后面的点仍然跟障碍物前面的地面比不会被抬起来。坡度平缓的点判为地面同时把基准推进到它身上让地面基准跟着地形走。第三段是回收。稀疏点云下射线容易断一段地面上只落了三五个点递推过程中断了剩下的点全被判成非地面rviz 里看就是一条一条的地面残留。回收逻辑很简单对每个被判非地面的点沿射线向两侧找最近的地面点如果高度差和半径差都在阈值内就改判为地面。这一段能把残留率压下去一个数量级代价是有可能把贴地的低矮障碍比如 5 cm 高的减速带一起回收掉阈值要在验收阶段盯着调。判定段依据主要参数拦住的典型误判全局坡度粗筛单点仰角general_max_slope树冠、建筑、货车厢体局部坡度递推与地面基准的坡度local_max_slope、min_height_threshold路沿、车轮、花坛边缘回收与邻近地面点的高差reclass_distance_threshold稀疏导致的条带残留3. 核心实现从 PointCloud2 到射线过滤主循环的代码落地3.1 点云预处理与坐标系对齐回调函数里第一件事不是滤波是把数据弄干净。步骤固定为fromROSMsg转成 PCL 类型、按需做 TF 变换、剔除 NaN 和 inf、丢掉过近和过远的点。顺序别乱先变换再剔除否则贴地的无效点会污染基准。void RayGroundFilterNode::cloudCallback(const sensor_msgs::PointCloud2ConstPtr msg) { const ros::WallTime t0 ros::WallTime::now(); // 1) 统一坐标系sensor_height 的口径必须和 target_frame 对齐 pcl::PointCloudpcl::PointXYZI::Ptr cloud(new pcl::PointCloudpcl::PointXYZI); if (!target_frame_.empty() msg-header.frame_id ! target_frame_) { sensor_msgs::PointCloud2 transformed; // 用输入帧的时间戳查 TF不能用 ros::Time::now()否则高速运动时会对不齐 if (!pcl_ros::transformPointCloud(target_frame_, msg-header.stamp, *msg, transformed, tf_buffer_)) { ROS_WARN_THROTTLE(5.0, TF %s - %s 查询失败丢弃该帧, msg-header.frame_id.c_str(), target_frame_.c_str()); return; } pcl::fromROSMsg(transformed, *cloud); } else { pcl::fromROSMsg(*msg, *cloud); } // 2) 剔除无效点atan2 拿到 inf 会让整条射线报废 cloud-is_dense false; std::vectorint valid; pcl::removeNaNFromPointCloud(*cloud, *cloud, valid); // 3) 复用成员变量避免每帧重新分配十万级点的内存 labels_.assign(cloud-size(), ray_filter::NON_GROUND); filter_.setSensorHeight(sensor_height_); filter_.filter(*cloud, labels_); publish(cloud, msg-header); // 沿用输入时间戳与 frame_id ROS_DEBUG_STREAM(单帧耗时 (ros::WallTime::now() - t0).toSec() * 1000.0 ms); }removeNaNFromPointCloud后面那个valid索引其实用不上但接口要求传留着不碍事。真正要留意的是cloud-is_dense false有些驱动会把is_dense标成 true 但实际带 NaNPCL 的某些函数会因此跳过检查。降采样这一步我一般不加。体素滤波会把点云打散同一个体素里只留一个代表点射线上的连续性被破坏坡度递推的输入质量反而下降。如果确实嫌点太多用 0.05 m 以下的半径滤波去掉孤立噪点别用体素栅格。3.2 射线过滤主循环的完整实现下面这段是整个节点的核心三个函数分别对应 2.3 里的三段判定。生产代码里RayFilter是个独立类不依赖 ROS这样离线拿 pcd 文件就能回归测试。namespace ray_filter { struct RayPoint { float radius; // 水平距离 sqrt(x^2 y^2) float z; // 高度与 sensor_height 同一坐标系 int index; // 回写原始点云标签用 }; // 第一步按方位角分箱箱内按半径升序 void RayFilter::buildRays(const pcl::PointCloudpcl::PointXYZI cloud) { const int ray_num std::max(1, static_castint(std::ceil(2.0 * M_PI / radial_divider_angle_))); rays_.assign(ray_num, {}); for (auto r : rays_) r.reserve(256); // 预留避免反复扩容 for (size_t i 0; i cloud.size(); i) { const auto p cloud[i]; const float r std::hypot(p.x, p.y); if (r min_point_distance_ || r max_range_) continue; // 盲区与超远噪点 float a std::atan2(p.y, p.x); if (a 0.0f) a 2.0f * static_castfloat(M_PI); // 归一化到 [0, 2pi) const int bin std::min(ray_num - 1, static_castint(a / radial_divider_angle_)); rays_[bin].push_back({r, p.z, static_castint(i)}); } for (auto ray : rays_) { std::sort(ray.begin(), ray.end(), [](const RayPoint a, const RayPoint b) { return a.radius b.radius; }); } } // 第二步逐条射线做三段判定 void RayFilter::classifyRay(const std::vectorRayPoint ray, std::vectoruint8_t label) const { if (ray.empty()) return; bool has_ref false; float gr 0.0f, gz 0.0f; // 当前地面基准的半径与高度 for (size_t i 0; i ray.size(); i) { const RayPoint p ray[i]; // 段一全局坡度粗筛高处物体直接出局不参与基准递推 const float elevation std::atan2(p.z sensor_height_, p.radius) * RAD2DEG; if (std::fabs(elevation) general_max_slope_) { label[p.index] NON_GROUND; continue; } if (!has_ref) { // 射线上的第一个有效点作为初始基准 label[p.index] GROUND; gr p.radius; gz p.z; has_ref true; continue; } // 段二局部坡度递推 const float dr p.radius - gr; if (dr 1e-3f) { label[p.index] NON_GROUND; continue; } // 同半径重复点算不出坡度 const float dz p.z - gz; const float slope std::atan2(dz, dr) * RAD2DEG; if (slope local_max_slope_) { label[p.index] NON_GROUND; continue; } // 立面基准不动 label[p.index] GROUND; // 只有高度差足够小、或明显下坡的点才把基准推进过去 if (std::fabs(dz) min_height_threshold_ || slope 0.0f) { gr p.radius; gz p.z; } } } // 第三步回收贴地薄层压掉条带状的残留 void RayFilter::reclassify(const std::vectorRayPoint ray, std::vectoruint8_t label) const { for (size_t i 0; i ray.size(); i) { if (label[ray[i].index] GROUND) continue; for (int dir : {-1, 1}) { // 前后各找一次最近的地面点 int j static_castint(i) dir; while (j 0 j static_castint(ray.size()) label[ray[j].index] ! GROUND) j dir; if (j 0 || j static_castint(ray.size())) continue; if (std::fabs(ray[i].z - ray[j].z) reclass_distance_threshold_ std::fabs(ray[i].radius - ray[j].radius) reclass_radius_threshold_) { label[ray[i].index] GROUND; break; } } } } } // namespace ray_filterRAD2DEG就是180.0 / M_PI写成宏或 constexpr 都行别每次乘除里现算。几个参数的作用需要单独交代清楚。sensor_height是基准高度所有z sensor_height都在把点换算回相对地面的高度它填错全局坡度那一段就整体偏移。general_max_slope控制粗筛的松紧12° 是个稳妥的起点——平地场景调到 8° 更干净坡道场景得放到 15° 以上。local_max_slope是主体判据8° 对应大约 14% 的坡度能过掉正常路面的起伏又拦得住路沿。min_height_threshold是基准推进的门槛0.15 m 的意思是高差小于 15 cm 才敢相信这是地面它就是减速带能不能被保住的直接开关。reclass_distance_threshold反过来用越大越激进条带残留越少低矮障碍物也越容易被吃掉。标签回写的部分不在上面代码里逻辑很简单遍历一遍labels_值为 GROUND 的塞进ground_cloud其余的塞进nground_cloud两边都保留原始 intensity。rviz 里用 intensity 上色一眼就能看出哪块地面对不上。3.3 ROS 封装与 launch、参数的工程组织节点本体只负责 IO滤波逻辑放在独立类里这是能离线回归的前提。参数全部从参数服务器读构造时给默认值dynamic_reconfigure可选——答辩现场改参数能立刻看到 rviz 变化观感差别很大。launch arg namerviz defaulttrue/ !-- 生产环境建议用 nodelet省掉一次 PointCloud2 的序列化拷贝 -- node pkgray_ground_filter typeray_ground_filter_node nameray_ground_filter outputscreen rosparam commandload file$(find ray_ground_filter)/config/params_park.yaml/ param nametarget_frame valuebase_link/ remap frompoints_raw to/rslidar_points/ /node node if$(arg rviz) pkgrviz typerviz namerviz args-d $(find ray_ground_filter)/rviz/filter.rviz/ /launch# config/params_park.yaml —— 园区平地场景雷达离地 1.85 m sensor_height: 1.85 general_max_slope: 12.0 # 度粗筛仰角上限 local_max_slope: 8.0 # 度射线内相邻地面点坡度上限 radial_divider_angle: 0.18 # 弧度约 10.3°对应 2000 条射线见下方说明 concentric_divider_distance: 0.05 min_point_distance: 1.5 # 米盲区小于它的点直接丢 max_range: 60.0 min_height_threshold: 0.15 reclass_distance_threshold: 0.10 reclass_radius_threshold: 0.20注意radial_divider_angle的单位代码里用的是弧度。0.18 弧度是 10° 左右只够切 35 条射线太粗了实际要的是 0.18 度换算成弧度是 0.00314。这个单位错在第一次跑的时候几乎必然出现表现是过滤结果像马赛克。yaml 里我习惯直接写弧度并在注释里标度数避免二次换算出错。4. 参数标定与场景适配六个参数按顺序调4.1 sensor_height 与两处坡度的标定流程sensor_height不要拿卷尺一量就完事。车停稳、胎压正常、空载把车开到一块尽量平的水泥地上录 30 秒静止的 bag然后看/points_ground的高度分布取中位数和-sensor_height比。差 5 cm 以内可以接受差 10 cm 以上说明要么高度量错了要么这块地本身不平。我一般会选三块不同的场地各测一次取一致的那个值。general_max_slope的标定需要一个已知有坡的场景。找个缓坡或者地下车库出入口把参数从 5° 开始往上加每次加 3°直到坡面上的地面点不再大片丢失为止然后再加 2° 留余量。这个值偏小的后果比偏大严重得多偏小会让坡面点被判成障碍物下游聚类直接多出一堵墙偏大只是让一些高处物体混进候选后面局部坡度还能兜住。local_max_slope反过来调。在平地上从 15° 往下减盯着 rviz 里减速带和井盖还在不在。减到减速带开始被削平就回退 1° 到 2°。多数园区场景最后落在 7° 到 9° 之间。4.2 调参顺序与场景对照调参有个固定顺序跳步会来回打架先sensor_height再min_point_distance与max_range定作用域然后general_max_slope定粗筛接着local_max_slope定主体再radial_divider_angle与concentric_divider_distance定分辨率最后才是两个回收阈值定残留率。回收阈值放最后是因为它本质上是补救前面的参数没定好补救参数怎么调都是在给错误打补丁。场景sensor_heightgeneral_max_slopelocal_max_slope主要风险地下车库1.6~1.9107顶棚立柱误判、坡道入口丢失园区平地道路1.8~1.9128减速带被削平有连续坡道1.8~2.016~1810~12高处树冠混入需配合高度裁剪工地/草地1.7~2.01512坑洼与起伏被当成障碍需放宽回收阈值表里的数字是起点不是终点同一套参数换一台车、换一个安装角度就得重调。我的习惯是每组场景存一份 yamllaunch 文件用arg切换别在一个文件里注释来注释去过两周自己都看不懂哪行是有效的。4.3 和 RANSAC 平面拟合、栅格高度图的边界射线法不是唯一选择把它和另外两种常见做法摆在一起看选型边界很清楚。方案原理坡道表现单帧耗时参数敏感度适合场景RANSAC 平面拟合拟合一个全局平面距离阈值内算地面差单一平面表达不了坡度变化5~15 ms中距离阈值关键室内平地、快速验证射线法 ray filter按方位角切射线坡度递推好地面基准跟着地形走10~20 ms中高参数多但物理意义明确园区、车库、城市道路栅格高度图XY 分格取最低点或分位数一般远处格子稀疏5~10 ms低大范围可行驶区域估计RANSAC 的问题很直接它假设地面是一个平面坡道、桥接、起伏路面全都违反这个假设一上坡地面就被切成两半。栅格的问题在远处格子边长固定30 m 外一个格子可能一个点都没有地面高度估不出来。射线法用极坐标绕开了这两个问题代价是参数多一些。什么时候不该用射线法越野地形地面本身就有大起伏和坑洼任何坡度先验都不成立多层结构比如立交桥上下两层投影到同一方位角射线法会直接把下层当地面、上层当障碍或者反过来。这两种情况得靠分区域处理或者换地面分割算法射线法只能当预筛。5. 避坑与排查五类在实车上反复出现的翻车现场5.1 坐标系与安装高度类误判现象第一帧下去整片地面被删得干干净净/points_ground几乎空/points_no_ground里全是地面点。原因sensor_height和target_frame的口径没对齐。点云已经变换到 base_link但sensor_height还是按雷达坐标系填了 0或者反过来点云留在雷达坐标系参数却填了 1.85。两种都会让z sensor_height整体偏 1.8 m全局坡度那一段把所有地面点当成仰角超限的高处物体。解决在回调里打一行日志输出变换后的 frame_id 和点云 z 的中位数。平地场景下这个中位数应该接近-sensor_height差得远就是口径错了。这一步加到启动自检里比事后猜快得多。现象平地上好好的一上坡地面大片丢失坡道被识别成一堵墙。原因general_max_slope按平地调的8° 的坡就超限了坡面上的点在第一段粗筛就被判成非地面根本进不了递推。解决按 4.1 的流程在真实坡道上重标往上放 5° 到 8°。同时确认sensor_height没有偏大——偏大等价于把整个地面往下压效果和坡度阈值偏小一样。5.2 射线断裂与条带残留现象rviz 里地面点云呈放射状条纹一条有一条没有或者远处地面残留成一道道弧线。原因radial_divider_angle和雷达水平角分辨率没对齐。参数比实际角分辨率还小一条射线里只有零星几个点甚至一个点坡度算不出来递推直接断或者concentric_divider_distance设得太小正常的地面点被切成一堆碎段每段只有两三个点。解决先查雷达手册确认水平角分辨率把radial_divider_angle取到分辨率附近或略大别贪细。concentric_divider_distance一般给 0.05 m 左右稀疏的远距离区域可以放宽到 0.1 m。改完盯着/points_ground看条纹有没有合上。现象减速带、井盖、地面标线被削平了下游检测对这些低矮目标的召回直接掉。原因local_max_slope太松加上reclass_distance_threshold太大十几厘米高的凸起被回收段当成了贴地噪点吃掉。解决先把回收阈值降到 0.05 m 到 0.1 m 之间看效果还不够就压local_max_slope。这里有个取舍要摆明回收阈值调小条带残留会回来一些需要在验收阶段用同一批 bag 反复比对别凭感觉定。5.3 工程集成侧的坑现象单独跑节点 12 ms 一帧接到整条链路里掉到 5 HzCPU 单核跑满。原因每帧都在重新分配十万级的 vectorrays_.assign()里外扩容好几次或者回调参数按值传递白拷一份点云再或者用了默认的ros::spin订阅队列设成 10前一帧没处理完后一帧已经在排队堆积起来延迟越来越高。解决rays_和labels_提成成员变量reserve一次之后复用回调参数用constPtr订阅队列压到 1 到 2宁可丢帧也不要堆积。点云量大到内存拷贝成瓶颈时换 nodelet能省掉一次序列化。现象下游 TF 报 extrapolation或者融合模块里点云和图像对不齐。原因发布时用了ros::Time::now()而不是输入消息的时间戳处理耗时被算进了传感器延迟里。解决header.stamp msg-header.stamp; header.frame_id target_frame_;两行写死别偷懒。这个坑不报错、不崩溃只是精度悄悄变差最难查。6. 验收与答辩用 rosbag 回放和三个量化指标证明节点可用验收不要靠 rviz 目测。我一般会录 3 段 bag平地直道、坡道进出、有减速带和人车混行的路段每段 30 秒左右回放时同时录下/points_raw、/points_ground、/points_no_ground然后用固定参数跑离线回归。指标就三个地面残留率被判为非地面的点里实际属于地面的比例、前景误删率实际障碍物点被删掉的比例、单帧耗时。前两个抽 20 帧手工标注就够不用做全量。落地的方式是把标注存成索引文件用一个脚本逐帧比对。# 1) 采一段验证数据同时录原始点云和两路输出 rosbag record -O validate_park.bin /points_raw /points_ground /points_no_ground # 2) 半速回放保证不丢帧方便逐帧对照 rosbag play --clock -r 0.5 validate_park.bin # 3) 看处理频率和单帧耗时分布 rostopic hz /points_no_ground指标目标值测法地面残留率低于 3%20 帧手工标注比对前景误删率低于 5%同上重点看 0.3 m 以下的低矮目标单帧耗时低于 30 ms均值ros::WallTime打点跑 300 帧取分位数答辩时最有说服力的不是算法讲得多花是三样东西一张过滤前后的并排点云图、一张四组场景参数表、一条耗时随点数变化的曲线。老师或者评委追问为什么不用深度学习时口径可以很干脆这套方案参数物理意义明确、单帧十几毫秒、不需要标注数据、在嵌入式平台上能跑作为感知链路的前置过滤环节性价比比一个分割网络高网络放在后面的检测环节更合适。有个教训我印象很深早期版本我只看平地场景的效果参数调得挺漂亮结果一到坡道入口地面整片丢失下游把坡道识别成了一堵墙车直接停在坡底不动。从那以后我每次改完参数都强制走一遍平地—坡道—减速带三段回放三个场景的指标同时看任何一个退化就回退。这套流程笨但省下来的返工时间比调参本身多得多。希望帮到你。本文还有配套的精品资源点击获取