【自动驾驶传感器融合系列】02自动驾驶中的多传感器同步理论

文前白话

同步理论整体架构：

1、传感器时空同步概念

概述：

传感器间空间同步，就是不同传感器坐标系下的测量值转换到同一坐标系下（也可以理解为传感器在整车坐标系下的标定参数）。传感器标定是自动驾驶的基本需求，也是多传感器融合的基础，需要首先确定各传感器之间的坐标关系以及各传感器与整车坐标系之间的转换关系。同样，理解空间同步的本质，可以拓展到车体坐标系如何转换到世界坐标系下的过程。

2、传感器时间同步

概述：

自动驾驶除了空间坐标系需要进行精确同步外，各个设备(或者传感器)之间的时间坐标系也需要进行同步。只要提到传感器融合，时间同步就是不可避免的操作步骤。
时间同步就是通过统一的主机给各个传感器提供基准时间，各传感器根据已经校准后的各自时间为各自独立采集的数据加上时间戳信息，以做到所有传感器时间戳同步。

· 时间硬同步

相关的概念：

• 传感器时间戳：（一些时间的标准）

TAI：即International Atomic Time，称为国际原子时，一种国际参照时标，是均匀的时间尺度，与地球的空间位置不关联，起点为1958年1月1日0时0分0秒。

UTC，即Coordinated Universal Time，它是最主要的世界时间标准，是基于国际原子时，并通过不规则的加入闰秒来抵消地球自转变慢的影响。所以又被称为世界标准时间。

GMT , Greenwich Mean Time

系统时间，一般指的是SOC所搭载的系统对应的时间，是以UNIX或者UNIX系统使用的时间进行表示，一般定义为从UTC时间的1970年1月1日0时0分0秒起至现在的总秒数（10位是精确到秒，13位是精确到毫秒），如果考虑闰秒的话，也需要加上经闰秒调整后的总秒数。所以，系统时
间戳，有时候也称为Unix或POSIX时间戳。

传感器	时间机制
GPS/GNSS时间戳	GPS时间指的是GPS原子时，是以UTC时间1980年1月6日0时0分0秒为时间基准，以后按照TAI秒长累计计时。
相机时间戳	自动驾驶上使用rolling shutter相机是支持外部触发曝光的，但由于相机帧周期包括曝光时间和readout时间（整帧像素点读出），所以需要关注曝光时间，对于相同cmos芯片的相机，其readout时间是固定的，来反推图像真实时间戳（一般采用曝光中间时间）。
激光雷达时间戳	目前自动驾驶中所使用的激光雷达，从硬件层面上就支持授时，即有硬件trigger触发LiDAR的数据，并给这一帧数据打上时间戳，LiDAR通常有两种时间同步接口：基于IEEE 1588的以太网时间同步机制和PPS+NMEA协议 （基于GPS的时间同步机制）。
毫米波雷达时间戳	目前主流的车载毫米波雷达采用FMCW调制方式，其上电后开始进行信号的发送和接收，内部有专门的时间机制，无法接收外部的时间。另外毫米波雷达周期性发送can信号，所以可以从can信号中获取数据时间。
IMU时间戳	一般IMU与GPS集成在一起的，假设集成在FPGA上，那么接收FPGA输出的高精度时间脉冲，从而将传感器信号打上高精度时间戳。

• 传感器的一般频率取值：

由于每种传感器的采样频率不一致，如LiDAR通常为10Hz，camera通常为25或者30Hz，不同传感器之间的数据传输还存在一定的延迟，那么可以通过寻找相邻时间戳的方法找到最近邻帧，但如果两个时间戳相差较大，且传感器或障碍物又在运动，那么最终会得到较大的同步误差。如下图展示：

传感器类型	常见的频率
车载相机	15Hz 、20Hz、25Hz、30Hz等
车载激光雷达	10Hz
车载毫米波雷达	13~15 Hz
IMU	100~200 Hz

**时间硬同步，**就是通过唯一的时钟源给各传感器提供相同的基准时间，各传感器根据提供的基准时间校准各自的时钟时间，从硬件上实现时间同步。
目前自动驾驶中主流时间同步是以GPS时间为基准时间 ，采用PTP/gPTP时钟同步协议来完成各传感器之间的时间同步，PTP 前提是需要交换机支持PTP协议，才能实现高精度同步。
与PTP同时出现的还有一种NTP，即网络时间协议，不同的是PTP是在硬件级实现的，NTP是在应用层级别实现的。

整车的时间硬同步示意图：

GNSS与IMU时间同步下的数据融合示意图：

• 1、统一时钟源

