多传感器时间同步详解

多传感器时间同步

每个传感器都有自己的时钟源
不同传感器具有不同的采样频率
另外数据传输、camera曝光等都会产生不可控的延迟

为了有效融合多个传感器的感知数据，必须进行时间同步

在这里插入图片描述

在自动驾驶中，需要用到很多传感器的数据（Lidar，Camera，GPS/IMU)，如果计算单元接收到的各传感器的消息时间不统一，则会造成例如障碍物识别不准等问题

自动驾驶中，时间同步可以分为几部分的内容：统一时钟源，硬件同步，软件同步

之前一直不太明白时间同步，空间同步还是很好理解的，看了一些文章和博客，时间同步确实要更难

主要分为硬件同步和软件同步，实验室目前是无法实现硬件同步的，但也需要了解下

统一时钟源和硬同步

通过统一的时钟源给各传感器提供相同的基准时间，各传感器根据提供的基准时间校准各自的时钟时间，从硬件上实现时间同步。因为时钟源都有钟漂，而且每个时钟源钟漂不同，所以即使把各个传感器时间戳在初始时刻对齐，运行一段时间之后，之前对齐的结果就会偏离。解决这个问题的办法就是在硬件上把时钟源统一

在这里插入图片描述

假设各传感器都有自己的内部时钟，但由于每个时钟的钟漂不一样，导致各时钟之间存在时间差，所以需要统一各传感器的时间源

一般采用GPS时间作为统一时间源，通过GPS给各个传感器提供基准时间，各传感器根据提供的基准时间校准各自的时钟时间

各传感器根据已经校准后的各自时间为各自独立采集的数据加上时间戳信息，来做到所有传感器时间戳同步

统一时钟源的常见方式有两种，一种是基于GPS的“PPS+NMEA”，另一种是基于以太网的IEEE 1588（或IEEE 802.1AS）时钟同步协议

PPS+NMEA

GPS能够从卫星获得高精度的时钟信号，因此通常作为整个系统的时钟源。常规的GPS单元都支持输出精确到毫秒的秒脉冲信号PPS和包含年月日时分秒信息的NMEA指令，通过PPS和NMEA的组合就能够实现对激光雷达或主机的毫秒级时钟同步。将主机时间和激光雷达时间都对齐到GPS时间，即完成了主机和激光雷达的时间源统一。具体可参考：时间同步，自动驾驶里的花好月圆

PPS+NMEA的优点是协议简单，容易实现；缺点是必须基于RS232。多个设备之间实现同步比较困难

PTP（IEEE 1588或IEEE 802.1AS）

PTP(Precision Time Protocol，1588 V2)是基于以太网的高精度时钟同步协议，能够实现以太网中多个从节点（各种传感器）与主节点（主机）之间的亚微秒级时钟同步，前提是所有节点之间都通过以太网互联，交换机支持PTP协议，并且每个节点都支持PTP协议。

在这里插入图片描述

在统一时钟源以后，只能保证时间差没有累计漂移了，但由于不同传感器各自采集周期相互独立，无法保证同一时刻采集数据。因此需要设计一种trigger来同时触发不同的传感器成像。GNSS系统除了可以作为统一的时钟源外，还可以利用其PPS脉冲来触发trigger对传感器下达收集数据的指令，以此来改变传感器的数据采集频率，在指定时间点采集数据

由于每种传感器的采样频率不一致，如lidar通常为10Hz，camera通常为25/30Hz，不同传感器之间的数据传输还存在一定的延迟，那么可以通过寻找相邻时间戳的方法找到最近邻帧，但如果两个时间戳相差较大，且传感器或障碍物又在运动，那么最终会得到较大的同步误差。这个情况可以采用硬件同步触发的方法来缓解查找时间戳造成的误差现象

如果传感器支持硬件触发的情况下，可以采用GPS时间戳作为基准进行硬件触发，这时传感器给出的数据中包含的时间戳即为全局时间戳（GPS时间戳）而非传感器时间戳。GNSS传感器自带秒脉冲发生器，所以可以直接使用

在这里插入图片描述

但是，由于lidar和camera采集数据的方式不同，我们只能近似地保证lidar和camera的同步采集。如下图所示，lidar的激光束不断地360度旋转，假设帧率为10Hz的话，那么一帧点云中采集时刻最早的一个点和采集时刻最晚的一个点时间相差100ms。而camera就不同了，camera是瞬间曝光的，因此图像里面所有像素点的采集时刻都是一样的。这就导致了我们只能近似地保证lidar和camera的同步采集。为什么说是近似的呢？对于camera视野中央的那些点云，它们的采集时间是跟图像采集时间一致的，但是对于camera视野边缘的那些点云，它们的采集时间就跟图像采集时间要有一定的时间偏差了，根据camera视野的大小和点云采集帧率的不同，时间偏差可能会有5ms~20ms左右。

在这里插入图片描述

