具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。

图1为本实施例提供的一种视觉SLAM的增量建图方法的流程示意图。如图1所示，该方法的主要流程描述如下(步骤S100～S500)：

步骤S100，基于传感器数据获取视觉特征和里程计数据；

本实施例中，传感器数据包括图像数据和尺度观测数据。其中，图像数据的图像形式可以为单目、双目或者多目视觉，可以由普通相机、鱼眼相机等图像采集装置进行采集。

对图像数据提取视觉描述子，视觉特征可以为FAST角点、Harris角点、SIFT、SURF及其变种，描述子可以选取HOG描述子、BRIEF描述子等，也可以使用深度学习的描述子。为保证描述子提取效率，可以选取FAST角点和BRIEF描述子的组合进行视觉特征的描述。

尺度观测数据由尺度观测里程计采集得到，可以使用轮式编码器、轮式编码器+惯性测量单元、RTK、GPS中的一种或多种装置构成尺度观测里程计，对于尺度观测里程计，只要能构成具有真实尺度信息的里程计即可，对此本实施例不做具体限定。

以轮式编码器+惯性测量单元为例，通过轮式编码器采集位移量，通过惯性测量单元采集旋转量，将位移量和旋转量构成里程计，里程计数据作为移动机器人相对定位的有效传感器，为机器人提供了实时的位姿信息，用于后续的尺度恢复。

步骤S200，获取第一地图，并基于第一地图、视觉特征和里程计数据对第二地图进行初始化；

本实施例中，将增量建图的基准地图即原始地图称为第一地图，将增量地图称为第二地图。第一地图是由其他建图模块提供的，其由图像、轮速等建图数据构建而成。

对第二地图的初始化为增量建图的开启方法，只要满足当前帧在第一地图上获得唯一的具有高置信度的定位即可。

作为本实施例的一种可选实施方式，如图2所示，步骤S200包括：

步骤S210，基于第一地图中的关键帧和地图点构建第二地图的固定区域；

使用第一地图初始化第二地图，将第一地图中的关键帧和地图点复制到第二地图中，成为第二地图的固定区域，因此，第一地图与第二地图的固定区域的元素存在一一对应的关系。

步骤S220，基于视觉特征和里程计数据建立临时地图；

对当前帧与前一帧的图像特征点进行语义匹配和几何约束检验，得到第一匹配关系；基于第一匹配关系计算当前帧与前一帧对应里程计提供的相对位姿，使用三角化方法对临时地图进行初始化；利用当前帧与参考关键帧的匹配关系以及当前帧和参考关键帧对应里程计提供的相对位姿，建立第二地图的新增区域的关键帧和地图点。

本实施例中，帧代表由每一帧图像生成的数据结构，关键帧是指经过筛选后保存到地图中的帧。虽然帧不存在于地图中，但帧可以在地图上进行定位。对于帧来说，与其共视点最多的关键帧称为参考关键帧。

步骤S230，基于视觉特征在第二地图中进行重定位；

先查找与当前帧相似的关键帧作为候选关键帧，将当前帧与候选关键帧的图像特征点进行语义匹配和几何约束检验，得到第二匹配关系。可以采用词袋模型、VLAD系列方法等方法对候选关键帧进行筛选，对此本实施例不做具体限定。

然后，基于第二匹配关系计算当前帧与候选关键帧对应里程计提供的相对位姿；具体可以采用RANSAC+PnP的方法进行相对位姿的计算，对此本实施例不做具体限定。

再基于当前帧与候选关键帧对应里程计提供的相对位姿获取第二地图的局部地图，并基于该局部地图对关键帧的位姿进行优化，完成重定位操作。

步骤S240，若当前帧在第二地图中的定位置信度超过第一预设阈值τ，则将第二地图与临时地图进行对齐处理；

本实施例中，可以选取当前帧在第二地图中观测到的地图点的数量作为定位置信度计算的方法，即，若当前帧在第二地图中观测到的地图点的数量超过第一预设阈值τ，此时定位具有高置信度，则基于当前帧在第二地图中的定位位姿和当前帧在临时地图中的位姿计算临时地图与第二地图的相对位姿基于相对位姿将临时地图旋转至第二地图的坐标系下。

可选的，第一预设阈值τ的计算方式如下：

利用第一地图的建图数据在第一地图上进行定位，并记录每一帧观测到的地图点的数量，生成集合N；基于集合N的均值μ_N与方差σ_N计算第一预设阈值τ，计算公式为τ＝μ_N-3*σ_N。其中，由于仅是在第一地图上进行定位操作，因此，只需建图数据中的图像数据即可。

