跳到主要内容

地面分割算法

摘要：本文整理 Ray、Scan 与 RANSAC 三种地面分割方法，说明几何判据、参数作用与适用场景，并分别通过核心代码片段说明三种算法的分组、地面判断与输出过程，理解 PCL 在点云处理链路中的作用。

1. 地面分割的目标​

地面分割将输入点云划分为地面点与非地面点，为障碍物聚类提供输入。非地面点中仍可能包含噪声，不能直接等同于有效障碍物。处理前需要统一坐标系、移除 NaN/Inf，并明确传感器高度和车辆自身的过滤范围。

下面以 Autoware 的三种地面分割方法为例整理。表中的默认值来自参考文档，实际部署应以所用版本的源码与配置为准。

2. Ray Ground Filter（射线法）​

参考：https://autowarefoundation.github.io/autoware_universe/main/perception/autoware_ground_segmentation/docs/ray-ground-filter/

流程：点云沿径向分离，可以根据各个角度的射线，得到点之间的距离与角度信息，从靠近车开始，依次对每条射线上的地面进行分类。

对点 pi=(xi,yi,zi)p_i=(x_i,y_i,z_i)，柱坐标表示为：

ri=xi2+yi2,θi=atan2⁡(yi,xi),hi=zi.\begin{aligned} r_i &= \sqrt{x_i^2+y_i^2},\\ \theta_i &= \operatorname{atan2}(y_i,x_i),\\ h_i &= z_i. \end{aligned}

atan2 可以区分四个象限。按方位角分组后，每组按半径由近到远排序。相邻点的变化量为：

Δri=ri−ri−1,Δzi=zi−zi−1.\Delta r_i=r_i-r_{i-1},\qquad \Delta z_i=z_i-z_{i-1}.

当 Δri>0\Delta r_i>0 时，局部坡角可写为：

αi=arctan⁡(∣Δzi∣Δri).\alpha_i=\arctan\left(\frac{|\Delta z_i|}{\Delta r_i}\right).

用于理解局部高度容差的简化判据为：

∣Δzi∣≤max⁡(hmin⁡,Δritan⁡αmax⁡).|\Delta z_i|\leq\max\left(h_{\min},\Delta r_i\tan\alpha_{\max}\right).

其中 hmin⁡h_{\min} 是最小高度容差。实际分类还要结合全局坡度、前一点的类别与重新分类条件，不能只凭一次坡度比较判断地面。坐标系中的地面高度也不一定为零。

04ed09acb89f816a22d792d876e72e4e

核心参数：

NameTypeDescriptionDefaultRange
min_xfloat手动设置车辆自身过滤范围的边界 [m]-0.01N/A
max_xfloat手动设置车辆自身过滤范围的边界 [m]0.01N/A
min_yfloat手动设置车辆自身过滤范围的边界 [m]-0.01N/A
max_yfloat手动设置车辆自身过滤范围的边界 [m]0.01N/A
use_vehicle_footprintboolean是否使用车辆自身的占地范围过滤点云falseN/A
general_max_slopefloat由 general_max_slope 形成的三角区域称为 global cone，全局锥形区域。如果点落在 global cone 外部，则认为该点不是地面点。8.0N/A
local_max_slopefloat由 local_max_slope 形成的三角区域称为 local cone，局部锥形区域。该参数用于根据相邻点之间的连续性判断点是否属于地面。6.0N/A
initial_max_slopefloat由于传感器近处存在盲区，物体首次被扫描到的位置通常离车较远，该处点云分辨率与后续区域不同，因此单独设置一个初始坡度阈值。3.0N/A
radial_divider_anglefloat用于按角度将点云切分成一条条 ray/射线。1.0N/A
min_height_thresholdfloat当相邻点距离太近时，仅靠 local cone 难以判断连续性，因此使用该参数设置局部高度容差下限。0.15N/A
concentric_divider_distancefloat同心分组的径向距离尺度 [m]；不可直接理解为整帧点云的最小检测半径。0.0N/A
reclass_distance_thresholdfloat用于判断当前点是否距离前一个点太远。如果距离太远，则不再直接沿用前一个点的连续性判断，而是重新分类。0.1N/A
publish_processing_time_detailboolean是否发布详细处理耗时信息。主要用于调试和性能分析。falseN/A

2.1 代码讲解：按方位角分组并排序​

以下是按算法步骤整理的 C++ 核心片段，用于解释处理逻辑，并非 Autoware 源码原样摘录。输入使用 pcl::PointCloud<pcl::PointXYZ>，已去除无效点，坐标单位为米。片段中的配置变量由调用方提供，角度先换算为弧度。

