CN113345017B - 一种使用标志辅助视觉slam的方法 - Google Patents

Abstract

本发明公开了一种使用标志辅助视觉SLAM的方法，所述方法包括：获取预设的标志，根据所述标志确定初始地图数据；根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；根据所述标志对所述地图数据进行调整，得到目标地图数据。本发明通过在视觉SLAM的初始化、跟踪定位以及地图优化等步骤中使用预先设置的同一套标志辅助定位与建图，提高了视觉SLAM系统的精度和鲁棒性，提高了时间效率与空间效率。解决了现有技术中由于视觉SLAM系统的鲁棒性受限于复杂环境与动态环境，从而导致跟踪失败的问题。

Description

一种使用标志辅助视觉SLAM的方法

技术领域

本发明涉及视觉同步定位与建图领域，尤其涉及的是一种使用标志辅助视觉SLAM的方法。

背景技术

SLAM技术是一种同步定位与建图技术，其最早被应用在机器人领域，其目标是在没有任何先验知识的情况下，根据传感器数据实时构建周围环境地图，同时根据这个地图推测自身的定位。视觉SLAM系统在精度上大都维持在轨迹误差1％的程度(即走100m，跟踪会偏移1m)。由于相机具有丰富的视觉信息，以及硬件成本低等优点，因此出现了只利用相机作为外部感知传感器的SLAM，即视觉SLAM。目前，由于视觉SLAM系统的鲁棒性受限于复杂环境(低光照、重复纹理、狭窄环境等)与动态环境(行人、车辆、光照变化等)，从而容易导致跟踪失败。

因此，现有技术还有待改进和发展。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种使用标志辅助视觉SLAM的方法，旨在解决现有技术中由于视觉SLAM系统的鲁棒性受限于复杂环境与动态环境，从而导致跟踪失败的问题。

本发明解决问题所采用的技术方案如下：

第一方面，本发明实施例提供一种使用标志辅助视觉SLAM的方法，其中，所述方法包括：

获取预设的标志，根据所述标志确定初始地图数据；

根据所述标志进行跟踪定位，得到所述拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；

根据所述标志对所述地图数据进行调整，得到目标地图数据。

在一种实施方式中，所述获取预设的标志，根据所述标志确定初始地图数据，包括：

将所述标志中位于起点的标志作为起点标志，获取拍摄到所述起点标志的相邻两帧影像帧，并将所述相邻两帧影像帧分别对应的二维影像平面的相机坐标系作为第一坐标系和第二坐标系；

获取所述起点标志分别在所述第一坐标系和所述第二坐标系下的坐标数据，得到第一相机坐标数据和第二相机坐标数据；

获取所述起点标志对应的测量角点坐标数据，根据所述起点标志对应的测量角点坐标数据和所述第一相机坐标数据，确定相机相对于测量角点坐标系的第一位姿数据；并根据所述起点标志对应的测量角点坐标数据和所述第二相机坐标数据，确定相机相对与测量角点坐标系的第二位姿数据；

根据所述第一位姿数据和所述第二位姿数据，计算出相邻两帧影像帧的相对位姿；

将所述相邻两帧影像帧的相对位姿作为初始化的初值进行特征点匹配，当所述特征点匹配的结果满足预设条件时，确定初始化结果为成功，并以所述测量角点坐标数据为尺度基准，确定初始地图数据。

在一种实施方式中，所述当所述特征点匹配的结果满足预设条件时，确定初始化结果为成功，包括：

当所述相邻两帧影像帧都能提取大于特征阈值的特征点；且所述相邻两帧特征点的匹配成功数量大于匹配阈值；且所述相邻两帧特征点中特征匹配成功的点对数量大于匹配阈值；且第一平均重投影误差和第二平均重投影误差的平均值小于重投影误差阈值时，确定初始化结果为成功；

其中，所述第一平均重投影误差为将计算出的所述相邻两帧影像帧中左帧的匹配成功的特征点的相对位姿投影到右帧得到的平均重投影误差；所述第二平均重投影误差为将计算出的所述右帧匹配成功的特征点的相对位姿投影到所述左帧得到的平均重投影误差。

在一种实施方式中，所述以所述测量角点坐标数据为尺度基准，确定初始地图数据，包括：

在所述相邻两帧影像帧中获取初始化成功的两帧中匹配成功的特征点的二维坐标，并根据所述二维坐标进行三角化计算，得到所有匹配成功的特征点分别对应的三维坐标；

确定标志提取算法提取到的标志在初始化成功的两帧中的二维坐标，得到二维标志坐标，根据所述二维标志坐标以及初始化成功的两帧影像帧的相对位姿，计算所述标志上各个角点的三维坐标，得到计算角点坐标数据；

根据所述测量角点坐标数据和所述计算角点坐标数据，确定比例信息；

根据所述比例信息确定所述起点标志对应的原始尺度数据，并将所述起点标志对应的原始尺度数据作为起点尺度数据；

将所述起点尺度数据与所有匹配成功的特征点对应的三维坐标相乘，得到所有匹配成功的特征点各自对应的真实尺度的三维坐标数据；

根据所有匹配成功的特征点各自对应的真实尺度的三维坐标数据，确定初始地图数据。

在一种实施方式中，根据所述标志进行跟踪定位，得到所述拍摄相机的位姿数据，包括：

当前影像帧跟踪失败时，确定跟踪失败的场景位置，并根据所述跟踪失败的场景位置确定新增标志；

