VINS-Mono 时延估计论文翻译

Online Temporal Calibration for Monocular Visual-Inertial Systems

摘要

精确的状态估计是机器人导航、自主驾驶、虚拟现实和增强现实等各种智能应用的基础模块。视觉惯性融合是近年来流行的六自由度状态估计技术。记录不同传感器测量值的时刻对系统的鲁棒性和准确性至关重要。在实践中，每个传感器的时间戳通常会受到触发和传输延迟的影响，从而导致不同传感器之间的时间偏差（时间偏移）。这种时间偏移严重影响传感器融合的性能。为此，我们提出了一种标定视觉和惯性测量元件之间时间偏移的在线方法。该方法通过对SLAM系统中的时间偏移、相机和IMU状态以及特征位置的联合优化，实现了时间偏移的标定，而且该方法是一种通用模型，可用于多个基于特征的优化框架。仿真和实验结果表明，该方法与现有的离线工具相比具有较高的精度。通过与其它方法的VIO比较，证明了在线时间标定对视觉惯性系统有很大的好处。

引言

近几十年来，状态估计一直是机器人和计算机视觉领域的一个基础研究课题。机器人导航、自主驾驶、虚拟现实（VR）和增强现实（AR）等各种应用都高度依赖于精确的状态估计。由于其体积小，功耗低，机械配置简单，我们特别感兴趣只涉及一个相机的状态估计解决方案。纯单目纯视觉技术已经取得了很好的结果，该技术可以计算出精确的相机运动和符合场景真实比例的环境结构。为了解决众所周知的纯单目尺度模糊问题，多传感器融合方法越来越受到人们的重视。许多研究利用IMU（惯性测量单元）辅助相机，在6自由度SLAM（同时定位和建图）中取得了令人印象深刻的性能。一方面，惯性测量使俯仰角和滚转角，以及尺度变得可观。另一方面，当视觉跟踪失败时，惯性测量通过桥接间隙来提高运动跟踪性能。

为了融合来自不同传感器的数据，必须精确地知道记录测量值的时刻。在实际应用中，每个传感器的时间戳通常会受到触发和传输延迟的影响，导致不同传感器流之间的时间不对齐（时间偏移），因此，传感器的时间同步对多传感器系统来说是一个关键问题。在视觉惯性系统中，相机与IMU之间的时间偏移会影响系统的鲁棒性和精度。大多数视觉惯性方法假设测量的时间戳在单个时钟是精确同步的。因此，这些方法适用于一些严格硬件同步的传感器。对于大多数低成本和自组装的传感器组，硬件同步是不可用的。由于触发和传输延迟，相机和IMU之间总是存在时间不对齐（时间偏差），时间偏差通常在几毫秒到几百毫秒之间。几十毫秒将导致MU序列与图像流完全不一致，从而严重影响视觉惯性系统的性能。

为此，我们提出了一种视觉惯性系统的时间偏移在线标定方法。我们假设时间偏移是一个常数但未知的变量。我们通过在线联合估计时间偏移、相机和惯性测量单元状态以及在SLAM系统中的特征位置来标定它。我们的标定方法是一个通用方法，可以很容易地用于其他基于特征的视觉惯性优化框架。虽然我们使用单目传感器套件来展示我们的方法，但所提出的方法可以很容易地应用于多像机视觉惯性系统。我们强调我们的贡献如下:

• 我们提出了一种在视觉惯性系统中在线标定相机和IMU之间时间偏移的方法
• 我们通过模拟和真实世界的实验展示了在线时间标定的重要性
• 代码开源

相关工作

在过去的几十年里，视觉惯性里程计技术得到了极大的发展，其目标是高精度地计算像机运动和环境结构。流行的技术要么是基于滤波的框架[9]–[12，17]，要么是批量优化[13]–[16，18]。大多数视觉惯性算法通过提取鲁棒的稀疏特征来处理图像，而不是对稠密图像进行操作。在这些工作中，[9，10，18]使用了弱结构的视觉因子，通过将视觉残差投影到零空间来消除特征。他们更注重估计相机或惯性测量单元的运动，而不是特征位置。[13，14，16]选择性地将关键帧和特征保存在一个包中，这将共同优化相机运动和特征。所有这些方法都假设惯性测量单元和像机精确同步，没有时间偏差。

