CN114088081A - 一种基于多段联合优化的用于精确定位的地图构建方法 - Google Patents

Abstract

一种基于多段联合优化的用于精确定位的地图构建方法，属于基于先验地图的机器人或自动驾驶汽车精确定位领域。本发明(1)对需要构建定位地图的场景，使用基于特征点的视觉SLAM方法，通过视觉里程计与局部地图优化得到多组单段子地图，其中视觉里程计和局部地图优化是在两个单独的线程中并行运行；(2)利用关键帧中的ORB描述子，并采用基于词袋模型的场景识别策略进行子地图之间快速的重叠检测；(3)利用分配给每个子地图的锚点，在一个全局坐标系中执行多段位姿图优化；(4)先将所有子地图合并为一个整体地图，然后对整个地图执行全局BA优化，从而得到更准确的可用于精确定位的离线地图。

Description

一种基于多段联合优化的用于精确定位的地图构建方法

技术领域

本发明中设计一种基于多段联合优化的用于精确定位的地图构建方法。本方法只使用廉价的双目相机作为输入传感器，并由两个关键模块组成。第一个模块是双目视觉SLAM 前端，其利用基于特征点的视觉SLAM(Simultaneous Localization and Mapping，同时定位与地图构建)方法对同一场景分别获得多个单段子地图。第二个关键模块是多段合并后端，在这个模块中，对多个单段子地图进行联合优化，从而得到更准确的地图。该方法可应用于基于先验地图的机器人或自动驾驶汽车精确定位领域，可以使用低成本、低精度的相机传感器来获得用于精确定位的先验地图。

背景技术

随着移动机器人与自动驾驶汽车的不断发展，基于先验的离线地图进行精确定位的方式得到了越来越广泛的应用，传统的构建精确的定位地图的方法都使用了昂贵的传感器，这给精确地图构建的普遍化和商业化带来了困难。

目前，构建用于定位的离线地图的方法主要是通过在目标场景中执行在线的SLAM算法来得到离线地图。一般来说，SLAM算法按传感方式分为两大类：激光雷达SLAM和视觉SLAM。激光雷达SLAM无需借助耗时的三角测量或立体装备，可以直接恢复场景中的三维散乱点并且可以产生用于精确定位的点云地图。但是它的缺点是其高的功率消耗和计算要求以及高的实施成本。相比之下，基于相机的技术精度较低，但成本明显更低，因此这种方法具有很大的潜力。视觉SLAM方法在机器人应用中被广泛采用，通常按照是否使用特征点与描述子可以分为稀疏特征点法和直接法。但是由于直接法依赖于光度一致性的假设，所以直接法并不适合用来产生用于定位的离线地图，而通过稀疏特征点法可以生成具有稀疏路标点定位地图，但是仅仅使用视觉SLAM的方法并不能对一个场景构建出精确的先验地图。近些年，也有基于多段联合优化来构建目标场景的用于精确定位的先验地图的方法被提出。例如，Schuster等人提出的基于毫米波雷达的众包建图方法(F.Schuster,W. Zhang,C.Keller,M.Haueis,and C.Curio,“Joint graph optimization towards crowd basedmapping,”in Proc.IEEE 20th International Conference on IntelligentTransportation Systems (ITSC).IEEE,Oct 2017,pp.1–6)，该方法通过联合优化目标场景的多个稀疏特征地图来提高建图精度。但是，相比相机传感器，该方法所使用的毫米波雷达费用仍然较高，而且充噪声的雷达数据使得特征提取变得更为复杂，从而影响精度。

因此，为了使用低成本传感器生成精确的定位地图，本发明提出一种基于多段联合优化的用于精确定位的地图构建方法。首先以双目相机作为传感器，利用基于特征点的视觉 SLAM方法对同一场景分别获得多个单段子地图，然后对多个单段子地图进行多段联合优化，依靠来自同一场景的多个数据来补偿传感器的低精度，最终获得精确的定位地图。

发明内容