当定位置信度不满足上述条件，即当前帧在第二地图中观测到的地图点的数量没有超过第一预设阈值τ时，重复上述操作，直到获取到高置信度的定位，再进行对齐处理。

步骤S250，将对齐处理后的第二地图与临时地图进行融合、替换。

使用当前帧在第二地图中观测到的地图点集合与当前帧在临时地图中观测到的地图点集合进行融合、替换，并进行局部优化，完成第二地图的初始化。

其中，N为当前帧提取到的特征点的数量，与表示与当前帧中第i个特征点关联的第二地图、临时地图中的地图点，当当前帧的第i个特征点在临时地图中没有对应的地图点时，为空。

步骤S300，利用当前帧在第一地图和初始化后的第二地图中分别进行协同定位，得到第一定位信息和第二定位信息；

本步骤用以给出符合第一地图尺度的第二地图的初始位姿，以方便后续的建图操作，并为后续的矫正操作提供在第一地图中的参考位姿。

首先，利用视觉特征在第一地图上进行定位，得到当前帧在第一地图中的定位信息在常见的定位方法中，首先会通过RANSAC+PnP或其他方法给出当前帧和参考关键帧的相对位置关系，并根据参考关键帧的位置确定当前帧的初值，再在结合局部地图中的点与当前帧的匹配关系，使用优化的方法完成定位。

若定位信息中的置信度大于第二预设阈值τ_C，则将定位信息作为当前帧在第二地图中的初始定位信息利用第二地图的局部地图对初始定位信息进行优化，得到当前帧在第二地图中的定位信息

由于第二地图包含第一地图的全部信息，即第一地图与第二地图的固定区域的元素存在一一对应关系，因此，可以将当前帧在第一地图的定位结果作为在第二地图定位的初值，进行定位，该操作总是可以成功。

若定位信息中的置信度不大于第二预设阈值τ_C，则基于视觉特征和里程计数据得到当前帧在第二地图中的初始位姿，在初始位姿的基础上利用第二地图的局部地图与当前帧的匹配关系进行定位，得到当前帧在第二地图中的定位信息由于第二地图包含新增区域，因此，第二地图的置信度总是较高的，该定位结果可信。

将定位信息作为当前帧在第一地图中的初始定位信息利用第一地图的局部地图对初始定位信息进行优化，得到当前帧在第一地图中的定位信息若定位信息中的置信度大于置信度则将定位信息作为第一定位信息，并将定位信息作为第二定位信息。

步骤S400，基于第二定位信息对初始化后的第二地图进行融合建图，得到第三定位信息；

作为本实施例的一种可选实施方式，如图3所示，步骤S400包括：

步骤S410，基于第二定位信息和固定区域中的关键帧建立新增区域的地图点；

基于语义条件和几何约束条件对当前关键帧与固定区域中的临近关键帧的图像特征点进行匹配。在当前关键帧K_i与其临近关键帧中，用特征表示关键帧K_i的第m个特征，特征表示关键帧K_j的第n个特征。当这两个特征描述子的汉明距离大于给定阈值时，判定该两个特征满足语义条件。

在几何约束条件判别中，已知特征点在图像中的位置、相机的内参、两帧图像对应的相对位姿，使用辛普森误差判定特征与是否满足几何约束。在同时满足语义条件和几何约束条件的情况下，认为特征与匹配。

本实施例中，临近关键帧是指与当前关键帧具有一定可视地图点数量的关键帧。

对于匹配成功的两个图像特征点，若两个图像特征点对应的地图点均为空，则基于两个图像特征点在两帧图像中的位置以及两帧图像的位姿获取两个图像特征点对应的地图点p1，将地图点p1赋值给两个图像特征点，并将地图点p1加入新增区域中；若两个图像特征点对应的地图点有且仅有一个为空，则将非空值赋值给值为空的地图点；若两个图像特征点对应的地图点均为非空，则判断两个图像特征点对应的地图点中是否存在属于固定区域的地图点，若是，则由属于固定区域的地图点替代另一个地图点，若否，则任选一个地图点替代另一个地图点。

步骤S420，基于第二定位信息和新增区域的关键帧建立新增区域的地图点；

建立地图点包括筛选匹配特征点和三角化特征点两个部分。

首先，基于语义条件和几何约束条件对当前关键帧与新增区域中的临近关键帧的图像特征点进行匹配。

