Skip to content
Anima Mundi Anima Mundi

使用 3D 激光雷达进行地面分割

概述

Section titled “概述”

本教程介绍一个基于 3D 激光雷达地面分割的 Nav2 地形感知代价地图层。地面一致性代价地图层（ground consistency costmap layer） 能将地形划分为可通行的地面与障碍物，从而在非平坦地形中实现更智能、更安全的户外导航，构建出更可靠的代价地图。

Ground Consistency Layer Demo

地面一致性层运行效果：绿色点为地面，洋红色点为叠加在代价地图上的障碍物

环境要求

Section titled “环境要求”

本教程需要 ROS 2 Jazzy 和 Nav2。如尚未安装 Nav2，请先查看快速入门页面。

安装步骤

Section titled “安装步骤”

本教程使用 vcstool 来管理仓库依赖。使用 apt 安装它：

Terminal window
sudo apt-get install python3-vcstool

搭建教程软件包和依赖

Section titled “搭建教程软件包和依赖”

创建并准备工作空间：

Terminal window
# 创建工作空间
mkdir -p ~/nav2_ws/src
cd ~/nav2_ws/src


# 克隆 navigation2_tutorials 仓库
git clone -b jazzy https://github.com/ros-navigation/navigation2_tutorials.git

接下来安装所有依赖（包括地面分割、KISS-ICP 里程计和地面一致性代价地图插件）：

Terminal window
cd ~/nav2_ws
# 从 .repos 文件导入所有演示依赖
vcs import src < src/navigation2_tutorials/nav2_lidar_ground_segmentation_demo/dependencies.repos


# 安装 ROS 软件包依赖
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src --rosdistro jazzy -y

构建演示

Section titled “构建演示”

构建教程软件包及其所有依赖：

Terminal window
cd ~/nav2_ws
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --packages-up-to nav2_lidar_ground_segmentation_demo --cmake-args -DCMAKE_BUILD_TYPE=RELEASE
source install/setup.bash

地面分割概述

Section titled “地面分割概述”

地面分割是指将点云数据中的地面点与障碍物点分离，在非平坦地形上运行时尤为重要。 该任务是移动机器人感知的关键环节——安全性和可靠运行取决于对障碍物和可通行区域的精确检测。单次激光雷达扫描 可能包含超过一百万个点，覆盖地面和障碍物表面，但并非所有点对下游感知任务都同等重要。准确的地面分割 对于自主系统的可靠感知、可通行性分析、导航和地图构建至关重要。有效区分地面点和障碍物点后， 就能构建更准确的代价地图，使机器人在复杂环境中安全高效地导航。

地面分割的好处：

  • 增强的可通行性分析：准确识别可通行地形，为安全导航规划提供依据
  • 更好的障碍物检测：将处理资源集中在真正的障碍物上，而非地形起伏
  • 稳健的坡度处理：区分可通行的坡度与真正阻挡通行的障碍物
  • 提升计算效率：在后续处理前过滤无关点，降低内存和 CPU 开销
  • 地下/隧道导航：在传统代价地图失效的复杂环境中实现导航

Raw Input Point Cloud

原始输入点云

Segmented Ground and Obstacle Points

基于 GSeg3D 分割的地面（绿色）和障碍物（洋红色）点

传统做法是将地面点从代价地图中过滤掉，但本层另辟蹊径：在决策过程中利用地面点实现证据累积和基于高度的分类。这种方法在复杂的室外环境中能构建更准确、更稳定的代价地图。

地面一致性层

Section titled “地面一致性层”

地面一致性（Ground Consistency）是一个代价地图层，利用地面分割在复杂的室外环境中为导航提供更可靠的占用估计。该层可与任何地面分割算法配合使用，但推荐使用 GSeg3D——该插件最初就是为与 GSeg3D 集成而开发的。

关键特性：

  • 基于证据的概率方法：在多次传感器观测中累积地面点和障碍物点的证据，而非对单次测量做二元判断
  • 证据竞争：地面点与障碍物点相互竞争以确定每个代价地图单元的真实占用状态，减少假阳性和假阴性
  • 基于高度的分类：通过评估障碍物相对于局部地面的高度，区分真正的障碍物与地形变化（如坡度、小颠簸）
  • 时间稳定性：证据随时间累积和衰减，在空闲与占用状态之间产生平滑过渡，同时保持对环境变化的响应能力
  • 抗噪声能力：在将单元标记为占用之前要求持续的证据，从而过滤孤立的传感器噪声

配置与调优

Section titled “配置与调优”

下面配置 Nav2 在局部代价地图中使用地面一致性层。建议在局部代价地图中使用该层， 因为它依赖实时传感器数据，专为短期占用估计而设计。同时建议将该层与膨胀层配合使用， 在检测到的障碍物周围创建缓冲区，以提升安全性。

以下是局部代价地图的示例配置：

