CN113223161B - 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法，应用于地面移动测量系统，地面轮式机器人，自动驾驶等领域。

背景技术

视觉SLAM(Visual Simultaneous Localization and Mapping)在过去20年被深入研究且已经广泛应用于机器人，自动驾驶，AR等新兴领域。现有的视觉SLAM算法可以根据前端跟踪模式的不同分为基于特征点的间接法与基于灰度信息的直接法两类。而根据后端优化方法不同，又可以分成基于滤波和基于光束法平差两种方案。在基于单目的SLAM模式下，受制于相机视场角不足的问题，当发生剧烈运动，光照变化，或者纹理稀少的情况时，无论是基于特征点法还是直接法都不够鲁棒。而全景视觉传感器可以获取周围360°的场景信息，恰好可以有效解决视场角不足的问题。但是由于视觉传感器本身的缺陷，即便使用全景相机仍然还是会发生跟踪丢失的现象。

由于IMU与视觉天然的互补特性，VIO(Visual Inertial Odometry)近几年成为了SLAM领域的研究热点。即使发生剧烈的光照变化，或纹理缺失等极端问题而导致视觉里程计发生跟踪丢失，IMU也可以在短时间内提供较好的定位结果，有效提高视觉里程计的鲁棒性。MSCKF和VINS-Mono分别为基于图优化和基于滤波两种典型VIO系统的代表。其中基于图优化的方案需要进行迭代步骤，会不断对IMU进行积分操作而导致计算量急剧上升，因此IMU预积分理论被提出。IMU预积分将帧与帧之间的绝对测量转换为相对测量，有效地解决了这一问题。在VIO系统中，可观性分析非常重要。对于常规运动，VIO系统有四个不可观方向(三个全局平移和一个全局偏航角)，但是当VIO系统的载体为地面移动平台时，常常出现匀加速度运动，导致VIO系统的尺度不可观，或者出现无旋转运动，导致VIO系统三个旋转都变为不可观。考虑到轮式机器人或者汽车可以轻易获得轮速计数据，而轮速计可以提供地面载体双轮前进方向速度，从可观性分析上证明VIO加入轮速计后可以解决上述问题。

目前并没有将多镜头组合式全景相机与IMU进行融合，同时解决轮式机器人或者车载场景下全景VIO退化问题的具体方案。

发明内容

本发明的目的是克服视觉SLAM不够鲁棒以及地面载体平台下VIO系统出现的尺度，旋转不可观的问题，而提出基于IMU，轮速计和全景相机紧耦合的一种鲁棒全景SLAM方法，同时，也为地面移动测量系统所搭载的主流传感器提供了一种新的定位建图解决方案，实现更鲁棒更高精度的定位结果。

实现本发明目的采用的技术方案是：一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，具体包括如下模块：

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

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

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

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

进一步的，传感器测量数据预处理模块中对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面的具体实现方式如下；

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

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

进一步的，传感器测量数据预处理模块中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测量值之间的时间间隔。

进一步的，传感器测量数据预处理模块中轮速计预积分的计算的具体实现方式如下；

基于双轮差速获得轮速计预积分，右轮线速度为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)个轮速计测量值之间的时间间隔。

进一步的，系统初始化模块的具体实现方式如下，

步骤2.1，在纯全景视觉初始化中，先对两帧影像进行特征点提取与匹配，若匹配点数小于n1，则重新进行步骤2.1，否则进行后续步骤；根据全景球面对极约束关系以及RANSAC方法求解本质矩阵E恢复姿态信息，包括旋转R和平移t，并三角化初始化地图，即为初始的局部地图，最后通过调用开源优化库G2O对旋转、平移以及初始化地图进行优化；若优化后的地图点大于n2个则认为两帧影像三角化成功，否则为失败，重新接受影像进行步骤2.1；并保证至少n3个关键帧进入，以建立纯全景视觉的初始化地图，为后续轮速计以及IMU的初始化做准备；若不够n3帧，则进入步骤3，否则进入步骤2.2；

步骤2.2，全景视觉-轮速计对齐；根据两个关键帧之间轮速计位移预积分Δp_i,j与视觉位移

的关系，求解出尺度s，并将视觉恢复到绝对尺度之下，具体求方法如下公式：

式中p_OS为全景相机与轮速计之间的外参，

和

为从世界坐标系到第i和j帧所在的轮速计坐标系的旋转矩阵，

和

