VIO系统的IMU与相机时间偏差标定

  视觉里程计（VIO）作为一种空间定位方法，广泛应用于VR/AR、无人驾驶和移动机器人，比如近年火热的苹果 AR-Kit和谷歌AR-Core都使用了VIO技术进行空间定位。通常，VIO系统忽略IMU与相机时间偏差，认为IMU和相机时间是同步和对齐的，然而由于硬件系统的触发延时、传输延时和没有准确同步时钟等问题，IMU和相机之间通常存在时间偏差，估计并纠正这个偏差将有效提升VIO系统的性能。本文将介绍一种简单有效方法，该方法发表与IROS2018，是港科大沈劭劼老师团队的成果，并且该方法对应的文章获得了IROS2018 Best Student Paper，特别值得我们一起看一看。

 图1-1 VIO系统 IMU和Camera # 1. 介绍   一般的视觉里程计(VIO)系统包含两个传感器，一个视觉传感器（相机）和一个惯性传感器(IMU),如图1-1。系统对两个传感器进行采样，获得相应的数据（图像、IMU数据）与对应的采样时间戳。通常，我们假设获得的采样时间戳即为传感器采样时刻对应的时间，比如相机曝光时刻（通常曝光持续几毫秒到几十毫秒，认为曝光时刻为曝光持续的中间时刻），然而，系统存在的触发延时、传输延时和没有准确同步时钟等问题，使得相机和IMU之间的时间不对齐。  图1-2 - 相机和IMU数据流时间不对齐   如图1-2，上方为相机真实的采样时间点，下方为对应采样记录的时间戳，相机时间戳与真实采样时间存在一个$t_d$的时间偏差。相机与IMU数据流时间戳上的偏差将影响VIO系统的工作，比如在[VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono)系统中，两个KeyFrame图像之间的IMU预积分项将因为时间不对齐而不准确，从而给状态估计带来偏差。因此，IMU与相机时间对齐标定是一个重要的问题，笔者准备对几种IMU与相机时间对齐标定方法进行整理和总结。

IMU和相机时间偏差标定的几种方法包括：

• 港科大沈劭劼团队 - 基于VINS-Mono的在线时间标定方法
• 李名扬 - 基于MSCKF的在线时间标定方法
• Kalibr 工具箱中离线的时间标定方法

  本文先对其中第一种方法，港科大沈劭劼老师团队的 基于VINS-Mono的在线时间标定方法[1]进行梳理。这篇文章获得了机器人领域顶级会议 IROS2018 的 Best Student Paper 奖项，是一篇很值得了解的文章。

2. 基于VINS-Mono的在线时间标定方法

• 文献：Online Temporal Calibration for Visual Intertial System
• 作者：Tong Qin and Shaojie Shen, HKUST
• 会议：IROS2018
• 荣誉：Best Student Paper

2.1 基本思想

  该方法的基本思想是将时间偏差作为状态变量的一部分，关键问题在于如何构建带时间偏差的VIO数学模型。设想，在IMU和相机存在时间偏差\(t_d\)的情形下,令\(t_{IMU} = t_{cam} + t_d\)，如图1-2，相机数据的时间戳比对应数据真实的采样时间戳大\(t_d\)。假设第\(k\)帧图像\(I_k\)对应的时间戳为\(t_k\)，那么其真实的采样时间为\(t_k - t_d\)。对于\(I_k\)上的一个特征点\(\mathbf{z}_l^k\),其图像坐标\([u_l^k, v_l^k]^T\),在\(t_k\)时间的位置已经发生偏移，如图2-1。

 图2-1 $t_k$时刻特征点位置发生偏移

作者希望估计\(t_k\)时(采样时间)特征点的位置，假设

特征点在短时间（比如\(t_d\)）内在图像平面内的运动为匀速运动。

那么\(t_k\)时刻估计的特征点坐标（时间偏差校正后的坐标）\(\mathbf{z}_l^k(t_d)\)为：

\[\begin{equation} \mathbf{z}_l^k(t_d) = [u_l^k, v_l^k]^T + t_d\mathbf{V}_l^k \tag{2-2} \end{equation} \]

