机器人 / 系统课时

机器人快速运动时如何定位扫描畸变

机器人快速运动时如何定位扫描畸变

当机器人一边快速转向一边采集激光数据时,一帧扫描中不同光束对应的机器人姿态可能已经发生明显变化。如果建图系统把整帧数据当成同一时刻的观测,直墙就可能变弯、拉长或出现重影。本课通过扫描时间、角速度和 ROS 2 的 TF 数据定位这种畸变,并给出可验证的修复方法。

学习目标

  • 识别快速运动引起的扫描畸变,并与里程计漂移、扫描匹配失败区分。
  • 使用 LaserScan 时间字段和机器人速度估算单帧内的运动量。
  • 检查时间戳与 TF,判断何时需要降低速度或进行运动补偿。

从地图形状和运动条件识别畸变

常见的旋转式二维激光雷达不是同时测量所有方向,而是在一段时间内依次发射和接收光束。机器人静止时,这些光束可以近似使用同一个传感器姿态;机器人快速移动时,第一束和最后一束对应的传感器位置、朝向可能明显不同。

典型现象是:机器人静止或缓慢行驶时,走廊墙线比较笔直;快速转弯时,同一面墙被画成弧线、斜线或局部拉长。若连续多帧叠加,还可能出现双层墙面。

快速转向导致单帧激光扫描畸变及运动补偿前后对比

可以安排一次受控对比实验:在安全、静态的走廊中,分别以约 0.1 rad/s 和 1.2 rad/s 的角速度转向,尽量保持相同的雷达位置和观测距离。在 RViz 2 中观察激光点与地图墙线的关系,并记录两种速度下的异常程度。

如果角速度增加后,单帧扫描中的静态墙面明显弯折,应重点调查扫描内部运动。反过来,如果每一帧点云形状都正常,只是多帧叠加之后墙面出现重影,应优先检查扫描匹配、里程计和时间同步。

读取扫描时间和机器人速度

ROS 2 的 sensor_msgs/msg/LaserScan 包含三个与时间有关的重要信息:header.stamp、time_increment 和 scan_time。它们的含义不能混淆。

字段 实际含义 排查用途
header.stamp 按消息定义,第一束激光的采集时间 确定单帧时间起点
time_increment 相邻测量之间的时间间隔,单位秒 估算各束采集时间
scan_time 相邻两帧扫描之间的时间,单位秒 检查扫描周期和频率
ranges 各束激光的距离数组 确定光束数量及距离

对于包含 NN 束激光、按固定时间间隔采样的扫描,一帧从第一束到最后一束的采集跨度为:

T=(N−1)ΔtT=(N-1)\Delta t

其中 Δt\Delta t 是 time_increment。注意,scan_time 表示相邻扫描的时间间隔,不应未经确认就直接替代单帧采集跨度。

在已经启动 ROS 2、雷达驱动和里程计发布节点的终端执行以下命令。本例使用 /scan 和 /odom,实际话题名称应以现场系统为准。

bash
# 查看第一束激光的时间戳及逐束采样间隔
ros2 topic echo /scan --once --field header.stamp --qos-reliability best_effort
ros2 topic echo /scan --once --field time_increment --qos-reliability best_effort
ros2 topic echo /scan --once --field scan_time --qos-reliability best_effort

# 查看里程计报告的线速度和角速度
ros2 topic echo /odom --once --field twist.twist

预期结果:第一条显示包含 sec、nanosec 的时间戳;第二、三条显示以秒为单位的数值;最后一条显示 linear 与 angular 分量。具体数值因设备和机器人运动而异,重点记录 linear.x 与 angular.z,并确认速度读数与扫描时间段对应。

如果扫描消息中的 time_increment 为零,不能直接认为所有光束在同一时刻采集。部分驱动没有正确提供逐束时间信息,此时必须检查设备协议或驱动实现,不能凭空生成精确的运动补偿时间戳。

计算一帧扫描中的运动量

考虑一台二维激光雷达,其单帧包含720束测量,相邻光束时间间隔为0.000139秒。机器人在该时间段内以约 1.2 rad/s 转向,同时以 0.8 m/s 平移。假设角速度和速度近似恒定,可以先估算扫描过程中累计的位移与转角。

旋转量由 Δθ=∣ω∣T\Delta\theta=|\omega|T 计算,其中 ω\omega 为角速度。对于距离约5米的物体,可以用 Rsin⁡(Δθ)R\sin(\Delta\theta) 估计首末时刻方向变化对应的横向几何量级。

下面的 Python 3 程序不依赖 ROS 2,可直接运行。实际排查时,把参数替换为雷达消息和运动数据中的测量值。

python
import math

beams = 720
time_increment = 0.000139  # 相邻光束时间间隔,单位 s
yaw_rate = 1.2  # 机器人角速度,单位 rad/s
speed = 0.8  # 机器人平移速度,单位 m/s
distance = 5.0  # 目标距离,单位 m

duration = (beams - 1) * time_increment
dyaw = abs(yaw_rate) * duration