从第i和j帧所在全景相机坐标系到世界坐标系的平移矩阵；

若求解的尺度小于m，则判断尺度求解失败，重新进行步骤2.2，否则进入步骤2.3；

步骤2.3，全景视觉-IMU对齐，这一步的目标是获取IMU的初始化参数，初始化参数为：

其中R_Wg∈SO3为重力在世界坐标系中的方向，b_g,b_a分别为陀螺仪与加速度计的偏置，v^0:k _WI为从第0个关键帧到第k个关键帧恢复到绝对尺度下的速度；

最后，求解完成后利用求解得到的R_Wg将整个坐标系旋转至与重力方向对齐；

步骤2.4，全景视觉-IMU-轮速计联合平差；将步骤2.2得到的恢复到绝对尺度下的全景视觉信息、IMU预积分、轮速计预积分加入到同一个优化函数中，对在步骤2.2中绝对尺度下计算的旋转、平移和初始化地图，以及步骤2.3中得到的偏置和速度进行优化，利用优化后的IMU参数，即陀螺仪与加速度计的偏置以及速度，对IMU预积分项进行更新。

进一步的，跟踪模块的具体实现方式如下；

步骤3.1，若当前总帧数小于n3帧，采用跟踪恒速模型来估计当前帧的位姿，当恒速模型失败后，则采用跟踪关键帧模型来进行纯视觉跟踪，得到当前帧的位姿信息；

步骤3.2，利用IMU测量值预测推断当前帧的位姿，在前一帧位姿的基础上，对当前帧与前一帧之间的IMU信息进行积分，得到当前帧位姿的初始值；

步骤3.3，得到当前帧位姿的初始值后，利用该值对局部地图中的三维点投影，与当前帧所提取的特征点进行匹配，并进行联合平差；

步骤3.4，选取关键帧，判断当前帧是否为关键帧，若不是关键帧，则继续接收新的数据重复步骤1.1～3.4，若被判断为关键帧，则将关键帧插入局部地图构建模块；判断是否为关键帧的具体条件如下，m1-m5均为常数：

(1)若初始化模块中不足m1帧时，只要当前帧与上一帧的时间间隔大于m2秒，则被判定关键帧；

(2)若当前帧与上一关键帧相隔超过相机帧率数，则被判定为关键帧；

(3)若当前帧跟踪到的地图点数小于上一帧的m3％，则判定为关键帧；

(4)若当前帧与上一帧的时间间隔大于m4秒，则判定为关键帧；

(5)若当前帧匹配到的地图点数小于m5个，则判定为关键帧。

本发明还提供一种基于IMU和轮速计紧耦合的鲁棒全景SLAM方法，通过联合平差方法，将多镜头组合式全景相机、惯性导航单元IMU和轮速计三种传感器获取的数据进行融合，具体包括如下步骤：

步骤1，对传入的传感器的原始数据进行处理，包括对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面；IMU预积分的计算，以及轮速计预积分的计算；

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

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

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

进一步的，步骤1中对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面的具体实现方式如下；

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

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

进一步的，步骤1中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测量值之间的时间间隔。

进一步的，步骤1中轮速计预积分的计算的具体实现方式如下；

基于双轮差速获得轮速计预积分，右轮线速度为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)个轮速计测量值之间的时间间隔。

与现有技术相比，本发明具有如下优点和有益效果：

1)加入轮速计解决VIO中由于特殊场景导致的退化问题，并以此提出了一套更为鲁棒的系统初始化方案。

2)拓展了全景SLAM方法，引入IMU和轮速计两种传感器，实现了全景视觉-IMU-轮速计的联合平差，实现了更鲁棒更高精度的定位结果。

3)此系统能用于地面移动测量系统中，在移动机器人或者车载移动测图系统中有望具体广泛的应用。

附图说明

下面结合附图和具体实施方式对本发明作进一步的说明。

图1为本发明实施例所属的基于IMU和轮速计紧耦合的全景SLAM方法的流程图。

图2为本发明提出的多传感器测量模型示意图。

图3为本发明的局部地图构建模块示意图。

图4为NCLT数据集2012-06-15序列使用本发明进行定位与建图的效果图。

图5为验证实验1中算法的轨迹对比图。

图6为验证实验2对图像宽基线模式下的轨迹对比图。

具体实施方式

