机器人 / 系统课时

对齐传感器时间戳避免融合错位

对齐传感器时间戳避免融合错位

雷达扫描与 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 的时间戳配对窗口和错误配对对比

先检查时钟与消息

在已启动雷达和 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 话题不同,请改为真实名称。

python
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。

bash
source /opt/ros/jazzy/setup.bash
python3 scan_imu_sync.py --ros-args -p use_sim_time:=true

预期输出(示意格式,需有匹配消息才能出现):

text
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 做逐束去畸变。

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

目录与阅读设置

阅读设置

17

记录一个想法

对齐传感器时间戳避免融合错位

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

登录哪都通