基于CSAO地图融合的同步定位与建图

何世琼，陈雨，周青华

（1.四川大学电子信息学院，成都 610065；2.四川大学空天科学与工程学院，成都 610065）

0 引言

基于视觉的同时定位与地图构建技术作为机器人感知未知环境的核心技术，也是实现机器人自动化的关键技术［1］。视觉传感器作为机器人的“眼睛”，在没有先验的环境中为机器人的自主感知提供了可行性。传统雷达、声呐等测距仪器无法获得精确丰富的信息，视觉传感器克服了传统传感器体积大、精度低等缺点［2］，可以为机器人获取更全面的环境信息。今天，SLAM技术已经成为机器人自主导航、自动驾驶、无人机导航等领域最关键的技术之一。

2007年，Davison提出了第一个单目视觉SLAM系统MonoSLAM［3］。同年，Klein等人提出了PTAM（parallel tracking and mapping）［4］，该算法首次提出视觉SLAM中区分前后端的概念，实现了跟踪与建图的并行化。PTAM第一次使用了非线性优化，在视觉SLAM的研究历程中具有重要意义，此后，视觉SLAM的后端以非线性优化为主要手段。2014年Forster等人提出了基于稀疏直接法的SVO（semi-direct visual odoemtry）［5］，SVO跟踪一些关键点并根据其周围的信息估计相机运动和位置。为了实现算法的效率和轻量化，SVO不具备回环检测和后端优化功能，因此建图效果差并且存在累计误差。

ORB-SLAM系列是基于特征的视觉SLAM的优秀代表，2015年ORB-SLAM［6］被Mur-Artal等人提出。它不仅支持单目、双目和RGBD三种模式，还创新地提出了使用三线程来完成SLAM。在ORB-SLAM的基础上，该团队在2017年提出了ORB-SLAM2［7］，它能实现地图重用和回环检测，在后端采用基于单目和双目的非线性光束平差（bundle adjustment，BA），ORB-SLAM2中还提出了重定位，其将地图中特征点的数量小于某个阈值时视为丢失跟踪，该方法在相机平稳运行并且纹理丰富的地图中表现出良好的精度。但是，由于该算法的地图管理只针对单张地图，所以重定位失败的情况常有发生，从而造成构建的全局地图精度下降。2018年该团队在其基础上增加了一个鲁棒的多地图处理系统Atlas［8］，第二年，整合了ORB-SLAM系列功能和最大后验概率的ORB-SLAM3［9］被公开发表，该算法串联了前两代的工作并且增加了针孔和鱼眼相机模型，创新地在IMU初始化阶段引入了MAP，大大提高了算法的鲁棒性，但是由于其依赖ORB特征，在实时性上依然存在不足。

综上，本文提出一种基于地图融合的Mutimap-SLAM算法，该算法在Atlas多地图系统的基础上，使用CSAO地图融合方法，CSAO先计算出关键帧位姿之间的转换关系，然后Atlas中的多张子地图依据该转换关系实现地图融合，以此来提高算法在困难场景中的精度和性能。最后，通过实验验证了该方法的可行性和算法的精度。

1 基于CSAO地图融合的SLAM算法

1.1 基于At l as的ORB-SLAM算法

Atlas多地图系统是一个由多张子地图组成的地图集，如图1所示，将整体地图部分划分为活跃地图（active map）和非活跃地图（inactive map），活跃地图是指新创建的关键帧所更新的那个地图。Atlas中的每张子地图都有自己的关键帧、地图点、共视图和生成树，并且所有子地图信息都被存储在一个DBoW数据库中。

图1 多地图系统示意图

在Atlas多地图系统中，一方面Active Map部分作为当前工作模块，用于和关键帧进行匹配和融合。另一方面，为了降低特征匹配的计算量，InActive Map部分不需要匹配。在ORB-SLAM特征跟踪过程中，每次关键帧的插入都需要与全局地图中的词袋数据库进行对照。如果与当前帧匹配上的关键帧位于非活跃地图中，则当前活跃的地图吞并对应的非活跃地图。

Atlas多地图系统让SLAM算法即使在特征点缺失的情况下，也可以通过不断地生成关键帧来实现重定位找回。增加了Atlas多地图系统的ORB-SLAM3算法结构如图2所示，该算法可以在保证算法的鲁棒性的同时，提高建图精度。

图2 ORB-SLAM3算法结构

1.2 CSAO地图融合

为了获得更精确的全局地图，需要将非活跃地图的信息补充到当前活跃地图中，马树军等［10］提出了一种在后端利用卡尔曼滤波融合子地图的方法，但是因为基于卡尔曼滤波框架，导致算法的复杂度高并且对数据的关联错误十分敏感。本文提出单位四元数求绝对旋转的封闭解（CSAO）的方式，先计算出关键帧之间的映射关系，再根据映射实现地图融合。假设当前Active Map中的关键帧表示为Ka，与之对应的Inactive Map待吞并关键帧表示为Ks，当前活跃地图要融合非活跃地图，需要通过计算出两帧之间的变换关系，后者即可通过该变化关系映射到前者，从而实现地图融合。

将求解变换关系拆分为求解平移、旋转、和尺度因子三个部分。假设当前有n对匹配的三维点，记为和，将两个位置的三位点构建新的三维坐标记为和，则误差项可以表示为公式（1），

其中，

则代价函数可以写为公式（3），

将公式（3）展开可得公式（4），

