RuyiSDK Board Docs

RISC-V ROS2 机器人操作系统编程技术

Source repository

ch17 · 全局代价地图

TextbookProgramming languageEnvironmentLessonLab
RISC-VC++17SpacemiT K3 CoM260 Kit / Bianbu 4.0.6 / Humble, with an x86 Ubuntu 22.04 / Humble / Harmonic course containerRead lessonStart lab
x86PythonUbuntu 22.04 / Humble or Ubuntu 24.04 / JazzyRead lessonStart lab

This document is not yet available in English. The Chinese original is shown below.

第17章 全局代价地图

课程:ROS2 Python 编程
章节:第17章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示


学习目标

本章学习目标包括:理解代价地图(Costmap2D)的原理和工作机制,掌握静态层、障碍物层、膨胀层的配置方法,熟悉代价地图的分层架构和插件机制,能够根据应用场景调优代价地图参数,掌握全局与局部代价地图的区别和配合。

17.1 代价地图基本原理

17.1.1 什么是代价地图

代价地图 (Costmap) 是导航系统中将物理空间建模为带代价值栅格的核心数据结构。每个栅格(cell)包含一个代价值,表示该位置是否适合机器人通过。

代价值的含义:

值范围 标签 含义
0 Free (空闲) 无障碍物,可自由通行
1-127 Inscribed (刻划) 靠近障碍物的区域,代价值随距离增加
128-253 Obstacle (障碍物) 障碍物膨胀区域,不宜通行
254 Lethal (致命) 障碍物占据,不可通行
255 Unknown (未知) 未探索区域

代价地图的作用: 为路径规划提供环境模型,编码障碍物信息和安全距离,支持动态障碍物实时更新,并通过分层架构支持多种信息来源。

17.1.2 Costmap2D数据结构

import numpy as np
from nav_msgs.msg import OccupancyGrid
 
class Costmap2D:
    """代价地图数据结构"""
    def __init__(self, width: float, height: float, 
                 resolution: float = 0.05):
        self.resolution = resolution
        self.width = int(width / resolution)
        self.height = int(height / resolution)
        self.origin_x = -width / 2.0
        self.origin_y = -height / 2.0
        
        # 主代价数组 (0-255)
        self.costmap = np.zeros((self.height, self.width), dtype=np.uint8)
        
        # 未知区域初始化为255
        self.costmap[:] = 255
        
    def world_to_grid(self, wx: float, wy: float) -> tuple:
        """世界坐标转栅格坐标"""
        gx = int((wx - self.origin_x) / self.resolution)
        gy = int((wy - self.origin_y) / self.resolution)
        return gx, gy
    
    def grid_to_world(self, gx: int, gy: int) -> tuple:
        """栅格坐标转世界坐标"""
        wx = gx * self.resolution + self.origin_x
        wy = gy * self.resolution + self.origin_y
        return wx, wy
    
    def set_cost(self, wx: float, wy: float, cost: int):
        """设置某点的代价值"""
        gx, gy = self.world_to_grid(wx, wy)
        if 0 <= gx < self.width and 0 <= gy < self.height:
            self.costmap[gy, gx] = np.clip(cost, 0, 255)
    
    def get_cost(self, wx: float, wy: float) -> int:
        """获取某点的代价值"""
        gx, gy = self.world_to_grid(wx, wy)
        if 0 <= gx < self.width and 0 <= gy < self.height:
            return int(self.costmap[gy, gx])
        return 255
    
    def to_occupancy_grid(self) -> OccupancyGrid:
        """转换为ROS OccupancyGrid消息"""
        msg = OccupancyGrid()
        msg.header.frame_id = 'map'
        msg.info.resolution = self.resolution
        msg.info.width = self.width
        msg.info.height = self.height
        msg.info.origin.position.x = self.origin_x
        msg.info.origin.position.y = self.origin_y
        
        # 转换代价为-1,0,100格式
        occ = np.zeros_like(self.costmap, dtype=np.int8)
        occ[self.costmap == 0] = 0        # 空闲
        occ[self.costmap == 255] = -1     # 未知
        occ[self.costmap >= 254] = 100    # 障碍物
        occ[(0 < self.costmap) & (self.costmap < 254)] = 0
        
        msg.data = occ.flatten().tolist()
        return msg
    
    def reset(self):
        """重置代价地图"""
        self.costmap[:] = 255

