3. 环境建模：栅格地图、ESDF、Voxel Map，动态障碍物表示，语义地图融合
环境建模，说白了就是让机器人知道「我在哪，周围有什么」。
我刚开始做机器人那会儿，觉得SLAM建个图就完事了。后来做运动规划才发现——地图怎么存、怎么查、怎么更新，直接决定了你的规划器能不能跑起来。今天咱们就把这几种主流的地图表示方式掰开揉碎了讲。
3.1 栅格地图（Occupancy Grid Map）
这是最基础、最直观的表示方式。把空间切成一个个小格子，每个格子存一个概率值——0表示空闲，1表示占据，0.5表示未知。
核心数据结构
// 一个简单的栅格地图实现
struct GridMap {
double resolution; // 分辨率，比如0.05m
int width, height; // 栅格数量
double origin_x, origin_y; // 地图原点（世界坐标系）
std::vector<double> data; // 存储占据概率
};
// 查询某个世界坐标对应的栅格值
double getOccupancy(GridMap& map, double wx, double wy) {
int ix = (int)((wx - map.origin_x) / map.resolution);
int iy = (int)((wy - map.origin_y) / map.resolution);
if (ix < 0 || ix >= map.width || iy < 0 || iy >= map.height)
return -1.0; // 未知区域
return map.data[iy * map.width + ix];
}我踩过的坑
分辨率的选择是个大问题。我早期做过一个扫地机器人项目，为了省内存把分辨率设成0.2m。结果呢？椅子腿之间的空隙全被忽略了，机器人老往缝里钻，卡得死死的。后来改成0.05m，内存翻了好几倍，但规划出来的路径终于能走了。
3.2 ESDF（欧几里得符号距离场）
栅格地图有个硬伤——它只告诉你「这格有没有东西」，但没告诉你「离障碍物有多远」。规划路径时，我们需要知道机器人离墙还有多远，才能安全通过。
ESDF就是干这个的。每个栅格存一个距离值：正数表示到最近障碍物的距离，负数表示在障碍物内部。
为什么ESDF对规划这么重要？
- 梯度信息：可以计算障碍物的方向，引导路径平滑绕开
- 碰撞检测：直接查距离值，不用遍历所有障碍物
- 优化目标：把「远离障碍物」变成数学上的代价函数
快速构建ESDF
// 从栅格地图构建ESDF（简化版）
void buildESDF(const GridMap& occupancy, ESDFMap& esdf) {
// 第一步：初始化，障碍物内设为0，空闲设为无穷大
for (int i = 0; i < esdf.size(); i++) {
if (occupancy.data[i] > 0.5) // 障碍物
esdf.data[i] = 0.0;
else
esdf.data[i] = INFINITY;
}
// 第二步：双向距离传播（这里用简单的BFS示意）
// 实际工程中会用更高效的算法，比如Fast Marching Method
std::queue<int> q;
for (int i = 0; i < esdf.size(); i++) {
if (esdf.data[i] == 0.0) q.push(i);
}
while (!q.empty()) {
int idx = q.front(); q.pop();
// 检查四个邻居
for each neighbor n of idx {
double new_dist = esdf.data[idx] + resolution;
if (new_dist < esdf.data[n]) {
esdf.data[n] = new_dist;
q.push(n);
}
}
}
}3.3 Voxel Map（体素地图）
二维栅格不够用怎么办？无人机、机械臂这些需要在三维空间里规划的，就得用体素地图了。
体素就是三维版的像素。每个小立方体存占据信息。但三维地图有个大问题——内存爆炸。一个100m×100m×10m的空间，0.1m分辨率，就是1亿个体素。每个体素存一个float，400MB就没了。
常用的优化方案
| 方案 | 原理 | 适用场景 |
|---|---|---|
| 八叉树（OctoMap） | 递归分割，空白区域用大节点表示 | 静态环境，内存敏感 |
| 哈希表（Voxel Hashing） | 只存储被观测到的体素 | 动态环境，实时重建 |
| TSDF（截断符号距离场） | 只存储表面附近的距离值 | RGB-D重建，精度要求高 |
我个人比较喜欢用八叉树。为什么呢？因为大部分空间其实是空的。一个房间，墙和家具只占不到10%的体积。八叉树天然就能压缩这些空白区域，内存能省90%以上。
3.4 动态障碍物表示
静态地图好办，建一次用一年。但现实世界哪有那么安静？行人、车辆、其他机器人，这些都是会动的。
动态障碍物的几种表示方式
- 速度障碍物（VO）：把动态障碍物映射到速度空间，判断哪些速度会导致碰撞。常用于多机器人避碰。
- 时空障碍物：在时间维度上扩展障碍物。比如一个行人以1m/s的速度走过，那他在未来2秒内会占据一条「时空走廊」。
- 概率预测：用卡尔曼滤波或粒子滤波预测障碍物未来位置，存成概率分布。
我实际项目中的做法
// 动态障碍物结构体
struct DynamicObstacle {
int id; // 唯一标识
Vector3 position; // 当前位置
Vector3 velocity; // 当前速度
double radius; // 包围球半径
double confidence; // 跟踪置信度
std::deque<Vector3> history; // 历史轨迹（用于预测）
// 预测未来t秒后的位置
Vector3 predict(double dt) {
// 简单线性预测，实际可以用更复杂的模型
return position + velocity * dt;
}
};3.5 语义地图融合
几何信息告诉你「这里有东西」，语义信息告诉你「这是什么东西」。把两者结合起来，就是语义地图。
为什么要做语义融合？举个例子：
- 一个「门」和一个「墙」，在几何上都是障碍物。但规划时，门是可以打开的，墙不行。
- 一个「桌子」和一个「椅子」，桌子下面可以钻过去，椅子下面不行（太矮）。
- 「人行道」和「机动车道」，机器人应该走哪条？
语义地图的存储结构
// 语义体素
struct SemanticVoxel {
uint8_t occupancy; // 占据概率 0-255
uint8_t semantic_class; // 语义类别：0=未知, 1=地面, 2=墙壁, 3=门, 4=桌子...
uint8_t traversability; // 可通行性：0=不可通行, 1=谨慎通行, 2=自由通行
uint16_t instance_id; // 实例ID，区分不同的门、不同的桌子
};
// 语义地图查询
Traversability getTraversability(SemanticMap& map, Vector3 pos) {
SemanticVoxel voxel = map.query(pos);
if (voxel.occupancy > 200) return BLOCKED; // 高概率占据
if (voxel.semantic_class == DOOR && isDoorOpen(voxel.instance_id))
return FREE; // 门是开的，可以走
return static_cast<Traversability>(voxel.traversability);
}融合的难点
语义融合最大的坑在于——语义标签会出错。我做过一个项目，视觉模型把「玻璃门」识别成了「墙壁」，结果机器人对着玻璃门一头撞上去。还好速度慢，不然就出事故了。
后来我加了一个规则：如果语义标签和几何信息冲突（比如视觉说是墙，但激光雷达打过去是空的），就降低这个语义标签的置信度，优先相信几何数据。
3.6 几种地图的选型指南
| 场景 | 推荐方案 | 理由 |
|---|---|---|
| 室内扫地机器人 | 栅格地图 + ESDF | 二维够用，ESDF保证安全距离 |
| 无人机导航 | 八叉树 + TSDF | 三维空间，内存敏感 |
| 仓储AGV | 栅格地图 + 语义层 | 环境结构化，语义信息丰富 |
| 人机协作 | 体素哈希 + 动态障碍物 | 需要快速更新，处理移动的人 |
嗯，环境建模这块就聊到这儿。说白了，没有最好的地图，只有最合适的。你想想看，你的机器人要在什么环境下跑？需要多高的精度？内存和算力有多少？想清楚这些，选型就简单了。
下一章咱们聊聊路径搜索——有了地图，怎么找到一条好路？