struct RayPoint {
    int index;       // 保留原始点云索引
    double radius;
    double height;
};
const double pi = std::acos(-1.0);
const double angle_step = radial_divider_angle * pi / 180.0;
// angle_step 必须大于 0。
const int ray_count = static_cast<int>(std::ceil(2.0 * pi / angle_step));
std::vector<std::vector<RayPoint>> rays(ray_count);

for (int i = 0; i < static_cast<int>(cloud->size()); ++i) {
    const auto& p = cloud->points[i];
    double theta = std::atan2(p.y, p.x);
    if (theta < 0.0) theta += 2.0 * pi;
    const int bin = std::min(ray_count - 1,
        static_cast<int>(theta / angle_step));
    rays[bin].push_back({i, std::hypot(p.x, p.y), p.z});
}
for (auto& ray : rays) {
    std::sort(ray.begin(), ray.end(), [](const auto& a, const auto& b) {
        return a.radius < b.radius;
    });
}

atan2 得到方位角，负角度转换到正区间后即可计算射线编号。hypot 计算水平距离；sort 保证每条射线由近到远处理，让前一点成为当前点的几何参考。index 始终指向原始 PCL 点云，后续输出不需要复制整份点数据。

2.2 代码讲解：局部连续性与全局约束​

下面展示简化分类器：前一点是地面时沿局部连续性扩展；前一点不是地面时，需要全局约束辅助恢复地面。它省略了完整实现中的初始坡度、车辆轮廓与特殊重新分类分支。

const double local_tan = std::tan(local_max_slope * pi / 180.0);
const double global_tan = std::tan(general_max_slope * pi / 180.0);
pcl::PointIndices ground_indices, non_ground_indices;

for (const auto& ray : rays) {
    double prev_r = 0.0;
    double prev_z = ground_z;  // 当前坐标系中的参考地面高度
    bool prev_ground = true;
    for (const auto& p : ray) {
        const double dr = p.radius - prev_r;
        const double local_limit = std::max(min_height_threshold,
                                            dr * local_tan);
        const double global_limit = std::max(min_height_threshold,
                                             p.radius * global_tan);
        const bool local_ok = std::abs(p.height - prev_z) <= local_limit;
        const bool global_ok = std::abs(p.height - ground_z) <= global_limit;
        const bool is_ground = local_ok && (prev_ground || global_ok);
        (is_ground ? ground_indices : non_ground_indices).indices.push_back(p.index);
        prev_r = p.radius;
        prev_z = p.height;
        prev_ground = is_ground;
    }
}

dr * local_tan 将坡角约束转换为允许的高度差，min_height_threshold 避免点间距很小时容差过小。prev_ground 保存分类状态：遇到障碍物后不能仅因其表面平坦就继续认作地面。这里展示的是状态更新思路，不包含全部恢复条件。

最后可用 pcl::ExtractIndices 配合两组索引输出地面与非地面点云。Ray 的核心是分组、排序和条件判断，PCL 主要提供点云容器与索引提取。

3. Scan Ground Filter（扫描法）​

参考：https://autowarefoundation.github.io/autoware_universe/main/perception/autoware_ground_segmentation/docs/scan-ground-filter/

流程：

  1. 原始点云
  2. 按方向切成很多扫描线
  3. 每条扫描线的点，由近到远排序
  4. 在每条线上划成一个个格子grid
  5. 判断每个点：地面/非地面/超出范围

90f5c3afb024bd028b8e462bbba30a48

核心参数：