本发明通过联合优化来自相同场景的多个低精度的单段子地图，来获得该场景的精确的可用于定位的离线地图。整个系统由两个关键模块组成。第一个模块是双目视觉SLAM 前端，其利用基于特征点的视觉SLAM方法对同一场景分别获得多个单段子地图。第二个关键模块是多段合并后端，在这个模块中，对多个单段子地图进行联合优化，从而得到更准确的定位地图。

为了实现上述目的，本发明提供了如下方案：

一种基于多段联合优化的用于精确定位的地图构建方法，所述方法包括：

步骤1：获得包含关键帧和稀疏地图点的单段子地图；

步骤2：单段子地图之间的重叠检测；

步骤3：多段位姿图联合优化；

步骤4：对整体地图执行全局BA(Bundle Adjustment，光束平差法)优化。

所述的单段子地图的获得过程中：

使用基于特征点的视觉SLAM方法，通过视觉里程计与局部地图优化得到单段子地图，其中视觉里程计和局部地图优化是在两个单独的线程中并行运行。视觉里程计线程将定位相机的每一帧，并决定是否将当前帧生成为新的关键帧。局部地图优化线程只处理关键帧，并执行局部BA优化，以实现对当前关键帧的周围环境的最优重建。视觉里程计线程创建的新关键帧将在局部地图中建立与其他关键帧的连接，并使用相连接的关键帧之间新的对应的ORB特征点(E.Rublee,V.Rabaud,K.Konolige,and G.Bradski,“ORB:An efficientalternative to SIFT or SURF,”in Proc.IEEE Int.Conf.Comput.Vision,Barcelona,Spain,Nov. 2011,pp.2564–2571.)三角化出新的地图点。任意两个关键帧如果观测到足够多的相同的地图点(至少15个)，则它们将通过无向加权边进行连接，由此形成基于图的地图。每个关键帧都被指定一个全局唯一的ID(识别编号)，该全局ID由关键帧在包含它的子地图中的关键帧ID和子地图自身的ID组成，以使其全局可识别。

所述的单段子地图之间的重叠检测，具体包括如下步骤：

首先将来自不同子地图的所有关键帧添加到关键帧数据库中，然后对于每个子地图中的每个关键帧，根据相似度得分在关键帧数据库中查询候选重叠关键帧。对于每对候选匹配关键帧，候选重叠关键帧KF_o和查询关键帧KF_q，计算一个相似性变换，用于进一步的重叠几何验证。利用KF_o和KF_q中对应的地图点，可以通过RANSAC(Random SampleConcensus，随机采样一致性)迭代方法计算得到相似性变换。如果找到了一个相似性变换S_ql，那么利用S_ql的引导搜索两帧之间更多的对应点并对该相似性变换进行优化。当足够的内点 (至少100个)支撑这个S_ql时，候选重叠关键帧KF_o就被接受为匹配关键帧KF_m，最后将关键帧匹配对和相似性变换作为约束传到下一步。

所述的多段位姿图联合优化过程中：

多段位姿图不仅包含每个来自所对应的单个子地图的本质图，还包含各个位姿图之间的重叠边，其中，本质图由生成树、共视图中具有高共视性的边组成，位姿图之间的重叠边即由子地图之间的重叠带来的约束。任意两个子地图s和s′之间的任意一个重叠约束是连接分别来自s和s′的两个关键帧位姿T^s和T^s′的测量。优化时利用描述了子地图相对于全局坐标系偏移量的锚点，将每个位姿图中局部坐标系下的每个关键帧位姿转换到全局坐标系下。而对于多段位姿图优化，将多段位姿图中每一条二元边的误差定义为：

