简介：面向机器人感知与自动驾驶点云处理方向的开发者，这份基于 Ray Filter 的雷达点云地面过滤 ROS 节点给出了一套可直接参考的工程实现，重点解决 LiDAR 点云中地面点剔除与非地面特征保留的问题，可服务于导航、建图与环境感知等任务。压缩包共 8 个文件，约 4KB，包含 2 个 cpp 源文件、1 个头文件，以及 launch 启动配置、package.xml 与 CMakeLists.txt 等 ROS 工程描述文件，另有 json 配置和 txt 说明，整体结构精简，便于快速理解节点组织与 PCL 算法调用方式。已有 7714 人学习，说明该主题在机器人导航与环境感知场景中关注度较高。读者可从中获得 Ray Filter 地面分割的实现思路、ROS 节点封装方式及 PCL 点云处理代码框架，适合作为地面过滤实验、课程项目或工程原型的起步参考，并据此调整射线长度、地面判定阈值等参数以适配不同场景。

1. 从一帧被地面点淹没的雷达数据说起

去年在一个封闭园区跟一台低速小车，激光雷达装在一米八的车顶。第一次上路地面过滤就没过：一帧十几万个点里地面占了七成多，代价地图上刷出一圈贴地的假障碍，规划器一脚刹车踩死。改用 PCL 里的 RANSAC 平面拟合，遇到缓坡和路沿直接歇菜，它默认整个场景只有一个平面。后来把这块换成射线法：按方位角把点云切成细长扇形列，列内沿半径由近到远比较相邻点的高差与坡度，坡够缓的就串成地面。这就是基于 ray filter 的雷达点云地面过滤 ROS 节点干的事，PCL 管点云容器与滤波，ROS 管订阅、参数和发布。适合做低速无人车、巡检机器人，或者手里有机械式激光雷达要接进导航栈的人。

2. 射线法地面分割：三种思路的取舍与一帧点云的五段链路

2.1 高度阈值、平面拟合、射线法，三条路各自的死法

地面分割这个活儿听着简单，实际是点云处理里最容易翻车的一环，因为它的评价标准不是"平均效果好"，而是"最坏那一帧不能出错"。一帧错判，代价地图上就多一圈障碍，规划器要么急刹要么绕远，用户体感直接崩。所以先要把可选方案摆开看清楚，别上来就抄参数。

最省事的是高度阈值法：雷达装在离地h米，那么地面点的 z 大约在-h附近，取|z + h| < δ就算地面。一行 if 就能跑，平地场景快得没感觉。问题是它对坡度零容忍。5 度的坡走 20 米，地面抬升 1.75 米，远超 δ；车身一压缩悬挂、一加速抬头，整帧的基准面都跟着漂。这种方案只在室内平地或者扫地机那种场景能用。

平面拟合是 PCL 用户最先想到的，pcl::SACSegmentation配SACMODEL_PLANE，RANSAC 迭代几百次找最大平面。它在单平面场景确实稳，但代价有两个：一是每次迭代都要随机采样，耗时抖动大，帧率高的时候很要命；二是它必须二选一，遇到"S 弯坡道""上下两层地面""斜坡加平台"这种多平面场景，它只会把最大的那个平面判成地面，剩下的全推给障碍。RANSAC 还有个隐性成本，setDistanceThreshold是个绝对值，半径 5 米和 40 米用同一个阈值，远处的地面点必然被漏掉。

射线法走的是另一条路：不做全局平面假设，只假设"地面在局部是连续的，且沿射线方向的坡度有上界"。它把一帧点云按方位角切成很多细长的扇形列，每一列内的点在地理上就是沿着同一条射线往前排的，于是可以在列内做一维的相邻点比较。远处点稀、近处点密的问题被自然消化掉了，因为比较的是同一列内的相邻点，而不是全局距离阈值。代价是需要一个靠谱的安装高度，而且对分列参数、坡度阈值相当敏感，这几个参数调不好，效果比 RANSAC 还难看。

