CN110726406A

CN110726406A - 一种改进的非线性优化单目惯导slam的方法

Info

Publication number: CN110726406A
Application number: CN201910477264.9A
Authority: CN
Inventors: 危双丰; 刘振彬
Original assignee: Beijing University of Civil Engineering and Architecture
Current assignee: Beijing University of Civil Engineering and Architecture
Priority date: 2019-06-03
Filing date: 2019-06-03
Publication date: 2020-01-24

Abstract

本发明公开了一种改进的非线性优化单目惯导SLAM的方法，以VINS‑mono工程为基础，采用非线性优化法以紧耦合的方式耦合IMU数据，其方法包括：图像和IMU预处理；初始化；后端滑窗优化；闭环检测和优化。本发明基于已有性能较好的VINS‑mono，进行算法改进，在公开数据集EuRoC上进行了测试，并与之前的VINS‑mono方案进行了对比分析。其结果表明，IMU初始化能够很好的恢复系统的尺度因子，解决了单目SLAM尺度不确定性问题，能够较快、较准确的完成初始化，并且能够鲁棒的进行实时运动估计和稀疏地图构建，改进后的系统在精度和鲁棒性上都得到了有效提升。

Description

一种改进的非线性优化单目惯导SLAM的方法

技术领域

本发明属于机器人视觉定位导航技术领域，特别是涉及一种改进的非线性优化单目惯导SLAM的方法。

背景技术

同时定位与地图构建(Simultaneous Localization and Mapping，简称SLAM) 技术是移动机器人定位导航技术的重要方向，指运动物体在运动过程仅依赖所携带的传感器实现对自身的定位，同时对周围环境进行地图构建。由于相机较激光传感器价格更低，体积更小，且获取的图像所含信息更为丰富，基于视觉的SLAM(V-SLAM，简称视觉SLAM)技术成为了研究热点；相较于计算量较大的双目相机以及RGB-D深度相机，单目相机具备耗电量小，体积小，重量轻等优点，尤其在小型移动机器人(如微型无人机)平台上有着巨大的优势。因此单目SLAM技术可应用于不同的移动机器人平台，有着重要的研究价值。

然而仅依赖单目相机信息进行移动机器人自身的定位于导航，存在尺度模糊的问题，无法获得运动轨迹的真实长度，这就是单目SLAM中存在的尺度模糊性。这种缺陷限制了其大量的应用。为了解决这一缺陷，当前却来越多的解决方案倾向于传感器融合的方式，利用传感器获取数据的不同特性，进行优势互补，取得更好的效果。在不同的传感器模态中，相机与IMU的组合鲁棒性好且成本低廉，是很有潜力的解决方案。单目视觉和IMU融合的SLAM技术又称为视觉惯性导航技术(Visual-Inertial Navigation System，VINS)。一方面，相机提供了丰富的环境信息，可构建三维模型、运动恢复和识别已访问的地点；另一方面，IMU传感器提供了自身运动状态信息，可以恢复单目视觉的尺度信息，估计重力方向，提供可视的绝对俯仰和滚动信息。二者的互补性使得它们融合后可获得更高精度和更好的系统鲁棒性。然而，在单目SLAM中融入IMU会使得系统变得更加复杂，IMU在测量过程中既存在测量噪声，又存在加速度和角速度偏差(bias)，从而引来大量新的问题，包括：不同传感器时间同步问题、系统状态变量的初始化问题、IMU预积分问题、非线性优化问题、闭环检测和全局优化等问题。

发明内容

本发明的目的就是提供一种改进的非线性优化单目惯导SLAM的方法，基于目前单目视觉和IMU融合性能最好的VINS-mono系统，改进初始化方案，增加了加速度bias偏差的初始化，使其能够适用于低成本的IMU传感器，将滑动窗口优化和初始化结合起来，其精度和鲁棒性都得到了有效提高。方案如下：

一种改进的非线性优化单目惯导SLAM的方法，以VINS-mono工程为基础，采用非线性优化法以紧耦合的方式耦合IMU数据，其方法包括：1)图像和IMU 预处理；2)初始化；3)后端滑窗优化；4)闭环检测和优化。