惯性测量单元和摄像机之间的时间偏移是低成本和自组装设备中的典型问题。由于时钟不同步、触发延迟和传输延迟导致测量的时间戳与实际采样时刻不一致。该时间偏移未知并且需要标定。几项研究集中于标定它。Mair [19]提出了一种时间和空间校准的初始化方法，该方法使用互相关或相位一致性。这种方法以新颖和特殊的视角制定了标定程序。它将标定变量与其他未知变量(位姿、特征位置)分开。因此，它可以在不受其他变量影响的情况下提供一个好的先验。此外，这个方法以更精确的形式对时间偏移进行了建模。Kelly[20]对齐相机和惯性测量单元的旋转曲线，以标定时间偏移。它利用ICP(迭代最近点)方法的变体来逐渐匹配两条旋转曲线。来自Furgale [21]的Kalibr，在连续批量优化过程中估计时间偏移、相机运动以及相机和惯性测量单元之间的外参。Kalibr取得了令人印象深刻的性能，并成为一个受欢迎的工具箱。然而，这两种方法使用固定的平面模版(如棋盘格)离线运行。标定模版为他们提供了鲁棒的特征跟踪和关联，以及精确的三维位置。此外，Li在[22]中为视觉惯性系统提出了一种带在线时间标定的运动估计方法。时间偏移在多状态约束EKF框架中校准。他的方法在计算复杂性方面有显著优势，从而可用于便携式移动设备。与他的方法相比，我们基于优化的算法在精度方面表现更好，因为我们可以迭代优化一个大束中的许多变量，而不是过早地修正线性化误差。

算法

在这一节中，我们将时间偏移建模为视觉因子，并在基于优化的VIO框架中将其与特征、惯性测量单元和相机状态一起在线计算。

我们简单地定义坐标系和符号如下。$(\cdot {)}^w$表示全局坐标系。$(\cdot {)}^c$标识局部相机坐标系。$(R_c^w,p_c^w)$是全局坐标系中的相机位姿，它可以将3D特征从相机坐标系转换到全局坐标系。

A. 时间偏移

对于低成本和自组装的视觉惯性传感器组，相机和惯性测量单元放在一起时没有严格的时间同步。由于触发延迟、传输延迟和时钟不同步，生成的时间戳不等于测量采样的时刻。因此，不同测量之间通常存在时间偏移。一般情况下，传感器之间的时间偏移是一个恒定但未知的值。在某些更糟糕的情况下，传感器是用不同的时钟采集的，时间偏移会随着时间漂移。这种传感器不适合进行传感器融合。

在本文中，我们考虑一般情况，其中时间偏移是一个恒定但未知的值。在下图中展示了时间偏移。

在图中，上图代表采样时刻。下图显示了时间戳时刻。由于触发延迟、传输延迟和时钟不同步，生成的时间戳与实际采样时间不相等，导致摄像机和惯性测量单元之间存在时间错位。具体来说，我们定义了$t_d$，
$$t_{IMU}=t_{cam}+t_d\text{\hspace{1em}(1)}$$
时间偏移$t_d$指我们应该移动相机时间戳的时间量，以便相机和惯性测量单元数据流在时间上保持一致。时间偏移$t_d$可以是正值或负值。如果相机序列比惯性测量单元序列具有更长的延迟，$t_d$为负值。否则，$t_d$为正值。

B. 图像平面上的特征速度

为了使相机和惯性测量单元数据流在时间上保持一致，相机序列应根据$t_d$向前或向后移动。我们没有移动整个相机或惯性测量单元序列，而是在时间轴上移动特征的观测。为此，我们引入特征速度来建模和补偿时间错位。

在很短的时间内(几毫秒)，相机的运动可以被视为恒速运动。因此，特征在短时间内在图像平面上以近似恒定的速度移动。基于这个假设，我们在图像平面上计算特征的速度。

