自动驾驶中的多传感器时间同步

前言

1.多传感器时间特点

2.统一时钟源

2.1 时钟源

① ROS 中的 message_filters 包

② 双端队列 std::deque

参考：

前言

对多传感器数据（Lidar，Camera，GPS/IMU）进行高精度的时间同步的原因：

1.每个传感器拥有自己的内部时钟，时钟之间存在“钟漂”，导致各传感器的时间基准不一致；
2.不同的传感器采样频率不一样；
3.数据传输、Camera 曝光等会产生不可控的延迟。

1.多传感器时间特点

传感器	时间特点
GNSS	GNSS 接收机在接收到卫星信号后，通过解算即可获得接收机系统时间与卫星原子钟之间钟差，并通过钟差来校准自己的系统时间，完成授时功能。
Camera	自动驾驶上使用的相机（Rolling Shutter）一般是支持外部触发曝光的。 Camera 帧周期包括曝光时间（exposure time）和读出时间（readout time，cmos 相同时固定）。
Lidar	自动驾驶中所使用的 Lidar，例如 Mid-360，从硬件层面上支持PPS+NMEA协议（PPS硬件触发，GPRMC授时）。LiDAR 通常支持两种时间同步接口：基于以太网的PTP/gPTP时间同步和基于GPS的PPS+NMEA协议。
Rader	主流的车载毫米波雷达采用 FMCW 调制方式，上电后开始进行信号的发送和接收，内部有专门的时间机制，无法接收外部的时间。但毫米波雷达周期性发送 CAN 信号，所以可以从 CAN 信号中获取时间信息。
IMU	一般和 Lidar/GNSS 集成，不需要额外考虑。

2.统一时钟源

为了解决“钟漂”问题，提供一个所有传感器都遵循的共同时间基准，我们引入了统一时钟源的概念，如下图所示：

统一时钟源有两种常见方式：一种是基于 GPS 的 “PPS+NMEA” （秒脉冲+GPRMC），另一种是基于以太网的 PTP（IEEE 1588）、gPTP（IEEE 802.1AS）时钟同步协议。自驾领域主流的方案是结合上述两种方案，以 GNSS 时钟时间为基准时间，采用 PTP/gPTP 时钟同步协议统一各传感器/域之间的时钟源，完成时间基准的统一。

2.1 时钟源

        自动驾驶系统目前绝大多数标配高精度 GNSS 接收机，而 GNSS 中导航卫星内置高精度原子钟，GNSS接收机通过解算导航卫星信号（大于等于 4 颗卫星的信号），可以获得接收机系统时间与卫星原子钟之间钟差，并通过钟差来校准自己的系统时间，得到超高精度的时钟信号，这就是GNSS 的授时功能。
        原子钟是人类目前最精确的时间测量仪器，原子在不同能级之间的移动称为“跃迁”，且由高能级跃迁到低能级时，会释放电磁波。而对同一种原子来说，这种频率是固定的，且不受温度和压力影响，只与自身能量有关，物理学上称之为“共振频率”。物理学家通过一些物理手段，获得共振频率的准确物理值。并以此值作为产生时间信号的基本节拍，即丈量时间的基本单位。据相关报道，北斗三号卫星上的原子钟300年才会有1s累积误差。

2.2 PPS+GPRMC

GNSS 接收机获取时钟信号后，会输出两类同步信号：①同步脉冲信号 PPS。其时间周期为 1s ，脉冲宽度5ms~100ms；②GPRMC 时间同步报文。通过标准串口输出，符合 GPRMC 格式，用于提供精确的时间同步信息。

GPRMC 是一种包含UTC时间（精确到秒），经纬度定位数据的标准格式报文。其格式如下：

# 示例数据
$GPRMC,001155.00,A,2237.496474,N,11356.089515,E,0.0,225.5,230520,2.3,W,A*28

