一种单目视觉ORB-SLAM/INS组合导航方法

周绍磊，吴修振，刘 刚，张 嵘，徐海刚

（1. 海军航空工程学院 控制工程系，烟台 264001；2. 清华大学 精密仪器系，北京100084；3. 北京自动化控制设备研究所，北京 100074）

一种单目视觉ORB-SLAM/INS组合导航方法

周绍磊1，吴修振1，刘 刚1，张 嵘2，徐海刚3

（1. 海军航空工程学院 控制工程系，烟台 264001；2. 清华大学 精密仪器系，北京100084；3. 北京自动化控制设备研究所，北京 100074）

针对惯性/卫星组合导航系统在卫星导航失效时无法使用的问题，提出了单目视觉ORB-SLAM/INS组合导航方法，用于扩展组合导航系统在强干扰环境和室内环境的应用范围。该算法分为两个阶段：初始化阶段，当ORB-SLAM形成闭环时设计算法在线估计单目视觉ORB-SLAM算法的尺度因子；导航阶段，ORB-SLAM系统输出的位置信息经过尺度变换后作为观测量进行卡尔曼滤波，估计INS导航系统的误差状态量从而修正惯导系统的误差。设计了硬件和软件平台对提出的组合导航方法进行试验验证。跑车实验结果表明：所设计的 ORB-SLAM/INS组合导航系统具有较高的定位精度，导航时间6 min定位误差为1.162 m，且不随时间漂移，具有很强的应用价值。

ORB-SLAM；INS；尺度因子；卡尔曼滤波；组合导航

随着社会需求的增加和科学技术的进步，移动机器人越来越多地融入日常的生活中，提高了生活效率，节约了生活成本，正在创造越来越多的社会价值。在移动机器人的技术中，首要解决的是机器人的定位问题，这是机器人完成特定任务的基础。

对于室外机器人（如四旋翼无人机），定位主要依靠卫星导航系统和惯性导航系统；对于室内机器人，定位主要依靠光学运动捕捉系统（如 Vicon）或者利用激光或者相机进行实时定位和构图（Simultaneous Localization and Mapping, SLAM）。卫星导航属于有源定位方法，当信号受到干扰时定位系统无法正常工作，光学运动捕捉系统虽然定位精度高，但成本昂贵且环境适应性差。惯性导航和视觉SLAM属于无源定位方法，惯导误差会随着时间积累并发散，视觉SLAM由于具备闭环修正技术因此定位精度较高。单纯地依赖一种定位方法很难取得理想的定位效果，本文的着眼点在于将惯性导航和单目视觉SLAM两种无源定位方法相互融合，取长补短，构建具有较高定位精度和鲁棒性的无源定位系统。

单目视觉SLAM技术近年来发展迅速，由最初的基于滤波的方法逐渐发展到基于关键帧的方法，Strasdat等人[1]证明基于关键帧的方法比基于滤波的方法精度更高。基于关键帧的方法最具代表性的是Klein和Murray提出的PTAM算法[2]，第一次提出将跟踪定位和构图分为两个线程并行处理，检测的特征点为FAST特征点。Pirker等人[3]提出了CD-SLAM算法，是一个包括闭环修正、重定位等比较完善的SLAM系统，但没有涉及地图的初始化方法。Song等人[4]提出的视觉里程计算法将ORB特征点（FAST+BRIEF）用于跟踪和光束平差法（Bundle Adjustment，BA）优化，但没有全局重定位和闭环修正。Engel等人[5]提出了LSD-SLAM算法，能够建立半密度地图，相对稀疏特征点地图更具有实用意义，直接使用像素密度进行优化而不是通过基于特征点的光束平差法，但此算法依然需要稀疏特征点进行闭环检测。R.Mur-Artal和J.D.Tardos等人[6-7]提出了 ORB-SLAM 算法，融合了PTAM算法的主要思想和Strasdat提出的闭环修正方法，整个过程均采用ORB特征点[8-9]进行跟踪、构图、重定位和闭环修正，与前面提出的SLAM方法相比，具有较高的定位精度和实时性。

单目视觉SLAM的优点是成本低，实际应用简单方便，但由于单目相机无法获得深度信息，这就导致单目视觉SLAM无法得到准确的尺度信息，因此必须通过一定的方法估计出尺度因子，单目视觉SLAM系统输出的位置信息经过尺度变换后才能用于实际导航。

