机器人 / 系统课时
占据栅格如何表示空闲和障碍区域
占据栅格如何表示空闲和障碍区域
激光雷达提供的是距离测量,机器人却需要判断地面上的位置能否通行。本课把连续空间划成二维栅格,说明空闲、占据和未知的概率含义,并通过一个可运行的坐标换算示例,从地图位置准确找到对应的数组元素。
学习目标
- 区分占据概率与未观测状态,正确解释
-1、0和100。 - 读懂 ROS 2 栅格地图的分辨率、原点、尺寸和行优先存储。
- 将世界坐标换算为栅格索引,并检查越界和坐标系错误。
栅格值表达的是占据证据
二维占据栅格把地面切成边长相同的小方格,每格记录“此处被障碍物占据”的估计。ROS 2 使用 nav_msgs/msg/OccupancyGrid 消息承载地图;其 data 是一维 int8 数组,常见地图约定是用 0~100 表示百分比占据概率,-1 表示未知。消息定义允许发布者采用特定编码,因此接入新地图时仍要核对发布节点的约定。
| 存储值 | 常见含义 | 应如何理解 |
|---|---|---|
-1 |
未知 | 尚无足够观测,不能当作空闲 |
0 |
占据概率约 0% | 明显偏向空闲 |
20 |
占据概率约 20% | 较可能空闲 |
50 |
占据概率约 50% | 有数值估计,但无法判断 |
80 |
占据概率约 80% | 较可能有障碍 |
100 |
占据概率约 100% | 明显偏向占据 |
例如一束激光从机器人射向墙面:射线穿过的格子提供空闲证据,墙面命中的终点格提供占据证据,射线以外未扫描到的格子保持未知。多次观测会积累证据;不能把一次命中直接等同于绝对的 100。
分辨率和原点如何定位地图
在 OccupancyGrid 中,info.resolution 的单位是米/格,info.width 与 info.height 分别是横向和纵向格数;info.origin 给出栅格 (0,0) 左下角在地图参考坐标系中的位姿。header.frame_id 常为 map,表示后续坐标必须先换算到该坐标系。
设分辨率为 0.5 m/格、宽为 8 格、高为 6 格,地图覆盖 4 m × 3 m,数组应有 48 个元素。若原点位置为 (-2.0,-1.0) 米且旋转为零,覆盖范围就是 -2.0 ≤ x < 2.0、-1.0 ≤ y < 2.0。

从世界坐标找到数组元素
当地图原点没有旋转时,世界点 (x,y) 转栅格列、行索引的计算是:
其中 r 为分辨率,(x_0,y_0) 为原点位置。索引必须满足 0 ≤ i < width 且 0 ≤ j < height。data 按行优先排列,线性位置为 ;(1,0) 紧跟 (0,0),(0,1) 则从下一个宽度位置开始。
下面是仅依赖 Python 标准库的完整演示,模拟一行中两个空闲格和一个障碍格,不需要启动 ROS 2:
import math
resolution = 0.5
width, height = 8, 6
origin_x, origin_y = -2.0, -1.0
data = [-1] * (width * height)
# 模拟一束激光:先穿过两个空闲格,再命中障碍。
data[2 * width + 0] = 0
data[2 * width + 1] = 0
data[2 * width + 2] = 100
x, y = -0.75, 0.25
# 向下取整,不要用 int() 代替负数位置的 floor()。
i = math.floor((x - origin_x) / resolution)
j = math.floor((y - origin_y) / resolution)
if not (0 <= i < width and 0 <= j < height):
raise ValueError("坐标不在地图范围内")
index = j * width + i
print(f"grid=({i}, {j}), index={index}")
print(f"value={data[index]}")
print(f"row={data[j * width:(j + 1) * width]}")预期输出:
grid=(2, 2), index=18
value=100
row=[0, 0, 100, -1, -1, -1, -1, -1]手算验证:横纵方向都从原点偏移 1.25 m,分别除以 0.5 m/格 得到 2.5,向下取整为 (2,2);线性索引 2 × 8 + 2 = 18。实际订阅 /map 时,把示例尺寸和数组替换为收到的 msg.info 与 msg.data 即可。
读取地图时要防止三类误判
- 坐标系不一致:
base_link或odom中的点不能直接拿来查map中的格子;先通过 TF 变换到header.frame_id所指坐标系。 - 边界与方向错误:本例
x=2.0已在地图右边界之外,不能访问第 8 列;图像数组常从左上角显示第一行,而占据栅格约定(0,0)在左下角,直接按图像行号索引会导致上下颠倒。 - 忽略地图旋转:上述公式仅适用于
info.origin朝向为零的情况;若原点存在非零偏航角,应先使用原点位姿的逆变换将世界点转入地图局部坐标,再执行取整。
处理实际消息前还应核对 len(data) == width * height,并明确导航中的未知区域策略。占据概率描述传感器对障碍的估计,并不直接等于规划器的通行代价;能否通行仍需结合阈值、机器人尺寸及代价地图膨胀设置判断。