本发明方法的具体作业流程如附图1所示，包括传感器测量数据预处理模块，系统初始化模块，跟踪模块，局部地图构建模块。第一部分是传感器测量数据处理模块，主要负责多镜头鱼眼图像的特征提取，特征全景球面投影，两个图像帧之间的IMU测量值的预积分，两个图像帧之间的轮速计测量值的预积分。若系统还未初始化，传感器测量数据处理模块的结果会先被传入出初始化模块，以完成初始化工作，若系统已经初始化，传感器测量数据处理模块的结果会被送入跟踪模块以进行位姿跟踪和关键帧的选择。第二部分是初始化模块，通过全景视觉初始化、全景视觉-轮速计对齐、全景视觉-IMU对齐、全景视觉-IMU-轮速计联合平差，四个步骤对SLAM系统进行初始化。第三部分跟踪模块利用IMU测量值积分得到当前帧位姿，为后续平差提供初值，并判断当前帧是否为关键帧，若为关键帧则传入局部地图构建模块，完成后返回测量数据处理模块，继续接收传感器测量数据，并进行处理。第四部分局部地图构建模块与跟踪模块并行，一旦有关键帧插入，负责将插入的关键帧进行处理，与之前帧匹配进行新地图点的三角化，并进行局部地图的全景视觉-IMU-轮速计的联合平差，精化地图点三维坐标以及各关键帧位姿，若无关键帧插入则空闲不工作。

1.传感器测量数据处理模块具体步骤如下：

本模块负责对传入的传感器的原始数据进行处理，包括全景的特征点提取与投影，IMU预积分计算，轮速计预积分计算，为后续初始化，跟踪，局部地图构建模块提供所需数据。

1.1：多镜头组合式全景相机特征点提取与投影。对传入的影像提取ORB特征点，并根据公式(1)将特征点的像素坐标x转换为球面坐标x′，由于独立鱼眼镜头中心C和全景球面中心S不可能完全重合，导致鱼眼镜头中的特征点需要经过第i个鱼眼相机相对于相机坐标系的平移T_i和旋转R_i才可以正确投影到球面，K_i为第i个鱼眼相机的内参矩阵，m为球面半径r所确定的比例系数；

1.2：计算IMU预积分。对传入的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测量值之间的时间间隔。

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

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

类似IMU预积分，这里采用中值积分的形式，计算从第k个轮速计测量值到第(k+1)个轮速计测量值的积分形式为(k＝i,...,j-1)：

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

2.初始化模块具体步骤如下：

完成传感器测量数据处理模块的工作后，进入初始化模块，实现对系统的初始化。初始化的目的是启动系统，获得初始位姿，尺度信息，初始IMU参数，并建立初始化地图。

2.1：在纯全景视觉初始化中，先对两帧进行特征点提取匹配，若匹配点数小于50，则重新进行步骤2.1，否则进行后续步骤。根据全景球面对极约束关系以及RANSAC方法求解本质矩阵E恢复姿态信息包括旋转R和平移t，并三角化初始化地图(初始的局部地图)，最后通过调用Rainer Kuemmerle所发布的开源优化库G2O对旋转平移以及初始化地图进行优化。若优化后的地图点大于80个则认为两帧三角化成功，否则为失败，重新接收影像进行步骤2.1。此后保证至少20个关键帧进入，建立较为大规模的纯全景视觉的初始化地图，为后续轮速计以及IMU的初始化做准备。若不够20帧，则进入步骤3.1，否则进入步骤2.2。

2.2：全景视觉-轮速计对齐。根据两个关键帧之间轮速计位移预积分Δp_i,j与视觉位移

的关系，可以求解出尺度s，并将视觉的旋转和平移以及初始化地图恢复到绝对尺度之下，具体求方法如下公式：

式中p_OS为全景相机与轮速计之间的外参，

和

为从世界坐标系到第i和j帧所在的轮速计坐标系的旋转矩阵，

和

从第i和j帧所在全景相机坐标系到世界坐标系的平移矩阵。

若求解的尺度小于0.1，则判断尺度求解失败，重新进行步骤2.2，否则进入步骤2.3。

2.3：全景视觉-IMU对齐。这一步的目标是获取IMU的初始化参数。利用步骤2.2中得到的恢复绝对尺度后的全景视觉的旋转平移与步骤1.2中得到的IMU预积分项之间的关系，构建位移，速度，旋转的误差函数，构建优化问题进行求解，求解时可以将陀螺仪与加速度计的偏置初始值设为0，重力方向则沿加速度计测量值的平均方向，求解得到的初始化参数为：