根据所述新增标志进行跟踪定位，得到所述拍摄相机在所述当前影像帧中的位姿数据。

在一种实施方式中，根据所述标志进行跟踪定位，得到所述拍摄相机的位姿数据，还包括：

当将所述新增标志作为纹理补充，并根据所述新增标志进行跟踪定位，仍无法得到所述拍摄相机在所述当前影像帧中的位姿数据时，获取所述当前影像帧之前的若干帧影像帧，并将所述当前影像帧之前的若干帧影像帧作为历史影像帧；

获取所述当前影像帧与所述历史影像帧中出现的相同ID的标志，将所述相同ID的标志作为辅助跟踪标志；

获取所述当前影像帧的相对位姿与所述历史影像帧的相对位姿，根据所述当前影像帧的相对位姿、所述历史影像帧的相对位姿以及所述辅助跟踪标志转换出所述拍摄相机在所述当前影像帧中的位姿数据。

在一种实施方式中，根据所述标志进行跟踪定位，得到所述拍摄相机的位姿数据，还包括：

当所述当前影像帧跟踪成功时，获取关键影像帧，并在所述关键影像帧中选择出与所述当前影像帧存在的相同ID的标志的关键影像帧；

将所述与所述当前影像帧存在的相同ID的标志的关键影像帧，作为所述当前影像帧对应的闭环备选帧；

获取预设的帧数阈值和距离阈值，并获取所述闭环备选帧分别与所述当前影像帧之间的间隔帧数和直线距离值，将所述间隔帧数小于所述帧数阈值，且所述直线距离值小于所述距离阈值的闭环备选帧，作为有效闭环备选帧；

当所述有效闭环备选帧前后连续预设数量的影像帧均为有效的闭环备选帧时，将所述有效闭环备选帧作为目标闭环帧。

在一种实施方式中，根据所述标志对所述地图数据进行调整，得到目标地图数据，包括：

当所述地图数据的数量大于1时，将所述地图数据作为待融合地图数据；

获取所述待融合地图数据中出现的相同ID的标志，并将所述待融合地图数据中出现的相同ID的标志作为融合标志；

获取所述融合标志在所述待融合地图数据各自对应的坐标系下的坐标数据，得到第一地图坐标数据和第二地图坐标数据；

根据所述第一地图坐标数据和所述第二地图坐标数据确定三维相似变换矩阵数据，并根据所述三维相似变换矩阵数据分别将所述待融合地图数据中的点的坐标数据转换至同一坐标系下，得到目标地图数据。

在一种实施方式中，根据所述标志对所述地图数据进行调整，得到目标地图数据，还包括：

将位于终点的标志作为终点标志，将拍摄到所述终点标志的影像帧作为终点影像帧；

获取所述终点标志的测量角点坐标数据，根据所述终点标志的测量角点坐标数据确定所述终点标志对应的原始尺度数据，得到原始终点尺度数据；

根据所述起点尺度数据和所述原始终点尺度数据，确定目标终点尺度数据

根据所述起点尺度数据和所述目标终点尺度数据对中间影像帧对应的尺度数据进行修正，从而影响每两帧之间的相对位姿和匹配成功的特征点对应的三维坐标，修正后得到新的轨迹位姿与目标地图数据；所述中间影像帧为位于起点影像帧与所述终点影像帧之间的影像帧；所述起点影像帧为拍摄到所述起点标志的影像帧。

在一种实施方式中，方法还包括：

获取应用任务，根据应用任务确定目标坐标系；

获取所述目标地图数据对应的地图坐标系，并确定同时位于所述地图坐标系中与所述目标坐标系中的预设个数的标志，得到辅助转换标志；

获取所述辅助转换标志在所述地图坐标系下的坐标数据，得到第一转换坐标数据，并获取所述辅助转换标志在所述目标坐标系下的坐标数据，得到第二转换坐标数据；

根据所述第一转换坐标数据和所述第二转换坐标数据，确定转换矩阵，并根据所述转换矩阵将所述目标地图数据中的点的坐标数据转换至所述目标坐标系中。

本发明的有益效果：本发明实施例通过获取预设的标志，根据所述标志确定初始地图数据；根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；根据所述标志对所述地图数据进行调整，得到目标地图数据。本发明通过在视觉SLAM的初始化、跟踪定位以及地图优化等步骤中使用预先设置的同一套标志辅助定位与建图，提高了视觉SLAM系统的精度和鲁棒性，提高了时间效率与空间效率。解决了现有技术中由于视觉SLAM系统的鲁棒性受限于复杂环境与动态环境，从而导致跟踪失败的问题。

附图说明

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

图1是本发明实施例提供的使用标志辅助视觉SLAM的方法的流程示意图。

图2是本发明实施例提供的组成标志的二维码的示意图。

图3是本发明实施例提供的二维码组成不同ID的标志的示意图。

图4是本发明实施例提供的尺度误差累积的结果图。

图5是本发明实施例提供的使用标志辅助视觉SLAM的装置的内部模块示意图。

图6是本发明实施例提供的视觉SLAM系统的原理图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

SLAM技术是一种同步定位与建图技术，其最早被应用在机器人领域，其目标是在没有任何先验知识的情况下，根据传感器数据实时构建前后环境地图，同时根据这个地图推测自身的定位。举例说明，如图6所示，假设机器人携带传感器(相机)在未知环境中运动，为方便起见，把一段连续时间的运动变成离散时刻t＝1，…k，而在这些时刻，用x表示机器人的自身位置，则各时刻的位置就记为x1，x2…xk，它构成了机器人的轨迹。地图方面，假设地图由许多个路标组成，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。设路标点共有N个，用y1，y2…yn表示。通过运动测量u和传感器读数z来求解定位问题(估计x)和建图问题(估计y)。由于相机具有丰富的视觉信息，以及硬件成本低等优点，因此出现了只利用相机作为外部感知传感器的SLAM，即视觉SLAM。