17.1.3 官方要点——Costmap2D 的代价语义:Lethal、Inscribed 与 Unknown

官方 Costmap2D 文档定义了与本章 17.2.2 对应的栅格代价等级:0(FREE_SPACE,确定可通行)、99(INSCRIBED_INFLATED_OBSTACLE,机器人内切圆与障碍物接触的临界值)、100(LETHAL_OBSTACLE,障碍物本体)、255(NO_INFORMATION,未知)。文档特别强调一个常见误解:255 并非"最危险",而是"传感器从未观测过";track_unknown_space 参数决定把未观测区域保留为 255 还是初始化为 0——室内有完整静态地图时通常保留未知以禁止穿越,而反复重扫的场景会将其清零。

Inflation Layer 文档还给出了代价衰减公式的指数形式:exp(-1.0 * inflation_radius * (distance - inscribed_radius) * cost_scaling_factor)。这正是 17.5.2 中 inflation_radius 与 cost_scaling_factor 这对参数的数学出处:前者决定"危险圈"的物理边界,后者决定衰减坡度——坡度太小会导致梯度平缓区域过长、规划器抄近道贴墙,太大则梯度在远区几乎为零、失去引导作用。

17.2 分层代价地图架构

17.2.1 分层设计思想

Nav2将代价地图设计为分层架构,每一层负责维护不同的代价信息,最终融合为一张主代价地图:

          主代价地图 (Master Costmap)
          ┌────────────────────────┐
          │  融合各层代价信息       │
          │  cost = max(all_layers) │
          └──────────┬─────────────┘
                     │
     ┌───────────────┼───────────────┐
     │               │               │
┌────┴────┐   ┌────┴────┐   ┌─────┴─────┐
│Static   │   │Obstacle │   │ Inflation │
│Layer    │   │Layer    │   │ Layer     │
│静态地图  │   │动态障碍物│   │障碍物膨胀  │
└─────────┘   └─────────┘   └───────────┘

各层功能:

图层 插件名称 功能 更新方式
静态层 StaticLayer 加载预建栅格地图 静态加载
障碍物层 ObstacleLayer 传感器实时障碍物检测 实时更新
膨胀层 InflationLayer 障碍物膨胀/安全距离 计算生成
体素层 VoxelLayer 3D体素障碍物感知 实时更新
范围层 RangeLayer 距离传感器更新 实时更新
禁飞层 KeepoutLayer 禁止进入区域 配置加载

17.2.2 全局代价地图 vs 局部代价地图

特性 全局代价地图 (Global Costmap) 局部代价地图 (Local Costmap)
坐标系 map odom
范围 整个已知地图 机器人周围滚动窗口
更新频率 低 (1-2 Hz) 高 (5-10 Hz)
分辨率 较高 (0.05m) 较高 (0.05m)
用途 全局路径规划 局部路径规划和避障
图层配置 static+obstacle+inflation obstacle+inflation (无static)

17.2.3 代价地图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     # 机器人基座坐标系
      robot_radius: 0.22              # 机器人半径 (用于足迹)
      resolution: 0.05                # 地图分辨率 (m/像素)
      track_unknown_space: true       # 跟踪未知区域
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true  # 使用transient_local QoS
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: "scan"
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0      # 代价衰减因子
        inflation_radius: 0.55        # 膨胀半径 (m)
 
# local_costmap配置
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom              # 使用里程计坐标系
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.05
      rolling_window: true            # 滚动窗口模式
      width: 4                        # 窗口宽度 (m)
      height: 4                       # 窗口高度 (m)
      track_unknown_space: true
      plugins: ["voxel_layer", "inflation_layer"]
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        observation_sources: "scan"
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

