同时定位和建图（SLAM）是机器人和自主导航中的一个重要领域。它是指机器人在未知环境中移动并使用其传感器和里程计信息构建环境地图并同时估计其在此地图中位置的过程。SLAM对于机器人系统的自动操作是不可或缺的，如室内的自动扫地机器人与室外复杂环境下的自动驾驶汽车。使用像LiDAR和Kinect这样的复杂3D传感器进行SLAM目前已经有了较为成熟的技术方案，尤其是在大多数的室内环境中进行SLAM已经有了比较完善的方法[1]。然而，这些3D传感器诸如LiDAR的高成本和Kinect的有限识别范围之类的限制，使得LiDAR不能用于低成本的SLAM系统而Kinect则难以用于户外场景。因此，视觉SLAM（一个或两个2D摄像机用作传感器）仍然是一个很受欢迎的研究领域。但同时这也是非常具有挑战性的，因为缺少直接的3D信息，所以场景的3D结构必须通过从不同视角拍摄的多个图像中提取特征来进行推断。但是有时即使在存在3D传感器的情况下也需要来自相机的丰富视觉信息进行环闭检测。所以如果通过相机获得的3D信息也可以用于地图生成和定位，则可能大大的提高系统的性能。

ORB-SLAM [2][3]是一种较为成熟的视觉SLAM方案，能够仅使用单目摄像机创建基于点云的地图，在复杂的室外环境下也能有较好效果。虽然ORBSLAM获得的稀疏点云可以用于获得环境的3D结构，但是对于使用需要2D栅格地图[4]作为输入的路径规划和导航算法而言，这种地图并不能起到作用。ORB SLAM产生的点云是稀疏的，这使得其难以生成包含大部分障碍的栅格图，这些栅格图可以在自由空间中为机器人提供足够的连续性，以保证路径规划的顺利进行。该项目旨在通过使用ORB SLAM产生的3D点云实时构建2D栅格图的方法来解决稀疏点云不能用于导航的问题。栅格地图应该足以使ROS [5]的标准导航包使用它来生成导航命令，该命令可以允许机器人（实体或虚拟）跟踪ORB SLAM生成的摄像机轨迹。

Santana等人基于颜色将图片中的区域分为不同的连通域，然后使用由SLAM产生的homograhy矩阵对同一个连通域进行建图。但是这种方法对计算资源的需求比较大，对于需要通过视觉实时建图的系统来说具有较大的困难。另外一种构建地图的方法是将3D的点云信息转化为3D的激光雷达数据，然后调用ROS包，将得到的数据输入到Gmapping节点中。但是这种方法会带来额外的不确定性，对于建图的稳定性具有较大的挑战。

Goeddel等人最近的一项工作[6]提出了一种从3D LiDAR数据中提取2D地图以进行定位的方法。它的工作原理是通过对估计点所在的平面斜率进行阈值处理，对每个点施加垂直度约束。为了区别出合适大小的障碍物，它使用两个不同的阈值，一个较小的阈值（15度）用于检测导航过程中是否有危险，而较大的一个（80度）用于检测是否会发生碰撞。对障碍检测施加了两个额外的限制以进一步减少错误概率：1.只考虑那些z轴长度在设置范围内的点；2.该点的垂直线中包含的点云的数量需要超过阈值。Huesman [7]在将3D点云转换为2D栅格图的过程中也是基于斜率阈值法确定障碍物这一核心思想，本次项目中也使用的方法也将此作为检测虚假障碍的方法之一。

       本文首先分析了普通单目相机的内外参数，并对使用相机进行了标定。然后介绍了视觉SLAM中几种算法，接着在ORBSLAM2构建的3D点云图上实现了二维地图的构建。并通过实验验证了ORBSLAM2的效果以及构建2D地图的实时性以及准确性。