local_costmap:
  local_costmap:
    ros__parameters:
      # ... other costmap settings ...
      plugins: ["ground_consistency", "inflation_layer"]


      ground_consistency:
        plugin: "nav2_ground_consistency_costmap_plugin::GroundConsistencyLayer"
        ground_points_topic: /ground_segmentation/ground_points
        nonground_points_topic: /ground_segmentation/obstacle_points
        tf_timeout: 0.1
        min_clearance: 0.1
        robot_height: 0.92
        maximum_height_filter: 2.0
        ground_inc: 1.0
        nonground_inc: 1.5
        nonground_decay: 0.93
        ground_decay: 0.80
        max_score: 5000.0
        nonground_occ_thresh: 6.0
        nonground_prob_thresh: 0.75
        enable_kpi_logging: false
        discretize_costs: true
        max_data_range: 50.0
        ground_neighbor_search_cells: 0


      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.7

该层的行为取决于具体使用场景（地形类型、机器人尺寸、传感器特性）。以下是关键参数及调优建议：

机器人高度（robot_height）

  • 设置为机器人的实际高度
  • 该层根据障碍物相对于地面的高度，将其分类为阻挡或非阻挡。
  • 示例：Husky 机器人 = 0.92 m

证据累积（ground_inc、nonground_inc）

  • 增量越高 = 对观测的响应越快
  • 应保持 nonground_inc > ground_inc，使非地面证据比地面证据累积得更快，从而产生偏向安全的倾向。
  • 典型比例：地面 1.0，非地面 1.5

证据衰减（ground_decay、nonground_decay）

  • 地面衰减（较低值）：较快地衰减地面证据，使代价地图能迅速适应地形变化（例如驶入坡道）。建议使用 0.80–0.85 以获得灵敏的地形适应能力。
  • 非地面衰减（较高值）：让障碍物持续更久，避免因传感器噪声导致代价地图闪烁。建议使用 0.90–0.95 以获得稳定的障碍物标记。
  • 二者的差异产生时间滞后：障碍物证据比地面证据更晚被清除。

高度过滤（min_clearance、maximum_height_filter）

  • min_clearance：机器人能检测到的最小颠簸高度。设置过低 = 对噪声敏感；过高 = 遗漏小障碍物。
  • maximum_height_filter：头顶遮挡物/天花板的高度阈值，高于此值的物体将被忽略。
  • 有时地面分割会将地面点误判为障碍物（例如传感器噪声或地形极为不平整时）。合理设置 min_clearance 可防止这类误分类阻挡导航。

邻域搜索（ground_neighbor_search_cells）

  • 0 = 仅使用当前单元中的地面点进行高度估计
  • 1–3 = 从相邻单元平均地面高度（更稳定，但响应更慢）
  • 适用于非常不平整的地形：此时单元中可能缺少地面点，或低分辨率激光雷达导致地面点稀疏。

决策阈值（nonground_occ_thresh、nonground_prob_thresh） 这两个阈值在两级过滤中协同工作：

  • nonground_occ_thresh：所需的最低分数（累积证据）。使用默认值 nonground_inc: 1.5 时，设为 6.0 意味着将单元标记为 LETHAL 之前需要约 4 次障碍物点观测。这是抵御传感器杂散噪声的主要防线。

  • nonground_prob_thresh：所需的概率置信度（分数通过后生效）。即使累积证据充足，观测也必须保持高一致性（默认 75%）才会被标记为 LETHAL。

    为什么需要两者？ 单靠 nonground_prob_thresh 会因单个高置信度假阳性点而触发。两者结合后则同时要求：持续的证据（多次观测）且高一致性（高置信度），从而有效防止孤立的假阳性阻挡导航。

  • 激进导航（狭窄空间）：将 nonground_occ_thresh 降至 4–5，将 nonground_prob_thresh 提高到 0.85（对每个点要求更强的证据）

  • 保守导航（安全优先）：将 nonground_occ_thresh 提高到 8–10，将 nonground_prob_thresh 降低到 0.5（放宽证据累积要求）

外部参数

Section titled “外部参数”

地面分割参数

  • 可能还需针对你的机器人调整地面分割算法的参数（如坡度阈值、传感器到地面的距离等），以确保地面点与障碍物点的正确分类，因为这直接影响该层的性能。请参考 GSeg3D 参数 文档。

有关所有可用参数的完整参考，请查阅插件文档。

使用场景

Section titled “使用场景”
使用场景挑战解决方案
不平整地形与坡度标准代价地图将坡度和高程变化标记为障碍物，阻止机器人在可通行地形上导航地面一致性利用相对于地形的高度，区分可通行的坡度与真正阻挡的障碍物。较低的衰减值能快速响应地形变化。
结构物下方导航头顶结构物（桥梁、树冠）被错误标记为阻挡设置 maximum_height_filter 过滤头顶障碍物。同时确保 robot_height 配置准确——当头顶障碍物高度变化时，该参数用于确定哪些障碍物真正阻挡机器人
杂乱环境树枝、树叶和小碎片产生假阳性障碍物使用 min_clearance 过滤机器人高度以下的碎片；只有达到阻挡高度的障碍物才会被标记
动态环境传感器噪声导致代价地图闪烁，频繁切换占用/空闲状态证据的累积与衰减随时间平滑代价地图，提供时间稳定性