17.2.4 官方要点——层插件体系与滚动窗口

nav2_costmap_2d 文档把地图组织为可插拔层:static_layer 加载 pgm/yaml 静态地图;obstacle_layer 消费 LaserScan/PointCloud,支持 Raytracing(把观测到的空区标记为可通行)与 Observation Buffer 的两套缓存机制;voxel_layer 在 obstacle_layer 基础上维护 3D 体素,用于桌子下沿等悬空障碍(对应本章 17.6 的 3D 感知);inflation_layer 必须放最后,对合成结果统一膨胀。官方明确建议:全局地图用"静态层 + 障碍层 + 膨胀层",局部地图用"障碍层/体素层 + 膨胀层"且开启 rolling_window: true——与本章 17.3.2 的推荐组合一致。

滚动窗口参数中,width/height/resolution 决定局部窗口的物理尺寸(典型 3m×3m、0.05m 分辨率);update_frequency 与 publish_frequency 分控制与发布两个环路,文档建议 update 频率不低于控制器频率的一半,否则控制器消费的是过期数据。

17.3 静态层配置

17.3.1 静态层作用

静态层 (StaticLayer) 负责加载预先构建的栅格地图,将其代价信息传递给主代价地图。

静态层的输入: map 话题上的 OccupancyGrid 消息,或通过 map_server 加载的 YAML/PGM 地图文件。

静态层的工作流程:

1. 订阅 /map 话题
2. 将地图数据转换为costmap格式
3. 标记已知区域为对应代价
4. 标记未知区域为255
5. 持续监听地图更新

17.3.2 地图文件格式

# office_map.yaml
image: office_map.pgm          # 地图图像文件
resolution: 0.050000           # 地图分辨率 (m/像素)
origin: [-10.0, -10.0, 0.0]   # 地图原点 (x, y, yaw)
negate: 0                      # 是否反转像素值
occupied_thresh: 0.65          # 占据阈值 (>0.65视为障碍物)
free_thresh: 0.196             # 空闲阈值 (<0.196视为空闲)

17.3.3 地图加载机制

import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from nav2_msgs.srv import LoadMap
import yaml
 
class MapLoader(Node):
    """地图加载管理器"""
    def __init__(self):
        super().__init__('map_loader')
        
        self.map_pub = self.create_publisher(
            OccupancyGrid, '/map', 10, 
            qos_profile=rclpy.qos.QoSProfile(
                depth=1,
                durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL
            ))
            
        self.load_map_service = self.create_service(
            LoadMap, '/map_server/load_map', self.load_map_callback)
            
    def load_map_from_yaml(self, yaml_path: str) -> OccupancyGrid:
        """从YAML文件加载地图"""
        with open(yaml_path, 'r') as f:
            meta = yaml.safe_load(f)
            
        # 读取PGM图像
        import cv2
        image = cv2.imread(meta['image'], cv2.IMREAD_GRAYSCALE)
        
        height, width = image.shape
        resolution = meta['resolution']
        origin = meta['origin']
        
        # 转换PGM到OccupancyGrid
        msg = OccupancyGrid()
        msg.header.frame_id = 'map'
        msg.info.resolution = resolution
        msg.info.width = width
        msg.info.height = height
        msg.info.origin.position.x = origin[0]
        msg.info.origin.position.y = origin[1]
        
        # 像素值转换 (0=白=空闲, 255=黑=障碍物)
        occupied_thresh = meta.get('occupied_thresh', 0.65)
        free_thresh = meta.get('free_thresh', 0.196)
        negate = meta.get('negate', 0)
        
        data = []
        for row in image:
            for pixel in row:
                if negate:
                    pixel = 255 - pixel
                    
                normalized = pixel / 255.0
                if normalized > occupied_thresh:
                    data.append(100)  # 占据
                elif normalized < free_thresh:
                    data.append(0)    # 空闲
                else:
                    data.append(-1)   # 未知
                    
        msg.data = data
        return msg
    
    def load_map_callback(self, request, response):
        """服务回调:加载地图"""
        try:
            occupancy_grid = self.load_map_from_yaml(request.map_url)
            self.map_pub.publish(occupancy_grid)
            self.get_logger().info(f'地图已加载: {request.map_url}')
            response.result = LoadMap.Response().RESULT_SUCCESS
        except Exception as e:
            self.get_logger().error(f'地图加载失败: {e}')
            response.result = LoadMap.Response().RESULT_UNDEFINED_FAILURE
        return response

