CN112240768A - 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 - Google Patents

Abstract

本发明属于无人驾驶技术领域，公开了一种基于Runge‑Kutta4改进预积分的视觉惯导融合SLAM方法，用于解决现有视觉ORB‑SLAM2方法在快速运动、环境特征稀疏等场合存在的定位精度低、鲁棒性差等问题的技术问题；实现步骤为：输入双目图像对信息；输入IMU信息；对双目图像进行预处理；利用Runge‑Kutta4算法对IMU进行预积分；系统初始化；联合状态估计；滑动窗口局部优化；回环检测与全局位姿图优化。本发明可以在不同难度等级的场景中有效地进行定位估计和地图创建，与原视觉ORB‑SLAM2方法相比，本发明的方法具有更高的定位精度，可用于无人系统导航、虚拟现实等技术领域。

Description

基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法

技术领域

本发明属于无人驾驶技术领域，尤其涉及一种基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法。

背景技术

目前：具有自主导航功能的无人机和移动机器人等无人系统逐渐被应用于各个领域。在众多的关键技术中，同时定位与地图创建(Simultaneous Localization AndMapping,SLAM)是实现无人系统自主导航功能的基础和核心。视觉SLAM依靠相机捕获的图像序列估计无人系统的位姿并创建环境地图，但在无人系统运动过快或环境特征缺失的情况下，其精度和鲁棒性会急剧下降，有时甚至导致定位失败。随着硬件设计和制造的不断进步，低成本的轻型微电子机械IMU已经无处不在。IMU和相机在性能上有很强的互补性，一方面IMU测量不依赖环境特征并有很高的测量频率，在系统快速运动导致图像模糊时仍可提供较好的位姿估计；另一方面，视觉SLAM可以对长时间下IMU测量积分产生的发散进行约束。所以，视觉惯性导航系统VINS(Visual Inertial Navigation System)逐渐成为当下SLAM领域的研究热点。

VINS的主要目标是最佳地融合IMU测量值和图像信息。目前，按照融合方案，VINS可以分为松耦合和紧耦合两类。松耦合方式分别利用相机和IMU的信息估计系统运动，然后将其估计结果进行融合，而紧耦合方式将相机和IMU的状态信息建立统一的目标函数，然后进行系统运动估计。按照后端数据处理方法，VINS又可以分为滤波和优化两类。基于滤波的方法一般采用EKF框架，分为预测和更新两个过程，预测阶段使用IMU信息预测系统的状态，然后在更新阶段根据图像对系统状态进行修正和优化。而基于优化的方法考虑过去一段时间的数据，然后通过优化方法对系统状态量进行反复迭代优化。其中，基于滤波的方法只考虑当前时刻的数据，在更新状态时也仅利用了图像数据，故精度有限，基于优化的方法精度更高，但计算量大。

虽然现有视觉与惯性信息融合SLAM系统层出不穷，然而视觉与IMU信息仍无法得到有效的融合。如何解决相机与IMU频率不一致问题，如何充分地融合视觉与IMU信息仍然是VINS亟需解决的问题。

通过上述分析，现有技术存在的问题及缺陷为：

(1)视觉与IMU信息仍无法得到有效的融合。如何解决相机与IMU频率不一致问题，如何充分地融合视觉与IMU信息仍然是VINS亟需解决的问题。

(2)在环境信息缺失以及载体快速运动等极端情况下定位精度与鲁棒性差。

解决以上问题及缺陷的难度为：在保证方法运行速度较快的条件下，提高方法运算结果的准确性。让使用者既不需要等待太久时间，又可以解决系统在环境信息缺失、光照变化、快速运动等极端环境下性能不佳的问题。即在保证时间效率的前提下，提高系统定位的精度与建立环境地图的鲁棒性。

解决以上问题及缺陷的意义为：提高方法的运行速度和稳定性，增强方法的求解能力。使得视觉惯导融合SLAM系统性能得到提高，让使用者在环境信息缺失以及载体快速运动等极端情况下解决系统的定位精度差等问题，在需求相同的情况下，创建高精度的环境地图。

发明内容

针对现有技术存在的问题，本发明提供了一种基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法。

本发明是这样实现的，一种视觉惯导融合SLAM方法，所述视觉惯导融合SLAM方法包括：

输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；

设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

进一步，所述图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对包括：