其中，Sim(3)表示相似性变换，S_ij表示任意两个共视关键帧i和j之间相对Sim(3)，S_iw和 S_jw分别表示关键帧i和j与世界坐标系之间的相对Sim(3)，对于子地图内部的共视关键帧，由于使用双目相机避免了尺度漂移，因此相对Sim(3)可由两关键帧的相对位姿变换SE(3) 计算得到。但是在重叠边约束的情况下，这个相似性变换是的求解采用了Horn提出的方法(B.K.P.Horn,“Closed-form solution of absolute orientation using unitquaternions,”J.Opt. Soc.Amer.A,vol.4,no.4,pp.629–642,1987.)。log_sim(3)表示通过对数映射将Sim(3)转换到正切空间，所以误差项是一个7维的向量，即

表示实数。目标是通过最小化下式的代价函数来优化关键帧位姿：

其中Λ_i,j是误差边的信息矩阵，设置为单位矩阵。

所述的对整体地图执行全局BA优化具体步骤如下：

首先将每个地图点的位置根据其参考关键帧进行更正，然后对于任意两个子地图，融合冗余地图点，并在子地图之间插入新的共视边，从而形成一个整体地图。以两个子地图的合并优化为例，如果在子地图2中为子地图1中的关键帧KF_i找到重叠关键帧KF_l，则将KF_l及其共视关键帧观察到的所有地图点投影到KF_i中，并在投影周围的狭窄区域中搜索特征匹配。所有匹配上的地图点都将合并，而且合并中涉及的所有关键帧将更新其共视连接，从而创建连接着来自不同子地图的关键帧的共视边。最后，对合并的地图执行全局BA 优化，以获得最佳的地图，为了减少外点的影响，优化时采用了鲁棒核函数。对于全局BA 优化，通过最小化三维地图点的位置

和匹配的特征点坐标

的重投影误差，来同时优化了三维地图点的位置和关键帧的位姿R_iw∈SO(3)和

R和t分别表示位姿的旋转与平移分量，其中w表示世界参考系，i表示关键帧，j表示观测到的地图点，

表示实数，重投影误差为

e_i,j＝x_i,j-π(R_iwX_w,j+t_i,w) (3)

其中，π是投影函数：

表示地图点在相机坐标系下的三维坐标，由(R_iwX_w,j+t_i,w)计算得到。而(f_x,f_y)是焦距，(c_x,c_y)是主点，b是基线，可以通过相机标定得到。

全局BA优化了除初始关键帧外的所有关键帧

和合并地图中的所有地图点

将

定义为

中与关键帧i中的特征点匹配的地图点j的集合，

则优化问题如下所示：

其中ρ_h是Huber鲁棒核函数，

是与检测到该特征点的图像金字塔层相关的协方差矩阵，σ²表示图像金字塔层的缩放系数的平方，图像金字塔共8层，最底层为原始图像，在原始图像上检测到的特征点对应的σ²为1，相邻两层之间低一层的σ²是高一层的1.44倍，I_3×3为单位矩阵。

有益效果：

本发明提出一种基于多段联合优化的用于精确定位的地图构建方法，本方法只使用廉价的双目摄像头作为输入传感器，并由两个关键模块组成。第一个模块是双目视觉SLAM 前端，其利用基于特征点的视觉SLAM方法对同一场景分别获得多个单段子地图。第二个关键模块是多段合并后端，在这个模块中，对多个单段子地图进行联合优化，从而得到更准确的地图。

附图说明

图1是本发明提供的基于多段联合优化的用于精确定位的地图构建方法的流程图；

图2是本发明提供的基于多段联合优化的用于精确定位的地图构建方法的实施例处理流程示意图；

图3是本发明提供的基于多段联合优化的用于精确定位的地图构建方法在KITTI数据集上的实验结果对比图，(a)和(b)为第一组，(c)和(d)为第二组，(e)和(f)为第三组，(g)和(h) 为第四组。

具体实施方式

本发明的目的是提供基于多段联合优化的用于精确定位的地图构建方法，首先利用基于特征点的视觉SLAM方法对同一场景分别获得单段子地图，然后对多个单段子地图进行多段联合优化，使用来自同一场景的多个数据来补偿传感器的低精度，最终获得精确的定位地图。

下面将结合附图对本发明加以详细说明，应指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