17.4 障碍物层配置

17.4.1 障碍物层作用

障碍物层 (ObstacleLayer) 使用传感器数据实时检测动态障碍物并更新代价地图。

障碍物层的工作流程:

1. 订阅传感器话题 (LaserScan/PointCloud2)
2. 将传感器数据转换到地图坐标系
3. 标记传感器点到代价地图 (marking)
4. 清除传感器路径上的代价 (clearing)
5. 障碍物标记为254 (lethal)

17.4.2 多传感器源配置

# 多传感器障碍物层配置
obstacle_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  enabled: true
  observation_sources: "scan front_sonar rear_sonar"
  
  scan:
    topic: /scan
    sensor_frame: laser
    max_obstacle_height: 2.0
    min_obstacle_height: 0.0
    clearing: true
    marking: true
    data_type: "LaserScan"
    raytrace_max_range: 3.0
    raytrace_min_range: 0.1
    
  front_sonar:
    topic: /sonar/front
    sensor_frame: front_sonar_link
    clearing: true
    marking: true
    data_type: "Range"
    min_range: 0.1
    max_range: 2.0
    
  rear_sonar:
    topic: /sonar/rear
    sensor_frame: rear_sonar_link
    clearing: true
    marking: true
    data_type: "Range"
    min_range: 0.1
    max_range: 1.5

17.4.3 传感器数据处理

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2
import numpy as np
import sensor_msgs_py.point_cloud2 as pc2
 
class ObstacleLayerProcessor(Node):
    """障碍物层数据处理节点"""
    def __init__(self):
        super().__init__('obstacle_layer_processor')
        
        self.scan_sub = self.create_subscription(
            LaserScan, '/scan', self.scan_callback, 10)
        self.cloud_sub = self.create_subscription(
            PointCloud2, '/point_cloud', self.cloud_callback, 10)
            
        self.obstacle_map = np.zeros((200, 200), dtype=np.uint8)
        self.resolution = 0.05
        
    def scan_callback(self, msg: LaserScan):
        """处理激光扫描数据"""
        ranges = np.array(msg.ranges)
        angles = np.linspace(msg.angle_min, msg.angle_max, len(ranges))
        
        # 过滤无效点
        valid = (ranges > msg.range_min) & (ranges < msg.range_max)
        valid_ranges = ranges[valid]
        valid_angles = angles[valid]
        
        # 转换为笛卡尔坐标
        x = valid_ranges * np.cos(valid_angles)
        y = valid_ranges * np.sin(valid_angles)
        points = np.column_stack((x, y))
        
        # 标记障碍物 (marking)
        for point in points:
            gx = int(point[0] / self.resolution)
            gy = int(point[1] / self.resolution)
            if 0 <= gx < 200 and 0 <= gy < 200:
                self.obstacle_map[gy, gx] = 254  # lethal
                
        # 清除传感器路径 (clearing)
        for angle, distance in zip(valid_angles, valid_ranges):
            for d in np.arange(0.1, distance, 0.1):
                px = d * np.cos(angle)
                py = d * np.sin(angle)
                gx = int(px / self.resolution)
                gy = int(py / self.resolution)
                if 0 <= gx < 200 and 0 <= gy < 200:
                    # 如果栅格在路径上非障碍物,设为0
                    if self.obstacle_map[gy, gx] != 254:
                        self.obstacle_map[gy, gx] = 0
                        
    def cloud_callback(self, msg: PointCloud2):
        """处理点云数据(3D障碍物投影到2D)"""
        points = pc2.read_points(msg, field_names=("x", "y", "z"), 
                                  skip_nans=True)
        
        for point in points:
            x, y, z = point
            # 只考虑一定高度范围内的障碍物
            if -0.1 < z < 2.0:
                # 投影到2D代价地图
                gx = int(x / self.resolution)
                gy = int(y / self.resolution)
                if 0 <= gx < 200 and 0 <= gy < 200:
                    self.obstacle_map[gy, gx] = 254

