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

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115930943A(43)申请公布日2023.04.07(21)申请号202211555776.0G06V10/40(2022.01)(22)申请日2022.12.06G06V10/80(2022.01)(71)申请人安徽大学地址230039安徽省合肥市蜀山区肥西路3号(72)发明人樊渊张弼泽杨国志方笑晗程松松潘天红张德祥董翔李腾樊富友(74)专利代理机构合肥市浩智运专利代理事务所(普通合伙)34124专利代理师朱文振(51)Int.Cl.G01C21/00(2006.01)G01C21/16(2006.01)G06V10/10(2022.01)权利要求书7页说明书16页附图4页(54)发明名称基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统(57)摘要本发明提供基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法及系统，方法包括：通过相机和惯性测量单元IMU分别采集图像及惯性数据，并将图像及惯性数据送入预置频率参数的单目REKF‑VIO系统；对单目REKF‑VIO系统执行状态预测、更新以及状态扩充操作，以估计当前帧的状态后验均值和系统协方差；初始化单目REKF‑VIO系统，以构建初始全局地图的初始地图点；根据初始地图点进行筛选处理，据以得到视觉关键帧；根据视觉关键帧，利用图优化及EKF互补框架获取优化全局地图以及运动状态信息；将运动估计信息送入优化全局地图，据以进行局部地图的更新和闭环优化。本发明解决了前端图片帧处理效率低、误差干扰及精度较低的技术问题。CN115930943ACN115930943A权利要求书1/7页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系统的所述状态向量XS表示为：式中，表示当前帧的IMU状态，表示地图点的状态，表示从IMU坐标系到世界坐标系的旋转，分别表示IMU坐标系相对于世界坐标系的位置和速度，分别表示陀螺仪和加速度计的零偏，表示用逆深度坐标表示的第l个地图点在世界坐标系下的位置；S212、利用惯性测量单元IMU测量相对于其自身机体坐标系的自身角速度和加速度ωm和am：Bωm＝ω+bg+ng2CN115930943A权利要求书2/7页式中，代表世界坐标系和IMU坐标系之间的变换矩阵，gw为世界系下已知的重力向量，bg，ba为传感器零偏，ng，na为零均值高斯白噪声。S213、利用下述逻辑对所述惯性测量单元IMU进行预计分：式中，分别是对应所述所述惯性测量单元IMU从bk到bk+1时刻的位置，速度和姿态四元数，为四元数乘法，am为加速度观测、为旋转矩阵，Δt为预测时间；S214、对前述预积分等式两边同时乘以得到以下等式关系：上式中：及为不依赖初始状态的IMU预积分项。3.根据权利要求1所述的基于图优化和EKF框架下融合单目视觉和IMU的SLAM方法，其特征在于，所述S22包括：S221、利用逆深度坐标表示第l个地图点位置并利用下述逻辑将所述第l个地图点位置转换为对应的欧