CN115619851A - 基于锚点的vslam后端优化方法、装置、介质、设备和车辆 - Google Patents

Abstract

本公开涉及基于锚点的VSLAM后端优化方法、装置、介质、设备和车辆，该方法包括：获取多个对齐锚点和相机采集的多个图像；所述对齐锚点的时间戳与所述图像的时间戳分别对应；所述对齐锚点为采用其他定位源生成的位姿数据；基于所述图像，生成地图点和关键帧；其中，每一个所述关键帧都对应有对齐锚点；若满足局部后端优化条件，则基于关键帧对应的对齐锚点，进行VSLAM后端优化。由此，本公开提供的技术方案，将对齐锚点引入VSLAM后端优化的步骤中，以位姿精度较高的对齐锚点作为对应的关键帧的先验位姿真值，在VSLAM后端优化步骤中添加了关键帧与对齐锚点之间的位姿约束，从而有利于提高后端优化精度，进而提升VSLAM建图精度，改善累计误差较大且持续增大的问题。

Description

基于锚点的VSLAM后端优化方法、装置、介质、设备和车辆

技术领域

本公开涉及计算机视觉技术领域，尤其涉及一种基于锚点的VSLAM后端优化方法、装置、介质、设备和车辆。

背景技术

计算机视觉应用于无人驾驶场景或辅助驾驶场景中时，可应用到定位技术中。目前主流的定位技术通常是多传感器融合的技术，参与融合的定位源主要包括全球定位***(Global Positioning System，GPS)定位、基于激光雷达的LSLAM定位、基于纯视觉的VSLAM以及视觉配合惯性测量单元(Inertial measurement unit，IMU)的视觉惯性里程计(visual-inertial odometry，VIO)等。其中，VSLAM经过多年的发展，已经形成了较为成熟的框架，如图1所示。

相关技术中，应用于VSLAM的传感器主要分为单目相机、双目相机以及RGB-D相机。RGB-D相机可以获得深度信息，但是由于相关的硬件能力限制，RGB-D相机能感知的深度范围有限，很少用在无人驾驶和辅助驾驶场景中。单目相机和双目相机目前是无人驾驶领域VSLAM技术的常用传感器，其对应的定位技术分别称为单目VSLAM和双目VSLAM。在单目VSLAM和双目VSLAM中，通常包括前端生成局部地图以及后端优化的步骤，以期减小累计误差，得到全局一致的轨迹和地图。目前后端优化通常基于前端生成的局部地图所对应的关键帧和地图点进行，利用关键帧与地图点之间的约束、关键帧之间的相互约束以及回环信息，采用迭代的方式不断减小累计误差。但是由于噪声的存在，后端优化的精度较差，仍存在较大的累计误差，且累计误差会随地图尺寸的增大而增大。

发明内容

为了解决上述技术问题或者至少部分地解决上述技术问题，本公开提供了一种基于锚点的VSLAM后端优化方法、装置、介质、设备和车辆。

本公开提供了一种基于锚点的VSLAM后端优化方法，包括：

获取多个对齐锚点和相机采集的多个图像；所述对齐锚点的时间戳与所述图像的时间戳分别对应；所述对齐锚点为采用其他定位源生成的位姿数据；

基于所述图像，生成地图点和关键帧；其中，每一个所述关键帧都对应有对齐锚点；

若满足局部后端优化条件，则基于关键帧对应的对齐锚点，进行VSLAM后端优化。

可选地，所述局部后端优化条件包括第一条件和第二条件中的至少一个；

所述第一条件，包括：新增关键帧的数量大于所述预设数量阈值；所述新增关键帧的数量为距离上一次局部后端优化，已经新增的关键帧的数量；

所述第二条件，包括：位姿误差大于所述预设误差阈值；所述位姿误差为基于当前的关键帧位姿和相对应的锚点位姿确定的误差。

可选地，所述进行VSLAM后端优化，包括：

基于所述对齐锚点，进行局部后端优化；

判断是否所有所述图像均处理完毕；

若未处理完毕，则返回所述基于所述图像，生成地图点和关键帧；

若均处理完毕，则基于所述对齐锚点，进行全局后端优化。

可选地，所述进行局部后端优化，或者所述进行全局后端优化，包括：

采用图优化的方式，优化重投影误差及关键帧与所对应的对齐锚点之间的误差。

可选地，所述进行局部后端优化，或者所述进行全局后端优化，包括：

确定VSLAM观测模型对应的代价函数；

通过迭代优化，使所述代价函数趋于最小；

其中，所述代价函数为：

