机器人 / 系统课时

占据栅格如何表示空闲和障碍区域

占据栅格如何表示空闲和障碍区域

激光雷达提供的是距离测量,机器人却需要判断地面上的位置能否通行。本课把连续空间划成二维栅格,说明空闲、占据和未知的概率含义,并通过一个可运行的坐标换算示例,从地图位置准确找到对应的数组元素。

学习目标

  • 区分占据概率与未观测状态,正确解释 -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) 转栅格列、行索引的计算是:

i=⌊x−x0r⌋,j=⌊y−y0r⌋i=\left\lfloor\frac{x-x_0}{r}\right\rfloor,\qquad j=\left\lfloor\frac{y-y_0}{r}\right\rfloor

其中 r 为分辨率,(x_0,y_0) 为原点位置。索引必须满足 0 ≤ i < width 且 0 ≤ j < height。data 按行优先排列,线性位置为 k=jW+ik=jW+i;(1,0) 紧跟 (0,0),(0,1) 则从下一个宽度位置开始。

下面是仅依赖 Python 标准库的完整演示,模拟一行中两个空闲格和一个障碍格,不需要启动 ROS 2:

python
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]}")

预期输出:

text
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,并明确导航中的未知区域策略。占据概率描述传感器对障碍的估计,并不直接等于规划器的通行代价;能否通行仍需结合阈值、机器人尺寸及代价地图膨胀设置判断。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

占据栅格如何表示空闲和障碍区域

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通