NameTypeDescriptionDefaultRange
global_slope_max_angle_degfloat要分类为地面或物体的全局角度[deg]。较大的阈值可能会减少高边坡道路分类的假阳性，但可能会导致非地面分类的假阴性增加，特别是对于小物体。10.0N/A
local_slope_max_angle_degfloat与相邻点比较时，分类为地面或物体的局部角度[deg]。较小的值可以提高倾斜表面物体的分类精度。这应该与split_points_distance_tolerance值一起考虑。13.0N/A
split_points_distance_tolerancefloat区分远近的xy距离阈值[m]0.2N/A
use_virtual_ground_pointboolean是否使用前轮的接地中心作为虚拟接地点trueN/A
split_height_distancefloat与相邻点比较时区分地面和非地面点云的高度阈值[m]。小阈值提高了非地面点的分类，特别是对于垂直分辨率较高的激光雷达。然而，它可能会导致小台阶路面或未对齐的多激光雷达配置出现误报。0.2N/A
non_ground_height_thresholdfloat相对预测地面的非地面高度阈值 [m]，仅用于 elevation_grid_mode0.2N/A
grid_size_mfloat第一个网格大小[m]，仅适用于elevation_grid_mode。较大的值可以提高地表的预测稳定性。适用于粗糙表面或多激光雷达配置0.1N/A
gnd_grid_buffer_sizeinteger用于估算局部地面坡度的网格数量，仅适用于elevation_grid_mode4N/A
detection_range_z_maxfloat探测范围的最大高度[m]，仅适用于elevation_grid_mode2.5N/A
elevation_grid_modeboolean高程网格扫描模式选项trueN/A
low_priority_region_xfloat后侧的非零x阈值，小物体检测的优先级较低[m]-20.0N/A
center_pcl_shiftfloat附加 LiDAR 相对于车辆质心在 x 轴方向上的偏移量 [m]；建议仅在 elevation_grid_mode 下用于附加 LiDAR。0.0N/A
radial_divider_angle_degfloat用于将整帧点云按角度切分成多个扇区/射线组的角度间隔 [deg]。1.0N/A
use_recheck_ground_clusterboolean是否启用地面聚类复检。trueN/A
recheck_start_distancefloat开始对地面聚类进行复检的距离 [m]。20.0≥0.0
use_lowest_pointboolean复检地面聚类时，是否选择最低点作为参考点；若为 false，则选择中间点作为参考点。trueN/A
publish_processing_time_detailboolean是否发布详细的处理耗时信息。falseN/A

3.1 配置与调参​

以下参数属于 Scan，不应与 Ray 的参数名混用。配置路径是原笔记对应版本的示例，升级后应检查实际节点加载的 YAML。

调参文件：src/universe/autoware.universe/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml

参数默认值调参作用
global_slope_max_angle_deg10.0全局最大坡角；调大可保留较陡坡路，但也可能误删障碍物；调小则容易把坡路误判为非地面
local_slope_max_angle_deg13.0局部坡角，决定相邻点连续性判断
split_points_distance_tolerance0.20当前点和前一个点在 xy 平面上的距离超过多少，就认为它们“隔得比较远”。
non_ground_height_threshold0.20相对预测地面的高度阈值，调大→过滤更多低矮噪声，调小→检出更小的障碍
split_height_distance0.20相邻点比较时，区分地面和非地面的高度差阈值
use_virtual_ground_pointtrue用前轮接地点补盲区，一般开
grid_size_m0.1elevation_grid 模式的网格大小
grid_mode_switch_radius20.0近处按距离/远处按角度切换的半径
gnd_grid_buffer_size4估计局部坡度用的网格数
detection_range_z_max2.5检测最大高度
elevation_grid_modetrue是否启用高程网格模式；效果需结合路面与点云密度验证

3.2 代码讲解：沿射线建立径向网格​

Scan 同样先按方位角分组、按半径排序，但高程网格模式还会用一段地面历史预测当前网格。下面用固定径向网格演示核心过程；实际 Autoware 还包含远近网格切换、虚拟接地点与复检逻辑。

struct Grid {
    std::vector<int> indices;
    double radius_sum = 0.0;
};
std::map<int, Grid> grids;  // 单条射线，按编号从近到远遍历
for (const auto& p : ray) {
    const int key = static_cast<int>(std::floor(p.radius / grid_size_m));
    grids[key].indices.push_back(p.index);
    grids[key].radius_sum += p.radius;
}

这里的 ray 沿用上一节的 RayPoint 表示。网格内保存原始索引，map 按网格编号有序遍历。Scan 的径向网格不等同于普通 PCL 的三维 VoxelGrid：前者服务于沿扫描方向的地面预测，后者用于空间下采样。

3.3 代码讲解：预测地面并更新历史​

设历史地面网格的代表位置为 (rj,zj)(r_j,z_j)，用局部直线预测高度：

z^(r)=z0+k(r−r0).\hat{z}(r)=z_0+k(r-r_0).

以下片段用最近两个有效网格估计斜率，再根据当前网格中通过分类的点更新历史。history 初始包含参考接地点，gnd_grid_buffer_size 至少为 2；这是教学简化版，不是官方完整高程预测器。

struct GroundSample { double radius, height; };
std::deque<GroundSample> history{{virtual_ground_r, ground_z}};
const double slope_limit = std::tan(local_slope_max_angle_deg * pi / 180.0);

