机器人 / 系统课时

用占据栅格表达未知与空闲区域

用占据栅格表达未知与空闲区域

机器人不能把没有探测过的地方直接当成空地。本课使用二维占据栅格表示环境,区分未知、空闲和障碍物区域,理解传感器观测如何改变单元状态,并通过 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 保留未观测状态。

根据传感器观测更新栅格

二维激光雷达的一束有效测距,可以为多个栅格提供不同类型的证据。

  1. 根据激光雷达位姿,将测量起点和终点转换到地图坐标系。
  2. 沿测量射线遍历经过的栅格,将障碍物之前的单元作为空闲证据。
  3. 如果测距确实命中有效障碍物,则将终点所在单元作为占据证据。
  4. 射线没有覆盖的区域继续保持未知,不因为附近存在空闲单元而自动变成空闲。

激光测量如何区分占据栅格中的未知、空闲与障碍区域

实际建图通常不会因为一次测量就把某个单元永久设成 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 方向进入下一行。

对于宽度为 WW 的地图,单元 (ix,iy)(i_x,i_y) 对应的数组下标为:

k=iy×W+ixk=i_y\times W+i_x

在地图未旋转、分辨率为 rr、原点为 (x0,y0)(x_0,y_0) 时,世界坐标转换为栅格索引可以使用:

ix=⌊x−x0r⌋,iy=⌊y−y0r⌋i_x=\left\lfloor\frac{x-x_0}{r}\right\rfloor,\qquad i_y=\left\lfloor\frac{y-y_0}{r}\right\rfloor

例如,地图原点为 (-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,填写以下完整代码:

python
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 是人工设定的教学数据,并非激光传感器实时计算结果。

在第一个终端运行:

bash
source /opt/ros/jazzy/setup.bash
python3 demo_grid.py

预期输出:

text
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 的情况下向后加入者提供已有地图,而不是持续重复发布不变的数据。

验证未知区域没有变成空闲

保持第一个终端运行,在第二个终端执行:

bash
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

还可以查看话题的消息类型:

bash
ros2 topic type /demo_grid

预期输出:

text
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,而其他空闲单元不受影响。

这一验证说明,占据栅格的三种状态承载着不同的环境知识:已确认空闲的区域提供通行证据,被占据的区域提供障碍证据,而未观测区域必须保留信息缺失这一事实。后续路径规划可以根据自身的未知区域处理策略决定是否允许探索,但不应在建图阶段提前抹掉这种区别。

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

目录与阅读设置

阅读设置

17

记录一个想法

用占据栅格表达未知与空闲区域

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

登录哪都通