3、环境建模：栅格地图、八叉树地图、拓扑地图、几何地图、代价地图的构建与转换

环境建模是航迹规划从“感知数据”走向“可计算空间”的关键桥梁。无论无人机、地面机器人还是自动驾驶车辆，其规划算法都需要一个结构化的空间表达来执行碰撞检测、路径搜索与运动约束校验。本章将系统讲解五种主流地图模型——栅格地图、八叉树地图、拓扑地图、几何地图与代价地图——的数学本质、构建方法、适用场景及相互转换策略，帮助学习者在实际工程中做出合理的地图选型与融合设计。

3.1 栅格地图：占据概率与分辨率权衡

核心原理：栅格地图（Occupancy Grid Map）将连续空间离散化为均匀的二维或三维网格单元，每个单元存储一个占据概率值（0~1）。其数学基础是贝叶斯滤波——通过激光雷达或深度相机的测量模型，不断更新每个栅格的占据概率。栅格地图的构建公式为：

P(m_i | z_{1:t}, x_{1:t}) = 1 - 1 / (1 + exp(l_i))

其中 l_i 为栅格 i 的对数几率（log-odds），通过累加测量更新实现增量式建图。栅格地图的核心优势在于实现简单、碰撞检测直接（查表即可），且天然支持多传感器融合。

判断方法与操作流程：选择栅格地图时需评估三个关键参数：分辨率、地图尺寸和更新频率。分辨率决定精度与内存的平衡——0.05m分辨率下，100m×100m区域需要400万个栅格，内存约16MB（浮点存储）。构建流程包括：① 传感器数据预处理（去畸变、时间同步）；② 占据概率更新（射线投射或体素映射）；③ 地图后处理（膨胀、滤波、平滑）。

案例拆解：某仓储AGV项目采用2D激光雷达构建栅格地图，初始分辨率设为0.1m，建图后发现货架腿（直径0.08m）无法被可靠检测。将分辨率提升至0.05m后，内存占用从8MB增至32MB，但检测成功率从82%提升至97%。代价是建图时间从3分钟延长至11分钟。最终方案采用双层栅格：全局地图0.1m用于路径搜索，局部地图0.05m用于实时避障。

3.2 八叉树地图：多分辨率空间表达

核心原理：八叉树地图（OctoMap）采用递归八分策略，将三维空间划分为八个子立方体，每个节点存储占据状态。当子节点状态一致时，可合并为父节点，从而自适应地表达不同区域的细节程度。其核心优势是内存效率极高——对于大范围稀疏环境，八叉树比均匀栅格节省90%以上内存。八叉树支持概率更新，每个节点存储占据概率的对数几率，并支持多分辨率查询。

构建流程与操作要点：八叉树建图的关键参数包括：最大深度（决定最小体素尺寸）、概率阈值（决定节点是否合并）和更新策略（插入/删除）。典型构建流程为：

1. 将点云数据按传感器位姿变换到世界坐标系；
2. 对每个点执行射线投射，更新路径上所有节点的占据概率；
3. 递归检查子节点状态，若一致则剪枝合并；
4. 定期执行地图压缩与内存回收。

实战场景：某无人机电力巡检项目需构建1km×1km×200m的三维环境地图。若使用0.1m均匀栅格，需要2亿个栅格，内存超800MB；改用八叉树（最大深度16层，最小体素0.1m）后，内存仅需120MB，且支持实时更新。在树冠密集区域自动细分至0.1m精度，在空旷区域合并为1.6m体素，实现了精度与效率的平衡。

3.3 拓扑地图与几何地图：从精确到抽象

核心原理：拓扑地图将环境抽象为节点（关键位置）和边（可通行路径），强调连通性而非精确几何。其构建通常基于栅格或几何地图的骨架化（如Voronoi图）或人工标注。几何地图则保留环境的精确几何信息，包括点云、多边形边界、参数化曲面等，是CAD模型和SLAM点云地图的直接表达。

对比分析：两种地图在航迹规划中扮演不同角色——拓扑地图适合全局路径规划（如A*在拓扑图上搜索），计算效率高但缺乏局部细节；几何地图适合精确碰撞检测和运动规划，但计算量大。实际工程中常将两者结合：拓扑地图提供全局引导，几何地图负责局部验证。

案例拆解：某自动驾驶园区接驳车项目，使用高精几何地图（厘米级点云）进行精确停靠，同时从几何地图自动提取车道级拓扑网络（节点为路口，边为车道中心线）。全局规划在拓扑图上运行，耗时<10ms；局部规划在几何地图上运行，耗时50ms。当遇到临时施工区域时，仅更新几何地图中的障碍物，拓扑网络保持不变，实现了快速响应。

3.4 代价地图：多层融合与实时更新

核心原理：代价地图（Costmap）是导航系统中用于实时避障和路径优化的核心数据结构。它由多个代价层（Costmap Layer）叠加而成，每层代表一种风险或约束，最终合并为总代价。典型层次包括：静态障碍层（来自栅格/八叉树）、障碍物膨胀层（考虑机器人半径）、动态障碍层（实时传感器数据）、危险区域层（如靠近悬崖）和摩擦/粗糙度层（地面条件）。

构建与更新流程：代价地图的构建遵循“订阅-融合-发布”模式：

1. 订阅传感器数据（激光/深度/超声）
2. 各层独立更新（膨胀、滤波、衰减）
3. 加权叠加生成主代价图
4. 发布供规划器查询
5. 按需执行代价重置（如障碍物消失）

实战场景：某餐厅配送机器人使用代价地图实现动态避障。初始静态代价层来自预先构建的栅格地图，膨胀半径设为0.3m（机器人半径0.25m+安全余量0.05m）。实时动态层以10Hz更新，检测到行人时在行人周围生成高斯型代价峰值。当行人移动时，代价峰值以0.5m/s的速度衰减，避免路径抖动。测试表明，加入代价地图后，机器人急停次数从平均8次/小时降至1次/小时。

3.5 地图转换策略与工程实践

核心原理：不同地图模型各有优劣，实际系统往往需要多地图协同。转换策略的核心是保持信息无损或可控损失，同时满足不同规划阶段的需求。典型转换链路为：传感器数据 → 几何地图（精确） → 栅格/八叉树（离散化） → 代价地图（实时避障） → 拓扑地图（全局引导）。

转换方法与注意事项：

• 几何→栅格：通过射线投射或体素化，注意分辨率选择与信息损失评估；
• 栅格→八叉树：递归合并相同状态区域，需设定概率阈值（如0.7以上为占据，0.3以下为自由）；
• 栅格→拓扑：使用Voronoi图或广义Voronoi图（GVD）提取骨架，再简化节点；
• 多地图→代价：各层独立维护，通过加权求和或最大值融合。

案例拆解：某大型仓库（200m×150m）部署多台AGV，采用“四层地图架构”：① 离线构建的高精度几何地图（点云，用于精确定位）；② 八叉树地图（用于三维碰撞检测，考虑货架高度）；③ 2D代价地图（用于实时路径规划，膨胀半径0.4m）；④ 拓扑地图（用于多AGV交通管理，节点为交叉口）。系统运行中，当某区域货架变动时，仅更新八叉树和代价地图，拓扑网络保持不变，重规划时间从秒级降至毫秒级。