综上所述，本文采用 R.Mur-Artal等人提出的ORB-SLAM算法，提出了单目视觉ORB-SLAM/INS组合导航方法。组合导航的过程分为两个阶段：

第一阶段：初始化阶段。通过规划运动航迹使ORB-SLAM系统形成闭环，用ORB-SLAM系统输出的位置信息和差分 GPS输出的准确位置信息在线估计ORB-SLAM系统的尺度因子。

第二阶段：导航阶段。ORB-SLAM系统输出的位置信息经过尺度变换后作为观测量进行卡尔曼滤波，估计INS导航系统的误差状态量后，用于修正惯导系统的误差。

1 单目视觉ORB-SLAM算法

单目视觉ORB-SLAM算法分为三个并行的线程：跟踪、本地地图构建以及闭环控制，算法总体框架如图1所示。设计该算法的具体实施过程如下：

图1 单目视觉ORB-SLAM算法Fig.1 Monocular ORB-SLAM algorithm

1）通过张正友棋盘标定法对摄像头的畸变参数进行标定，用标定好的参数对每一帧图像进行畸变修正，然后对图像进行高斯滤波，消除噪声，为特征点提取做准备。

2）提取图像的ORB特征点[10]，即采用具有方向信息的多尺度FAST特征点，用32个字节256位比特数的BRIEF作为描述符，即一个特征点用一个256位的0或1表示，这样就构成了256维空间的特征点空间。

3）利用词袋技术[11]对特征点空间进行分类，分为M×N层，通过汉明距离就可以把提取的ORB特征点在空间中进行分类。

4）前后帧图像中的特征点经过分类就可以在不同的空间位置上进行快速匹配，通过匹配特征点满足的极线约束关系就可以建立前后帧图像几何关系，从而得到相机的实时位姿信息。

5）对每一帧图像进行上述操作，存储关键点和关键帧，从而构建环境地图信息，对新增加的关键点和关键帧在已建立的地图中搜索匹配实现重定位或者闭环检测。在整个SLAM过程中，通过光束平差法不断对误差进行修正，当检测到闭环时，对整个地图进行优化，从而提高SLAM定位的精度。

综上所述，单目视觉ORB-SLAM算法融合了近年来在视觉SLAM领域最新的研究成果，在特征点选择、地图跟踪、闭环检测、重定位、关键帧地图点的取舍等方面都有比较全面的优化和改进，位姿估计精度高，鲁棒性强，从而使得其与惯性导航系统进行组合成为可能。但由于单目视觉本身不具有深度信息，尺度因子无法确定，因此尺度因子的估计问题是单目视觉ORB-SLAM/INS组合导航系统首要解决的问题。

2 单目视觉ORB-SLAM尺度因子的估计

根据文献[7]，ORB-SLAM 输出的位置信息与相机真实的位置信息（由差分GPS得到）满足相似变换的关系，因此通过相似变换的求解可以估计单目视觉ORB-SLAM 尺度因子，此过程为单目视觉 ORBSLAM/INS组合导航的第一阶段——初始化阶段。

单目视觉ORB-SLAM系统的导航参考坐标系为地图初始化时的相机坐标系 Ocxcyczc，惯性导航系统和差分GPS的导航坐标系为东北天地理系 Otxtytzt，如图2所示。

图2 坐标系定义Fig.2 Definition of coordinate system

当 ORB-SLAM 系统第一次形成闭环时，假设ORB-SLAM 系统输出的相机位置为，对应差分 GPS输出真实的位置信息，理想情况下，根据相似变换的原理，则有

其中，R为坐标系 Ocxcyczc到坐标系 Otxtytzt的旋转矩阵，t0平移向量。R和t0可通过组合导航系统的标定参数直接获得；s为ORB-SLAM系统的尺度因子，在初始化阶段需要估计此参数的值。

由于系统测量误差的存在，式(1)的左右两边是不等的，记残差为

尺度因子s的估计问题转化为求取最优值s使得：

分别求取n个位置点的中心，记为

中心平移后的位置点记为

根据文献[15]，满足式(3)尺度因子s的最优值为

3 ORB-SLAM/INS组合导航算法设计

求得尺度因子最优值s后，ORB-SLAM/INS组合导航系统就进入导航阶段，通过式(1)把 ORB-SLAM得到的位置信息转化到惯导系统的地理导航系中作为观测量估计惯导系统的误差状态量。下面设计卡尔曼滤波模型估计惯导系统的误差[12-13]。