如图2所示，$I^k$和$I^{k+1}$是两个连续的图像帧。假设相机在短时间周期$[t_k,t_{k+1}]$内以恒定速度从$C_k$到$C_{k+1}$移动。因此，我们大致认为特征$l$也在这个短时间内以恒定的速度$V_l^k$在图像平面上移动。速度$V_l^k$的计算如下:
$$V_l^k=(\left[\begin{array}{c}{u}_l^{k+1}\\ v_l^{k+1}\end{array}\right]-\left[\begin{array}{c}{u}_l^k\\ v_l^k\end{array}\right])/(t_{k+1}-t_k)\text{\hspace{1em}(2)}$$
其中$[u_l^k,v_l^k]$和$[u_l^{k+1},v_l^{k+1}]$分别是在图像平面$I_k$和$I_{k+1}$上的2D观测。

C. 带有时间偏移的视觉因子

在经典的稀疏视觉SLAM算法中，视觉测量被公式化为代价函数中的(重)投影误差。我们通过增加一个新的变量，时间偏移，来重构经典(重)投影误差。一个特征有两种典型的参数化。一些算法将特征参数化为其在全局坐标系中的三维位置，而另一些算法将特征参数化为相对于某个图像帧的深度或逆深度。接下来，我们分别用这两种参数化将时间偏移建模为视觉因子。

注意下面的所有的特征的像素坐标都是指的归一化相机平面的坐标

1). 3D位置参数化

特征被参数化为全局坐标系中的3D位置($P_l=[x_l,y_l,z_l{]}^T$)。传统的视觉测量被公式为投影误差，
$$\begin{gathered} e_l^k=z_l^k-\pi ({R_{c_k}^w}^T(P_l-p_{c_k}^w)) \\ {z}_l^k=[u_l^k,v_l^k{]}^T\text{\hspace{1em}(3)} \end{gathered}$$

$z_l^k$是特征$l$在坐标系$k$中的观测。($R_{c_k}^w,p_{c_k}^w$)是相机位姿，它将特征$P_l$从全局坐标系转换到局部相机坐标系。$\pi (\cdot )$表示相机投影模型，它将3D特征投影到具有畸变的图像平面中。

在上述公式中，相机位姿($R_{c_k}^w,p_{c_k}^w$)受到视觉测量的约束。它还受到惯性测量单元测量值的限制。在实践中，如果惯性测量单元和相机之间存在时间偏差，则惯性测量单元约束与视觉约束在时域中不一致。换句话说，我们应该向前或向后移动相机序列，以便相机和惯性测量单元数据流在时间上保持一致。我们没有移动相机或惯性测量单元序列，而是在时间轴上移动特征观测值。新公式如下:
$$\begin{gathered} e_l^k=z_l^k(t_d)-\pi ({R_{c_k}^w}^T(P_l-p_{c_k}^w)) \\ {z}_l^k=[u_l^k,v_l^k{]}^T+t_d{V}_l^k\text{\hspace{1em}(4)} \end{gathered}$$
$V_l^k$是特征在图像平面上的速度，从公式(2)获得。$t_d$是时间偏移的未知变量，它改变了特征在时域中的观测。通过优化$t_d$，我们可以在时域内找到与惯性测量单元约束相匹配的最佳相机位姿和特征观测。

2). 深度参数化

特征也可以被参数化为相对于一个图像坐标系的深度或逆深度。我们以图像$i$中的深度${\lambda }_i$为例。传统的从图像中$i$到图像$j$的重投影误差被写为
$$\begin{gathered} e_l^j=z_l^j-\pi ({R_{c_j}^w}^T(R_{c_i}^w{\lambda }_i\left[\begin{array}{c}{Z}_l^i\\ 1\end{array}\right]+p_{c_i}^w-p_{c_j}^w)) \\ {Z}_l^i=[u_l^i,v_l^i{]}^T,Z_l^j=[u_l^j,v_l^j{]}^T\text{\hspace{1em}(5)} \end{gathered}$$
特征$l$首先被投影到全局坐标系中，然后被反投影到局部相机坐标系$j$的图像平面上。残差是观测和反投影位置之间的位移。

