3. 环境建模基础：栅格地图、几何地图、拓扑地图的构建与选择
做路径规划这么多年，我越来越觉得一个道理：地图选不对，后面全白费。你想想看，算法再牛，如果地图模型本身就不适合你的场景，那跑出来的路径要么算不动，要么根本不能用。
这一章，咱们就来聊聊三种最经典的环境建模方式——栅格地图、几何地图、拓扑地图。我会结合我踩过的坑，告诉你什么时候该用哪种，怎么建，怎么选。
3.1 为什么环境建模是路径规划的第一步？
说白了，路径规划就是回答三个问题：
- 我在哪？
- 我要去哪？
- 怎么走才能不撞墙？
这三个问题，全都依赖一个东西——环境模型。没有地图，机器人就是个瞎子。我见过不少新手，一上来就调A*算法，结果地图都没建明白，路径当然跑飞了。
环境建模的核心任务，就是把真实世界的连续空间，变成计算机能理解的离散表示。嗯，这里要注意：离散化是关键词。真实世界是连续的，但计算机只能处理离散数据。怎么离散？用什么粒度？这就是学问了。
3.2 栅格地图（Occupancy Grid Map）
3.2.1 什么是栅格地图？
栅格地图，也叫占据网格地图。它把空间划分成一个个小格子，每个格子存一个概率值，表示这个格子被障碍物占据的可能性。
举个例子，一个10m×10m的房间，如果栅格分辨率是0.1m，那就有100×100=10000个格子。每个格子存一个0到1之间的数：0表示肯定空，1表示肯定占，0.5表示未知。
核心公式：
栅格地图本质上是一个二维数组：map[i][j] = p(occupied)
其中 i, j 是栅格索引，p 是占据概率。
3.2.2 栅格地图的构建方法
构建栅格地图，最经典的方法是贝叶斯更新。每次传感器扫到某个格子，就更新它的概率。
// 伪代码：栅格地图更新
void updateGrid(GridMap &map, Pose robotPose, LaserScan scan) {
for each laser beam in scan {
// 计算光束穿过的格子
cells = rayCast(robotPose, beam.angle, beam.range);
// 更新每个格子的占据概率
for each cell in cells {
if (cell == beam.endpoint) {
// 光束终点：障碍物
map.logOdds[cell] += log(0.9 / 0.1); // 增加占据概率
} else {
// 光束经过：空闲
map.logOdds[cell] += log(0.1 / 0.9); // 减少占据概率
}
}
}
}我在项目中遇到过一个问题：直接用概率相乘容易数值溢出。后来改用对数几率（log-odds）表示，加法更新，稳定多了。这是个很实用的技巧。
3.2.3 栅格地图的优缺点
| 优点 | 缺点 |
|---|---|
| 实现简单，直观易懂 | 分辨率固定，大场景内存爆炸 |
| 支持多传感器融合 | 路径搜索效率低（格子太多） |
| 天然支持不确定性 | 无法表达语义信息 |
| 适合局部路径规划 | 全局规划时计算量大 |
我的经验：栅格地图最适合室内小场景，比如扫地机器人。分辨率一般设0.05m到0.2m。太细了算不动，太粗了机器人钻不过门缝。
3.3 几何地图（Geometric Map）
3.3.1 什么是几何地图？
几何地图用几何基元来描述环境——点、线、面、多边形。它不关心每个格子，只关心障碍物的轮廓和形状。
最常见的几何地图形式是特征地图，用线段、圆弧、角点等特征来表示环境。比如走廊就是两条平行线，墙角就是一个直角。
3.3.2 几何地图的构建方法
构建几何地图，通常分两步：
- 特征提取：从传感器数据中提取几何特征
- 特征关联：把新特征和已有地图匹配
举个例子，用激光雷达数据提取线段：
// 伪代码：从激光点云提取线段
vector<LineSegment> extractLines(vector<Point> points) {
// 1. 分割：用RANSAC或Split-and-Merge
vector<PointCluster> clusters = splitAndMerge(points, threshold=0.05);
// 2. 拟合：对每个聚类拟合直线
vector<LineSegment> lines;
for each cluster in clusters {
LineSegment line = fitLine(cluster); // 最小二乘法
lines.push_back(line);
}
return lines;
}我曾经在一个工厂项目里用过几何地图。厂房里全是货架和通道，用线段表示走廊边界，路径规划快得飞起。但后来发现一个问题——特征提取不稳定。光线一变，或者传感器抖动，提取的线段就歪了。
3.3.3 几何地图的优缺点
| 优点 | 缺点 |
|---|---|
| 数据量小，存储高效 | 特征提取依赖环境结构 |
| 路径搜索速度快 | 对传感器噪声敏感 |
| 适合结构化环境 | 非结构化环境表现差 |
| 便于做SLAM回环检测 | 难以表达复杂障碍物 |
避坑指南：我曾经在一个堆满杂物的仓库里用几何地图，结果特征提取完全崩溃——到处都是乱七八糟的箱子，根本提不出干净的线段。后来老老实实换回了栅格地图。所以，几何地图只适合结构化环境，比如走廊、办公室、工厂通道。
3.4 拓扑地图（Topological Map）
3.4.1 什么是拓扑地图？
拓扑地图是最抽象的一种。它不关心精确的几何位置，只关心节点和边的关系。节点代表关键位置（比如房间、路口），边代表连通关系（比如走廊、门）。
你想想看，我们人类指路时怎么说？「往前走，第二个路口左转，看到麦当劳就到了。」——这就是拓扑地图的思路。我们不关心精确的米数，只关心拓扑关系。
3.4.2 拓扑地图的构建方法
构建拓扑地图，常见的方法有两种：
- 从栅格地图提取：先建栅格地图，然后用图像细化算法（如Voronoi图）提取骨架，骨架的交点就是节点，骨架的路径就是边。
- 从SLAM轨迹提取：利用机器人走过的轨迹，在关键位置（如转弯、岔路口）创建节点。
// 伪代码：从栅格地图提取拓扑图
TopoMap buildTopoFromGrid(GridMap &grid) {
// 1. 计算Voronoi图（可通行区域的骨架）
VoronoiDiagram voronoi = computeVoronoi(grid);
// 2. 提取节点：Voronoi图的交点
vector<Node> nodes = extractNodes(voronoi);
// 3. 提取边：Voronoi图的路径
vector<Edge> edges = extractEdges(voronoi, nodes);
return TopoMap(nodes, edges);
}3.4.3 拓扑地图的优缺点
| 优点 | 缺点 |
|---|---|
| 数据量极小，适合大场景 | 丢失了精确几何信息 |
| 路径规划速度极快 | 节点提取依赖环境特征 |
| 适合多楼层、园区级导航 | 定位需要语义理解 |
| 人类可理解 | 动态环境维护困难 |
我的建议：如果你做的是园区物流机器人，需要跑几百米甚至几公里，拓扑地图是首选。节点之间用A*搜索，几毫秒就出结果。但要注意，拓扑地图需要配合一个精确的定位系统，不然你都不知道自己在哪个节点上。
3.5 三种地图的选择策略
好了，三种地图都讲完了。你可能会问：到底该用哪种？
我的答案是：没有银弹，只有权衡。实际项目中，我经常是混合使用。
举个例子，我做过一个商场导购机器人：
- 全局规划用拓扑地图：商场有5层，每层有几十个店铺。拓扑节点就是每个店铺门口，边就是走廊。规划从A店到B店的路径，毫秒级搞定。
- 局部规划用栅格地图：到了当前楼层，用0.1m分辨率的栅格地图做避障。因为商场里人走来走去，栅格地图更新快，能实时避让。
- 定位辅助用几何地图：提取走廊的墙线和柱子，做辅助定位。万一激光雷达被遮挡，还能靠几何特征找回位置。
选择决策树：
- 场景小（<100m²）、障碍物复杂 → 栅格地图
- 场景大（>1000m²）、结构规则 → 拓扑地图
- 场景中等、结构清晰 → 几何地图
- 大多数实际项目 → 混合地图
3.6 本章小结
这一章我们聊了三种环境建模方式：
- 栅格地图：最直观，适合小场景和局部规划
- 几何地图：最紧凑，适合结构化环境
- 拓扑地图：最抽象，适合大场景和全局规划
我个人习惯是：先想清楚场景，再选地图类型。别一上来就写代码，花10分钟想清楚地图方案，能省你后面10小时的调试时间。
下一章，我们会深入讲路径搜索算法——有了地图，怎么找到最优路径？到时候你会发现，地图选得好，算法跑得顺。选得不好，再牛的算法也救不了你。
嗯，今天就到这儿。有问题欢迎交流。