# 数据说明
field 0：$GPRMC, 格式ID，表示该格式为建议的最低特定GPS / TRANSIT数据（RMC）推荐最低定位信息
field 1: UTC时间, 格式hhmmss.ssss，代表时分秒.毫秒
field 2: 状态 A:代表定位成功 V:代表定位失败 
field 3: 纬度 ddmm.mmmmmm 度格式（如果前导位数不足，则用0填充）
field 4: 纬度 N(北纬)  S(南纬)
field 5: 经度 dddmm.mmmmmm 度格式（如果前导位数不足，则用0填充）
field 6: 经度 E(东经) W(西经)
field 7: 速度（也为1.852 km / h）
field 8: 方位角，度（二维方向，等效于二维罗盘）
field 9: UTC日期 DDMMYY 天月年
field 10: 磁偏角（000-180）度，如果前导位数不足，则用0填充）
field 11: 磁偏角方向E =东W =西
field 12: 模式，A =自动，D =差分，E =估计，AND =无效数据（3.0协议内容）
field 13: 校验和

其中 PPS 前沿时刻与 GPRMC报文的发送在同一时刻，误差为 ns 级别，可以忽略。PPS 秒脉冲（通常为1PPS，即1次每秒）为物理电平输出，接收及处理 PPS 信号的时间在 ns 级别，依旧可以忽略。但 GPRMC数据一般通过波特率 9600 的串口发送，其发送、接收、处理时间 tx 在 ms 级别，是时间同步的关键。以下是使用 PPS+GPRMC 进行时间同步的原理。

（1）设备收到 PPS 秒脉冲信号后，将内部以晶振为时钟源的系统时间里的毫秒及以下时间清零，并由此开始计算毫秒时间。
（2）当收到 GPRMC 数据后，提取报文里的年、月、日、时、分、秒的 UTC 时间。
（3）将收到秒脉冲到解析出 GPRMC 中 UTC 时间所用的时间 tx，与 UTC 整秒时间相加，同步给系统时间，至此已完成一次时间同步。下一秒再进行相同的过程，每秒准确校准一次。

基于单纯的 PPS 和 GPRMC 实现整个自动驾驶系统的时间同步的局限：

PPS信号驱动能力不足：PPS 是一种低功率脉冲信号，驱动电流范围通常为 0.5mA 至 20mA，难以触发多个传感器的稳定工作。
PPS 信号抗干扰能力较弱：PPS 信号为无屏蔽的单线脉冲信号，在车辆复杂的电磁环境中容易受到干扰。当多条 PPS 线同时布置在车内时，可能难以区分干扰脉冲和有效同步脉冲，导致信号准确性下降。
GPRMC 报文的传输限制：GPRMC 通过 RS232 串口传输同步报文，RS232 为 1 对 1 的全双工通信形式，可通过主从方式实现有限的 1 对 N 数据传输。然而，若需支持十几台设备的同步传输，实际应用中可能面临困难，需通过实验验证可行性。此外，线束的复杂性会对工程设计造成较大挑战。导远 INS570D 车载组合导航定位系统的 PPS+GPRMC 信息如下：

若将其接入 Mid-360，则参考：Mid-360 时间同步说明

时钟源失效的严重后果：当时钟源丢失时，所有依赖同步的设备将失去统一基准，各设备时钟将自主运行，失去全局协调。这种情况在对功能安全要求极高的自动驾驶系统中是不可接受的，需要设计冗余机制以确保在主时钟故障时由备用时钟迅速接替，维持全系统的正常运行。

2.3 PTP

PTP(Precision Time Protocol，IEEE 1588 V2)是基于以太网的高精度时钟同步协议，是一种主从式的时间同步系统，能够实现主节点（Master Node）和从节点（Slave Node）之间的亚微秒级时钟同步，前提是所有节点之间都通过以太网互联，交换机支持 PTP 协议，并且每个节点都支持 PTP 协议。

设备中运行 PTP 协议的网络端口称为 PTP 端口，PTP主端口用来发布时间，PTP从端口用来接收时间。同时定义了三种时钟节点，边界时钟节点（BC，Boundary Clock）、普通时钟节点（OC，Ordinary Clock）和透明时钟节点（TC，Transparent clock）。