进一步，视觉初始化的目的是利用最开始一段时间内的关键帧图像序列进行位姿解算和三角化，并进一步进行全局优化；这里图像关键帧的选择主要根据视差的距离，当视差距离大于某阈值，即选为关键帧；利用基于视觉的SFM 技术得到关键帧序列较为精确的位姿和图像点坐标，为IMU的初始化提供较为精确的位姿参数；为了使得视觉初始化不依赖于场景，即无论初始化场景是平面还是非平面都能获得较为精确的初始值，本系统初始的两图像关键帧采用并行计算基础矩阵和单应性矩阵的方法，根据一定的机制选择合适的模型；根据初始的两关键帧固定场景尺度和初始化三角点，然后使用PnP算法恢复运动，并不断进行三角化，恢复地图点坐标；跟踪一段序列后，构建图像的重投影误差进行BA全局优化，得到优化后的地图点和位姿。

进一步，IMU初始化是利用视觉SLAM初始化的结果对IMU进行解耦，分别解算出其初始值，IMU初始化过程可分解成四个小问题进行求解：(1)陀螺仪偏差的估计；(2)尺度和重力加速度的估计；(3)加速度偏差的估计以及尺度和重力的优化；(4)速度的估计。

进一步，对单目VINS系统进行了初始化的操作，为后续的非线性提供较好的初始值，其中包括：后端滑窗非线性优化、边缘化的策略、闭环检测与优化、全局位姿优化。

与现有技术相比，本发明的有益效果在于：本发明基于已有性能较好的 VINS-mono，进行算法改进，在公开数据集EuRoC上进行了测试，并与之前的 VINS-mono方案进行了对比分析。其结果表明，IMU初始化能够很好的恢复系统的尺度因子，解决了单目SLAM尺度不确定性问题，能够较快、较准确的完成初始化，并且能够鲁棒的进行实时运动估计和稀疏地图构建，改进后的系统在精度和鲁棒性上都得到了有效提升。

附图说明

图1是单目VINS系统结构框架图；

图2是视觉SFM流程图；

图3是不同坐标系的转换关系图；

图4是边缘化策略示意图；

图5是闭环检测与优化流程图；

图6是闭环检测与优化示意图；

图7是选取的EuRoC数据集场景图；

图8是IMU序列初始化参数MH_01_easy数据集；

图9是IMU序列初始化参数V1_02_medium数据集；

图10是相机IMU标定结果；

图11是宿舍楼系统运行结果图；

图12是操场系统运行结果图；

图13是改进后的VINS-mono运行效果图；

图14是位移误差数据集；

图15是误差对比分析图。

具体实施方式

下面结合具体实施例和附图对本发明作进一步的说明。

针对目前纯视觉SLAM(Simultaneous Localization and Mapping)系统易受环境光照、纹理、以及动态环境等场景的影响，其中单目SLAM系统还存在着初始化尺度模糊的问题，本发明使用单目相机和IMU(Inertial Measurement Unit)融合的方式有效解决了单目SLAM系统中存在的尺度模糊问题，且这两种传感器的融合显著提升了系统的鲁棒性并能获得更高的定位精度。本发明基于目前单目视觉和IMU融合性能最好的VINS-mono系统，改进初始化方案，增加了加速度bias偏差的初始化，使其能够适用于低成本的IMU传感器。本发明借鉴ORB-SLAM方案，将基于ORB特征点法的前端应用于VINS-mono方案中，结合改进后的初始化方案，实现了较高精度的实时定位与稀疏地图重建。通过在 EuRoc数据集中不同场景下的实验与分析，证明了改进后的VINS-mono方案相比之前的精度和鲁棒性都得到了提高。

1.基于单目相机和IMU融合位姿估计研究现状。基于视觉的SLAM代表性的工作包括ORB-SLAM，PATM，SVO，LSD-SLAM，刘浩敏和高翔等人对其进行了详细的综述介绍。本发明重点阐述单目视觉与IMU融合的研究现状。单目视觉和IMU 融合最简单的方式是松耦合。这种方式将IMU看成独立的模块用于辅助视觉通过运动恢复结构求解的结果。松耦合的方式主要通过扩展卡尔曼滤波(Extended Kalman Filiter，EKF)算法实现即：IMU数据进行状态估计，由单目视觉算出的位姿进行更新。基于紧耦合方式的状态估计算法有EKF和图优化。紧耦合方式是对IMU和单目相机获得的原始数据进行联合优化。MSCKF方案是基于EKF方式效果较好的方法，该方案维护几个先前的相机姿态，使用相同的特征点进行多视角投影，形成多约束的更新。基于光束法平差的优化法对所有测量值进行全局优化获得最优解，由于对非线性系统进行进迭代求解需要消耗大量的计算资源，在有限计算资源的平台上，这种方式很难达到实时性。为了保证位姿估计的算法复杂度的一致性，通常使用滑动窗口滤波的方式。