相机将3D世界中的真实点转换到二维图像平面上，为了从图像中复原出3D世界的真实点，我们需要用到相机模型。其中最常用的相机模型是针孔模型，如图2.1。

 在图2.1中，右边为3D世界真实点，左边为相机获得的图像平面上的点。O为模型中的针孔[8]。由于图像是二维平面，因此还有Y坐标，根据相似三角形，有：

 在这里，图像平面不能表示图像的像素平面坐标，因此，定义像素坐标系o-u-v，而像素坐标是可以直接在图像文件中获得的。通常，像素平面的坐标原点为图像左上角，而图像平面的坐标原点为图像中心，因此两个坐标系存在一个尺度变换和一个平移变换，像素平面o-u-v和图像平面O-X’-Y’的转换关系为：

简化模型，转换坐标系，去掉式(2-1)中的负号，并代入式(2-2)，有：

转化为矩阵形式有：

在式(2-4)中，中间的3×3的矩阵被称为相机的内参矩阵K，内参矩阵反映了3D真实点到图像像素平面的变换关系。通常，相机在出厂后就已经标定好内参了，但是，由于一些外力和机械因素，内参可能会有些许变化。另一方面，当选择让相机输出不同分辨率的时候，内参矩阵也会发生变化，这种变化一般是与分辨率成线性关系的。

使用旋转矩阵R与平移矩阵t表示这一变化如式2.5。

         其中T表示旋转平移矩阵，也就是相机的外参。相机的外参也就是相对于世界坐标系的欧氏变换矩阵，它描述了相机相对于世界坐标系的相对位置和姿态，也就是平移和旋转量。我们要做的定位系统实际上就是为了实时求得相机的外参矩阵，如何求解这个矩阵是整个系统的核心问题。

       相机标定已经有了较为成熟的方案，本文采用张氏标定法进行相机内外参数的标定。”张正友标定”是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法[1]。文中提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。因此张氏标定法被广泛应用于计算机视觉方面。

       本文基于opencv，采用张氏标定法对相机进行标定，最终得到相机内参如表2-1.其中fx,fy表示相机的焦距，cx和cy表示的是相机的几何光轴(反映在图像平面上就是图像原点)相对于像素平面的像素偏移量

所以相机内参矩阵为：

本章主要介绍使用视觉进行SLAM时所使用的主要算法。包括特征的提取，特征的匹配以及如何根据匹配特征求解位姿。

       对图像进行帧间运动估计的第一步是获得相邻两帧图像3D特征配对点，选择高准确率、高效率的特征可以有效增加实时定位系统的鲁棒性和实时性。常用的二维特征点有ORB、SURF等，能够在实时视频流处理中使用，是特征点选择的首要指标。

如图3.1所示，特征检测可以分为角点检测、斑状检测和区域检测。在此介绍两种最常用的特征：SURF ( Speeded Up Robust Features, 加速稳健特征)和ORB(Oriented FAST and Rotated BRIEF,有方向的FAST和带旋转的BRIEF)。

       SIFT(Scale-invariant feature transform,尺度不变特征转换)是图像处理中用途广泛的算法，常用来处理机器视觉中的侦测和描述图像的局部特征。SIFT算法由David Lowe在1999年发表，并在2004年完善总结。后来的PCA-SIFT[10]和A-SIFT都是基于SIFT，采用主成分分析技术和改变采样空间衍生出来的算法。

SIFT算法包含SIFT检测子和SIFT描述子，并且具有尺度空间不变性和仿射不变性，对光线、噪声和微小的视差改变的容忍度也很高。因此，这些被提取出来的特征具有非常高的辨识度，很少出现错误匹配的状况。

SIFT首先建立DOG尺度空间，尺度空间的建立是通过将原图像经过带有不同标准差的高斯核进行卷积得到的，由此产生一系列模糊的图像来代替真正的各尺度空间[11]。接下来令相邻的尺度空间相减，从而得到差分的DOG空间，如图3.2。由于DOG空间是差分形成的，因此可以用来检测特征点。在DOG空间下，这些特征点与同一层邻近的8个点、上下层相邻的各9个点比较，如果该点比邻近的26个点都大或都小，则认为该点是极值点，也就是待定特征点，如图3.3。

