[发明专利]一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法

权利要求书

1.一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于，具体包括如下模块：

传感器测量数据预处理模块，用于对传入的传感器的原始数据进行处理，包括对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面；IMU预积分的计算，以及轮速计预积分的计算；为后续系统初始化模块、跟踪模块、局部地图构建模块提供所需数据；

系统初始化模块，用于实现系统的初始化，包括纯全景视觉初始化以获得初始位姿并建立初始化地图；全景视觉与轮速计的几何对齐以将视觉恢复到绝对尺度之下，全景视觉与IMU的几何对齐以获取IMU的初始化参数；全景视觉、IMU和轮速计的联合平差以对IMU初始化参数进行优化；

跟踪模块，用于对当前帧的位姿进行估计，当系统初始化未完成时，使用跟踪恒速模型以及关键帧模型来进行跟踪；当系统完成初始化后，利用IMU测量值直接预测当前帧位姿，并判断当前帧是否为关键帧；

局部地图构建模块，该模块与跟踪模块为并行线程，当有关键帧从跟踪模块传入后，对关键帧进行处理，包括对新传入的关键帧与之前相关联的关键帧进行匹配，并将匹配点三角化，局部地图的全景视觉、IMU和轮速计的联合平差以精化地图点三维坐标以及各关键帧位姿，并剔除冗余帧和误匹配点。

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

对传入的影像提取ORB特征点，并根据公式(1)将特征点的像素坐标x转换为球面坐标x′；

其中，T_i和R_i分别表示第i个鱼眼相机相对于相机坐标系的平移矩阵和旋转矩阵；K_i为第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坐标系下，b_a和b_g分别为加速度计偏置和陀螺仪偏置，g_W为重力加速度，ω和a为利用中值法计算得到的角速度和加速度，Exp表示从李代数到李群的转换，Δt_k,k+1为第k个IMU测量值到第k+1个IMU测量值之间的时间间隔。

4.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：传感器测量数据预处理模块中轮速计预积分的计算的具体实现方式如下；

基于双轮差速获得轮速计预积分，右轮线速度为v_r，左轮线速度为v_l，并且可以提前测量得到后轮轮距为d，根据双轮差速模型得到载体的2D线速度以及偏航角的角速度：

轮速计实际只能测得载体前进方向的速度，因此只有前进方向所在的轴的测量值，因此测量值的白噪声n～N(0,σ²)将前进方向的轴之外的两个轴的方差均设为无穷大；

采用中值积分的形式，计算从第k个轮速计测量值到第k+1个轮速计测量值的积分形式为，k＝i,...,j-1：

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