4. 代价地图构建：静态地图、动态地图、膨胀层与成本函数
大家好，我是你们的老朋友。今天咱们聊聊代价地图构建。说实话，这玩意儿在避障算法里，就像房子的地基——你上层规划做得再花哨，地图不准、代价算不对，机器人照样撞墙。我当年刚入行时，就吃过这个亏。
4.1 静态地图：先画好“底图”
静态地图，说白了就是环境的本底信息。比如一个工厂车间，墙在哪、柱子在哪、货架在哪，这些不会频繁变动的东西，都归静态地图管。
我习惯用栅格地图（Occupancy Grid Map）来表示。每个格子存一个概率值：0表示空闲，100表示占据，-1表示未知。嗯，这里要注意——未知区域和空闲区域，在代价计算里是完全不同的概念。
核心要点：静态地图是代价地图的“底色”。它通常由SLAM建图得到，加载后基本不变。但别以为它真的“静态”——如果环境有长期变化（比如新加了货架），你得重新建图或手动更新。
举个例子，我做过一个仓储机器人项目。静态地图里有个柱子，但后来仓库改造，柱子拆了。结果机器人每次路过那都绕个大圈，气得我直接手动把地图那块清空了。你想想看，这种坑是不是很常见？
4.2 动态地图：应对“活”的障碍物
动态地图处理的是会动的东西——行人、其他机器人、突然出现的箱子。这些信息必须实时更新，延迟超过100ms就可能出事。
动态地图的数据来源主要有两个：
- 激光雷达/深度相机：实时扫描，生成点云或障碍物列表
- 传感器融合：把多个传感器的检测结果合并，减少误报
我在项目中遇到过一个问题：动态障碍物检测太灵敏，连飘过的塑料袋都当成墙。结果机器人频繁急停，效率极低。后来我加了个“持久性检查”——障碍物必须连续出现3帧以上才标记为有效。这一招，说白了就是“让子弹飞一会儿”。
小技巧：动态地图的更新频率建议在10-20Hz。太快了CPU扛不住，太慢了避障反应迟钝。我一般设15Hz，平衡得不错。
4.3 膨胀层：给障碍物“穿衣服”
膨胀层，就是给障碍物加一圈“安全距离”。为什么需要？因为机器人不是质点，它有尺寸、有转弯半径。你想想看，如果贴着墙规划路径，稍微偏一点就撞上了。
膨胀的算法其实不复杂：
// 伪代码：对每个障碍物栅格，向外膨胀 radius 个格子
for each occupied cell (x, y):
for each neighbor cell (dx, dy) within radius:
if distance(x,y, dx,dy) <= radius:
mark neighbor as inflated
cost = 1.0 - distance / radius // 越靠近障碍物代价越高这里有个关键参数——膨胀半径。设大了，机器人过不了窄门；设小了，容易刮蹭。我建议根据机器人本体尺寸来定：
| 机器人类型 | 推荐膨胀半径 | 说明 |
|---|---|---|
| 小型扫地机 | 0.2 - 0.3 m | 本体小，可以紧贴障碍物 |
| 中型AGV | 0.4 - 0.6 m | 考虑转弯半径和惯性 |
| 大型叉车 | 0.8 - 1.2 m | 安全第一，宁可绕路 |
警告：膨胀层不是越大越好！我曾经在一个项目里把膨胀半径设到1米，结果机器人死活找不到路径穿过一个0.9米宽的通道。后来一查，通道被膨胀层完全堵死了。嗯，这个坑我替你们踩过了。
4.4 成本函数：把“感觉”变成“数字”
成本函数，就是把各种因素量化成一个数值。路径规划算法（比如A*、DWA）根据这个数值找最优路径。
一个典型的成本函数长这样：
total_cost = w1 * obstacle_cost + w2 * path_length_cost + w3 * smoothness_cost其中：
- obstacle_cost：来自膨胀层的代价，越靠近障碍物越高
- path_length_cost：路径长度，鼓励走捷径
- smoothness_cost：路径平滑度，避免急转弯
权重w1、w2、w3怎么调？说实话，没有标准答案。我个人的习惯是：先让避障优先级最高（w1设大），再慢慢加路径长度和平滑度的权重。调参时盯着机器人跑一圈，看它表现再微调。
避坑指南：我曾经把obstacle_cost的权重设得过高，结果机器人为了远离障碍物，在空旷区域走出S形路线。后来我加了个“距离阈值”——超过一定距离后，obstacle_cost不再增加。这样既保证了安全，又不会过度保守。
4.5 知识体系总览
下面这张图，是我自己总结的代价地图构建逻辑。你看一眼，基本就明白各层之间的关系了。
从图上你能看到，静态地图和动态地图是“原料”，膨胀层是“加工”，成本函数是“调味”。最终出来的代价地图，直接喂给路径规划模块。每一步都环环相扣，缺一不可。
好了，这一章的内容就到这。代价地图这东西，说起来简单，做起来全是细节。希望我的经验能帮你少走弯路。下次见！