(1)随着双目图像对的输入，通过灰度质心法和图像金字塔在FAST关键点和BRIEF特征描述子的基础上增加方向和尺度描述，提取双目图像对的ORB特征点；

(2)在左、右图像特征匹配时，对于左帧图像上的某一特征点，利用图像对行对齐这一先验信息，根据计算它和右帧图像中同行特征点间的汉明距离，将距离最小的的点作为最佳匹配点；

(3)在相邻帧图像特征点匹配时，把当前帧时刻的位姿T_k作为先验信息，其通过相机匀速模型估计：

T_k＝T_k-1k-2T_k-1；

其中，T_k-1为k-1时刻的相机位姿，T_k-1k-2为相机在k-1时刻到k-2时刻的位姿变换矩阵，T_k为当前帧的相机位姿；然后利用相机投影模型将前一帧图像对应的地图点投影到当前帧图像中，以投影点为中心，设置半径为r的圆形匹配区域，在圆形区域内寻找对应的最佳匹配点；

(4)利用RANSAC算法对前面匹配到的特征点进行筛选，剔除匹配错误的点对。

进一步，所述利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束包括：

(1)计算Runge-Kutta4算法在相邻两关键帧间的四个导数值，其表达式如下：

其中，k₁表示区间起始点(t_n,y_n)处的导数。

是采用k₁作为平均导数通过欧拉积分法估计的区间中点坐标，k₂表示其导数。

是采用k₂作为平均导数通过欧拉积分法估计的区间中点坐标，k₃表示其导数，(t_n+Δt,y_n+Δt·k₃)是采用k₃作为平均导数通过欧拉积分法估计的区间终点坐标，k₄表示其导数；

(2)计算Runge-Kutta4算法在相邻两关键帧间的平均导数，其表达式如下：

(3)结合IMU测量方程和运动方程，利用算出的平均导数对相邻两关键帧之间的IMU信息进行预积分，其表达式为：

其中，下标B表示载体坐标系，下标W表示世界坐标系，Δt表示两关键帧时刻间的时间差，_Bω_WBRK和_Ba_RK分别为根据Runge-Kutta4算法导数计算公式计算出的从k时刻到k+1时刻系统角速度和加速度的加权平均值，R_WB表示两坐标系间的旋转矩阵，_Wp_B、_Wv_B和_Wa_B分别表示世界坐标系下载体的位置、速度和加速度，_Wg代表世界坐标系下的重力加速度。

进一步，所述执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度包括：

(1)视觉初始化，根据匹配好的左右图像特征点，计算特征点的视差与深度值，然后，根据双目相机模型恢复特征点在相机坐标系下对应的空间坐标获得由空间点描述的初始地图；根据匹配好的前后帧图像特征点，利用BA集束调整法估计系统状态量的初始值；

(2)惯导初始化，根据视觉初始化获得的系统状态量的初始值，初始化陀螺仪和加速度计的零偏以及IMU的速度。

进一步，所述视觉惯导融合SLAM方法的构建视觉误差项包括：对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；

其中，ρ为核函数，Σ_k为重投影误差的协方差矩阵，

为双目相机得到的第k个路标点的投影坐标，

为该路标点在相机坐标系下的坐标，π为投影方程，满足：

其中，f_x、f_y、c_u和c_v为相机内参，b为基线；

构建IMU误差项，将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；

其中，Σ_IMU为预积分测量协方差矩阵，Σ_b为零偏随机游走协方差矩阵，e_IMU是IMU预积分测量残差，e_b是IMU零偏随机游走残差，有：

其中，

联合状态估计，结合上一关键帧的状态量和计算的IMU预积分数据，预测当前帧系统各个状态量，并将预测值作为联合状态估计的初始值；然后根据视觉误差项与IMU误差项构建目标函数：

其中，E_proj(j)表示视觉误差项，E_IMU(i,j)表示IMU误差项，Φ表示系统状态量，最后，利用高斯牛顿法求解(5c2)中的目标函数，得到系统的最佳状态量。

进一步，所述视觉惯导融合SLAM方法的滑动窗口局部优化包括：

(1)选取最近的10个关键帧作为滑动窗口的优化范围；

(2)计算滑动窗口内每个关键帧的联合误差项：

其中，N＝10表示滑动窗口的长度，χ表示需要优化的变量，包括系统在各个关键帧时刻的位姿、速度、IMU零偏以及这些关键帧观测到的地图点位置；

(3)利用高斯牛顿法求解目标函数，得到优化后的系统的状态量与地图点位置。

本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；

设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

本发明的另一目的在于提供一种实施所述视觉惯导融合SLAM方法的视觉惯导融合SLAM系统，所述视觉惯导融合SLAM系统包括：

双目图像对信息输入模块，用于输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

IMU信息输入模块，用于输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

双目图像预处理模块，用于对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

IMU预积分模块，用于利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

系统初始化模块，用于执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

联合状态估计模块，用于构建视觉误差项、构建IMU误差项，实现联合状态估计；

滑动窗口局部优化模块，用于设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

回环检测与全局位姿图优化模块，用于对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

本发明的另一目的在于提供一种无人系统导航系统，所述无人系统导航系统搭载所述的视觉惯导融合SLAM系统。

本发明的另一目的在于提供一种虚拟现实控制系统，所述虚拟现实控制系统搭载所述的视觉惯导融合SLAM系统。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明具体涉及一种视觉惯导融合SLAM方法，可用于无人系统导航、虚拟现实等要求定位和建图的场合。

本发明在视觉惯导融合之前，引入四阶龙格库塔算法改进IMU预积分过程，以Runge-Kutta4算法离散IMU运动方程，迭代计算下一时刻的IMU预积分值。相比于传统的欧拉积分法，Runge-Kutta4算法在积分区间内预估多个点的导数，然后利用加权平均得到的平均导数更新结果。如此可以避免原欧拉积分法因一阶近似带来的大量误差，为后续视觉和惯性信息的融合提供精度保证。

本发明在视觉ORB-SLAM2方法的基础上，由前述IMU预积分过程提供两关键帧之间的IMU约束，然后通过最小化视觉重投影误差和IMU预积分误差之和估计无人系统的状态，最后在滑动窗口内对系统状态和地图点位置同时进行优化。与原纯视觉SLAM方法以及现有的视觉与惯性信息融合SLAM方法相比，本发明方法具有更高的定位精度。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的视觉惯导融合SLAM方法流程图。

图2是本发明实施例提供的视觉惯导融合SLAM系统的结构示意图；

图2中：1、双目图像对信息输入模块；2、IMU信息输入模块；3、双目图像预处理模块；4、IMU预积分模块；5、系统初始化模块；6、联合状态估计模块；7、滑动窗口局部优化模块；8、回环检测与全局位姿图优化模块。

图3是本发明实施例提供的视觉惯导融合SLAM方法实现流程图。

图4是本发明实施例提供的在简单环境MH_02_easy序列下的精度对比示意图。

图5是本发明实施例提供的在中等环境V2_02_medium序列下的精度对比示意图。

图6是本发明实施例提供的在困难环境V1_03_difficult序列下的精度对比示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法，该方法通过IMU预积分提供两关键帧之间的IMU约束，然后通过最小化视觉重投影误差和IMU预积分误差之和估计系统状态，最后在滑动窗口内对系统状态和地图点位置进行优化。为了提高预积分阶段的精度，本发明创新性的提出利用四阶龙格库塔算法代替欧拉积分迭代计算下一时刻的预积分值，从而改进预积分过程。同时，对本发明所提到的SLAM方法进行数据集实验，搭建系统的软、硬件平台，从定位精度和鲁棒性两方面对本发明所提方法和现有SLAM方法进行实验对比分析，验证本发明所提方法的有效性。下面结合附图对本发明作详细的描述。

如图1所示，本发明提供的视觉惯导融合SLAM方法包括以下步骤：

S101：输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

S102：输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

S103：对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

S104：利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

S105：执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

S106：对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；

S107：设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

S108：对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

本发明提供的视觉惯导融合SLAM方法业内的普通技术人员还可以采用其他的步骤实施，图1的本发明提供的视觉惯导融合SLAM方法仅仅是一个具体实施例而已。

如图2所示，本发明提供的视觉惯导融合SLAM系统包括：

双目图像对信息输入模块1，用于输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

IMU信息输入模块2，用于输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

双目图像预处理模块3，用于对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

IMU预积分模块4，用于利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

系统初始化模块5，用于执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

联合状态估计模块6，用于构建视觉误差项、构建IMU误差项，实现联合状态估计；

滑动窗口局部优化模块7，用于设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

回环检测与全局位姿图优化模块8，用于对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

下面结合附图对本发明的技术方案作进一步的描述。

如图3所示，本发明提供的视觉惯导融合SLAM方法具体包括以下步骤：

步骤1)输入双目图像对信息：

输入由双目相机采集的当前帧双目图像对，包括左图像和右图像，在本实施例中，双目图像对采用苏黎世联邦理工大学的EuRoC数据集，该双目图像对由帧率为20Hz的MT9V034摄像机提供；

步骤2)输入IMU信息：

输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息，在本实施例中，IMU信息同样采用苏黎世联邦理工大学的EuRoC数据集，由200Hz的ADIS16448传感器提供；

步骤3)对双目图像进行预处理：

步骤3a)输入双目图像对，通过灰度质心法和图像金字塔在FAST关键点和BRIEF特征描述子的基础上增加方向和尺度描述，从而提取双目图像对的ORB特征点：

若某像素点与周围邻域内足够多的像素点亮度值差异较大，则这个像素点会被检测为一个FAST关键点。对图像上某一像素点p，围绕以其为圆心，半径为3个像素的圆上共有16个像素点，将p与圆上的每个像素点进行亮度值比较，若有连续N个的亮度差值大于设定的阈值ΔI，则认为p为FAST关键点。由于FAST关键点仅对像素点的亮度值进行比较，所以提取速度非常快。但其缺陷在于不具有方向和尺度信息，在图像旋转或发生尺度变化时，无法正确被检测，因此ORB在FAST关键点基础上增加了方向和尺度的描述。

ORB特征的尺度不变性由创建图像金字塔，并在金字塔的每一层上检测关键点来实现。图像金字塔由多层不同分辨率的图像组成，可以在多个尺度上表达图像。图像金字塔生成过程中，首先以原始图像作为Level-0层，然后在此基础上以一定的采样率进行降采样，生成Level-1层的图像，如此重复，直到某层的分辨率达到限定的最低分辨率。

ORB特征的方向不变性由灰度质心法实现，灰度质心法的基本思想是：以关键点为中心定义一个图像块，假设关键点和图像块的质心之间存在一个偏移量，然后将此偏移量的方向定义为关键点的方向。其具体实施过程如下：

(1)设某一关键点O，在关键点的邻域附近定义一个图像块pitch，并定义图像块pitch的零阶矩和一阶矩为：

其中，I(x,y)为图像块pitch中坐标(x,y)处的亮度值。

(2)通过图像块pitch的零阶矩和一阶矩，得到图像块的质心C：

(3)连接关键点O和质心C，定义偏移向量

则关键点的方向角定义为：

θ＝arctan(m₀₁/m₁₀) (3)

在提取完FAST关键点后，需要选择合适的描述子以表达关键点特征。ORB特征采用BRIEF二进制描述子，其本质是一个由0和1编码的N维向量，生成BRIEF描述子的具体过程如下：

(1)在关键点附近以高斯分布随机选取N个像素点对；

(2)比较各像素点对的灰度值大小，对于某像素点对(p_i,q_j)，如果p_i＞q_j，则结果记为1，否则记为0；

(3)重复步骤(2)，将得到的N个结果串起来，写成一个N维向量的形式。

原始的BRIEF描述子同样不具备旋转不变性，因为对于同一组点对，规定不同的起始位置，会产生不同的二进制组合。此时，需要利用前面计算出的方向角θ旋转关键点附近的N个点对，然后再提取描述子。具体过程如下：

(1)将FAST关键点选取的N个点对坐标表示成如下矩阵形式：

(2)利用前面FAST关键点的方向角θ，对N个点对进行旋转可得：

其中，R_θ表示方向角θ的旋转矩阵，S_θ表示旋转后N个点对的位置。

(3)利用旋转后的点对生成BRIEF描述子，此时的BRIEF描述子包含了方向信息。

步骤3b)在左、右图像特征匹配时，将相似度作为特征匹配的标准。ORB特征采用的是二进制BRIEF描述子，可用汉明距离来表征特征点之间的相似度。设左帧图像上的某一ORB特征的描述子为X，右帧图像上特征的描述子为Y。利用图像对行对齐这一先验信息，根据下式计算左右帧图像中同行特征点间的汉明距离，将距离最小的的点作为最佳匹配点；

步骤3c)在相邻帧图像特征点匹配时，把当前帧时刻的位姿T_k作为先验信息，然后将前一帧图像对应的地图点反投影到当前帧图像中进行搜索。虽然此时还不能确定当前帧相机位姿T_k的准确值，但因为SLAM系统中相机运动是平滑且连续的，在连续三帧的短暂时间内相机位姿变化幅度不大，此时可以用相机匀速模型估计当前帧时刻的相机位姿：

T_k＝T_k-1k-2T_k-1 (7)

其中，T_k-1为k-1时刻的相机位姿，T_k-1k-2为相机在k-1时刻到k-2时刻的位姿变换矩阵，T_k为当前帧的相机位姿；再利用相机投影模型将前一帧图像对应的地图点投影到当前帧图像中，以投影点为中心，设置半径为r的圆形匹配区域，在圆形区域内寻找对应的最佳匹配点；

步骤3d)利用RANSAC算法对前面匹配到的特征点进行筛选，剔除匹配错误的点对；

步骤4)利用Runge-Kutta4算法对IMU进行预积分：

步骤4a)计算Runge-Kutta4算法在相邻两关键帧间的四个导数值，其表达式如下：

其中，k₁表示区间起始点(t_n,y_n)处的导数。

是采用k₁作为平均导数通过欧拉积分法估计的区间中点坐标，k₂表示其导数。

是采用k₂作为平均导数通过欧拉积分法估计的区间中点坐标，k₃表示其导数。(t_n+Δt,y_n+Δt·k₃)是采用k₃作为平均导数通过欧拉积分法估计的区间终点坐标，k₄表示其导数。四阶龙格库塔算法综合了四个点的导数信息，且在两个中点处的导数具有更高的权重。

步骤4b)计算Runge-Kutta4算法在相邻两关键帧间的平均导数，其表达式如下：

步骤4c)结合IMU测量方程和运动方程，利用算出的平均导数对相邻两关键帧之间的IMU信息进行预积分，其表达式为：

其中，下标B表示载体坐标系，下标W表示世界坐标系，Δt表示两关键帧时刻间的时间差，_Bω_WBRK和_Ba_RK分别为根据Runge-Kutta4算法导数计算公式计算出的从k时刻到k+1时刻系统角速度和加速度的加权平均值，R_WB表示两坐标系间的旋转矩阵，_Wp_B、_Wv_B和_Wa_B分别表示世界坐标系下载体的位置、速度和加速度，_Wg代表世界坐标系下的重力加速度。相比于欧拉积分法，四阶龙格库塔法的积分值基本与微分方程的积分真值相同，积分误差也远远小于欧拉积分产生的误差。故四阶龙格库塔算法是一种更高阶的数值积分方法，具有更高的计算精度；

步骤5)系统初始化：

步骤5a)视觉初始化：

根据步骤4匹配好的左右图像特征点，计算特征点的视差与深度值，然后根据双目相机模型恢复特征点在相机坐标系下对应的空间坐标，获得由空间点描述的初始地图；根据步骤4匹配好的前后帧图像特征点，利用BA集束调整法估计系统状态量的初始值；

步骤5b)惯导初始化：

根据视觉初始化获得的系统状态量的初始值，初始化陀螺仪和加速度计的零偏以及IMU的速度。

假定陀螺仪零偏在连续两关键帧之间保持恒定，通过最小化陀螺仪预积分测量值与视觉估计的相对旋转之差，计算陀螺仪零偏，有：

其中，N为初始化需要的关键帧数量，ΔR_ii+1为相邻关键帧之间关于旋转的预积分测量值，

为由视觉估计的旋转矩阵，式(11)是一个最小二乘问题，通过高斯牛顿法可以求出陀螺仪的零偏。

计算出陀螺仪零偏后，为了消除陀螺仪零偏更新对预积分的影响，需要对IMU数据重新计算预积分。一般先通过视觉SLAM的位姿估计重力加速度，然后估计加速度计零偏。双目视觉SLAM中，可以获得物体的绝对尺度，且尺度因子为1。载体坐标系与相机坐标系在世界坐标系下的转换关系满足

_Wp_B＝_Wp_C+R_{WC C}p_B (12)

其中，_Wp_B和_Wp_C分别表示IMU和相机在世界坐标系下的位置，R_WC表示相机坐标系相对于世界坐标系的旋转矩阵，_Cp_B为通过标定得到的相机外参，表示IMU在相机坐标系下的位置。

联立式(10)与式(12)，有：

用式(13)描述三个连续关键帧，并利用式(10)中的速度等式消去关键帧的速度项，可得方程：

β(i)_Wg＝γ(i) (14)

其中，

其中，R_WB表示载体坐标系相对于世界坐标系的旋转矩阵，Δt表示两关键帧间的时间差，Δv₁₂为两关键帧间的相机速度。利用SVD求解式(14)，可得重力加速度的粗略估计_Wg^*。

利用式(14)求解的重力加速度没有考虑加速度零偏，为了提高可观性，下面引入重力加速度大小G≈9.8的约束。假设有一惯性坐标系I，其与世界坐标系共原点，二者之间的旋转矩阵为R_WI。重力加速度在坐标系I中的方向为

而通过上面计算得到的世界坐标系下重力加速度方向为

因此可用这两个方向向量之间的夹角计算R_WI。

此时，重力加速度_Wg可表示为：

引入加速度零偏后R_WI会有所变化，用一个扰动δθ描述该变化，并进行一阶近似，有：

其中，

联立式(13)和式(17)，同时考虑加速度计零偏对Δp_ij的改变，有：

同样地，考虑连续三个关键帧之间的约束，消掉速度项，可得方程：

其中，

通过SVD求解式(19)，可以得到加速度计的零偏b_a以及优化后的重力加速度。

根据计算的陀螺仪与加速度计零偏，结合式(10)，进一步可获得各关键帧时刻的系统速度。最后关键帧时刻系统速度为：

其它关键帧时刻系统速度为：

步骤6)联合状态估计：

步骤6a)构建视觉误差项

对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；

其中，ρ为核函数，Σ_k为重投影误差的协方差矩阵，

为双目相机得到的第k个路标点的投影坐标，

为该路标点在相机坐标系下的坐标，π为投影方程，满足：

其中，f_x、f_y、c_u和c_v为相机内参，b为基线。

步骤6b)构建IMU误差项

将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；

其中，Σ_IMU为预积分测量协方差矩阵，Σ_b为零偏随机游走协方差矩阵，e_IMU是IMU预积分测量残差，e_b是IMU零偏随机游走残差，有：

其中，

步骤6c)联合状态估计：

结合上一关键帧的状态量和(4c)中计算的IMU预积分数据，预测当前帧系统各个状态量，并将预测值作为联合状态估计的初始值；然后根据视觉误差项与IMU误差项构建目标函数：

其中，E_proj(j)表示视觉误差项，E_IMU(i,j)表示IMU误差项，Φ＝{R_i,p_i,v_i,b_gi,b_ai}表示系统状态量。最后，利用高斯牛顿法求解(5c2)中的目标函数，得到系统的最佳状态量。

步骤7)滑动窗口局部优化：

步骤7a)选取最近的10个关键帧作为滑动窗口的优化范围；

通过联合状态估计模块，估计出最新关键帧的状态信息，并把其观测到的路标点加入到局部地图中，成为地图点。接下来需要对局部地图中的系统状态和地图点位置进行局部优化。在纯视觉SLAM中，局部地图的定义是和最新关键帧有共视关系的所有关键帧、以及这些关键帧所能观测到的所有地图点。在优化过程中，与局部地图有共视关系的其他关键帧设置为固定，只对局部地图内部的系统位姿和地图点位置进行调整。

融合IMU信息后，由于IMU提供相邻两关键帧之间的约束，故局部地图中包含的关键帧必须是连续的，才可以用于优化。同时，因为IMU频率较高，融合IMU信息之后的计算规模会快速增加，为了平衡精度和计算效率，本发明在局部地图中采用滑动窗口策略进行局部优化。滑动窗口选择最近一段时间内连续的N个关键帧，在优化过程中只对滑动窗口内的关键帧和这些关键帧观测到的地图点进行优化，最近第N+1个关键帧以及其余和局部地图中关键帧有共视关系的关键帧则是固定的。为了保证局部优化的可靠性，本文局部地图中滑动窗口长度N定为10，该方法保证了在局部优化时，IMU预积分能够提供有效的约束。

步骤7b)计算滑动窗口内每个关键帧的联合误差项：

其中，N＝10表示滑动窗口的长度，

表示需要优化的变量，包括系统在各个关键帧时刻的位姿、速度、IMU零偏以及这些关键帧观测到的地图点位置；

步骤7c)利用高斯牛顿法求解(7b)中的目标函数，在每次迭代更新时重新确定滑窗内的系统状态，得到优化后的系统的状态量与地图点位置。

步骤8)回环检测与全局位姿图优化

检测场景中的图像信息，并判断当前位置是否为曾经到达过的地点，若发现回环则通过位姿图优化对回环内的系统状态进行重新优化，以消除累积误差，获得全局一致性的位姿和地图。

下面结合仿真对本发明的技术效果作详细的描述。

1、仿真条件：

仿真是在主频4.3GHZ的Intel Corei7-8700、内存16GB的硬件环境和Ubuntu16.0.4的软件环境下进行的。采用的数据集是苏黎世联邦理工大学的EuRoC数据集。

2、仿真内容及结果分析：

仿真一，对本发明中视觉惯导融合SLAM方法与原始视觉ORB-SLAM2方法以及未使用Runge-Kutta4的视觉惯导融合SLAM方法在简单环境MH_02_easy序列下进行仿真对比。

仿真二，对本发明中视觉惯导融合SLAM方法与原始视觉ORB-SLAM2方法以及未使用Runge-Kutta4的视觉惯导融合SLAM方法在中等环境V2_02_medium序列下进行仿真对比。

仿真三，对本发明中视觉惯导融合SLAM方法与原始视觉ORB-SLAM2方法以及未使用Runge-Kutta4的视觉惯导融合SLAM方法在困难环境V1_03_difficult序列下进行仿真对比。

如图4-图6所示，图4、5和6分别表示上述系统在三个序列下APE值的各项统计学指标对比图。

由图4-图6可以看出：

1.无论是环境状况良好的MH02序列，还是环境状况较为恶劣的V103序列，本发明中视觉惯导融合SLAM方法都有较好地定位精度。

2.在绝大部分时刻下，本发明融合IMU信息后的VI-SLAM系统的APE值要小于纯视觉ORB-SLAM2系统的APE值，并且平均误差和均方根误差等各项指标也都有所降低，说明VI-SLAM系统有更高的精度。尤其在MH03序列这种运动速度过快、采集图像模糊的情况下，通过两关键帧之间的IMU信息约束，可以有效减小图像质量不佳导致的误差，提高系统定位的精度。

3.与VI-SLAM系统相比，利用四阶龙格库塔算法改进后的VI-SLAM_RK4系统可以消除预积分过程中一阶近似产生的误差，故在各时刻有更低的APE值，且均方根误差和平均误差等APE指标也均有明显的降低，说明VI-SLAM_RK4系统进一步提高了系统定位的精度。

4.三个序列分别代表三种不同难度等级的运行环境，在任意一种等级环境下，从ORB-SLAM2到VI-SLAM再到VI-SLAM_RK4，系统误差波动范围逐渐缩小，且定位精度不断提高。

应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种视觉惯导融合SLAM方法，其特征在于，所述视觉惯导融合SLAM方法包括：

输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；

设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

2.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对包括：

(1)随着双目图像对的输入，通过灰度质心法和图像金字塔在FAST关键点和BRIEF特征描述子的基础上增加方向和尺度描述，提取双目图像对的ORB特征点；

(2)在左、右图像特征匹配时，对于左帧图像上的某一特征点，利用图像对行对齐这一先验信息，计算它和右帧图像中同行特征点间的汉明距离，将距离最小的的点作为最佳匹配点；

(3)在相邻帧图像特征点匹配时，把当前帧时刻的位姿T_k作为先验信息，其通过相机匀速模型估计：

T_k＝T_k-1k-2T_k-1；

其中，T_k-1为k-1时刻的相机位姿，T_k-1k-2为相机在k-1时刻到k-2时刻的位姿变换矩阵，T_k为当前帧的相机位姿；然后利用相机投影模型将前一帧图像对应的地图点投影到当前帧图像中，以投影点为中心，设置半径为r的圆形匹配区域，在圆形区域内寻找对应的最佳匹配点；

(4)利用RANSAC算法对前面匹配到的特征点进行筛选，剔除匹配错误的点对。

3.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束包括：

(1)计算Runge-Kutta4算法在相邻两关键帧间的四个导数值，其表达式如下：

其中，k₁表示区间起始点(t_n,y_n)处的导数，

是采用k₁作为平均导数通过欧拉积分法估计的区间中点坐标，k₂表示其导数，

是采用k₂作为平均导数通过欧拉积分法估计的区间中点坐标，k₃表示其导数，(t_n+Δt,y_n+Δt·k₃)是采用k₃作为平均导数通过欧拉积分法估计的区间终点坐标，k₄表示其导数；

(2)计算Runge-Kutta4算法在相邻两关键帧间的平均导数，其表达式如下：

(3)结合IMU测量方程和运动方程，利用算出的平均导数对相邻两关键帧之间的IMU信息进行预积分，其表达式为：

其中，下标B表示载体坐标系，下标W表示世界坐标系，Δt表示两关键帧时刻间的时间差，_Bω_WBRK和_Ba_RK分别为根据Runge-Kutta4算法导数计算公式计算出的从k时刻到k+1时刻系统角速度和加速度的加权平均值，R_WB表示两坐标系间的旋转矩阵，_Wp_B、_Wv_B和_Wa_B分别表示世界坐标系下载体的位置、速度和加速度，_Wg代表世界坐标系下的重力加速度。

4.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度包括：

(1)视觉初始化，根据匹配好的左右图像特征点，计算特征点的视差与深度值，然后，根据双目相机模型恢复特征点在相机坐标系下对应的空间坐标获得由空间点描述的初始地图；根据匹配好的前后帧图像特征点，利用BA集束调整法估计系统状态量的初始值；

(2)惯导初始化，根据视觉初始化获得的系统状态量的初始值，初始化陀螺仪和加速度计的零偏以及IMU的速度。

5.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述视觉惯导融合SLAM方法的构建视觉误差项包括：对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；

其中，ρ为核函数，Σ_k为重投影误差的协方差矩阵，

为双目相机得到的第k个路标点的投影坐标，

为该路标点在相机坐标系下的坐标，π为投影方程，满足：

其中，f_x、f_y、c_u和c_v为相机内参，b为基线；

构建IMU误差项，将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；

其中，Σ_IMU为预积分测量协方差矩阵，Σ_b为零偏随机游走协方差矩阵，e_IMU是IMU预积分测量残差，e_b是IMU零偏随机游走残差，有：

其中，

联合状态估计，结合上一关键帧的状态量和计算的IMU预积分数据，预测当前帧系统各个状态量，并将预测值作为联合状态估计的初始值；然后根据视觉误差项与IMU误差项构建目标函数：

其中，E_proj(j)表示视觉误差项，E_IMU(i,j)表示IMU误差项，Φ表示系统状态量，最后，利用高斯牛顿法求解目标函数，得到系统的最佳状态量。

6.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述视觉惯导融合SLAM方法的滑动窗口局部优化包括：

(1)选取最近的10个关键帧作为滑动窗口的优化范围；

(2)计算滑动窗口内每个关键帧的联合误差项：

其中，N＝10表示滑动窗口的长度，χ表示需要优化的变量，包括系统在各个关键帧时刻的位姿、速度、IMU零偏以及这些关键帧观测到的地图点位置；

(3)利用高斯牛顿法求解目标函数，得到优化后的系统的状态量与地图点位置。

7.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；

设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

8.一种实施权利要求1～6任意一项所述视觉惯导融合SLAM方法的视觉惯导融合SLAM系统，其特征在于，所述视觉惯导融合SLAM系统包括：

双目图像对信息输入模块，用于输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；

IMU信息输入模块，用于输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；

双目图像预处理模块，用于对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；

IMU预积分模块，用于利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；

系统初始化模块，用于执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；

联合状态估计模块，用于构建视觉误差项、构建IMU误差项，实现联合状态估计；

滑动窗口局部优化模块，用于设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；

回环检测与全局位姿图优化模块，用于对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。

9.一种无人系统导航系统，其特征在于，所述无人系统导航系统搭载权利要求8所述的视觉惯导融合SLAM系统。

10.一种虚拟现实控制系统，其特征在于，所述虚拟现实控制系统搭载权利要求8所述的视觉惯导融合SLAM系统。

Images