基于在线时间延迟估计的双目视觉惯性里程计

范涵奇,车金钊

(北方工业大学信息学院,北京 100144)

1 引言

视觉惯性里程计[1](Visual-inertial Odometry, VIO)是运动物体在未知环境中通过融合以相机为主的视觉传感器与惯性测量单元(Inertial measurement unit, IMU)数据实现自身定位和导航的技术,现主要应用于无人机[2]、AR/VR[3]等领域。

在VIO前端需要在算法启动阶段执行IMU初始化操作,期间陀螺仪的零偏往往难以收敛,出于该原因,将IMU信息与视觉信息采用联合初始化的方案[4,5]有一定几率会失败。因此本文在初始化阶段选择采用静止初始化的方式,Geneva等人[6]用静止初始化的方式,通过计算一段时间内加速度的方差大小来判断系统是处于静止还是运动状态,不同于他们划分系统静止与运动状态的方式,本文选择前n帧图像的时间内为系统静止状态并使用帧间的IMU数据进行初始化,从而快速完成初始化操作。

VIO在后端维护了一个高维的状态向量,根据是否将视觉信息加入到状态向量中可分为紧耦合与松耦合。由于松耦合将IMU与相机分别进行自身的运动估计,在视觉定位困难的地方不够鲁棒[7],因此大都采用紧耦合的方式。而根据后端实现的方式, VIO的后端分为基于滤波和基于优化[8]。基于滤波的紧耦合方案有MSCKF[9]、ROVIO[10]等,其主要优势在于降低了对计算的需求,但计算的精度却不如基于优化的方案。Forster等人[11]提出了IMU预积分理论,他们将两帧间的多组IMU数据积分得到一组IMU数据,随后与视觉信息融合一起优化,以达到减少优化结点的目的,在保持精度的同时提升优化的速度。

在IMU预积分理论中,要在预积分之前将图像数据的时间轴与IMU数据的时间轴对齐,以得到两图像间准确的IMU数据。但是在传感器融合中由于存在着传感器传输延迟、数据采样延迟、时钟不同步等问题而产生时间延迟(Temporal delay,td)[12,13],导致时间戳不能很好的对齐。为解决这个问题,Li和Mourikis[12]提出在线估计时间延迟的方法,他们将时间延迟作为扩展卡尔曼滤波中待估计的状态变量。Cen等人[14]则认为IMU数据相对于图像数据存在着时间延迟,他们将时间延迟加入到根据IMU数据构建的IMU误差模型中,随后在后端采用基于滤波的方式得到时间延迟量。但是他们计算结果的精度并不高。Qin和Shen[13]也采用在线的方式估计时间延迟,他们假设时间延迟是未知且恒定的并在优化中估计它。

针对上述分析的问题,本文提出一种基于在线时间延迟估计的双目视觉里程计,在IMU初始化阶段采用静止初始化的方式,在双目间对关键点采用往返追踪的策略构造准确的数据关联。考虑了传感器上的时间延迟后,在后端优化中,通过计算实际时刻图像,将时间延迟作为待优化变量加入到基于直接法的光度残差中,并使用紧耦合优化的方式进行估计,优化结束后使用估计的时间延迟量补偿图像时间戳,使得传感器时间一致,以消除时间延迟对数据融合的影响,从而提高算法的定位精度。

2 基于估计时间延迟的视觉惯性融合算法

本文算法框架如图1所示。本文在前端分别处理图像数据和IMU数据,在后端维护了一个拥有固定数量关键帧的滑动窗口,并采用基于优化的方式融合视觉信息与惯性信息共同估计相机位姿,同时估计时间延迟,提升视觉惯性融合的精度,优化结束后得到相机位姿。

图1 本文算法框架

2.1 IMU初始化

(1)

在静止时刻,陀螺仪的理想测量值应该为0,因此陀螺仪的零偏应该为前n帧间IMU数据中陀螺仪测量值的平均值。Geneva等人[6]通过加速度计测量值的平均值初始化了加速度计的零偏,但是Qin和Shen在文章[15]中通过实验指出在初始化中如果忽略加速度计的零偏则不会明显影响初始化结果,因此本文没有计算加速度计的零偏而是设初始值为0。在IMU初始化结束后将为后续的优化提供一个更好的初值从而减少IMU的噪声积累。

2.2 前端相机追踪

2.2.1 构建双目间准确的数据关联