17.5 膨胀层配置

17.5.1 膨胀层作用

膨胀层 (InflationLayer) 在障碍物周围生成代价值梯度,使规划器能够生成远离障碍物的安全路径。

膨胀的数学公式:

if distance < inscribed_radius:
    cost = 254 (lethal)
elif distance < inflation_radius:
    cost = exp(-scaling_factor * (distance - inscribed_radius)) * 253
else:
    cost = 0

17.5.2 膨胀参数详解

inflation_layer:
  plugin: "nav2_costmap_2d::InflationLayer"
  inflation_radius: 0.55        # 膨胀半径 (m)
  cost_scaling_factor: 3.0      # 代价衰减因子

参数说明:

inflation_radius:膨胀半径,决定障碍物的影响范围。半径越大,路径越远离障碍物(安全但可能绕路);半径越小,路径越贴近障碍物(高效但有碰撞风险)。

cost_scaling_factor:代价衰减速度。因子越高衰减越快,只有近距离有高代价;因子越低衰减越慢,影响范围越大。

import numpy as np
 
class InflationLayer:
    """膨胀层实现"""
    def __init__(self, inflation_radius: float = 0.55,
                 cost_scaling_factor: float = 3.0,
                 resolution: float = 0.05):
        self.inflation_radius = inflation_radius
        self.cost_scaling_factor = cost_scaling_factor
        self.resolution = resolution
        
        # 预计算膨胀核
        self.inflation_kernel = self._compute_inflation_kernel()
        
    def _compute_inflation_kernel(self) -> np.ndarray:
        """预计算膨胀核(用于快速膨胀)"""
        radius_cells = int(np.ceil(self.inflation_radius / self.resolution))
        size = 2 * radius_cells + 1
        kernel = np.zeros((size, size), dtype=np.float32)
        
        # 刻划半径(等于机器人半径)
        inscribed_radius = 0.22  # 机器人半径
        
        center = radius_cells
        for dy in range(-radius_cells, radius_cells + 1):
            for dx in range(-radius_cells, radius_cells + 1):
                distance = np.sqrt(dx**2 + dy**2) * self.resolution
                
                if distance < inscribed_radius:
                    kernel[center + dy, center + dx] = 254  # lethal
                elif distance < self.inflation_radius:
                    # 指数衰减
                    kernel[center + dy, center + dx] = (
                        np.exp(-self.cost_scaling_factor * 
                               (distance - inscribed_radius)) * 253
                    )
                else:
                    kernel[center + dy, center + dx] = 0
                    
        return kernel
    
    def inflate(self, costmap: np.ndarray) -> np.ndarray:
        """对代价地图执行膨胀"""
        inflated = costmap.copy()
        
        # 找到所有障碍物栅格
        obstacle_cells = np.where(costmap >= 254)
        
        kh, kw = self.inflation_kernel.shape
        kh2, kw2 = kh // 2, kw // 2
        
        for y, x in zip(*obstacle_cells):
            # 确定kernel覆盖范围
            y_start = max(0, y - kh2)
            y_end = min(costmap.shape[0], y + kh2 + 1)
            x_start = max(0, x - kw2)
            x_end = min(costmap.shape[1], x + kw2 + 1)
            
            ky_start = kh2 - (y - y_start)
            ky_end = ky_start + (y_end - y_start)
            kx_start = kw2 - (x - x_start)
            kx_end = kx_start + (x_end - x_start)
            
            # 膨胀操作:取各层最大值
            region = inflated[y_start:y_end, x_start:x_end]
            kernel_region = self.inflation_kernel[
                ky_start:ky_end, kx_start:kx_end
            ]
            
            mask = kernel_region > region
            region[mask] = kernel_region[mask]
            
        return inflated
    
    def compute_cost(self, distance: float) -> float:
        """计算给定距离的代价值"""
        inscribed_radius = 0.22
        
        if distance < inscribed_radius:
            return 254.0
        elif distance < self.inflation_radius:
            cost = (np.exp(-self.cost_scaling_factor * 
                          (distance - inscribed_radius)) * 253)
            return cost
        else:
            return 0.0

