一种基于分层架构的视觉SLAM后端优化方法

摘要

本发明涉及一种基于分层架构的视觉SLAM后端优化方法，其特征在于，包括：建立底层优化层，对关键帧进行优化，得到当前关键帧的最佳位置；建立高层优化层，对关联帧单元内的关键帧进行优化，得到关联帧单元的最佳位置。本发明通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差，减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象，提高了SLAM定位结果和地图的准确性和稳定性，通过将本发明方法和目前效果最好的SLAM方法得到的定位结果进行比较，证明了本发明的有效性。

说明书

技术领域

本发明涉及导航技术领域，尤其涉及一种基于分层架构的视觉SLAM后端优化方法。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)最早于1988年提出，最开始的时候用于描述机器人在未知环境的未知地点中同步的进行地图构建和自身定位。机器人通过传感器获取的环境数据对位置环境进行地图创建，然后根据当前观测到的环境特征与地图中的特征进行匹配从而进行自身的定位。根据使用的传感器不同，SLAM主要可以分为视觉SLAM和激光SLAM。由于图像具有丰富的颜色信息和纹理信息，再加上近年来计算能力的提升，基于图像的视觉SLAM成为研究热点。

在SLAM算法问题中，可以分为前端和后端两个部分。前端的主要作用是计算帧与帧间的相对关系。包括特征点提取、特征点的匹配、利用匹配点计算位姿。后端的作用主要是对前端的输出结果进行优化，得到最优的位姿估计。传统的视觉SLAM方法中，后端优化一般分为两部分，一个是基于当前时刻图像帧的局部优化，另一个是当运动载体回到运动初始点或者行驶过的路径时进行回环检测，利用回环检测结果，从回环起点到终点进行一个整体优化。

传统SLAM方法的缺点在于，局部小窗口的优化对定位误差的矫正效果有限，当运动路线较长时，局部小窗的优化方式仍然避免不了大的位姿漂移。闭环整体优化可以大幅度矫正这种漂移，然而当SLAM技术运用在车辆上时，由于车辆行驶路线的不确定性，在行驶过程中未必能够形成闭环，况且由于车辆行驶路线较长，局部误差累计到足够大时，闭环检测对误差的矫正能力也会受限。

发明内容

鉴于上述的分析，本发明旨在提供一种基于分层架构的视觉SLAM后端优化方法，用以解决现有技术存在位姿漂移、传统SLAM优化方法依赖闭环检测的问题。

本发明的目的主要是通过以下技术方案实现的：

一种基于分层架构的视觉SLAM后端优化方法，包括以下步骤：

步骤S1：建立底层优化层，对关键帧进行优化，得到当前关键帧的最佳位置；

步骤S2：建立高层优化层，对关联帧单元进行优化，得到关联帧单元的最佳位置。

所述步骤S1包括以下步骤：

步骤S101：当生成新的关键帧时，将历史关键帧中与当前关键帧存在连接关系的关键帧作为待优化关键帧，并放入待优化关键帧列表，将待优化关键帧对应的所有地图点作为待优化地图点；

步骤S102：取出所有能够观察到待优化地图点的关键帧，将待优化关键帧以外的其他关键帧全部固定，作为优化参考；

步骤S103：使用图优化的方法，将待优化关键帧和待优化地图点作为优化顶点，所有待优化地图点和能观察到待优化地图点的关键帧的投影关系作为优化边，进行优化迭代，得到最终的优化结果，并将优化过的关键帧设置标记；

步骤S104：从关键帧缓存列表起点开始，取出未被标记的关键帧，进行优化。

所述图优化的方法是通过G2O的开源c++框架来建立的，G2O中定义好了图优化的优化步骤和优化流程，用户在使用G2O时，只需建立误差函数以及优化边。

所建立的误差函数为待优化关键帧对应的三维点投影在图像平面上时，与对应的图像中的二维特征点的位置的误差，称为重投影误差函数，如下式所示：

式中为图像中的特征点二维坐标；K为相机内参矩阵；R_k为待优化关键帧相对于世界坐标系的三维旋转矩阵；t_k为待优化关键帧相对于世界坐标系的三维平移向量；Pⁱ为待优化空间三维点坐标。

进一步地，从关键帧缓存列表中的第一帧开始，逐帧检查关键帧是否被标记，将未被标记的关键帧取出，放入待优化关键帧列表；

在待优化关键帧列表中，从未被标记帧开始向前数两帧，向后数两帧，这五帧固定第一帧和最后一帧，优化中间的三帧；倘若最后一帧也为未标记帧，则放弃该帧，固定第一帧，优化后三帧。

所述步骤S2包括以下步骤：

步骤S201：将一定数量的关键帧划分为一个关联帧单元，确定关联帧单元的大小；

步骤S202：通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系；

步骤S203：将关联帧单元的单元位姿和地图点位姿分别作为优化顶点，通过图优化的方法对关联帧单元进行优化。

进一步地，取一个尺寸固定大小的窗口，窗口不断地在待优化关键帧列表中滑动，取出窗口内的关联帧单元进行优化，窗口滑动的间隔有一定的重叠。