此时，求解姿态的转换关系已经简化为求解旋转量，再根据旋转量即可求得平移和尺度因子。旋转用四元数Rrl表示，因为两个四元数的向量积结果不一定时纯虚数，所以用来表示，于是新的代价函数可以表示为公式（7）。此时，即可构造M矩阵公式（8）求出矩阵公式N（9），N的最大特征向量即为所求旋转四元数。

1.3 绝对轨迹误差

本文使用轨迹地图的绝对轨迹误差（absolute trajectory error，ATE）来评价算法精度，直接计算估计位姿与真实位姿之间的差值，避免了里程计的平移误差和旋转误差的计算，减小计算量的同时能直观地反映算法精度和轨迹全局一致性。第i帧的ATE定义如公式（10）。

其中，

公式（11）中Qi∈SE(3)表示算法的真实位姿，Pi∈SE(3)表示算法的估计位姿。将两帧之间的时间间隔用Δ表示，使用均方根误差RMSE统计公式（11）得到公式（10），其中trans(Fi)表示每两帧之间的平移误差。

2 实验结果与分析

2.1 实验环境与数据集

该算法在Intel Core i7（4核@3.6 Hz）台式计算机上测试，程序在ubuntu18.04上运行，涉及的软件有Pangolin、Opencv、PCL、Eigen 3和g2o等开源库，还使用了evo工具来评估系统的性能。实验使用的是由苏黎世联邦理工学院计算机视觉组采集制作的双目+IMU数据集［11］，包含了普通房间和machine hall两个场景，实验所用数据集详细信息见表1。

表1 实验采用的数集

2.2 全局地图精度

由于1—3号简单数据集中的场景纹理丰富，ORB-SLAM和MultiMap-SLAM得到的RMSE ATE相似，无法突出MultiMap-SLAM的优越性。选择4—7号困难数据集对比两种算法的建图精度，实验结果如表2所示。

表2 ORB-SLAM和MultiMap-SLAM在Euroc数据集中的RMSE ATE比较

在4号数据集中，MultiMap-SLAM的ATE略高于ORB-SLAM，但是基本持平；在5号数据集中，MultiMap-SLAM的ATE最大值为0.113，中值为0.071，最小值为0.009，相较ORB-SLAM的0.133，0.060，0.007分 别 下 降 了15.00%，15.49%，22.22%；在6号数据集中，MultiMap-SLAM的ATE最大值为0.027，中值为0.015，最小值为0.003，相比ORB-SLAM的0.401，0.212，0.007分别下降了93.26.00%，92.93%，57.14%；在7号数据集中，MultiMap-SLAM的ATE最大值为0.051，中值为0.027，最小值为0.003，相较ORB-SLAM的0.401，0.212，0.007分别下降了84.21.00%，83.92%，0.00%。此外，图3展示了两种算法构建的全局地图，结果表明，基于CSAO地图融合的MultiMap-SLAM能有效降低全局地图的绝对轨迹误差，获得更精确的全局轨迹地图。

图3 估计轨迹与真实轨迹的绝对轨迹误差

综上可知，在特征缺失的困难数据集中，本文提出的基于地图融合的算法具有有效性，能将建图精度提高至少30%，并得到更准确的轨迹地图。

2.3 Mut i map-SLAM算法性能

本文将算法的性能定义为多个会话窗口同时运行该算法时，算法所表现出的精度。先单独用1—5号数据集运行Mutimap-SLAM得到RMSE ATE的结果，再同时用1—5号数据集运行Mutimap-SLAM得到RMSE ATE结果，直接引用VINSstereo［12］和VINSMonocular Inertial［13］原 文 中的性能测试数据进行对比。如表3所示，Mutimap-SLAM的平均RMSE ATE为0.074，VINS Monocular Inertial的平均RMSE ATE为0.210，相比之下Mutimap-SLAM将多会话RMSE ATE降低了64.7%。

表3 Mutimap-SLAM与VINS-SLAM的多会话性能比较

此外，将Mutimap-SLAM与CCM-SLAM进行比较验证其多会话性能，CCM-SLAM［14］是一个基于ORB-SLAM的分布式多无人机地图的系统，整个系统包括一个中央服务终端和多个运行在各个无人机上的子端，每个子端都可以独立自主地运行SLAM前端视觉里程计，建立局部的子地图，服务终端则用来处理子端的地图数据。实验先在将1、2、3号数据集在本算法中同时运行，再与CCM-SLAM服务器端同时处理三个数据集的地图信息对比，得到全局地图的RMSE ATE如表4所示，Mutimap-SLAM（Mono）的单目和双目全局地图RMSE ATE分别为0.026和0.037，相比CCMSLAM的0.77提高了2～3倍。

表4 Mutimap-SLAM与CCM-SLAM的多会话性能比较

上述实验结果均表明，即使与以性能著称的SLAM算法对比，Mutimap-SLAM算法也表现出良好的性能。

3 结语

本文在基于ORB-SLAM算法基础上，基于CSAO地图融合方法，提出了一种使用多地图系统的视觉SLAM算法Mutimap-SLAM。通过相同数据集上的对比实验，得到以下结论。

（1）CSAO地图融合方法应用在ORB-SLAM中具有可行性，可以实现同时定位与建图。

（2）在特征稀少的困难场景中，基于CSAO地图融合方法的Mutimap-SLAM可以提高全局建图精度。

（3）相比性能良好的视觉SLAM算法，Mutimap-SLAM依然可以保持较高的性能。