选取15维状态量，分别为姿态误差、速度误差、位置误差、陀螺零位误差、加速度计零位误差：

状态转移方程为：

其中，子矩阵Fij各个分量值见文献[14]。

以ORB-SLAM视觉导航位置作为Kalman滤波的量测值，量测方程为

估计惯导系统误差状态后，使用闭环反馈方式对惯性器件误差、惯性导航误差进行修正，约束惯导误差发散。

4 试验验证与分析

4.1 试验平台搭建

搭建试验平台对设计的单目视觉/INS组合导航算法进行跑车试验，试验平台如图3所示。

试验所用的主要硬件设备型号及参数如下：

1）摄像头

IDS工业相机：UI-1240LE-C-6；分辨率：1280×1024。

图3 试验平台Fig.3 Experimental platform

定焦镜头：UH0620-10M；视场角：54.1H°×41.9V°。

2）惯导系统

中星测控惯性测量单元，CS-IMU-07。陀螺测量范围：≥±200 (°)/s；陀螺零偏：≤0.2 (°)/s；陀螺零偏稳定性：≤0.015 (°)/s；加表测量范围：≥±20g；加表零偏：≤0.05g。

3）差分GPS

Novatel公司，FlexPak6，定位精度为5 cm

4.2 试验过程

试验地点：烟台市体育公园；试验时间：2016年8月9日；

试验过程：在120 m×120 m的空旷场地范围内进行闭环跑车试验（无大面积遮挡，保证 GPS信号有效），共跑3圈，共耗时333 s。试验过程中采集摄像头、惯性系统以及差分GPS的数据。

4.3 试验结果与分析

假设此范围内地理系是固定的，以试验开始时的东北天地理坐标系为参考坐标系O-XYZ，差分GPS的定位数据为基准参考值，经纬度信息转换到参考坐标系中，单目视觉SLAM /INS组合导航的结果也转换到此坐标系中，在参考坐标系O-XYZ进行定位误差分析与比较。

单目视觉ORB-SLAM的试验结果如图4所示，其中：图4(a)显示了特征点检测过程，绿色方框代表当前帧匹配上的FAST特征点；图4(b)代表单目ORB-SLAM地图构建和定位过程，黑色和红色点代表存储的地图点，蓝色表示存储的关键帧，绿色代表了轨迹。

图4 单目视觉ORB-SLAM的试验结果Fig.4 Experiment result of monocular ORB-SLAM

单目视觉ORB-SLAM与基准差分GPS的轨迹对比如图5所示。纯惯性导航的导航误差如图6所示。单目视觉ORB-SLAM的导航误差如图7所示。单目视觉/INS组合导航误差如图8所示。

图5 ORB-SLAM与差分GPS的轨迹对比Fig.5 Trajectory comparison of ORB-SLAM and differential GPS

由图5～8可知，纯惯性导航结果随时间发散，与纯惯性导航相比，单目视觉ORB-SLAM/INS组合导航算法具有较高的定位精度。由数据统计分析可知，组合导航定位误差为 1.162 m，因此，单目视觉ORB-SLAM/INS组合导航能够实现无GPS条件的导航与定位。

图6 纯惯性导航误差Fig.6 Navigation error of pure INS

图7 ORB-SLAM导航误差Fig.7 Navigation error of ORB-SLAM

图8 单目视觉ORB-SLAM/INS组合导航误差Fig.8 Integrated navigation error of monocular ORB-SLAM/INS

5 结 论

本文把单目视觉 ORB-SLAM与惯性导航系统相结合，提出了单目视觉 ORB-SLAM/INS组合导航算法，并设计了试验平台对所提出的算法进行了验证。结果表明，所设计的 ORB-SLAM/INS组合导航系统是切实可行的，具有较高的定位精度。

目前提出的组合导航算法处于浅组合阶段，下一步将开展单目视觉 ORB-SLAM/INS深组合导航算法研究，进一步提高导航精度和鲁棒性，主要体现在以下两个方面：

1）ORB-SLAM系统进行特征点的跟踪时，利用惯性导航系统的姿态信息进行特征点的运动估计从而提高特征点匹配的速度和准确度；

2）当ORB-SLAM系统跟踪失败时，利用INS导航系统输出的位置和姿态信息作为初始状态进行ORB-SLAM系统的重启动，从而提高了ORB-SLAM系统的鲁棒性。