在实际应用中，IMU的测量频率一般为100HZ-1000HZ，比相机拍摄频率高很多。会导致状态估计问题变得更加复杂。为此，最直接的解决方式就是使用 EKF方法，利用IMU测量进行状态估计，再利用相机数据进行状态预测值进行更新。另外一种方式预积分理论方法，它是在图优化框架内，为了避免重复对IMU 数据进行积分，减少计算量，而出现的，该理论最早由T Lupton提出，使用欧拉角的方式参数化旋转误差；S.Shen等人对旋转使用流行上的参数化，并推导出了误差传播过程的协方差。然而其没有考虑IMU测量过程中的随机游走误差， C.Forster^[21]对其进一步完善了该理论，增加了对IMU随机游走偏差的改正。

综合国内外现状，基于单目相机和IMU融合的定位导航系统的研究取得了一定的成果，如表1，但几乎都是处于理论研究阶段，无法实现大场景高精度鲁棒性的实时定位与建图，仍无法满足移动平台实现实时定位与导航的现实需求。因此，单目相机和IMU融合的VINS系统仍具有很高的研究价值。

表1视觉IMU融合的VINS方案

2.视觉惯导融合初始化。单目VINS和视觉SLAM本质上都是一个状态估计问题。本发明以VINS-mono工程为基础，采用非线性优化法以紧耦合的方式耦合IMU数据。VINS-mono的功能模块包括五个部分：数据预处理、初始化、后端非线性优化，闭环检测，以及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化、闭环检测、闭环优化。本发明的主要框架和 VINS-mono相同，主要在前端图像跟踪线程和后端非线性优化线程的初始化部分进行了改进。改进的VINS-mono系统整体框架如图1所示。各个功能模块的主要作用有：1)图像和IMU预处理。提取图像进行金字塔处理，对每层图像提取 ORB特征点，根据特征点法进行匹配相邻帧，通过RANSAC去除异常点，最后将跟踪到的特征点push到图像队列中，并通知后端进行处理。将IMU数据进行积分，得到当前时刻的位置、速度、和旋转(PVQ)，同时计算在后端优化中将用到的相邻帧的预积分增量，及预积分误差的Jacobian矩阵和协方差项。2)初始化。利用SFM进行纯视觉估计滑窗内所有帧的位姿和3D点的位置，最后与IMU 预积分结果对其进行求解初始化参数。3)后端滑窗优化。将视觉约束、IMU约束、和闭环约束放在一个大的目标函数进行非线性优化，求解滑窗内所有帧的速度、位置、姿态、bias等。4)闭环检测和优化。利用DBoW3进行闭环检测，当检测成功后进行重定位，最后对整个相机轨迹进行闭环优化。

2.1基于特征点法的视觉SFM。视觉初始化的目的是利用最开始一段时间内的关键帧图像序列进行位姿解算和三角化，并进一步进行全局优化。这里图像关键帧的选择主要根据视差的距离，当视差距离大于某阈值，即选为关键帧。利用基于视觉的SFM技术得到关键帧序列较为精确的位姿和图像点坐标。为下文IMU的初始化提供较为精确的位姿参数。为了使得视觉初始化不依赖于场景，即无论初始化场景是平面还是非平面都能获得较为精确的初始值，本系统初始的两图像关键帧采用并行计算基础矩阵和单应性矩阵的方法，根据一定的机制选择合适的模型。根据初始的两关键帧固定场景尺度和初始化三角点，然后使用PnP算法恢复运动，并不断进行三角化，恢复地图点坐标。跟踪一段序列后，构建图像的重投影误差进行BA全局优化，得到优化后的地图点和位姿，如图2。

2.1.1并行计算两个模型与模型选择。在相机运动过程中，系统的初始化可能发生纯旋转或特征点分布在同一平面的情况，这种情况下本质矩阵的自由度会发生退化，无法求得唯一解。因此本发明为使得初始化更为鲁棒，采取并行计算本质矩阵E和单应性矩阵H两个模型。最后选择重投影误差较小的模型，恢复运动和地图构建。在实践中，特征匹配存在大量的误匹配，直接使用这些点对解算本质矩阵和单应矩阵是不准确的。因此系统中使用随机采样一致性 (Random Sample Concensus，RANSAC)算法首先对误匹配的点对进行去除。然后利用剩下的点对求解本质矩阵或单应矩阵，进一步得出不同模型下的位姿变换，由于单目视觉存在尺度不确定性，因此在分解过程中需要对尺度进行归一化处理。根据这两个模型下的R和t可进一步三角化出初始地图点。最后利用这两个模型恢复出来的位姿参数分别计算重投影误差，选择重投影误差较小的模型进行运动恢复和地图构建。

2.1.2运动恢复与BA优化。系统中选取10秒的关键帧数据解算变量的初始值，在视觉初始化之后，已经有了三角化的地图点，因此剩下的关键帧数据可利用3D-2D的方式进行姿态求解，为防止地图点退化，在这过程需要不断地三角化地图点。最后根据这10秒内的关键帧数据构建光束法平差(Bundle Adjustment，BA)。对相机的位姿和地图点的坐标进行优化，此时优化的目标函数为：

式中N——关键帧的数量；N_j——每一帧能够看到的地图点的个数。

2.2IMU初始化。IMU初始化的思路是利用视觉SLAM初始化的结果对IMU 进行解耦，分别解算出其初始值。IMU初始化过程可分解成四个小问题进行求解： (1)陀螺仪偏差的估计；(2)尺度和重力加速度的估计；(3)加速度偏差的估计以及尺度和重力的优化；(4)速度的估计。为了描述刚体在三维空间的运动，以及搭载在上面相机和IMU传感器的位姿关系，本发明定义其位姿转换关系如图3。定义IMU坐标系和刚体坐标系(body)重合，T_BC代表相机坐标下的坐标转换到IMU坐标系下的变换矩阵，由R_BC和t_BC组成。T_WB表示刚体坐标系与世界坐标系的变换关系(R_WB代表旋转部分，W_P表示平移部分)。

2.2.1陀螺仪偏差估计。陀螺仪的bias偏差可以从由视觉初始化过程解算的旋转与IMU预积分的结果进行解耦解算出来。在初始化过程中，由于时间短暂，可假定b_g是个是个常量，不随时间变化。在整个初始化过程中，由视觉SFM 可求解相邻关键帧的旋转，由IMU的预积分也可得到相邻帧之间的旋转，假定由视觉SFM得到的旋转矩阵是准确的，因此通过这两个旋转矩阵对应李代数的差值即可间接求出b_g的大小。具体计算公式如下：

上式N代表关键帧的数量，

△R_i,i+1代表两相邻关键帧之间陀螺仪的积分值。上标i代表第i关键帧的时刻，i+1同理。

可有视觉SFM获得R_CB是IMU坐标系在相机坐标系下的旋转矩阵，由事先标定得到，这里不展开阐述相机和IMU的标定方式。由第二张章节讲述的基于非线性优化的高斯牛顿法可对公式(2)求解，解耦出b_g的值。

2.2.2尺度和重力估计。计算出b_g，重新将其带入预积分公式，计算出相邻帧之间的位移，速度，和旋转。由于视觉SFM结算出来的位移是不具备真实尺度的，因此相机坐标系和IMU坐标系的转换包含一个大小为s的尺度因子。两坐标系的转换方程如下：

_WP_B＝s_WP_C+R_WCCP_B (3)

由IMU预积分^[21]可知。两连续关键帧之间的运动可写成如下形式：

式中W——世界坐标系(惯性系)；B——IMU坐标系和载体坐标系。将公式(4)带入(3)中，由于加速度计的bias偏差相对于重力加速度而忽略不计，此时先忽略加速度计的偏差。得到下式：

该公式的目的在于解算尺度因子s和重力向量。可看出上式包含速度变量，为减少计算量并消除速度变量，进行下列变换。公式(5)是一个三维的线性方程，利用三个关键帧的数据可列出上述两组线性方程并利用公式(4)的速度关系式进行相关替换，即可将速度变量消去。将公式(5)写成矩阵形式。

为方便描述将三个关键帧符号i，i+1，i+2写成1，2，3。具体形式如下：

方程(6)可构造成A_3(N-2)×4x_4×1＝B_3(N-2)×1形式的线性系统。尺度因子和重力向量组成了一个4维的变量，因此至少需要4帧关键帧数据进行求解。可利用 SVD分解求解s^*和

2.2.3加速度偏差的估计以及尺度和重力的优化。由公式(5)可看出，在求解重力和尺度因子过程中没有考虑加速度计bias偏差。因为加速度的bias 偏差和重力很难进行区分。但如果不考虑加速度计的bias很容易形成病态系统，为增加系统的可观性，在此引入一些先验知识。已知在地球上重力加速度大小G 为9.8，方向指向地心，惯性参考系下，认为重力的方向向量为g_I＝{0,0,-1}。根据

可计算得到重力的方向

进一步可计算得出惯性坐标系到世界坐标系的旋转矩阵R_WI，如下式：

因此重力向量可表达为：g_W＝R_WIg_IG (9)

将世界坐标系的z轴和重力方向对齐，因此R_WI仅需对围绕轴和轴的角度进行参数化并加以优化。使用扰动δθ对对旋转加以优化，如下式：

对公式(10)取一阶近似得：

g_W≈R_WIg_IG-R_WI(g_I)_×Gδθ (11)

将公式(11)带入公式(3)，此时考虑加速度bias偏差，得到下式：

和公式(6)类似，在连续三个关键帧内，可以消除速度变量，得到下列形式：

其中λ(i)和公式(6)相同，φ(i)，

ψ(i)具体形式如下：

其中表示矩阵的前两列。根据连续关键帧之间的关系，根据公式(13)可以构造类似形式的线性系统A_3(N-2)×6x_6×1＝B_3(N-2)×1。这个线性系统有6个变量，个方程，因此至少需要4个关键帧数据求解该线性系统。通过SVD分解求得最大奇异值和最小奇异值的比值，检验问题是否得到很好解决，如果比值小于一定的阈值，可重新线性化公式(12)迭代求解该问题。

2.2.4速度的估计。根据上述几个小节，已经解耦出b_g，s，g_W，b_a。至此，只剩下3N个有关速度的变量。此时，将b_g，s，g_W，b_a代入公式(12)即可求解出各个关键帧的速度。

3.单目VINS的融合定位与建图。对单目VINS系统进行了初始化的操作，为后续的非线性提供较好的初始值，本节详细阐述单目VINS的融合定位与建图的详细过程，其中包括：后端滑窗非线性优化、边缘化的策略、闭环检测与优化、全局位姿优化等内容。

3.1后端滑窗非线性优化。随着系统的运行，需要进行优化的变量会逐渐增多，为了使得计算量维持在一定的范围，保证系统的实时性，单目VINS-mono 采用的是基于滑窗的非线性优化。只让当前一段时间内的关键帧数据参与位姿的计算。滑窗内的状态变量共包含个时刻的所有相机的状态(包括位置、朝向、速度、加速度计bias和陀螺仪bias)、相机到IMU的外参、个3D点的逆深度，数学符号表示如下：

上述的选择根据计算机的性能，当然越大，需要优化的状态变量就会越多，对计算机的性能要求也就越高。

3.1.1单目VINS目标函数。单目VINS的目标函数包括三个残差项，分别为边缘化的先验信息、视觉重投影误差项、IMU测量残差项。写成方程形式如下：

VINS-mono对IMU预积分采用的是基于Hamilton四元数的方式进行推导而来，基于四元数推导得到的误差测量模型为：

基于四元数推导得出的IMU预积分误差传播方程可参考，具体形式如下：

误差协方差可按下式逐步更新。初值

Q是噪声

对角协方差矩阵，雅可比矩阵的更新如下：

J_t+δt＝(I+F_tδt)J_t，t∈[k,k+1] (19)

基于视觉的重投影误差在第三章的光束法已进行了介绍。视觉约束的噪声协方差与标定相机内参时的重投影误差，也就是偏离几个像素有关，这里取的 1.5个像素，对应到归一化相机平面上的协方差矩阵需除以焦距f，信息矩阵等于协方差矩阵的逆，得到：

3.1.2单目VINS目标函数边缘化。VINS-mono根据次新帧是否为关键帧，分为两种边缘化策略，如图4。A.当次新帧为关键帧时，边缘化掉最老帧，及其看到的路标点和相关联的IMU数据，将其转化为先验信息加到整体的目标函数中。B.当次新帧不是关键帧时，直接扔掉次新帧及它的视觉观测边，而不对次新帧进行边缘化，因为此时认为当前帧和次新帧很相似，也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近，直接丢弃并不会造成整个约束关系丢失过多信息。但是值得注意的是，应保留次新帧的IMU数据，从而保证IMU预积分的连贯性。通过以上过程先验项就构造完成了，在对滑动窗口内的状态量进行优化时，把它与IMU残差项和视觉残差项放在一起优化，从而得到不丢失历史信息的最新状态估计的结果。

3.2闭环检测与优化。VINS-mono使用词袋模型进行回环检测，改进的 VINS-mono系统前端使用的特征提取的ORB特征点只有几十个，对于闭环检测远远不够，因此会对新来的关键帧(后端非线性优化处理完的帧)再重新提取300 个ORB特征点进行闭环检测使用。同时计算当前帧与词袋模型的相似度分数，并与关键帧数据库中所有的帧对比，进行闭环一致性检测，获得闭环的候选帧。然后利用当前帧的特征点和检测到的闭环帧利进行求解基础矩阵并对异常点进行RANSAN排除，当匹配点的数量超过阈值，则认为该候选帧是一个正确的闭环帧。闭环校正优化的时候为了减少计算量，只对系统的位姿图进行优化，闭环线程优化位姿时也不同于运动跟踪线程和构建地图线程，由于检测出的闭环关键帧和当前帧之间的运动约束随着时间和空间规模的扩大越来越不确定，所以闭环线程不对IMU的偏差和线性速度进行优化。闭合回环线程的主要流程如图5 所示。当检测到当前帧与之前某一帧(记为第v帧)存在闭环时，则将第v帧的位姿和相关特征点作为视觉约束项，加到后端非线性优化的整体目标函数中，并固定第v帧的参数，来优化滑窗内的其他参数。根据上述思想，目标函数可写为：

这样，即可以根据当前帧的闭环帧对滑动窗口内的位姿进行优化，当下一帧来时，一般也会与数据库中的某一帧为闭环帧，因此可以用多个闭环帧对滑窗内位姿进行多重约束优化，如图6。

3.3全局位姿优化。上述的关键帧数据库，是当滑动窗口内的关键帧被移出滑窗时才会添加进数据库中，这样可以保证添加进来的关键帧位姿基本正确，因为已经经过前面的重定位。当然，即使不正确也无所谓，因为会在闭环优化时进行修正。数据库中的帧对于下一步的闭环优化会提供两种边：

a.序列边(Sequential edge)：是指通过VIO计算的两帧之间的相对位姿：

b.闭环边(Loop edge)：是指检测到闭环的两帧。

随着运行时间增加，数据库将变得越来越大，导致闭环检测和闭环优化的耗时也将越来越长。虽然前面已经仅保留每帧图像的位姿和特征点描述子，已扔掉原始图像，但是当运行几小时后仍将无法实时。因此，本发明根据分布密度对数据库进行下采样，保留那些与周围帧的位置和角度不同的，而丢弃集中在一起的帧。当从滑窗内滑出的帧检测为闭环帧时，则对数据库的所有帧进行闭环优化。因为前面已经跟重力对齐，根据重力方向可以观测出俯仰角θ和翻滚角φ，即pitch和roll可观。因此闭环优化时，仅需优化x，y，z，和yaw这四个自由度。那么，第i帧和第j帧的残差可写为：

所有序列边和回环边的整体目标函数可写为：

其中，S为序列边，ζ为回环边。ρ(·)为huber核函数，能够有效的减弱错误边带来的误差，定义如下。至此就完成了整个的全局位姿优化。

4.实验结果及分析。本实验是基于公开数据集EuRoC上的数据进行精度分析与验证。EuRoC数据集由苏黎世联邦工学院采集生成。该数据集基于搭载到无人机上的双目VIO相机采集得到，是测试单目VINS系统的主流数据集。该数据集包含两个场景，工业厂房，和普通室内场景如图7，并根据环境特征纹理的多少，光线的变化，运动速度和图像质量，分为简单，中等和复杂三类。本实验只采用IMU测量的加速度和加速度信息以及由左目相机采集的图像序列。

4.1IMU初始化实验结果与分析。本发明分别在不同场景的两个难度级别的数据集(MH_01_easy，V2_02_medium)上进行了实验。该实验的目的是为了测试初始化过程系统变量的收敛情况，如图8和图9是初始化过程加速度计的 bias，角速度计的bias，尺度以及系统条件数的曲线图，很好的反映出了各个变量初始化过程的收敛情况。从图8和图9可看出，大约10秒左右的时间里， IMU的加速度计的bias，角速度计的bias基本都已经收敛到了一个稳定的值。尺度估计值也基本和优化后的值接近。优化后的值由位姿估计值和真实值位姿的相似变换求得。在10秒之后条件数明显下降并收敛情况表明了系统初始化对时间的要求，即只有各个IMU初始值得到很好的估计，保证系统的能观行，系统才能很好的初始化。

为测试改进后的初始化算法对低成本传感器的适用性。本发明采用小觅双目摄像头标准版^[26]进行初始化测试。相机和IMU两传感器之间的融合需要知道两者坐标系间的关系T_bc。因此本发明在初始化前对相机和IMU进行了离线标定，并在后续计算中进行固定，不参与优化。本发明使用Kalibr工具包对其进行标定如图10。将标定好的参数带入到系统文件中，进行校园环境的测试。在实验过程中，这两个环境下，VINS-mono均不能完成初始化工作，因为小觅相机内的 IMU传感器加速度偏差较大，其产生的误差造成初始化过程中变量不收敛，进而造成初始化失败。改进后的VINS-mono运行结果如图11和图12，可看出改进后的VINS-mono算法能够较好的适用于低成本的IMU传感器中，成功完成初始化并进行定位与建图。通过上述得到，通过视觉SFM对视觉IMU初始化变量分别解耦的思路是一个正确有效的方法，该方式能够有效的完成系统的初始化，并且能够适用于低成本传感器。由图8和图9可看出，该初始化算法大约15秒左右就可完成整个初始化的过程。

4.2单目VINS同步定位与建图实验。本实验采用的数据集和IMU初始化实验相同(MH_01_easy，V2_02_medium)，对改进后的单目VINS系统在不同难度等级，不同的环境下进行测试。本发明对不同数据集下运行产生的轨迹进行了精度分析，并进一步与VINS-mono在精度上进行了对比分析。通过跟当前性能较好的VINS-mono进行精度对比，可直观的看出改进后的单目VINS系统的性能是否得到了提升。本实验通过运行两个不同测试环境下的数据集可得到载体的运动轨迹如图13，由于数据集内含有真实轨迹坐标，通过计算估计的轨迹与真实轨迹之间的误差，即可得出改进后系统的轨迹精度。根据图14可看出系统在运行MH_01_easy数据时，轨迹结果误差较小，且累计误差得到了很好的消除，这是由于MH_01_easy序列，数据采集时无人机速度缓慢，系统成功检测到闭环并进行了全局优化操作。测试完改进后的VINS系统在两不同环境下的结果，同样，本发明测试了改进后单目VINS系统在EuRoc剩下的所有数据集的轨迹精度，如图15和表2。

为了精确的分析得到改进后单目VINS的性能，本发明使用均方根误差 (RMSE)的形式描述改进后单目VINS的系统精度。将关键帧估计位姿表示为 Q_i∈SE(3)，i＝1K n，真值位姿表示为L_i∈SE(3)，i＝1K n。对所有关键帧解算的误差求其均方根误差，得到最终的轨迹精度结果，如下式：

根据表2可看出改进后的VINS-mono系统除了V2_03_difficult和 MH_04_diffeicut两个序列外，其余序列的数据的测试轨迹精度均低于2％，尺度误差除了MH_04_diffeicut序列，均低于2％。除此，改进后的VINS-mono与 VINS-mono也进行了精度的对比分析。如图12，表3。从表3得到，不同的数据下均表明，改进后的单目VINS系统在精度上均要好于改进前的VINS-mono系统。

表2单目VINS的轨迹精度(EuRoc数据集)

表3改进后的VINS-mono系统与VINS-mono的对比

5.结论。本发明基于改进后的单目VINS-mono方案，在公开数据集EuRoC 上进行了测试，并与之前的VINS-mono方案进行了对比分析。实验结果表明， IMU初始化能够很好的恢复系统的尺度因子，解决了单目SLAM尺度不确定性问题，能够较快，较准确的完成初始化，并且能够鲁棒的进行实时运动估计和稀疏地图构建，改进后的系统在精度和鲁棒性上都得到了提升。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.一种改进的非线性优化单目惯导SLAM的方法，其特征在于：包括的方法有：1)图像和IMU预处理，提取图像进行金字塔处理，对每层图像提取ORB特征点，根据特征点法进行匹配相邻帧，通过RANSAC去除异常点，最后将跟踪到的特征点存储到图像队列中，并通知后端进行处理；将IMU数据进行积分，得到当前时刻的位置、速度和旋转，同时计算在后端优化中将用到的相邻帧的预积分增量，及预积分误差的Jacobian矩阵和协方差项；2)初始化，利用SFM进行纯视觉估计滑窗内所有帧的位姿和3D点的位置，最后与IMU预积分结果对其进行求解初始化参数；3)后端滑窗优化，将视觉约束、IMU约束、和闭环约束放在一个大的目标函数进行非线性优化，求解滑窗内所有帧的速度、位置、姿态、偏差；4)闭环检测和优化，利用DBoW3进行闭环检测，当检测成功后进行重定位，最后对整个相机轨迹进行闭环优化。

2.根据权利要求1所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：视觉初始化的目的是利用最开始一段时间内的关键帧图像序列进行位姿解算和三角化，并进一步进行全局优化；具体方法是：利用基于视觉的SFM技术得到关键帧序列较为精确的位姿和图像点坐标，为IMU的初始化提供较为精确的位姿参数；为了使得视觉初始化不依赖于场景，即无论初始化场景是平面还是非平面都能获得较为精确的初始值，本系统初始的两图像关键帧采用并行计算基础矩阵和单应性矩阵的方法，根据一定的机制选择合适的模型；根据初始的两关键帧固定场景尺度和初始化三角点，然后使用PnP算法恢复运动，并不断进行三角化，恢复地图点坐标；跟踪一段序列后，构建图像的重投影误差进行BA全局优化，得到优化后的地图点和位姿。

3.根据权利要求2所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：IMU初始化过程分解成四个小问题进行求解：(1)陀螺仪偏差的估计；(2)尺度和重力加速度的估计；(3)加速度偏差的估计以及尺度和重力的优化；(4)速度的估计。

4.根据权利要求3所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：对单目VINS系统进行了初始化的操作，为后续的非线性提供较好的初始值，其中包括：后端滑窗非线性优化、边缘化的策略、闭环检测与优化、全局位姿优化。

5.根据权利要求4所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：边缘化的策略，VINS-mono根据次新帧是否为关键帧，分为两种边缘化策略：A.当次新帧为关键帧时，边缘化掉最老帧，及其看到的路标点和相关联的IMU数据，将其转化为先验信息加到整体的目标函数中；B.当次新帧不是关键帧时，直接扔掉次新帧及它的视觉观测边，而不对次新帧进行边缘化，但保留次新帧的IMU数据，从而保证IMU预积分的连贯性。

6.根据权利要求5所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：闭环检测与优化，是对新来的关键帧,再重新提取300个ORB特征点进行闭环检测使用，其目标函数可写为：

7.根据权利要求6所述的改进的非线性优化单目惯导SLAM的方法，其特征在于：全局位姿优化，数据库中的帧对于闭环优化会提供两种边：a.序列边：是指通过VIO计算的两帧之间的相对位姿；b.闭环边：是指检测到闭环的两帧；当从滑窗内滑出的帧检测为闭环帧时，则对数据库的帧进行相应闭环优化。