对于匹配成功的两个图像特征点，若两个图像特征点对应的地图点均为空，则基于两个图像特征点在两帧图像中的位置以及两帧图像的位姿获取两个图像特征点对应的地图点p2，将地图点p2赋值给两个图像特征点，并将地图点p2加入新增区域中；若两个图像特征点对应的地图点有且仅有一个为空，则将非空值赋值给值为空的地图点；若两个图像特征点对应的地图点均为非空，则判断两个图像特征点对应的地图点中是否存在属于新增区域的地图点，若是，则由属于新增区域的地图点替代另一个地图点，若否，则任选一个地图点替代另一个地图点。

步骤S430，若新增区域的地图点和/或关键帧符合预设冗余条件，则将对符合预设冗余条件的地图点和/或关键帧进行删除处理；

具体的，遍历新增区域的地图点，若地图点的可见率低于第三预设阈值，则认为该地图点携带的信息量较低，删除该地图点；和/或，遍历新增区域的关键帧，若关键帧的可视地图点与其他关键帧存在预设重叠区域，例如95％，则移除该关键帧，以减少地图的冗余。

步骤S440，对新增区域的位姿进行局部优化，得到第三定位信息。

在进行局部优化的过程中，锁定第二地图中的固定区域，其参与位姿优化计算但位姿不更新；对于第二地图中的新增区域的位姿通过局部优化进行更新，输出第三定位信息。

通过步骤S400，将当前关键帧与不同优先级的关键帧进行三角化，确保新增关键帧与地图点更为贴合第一地图的尺度。

步骤S500，基于第一定位信息和第三定位信息对融合建图后的第二地图进行矫正，得到增量地图。

地图矫正可分为三个环节：位姿对齐、可视关系的更新以及pose graph优化与全局优化。参与对齐、可视关系调整的关键帧是当前帧及近邻帧中在第一地图中具有高置信度定位的关键帧，这些关键帧均来自于第二地图的新增部分这些关键帧的集合记为

首先，需要判断当前关键帧在第一定位信息中的置信度是否不小于第四预设阈值，若是，则判断当前关键帧的第一定位信息与第三定位信息之间的差异是否满足预设差异条件。

可选的，预设差异条件包括以下的至少一种条件：

(1)第一定位信息与第三定位信息中的位姿的平移量差异超过第四预设阈值，例如，第四预设阈值可设置为0.5；

(2)第一定位信息与第三定位信息中的位姿的旋转角差异超过预设角度，例如，预设角度可设置为3度；

(3)当前关键帧在第二地图的固定区域观测到的地图点数量与当前关键帧在第一地图观测到的地图点数量的绝对值之比小于第五预设阈值，例如，

若满足预设差异条件，则将当前关键帧在第一地图中的定位位姿设置为该关键帧在第二地图的位姿，完成位姿对齐。具体的，对于关键帧K_i，将其在第一地图中的定位位姿设置为其在第二地图的位姿即完成位姿对齐。

然后，基于当前关键帧在第一地图中与可视地图点的可视关系更新第二地图的可视关系。

具体的，使用关键帧K_i在第一地图中的可视地图点 对其在第二地图中的可视地图点进行融合，其中为当前帧K_i的特征点数量，分别表示关键帧K_i的第j个特征点对应的第一、二地图中的可视地图点。

如果存在，在第二地图的对应地图点来自于第二地图的新增区域，且与不同，则使用地图点代替并在地图中移除地图点

最后，对新增区域进行pose graph优化和全局优化。

在pose graph优化中，获取第二地图的关键帧集合以及固定部分的关键帧集合固定关键帧集合中的位姿以及当前关键帧K_c的位姿。使用更新前位姿的相对位姿作为目标，使用更新后位姿作为观测，进行优化，目标函数如下：

在全局优化中，使用第二地图的关键帧的位姿、地图点位置作为优化变量，关键帧与地图点的可视关系作为约束，重投影误差作为目标函数，进行局部地图优化。在优化过程中，固定来自于第二地图的固定部分的关键帧的位姿和地图点的位置。目标函数如下：

其中，表示第二地图中关键帧的数量，表示第二地图中地图点的数量，z_ij表示第i个关键帧观察第j个地图点的测量值，T_i表示第i个关键帧的位姿，p_j表示第j个地图点，g(T_i，p_j)表示地图点p_j在位姿T_i下的观测值。

pose graph优化和全局优化是一种通用的办法，实际上是对从当前关键帧开始到上一个具有高置信度定位的新建关键帧为止的关键帧对应的局部地图的优化，在优化过程中固定局部地图中固定部分的位姿和位置。

通过上述矫正方法，能够应对因光照、环境差异产生的在第一地图定位效果不好、短期定位丢失或因地图区域扩展产生的长期定位丢失的带来的新建地图尺度改变的情况，提高系统的鲁棒性。

