机器人 / 系统课时

从激光扫描提取障碍物点集

从激光扫描提取障碍物点集

本课把一帧 LaserScan 距离测量转换为二维障碍物点集。你会保留射线原始索引,剔除无穷值、NaN 与超量程读数,用 ROS 2 Jazzy 的 Python 示例验证结果,并核对点集所属坐标系。

学习目标

  • 根据 angle_min 和 angle_increment 计算每束激光的角度。
  • 使用量程边界与有限值检查筛选有效回波,并换算二维坐标。
  • 验证输出点集,区分障碍物端点与用于清除空闲空间的射线。

按原始索引确定射线方向

LaserScan.ranges 按扫描顺序存放距离,单位为米。第 i 个读数的角度由 angle_min + i * angle_increment 决定;应始终使用过滤前的原始索引,不能先删除无效值再重新编号,否则剩余点的方向会错位。

在典型水平安装的 laser_frame 中,0° 沿 +X,正角度绕 +Z 逆时针转向 +Y。假设 angle_min=-90°、angle_increment=45°,五束射线依次位于 -90°、-45°、0°、45°、90°。实际角度使用弧度,header.frame_id 标明点的所属坐标系,header.stamp 标记首束射线的采集时间。

过滤距离并计算障碍点

一束测量要成为障碍物端点,距离 r 必须是有限数,且满足 range_min <= r <= range_max。例如传感器有效范围为 0.12~8.00 m 时,+inf、NaN、0.05 m 和 9.00 m 都不能生成障碍点。+inf 通常表示没有收到有限距离回波,不应被画成一个无限远障碍物。

有效点按 θi=θ0+iΔθ\theta_i=\theta_0+i\Delta\theta 求射线角度,再计算 xi=ricos⁡θix_i=r_i\cos\theta_i、yi=risin⁡θiy_i=r_i\sin\theta_i。这得到的是二维雷达坐标点,不是地图坐标。

LaserScan 有效回波过滤与二维点坐标示意

运行可复现的 Python 示例

以下代码使用真实的 sensor_msgs.msg.LaserScan 消息类型,但自行构造一帧数据,不依赖实体雷达。适用于已安装 ROS 2 Jazzy 的 Ubuntu 24.04;先在终端执行 source /opt/ros/jazzy/setup.bash,将代码保存为 scan_points.py,再执行 python3 scan_points.py。

python
import math
from sensor_msgs.msg import LaserScan

def extract_points(scan: LaserScan) -> list[tuple[float, float]]:
    points = []
    for index, distance in enumerate(scan.ranges):
        # 先筛选距离,再用原始 index 计算角度,避免射线错位。
        if not math.isfinite(distance):
            continue
        if not scan.range_min <= distance <= scan.range_max:
            continue
        angle = scan.angle_min + index * scan.angle_increment
        points.append((distance * math.cos(angle),
                       distance * math.sin(angle)))
    return points

def main():
    scan = LaserScan()
    # 构造五束射线,覆盖无回波、过近和超量程数据。
    scan.header.frame_id = "laser_frame"
    scan.angle_min = -math.pi / 2
    scan.angle_increment = math.pi / 4
    scan.angle_max = math.pi / 2
    scan.range_min = 0.12
    scan.range_max = 8.0
    scan.ranges = [float("inf"), 1.0, 0.05, 2.0, 9.0]

    points = extract_points(scan)
    print(f"frame={scan.header.frame_id}, valid={len(points)}")
    for x, y in points:
        print(f"x={x:.3f} m, y={y:.3f} m")

if __name__ == "__main__":
    main()

预期输出:

text
frame=laser_frame, valid=2
x=0.707 m, y=-0.707 m
x=1.414 m, y=1.414 m

这里仅原始索引 1 和 3 合格;索引 0 的无穷值、索引 2 的近距离无效值及索引 4 的超量程值均被排除。可把任一读数改为 float("nan"),再次运行,检查该读数同样不会产生障碍点。

接入真实扫描时检查边界

在发布雷达数据的 ROS 2 环境中,执行 ros2 topic type /scan,预期消息类型为 sensor_msgs/msg/LaserScan;再用 ros2 topic echo /scan --once 查看实际的 header.frame_id、angle_min、angle_increment、range_min、range_max 和 ranges。若没有输出,先确认雷达驱动正在发布 /scan,再检查话题名与订阅 QoS。

如果导航模块需要 base_link 或 map 坐标下的点,必须按消息时间戳通过 tf2 变换,不能直接把 laser_frame 的 (x, y) 当成地图坐标。机器人运动较快且单帧扫描持续时间较长时,还需考虑逐束时间偏移带来的运动畸变。

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

目录与阅读设置

阅读设置

17

记录一个想法

从激光扫描提取障碍物点集

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

登录哪都通