一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法技术方案

本发明专利技术提出了一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法。包括传感器数据预处理，系统初始化，跟踪和局部地图构建四个模块，传感器数据预处理模块负责多镜头鱼眼图像的特征提取，特征球面投影，图像帧间的IMU预积分，轮速预积分。初始化模块，通过处理视觉，IMU和轮速计测量值对SLAM系统进行初始化。跟踪模块利用IMU测量值积分得到当前帧位姿，为后续优化提供初始化值，并判断当前帧是否为关键帧。局部地图构建模块负责将插入的关键帧进行处理，与之前帧匹配进行新地图点的三角化，并进行局部地图的全景视觉‑IMU‑轮速计的联合平差。本发明专利技术通过多传感器融合互补实现复杂场景下的高鲁棒性高精度定位。

【技术实现步骤摘要】
一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法
本专利技术涉及一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法，应用于地面移动测量系统，地面轮式机器人，自动驾驶等领域。
技术介绍
视觉SLAM(VisualSimultaneousLocalizationandMapping)在过去20年被深入研究且已经广泛应用于机器人，自动驾驶，AR等新兴领域。现有的视觉SLAM算法可以根据前端跟踪模式的不同分为基于特征点的间接法与基于灰度信息的直接法两类。而根据后端优化方法不同，又可以分成基于滤波和基于光束法平差两种方案。在基于单目的SLAM模式下，受制于相机视场角不足的问题，当发生剧烈运动，光照变化，或者纹理稀少的情况时，无论是基于特征点法还是直接法都不够鲁棒。而全景视觉传感器可以获取周围360°的场景信息，恰好可以有效解决视场角不足的问题。但是由于视觉传感器本身的缺陷，即便使用全景相机仍然还是会发生跟踪丢失的现象。由于IMU与视觉天然的互补特性，VIO(VisualInertialOdometry)近几年成为了SLAM领域的研究本文档来自技高网...

【技术保护点】
1.一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于，具体包括如下模块：/n传感器测量数据预处理模块，用于对传入的传感器的原始数据进行处理，包括对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面；IMU预积分的计算，以及轮速计预积分的计算；为后续系统初始化模块、跟踪模块、局部地图构建模块提供所需数据；/n系统初始化模块，用于实现系统的初始化，包括纯全景视觉初始化以获得初始位姿并建立初始化地图；全景视觉与轮速计的几何对齐以将视觉恢复到绝对尺度之下，全景视觉与IMU的几何对齐以获取IMU的初始化参数；全景视觉、IMU和轮速计的联合平差以对IMU初始化参数进行优化...

【技术特征摘要】
1.一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于，具体包括如下模块：
传感器测量数据预处理模块，用于对传入的传感器的原始数据进行处理，包括对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面；IMU预积分的计算，以及轮速计预积分的计算；为后续系统初始化模块、跟踪模块、局部地图构建模块提供所需数据；
系统初始化模块，用于实现系统的初始化，包括纯全景视觉初始化以获得初始位姿并建立初始化地图；全景视觉与轮速计的几何对齐以将视觉恢复到绝对尺度之下，全景视觉与IMU的几何对齐以获取IMU的初始化参数；全景视觉、IMU和轮速计的联合平差以对IMU初始化参数进行优化；
跟踪模块，用于对当前帧的位姿进行估计，当系统初始化未完成时，使用跟踪恒速模型以及关键帧模型来进行跟踪；当系统完成初始化后，利用IMU测量值直接预测当前帧位姿，并判断当前帧是否为关键帧；
局部地图构建模块，该模块与跟踪模块为并行线程，当有关键帧从跟踪模块传入后，对关键帧进行处理，包括对新传入的关键帧与之前相关联的关键帧进行匹配，并将匹配点三角化，局部地图的全景视觉、IMU和轮速计的联合平差以精化地图点三维坐标以及各关键帧位姿，并剔除冗余帧和误匹配点。


2.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：传感器测量数据预处理模块中对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面的具体实现方式如下；
对传入的影像提取ORB特征点，并根据公式(1)将特征点的像素坐标x转换为球面坐标x′；



