室内环境下基于图优化的视觉惯性SLAM方法

徐晓苏，代 维，杨 博，李 瑶，董 亚

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学 仪器科学与工程学院，南京 210096）

室内环境下基于图优化的视觉惯性SLAM方法

徐晓苏1,2，代 维1,2，杨 博1,2，李 瑶1,2，董 亚1,2

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学 仪器科学与工程学院，南京 210096）

基于图优化的即时定位与同步构图（SLAM）方法是在视觉里程计运动估计的基础上通过增加一个回环检测，从而对非线性多约束进行优化来提高定位精度。在视觉运动估计中，针对视觉特征点匹配错误率高的问题，提出了一种ORB特征点聚类抽样匹配跟踪的方法。在位姿图优化上，提出了一种改进型的回环检测方法，减少了两种错误匹配的可能性。最后将视觉SLAM与惯性导航进行组合，提高了系统的稳定性和定位精度。使用公开的室内SLAM测试数据集进行仿真，结果表明，该方法的定位均方根误差在厘米级，生成的点云地图清晰可见。

即时定位与同步构图；图优化；聚类抽样；回环检测；组合定位

在陌生的环境中实现移动机器人的自主导航是机器人领域的热点问题，SLAM被认为是实现这一技术的核心，最早由Stanford的Smith、Self等人在20世纪80年代提出[1]。SLAM的本质上是一个贝叶斯状态估计的问题，可以表示为状态方程以及观测方程。因此传统的视觉SLAM(Visual SLAM，vSLAM)方法多基于Kalman滤波[1]的方式利用自身运动状态以及相机观测环境中的路标进行定位。该方式当前位置仅与上一位置有关，即无法保证全局最优。

随着稀疏矩阵、非线性优化等理论的发展，基于图优化的SLAM应运而生，其本质上是基于平滑的原理，在视觉里程计(Visual Odometry，VO)进行定位的同时建立位姿图，并通过回环检测建立位姿间的多约束，在后端通过非线性优化算法来求解[2]。目前基于图优化的视觉SLAM在前端上存在以下问题[3]：一是视觉特征点匹配错误率高，导致视觉里程计估计精度下降，文献[4]和文献[5]通过对随机抽样一致性算法(RANSAC)进行预验证、假设评价等方式进行改进；二是回环检测方法不合理，导致位姿图中出现错误边约束，文献[7]等采用分割式视觉字典的方式进行改进。

本文针对上述的问题，首先提出了一种快速特征（ORiented Brief，ORB）聚类抽样的算法（Improved clustering Sample Consensus，ICSAC），并且做了全局最优的验证。其次，对视觉字典进行多尺度改进，并对关键帧进行回环检测，降低了“假阴性”检测(少检测)的可能，在二次验证时，采用ICSAC进行错误去除，避免了“假阳性”(多、错检测)的可能。针对视觉SLAM易受环境影响的缺点，以视觉惯性组合定位并基于视觉完成地图的构建，从而建立视觉惯性的SLAM模型。

1 视觉里程计运动估计

视觉里程计通过提取图像间的特征点进行匹配、跟踪，从而完成对机器人三维运动的估计。在室内环境下选用深度相机Kinect进行信息的采集。

1.1 图像配准

Kinect上装有RGB摄像头和深度摄像头，分别用来采集彩色图像和深度图像。由于安装的位置不同，两幅图像在同一时间得到的图像会有细微的差别，其中的差别可以用一组旋转矩阵和平移向量表示，因此要实现图像间的配准。以OpenNI+OpenCV为平台，两幅图像的配准如图1所示。

图1 深度图与RGB图配准Fig.1 Registration of RGB and depth map

在配准后的图像中，每个RGB图上的像素点都对应其深度值。如图2，记像素坐标系中的点(Se2)为(u, v, d)，三维世界坐标系中的点(Se3)坐标为(xd,yd,zd)，s为深度单位，d=zd·s，fu、fv分别是沿像素平面两轴方向的焦距，f为像素归一化参数，考虑到相机自身的畸变引入小角度θ进行修正，u0、v0是相机的光圈中心。称为相机外参，可以通过标定来确定。由像素点对应空间点可以表示为：