还有一类是 2.5D 栅格法，把点云投影成高程栅格，每格存最低点或者高程统计量，再做形态学开闭运算。它对起伏地形适应性最好，代价是内存和更新开销，栅格分辨率一细就吃不消。低速园区车用射线法性价比最高：路况是铺装路面加缓坡，没有太多垂直结构，射线法的假设基本成立。

2.2 一帧点云在节点里走过的五段链路

射线法节点的结构其实很朴素，一帧点云进来，走完五段再出去。把这个链路记牢，后面调参和排错时就知道该在哪一段下钩子。

第一段的接入层要处理一个容易被忽略的点：如果下游没人订阅，这帧点云根本不该算。地面过滤是纯 CPU 活儿，一帧几十万点分列加排序能吃掉十几毫秒，多个节点一起跑就把核占满了。在回调开头判一下getNumSubscribers()，没人要就直接返回，这是最便宜的一行优化。

第二段裁剪是纯粹为了省时间。半径外和高度外的点不参与分列，能砍掉三到五成的点。这里的裁剪不是"精确过滤"，而是"粗筛"，所以阈值可以放宽，宁可多留也别误杀，精确判断交给第四段。

第三段分列是射线法的骨架。它决定了"谁和谁比"，分错了列，后面再怎么调坡度阈值都是白搭。这一段也是内存分配的高发区，如果每帧都重新构造一个vector<vector<Point>>，跑十几分钟就能看到内存曲线锯齿状爬升。

第四段判定是算法核心，也是唯一需要仔细推敲的地方。第五段发布看似简单，但 header、frame_id、字段类型这三样丢一样，rviz 里就是一片空白，后面第五节会专门讲。

2.3 分列的数学：方位角离散化和列内坡度判据

分列这一步的数学只有两行，但里面的坑不少。对每个点算方位角：

az = atan2(y, x) // 返回值在 (-π, π] az = az < 0 ? az + 2π : az // 归一化到 [0, 2π) idx = floor(az / bin_angle) // bin_angle 单位为弧度

atan2的返回值是(-π, π]，如果不做归一化，负角度那一半会被映射到负索引上，直接越界。这个错误在小范围测试时不容易暴露，因为车头正前方的点 x 都大于 0，方位角是正的；一掉头或者一进弯，后半圈的点全部丢失，现象是"地面点云只显示半边"。这类问题排查起来非常费时间，放在代码里就是一行归一化的事。

列数由角分辨率决定：N = ceil(2π / bin_angle)。bin_angle取 0.1° 时 N 是 3600，取 0.2° 时是 1800。这个参数不能拍脑袋定，必须和雷达的水平角分辨率对齐。假设雷达水平角分辨率是 0.2°，一圈 1800 个点：

• 若bin_angle = 0.05°，一列 7200 个桶，平均每桶 0.25 个点，绝大多数列只有一个点甚至空列，相邻点比较根本无从谈起，判出来的地面千疮百孔。
• 若bin_angle = 0.4°，每列平均 2 个点，但这两个点在物理上已经横跨了 0.4°，40 米外的水平间距接近 0.28 米，这个距离上的高差可能来自两栋建筑的边缘，坡度算出来毫无意义。
• 取 0.1°~0.2°，大约等于雷达水平角分辨率的一半到一倍，是比较稳的区间。点云稀的雷达往小取，点云密的往大取，让每列平均有 1.5~3 个点最舒服。

判据本身用坡度而不是纯高差，是为了对付缓坡。设列内相邻两点半径差Δr、高度差Δz，则

slope = atan2(|Δz|, Δr) // 单位弧度，转成度更直观

用atan2而不是直接|Δz| / Δr，是因为Δr可能趋近于 0（同一列里有两个点半径相同，多线雷达在近距离很常见），除法会炸出 inf。atan2在Δr = 0时返回π/2，也就是"垂直"，自然被判成非地面，行为是安全的。

判据有两条腿：局部坡度约束和累计高度约束。局部坡度管"这一步不能跨太陡"，累计高度管"从最近的地面点一路走过来，总抬升不能超过某个量"。只有局部约束的话，一个 3 度的缓坡连续走 100 个点，累计能抬升好几米，坡顶上的墙面就被吃成地面了。加上累计约束，能把这个漂移截住。

