ORB-SLAM算法在智能家庭清扫机器人中的应用　

作者 张汪洋1 孙伟伟1 肖琦1 鹿飞2

　　1.中国矿业大学徐海学院(江苏 徐州 221000)2.上海东软载波微电子有限公司(上海 200000)

摘要：对于家庭智能清扫机器人而言，机器人自我定位与路径规划是其实现智能化、全自动化的理论基础。实时的定位与构图(simultaneous localization and mapping，SLAM)技术可以很好的解决这个问题。该技术可以利用机器人自身所携带的各式传感器对周围环境进行采集，并利用采集结果递增地创建环境地图，同时使用该地图实现机器人对自身的定位与导航功能。本文将通过讨论ORB-SLAM算法系统的执行过程，并进行仿真实验，验证该算法在对家庭清扫机器人的自我定位与导航的实用性。

0 引言

　　近年来智能家居的发展使得我们的居家生活变得更加充满乐趣与效率。然而对于像智能清扫机器人这类室内移动型机器人而言，机器人自行定位和对周边环境的识别是其能否正常高效工作的根本。激光或相机进行实时的定位与构图(simultaneous localization and mapping，SLAM)^[1]是一种相对比较完善的移动机器人自主定位的方法，当机器人处于一个未知的环境中，通过其所携带的图像采集模块对机器人周边的环境进行观测，再递增地构建环境地图，并且通过构建的环境地图对机器人实现实时的定位。

　　近年来SLAM算法取得了巨大的发展，进而产生了对单目摄像机支持度较高的ORB-SLAM算法。ORB-SLAM是由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年发表在IEEE Transactions on Robotics上。该算法融合了PTAM算法的主要思想以及Strasdat提出的闭环修正方法[1]并采用ORB特征点进行跟踪、构图、重定位、闭环修正以及初始化，并且算法的环境适应力强，对剧烈运动也很鲁棒。正因为ORB-SLAM是基于特征点的SLAM系统，因此其能够实时的计算出相机的运动轨迹，并且生成稀疏的场景三维重建结果。

1 ORB特征点提取原理

　　ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点的提取与描述的算法。ORB算法通常分为两部分，分别是特征点提取和特征点描述。

　　特征点提取是由FAST(Features from Accelerated Segment Test)算法发展而来的，即为OFAST(FAST Key pointOrientation)也就是说在使用FAST提取特征点之后再给特征点定义一个方向，以此来实现特征点的旋转不变性。通常是使用灰度质心法来计算以特征点为圆心半径为r的圆形领域范围内的灰度质心的位置，并根据从中心位置到质心位置的向量来定义该特征点的主方向。

　　定义矩的计算公式为：

(1)

　　在上式中I(x,y)为图像的灰度表达式，则该矩质心为：

(2)

　　假设点坐标为O，则向量角即为该特征点的方向。计算公式为：

(3)

　　特征点描述则是根据BRIEF(Binary Robust Independent Elementary Features)特征描述算法改进而来的。BRIEF为了解决旋转不变性将特征点的Patch旋转至主方向上(steered BRIEF)。具体操作步骤不再赘述。

　　ORB的特征点检测法则是将FAST特征点检测法同BRIEF特征描述算法相互结合起来并加以改进与优化。

2 ORB-SLAM算法原理

　　ORB-SLAM算法通常利用三个线程分别进行追踪(Tracking)、地图构建(LocalMapping)和闭环检测(LoopClosing)。如图1所示。

　　2.1 追踪阶段

　　在摄像头输入图像帧之后的追踪阶段通常分为以下几步进行运行，即ORB特征提取、帧间初始姿态估计/重定位、姿态优化、选取新的关键帧[2]。

　　2.1.1 初始姿态估计/重定位

　　ORB-SLAM在初始姿态估计部分主要使用了以下几种运动模型(Tracking with motion model)、关键帧(Tracking with reference key frame)和重定位(Relocation)^[3]。

　　运动模型的本质在于使用上一帧的位姿与速度来估计当前帧的位姿，而上一帧的速度则是由前面几帧的位姿计算得出的。由于运动模型的计算本质限制，运动模型通常适用于相对于机器人来说匀速运动的物体。但是如果是运动相对随意的目标该模型就会失效，这个时候系统就会使用关键帧与重定位了。如果运动模型无法满足条件时，系统会尝试和距离当前帧最近的一个关键帧去进行匹配，在ORB-SLAM中利用了BOW(Bag of Words)来加速关键帧与当前帧的匹配。如果当前帧与邻近帧的匹配也失效的话，则说明此时的当前帧已经丢失，所以无法通过上述方法确定机器人的真实位置。这个时候就只能去与所有的关键帧进行匹配，以此来查找合适的位置。

　　2.1.2 姿态优化

　　在初始姿态的估计进行完毕之后就是进行姿态优化，即是在当前帧与地图之间尽可能多的查找对应关系，来对当前帧的位姿进行优化。位姿优化是非关键帧姿态估计的精确度与鲁棒性的保证。

　　2.1.3 选取关键帧

　　当追踪阶段完成之后首先对运动模型进行更新，并且通过之前的计算我们得到一个对机器人位姿的初始估计，之后通过投影从已经生成的地图点中找到更多的对应关系对结果进行精确化。由于全局图中进行投影的较为复杂，通常只是在局部图中进行投影。

　　将局部地图中和当前帧相类似的点关键帧序列称为K1，在Covisibility Graph中与K1相邻的关键帧序列称为K2。局部地图有意者参考关键帧Kref∈K1，该关键帧具有和当前帧最多的共同地图云点。