在前端相机追踪部分中,读取相机传来的双目图像,并对图像进行处理。在左目图像中提取关键点,在关键点的选择方面,本文在图像中尽可能提取分布均匀且具有足够像素梯度的点,根据当前帧的左右图像恢复点的深度信息,并采用逆深度参数化的方式表示关键点。在基于直接法的方案中需要通过当前帧的双目图像上关键点的光度误差建立双目间的数据关联,恢复图像上关键点的逆深度。具体步骤需要将相机左侧图像提取的关键点变换到右侧图像,寻找右侧图像上与之关联的关键点。Rui Wang等人[16]使用极线搜索的方式,通过建立两点间的光度残差使用高斯牛顿优化的方式得到相关联的关键点,最后恢复深度信息。本文采用该方法,但是该方法难免会存在构建错误数据关联的情况。为此本文认为如果左目图像上一点pL能够在右目图像中找到与之关联的点pR,则将pR以同样的方式再变换回左目图像中,如果依然能够找到与之关联的点,则认为pL和pR是一对正确的关联点否则就是一对错误的关联点,本文使用往返追踪描述这种将关键点从左侧图像变换到右侧图像再变换回左侧图像的操作。如图2图3所示,为了便于观察,此处降低了所挑选关键点数量的阈值,采用往返追踪可以有效的减少双目间错误的数据关联,进而提高双目间数据关联的准确性。

图2 使用本文方法剔除的错误数据关联

图3 使用本文方法得到的最终结果

2.2.2 帧间追踪

在追踪模块中,利用滑动窗口中的最新关键帧作为参考帧,对新的帧进行追踪。一般来说短暂的帧间运动可以看作是匀速的、平滑的运动,本文采用匀速运动模型来表示帧间运动,追踪最新的关键帧。由于上一帧的运动与当前帧的运动是相似的,因此可以用上一帧的运动代替当前帧的运动,求得当前帧到最新关键帧的位姿变换,这样可以将最新关键帧上的点投影到当前帧上:

(2)

本文将投影到当前帧上的点与参考帧上的点建立光度残差, 随后使用高斯牛顿法迭代优化位姿。 最后决定当前帧是否为新的关键帧, 新的关键帧将加入到滑动窗口中并更新参考帧, 而优化的位姿将用于关键点的逆深度值更新。

2.3 时间延迟对图像信息的影响

根据IMU预积分模型[11]可以看出在做预积分时,将两帧图像的时间戳与IMU时间戳作对比找到两帧之间的IMU数据是非常关键的,对于异构传感器中存在通信延迟、传输延迟、时钟不同步等问题,使得相机实际捕获图像的时间戳与数据集给出的对应图像时间戳并非严格相等,二者之间存在着时间延迟。

时间延迟是一个恒定且未知的值,它的存在使得在做IMU预积分前,即便使用了插值的方式仍然不能准确的根据图像时间戳找到两帧间IMU数据。如果不能很好的解决这个问题,将影响到IMU预积分、多传感器数据融合和算法优化的结果,进而使得算法精度下降。

基于上述分析,准确的估计时间延迟并且矫正图像时间戳成为了非常重要的一步。在多传感器数据融合时,由于数据集给出的图像时间戳并不等于相机在实际时刻捕获图像的时间,因此图像也不是相机在实际时刻捕获的图像。 时间延迟的存在导致无法获取相机在实际时刻捕捉的图像以及对应的图像时间戳,因此需要预测相机在实际时刻捕获的图像信息以及对应的时间戳。接下来本文将相机在实际时刻捕获的图像称为实际时刻图像, 记为I(treal),该图像对应的时间戳为treal,如图4所示,图4描述了时间轴上不同传感器数据的时间戳之间的关系以及图像时间戳对应的图像表示,其中td表示相机实际时刻捕获图像的时间与数据集给出图像的时间戳之间的时间延迟,假设在第tk时刻有图像Ik,根据本文的方法可以求出td,同时计算第k个相机实际时刻捕获的图像Ik(tk＿real),以及对应的时间tk＿real。一般来说,td的值可能是正的也可能是负的,如果实际时刻的图像时间戳相对于数据集提供的图像时间戳有着更高的时间延迟,那么td的值就是正的,否则就是负的。

图4 不同传感器上时间戳关系图示

