ORB-SLAM2车载双目视觉的自主导航性能评估

沈燕芬，张小红，2，朱 锋

(1.武汉大学 测绘学院，武汉 430079；2.地球空间信息技术协同创新中心，武汉 430079)

0 引言

近几年来，随着无人机、无人驾驶汽车、智能移动机器人相关技术的飞速发展，视觉导航作为一种非传统的定位技术被广泛应用。文献[1-2]提到：视觉导航仅利用单一相机传感器完成，精度稳定且无需场景和运动的先验信息，可避免由于传感器精度降低或低成本惯性测量单元(inertial measurement unit，IMU)漂移严重引起的误差发散问题；其被动无源的特点，也弥补了全球卫星导航系统(global navigation satellite system，GNSS)在复杂城市或室内环境下信号易受遮挡而无法定位的缺陷。

自1982年文献[3]首次提出了一套完整的基于计算机视觉的理论框架以来，视觉导航领域的相关算法和应用层出不穷。视觉导航算法根据其对图像信息的利用程度可分为特征法和直接法。直接法直接对图像所有像素点进行操作，可充分利用环境几何信息，其代表作为文献[4]的基于直接法的大范围单目即时定位和地图构建方法(large-scale direct monocular simultaneous localization and mapping，LSD-SLAM)，在精确计算自身位姿的前提下，还可构建出大规模的半稠密地图。而单目定位构图(monocular simultaneous localization and mapping，MonoSLAM)、平行跟踪与地图构建(parallel tracking and mapping，PTAM)等算法则采用了对图像关键信息进行检测提取的特征法。MonoSLAM基于扩展卡尔曼滤波器实现，将相机位姿及地图点3D坐标作为状态向量进行观测更新，算法计算复杂度较高，且线性化也造成了一定误差。文献[5]提出基于关键帧提取及光束法平差优化的PTAM算法，首次将定位与构图分为2个独立工作的线程。2014年文献[6]提出的半稠密单目视觉里程计算法(semi-direct monocular visual odometry，SVO)则融合了特征法与直接法，首先采用加速分割检测特征(features from accelerated segment test，FAST)角点跟踪关键点，再根据关键点的周围信息估计帧间位姿，其实时性高、算法简洁，非常适用于平台受限的场合，如无人机、手持增强现实(augmented reality，AR)及虚拟现实(virtual reality，VR)设备等，该法对位姿估计精度依赖性强，易出现丢失和重定位的情况。为达到实时性的自主导航定位要求，文献[7]将特征信息在用于位姿估计后不进行保留，后续优化仅依据位姿关系，减轻了存储计算负担，其基于光束法平差(bundle adjustment，BA)的实时视觉定位构图算法成为了在大范围内进行定位构图的开创性作品。文献[8]的基于外观的实时定位构图方案(real-time appearance-based mapping，RTAB-MAP)则从内存管理机制角度出发，将定位点根据被访问的频率及存在的时间长度设置权重，以此为依据进行分类与修剪，从而提高了回环检测的效率。

文献[9-10]提出的基于旋转不变特征点的实时定位构图系统(oriented brief-SLAM，ORB-SLAM)被公认为现有综合性能最好的视觉导航算法：在整体架构上延续了PTAM的多线程思想；但改进了PTAM需手动进行初始化的缺陷，扩展了应用环境的多样性和系统的交互性。ORB-SLAM实现了基于单目相机的实时算法，而ORB-SLAM2在此基础上扩展为双目、RGBD相机，并被广泛应用于采集车、无人机及室内外机器人等设备。

本文采用不同采集环境的车载数据对双目视觉自主导航性能进行评估，重点分析其整体定位定姿精度、累积误差随距离的变化情况以及回环检测对定位结果的影响。

1 利用ORB-SLAM2进行位姿估计

ORB-SLAM2整个系统分为位置跟踪、局部地图优化、回环检测3个并行处理的线程，其整体架构如图1所示。

图1 ORB-SLAM2算法整体架构