其中，e_ij代表累计代价，x_i代表相机在当前图像帧对应的位置，y_i代表对应的地图点，z_ij代表针对该地图点由相机采集到的观测数据，ax_i代表对齐锚点对应的位置。

可选地，所述位姿误差包括位置误差、偏航角误差、俯仰角误差以及横滚角误差中的至少一个；所述位姿误差采用如下公式计算：

E_yaw＝(yaw_i-yaw_ai)

E_pitch＝(pitch_i-pitch_ai)

E_roll＝(roll_i-roll_ai)

其中，(x_i，y_i，z_i，yaw_i，pitch_i，roll_i)代表当前关键帧的位姿，(x_ai，y_ai，z_ai，yaw_ai，pitch_ai，roll_ai)代表对齐锚点的位姿；Epos代表当前关键帧与相对应的对齐锚点之间的位置误差，Eyaw代表当前关键帧与相对应的对齐锚点之间的偏航角误差，Epitch代表当前关键帧与相对应的对齐锚点之间的俯仰角误差，Eroll代表当前关键帧跟对齐锚点之间的横滚角误差。

本公开还提供了一种基于锚点的VSLAM后端优化装置，包括：

获取模块，用于获取多个对齐锚点和相机采集的多个图像；所述对齐锚点的时间戳与所述图像的时间戳分别对应；所述对齐锚点为采用其他定位源生成的位姿数据；

生成模块，用于基于至少所述图像，生成地图点和关键帧；其中，每一个所述关键帧都对应有对齐锚点；

优化模块，用于在判定满足局部后端优化条件之后，基于关键帧对应的对齐锚点，进行VSLAM后端优化。

本公开还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序用于执行上述任一种方法的步骤。

本公开还提供了一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述可执行指令以实现上述任一种方法的步骤。

本公开还提供了一种车辆，包括上述任一种电子设备。

本公开提供的技术方案与现有技术相比具有如下优点：

本公开提供的基于锚点的VSLAM后端优化方法包括：获取多个对齐锚点和相机采集的多个图像；对齐锚点的时间戳与图像的时间戳分别对应；对齐锚点为采用其他定位源生成的位姿数据；基于图像，生成地图点和关键帧；其中，每一个关键帧都对应有对齐锚点；若满足局部后端优化条件，则基于关键帧对应的对齐锚点，进行VSLAM后端优化。其中，对齐锚点为基于其他定位源生成的位姿数据，能够给对应时间戳的图像和关键帧赋予较为准确的位姿数据；由此，通过结合对齐锚点进行VSLAM后端优化，相当于将对齐锚点引入VSLAM后端优化的步骤中，以位姿精度较高的对齐锚点作为对应的关键帧的先验位姿真值，在VSLAM后端优化步骤中添加了关键帧与对齐锚点之间的位姿约束，从而有利于提高后端优化精度，进而提升VSLAM建图精度，改善累计误差较大且持续增大的问题。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。

为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本公开实施例提供的一种VSLAM的流程框架示意图；

图2为相关技术提供的一种图优化方式的原理示意图；

图3为本公开实施例提供的一种基于锚点的VSLAM后端优化方法的流程示意图；

图4为本公开实施例提供的一种图优化方式的原理示意图；

图5为本公开实施例提供的另一种基于锚点的VSLAM后端优化方法的流程示意图；

图6为本公开实施例提供的一种基于锚点的VSLAM后端优化装置的结构示意图；

图7为本公开实施例提供的一种电子设备的结构示意图。

具体实施方式

为了能够更清楚地理解本公开的上述目的、特征和优点，下面将对本公开的方案进行进一步描述。需要说明的是，在不冲突的情况下，本公开的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本公开，但本公开还可以采用其他不同于在此描述的方式来实施；显然，说明书中的实施例只是本公开的一部分实施例，而不是全部的实施例。

本公开实施例主要涉及到无人驾驶定位技术。具体地，随着计算机视觉技术的发展，在国家新基建的背景下，无人驾驶作为典型应用场景之一获得了广泛关注。在无人驾驶关键技术中，定位技术占据着极其重要的地位。其中，VSLAM的框架如图1所示，其通常包括获取传感器数据、前端视觉里程计(即前端)、后端非线性优化(即后端优化)、回环检测以及建图等步骤。

其中，前端视觉里程计的步骤中，对相机采集到的图像进行信息提取，估计相邻图像间对应的相机的运动，生成局部地图。针对前端视觉里程计的实现方法，按照是否需要提取特征来分类，可分为特征点法和直接法。其中，由于噪声的存在，前端视觉里程计会产生不断累积的误差；对应地，后端优化步骤中接收不同时刻的相机位姿(即不同图像对应的相机位姿)以及回环检测的信息，并进行优化，以减小累积误差，从而利于得到全局一致的轨迹和地图。