接下来在选定的极值点中通过泰勒二阶展开去除小于设定阈值的不稳定点，再通过计算剩余极值点的邻域梯度来获得具有旋转不变性的关键点。然后将坐标轴旋转为关键点的方向，确保旋转不变性，通过一个128维的特征向量来作为SIFT描述符。再对这个特征向量进行归一化，从而去除光照变化带来的影响。从SIFT算法中可以看出，由于SIFT算法建立了许多金字塔，造成尺度过大，虽然算法稳定性较好，但是计算时间过长，不适合在实时定位系统中应用。

       ORB(Oriented FAST and Rotated BRIEF,有方向的FAST和带旋转的BRIEF)特征，是一种应用于实时视频流处理的特征。ORB特征分为关键点“Oriented FAST”和描述子“Rotated BRIEF”两部分。所以说，ORB特征可以说是FAST特征点的检测方法和BRIEF特征描述子的结合和改进[11]。因此，提取ORB特征分为以下两个步骤：

1. Oriented FAST关键点提取：找到图像中的关键点，即角点，也就是图像中灰度变化较大的地方。Oriented FAST在FAST的基础上，另外计算了关键点的主方向，为接下来的BRIEF描述子增加了旋转不变的特性。要想判断一个像素点p是不是FAST关键点，只需要判断其周围的16个像素点中是否有连续N个点的灰度值与p的差超出阈值，16个点的位置如图3.4所示。N一般取12，称为FAST-12，常用的还有FAST-9，FAST-11，阈值一般为p点灰度值的20%。找出关键点后，还要计算该特征的方向，使用灰度质心法实现。灰度质心是指一小块图像中以每个像素的灰度值作为权重计算加权后的中心点。如图3.4以p点为中心的小块区域中，根据各个点的灰度值可以计算出一个灰度质心，通常不与p点重合，这样从p点到灰度质心的连线就是这个特征点的方向。
2. 形成BRIEF描述子：根据提取到的FAST关键点，对其周围图像区域进行描述。BRIEF是一种二进制描述子，其描述向量由编码0和1组成。0和1分别编码了关键点附近两个像素a、b亮度之间的大小关系:若Ia>Ib，则取1，反之则取0，其中像素a和像素b的选取则是通过随机分布得到的。ORB算法在FAST特征点提取阶段计算了关键点的方向信息，因此可以利用得到的方向信息，计算旋转之后的“Steer BRIEF”特征，使得ORB描述子具有更好的旋转不变性。

综上所述，ORB特征使用了FAST和BREIF两种高效算法，并在其基础上进行了改良，拥有较高的效率和鲁棒性，在实时视频流处理中应用十分广泛。

       特征匹配就是匹配两帧图像之间相同的特征点，然后根据匹配的特征点将两帧图像联系起来。图像之间的特征匹配是通过描述子进行的，其方法就是计算描述子之间的汉明距离，汉明距离越小，两个 ORB 特征就越相似。ORB进行特征匹配的效果如图3.5所示

       上一节介绍两帧图像之间进行匹配的方法，知道两帧图像之间的关系之后就可以根据这一关系求解出相机在运动中的位姿变换。通过ORB特征进行了帧间相应点的匹配，获得的匹配点具有深度信息，通过匹配点的3D点来计算运动中的位姿变化。假设有一组匹配好的3D点：

对于任意的匹配点，都有相同的欧氏变换R，t：

上述问题可以用ICP(Iterative Clotest Point,迭代就近点)[12]算法求解。在上一节中，特征点法较好的求解了两帧图像之间的匹配关系，所以，可以用ICP求解相机运动中的位姿变换。ICP的求解分为两种方式：使用线性代数求解，最常用的是SVD（singular value decomposition，奇异值分解）和使用非线性优化方式求解。

