具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本申请提供了一种融合MSCKF和图优化的视觉SLAM定位方法，以解决现有技术中无法同时保证运算效率与运算精度的问题。

可选地，为了实现本申请公开的融合MSCKF和图优化的视觉SLAM定位方法，本申请实施例提供了一种融合MSCKF和图优化的视觉SLAM定位系统，如图1所示，具体包括：摄像设备101、惯性传感器102、前端103以及后端104。

其中，前端103中包含有滤波器，具体可以为(Multi-State Constraint KalmanFilter，MSCKS)多状态约束下的Kalman滤波器。主要用于对摄像设备101拍摄的图像，即视觉特征以及惯性传感器(Inertial Measurement Unit，IMU)102采集到的惯性数据进行处理，从而对系统状态进行预测和更新，并实时输出以及输入后端，从而紧耦合融合视觉特征和惯性数据。

后端104中包括有光束法平差(Bundle Adjustment，BA)模块1041以及回路闭合检测模块1042。回环闭合检测模块1042主要用于利用视觉特征进行回环检测，将回环检测后的回环检测信息反馈给BA模型，其中，回环检测信息为回环检测得到的视觉特征。BA模块1041主要根据回环检测信息进行回环矫正，并利用历史全局信息对滤波器进行全局优化。

可选地，还可以进一步包括有地图库，用于存储关键帧的信息、运动的关键信息、系数点云图等，在数据处理过程中产生的数据。

基于上述提供的及时定位系统，本申请另一实施例提供的一种融合MSCKF和图优化的视觉SLAM定位方法，如图2所示，具体包括以下步骤：

S201、实时获取当前图像以及当前惯性数据。

首先需要说明的是，本申请指的同时定位指的是同时定位与地图构建(simultaneous localization and mapping，SLAM)，也称为并发建图与定位(ConcurrentMapping and Localization，CML)。

其中，惯性数据由惯性传感器测量得到，具体包括加速度和角速度。

S202、每获取到一帧当前图像，则基于当前的惯性数据，跟踪得到当前图像上的多个特征点。

具体的，先通过对惯性数据的角速度的积分预测，然后在利用光流法跟踪上一帧获取到的图像上的所有的特征点在当前图像上的二维坐标。若在当前图像上跟踪到的特征点的数据量未满足预设要求，则可以在当前图像上提取新的特征点。

S203、将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及多个特征点的信息。

其中，关键帧的信息在当前图像被确定为关键帧时输出。

具体的，滤波器根据各个特征点的信息以及惯性数据，对状态变量进行预测和更新。其中，中滤波器的状态变量由一组长度为N的帧信息组成，其包括每一帧图像的位置和姿态信息以及相机的内参外参、惯性数据的状态变量。

需要说明的是，在执行步骤S203时，滤波器会判断当前图像是否为关键帧，若判断出当前图像为关键帧，则输出当前位姿、多特征点的信息以及关键帧的信息，即被确定为关键帧的当前图像的信息。若判断出当前图像不属于关键帧，说明可以忽略当前图像，所以可以仅输出当前位姿。

具体的，步骤S203的一种具体实施方式，如图3所示，具体包括以下步骤：

S301、将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，得到当前位姿。

S302、根据跟踪帧数是否小于预设帧数，将多个特征点划分为两个类别。

其中，特征点的跟踪帧数指的是一个特征点被连续跟踪到的帧数，即存在该特征点，并且是连续获取到的图像的数量。

由于，跟踪帧数会影响特征点的处理方式，所以根据跟踪帧数，将特征点划分为跟踪帧数不小于预设帧数的特征点以及跟踪帧数小于预设帧数的特征点。

S303、基于当前图像的多个预设类型的信息，确定当前图像是否为关键帧。

其中，预设类型的信息中最主要的为当前图像距离上一帧被确定为关键帧的图像的时间间隔。当然，还可以包括有位移长短、图像上新的特征点的数量等。从而更具这多个预设类型的信息，确定当前图像对于同时定位而言，是否比较关键，从而判断是否将其确定为关键帧。

需要说明的是，若执行步骤S303将当前图像确定为关键帧，即当前图像属于关键帧，则执行步骤S304。若执行步骤S303确定当前图像不属于关键帧，则执行步骤S305。

S304、输出当前位姿。

S305、输出当前位姿、所述关键帧的信息以及两类特征点的信息。

S204、从关键帧队列中提取出目标关键帧；目标关键帧为关键帧队列中获取时间距离当前最长的关键帧。

其中，关键帧队列包括所有待处理的关键帧。