for (const auto& entry : grids) {
    const auto& grid = entry.second;
    const double r = grid.radius_sum / grid.indices.size();
    const auto last = history.back();
    double slope = 0.0;
    if (history.size() >= 2) {
        const auto before = history[history.size() - 2];
        const double dr = last.radius - before.radius;
        if (dr > 1e-6) slope = (last.height - before.height) / dr;
    }
    slope = std::clamp(slope, -slope_limit, slope_limit);
    double accepted_height = 0.0;
    int accepted_count = 0;
    for (int index : grid.indices) {
        const auto& p = cloud->points[index];
        const double point_r = std::hypot(p.x, p.y);
        const double predicted_z = last.height + slope * (point_r - last.radius);
        const bool is_ground = std::abs(p.z - predicted_z)
                               <= non_ground_height_threshold;
        (is_ground ? ground_indices : non_ground_indices).indices.push_back(index);
        if (is_ground) {
            accepted_height += p.z;
            ++accepted_count;
        }
    }
    if (accepted_count > 0) {
        history.push_back({r, accepted_height / accepted_count});
        while (history.size() > static_cast<std::size_t>(gnd_grid_buffer_size))
            history.pop_front();
    }
}

predicted_z 是当前点处的预测地面高度，高度残差决定其是否通过简化分类。只有通过分类的点才更新地面历史，否则墙面或车辆表面可能拉高后续预测。clamp 限制预测坡度，防止局部异常产生过大的外推。

这段代码没有实现全局坡角、远距离断点与高度检测范围检查。没有有效地面点的网格不会更新历史，长距离缺测后仍需重新初始化或复检。片段使用 C++17 的 std::clamp；相关容器需要 <map>、<deque>，几何计算与排序需要 <cmath>、<algorithm>。

4. RANSAC Ground Filter（平面拟合法）​

参考：

https://autowarefoundation.github.io/autoware_universe/main/perception/autoware_ground_segmentation/docs/ransac-ground-filter/

原理：在地面候选区域内反复随机采样并拟合平面，以内点支持评价模型，再检查平面方向与位置，避免将墙面或车顶误当作地面。

流程：

  1. 随机选择三个不共线的点，建立候选平面
  2. 检查有多少个点支持这个平面———点到平面的距离
  3. 在 max_iterations 限制内重复采样，选择内点支持较好的平面
  4. 验证平面方向与位置，再通过 height_threshold 分类地面点与非地面点

优点：地面近似平面且内点比例足够时，可以容忍一定比例的离群点；有限次随机采样不保证一定找到真实地面。

局限：单个平面难以描述起伏路面或连续变化的坡度。恒定坡度的路面仍可拟合，但需要满足坡角约束；输入点云应先限制到合适的候选区域。

核心参数：

NameTypeDescriptionDefaultRange
base_framestringbase_link framebase_linkN/A
unit_axisstringThe axis which we need to search ground planezN/A
max_iterationsintegerThe maximum number of iterations1000N/A
min_trialintegermin_trial5000N/A
min_pointsintegermin_points1000N/A
outlier_thresholdfloat点到当前拟合平面的距离小于该阈值时，认为该点支持该平面模型；距离超过该阈值时，认为是离群点。0.01N/A
plane_slope_thresholdfloat用于防止 RANSAC 拟合出错误的地面平面。如果拟合出的平面坡度过大，则认为该平面不像地面。10.0N/A
voxel_size_xfloatvoxel size x [m]0.04N/A
voxel_size_yfloatvoxel size y [m]0.04N/A
voxel_size_zfloatvoxel size z [m]0.04N/A
height_thresholdfloat点到最终地面平面的高度/距离超过该阈值时，认为是非地面点；小于该阈值时，认为是地面点。0.01N/A
debugbooleantrue：输出调试信息；false：不输出调试信息。falseN/A
publish_processing_time_detailboolean是否发布详细处理耗时信息。主要用于调试和性能分析。falseN/A

4.1 代码讲解：下采样后拟合平面​

下面按“候选点下采样 → RANSAC 拟合 → 平面验证 → 原始点分类”拆解核心片段。candidates 是已移除无效点、限定到地面候选区域的 PCL 点云，cloud 是待分类的原始分辨率点云；两者使用同一坐标系。配置阈值由调用方提供。