　　为了搜索K1、K2可见的每个地图云点我们首先将地图点对当前帧进行投影，倘若超出了图像的范围就将其舍弃;之后计算当前视线方向向量v与地图点云平均视线方向的方向向量n的夹角，舍弃n-v

　　2.2 地图构建阶段

　　当追踪完成之后我们可以得到一个新关键帧Ki，接下来将根据Ki进行局部建图。在局部建图通常包括关键帧插入，冗余地图点和关键帧剔除以及局部集束调整等步骤。

　　2.2.1 关键帧的插入

　　将由追踪阶段所得到的关键帧Ki作为新的节点加入Covisibility Graph，同时对关键帧Ki的生长树和能够共享地图点的关键帧节点相互连接的边进行更新，并计算表示关键帧的词袋BOW。之后依据计算词袋的结果将地图点整合到新的关键帧之后，对Covisibility Graph进行更新，把新的关键帧插入Map中。

　　2.2.2 冗余地图点的剔除

　　为了保证地图点可追踪，且不易在三角化时出现较大的误差，则需要在创建点云的前三帧进行约束测试，通过的地图点才可以真正的被保存。通常一个点同时满足：该点应当在可以观察到该点的关键帧中，并且应有超过25%的关键帧可以对该点进行跟踪。若要构建该点为地图点则它必须被超过两个关键帧观察到(硬件如果是双目摄像头则为三个关键帧)。当地图点被少于三个关键帧能够观察到时该点就会被当作冗余点剔除。这样有效的防止了外点影响效果。

　　2.2.3 新的地图点创建

　　通过检测到的ORB特征点，查找Covisibility Graph中与之相连的关键帧Kc，匹配方法则是上文中提到的约束测试的方法进行筛选匹配，并且将不满足要求的点全部舍弃。对ORB特征点进行三角化之后检查正向的景深、视差、反投影误差以及尺度一致性，如果无误即可得到地图点。通常一个地图点由两个关键帧所观察得到的，并且它也可以投影到与之相连的其他关键帧中，此时为了得到更多的地图点一般算法会使用追踪局部地图的方法来在附近的关键帧中找到可以与之匹配的关键帧。

　　2.2.4 局部集束调整

　　局部集束调整就是将当前处理的关键帧Ki进行优化，通常参与优化的关键帧只参与优化中的约束，并不作为变量去改变值。同时优化时产生的外点会在优化的中后期进行剔除。

　　2.2.5 局部关键帧的剔除

　　为了控制地图重建的紧凑度，在这一阶段会检测并剔除冗余的关键帧，这样可以有力的控制因为关键帧数目增长之后集束调整的复杂程度。

　　在结束完这一系列步骤之后，关键帧会被记录到数据库列表中，并将标志设置为Set Accept Key Frames(true)以允许追踪线程可以继续得到关键帧。自此完成局部地图构建的全部工作。

　　2.3 闭环检测线程

　　在实际应用情况下即使在之前的步骤中我们也利用了诸如集束调整等方法去消减误差，但随着运行时间的增加依然会存在累积误差[5]。消除累积误差最为有效的方法是对之前运行得出的结果进行闭环检测。

　　首先算法会进行闭环条件检测，计算出当前关键帧与Covisibility Graph中与其相连的关键帧之间的词袋(BOW)的相似度。通常使用DBoW2库进行闭环条件检测。

　　之后算法将进行Sim3的计算。由于针对一个三维的点而言，单目相机不能对其在三角化中的具体位置进行确定。因此单目SLAM是一个Sim3的相似变换群，拥有7个自由度，即为[sR|t]。

　　接下来是对重复出现的点云进行闭环融合，同时在Covisibility Graph中插入新的边，连接闭环。

　　最后对上述所得到的位姿图用本质图(Essential Graph)进行优化，将闭环的误差进行分散，保证闭环检测的准确性。

3 实验测试

　　为了验证ORB-SLAM在扫地机器人上进行自我定位的实用性，我们使用了单目摄像头与树莓派3平台进行了搭载ORB-SLAM2系统。

　　实验过程：将搭建好的扫地机器人平台放在一个简单模拟家居环境中，并操作使其对周围的环境图像进行采集并生成一个稀疏的三维环境地图以及机器人行进轨迹。将地图传回PC机并与实际地形进行对比。

4 结论

　　根据上述实验结果来看，ORB-SLAM在实验平台上可以很好的实现机器人的自我定位与导航。这验证了ORB-SLAM在扫地机器人上推广使用的可行性。同时减少了日后扫地机器人进行视觉定位与导航的成本，大大加快了扫地机器人真正普及千家万户的步伐。 

　　参考文献：

　　[1]周绍磊,吴修振,刘刚,等.一种单目视觉ORB-SLAM/INS组合导航方法[J].中国惯性技术报,2016,24(5):633-637.

　　[2]林连秀,叶芸,姚剑敏,等.基于ORB-SLAM的移动机器人嵌入式实现与优化[J].微机与应用,2017,36(5):50-53.

　　[3]刘浩敏,章国锋,鲍虎军.基于单目视觉的同时定位与地图构建方法综述[J].计算机辅助设计与图形学学报,2016,28(6):855-867.

　　[4]高翔,张涛,刘毅,等.视觉SLAM十四讲 从理论到实践[M].北京:电子工业出版社,2017,3.

　　[5]刘浩敏,章国锋,鲍虎军.面向大尺度场景的单目同时定位与地图构建[J].中国科学,2016,46(12):1748-1761.

　　本文来源于《电子产品世界》2018年第9期第40页，欢迎您写论文时引用，并注明出处。