跟踪线程主要负责每帧影像的位姿估计。经畸变矫正的双目序列影像和相机的标定参数输入至系统后，采用具有方向信息的多尺度FAST算子对影像进行特征提取，并用256位的具有方向信息的二元加速鲁棒特征(oriented binary robust independent elementary features，rBRIEF)二进制描述符表示特征点。根据小孔成像原理以及双目相机高度一致、光轴平行的特点，左右影像匹配后可重构出每对匹配点在相机坐标系中的坐标。将第一帧影像的左相机坐标系作为系统参考坐标系，通过序列影像的帧间匹配可将特征点的世界坐标进行传递和扩展，在帧间进行3D-3D对应估计可获得参考坐标系与每帧影像的相对位姿关系。设第i帧中某匹配特征点在该帧相机坐标系中的坐标为(x,y,z)，与i-1帧进行匹配可得该点在系统参考坐标系中的坐标为(X,Y,Z)，即

(1)

(2)

式中：(uj,vj)为第j个特征点的像素坐标；N为匹配到的特征点总数；ρ为Huber损失函数用于外点的剔除；project()表示3D-2D重投影函数；Xj为第j个匹配点在系统参考坐标系中的x坐标；Σ为和每个特征点的尺度相关的信息矩阵。

在对每帧影像进行以上操作的同时，局部地图优化线程存储关键点和关键帧，实时地优化局部若干帧的位姿及相应3D点的位置，对误差进行修正；而回环检测线程则通过回环探测和融合实现全局位姿优化。

通过位姿估计得到帧间的旋转平移关系后，可以初始位置为坐标原点，仅考虑车辆在系统参考坐标系的运动情况，反演出载体的行驶轨迹为

(3)

式中：P0为初始时刻相机在系统参考坐标系的位置坐标；Pi表示第i帧影像的相机拍摄中心在系统参考坐标系下的位置坐标；m为总帧数。若已知初始坐标原点的绝对位置，则可通过坐标变换递推得车辆在世界坐标系下的绝对行驶轨迹，实现车载双目视觉导航。

2 ORB-SLAM2位姿估计的关键技术

ORB-SLAM2可有效对目标物体进行跟踪，即使丢帧也可快速重定位，能够实现可靠的自主导航；其关键技术集中在特征提取与匹配和位姿精度优化2部分。文献[11]提到，特征提取与匹配的准确度保证了每帧左右及帧间影像重构的三维点的准确度，从而决定了位姿估计的整体精度，故提高特征提取与匹配的正确率是算法提升自主导航鲁棒性的有力措施；而多次的位姿精度优化则是在此基础上的双重保障。

1)特征提取与匹配：特征在提取时候做了网格处理，从而避免了特征点选取的不均匀性，在左右影像间进行特征匹配时相继用到了带状搜索、绝对差值和SAD(sum of absolute differences，一种图像匹配算法)滑窗以及抛物线拟合以确保匹配点达到亚像素级的精度；帧间匹配，在缺乏先验信息时，抛弃了传统的高维图像特征描述子比对，转而使用词袋模型(bag of words，BoW)对描述子进行分类，通过类别节点的设置大大简化了运算，反之则通过重构特征点、三角化或相似变换约束缩小搜索范围以加速匹配，并对特征点加入了正向景深、视差距离、反投影误差、尺度及旋转一致性的严格判定。

2)位姿精度优化：除单帧位姿优化外，系统还将在关键帧插入后对局部窗口帧及地图点进行局部光束法平差(local bundle adjustment，Local BA)，在成功进行回环修正后对所有关键帧及地图点进行全局光束法平差(full bundle adjustment，Full BA)。Local BA对由关键帧和地图点的连接关系进行优化，构造而成的共视图(covisibility graph，CG)图2所示。

图2 Local BA优化共视