目前常用的后端优化方式是非线性优化，主要是针对前端生成的关键帧和地图点(即得到的三维点)，利用关键帧和地图点之间的约束、关键帧与关键帧之间的约束以及回环信息，采用迭代的方式不断减小整体误差。但是，由于噪声的存在，仅仅依靠VSLAM过程中出现的这些内在约束参与后端优化，后端优化后的地图仍然存在较大的误差，并且这些误差会随着地图尺寸的增大而增大。

此外，后端优化还可采用图优化方式实现，可参考图2。其中示出了前端视觉里程计生成的地图点和关键帧；地图点{y1,y2,y3,...,yn}在图2中用圆点示出，对应的这些地图点的真实位置{ry1,ry2,ry3,...,ryn}在图2中以方形示出，关键帧{x1,x2,x3,...,xm}在图2中用三角形示出，对应的这些关键帧的真实位置{rx1,rx2,rx3,...,rxn}在图2中以六角形示出。地图点位置和关键帧位姿构成了图优化的顶点。其中，由于噪声的存在，关键帧对地图点的观测存在重投影误差，构成了图优化的边，如图2中虚线L1所示。

基于此，采用图优化方式进行后端优化是指通过不断优化使得整体误差边的值趋于最小，进而使地图点和关键帧的位置和位姿逼近真实值。此图优化方式仅仅依靠前端视觉里程计生成的地图点、关键帧及二者之间的内在约束，无法使优化后的地图精度足够高，并且当前端视觉里程计因为误匹配等原因给出错误的关键帧和地图点位置初值时，后端优化也不能够很好的纠正过来。同时，这种后端优化方法无可避免的存在着累积误差，虽然有回环检测的存在，对于轨迹成环的地图可以一定程度上纠正过来，但误差遗留程度仍然较大。而对于轨迹不成环的开放轨迹，累积误差会更大。

针对此，本发明提出了一种基于锚点的VSLAM后端优化方法。本公开实施例中的锚点，是指采用其他定位源，例如GPS、LSLAM或者融合定位等，生成的轨迹数据(即位姿数据)，其可离线生成，也可在线生成，在此不限定。能够理解的是，本公开实施例中，锚点的特点是具有很高的位姿精度，通过将锚点与图像进行时间戳对齐，赋予对应的图像(以及基于图像确定的关键帧)锚点的位姿数据，能够为对应的图像及关键帧帧赋予较高精度且较准确的位姿数据，由此通过将锚点引入到VSLAM后端优化方法中，能够将关键帧与锚点之间的约束增加至后端优化过程中，从而提高后端精度，降低累计误差；进而提升VSLAM所建成的视觉地图的精度，且该视觉地图的精度不随地图尺寸增大而发生衰减，即该视觉地图的精度能够针对任意尺寸地图均保持较高精度，从而改善累计误差持续增大的问题。同时，该方法适用于各种轨迹对应的地图，无论是轨迹成环，还是轨迹不成环，都能够实现较高的精度。

能够理解的是，本公开实施例提供的技术方案主要解决如何提升VSLAM后端优化得到的地图精度的问题。具体的，是针对采用特征点法前端和图优化后端的VSLAM，解决如何提升后端优化精度的问题。

下面结合附图，对本公开实施例提供的基于锚点的VSLAM后端优化方法、装置、介质、设备和车辆进行示例性说明。

示例性地，图3为本公开实施例提供的一种基于锚点的VSLAM后端优化方法的流程示意图。参照图3，该方法可包括如下步骤：

S110、获取多个对齐锚点和相机采集的多个图像。

其中，对齐锚点的时间戳与图像的时间戳分别对应；对齐锚点为采用其他定位源生成的位姿数据。示例性地，其他定位源可包括GPS、LSLAM或者融合定位等。能够理解的是，对齐锚点相对于图像具有较高的位姿精度，从而能赋予对应时间下的图像准确度较高的真实尺度下的位姿数据。

在一些实施方式中，其他定位源与相机的采用率和采样时间中的至少一个可能存在差异，进而，其他定位源产生的初始锚点的时间戳与图像的时间戳并不对应，因此，在该步骤之前，还可包括基于时间戳对初始锚点进行插值，以获得时间戳与图像一一对应的锚点，即对齐锚点。本公开实施例中，以相机采集到的图像的时间戳为参考，对其他定位源的位姿数据进行插值(例如线性插值)，得到对应于图像的对齐锚点，从而得到每个图像对应的较为准确的位姿数据，为后续执行S120和S130作准备。