然而至今，视觉SLAM系统在精度上大都维持在轨迹误差1％的程度(即走100m，跟踪会偏移1m)。视觉SLAM系统的鲁棒性往往受限于复杂环境(低光照、重复纹理、狭窄环境等)与环境动态变化(行人、车辆、光照变化等)导致的跟踪失败。

针对现有技术的上述缺陷，本发明提供了一种使用标志辅助视觉SLAM的方法，通过获取预设的标志，根据所述标志确定初始地图数据；根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；根据所述标志对所述地图数据进行调整，得到目标地图数据。本发明通过在视觉SLAM的初始化、跟踪定位以及地图优化等步骤中使用预先设置的同一套标志辅助定位与建图，提高了视觉SLAM系统的精度和鲁棒性，提高了时间效率与空间效率。解决了现有技术中由于视觉SLAM系统的鲁棒性受限于复杂环境与动态环境，从而导致跟踪失败的问题。

如图1所示，本实施例提供，所述方法包括如下步骤：

步骤S100、获取预设的标志，根据所述标志确定初始地图数据。

具体地，为了提高了视觉SLAM系统的精度和鲁棒性，本实施例预先在场景中布设了一套标志。在一种实现方式中，标志的选取可以通过ChArUco标定棋盘生成。基于ChArUco标定棋盘生成的ChArUco标志如图2所示，ChArUco标志是基于基于西班牙科尔多瓦大学A.V.A研究组ArUco二维码组合而成的。在一种实现方式中，可以通过开源的OpenCV函数，并根据任务需要设计参数，然后将设计的参数输入OpenCV函数生成该标志。

可以理解的是，ChArUco每个标志都是唯一，并附带有各自专属的ID，使用时可使用特定的算法提取标志和其ID。制作完成标志以后，可以采用A3纸打印出ChArUco标志，然后将打印出的ChArUco标志布设在薄木板或玻璃板上并放置在场景中，或直接张贴于场景内的平面上。此外，每一个标志通常由4个角点，各角点的相对坐标关系在标志制作时已知，如图3所示，设计标志时会确定大棋盘格的横向数量9，大棋盘格的纵向数量6，棋盘格正方形的边长100像素，组成标志的二维码的边长80像素，棋盘格边缘的宽度50像素，57个二维码的编号，标志外的黑框宽度1格。

在一种实现方式中，本实施需要在拍摄相机的移动路线的起点位置布设1个标志，并在移动路线的中途一些特定位置布设1个或者多个标志，用于跟踪定位中保证跟踪成功。其中所述移动路线的中途一些特定位置包括：纹理稀缺处(如白墙)、狭窄拐角处、重复纹理处(没有其他参照只有同样的地板砖、天花板砖)。此外，本实施例还需要在移动路线的终点处布设1个标志，在一种实现方式中，移动路线的起点和终点可以重合，即位于起点的标志与位于终点的标志为同一标志。

制作以及布设完标志以后，在一种实现方式中，需要测量每一个标志的角点的三维坐标。具体测量坐标的步骤如下：

步骤A、对使用的棋盘格的边长进行测量，得到棋盘格边长数据；

步骤B、建立以左上棋盘交点(即第一个ArUco二维码和右下ArUco二维码的交点)为原点，棋盘横向边为x轴，纵向边为y轴的坐标系，垂直于棋盘向背面为z轴的坐标系，得到测量坐标系；

步骤C、根据棋盘格边长数据确定每一个标志其对应的各个角点在所述测量坐标系下的三维坐标，得到每一个标志的角点对应的测量坐标数据。

在一种实现方式中，本实施例还可以对标志进行标定。具体地，拍摄相机的参数通常分为内参数和外参数，为了实现视觉同步定位与建图，本实施例需要对标志进行标定，利用拍摄相机自身的摄像头进行数据获取，得到相机的内参数。

在一种实现方式中，将标定与初始化依次进行，针对位于路线起点的标志(即起点标志)进行标定，需要不断移动拍摄相机从各个角度获取多张拍摄到起点标志的影像，并且尽量使起点标志在该影像上扫过整个影像区域，直至标定成功。由于视觉SLAM系统为实时采集与计算的系统，拍摄相机采集通常为固定的fps，如30fps，因此本实施例设置有一个可视化的屏幕输出，从该屏幕上可获知起点标志是否标定成功。标定成功后紧接着进行初始化，即缩小拍摄像机的移动，尽量正对着起点标志拍摄，且移动以平移为主。使用起点标志进行标定可以使得SLAM流程一体化，不需要在SLAM初始化之前进行单独标定和输入标定结果。

在另一种实现方式中，单独进行标定，针对布设的任意一个标志进行标定，首先同样需要通过拍摄相机获取拍摄到其他标志的影像，并通过源库OpenCV中的的ArUco模块自动识别出其他标志分别对应的各个角点(即各棋盘格交点)在影像上的二维坐标。最后将每一个标志的角点对应的二维坐标与每一个标志的角点对应的测量坐标数据输入特定的标定程序，并获取该标定程序基于输入的数据进行计算后输出的拍摄相机的内参数，即完成其他标志的标定。

