1. 为什么机器人导航需要'膨胀'地图?
想象一下,你开着一辆小轿车在狭窄的巷子里穿行。你心里很清楚,你不能紧贴着墙壁开,必须和墙壁保持一定的安全距离,否则后视镜或者车身很容易刮蹭。对于机器人来说,这个道理完全一样。
在 ROS 的导航框架里,机器人通过激光雷达、深度相机等传感器感知世界,生成一张代价地图(Costmap)。这张地图把环境划分成一个个小格子(栅格),每个格子都有一个'代价'值,用来表示机器人到达这个位置的'风险'或'难度'。障碍物所在的格子代价最高(比如 254),完全自由的区域代价最低(0)。
但问题来了:如果机器人只是一个没有体积的'点',那么它贴着障碍物走是没问题的。可现实是,机器人有实实在在的'身体'(我们称之为Footprint,即机器人的垂直投影轮廓)。如果只把障碍物本身标记为高风险,机器人规划路径时,它的'中心点'可能会紧贴着障碍物划过,而它的'身体'早就撞上去了。
这就是 Costmap2d 膨胀层(InflationLayer) 要解决的核心问题:为障碍物创建一个'安全缓冲区'。它通过算法,让障碍物周围一定距离内的栅格,也产生从高到低递减的代价。这样,机器人在路径规划时,就会本能地'嫌弃'这些靠近障碍物的区域,从而规划出一条不仅不撞上、而且保持安全距离的路线。
我刚开始做机器人项目时就踩过坑,没调好膨胀参数,机器人总是'擦着'桌腿过去,把桌腿上的漆都刮花了。后来明白了,膨胀层不是可选项,而是保障机器人物理安全的必选项。
2. 膨胀层的核心:两张神奇的预计算表
膨胀算法听起来要计算每个栅格到所有障碍物的距离,那计算量岂不是爆炸?ROS 的开发者们用了一个非常巧妙的优化:预计算查找表(Lookup Table)。这是理解膨胀层效率的关键。
膨胀层在初始化时,会根据你设置的 inflation_radius(膨胀半径)和 cost_scaling_factor(代价衰减因子),预先算好两张表:距离表(cached_distances_) 和 代价表(cached_costs_)。
2.1 距离表:快速计算'栅格距离'
距离表是一个二维数组。它的下标 [dx][dy] 表示两个栅格在 x 和 y 方向上的绝对坐标差。表里存储的值就是这两个栅格之间的欧几里得距离(以栅格为单位)。
举个例子,假设一个栅格坐标是 (2,2),另一个是 (5,5)。那么 dx = |5-2| = 3,dy = |5-2| = 3。直接查表 cached_distances_[3][3],就能立刻得到距离 sqrt(3² + 3²) ≈ 4.24,而不需要每次都做开方运算。
这种设计利用了障碍物膨胀的对称性。计算任意栅格到障碍物的距离时,只需要计算坐标差,然后查表即可,速度极快。
// 实际代码中的查表函数大致长这样
inline double distanceLookup(int mx, int my, int src_x, int src_y) {
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_distances_[dx][dy]; // 直接返回预计算好的距离
}

