引言

ROS1 导航架构概述

ROS1 Navigation依赖move_base 管理全局/局部规划/costmap以及恢复行为，通过硬编码状态机的方式实现，节点在（PLANNING，CONTROLLING， CLEARING ）几个状态进行切换。

• 下图为ROS1 Navigation Stack的核心架构：

• 若链接CostMap与规划器，则更加复杂：

ROS2 导航架构概述

ROS2 Nav2 将 ROS1 单体的 move_base 拆分为多个独立 Server（模块化解耦）每个 Server 专注单一功能，通过标准化接口协作，配合行为树（BT） 完成导航。

Nav2 的核心思想是 “服务器（Server）+ 行为树（Behavior Tree）+ 生命周期管理（Lifecycle Manager）”。它不再是一个像 move_base 那样的单体大节点，而是由多个独立的、可管理的节点组成。

• 下图为ROS2核心架构与工作流程：

解读costmap

ROS1/ROS2架构中，代价地图层都是核心部件（ROS2的为nav2_costmap_2d也是集成自costmap_2d但重构为生命周期节点,可见更为重要），接下来对其进行深入解读。首先看看什么是代价地图~~~

代价地图（CostMap）是一种用于描述环境中各个位置(栅格)的导航代价的地图。这个导航代价反映了机器人在不同区域移动的相对难易程度。 代价地图可以有如下的意义：

• 路径规划： 代价地图用于路径规划算法，帮助机器人找到从起点到目标的最优路径。代价地图中的代价值通常表示机器人在特定位置的可行性和安全性，路径规划算法会优先选择代价较低的路径。
• 避障： 通过代价地图，机器人能够避开障碍物、危险区域或其他不利于移动的区域。代价地图中高代价的区域通常对应于障碍物或其他需要规避的物体，从而确保机器人选择安全的路径。
• 动态环境适应： 代价地图可以反映环境的动态变化，比如移动的障碍物或临时障碍。通过实时更新代价地图，机器人能够及时调整路径规划，适应环境的变化。
• 速度调整： 代价地图中的代价值可以与机器人的速度相关联。在规划路径时，代价地图可以影响机器人在不同区域的运动速度，使其在复杂环境中更加灵活和高效。
• 全局与局部规划结合： 代价地图可以支持全局路径规划和局部路径规划的结合。全局规划负责长距离导航，而局部规划负责处理机器人在实时环境中的微调，以应对临时障碍或其他突发情况。

costmap对应的是costmap_2d功能包，该功能包是机器人导航系统的核心组件，负责维护环境成本的 2D 表示。它采用分层架构（Layered Architecture），将不同类型的信息（静态地图、实时传感器数据和安全缓冲区）聚合为一个单一的“主”代价地图（Master Costmap），供规划器使用。

代价地图层次架构分类

代价地图是由多层数据融合，获取的，主要包括以下：

• 静态层（Static Layer）：来自预先构建的静态地图
• 障碍层（Obstacle Layer）：实时传感器数据（如激光雷达/深度相机）检测到的动态障碍物
• 膨胀层（Inflation Layer）：对障碍物进行膨胀，以确保机器人与障碍物之间保持安全距离。
• 体素层（Voxel Layer）

代价地图通用参数costmap_common_params.yaml：

• obstacle_range 障碍物探测范围 (米)。传感器读数超出此范围则不会作为障碍物加入代价地图。典型值: 2.5。
• raytrace_range 光线追踪范围 (米)。用于清除已知障碍物之间空间的传感器读数范围，通常略大于 obstacle_range。典型值: 3.0。
• robot_radius (圆形机器人) 机器人半径 (米)。用于足迹计算（圆形模型简化）。
• footprint (多边形机器人) 机器人足迹。以坐标点列表描述机器人轮廓（多边形模型更精确），有这个应该就可以不用上面一个参数了。
• inflation_radius 膨胀半径 (米)。设置障碍物产生代价的距离，值越大，机器人离障碍物越远。典型值: 0.55。
• cost_scaling_factor 代价缩放因子。控制代价随距离衰减的速度，值越大，代价衰减越快。

核心文件及主要功能解读：

1. costmap_2d_ros.cpp

【主要逻辑，图层管理】ROS 节点的顶层接口，负责将代价地图与ROS系统连接起来。

• Costmap2DROS类：主类，它封装了 Costmap2D（通过 LayeredCostmap/layered_costmap.cpp）并协调与 ROS 的交互。它管理图层的加载、更新、发布可视化信息，并通过 tf 将地图关联到机器人坐标系。
  • 关于TF树：需要完整(如map-->base_link)，若定位没有启动，代价地图是无法运行的
  • plugins:以插件的形式加载不同的图层。不硬编码“静态地图”或“障碍物”，而是通过 plugin_loader_动态加载。