综上所述为了能够估计,Qin和Shen[13]通过计算特征点的移动速度求解特征点在时间延迟前的坐标,从而将带入到优化方程中。由于本文使用的是直接法,根据特征匹配求特征点速度是不可行的,且直接法在后端优化中根据特征点之间的灰度值差异构造光度残差而非通过几何位置的差异构造的重投影误差,因此迫切需要找到一种新的方法将时间延迟带入到优化中去。与Qin和Shen的做法不同的是,本文假设相机在极短的时间内(几毫秒)捕获的连续两帧图像上每一个像素点的灰度值随时间发生着均匀变化,因此当滑动窗口中加入新的关键帧时,可以通过时间延迟计算出该帧图像上的像素点在实际时刻的灰度值,通过实际时刻的灰度值构造光度残差,从而将时间延迟带入到优化中去。

2.4 带有时间延迟的视觉惯性联合优化

在考虑时间延迟后采用紧耦合的方式将视觉部分与IMU测量值数据进行融合,使用两个模型共同估计一组状态向量, 将时间延迟考虑进状态向量S

S=[c,td,s0,s1,…,sn,d0,d1,…,dm]

(3)

其中c为相机内参矩阵,d为关键点的逆深度,m为滑动窗口中参与优化的关键点总数,n为滑动窗口中关键帧的总数,p为位置,v为速度,q为旋转的四元数表示。 滑动窗口内的目标函数为最小化IMU项、视觉项与来自边缘化之后先验项之和

(4)

对于加入到滑动窗口中的关键帧来说,往往根据两关键帧上关键点的灰度值差异构造光度残差以优化相机位姿

rij=Ij[p′]-Ii[p]

(5)

其中p′表示第i帧上关键点p变换到第j帧上的点的坐标。由于时间延迟的存在,关键点p与p′的灰度值不再是该点所在图像的灰度值, 而是该点在实际时刻的灰度值。因此首先求出该点在实际时刻的灰度值。

基于上节的假设,首先获取实际时刻图像。记滑动窗口中第i个关键帧对应的图像为第k个相机的左侧图像,通过与其相邻的上一帧图像,可以求得帧上每一个像素点的灰度值随时间的变化量,用如下公式表示

(6)

(7)

获得关键帧的实际时刻图像后, 对于之前在原图像上提取的关键点, 由于时间延迟的存在, 即可在实际时刻图像中根据之前提取关键点的坐标信息得到关键点在实际时刻的灰度值。 从而将时间延迟代入到残差函数式(5), 由于第i帧中左目图像与右目图像构建的残差函数与IMU信息无关,因此仅将时间延迟考虑进第i帧中左目图像与第j帧中左目图像上的关键点构造的残差函数。 在考虑了时间延迟后残差函数式(5)改为如下所示

(8)

接下来使用非线性最小二乘求解,通过增量更新状态,在每次迭代后估计的时间延迟在下次迭代优化前使用式(9)对图像时间戳tcam进行补偿得到实际时刻treal

treal=tcam+td

(9)

经过补偿后在IMU预积分时根据图像时间戳将挑选更准确的IMU数据。本文算法为了能够控制滑动窗口中优化规模,以保持系统的实时性,使用舒尔补通过边缘化删除旧变量[17], 包括帧的位姿、速度以及该帧上提取的点等, 同时将旧变量的信息保留在滑动窗口中。在初始阶段,经过优化后估计的时间延迟会不稳定, 但在后续优化中会逐渐收敛,从而证明文中的假设,这一点将在实验中说明。

3 实验与分析

本文在EuRoC数据集[18]上测试提出的算法。实验环境为装有Ubuntu 18.04, 配置为Intel®CoreTMi7-4770CPU, 8G RAM的计算机。EuRoC数据集提供11个双目图像序列以及与其对应对齐的 IMU加速度、角速度测量值,同时还有11个图像序列的真实轨迹。数据集总共包含两个场景, 一个是苏黎世联邦理工学院的机器房, 另一个是普通房间。 本文算法在基于优化的直接法视觉里程计的基础上融合IMU数据, 同时考虑了数据集的图像时间戳与捕获该图像的实际时刻之间时间延迟对算法精度的影响, 传感器上的时间延迟是未知的, 本文在假设时间延迟存在且初值为一定值情况下通过计算实际时刻图像将时间延迟带入到优化中估计时间延迟。本文使用均方根误差(RMSE)的大小来表示算法的准确性, 均方根误差越小代表算法结果越精确。