其中R_Wg∈SO3为重力在世界坐标系中的方向，b_g,b_a分别为陀螺仪与加速度计的偏置，v^0:k _WI为从第0个关键帧到第k个关键帧恢复到绝对尺度下的速度。

最后，求解完成后利用求解得到的R_Wg将整个坐标系旋与重力方向进行对齐。

2.4：全景视觉-IMU-轮速计联合平差。如附图2为多传感器测量模型示意图，全景相机测量值，IMU测量值，轮速计测量值之间通过彼此外参相关联。将步骤2.2得到的恢复到绝对尺度下的全景视觉信息，步骤1.2得到的IMU预积分，步骤1.3得到的轮速计预积分加入到同一个优化函数中，对步骤2.2中得到的绝对尺度下的旋转、平移和初始化地图，步骤2.3中得到的速度和偏置，进行优化。利用优化后的IMU参数(偏置，速度)对步骤1.2计算的IMU预积分项进行更新(更新方式参考Christian Forster在2015年发表的IMUpreintegration on manifold for efficient visual-inertial maximum-a-posterioriestimation)，完成系统初始化全部步骤，进入跟踪模块，步骤3.2。

3.跟踪模块具体步骤如下：

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

3.1：当初始化模块中发现当前关键帧数小于20帧，采用跟踪恒速模型来估计当前帧的位姿，当恒速模型失败后，则采用跟踪关键帧模型来进行纯视觉跟踪，得到当前帧的位姿信息。具体跟踪模型和方案参考PAN-SLAM(Shunping Ji等人于2020年发表的PanoramicSLAM from a multiple fisheye camera rig)。

3.2：利用IMU测量值(即IMU传感器最原始的测量值)预测推断当前帧的位姿。在前一帧位姿的基础上，对当前帧与前一帧之间的IMU信息进行积分，得到当前帧位姿的初始值。

3.3：得到当前帧位姿的初始值后，利用该值，对局部地图中的三维点进行投影，与当前帧所提取的特征点进行匹配，并进行联合平差。采用ORB-VI(Ra′ul Mur-Artal等人于2017年发表的Visual-Inertial Monocular SLAM with Map Reuse)中跟踪模块的相同的优化策略，唯一不同的是加入了额外的轮速计的约束。

3.4：选取关键帧，判断当前帧是否为关键帧，若不是关键帧，则继续接收新的数据重复步骤1.1～3.4，若被判断为关键帧，则将关键帧插入局部地图构建模块。判断是否为关键帧的具体条件如下：

①若初始化模块中不足20帧时，只要当前帧与上一帧的时间间隔大于0.25秒，则被判定关键帧。

②若当前帧与上一关键帧相隔超过相机帧率数，则被判定为关键帧。

③若当前帧跟踪到的地图点数小于上一帧的25％，则判定为关键帧。

④若当前帧与上一帧的时间间隔大于0.5秒，则判定为关键帧。

⑤若当前帧匹配到的地图点数小于75个，则判定为关键帧。

4.局部地图构建模块具体步骤如下：

局部地图构建模块与跟踪模块为并行线程，当有关键帧从跟踪模块传入后，负责对关键帧进行处理，主要包括三角化新的地图点，联合平差，冗余帧，误匹配点剔除。

4.1：当接收到从跟踪模块中传入的关键帧后，首先得到当前关键帧与其相关连的关键帧的匹配点，并将其三角化。当初始化完成后，可以获得绝对尺度信息，因此将新三角化出的，距离当前帧相机中心200m以外的地图点进行滤除，以减弱远点以及天空云对定位结果的影响。

4.2：进行局部地图的联合平差。如附图3所示，选取最近N个关键帧作为局部窗口，以及N个关键帧视野范围内的地图点进行优化。其他非窗口内的共视帧贡献损失函数但是不进行优化更新。值得注意的是，由于IMU与轮速预积分的需要，最近N+1帧也需要加入误差函数，但同样不对其位姿进行优化更新。

4.3：剔除误匹配点和冗余关键帧，更新关键帧与地图点的连接关系，更新局部地图与全局地图。

实验验证

