基于射线法的激光雷达点云地面过滤ROS节点实践

简介面向机器人感知与自动驾驶点云处理方向的开发者这份基于 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_PLANERANSAC 迭代几百次找最大平面。它在单平面场景确实稳但代价有两个一是每次迭代都要随机采样耗时抖动大帧率高的时候很要命二是它必须二选一遇到S 弯坡道上下两层地面斜坡加平台这种多平面场景它只会把最大的那个平面判成地面剩下的全推给障碍。RANSAC 还有个隐性成本setDistanceThreshold是个绝对值半径 5 米和 40 米用同一个阈值远处的地面点必然被漏掉。射线法走的是另一条路不做全局平面假设只假设地面在局部是连续的且沿射线方向的坡度有上界。它把一帧点云按方位角切成很多细长的扇形列每一列内的点在地理上就是沿着同一条射线往前排的于是可以在列内做一维的相邻点比较。远处点稀、近处点密的问题被自然消化掉了因为比较的是同一列内的相邻点而不是全局距离阈值。代价是需要一个靠谱的安装高度而且对分列参数、坡度阈值相当敏感这几个参数调不好效果比 RANSAC 还难看。还有一类是 2.5D 栅格法把点云投影成高程栅格每格存最低点或者高程统计量再做形态学开闭运算。它对起伏地形适应性最好代价是内存和更新开销栅格分辨率一细就吃不消。低速园区车用射线法性价比最高路况是铺装路面加缓坡没有太多垂直结构射线法的假设基本成立。2.2 一帧点云在节点里走过的五段链路射线法节点的结构其实很朴素一帧点云进来走完五段再出去。把这个链路记牢后面调参和排错时就知道该在哪一段下钩子。阶段输入处理输出接入sensor_msgs/PointCloud2订阅、检查订阅者、转 PCLpcl::PointCloudPointXYZ裁剪全量点云半径过滤 高度粗切候选点云分列候选点云方位角离散化 列内按半径排序扇形列数组判定每列点序列双向扫描 坡度/高差判据每点的地面标记发布带标记的点集拆成两片云、回填 headerground_cloud/obstacle_cloud第一段的接入层要处理一个容易被忽略的点如果下游没人订阅这帧点云根本不该算。地面过滤是纯 CPU 活儿一帧几十万点分列加排序能吃掉十几毫秒多个节点一起跑就把核占满了。在回调开头判一下getNumSubscribers()没人要就直接返回这是最便宜的一行优化。第二段裁剪是纯粹为了省时间。半径外和高度外的点不参与分列能砍掉三到五成的点。这里的裁剪不是精确过滤而是粗筛所以阈值可以放宽宁可多留也别误杀精确判断交给第四段。第三段分列是射线法的骨架。它决定了谁和谁比分错了列后面再怎么调坡度阈值都是白搭。这一段也是内存分配的高发区如果每帧都重新构造一个vectorvectorPoint跑十几分钟就能看到内存曲线锯齿状爬升。第四段判定是算法核心也是唯一需要仔细推敲的地方。第五段发布看似简单但 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.rvizpoint_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 nameinput_topic default/lidar_points/ node pkgray_ground_filter typeray_ground_filter_node nameray_ground_filter outputscreen !-- 安装高度务必和实际测量值一致 -- param namesensor_height value1.80/ param namemin_range value1.5/ param namemax_range value45.0/ param nameradial_divider_angle value0.12/ param namegeneral_max_slope value8.0/ param namelocal_max_slope value12.0/ param namelocal_height_threshold value0.25/ param nameinitial_height_cut value0.15/ remap frominput to$(arg input_topic)/ /node /launchremap那一行是关键。很多人把订阅话题写死在nh.subscribe(/lidar_points, ...)里换个雷达就得改代码。写成相对话题input再用 remap 映射同一份 launch 能在不同车上复用。3.4 参数表六个阈值决定成败参数单位建议值作用调大的后果调小的后果sensor_heightm实测值雷达相对地面的安装高度地面基准面上移真实地面全成障碍基准面下移低矮障碍被吃成地面min_rangem1.5~2.0处理半径下限近处地面残留一圈车体自身反射点参与判定误判多max_rangem40~50处理半径上限远处点稀坡度失真有效感知距离缩短radial_divider_angle°0.1~0.2每列角分辨率列内点横跨角变大坡度失真每列点太少无法比较general_max_slope°6~10累计坡度上限坡顶墙面被吃成地面缓坡被截断坡面成障碍local_max_slope°10~15相邻点坡度上限路沿、台阶被抹平正常路面起伏被判成障碍local_height_thresholdm0.2~0.3相邻点高差上限减速带被当地面路面接缝被判成障碍initial_height_cutm0.1~0.2种子点高度容差第一点误判成地面的概率升高找不到地面种子整列全判非地面看这张表能发现一个规律所有调大的后果都是吃障碍所有调小的后果都是吃地面。射线法的调参本质是在这两个方向的误判之间找平衡点没有一组参数能同时最优只能按场景取舍。园区道路我一般偏保守——宁可多留几个地面点让代价地图多做一次膨胀也不要把路沿漏成障碍因为漏判地面最多让代价地图糊一点漏判障碍是真的会撞。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_castsize_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_.advertisesensor_msgs::PointCloud2(ground_cloud, 1); pub_nonground_ nh_.advertisesensor_msgs::PointCloud2(obstacle_cloud, 1); } // ... private: ros::NodeHandle nh_, pnh_; ros::Subscriber sub_; ros::Publisher pub_ground_, pub_nonground_; std::vectorstd::vectorPointRT 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()是因为vectorvectorT每次重新构造会触发 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::PointCloudpcl::PointXYZ::Ptr raw(new pcl::PointCloudpcl::PointXYZ); pcl::fromROSMsg(*msg, *raw); if (raw-empty()) return; // 每帧复用避免反复分配 for (auto bin : bins_) bin.clear(); const float min_r2 static_castfloat(min_range_ * min_range_); const float max_r2 static_castfloat(max_range_ * max_range_); // 高度粗切地面以下留 1m 容差车体以上 1m 之外不可能是地面 const float z_low static_castfloat(-sensor_height_ - 1.0); const float z_high static_castfloat(-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_castsize_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::vectorstd::vectorPointRT 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::vectorchar fwd scanOneDirection(bin, /*reverse*/false); const std::vectorchar 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::vectorchar RayGroundFilter::scanOneDirection( const std::vectorPointRT bin, bool reverse) { const size_t n bin.size(); std::vectorchar 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_castfloat(sensor_height_)); if (seed_dz static_castfloat(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_castfloat(local_max_slope_) std::fabs(dz) static_castfloat(local_height_threshold_) global_slope static_castfloat(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::PointCloudpcl::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太大会让台阶顶面的第一点被当成种子整条列往下判本文还有配套的精品资源点击获取