ROS激光雷达点云射线法地面过滤节点设计与调参

简介：面向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 就知道。

话题契约建议一开始就定死，别中途改：

坐标系保持一致是最容易被忽略的一条。滤波在哪个坐标系下做，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 高的减速带）一起回收掉，阈值要在验收阶段盯着调。

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::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::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::vector<int> 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 但实际带 NaN，PCL 的某些函数会因此跳过检查。

降采样这一步我一般不加。体素滤波会把点云打散，同一个体素里只留一个代表点，射线上的连续性被破坏，坡度递推的输入质量反而下降。如果确实嫌点太多，用 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::PointCloud<pcl::PointXYZI>& cloud) { const int ray_num = std::max(1, static_cast<int>(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_cast<float>(M_PI); // 归一化到 [0, 2pi) const int bin = std::min(ray_num - 1, static_cast<int>(a / radial_divider_angle_)); rays_[bin].push_back({r, p.z, static_cast<int>(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::vector<RayPoint>& ray, std::vector<uint8_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::vector<RayPoint>& ray, std::vector<uint8_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_cast<int>(i) + dir; while (j >= 0 && j < static_cast<int>(ray.size()) && label[ray[j].index] != GROUND) j += dir; if (j < 0 || j >= static_cast<int>(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_filter

RAD2DEG就是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 name="rviz" default="true"/> <!-- 生产环境建议用 nodelet，省掉一次 PointCloud2 的序列化拷贝 --> <node pkg="ray_ground_filter" type="ray_ground_filter_node" name="ray_ground_filter" output="screen"> <rosparam command="load" file="$(find ray_ground_filter)/config/params_park.yaml"/> <param name="target_frame" value="base_link"/> <remap from="points_raw" to="/rslidar_points"/> </node> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" 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（定分辨率），最后才是两个回收阈值（定残留率）。回收阈值放最后，是因为它本质上是"补救"，前面的参数没定好，补救参数怎么调都是在给错误打补丁。

表里的数字是起点不是终点，同一套参数换一台车、换一个安装角度就得重调。我的习惯是每组场景存一份 yaml，launch 文件用arg切换，别在一个文件里注释来注释去，过两周自己都看不懂哪行是有效的。

4.3 和 RANSAC 平面拟合、栅格高度图的边界

射线法不是唯一选择，把它和另外两种常见做法摆在一起看，选型边界很清楚。

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 Hz，CPU 单核跑满。

原因：每帧都在重新分配十万级的 vector，rays_.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

答辩时最有说服力的不是算法讲得多花，是三样东西：一张过滤前后的并排点云图、一张四组场景参数表、一条耗时随点数变化的曲线。老师或者评委追问"为什么不用深度学习"时，口径可以很干脆：这套方案参数物理意义明确、单帧十几毫秒、不需要标注数据、在嵌入式平台上能跑，作为感知链路的前置过滤环节，性价比比一个分割网络高，网络放在后面的检测环节更合适。

有个教训我印象很深：早期版本我只看平地场景的效果，参数调得挺漂亮，结果一到坡道入口，地面整片丢失，下游把坡道识别成了一堵墙，车直接停在坡底不动。从那以后我每次改完参数，都强制走一遍"平地—坡道—减速带"三段回放，三个场景的指标同时看，任何一个退化就回退。这套流程笨，但省下来的返工时间比调参本身多得多。希望帮到你。

本文还有配套的精品资源，点击获取