17.5.3 膨胀效果调优

膨胀参数对路径规划的影响:

def visualize_inflation_effects():
    """展示不同膨胀参数的视觉效果"""
    import matplotlib.pyplot as plt
    
    inflation = InflationLayer(0.55, 3.0)
    distances = np.linspace(0, 1.0, 100)
    
    # 不同scaling_factor的对比
    factors = [1.0, 3.0, 5.0, 10.0]
    
    plt.figure(figsize=(10, 6))
    for factor in factors:
        costs = [
            InflationLayer(0.55, factor).compute_cost(d)
            for d in distances
        ]
        plt.plot(distances, costs, label=f'scaling={factor}')
    
    plt.xlabel('距离障碍物距离 (m)')
    plt.ylabel('代价值')
    plt.title('膨胀代价曲线')
    plt.legend()
    plt.grid(True)
    plt.show()

17.6 代价地图参数调优

17.6.1 分辨率选择

# 不同场景的推荐分辨率
# 室内小场景 (50-500m²)
resolution: 0.05  # 精细
 
# 中型环境 (500-2000m²)  
resolution: 0.05  # 平衡精度和计算量
 
# 大型环境 (>2000m²)
resolution: 0.1   # 降低计算负担

17.6.2 更新频率调优

# 全局代价地图(低频)
global_costmap:
  update_frequency: 1.0     # 1Hz更新
  publish_frequency: 1.0    # 1Hz发布
 
# 局部代价地图(高频)
local_costmap:
  update_frequency: 5.0     # 5Hz更新
  publish_frequency: 2.0    # 2Hz发布

17.6.3 膨胀半径选择

def recommend_inflation_radius(robot_radius: float, 
                                environment_type: str) -> float:
    """
    根据机器人尺寸和环境推荐膨胀半径
    """
    base_radius = robot_radius
    
    if environment_type == 'narrow_corridor':
        # 狭窄走廊:尽量靠近障碍物
        multiplier = 1.5
    elif environment_type == 'open_space':
        # 开阔空间:保持安全距离
        multiplier = 3.0
    elif environment_type == 'warehouse':
        # 仓库:标准设置
        multiplier = 2.0
    elif environment_type == 'office':
        # 办公室:兼顾安全和通过性
        multiplier = 2.5
    else:
        multiplier = 2.0
        
    return base_radius * multiplier

17.6.4 滚动窗口配置

局部代价地图的滚动窗口大小影响导航性能:

# 差速机器人
local_costmap:
  width: 3.0    # 窗口宽度
  height: 3.0   # 窗口高度
 
# 阿克曼转向机器人(需要更大的前瞻距离)
local_costmap:
  width: 5.0
  height: 5.0
 
# 高速机器人
local_costmap:
  width: 8.0
  height: 8.0

17.6.5 多图层融合策略

