机器人 / 系统课时
用占据栅格表达未知与空闲区域
用占据栅格表达未知与空闲区域
机器人不能把没有探测过的地方直接当成空地。本课使用二维占据栅格表示环境,区分未知、空闲和障碍物区域,理解传感器观测如何改变单元状态,并通过 ROS 2 Jazzy 发布一张可检查的示例地图。
学习目标
- 区分未知、空闲和占据状态,理解栅格数值与占据概率的关系。
- 根据地图分辨率、原点和数组索引定位单元。
- 使用
nav_msgs/msg/OccupancyGrid发布地图,并验证未观测区域仍然保持未知。
用概率区分三种空间状态
占据栅格将连续的二维空间划分成固定大小的单元,每个单元保存该区域被障碍物占据的估计。地图不必在所有位置都拥有观测;没有有效观测的单元应保留未知状态。
例如,一个宽 3 米、高 2 米的区域采用 0.5 米分辨率,可以划分为 6 列、4 行,共 24 个单元。每个单元对应实际空间中的 0.5 米 × 0.5 米区域。
在 ROS 2 常用的占据概率编码约定中:
| 栅格值 | 含义 | 典型处理 |
|---|---|---|
-1 |
未知,尚无有效占据估计 | 不直接当成空闲 |
0 |
空闲,占据概率约为 0% | 可以作为候选通行区域 |
45 |
已有估计,占据概率约为 45% | 需要按规划策略判断 |
100 |
被占据,占据概率约为 100% | 作为障碍区域处理 |
未知不等于空闲,也不等于概率为 50%。 未知表示缺少可用证据;50 则是已经给出的一种不确定占据估计。虽然概率建模时常使用 0.5 作为初始先验,发布地图时仍应通过 -1 保留未观测状态。
根据传感器观测更新栅格
二维激光雷达的一束有效测距,可以为多个栅格提供不同类型的证据。
- 根据激光雷达位姿,将测量起点和终点转换到地图坐标系。
- 沿测量射线遍历经过的栅格,将障碍物之前的单元作为空闲证据。
- 如果测距确实命中有效障碍物,则将终点所在单元作为占据证据。
- 射线没有覆盖的区域继续保持未知,不因为附近存在空闲单元而自动变成空闲。