与公式(4)相似，我们也可以将时间偏移$t_d$考虑进去
$$\begin{gathered} e_l^j=z_l^j(t_d)-\pi ({R_{c_j}^w}^T(R_{c_i}^w{\lambda }_i\left[\begin{array}{c}{Z}_l^i(t_d)\\ 1\end{array}\right]+p_{c_i}^w-p_{c_j}^w)) \\ {Z}_l^i(t_d)=[u_l^i,v_l^i{]}^T+t_d{V}_l^i,Z_l^j=[u_l^j,v_l^j{]}^T+t_d{V}_l^j\text{\hspace{1em}(6)} \end{gathered}$$

上图描述了重投影过程。虚线代表没有时间偏移建模的传统重投影过程。实线表示考虑时间偏移的重投影过程。黄线表示惯性测量单元约束。惯性测量单元约束与传统的重投影约束在时间上不一致。通过优化$t_d$，我们可以在时域内找到与惯性测量单元约束相匹配的最佳相机位姿和特征观测值。

D. 带时间偏移的优化

通过利用上述视觉因子，我们可以很容易地将时间标定函数添加到典型的基于优化的视觉惯性框架中，如[13，16，23]。在这些框架中，视觉-惯性定位和建图被表述为一个非线性优化问题，它将视觉和惯性测量紧密耦合。

如上图所示，几个相机位姿和惯性测量单元测量保持在一个束中。束的大小通常受限于有限的计算复杂度。局部BA联合优化相机和惯性测量单元状态以及特征位置。

我们可以很容易地将改进的的视觉因子添加到这种框架中。具体来说，整个状态变量被时间偏移增强，定义如下:
$$\begin{gathered} \chi =[x_0,x_1,...,x_n,P_0,P_1,...,P_l,t_d] \\ {x}_k=[p_k^w,v_k^w,R_k^w,b_a,b_g],k\in [0,n]\text{\hspace{1em}(7)} \end{gathered}$$
其中第k个IMU状态由全局坐标系中的位置$p_k^w$、速度$v_k^w$、旋转矩阵$R_k^w$和局部机体坐标系中的IMU零偏$b_a$、$b_g$组成。特征$P_l$由全局坐标系中的三维位置或相对于某个图像坐标系的深度来定义。

整个问题被表述为一个代价函数，包含惯性测量单元积分因子、重投影因子以及某个先验因子。因此，我们使用增强的视觉因子来实现时间偏移标定

$e_B(Z_{k+1}^k,\chi )$是来自IMU积分的误差项。$B$是所有惯性测量单元测量值的集合。$e_C(Z_l^j,\chi )$是增强的视觉(重)投影误差，包括时间偏移变量。

$C$在图像帧中观察到至少两次的特征的集合。误差由它们的逆协方差矩阵$P$加权。 { e p , H p } \{e_p,H_p\} {ep​,Hp​}是来自先验知识和边缘化的先验信息。只有少量的测量值和状态被保持在优化束中，而其他的被边缘化并被转换成先验。使用高斯-牛顿法可以有效地优化非线性最小二乘代价函数。

E. 时间偏移的补偿

每次优化后，我们通过移动后续视频流的时间戳来补偿时间偏移，$t_{cam}^{{}^{\prime }}=t_{cam}+t_d$。随后，系统在补偿后的视觉测量和惯性测量之间估计${\delta }_{t_d}$，${\delta }_{t_d}$将在后续数据流中迭代优化，并最终收敛到零。随着时间间隔${\delta }_{t_d}$的减小，我们的基本假设(特征在短时间间隔内在图像平面上匀速运动)越来越合理。即使开始时存在巨大的时间偏移(例如数百毫秒)，该过程也会逐渐从粗略到精细地对其进行补偿。

结论

本文提出了一种在线标定惯性测量单元和相机之间时间偏移的方法。我们的方法是通用模型，可以很容易地在基于优化的视觉惯性框架中采用。时间偏移与惯性测量单元和相机状态以及特征一起被联合优化。仿真和实验结果表明，该方法在时间偏移标定和系统运动估计方面都能达到较高的精度，甚至比其他现有的离线方法都要高。尽管我们在本文中使用了单目传感器套件来展示我们的方法，但所提出的方法可以很容易地推广到多相机视觉惯性系统。