示例性地，对齐锚点可为最多6个自由度的位姿数据，以三维空间为例，6个自由度即x、y、z、yaw、pitch、roll。但能够理解的是，在对齐锚点用于后续步骤的过程中，不一定全部利用6个自由度的信息。示例性地，针对无人驾驶的车辆而言，用到的自由度可为3个，例如分别为x、y和yaw；针对无人机而言，用到的自由度可为6个，即上述全部的自由度。

在一些实施方式中，生成GPS对应的初始锚点可包括：直接提取GPS数据中的east、north、height、yaw、pitch、roll数据，经全局坐标向局部坐标转换，即统一到同一个坐标系下，对应生成GPS锚点。

在一些实施方式中，生成雷达(LIDAR)对应的初始锚点可包括：基于雷达采集的数据，即点云，通过建图和定位，生成LSLAM地图，同时生成雷达对应的初始锚点。

S120、基于图像，生成地图点和关键帧。

其中，每一个关键帧都对应有对齐锚点。由于关键帧为从所有图像中选出来的部分图像，即关键帧的集合为图像的集合的子集；而且图像都存在对应的对齐锚点，因此每一个关键帧也都对应有对齐锚点。

该步骤中，以图像和对齐锚点作为输入，进行VSLAM建图，生成地图点和关键帧，每一个关键帧都有相对应的对齐锚点。

S130、若满足局部后端优化条件，则基于关键帧对应的对齐锚点，进行VSLAM后端优化。

其中，局部后端优化条件为用来判断是否可以进行VSLAM后端优化的条件，也可理解为判断是否达到局部后端优化的条件。若满足该局部后端优化条件，则进行VSLAM后端优化，具体地，基于关键帧对应的对齐锚点进行VSLAM后端优化，详见后文；若不满足局部后端优化条件，则不进行VSLAM后端优化，而是返回VSLAM建图，继续积累地图点和关键帧。

在一些实施例中，局部后端优化条件包括第一条件和第二条件中的至少一个；其中，第一条件包括：新增关键帧的数量大于预设数量阈值；新增关键帧的数量为距离上一次局部后端优化，已经新增的关键帧的数量；第二条件包括：位姿误差大于预设误差阈值；位姿误差为基于当前的关键帧位姿和相对应的锚点位姿确定的误差。

具体地，针对上述第一条件和第二条件，满足其中一个，或者同时满足两个，均满足局部后端优化条件。

其中，第一条件中，预设数量阈值用于衡量新增的关键帧是否足够多；若新增关键帧的数量大于预设数量阈值，则表明距离上一次进行局部后端优化，已经新增了足够多的关键帧；对应地，满足局部后端优化条件，后续再次进行局部后端优化。

其中，第二条件中，锚点位姿为对齐锚点的位姿数据，预设误差阈值用于衡量位姿误差是否过大；若位姿误差大于预设误差阈值，则表明当前的关键帧位姿距离相对应的锚点位姿之间的误差过大；对应地，满足局部后端优化条件，后续再次进行局部后端优化。

在一些实施例中，位姿误差包括位置误差、偏航角误差、俯仰角误差以及横滚角误差中的至少一个；位姿误差采用如下公式计算：

E_yaw＝(yaw_i-yaw_ai)

E_pitch＝(pitch_i-pitch_ai)

E_roll＝(roll_i-roll_ai)

其中，(x_i,y_i,z_i,yaw_i,pitch_i,roll_i)代表当前关键帧的位姿，(x_ai,y_ai,z_ai,yaw_ai,pitch_ai,roll_ai)代表对齐锚点的位姿；Epos代表当前关键帧与相对应的对齐锚点之间的位置误差，Eyaw代表当前关键帧与相对应的对齐锚点之间的偏航角误差，Epitch代表当前关键帧与相对应的对齐锚点之间的俯仰角误差，Eroll代表当前关键帧跟对齐锚点之间的横滚角误差。

其中，结合上文示例的三维空间对应的6个自由度，位置误差可以基于x、y和z共同计算得到的位置误差，其对应三维空间内的一个距离误差；在其他实施方式中，还可基于x、y和z中的一个或两个自由度计算位置误差，以得到对应自由度上的距离误差。其中，角度误差为在三个角度自由度上对应计算得到的角度误差，且在三个不同的角度自由度上分别计算。

在一些实施例中，对于一般不可能发生转动的运动主体，例如自动驾驶车辆而言，位姿误差主要包括位置误差和偏航角误差；对于会发生转动的运动主体，例如无人机而言，位姿误差不仅包括位置误差，还包括三个角度误差。