表1展示了在EuRoC数据集的部分图像序列中, 假设当前系统存在的时间延迟为5ms时,是否使用本文的方法估计时间延迟对本文算法的影响, 其中前两行分别为不估计与估计时间延迟时算法的RMSE, 可以看出估计时间延迟的RMSE更小,这是由于估计时间延迟提升了算法中数据融合的效果, 进而提高了本文算法的精度。 另外选择估计时间延迟会提高算法的计算量, 本文统计了在后端优化执行次数和执行总时长, 别得到了估计与不估计时间延迟时优化的平均时长, 在第三行和第四行中分别展示了当估计时间延迟时, 算法在滑动窗口中执行优化的平均时长增加量和增加时间占原优化时间百分比, 增长维持在20%～30%, 总的来说估计时间延迟在不增加太多的计算量情况下可以提高算法的精度。

表1 估计与不估计时间延迟的轨迹与地面真值之间的对比,以及进行时间延迟估计后对系统平均优化时间的影响

图5显示了在V1＿01＿easy序列中本文算法选择对时间延迟是否进行估计的轨迹同真实轨迹对比的结果, 其中虚线为轨迹真实值, 蓝色线条为不估计时间延迟后的轨迹, 橙色线条为估计时间延迟后的轨迹。同样可以看出时间延迟存在时, 使用本文的方法估计时间延迟对轨迹精度有所提升。

图5 是否估计时间延迟的轨迹对比

图6显示了时间延迟存在下, 使用本文方法在V1＿01＿easy序列上估计的时间延迟量的过程, 可以看出随着程序运行, 待估计的时间延迟在第500帧左右开始稳定的收敛到一定值,随后趋于稳定, 最后估计的时间延迟量为4.35ms。

图6 本文方法估计时间延迟收敛情况

VINS-Mono[4]是香港科技大学的Shen 等人开源的一套基于特征点法的单目视觉惯性融合定位算法, 而VINS-Fusion[19]则是Shen 等人在VINS-Mono的基础上扩展了双目视觉惯性融合等功能,本文选择使用VINS-Fusion中的双目视觉惯性里程计同本文算法进行比较, 同时开启二者的回环检测功能。 本文算法与VINS-Mono、VINS-Fusion在EuRoC数据集的不同序列的均方根误差在表2中所示, 由表2可以看出假设当前系统存在时间延迟时(=5ms), 本文算法在图像序列中的均方根误差要小于VINS-Mono和VINS-Fusion, 取得的定位精度更高。

表2 本文算法同其它算法的RMSE/m对比

在表2中, V1＿02＿medium和V1＿03＿difficult序列中存在大量的回环, 本文算法由于没有回环检测, 难以消除累积误差, 因此轨迹误差较大, 此外序列中还存在一些复杂的情况,比如光照剧烈变化, 这些情况对直接法非常不友好,本文通过融合IMU以弥补视觉方面的不足, 同时本文通过计算实际时刻图像将时间延迟带入到优化中求解, 对图像时间戳进行补偿, 提升了算法融合的效果。 最后序列中存在着特征丢失等特征点法难以应对的问题, 而这在直接法中得到了很好的解决。 因此本文算法在V1＿02＿medium序列中的均方根误差尽管与VINS-Mono、VINS-Fusion的均方根误差相近但仍然小于二者的均方根误差。

图7和图8展示了当假设系统存在5ms的时间延迟时,本文算法与VINS-Mono以及VINS-Fusion在EuRoC数据集中部分图像序列上的轨迹和地面真值对比, 进一步显示了本文算法可以在场景中取得较好的定位。 其中虚线为轨迹真实值, 蓝色线条为本文算法轨迹, 橙色线条为VINS-Mono, 绿色线条为VINS-Fusion。

图7 在MH＿03＿Medium序列上同其它算法的轨迹对比

图8 在V2＿01＿easy序列上同其它算法的轨迹对比

4 结语

本文基于在线估计时间延迟, 提出了一种精确的双目视觉惯性里程计算法, 为了消除时间延迟带来的影响, 在紧耦合优化的视觉惯性里程计中将时间延迟作为待优化变量, 通过优化求得正确的时间延迟, 补偿数据集中的图像时间戳, 使得两关键帧之间的IMU预积分项更加准确, 进而提升数据融合效果以及定位精度。 在EuRoC数据集上的实验结果表明本文提出的融合算法能够很好的估计时间延迟同时消除时间延迟带来的影响, 与特征点法相比本文算法有着较好的定位精度。