需要说明的是，在本申请实施例中，步骤S201～S203实时不断在进行的，从而可以不断地更新位姿，实现同时定位。而由于后续对关键帧进行处理以及回环检测过程能对前端进行优化，保证同时定位的精度，但是相对比较费时。所以可选地，后续的步骤可以选择性地执行，即可以在预设的第一唤醒时间时执行。所以，本申请实施例中，将步骤S203确定出的每一帧关键帧统一添加到关键帧队列中以备后续处理。

S205、将关键帧队列中的多帧关键帧中与目标关键帧共视的特征点，还原到关键帧列表中获取时间最新的关键帧中。

具体的，从关键帧队列中查找出与目标关键帧存在共视关系的其他关键帧，即查找出与目标关键帧中能看到同一场景的其他关键帧。然后通过共视关系，将共视的特征点，还原到关键帧列表中获取时间最新的关键帧中。

S206、从关键帧队列的各个关键帧上的特征点中筛选出多个地图点。

具体的，步骤S3206的一种具体实施方式为：对于关键帧队列的任一关键帧上的任一特征点，若任一特征点属于第一类别，且至少在关键帧队列中的两帧关键帧上，或任一特征点属于第二类别，且具有新的共视关系，则将任一特征点确定为地图点。其中，第一类别的特征点指代跟踪帧数不小于预设帧数的特征点，第二列表指代跟踪帧数小于预设帧数的特征点，从而可以有效地保证选取的地图点的数量和质量。

S207、针对目标关键帧进行回环检测，并判断回环检测是否成功。

可选地，对于回环检测也可以是，选择性的开启检测。可以基于BOW进行回环检测。具体的，检测方式同开源算法ORB-SLAM3相同，因此具体不再赘述。

其中，若判断出回环检测失败，则执行步骤S208。若判断出回环检测成功，则执行步骤S208。

S208、将多个地图点、目标关键帧及其共视关系，更新到地图库中。

可选地，可以是在进行多次回环检测，得到数量相对较多的回环帧后，再执行步骤S209进行优化，从而可以避免频繁的优化计算。

S209、将相关关键帧的信息输入全局光束平差法模型中，通过全局光束平差法模型对相关关键帧以及回环地图点进行优化；相关关键帧包括回环检测到的回环帧以及目标关键帧，回环地图点为回环检测到的地图点。

具体的，通过对相关关键帧的新以及地图点构建成本函数(cost function)，来进行迭代，优化相关关键帧以及地图点的状态变量。

S210、将优化后的相关关键帧以及回环地图点更新至地图库，以及返回给滤波器进行状态更新。

在优化相关关键帧以及回环地图点后，就可以对滤波器进行更新，从而保证同时定位的精确度。

可选地，本申请另一实施例中，步骤S210的具体实施方式，如图4所示，具体包括以下步骤：

S401、将优化后的相关关键帧以及回环地图点更新至地图库中。

S402、判断滤波器的滑动窗口中是否存在相关关键帧以及回环地图点。

其中，若判断出判断滤波器的滑动窗口中存在相关关键帧以及回环地图点，则对执行步骤S403。若判断出所述滤波器的滑动窗口中不存在所述相关关键帧或所述回环地图点，则执行步骤S404。

S403、将滤波器中存在的相关关键帧以及回环地图点，相应地更新为优化后的相关关键帧或优化后的回环地图点。

S404、将当前的惯性数据更新至滤波器中。

本申请实施例提供的一种融合MSCKF和图优化的视觉SLAM定位方法，实时获取当前的图像以及惯性数据，并在每获取到一帧当前图像时，基于当前的惯性数据，跟踪得到当前图像上的多个特征点，然后将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及所述多个特征点的信息。从而基于滤波保证同时定位的效率。并当达到第一唤醒时间时，从关键帧队列中提取出目标关键帧，从关键帧队列中查找出与目标关键帧具有共视关系的多帧关键帧，将多帧关键帧与所述目标关键帧共视的特征点，还原到关键帧列表中获取时间最新的关键帧中。从关键帧队列的各个关键帧上的特征点中筛选出多个地图点，并针对目标关键帧进行回环检测。若回环检测失败，则将多个地图点、目标关键帧及其共视关系，更新到地图库中。若回环检测成功，则将相关关键帧的信息输入全局光束平差法模型中，通过全局光束平差法模型对相关关键帧以及回环地图点进行优化，最后将优化后的相关关键帧以及回环地图点更新所述地图库，以及更新滤波器，从而定时基于非线性算法对梯度进行优化，从而又可以保证定位的精度。所以，通过将滤波算法与非线性优化有效地融合，进行同时定位，既保证了定位的高效性，又保证了定位的精度。