能够理解的是，当上述位置误差和三个角度误差中的至少一个超出对应的预设误差阈值时，则表明误差偏差大，需要进行后端优化。

在一些实施例中，进行后端优化可包括进行局部后端优化和全局后端优化，以得到全局一致且精度较高的地图。

示例性地，在图1的基础上，S130中的进行VSLAM后端优化，具体可包括如下步骤：

基于对齐锚点，进行局部后端优化；

判断是否所有图像均处理完毕；

若未处理完毕，则返回基于图像，生成地图点和关键帧；

若均处理完毕，则基于对齐锚点，进行全局后端优化。

具体地，首先基于对齐锚点进行局部后端优化；并进一步判断是否所有图像均处理完毕；若未处理完毕，则返回基于图像和对齐锚点进行VSLAM建图，生成地图点和关键帧；直至所有图像均处理完毕之后，基于对齐锚点，进行全局后端优化，以进一步统一全局尺寸。

需要说明的是，局部后端优化和全局后端优化可采用相同的优化方式，也可采用不同的优化方式，在此不限定，后文中对具体优化方式进行示例说明。

在一些实施例中，进行局部后端优化，或者进行全局后端优化，具体可包括如下步骤：

采用图优化的方式，优化重投影误差及关键帧与所对应的对齐锚点之间的误差。

本公开实施例中，采用图优化的方式进行VSLAM后端优化，并以对齐锚点的位姿为参考，进而使得地图点和关键帧的位置和位姿更接近真实值，有利于减小累计误差，使得优化后的地图精度较高。

示例性地，图4为本公开实施例提供的一种图优化方式的原理示意图。在图2的基础上，参照图4，其中相同的标记不再重复解释；与图2不同的，图4中引入了锚点位姿，具体地，{a1,a2,a3,...,an}代表图像(即关键帧)所对应的对齐锚点的位姿，虚线L2代表关键帧位姿与锚点位姿之间的误差构成的边。采用图优化同时优化重投影误差及关键帧位姿与锚点位姿之间的误差，能够极大地缩小整体误差，使得关键帧位姿贴近锚点位姿，进而关键帧位姿贴近真实位姿，以使地图点的位置更贴近真实位置。

需要说明的是，图2和图4中仅示例性地示出了4个不同的点，即n的取值为4，在其他实施方式中，n的取值还可设置为其他任意数值，在此不限定。

在一些实施例中，进行局部后端优化，或者进行全局后端优化，包括：

确定VSLAM观测模型对应的代价函数；

通过迭代优化，使代价函数趋于最小；

其中，代价函数为：

其中，e_ij代表累计代价，x_i代表相机在当前图像帧对应的位置，y_i代表对应的地图点，z_ij代表针对该地图点由相机采集到的观测数据，ax_i代表对齐锚点对应的位置。

具体地，VSLAM问题对应两个基本的模型，即运动模型和观测模型，其均可以采用方程的形式示出，本公开实施例中，只关注观测模型，观测模型对应的方程可表示为：

z_k，j＝h(y_j，x_k，v_k，j).

其中，x代表相机的位姿，即关键帧的位姿；y代表相机能观测到的一个地图点的位置，即地图点的坐标；v代表观测噪声，z代表观测数据，即地图点在图像上的投影。此方程的意义为：相机在x_k位置上观测到某个地图点yj，对应产生一个观测数据z_k,j，且其中的观测噪声为v_k,j。

VSLAM的后端优化是针对于观测模型采用迭代优化的求解方式，通过设定代价函数，并沿着梯度下降的方向不断迭代，使整体代价函数的值越来越小，进而使得x,y的值越来越接近真实值。基于观测模型对应的方程，选取最小二乘方法进行求解，则代价函数可表示为：

其中，由于观测噪声的存在，实际的观测数据z_ij不会精确的等于观测理论值h(x_i,y_j)，二者对应构成观测数据误差。基于此，把所有的观测数据误差加起来进行最小二乘求解即可得出上述代价函数的方程形式。

本公开实施例中，在后端优化中引入了对齐锚点。由此，除了观测模型对应的方程的误差部分，还加入了相机位置x_i与对应的对齐锚点的位置ax_i之间的误差部分，由此，本公开实施例中的代价函数是上述两部分误差加起来的结果，如下式所示：

能够理解的是，上述代价函数不是线性函数，需要采用非线性优化手段来进行优化。

在一些实施例中，图5为本公开实施例提供的另一种基于锚点的VSLAM后端优化方法的流程示意图，示出了基于在线生成锚点的方式进行VSLAM后端优化的方法的流程。参照图5，该方法可包括：