• Costmap2D 类： 核心数据结构，存储底层二维网格代价地图。
• 地图更新主线程mapUpdateLoop，根据设定的频率循环调用地图更新逻辑。注意，这部分应该是当参数发生变化是才更新reconfigureCB(参数不变，是不需要更新的)
  • reconfigureCB 的设计体现了“彻底同步”的思想。它不是简单地改两个数字，而是确保了从底层的内存大小（Resize）、到算法层的碰撞半径（Footprint）、再到执行层的刷新工作频率（Thread），全部刷新一遍。这种做法虽然在执行的一瞬间会有短暂的 CPU 峰值，但保证了系统参数在动态调整过程中的绝对一致性。
• void updateMap(): 核心方法，循环调用所有图层的 updateBounds 和 updateCosts 来更新代价地图。
  • 更新边界 (Update Bounds):
    • 调用每个层，根据新的传感器数据或机器人移动，报告其需要更新的区域。
    • 这些边界被合并为一个单一的矩形边界框。
  • 更新代价值 (Update Costs):
    • 在边界框内清除主代价地图。
    • 每个层按顺序（在 plugins.xml 或 YAML 配置中定义）将其数据写入主网格。写入方式包括覆盖（Overwrite）、取最大值（Max）等。
• 图层管理: 负责根据参数加载插件图层，并初始化它们。
• TF 关联: 通过 tf 缓冲区查找机器人的实时位姿，将代价地图关联到机器人坐标系（如 base_link 到 map）。
• 可视化与发布: 通过 Costmap2DPublisher 发布代价地图数据及其可视化信息（网格、足迹等）。
• 动态调参: 集成了 dynamic_reconfigure，允许在运行时修改更新频率、代价地图大小等参数。
• footprint管理: 订阅并处理机器人模型（footprint/机器人在投影到地面的 2D 多边形轮廓），用于后续的膨胀和碰撞检查。注意，最终应该只有膨胀层会根据footfrpint进行设置。

2. layered_costmap.cpp

所有代价地图插件的中央管理器。代价地图的核心在于“分层聚合”。通过多个继承自 Layer 的插件共同协作。

• 持有主 Costmap2D 对象。
• 编排更新流程，按特定顺序调用各个插件。
• 管理滚动窗口（Rolling Window）逻辑（地图中心随机器人移动）。
• 在一帧的更新周期内，它指挥所有插件完成“两阶段”融合：
  • 第一阶段：UpdateBounds (边界收集)
    • 每个图层（Layer）报出自己本次变动的地理范围（MinX, MinY, MaxX, MaxY）。
  • 第二阶段：UpdateCosts (代价绘制)
    • 在上述计算的区域，先清空旧数据。
    • 然后让各个图层依次（按照加载顺序）将新的代价值填入主地图。

3. layer.cpp

图层基类，定义了标准化的接口

• onInitialize(): 插件被加载时的初始化。
• updateBounds(): 告诉系统哪里变了。
• updateCosts(): 告诉系统变成了什么值。

• matchSize(): 当主地图尺寸变化时同步调整自己的缓冲区。

• src/static_layer.cpp - 静态地图图层
  • 加载预构建的占据栅格地图（nav_msgs/OccupancyGrid，通常来自 map_server）
  • 初始化onInitialize函数,订阅了地图话题，并进入while循环阻塞等待，在静态地图到位之前，整个 costmap_2d 节点不会完成初始化，这是为了确保路径规划有基准底图。接收到topic后就会回调函数incomingMap。
  • interpretValue: 将 OccupancyGrid [0, 100] 的概率映射到 [0, 254] 的代价：
    • 未知空间处理：如果 track_unknown_space 为真，-1 (unknown) 映射为 255 (NO_INFORMATION)。
    • 三值化 (Trinary)：默认情况下，只要概率值小于 lethal_threshold，就统一设为 0 (FREE_SPACE)；大于等于则设为 254 (LETHAL)。这极大地简化了全局规划。
    • 连续映射：如果禁用了三值化，它会线性缩放概率值到 [0, 254]。
  • 同步锁定: 在接收到第一张地图后，通常会锁定代价地图的大小和分辨率。
• src/obstacle_layer.cpp - 障碍物图层
  • 处理实时传感器数据（如激光雷达、点云）。通过射线追踪（Raytracing）清除空间，并通过标记（Marking）添加新障碍物。
  • ObservationBuffer：为每个源创建一个 ObservationBuffer。这是一个线程安全的类，负责将接收到的数据暂存在一个队列中，并根据 observation_persistence 设置过期时间。
  • Marking (标注): 解析传感器点云，在对应栅格位置写入代价（LETHAL_OBSTACLE）。
  • Clearing (清除): 应用射线追踪 (Raytracing) 算法（通常是 Bresenham 算法），清除传感器原点与探测到的障碍点连线路径上的无效障碍，维持地图的实时准确性。
• src/inflation_layer.cpp - 膨胀图层
  • 在障碍物周围添加安全缓冲区。成本随离障碍物的距离呈指数级衰减。

    4. costmap_2d.cpp

代价地图实现

参考资料

• ROS 1的Navigation 和 ROS 2的Navigation2（Nav2）