基于视觉与IMU实时信息融合的SLAM算法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115900699A(43)申请公布日2023.04.04(21)申请号202211387717.7(22)申请日2022.11.07(71)申请人天津大学地址300072天津市南开区卫津路92号(72)发明人董娜刘帅麦晓明秦明辉(51)Int.Cl.G01C21/16(2006.01)G01C22/00(2006.01)G01C21/00(2006.01)G06F18/25(2023.01)权利要求书2页说明书7页附图5页(54)发明名称基于视觉与IMU实时信息融合的SLAM算法(57)摘要本发明公开了一种基于视觉与IMU实时信息融合的SLAM算法，主要包括：在ORB‑SLAM2的基础上构建基于视觉与IMU实时信息融合的SLAM系统；设计一种基于三轴IMU数据的权值自适应均值滤波算法；利用滤波处理的IMU数据，与视觉里程计数据融合，实现位姿估计；最后优化地图点和进行回环检测。本发明提出的视觉与IMU融合的SLAM系统中，将IMU数据滤波以去除脉冲及高斯噪声，通过加权计算使得数据能更好反应现实情况；权值自适应均值滤波算法在处理数据方面有更好的零偏稳定性；转角误差概率密度集中分布在更小的区间范围内，跟踪性能更好，轨迹平滑性更好、闭合性更佳；从而进一步提高系统定位精度和鲁棒性及其在复杂环境下的稳定性和抗干扰能力。CN115900699ACN115900699A权利要求书1/2页1.一种基于视觉与IMU实时信息融合的SLAM算法，其特征在于，包括下述步骤：S1、在ORB‑SLAM2的基础上构建基于视觉与IMU实时信息融合的SLAM系统；S2、设计一种基于三轴IMU数据的权值自适应均值滤波算法；S3、利用S2处理的IMU数据，与视觉里程计数据融合，实现位姿估计；S4、优化地图点和进行检测。2.根据权利要求1所述的基于视觉与IMU实时信息融合的SLAM算法，其特征在于，S1的过程是：构建基于视觉与IMU实时信息融合的SLAM系统，该SLAM系统依赖于ORB‑SLAM2；在跟踪线程中，输入由视频帧的单一输入改变为视频帧和IMU数据的组合双输入；在局部建图这一线程中，添加了IMU的相关操作，在完成对视频帧的操作后，需要对IMU进行初始化，当IMU初始化完成之后再对冗余的关键帧进行删除操作，最后对IMU数据的规模进行调整，即可传入到闭环线程。3.根据权利要求1所述的基于视觉与IMU实时信息融合的SLAM算法，其特征在于，所述S2包括以下子步骤：S21：测试数据集采用EuRoC数据集，选取固定滤波窗口W，以每一个数据点为中心，去除窗口内的极大值、极小值数据点；S22：剩余数据的集合记为H，计算H内所有数据的均值和集合H内各数据与其均值的差的绝对值Dk，与选定阈值T相比较；S23：利用阈值优化原则，如果集合H内的某一点的Dk大于阈值T，则权值由Dk决定；如果集合H内的某一点的Dk小于或等于阈值T，则权值由T决定；S24：计算得到集合H内各数据点所对应的权值，并做归一化处理；S25：计算得到该中心点的带有权值的均值。4.根据权利要求1所述的基于视觉与IMU实时信息融合的SLAM算法，其特征在于，所述步骤S21中：以含噪声的数据集中的每一个数据点为中心选取一个固定大小的滤波窗口W，将窗口内的极大值、极小值数据点去除，剩余数据的集合记为H，如下式：H[f(i)]＝{f(i)|f(i)≠Max(W[f(i)])或f(i)≠Min(W[f(i)])}(1)式(1)中，Max(W[f(i)])表示窗口内的极大值点，Min(W[f(i)])表示窗口内的极小值点；H[f(i)]表示去除极大值点和极小值点的集合；所述步骤S22中：计算H内的所有数据的均值Mean(H[f(i)])，Dk表示集合H内各数据与其均值的差的绝对值，计算公式如(2)所示；对阈值T的选取规则如公式(3)所示，即所有Dk的平均值；Dk＝|Hk‑Mean(H[f(i)])|(2)所述步骤S24中：利用阈值优化原则计算集合H内各数据点所对应的权值，并对其进行归一化处理，计算如下式：2CN115900699A权利要求书2/2页其中，ωk表示集合H内各数据点的权值，k为集合H内数据点的个数；所述步骤S25中：将集合H内的所有的数据与其所对应的权值进行加权计算，计算结果即为所要求的带有权值的均值，计算公式如式(5)所示；5.根据权利要求1所述的基于视觉与IMU实时信息融合的SLAM算法，其特征在于，所述S3包括以下子步骤：S31：对IMU数据预积分处理；S32：通过传感器读取当前帧，先在8层图像金字塔中提取FAST特征点；对提取到的FAST特征点用ORB来描述；利用IMU数据，选取参考帧，将描述与参考帧进行匹配；如果匹配点对数小于最低匹配阈