目前视觉SLAM的已经有了很大的发展，有许多优秀的算法都可以实现。比如基于EKF的Mono SLAM，采用关键帧的PTAM，围绕ORB特征计算的ORBSLAM，以及最近提出的许多基于深度学习的VSLAM方案。考虑到计算的实时性以及计算资源的限制，本文选取了ORB_SLAM进行３D地图的构建，其构建的是３D的稀疏点云，其建图的大概效果如图４.１，可以发现基本能把轮廓描述出来，但是并不包含更多的详细信息。

       ORB-SLAM是Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos在2015年发表，ORB-SLAM的模块包括跟踪、建图、重定位和闭环检测[13]，但是只支持单目相机。ORB-SLAM2在ORB-SLAM的基础上做出了改进，支持双目相机和RGBD相机。ORB-SLAM2可以计算相机的位姿和轨迹，并对地图进行3D重构。整个系统分为3个线程：跟踪（Tracking）、局部建图（Local Maping）和回环检测（Loop Closing）。其结构如图4.２所示。

       ORB-SLAM2系统第一步要初始化，首先选取有足够匹配点的两帧图像，作为参考帧和当前帧，利用这两帧计算基本矩阵和单应矩阵。其中基本矩阵F是由对极几何理论得到的，基本矩阵计算了点到直线的映射，反映了图像到图像的对应关系。单应矩阵H描述了两个平面之间的映射关系，用来计算场景中的特征点落在同一个平面上时的旋转矩阵R和平移矩阵t。从定义可以看出，基本矩阵F和单应矩阵H只差了一个相机内参。

在接下来的地图构建过程中，ORB-SLAM2采用了较为宽松的策略来添加地图点，以达到较高的运行速度。而后续地图点的筛选和优化则用到了闭环检测模块，这部分将在4.5章节中讲述。下面将依次介绍ORBSLAM2中的跟踪线程，局部建图线程以及回环检测线程。

       ORB-SLAM2系统的跟踪模块分为两个子模块：全局重定位和局部地图跟踪。全 度和较高的稳定性。

       当系统需要初始化位姿估计或局部地图跟踪失败时，需要进行全局重定位。基本流程为：提取当前帧的ORB特征，计算当前帧的BOW向量，然后通过搜索图像数据库中所有关键帧，来寻找适用于重定位的拥有足够多匹配点的关键帧。在这里，图像数据库使用了BOW（Bag-of-words model，词袋模型）。

BOW模型是信息检索领域较为常用的文档表示方法，被广泛应用在文件分类中。词（word）出现的频率可以用来当作训练分类器的特征。在信息检索时，BOW模型忽略一个文档的词的顺序等要素，仅把文档看作是词的集合。

       当已经计算出了上一帧的位姿后，就不再需要使用全局定位了，这时候，使用局部地图跟踪能够更精确更快速地计算当前帧的位姿，尤其是在地图较大时。

局部地图跟踪的原理：将和当前帧K有相同点的关键帧序列设为K1，将与K1在Covisibility Graph邻近的关键帧序列K2(拥有足够多的相同点的关键帧)。根据K1和K2来匹配当前帧的地图点，最终解算出相机的位姿并优化，流程如图4.3所示。

       局部建图分为关键帧插入、地图点云筛选、新地图点云创建、局部约束调整和局部关键帧筛选，见图4.3。

1. 关键帧插入：检测是否有关键帧在序列中，如果有，计算新关键帧的词袋描述子，并插入地图中。
2. 地图点云筛选：当地图点出现在足够多的关键帧中时，认为该地图点是稳定点，否则剔除该地图点。
3. 新地图点云创建：根据三角化ORB特征向量，计算新点云在地图中的位置，并更新地图。
4. 局部约束调整（Bundle Adjustment）：使用Bundle Adjustment优化当前关键帧的位姿。
5. 局部关键帧筛选：为了精简关键帧的数量，如果当前关键帧能够被其他三个关键帧看到，且有90%点云相同，则删除这个关键帧。

       回环检测（Loop Closing）的主要目标是检测当前关键帧是否经过历史位置。如有经过，则利用回环检测得到的回环帧去修正整个SLAM长期跟踪过程中带来的累积误差、尺度漂移等。如果仅有前两个线程的话，仅仅完成了一个很好的视觉里程计（VO），这个线程会对全局地图以及关键帧进行回环检测，以消除上述累积误差。主要包括：