# 估计整帧首末时刻的运动差异及远处目标的横向量级
print(f"采集跨度: {duration:.3f} s")
print(f"累计转角: {math.degrees(dyaw):.2f} deg")
print(f"移动距离: {abs(speed) * duration:.3f} m")
print(f"5 m处横向量级: {distance * math.sin(dyaw):.3f} m")

预期输出:

text
采集跨度: 0.100 s
累计转角: 6.87 deg
移动距离: 0.080 m
5 m处横向量级: 0.598 m

这意味着机器人在约0.1秒的扫描期间转过了6.87度,并移动了约8厘米。对于5米处的目标,首末姿态的角度变化对应约60厘米的横向几何量级。

这里的0.598米是用于判断风险的量级估计,并不等于最终地图必然偏离0.598米。真实误差取决于每束光线的采集时刻、目标方向、雷达安装位置,以及建图系统采用哪一时刻作为参考姿态。

如果建图分辨率为 0.05 m/格,上述旋转产生的潜在几何差异远大于单个栅格,不能只靠缩小地图分辨率修复畸变。

排除时间戳、TF 和扫描匹配故障

发现快速运动时墙面拉伸,并不能立即认定根因是缺少运动补偿。错误的时间戳、里程计延迟或雷达外参,也可能造成相似现象。排查时应先确认数据和坐标关系,再根据单帧与跨帧的表现分类。

对于常见的二维建图配置,需要检查 odom 到 base_link 的动态变换,以及 base_link 到雷达坐标系的安装变换。雷达坐标系名称由 /scan 消息中的 header.frame_id 决定,不一定叫作 laser。

可以使用以下 ROS 2 命令观察里程计对应的 TF 是否持续更新:

bash
ros2 run tf2_ros tf2_echo odom base_link

预期结果:终端持续打印 Translation、Rotation 等变换信息,机器人运动时相关数值会发生变化;按 Ctrl+C 停止。如果提示找不到坐标系、变换链断开或无法查询变换,应优先修复 TF。该命令主要检查当前可用的变换,不能单独证明历史扫描时间段内的每个时刻都可以成功插值。

可以按以下现象缩小故障范围:

观察证据 优先怀疑 下一步检查
静止时单帧墙线也明显异常 测距异常、环境反射或传感器标定 检查原始距离值和静态目标
慢速正常,快速旋转时单帧墙线弯折 扫描期间姿态变化未被补偿 检查 time_increment、角速度和补偿链路
单帧墙线正常,多帧叠加出现双墙 里程计漂移、时间偏差或扫描匹配失败 对照 /scan、/odom 时间戳及 TF
转向时点云整体错位,且伴随 TF 查询失败 时间同步或变换历史不足 检查各节点时间源和 TF 缓存覆盖范围

修复畸变并验证地图质量

确认单帧扫描畸变后,可以按照从低成本验证到正式修复的顺序处理。

  1. 先限制建图速度。 假设扫描跨度约0.1秒、目标距离5米,希望旋转造成的横向几何量级控制在0.05米以内,则可把角速度暂时限制在约 0.1 rad/s 以下;若平移误差预算同样为0.05米,对应的匀速平移上限约为 0.5 m/s。这些只是本算例的诊断阈值,不是所有机器人的通用运行参数。
  2. 修正采集时间信息。 确保驱动按照 LaserScan 消息语义填写第一束时间戳和真实逐束间隔。雷达、里程计与 IMU 必须采用一致的时间基准;录制和回放时还要确认仿真时间设置一致。
  3. 按光束时间执行运动补偿。 对第 ii 束光线,使用 ti=t0+iΔtt_i=t_0+i\Delta t 确定采集时刻,利用该时刻的里程计、IMU 或 TF 插值得到传感器姿态,再将测量点统一转换到选定的参考时刻。实现时必须确保运动估计覆盖整个采集时间窗,而不是对整帧只查询一次 TF。
  4. 检查补偿后的数据接口。 逐束变换得到的点通常需要先表示为统一坐标系下的点云。如果后端仍使用 slam_toolbox 的 LaserScan 输入,就需要按照目标接口的角度采样规则重新投影或重采样,并妥善处理无效距离和时间信息;不能把任意 PointCloud2 数据直接当成 LaserScan 使用。
  5. 重新采集并对比。 在同一静态墙面前,分别进行低速和快速转向测试。比较补偿前后的单帧墙线弯折、跨帧双墙程度及地图边界厚度,同时检查是否出现 TF 插值或外推错误。

一个可操作的验收方法是:选取距离约5米、无明显反射干扰的直墙,在相同速度条件下采集补偿前后的数据,计算墙面点到拟合直线的横向残差。可以先把快速转向段的残差目标设为 0.10 m 以内,再根据雷达噪声、墙面质量和地图分辨率调整标准。

如果降低角速度后墙线立即恢复,而快速转向时只有引入逐束补偿才能恢复,应将扫描畸变作为主要根因记录。若补偿后单帧墙线已变直,地图仍持续出现整体错位,则应继续调查里程计、时间同步和扫描匹配,而不是反复调整激光扫描时间参数。

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

目录与阅读设置

阅读设置

17

记录一个想法

机器人快速运动时如何定位扫描畸变

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

登录哪都通