双目视觉SLAM技术详解

为了最终实现机器鼠与实验鼠能够在复杂化和多样化的环境下进行高效行为交互实验，需要机器鼠能够通过视觉反馈系统感知周围环境，并利用这些信息来确定自身位置、了解环境状态。视觉SLAM技术可以在实时定位自身的同时对周围环境进行建模，是实现机器鼠与实验鼠自主交互的重要条件。为此，将基于ORB-SLAM2实现机器鼠的实时位姿估计与地图构建功能。

一、视觉SLAM原理及ORB-SLAM算法分析

（一）视觉SLAM模型简介

视觉SLAM[8]指的是机器人运动过程中，在没有环境先验信息的情况下，仅依靠视觉传感器实时估计自身的位姿，并建立周围环境的模型。用于SLAM的相机种类可分为单目、双目和RGB-D三大类。单目相机简单而且容易实现，但是无法依靠单张图片计算出三维空间中的真实尺度；双目相机解决了单目相机的缺点，可以消除尺度不确定性，但是计算视差需要消耗大量计算资源，而且配置及标定相对复杂；RGB-D相机解决了双目相机计算量大的缺点，利用红外结构光或者飞行时间法获得物体的深度信息，但是无法测量透射性材料，并且存在视野小、易受光照干扰、噪声大等缺点。

视觉SLAM可以描述为一个从含噪声的相机的观测量中估计周围环境地图和机器人位姿的数学问题：输入为相机获取的带噪声的信息，输出为机器人位置和环境地图。设环境地图由一系列路标点组成Q={q1，q2，…，qNq}，机器人在该环境中进行连续的运动。将机器人连续的运动过程离散化为t={1，2，…，T}，设其各个时刻的位置记为P={p1，p2，…，pT}，任意t时刻机器人的位置为pt，相机的输入为ut，对应的噪声为wt，则pt可以用一个通用数学模型来表述：

式（6-18）表示机器人当前的位置可以由前一个时刻的位置、当前含噪声的相机的输入等信息推断得出，称为运动方程。另外，当机器人处于pt位置时，其通过相机观测到所在路径上的某个路标点qn，从而产生观测数据ot，n。类似地，ot，n也可以用一个抽象的数学模型来表述：

式中，vt，n为此次观测的噪声，该式称为观测方程（视觉SLAM中，观测方程即为相机成像模型，见4.2节）。

由此，可以将视觉SLAM过程总结为两个抽象的、通用的基本方程，即式（6-18）和式（6-19）。这两个公式描述了最基本的视觉SLAM问题：根据相机输入和观测数据，求机器人定位问题和建图问题。整个视觉SLAM框架如图6-14所示，由相机数据读取、前端视觉里程计、后端优化、回环检测和建图五大部分组成[9]。

图6-14　视觉SLAM框架

视觉里程计，即视觉SLAM系统的前端，其主要任务是估算相机在相邻两幅图像之间进行的运动。目前，视觉里程计的算法主要分为两类，即特征点法和直接法。特征点法是当前主流的方法，通过提取两幅图像中关键点，并在两幅图像中进行匹配。由于特征点法只需要对特征点进行计算，不需要处理图像中其他信息，因此计算量较少，实时性较强。然而，特征点法忽略了图像中除特征点以外的信息，使得计算结果不够准确，同时特征点的提取与计算耗时很大。直接法根据图像的像素灰度信息来计算相机运动，从而可以利用图像的全面信息，并且节约了计算量。但是，直接法易受光照影响，而且要求相机运动速度比较慢。仅使用视觉里程计来估计相机的运动会造成漂移，这就需要后端优化和回环检测进行校正。

后端优化主要为了解决系统噪声的问题。常用的方法有滤波算法和非线性优化算法。视觉SLAM早期通常使用以扩展卡尔曼滤波（EKF）为主的滤波方法估计相机位姿和路标点的状态，但是使用EKF造成的非线性误差会导致系统的误差较大。目前的主流方法是基于非线性优化理论的算法，来进行状态估计。

回环检测主要是为了解决位姿估计中的漂移问题，目的是让机器人识别出之前到达过的场景。通常采用基于外观的回环检测算法，通过判断图像间的相似度来实现。

建图是对环境的描述，可以根据具体的任务来构建不同形式的地图。稀疏地图可以应用到定位任务中，稠密地图可以应用到导航和避障任务中，语义地图可以应用到人与地图直接的交互（AR、VR）任务中。