下面来看几个地面一致性层在实际运行中的案例：

  1. 在德国不来梅的机器人创新中心（DFKI），使用 Arter 挖掘机 演示地面一致性层：

Ground Consistency Layer Use Cases

  1. 在 BotanicGarden 数据集中的演示：

Ground Consistency Layer Use Cases

  1. 在 CitrusFarm 数据集中的演示：

Ground Consistency Layer Use Cases

实际示例：运行演示

Section titled “实际示例：运行演示”

安装和构建完成后，接下来运行实际示例，演示地面一致性层的效果。

启动仿真

Section titled “启动仿真”

本教程使用 Gazebo 中的 Baylands 户外世界和 Husky 机器人。

启动完整仿真：

Terminal window
ros2 launch nav2_lidar_ground_segmentation_demo lidar_ground_segmentation_demo.launch.py 2>&1 | grep -v "SampleConsensus"

其中 grep -v "SampleConsensus" 用于过滤地面分割算法中不影响演示的预期警告。如需查看完整输出，可省略此参数。

你应该会看到 Gazebo 启动，Husky 出现在 Baylands 世界中：

Gazebo Baylands world

观察地面一致性层的运行效果

Section titled “观察地面一致性层的运行效果”

一个 RViz2 窗口也会打开，显示代价地图层。

可视化地面分割点（可选）

将以下 PointCloud2 话题添加到 RViz2 显示中，以可视化地面与障碍物的分类结果：

  • /ground_segmentation/ground_points
  • /ground_segmentation/obstacle_points

重要：将 RViz2 目标帧率设置为 约 8 Hz，以匹配 Gazebo 中激光雷达传感器的发布速率。设置过高会导致点云间歇性显示。

你现在应该会看到分割算法输出的地面点和障碍物点叠加在应用了地面一致性层的代价地图上：

RViz2 window showing costmap layers

测试导航

使用 RViz2 中的「Nav2 Goal」工具为机器人设置导航目标。尝试在地图的不同区域设置目标——例如坡道上、树冠下方以及不平整地形。观察地面一致性层如何通过正确分类障碍物和可通行地面，使机器人顺利穿越这些复杂地形：

Robot navigating through terrain with ground consistency layer

结论

Section titled “结论”

地面一致性代价地图层通过以下方式实现地形感知导航：

  1. 理解地形几何——区分可通行的坡度与阻挡的障碍物
  2. 高度感知过滤——只标记实际阻挡机器人的障碍物
  3. 时间平滑——利用证据累积构建稳定且响应灵敏的代价地图
  4. 灵活集成——可与任何地面分割算法配合使用

将该层集成到任何基于 Nav2 的机器人中，只需以下步骤：

  1. 安装插件
  2. 提供地面/障碍物点云（来自所选的传感器/算法）
  3. 根据机器人尺寸和地形配置参数
  4. 将该层添加到代价地图配置中

该层高度可调，建议先从默认值开始，再根据具体地形和机器人特性逐步调整。

扩展主题

Section titled “扩展主题”

故障排除

Section titled “故障排除”

层未在代价地图中使用

  • 确认插件已安装：ros2 pkg list | grep ground_consistency
  • 检查配置中的层名称是否匹配（ground_consistency）
  • 确保插件名称完全限定：nav2_ground_consistency_costmap_plugin::GroundConsistencyLayer

没有单元被标记为障碍物

  • 确认地面点和障碍物点话题正在发布
  • 检查点云数据是否到达：ros2 topic hz /ground_segmentation/obstacle_points
  • 如果没有数据，可能是分割算法未运行或发布到了错误的话题
  • 检查 nonground_occ_thresh 是否设置过高

代价地图过于保守（标记了过多障碍物）

  • 降低 nonground_inc（减缓证据累积）
  • 提高 ground_decay（加快地面证据衰减）
  • 提高 nonground_occ_thresh（标记占用需要更多证据）
  • 检查 robot_height 是否设置正确

代价地图过于激进（遗漏障碍物）

  • 提高 nonground_inc（加快证据累积）
  • 降低 nonground_decay（让障碍物持续更久）
  • 降低 nonground_occ_thresh（标记占用所需证据更少）
  • 检查来自分割算法的点云数据质量

头顶结构物阻挡导航

  • 将 maximum_height_filter 提高到结构物的高度
  • 确认头顶的点确实存在于点云中
  • 检查 robot_height 是否设置正确

性能问题

  • 如果只需要附近点，降低 max_data_range
  • 在生产环境中禁用 enable_kpi_logging
  • 确认 ground_neighbor_search_cells 设置合理（0 最快）

资金支持

Section titled “资金支持”

这项工作部分由 VaMEx3-APO（德国联邦研究、技术和航天部（BMFTR）/德国航空航天中心（DLR），资助号 50RK2252C）和 ROBDEKON2（德国联邦研究、技术和航天部（BMFTR），资助号 13N16537）资助。所展示代码库的开发得到了这两个项目在德国人工智能研究中心（DFKI）机器人创新中心的支持。