基于点转移矫正的视觉惯性里程计

张梦龙，张凯杰，刘昌林，周 琦，李京波

（华南师范大学 半导体科学与技术学院，广州 528225）

视觉惯性里程计[1（]Visual-inertial Odometry,VIO）是一种利用视觉和惯性测量单元（Inertial Measurement Unit,IMU）进行位置和姿态估计的技术，在自动驾驶、无人机[2]、增强现实等领域有广泛的应用需求。视觉信息能够减少IMU 长时间积分带来的累积误差，同时IMU 能够提供尺度信息。在面对图像模糊、图像像素信息不准确时，IMU 信息作为补偿能够提高算法的鲁棒性和准确性。VIO 主要分为基于滤波框架[3]、基于优化框架[4-7]和基于深度学习框架[8]三类原理。基于优化的VIO 采用关键帧策略，本质上是一个非线性的最小二乘问题；基于深度学习的VIO 采用神经网络，因此十分依赖硬件的性能；基于滤波的VIO因其对计算资源的要求更低而受到广泛关注。

卷帘相机因其成本更低在嵌入式设备中的应用比全局快门相机更为广泛，但由于卷帘相机是依次读取图像上的每一行像素，所以图像各行像素不是在同一时间获取。卷帘效应如图1 所示，左图表示手机在快速运动下拍摄的图像，相较于静止状态下手机拍摄的照片（右图）有明显的失真。因此，当系统处于较快的运动时，卷帘效应导致像素信息不准确，从而严重降低系统的定位精度。文献[9][10]基于匀速假设对运动轨迹进行建模，估计每个关键帧的速度并对连续关键帧之间的位姿进行线性插值，有效避免了姿态解算错误或无法解算等情况。虽然基于匀速假设的运动模型可以节约计算资源，但定位精度较低。文献[11]～[13]采用B 样条曲线参数化运动轨迹以提高定位精度，文献[4]采用神经网络估计两帧之间的矫正场，将卷帘图像恢复成全局图像。但是，这些方法在资源受限的移动端上实时运行起来较为困难。

图1 卷帘效应Fig.1 Rolling shutter effect

综上所述，低维的运动模型会降低位姿估计的精度[4]，但是高维模型会引入更多需要优化的变量，导致计算量大大增加。考虑到采用运动模型优化轨迹的方法具有一定的局限性，本文提出了一种基于三焦点张量的点转移方法。在不引入更多变量的情况下，系统融合视觉和IMU 信息，并利用三焦点张量的点转移方法提供严格的几何约束信息，将卷帘图像上的部分像素点（Rolling-shutter Point,RS 点）矫正成全局相机下的像素点（Global-shutter Point,GS 点）。采用扩展卡尔曼滤波（Extended Kalman Filter,EKF）和滑动窗口的方法，提高视觉惯性里程计的定位精度并降低计算量。

1 系统框架

基于点转移矫正的VIO系统滤波算法包括状态向量、IMU 运动学模型和相机观测模型三部分，其整体框架如图2 所示。

图2 基于点转移矫正的视觉惯性里程计Fig.2 VIO based on point transfer correction

1.1 状态向量

k时刻的状态向量xk是由当前时刻IMU 状态xI、相关参数xW、n帧IMU 历史位姿xC和SLAM点特征f组成：

其中，相关参数xW包括相机到IMU 的外参：IMU 和相机之间的旋转四元数和平移向量CpI、相机和IMU 之间的时间差td、卷帘相机的读取时间tr和相机的内参K。

定义x为真实值，为估计值，为误差值。

1.2 IMU 运动学模型

IMU 运动学模型的输入为加速度计和陀螺仪的测量值am和ωm：

其中，a和ω为真实的局部加速度和角速度；重力加速度矢量Gg≈ [0 0 9.81]T；为IMU 在世界坐标系下的旋转矩阵；na和ng为零均值高斯噪声。

利用非线性IMU 运动学模型Ε(∙)将状态向量估计值和协方差矩阵Pk|k从tk时刻传播到tk+1时刻，得到tk+1时刻的状态估计量为k+1|k和协方差矩阵为Pk+1|k：

其中，Qd为离散测量噪声协方差矩阵；Gk为噪声雅可比矩阵；Φ(tk+1,tk)为系统雅克比矩阵。

1.3 相机观测模型