S201、同时采集相机及至少一种其他定位源的数据。

其中，“同时”是指采集时段相同，而并非采样率和采样时间完全一致。该步骤中，在相同的时段内，获取利用相机和至少一种其他定位源采集的数据。示例性地，其他定位源可包括雷达或GPS；雷达采集的数据可为点云数据，GPS采集的数据可为GPS数据，相机采集的数据可为图像，且三者的数据均携带时间戳，以便后续步骤中进行时间戳对齐。

S202、基于其他定位源中至少之一，生成初始锚点。

其中，可基于GPS数据，通过数据提取，生成GPS锚点，且将GPS锚点作为初始锚点；或者基于雷达的点云数据，通过建图和定位，生成LSLAM锚点，即雷达对应的初始锚点，并将LSLAM锚点作为初始锚点。

在其他实施方式中，还可采用精度较高的其他定位源采集的数据，并生成初始锚点，在此不限定。

能够理解的是，当基于某定位源直接得到的位姿数据为相对位姿数据时，还可结合GPS等其他定位源的数据将该相对位姿数据转换为绝对位姿数据，即全局坐标下的位姿数据，以得到初始锚点。

S203、基于初始锚点，插值得到与相机的数据对应的对齐锚点。

其中，由于相机与GPS、雷达等其他定位源采集数据的采样率和采样时间不完全一致，导致其时间戳并不完全相同。该步骤中，通过插值，得到与相机的数据时间戳一一对应的锚点，即对齐锚点。

S204、VSLAM建图。

具体的，针对单目VSLAM，可基于图像和对齐锚点，进行VSLAM建图，生成地图点和关键帧；或者，针对双目VSLAM，可基于图像，进行VSLAM建图，生成地图点和关键帧。VSLAM建图的具体过程可包括：提取图像中的特征点，基于特征点法进行VSLAM建图。

需要说明的是，本公开实施例中，针对单目VSLAM，结合对齐锚点进行建图，能够基于对齐锚点提供VSLAM过程中的尺度信息和全局坐标，从而有利于实现真实尺度的还原，同时提高成功率和精度。

S205、达到局部后端优化的条件？

具体的，判断是否达到局部后端优化的条件。若达到局部后端优化的条件(即Y)，则进行后端优化，即执行S206及后续步骤；若未达到局部后端优化的条件(即N)，则返回S204。

S206、基于对齐锚点进行局部后端优化。

具体地，以对齐锚点为关键帧位姿的参考，进行局部后端优化。

S207、所有图像处理完毕？

具体的，判断是否所有图像均处理完毕。若所有图像均处理完毕，则进行后续全局后端优化步骤，即执行S208；若并非所有图像均处理完毕，即仍存在未处理完毕的图像，则返回VSLAM建图步骤，即S204，继续处理图像，直至所有图像均处理完毕后，再进行全局后端优化。

需要说明的是，该步骤中的所有图像是指从前端视觉里程计生成地图点和关键帧，并确定初始化成功的这一关键帧所对应的图像和时序上位于其后面的所有图像，可不包括初始化成功前的图像；例外的，针对存在从初始化成功对应的关键帧向前建图的方法而言，所有图像还包括初始化成功前的图像。

S208、基于对齐锚点进行全局后端优化。

具体地，以对齐锚点为关键帧位姿的参考，进行全局后端优化。

需要说明的是，全局后端优化和局部后端优化可采用相同的优化方式，其区别仅在于优化的范围不同。具体地：局部后端优化是针对当前关键帧，选择小范围附近的有共视关系的几个关键帧作为优化对象，求解局部自由解，且在后执行的局部后端优化会影响在前局部后端优化的结果；而全局后端优化是将所有关键帧作为优化对象，求解全局自由解，同一全局尺度。

S209、生成VSLAM地图。

具体地，得到优化后的地图，进一步地，还可保存生成的VSLAM地图。

能够理解的是，图5示出的方法中，有别于传统VSLAM建图的步骤在于：需要同步采集至少一种其他定位源，例如雷达、GPS等中的至少一种传感器的数据，对应生成LIDAR锚点和GPS锚点中的至少一种，并将其作为位姿参考用于VSLAM建图过程中。在其他实施方式中，还可基于其他定位源采集到的数据离线生成锚点，并用于VSLAM建图过程中，在此不赘述也不限定。