图1是本发明提供的基于多段联合优化的用于精确定位的地图构建方法的流程图；图 2是本发明提供的基于多段联合优化的用于精确定位的地图构建方法的实施例处理流程示意图；图3是本发明提供的基于多段联合优化的用于精确定位的地图构建方法在KITTI数据集上的实验结果对比图。由于轨迹精度可以作为地图重建精度的间接度量方式，因此本实施例中对本发明方法与视觉SLAM方法ORBSLAM2和激光SLAM方法LOAM在KITTI 数据集子序列上获得的关键帧轨迹做了对比。自上而下一共4组实验结果，(a)和(b)为第一组，(c)和(d)为第二组，(e)和(f)为第三组，(g)和(h)为第四组，每一组实验结果的上下两图分别表示该组数据中的两段包含相同场景的不同子序列在不同方法下的关键帧轨迹对比图，本实施例中MJO代表本发明中提出的方法。通过把采用不同方法得到的关键帧轨迹和真值轨迹的对比可知，利用本发明方法获得的关键帧轨迹明显比采用ORBSLAM2方法更接近真值轨迹，甚至在某些序列接近LOAM的轨迹精度。由此可知，采用本发明方法可以获得用于精确定位的先验地图。

本发明提供的基于多段联合优化的用于精确定位的地图构建方法具体包括：

步骤1：获得包含关键帧和稀疏地图点的单段子地图；

对需要构建定位地图的场景，采用ORBSLAM2(R.Mur-Artal and J.D.Tardos,“ORB-SLAM2:An open-source SLAM system for monocular,stereo,and RGB-Dcameras,” IEEE Transactions on Robotics,vol.33,no.5,pp.1255–1262,2017)的跟踪与建图模块，通过视觉里程计与局部地图优化得到多组关于该场景的单段子地图。该过程中，将双目RGB 图像作为输入，提取ORB特征点并计算描述子，把通过三角测量获得三维坐标的ORB特征作为场景路标点。视觉里程计通过跟踪场景路标点来估计每帧的相机位姿，局部地图并非包含每一帧中，而是仅包含那些最具代表性的帧，也就是关键帧，关键帧观察到的那些路标点将被存储为地图中的三维地图点。

本发明实施过程中，视觉里程计和局部地图优化是在两个单独的线程中并行运行。视觉里程计线程将定位相机的每一帧，并决定是否将当前帧生成为新的关键帧。局部地图优化线程只处理关键帧，并执行局部BA优化，以实现对当前关键帧的周围环境的最优重建。任意两个关键帧如果观测到足够多的相同的地图点(至少15个)，则它们将通过无向加权边进行连接，由此形成基于图的地图。每个关键帧都被指定一个全局唯一的ID，该全局ID由关键帧在包含它的子地图中的关键帧ID和子地图自身的ID组成，以使其全局可识别。

步骤2：单段子地图之间的重叠检测；

本发明采用了一种基于DBoW2(D.Galvez-Lopez and J.D.Tardos,“Bags ofbinary words for fast place recognition in image sequences,”IEEETrans.Robot.,vol.28,no.5,pp. 1188–1197,Oct.2012.)词袋模型的场景识别策略，并利用倒排索引为每个关键帧实现快速的候选重叠关键帧搜索，具体步骤如下：首先将来自不同子地图的所有关键帧添加到关键帧数据库中，然后对于每个子地图中的每个关键帧，根据相似度得分在关键帧数据库中查询候选重叠关键帧。对于每对候选匹配关键帧，候选重叠关键帧KF_o，和查询关键帧KF_q，计算一个相似性变换，用于进一步的重叠几何验证。利用KF_o和KF_q中对应的地图点，可以采用Horn提出的方法，通过RANSAC迭代计算得到相似性变换。如果找到了一个相似性变换S_ql，那么利用S_ql的引导搜索两帧之间更多的对应点并对该相似性变换进行优化。当足够的内点(至少100个)支撑这个S_ql时，候选重叠关键帧KF_o就被接受为匹配关键帧KF_m，最后将关键帧匹配对和相似性变换作为约束传到下一步。

