机器人 / 系统课时

激光雷达量测如何转换为二维点

激光雷达量测如何转换为二维点

ROS 2 的激光扫描消息给出的是按顺序排列的距离,而不是地图上的现成坐标。本篇从一帧 LaserScan 读取每束光线的角度和距离,剔除无效回波,算出雷达坐标系内的二维点,并用具体数值检验方向和精度。

学习目标

  • 根据 angle_min、angle_increment 和束索引计算观测角度。
  • 将有效距离转换为二维坐标,并识别超量程和无回波数据。
  • 用数值检查点的方向,区分量测噪声与坐标变换问题。

从 LaserScan 取出角度与距离

ROS 2 使用 sensor_msgs/msg/LaserScan 表示平面激光扫描。ranges[i] 是第 i 束距离,单位为米;angle_min 是首束角度,angle_increment 是相邻束的角度间隔,两者均为弧度。range_min 与 range_max 规定可接受的测距范围。

每束的方向应由原始索引计算,而不能将数组位置直接看成角度:

θi=θ0+iΔθ\theta_i=\theta_0+i\Delta\theta

其中 θ0\theta_0 对应 angle_min,Δθ\Delta\theta 对应 angle_increment。例如首束为 -90°、角度增量为 45° 时,第 1 束角度是 -45°。末束角应与 angle_max 大致一致,允许浮点误差;不要另用首末角对数组重新等分。

将极坐标投影为二维点

按照 ROS 的右手系约定,雷达坐标系通常为 x 向前、y 向左、z 向上。角度从 +x 方向开始,绕 +z 逆时针增加。已知有效距离 rir_i 与方向 θi\theta_i,可求二维点:

xi=ricos⁡θi,yi=risin⁡θix_i=r_i\cos\theta_i,\qquad y_i=r_i\sin\theta_i

例如某束距离为 2\sqrt{2} 米、角度为 -45°,得到 (x,y)=(1,−1)(x,y)=(1,-1) 米,即障碍物在雷达前方 1 米、右侧 1 米。该点只属于消息 header.frame_id 指定的传感器坐标系,尚不是机器人或地图坐标。

激光扫描射线在雷达坐标系中的极坐标与直角坐标关系

过滤无效距离并运行示例

ranges 可能包含 inf、nan、过近或超出最大量程的值。处理次序是先检查有限数,再应用含端点的区间 range_min <= r <= range_max;无效束直接跳过,不把 inf 当作最大距离,否则会凭空画出障碍物。

下面的 Python 3 示例模拟一帧 6 束扫描,不依赖 ROS 运行时。将代码保存为 scan_to_points.py,执行 python3 scan_to_points.py;保留原束索引是为了后续匹配强度或排查坏点。

python
import math

angle_min = -math.pi / 2
angle_increment = math.pi / 4
range_min, range_max = 0.2, 5.0
ranges = [1.0, math.sqrt(2), float("inf"), 0.1, 6.5, 1.0]

points = []
for i, distance in enumerate(ranges):
    # 先排除非有限值,再按雷达标称量程筛选
    if not math.isfinite(distance) or not (range_min <= distance <= range_max):
        continue
    angle = angle_min + i * angle_increment
    # 坐标仍在扫描消息所属的雷达坐标系中
    points.append((i, distance * math.cos(angle), distance * math.sin(angle)))

print(f"有效点数: {len(points)}")
for i, x, y in points:
    print(f"束 {i}: ({x:.3f}, {y:.3f})")

预期输出:

text
有效点数: 3
束 0: (0.000, -1.000)
束 1: (1.000, -1.000)
束 5: (-0.707, 0.707)

第 2 束为无穷远,第 3 束小于 0.2 米,第 4 束大于 5 米,因此只有索引 0、1、5 生成点。过滤后仍用原索引算角度:若把第 5 束误当成过滤后的第 2 束,会把真实方向旋转 135°。

检查噪声和坐标系

即使距离有效,也可能有随机抖动或反光导致的离群值。让雷达与墙面保持静止,对同一束连续读取 5 帧,例如得到 1.99、2.02、2.01、2.47、2.00 米;中位数是 2.01 米,可减轻单个 2.47 米异常值的影响。机器人移动时不要直接对不同时间的原始束做这种平均;在墙角或细杆边缘横向平滑也可能抹掉真实障碍物。

用三个几何条件自检:0° 的点应近似为 (r,0)(r,0),+90° 应近似为 (0,r)(0,r),-90° 应近似为 (0,−r)(0,-r)。出现左右翻转,先核查角度正方向;整体位置偏移,检查 header.frame_id 与雷达安装外参。要把这些点送入建图,需按时间戳通过 tf2 转到 base_link、odom 或 map;雷达运动较快时,还须考虑 header.stamp(首束采集时间)和 time_increment(束间时间)的扫描运动畸变。

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

目录与阅读设置

阅读设置

17

记录一个想法

激光雷达量测如何转换为二维点

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

登录哪都通