其中，\(\mathbf{V}_l^k\)为特征点在像素平面内的速度，可通过前后两帧特征点的位置进行估计，即

\[\begin{equation} \mathbf{V}_l^k=(\begin{bmatrix}u_l^{k+1} \\ v_l^{k+1}\end{bmatrix} - \begin{bmatrix}u_l^k \\ v_l^k\end{bmatrix}) / (t_{k+1} - t_k) \tag{2-1} \end{equation} \]

  基于上述简单的假设，在VIO系统中将经时间偏差校正后的特征点坐标\(\mathbf{z}_l^k(t_d)\)（带参数\(t_d\)）替换之前的特征点坐标\(\mathbf{z}_l^k\)，从而将时间偏差参数\(t_d\)引入了优化方程相机残差项，即重投影误差可以写成：

\[\begin{equation} \mathbf{e}_l^k = \mathbf{z}_l^k(t_d) - \pi(\mathbf{R}_{wc_k}^T(\mathbf{P}_l-\mathbf{p}_{c_k})) \tag{2-3} \end{equation} \]

状态变量增加时间偏差\(t_d\)为

\[\begin{equation} \chi = [\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_n, \mathbf{P}_0, \mathbf{P}_1,\dots, \mathbf{P}_l, \color{red}{t_d}] \\ \mathbf{x}_k = [\mathbf{p}_k, \mathbf{v}_k, \mathbf{R}_k, \mathbf{b}_a, \mathbf{b}_g], k \in [0, n] \tag{2-4} \end{equation} \]

对于优化目标函数，将特征点重投影误差项替换为式(2-3)，形式与VINS-Mono原来保持一致，即：

\[\begin{equation} min\bigg\{ \Big\|e_p - \mathbf{H}_p\chi \Big\|^2 + \sum_{k\in B}{\Big\|\mathbf{e}_B(\mathbf{z}_{k+1},\chi)\Big\|_{\mathbf{P}_{k+1}^k}^2} + \sum_{ (l, j) \in C}{\Big\| \mathbf{e}_C(\color{red}{\mathbf{z}_l^j(t_d)}, \chi) \Big\|_{\mathbf{P}_l^j}^2} \bigg\} \tag{2-5} \end{equation} \]

