机器人 / 系统课时
射线追踪怎样标记激光经过的空闲区
射线追踪怎样标记激光经过的空闲区
一束激光返回的距离只告诉我们测到了哪里,建图还要判断光束途中哪些栅格可能为空。这里用二维栅格上的射线遍历,把沿途空闲证据和末端障碍证据分开处理,并通过可运行示例检查每个格子的更新结果。
学习目标
- 将激光起点和终点从米制坐标转换成地图栅格索引。
- 区分命中、无回波和无效量测对栅格的不同影响。
- 用 Bresenham 射线遍历生成空闲格,并检查更新边界。
从米制坐标定位射线两端
ROS 2 的 sensor_msgs/msg/LaserScan 按 angle_min + k * angle_increment 给出第 k 束的方向,距离来自 ranges[k]。对有效距离 d,先在雷达坐标系计算终点 ,再使用量测时刻的 TF 将雷达原点与终点变换到地图坐标系;不要直接把雷达坐标当地图坐标。
设地图原点平移为 (x0, y0)、原点朝向为零、分辨率为 r 米/格,则格坐标为 、。例如 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;读取或写入前确认索引落在地图边界内: 且 。
分清途中空闲与末端障碍
射线追踪从起点格走到终点格,产生两种不同证据:
- 确认命中障碍:不把雷达自身所在格当作这束射线的更新对象;起点与命中格之间的格子记为空闲观测,最后的命中格记为占据观测。绝不能穿过命中格继续清空后面的区域。
- 明确无命中:只在传感器驱动明确表示某个方向直到给定有效距离都没有回波时,把沿途至截断终点的格子都作为空闲观测,不创建障碍格。
- 量测无效:距离小于
range_min、NaN或超出有效范围时丢弃;不要擅自将+inf等异常值解释为无障碍,除非已核对设备驱动约定。

用 Bresenham 逐格更新
Bresenham 算法只使用整数增量,按离散直线依次给出栅格;对于起止格在横、纵方向的差值,一次遍历的时间复杂度为 。下面的 Python 3 示例不依赖 ROS 2,用 -1 表示未知、0 表示空闲、100 表示占据,先演示命中射线,再演示无命中射线。
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])预期输出:
命中射线空闲: [(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。给出一次可检查的处理顺序:
- 读取
angle_min、angle_increment、range_min、range_max和各束ranges[k],先判定量测是否有效、是否确认命中。 - 在
map坐标系中求出起终点,按info.origin与info.resolution换算栅格;射线超出地图时先裁剪,禁止负索引绕回数组末尾。 - 遍历经过的格子:向空闲观测累加负证据,仅对确认命中的末格累加正证据;未被观测的格子保留未知。
- 在 RViz 2 的 Displays → Add → By topic 中添加
/map的Map与/scan的LaserScan,设置 Global Options → Fixed Frame 为map;让机器人面向一堵直墙,检查墙前逐渐出现空闲区、墙所在格出现连续占据边缘、墙后仍为未知。
上面的直接赋 0/100 只用于教学。实际 SLAM 通常维护占据对数几率,重复观测时累加空闲或占据证据并设上下限;否则一次误测会被永久固定。还应核对激光时间戳与 TF 是否匹配:若扫描错位,表现通常是墙体双影或空闲格穿过墙体,而非射线遍历算法本身出错。