图2 像素与三维坐标对应关系Fig.2 Relationship of pixel and 3D-coordinate

1.2 运动估计

ORB特征方法是在2011年国际计算机视觉大会(International Conference of Computer Vision，ICCV)上提出的一种新型的方法[8]，其相对于传统的SIFT，SURF最为显著的特点就是速度快。ORB方法通过FAST特征提取和BREIF特征匹配来完成的，但是其缺乏旋转不变性和尺度不变性。为了增加其旋转特性，考虑特征点O的邻域为S，在其邻域内计算其质心位置C，以特征点为起点，质心为终点构建向量。图像特征点邻域S内的矩可以表示为

式中，I是像素点的灰度值，r表示邻域半径。

邻域质心位置可以表示为

因此在BREIF特征描述时，对其邻域内的n个点按其主方向进行旋转：

为了增加其尺度不变性，可以参考SIFT特征提取中图像金字塔的方式，将一张图像降采样处理，对n维的尺度空间中的描述子分别匹配，则改进的描述子可以表示为

利用上述描述子，对两帧图像(F1, F2)匹配，得到对应的两组特征点记为

由于环境中各项误差的存在，将上述问题转化为

ORB特征匹配的结果如图3(a)所示，由于误匹配较多，因此求解出来的无法用于两帧间的运动估计。传统的RANSAC方法可以进行部分错误去除，但是由于取样点的随机性，会产生效率低下、易退化等缺点。针对此，本文结合图像的先验信息提出一种ICSAC算法用于降低匹配错误率，具体如下：

步骤1 对匹配的两帧图像提取ORB特征，并基于空间一致性的SCRNMSAC滤波器[9]进行点集的预处理。

步骤2 基于图像的光流主成分进行谱聚类，用S表示相似矩阵，构建谱矩阵，其中，sij为n点相似性分数，。对谱矩阵分解得到的k个特征向量聚类，其中k为聚类数，得到关于n个点集的超球面。选取点σ为圆心，r为半径，其中r=Ix-Iy表示光流阈值均匀选取点集，从而构成抽样的最小集。

步骤3 由于估计的内点之间存在联系，因此在较大的聚类点中含有内点的可能性最大，因而估计的最小集样本点均在较大类中选取。根据最小集估计两帧之间的，建立模型，并用超球面边缘的点对模型作预验证，去除明显不符合的模型。

步骤5 给定数值矩阵A，其行表示估计点约束的关系，通过观测矩阵的秩rA使用本方法不断迭代前次的内点并增加此约束[10]，作为退化补偿，并选取部分非聚类点进行模型验证，避免由于聚类性导致的局部最优，最终选取内点最多的模型。

图3(a)(b)(c)分别表在相同时间下同一图片特征点经过旋转后的原始匹配、经过RANSAC改进后的匹配，以及经过ICSAC后的匹配。相比RANSAC，在同时间下的ICSAC算法匹配的特征点体现出聚类性及正确性。

经过ICSAC去除后的特征点即可用于三维运动估计。由于像素点与三维坐标点的关系已经由上文得到，因此通过像素坐标的旋转、平移变换即可估计三维空间的运动。用表示机器人的实际视觉位姿信息，CI、SO分别表示相机内外参数矩阵，则有：

求得旋转矩阵R3×3即为机器人视觉姿态矩阵，即为机器人的位置。

图3(a) 原始匹配Fig.3(a) Original matching

图3(b) RANSAC改进匹配Fig.3(b) Matching improved by RANSAC

图3(c) ICSAC改进匹配Fig.3(c) Matching improved by ICSAC

2 视觉SLAM优化

2.1 回环检测

位姿图是指机器人的位置、姿态构成的拓扑图[2]，其顶点表示位姿，边表示约束关系，表示为，其中：，，qi为姿态四元数。