在一种实现方式中，为了提高标志标定的成功率，需要保证标定时拍摄相机可移动范围足够大，尽量从不同角度、不同远近拍摄，使标志在多张影像中的位置覆盖全部影像区域。其次，需要保证标定区域的光照明亮且光照连续稳定，突出黑白色差。最后，需要保证标定区域后方的背景不能与黑白棋盘标志太相似。

标定完成以后，所有标志即可用于辅助视觉同步定位与建图。拍摄相机首先从移动路线起点开始移动并进行初始化，尽量正对着起点标志拍摄，且移动以平移为主当拍摄到位于起点位置的标志时，即可根据该起点标志生成一个局部的地图数据，即初始地图数据，并确定拍摄相机当前的位置信息。可以理解的是，随着拍摄相机继续移动该初始地图数据会不断进行更新，直至拍摄相机移动至移动路线的终点，即可得到关于整个场景的地图数据。

在一种实现方式中，所述步骤S100具体包括：

步骤S101、将所述标志中位于起点的标志作为起点标志，获取拍摄到所述起点标志的相邻两帧影像帧，并将所述相邻两帧影像帧分别对应的二维影像平面的相机坐标系作为第一坐标系和第二坐标系；

步骤S102、获取所述起点标志分别在所述第一坐标系和所述第二坐标系下的坐标数据，得到第一相机坐标数据和第二相机坐标数据；

步骤S103、获取所述起点标志对应的测量角点坐标数据，根据所述起点标志对应的测量角点坐标数据和所述第一相机坐标数据，确定相机相对于测量角点坐标系的第一位姿数据，并根据所述起点标志对应的测量角点坐标数据和所述第二相机坐标数据，确定相机相对于测量角点坐标系的第二位姿数据；

步骤S104、根据所述第一位姿数据和所述第二位姿数据，计算出相邻两帧影像帧的相对位姿；

步骤S105、将所述相邻两帧影像帧的相对位姿作为初始化的初值进行特征点匹配，当所述特征点匹配的结果满足预设条件时，确定初始化结果为成功，并以所述测量角点坐标数据为尺度基准，确定初始地图数据。

概括地讲，本实施例需要基于拍摄到起点标志、且符合预设条件的相邻两帧影像帧来判断初始化是否成功，若第一第二帧初始化不成功则判断之后第二第三帧影像帧是否能初始化成功，依此直到初始化成功后，确定初始地图数据。具体地，本实施例按照拍摄时间的先后顺序依次获取能够拍摄到起点标志的相邻两帧影像帧，并对该相邻两帧影像帧进行特征点匹配。当该相邻两帧影像帧经过特征点匹配并判断初始化成功时，即可根据该相邻两帧影像帧生成初始地图数据。当该相邻两帧影像帧经过特征点匹配后判断未通过初始化时，则获取并检测该相邻两帧影像帧后面拍摄到起点标志的连续两帧影像帧，直至初始化成功。其中，特征点检测和特征点描述可以使用较快速的方法从而实现实时处理，如使用FAST特征检测方法和二进制特征描述法(BRIEF、FREAK、BRISK、BinBoost等)。举例说明，可以使用FAST+BRIEF结合的ORB特征点。本实施例中的初始化方法利用标志辅助获得初始姿态，相比于使用H矩阵和F矩阵进行质量判断的传统方法有明显的时间和空间优势。

在一种实现方式中，根据所述ORB特征点匹配的结果确定初始化结果为成功的条件是：当所述相邻两帧影像帧都能提取大于特征阈值的特征点；且所述相邻两帧特征点的匹配成功数量大于匹配阈值；且所述相邻两帧特征点中特征匹配成功的点对数量大于匹配阈值；且第一平均重投影误差和第二平均重投影误差的平均值小于重投影误差阈值时，确定初始化结果为成功；其中，所述第一平均重投影误差为将计算出的所述相邻两帧影像帧中左帧的匹配成功的特征点的相对位姿投影到右帧得到的平均重投影误差；所述第二平均重投影误差为将计算出的所述右帧匹配成功的特征点的相对位姿投影到所述左帧得到的平均重投影误差。

初始化成功以后，以测量角点坐标数据为尺度基准。具体地，在所述相邻两帧影像帧中获取初始化成功的两帧中匹配成功的特征点的二维坐标，并根据所述二维坐标进行三角化计算，得到所有匹配成功的特征点分别对应的三维坐标。然后确定标志提取算法提取到的标志在初始化成功的两帧中的二维坐标，得到二维标志坐标。最后根据所述二维标志坐标以及初始化成功的两帧影像帧的相对位姿，计算所述标志上各个角点的三维坐标，得到计算角点坐标数据。

然后，根据所述测量角点坐标数据和所述计算角点坐标数据，确定比例信息。可以理解的是，标志的角点坐标数据可以反映标志的各个端点的位置信息，因此根据标志的角点坐标数据可以计算出标志的尺寸大小，而测量角点坐标数据和计算角点坐标数据会分别计算出同一个标志的尺寸，得到的2个尺寸，这2个尺寸的比值即为该比例信息。然后，根据所述比例信息确定所述起点标志对应的原始尺度数据，并将所述起点标志对应的原始尺度数据作为起点尺度数据。之后，将所述起点尺度数据与所有匹配成功的特征点对应的三维坐标相乘，得到所有匹配成功的特征点各自对应的真实尺度的三维坐标数据。最后，根据所述匹配成功的特征点各自对应的真实尺度的三维坐标数据，确定初始地图数据。

