机器人 / 系统课时

将 PointCloud2 映射到障碍层

将 PointCloud2 映射到障碍层

深度相机给出的是带坐标和时间戳的三维点云,不能直接当作二维导航障碍。本课用 Nav2 的 ObstacleLayer 接收 PointCloud2,经 TF 变换、按高度过滤,再将有效点投影到局部代价图,最终在 RViz 2 中检查障碍是否随观测更新。

学习目标

  • 解释点云坐标变换、高度筛选与二维网格标记的先后关系。
  • 配置 Nav2 局部障碍层订阅深度点云并设置高度、距离和清除参数。
  • 用话题、TF 和 RViz 2 排查点云存在但障碍图不更新的问题。

确定点云的坐标与高度基准

假设深度相机在距地面 0.35 m 处发布 /camera/depth/points,消息类型为 sensor_msgs/msg/PointCloud2,header.frame_id 为 camera_depth_optical_frame。相机光学坐标系通常以 Z 轴指向镜头前方,因此不能拿点云原始 z 值当离地高度。

局部代价图设置 global_frame: odom,Nav2 通过带时间戳的 TF 链 odom、base_link、camera_depth_optical_frame 将点变换到 odom,再检查变换后的 z 值并把 x、y 映射到二维网格。以下高度阈值假设平地且 odom 的 z≈地面高度;坡道或底盘起伏明显时,应先做相对地面高度估计,不能照搬固定窗口。

以 0.10~1.20 m 为障碍高度窗口:地板点 z=0.02 m 被忽略;椅腿上的点 z=0.45 m 被标记;离地 1.50 m 的横梁被忽略。低于 0.10 m 的真实小障碍也会漏检,阈值必须结合机器人最低离地间隙实测。

深度点云通过 TF 变换、高度筛选后投影到二维局部障碍层

配置局部 ObstacleLayer

下面是 nav2_params.yaml 中 local_costmap 的参数段(片段示例):应合并到已有的 Nav2 参数文件,其他导航节点的配置仍须保留。前提是相机持续发布上述点云、相机外参 TF 正确,且 odom 到 base_link 的变换已由里程计提供。

yaml
local_costmap:
  local_costmap:
    ros__parameters:
      global_frame: odom
      robot_base_frame: base_link
      update_frequency: 5.0
      publish_frequency: 2.0
      rolling_window: true
      width: 4.0
      height: 4.0
      resolution: 0.05
      robot_radius: 0.25
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        max_obstacle_height: 1.20
        observation_sources: depth_cloud
        depth_cloud:
          topic: /camera/depth/points
          data_type: "PointCloud2"
          min_obstacle_height: 0.10
          max_obstacle_height: 1.20
          obstacle_min_range: 0.20
          obstacle_max_range: 2.50
          raytrace_min_range: 0.0
          raytrace_max_range: 3.0
          marking: true
          clearing: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.35
        cost_scaling_factor: 3.0

4.0 m × 4.0 m、0.05 m 分辨率对应 80×80 个单元,窗口随机器人移动。obstacle_max_range 限制标记距离,raytrace_max_range 限制沿有效观测射线清除旧障碍的距离;二者不是同一个开关。marking 负责写入占据单元,clearing 利用有效射线清除可见空闲区域。没有深度返回的区域不能直接判定为空闲。

验证障碍是否写入代价图

使用现有启动方式加载修改后的参数并激活 Nav2 节点。在相机和 Nav2 都运行后执行:

bash
ros2 topic type /camera/depth/points
ros2 param get /local_costmap/local_costmap obstacle_layer.depth_cloud.max_obstacle_height

预期输出:

text
sensor_msgs/msg/PointCloud2
Double value is: 1.2

如果节点名被命名空间重映射,第二条命令需要改用实际节点全名。随后执行 ros2 run tf2_ros tf2_echo odom camera_depth_optical_frame,预期持续看到平移和旋转数值;若一直等待变换或报告外推错误,先核对 header.frame_id、TF 链及消息时间戳。时间戳不一致时,还应检查各节点的 use_sim_time 是否统一。

打开 RViz 2,将 Global Options → Fixed Frame 设为 odom,通过 Add → By topic 添加 /camera/depth/points 的 PointCloud2 显示和 /local_costmap/costmap 的 Map 显示。把纸箱放在前方 1 m、距地约 0.4 m 的观测区内:应看到纸箱点云对应位置出现障碍单元及周围膨胀代价;移开后,在有效清除观测覆盖下障碍应消退。

根据现象调整阈值与清除

  • 有点云、无障碍:先看 PointCloud2 的 frame_id 和 TF 是否连通,再检查坐标变换后的实际离地高度是否落在 0.10~1.20 m,以及距离是否在 0.20~2.50 m。
  • 地面大面积误标:核对相机外参、机器人姿态与 odom 的高度基准;仅在确认地面高度分布后,逐步提高 min_obstacle_height,例如从 0.10 调到 0.15 m,同时重新测试低矮障碍。
  • 障碍移走仍残留:确认 clearing: true、有效射线覆盖原障碍区域,必要时检查点云丢帧和时间同步。无效深度点不会自动清空旧障碍,不能靠无限增大清除距离解决。
  • 多个高度物体互相遮挡:ObstacleLayer 是三维点经高度筛选后的二维投影,不保留每个网格内的多层高度结构;需要精细处理悬空物体或垂直占据时,再考虑 VoxelLayer 或点云预处理。

验收时不要只看点云显示正常,还要确认同一障碍在局部代价图中出现、离开后合理消退,并且地面和机器人可通过的上方空间没有被错误标记。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

将 PointCloud2 映射到障碍层

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通