为验证本发明的实际效果，在NCLT数据集的2段数据上进行了验证实验，对比了本发明方法和PAN-SLAM，OpenVSLAM(panoramic/equirectangular)，VINS-Mono，ORB-SLAM3(mono)，ORB-SLAM3-IW(mono)。附图4为NCLT数据集2012-06-15序列使用本发明进行定位与建图的效果图。本发明方法和PAN-SLAM对Ladybug3的5个1616×1232分辨率的鱼眼图像进行处理，VIN-Mono，ORB-SLAM3(mono)，ORB-SLAM3-IW(mono)均采用Ladybug3的5号鱼眼相机(前进方向)去畸变后的1616×1232分辨率的单目图像进行实验，OpenVSLAM在拼接好的分辨率为2000×1000的全景图像上进行实验。其中ORB-SLAM3-IW(mono)算法是在ORB-SLAM3单目惯性模式的基础上，引入本发明方法所述的轮速计模型。此外，在2012-06-15数据集上进行了抽帧，人为创造宽基线场景(基线长度接近1-2m)，将原本0.2m左右的基线长度延长到1m左右，来评测本发明方法与PAN-SLAM宽基线场景下的鲁棒性与定位精度。另外所有的试验均在Intel i5-7400(4核3.00Hz)，16GB RAM的计算机上进行。

1.轨迹验证实验

在NCLT数据集上测试了本发明方法和目前最新的SLAM和VIO算法，并使用evo工具箱对轨迹进行了评估。表1比较了在不同场景数据段下的不同算法的平移和旋转的均方根误差(RMSE)，并统计了总的跟踪帧数和跟踪的段数，作为鲁棒性定量分析的指标，需要注意的是，除本发明方法外其他算法都会发生若干次跟踪失败，因此使用每段跟踪的帧数作为权重，并只保留了跟踪帧数在200帧以上的段，计算了各段RMSE的加权平均值作为一整段轨迹的RMSE。最后使用平移和旋转的RMSE作为评估定位精度的定量指标，跟踪的总帧数和总段数作为评估鲁棒性的定量指标。从表1中可以定量地看出本发明方法的定位精度和鲁棒性均优于其余方法。附图5所示为不同方法与真值的轨迹对比图以及跟踪中断情况。从附图5中可以看出本发明方法的跟踪轨迹相较于其他方法更加吻合轨迹真值，并且本发明没有发生跟踪中断情况，其他方法均发生多少跟踪中断的情况(“X”)。

表1平移和旋转的RMSE，正确跟踪的帧数和跟踪段数的对比

附图5为将NCLT数据2012-06-15序列，输入本发明进行定位于稀疏地图构建的结果总览，在图中可以清晰的看出车辆行驶的轨迹(连续红色点形成的轨迹线)与周围地物的轮廓(黑色杂乱点)。

2.宽基线模式

为了进一步验证本发明方法的鲁棒性，对图像序列进行了抽帧，以模拟地面移动测量中的宽基线场景。因为数据集平均基线长度约为0.2m，所以每隔4张选择一张影像，使得连续两帧影像的基线长度达到1m。由于纯单目效果较差，因此在本实验当中，只对基于全景视觉的本发明方法，PAN-SLAM和OpenVSLAM进行比较，统计了平移和旋转的RMSE，以及跟踪帧数和段数。

对比表2和表1中可以看出，PAN-SLAM在2012-06-15中跟踪失败总数达到了9次，同时定位精度也出现了明显的下降。而本发明方法则不会发生跟踪失败的情况，并且从精度上分析，仅在2012-06-15的第三段数据当中，精度有着明显的下降(从0.815m/0.819°)下降到1.860m/0.882°)，而其余三段精度均只发生微弱下降。附图6展示了本发明方法，PAN-SLAM和OpenVSLAM轨迹与地面真值对比。可以看出PAN-SLAM和OpenVSLAM轨迹在多处与真值相比出现明显偏移，并且跟踪失败(“X”)也频繁出现，而本发明方法轨迹与真值较为吻合，并且没有出现跟踪失败的情况。

表2抽帧实验结果对比

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (10)

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个轮速计测量值之间的时间间隔。

5.如权利要求1所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：系统初始化模块的具体实现方式如下，

步骤2.1，在纯全景视觉初始化中，先对两帧影像进行特征点提取与匹配，若匹配点数小于n1，则重新进行步骤2.1，否则进行后续步骤；根据全景球面对极约束关系以及RANSAC方法求解本质矩阵E恢复姿态信息，包括旋转R和平移t，并三角化初始化地图，即为初始的局部地图，最后通过调用开源优化库G2O对旋转、平移以及初始化地图进行优化；若优化后的地图点大于n2个则认为两帧影像三角化成功，否则为失败，重新接受影像进行步骤2.1；并保证至少n3个关键帧进入，以建立纯全景视觉的初始化地图，为后续轮速计以及IMU的初始化做准备；若不够n3帧，则进入跟踪模块，否则进入步骤2.2；