步骤3：多段位姿图联合优化；

本发明实施过程中，优化时利用描述了子地图相对于全局坐标系偏移量的锚点，将每个位姿图中局部坐标系下的每个关键帧位姿转换到全局坐标系下。多段位姿图联合优时，将作为误差边的信息矩阵，设置为单位矩阵，使得优化时误差各分量权重相同。

步骤4：对全局地图执行全局BA优化

首先将每个地图点的位置根据其参考关键帧进行更正，然后融合子地图之间冗余的地图点，将所有子地图合并为一个整体地图，最后执行全局BA优化。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换和替代，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (5)

1.一种基于多段联合优化的用于精确定位的地图构建方法，其特征在于，包括以下步骤：

步骤1：获得包含关键帧和稀疏地图点的单段子地图；对需要构建用于精确定位的地图的场景，使用基于特征点的视觉SLAM方法，通过视觉里程计与局部地图优化得到多组关于该场景的单段子地图；该过程中，将双目RGB图像即红绿蓝三色图像作为输入，提取ORB特征点并计算描述子，把通过三角测量获得三维坐标的ORB特征作为场景路标点；视觉里程计通过跟踪场景路标点来估计每帧的相机位姿，局部地图并非包含每一帧中，而是仅包含那些最具代表性的帧，也就是关键帧，关键帧观察到的那些路标点将被存储为地图中的三维地图点；

步骤2：单段子地图之间的重叠检测；利用关键帧中的ORB描述子，并采用基于词袋模型的场景识别策略进行快速的重叠检测；

步骤3；多段位姿图联合优化；利用分配给每个子地图的锚点，在一个全局坐标系中执行多段位姿图优化，其中锚点描述了子地图相对于全局坐标系的位姿偏移；

步骤4：对整体地图执行全局BA(Bundle Adjustment，光束平差法)优化；首先将每个地图点的位置根据其参考关键帧进行更正，然后融合子地图之间冗余的地图点，将所有子地图合并为一个整体地图，最后执行全局BA优化。

2.根据权利要求1所述的用于精确定位的地图构建方法，其特征在于，步骤1所述的单段子地图的获得采用ORBSLAM2的跟踪与建图模块，整个过程中，视觉里程计和局部地图优化是在两个单独的线程中并行运行；视觉里程计线程将定位相机的每一帧，并决定是否将当前帧生成为新的关键帧；局部地图优化线程只处理关键帧，并执行局部BA优化，以实现对当前关键帧的周围环境的最优重建；视觉里程计线程创建的新关键帧将在局部地图中建立与其他关键帧的连接，并使用相连接的关键帧之间新的对应的ORB特征点三角化出新的地图点；任意两个关键帧如果观测到至少15个的相同的地图点，则它们将通过无向加权边进行连接，由此形成基于图的地图；每个关键帧都被指定一个全局唯一的ID即识别编号，该全局ID由关键帧在包含它的子地图中的关键帧ID和子地图自身的ID组成，以使其全局可识别。

3.根据权利要求1所述的用于精确定位的地图构建方法，其特征在于，步骤2所述的单段子地图之间的重叠检测采用了一种基于DBoW2词袋模型的场景识别策略，并利用倒排索引为每个关键帧实现快速的候选重叠关键帧搜索，具体步骤如下：首先将来自不同子地图的所有关键帧添加到关键帧数据库中，然后对于每个子地图中的每个关键帧，根据相似度得分在关键帧数据库中查询候选重叠关键帧；对于每对候选匹配关键帧，候选重叠关键帧KF_o，和查询关键帧KF_q，计算一个相似性变换，用于进一步的重叠几何验证；利用KF_o和KF_q中对应的地图点，通过RANSAC(Random Sample Concensus，随机采样一致性)迭代方法计算得到相似性变换；如果找到了一个相对相似性变换S_ql，那么利用S_ql的引导搜索两帧之间更多的对应点并对该相似性变换进行优化；当至少100个内点支撑这个S_ql时，候选重叠关键帧KF_o就被接受为匹配关键帧KF_m，最后将关键帧匹配对和相似性变换作为约束传到下一步。