其中，Ti和Ri分别表示第i个鱼眼相机相对于相机坐标系的平移矩阵和旋转矩阵；Ki为第i个鱼眼相机的内参矩阵，m为球面半径r所确定的比例系数。


3.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：传感器测量数据预处理模块中IMU预积分的计算的具体实现方式如下；
对传入的IMU测量值进行预积分计算，采用中值积分方法，积分起始位置为第i个IMU测量值，其中第i个IMU测量值通过视觉帧的视觉戳来确定，预积分项从第k个IMU测量值到第(k+1)个测量IMU测量值的积分形式为，k＝i,...,j-1：



其中，两帧间的相对位移Δp，相对速度Δv，相对旋转ΔR分量即为IMU预积分项，上标表示两帧的序号，下标I表示变量在IMU坐标系下，ba和bg分别为加速度计偏置和陀螺仪偏置，gW为重力加速度，ω和a为利用中值法计算得到的角速度和加速度，Exp表示从李代数到李群的转换，Δtk,k+1为第k个IMU测量值到第(k+1)个IMU测量值之间的时间间隔。


4.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：传感器测量数据预处理模块中轮速计预积分的计算的具体实现方式如下；
基于双轮差速获得轮速计预积分，右轮线速度为vr，左轮线速度为vl，并且可以提前测量得到后轮轮距为d，根据双轮差速模型得到载体的2D线速度以及偏航角的角速度：



轮速计实际只能测得载体前进方向的速度，因此只有前进方向所在的轴的测量值，因此测量值的白噪声n～N(0,σ2)将前进方向的轴之外的两个轴的方差均设为无穷大；
采用中值积分的形式，计算从第k个轮速计测量值到第(k+1)个轮速计测量值的积分形式为，k＝i,...,j-1：



其中，两帧间的相对位移Δp，和相对旋转ΔR分量为轮速计预积分项，上标表示两帧的序号，下标O表示变量在轮速计坐标系下，ω和a为利用中值法计算得到的角速度和加速度，Δtk,k+1为第k个轮速计测量值到第(k+1)个轮速计测量值之间的时间间隔。


5.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：系统初始化模块的具体实现方式如下，
步骤2.1，在纯全景视觉初始化中，先对两帧影像进行特征点提取与匹配，若匹配点数小于n1，则重新进行步骤2.1，否则进行后续步骤；根据全景球面对极约束关系以及RANSAC方法求解本质矩阵E恢复姿态信息，包括旋转R和平移t，并三角化初始化地图，即为初始的局部地图，最后通过调用开源优化库G2O对旋转、平移以及初始化地图进行优化；若优化后的地图点大于n2个则认为两帧影像三角化成功，否则为失败，重新接受影像进行步骤2.1；并保证至少n3个关键帧进入，以建立纯全景视觉的初始化地图，为后续轮速计以及IMU的初始化做准备；若不够n3帧，则进入步骤3，否则进入步骤2.2；
步骤2.2，全景视觉-轮速计对齐；根据两个关键帧之间轮速计位移预积分Δpi,j与视觉位移的关系，求解出尺度s，并将视觉恢复到绝对尺度之下，具体求方法如下公式：



式中pOS为全景相机与轮速计之间的外参，和为从世界坐标系到第i和j帧所在的轮速计坐标系的旋转矩阵，和从第i和j帧所在全景相机坐标系到世界坐标系的平移矩阵；
若求解的尺度小于m，则判断尺度求解失败，重新进行步骤2.2，否则进入步骤2.3；
步骤2.3，全景视觉-IMU对齐，这一步的目标是获取IMU的初始化参数，初始化参数为：



其中RWg∈SO3为重力在世界坐标系中的方向，bg,ba分别为陀螺仪与加速度计的偏置，v0:kWI为从第0个关键帧到第k个关键帧恢复到绝对尺度下的速度；
最后，求解完成后利用求解得到的...

【专利技术属性】
技术研发人员：季顺平，姜帆，陈佳港，
申请(专利权)人：武汉大学，
类型：发明
国别省市：湖北;42