（1）边界时钟节点拥有多个PTP端口，其中一个用来同步上游设备时间，其余端口用来向下游设备发送时间。当边界时钟节点的上游时间同步设备是GNSS接收机时，此时的边界时钟节点就是一个主时钟节点（最优时钟）
（2）普通时钟节点只有一个PTP端口，用来同步上游时钟节点的时间。
（3）透明时钟节点具有多个PTP端口，收到什么时间，转发什么时间，不进行协议解析，内部不参与时间同步。

PTP通过在主从设备之间交互同步报文，并记录下报文发送时间，从而计算网络传输延迟和主从设备间时钟的偏差。PTP定义了四条同步报文：Sync、Follow_Up、Delay_Req、Delay_Resp，精确同步过程如下：

（1）PTP 主端口向从端口发送 Sync 报文，同步记录下 Sync 发送的时间 t1。从端口收到 Sync 报文后，记录下收到的时间 t2。
（2）紧接着主端口将 t1 时间放到 Follow_Up 报文发送给从端口，从端口收到此报文后就可以解析出 t1，并由此得到第一个方程式：t1 + T_delay（网络延时）+ T_offset（时钟偏差）= t2。
（3）从端口向主端口发送 Delay_Req 报文，同步记录下 Delay_Req 发送的时间 t3。主端口收到报文后，记录下收到的时间 t4。
（4）紧接着主端口将 t4 时间放到 Delay_Resp 报文发送给从端口，从端口收到此报文后就可以解析出 t4，并由此得到第二个方程式：t3 + T_delay（网络延时）- T_offset（时钟偏差）= t4。

这里假设网络延迟是对称的，即上下行的延迟相等。解方程组得：

$T_{delay}=\frac{[(t2-t1)+(t4-t3)]}{2}$

$T_{offset}=\frac{[(t2-t1)-(t4-t3)]}{2}$

2.4 全域架构时间同步方案

        全域架构下，智驾域控制器因为直接连接GNSS接收机（或内置），而GNSS又是绝佳的时钟源，因此智驾域控制器自然而然成为主时钟节点，中央网关域控制器通过车载以太网主干网串联起其它域控制器，自然而然成为边界时钟的最佳选择，这样在时钟源丢失的时候，边界时钟节点同步主时钟节点的系统时间，仍然可以保持整个全域架构内相对时间一致。
        其它域内传感器、执行器的时间同步需求，若没有，此域控制器设计成普通时钟节点即可。如有，可以设计成边界时钟，以保证无时钟源时的相对时间统一。
        基于以太网设备的时间同步方案已经完善，而对于非车载以太网设备但有非常强烈同步需求的相机，我们还得特殊处理一下。将相机设置为外触发模式，通过主控给相机外触发脉冲信号，即 PPS。相机拍照时，曝光时刻也会产生脉冲信号发送给主控，主控记录此时系统时间，并将时间戳数据放到相机的图像数据里。

3.时间戳误差

完成时钟源的统一后，每个传感器数据都有了全局一致的时间参考。但会面临一个新问题，不同的传感器采样频率不一样，比如激光雷达（通常为 10Hz）和相机（通常为 30Hz）。导致在特定时间获取同步数据可能会有延迟，在动态环境中可能造成较大的误差。
如下图所示，三个传感器具有不同的采样频率。在 T1 时刻，传感器2 有一个数据，此时，我们需要对应传感器1 和 3的数据是多少，就会进行查找。查找的方式就是找对应的传感器数据和传感器2时间差最近的数据包。如果查找的数据包时间和 T1 时刻传感器2 数据包的差距较大，在加上车身和障碍物都在移动，这样误差会比较大。为了缓解查找时间戳造成的误差现象，主要采用的方式有硬件同步和软件同步。

3.1 硬件同步