4.根据权利要求1所述的用于精确定位的地图构建方法，其特征在于，步骤3所述的多段位姿图联合优化过程中，多段位姿图不仅包含每个来自所对应的单个子地图的本质图，还包含各个位姿图之间的重叠边，其中，本质图由生成树、共视图中具有高共视性的边组成，位姿图之间的重叠边即由子地图之间的重叠带来的约束；任意两个子地图s和s′之间的任意一个重叠约束是连接分别来自s和s′的两个关键帧位姿T^s和T^s′的测量；优化时利用描述了子地图相对于全局坐标系偏移量的锚点，将每个位姿图中局部坐标系下的每个关键帧位姿转换到全局坐标系下；而对于多段位姿图优化，则将多段位姿图中每一条二元边的误差定义为：

其中，Sim(3)表示相似性变换，S_ij表示任意两个共视关键帧i和j之间相对Sim(3)，S_iw和S_jw分别表示关键帧i和j与世界坐标系之间的相对Sim(3)，对于子地图内部的共视关键帧，由于使用双目相机避免了尺度漂移，因此相对Sim(3)可由两关键帧的相对位姿变换SE(3)计算得到；但是在重叠边约束的情况下，这个相似性变换是的求解采用了Horn提出的方法；log_Sim(3)表示通过对数映射将Sim(3)转换到正切空间，所以误差项是一个7维的向量，即

表示实数；目标是通过最小化下式的代价函数来优化关键帧位姿：

其中A_i，j是误差边的信息矩阵，设置为单位矩阵。

5.根据权利要求1所述的用于精确定位的地图构建方法，其特征在于，步骤4所述的对整体地图执行全局BA优化具体步骤如下：首先将每个地图点的位置根据其参考关键帧进行更正，然后对于任意两个子地图，融合冗余地图点，并在子地图之间插入新的共视边，从而形成一个整体地图；以两个子地图的合并优化为例，如果在子地图2中为子地图1中的关键帧KF_i找到重叠关键帧KF_l，则将KF及其共视关键帧观察到的所有地图点投影到KF_i中，并在投影周围的狭窄区域中搜索特征匹配；所有匹配上的地图点都将合并，而且合并中涉及的所有关键帧将更新其共视连接，从而创建连接着来自不同子地图的关键帧的共视边；最后，对合并的地图执行全局BA优化，以获得最佳的地图，为了减少外点的影响，优化时采用了鲁棒核函数；对于全局BA优化，通过最小化三维地图点的位置

和匹配的特征点坐标

的重投影误差，来同时优化了三维地图点的位置和关键帧的位姿R_iw∈SO(3)和

R和t分别表示位姿的旋转与平移分量，其中w表示世界参考系，i表示关键帧，j表示观测到的地图点，

表示实数，重投影误差为

e_i，j＝x_i，j-π(R_iwX_w，j+t_i，w) (3)

其中，π是投影函数：

表示地图点在相机坐标系下的三维坐标，由(R_iwX_w，j+t_i，w)计算得到；而(f_x，f_y)是焦距，(c_x，c_y)是主点，b是基线，通过相机标定得到；

全局BA优化了除初始关键帧外的所有关键帧

和合并地图中的所有地图点

将

定义为

中与关键帧i中的特征点匹配的地图点j的集合，则优化问题如下所示：

其中ρ_h是Huber鲁棒核函数，

是与检测到该特征点的图像金字塔层相关的协方差矩阵，σ²表示图像金字塔层的缩放系数的平方，图像金字塔共8层，最底层为原始图像，在原始图像上检测到的特征点对应的σ²为1，相邻两层之间低一层的σ²是高一层的1.44倍，I_3×3为单位矩阵。

Images