自动驾驶常用的时间同步方案

主机通过PPS+NMEA实现与GPS的时钟同步；
激光雷达若支持PTP，则主机作为PTP的master，激光雷达作为PTP的slave，以此完成对激光雷达的授时。若激光雷达不支持PTP，则通过PPS+NMEA完成对激光雷达的授时；
相机可以：①若相机支持PTP协议，通过PTP确保相机的时钟与主机同步。然后为每一帧图像都增加一个时间戳，并确保在主机上能够读取到这个时间戳，这样就可以避免编码/解码、以及传输延时引入的同步误差。②想要更高的同步精度，需采取硬件同步触发的方式，根据激光雷达的帧周期同步触发相机的拍摄，实现雷达和相机的完全帧同步，实现完全的毫秒级同步

软件同步

仅仅硬件同步无法弥补各个传感器的频率在几个周期内都无法重叠的问题，例如相机50ms曝光一次，激光雷达100ms扫描一次，那只需要每2次摄像头周期和每1次激光雷达周期硬同步一次就够了。但有些传感器周期可能是30ms、27ms等等，传感器之间的频率不是整数倍关系，这样它们的重合周期就有点大了。这时候就需要用到软同步

软同步的目的是在原本的传感器固有采集频率的基础上进行算法推算，形成虚拟帧，获取同一时刻的信息。一般camera的采集频率是20Hz，也就是50ms的周期，radar输出周期为50-100ms，当我们进行传感器融合算法时，想获得在camera采集时刻的车辆状态，就要根据camera前后时刻的radar信息，通过插值等办法计算出一个等效值，这就是获取同一时刻信息的含义

插值法主要对目标级的信息进行推算。获得同一时刻的传感器数据后，才能进行相应的融合算法

在这里插入图片描述

常用的插值法为内插外推法。适用于传感器间频率不存在倍数关系的情况，或者传感器频率不稳定的情况。主要利用两个传感器帧上的时间标签，计算出时间差，然后通过包含有运动信息的目标帧与时间差结合，对帧中每一个目标的位置进行推算，推算出新的帧时各个目标的位置，并于原有的两帧之间建立新的帧

💡 内插外推法的优点是可以适用于任意两个传感器之间的时间同步，缺点是要进行目标跟踪，因为需要前后帧的同一个目标的数据进行加权。插值法使用场景是多传感器后融合方案，也就是目标级融合，各个传感器已经独立的输出目标级结果

插值法可以归纳为

选定一个主传感器，获取主传感器在 t 时刻的数据，以及对应的时间戳
索引其他传感器在 t 时刻的前后帧数据，并计算得到前后帧数据的权重

设插值时间为sync_time, 其他传感器的前后帧数据为front_data和back_data
则 front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time)
back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time)

3.利用插值得到其他传感器在 t 时刻的数据

线性插值结果：synced_data = front_data * front_scale + back_data * back_scale

ROS中的传感器时间同步机制

ROS提供了message_filters来进行时间同步，message_filters类似一个消息缓存，分别订阅不同的需要融合的传感器的topic，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出，产生一个同步结果并给到回调函数，在回调函数里处理同步时间后的数据

message_filters并不会修改各个topic传过来的数据（也就是缓冲区中的数据），而是将各个缓冲区建立时间线，然后根据时间线上的各个时间点来判断是否需要输出

在ROS（机器人操作系统）中，实现传感器时间同步的主要机制是使用**ros::Time和ros::TimeSynchronizer**等工具。以下是ROS中传感器时间同步的一般步骤和关键组件：

ros::Time： ROS中的时间表示是通过**ros::Time**类来实现的。该类提供了处理时间戳的方法，可以用于记录传感器数据的时间戳
时间戳发布： 传感器节点在发布数据时，通常会将**ros::Time::now()**的返回值作为时间戳附加到数据消息上。这样，每个传感器数据都有一个时间戳，表示数据生成的时间
时间同步节点： 在ROS中，可以使用**ros::TimeSynchronizer类来实现多个传感器数据的时间同步。ros::TimeSynchronizer**允许将多个话题上的消息在时间上进行同步，确保它们具有相同的时间戳
消息过滤器： 另一个常用的工具是**message_filters库，其中包含了用于数据同步的类，如TimeSynchronizer和ApproximateTime**。这些类可用于确保多个传感器数据的时间戳在一定的时间窗口内相近

下面是一个简单的ROS代码片段，演示了如何使用**ros::TimeSynchronizer**进行两个传感器数据的时间同步：

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

void callback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
    // 处理同步后的传感器数据
    // image_msg 和 pc_msg 具有相同的时间戳
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sensor_sync_node");
    ros::NodeHandle nh;

    // 创建订阅器
    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image_topic", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "pointcloud_topic", 1);

    // 设置时间同步器，同步 image_topic 和 pointcloud_topic 的消息
    typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, pc_sub);
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();

    return 0;
}