第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[:] = 25517.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.5517.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 response17.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.517.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] = 25417.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.017.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 * multiplier17.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.017.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_cost17.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/costmap17.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 圆形足迹二选一,膨胀层用内切半径/外接半径区分"必然碰撞"与"可能碰撞"两档,这是机器人外形不规则时精细调参的依据。
课后练习
-
原理题: 解释代价地图中各代价值(0, 1-127, 128-253, 254, 255)的含义,说明膨胀层的代价计算公式。
-
配置题: 为一个半径0.3m的差速机器人配置全局和局部代价地图,要求:全局地图分辨率0.05m,膨胀半径0.6m;局部地图为3m滚动窗口,更新频率5Hz。
-
编程题: 实现一个简单的代价地图膨胀层,给定障碍物位置和膨胀半径,计算每个栅格的代价值。
-
分析题: 比较全局代价地图和局部代价地图在坐标系、更新频率、大小和插件配置上的差异,分析两者配合工作的机制。
-
操作题: 通过ROS2命令行工具查看当前代价地图的状态,分析代价分布情况,识别潜在导航问题。
-
设计题: 某机器人需要在包含玻璃墙的展览馆导航。玻璃墙对激光雷达透明但物理存在。设计代价地图方案,包括如何标记玻璃墙位置、配置传感器融合和膨胀参数以保证导航安全。
仿真结合实例(当前仓库):观察静态层与障碍物层输入
目标与知识点对应
在 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:=falseros2 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 为准;启动日志不能替代代价层可视化检查。
学习材料:
- Nav2 官方文档 —— Costmap2D 与 Inflation Layer:https://docs.nav2.org/
- docs.ros.org —— nav2_costmap_2d 包文档:https://docs.ros.org/en/jazzy/p/nav2-costmap-2d/
- ROS Wiki —— costmap_2d(ROS 1 语义参考):https://wiki.ros.org/costmap_2d
- The Construct —— ROS 2 Navigation 代价地图课程:https://www.theconstructsim.com/
- Robotics Back-End —— Costmap 调参与传感器融合教程:https://roboticsbackend.com/
RuyiSDK Board Docs