解析Navigation2中代价地图的参数配置

1. 机器人坐标系

在ROS (Robot Operating System) 和 Navigation2 (Nav2) 中，坐标系（Frames）的管理是实现机器人自主导航的核心。TF (Transform) 系统负责跟踪和广播不同坐标系之间的关系。

1.1 坐标系概览

以下是Nav2中最常用的几个坐标系：

重点说明：/map 坐标系的三种来源方式及其区别：

• 预建图导航 (Map Server):

  • 来源: 在导航开始前，通过 map_server 加载一个已经绘制好的、静态的地图文件（通常是 .yaml 和 .pgm 格式）。
  • 特点: /map 坐标系的原点和方向是固定的，由地图文件定义。整个导航过程中，/map 坐标系本身不会发生任何变化。定位模块（如 AMCL）的主要任务是持续计算机器人（/base_link）在这个固定 /map 中的位姿。
  • 应用场景: 适用于环境结构固定、已知的场景，如办公室、仓库、家庭等。

• SLAM边建图边导航 (Simultaneous Localization and Mapping):

  • 来源: 由 SLAM 算法（如 slam_toolbox）在运行时动态创建和更新。
  • 特点: /map 坐标系是在机器人探索环境的过程中逐步建立的。当 SLAM 算法进行回环检测和图优化时，可能会对地图进行修正，这会导致 /map -> /odom 的变换发生跳变，以反映更准确的地图和机器人位姿。也就是说，/map 坐标系本身可能会在导航过程中被优化调整。
  • 应用场景: 适用于未知环境的探索、建图或环境经常发生变化的场景。

• GPS/IMU 全局定位 (robot_localization + navsat_transform) 参考官方 Navigating Using GPS Localization

  • 来源：navsat_transform_node 将 GPS 经纬度 + IMU 朝向 + 里程计，转换为局部平面坐标（ENU/UTM），并由 robot_localization EKF 发布 map → odom 变换。
  • 特点：
    • /map 原点 = datum：可显式配置 (datum 参数) 或使用首帧 GPS 自动设定；这决定了 /map 与真正地理坐标 (/earth) 的对齐关系。
    • /map 本身在会话内不再优化，但机器人位姿会随着 EKF 融合持续平滑更新；若重新设定 datum，则连续任务之间 /map 也保持一致。
    • 适配 Nav2 的三种全局代价地图配置：滚动窗口、静态层、固定尺寸。
  • 应用场景：室外大尺度任务、需直接使用经纬度目标或多机器人在统一地理框架下协同。

1.2 /map 坐标系“本质”是什么？

简言之，/map 只是“全局静止坐标系”这一角色的代号；它可以来自静态离线地图、来自 SLAM 动态优化，也可以来自 GPS datum 的投影平面。理解这一点后，你就能根据自己的传感器与任务需求，选择最合适的 /map 来源，以及相应的 Nav2 全局代价地图配置。

2. 代价地图类型区分

代价地图是Nav2进行路径规划和避障的基础，它将世界表示为一个二维栅格，每个栅格的值（0-255）代表了通过该区域的“代价”或风险。

参考视频：ROS导航系统 | 代价地图的参数设置

3. 全局代价地图参数配置

全局代价地图主要用于生成一个长期的、穿越整个环境的规划路径。

3.1 核心参数解释

以下是 nav2_params.yaml 文件中 global_costmap 下的常见配置：

global_costmap:
  global_costmap:
    ros__parameters:
      # 基础设置
      update_frequency: 1.0          # 代价地图更新频率 (Hz)
      publish_frequency: 1.0         # 发布频率 (Hz)
      global_frame: map              # 全局坐标系
      robot_base_frame: base_link    # 机器人基座坐标系
      
      # 静态地图使用配置
      use_sim_time: True             # 是否使用仿真时间
      robot_radius: 0.22             # 机器人半径 (m)，用于膨胀障碍物
      resolution: 0.05               # 地图分辨率 (m/pixel)
      track_unknown_space: true      # 是否跟踪未知空间
      
      # 地图大小和滚动设置
      # 全局代价地图通常使用静态地图，不需要rolling_window
      rolling_window: false          # 不使用滚动窗口
      width: 50                      # 地图宽度 (cells)，rolling_window为false时可忽略
      height: 50                     # 地图高度 (cells)，rolling_window为false时可忽略
      
      # 代价值设置
      unknown_cost_value: 255        # 未知空间的代价值
      lethal_cost_threshold: 100     # 致命障碍物阈值
      trinary_costmap: true          # 使用三值代价地图 (0, 254, 255)
      
      # 地图变换设置
      transform_tolerance: 0.3       # 变换容忍度 (s)
      always_send_full_costmap: false # 是否总是发送完整代价地图
      
      # 插件配置 - 全局代价地图主要使用静态地图
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      
      # 静态地图层配置 - 这是全局代价地图的核心
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True    # 订阅持久化的地图话题
        enabled: True                          # 启用该层
        subscribe_to_updates: false            # 不订阅地图更新
        map_topic: "/map"                      # 静态地图话题名称
        
      # 障碍物层配置 - 处理传感器数据
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan              # 观测数据源
        footprint_clearing_enabled: true       # 启用足迹清除
        max_obstacle_height: 2.0               # 最大障碍物高度 (m)
        combination_method: 1                  # 组合方法：1为最大值
        
        # 激光雷达配置
        scan:
          topic: /scan                         # 激光雷达话题
          max_obstacle_height: 2.0             # 该传感器的最大障碍物高度
          clearing: True                       # 是否用于清除障碍物
          marking: True                        # 是否用于标记障碍物
          data_type: "LaserScan"               # 数据类型
          raytrace_max_range: 3.0              # 光线追踪最大范围
          raytrace_min_range: 0.0              # 光线追踪最小范围
          obstacle_max_range: 2.5              # 障碍物检测最大范围
          obstacle_min_range: 0.0              # 障碍物检测最小范围
      
      # 膨胀层配置 - 为路径规划提供安全边界
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0               # 代价缩放因子
        inflation_radius: 0.55                 # 膨胀半径 (m)
        enabled: True