步骤2.2，全景视觉-轮速计对齐；根据两个关键帧之间轮速计位移预积分Δp_i,j与视觉位移

的关系，求解出尺度s，并将视觉恢复到绝对尺度之下，具体求方法如下公式：

式中p_OS为全景相机与轮速计之间的外参，

和

为从世界坐标系到第i和j帧所在的轮速计坐标系的旋转矩阵，

和

从第i和j帧所在全景相机坐标系到世界坐标系的平移矩阵；

若求解的尺度小于m，则判断尺度求解失败，重新进行步骤2.2，否则进入步骤2.3；

步骤2.3，全景视觉-IMU对齐，这一步的目标是获取IMU的初始化参数，初始化参数为：

其中R_Wg∈SO3为重力在世界坐标系中的方向，b_g,b_a分别为陀螺仪与加速度计的偏置，v^0:k _WI为从第0个关键帧到第k个关键帧恢复到绝对尺度下的速度；

最后，求解完成后利用求解得到的R_Wg将整个坐标系旋转至与重力方向对齐；

步骤2.4，全景视觉-IMU-轮速计联合平差；将步骤2.2得到的恢复到绝对尺度下的全景视觉信息、IMU预积分、轮速计预积分加入到同一个优化函数中，对在步骤2.2中绝对尺度下计算的旋转、平移和初始化地图，以及步骤2.3中得到的偏置和速度进行优化，利用优化后的IMU参数，即陀螺仪与加速度计的偏置以及速度，对IMU预积分项进行更新。

6.如权利要求5所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统，其特征在于：跟踪模块的具体实现方式如下；

步骤3.1，若当前总帧数小于n3帧，采用跟踪恒速模型来估计当前帧的位姿，当恒速模型失败后，则采用跟踪关键帧模型来进行纯视觉跟踪，得到当前帧的位姿信息；

步骤3.2，利用IMU测量值预测推断当前帧的位姿，在前一帧位姿的基础上，对当前帧与前一帧之间的IMU信息进行积分，得到当前帧位姿的初始值；

步骤3.3，得到当前帧位姿的初始值后，利用该值对局部地图中的三维点投影，与当前帧所提取的特征点进行匹配，并进行联合平差；

步骤3.4，选取关键帧，判断当前帧是否为关键帧，若不是关键帧，则继续接收新的数据重复执行传感器测量数据预处理模块、系统初始化模块和跟踪模块，若被判断为关键帧，则将关键帧插入局部地图构建模块；判断是否为关键帧的具体条件如下，m1-m5均为常数：

(1)若初始化模块中不足m1帧时，只要当前帧与上一帧的时间间隔大于m2秒，则被判定关键帧；

(2)若当前帧与上一关键帧相隔超过相机帧率数，则被判定为关键帧；

(3)若当前帧跟踪到的地图点数小于上一帧的m3％，则判定为关键帧；

(4)若当前帧与上一帧的时间间隔大于m4秒，则判定为关键帧；

(5)若当前帧匹配到的地图点数小于m5个，则判定为关键帧。

7.一种基于IMU和轮速计紧耦合的鲁棒全景SLAM方法，其特征在于：通过联合平差方法，将多镜头组合式全景相机、惯性导航单元IMU和轮速计三种传感器获取的数据进行融合，具体包括如下步骤：

步骤1，对传入的传感器的原始数据进行处理，包括对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面；IMU预积分的计算，以及轮速计预积分的计算；

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

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

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

8.如权利要求7所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM方法，其特征在于：步骤1中对多镜头组合式全景相机获取的影像进行特征点提取，并将特征点投影至全景球面的具体实现方式如下；

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

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

9.如权利要求7所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM方法，其特征在于：步骤1中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测量值之间的时间间隔。

10.如权利要求7所述的一种基于IMU和轮速计紧耦合的鲁棒全景SLAM方法，其特征在于：步骤1中轮速计预积分的计算的具体实现方式如下；

基于双轮差速获得轮速计预积分，右轮线速度为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个轮速计测量值之间的时间间隔。

Images