根据第1节建立的视觉里程计虽然可以进行定位，但是在长时间的运动中会产生累计误差。如果机器人在运动中回到之前的某个位置，这样的信息就可以用来优化机器人的运动，表现在位姿图上就是在原有的链式位姿图上增加了新的边约束。传统的回环检测主要通过建立“视觉字典”[11]来聚类，将描述子抽象为“视觉单词”，根据相邻帧的单词进行分类查找然后匹配。但是由于匹配的错误，可能会产生“假性”检测，且“假阳性”检测对整个系统的危害更大。由于移动过程中图像的尺度变换是最重要的变换，因此提出了一种多尺度视觉字典并将ICSAC算法应用于其中的回环检测方法，从而降低两种“假性”检测的可能性，并且回环检测针对关键帧，降低了位姿图的冗余性。该方法具体描述为：

步骤1 初始化关键帧序列I*：加入数据集第一帧图像I0，对于新帧Ij，利用ICSAC判断：①是否可以匹配；②匹配内点数小于阈值。若同时满足则该帧称为回环检测关键帧，记得到的关键帧集合为

式中，ϑ1、ϑ2表示两次判断的阈值。

步骤3 根据每个叶子节点在整个视觉字典中的频率高低定义其区分度：ω1,i=lg(M Mi)，其中，M表示字典中所有的图像数，Mi表至少含有节点i中的一个描述子的图像数。

步骤4 对于某帧图像I*j，其中节点i出现的频率定义为

2.2 非线性优化

当回环检测新建出的位姿图的约束边时，即边Ti,j有误差，因此可以优化不一致性误差：

其中，x*是x的估计值。在优化过程中，根据前端给定初始值，通过目标函数对x的梯度：

3 视觉惯性组合定位

上述视觉SLAM定位较为精准，但是易受到环境特别是光照等影响，稳定性有所不足。将上述视觉位姿信息与惯性进行组合定位，一方面可以在视觉失锁的情况下保持系统的稳定性，另一方面可以通过组合滤波的方式提高定位精度。以惯导东北天坐标系对应视觉XYZ坐标系建立组合系统，以表示视觉向惯性的转化矩阵，则惯导与视觉的导航数据变换关系可以表示为：

假设系统满足小失准角模型，即

视觉惯性组合系统状态方程为：

视觉惯性组合系统的观测方程为

4 实验验证分析

以视觉SLAM公开数据集TUM的freiburg1_xyz和freiburg1_desk为测试样本，其内容是室内环境下1m³空间内利用30Hz频率深度相机采集的多帧彩色RGB图像及对应的深度图，并且该数据集中含有真实运动轨迹，是通过16个高精度相机以100Hz频率从不同角度拍摄并计算得到，此公开数据集特别适合用来测定算法。为了避免帧间的冗余性，并且由于数据集大小不同，分别选100帧和200帧作为关键帧进行验证，数据集中已经做好了图像的配准，仿真时可略去。分别采用传统RANSAC的ORB特征提取匹配跟踪方法和ICSAC的ORB方法，建立了视觉里程计模型，其视觉X方向定位误差如图4所示。

分析图4可以得到以下结论：

1）两种方法的视觉里程计模型的定位误差均随着时间不断增加，即含有累计误差，符合一般里程计的误差模型。

2）使用ICSAC算法里程计误差增长平稳，说明该方法所用聚类性特征点估计具有一定的稳定性。

3）在室内环境下的两个数据集使用ICSAC相较于RANSAC的定位精度提高约15%，说明了本文提出的ICSAC算法对特征点去除错误匹配的正确性。

图4(a) fr1_xyz改进前后视觉里程计定位误差Fig.4(a) Position error of visual odometerof fr1_xyz

图4(b) fr1_desk改进前后视觉里程计定位误差Fig.4(b) Position error of visual odometer of fr1_desk

以SLAM公开数据集TUM和NYU为测试样本，以Ubuntu14.04为实验平台，基于改进的视觉回环检测方法对位姿图进行优化，可以得到如图5所示的回环检测后的位姿图。