需要说明的是，置信度阈值的计算方法因置信度的计算方法而异，对于同样的置信度计算方法可以有多种余值计算方法，如高斯分布下的三西格玛准则，假设检验等方法。本申请实施例使用的视觉SLAM主体框架可以为ORB_SLAM、VINS等系列框架；只要在增量建图中使用的SLAM框架与原有地图建立时使用的框架保持一致即可，对此本实施例不做具体限定。

图4为本申请实施例提供的一种视觉SLAM的增量建图装置600的结构框图。如图4所示，该视觉SLAM的增量建图装置600主要包括：

获取模块610，用于基于传感器数据获取视觉特征和里程计数据；

初始化模块620，用于获取第一地图，并基于第一地图、视觉特征和里程计数据对第二地图进行初始化；

协同定位模块630，用于利用当前帧在第一地图和初始化后的第二地图中分别进行协同定位，得到第一定位信息和第二定位信息；

融合建图模块640，用于基于第二定位信息对初始化后的第二地图进行融合建图，得到第三定位信息；

矫正模块650，用于基于第一定位信息和第三定位信息对融合建图后的第二地图进行矫正，得到增量地图。

本申请实施例中的各功能模块可以集成在一起形成一个独立的单元，例如集成在一个处理单元中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成形成一个独立的单元。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台电子设备(可以是个人计算机、服务器或者网络设备等)或处理器(processor)执行本申请各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。

本申请实施例提供的方法中的各种变化方式和具体实例同样适用于本实施例提供的一种视觉SLAM的增量建图装置，通过前述对视觉SLAM的增量建图方法的详细描述，本领域技术人员可以清楚的知道本实施例中的视觉SLAM的增量建图装置的实施方法，为了说明书的简洁，在此不再详述。

图5为本申请实施例提供的一种机器人700的结构框图。如图5所示，机器人700包括存储器701、处理器702、通信总线703、通信接口704、传感器设备705；存储器701、处理器702、通信接口704通过通信总703线相连。

传感器设备705与通信接口704连接，用于采集传感器数据。传感器设备705可以包括但不限于图像采集装置和尺度观测里程计，其中，图像采集装置可以是普通相机、鱼眼相机等，尺度观测里程计可以由轮式编码器、轮式编码器+惯性测量单元、RTK、GPS中的一种或多种装置构成。

存储器701可用于存储指令、程序、代码、代码集或指令集。存储器701可以包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于至少一个功能的指令以及用于实现上述实施例提供的视觉SLAM的增量建图方法的指令等；存储数据区可存储上述实施例提供的视觉SLAM的增量建图方法中涉及到的数据等。

处理器702可以包括一个或者多个处理核心。处理器702通过运行或执行存储在存储器701内的指令、程序、代码集或指令集，调用存储在存储器701内的数据，执行本申请的各种功能和处理数据。处理器702可以为特定用途集成电路(Application SpecificIntegrated Circuit，ASIC)、数字信号处理器(Digital Signal Processor，DSP)、数字信号处理装置(Digital Signal Processing Device，DSPD)、可编程逻辑装置(ProgrammableLogic Device，PLD)、现场可编程门阵列(Field Programmable Gate Array，FPGA)、中央处理器(Central Processing Unit，CPU)、控制器、微控制器和微处理器中的至少一种。可以理解地，对于不同的设备，用于实现上述处理器702功能的电子器件还可以为其它，本申请实施例不作具体限定。

通信总线703可包括一通路，在上述组件之间传送信息。通信总线703可以是PCI(Peripheral Component Interconnect，外设部件互连标准)总线或EISA(ExtendedIndustry Standard Architecture，扩展工业标准结构)总线等。通信总线703可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一个双箭头表示，但并不表示仅有一根总线或一种类型的总线。

其中，机器人包括但不限于：自动驾驶车辆、物流机器人、扫地机器人、移动机器人、AR机器人。图5示出的机器人仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。

本申请实施例提供一种计算机可读存储介质，存储有能够被处理器加载并执行如上述实施例提供的视觉SLAM的增量建图方法的计算机程序。

本实施例中，计算机可读存储介质可以是保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。具体的，计算机可读存储介质可以是便携式计算机盘、硬盘、U盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、讲台随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、光盘、磁碟、机械编码设备以及上述任意组合。

本实施例中的计算机程序包含用于执行图1所示的方法的程序代码，程序代码可包括对应执行上述实施例提供的方法步骤对应的指令。计算机程序可从计算机可读存储介质下载到各个计算/处理设备，或者通过网络(例如因特网、局域网、广域网和/或无线网)下载到外部计算机或外部存储设备。计算机程序可完全地在用户计算机上执行、作为一个独立的软件包执行。

在本申请所提供的实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

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

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。