3.2 关键问题分析

• 更新频率设置的原因:
  全局代价地图的 update_frequency 通常设置得较低（如1.0 Hz）。这是因为：

  1. 基于静态地图: 它的主要信息来源是静态地图，这个地图本身是不变的。
  2. 性能考虑: 全局代价地图通常很大，频繁更新会消耗大量计算资源，而这种更新对于长期的全局路径规划来说并非必要。
  3. 关注长期变化: 全局代价地图的主要任务是反映环境中的长期或永久性变化（例如，通过SLAM更新地图），而不是像行人那样的高频动态障碍物。

• 全局代价地图为什么需要更新？:
  即便主要基于静态地图，更新仍然是必要的：

  1. SLAM场景: 在SLAM模式下，map_server 会动态发布更新后的地图，全局代价地图需要更新以反映这些变化（如新发现的区域或闭环优化后的地图）。
  2. 动态障碍物（可选）: 如果配置了障碍物层（obstacle_layer），全局代价地图可以缓慢地将一些半永久性的障碍物（如被移动的家具）添加进来，为下一次全局规划提供更准确的信息。
  3. 机器人位置更新: 更新过程会根据最新的TF变换重新计算机器人在地图上的位置，并更新与机器人位置相关的代价信息（如膨胀层）。

• 全局路径/plan是否也会更新？:
  会。全局路径的重新规划不是由全局代价地图的 update_frequency 直接触发的，而是由行为树（Behavior Tree）中的 ComputePathToPose 节点触发。以下情况会导致全局路径重新规划：

  1. 初始规划: 导航任务开始时。
  2. 路径失效: 当局部规划器（Controller）发现当前的全局路径被新的障碍物堵死，无法继续前进时，它会返回失败，从而触发行为树重新进行全局路径规划。
  3. 周期性重新规划: 可以配置行为树，使其周期性地（例如每隔几秒或几米）重新规划路径，以确保路径始终是最优的。

• 每个图层设置是否必须？:
  不完全是，但通常建议包含核心图层。plugins 列表定义了代价地图如何构建。

  • static_layer: 对于预建图导航，这是必须的，它负责加载静态地图。
  • obstacle_layer: 如果你想让全局代价地图能反映一些非静态的障碍物，则需要此层。在纯静态环境中可以省略。
  • inflation_layer: 强烈建议必须包含。它在障碍物周围创建一片缓冲区（膨胀区），确保规划出的路径与障碍物保持安全距离，防止机器人本体撞到障碍物。没有它，规划器可能会生成紧贴障碍物的路径。

• plugins参数中的层与master costmap生成的关系:
  代价地图的最终生成是一个“图层叠加”的过程。master costmap（即最终发布的 /local_costmap/costmap）是按照 plugins 列表中定义的顺序，将各个层绘制的结果叠加在一起生成的。

4. 局部代价地图参数配置

局部代价地图是机器人实时避障的关键，它围绕机器人中心动态更新。

4.1 核心参数解释