随着视觉SLAM技术的飞速发展，产生了一系列优秀的视觉SLAM算法。表6-4列出了一些常用的、具有代表性的视觉SLAM方案[10]。Mur-Artal等人于2015年提出的基于ORB特征[11]的ORB-SLAM算法[12]是现代视觉SLAM算法中功能完善的算法之一。它继承了PTAM算法的多线程思想并弥补了该算法的缺陷，既可以应用在室内小场景，也可以用在室外大场景中。ORBSLAM2算法[13]在ORB-SLAM基础上支持了双目和RGB-D相机，并进行了一定的优化。我们选择ORB-SLAM2算法作为机器鼠实时位姿估计与地图构建方法，主要原因如下。

表6-4　一些具有代表性的视觉SLAM算法

（1）ORB-SLAM2是一套非常完善、易用的基于特征点法的视觉SLAM系统，支持单目、双目和RGB-D 3种模式，适用性强。

（2）整个系统围绕ORB特征进行计算，包括前端中的ORB特征和回环检测中的ORB字典[14]。由于ORB相比于SIFT[15]和SURF[16]速度快，可以在CPU上实时进行计算，同时ORB又具有尺度和旋转不变性，因此体现出一种在效率和精度之间的优秀的折中方案。

（3）ORB-SLAM2进行了许多优化，包括特征点、关键帧、局部优化等，使得系统具有很好的精度并且能够在不同场景下顺利执行。

（4）系统的回环检测算法功能强大，能够有效地减少累计误差。另外，系统加载在运行前需要载入一个ORB字典文件，保证一旦发生丢帧可以快速找回。

（二）ORB-SLAM2算法分析

ORB-SLAM2是一种基于特征点法和全局非线性优化的视觉SLAM系统。本书采用双目图像作为ORB-SLAM2算法系统的输入，算法的整体架构如图6-15所示，主要由跟踪、局部地图构建和回环检测3个线程构成。

(www.xing528.com)

图6-15　ORB-SLAM2算法框架

跟踪线程是ORB-SLAM2算法中最重要的一步，主要负责完成每帧图像中相机的位姿估计与优化。首先，根据预处理阶段的ORB特征提取与双目匹配估计相机的运动；然后，利用光束平差法（BA）来最小化重投影误差，从而对相机位姿进行优化，基本原理见式（6-20）；最后，需要根据当前帧的特征点比例、局部地图是否空闲等规则判断是否选取当前帧为关键帧。

优化后的相机姿态可以表示为

式中，{R，t}为优化前的相机姿态；{R′，t′}为优化后的相机姿态；qi为当前帧中第i个特征点的空间坐标；ui为对应特征点在图像上的坐标（i=1，2，…，NF）；π（·）为投影函数，可将三维点投射到二维图像平面中；ρ（·）为鲁棒核函数，主要作用是平衡误差从而防止二范数误差增长过快。

局部地图构建线程主要负责管理关键帧和地图点、建立局部地图并进行优化。首先，加入跟踪线程提供的关键帧并更新地图；然后，验证新加入的地图点并进行筛选去除离群点，生成新的地图点；其次，使用局部BA对关键帧位姿和地图点进行优化（见式（6-21）），可以看出式（6-20）与式（6-21）不同的是，后者将关键帧位姿和地图点同时当作优化变量进行优化；最后，需要再次筛选关键帧，去除不必要的关键帧。

式中，KL为共视关键帧；KF为除KL外的其他关键帧；QL为所有共视关键帧中的关键点；为所有第k帧中所有QL中的关键点。

回环检测线程主要负责检测是否存在回环，从而降低累计误差。系统在初始化时需要加载一个ORB词袋模型：首先，通过匹配当前帧与其他关键帧的词袋相似度来探测回环的存在；然后，计算相似变换并进行回环融合；最后，进行位姿图优化更正累计漂移误差。在位姿优化之后，会启动第四个线程执行全局BA算法，来计算整个系统最优结构和运动的结果。

经过以上线程完成了相机位姿的估计与点云地图的构建。由于ORBSLAM2算法中的地图点来自关键帧中的特征点，因此构建的是稀疏点云地图。

二、实验场景下机器鼠的实时位姿估计与建图

为了测试机器鼠的实时位姿估计与建图功能，需要设计一个具体实验场景。本实验选择实验室中的实验桌作为实验场地，划分出1 m×1 m作为实验区域。为了测试算法的回环检测功能，本实验将机器鼠的运动路线设计为一个完整的闭环。对于转弯过程，由于机器鼠的转弯是依靠后肢两轮的差速实现的，而双目视觉系统的位置与轮子的位置不在同一条垂直轴线上，因此无法完成纯直角转弯，故本实验将转弯处设计为圆弧形状。当转弯时，协调控制机器鼠的头部、腰部和后肢电机，使机器鼠沿着预定弧线完成转弯。因此，将机器鼠的整体运动路线设计为长0.9 m，宽0.7 m的带有圆角的矩形。