优化后的位姿图呈现环状，因此可以用于多约束目标函数非线性优化。

以TUM数据集的freiburg1_xy、freiburg1_desk以及NYU数据集v2为例，惯性仿真选取的陀螺零漂0.006 (°)/h，随机漂移0.006 (°)/h1/2，加表零偏50μg，分别给出三种方式（①未经过回环检测的视觉里程计与惯性组合（VO-INS）；②经过回环检测的视觉SLAM与惯导组合（vSLAM-INS）；③改进回环检测下的视觉SLAM与惯性组合（I-vSLAM-INS））下的东向定位误差，并构建了地图（其中NYUv2数据集无真实轨迹），如图6所示。

由上述定位误差图可知，在fr1_xyz中，I-vSLAMINS算法的单向定位最大误差为1.035cm，单向定位均方根误差(RMSE)为0.418cm，则三方向总体定位RMSE约为。fr1_desk中，I-vSLAM-INS算法的单向定位最大误差为1.247cm，单向RMSE为0.554cm，则三方向总体定位RMSE约为。

由图6(a)(b)(c)所示点云地图可以看出，改进后的视觉回环检测算法可以使人物、环境轮廓更加清楚。结合各类开源SLAM算法精度（ORB-SLAM，RGBD-SLAM）以及本文的各个算法可以得到TUM的5个不同数据集的定位RMSE，如表1所示。

图5(a) TUM位姿图检测后Fig.5(a) TUM pose-graph after detection

图5(b) NYU位姿图检测后Fig.5(b) NYU pose-graph after detection

图6(a) fr1_xyz三种方式的构图与定位误差Fig.6(a) fr1_xyz mapping and positional error by 3 methods

图6(b) fr1_desk三种方式的构图与定位误差Fig.6(b) fr1_desk mapping and positional error by 3 methods

图6(c) NYUv2三种方式的构图Fig.6(c) NYUv2 mapping by 3 methods

本文提出的在视觉里程计运动估计和视觉回环检测中的两个改进方法在表1中体现在：①利用ICSAC算法的视觉里程计定位精度相比RANSAC提高了约15%。②改进回环检测的视觉SLAM相比于传统方法精度提高10%～15%，相比于不回环检测精度提高40%以上，说明了视觉回环检测的重要性，同时说明改进方法的正确性。

本文最终建立的视觉惯性SLAM方法I-vSLAM-INS相比于一些开源项目的定位精度更高，并且可以在视觉受环境影响失锁状态下使得机器人能够稳定工作，具有一定的实用价值。

表1 各种算法的定位均方根误差Tab.1 RMSE of various algorithms cm

5 结 论

本文针对SLAM视觉运动估计中特征点错误匹配的问题提出了一种ORB特征聚类抽样估计的算法ICSAC，利用图像的先验知识加速算法并基于聚类性特征对运动进行估计从而提高了精度，建立了视觉里程计模型并加以验证。针对回环检测，以多尺度的视觉字典减少了“假阴性”匹配的机率，并基于ICSAC做帧间匹配减少了“假阳性”匹配机率，提高了回环检测的准确性。针对视觉SLAM定位的不稳定性，将视觉SLAM与惯性导航进行组合定位，最终建立了视觉惯性SLAM模型。

通过不同的SLAM公开数据集的仿真实验，验证了本文两个改进方法的正确性。同时将本文建立的视觉惯性SLAM与已有开源SLAM项目对比，在定位精度和稳定性上具有提升，说明了本文的研究具有一定的价值。本文在SLAM未来的研究方向上打算利用视觉惯性的紧组合系统进行定位[13]，在地图创建上利用深度学习等方法建立环境语义地图。

（References）：

[1] Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68.

[2] Thrun S, Montemerlo M. The graph SLAM algorithm with applications to large-scale mapping of urban structures[J]. International Journal of Robotics Research, 2006,25(5-6): 403-429.

