机器人 / 系统课时
激光雷达量测如何转换为二维点
激光雷达量测如何转换为二维点
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 规定可接受的测距范围。
每束的方向应由原始索引计算,而不能将数组位置直接看成角度:
其中 对应 angle_min, 对应 angle_increment。例如首束为 -90°、角度增量为 45° 时,第 1 束角度是 -45°。末束角应与 angle_max 大致一致,允许浮点误差;不要另用首末角对数组重新等分。
将极坐标投影为二维点
按照 ROS 的右手系约定,雷达坐标系通常为 x 向前、y 向左、z 向上。角度从 +x 方向开始,绕 +z 逆时针增加。已知有效距离 与方向 ,可求二维点:
例如某束距离为 米、角度为 -45°,得到 米,即障碍物在雷达前方 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;保留原束索引是为了后续匹配强度或排查坏点。
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})")预期输出:
有效点数: 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° 的点应近似为 ,+90° 应近似为 ,-90° 应近似为 。出现左右翻转,先核查角度正方向;整体位置偏移,检查 header.frame_id 与雷达安装外参。要把这些点送入建图,需按时间戳通过 tf2 转到 base_link、odom 或 map;雷达运动较快时,还须考虑 header.stamp(首束采集时间)和 time_increment(束间时间)的扫描运动畸变。