如图1所示，所述方法还包括如下步骤：

步骤S200、根据所述标志进行跟踪定位，得到所述拍摄相机的位姿数据。

具体地，本实施例采用同一套标志对拍摄相机进行跟踪定位，即随着拍摄相机的移动实时输出该拍摄相机对应的位置信息以及姿态信息。由于本实施例在一些容易跟踪失败的地方提前设置了标志，因此可以提高跟踪定位的成功率。

在一种实现方式中，所述步骤S200具体包括如下步骤：

步骤S201、当前影像帧跟踪失败时，确定跟踪失败的场景位置，并根据所述跟踪失败的场景位置确定新增标志；

步骤S202、根据所述新增标志进行跟踪定位，得到所述拍摄相机在所述当前影像帧中的位姿数据。

由于在一些空旷且场景特征较少的地方还是难以避免出现跟踪失败的情况，即根据当前影像帧无法确定拍摄相机的当前位姿数据。因此本实施例还提供了一种用于解决跟踪失败的方法。当出现当前视频帧跟踪失败的情况时，首先确定跟踪失败对应的场景位置，然后在该位置上重新增加新的标志，例如将新增标志贴在平面上或者贴在薄板上放置在该位置上。然后再重新进行跟踪定位，得到拍摄相机在当前影像帧中的位姿数据。简言之，本实施例通过把标志当做跟踪失败的场景的一部分，利用标志丰富的纹理来解决该场景跟踪失败的问题。

此外，本实施例还可以根据经验，提前在复杂场景布设标志(如光线阴暗、大面积的缺少纹理的墙面地面、狭长走道的直角转角等)。提前多张贴标志和试错增添标志的程度可以根据花费时间的预估来进行，如规划的SLAM路线较长，可以提前多在可能跟踪失败地方布设标志，从而避免只要失败一次就要从头再来；若路线较短，则可以试错。

在一种实现方式中，所述步骤S200还包括如下步骤：

步骤S203、当将所述新增标志作为纹理补充，并根据所述新增标志进行跟踪定位，仍无法得到所述拍摄相机在所述当前影像帧中的位姿数据时，获取所述当前影像帧之前的若干帧影像帧，并将所述当前影像帧之前的若干帧影像帧作为历史影像帧；

步骤S204、获取所述当前影像帧与所述历史影像帧中出现的相同ID的标志，将所述相同ID的标志作为辅助跟踪标志；

步骤S205、获取所述当前影像帧的相对位姿与所述历史影像帧的相对位姿，根据所述当前影像帧的相对位姿、所述历史影像帧的相对位姿以及所述辅助跟踪标志转换出所述拍摄相机在所述当前影像帧中的位姿数据。

其中，获取所述当前影像帧的相对位姿与所述历史影像帧的相对位姿的方法可以参考前述的初始化步骤S101-S104的方法。概括地讲，本实施例提供了两种解决场景跟踪失败的方法，当采用第一种方法(即步骤S201～S202对应的方法)还是无法解决场景跟踪失败的问题时，则采用第二种方法(即步骤S203～S205对应的方法)。在一种实现方式中，也可选取两种方法的更优者，当采用第一种方法成功解决了场景跟踪失败的问题时，将采用第一种方法计算出的拍摄相机在当前影像帧中的位姿数据，作为第一相机位姿数据。并继续采用第二种方法计算拍摄相机在当前影像帧中的位姿数据，将采用第二种方法计算拍摄相机在当前影像帧中的位姿数据，作为第二相机位姿数据。然后根据该第一相机位姿数据和该第二相机位姿数据分别计算当前视频帧与前一视频帧之间的重投影误差，得到第一重投影误差(对应第一相机位姿数据)，以及第二重投影误差(对应第二相机位姿数据)。然后，将所述第一重投影误差值与所述第二重投影误差值进行比较，并将数值最小的重投影误差值所对应的相机位姿数据，作为拍摄相机在当前影像帧中的位姿数据。

在一种实现方式中，为了提高相机位姿和地图坐标的精度，本实施例还需要进行闭环优化。现有技术中，虽然也存在闭环优化的操作，但是现有技术通常在若干个关键影像帧中筛选出，与当前影像帧中存在相同标志的影像帧作为当前影像帧对应的闭环备选帧。并且，在选择出闭环备选帧以后就不在对闭环备选帧进行任何检测了，会存在较多错误与冗余的闭环因此难以保障闭环优化的正确性和时间效率。本实施例中为了获得更好的闭环效果，需要对闭环备选帧进行再次筛选，从而更加精确地确定当前视频帧对应的闭环备选帧

具体地，当所述当前影像帧跟踪成功时，获取关键影像帧，并在所述关键影像帧中选择出与所述当前影像帧存在相同ID的标志的关键影像帧。然后，将所述与所述当前影像帧存在的相同ID的标志的关键影像帧，作为所述当前影像帧对应的闭环备选帧。之后，获取预设的帧数阈值和距离阈值，并获取所述闭环备选帧分别与所述当前影像帧之间的间隔帧数和直线距离值，将所述间隔帧数小于所述帧数阈值，且所述直线距离值小于所述距离阈值的闭环备选帧，作为有效闭环备选帧。当所述有效闭环备选帧前后连续预设数量的影像帧均为有效的闭环备选帧时，将所述有效闭环备选帧作为目标闭环帧。