3. 工程结构：功能包目录、launch 与六个核心参数

3.1 目录树与每个文件的职责

一个能长期维护的射线法节点，建议按"算法与 ROS 解耦"来切文件。这不是为了好看，而是为了能离线跑。地面过滤的参数特别依赖场景，园区和地下车库的阈值完全不一样，如果不能拿 bag 离线快速试参数，每次都要上车跑一圈，一天试不了三组。

ray_ground_filter/ ├── CMakeLists.txt ├── package.xml ├── include/ray_ground_filter/ │ ├── point_rt.h // 半径-方位角点结构，无 ROS 依赖 │ └── ray_ground_filter.h // 算法类声明 ├── src/ │ ├── ray_ground_filter.cpp // 纯算法：分列、排序、坡度判定 │ └── ray_ground_filter_node.cpp // ROS 壳：参数、订阅、发布 ├── launch/ │ └── ray_ground_filter.launch ├── config/ │ └── params.yaml └── rviz/ └── ground_filter.rviz

point_rt.h只定义结构体，ray_ground_filter.cpp只依赖 PCL 和 Eigen，不 include 任何ros/ros.h。这样算法类可以被一个只读 pcd 文件的小程序直接调用，调参循环从"上车跑一圈"缩短到"离线跑一百帧"。这是我在几个项目里反复吃到甜头的做法，尤其是地面这种参数敏感、效果又不好量化的模块。

ROS 壳那一层只做四件事：读参数、转消息、调算法、发消息。壳越薄越好，壳里塞逻辑，后面想换消息类型或者加个服务接口都得动核心代码。

3.2 CMakeLists.txt 与 package.xml 里不能省的那几行

PCL 和 ROS 混用时，依赖声明漏一项的症状通常是链接期报一堆undefined reference to pcl::...，或者运行期 fromROSMsg 直接段错误。

cmake_minimum_required(VERSION 3.0.2) project(ray_ground_filter) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs pcl_ros # 提供 pcl::fromROSMsg 的消息桥接 pcl_conversions # 提供时间戳与 header 转换 ) find_package(PCL REQUIRED COMPONENTS common filters io) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS roscpp sensor_msgs pcl_ros pcl_conversions DEPENDS PCL ) include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_executable(ray_ground_filter_node src/ray_ground_filter_node.cpp src/ray_ground_filter.cpp ) target_link_libraries(ray_ground_filter_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} )

pcl_ros和pcl_conversions两个包容易只写一个。pcl_conversions/pcl_conversions.h才是pcl::fromROSMsg/pcl::toROSMsg的实际声明处，pcl_ros更多是给pcl_ros::transformPointCloud这类节点级工具用的。只想做格式转换的话，pcl_conversions是必须的，pcl_ros可以按需。

PCL REQUIRED COMPONENTS里只写实际用到的模块。全量find_package(PCL REQUIRED)会把所有模块的头文件路径塞进 include，编译时间明显变长，在算力紧张的工控机上尤其明显。

3.3 launch 文件：把阈值从代码里赶出去

参数硬编码在构造函数里，第一次调参就要重新编译，上车试三组就天黑。用 launch 的rosparam把参数外置，改阈值只需要改文件重启节点。

<launch> <arg name="input_topic" default="/lidar_points"/> <node pkg="ray_ground_filter" type="ray_ground_filter_node" name="ray_ground_filter" output="screen"> <!-- 安装高度，务必和实际测量值一致 --> <param name="sensor_height" value="1.80"/> <param name="min_range" value="1.5"/> <param name="max_range" value="45.0"/> <param name="radial_divider_angle" value="0.12"/> <param name="general_max_slope" value="8.0"/> <param name="local_max_slope" value="12.0"/> <param name="local_height_threshold" value="0.25"/> <param name="initial_height_cut" value="0.15"/> <remap from="input" to="$(arg input_topic)"/> </node> </launch>