（References）：

[1]Strasdat H, Montiel J M M, Davison A J. Visual SLAM:why filter[J]. Image and Vision Computing, 2012, 30(2):65-77.

[2]Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//2007 IEEE and ACM International Symposium on Mixed and Augmented Reality(ISMAR). 2007: 225-234.

[3]Pirker K, Ruther M, Bischof H. CD SLAM-continuous localization and mapping in a dynamic world[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2011: 3990-3997.

[4]Song S, Chandraker M, Guest C C. Parallel, real-time monocular visual odometry[C]//2013 IEEE International Conference on Robotics and Automation (ICRA). 2013:4698-4705.

[5]Engel J, Schops T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]//2014 European Conference on Computer Vision (ECCV). 2014: 834–849.

[6]Mur-Artal R, Tardos J D. Fast relocalisation and loop closing in keyframe-based SLAM[C]//2014 IEEE International Conference on Robotics and Automation (ICRA).2014: 846-853.

[7]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, 1:16.

[8]Rosten E, Drummond T. Machine learning for high-speed corner detection[C]//2006 European Conference on Computer Vision. 2006: 430-443.

[9]Calonder M, Lepetit V, Strecha C, et al. BRIEF: Binary robust independent elementary features[C]//2010 European Conference on Computer Vision. 2010: 778-792.

[10]Rublee E, Rabaud V, Konolige K, et al. ORB: an efficient alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision (ICCV). Barcelona, Spain,2011: 2564-2571.

[11]Galvez-Lopez D, Tardos J D. Bags of binary words for fast place recognition in image sequences[J]. IEEE Transactions on Robotics, 2012, 28(5): 1188-1197.

[12]王云舒, 刘建业, 曾庆化, 等. 结构光辅助的惯性/视觉室内导航三维环境重构方法[J]. 中国惯性技术学报,2016, 24(1): 53-58.Wang Yun-shu, Liu Jian-ye, Zeng Qing-hua, et al. 3D environment restructure method with structured light for indoor vision/inertial navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 53-58.

[13]张红良, 郭鹏宇, 李壮. 一种视觉辅助的惯性导航系统动基座初始对准方法[J]. 中国惯性技术学报, 2014,22(4): 470-475.Zhang Hong-liang, Guo Peng-yu, Li Zhuang. Vision aided alignment method for inertial navigation system on moving base[J]. Journal of Chinese Inertial Technology,2014, 22(4): 470-475.

[14]刘刚. 基于Kalman跟踪环路的GNSS/INS标量深组合系统研究[D]. 北京: 清华大学, 2015.Liu Gang. GNSS/INS scalar deeply integrated system research based on Kalman tracking loop[D]. Beijing:Tsinghua University, 2015.

Integrated navigation method of monocular ORB-SLAM/INS

ZHOU Shao-lei1, WU Xiu-zhen1, LIU Gang1, ZHANG Rong2, XU Hai-gang3
(1. Department of Control Engineering, Naval Aeronautical and Astronautical University, Yantai 264001, China;2. Department of Precision Instrument, Tsinghua University, Beijing 100084, China;3. Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

In order to solve such problem that an integrated INS/GNSS navigation system stops working in satellite failure situation, a deeply-integrated navigation method of monocular ORB-SLAM/INS is proposed to extend the application range of the integrated navigation system in severe interference environment and indoor circumstances. The method can be divided into two phases: in initialization phase, an algorithm is designed to estimate the scale factor online when the ORB-SLAM forms a closed loop; in navigation phase,the error of INS is corrected after the error states of INS are estimated through a Kalman filter whose observations is ORB-SLAM positions. The hardware and software platforms are designed to test the proposed integrated navigation method. Field test results indicate that the position accuracy of the integrated navigation system is satisfactory, with 1.162 m mean error in 6 min and no drift with time, showing that the proposed method has a notable application value.

ORB-SLAM; INS; scale factor; Kalman filter; integrated navigation

U666.1

A

1005-6734(2016)05-0633-05

10.13695/j.cnki.12-1222/o3.2016.05.013

2016-07-09；

2016-09-28

国家自然科学基金（61004002）

周绍磊（1963—），男，教授，博士生导师，从事导航、制导与控制方面研究。

联 系 人：吴修振（1988—），男，博士研究生，从事导航、制导与控制方面研究。E-mail: wxz_lucky@163.com