进一步地，计算每一个关联帧单元内关键帧相对于关联帧单元的相对位姿R_kr、t_kr，并使其保持不变，进行优化时，将关联帧单元位姿和单元间共视点位姿作为优化顶点，通过关键帧与三维点的位置关系，构建关联帧单元与三维点的位置关系，得到误差函数。

关联帧单元位姿与关联帧单元内关键帧位姿的关系如下式所示：

其中T_k为关联帧单元内关键帧位姿，T_r为关联帧单元位姿，T_kr代表单元内关键帧相对于单元的六自由度位姿变换，R_r、t_r为待优化单元位姿；T表示关键帧相对于世界坐标系的六自由度三维变换矩阵，包括旋转和平移。

关联帧单元的误差函数为：

通过图优化的方法求出关联帧单元的误差函数最小时对应的R_r和t_r的值，得到关联帧单元的最佳位置。

本发明有益效果如下：

本发明提出了一种基于通用分层架构的视觉SLAM优化方法，从多个层次多个尺度对SLAM定位结果进行在线优化，且计算量不会随着优化层的增加而显著增加，相较于传统SLAM优化方法，本方法不依赖回环检测的方式来进行偏差矫正，即使没有形成闭环也可以显著的优化定位结果。同时本方法也支持回环检测，在本方法的基础上进行回环检测会得到更好的定位效果，提高了SLAM定位结果的鲁棒性，通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差，减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象，提高了SLAM定位结果和地图的准确性和稳定性。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1分层优化流程图；

图2优化单元及优化窗口尺寸示意图；

图3是单元内外点示意图；

图4是底层优化层优化窗口示意图；

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本发明实施例提供了一种基于分层架构的视觉SLAM后端优化方法，该方法流程图如图1所示，提出的后端分层优化架构总体上分为底层和上层，底层的任务是将当前关键帧与临近的关键帧和地图点建立连接，通过最小化重投影误差来得到当前关键帧的最佳位置；上层的任务是将固定数量的关键帧绑定作为一个独立的局部关联帧单元，将多个局部关联帧单元建立连接，通过最小化重投影误差来得到关联帧单元的最佳位置。

将程序运行时的第一个图像帧作为关键帧，在以后的关键帧选取中，若某时刻的图像帧与上一个关键帧所提取到的特征点的重复率小于一定阈值(在本实施例中为0.25～0.3)，则把该时刻的图像帧作为关键帧。

在当前关键帧中可以提取若干二维特征点，每个二维特征点都在三维世界中对应一个三维地图点，这些三维地图点也称为当前关键帧对应的三维地图点。若当前关键帧所对应的三维地图点与某一个关键帧所对应的三维地图点中存在重合的地图点，则认为当前帧与该关键帧存在连接关系。所述将当前关键帧与临近的关键帧和地图点建立连接，是指找出与当前关键帧存在连接关系的所有关键帧，与当前帧一起通过最小化重投影误差的方式来得到当前关键帧的最佳位置。

具体地，所述基于分层架构的视觉SLAM后端优化方法包括以下步骤：

步骤S1：建立底层优化层，对关键帧进行优化，得到当前关键帧的最佳位置。

在车辆行驶的过程中，SLAM前端会不断生成新的关键帧，从所生成的第一帧关键帧一直到当前关键帧，按照时间序列存入关键帧缓存列表，提取出与当前关键帧存在连接关系的关键帧和地图点，通过固定一部分关键帧的位姿来优化其余的关键帧和地图点，将被优化过的关键帧做上标记。

具体地，所述步骤S1包括以下步骤：

步骤S101：当生成新的关键帧时，将历史关键帧中与当前关键帧存在连接关系的关键帧作为待优化关键帧，并放入待优化关键帧列表，然后将待优化关键帧对应的所有地图点作为待优化地图点。

步骤S102：取出所有能够观察到待优化地图点的关键帧，将待优化关键帧以外的其他关键帧全部固定，作为优化参考。

步骤S103：使用图优化的方法，将待优化关键帧和待优化地图点作为优化顶点，所有待优化地图点和能观察到待优化地图点的关键帧的投影关系作为优化边，进行一定次数优化迭代，得到最终的优化结果，并将优化过的关键帧设置标记。

所述投影关系是指本发明领域中通用的透视投影相机模型所建立的投影关系；

理论上，优化迭代的次数越多，优化效果越好，但是耗费时间也越长，所以优化过程中需要平衡优化精度和消耗时间，经过试验，优化迭代的次数为10-20时能够满足要求；

所述图优化的方法是通过G2O的开源c++框架来建立的，G2O中定义好了图优化的优化步骤和优化流程，用户在使用G2O时，只需建立误差函数以及优化边；

在本实施例中，所建立的误差函数即为待优化关键帧对应的三维点投影在图像平面上时，与对应的图像中的二维特征点的位置的误差，称为重投影误差函数，如下式所示：