举例说明，假设帧数阈值为1000帧，距离阈值为20米，则与当前视频帧间隔帧数为1050帧的闭环备选帧就是无效闭环帧，或者拍摄相机在当前视频帧中的位置与在某一闭环备选帧中的位置的直线距离为30米，则该闭环备选帧也是无效闭环帧。然后，判断该有效闭环备选帧前后连续预设数量的影像帧都为有效或无效闭环备选帧，以此来验证该有效闭环备选帧是否为正确的闭环帧(形成闭环需要对正确性precision的要求远高于召回率recall，即要遵循宁杀错不放过)。例如某一有效闭环备选帧及其前后各连续3帧都检测为有效的闭环备选帧，则将该有效闭环备选帧设定为目标闭环帧，并将该目标闭环帧前后帧设为无效闭环帧。

在一种实现方式中，当闭环备选帧与当前影像帧具有相同ID的标志时，可以采取比前述帧数阈值、距离阈值的数值更低的帧数阈值、距离阈值来检测其是否为无效闭环帧，以避免将正确的闭环备选帧错误地剔除。本实施例中采用标志辅助的闭环检测方法，相较于传统不使用标志的DBoW的特征词典方法有更好的时间效率和更少的内存占用，并相较于传统使用标志的闭环确定法可以更完善地剔除错误与冗余。

如图1所示，所述方法还包括步骤：

步骤S300、根据所述标志对所述地图数据进行调整，得到目标地图数据

具体地，视觉SLAM由于采集精度或者受到环境因素的干扰存在一定的误差，其最终生成的地图数据也会存在一定的误差。并且在实际采集中，当跟踪失败，系统为了避免崩溃可以新建一个地图，导致最后得到多个地图。因此本实施例最后还需要根据同一套标志对地图数据进行调整，从而提高地图的精度。

在一种实现方式中，所述步骤S300具体包括如下步骤：

步骤S301、当所述地图数据的数量大于1时，将所述地图数据作为待融合地图数据；

步骤S302、获取所述待融合地图数据中出现的相同ID的标志，并将所述待融合地图数据中出现的相同ID的标志作为融合标志；

步骤S303、获取所述融合标志在所述待融合地图数据各自对应的坐标系下的坐标数据，得到第一地图坐标数据和第二地图坐标数据；

步骤S304、根据所述第一地图坐标数据和所述第二地图坐标数据确定三维相似变换矩阵数据，并根据所述三维相似变换矩阵数据分别将所述待融合地图数据中的点的坐标数据转换至同一坐标系下，得到目标地图数据。

目前，当视觉SLAM系统输出多个地图数据时，通常需要刻意使拍摄路线经过之前访问的地方保证重定位成功，以实现将多个地图数据进行融合，若一直没有重定位成功，则跟踪失败后系统一直处于崩溃状态，极为不便。而本实施例提供的方法，可以实现自动融合多个地图数据。具体地，首先获取待融合地图数据中出现的相同ID的标志，即融合标志，然后获取该融合标志在待融合地图数据各自对应的坐标系下的坐标数据，并以此确定一个三维相似变换矩阵数据，由于该三维相似变换矩阵数据可以反映不同待融合地图之间的坐标转换关系，因此可以根据所述三维相似变换矩阵数据分别将所述待融合地图数据中的点的坐标数据转换至同一坐标系下，得到目标地图数据，从而实现将待融合地图数据进行融合。

在一种实现方式中，由于视觉SLAM地图中存在一定的尺度误差，如图4所示，在跟踪过程中当前影像帧新加入的局部地图的尺寸相对于上一帧影像帧的局部地图尺寸有细微变化。例如上一帧影像帧中，一个物体的大小与真实世界坐标系相符，下一帧影像帧由于跟踪的误差导致物体整体缩放一定比例，该缩放通常由系统误差导致，比例变化误差会相对比较稳定，以同样的趋势进行累积。图4右上角为起点，该路径真实应是一个闭合的矩形，但由于尺度误差的累积导致地图越来越小。因此为了提高地图的精度，本实施例还需要对地图数据进行尺度修正。

具体地，本实施例中在第二模块(跟踪与闭环)中已经存储了之前所有关键影像帧对应的相机位姿数据。本实施例首先需要对终点影像帧的尺度进行修正：本实施例将位于终点的标志作为终点标志，将拍摄到所述终点标志的影像帧作为终点影像帧。然后，获取所述终点标志的测量角点坐标数据，根据所述终点标志的测量角点坐标数据确定所述终点标志对应的原始尺度数据，得到原始终点尺度数据。之后，根据所述起点尺度数据和所述原始终点尺度数据，确定目标终点尺度数据。例如，可以根据原始起点尺度数据对原始终点尺度数据进行一定比例的缩放，以得到目标终点尺度数据，可以理解的是，由于原始起点尺度数据是正确的尺度，因此可以将其作为尺度修正过程中的标准参数。修正完终点影像帧的尺度以后，本实施例再对中间影像帧的尺度进行修正：根据所述起点尺度数据和所述目标终点尺度数据对中间影像帧对应的尺度数据进行修正，从而影响每两帧之间的相对位姿和匹配成功的特征点对应的三维坐标，修正后得到新的轨迹位姿与目标地图数据；所述中间影像帧为位于起点影像帧与所述终点影像帧之间的影像帧；所述起点影像帧为拍摄到所述起点标志的影像帧。在一种实现方式中，对中间影像帧的修订可以基于特定的平滑算法实现。在一种实现方式中，将中间能拍摄到标志的关键影像帧也可作为终点影像帧一样的尺度标准，其原因是各个不同ID的标志的测量坐标(正确尺度)均已知。

