机器人 / 系统课时
对齐传感器时间戳避免融合错位
对齐传感器时间戳避免融合错位
雷达扫描与 IMU 数据即使都在持续发布,也可能因为采样时刻不同而配错。错位会让障碍物位置和机器人姿态互相矛盾。本课在 ROS 2 Jazzy 中检查 header.stamp、仿真时钟与缓存窗口,并运行一个能打印配对时间差的同步节点。
学习目标
- 区分传感器采样时间、消息到达时间与 ROS 仿真时间。
- 根据采样频率设置近似同步的
slop和queue_size。 - 用
message_filters验证配对结果并定位常见错位原因。
按采样时刻配对
LaserScan 和 Imu 都有 header.stamp,其中 sec 是整秒,nanosec 是不足一秒的纳秒数。对同一时间基准的两条消息,时间差可按 |t_scan - t_imu| 计算。LaserScan.header.stamp 通常表示一帧扫描中第一束激光的采集时刻,不能直接当成整个扫描的中点。
假设雷达在 12.000 s 和 12.100 s 采样,待选 IMU 消息分别为 12.018 s 和 12.118 s。按时间戳匹配,两组相差 18 ms;如果因为回调到达顺序混乱,将首帧雷达错配到 12.118 s 的 IMU,误差就变成 118 ms。机器人以 0.5 m/s 匀速运动时,118 ms 对应约 5.9 cm 位移,足以影响局部建图。

先检查时钟与消息
在已启动雷达和 IMU 发布者的 ROS 2 Jazzy 环境中,用 ros2 topic list -t 确认话题及类型,再分别执行 ros2 topic echo /scan --once --field header.stamp --qos-reliability best_effort 和 ros2 topic echo /imu/data --once --field header.stamp --qos-reliability best_effort。重点检查时间戳是否为零、秒数是否处于同一数量级;去掉 --once 连续观察还能检查时间戳是否递增。若话题名不同,使用实际名称。
仿真或带 /clock 的 rosbag 回放中,使用 ros2 topic echo /clock --once 核实模拟时间正在发布,并把相关节点的 use_sim_time 设为 true;真实机器人通常保持 false。只有接收节点开启仿真时间,不会自动修正发布者用系统时钟写入的错误 header.stamp。不同主机的真实传感器还应校准设备时钟,必要时使用 NTP 或 PTP。关于时间源切换,可对照 ROS 2 时间设计。
确定同步窗口和缓存深度
ROS 2 的 message_filters.ApproximateTimeSynchronizer 按消息头时间戳组对,适合雷达与 IMU 没有完全相同采样时刻的情况。这里先取 slop=0.05,表示配对时间戳最大差距为 50 ms;queue_size=20 则是在同步器中为每路输入保留的消息数量,不代表 20 ms,也不保证固定的等待时长。
例如雷达 10 Hz、IMU 100 Hz,雷达周期为 100 ms,IMU 周期为 10 ms。50 ms 可以作为观察配对率的初始窗口,却不是通用安全阈值:若机器人速度为 1 m/s,50 ms 就对应 5 cm 位移。需要厘米级定位时,应根据运动速度、硬件时间偏差与融合误差预算缩小窗口,不能靠无限扩大 slop 掩盖错误时钟。
编写并运行近似同步节点
下面是完整的 scan_imu_sync.py。前提是已在 Ubuntu 24.04 安装 ROS 2 Jazzy、Python 包 message_filters(对应 apt 包为 ros-jazzy-message-filters),并且 /scan 和 /imu/data 分别发布 sensor_msgs/msg/LaserScan 与 sensor_msgs/msg/Imu。若实际雷达或 IMU 话题不同,请改为真实名称。
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from sensor_msgs.msg import LaserScan, Imu
from message_filters import Subscriber, ApproximateTimeSynchronizer
class ScanImuSync(Node):
def __init__(self):
super().__init__('scan_imu_sync')
# 订阅端采用与常见传感器发布者兼容的 QoS。
qos = QoSProfile(
depth=20,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.scan_sub = Subscriber(self, LaserScan, '/scan', qos_profile=qos)
self.imu_sub = Subscriber(self, Imu, '/imu/data', qos_profile=qos)
# 保留引用,并按 header.stamp 在 50 ms 窗口内配对。
self.sync = ApproximateTimeSynchronizer(
[self.scan_sub, self.imu_sub], queue_size=20, slop=0.05
)
self.sync.registerCallback(self.on_pair)
def on_pair(self, scan, imu):
# 整数纳秒运算避免混入接收时刻。
a = scan.header.stamp
b = imu.header.stamp
a_ns = a.sec * 1_000_000_000 + a.nanosec
b_ns = b.sec * 1_000_000_000 + b.nanosec
delta_ms = abs(a_ns - b_ns) / 1_000_000
print(
f'scan={a.sec}.{a.nanosec:09d} '
f'imu={b.sec}.{b.nanosec:09d} delta_ms={delta_ms:.1f}',
flush=True,
)
def main():
rclpy.init()
node = ScanImuSync()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()预期结果:节点会等待两路消息,只有同步器找到时间戳差不超过 50 ms 的消息对,才会调用 on_pair 并打印时间差。可查阅 ROS 2 Jazzy message_filters 文档 对照该接口。
保存文件后,在已启动仿真和传感器发布者的终端运行;真实机器人运行时去掉 --ros-args -p use_sim_time:=true。
source /opt/ros/jazzy/setup.bash
python3 scan_imu_sync.py --ros-args -p use_sim_time:=true预期输出(示意格式,需有匹配消息才能出现):
scan=<TIME> imu=<TIME> delta_ms=<DELTA><TIME> 是实际消息时间戳,<DELTA> 是本次配对的毫秒差,通常落在 0~50 ms。按 Ctrl+C 结束程序;没有匹配消息时没有配对行,并不代表程序已经崩溃。
配对失败时怎样定位
如果两路数据都在发布却迟迟没有输出,先用 ros2 topic info /scan --verbose 和 ros2 topic info /imu/data --verbose 检查发布者 QoS 与订阅者是否兼容,再核对两路 header.stamp 是否一个用系统时间、另一个用仿真时间。运行期间可通过 ros2 param get /scan_imu_sync use_sim_time 检查同步节点是否启用了仿真时钟。
若两路时间基准一致但抖动较大,可暂将 slop 从 0.05 调整为 0.10 对比配对率,同时记录输出的 delta_ms;确认延迟来源后再收紧阈值。仿真复位或 rosbag 循环回放导致时间倒退时,测试中应重启同步节点以清理旧缓存。最后要注意:配对成功不等于已经完成运动补偿,运动中的整帧 LaserScan 还可能需要结合 time_increment 做逐束去畸变。