式中为图像中的特征点二维坐标；K为相机内参矩阵；R_k为待优化关键帧相对于世界坐标系的三维旋转矩阵，是一个3*3的正交矩阵，表示关键帧相对于世界坐标系的三自由度旋转；t_k为待优化关键帧相对于世界坐标系的三维平移向量，是一个3*1的向量，表示关键帧相对于世界坐标系的三自由度平移；Pⁱ为待优化空间三维点坐标。

步骤S104：从关键帧缓存列表起点开始，取出未被标记的关键帧，进行优化。

由于车辆的不断向前行驶，会不断的产生新的关键帧，而步骤S101～S103中的优化过程需要一定时间才能完成，因此在优化的过程中产生的关键帧有可能会被跳过，所以从关键帧缓存列表起点开始，将未被标记的关键帧取出，进行优化；

具体地，从关键帧缓存列表中的第一帧开始，逐帧检查关键帧是否被标记，将未被标记的关键帧取出，放入待优化关键帧列表，如图2所示，在待优化关键帧列表中，从未被标记帧开始向前数两帧，向后数两帧，这五帧固定第一帧和最后一帧，优化中间的三帧；倘若最后一帧也为未标记帧，则放弃该帧，固定第一帧，优化后三帧；优化过程同步骤S103相同。

步骤S2：建立高层优化层，对关联帧单元进行优化，得到关联帧单元的最佳位置。

如图3所示，高层优化层随着程序的运行不断地创建新的高层优化层，每层高层优化层中以一定数量的关键帧绑定作为一个单独的优化单元，作为关联帧单元，随着层数的增加，关联帧单元内的关键帧的数量也在不断地增加。

具体地，所述步骤S2包括以下子步骤：

步骤S201：根据一定数量规律将一定数量的关键帧划分为一个关联帧单元，确定关联帧单元的大小；

所述一定数量规律是指关联帧单元中关键帧数量n与所在层数l呈指数关系，n＝2^l+1，采用指数映射的关系可以避免相邻高层优化层中关联帧单元内待优化关键帧的数量过于接近，从而避免产生因计算资源分布不均而导致的优化阻滞的现象；

本实施例中，关联帧单元中关键帧的数量为4，根据实验效果得知，此时的优化效果较好。

步骤S202：通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系；

如图4所示，将只有关联帧单元内的关键帧可以观察到的地图点称为单元内点，将相邻关联帧单元间的关键帧所共同观察到的地图点称为单元间共视点；

所述关联帧单元的单元位姿用处于关联帧单元中间位置的关键帧的位姿来表示，单元位姿与关联帧单元内的关键帧的位姿存在相对位置关系，关联帧单元内的关键帧与单元间共视点之间存在投影关系，因此，通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系；

步骤S203：将关联帧单元的单元位姿和地图点位姿分别作为优化顶点，通过图优化的方法对关联帧单元进行优化；

在进行优化时，关联帧单元作为整体进行优化，关联帧单元位姿发生改变后，单元内点、单元内关键帧位姿都要随之发生改变，取一个尺寸固定大小的窗口，窗口不断地在待优化关键帧列表中滑动，取出窗口内的关联帧单元进行优化，为了使优化后的关联帧单元位姿和地图点保持连续而不发生断层的现象，窗口滑动的间隔有一定的重叠；

由于单元间共视点的数量只由相邻关联帧单元临近的几帧关键帧决定，数量基本固定，不随着关联帧单元内关键帧数量的增加而增加，并且每次优化的窗口大小也是一定的，所以优化时的计算量不会随着关联帧单元内关键帧的增加而增加，随着优化层的层数增加，优化的计算量基本保持不变；

计算每一个关联帧单元内关键帧相对于关联帧单元的相对位姿R_kr、t_kr，并使其保持不变，进行优化时，将关联帧单元位姿和单元间共视点位姿作为优化顶点，通过关键帧与三维点的位置关系，构建关联帧单元与三维点的位置关系，得到误差函数。关联帧单元位姿与关联帧单元内关键帧位姿的关系如下式所示：

其中T_k为关联帧单元内关键帧位姿，T_r为关联帧单元位姿，T_kr代表单元内关键帧相对于单元的六自由度位姿变换，R_r、t_r为待优化单元位姿。

T为三维变换矩阵，是一个4*4的矩阵，表示关键帧相对于世界坐标系的六自由度变换，包括旋转和平移。其左上角的3*3个元素由R构成，右上角的3*1个元素由t构成，第四行的四个元素中前三个为0，最后一个为1，最终可以写成的形式。

将式(2-2)带入(2-1)中，可得：

式(2-3)为关联帧单元内一个关键帧的重投影误差函数，因为优化的是一个整体单元，需要将单元内所有关键帧的重投影误差考虑在内，因此关联帧单元总的误差函数为：

通过图优化的方法求出误差函数(2-4)最小时对应的变量值，即R_r，t_r，得到关联帧单元的最佳位置。

综上所述，本发明实施例提供了一种基于分层架构的视觉SLAM后端优化方法，通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差，减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象，提高了SLAM定位结果和地图的准确性和稳定性，通过将本发明方法和目前效果最好的SLAM方法得到的定位结果进行比较，证明了本发明的有效性。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。