本公开实施例提供的基于锚点的VSLAM后端优化方法，通过引入对齐锚点，能够充分利用对齐锚点的较高精度，提高后端优化的精度，从而使得VSLAM建图过程中的整体误差不再随着地图尺度增大而增大，而是跟对齐锚点的精度相关，从而能显著的减小VSLAM建图的累计误差；具体地，将对齐锚点作为关键帧的较高精度的先验位姿真值，在后端优化中，通过添加关键帧与对齐锚点之间的位姿约束，能够提高建图精度。此外，该方法中利用对齐锚点可提供真实尺度，省去了传统VSLAM建图的回环检测流程，可以解决单目VSLAM建图尺度不确定性的问题。

在上述实施方式的基础上，本公开实施例还提供了一种基于锚点的VSLAM后端优化装置，该装置用于执行上述实施方式提供的任一种方法，能够实现对应的有益效果。

示例性地，图6为本公开实施例提供的一种基于锚点的VSLAM后端优化装置的结构示意图。参照图6，该装置300包括：获取模块310，用于获取多个对齐锚点和相机采集的多个图像；对齐锚点的时间戳与图像的时间戳分别对应；对齐锚点为采用其他定位源生成的位姿数据；生成模块320，用于基于至少图像，生成地图点和关键帧；其中，每一个关键帧都对应有对齐锚点；优化模块330，用于在判定满足局部后端优化条件之后，基于关键帧对应的对齐锚点，进行VSLAM后端优化。

本公开实施例提供的基于锚点的VSLAM后端优化装置，通过上述各功能模块之间的协同作用，能够引入对齐锚点至该装置中；其中，对齐锚点为基于其他定位源生成的位姿数据，能够给对应时间戳的图像帧赋予较为准确的位姿数据；由此，该装置通过结合对齐锚点进行VSLAM后端优化，相当于以位姿精度较高的对齐锚点作为对应的关键帧的先验位姿真值，在VSLAM后端优化时添加了关键帧与对齐锚点之间的位姿约束，从而有利于提高后端优化精度，进而提升VSLAM建图精度，改善累计误差较大且持续增大的问题。

在一些实施例中，局部后端优化条件包括第一条件和第二条件中的至少一个；第一条件，包括：新增关键帧的数量大于预设数量阈值；新增关键帧的数量为距离上一次局部后端优化，已经新增的关键帧的数量；第二条件，包括：位姿误差大于预设误差阈值；位姿误差为基于当前的关键帧位姿和相对应的锚点位姿确定的误差。

在一些实施例中，在图6的基础上，优化模块330用于进行VSLAM后端优化，包括优化模块330具体用于：

基于对齐锚点，进行局部后端优化；

判断是否所有图像均处理完毕；

若未处理完毕，则返回基于图像，生成地图点和关键帧；

若均处理完毕，则基于对齐锚点，进行全局后端优化。

在一些实施例中，优化模块330用于进行局部后端优化，或者进行全局后端优化，包括优化模块330具体用于：

采用图优化的方式，优化重投影误差及关键帧与所对应的对齐锚点之间的误差。

在一些实施例中，优化模块330用于进行局部后端优化，或者进行全局后端优化，包括优化模块330具体用于：

确定VSLAM观测模型对应的代价函数；

通过迭代优化，使代价函数趋于最小；

其中，代价函数为：

其中，e_ij代表累计代价，x_i代表相机在当前图像帧对应的位置，y_i代表对应的地图点，z_ij代表针对该地图点由相机采集到的观测数据，ax_i代表对齐锚点对应的位置。

在一些实施例中，位姿误差包括位置误差、偏航角误差、俯仰角误差以及横滚角误差中的至少一个；位姿误差采用如下公式计算：

E_yaw＝(yaw_i-yaw_ai)

E_pitch＝(pitch_i-pitch_ai)

E_roll＝(roll_i-roll_ai)

其中，(x_i，y_i，z_i，yaw_i，pitch_i，roll_i)代表当前关键帧的位姿，(x_ai，y_ai，z_ai，yaw_ai，pitch_ai，roll_ai)代表对齐锚点的位姿；Epos代表当前关键帧与相对应的对齐锚点之间的位置误差，Eyaw代表当前关键帧与相对应的对齐锚点之间的偏航角误差，Epitch代表当前关键帧与相对应的对齐锚点之间的俯仰角误差，Eroll代表当前关键帧跟对齐锚点之间的横滚角误差。

能够理解的是，图7示出的装置能够实现上述实施方式提供的任一种方法，具有对应的有益效果，具体可参照上文理解，在此不赘述。

在上述实施方式的基础上，如图7所示，为本公开实施例提供的一种电子设备的结构示意图。参照图7，该电子设备400包括：处理器420；用于存储处理器420可执行指令的存储器410；处理器420，用于从存储器410中读取可执行指令，并执行可执行指令以实现上述实施方式提供的任一种的方法的步骤，具有对应的有益效果，为避免重复描述，在此不再赘述。