[3] 梁明杰, 闵华清, 罗荣华. 基于图优化的同时定位与地图创建综述[J]. 机器人, 2013, 35(4): 500-512.Liang Ming-jie, Min Hua-qing, Luo Rong-hua. Graphbased SLAM: A survey[J]. Robot, 2013, 35(4): 500-512.

[4] 罗文超, 刘国栋, 杨海燕. SIFT和改进的RANSAC算法在图像配准中的应用[J]. 计算机工程应用, 2013,(15): 147-149, 156.Luo Wen-chao, Liu Guo-dong, Yang Hai-yan. Application of SIFT and advanced RANSAC algorithm on image registration[J]. Computer Engineering and Applications,2013, 49(15): 147-149, 156.

[5] 曲天伟, 安波, 陈桂兰. 改进的RANSAC算法在图像配准中的应用[J]. 计算应用, 2010(7): 1849-1851, 1872.Qu Tian-wei, An Bo, Chen Gui-lan. Application of improved RANSAC algorithm to image registration[J]. Computer application, 2010(7): 1849-1851, 1872.

[6] 刘春燕. 基于多源信息融合的行人航位推算室内定位方法[J]. 中国惯性技术学报, 2016, 24(2): 208-214.Liu Chun-yan. Indoor localization method based on pedestrian dead reckoning aided by multi-source fusion[J]. Journal of Chinese Inertial Technology, 2016, 24(2): 208-214.

[7] Henry P, Krainin M, Herbst E, et al. RGB-D mapping:Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. International Journal of Robotics Research, 2012, 31(5): 647-663.

[8] Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[C]//International Conference of Computer Vision. 2011: 2564-2571.

[9] Sattler T, Leibe B, Kobbelt L. SCRAMSAC: Improving RANSAC’s efficiency with a spatial consistency filter[C]//12th IEEE International Conference on Computer Vision.2015: 2090-2097.

[10] Raguram R, Chum O, Pollefeys M, et al. USAC: A universal framework for random sample consensus[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence,2013, 35(8): 2022-2038.

[11] Liu Y, Cormack L K, Bovik A C. Statistical modeling of 3-D natural scenes with application to Bayesian stereosis[J]. IEEE Transactions on Image Processing, 2011,20(9): 2515-2530.

[12] Skibinski S, Weichert F. Parametric fusion of complex landmark observations present within the road network by utilizing bundle-adjustment-based full-SLAM[C]//IEEE Conference Publications. 2016: 917-926.

[13] 王云舒, 刘建业, 曾庆化, 等. 结构光辅助的惯性/视觉室内导航三维环境重构方法[J]. 中国惯性技术学报,2016, 24(1): 51-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): 51-58.

Visual-aid inertial SLAM method based on graph optimization in indoor

XU Xiao-su1,2, DAI Wei1,2, YANG Bo1,2, LI Yao1,2, DONG Ya1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

To realize the accurate autonomous navigation of mobile robot in unfamiliar environment, a graph-based SLAM method based on the visual odometry is proposed, which adds loop detection into the motion estimation of visual odometry and thus optimizes the nonlinear constraints to improve the positioning accuracy. In the motion estimation of visual odometry, a cluster sampling matching track method based on ORB feature is be proposed to deal with the high mismatch rate. In the pose graph optimization, an improved loop detection method is proposed to reduce the mismatch possibility. Finally, the inertial navigation system is combined with the visual SLAM to further improve the steady and accuracy of the system. The simulations using open indoor SLAM test datasets show that the locating RMSE of the proposed method is in centimeter level, and the generated point cloud map is clearly visible.

simultaneous localization and mapping (SLAM); graph optimization; cluster sampling; loop detection; integrated location

U666.1

：A

1005-6734(2017)03-0313-07

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

2017-03-11；

：2017-05-20

国家自然科学基金项目（51175082，61473085）

徐晓苏（1961—），男，博士生导师，从事测控技术与导航定位领域的研究。E-mail: xxs@seu.edu.cn