由于每个传感器都有自己的时间戳，这里 统一时钟就是来同步不同传感器时间戳的。如果传感器支持硬件触发的情况下，可以采用GPS时间戳作为基准进行硬件触发，这时传感器给出的数据中包含的时间戳即为全局时间戳（GPS时间戳）而非传感器时间戳。不同传感器之间统一时钟源效果的示意图如下：

• 2、硬件同步触发

由于每种传感器的采样频率不一致，并且不同传感器之间的数据传输还存在一定的延迟，如果两个时间戳相差较大，且传感器或障碍物又在运动，那么最终会得到较大的同步误差。这种情况下，可以采用硬同步触发的方法来缓解查找时间戳造成的误差现象，也可以调整传感器的固有频率，如将相机调整为20Hz，减少时间差问题。

硬件同步触发示意图：

· 时间软同步

经过时间的硬同步，为什么还要进行软同步？

： 考虑到相机成像、传输过程以及毫米波雷达不支持硬件触发(毫米波无法接收外界信号触发)等情况，在算法上，还要进行传感器之间的时间软同步。

时间软同步的方法有：时间索引----时间直接配准法、时间插值----时间最小二乘拟合法、内插外推法 以及虚拟融合方法，泰勒展开方法等等其他方法，。

• 1、直接配准法：
• 适合传感器之间的帧率具有整数倍数关系，以频率低的为基准，找出时间戳
  对应的数据即可。如下图，这种方法误差比较大，而且要求传感器之间的帧率是整数倍。

• 2、时间最小二乘拟合法：

• 最小二乘法是时间配准中常用的一种方法，适合帧率非整数倍关系的传感器之间的时间对齐。就是将其中一个传感器的观测数据通过某种特定的拟合原则得到一个关于时间的曲线，然后通过该曲线得到与另一个传感器对应时刻的数值，即可得到同一时刻两种种传感器的数据配准。如下：

• 3、内插外推法：

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

• 4、其他方法：

其他方法如：虚拟融合方法，泰勒展开方法等等，还需要实际情况提出种种改进措施。

3、传感器时空同步

概念：运动补偿

即是对传感器测量过程，物体的运动或者传感器的运动，造成实际的运动数据偏差，进行量化弥补。

运动补偿情况主要分为两类：

• 自身车辆运动引起偏差（传感器在采集过程中的某一时间戳内，由于车辆自身的运动，传感器坐标系原点发生了变化，即采集的对象在该时间戳内发生相对位移变化。）

• 非自身车辆运动引起的偏差（传感器在采集的过程中，由于目标运动造成的相对位移的变化（这个在相机中最明显，如拖尾现象）。）
  

运动补偿的方法

• 1、纯估计方法

纯估计方法一般是基于激光雷达匀速假设，通过帧间点云匹配来计算激光雷达的位置来进行线性补偿的。常用的方法包括ICP(Iterative Closest Point，迭代最近点算法)以及其相关的变种(VICP)，以及ANN(Approximate Nearest Neighbor)。

VICP缺点：
◆ 低频率激光，匀速运动假设不成立；
◆ 数据预处理和状态估计过程耦合；

• 2、里程计辅助方法

里程计辅助方法是利用IMU信息对激光数据中每个激光点对应的传感器位姿进行求解，即求解对应时刻传感器的位姿，然后根据求解的位姿把所有激光点转换到同一坐标系下，然后封装成一帧激光数据，发布出去（可以理解为激光点云的去畸变）。

Odom 与 IMU ：

• 极高的位姿更新频率，能够比较准确地反应运动情况
• 较高精度的局部位姿估计
• 跟状态估计完全解耦

具体的处理过程：

• ① 通过IMU与点云数据时间对齐，然后对每个点进行速度补偿，通过时间戳进行线性插值，然后将所有点云数据转换到初始点云的IMU坐标下；

• ② 与上一帧的去完畸变的点云数据进行帧间匹配，计算激光姿态。并通过计算的姿态对每个点云进行线性补偿，将所有的点云数据根据时间戳转换到最后一个点云数据时间戳下，即完成了里程计方法的补偿。

• 3、融合的方法 ： Odom与匹配方法的结合

处理过程：

• 用里程计方法进行矫正，去除绝大部分的运动畸变；
• 认为里程计存在误差，但是误差值是线性分布的；(注意：位置误差的线性假设比位置线性假设更合理！！！)
• 用ICP的方法进行匹配，匹配的结果作为真值，得到里程计的误差值；
• 把误差值均摊在每一个点上，重新进行激光点位置修正；
• 再次进行ICP迭代，直到收敛为止；

如下图，绿色部分为原始的点云数据，粉色为去除运动畸变的点云数据，很明显前半部分粉色的点云数据超前绿色的点云数据，后半部分的点云数据近乎重合。局部放大发现，粉色的点云数据超前于绿色的点云数据，但是形状依旧类似，怀疑与没有IMU数据校准相关。