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](https://i.ibb.co/gr8xpwg/bluefox2-mpu6050-synchronize.png) 图1-1 VIO系统 IMU和Camera
# 1. 介绍   一般的视觉里程计(VIO)系统包含两个传感器，一个视觉传感器（相机）和一个惯性传感器(IMU),如图1-1。系统对两个传感器进行采样，获得相应的数据（图像、IMU数据）与对应的采样时间戳。通常，我们假设获得的采样时间戳即为传感器采样时刻对应的时间，比如相机曝光时刻（通常曝光持续几毫秒到几十毫秒，认为曝光时刻为曝光持续的中间时刻），然而，系统存在的触发延时、传输延时和没有准确同步时钟等问题，使得相机和IMU之间的时间不对齐。
![图1-2 相机和IMU数据流时间不对齐](https://i.ibb.co/VCvZW2p/1545892090424-2.png) 图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的在线时间标定方法[1]进行梳理。这篇文章获得了机器人领域顶级会议 IROS2018 的 Best Student Paper 奖项，是一篇很值得了解的文章。

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

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$时刻特征点位置发生偏移](https://i.ibb.co/d6q0b11/1545899202191-3.png) 图2-1 $t_k$时刻特征点位置发生偏移

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

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

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

[egin{equation} mathbf{z}_l^k(t_d) = [u_l^k, v_l^k]^T + t_dmathbf{V}_l^k ag{2-2} end{equation} ]

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

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

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

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

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

[egin{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] ag{2-4} end{equation} ]

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

[egin{equation} minigg{ Big|e_p - mathbf{H}_pchi Big|^2 + sum_{kin 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} igg} ag{2-5} end{equation} ]

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

[egin{equation} t_{cam}'=t_{cam}color{red}-t_d ag{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对比](https://i.ibb.co/dgXQr3f/compare-with-kalibr.png) 图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在不同时间偏差下的表现](https://i.ibb.co/yFD8rY8/proposed-and-vins-mono-with-different-offset.png) 图2-3 对比本方法和VINS-Mono在不同时间偏差下的表现

2.3 笔者评价

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

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

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

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

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

  那么我们可否从另一个角度假设图像的时间戳是对的，即认为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.

【推广】 免费学中医，健康全家人
原文地址：https://www.cnblogs.com/xzd1575/p/vio_timming.html