当系统接收新的一帧图像时，系统增广状态向量和协方差矩阵。状态向量加入新的IMU 位姿，该位姿对应的时间戳被设为图像中间行的时间戳，其中t是图像第一行对应的时间戳，是卷帘图像的读取时间。由于IMU 和相机之间存在时间差，因此使用时刻的IMU 测量值，并用该估计值增广状态向量。

EKF 协方差矩阵被增广为：

当系统检测和跟踪一个3D 点特征时，对应k时刻图像的像素坐标zk为：

其中，h(∙)为透视投影方程；nk为测量噪声向量，并且该噪声服从零均值白高斯分布；为相对于相机帧坐标系（C系）的点特征位置。

将对应于单个点特征的所有测量值堆叠起来，并通过零空间操作对点特征位置进行线性边缘化，最终得到EKF 的实际值和估计值之间的残差：

其中，Hk为测量的雅克比矩阵；残差用来更新状态向量和协方差矩阵。

2 卷帘相机模型

2.1 融合IMU 信息求取RS 位姿

VIO 系统中的卷帘相机模型如图3 所示。模型的实质是点特征矫正，将卷帘图像的像素坐标（RS 点）矫正为真实的像素坐标（GS 点），GS 点是在卷帘图像读出时间tr的中间时刻（图3 中红色虚线）全局相机记录的坐标点。

图3 卷帘相机模型Fig.3 Rolling shutter camera model

滑动窗口内会存放多帧IMU 历史位姿（GS 位姿），对应的时间戳是卷帘图像读出时间的中间时刻，即全局图像上GS 点对应的时间戳。而系统接受的卷帘图像时间戳对应的是卷帘图像第一行的时间t。由于卷帘效应，图像的每行像素对应的时间戳是不一致的，导致同一帧卷帘图像上提取的RS 点对应不同时刻，即不同RS 点对应不同的相机位姿（RS 位姿）。如图3 所示，通过融合高频的IMU 信息和滑窗内历史已优化的GS位姿求解图像第n行tn时刻对应的RS 位姿

如图3 所示，Δti是连续两帧IMU 之间的时间差。卷帘图像的中间行（紫色箭头）对应的时刻和第n行（红色箭头）对应的时刻之间有k-1帧IMU 位姿。Δtk是卷帘图像第n行（红色箭头）对应时刻与其最近的IMU 位姿（蓝色箭头）对应时刻之间的时间差。IMU的角速度为全局坐标系下的加速度为全局坐标系下的速度为通过上述方法，最终得到图像第n行tn时刻对应的RS 位姿。

2.2 点转移

当系统接收到新图像时，存在多个3D 点特征X被当前卷帘相机帧IR1和滑动窗口内的历史帧IR2同时观察到，如图4 所示。当前卷帘相机帧IR1对应的全局相机帧为IG1，当前卷帘相机帧的RS 点像素坐标为x1，与x1匹配的滑窗内历史帧的RS 点像素坐标为x2。将通过点x2的直线l2选在历史图像帧上，基于三焦点张量的点转移的方法，将当前帧IR1上的RS 点x1转移到全局图像IG1上，最终得到矫正后的GS 点xG-1：

图4 采用三焦点张量的点转移Fig.4 Point transfer using trifocal tensors

若l2=F21x1是对应于x1的对极线，则即无法计算出xG-1，所以应该尽量避免选择到对极线。一种方法是利用x2的两条或者三条直线，计算每条直线对应的xG-1并保留最大范数的一个；另一种方法是总选取垂直于对极线的直线。后者与前者相比，计算量更少。如图4 所示，在相机帧IR1上x1对应的匹配点为相机帧IR2上的归一化坐标点x2=(y1,y2,1)T，然后过点x2且垂直于直线（绿线）得到x1的对极线，即（红线）。

已知基础矩阵F21、F31和F32，由于第三个相机矩阵P3必须满足条件是反对称矩阵，所以这些矩阵中的每一个矩阵都能产生10 个关于P3元素的线性方程，一共可以得到关于P3的12 个元素的20 个方程，从而线性求出P3。最终得到的图像帧相机矩阵对表示为：

根据相机矩阵对获取张量：

其中，矩阵P2的(i,j)元记为；矩阵P3的(i,j)元记为。

至此，得到了当前卷帘相机帧IR1对应的全局相机帧IG1下的GS 点xG-1。随着点特征追踪长度的增加，滑窗内不同的历史帧对应在全局图像上矫正后的多个GS 点，采用最小二乘法计算出最优的GS 点的像素坐标。

2.3 静态检测和异常检测

当系统处于静止状态时，图像上不同行像素对应的RS 位姿都是一致的，相当于不同行的像素是在同一时刻获取。因此，卷帘图像可以被当作全局图像，即可将图像直接输入VIO 系统，无须使用卷帘相机模型。将基于IMU 信息的检测方法引入静态检测中，有：

检测过程需要采用合适的卡方阈值χ2，并对惯性测量噪声R放大α（α∊ [50,100]）倍，检测系统是否处于零速状态。

其中，H和P分别为关于ba、bg和的雅克比矩阵和协方差矩阵。

当检测到系统处于零速状态时，系统将关闭卷帘相机模块，防止无效的点转移影响系统的定位精度和稳定性。当系统进行点特征匹配时，点特征匹配对可能是错误的，错误的匹配对导致矫正后的GS 点偏离真实值。因此，需采用随机采样一致性（Random Sample Consensus,RANSAC）算法[14]剔除偏离点，从而保证输入到VIO 系统的GS 点坐标值的准确性。

3 实验验证

为了验证本文算法的性能，采用公开的TUM-RSVI 数据集[15]进行实验，并与其他算法进行对比。同时，使用智能手机收集室内外数据，对各种算法进行测试，进一步验证本文算法的适用性和鲁棒性。

3.1 TUM-RSVI 数据集

TUM-RSVI 数据集由慕尼黑工业大学发布，是首个同时包含IMU、全局相机和卷帘相机的VIO 评估公开数据集。该数据集的卷帘相机和全局相机同步记录图像信息，并且IMU 和运动捕捉系统的数据在时间上是对齐的。数据集包含10 个室内数据，卷帘相机和全局相机均以20 Hz 的频率和1280×1024 的分辨率同步记录图像，IMU 的频率为200 Hz，运动捕捉系统以120 Hz 的频率记录真实运动，同时数据集给矫正后的卷帘图像提供了同一时刻的全局图像作为参照系。为了更好地验证卷帘模型的有效性，分别采用全局图像数据测试未带卷帘相机模型的VIO（OpenVINS[3]、VINS-Mono[4]）和使用卷帘图像测试带卷帘相机模型的VIO（RS-VINS-Mono[4]、文献[9]算法、本文算法（图、表中用OUR 表示））。

本文算法的状态向量里最多可存放75个“SLAM”特征，滑动窗口大小为11，每帧最大跟踪的特征数为150。在公共TUM-RSVI 数据集中，所有算法都是在配备Intel Core i7CPU、3.2 GHz 和32 GB RAM 的电脑上运行。

图5 和图6 展示了不同算法在TUM-RSVI 数据集中第三个和第四个序列上的轨迹。VINS-Mono 是基于优化的方法，RS-VINS-Mono 是VINS-Mono 的带卷帘相机模型的VIO 算法，其处理卷帘效应的方法类似于文献[16]。文献[9]和本文算法都有卷帘相机模型，并且都是基于滤波的VIO 系统，但文献[9]是基于恒速运动假设的线性模型，而本文算法则是基于非线性模型。OpenVINS 作为基于滤波的基线方法为本文算法提供参照。

图5 不同算法在TUM-RSVI 数据集中第三个序列的轨迹Fig.5 Trajectories of the different algorithm for the third sequence in TUM-RSVI dataset

图6 不同算法在TUM-RSVI 数据集中第四个序列的轨迹Fig.6 Trajectories of the different algorithm for the fourth sequence in TUM-RSVI dataset

计算各种算法在每个序列上绝对轨迹误差的均方根误差，每个序列进行六次实验，取平均值作为最终结果。实验结果如表1 所示。

表1 不同算法在TUM-RSVI 数据集上的绝对轨迹均方根误差（单位：米）Tab.1 Root mean square error of absolute trajectory for different algorithms on TUM-RSVI dataset (Unit: m)

表1 中图像数据类型包括全局图像（GS）和卷帘图像（RS）。从表1 可以看出，未带卷帘相机模型的VIO 算法（OpenVINS、VINS-Mono）在全局图像上保持不错的定位精度，但在卷帘图像上，尤其是在平均角速度特别大的序列9 和序列10 上，其绝对轨迹误差值较大，说明卷帘效应对系统定位精度的影响十分明显。通过对比表 1 中基线方法（OpenVINS、VINS-Mono）在卷帘图像上的误差，带卷帘相机模型的VIO 算法（RS-VINS-Mono、文献[9]算法、本文算法）在处理卷帘图像时能有效降低卷帘效应对系统定位精度的影响，并与未带卷帘相机模型的VIO 在全局图像上精度相当。如表1 所示，未带卷帘相机模型的基准算法（OpenVINS）在卷帘图像数据集上的平均绝对轨迹误差为47.352 m，本文算法在卷帘图像数据集上的平均绝对轨迹误差为0.076 m，定位精度提高了99%；与带有卷帘相机模型的RS-VINS-Mono 方法相比，本文算法的定位精度提高了27%；与文献[9]算法相比，本文算法的定位精度提升了 47%。RS-VINS-Mono 和文献[9]算法都是基于恒速运动模型的方法，在面对大的加速度场景时会出现较大的定位误差。相比之下，本文算法利用高频IMU 信息对RS点进行校正，不受场景限制和恒速运动假设的影响，实验结果也验证了本文算法在定位精度方面表现更好。

3.2 现实世界数据集

为了进一步评估本文算法在更具挑战性的现实场景下的性能，将系统集成到一个安卓手机应用程序中，并在一台华为手机上进行测试。卷帘相机以每秒25帧左右的频率和640×480 的分辨率记录图像，IMU 以每秒250 次的频率读取。如图7 所示，应用程序的界面左侧实时显示预览画面，左下角是启动VIO 按钮，右侧显示手机的运动轨迹（红色线）和3D 点云（白色点），fps 代表华为手机处理卷帘图像的帧率，一般每秒处理图像帧数保持在15 帧～35 帧之间。

图7 手机应用的运行界面Fig.7 Mobile application interface

打开安卓应用，并手持华为手机在室内环境移动，采用Nokov 运动捕捉系统记录定位真值，室内环境如图8 所示。

图8 Nokov 运动捕捉系统Fig.8 Nokov motion capture system

为了更好地验证本文算法在不同场景下的定位精度，采集了更多具有挑战性的数据，并制作了一个真实世界数据集。该数据集包含四个序列，每个序列都基于不同的运动模式，其中包括加速度变化显著的运动（A-S）、加速度稳定的运动（A-L）、带有部分静态状态的运动（V-C）和全程无静止状态的运动（V-N）。表2 给出四种带卷帘相机模型的VIO 系统在这些数据集上的性能。

表2 不同算法在现实世界数据集上的绝对轨迹均方根误差（单位：米）Tab.2 Root mean square error of absolute trajectory for different algorithms in real-world dataset (Unit: m)

在不同运动模式下，相较于RS-VINS-Mono，本文算法的定位精度提升了49%；相较于文献[9]算法，本文算法的定位精度提升了84%。另外，OURS-Z 是本文算法去除了零速检测模块，对比这两种算法在V-N 和V-C 两个序列上的实验结果可以看出，对于全程无静止状态的数据V-N，OURS-Z 和OURS 的精度几乎相当，但是对于带有部分静止状态的数据V-C，去除零速检测模块的OURS-Z 的性能相较于OURS 明显下降，进一步验证了零速检测模块的有效性。

3 结论

针对卷帘快门相机由于卷帘效应引起图像扭曲进而影响系统定位精度的问题，本文提出了一种融合IMU 信息的点转移矫正VIO 方法。通过基于三焦点张量的点转移的方法修正卷帘图像，相当于输入系统的是全局图像，提高了系统定位的准确性。本文算法是在基于滤波的框架下实现的，保证了整体算法在嵌入式设备上的实时性。并且，本文算法引入了静态检测和异常检测来提高算法的鲁棒性。在公共数据集上的实验结果表明，与带有卷帘相机模型的RS-VINS-Mono 方法相比，本文算法的定位精度提升了27%；与文献[9]算法相比，本文算法的定位精度提升了47%。本文算法与输入全局图像的基线方法（OpenVINS）的精度相当，进一步说明本文算法能有效降低卷帘效应带来误差，提升系统的定位精度。在更具挑战性的室内数据集上的实验结果表明，相比于其他算法，本文算法的定位精度提升了49%以上。此外，本文算法被嵌入到手机应用程序中，在手机上以25 Hz 左右的帧率实时处理卷帘图像，对解决基于卷帘快门相机的VIO在未知环境中的定位问题具有一定指导意义。