其中，处理器420可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制计算机中的其他组件以执行期望的功能。

存储器410可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器420可以运行所述程序指令，以实现上文所述的本申请的各个实施例的方法步骤以及/或者其他期望的功能。

除了上述方法和电子设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本申请各种实施例的方法步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本公开实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，计算机程序指令在被处理器420运行时使得所述处理器420执行本申请各种实施例的方法步骤。

计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

在上述实施方式的基础上，本公开实施例还提供了一种车辆，包括上述电子设备，具有对应的有益效果，为避免重复描述，在此不再赘述。

在其他实施方式中，该车辆还包括无人驾驶***，无人驾驶***包括感知模块、决策模块和底盘执行模块等，在此不限定。

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

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

Claims (10)

1.一种基于锚点的VSLAM后端优化方法，其特征在于，包括：

获取多个对齐锚点和相机采集的多个图像；所述对齐锚点的时间戳与所述图像的时间戳分别对应；所述对齐锚点为采用其他定位源生成的位姿数据；

基于所述图像，生成地图点和关键帧；其中，每一个所述关键帧都对应有对齐锚点；

若满足局部后端优化条件，则基于关键帧对应的对齐锚点，进行VSLAM后端优化。

2.根据权利要求1所述的方法，其特征在于，所述局部后端优化条件包括第一条件和第二条件中的至少一个；

所述第一条件，包括：新增关键帧的数量大于所述预设数量阈值；所述新增关键帧的数量为距离上一次局部后端优化，已经新增的关键帧的数量；

所述第二条件，包括：位姿误差大于所述预设误差阈值；所述位姿误差为基于当前的关键帧位姿和相对应的锚点位姿确定的误差。

3.根据权利要求1或2所述的方法，其特征在于，所述进行VSLAM后端优化，包括：

基于所述对齐锚点，进行局部后端优化；

判断是否所有所述图像均处理完毕；

若未处理完毕，则返回所述基于所述图像，生成地图点和关键帧；

若均处理完毕，则基于所述对齐锚点，进行全局后端优化。

4.根据权利要求3所述的方法，其特征在于，所述进行局部后端优化，或者所述进行全局后端优化，包括：

采用图优化的方式，优化重投影误差及关键帧与所对应的对齐锚点之间的误差。

5.根据权利要求3所述的方法，其特征在于，所述进行局部后端优化，或者所述进行全局后端优化，包括：

确定VSLAM观测模型对应的代价函数；

通过迭代优化，使所述代价函数趋于最小；

其中，所述代价函数为：

其中，e_ij代表累计代价，x_i代表相机在当前图像帧对应的位置，y_i代表对应的地图点，z_ij代表针对该地图点由相机采集到的观测数据，ax_i代表对齐锚点对应的位置。

6.根据权利要求2所述的方法，其特征在于，所述位姿误差包括位置误差、偏航角误差、俯仰角误差以及横滚角误差中的至少一个；所述位姿误差采用如下公式计算：

E_yaw＝(yaw_i-yaw_ai)

E_pitch＝(pitch_i-pitch_ai)

E_roll＝(roll_i-roll_ai)

其中，(x_i，y_i，z_i，yaw_i，pitch_i，roll_i)代表当前关键帧的位姿，(x_ai，y_ai，z_ai，yaw_ai，pitch_ai，roll_ai)代表对齐锚点的位姿；Epos代表当前关键帧与相对应的对齐锚点之间的位置误差，Eyaw代表当前关键帧与相对应的对齐锚点之间的偏航角误差，Epitch代表当前关键帧与相对应的对齐锚点之间的俯仰角误差，Eroll代表当前关键帧跟对齐锚点之间的横滚角误差。

7.一种基于锚点的VSLAM后端优化装置，其特征在于，包括：

获取模块，用于获取多个对齐锚点和相机采集的多个图像；所述对齐锚点的时间戳与所述图像的时间戳分别对应；所述对齐锚点为采用其他定位源生成的位姿数据；

生成模块，用于基于至少所述图像，生成地图点和关键帧；其中，每一个所述关键帧都对应有对齐锚点；

优化模块，用于在判定满足局部后端优化条件之后，基于关键帧对应的对齐锚点，进行VSLAM后端优化。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序用于执行如权利要求1-6任一项所述方法的步骤。

9.一种电子设备，其特征在于，包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述可执行指令以实现如权利要求1-6任一项所述的方法的步骤。

10.一种车辆，其特征在于，包括权利要求9所述的电子设备。

Images