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

本发明专利技术属于无人驾驶技术领域，公开了一种基于Runge

【技术实现步骤摘要】
基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法


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

技术介绍

[0002]目前：具有自主导航功能的无人机和移动机器人等无人系统逐渐被应用于各个领域。在众多的关键技术中，同时定位与地图创建(Simultaneous Localization And Mapping,SLAM)是实现无人系统自主导航功能的基础和核心。视觉SLAM依靠相机捕获的图像序列估计无人系统的位姿并创建环境地图，但在无人系统运动过快或环境特征缺失的情况下，其精度和鲁棒性会急剧下降，有时甚至导致定位失败。随着硬件设计和制造的不断进步，低成本的轻型微电子机械IMU已经无处不在。IMU和相机在性能上有很强的互补性，一方面IMU测量不依赖环境特征并有很高的测量频率，在系统快速运动导致图像模糊时仍可提供较好的位姿估计；另一方面，视觉SLAM可以对长时间下IMU测量积分产生的发散进行约束。所以，视觉惯性导航系统VINS(Visual Inertial Navigation System)逐渐成为当下SLAM领域的研究热点。
[0003]VINS的主要目标是最佳地融合IMU测量值和图像信息。目前，按照融合方案，VINS可以分为松耦合和紧耦合两类。松耦合方式分别利用相机和IMU的信息估计系统运动，然后将其估计结果进行融合，而紧耦合方式将相机和IMU的状态信息建立统一的目标函数，然后进行系统运动估计。按照后端数据处理方法，VINS又可以分为滤波和优化两类。基于滤波的方法一般采用EKF框架，分为预测和更新两个过程，预测阶段使用IMU信息预测系统的状态，然后在更新阶段根据图像对系统状态进行修正和优化。而基于优化的方法考虑过去一段时间的数据，然后通过优化方法对系统状态量进行反复迭代优化。其中，基于滤波的方法只考虑当前时刻的数据，在更新状态时也仅利用了图像数据，故精度有限，基于优化的方法精度更高，但计算量大。
[0004]虽然现有视觉与惯性信息融合SLAM系统层出不穷，然而视觉与IMU信息仍无法得到有效的融合。如何解决相机与IMU频率不一致问题，如何充分地融合视觉与IMU信息仍然是VINS亟需解决的问题。
[0005]通过上述分析，现有技术存在的问题及缺陷为：
[0006](1)视觉与IMU信息仍无法得到有效的融合。如何解决相机与IMU频率不一致问题，如何充分地融合视觉与IMU信息仍然是VINS亟需解决的问题。
[0007](2)在环境信息缺失以及载体快速运动等极端情况下定位精度与鲁棒性差。
[0008]解决以上问题及缺陷的难度为：在保证方法运行速度较快的条件下，提高方法运算结果的准确性。让使用者既不需要等待太久时间，又可以解决系统在环境信息缺失、光照变化、快速运动等极端环境下性能不佳的问题。即在保证时间效率的前提下，提高系统定位的精度与建立环境地图的鲁棒性。
[0009]解决以上问题及缺陷的意义为：提高方法的运行速度和稳定性，增强方法的求解
能力。使得视觉惯导融合SLAM系统性能得到提高，让使用者在环境信息缺失以及载体快速运动等极端情况下解决系统的定位精度差等问题，在需求相同的情况下，创建高精度的环境地图。

技术实现思路