class MasterCostmap:
    """主代价地图:融合各层代价"""
    def __init__(self):
        self.layers = {}
        self.master = None
        
    def add_layer(self, name: str, layer: np.ndarray):
        """添加代价图层"""
        self.layers[name] = layer
        
    def update_master(self) -> np.ndarray:
        """融合所有图层到主代价地图
        
        融合策略:
        1. 取所有层的最大值
        2. 静态层优先级最高(255覆盖一切)
        3. 障碍物层覆盖膨胀层
        """
        if not self.layers:
            return None
            
        # 初始化为所有层中最大的
        self.master = np.zeros_like(
            next(iter(self.layers.values())), dtype=np.uint8
        )
        
        for name, layer in self.layers.items():
            # 逐层取最大值融合
            mask = layer > self.master
            self.master[mask] = layer[mask]
            
        return self.master
    
    def get_cost_at_pose(self, x: float, y: float, 
                          robot_radius: float) -> int:
        """查询机器人占用区域的最高代价"""
        # 检查机器人足迹覆盖的栅格
        gx = int(x / 0.05)
        gy = int(y / 0.05)
        radius_cells = int(robot_radius / 0.05)
        
        max_cost = 0
        for dy in range(-radius_cells, radius_cells + 1):
            for dx in range(-radius_cells, radius_cells + 1):
                if dx*dx + dy*dy <= radius_cells*radius_cells:
                    cx, cy = gx + dx, gy + dy
                    if (0 <= cx < self.master.shape[1] and 
                        0 <= cy < self.master.shape[0]):
                        cost = self.master[cy, cx]
                        max_cost = max(max_cost, cost)
                        
        return max_cost

17.6.6 官方要点——调参纪律与性能预算

The Construct 课程与 Nav2 文档共同总结出三条调参纪律:第一,先保证 TF 链正确再调 costmap——global_frame、robot_base_frame 与 transform_tolerance 配置错误时,层会静默丢弃观测,表现为"代价地图空白但无报错";第二,用 RViz2 的 Costmap 主题着色(map 命名空间下的 occupancy grid + costmap_transparency)可视化代价梯度,逐层关闭插件(enabled: false)定位问题层;第三,监控 map_update_loop 的实际耗时,CPU 预算不足时优先降低体素层分辨率与观测缓存大小(observation_persistence、expected_update_rate),而非简单降低 update_frequency。Nav2 也提供 nav2_costmap_2d 的 Debug 主题(/global_costmap/footprint、/global_costmap/published_layers)辅助在线诊断。

17.7 实战与调试

17.7.1 代价地图监控

# 查看代价地图话题
ros2 topic echo /global_costmap/costmap --once
ros2 topic echo /local_costmap/costmap --once
 
# 查看代价地图更新频率
ros2 topic hz /global_costmap/costmap
ros2 topic hz /local_costmap/costmap
 
# 在RViz2中可视化代价地图
rviz2
# 添加 → Map → Topic: /global_costmap/costmap
# 添加 → Map → Topic: /local_costmap/costmap

17.7.2 代价地图调试工具

import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
import numpy as np
 
class CostmapDebugger(Node):
    """代价地图调试工具"""
    def __init__(self):
        super().__init__('costmap_debugger')
        
        self.global_costmap_sub = self.create_subscription(
            OccupancyGrid, '/global_costmap/costmap', 
            self.global_callback, 10)
        self.local_costmap_sub = self.create_subscription(
            OccupancyGrid, '/local_costmap/costmap', 
            self.local_callback, 10)
            
    def analyze_costmap(self, msg: OccupancyGrid, name: str):
        """分析代价地图统计数据"""
        data = np.array(msg.data).reshape(
            msg.info.height, msg.info.width)
        
        total = data.size
        
        lethal = np.sum(data == 100)
        inscribed = np.sum((data > 0) & (data < 100))
        free = np.sum(data == 0)
        unknown = np.sum(data == -1)
        
        self.get_logger().info(
            f'{name}分析: '
            f'致命={lethal/total*100:.1f}%, '
            f'膨胀={inscribed/total*100:.1f}%, '
            f'空闲={free/total*100:.1f}%, '
            f'未知={unknown/total*100:.1f}%'
        )
        
    def global_callback(self, msg):
        self.analyze_costmap(msg, '全局代价地图')
        
    def local_callback(self, msg):
        self.analyze_costmap(msg, '局部代价地图')