此外，在每次优化之后，利用估计得到的\(t_d\)对相机数据流的时间戳进行修正，即新时间戳\(t_{cam}'\)

\[\begin{equation} t_{cam}'=t_{cam}\color{red}-t_d \tag{2-6} \end{equation} \]

  原文献中为"\(t_{cam}'=t_{cam}\color{red}+t_d\)"，我认为存在错误，比如假设\(t_d\)为正数，式(2-2)表示在\(t_k\)时刻，特征点\(l\)的位置向未来运动了\(t_d\)时间，即认为采样时刻为\(t_k - t_d\)，所以，对所有的时间戳纠正，使其更接近采样时间的方式应为\(t_{cam}'=t_{cam}\color{red}-t_d\)。
  下一次优化中，\(t_d\)表示修正后的相机时间戳和IMU时间戳之间的偏差，这个偏差在逐次优化中将不断收敛到零，从而使得“特征点在短时间（比如\(t_d\)）内在图像平面内的运动为匀速运动”的假设更加合理，这也可以看成是一种由粗到细corse-to-fine的模式。

2.2 实验

  文献作者已经将此方法开源集成在VINS-Mono系统中，其实验也是基于VINS-Mono系统的一些实验。作者从两个角度设计实验：

• 评估本方法时间偏差标定效果
• 评估本方法对VIO系统性能提升效果

2.2.1 时间偏差标定实验

  首先，作者设计了仿真实验。在仿真实验环境下，相机与IMU的时间戳偏差可调，并且GroudTruth值可知。作者的结果也显示出其能够有效估计时间偏差，在30ms的时间偏差下，时间偏差\(t_d\)估计的RMSE为0.68ms，具体数据参考原文，不在此赘述。
  其次，设计实验估计真实传感器的时间偏差。使用的传感器为RealSense ZR300系列相机（笔者刚好也使用过这款传感器），这是一款相对廉价（几千元级别）的消费级传感器。作者分别使用本文方法和Kalibr工具箱进行相机和IMU时间偏差标定。Kalibr标定方式相对本文方法有两个缺点，一是需要棋盘格Pattern，二是离线标定。也就是说Kalibr依赖外部环境和离线整体优化，虽然这样，本方法得到了和Kalibr接近的标定结果，并且多次估计的一致性更好，见图2-2。

 图2-2 本方法和Kalibr对比

2.2.2 VIO系统性能实验

作者利用3个实验，评估使用本方法进行时间偏差修正后VIO系统性能的改善：

1. 在时间严格对齐的数据集（Euroc数据集）上，人为添加时间戳偏差，对比本方法和VINS-Mono
2. 在时间严格对齐的数据集（Euroc数据集）上，人为添加时间戳偏差，对比本方法和OKVIS
3. 使用真实传感器，在线标定并修正时间偏差，对比本方法和OKVIS

  实验1结果显示，人为添加的偏差越大，VINS-Mono表现越差，而本方法可以准确估计时间偏差，因此性能不受影响，保持VINS-Mono系统最高的精度，见图2-3。实验2结果显示OKVIS也明显受时间不对齐的影响，当人为设置的时间偏差较大时，OKVIS的表现明显变差，具体数据分析参看原文，基于实验2，作者还对\(t_d\)的收敛时间进行分析，表明大约在几秒（3秒左右）的时间内，\(t_d\)就已经收敛。实验3表明本方法适用于廉价传感器，此类传感器中的IMU一般都是几美元价格的消费级IMU，此类IMU的测量数据噪声较大给VIO系统和时间偏差标定带来挑战，但是本方法依然能够有效提升VIO系统性能，在长时间长距离运行的情形下，优势更加明显。

 图2-3 对比本方法和VINS-Mono在不同时间偏差下的表现

2.3 笔者评价

  看完整个方法，第一感觉是这个方法也没什么高级，很简单呀，然而，相比Kalibr时间标定方法而言，本方法在线实时标定和无需标定pattern的优势特别明显，并且，实验结果也表明效果也非常好。不得不对这样一个既简单又有明显成效的方法表示赞叹，作者当之无愧获得Best Student Paper的奖项。
  斗胆猜测一下作者提出此方法的思路，关于时间偏差，我们估计的偏差为IMU和相机数据之间的时间戳偏差，只需要估计两者之间的相对偏差，即比如作者假设IMU的时间戳是对的，那么图像数据对应的时间戳是有偏差的。基于这样的思考，那就想如何纠正图像数据的时间戳偏差呢？两条路径：

1. 能否得到图像对应的准确时间戳呢？

看起来不能直接得到，这和我们估计时间偏差是一个问题

2. 能不能得到对应时间戳的图像呢？

看起来也不容易，但是图像上不是每个像素都是我们关心的，我们只关心特征点像素位置，只要能够估计特征点像素在对应时间戳的位置，问题就解决了。那么正如作者，假设像素的运动时匀速的（每个像素各自的运动时匀速的），就可以估计对应时间戳的所有特征点位置。

  那么我们可否从另一个角度假设图像的时间戳是对的，即认为IMU的时间戳是存在偏差的呢？那么需要在IMU的预积分项中引入\(t_d\)，比如，在预积分起始时间\(t_i\)处向后多做\(t_d\)时长的积分，在预积分结束的地方\(t_j\)处向后砍掉\(t_d\)时长积分，想想公式会比假设特征点匀速运动复杂很多。
  最后，美中不足，作者没有对\(t_d\)的可观测性作出一些数学的分析，感觉是比较遗憾的。联想起李名扬的文章一般会有一些变量可观测的分析，这样显得更加严谨，更有数学之美。

参考文献

[1] Qin T, Shen S. Online Temporal Calibration for Monocular Visual-Inertial Systems[J]. arXiv preprint arXiv:1808.00692, 2018.