1. 候选关键帧检测：当前关键帧仅有与历史关键帧足够相似才可能成为回环候选帧，该模块通过一定的筛选策略对当前关键帧进行筛选，判断其是否为闭环候选关键帧。由于在实际闭环检测过程中，回环候选帧及其共视关键帧，在一定连续的时间内都可能被观测到。该模块主要通过利用这一条件，对闭环候选关键帧进一步地筛选，通过筛选条件的候选关键帧将进行下一步的判断。
2. 相似性变换计算：考虑到单目SLAM的尺度漂移，当前帧和回环帧之间的相对位姿应是一个相似变换，并且，二者之间应具有足够多的匹配点。该模块主要是通过循环计算当前帧和上述经过筛选后的候选关键帧之间的相似变换，直到找到一个和当前帧具有足够多匹配点的相似变换，对应的候选关键帧即为最终的回环帧。
3. 回环修正：受累积误差的影响，时间越久，越接近当前帧的关键帧及相应的地图点，误差将越大。若寻找到的回环帧，当前帧位姿及其对应的地图点会更精确。该模块就是为了修正累积误差，利用回环帧及其共视关键帧，以及对应的地图点，来修正当前帧及其共视关键帧的位姿以及对应的地图点的世界坐标。紧接着进行地图点融合，更新共视图，然后通过本质图优化相机位姿，最后进行全局BA来修正整个SLAM的累积误差（相机位姿以及地图点）。

通过上一章介绍，已经可以得到环境的3D结构信息。由3D环境信息获得2D平面图，只需要沿高度轴将3D结构图投影到2D平面即可，如图5.1。因为相机的Z轴对应于物理世界的Y轴，所以建立的2D地图就是3D地图的xz平面。

首先将ＯＲＢＳＬＡＭ中生成的所有关键帧投射到xz平面，这一投影可以通过去除关键帧中y轴的信息获得。对于每一个关键帧，对其所有的关键点进行操作，首先从相机的位置开始向每一个点处按照布雷森汉姆直线算法绘点（Bressenham's line drawing algorithm）[14]，然后对xz平面的每一个位置处增加两个个计数器：访问计数器V和占用计数器O，分别用来统计映射到xz平面上该位置处的访问点(visit)与关键点(occupied)的数量，访问点就是布雷森汉姆直线算法处理之后经过的点。访问计数器和占用计数器是与地图点大小相同的整数数组。

因为ORB SLAM2将XZ平面视为水平面，因此将点的y坐标视为其高度。处理完所有关键帧后，计算网格图中每个单元的占用概率为：

其中 表示此点不是障碍物的概率， 表示投影到此位置的关键点数量， 表示投影到此位置的所有访问点的数量。这样就可以得到一副二维包含概率的二维地图，然后通过设置两个阈值free_thresh和occupied thresh，将概率大于的free_thresh点认为没有障碍，将概率小于occupied thresh的点作为障碍点，概率在二者之间的表示未知点。图5.2为对CSC数据集处理后的概率图及阈值化之后的二维地图。

应用局部和全局计数器的想法源于3D点到XZ平面的简单2D投影的问题。所谓全局计数器是指在所有关键帧处理完之后，在计算每一个位置点处的占有情况，而局部计数器是指在处理完一帧后就计数，然后将数据叠加。首先考虑单个关键帧的情况，其中多个点的投影在2D平面中是共线的，如图5.4。在这种情况下，如果我们仅使用一个全局计数器来生成2D地图，有可能会得到一些不准确的地图信息。如图5.4中的中间点实际是有被占用的，但是由于上面一条线与下面一条重合，导致在计数时此位置只统计到了一个访问点而漏掉了占用点。这意味着，如果仅使用全局计数器，我们有可能将一些障碍认为是空闲的区域。

图5.3显示了在CSC数据集上使用和不使用局部计数器的比较。我们可以看到，如果不使用本地计数器，很多实际占用的点将被替换为可用空间。

       本部分主要测试该算法的性能以及实际效果。图6.1是带有相机运动轨迹的点云图，图6.2是一个闭环的点云俯视图。