如图所示，邻接共视帧KL1、KL2、KL3的位姿及其共视点PL1、PL2，PL3的位置在优化中被修正，而包含共视点但非邻接关键帧KF1、KF2的位姿则在优化中被设为固定，只参与计算，不进行优化。若帧间特征点的匹配关系(用χ表示)已知，则其优化函数可表示为

(4)

式中：R、t分别为对应帧的旋转矩阵和平移向量；下标l表示对应的共视帧；下标k表示共视帧与关键帧的交集。当通过一系列自适应阈值筛选得到可靠的候选回环帧并进行回环修正后，系统会另开辟线程执行Full BA。Full BA是Local BA的扩展，除初始帧外，所有关键帧和地图点均被优化；故可有效解决因误差累积导致的尺度漂移问题，大大提高系统的定位测姿精度，提升双目自主导航的可靠性与稳定性。

3 实验与结果分析

为了对基于ORB-SLAM2的车载双目视觉自主导航进行全面的性能评估，本节将采用不同道路采集环境的数据对其算法的整体定位测姿性能及其累积误差随载体行驶距离的变化、回环检测对自主导航结果的影响等方面做细致分析。

本次分析采用的数据为卡尔斯鲁厄工学院和丰田技术学院(Karlsruhe Institute of Technology and Toyota Technological Institute，KITTI)联合提供的数据集的序列影像，其采集环境主要为高速公路、公路及居民街区。实验设备包含1套OXTS RT 3003的GPS/IMU组合导航系统、4个灰点公司的Flea 2系列相机、4个埃蒙德的4～8 mm变焦镜头，分析数据仅涉及其中2个灰色相机。相机与地平面平行安装，采样帧率为10 Hz，其实验设备和不同环境的采集影像如图3所示。

3.1 整体精度分析

以差分GPS/IMU的组合定位结果为参考值，解算轨迹与参考值的偏差值大小，结果由不同灰度值表示，如图4所示，灰度越深代表误差越大。

图3 实验装备与数据采集

图4 整体轨迹误差

由图4可知，所有轨迹均与真值重合度较高，整体定位精度约为1.02 %，即行驶100 m偏离真值1 m，角度误差为0.001 9 deg/m，各方向定位精度在4～5 m左右，定姿精度在0.6～0.7 deg之间，如表1所示。

由表1可知：回环轨迹的整体定位定姿精度显著优于无回环轨迹；单闭环轨迹在回环融合附近的误差修正效果明显，而无回环轨迹随着行驶距离的增加误差明显逐渐发散。对多回环影像序列2、序列3的异常轨迹分析发现，大转弯、照片过曝或阴影遮挡会影响定位测姿精度，而影像中的在相机视野内持续运动物体会严重干扰位姿估计，如图5所示，3号轨迹中异常轨迹段中，有一辆摩托跟随行驶了500多米。

图5 3号序列影像中特殊场景分析

3.2 累积误差随距离变化的影响分析

增量式的位姿估计会导致绝对位置误差不断累积，位姿愈发发散，从而最终限制系统的整体精度。

选取无回环的9号高速公路影像(全长2 451 m)及多回环的1号居民街区影像(全长2 204 m)进行处理，并统计其累积误差(滑窗宽度为100 m，统计窗口内为位置误差及姿态估计误差的均方根误差)，如图6所示。

图6 位置、姿态累积误差随距离的变化

由图6可知：高速公路环境下的行驶路线，各方向位置精度随距离增长呈明显线性发散趋势，行驶到2 500 m左右时，侧向精度将近达到25 m，而居民街区轨迹由于多次的回环修正，各方向精度均始终保持在2 m左右；姿态精度在2种采集模式下无明显规律可循，均始终保持在1 deg以内，原因在于相对姿态误差在局部的位姿优化中有很大程度的修正，而回环影像的优势主要在于可修正由误差积累所导致的尺度漂移的问题。故无回环轨迹各方向的位置精度随距离呈约1 %的线性发散，而经过回环修正则可使精度始终保持在2 m，提高了车载自主导航的鲁棒性。

