多传感器时间同步问题记录

1. 多传感器时间同步问题描述

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

1.每个传感器拥有自己的内部时钟，时钟之间存在“钟漂”，导致各传感器的时间基准不一致；

2.不同的传感器采样频率不一样；

3.数据传输、Camera 曝光等会产生不可控的延迟。

2. 多传感器时间特点

传感器	时间特点
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 集成，不需要额外考虑。

3. 统一时钟源

为了解决"钟漂"问题，我们引入了统一时钟源的概念。统一时钟源的作用是提供一个所有传感器都遵循的共同时间参考。

3.1 PPS + GNSS

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 进行时间同步的原理。

3.2 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）。

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

4. 时间戳误差

完成时钟源的统一后，每个传感器数据都有了全局一致的时间参考。但会面临一个新问题，不同的传感器采样频率不一样，比如激光雷达（通常为 10Hz）和相机（通常为 30Hz）。导致在特定时间获取同步数据可能会有延迟，在动态环境中可能造成较大的误差。

如下图所示，三个传感器具有不同的采样频率。在 T1 时刻，传感器2 有一个数据，此时，我们需要对应传感器1 和 3的数据是多少，就会进行查找。查找的方式就是找对应的传感器数据和传感器2时间差最近的数据包。如果查找的数据包时间和 T1 时刻传感器2 数据包的差距较大，在加上车身和障碍物都在移动，这样误差会比较大。为了缓解查找时间戳造成的误差现象，主要采用的方式有硬件同步和软件同步。

4.1 ROS时间对齐

ROS自带的同步工具包：message_filters，它可以实现对多个话题进行时间同步，并且支持不同话题的时间误差。

4.2 自定义软件时间同步

自定义软件时间同步的核心思想是：根据不同传感器的时间戳，计算出一个参考时间，然后将所有传感器的时间戳对齐到这个参考时间。对齐的过程中，需要考虑不同传感器之间的时间误差，以及传感器数据的采集频率。对齐后的时间戳，可以用于后续的时间同步和数据处理。

例如：共享内存的方式将一个传感器的时间戳作为参考时间，其他传感器在读取共享的时间戳后修改自己的消息为参考时间戳，这样所有传感器的时间戳就对齐了。参考代码如下：

// 定义共享内存位置
const char *user_name = getlogin();
std::string path_for_time_stamp = "/home/" + std::string(user_name) + "/timeshare";
const char *shared_file_name = path_for_time_stamp.c_str();
int fd = open(shared_file_name, O_RDWR);
pointt = (time_stamp *)mmap(NULL, sizeof(time_stamp), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

// 读取时间
if(pointt != MAP_FAILED && pointt->low != 0)
{
    // 赋值共享内存中的时间戳给相机帧
    int64_t b = pointt->low;
    double time_pc = b / 1000000000.0;
    camera::refframetime = ros::Time(time_pc);
}
else
{
    camera::refframetime = ros::Time::now();
}

5. 参考链接

https://www.bilibili.com/video/BV15cK6ziE6Z
https://keymotek.com/solution_datalogging/sensor-time-synchronization/
https://github.com/xmcchv/HIKROBOT-MVS-CAMERA-ROS