为了使机器鼠能够沿着预定路线进行运动，同时减少对SLAM过程中的干扰，本实验在预定运动路线上粘贴透明胶带，从而在实验过程中可以控制机器鼠沿着透明胶带进行运动。另外，由于ORB-SLAM2是一种基于特征点法的SLAM算法，因此在实验区域内放置一些实验室常用物品（如工具刀、万用表等），以增加算法能够提取到的特征。同时，实验区域为开放区域，周围无遮挡，因此实验室的环境信息也可以为算法提供远处的特征。实验场景如图6-16所示，实验过程中需要控制机器鼠沿着预定轨迹（图中黑色虚线）进行运动。

图6-16　实验场景：控制机器鼠沿着预定轨迹（黑色虚线）进行运动

机器鼠在运动过程中需要对双目系统的位姿进行实时估计，因此需要跟踪线程处理速度要足够快。根据分析与测试可知，跟踪线程的耗时主要集中在ORB特征的提取与匹配上，因此需要对该过程进行加速处理。为此，采用CUDA作为CPU与GPU混合编程框架，加速ORB特征的提取与匹配，并通过ROS系统完成算法与机器鼠相机之间的通讯。

由于双目系统内参、外参和畸变系数和校正后的变换矩阵已经在4.2.2节中通过标定和立体校正得到，这里可以直接应用到ORB-SLAM2的参数中。ORB-SLAM2算法中使用双目近远特征点进行位姿估计，将计算出的深度值小于ThDepth倍基线的特征点定义为双目近特征点，反之则为双目远特征点。一帧中的近特征点可以有效地进行三角测量从而得到深度信息，并且能够提供尺度、平移和旋转信息。远特征点能够提供准确的旋转信息，但是尺度和平移信息就不准确了。由于机器鼠的双目视觉系统基线只有14 mm，因此可以测量得到的准确深度范围较小（这也符合实验鼠的视觉特征）。为此，取ThDepth阈值为25，即有效深度为0.35 m。由于ORB-SLAM2是基于特征点法，需要提取足够多的特征点用于位姿估计，因此设置每张图片提取的特征点数量为1 200个。此外，还设置图像金字塔的层数为8，变化尺度因子为1.2，提取FAST特征点的默认阈值为20，如果默认阈值提取不出足够的FAST特征点则使用最小阈值8。

三、实验结果与分析

为了测试基于ORB-SLAM2算法的机器鼠实时位姿估计与建图效果，我们在图6-16所示实验场景下进行实验。在实验过程中，上位机发送控制指令控制机器鼠按照预先制定的路线运动一圈。为了便于分析算法的定位精度，假设机器鼠能够完全沿着预定的路线运动。因此，将预定路线作为实验的真值轨迹，同时忽略机器鼠俯仰方向的运动，只对水平二维平面的运动轨迹进行分析。实验过程中共获得314个关键帧，将关键帧坐标绘制成二维轨迹，实验结果轨迹与真值轨迹对比如图6-17（a）所示。其中，黑色虚线表示真值运动轨迹，红点表示算法估计运动轨迹。本实验采用均方根误差（RSME）度量该实验场景下算法的定位精度。RSME是测量值与真值偏差的平方和与测量次数比值的平方根，具体计算公式为

式中，Ne为测量次数（取314）；qi实验中为真值轨迹点；为算法估计的轨迹点。

由于算法估计的轨迹点与真值轨迹点不是一一对应关系，因此本实验只是计算算法估计轨迹点与真值轨迹线最短距离作为偏差，最终计算出的RSME结果为0.081 m，表现出了较好的定位精度。根据实验结果可以看出，算法在直线运动阶段表现很好，与真实轨迹误差较小，但是在转弯阶段表现不佳，最大误差可达0.113 m。原因在于：一方面，相比于直线运动阶段，转弯过程中帧间的特征点变化明显，跟踪容易丢失，造成较大误差；另一方面，由于双目视觉系统的发射端安装在机器鼠的腰部，在转弯阶段腰部的运动有时会触碰到发射端的天线，对图传质量产生影响，一定程度上也降低了定位的精度。本实验的跟踪阶段每帧图像平均耗时约为24 ms，小于双目视觉系统相邻帧之间的时延（33 ms），说明本系统具有较高的实时性。

地图构建结果在Pangolin中进行可视化展现，如图6-17（b）所示[17]。由图可以看出，ORB-SLAM2构建的是稀疏点云地图，蓝色框为关键帧位置，绿色框为当前关键帧所在位置，黑色和红色点为稀疏地图点，其中红色点为当前局部地图点。此外，当前相机位置在图6-17（a）中以相同颜色标出，其箭头代表机器鼠的运动方向。

图6-17　实时位姿估计与建图结果（见彩插）

（a）SLAM轨迹与真值轨迹；（b）SLAM构建的稀疏点云