硬件同步是一种通过物理信号来确保不同传感器数据采集时间一致性的方法。一种常见的硬件同步方法是使用 PPS 信号作为触发器。PPS 信号是一个精确的时钟信号，可以触发传感器在特定的时间点采集数据，以此来改变传感器的数据采集频率。GNSS系统除了可以作为统一的时钟源外，还可以利用其 PPS 脉冲来触发传感器在特定的时间点采集数据，当使用 GNSS 的 PPS 脉冲时，传感器给出的数据包中的时间戳即为对齐到绝对时间的上的全局时间戳（GPS时间戳）而非传感器时间戳。由于 GNSS 的 PPS 的频率通常只有 1Hz，所以通常需要一个设备把 PPS 信号转发为任意频率（分频，1Hz -> 10Hz）、但是跟原始 PPS 信号同相位的方波，这样就可以控制各传感器的采集频率了。
例如，激光雷达和相机可以配置为在 PPS 信号的上升沿采集数据，从而确保两者的数据采集是同步的。具体来说，激光雷达可以利用其相位锁定功能来实现与 PPS 信号的同步，如下图所示。通过设置激光雷达的相位锁定角度与相机视野的中心对齐，可以在激光雷达的激光束旋转到相机视野中心线时触发相机，实现两者的同步采集。

当然，由于激光雷达是连续旋转采集数据，而相机则是瞬间曝光，因此硬件同步只能近似实现。例如，激光雷达的帧率若是 10Hz，那么一帧点云中最早和最晚采集的点之间的时间差可能达到 100ms。相机由于曝光是瞬时的，其所有像素点的采集时刻是一致的。因此，对于相机视野中心的点云，采集时间与图像采集时间一致，但对于视野边缘的点云，存在一定的时间偏差，这个偏差可能在 5ms 到 20ms 之间。

3.2 软件同步

软件同步是一种在数据处理阶段对传感器数据进行时间校正的方法。当硬件同步无法实现或不足以满足系统要求时，软件同步提供了一种解决方案，利用已知的时间标签和传感器的运动信息来推算传感器数据的准确时间点。
内插外推法是软件同步中常用的一种算法。通过以下步骤实现同步：

时间差计算：首先，计算两个传感器数据帧之间的时间差。例如，如果有一个激光雷达（Lidar）数据帧和一个相机数据帧，它们的时间标签可能不同，我们需要找出这两个时间标签之间的差异。
运动信息获取：收集传感器在两个时间标签期间的运动信息，这通常包括速度、加速度和旋转等。
位置推算：利用传感器的运动信息和时间差，通过物理模型或机器学习模型推算目标在两个时间点之间的位置变化。
建立新帧：根据推算出的目标位置，创建一个新的数据帧，这个新帧代表了两个原始数据帧之间的某个时间点的状态。

软件同步通过智能的数据处理技术弥补了硬件同步的不足，提高了传感器数据的同步精度，当然，它也需要额外的计算和实时性要求，需要精心设计和优化算法来实现高效准确的同步。

3.2.3 其他方式

① ROS 中的 message_filters 包

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

message_filters 只是输出时间轴上相近的不同传感器的数据，不能做到主动去同步！详情参考 ROS时间同步----使用message_filters进行时间软同步。

② 双端队列 std::deque

使用双端队列 std::deque 存储不同传感器的数据，根据不同传感器数据的时间戳进行判断，对满足时间同步要求的数据进行处理。类似 message_filters 包，只是输出时间轴上相近的不同传感器的数据。

详情可参考：从零开始做自动驾驶定位(六): 传感器时间同步

std::queue buf1;
std::queue buf2;
std::thread process;
//typedef M 传感器数据类型，比如sensor_msg::PointCloud2、sensor_msg::Image等等
void callback1(M& msg){
	//数据入队
	buf1.push(msg);
	//其他代码...
}
void callback2(M& msg){
	//数据入队
	buf2.push(msg);
	//其他代码...
}
void process_comparation(){
	while(ros::ok()){
		M data1=buf1.front();
		M data2=buf2.front();
		if(data1.header.timestamp.toSec()>data2.header.timestamp.toSec()){
			buf2.pop();
		}
		else 
			buf1.pop();
		//其他操作data1和data2的代码
	}
}
int main(int argc,char** argv){
	//initialization
	
}