实际建图通常不会因为一次测量就把某个单元永久设成 0 或 100。SLAM 系统会结合传感器模型累计证据,通过概率或对数赔率更新占据估计。
例如,某单元初始占据概率为 0.5。假设一次有效障碍命中提供的占据证据使估计达到 0.7,在相互独立、质量相同的第二次证据下,按赔率相乘计算,概率约为 0.845,可以编码成约 85。这是简化的概率更新算例,并不是 ROS 2 强制规定的传感器模型。
真实系统还要处理测量噪声、重复观测相关性、激光最大量程和概率饱和。特别是没有检测到障碍物,不代表可以将整条最大量程之外的区域标记为空闲。
理解地图坐标和数组索引
ROS 2 使用 nav_msgs/msg/OccupancyGrid 表示二维栅格地图。消息由 header、info 和 data 三部分组成。
| 字段 | 含义 | 本课示例 |
|---|---|---|
header.frame_id |
地图所在坐标系 | map |
info.resolution |
每格边长,单位米 | 0.5 |
info.width |
x 方向单元数量 | 6 |
info.height |
y 方向单元数量 | 4 |
info.origin |
地图原点的位姿 | (-1.0, -1.0),朝向为零 |
data |
按行优先展开的占据值 | 24 个整数 |
data 是一维数组,不是二维矩阵。它从网格 (0,0) 开始存放,先沿 x 方向增加列索引,再沿 y 方向进入下一行。
对于宽度为 的地图,单元 对应的数组下标为:
在地图未旋转、分辨率为 、原点为 时,世界坐标转换为栅格索引可以使用:
例如,地图原点为 (-1.0, -1.0),分辨率为 0.5 米,查询世界坐标 (0.25, -0.25):
- x 方向索引为
floor((0.25 + 1.0) / 0.5) = 2。 - y 方向索引为
floor((-0.25 + 1.0) / 0.5) = 1。 - 一维数组索引为
1 × 6 + 2 = 8。
因此需要读取 data[8]。该点恰好位于单元 (2,1) 的中心。
发布一张最小占据栅格地图
下面使用 ROS 2 Jazzy 和 Python rclpy 发布一张静态示例地图。运行环境为已安装 ROS 2 Jazzy、rclpy 和 nav_msgs 的 Ubuntu 24.04。
创建 demo_grid.py,填写以下完整代码:
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
class DemoGridPublisher(Node):
def __init__(self):
super().__init__('demo_grid_publisher')
self.publisher = self.create_publisher(
OccupancyGrid, '/demo_grid', 10
)
self.grid = OccupancyGrid()
self.grid.header.frame_id = 'map'
self.grid.info.resolution = 0.5
self.grid.info.width = 6
self.grid.info.height = 4
self.grid.info.origin.position.x = -1.0
self.grid.info.origin.position.y = -1.0
self.grid.info.origin.orientation.w = 1.0
# 所有单元初始未知,只更新有观测证据的区域。
width = self.grid.info.width
height = self.grid.info.height
self.grid.data = [-1] * (width * height)
observations = [
(1, 1, 0), (2, 1, 0), (3, 1, 100),
(1, 2, 0), (2, 2, 0), (3, 2, 45),
]
for x, y, value in observations:
self.grid.data[y * width + x] = value
values = self.grid.data
print(
f'cells={len(values)} '
f'unknown={values.count(-1)} '
f'free={values.count(0)}',
flush=True,
)
print(f'cell(2,1)={values[1 * width + 2]}', flush=True)
# 周期发布便于新启动的订阅者收到示例地图。
self.timer = self.create_timer(1.0, self.publish_grid)
def publish_grid(self):
self.grid.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(self.grid)
def main():
rclpy.init()
node = DemoGridPublisher()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()预期结果:节点在 /demo_grid 发布类型为 nav_msgs/msg/OccupancyGrid 的消息,每秒发布一次。地图共 24 个单元,其中 18 个未知、4 个空闲、1 个占据、1 个具有中间概率值。示例中的 45 是人工设定的教学数据,并非激光传感器实时计算结果。
在第一个终端运行:
source /opt/ros/jazzy/setup.bash
python3 demo_grid.py预期输出:
cells=24 unknown=18 free=4
cell(2,1)=0终端会继续运行节点,按 Ctrl+C 可以退出。运行后不会自动打开地图界面,因为本示例只负责发布消息。
这里有三个需要理解的实现细节。
首先,self.grid.data 的长度必须等于 width * height。如果宽高配置与数组大小不一致,订阅端无法正确按二维坐标解释地图。
其次,origin.orientation.w = 1.0 将原点朝向设置成单位四元数,表示没有旋转。不能把四元数四个分量全部保留为零,因为那不是有效的单位旋转。
最后,示例使用每秒一次的周期发布,目的是让后启动的订阅者也容易收到地图。生产中的静态地图通常可以结合 TRANSIENT_LOCAL 持久性 QoS,在订阅端请求兼容 QoS 的情况下向后加入者提供已有地图,而不是持续重复发布不变的数据。
验证未知区域没有变成空闲
保持第一个终端运行,在第二个终端执行:
source /opt/ros/jazzy/setup.bash
ros2 topic echo --once /demo_grid --field data预期结果:终端打印包含 24 个元素的 data 序列。无论具体 YAML 展示如何换行,数组中的关键值应满足:
| 检查位置 | 数组下标 | 预期值 |
|---|---|---|
(0,0) 未观测 |
0 | -1 |
(2,1) 空闲 |
8 | 0 |
(3,1) 障碍 |
9 | 100 |
(3,2) 不确定 |
15 | 45 |
(5,3) 未观测 |
23 | -1 |
还可以查看话题的消息类型:
ros2 topic type /demo_grid预期输出:
nav_msgs/msg/OccupancyGrid如果话题没有数据,先确认发布节点仍然运行,再检查两个终端是否加载了同一 ROS 2 环境、是否设置了相同的 ROS_DOMAIN_ID,以及发布者与订阅者的 QoS 是否兼容。
如果读出的地图出现行列错位,优先检查是否误用了 x * height + y,正确下标应为 y * width + x。如果地图整体发生平移,则检查 info.origin;如果方向不正确,还需要核对原点四元数以及地图坐标系与机器人坐标系之间的变换。
最后,可以尝试将示例中的 (3,1,100) 改成 (3,1,-1) 并重新运行。原先的障碍单元应重新成为未知,未知数量增加到 19,占据数量降为 0,而其他空闲单元不受影响。
这一验证说明,占据栅格的三种状态承载着不同的环境知识:已确认空闲的区域提供通行证据,被占据的区域提供障碍证据,而未观测区域必须保留信息缺失这一事实。后续路径规划可以根据自身的未知区域处理策略决定是否允许探索,但不应在建图阶段提前抹掉这种区别。