<remap>那一行是关键。很多人把订阅话题写死在nh.subscribe("/lidar_points", ...)里，换个雷达就得改代码。写成相对话题input，再用 remap 映射，同一份 launch 能在不同车上复用。

3.4 参数表：六个阈值决定成败

看这张表能发现一个规律：所有"调大"的后果都是"吃障碍"，所有"调小"的后果都是"吃地面"。射线法的调参本质是在这两个方向的误判之间找平衡点，没有一组参数能同时最优，只能按场景取舍。园区道路我一般偏保守——宁可多留几个地面点让代价地图多做一次膨胀，也不要把路沿漏成障碍，因为漏判地面最多让代价地图糊一点，漏判障碍是真的会撞。

4. 核心代码拆解：从点云回调到两片云发布

4.1 节点骨架与回调入口

先看结构体和类的骨架。把点从PointXYZ转成带半径和方位角的结构体，是后面所有比较的前提，因为atan2和sqrt这两个三角函数每帧要算几十万次，只算一遍存下来，比在比较循环里反复算要快得多。

// include/ray_ground_filter/point_rt.h struct PointRT { float x, y, z; float radius; // 水平半径 sqrt(x*x + y*y) float azimuth; // 方位角，已归一化到 [0, 2π) bool is_ground; // 判定结果 };

// src/ray_ground_filter_node.cpp（节选） class RayGroundFilterNode { public: RayGroundFilterNode() : nh_(), pnh_("~") { pnh_.param("sensor_height", sensor_height_, 1.8); pnh_.param("min_range", min_range_, 1.5); pnh_.param("max_range", max_range_, 45.0); pnh_.param("radial_divider_angle", radial_divider_angle_, 0.12); pnh_.param("general_max_slope", general_max_slope_, 8.0); pnh_.param("local_max_slope", local_max_slope_, 12.0); pnh_.param("local_height_threshold", local_height_threshold_, 0.25); pnh_.param("initial_height_cut", initial_height_cut_, 0.15); // 列数只和角分辨率有关，构造时算一次，避免每帧重复 const float bin_rad = radial_divider_angle_ * M_PI / 180.0f; bin_num_ = static_cast<size_t>(std::ceil(2.0 * M_PI / bin_rad)); bin_rad_ = bin_rad; bins_.resize(bin_num_); // 只在这里分配一次，之后每帧 clear sub_ = nh_.subscribe("input", 2, &RayGroundFilterNode::cloudCallback, this, ros::TransportHints().tcpNoDelay()); pub_ground_ = nh_.advertise<sensor_msgs::PointCloud2>("ground_cloud", 1); pub_nonground_ = nh_.advertise<sensor_msgs::PointCloud2>("obstacle_cloud", 1); } // ... private: ros::NodeHandle nh_, pnh_; ros::Subscriber sub_; ros::Publisher pub_ground_, pub_nonground_; std::vector<std::vector<PointRT>> bins_; size_t bin_num_ = 0; float bin_rad_ = 0.0f; double sensor_height_ = 1.8, min_range_ = 1.5, max_range_ = 45.0; double radial_divider_angle_ = 0.12; double general_max_slope_ = 8.0, local_max_slope_ = 12.0; double local_height_threshold_ = 0.25, initial_height_cut_ = 0.15; };

这里有两个刻意的选择。bins_.resize(bin_num_)放在构造函数里，之后每帧只做clear()，是因为vector<vector<T>>每次重新构造会触发 bin_num_ 次小分配，3600 个桶乘上几十赫兹，内存碎片和分配开销都很可观。subscribe的队列长度设 2 而不是常见的 10，是因为地面过滤本身耗时不稳定，队列积压只会让延迟越来越大，宁可丢帧也别排队，下游拿到一帧旧点云比丢一帧更危险。ros::TransportHints().tcpNoDelay()关掉 Nagle 算法，在小消息高频场景下能省掉几十毫秒的攒包延迟。

4.2 半径裁剪与高度粗切