[0010]针对现有技术存在的问题，本专利技术提供了一种基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法。
[0011]本专利技术是这样实现的，一种视觉惯导融合SLAM方法，所述视觉惯导融合SLAM方法包括：
[0012]输入由双目相机采集的当前帧双目图像对，包括左图像和右图像；
[0013]输入分别由IMU的陀螺仪和加速度计采集的相邻两关键帧之间的角速度和加速度信息；
[0014]对图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对；
[0015]利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束；
[0016]执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度；
[0017]对左、右图像恢复出的地图点进行重投影，统计各特征点对应的投影点与匹配点的误差和，定义为视觉误差项；将相邻关键帧间的IMU预积分噪声项与IMU零偏随机游走噪声项定义为IMU误差项；将视觉误差项与IMU误差项之和作为联合误差项，构建目标函数；将系统的旋转、位置和速度，陀螺仪与加速度计的零偏作为状态量，对系统的各状态量进行估计；
[0018]设置滑动窗口，并利用高斯牛顿法对滑动窗口内的系统状态量和地图点位置进行局部优化，得到优化后系统状态量与地图点位置；
[0019]对全局地图进行回环检测，若检测到回环，则利用位姿图优化修正系统的漂移，消除累积误差。
[0020]进一步，所述图像序列提取ORB特征，利用暴力匹配法结合先验信息进行特征匹配；并采用RANSAC剔除误匹配点，获取左、右图像间正确的匹配对包括：
[0021](1)随着双目图像对的输入，通过灰度质心法和图像金字塔在FAST关键点和BRIEF特征描述子的基础上增加方向和尺度描述，提取双目图像对的ORB特征点；
[0022](2)在左、右图像特征匹配时，对于左帧图像上的某一特征点，利用图像对行对齐这一先验信息，根据计算它和右帧图像中同行特征点间的汉明距离，将距离最小的的点作为最佳匹配点；
[0023][0024](3)在相邻帧图像特征点匹配时，把当前帧时刻的位姿T
k
作为先验信息，其通过相机匀速模型估计：
[0025]T
k
＝T
k-1k-2
T
k-1
；
[0026]其中，T
k-1
为k-1时刻的相机位姿，T
k-1k-2
为相机在k-1时刻到k-2时刻的位姿变换矩阵，T
k
为当前帧的相机位姿；然后利用相机投影模型将前一帧图像对应的地图点投影到当前帧图像中，以投影点为中心，设置半径为r的圆形匹配区域，在圆形区域内寻找对应的最佳匹配点；
[0027](4)利用RANSAC算法对前面匹配到的特征点进行筛选，剔除匹配错误的点对。
[0028]进一步，所述利用Runge-Kutta4算法离散化IMU运动方程，对相邻两关键帧之间的IMU信息进行预积分，为后续的视觉惯导融合提供IMU约束包括：
[0029](1)计算Runge-Kutta4算法在相邻两关键帧间的四个导数值，其表达式如下：
[0030][0031]其中，k1表示区间起始点(t
n
,y
n
)处的导数。是采用k1作为平均导数通过欧拉积分法估计的区间中点坐标，k2表示其本文档来自技高网...

【技术保护点】

【技术特征摘要】
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-2
T
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算法在相邻两关键帧间的四个导数值，其表达式如下：其中，k1表示区间起始点(t
n
,y
n
)处的导数，是采用k1作为平均导数通过欧拉积分法估计的区间中点坐标，k2表示其导数，是采用k2作为平均导数通过欧拉积分法估计的区间中点坐标，k3表示其导数，(t
n
+Δt,y
n
+Δt
·
k3)是采用k3作为平均导数通过欧拉积分法估计的区间终点坐标，k4表示其导数；(2)计算Runge-Kutta4算法在相邻两关键帧间的平均导数，其表达式如下：(3)结合IMU测量方程和运动方程，利用算出的平均导数对相邻两关键帧之间的IMU信息进行预积分，其表达式为：其中，下标B表示载体坐标系，下标W表示世界坐标系，Δt表示两关键帧时刻间的时间差，
B
ω
WBRK
和
B
a
RK
分别为根据Runge-Kutta4算法导数计算公式计算出的从k时刻到k+1时刻系统角速度和加速度的加权平均值，R
WB
表示两坐标系间的旋转矩阵，
W
p
B
、
W
v
B
和
W
a
B
分别表示世界坐标系下载体的位置、速度和加速度，
W
g代表世界坐标系下的重力加速度。4.如权利要求1所述的视觉惯导融合SLAM方法，其特征在于，所述执行视觉的初始化，根据第一帧图像信息估计系统的初始状态，然后根据视觉部分的估计结果初始化陀螺仪和加速度计的零偏以及IMU的速度包括：(1)视觉初始化，根据匹配好的左右图像特征点，计算特征点的视差与深度值，然后，根据双目相机模型恢复特征点在相机坐标系下对应的空间坐标获得由空间点描述的初始地图；根据匹配好的前后帧图像特征点，利用BA集束调整法估计系统状态量的初始值；(2)惯导初始化，根据视觉初始化...

【专利技术属性】
技术研发人员：崔家山，冯冬竹，焦子涵，付秋军，赤丰华，滕锐，张舫瑞，田淇臣，
申请(专利权)人：北京临近空间飞行器系统工程研究所，
类型：发明
国别省市：