基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统技术方案

本发明专利技术提供基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统，方法包括：通过相机和惯性测量单元IMU分别采集图像及惯性数据，并将图像及惯性数据送入预置频率参数的单目REKF

【技术实现步骤摘要】
基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统


[0001]本专利技术涉及室内移动机器人的实时定位与建图领域，具体涉及基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统。

技术介绍

[0002]随着计算机技术的不断发展，人工智能是未来发展的重要方向，其中涉及到图形图像处理、计算机视觉、模式识别等热门研究领域。SLAM(Simultaneous Localization and Mapping，同步定位与建图)技术为室内自主移动机器人的相关应用提供了行之有效的解决方案。
[0003]在室内条件下，GPS定位精度出现退化，单目相机与IMU传感器联合使用可以作为GPS的替代品，不仅因为它们具有小巧轻便，价格低廉的特点，而且在位姿估计中具有一定的互补作用。单目视觉SLAM在纹理丰富的环境下可以提供良好的运动估计和地图信息，但是其鲁棒性较差，并且单目视觉具有尺度不确定性。互补地，IMU则能够高频的输出机体自身的加速度与角速度信息，不受环境因素的影响，能够鲁棒的提供帧间运动估计及运动的绝对尺度。但是，IMU长期运行不本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法，其特征在于，所述方法包括：S1、通过相机和惯性测量单元IMU分别采集图像及惯性数据，并将所述图像及所述惯性数据送入预置频率参数的单目REKF
‑
VIO系统；S2、对所述单目REKF
‑
VIO系统执行状态预测、更新以及状态扩充操作，以估计当前帧的状态后验均值和系统协方差，其中，所述步骤S2包括：S21、利用所述单目REKF
‑
VIO系统根据所述图像及所述惯性数据，执行单目REKF
‑
VIO状态预测；S22、对所述单目REKF
‑
VIO系统执行更新操作，据以估计当前帧状态后验均值和当前帧协方差；S23、在所述单目REKF
‑
VIO系统的初始全局地图未建立时，执行预设状态扩充逻辑，以扩充所述单目REKF
‑
VIO系统的所述系统状态后验均值和所述系统协方差；S3、初始化所述单目REKF
‑
VIO系统，以构建所述初始全局地图的初始地图点；S4、根据所述初始地图点进行筛选处理，据以得到视觉关键帧；S5、根据所述视觉关键帧，利用图优化及EKF互补框架获取优化全局地图以及运动状态信息，所述步骤S5还包括：S51、将所述视觉关键帧送入预设全局地图辅助EKF反馈机制模块，以对所述视觉关键帧进行EKF状态修正；S52、根据所述所述视觉关键帧进行EFK地图修正，利用图优化方法优化所述初始全局地图，以得到所述优化全局地图，据以再更新并扩充所述单目REKF
‑
VIO系统的状态向量，以得到所述运动估计信息；S6、将所述运动估计信息送入所述优化全局地图，据以进行局部地图的更新和闭环优化。2.根据权利要求1所述的基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法，其特征在于，所述步骤S21包括：S211、设当前帧观测到m个地图点，则利用下述逻辑将所述单目REKF
‑
VIO系统的所述状态向量X
S
表示为：式中，表示当前帧的IMU状态，表示地图点的状态，表示从IMU坐标系到世界坐标系的旋转，分别表示IMU坐标系相对于世界坐标系的位置和速度，分别表示陀螺仪和加速度计的零偏，表示用逆深度坐标表示的第l个地图点在世界坐标系下的位置；S212、利用惯性测量单元IMU测量相对于其自身机体坐标系的自身角速度和加速度ω
m
和a
m
：ω
m
＝ω
B
+b
g
+n
g
式中，代表世界坐标系和IMU坐标系之间的变换矩阵，g
w
为世界系下已知的重力向量，b
g
，b
a
为传感器零偏，n
g
，n
a
为零均值高斯白噪声。S213、利用下述逻辑对所述惯性测量单元IMU进行预计分：S213、利用下述逻辑对所述惯性测量单元IMU进行预计分：S213、利用下述逻辑对所述惯性测量单元IMU进行预计分：式中，分别是对应所述所述惯性测量单元IMU从b
k
到b
k+1
时刻的位置，速度和姿态四元数，为四元数乘法，a
m
为加速度观测、为旋转矩阵，Δt为预测时间；S214、对前述预积分等式两边同时乘以得到以下等式关系：得到以下等式关系：得到以下等式关系：上式中：上式中：上式中：上式中：及为不依赖初始状态的IMU预积分项。3.根据权利要求1所述的基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法，其特征在于，所述S22包括：S221、利用逆深度坐标表示第l个地图点位置并利用下述逻辑将所述第l个地图点位置转换为对应的欧几里德XYZ坐标
S222、根据预置相机投影模型，利用下述逻辑，获取所述第l个地图点在当前帧F
s
图像平面上的视觉观测模型：式中，表示当前帧F
s
对第l个地图点的实际观测值，表示视觉观测噪声，分别表示相机坐标系与IMU坐标系之间的旋转和平移；S223、在所述当前帧F
s
的状态先验均值处，利用下述一阶泰勒展开逻辑，对所述视觉观测模型进行线性化估计，以得到所述当前帧F
s
图像平面上的观测值图像平面上的观测值式中，和J
sl
分别表示相对于IMU状态第l个地图点位置和状态向量X
s
的雅可比矩阵，J
sl
为测量矩阵；S224、利用下述逻辑，可以获得所述第l个地图点在所述当前帧F
s
图像平面上的观测值与对应预测值之间的测量误差及线性化估计：S225、所述当前帧F
s
对所述状态向量中所有地图点的观测数据为通过堆叠所有所述测量误差r
sl
、所述测量矩阵J
sl
和所述视觉观测噪声以得到所述当前帧的状态后验均值和所述系统协方差的更新方程：前帧的状态后验均值和所述系统协方差的更新方程：前帧的状态后验均值和所述系统协方差的更新方程：式中，且4.根据权利要求1所述的基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法，其特征在于，所述步骤S23包括：S231、在系统的初始全局地图未建立时，设所述当前帧F
s
选用新地图点α扩充所述状态向量，则其初始逆深度坐标为：S232、根据所述当前帧的IMU状态后验均值，利用下述逻辑计算所述新地图点α的初始逆深度坐标相机位置：
S233、根据所述当前帧F
s
对所述新地图点α的观测值利用下述逻辑，计算所述新地图点的初始逆深度坐标方位角、俯仰角：S234、根据初始逆深度决定参数得到所述新地图点α的初始逆深度坐标逆深度值：S235、根据所述新地图点α的所述初始逆深度坐标逆深度值，以下述逻辑扩充所述所述系统状态后验均值：S236、利用下述一阶泰勒展开逻辑，分别在当前帧F
s
的所述惯性测量单元IMU状态后验均值当前帧对地图点α的实际观测值以及决定初始逆深度值的参数处，对所述新地图点α的逆深度坐标做线性化估计得到：上述公式中：该式决定初始逆深度值的参数噪声，与分别是所述新地图点α的状态相对于所述惯性测量单元IMU状态观测值z
sα

【专利技术属性】
技术研发人员：樊渊，张弼泽，杨国志，方笑晗，程松松，潘天红，张德祥，董翔，李腾，樊富友，
申请(专利权)人：安徽大学，
类型：发明
国别省市：