17.7.3 手动清除代价地图

# 清除全局代价地图
ros2 service call /global_costmap/clear_entirely \
  nav2_msgs/srv/ClearEntireCostmap "{}"
 
# 清除局部代价地图
ros2 service call /local_costmap/clear_entirely \
  nav2_msgs/srv/ClearEntireCostmap "{}"
 
# 清除指定区域
ros2 service call /global_costmap/clear_around_local_costmap \
  nav2_msgs/srv/ClearCostmapAroundRobot "{}"

17.7.4 常见问题与解决方案

问题1:代价地图未更新,应检查传感器话题是否发布、TF变换是否正确以及生命周期节点状态。

问题2:路径穿过障碍物,常见原因包括膨胀半径过小、cost_scaling_factor过大(衰减太快)以及障碍物层未正确标记。

问题3:计算性能下降,可通过减小代价地图尺寸、降低更新频率或增大分辨率(如从0.05改为0.1)来改善。

17.7.5 官方要点——足迹、内切半径与"玻璃墙"类问题

针对练习第 6 题的玻璃墙场景,官方文档给出的原则是:costmap 只相信传感器观测与静态地图,传感器看不见的东西必须在静态层或通过其他传感器补齐。Robotics Back-End 的教程演示了同样的思路——把激光扫不到的玻璃墙用超声/毫米波雷达作为第二个 observation source 加入 obstacle_layer(observation_sources 支持逗号分隔多源,各自可配 obstacle_range、raytrace_range 与 marking/clearing 开关);若传感器方案无法解决,则在静态地图里把玻璃墙标注为 lethal 障碍(编辑 pgm 或用 map_server 叠加图层)。另外,footprint 参数的顶点多边形与 robot_radius 圆形足迹二选一,膨胀层用内切半径/外接半径区分"必然碰撞"与"可能碰撞"两档,这是机器人外形不规则时精细调参的依据。

课后练习

  1. 原理题: 解释代价地图中各代价值(0, 1-127, 128-253, 254, 255)的含义,说明膨胀层的代价计算公式。

  2. 配置题: 为一个半径0.3m的差速机器人配置全局和局部代价地图,要求:全局地图分辨率0.05m,膨胀半径0.6m;局部地图为3m滚动窗口,更新频率5Hz。

  3. 编程题: 实现一个简单的代价地图膨胀层,给定障碍物位置和膨胀半径,计算每个栅格的代价值。

  4. 分析题: 比较全局代价地图和局部代价地图在坐标系、更新频率、大小和插件配置上的差异,分析两者配合工作的机制。

  5. 操作题: 通过ROS2命令行工具查看当前代价地图的状态,分析代价分布情况,识别潜在导航问题。

  6. 设计题: 某机器人需要在包含玻璃墙的展览馆导航。玻璃墙对激光雷达透明但物理存在。设计代价地图方案,包括如何标记玻璃墙位置、配置传感器融合和膨胀参数以保证导航安全。


仿真结合实例(当前仓库):观察静态层与障碍物层输入

目标与知识点对应

在 Nav2 仿真中加载预置地图,并让规划器订阅 /map 和 /scan,对应全局代价地图的静态层、障碍物层和膨胀层配置。

运行步骤

source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch navigation_sim_demo_ros2 nav2_demo.launch.py \
  use_gazebo:=true use_rviz:=true gz_headless:=false
ros2 topic info /map
ros2 topic info /scan
ros2 param dump /planner_server

在 RViz 添加 Map、LaserScan 和 Nav2 的 Costmap 显示,移动机器人或改变障碍物后观察局部/全局代价变化。

源码与边界

参数位于 src/navigation_sim_demo_ros2/params/nav2_params.yaml,地图位于 src/navigation_sim_demo_ros2/maps/Software_Museum.yaml,仿真桥配置位于 src/robot_sim_demo/config/gazebo2_bridge.yaml。

代价地图的实际显示以本地 RViz 为准;启动日志不能替代代价层可视化检查。

学习材料: