基于动态边缘化的双目视觉惯性SLAM算法

龚 欢 何志琴

(贵州大学电气工程学院 贵州 贵阳 550025)

0 引 言

状态估计和3D重建是机器人应用中的关键任务。多年来，已有各种传感器被用于完成以上任务，例如：GPS、轮速计、惯性测量单元(IMU)、激光雷达、相机等。近十年来，因相机和IMU的低成本、小体积、轻便和低功耗等特点，基于两者的融合方案(VINS)受到研究者们的普遍关注[1]。

一方面，相机利用多视图的理论可在运动中估计相机的位姿并构建场景的三维模型；另一方面，IMU作为板载传感器测量局部线性角速度和加速度，能够为视觉系统提供运动约束，恢复单目SLAM的绝对尺度和重力方向，同时提供鲁棒及精确的帧间运动估计[2-8]。相机具备图像信息丰富、没有运动漂移的优点，但在低纹理区域或快速运动的情况下缺乏鲁棒性；IMU具备数据更新快、外部依赖少的优点，但易发生运动漂移；VINS系统利用相机和IMU的这种互补性来实现鲁棒且精确的姿态估计。目前，视觉惯性SLAM(VI-SLAM)和视觉惯性里程计(VIO)已广泛用于室内、城市峡谷或其他星球等GPS信号缺乏场景的自主定位导航和三维重建[1]。

1 相关工作

近年来，关于视觉SLAM的研究非常广泛。根据使用的视觉残差模型可大致分为间接法和直接法。ORBSLAM2[9]是当前间接法中最先进的一个案例，通过提取和跟踪环境中的点特征，同时使用几何重投影约束来进行基于图优化的相机位姿优化。与基于特征的间接法不同，直接法[10]使用图像中的原始像素强度来估计相机的运动，可以得到稀疏的重建效果。其中，由于光度一致性假设，直接法需要良好的初始值和较高的帧率，而间接法提取和匹配特征时需额外的计算资源。

由于相机和IMU的互补特性，研究者通过融合两类传感器进行了大量研究。现有的大多数VIO/VI-SLAM算法主要都集中在单目系统，例如MSCKF[2]、ROVIO[3]、OKVIS[4]、VI-ORB[5]、VINS-Mono[6]、Maplab[7]、VI-DSO[8]。尽管可以从IMU加速度计测量值中估计出单目VIO/VI-SLAM算法的尺度，但是无法从带噪声的加速度测量值中准确地减去重力向量[11]且后端边缘化时同样会边缘化IMU信息[8]，因此单目VIO/VI-SLAM估计的尺度通常是不精确的。然而，基于双目视觉的VIO/VI-SLAM算法[11-17]可利用对极约束提供额外的尺度信息来获得更高的精度。

基于双目的VIO/VI-SLAM算法通常分为以下两类[1]：基于过滤的方法(例如PIRVS[12]、S-MSCKF[13]和Trifo-VI[16])和基于非线性优化的方法(例如ICE-BA[15]和 VINS-Fusion[17])。由文献[18-19]的实验对比结果可见，基于优化的方法比基于过滤的方法在定位精度方面更具潜力，而基于滤波的方法在计算成本方面更具优势。

受文献[18-19]对前沿的VI-SLAM方案在多种嵌入式平台性能评测结果及VIDSO[5]中提出的动态边缘化的启发，本文在VINS-MONO[6]的基础上，提出了一种基于动态边缘化的双目视觉SLAM(DM-SVI-SLAM)算法。该系统通过分而治之的方法初始化IMU，并根据IMU预积分预测当前帧的位姿；然后采用带动态边缘化的滑动窗口，以紧耦合的方式融合IMU预积分和视觉SLAM的位姿估计，并在构建优化求解器时使用并行化的Dog-Leg算法加速运算，最后根据构建的稀疏地图进行回环检测和重定位。

与现有的VI-SLAM算法相比，本文方法的主要创新点如下：

(1)传统单目VI-SLAM算法的尺度恢复完全依赖IMU估计值，在边缘化IMU信息和IMU发生漂移时尺度恢复不可靠，本文算法同时包含来自双目对极约束尺度估计值及各时刻的IMU尺度估计值，使得尺度恢复更加精确；(2)传统双目SLAM或者双目VI-SLAM后端捆集调整成本函数仅考虑双目匹配成功的误差项，而本文算法同时考虑了双目匹配和单目匹配的误差项，充分利用滑窗内的有效信息；(3)引入动态边缘化策略，降低可能对系统造成干扰的较“旧”信息在滑窗内的比重，既减少了后端过程的计算量，又使得算法更加鲁棒。

本文算法的框架流程如图1所示。

图1 DM-SVI-SLAM算法的框架流程

2 IMU预积分

2.1 相对运动增量

本文使用的检测器件为双目相机和IMU，考虑IMU器件的偏置和随机游走噪声，IMU测量模型定义为：

(1)

结合IMU测量模型，可推导出IMU传感器的运动状态位置p、速度v和旋转q，为便于描述，下文记为PVQ。根据运动学和微积分知识可得出从第i时刻的PVQ到第j时刻的PVQ：

(2)

从上述IMU积分公式可以发现，j时刻的运动状态依赖于世界坐标系的状态，即每次“旧”时刻的状态变化后，都需要重新计算其后的PVQ值。处理时通常使用式(3)将上述积分模型转为预积分模型：

qwbt=qwbi⊗qbibt

(3)

PVQ积分公式中的积分项则变成相对于第i时刻的姿态，而不是相对于世界坐标系的姿态：

(4)

预积分量仅仅跟IMU测量值有关，将一段时间内IMU数据直接积分起来就得到了预积分量：

(5)

重整PVQ的积分公式，可得式(6)，这里假定短时间内陀螺仪和加速度计的偏置不变。

(6)

由于传感器测量值是由采样数据得出，故需要将预积分模型转化成离散形式，本文通过mid-point方法实现，即两个相邻(k时刻和k+1时刻)的位姿是用两个时刻的测量值a、ω的平均值来计算：

βbibk+1=βbibk+aδt

(7)

2.2 IMU测量噪声

如果偏差的估计值变化很小，可以使用一阶线性近似表示：

(8)

否则，当偏差估计值变化较大时，可在新的偏差估计值的基础上重新传播。这一策略为基于优化的后端算法节省了大量的计算资源(不需要多次重复传播IMU的测量值)。

3 DM-SVI-SLAM

本文提出的DM-SVI_SLAM系统主要步骤包含视觉跟踪和IMU预积分、系统初始化、基于动态边缘化的后端优化和回环检测。

3.1 视觉跟踪和IMU预积分

本系统前端图像特征提取的是Shi-Tomasi角点。处理时为避免特征点分布不均，将特征点半径为10像素区域内的值全设置为0。使用光流法特征追踪，由于引入IMU数据，DM-SVI-SLAM的光流预测既可运用视觉估计的运动模型，又可运用IMU的预测结果，故使得本系统在快速运动的图像特征模糊的场景下依然能保持良好的鲁棒性。由于视觉跟踪技术在开源库OpenCV中已经有较好的实现，故本文不作介绍。

数据处理部分还包括IMU数据的预积分计算，其计算方法参照式(7)，为mid-point方法。

3.2 系统初始化

1)初始陀螺仪偏置估计。

通过传感器标定，标定出外参数qbc和pbc，利用式(9)的旋转约束，可根据式(10)估计陀螺仪偏置。

(9)

(10)

式中：B表示所有图像的关键帧集合。结合预积分的一阶泰勒近似：

(11)

对于式(10)的最小二乘问题，可由其雅可比矩阵构建正定方程HX=b求解。

2)初始速度和重力向量估计。

需要估计的变量如下：

(12)

(13)

将世界坐标系w换成相机初始时刻坐标系co有：

(14)

将式(3)代入式(14)进行简单整理有：

(15)

将待估计变量放到方程右侧，有：

(16)

式(16)中，有：

(17)

式(16)转化成线性最小二乘问题对状态量进行求解：

(18)

对于优化重力向量gco，根据事先标定可以得出模长大小(如‖gco‖=9.81)，但式(16)求解重力向量gco过程中，并没有加入模长限制。三维向量gco实际只有两个自由度。可通过球面坐标进行参数化：

(19)

式中：w1、w2为待优化变量。

(20)

估计出重力方向后，将式(19)代入式(16)，整理得：

(21)

如此迭代，直至g收敛，得出初始速度和重力方向g，系统的初始化完成。

3.3 基于动态边缘化的后端优化

系统在初始化后得到了初始陀螺仪偏置、速度及重力方向，接着需要对位姿进行求解和优化，简称为后端优化。本系统使用非线性优化方法融合视觉和IMU信息，并充分利用双目相机的匹配测距的特性，使得求解工作更具鲁棒性。如式(22)所示，误差项包含单目点残差项、双目点残差项、IMU预积分残差项和滑动窗口先验残差项四部分。

{qwb,vwb,pwb,ba,bg}=

(22)

其中，单目点残差项表示只在左右目中某一个相机检测到三维特征点残差，由于只有二维坐标，使用重投影误差计算其残差项。双目点残差项表示经校正的左右目匹配成功的三维特征点残差，由于可通过双目测距原理求取深度，可用式(23)计算。IMU预积分残差项即利用IMU信息约束前后两帧的状态量，分为预积分平移残差、预积分速度残差和预积分旋转残差。先验残差项即边缘化操作消去了旧状态的变量，并把约束关系叠加到滑窗内而保留的先验信息，随着迭代推进，变量被不断优化，先验残差需跟随变化。

(23)

传统的单VI-SLAM系统完全依靠IMU信息来估计尺度，但边缘化IMU信息易导致尺度漂移；双目虽然能够根据测距原理计算尺度，但是仅依赖双目视觉信息在特征缺失或重复纹理下易出现较大误差。为使得边缘化更有效，本系统使用种动态边缘化的策略。Mcurr包含了滑窗内完整的边缘化信息，但由于累积的信息越来越多，信息越来越陈旧，反而会干扰尺度估计的准确性，故当尺度超过缩放间隔后旧的参数直接使用双目视觉Mvisual来重置边缘化。

(1)Mcurr←Mvisual,smiddle←smiddle·di，scurr>smiddle·di。

(2)Mcurr←Mvisual,smiddle←smiddle/di，scurr

(24)

3.4 回环检测

当相机再次到达某区域时，通过回环检测能有效减小轨迹的累积误差。由于VINS-Mono的回环重定位模块能帮助取得不错的定位精度且计算简便，本文回环检测采用VINS-Mono的方案，详细介绍见文献[6]。

4 实验仿真

4.1 实验条件

本文使用公开可用的EuRoC数据集[20]评估提出的DM-SVI-SLAM系统的有效性。该数据集通过MT9 V304型号相机提供20 Hz的立体视觉图像，ADIS 16448同步提供200 Hz的IMU测量数据，且配备一套精准的运动捕获系统提供真实的轨迹数据。EuRoC根据测试过程中光照、环境纹理、运动快慢和图像模糊程度划分出了简单、中等和困难三个等级，共包含11个测试序列。本文实验测试平台为CPU i7-4770HQ，内存16 GB的笔记本电脑。实验的精度和鲁棒性评估标准采用算法估计轨迹与真实轨迹的绝对轨迹误差(ATE)的均方根误差(RMSE)，记为ATErmse，形如：

(25)

4.2 精度和鲁棒性评估

实验中，单目VIO/VI-SLAM算法选取VI-ORB[5]和VINS-Mono[6]作为代表；双目VIO/VI-SLAM算法选取了开源的S-MSCKF[13]、VINS-Fusion[17]和Basalt[21]作为代表，S-MSCKF是基于滤波的VIO算法，VINS-Fusion和Basalt都是基于非线性优化的VI-SLAM算法；此外还选取ORBSLAM2[9]纯双目版本进行比较。为保证评估实验结果的公平性，在4.1节的实验条件下，对于开源算法VINS-Mono、ORBSLAM2、S-MSCKF、VINS-Fusion、Basalt及本文提出的DM-SVI-SLAM取独立运行10次的实验结果均值进行对比。而VI-ORB由于代码未开源，比对结果取自文献[5]中的公开数据。测试结果如表1所示，其中"×"代表跟踪失败。图2给出了本文的DM-SVI-SLAM算法在MH5序列中的追踪效果。图3显示了MH5和V13上估计轨迹的示例。

表1 EuRoC 数据集中的ATErmse对比 单位：m

图2 MH5序列中本文算法追踪效果

(a)MH5的估计轨迹与真值