pcl::PointCloud<pcl::PointXYZ>::Ptr sampled(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(candidates);
voxel.setLeafSize(voxel_size_x, voxel_size_y, voxel_size_z);
voxel.filter(*sampled);

pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(max_iterations);
seg.setDistanceThreshold(outlier_threshold);
seg.setInputCloud(sampled);
pcl::PointIndices inliers;
pcl::ModelCoefficients coefficients;
seg.segment(inliers, coefficients);

VoxelGrid 减少拟合点数，三个叶尺寸以米为单位且必须为正。SACMODEL_PLANE 选择平面模型，SAC_RANSAC 选择鲁棒估计方法。outlier_threshold 决定哪些采样点支持候选平面，max_iterations 限制随机试验次数。setOptimizeCoefficients(true) 用最终内点进一步优化系数。

4.2 代码讲解：检查法向量与平面有效性​

设平面模型为：

ax+by+cz+d=0.ax+by+cz+d=0.

点 pip_i 到平面的垂直距离为：

Di=∣axi+byi+czi+d∣a2+b2+c2.D_i=\frac{|ax_i+by_i+cz_i+d|}{\sqrt{a^2+b^2+c^2}}.

拟合阶段用 Di≤τfitD_i\leq\tau_{\mathrm{fit}} 判断内点。拟合阈值与最终地面分类阈值可以不同；拟合完成后可以用独立的分类阈值重新检查原始点云。

当坐标系的 z 轴向上时，平面相对水平面的倾角为：

β=arccos⁡(∣c∣a2+b2+c2).\beta=\arccos\left(\frac{|c|}{\sqrt{a^2+b^2+c^2}}\right).

绝对值消除了法向量正负的影响。倾角检查可以排除竖直墙面，却不能单独排除水平车顶，仍需结合候选区域与地面高度。

// 片段位于返回 bool 的处理函数中；失败交给调用方处理。
if (inliers.indices.empty() || coefficients.values.size() < 4)
    return false;
const auto& v = coefficients.values;
const double a = v[0], b = v[1], c = v[2], d = v[3];
const double norm = std::sqrt(a*a + b*b + c*c);
if (norm <= 1e-6) return false;
const double pi = std::acos(-1.0);
const double max_slope = plane_slope_threshold * pi / 180.0;
if (std::abs(c) / norm < std::cos(max_slope))
    return false;

coefficients.values 保存 a,b,c,da,b,c,d。abs(c)/norm 衡量法向量与竖直轴的对齐程度，用来拒绝倾角过大的平面。它不能区分水平车顶与真实地面，候选区域、预期地面高度和最少内点数仍需额外验证。拒绝最大平面后，这段代码不会自动寻找第二个平面。

4.3 代码讲解：用拟合系数分类原始点​

pcl::PointIndices ground_indices, non_ground_indices;
for (int i = 0; i < static_cast<int>(cloud->size()); ++i) {
    const auto& p = cloud->points[i];
    const double distance = std::abs(a*p.x + b*p.y + c*p.z + d) / norm;
    const bool is_ground = distance <= height_threshold;
    (is_ground ? ground_indices : non_ground_indices).indices.push_back(i);
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::PointIndices::Ptr ground_ptr(new pcl::PointIndices(ground_indices));
extract.setInputCloud(cloud);
extract.setIndices(ground_ptr);
extract.setNegative(false);
extract.filter(*ground_cloud);
extract.setNegative(true);
extract.filter(*non_ground_cloud);

拟合用的是 sampled，其内点索引不能直接用于 cloud。这里将平面系数应用到原始点，重新计算距离并建立原始索引，保留输出分辨率。height_threshold 是最终分类容差，与拟合内点阈值分开；本示例采用垂直于平面的距离，不能直接解释为世界坐标系中的 z 高度差。

setNegative(false) 提取地面索引，true 提取其补集。PCL 对应头文件为 <pcl/filters/voxel_grid.h>、<pcl/segmentation/sac_segmentation.h> 和 <pcl/filters/extract_indices.h>。以上片段共同说明处理链路，没有包含 ROS 消息转换与失败时的回退策略。

5. 方法选择与调参检查​

方法主要依据重点检查
Ray同一方向上的高度连续性坡角、稀疏点与近处盲区
Scan扫描方向与网格地面预测网格尺寸、坡度变化、小障碍物误删
RANSAC候选平面的内点支持候选区域、方向与距离阈值

调参时同时检查地面残留与低矮障碍物误删，并记录点数和耗时。角度参数通常以度给出，而 C++ 三角函数使用弧度；距离参数应与点云的米制单位一致。

参考：PCL 平面分割教程。Autoware 各方法的参考链接见对应章节。

本篇文章受 CC BY-NC-SA 4.0 协议保护，转载请注明出处。