local_costmap:
  local_costmap:
    ros__parameters:
      # 基础设置
      update_frequency: 5.0          # 更新频率 (Hz) - 比全局更频繁
      publish_frequency: 2.0         # 发布频率 (Hz)
      global_frame: odom             # 里程计坐标系
      robot_base_frame: base_link    # 机器人基座坐标系
      use_sim_time: True
      robot_radius: 0.22             # 机器人半径
      resolution: 0.05               # 分辨率，通常与全局保持一致
      track_unknown_space: true
      
      # 滚动窗口设置 - 局部代价地图的关键特性
      rolling_window: true           # 启用滚动窗口 - 以机器人为中心
      width: 3                       # 地图宽度 (m) - 注意这里是米，不是像素
      height: 3                      # 地图高度 (m)
      
      # 代价值设置
      unknown_cost_value: 255
      lethal_cost_threshold: 100
      trinary_costmap: true
      
      # 变换设置
      transform_tolerance: 0.3
      always_send_full_costmap: false
      
      # 插件配置 - 局部代价地图通常不使用静态地图层
      plugins: ["voxel_layer", "inflation_layer"]
      
      # 体素层配置 - 3D障碍物处理，适合动态环境
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True                # 发布体素地图用于可视化
        origin_z: 0.0                          # Z轴原点
        z_resolution: 0.05                     # Z轴分辨率
        z_voxels: 16                           # Z轴体素数量
        max_obstacle_height: 2.0               # 最大障碍物高度
        mark_threshold: 0                      # 标记阈值
        observation_sources: scan              # 观测数据源
        footprint_clearing_enabled: true       # 足迹清除
        
        # 激光雷达配置 - 针对局部环境优化
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0              # 局部环境的光线追踪范围
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          inf_is_valid: false                   # 无限远值是否有效
      
      # 膨胀层配置 - 为局部路径规划提供安全边界
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55                 # 膨胀半径
        enabled: True

4.2 关键问题分析

• 为什么更新频率相对全局更高？:
  局部代价地图的 update_frequency 设置得很高（如5-20 Hz），因为它的核心任务是实时避障。机器人需要快速响应环境中突然出现的动态障碍物（如行人、其他移动机器人）。高频率的更新确保了传感器捕捉到的最新障碍物信息能被迅速反映到局部代价地图上，从而让局部规划器（Controller）能够及时计算出安全的规避轨迹。

• global_frame设为odom的原因，这样设置是否会导致局部控制器跟随路径出现问题？:

  • 原因: 设置为 /odom 是为了平滑性和连续性。/odom 坐标系是连续的，不会像 /map 坐标系那样在SLAM回环修正时发生跳变。局部规划器需要在机器人周围一个稳定、无跳变的坐标系中进行高频的轨迹计算。如果以 /map 为参考，一旦 /map -> /odom 的变换发生跳变，整个局部地图的参考系都会瞬间移动，导致局部规划器计算混乱，机器人可能会突然“跳跃”或原地打转。
  • 是否会导致问题: 不会导致跟随路径出现问题。这是因为虽然局部代价地图在 /odom 框架下运行，但全局路径（plan）本身是在 /map 框架下生成的。在将全局路径交给局部规划器之前，Nav2会通过TF变换，将需要跟随的那一小段全局路径从 /map 坐标系转换到 /odom 坐标系下。这样，局部规划器就在同一个 /odom 坐标系中拥有了目标路径和局部代价地图，从而可以精确地进行跟随和避障。

5. 全局代价地图的三种官方配置

5.1 三种官方配置解释

Nav2 依旧在map坐标系下构建栅格代价地图。官方教程第二部分设置导航系统列出了三种做法（选其一即可）：

不论哪种方式，global_frame 都设为 map，局部 costmap 仍用 odom 或 map，与室内一致。

下面按 Nav2 官方文档给出的三种全局代价地图（global costmap）典型配置，逐一说明它们在运行时“如何刷新／保持”环境代价的差异。所有配置都共享同一组 Layer 插件（obstacle / inflation / …）；差别主要体现在地图边界随时间的变化方式与旧代价的保留策略。

5.2 进一步理解地图更新细节

• Layer 周期
  每张 costmap 由 master grid + 一系列 plugin layer 组成。update_frequency 控制主循环周期；在每个周期里：

  1. 若启用了 rolling_window，先计算新窗口原点并移动 master grid（复制或重置格点）。
  2. 调用各 Layer 的 updateBounds() / updateCosts()：
     • ObstacleLayer 用最新传感器数据 mark 障碍，并按 clearing 逻辑 clear 移除；
     • StaticLayer 仅在订阅到新 /map 时整体写入；平时跳过；
     • InflationLayer 根据 freshly‑updated master grid 重新膨胀成本。
  3. 发布 /costmap 或增量 /costmap_updates（若 always_send_full_costmap: false）。

• 为什么 Rolling Window 会丢掉历史
  官方 ROS Wiki 对 rolling_window 的说明：“保持机器人位于 costmap 中心，超出窗口的障碍信息被丢弃”。这就是资源恒定的根本原因，也是需要大窗口才能远距离规划的原因。

• StaticLayer 会覆盖你设的尺寸
  StaticLayer 文档指出它会把 costmap 大小调整为静态地图大小。因此在静态地图模式下，width/height 不起作用，想裁剪地图须在生成地图时就裁剪。

• 固定尺寸示例
  实践中常在 Nav2 参数里手动把 width: 20, height: 20, origin_x/-y 设为固定框，限制代价地图范围，以节省计算。只要不加载 StaticLayer，就不会被重设。