裁剪这一段的分工要清楚：半径裁剪解决"算不算"，高度粗切解决"值不值得细算"。

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) { // 没有人订阅就不要算，这一行能省掉大量无用 CPU if (pub_ground_.getNumSubscribers() == 0 && pub_nonground_.getNumSubscribers() == 0) return; pcl::PointCloud<pcl::PointXYZ>::Ptr raw(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *raw); if (raw->empty()) return; // 每帧复用，避免反复分配 for (auto& bin : bins_) bin.clear(); const float min_r2 = static_cast<float>(min_range_ * min_range_); const float max_r2 = static_cast<float>(max_range_ * max_range_); // 高度粗切：地面以下留 1m 容差，车体以上 1m 之外不可能是地面 const float z_low = static_cast<float>(-sensor_height_ - 1.0); const float z_high = static_cast<float>(-sensor_height_ + 1.0); for (const auto& p : raw->points) { if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; const float r2 = p.x * p.x + p.y * p.y; if (r2 < min_r2 || r2 > max_r2) continue; if (p.z < z_low || p.z > z_high) continue; float az = std::atan2(p.y, p.x); // (-π, π] if (az < 0.0f) az += 2.0f * M_PI; // 归一化，漏了这行就只剩半圈 size_t idx = static_cast<size_t>(az / bin_rad_); if (idx >= bin_num_) idx = bin_num_ - 1; // 浮点误差兜底 bins_[idx].push_back(PointRT{p.x, p.y, p.z, std::sqrt(r2), az, false}); } // ... 进入判定 }

std::isfinite那三行不是洁癖。机械式雷达在转速不稳或者强反光（玻璃幕墙、湿地面）时会吐出 NaN 和 inf，这些值进到atan2和sqrt里会污染整列的比较，NaN 参与的任何比较都返回 false，表现是"某一整条扇形区域完全没有地面点"。这类数据在离线看的 pcd 里是看不出来的，只有在线跑才会偶发。

高度粗切的上下界用-sensor_height_ ± 1.0，上界给 1 米是因为有些长坡加上车身俯仰，地面点在雷达坐标系里可能抬到接近 0；下界给 1 米容差是为了兜住安装高度测量误差和悬挂压缩。这个粗切只是筛掉明显不可能的，真正的判断在下一段。

4.3 分列与列内排序

分完列之后，每列内必须按半径从小到大排序，否则"相邻点"就没有物理意义。排序的比较函数只有一个字段，不要为了省事用PointRT的默认比较。

void RayGroundFilter::classifyBins(std::vector<std::vector<PointRT>>& bins) { for (auto& bin : bins) { if (bin.size() < 2) { // 单点列无法比较，直接判非地面 for (auto& p : bin) p.is_ground = false; continue; } // 按水平半径升序，保证"相邻"是同一条射线上的前后关系 std::sort(bin.begin(), bin.end(), [](const PointRT& a, const PointRT& b) { return a.radius < b.radius; }); const std::vector<char> fwd = scanOneDirection(bin, /*reverse=*/false); const std::vector<char> bwd = scanOneDirection(bin, /*reverse=*/true); // 双向都认为是地面才保留，压掉单向扫描的毛刺 for (size_t i = 0; i < bin.size(); ++i) { bin[i].is_ground = (fwd[i] && bwd[i]); } } }

bin.size() < 2直接判非地面的策略偏保守。有人会把它判成地面，理由是"孤点大概率是远处稀疏的地面"，但更常见的情况是孤点是噪点或者小物体。在障碍检测里，把障碍误判成地面是致命的，把地面误判成障碍只是让代价地图糊一点，所以这里选保守。

双向扫描的思路是：正向从最近点往远处扫，反向从最远点往近处扫，两次都判成地面的才保留。单向扫描有个固有缺陷——起点错了整条链就错了。正向扫描的起点是最近点，如果最近点恰好是车体反光或者地面上一个凸起，后面一整列都会被带偏；反向扫描从远处起步，远处的第一点通常是地面，能纠正过来。取交集相当于让两个方向互相校验，代价是列内要多排一次序（排序本身是同一份，只是遍历方向不同），CPU 增加不到 50%。

4.4 坡度判定与正向反向两遍扫描

这是整个节点的核心，二十来行代码决定了效果好坏。

std::vector<char> RayGroundFilter::scanOneDirection( const std::vector<PointRT>& bin, bool reverse) { const size_t n = bin.size(); std::vector<char> ground(n, 0); // 起点：最近（或最远）那个点必须在安装高度附近，才允许作为地面种子 const size_t start = reverse ? n - 1 : 0; const size_t step = reverse ? -1 : 1; const size_t end = reverse ? 0 : n - 1; const float seed_dz = std::fabs(bin[start].z + static_cast<float>(sensor_height_)); if (seed_dz > static_cast<float>(initial_height_cut_)) return ground; // 全 0 ground[start] = 1; // 记录种子的位置，用于累计坡度约束 float seed_z = bin[start].z; float seed_r = bin[start].radius; size_t prev = start; for (size_t i = start + step; ; i += step) { const float dr = std::fabs(bin[i].radius - bin[prev].radius); const float dz = bin[i].z - bin[prev].z; // 局部判据：这一步不能跨太陡 const float local_slope = std::atan2(std::fabs(dz), std::max(dr, 1e-3f)) * 180.0f / M_PI; // 累计判据：从种子点一路过来，总体坡度不能超限 const float global_slope = std::atan2(std::fabs(bin[i].z - seed_z), std::max(std::fabs(bin[i].radius - seed_r), 1e-3f)) * 180.0f / M_PI; ground[i] = (ground[prev] == 1 && local_slope <= static_cast<float>(local_max_slope_) && std::fabs(dz) <= static_cast<float>(local_height_threshold_) && global_slope <= static_cast<float>(general_max_slope_)) ? 1 : 0; if (ground[i]) prev = i; // 只在连续时才推进"上一个地面点" if (i == end) break; } return ground; }

几个参数背后的意图要说清楚。

local_slope用std::max(dr, 1e-3f)兜底，是为了防同一列里两点半径完全相同的情况。多线雷达在近距离（比如 2 米内）确实会出现两个点落在同一方位角桶里且半径接近，dr直接做除数会得到巨大值，atan2虽然不会炸但会返回接近 90 度，判成非地面，白白丢点。

prev只在ground[i]为真时推进，这是整个算法的连续性的来源。如果不管判定结果都推进prev，那么一旦中间出现一个障碍点，后面的比较基准就变成了障碍点，整列后半段会全部被带偏。只在连续的地面段内推进，相当于把"地面"当成一条链，链断了就重新找起点。

global_slope用固定种子而不是滑动窗口，是有意的。滑动窗口版本在长坡上表现更好，但会失去"累计抬升"这个约束，坡顶的墙还是会被吃进来。固定种子 + 累计坡度是保守版本，缺点是超长缓坡（比如连续 200 米、5 度的路）走到后面会被截断。实际园区的坡通常不超过 50 米，这个取舍是划算的；真遇到超长坡，把general_max_slope调大或者把max_range收紧，比换成滑动窗口简单。

local_height_threshold和local_slope的组合是两重保险。纯坡度判据在近距离会失效——Δr只有 0.1 米时，Δz = 0.05米就算出 26 度，会被判成障碍，但 5 厘米的高差其实就是路面起伏。加上绝对高差约束，5 厘米直接放行，不管坡度算出来多少。这一条对减速带和路面接缝特别有效。

4.5 点云重组与发布

判定完就剩最后一步，把带标记的点拆成两片云发出去。

void RayGroundFilterNode::publish(const std_msgs::Header& header) { pcl::PointCloud<pcl::PointXYZ> ground_cloud, obstacle_cloud; for (const auto& bin : bins_) { for (const auto& p : bin) { pcl::PointXYZ pt(p.x, p.y, p.z); if (p.is_ground) ground_cloud.push_back(pt); else obstacle_cloud.push_back(pt); } } sensor_msgs::PointCloud2 ground_msg, obstacle_msg; pcl::toROSMsg(ground_cloud, ground_msg); pcl::toROSMsg(obstacle_cloud, obstacle_msg); // 两个 header 都要填，尤其是 frame_id，漏了下游 TF 直接失败 ground_msg.header = header; obstacle_msg.header = header; pub_ground_.publish(ground_msg); pub_nonground_.publish(obstacle_msg); }

header直接透传输入消息的 header，包括时间戳和frame_id。这里千万不能自己造一个ros::Time::now()，否则下游做 TF 变换时点云时间和机器人位姿时间对不上，rviz 里看起来点云在"飘"。同理frame_id必须和输入的雷达坐标系一致，不能随手写"map"或者"base_link"。

pcl::PointXYZ丢掉 intensity 是常见取舍。如果下游需要强度信息（比如做反光板定位、做材质区分），就得用pcl::PointXYZI，并且在判定结构体里也加上强度字段一路带过来。中途转成PointXYZ再转回去，强度就没了，而且这种丢失是静默的，不会有任何报错，等到定位模块效果变差才回头查代码，前后能差两天。

5. 常见问题排查：五条踩过的坑和一套自检流程

5.1 现象：整片地面被判成障碍，地面点云几乎为空

现象最好认：rviz 里ground_cloud一片空白，obstacle_cloud密密麻麻铺满整个地面，代价地图直接封死车。

原因通常出在坐标系符号上。sensor_height是"雷达离地高度"，雷达坐标系下地面点的 z 应该是-sensor_height，不是+sensor_height。很多人第一次写会把符号搞反，判据变成|z - sensor_height| < δ，那匹配的是车顶上方 1.8 米处的点，地面上一个都不沾。

还有一种情况是雷达装反了（机械雷达倒装或者内参翻转），z 轴朝下，所有点的 z 变成正值。这时候光改sensor_height的符号不够，得在预处理里做一次z = -z，或者直接在 launch 里给个invert_z开关。

解决步骤：先用rostopic echo抓一帧点云，随便挑几个点看 z 值，站在车旁边的地面上的点 z 是多少，那个值的负数就是sensor_height。这个动作 30 秒能做完，比盯代码强。

5.2 现象：远处地面被削平，近处地面残留一圈

现象是近处 5 米内一圈地面判成障碍，20 米以外整片地面又被吃干净，中间正常。

这个组合症状几乎一定是radial_divider_angle和雷达水平角分辨率不匹配。分得太细（如 0.05°），近处点密还能凑出几个点，远处点稀直接成空列，空列跳过后后面的判定链就断了，看着像"远处地面消失"。分得太粗（如 0.5°），近处同一条扫描线上的点被塞进不同列、跨列了，队列里相邻的两点其实在物理上横着隔了一段，坡度算出来是乱数，近处反而判不准。

解决办法是按雷达实际水平角分辨率选参数，取 0.5~1.0 倍。拿不准就录一段 bag，用 0.08°、0.12°、0.18° 三档各跑一遍，看哪一档的地面点在 rviz 里最"连续成片"。这个离线实验半小时能做完，比在线试快十倍。

5.3 现象：减速带和路沿被当地面吃掉

现象是车过减速带、过路沿时不减速，或者在路沿旁边画出直线轨迹。

原因在local_height_threshold和local_max_slope设得太宽松。减速带的高度大约 5~10 厘米，如果local_height_threshold给到 0.3 米，那整个减速带都落在阈值内，一路被判成地面。路沿同理，10~15 厘米的路沿被 0.25 米的阈值直接抹平。

但直接调小也不行。减速带是横向连续的，整车宽度范围内都是同一个高度，调小到 0.05 米以后路面接缝、井盖边缘又会变成障碍。我的经验是反过来想：地面过滤的输出是什么？是obstacle_cloud。减速带本来就不该是障碍，车应该直接过；路沿是障碍，必须留下。所以真正要控制的是路沿那一档，10~15 厘米。取local_height_threshold = 0.12，路沿留下，减速带（5~8 厘米）被吃掉，正好。

还有一个副作用要注意：如果车体一侧有台阶，initial_height_cut太大会让台阶顶面的第一点被当成种子，整条列往下判

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