4. 环境表示方法：栅格地图、代价地图、八叉树地图
做避障算法，第一步不是写代码，而是想清楚一个问题：机器人怎么“看”这个世界？
你想想看，机器人没有眼睛，它只有传感器——激光雷达、超声波、深度相机。这些传感器吐出来的是一堆离散的点云或者距离数据。怎么把这些原始数据变成机器人能理解的环境模型？这就是环境表示方法要干的事。
我个人习惯把环境表示分成三个层次：栅格地图是基础，代价地图是升级版，八叉树地图是三维场景的利器。今天咱们一个一个聊透。
核心观点：没有最好的地图，只有最合适的地图。选哪种表示方法，取决于你的传感器、算力、以及机器人是在室内跑还是在野外飞。
4.1 栅格地图：最朴素的“格子世界”
栅格地图，说白了就是把环境切成一个个小格子。每个格子要么是“空的”，要么是“被占的”，要么是“未知的”。
我刚开始做机器人那会儿，觉得这玩意儿太简单了——不就是个二维数组嘛。但真正上手才发现，怎么处理传感器噪声才是关键。
举个例子，激光雷达扫到一堵墙，返回的距离是2.3米。但传感器有误差啊，可能实际是2.31米。你直接把这个点对应的格子标为“占用”，那下一帧数据来了，同一个格子又变成“空闲”，机器人就懵了。
所以栅格地图的核心是概率更新。每个格子存一个概率值，0表示绝对空闲，1表示绝对占用，0.5表示未知。每次传感器读数来了，用贝叶斯公式更新这个概率。
我的经验：初始化时把所有格子设为0.5（未知）。激光打到的格子概率增加，激光穿过的格子概率减少。这样即使传感器有噪声，多帧数据叠加后，概率会收敛到正确值。
代码实现其实不复杂，核心就两步：
// 伪代码：栅格地图更新
void updateGrid(GridMap& map, float range, float angle, float robot_x, float robot_y) {
// 1. 计算激光终点坐标
float end_x = robot_x + range * cos(angle);
float end_y = robot_y + range * sin(angle);
// 2. 更新终点格子：增加占用概率
int grid_x = (int)(end_x / map.resolution);
int grid_y = (int)(end_y / map.resolution);
map.cells[grid_x][grid_y].prob += 0.3; // 占用增量
// 3. 更新射线经过的格子：减少占用概率
// 用Bresenham画线算法遍历路径上的格子
for (cell in bresenhamLine(robot_x, robot_y, end_x, end_y)) {
if (cell != end_cell) {
cell.prob -= 0.1; // 空闲减量
}
}
// 4. 概率裁剪到[0, 1]
clampProbability(map);
}注意：分辨率的选择很讲究。分辨率太高（比如1cm），内存爆炸；分辨率太低（比如1m），机器人钻不过门缝。室内机器人我一般用5cm，室外用20cm。
4.2 代价地图：给障碍物“加个缓冲”
栅格地图有个问题——它只告诉你“这里有没有障碍物”，但没告诉你“离障碍物多远才安全”。
代价地图就是在栅格地图的基础上，给每个格子算一个代价。障碍物附近的格子代价高，远处的代价低。这样路径规划算法就会自动远离障碍物，而不是贴着墙走。
我记得有一次做AGV小车，用的纯栅格地图做A*规划。结果小车每次转弯都蹭到货架，因为路径规划出来的轨迹离障碍物只有1个格子的距离。后来换成代价地图，把障碍物周围3个格子都设成高代价，问题就解决了。
代价地图通常分三层：
- 静态层：来自预先建好的地图，比如墙壁、柱子
- 障碍物层：来自实时传感器，比如动态的人、临时放的箱子
- 膨胀层：把障碍物往外“吹”一圈，代价从中心向外递减
膨胀层的代价计算，我常用指数衰减：
// 代价计算：距离障碍物越近，代价越高
float computeCost(float distance_to_obstacle, float inflation_radius) {
if (distance_to_obstacle <= 0.0f) {
return 255; // 障碍物本身，最高代价
}
if (distance_to_obstacle >= inflation_radius) {
return 0; // 安全区域，零代价
}
// 指数衰减
float cost = 255.0f * exp(-3.0f * distance_to_obstacle / inflation_radius);
return cost;
}避坑指南：我曾经把膨胀半径设得太大（0.5米），结果机器人过不了窄门。后来改成动态膨胀——在狭窄通道自动缩小膨胀半径。这个技巧在ROS的costmap_2d里就有现成实现，叫“footprint inflation”。
4.3 八叉树地图：三维空间的“智能压缩”
前面两种地图都是二维的。但无人机、机械臂这些应用，需要在三维空间里避障。你总不能把整个三维空间切成均匀的小立方体吧？那内存会爆炸的。
八叉树地图（OctoMap）就是来解决这个问题的。它的思路很聪明：把空间递归地分成8个立方体。如果一个立方体里全是空的，就不继续分了；如果里面有障碍物，就继续细分。
你想想看，一个空旷的大房间，用八叉树表示可能只需要几百个节点。如果用均匀体素网格，可能要几百万个。这就是八叉树的威力。
OctoMap的更新逻辑和栅格地图类似，也是概率更新。但它的数据结构是树，所以插入和查询都是O(log n)的复杂度。
// OctoMap节点结构（简化版）
struct OctreeNode {
float probability; // 占用概率
bool is_leaf; // 是否是叶子节点
OctreeNode* children[8]; // 8个子节点
// 更新概率
void update(float measurement) {
// 使用log-odds形式更新
float log_odds = log(probability / (1 - probability));
log_odds += measurement;
probability = 1.0f / (1.0f + exp(-log_odds));
// 如果概率接近0或1，且节点足够小，可以剪枝
if (is_leaf && (probability < 0.01 || probability > 0.99)) {
// 可以合并子节点，节省内存
}
}
};关键点：八叉树地图的“剪枝”机制是它内存高效的核心。当一个节点的所有子节点概率都差不多时，就把子节点删掉，只保留父节点。这样空旷区域只占很少的内存。
我在做无人机室内避障时用过OctoMap。当时遇到一个坑：无人机飞太快，八叉树更新跟不上。后来发现是分辨率设得太细（0.02米）。改成0.05米后，更新速度从50ms降到了8ms，完全够用。
三种地图怎么选？
我列个表，方便你对比：
| 特性 | 栅格地图 | 代价地图 | 八叉树地图 |
|---|---|---|---|
| 维度 | 2D | 2D | 3D |
| 内存占用 | 中等（固定大小） | 中等（固定大小） | 低（自适应） |
| 更新速度 | 快 | 中等（需膨胀计算） | 中等（树操作） |
| 适用场景 | 室内导航、扫地机 | 动态避障、路径规划 | 无人机、机械臂 |
| 典型库 | ROS map_server | ROS costmap_2d | OctoMap库 |
嗯，总结一下我的个人建议：
- 做扫地机器人这种平面移动的，栅格地图就够了
- 做AGV、服务机器人需要动态避障的，上代价地图
- 做无人机、机械臂要在三维空间里闪转腾挪的，八叉树地图是首选
环境表示方法就聊到这儿。记住一句话：地图是算法的基石。地图选错了，后面的路径规划、避障策略再牛也白搭。