分析局部图发现人眼几乎很难分辨处详细信息，但是从完整闭环图中可以发现，点云还是能够包含3D场景的大部分信息。由于测试环境较为开放，如图6.2在构建的地图因为捕获的远近处的特征点都很多，所以导致看起来比较乱。图6.3是从网上下载的数据集进行测试的效果，隐去关键帧以及相机轨迹线之后，可以发现在相对封闭的环境中构建的3D地图相对较好。

       本节主要测试将3D地图映射为概率图以及二值化获得二维栅格的图的效果。如图6.4为将ORBSLAM产生点云映射到二维平面后的概率图，图6.5表示对概率图阈值化之后得到的栅格图。

       本次实验主要测试同时运行ORBSLAM2建图以及2D建图的效果及其建图速度。如图6.6，在ORBSLAM２回环检测完毕后，二维地图也能马上完成构建，并且在运行的过程中没有掉帧等现象。

本文首先分析了普通单目相机 的内外参数，并对使用相机进行了标定。然后介绍了视觉SLAM中几种算法，然后在ORBSLAM2构建的3D点云图上实现了二维地图的构建。在最终的实验中发现基于本文提出的2D地图构建方法能够实现较为完善的2D地图构建，而且其实时性也很好，可以满足机器人的实时导航需求。

本文并没有将产生的2D地图应用于实际机器人的导航中，后续的工作可以将其应用于实际的机器人导航系统中进行测试。采用本文方法可以使用普通的单目相机代替激光雷达或者RGBD相机，能够进一步的降低自主导航机器人的成本。

[1]. Ting S L, Kwok S K, Tsang A H C, et al. The study on using passive RFID tags for indoor positioning[J]. International journal of engineering business management, 2011, 3: 8.

[2]. 张炎华, 王立瑞, 战兴群等. 惯性导航技术的新进展和发展趋势. 中国造船, 2008, 10(183): 134～144

[3]. Riisgaard S, Blas M R. SLAM for Dummies[J]. A Tutorial Approach to Simultaneous Localization and Mapping, 2003, 22(1-127): 126

[4]. Scharstein D, Szeliski R. High-accuracy stereo depth maps using structured light[C]//Computer Vision and Pattern Recognition, 2003. Proceedings. 2003 IEEE Computer Society Conference on. IEEE, 2003, 1: I-I.

[5]. Pollefeys M, Koch R, Van Gool L. Self-calibration and metric reconstruction inspite of varying and unknown intrinsic camera parameters[J]. International Journal of Computer Vision, 1999, 32(1): 7-25.

[6]. Zhang Z. Camera Extrinsic Parameters[M]//Computer Vision. Springer US, 2014: 77-77.

[7]. Ke Y, Sukthankar R. PCA-SIFT: A more distinctive representation for local image descriptors[C]//Computer Vision and Pattern Recognition, 2004. CVPR 2004. Proceedings of the 2004 IEEE Computer Society Conference on. IEEE, 2004, 2: II-II.

[8]. Heikkila J, Silven O. A four-step camera calibration procedure with implicit image correction[C]//cvpr. 1997, 97: 1106.

[9]. 张吴明, 钟约先. 基于改进差分进化算法的相机标定研究[D]. , 2004.

[10]. Ke Y, Sukthankar R. PCA-SIFT: A more distinctive representation for local image descriptors[J]. CVPR (2), 2004, 4: 506-513.

[11]. Wang X, Thibodeau B, Trope M, et al. Histologic characterization of regenerated tissues in canal space after the revitalization/revascularization procedure of immature dog teeth with apical periodontitis[J]. Journal of endodontics, 2010, 36(1): 56-63.

[12]. Machniewicz T. Fatigue crack growth prediction models for metallic materials Part II: Strip yield model–choices and decisions[J]. Fatigue & Fracture of Engineering Materials & Structures, 2013, 36(4): 361-373.

[13]. Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE transactions on robotics, 2015, 31(5): 1147-1163.

[14]. Van Aken J R. An efficient ellipse-drawing algorithm[J]. IEEE Computer Graphics and Applications, 1984, 4(9): 24-35.