由于视觉SLAM系统生成的目标地图数据仅在自身对应的坐标系统中应用是不现实的，通常需要根据应用任务，将目标地图数据中的点转换至与应用任务相符的坐标系下。因此，在一种实现方式中，本实施例还可以获取应用任务，根据应用任务确定目标坐标系。然后，获取所述目标地图数据对应的地图坐标系，并确定同时位于所述地图坐标系中与所述目标坐标系中的预设个数的标志，得到辅助转换标志。之后，获取所述辅助转换标志在所述地图坐标系下的坐标数据，得到第一转换坐标数据，并获取所述辅助转换标志在所述目标坐标系下的坐标数据，得到第二转换坐标数据。最后，根据所述第一转换坐标数据和所述第二转换坐标数据，确定转换矩阵，并根据所述转换矩阵将所述目标地图数据中的点的坐标数据转换至所述目标坐标系中。在一种实现方式中，所述第一转换坐标数据可以使用传统测绘领域的仪器全站仪或者SLAM跟踪步骤中的标志提取获得，所述第二转换坐标数据可以通过固定站GPS、架设在已知坐标的控制点上的全站仪或计算机视觉等方法测量。

其次，在另一种实现方式中，还可以将标志作为参照物，通过参照物匹配或者特征匹配的方法，实现将所述目标地图数据中的点的坐标数据转换至所述目标坐标系中。具体地，由于一些情况下无法使用GPS等方法确定第二转换坐标数据。例如对于送餐机器人，已经获取到该送餐机器人对应的活动范围的平面图数据，假设需要确定该送餐机器人在平面图中的位置，则需要先确定作为参照物的标志在该平面图中的位置信息，例如作为参照物的标志位于一扇门的正中心，或者作为参照物的标志位于墙中心垂直2米的位置。再结合该作为参照物的标志在利用视觉SLAM系统构建的地图中的位置信息，从而转换出该送餐机器人在该平面中的位置。

基于上述实施例，本发明还提供一种使用标志辅助视觉SLAM的装置，如图5所示，该装置包括：

初始化模块01，用于获取预设的标志，根据所述标志确定初始地图数据；

跟踪与闭环模块02，用于根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；

地图调整模块03，用于根据所述标志对所述地图数据进行调整，得到目标地图数据。

综上所述，本发明公开了一种使用标志辅助视觉SLAM的方法，通过在视觉SLAM的初始化、跟踪定位以及地图优化等步骤中使用预先设置的同一套标志辅助定位与建图，提高了视觉SLAM系统的精度和鲁棒性，提高了时间效率与空间效率。解决了现有技术中由于视觉SLAM系统的鲁棒性受限于复杂环境与动态环境，从而导致跟踪失败的问题。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (9)

1.一种使用标志辅助视觉SLAM的方法，其特征在于，所述方法包括：

获取预设的标志，根据所述标志确定初始地图数据；

根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，并根据所述拍摄相机的位姿数据对所述初始地图数据进行更新，得到地图数据；

根据所述标志对所述地图数据进行调整，得到目标地图数据；

所述获取预设的标志，根据所述标志确定初始地图数据，包括：

将所述标志中位于起点的标志作为起点标志，获取拍摄到所述起点标志的相邻两帧影像帧，并将所述相邻两帧影像帧分别对应的二维影像平面的相机坐标系作为第一坐标系和第二坐标系；

获取所述起点标志分别在所述第一坐标系和所述第二坐标系下的坐标数据，得到第一相机坐标数据和第二相机坐标数据；

获取所述起点标志对应的测量角点坐标数据，根据所述起点标志对应的测量角点坐标数据和所述第一相机坐标数据，确定相机相对于测量角点坐标系的第一位姿数据；并根据所述起点标志对应的测量角点坐标数据和所述第二相机坐标数据，确定相机相对于测量角点坐标系的第二位姿数据；

根据所述第一位姿数据和所述第二位姿数据，计算出相邻两帧影像帧的相对位姿；

将所述相邻两帧影像帧的相对位姿作为初始化的初值进行特征点匹配，当所述特征点匹配的结果满足预设条件时，确定初始化结果为成功，并以所述测量角点坐标数据为尺度基准，确定初始地图数据。

2.根据权利要求1所述的使用标志辅助视觉SLAM的方法，其特征在于，所述当所述特征点匹配的结果满足预设条件时，确定初始化结果为成功，包括：

当所述相邻两帧影像帧都能提取大于特征阈值的特征点；且所述相邻两帧影像帧中特征点的匹配成功数量大于匹配阈值；且所述相邻两帧影像帧中特征匹配成功的点对数量大于匹配阈值；且第一平均重投影误差和第二平均重投影误差的平均值小于重投影误差阈值时，确定初始化结果为成功；

其中，所述第一平均重投影误差为将计算出的所述相邻两帧影像帧中左帧的匹配成功的特征点的相对位姿投影到右帧得到的平均重投影误差；所述第二平均重投影误差为将计算出的所述相邻两帧影像帧中右帧匹配成功的特征点的相对位姿投影到左帧得到的平均重投影误差。

3.根据权利要求2所述的使用标志辅助视觉SLAM的方法，其特征在于，所述以所述测量角点坐标数据为尺度基准，确定初始地图数据，包括：