3.3 回环修正对结果的影响分析

为进一步定量分析回环修正对定位测姿精度的改善情况，本小节选取4组长距离回环序列影像进行分析，对有回环修正和无回环修正2种模式下的定位测姿精度分别进行统计，并重点对6号序列影像进行细致分析，如表2所示。

表2 回环修正结果精度统计

由表可知，针对不同序列影像，闭环检测平均约可改善36 %的位置精度和50 %的姿态估计精度。

进一步分析比较6号影像轨迹(如图7所示)可以发现：没有进行回环修正的轨迹尺度漂移明显，各方向的位置逐渐偏离参考值，误差逐渐增大，姿态误差也存在发散趋势，但幅度较小；反之，成功的回环修正可使得起点与终点高度重合，明显修正位置误差，起到抑制误差发散、优化整体位姿精度的作用。

图7 6号影像轨迹比较

4 结束语

本文介绍了利用ORB-SLAM2车载双目视觉自主导航的基本位姿估计原理及其关键技术，重点对ORB-SLAM2的自主导航性能进行评估，分析了其整体定位测姿精度、累积误差随行驶距离的变化情况以及回环修正对结果的具体影响。对一定量序列影像数据分析得出：该算法的整体定位相对精度约为1.02 %，定姿相对精度约为0.001 9 deg/m；在无回环探测时，位置误差随行驶距离呈现明显的1 %线性发散；成功闭环探测约可提高36 %的位置精度和50 %的姿态精度，且无发散情况，定位精度始终保持在2 m左右。故该算法存在和其他导航系统或传感器，如GNSS、IMU等进行组合导航的理论基础。目前视觉导航发展的难点和热点为高精度、高效率及多传感器融合，故在接下来的研究中，将着重对视觉导航和IMU及GNSS组合系统的相对姿态标定以及组合导航方案进行深入研究，望实现一套完整可靠、低成本、高精度的双目视觉辅助惯导/GNSS的组合导航实时系统。

[1] SCARAMUZZA D，FRAUNDORFER F.Visual odometry：Part I：the first 30 years and fundamentals[J].IEEE Robotics & Automation Magazine，2011，18(4)：80-92.

[2] 权美香，朴松昊，李国.视觉SLAM综述[J].智能系统学报，2016，11(6)：768-776.

[3] 卢维.高精度实时视觉定位的关键技术研究[D].杭州：浙江大学，2015.

[4] ENGEL J，SCHÖPS T，CREMERS D.LSD-SLAM：large-scale direct monocular SLAM[C]//Anon.Proceedings of European Conference on Computer Vision.Zurich：Springer，2014：834-849.

[5] KLEIN G，MURRAY D.Parallel tracking and mapping for small AR workspaces[EB/OL].[2017-02-19].http：//www.robots.ox.ac.uk/～gk/publications/KleinMurray2007ISMAR.pdf.

[6] FORSTER C，PIZZOLI M，SCARAUZZA D.SVO：fast semi-direct monocular visual odometry[EB/OL].[2017-02-19].http：//rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf.

[7] KONOLIGE K，AGRAWAL M.FrameSLAM：from bundle adjustment to real-time visual mapping[J].IEEE Transactions on Robotics，2008，24(5)：1066-1077.

[8] LABBÉ M，MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics，2013，29(3)：734-745.

[9] MUR-ARTAL R，MONTIEL J M M，TARDS J D.ORB-SLAM：a versatile and accurate monocular SLAM system[J].IEEE Transactions on Robotics，2017，31(5)：1147-1163.

[10] MUR-ARTAL R，TARDS J D.ORB-SLAM2：an open-source slam system for monocular，stereo，and RGB-D cameras[J].IEEE Transactions on Robotics，2016，33(5)：1255-1262.

[11] FRAUNDORFER F，SCARAMUZZA D.Visual odometry：Part II：matching，robustness，optimization，and applications[J].IEEE Robotics & Automation Magazine，2012，19(2)：78-90.