第2章：环境建模与地图表示

各位同学，欢迎来到第二章。

上一章我们聊了路径规划的整体框架。今天要讲的，是规划算法赖以生存的「土壤」——地图。

你想想看，机器人要规划路径，总得知道周围长什么样吧？

这就像你要从北京去上海，手里得有张地图。没有地图，再厉害的导航算法也是白搭。

我个人习惯把地图表示分为三类：栅格地图、八叉树地图、代价地图。咱们一个一个来拆解。

2.1 栅格地图：最经典的地图表示

栅格地图，说白了就是把环境切成一个个小格子。

每个格子要么是障碍物，要么是自由空间。简单粗暴，但非常有效。

我在项目中遇到过一个问题：栅格大小怎么选？

选大了，地图模糊，路径不精确。选小了，计算量爆炸。

一般建议栅格大小取机器人直径的1/3到1/2。比如机器人直径0.5米，栅格设0.2米左右比较合适。

来看一个简单的栅格地图生成代码：

# 创建一个10x10的栅格地图
import numpy as np

grid_map = np.zeros((10, 10), dtype=int)
# 设置障碍物
grid_map[3:6, 4:7] = 1
grid_map[2, 2] = 1

print("栅格地图：")
print(grid_map)

嗯，这里要注意：栅格地图的索引顺序是[行][列]，对应现实世界的[y][x]坐标。我刚开始做的时候搞反过，结果机器人一直往墙上撞。

2.2 八叉树地图：三维空间的利器

二维环境用栅格地图就够了。但如果你做的是无人机、机械臂，或者任何需要三维感知的场景，就得请出八叉树地图了。

八叉树地图的核心思想是什么？

递归分割。一个立方体，如果内部有障碍物，就切成8个小立方体。小立方体还有障碍？继续切。直到达到最小分辨率。

八叉树地图的深度决定了分辨率。深度每增加1，分辨率翻倍，但节点数增长8倍。一般深度设10-12层就够了。

深度	分辨率（1米空间）	最大节点数
8	0.39 cm	~1.6万
10	0.098 cm	~10万
12	0.024 cm	~68万

为什么会这样？因为八叉树只在有障碍的地方细分。空旷区域一个节点就搞定了。

2.3 代价地图：让路径规划更聪明

栅格地图和八叉树地图都是「非黑即白」的。但现实世界哪有这么简单？

靠近墙壁的地方，机器人应该离远一点。有人的区域，最好绕开。这就是代价地图的用武之地。

代价地图本质上是在栅格地图的基础上，给每个格子加了一个「代价」值。

• 代价0：完全自由
• 代价255：完全障碍
• 中间值：越靠近障碍物代价越高

我曾经在项目中犯过一个错误：直接把障碍物膨胀一圈当代价地图用。结果机器人遇到狭窄通道直接放弃，因为所有路径都被高代价覆盖了。

代价地图的生成算法其实很简单：

def generate_cost_map(grid_map, inflation_radius):
    cost_map = grid_map.copy() * 255
    # 对每个障碍物进行膨胀
    obstacles = np.argwhere(grid_map == 1)
    for obs in obstacles:
        for dx in range(-inflation_radius, inflation_radius+1):
            for dy in range(-inflation_radius, inflation_radius+1):
                dist = np.sqrt(dx**2 + dy**2)
                if dist <= inflation_radius:
                    x, y = obs[0]+dx, obs[1]+dy
                    if 0 <= x < cost_map.shape[0] and 0 <= y < cost_map.shape[1]:
                        cost = int(255 * (1 - dist/inflation_radius))
                        cost_map[x, y] = max(cost_map[x, y], cost)
    return cost_map

这段代码虽然简单，但实际项目中很少这么写。因为效率太低。一般用距离变换算法，一次遍历就能搞定。

2.4 ROS中的map_server：地图的读写与发布

好了，地图建好了，怎么用？

ROS里有个现成的工具——map_server。它负责把地图文件加载成ROS话题，供其他节点使用。

map_server支持两种地图格式：

• PGM（便携式灰度图）：图像格式，直观
• YAML（配置文件）：描述地图参数

一个典型的YAML文件长这样：

image: my_map.pgm
resolution: 0.05
origin: [-10.0, -10.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

我来解释一下这些参数：

• image：地图图片路径
• resolution：每个像素代表的实际距离（米）
• origin：地图左下角在世界坐标系中的位置
• occupied_thresh：像素值大于此阈值视为障碍
• free_thresh：像素值小于此阈值视为自由

启动map_server的命令很简单：

rosrun map_server map_server my_map.yaml

启动后，你会看到两个话题：

• /map：发布nav_msgs/OccupancyGrid消息
• /map_metadata：发布地图元数据

嗯，这里要注意：map_server默认只发布一次地图。如果你需要动态更新地图，得用map_server的另一种模式——map_saver。

map_saver可以把当前的地图保存成文件：

rosrun map_server map_saver -f my_map

这会生成my_map.pgm和my_map.yaml两个文件。

2.5 三种地图的选型建议

讲了这么多，到底该用哪种？

我根据项目经验总结了一个简单的选型表：

场景	推荐地图类型	理由
室内扫地机器人	栅格地图	二维环境，简单高效
无人机导航	八叉树地图	需要三维信息，内存友好
动态环境（有人走动）	代价地图	可以实时更新代价，避开动态障碍
仓储机器人	栅格地图+代价地图	静态地图用栅格，动态避障用代价

最后说一句：地图只是工具，不是目的。

你的目标是让机器人安全、高效地到达目的地。选哪种地图，取决于你的具体场景和计算资源。

下一章我们会讲A*算法，到时候你会看到地图表示对算法效率的影响有多大。

好了，今天就到这里。有问题欢迎在课程群里讨论。