在所述相邻两帧影像帧中获取初始化成功的两帧中匹配成功的特征点的二维坐标，并根据所述二维坐标进行三角化计算，得到所有匹配成功的特征点分别对应的三维坐标；

确定标志提取算法提取到的标志在初始化成功的两帧中的二维坐标，得到二维标志坐标，根据所述二维标志坐标以及初始化成功的两帧影像帧的相对位姿，计算所述标志上各个角点的三维坐标，得到计算角点坐标数据；

根据所述测量角点坐标数据和所述计算角点坐标数据，确定比例信息；

根据所述比例信息确定所述起点标志对应的原始尺度数据，并将所述起点标志对应的原始尺度数据作为起点尺度数据；

将所述起点尺度数据与所有匹配成功的特征点对应的三维坐标相乘，得到所述匹配成功的特征点各自对应的真实尺度的三维坐标数据；

根据所述匹配成功的特征点各自对应的真实尺度的三维坐标数据，确定初始地图数据。

4.根据权利要求1所述的使用标志辅助视觉SLAM的方法，其特征在于，所述根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，包括：

当前影像帧跟踪失败时，确定跟踪失败的场景位置，并根据所述跟踪失败的场景位置确定新增标志；

根据所述新增标志进行跟踪定位，得到所述拍摄相机在所述当前影像帧中的位姿数据。

5.根据权利要求4所述的使用标志辅助视觉SLAM的方法，其特征在于，所述根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，还包括：

当将所述新增标志作为纹理补充，并根据所述新增标志进行跟踪定位，仍无法得到所述拍摄相机在所述当前影像帧中的位姿数据时，获取所述当前影像帧之前的若干帧影像帧，并将所述当前影像帧之前的若干帧影像帧作为历史影像帧；

获取所述当前影像帧与所述历史影像帧中出现的相同ID的标志，将所述相同ID的标志作为辅助跟踪标志；

获取所述当前影像帧的相对位姿与所述历史影像帧的相对位姿，根据所述当前影像帧的相对位姿、所述历史影像帧的相对位姿以及所述辅助跟踪标志转换出所述拍摄相机在所述当前影像帧中的位姿数据。

6.根据权利要求5所述的使用标志辅助视觉SLAM的方法，其特征在于，所述根据所述标志进行跟踪定位，得到拍摄相机的位姿数据，还包括：

当所述当前影像帧跟踪成功时，获取关键影像帧，并在所述关键影像帧中选择出与所述当前影像帧存在相同ID的标志的关键影像帧；

将所述与所述当前影像帧存在的相同ID的标志的关键影像帧，作为所述当前影像帧对应的闭环备选帧；

获取预设的帧数阈值和距离阈值，并获取所述闭环备选帧分别与所述当前影像帧之间的间隔帧数和直线距离值，将所述间隔帧数小于所述帧数阈值，且所述直线距离值小于所述距离阈值的闭环备选帧，作为有效闭环备选帧；

当所述有效闭环备选帧前后连续预设数量的影像帧均为有效的闭环备选帧时，将所述有效闭环备选帧作为目标闭环帧。

7.根据权利要求3所述的使用标志辅助视觉SLAM的方法，其特征在于，所述根据所述标志对所述地图数据进行调整，得到目标地图数据，包括：

当所述地图数据的数量大于1时，将所述地图数据作为待融合地图数据；

获取所述待融合地图数据中出现的相同ID的标志，并将所述待融合地图数据中出现的相同ID的标志作为融合标志；

获取所述融合标志在所述待融合地图数据各自对应的坐标系下的坐标数据，得到第一地图坐标数据和第二地图坐标数据；

根据所述第一地图坐标数据和所述第二地图坐标数据确定三维相似变换矩阵数据，并根据所述三维相似变换矩阵数据分别将所述待融合地图数据中的点的坐标数据转换至同一坐标系下，得到目标地图数据。

8.根据权利要求7所述的使用标志辅助视觉SLAM的方法，其特征在于，所述根据所述标志对所述地图数据进行调整，得到目标地图数据，还包括：

将位于终点的标志作为终点标志，将拍摄到所述终点标志的影像帧作为终点影像帧；

获取所述终点标志的测量角点坐标数据，根据所述终点标志的测量角点坐标数据确定所述终点标志对应的原始尺度数据，得到原始终点尺度数据；

根据所述起点尺度数据和所述原始终点尺度数据，确定目标终点尺度数据

根据所述起点尺度数据和所述目标终点尺度数据对中间影像帧对应的尺度数据进行修正，从而影响每两帧之间的相对位姿和匹配成功的特征点对应的三维坐标，修正后得到新的轨迹位姿与目标地图数据；所述中间影像帧为位于起点影像帧与所述终点影像帧之间的影像帧；所述起点影像帧为拍摄到所述起点标志的影像帧。

9.根据权利要求1所述的使用标志辅助视觉SLAM的方法，其特征在于，所述方法还包括：

获取应用任务，根据应用任务确定目标坐标系；

获取所述目标地图数据对应的地图坐标系，并确定同时位于所述地图坐标系中与所述目标坐标系中的预设个数的标志，得到辅助转换标志；

获取所述辅助转换标志在所述地图坐标系下的坐标数据，得到第一转换坐标数据，并获取所述辅助转换标志在所述目标坐标系下的坐标数据，得到第二转换坐标数据；

根据所述第一转换坐标数据和所述第二转换坐标数据，确定转换矩阵，并根据所述转换矩阵将所述目标地图数据中的点的坐标数据转换至所述目标坐标系中。

Images