由表1可以看出，与纯双目ORBSLAM2、VI-ORB和Basalt相比，本文方法在场景较大的工厂序列中的均方根误差结果更优，而在场景较小的室内Vicon序列中精度略低于这些方法，这是由于本文算法的动态边缘化策略和综合考虑单/双目匹配点的策略在大场景和较低速下作用更大。值得注意的是，VI-ORB算法在V13序列，ORBSLAM2和Basalt算法在V23序列这两个快速运动抖动剧烈场景下跟踪失败，而VINS-Mono和VINS-Fusion及本文方法都可以正常跟踪全部序列，说明光流法前端比ORB特征点法前端在快速运动下具有更好的鲁棒性。从精度方面比较，S-MSCKF方法总体来说表现最差，这是由于该算法没有回环检测模块且后端优化位姿误差时使用了局部线性化来简化计算导致。与开源的VINS-Mono和VINS-Fusion比，DM-SVI-SLAM在所有序列的均方根误差均有明显降低，说明后端BA优化构建成本函数，考虑单目匹配点误差和双目匹配点误差并引入动态边缘化的策略，可以有效提升位姿估计的精度。

4.3 实时性评估

为评估本文算法的实时性能，使用CPU i7-4770HQ，内存16 GB的笔记本电脑在EuRoC数据集上进行测试。值得注意的是，为说明引入动态边缘化对计算效率的影响，本文算法运行时采用的是和VINS-Fusion相同的Levenberg-Marquardt算法进行优化求解。表2显示了本文算法、S-MSCKF和VINS-Fusion处理一帧的平均耗时。由于S-MSCKF后端仅基于EKF进行一次优化，因此效率最高，但如表1所示，S-MSCKF获得的估计精度最差。与VINS-Fusion相比，本文算法的执行效率更高，这是因为本文方法使用动态边缘化策略可降低较“旧”信息在滑窗内的比重，从而节省了后端过程的计算资源。

表2 EuRoC数据集中平均处理时间对比

为进一步提升算法的实时性能，本文根据VINS-Mono的开源版本代码，发现程序中有MakeHessian函数和Marginalize函数两部分涉及Hessian矩阵构建。VINS-Mono中只在矩阵的拼接过程进行加速,与VINS-Mono不同，本文实验对Marginalize函数中每条边残差和雅可比矩阵的计算都进行四线程加速。

本节对比了文献[22]中记录的五种Levenberg-Marquardt算法和文献[23]记录的三种Dog-Leg算法。其中VINS-Mono开源程序中的后端求解器实现与文献[21]中的算法1原理相同，为便于比较，本节将文献[22]的五种Levenberg-Marquardt算法按原文顺序依次编号为1-5，而文献[23]的Dog-Leg方法依原文顺序依次编号为6-8。关于各算法的详细介绍见文献[22-23]。

实验硬件平台同4.2节实验，测试数据集均为EuRoC MH_05_diffcult。表3是前面介绍的几种后端求解算法的solve时间、轨迹精度误差及变化百分比(以算法1为基准)的对比结果。可见算法6(文献[23]第一种Dog-Leg)的精度最高(较VINS-Mono原方法提升了20.104%)，耗时也最短(较VINS-Mono原方法降低13.450%)。此外，Dog-Leg系列的算法在耗时方面均有较大提升。这是由于Dog-Leg在无效迭代过程中可直接利用之前计算得出的高斯牛顿法增量和最速下降法增量，结合当前置信区间delta，获得新的迭代增量，从而缩短计算时间。图4更直观地显示了不同算法的对比效果。

表3 不同方法在MH5上的solve时间和轨迹误差对比

图4 各方法solve时间和轨迹误差对比

5 结 语

本文提出了带动态边缘化的双目视觉惯性SLAM算法(DM-SVI-SLAM)，相比VI-ORB采用IMU辅助单目视觉的方案，双目VI-SLAM算法具有更多约束信息。通过EuRoC数据集验证了双目VI-SLAM在快速运动引起图像模糊及特征缺失等有挑战的场景下鲁棒性更好，且与其他两个两开源的双目VI-SLAM算法S-MSCKF和VINS-Fusion相比，本文算法的定位精度效果更高。最后，为降低双目视觉系统计算量增加的影响，本文通过实验对比了多种Levenberg-Marquardt算法、Dog-Leg算法的后端求解效果，结果表明采用Dog-Leg算法有助于降低计算耗时。本文并未对DM-SVI-SLAM使用GPU加速计算，下一步将研究更多加速方案以降低整个双目视觉惯性融合系统的计算耗时。此外，将该系统应用于移动机器人或无人机等也是未来研究方向。