根据上述实施例提供的融合MSCKF和图优化的视觉SLAM定位方法，可见本申请提供的方法主要分为三个部分，第一部分为对当前图像以及惯性信息进行实时处理，从而实现同时定位的，第二部分为对关键帧的信息以及特征点的信息进行处理，并对关键帧进行回环检测，最后一部分则为在找到足够的回环帧和共视帧后，对整个地图的优化。本申请实施例提供的同时定位系统，可以分为三个功能区以及一个地图库。其中，每个功能通过一个功能通过一个相应的线程来实现。具体如图5所示，包括有用于实现第一部分的即视觉惯性里程计(visual-inertial odometry，VIO)线程，该线程为MSCKF滤波算法所在线程，负责快速处理输入的当前图像和惯性数据，实时输出位姿。用于实现第二部分功能的后处理线程，负责对VIO线程的输入的信息进行处理，以及回环检测。用于实现第三部部分功能的全局光束法平差线程，是非线性优化算法所在的线程，负责在后处理线程找到足够的回环帧和共视帧后，启动对整个地图进行优化。而地图库用于存储关键帧、地图点、关键帧间的共视关系以及从属关系等。其中，共视关系通过共视图存储；从属关系通过拓展树存储。

本申请另一实施例提供了一种融合MSCKF和图优化的视觉SLAM定位装置，如图6所示，包括以下单元：

采集单元601，用于实时获取当前的图像以及惯性数据。

跟踪单元602，用于每获取到一帧当前图像，则基于当前的惯性数据，跟踪得到当前图像上的多个特征点。

定位单元603，用于将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，并输出当前位姿、关键帧的信息以及多个特征点的信息。

其中，关键帧的信息在当前图像被确定为关键帧时输出。

提取单元604，用于从关键帧队列中提取出目标关键帧。

其中，关键帧队列包括所有待处理的关键帧。目标关键帧为关键帧队列中获取时间距离当前最长的关键帧。

还原单元605，用于将关键帧队列中的多帧关键帧中与目标关键帧共视的特征点，还原到关键帧列表中获取时间最新的关键帧中。

筛选单元606，用于从关键帧队列的各个关键帧上的特征点中筛选出多个地图点。

回环检测单元607，用于针对目标关键帧进行回环检测。

第一更新单元608，用于在回环检测失败时，将多个地图点、目标关键帧及其共视关系，更新到地图库中。

优化单元609，用于在回环检测成功时，将相关关键帧的信息输入全局光束平差法模型中，通过全局光束平差法模型对相关关键帧以及回环地图点进行优化。

其中，相关关键帧包括回环检测到的回环帧以及目标关键帧。回环地图点为回环检测到的地图点。

第二更新单元610，用于将优化后的相关关键帧以及回环地图点更新至地图库，以及反馈给滤波器进行状态更新。

可选地，本申请另一实施例提供的融合MSCKF和图优化的视觉SLAM定位装置中的定位单元，如图7所示，包括以下单元：

状态更新单元701，用于将多个特征点的信息以及惯性数据输入滤波器中，进行滤波器预测和更新，得到当前位姿。

匹配单元702，用于根据跟踪帧数是否小于预设帧数，将多个特征点划分为两个类别，以及基于当前图像的多个预设类型的信息，确定当前图像是否为关键帧。

第一输出单元703，用于在当前图像不属于关键帧时，输出当前位姿。

第二输出单元704，用于在当前图像属于关键帧时，输出当前位姿、关键帧的信息以及两类特征点的信息。

可选地，本申请另一实施例提供的融合MSCKF和图优化的视觉SLAM定位装置中的筛选单元，包括：

筛选子单元，用于对于关键帧队列的任一关键帧上的任一特征点，若任一特征点属于第一类别，且至少在关键帧队列中的两帧关键帧上，或任一特征点属于第二类别，且具有新的共视关系，则将任一特征点确定为地图点。

其中，第一类别的特征点指代跟踪帧数不小于预设帧数的特征点，第二列表指代跟踪帧数小于预设帧数的特征点。

可选地，本申请另一实施例提供的融合MSCKF和图优化的视觉SLAM定位装置中的第二更新单元，如图8所示，包括以下单元：

地图更新单元801，用于将优化后的相关关键帧以及回环地图点更新至地图库中。

判断单元802，用于判断滤波器的滑动窗口中是否存在相关关键帧以及回环地图点。

第一前端更新单元803，用于在判断单元判断出滤波器的滑动窗口中存在相关关键帧或回环地图点时，将滤波器中存在的相关关键帧以及回环地图点，相应地更新为优化后的相关关键帧或优化后的回环地图点。

第二前端更新单元804，用于在判断单元判断出滤波器的滑动窗口中不存在相关关键帧或回环地图点时，将当前的惯性数据更新至滤波器中。

需要说明的是，本申请上述实施例提供的各个单元为上述实施例中提供的融合MSCKF和图优化的视觉SLAM定位系统中的各个模块中的单元，并且上述各个单元的具体工作过程可相应的参考上述方法实施例中的相应的步骤的实施过程，此处不再赘述。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。