多传感器融合之时间同步（一）
1.多传感器融合为什么要时间同步
- 每个传感器都有自己的时钟源
- 不同传感器具有不同的采样频率
- 另外数据传输、camera曝光等都会产生不可控的延迟
为了有效融合多个传感器的感知数据，必须进行时间同步。
2. 各传感器时间戳
| 传感器 | 时间戳 |
| GPS/GNSS时间戳 | GPS时间指的是GPS原子时，是以UTC时间1980年1月6日0时0分0秒为时间基准，以后按照TAI秒长累计计时 |
| camera时间戳 | 自动驾驶上使用rolling shuttercamera是支持外部触发曝光的，但由于camera帧周期包括曝光时间和readout时间（整帧像素点读出），所以需要关注曝光时间，对于相同cmos芯片的camera，其readout时间是固定的，来反推图像真实时间戳（一般采用曝光中间时间） |
| lidar时间戳 | 目前自动驾驶中所使用的lidar，从硬件层面上就支持授时，即有硬件trigger触发LiDAR的数据，并给这一帧数据打上时间戳，LiDAR通常有两种时间同步接口：基于IEEE 1588的以太网时间同步机制和PPS+NMEA协议 （基于GNSS的时间同步机制） |
| 毫米波雷达时间戳 | 目前主流的车载毫米波雷达采用FMCW调制方式，其上电后开始进行信号的发送和接收，内部有专门的时间机制，无法接收外部的时间。另外毫米波雷达周期性发送can信号，所以可以从can信号中获取数据时间 |
| IMU时间戳 | 一般IMU与GNSS集成在一起的，假设集成在FPGA上，那么接收FPGA输出的高精度时间脉冲，从而将传感器信号打上高精度时间戳 |
3. 统一时钟源
通过统一的时钟源给各传感器提供相同的基准时间，各传感器根据提供的基准时间校准各自的时钟时间，从硬件上实现时间同步。因为时钟源都有钟漂，而且每个时钟源钟漂不同，所以即使把各个传感器时间戳在初始时刻对齐，运行一段时间之后，之前对齐的结果就会偏离。解决这个问题的办法就是在硬件上把时钟源统一。
在自动驾驶的传感器配置里，GNSS接收机是必不可少的设备之一，GNSS中导航卫星内置高精度原子钟，GNSS接收机通过解算导航卫星信号，可以获得超高精度的时钟信号。GNSS信号能够达到定位要求时，自身时钟也会受到卫星上原子钟的校正，从而进一步提高精度。
一般可以采用GNSS时间作为统一时间源，通过GNSS给各个传感器提供基准时间，各传感器根据提供的基准时间校准各自的时钟时间；除了GNSS之外，根据需求还可以加入PTP和NTP授时方案，利用网络来进行时间同步。
几个概念：
- GNSS授时主要原理是通过PPS+NMEA（例如GPRMC）来授时，GPRMC报文提供年月日时分秒信息（一般通过波特率9600串口发送接收，处理时间为毫秒级别）、PPS则通常使用1PPS即频率为1次每秒的秒脉冲，接入工控机，当工控机支持时，设备接收到PPS脉冲后，毫秒及以下位清零重新计算。整体的时间就是与GPRMC报文中的UTC时间整秒相加，得到系统时间。具体可以参考PPS+GPRMC授时原理。
- NTP(Network Time Protocol)是用于不同计算机之间同步时钟的网络协议。它的设计目标是使所有的互连的机器之间的时钟与UTC时间只相差若干毫秒。
- PTP(Precision Time Protocol，1588 V2)是基于以太网的高精度时钟同步协议，能够实现以太网中多个从节点（各种传感器）与主节点（主机）之间的亚微秒级时钟同步，前提是所有节点之间都通过以太网互联，交换机支持PTP协议，并且每个节点都支持PTP协议。PTP通过在主从设备之间交互同步报文，并记录下报文发送时间，从而计算网络传输延迟和主从设备间时钟的偏差。具体可以参考PTP（IEEE1588），TSN时间同步方法
PPS：pulse per second，秒脉冲。1PPS = 1Hz = 1次/秒。就是一秒钟一个脉冲信号，用来指示整秒时刻，该时刻通常由秒脉冲的上升沿来标示。秒脉冲精度可达到ns级别，并且没有累积误差。
目前主流时间同步方案是以GNSS时间为基准时间，采用PTP/gPTP时钟同步协议来完成各传感器之间的时间同步，PTP 前提是需要交换机支持PTP协议，才能实现高精度同步。PTP是在硬件级别实现时间同步，而NTP是在应用层级别实现的，相对于NTP，PTP适用于精度要求高的数据采集设备之间的时间同步。
为什么有了GNSS授时，还需要加上PTP授时？
- PPS是无屏蔽的单线脉冲信号，极易受到车内恶劣电磁环境的干扰，届时根本无法区分出是干扰脉冲还是同步脉冲。
- 卫星信号容易受到楼宇、天气等因素的影响，GNSS时钟源丢失的时候，所有需要时间同步的设备都将无法获取时间数据。
如下图，集成了GNSS、PTP、NTP三种方式来统一时钟源，实际情况可以根据需求来配置。不同的传感器根据其自身特点可以选择不同的时间同步方式
时间硬同步（硬件时间同步）
在统一时钟源以后，只能保证时间差没有累计漂移了，但由于不同传感器各自采集周期相互独立，无法保证同一时刻采集数据。因此需要设计一种trigger来同时触发不同的传感器成像。GNSS系统除了可以作为统一的时钟源外，还可以利用其PPS脉冲来触发trigger对传感器下达收集数据的指令，以此来改变传感器的数据采集频率，在指定时间点采集数据。
如果传感器支持硬件触发的情况下，可以采用GPS时间戳作为基准进行硬件触发，这时传感器给出的数据中包含的时间戳即为全局时间戳（GPS时间戳）而非传感器时间戳。GNSS传感器自带秒脉冲发生器，所以可以直接使用。
以lidar和camera为例，在到达指定GPS时间后，同时触发数据采集信号。方式是以PPS信号为触发信号，实现lidar和camera均在PPS信号上升沿的时候采集数据，且打上各自时钟的时间戳。由于PPS的频率只有1Hz，所以通常需要一个设备把PPS信号转发为任意频率、但是跟PPS信号同相位的方波，这样就可以控制camera的采集频率了。
lidar制造商想出了一个相位锁定功能，也就是输入PPS，当PPS上升沿到来时，lidar的激光束恰好旋转到一定的角度。例如设置lidar的相位锁定角度为camera视野中央，如果camera视野朝着车辆正前方，lidar的坐标系也是朝着车辆正前方的话，相位锁定角度应该是0度。那么问题就这样解决了，每当lidar的激光束旋转到0度位置，也就是camera视野正中央（下图X轴方向），PPS上升沿刚好到来，camera也因此触发，就这样就实现了lidar和camera的同步采集。
但是，由于lidar和camera采集数据的方式不同，我们只能近似地保证lidar和camera的同步采集。如下图所示，lidar的激光束不断地360度旋转，假设帧率为10Hz的话，那么一帧点云中采集时刻最早的一个点和采集时刻最晚的一个点时间相差100ms。而camera就不同了，camera是瞬间曝光的，因此图像里面所有像素点的采集时刻都是一样的。这就导致了我们只能近似地保证lidar和camera的同步采集。为什么说是近似的呢？对于camera视野中央的那些点云，它们的采集时间是跟图像采集时间一致的，但是对于camera视野边缘的那些点云，它们的采集时间就跟图像采集时间要有一定的时间偏差了，根据camera视野的大小和点云采集帧率的不同，时间偏差可能会有5ms~20ms左右。
使用触发装置同步效果
时间软同步（软件时间同步）
仅仅硬件同步无法弥补各个传感器的频率在几个周期内都无法重叠的问题，例如相机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传过来的数据（也就是缓冲区中的数据），而是将各个缓冲区建立时间线，然后根据时间线上的各个时间点来判断是否需要输出。
import message_filters
from sensor_msgs.msg import Image, CameraInfo
def callback(image, camera_info):
# Solve all of perception here...
image_sub = message_filters.Subscriber('image', Image)
info_sub = message_filters.Subscriber('camera_info', CameraInfo)
ts = message_filters.TimeSynchronizer([image_sub, info_sub], 10)
ts.registerCallback(callback)
rospy.spin()- 通过TimeSynchronizer统一接收多个topic，只有在所有的topic都有严格相同的时间戳时才输出对应的结果
- 而ApproximateTimeSynchronizer则将多个topic上时间相近的数据同时输出
对于那些没有对齐的数据，进行同步处理后，并没有输出出来。最终pub出来的只是原始数据的子集。因此ros message_filters相当于对不同的数据都进行了下采样，只是输出时间轴上同步或近似同步的数据，不能做到主动去同步
如何选择时间同步方式
基本准则
- 统一时钟源是基本要求
- 如果要实现前融合，选择在硬件层实现实现同步，保证采集的数据是同一时刻的。利用PPS脉冲触发相机曝光是一个较好的选项
- 基于插值法的软同步是针对目标级别的推算，需要对前后帧的同一个目标的数据进行加权，也就要求了需要插值的传感器能够独立对目标进行跟踪，这种一般只适用于后融合场景，
不同类型的设备如何选择时间同步方案
- 通过GNSS时钟源给计算平台授时
- 计算平台部署PTP Master，NTP Server通过以太网接入计算平台(可选)
- 设备如果支持PTP协议，如Lidar，则将设备作为PTP slave，通过以太网接入交换机，进而连接计算平台中的PTP master
- 设备如果不支持PTP协议，如Camera，则可以利用Trigger Box接入GNSS系统，根据Lidar的采样周期同步触发Camera曝光
- FMCW Radar由于其特殊的成像原理，一般使用基于CAN的时间同步协议
小结
1）时间对齐：首先保证时钟源是一致的
把各个传感器时间戳在初始时刻对齐，硬件上把时钟源统一，通常利用的是GNSS自带的秒脉冲发生器PPS，捕获GNSS输出的pps脉冲信号上升沿作为绝对整秒的开始，在连续接收到两个pps脉冲时，将上一pps对应时间加一秒进行授时，同时清零主控单元内部累加计数器，保证内部秒脉冲与GNSS的PPS秒脉冲的信号边沿对齐，从而实现两者之间时间统一，建立高精度的时间基准。必要时使用PTP、NTP作为外部时钟源。
2）时间同步：获取同一时刻的信息
在解决硬件同步以后，只能保证时间差没有累计漂移了，但是各个传感器的采集时刻并不是相同的。这就需要对数据进行同步。
同步就是插值，具体来说是内插外推，两个时间点之间的数据用插值，时间点之外的，用历史数据进行推测。
关于插入时刻问题，因为我们是以雷达作为核心传感器，所以每收到一次雷达数据，就以当前雷达数据采集时刻作为要插入的时间点，从而获得除雷达以外的其他传感器的同一时刻等效信息。