机器人 / 系统课时

射线追踪怎样标记激光经过的空闲区

射线追踪怎样标记激光经过的空闲区

一束激光返回的距离只告诉我们测到了哪里,建图还要判断光束途中哪些栅格可能为空。这里用二维栅格上的射线遍历,把沿途空闲证据和末端障碍证据分开处理,并通过可运行示例检查每个格子的更新结果。

学习目标

  • 将激光起点和终点从米制坐标转换成地图栅格索引。
  • 区分命中、无回波和无效量测对栅格的不同影响。
  • 用 Bresenham 射线遍历生成空闲格,并检查更新边界。

从米制坐标定位射线两端

ROS 2 的 sensor_msgs/msg/LaserScan 按 angle_min + k * angle_increment 给出第 k 束的方向,距离来自 ranges[k]。对有效距离 d,先在雷达坐标系计算终点 p=(dcos⁡θ,dsin⁡θ)p=(d\cos\theta,d\sin\theta),再使用量测时刻的 TF 将雷达原点与终点变换到地图坐标系;不要直接把雷达坐标当地图坐标。

设地图原点平移为 (x0, y0)、原点朝向为零、分辨率为 r 米/格,则格坐标为 i=⌊(x−x0)/r⌋i=\lfloor(x-x_0)/r\rfloor、j=⌊(y−y0)/r⌋j=\lfloor(y-y_0)/r\rfloor。例如 r=0.5 米/格、原点 (0,0),雷达起点 (0.75,0.75) 对应 (1,1),命中点 (3.25,2.25) 对应 (6,4)。若 info.origin 带旋转,须先应用该姿态的逆变换,不能只减原点坐标。

nav_msgs/msg/OccupancyGrid 以 (0,0) 开始按行优先存储,索引为 j * info.width + i;读取或写入前确认索引落在地图边界内:0≤i<width0\le i<\mathrm{width} 且 0≤j<height0\le j<\mathrm{height}。

分清途中空闲与末端障碍

射线追踪从起点格走到终点格,产生两种不同证据:

  1. 确认命中障碍:不把雷达自身所在格当作这束射线的更新对象;起点与命中格之间的格子记为空闲观测,最后的命中格记为占据观测。绝不能穿过命中格继续清空后面的区域。
  2. 明确无命中:只在传感器驱动明确表示某个方向直到给定有效距离都没有回波时,把沿途至截断终点的格子都作为空闲观测,不创建障碍格。
  3. 量测无效:距离小于 range_min、NaN 或超出有效范围时丢弃;不要擅自将 +inf 等异常值解释为无障碍,除非已核对设备驱动约定。

命中射线和无回波射线的空闲格差异

用 Bresenham 逐格更新

Bresenham 算法只使用整数增量,按离散直线依次给出栅格;对于起止格在横、纵方向的差值,一次遍历的时间复杂度为 O(max⁡(∣Δi∣,∣Δj∣))O(\max(|\Delta i|,|\Delta j|))。下面的 Python 3 示例不依赖 ROS 2,用 -1 表示未知、0 表示空闲、100 表示占据,先演示命中射线,再演示无命中射线。

python
def bresenham(start, end):
    x, y = start
    tx, ty = end
    dx, dy = abs(tx - x), abs(ty - y)
    sx = 1 if x < tx else -1
    sy = 1 if y < ty else -1
    error = dx - dy
    cells = []
    while True:
        cells.append((x, y))
        if (x, y) == (tx, ty):
            return cells
        doubled = 2 * error
        if doubled > -dy:
            error -= dy
            x += sx
        if doubled < dx:
            error += dx
            y += sy

def update_grid(grid, width, height, start, end, hit):
    cells = bresenham(start, end)
    # 命中时保留末格作为障碍;无命中时末格也算空闲
    free_cells = cells[1:-1] if hit else cells[1:]
    for x, y in free_cells:
        if 0 <= x < width and 0 <= y < height:
            if grid[y * width + x] != 100:
                grid[y * width + x] = 0  # 演示中不让空闲覆盖已有障碍
    if hit:
        x, y = cells[-1]
        if 0 <= x < width and 0 <= y < height:
            grid[y * width + x] = 100
    return free_cells

width, height = 8, 6
grid = [-1] * (width * height)
hit_free = update_grid(grid, width, height, (1, 1), (6, 4), True)
update_grid(grid, width, height, (1, 1), (1, 5), False)
print("命中射线空闲:", hit_free)
print("命中格:", grid[4 * width + 6])
print("无回波末格:", grid[5 * width + 1])
print("未观测格:", grid[5 * width + 7])

预期输出:

text
命中射线空闲: [(2, 2), (3, 2), (4, 3), (5, 3)]
命中格: 100
无回波末格: 0
未观测格: -1

这里 cells[1:-1] 是命中射线的关键:若误写成 cells[1:] 并把终点也置零,真实墙体就会被擦掉。示例选择离散直线穿过的代表格;若必须覆盖连续射线穿过的每个格子,应改用 supercover 或二维 DDA,并明确射线恰好经过格角时的归属规则。

接入 ROS 2 并验证地图

真正的建图节点需从 /scan 读取 LaserScan,利用时间戳对应的 TF 把每束有效射线的起止点变换到 map,随后更新内存栅格并通过 /map 发布 nav_msgs/msg/OccupancyGrid。给出一次可检查的处理顺序:

  1. 读取 angle_min、angle_increment、range_min、range_max 和各束 ranges[k],先判定量测是否有效、是否确认命中。
  2. 在 map 坐标系中求出起终点,按 info.origin 与 info.resolution 换算栅格;射线超出地图时先裁剪,禁止负索引绕回数组末尾。
  3. 遍历经过的格子:向空闲观测累加负证据,仅对确认命中的末格累加正证据;未被观测的格子保留未知。
  4. 在 RViz 2 的 Displays → Add → By topic 中添加 /map 的 Map 与 /scan 的 LaserScan,设置 Global Options → Fixed Frame 为 map;让机器人面向一堵直墙,检查墙前逐渐出现空闲区、墙所在格出现连续占据边缘、墙后仍为未知。

上面的直接赋 0/100 只用于教学。实际 SLAM 通常维护占据对数几率,重复观测时累加空闲或占据证据并设上下限;否则一次误测会被永久固定。还应核对激光时间戳与 TF 是否匹配:若扫描错位,表现通常是墙体双影或空闲格穿过墙体,而非射线遍历算法本身出错。

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

目录与阅读设置

阅读设置

17

记录一个想法

射线追踪怎样标记激光经过的空闲区

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

登录哪都通