基于视觉和距离传感器的SLAM和导航方法的探新

花罡辰

（东京工业大学综合理工学研究科，日本 横滨 226-8503）

基于视觉和距离传感器的SLAM和导航方法的探新

花罡辰

（东京工业大学综合理工学研究科，日本 横滨 226-8503）

SLAM(Simultaneous Localization and Mapping)被广泛应用于生成地图和机器人导航领域。基于视觉特征的SLAM是一种低成本的解决方案，且能够提取环境的丰富的特征点用于机器人导航，但是，现实世界的可视特征往往是动态的，很多基于视觉特征的SLAM方法都不能排除环境中移动物体的影响，比如：在餐厅或购物中心的行人等等。ICGM(Incremental Center of Gravity Matching)是一个通过图像队列之间特征点的几何结构互相匹配而得到稳定特征点的方法，基于ICGM的SLAM能有效地排除环境中移动物体的影响。本研究在前人研究的基础上提出了一种新的算法，利用ICGM的方法提取环境稳定的视觉特征，结合Kinect距离传感器，在高度动态环境中实现了较高精度的机器人自动导航。并通过实验检验了这种算法，实验结果显示，本方法的速度能够达到实时处理的要求，与此前研究的其它方法相比，这种方法能够达到更高的精度，所提出的SLAM和导航系统更加接近实用要求。

SLAM；计算机视觉；机器人技术

1 关联研究

SLAM对多种机器人系统都是必不可少的，自动导航对于机器人系统来说也非常重要。

在SLAM和机器人导航领域，近年来有非常多的先进研究，其核心目标是提高机器人导航系统的速度、精度和可靠性。然而，在现实使用中，机器人周围的工作环境往往是动态的，因此，“如何在动态的环境中提高位置识别精度”，成为当前研究的热点。

大多数基于图像特征的SLAM或导航方法，是提取SIFT(Scale-Invariant Feature Transform)或SURF(Speeded Up Robust Feature)特征作为定位时的视觉识别标志，但是，如果使用SIFT或SURF，SLAM或导航系统就会把移动中的物体，比如行人、汽车等，作为定位时的视觉识别标志，显然会对定位的效率产生不好的影响。

KawewongA.et al[2,3]提出了位置不变稳定特征值PIRF (Position Invariant Robust Feature)。PIRF 是一个通过图像队列之间特征点的几何结构互相匹配而得到稳定特征点的方法。作为定位时的视觉识别标志，它能一定程度排除掉动态物体对导航精度的影响。基于PIRF，Morioka H.et al.[4]提出了一种基于PIRF的SLAM和机器人导航方法，即3D-PIRF。3D-PIRF从单眼全方位相机的连续图像中中提取PIRF特征，并结合机器人里程计的信息推算出所有PIRF特征的空间3D坐标。把这些PIRF特征和空间3D坐标学习后，构建一个混合地图(Hybrid Map)。便能进行导航。导航时，利用8点方法可以计算出机器人的全局位置。

虽然PIRF是一种较为有效的安定特征点提取方法。但是HUAGangchen et al[1].提出了一种比PIRF更有效的安定特征点提取方法，被称作ICGM。对比PIRF，ICGM在利用图像队列之间特征点互相匹配的基础上还利用了特征点的几何结构。把ICGM应用在只基于图像的在高度动态环境下的SLAM中可以达到比PIRF更高的精度。

本研究基于ICGM加上新颖的Kinect传感器，以期实现比3D-PIRF更好的SLAM和机器人导航系统。

本方法实现机器人自动导航分为两个步骤，学习阶段和导航阶段。SLAM即为学习阶段。导航阶段机器人利用SLAM的数据进行自动导航。

图1是本研究的基本结构：

图1 研究的基本结构Fig.1 Basic structure of the proposed method

图1右边所示为SLAM和导航机器人的硬件，机器人上端安装有一台Kinect传感器。左上为Kinect传感器得到的环境的距离信息（暖色代表离传感器距离近，冷色代表远）和图像信息。Kinect传感器能够同时获取环境中的距离信息和图像信息。左下为SLAM系统计算出的机器人的移动路径。

2 ICGM简介

ICGM(Incremental Center of Gravity Matching) 是一个通过图像队列之间特征点的几何结构互相匹配而得到稳定特征点的方法。

其基本思路如图2所示。假设Ia和Ib是在相同地点不同时刻拍摄的两张照片。

图2：Incremental Center of Gravity Matching的基本思路Fig.2 Basic idea of Incremental Center of Gravity Matching

图中的(a,a'),(b,b'),(c,c'),(d,d'),(e,e')为已经被匹配上的特征点（SURF或SIFT）对。(a,a'),(b,b'),(c,c'),(e,e')之间的匹配是稳定的匹配，(d,d')是不稳定的匹配。换句话说(a,a'),(b,b'),(c,c'),(e,e')在到拍摄的时间段内，位置没有大的变化，所以(a,a'),(b,b'),(c,c'),(e,e')是稳定的。而(d,d')的位置变化了，所以(d,d')之间的匹配是不稳定的。

以上所提到的稳定和不稳定的概念皆是时间尺度上的概念。假设Ia拍摄到Ib拍摄经过时间t，那么(d,d')在t内是不稳定的。

本研究利用ICGM方法用来排除不稳定的匹配。

首先假设系统已知(a,a'),(b,b'),(c,c')的匹配是稳定的。然后分别在Ia和Ib中计算a,b,c和a',b',c'的重心，得到o,o'。基于o,o'我们能够计算出重心向量（从已知重心到其他特征点的向量）：。显然地，但是。我们能够利用两张图的重心向量的关系来判断一个特征点的匹配对是否稳定。

因此，通过合理地设置一个阈值可以区分一个匹配对是否稳定。

实际使用时，ICGM方法使用了差值比例RoG（ratio of difference）来计算匹配对的稳定度。

RoG为重心向量的差的模和重心向量的模的和的比例。当式(2)成立时，一个特征点匹配对即被认为不稳定，反之则稳定。

实际的ICGM方法会首先随机地提取初始N个匹配对，分别计算他们的重心。再用(1)计算初始N个点的RoG，用式(2)判断它们稳定与否。当且仅当初始的N点都稳定时，才进入下一步计算。

得到N个初始的稳定的特征点后，通过它们的重心来测试剩下特征点匹配对的稳定度。

假设如图2，(a,b,c)和(a',b',c')是初始的3个稳定的匹配对。然后，(e,e')是被识别为稳定，ICGM会重新计算重心，用(a,b,c,e)和(a',b',c',e')的重心来计算剩下的匹配对的稳定度。因为(d,d')不稳定，所以(d,d')会被直接排除，重心也不会被重新计算。

ICGM最后能够排除所有不稳定的特征点，而留下稳定的特征点。

以上描述了ICGM排除不稳定匹配对的方法。如图3所示，假设为机器人在时间轴上连续的图片，在SLAM和导航中，我们分别在和，之间计算ICGM。假设A为计算ICGM后，的稳定特征点。假设B为计算ICGM后，的稳定特征点。我们取A和B的并集作为的稳定的特征点。

图3 基于ICGM从连续的图像队列中提取稳定特征值Fig.3 Robust feature extraction method based on ICGM

3 基于ICGM的SLAM和机器人导航

本研究在学习SLAM阶段会构建一个混合地图。导航阶段使用混合地图进行自动导航。

3.1 混合SLAM和混合地图的构建（学习阶段）

目前有很多SLAM都是只基于视觉信息的（vision-only），但只基于视觉信息的SLAM只能够获取系统当前的大概位置。而对于机器人导航，精确的6自由度的全局姿态是必要。6自由度的全局姿态即机器人在地图中的3维位置（x，y，z）和3维的机器人的·全局的的角度姿态（roll，pitch，yaw）。

为了获取机器人6自由度的全局姿态，我们开发了一种基于vision-only SLAM的混合SLAM。利用混合SLAM我们能够构建混合地图。

本方法的混合地图是基于可视里程器构建的。可视里程器通过计算3D点云间的6D刚性变换[6]实现。

一帧kinect数据先通过ICGM提取稳定特征点，然后通过kinect的距离数据算出稳定特征点的空间3D坐标，这些带有空间3D坐标的稳定特征点生成一个3D点云。我们把这样的一个点云叫做一个数据模型。我们能够连续得到一个的集合M。

如图5所示，随着学习路径的变长，里程计的误差会渐渐累计，导致很大的误差。学习过程中我们必须控制误差的累计。

本方法利用ICGM的vision-only SLAM[1]检测闭环。检测闭环是指通过可视特征检测当前的位置是不是一个之前已经来过的位置。

检测出闭环后，利用vision-only SLAM取得3个最好的数据模型，Mbest，Msecond_best，Mthird_ best。

然后我们尝试分别计算当前的数据模型Mcurrent和Mbest，Msecond_best，Mthird_best之间的刚性变换。假如都成功了，我们能得到3个相对姿态。假如都失败了，我们会认为此次闭环检测失败了。

然后根据RANSAC建模的结果好坏，我们从上面3个相对姿态选取一个最好的相对姿态和最好的闭环位置。进一步可以得到当前模型的通过闭环检测得到的准确的全局位置：。此为消除了累计误差之后的当前位置。

我们通过最小化(5)式来最优化current时刻之前的路径。式（5）中表示current时刻之前学习的6自由度的全局姿态。

3.2 基于混合地图的自动导航（导航阶段）

自动导航仍旧基于闭环检测。步骤和学习阶段类似，但是是基于学习阶段的混合地图实现定位。通常会把设置为1以达到快速的位置识别的响应。

机器人导航时同样利用ICGM的vision-only SLAM检测闭环，然后获取最佳的和。

4 实验

本实验包括两个阶段，学习阶段和导航阶段。

如图4，这个实验是在东京工业大学食堂进行的。食堂大小为20*20m。我们在晚上8点的时候检测自己所提出的方法。这时，食堂大概有70人。他们在这个环境中自由地进餐或走动。这是一个高度动态的环境。

图4 经过ICGM处理的结果:行人被ICGM作为动态物体排除了Fig.4 ICGM's processing result:pedestrians are ignored

在学习阶段，我们使用了游戏手柄控制机器人。机器人在移动过程中实时记录里程计和kinect的图像及距离信息。我们控制机器人绕了两圈。在学习的终点，我们让机器人回到学习的原点。总的移动的距离大概为80米。在整个过程中，机器人记录了6739帧数据。

虽然环境是高度动态的，但由于ICGM特征点的稳定性，学习结果：kinect的可视里程计的成功率为97.6%，所以机器人路径有97.6%由kinect的数据算出。闭环检出率为63.8%。

图5为机器人里程计计算出的路径，显然地，机器人里程计计算出的路径误差会渐渐累计，导致极大的误差。图5为通过本方法学习的路径。

由于环境较大，取得真实数据很难，所以较难做出精确的误差评价。但因为本方法通过检出闭环修正了路径，误差累计的问题显然被较好地解决了。

图5 单纯的里程计计算出的机器人路径Fig.5 Route calculated only by robot's odometry

图6 通过检出闭环修正了的路径。红色代表闭环被检出的位置Fig.6 Corrected route based on loop-closure detection.Red points represent loop-closure detected locations

表1为闭环检出率的对比。相比3D-PIRF[3]，本方法的闭环检出率大大提高了。

表1 闭环检出率的对比Tab.1 Comparisons of loop-closure detected ratio

在导航阶段，我们选取了一段长度为12米的路径作为规划路径。此路径包含在学习路径中。我们基于上一学习阶段构建的混合地图进行导航。

导航阶段每帧平均计算时间为321ms。机器人移动速度为125mm/s，用地板砖的分界线作为误差评判基准，移动阶段对于规划路径的平均偏移为60mm，最大偏移为92mm。

表2为本方法和3D-PIRF[3]的导航结果的对比。请注意，本方法运用了GPU加速，而3D-PIRF没有，所以计算速度的数据仅供参考。

表2 导航结果对比Tab.2 Comparisons of navigation's results

5 总结

本研究提出了一个基于混合使用视觉特征和距离传感器的SLAM和机器人导航方法，它能在高度动态的环境中稳定工作。实验结果显示，本方法的速度能够达到实时处理的要求，相比前人研究的方法，这种方法能够达到更高的精度，所提出的SLAM和导航系统更加接近实用要求。

在高度动态环境中工作是多种机器人必备的能力。如果机器人能利用一般的家用传感器，就能大大降低机器人制造成本。微软公司的Kinect是一种高速高精度且低成本的传感器，它非常适合未来的商用机器人。另外，除了机器人，一些车辆也能使用Kinect实现自动导航，巨大的应用需求给本方法提供了广阔的前景。

（

）

[1]HUAGangchen,HasegawaO.,A Robust Visual-Feature-Extraction Method for Simultaneous Localization and Mapping in Public Outdoor Environment[J].Vol.19 No.1,2015,11-22.

[2]Kawewong A.,Tangruamsub S.,and HasegawaO..Positioninvariant robustfeatures for long-term recognition of dynamic outdoor scenes[J].IEICE Transactionson Information and Systems,2010,E93-D(9):2587-2601.

[3]Kawewong A.,Tongprasit N.,Tangruamsub S.,and HasegawaO..Online incremental appearance-based slam in highly dynamic environments[J].Int.J.of Robotics Research,2011,30(1):33-55.

[4]MoriokaH.,Sangkyu Y.,and HasegawaO..Visionbased mobile robot's slam and navigation in crowded environments[R].IEEE/RSJ International Conferenceon Intelligent Robots and Systems(IROS),2011.

[5]FischlerM.A.and Bolles R.C..Random sample consensus:aparadigm for model fitting with applications to image analysis and automated cartography[J].Commun.ACM,1981,24(6):381-395.

[6]Berthold K.P.Horn.Closed-form solution of absolute orientation using unit quaternions[J].JOSA A,1987,4:629-642.

Anexploration on visionbased SLAM and navigation method combining range finder

HUA Gangchen
（Tokyo institute of technology,Interdisciplinary Graduate School of Science and Engineering,Yokohama 226-8503,Japan）

Simultaneous Localization and Mapping(SLAM) is widely used for generation of maps for autonomous robotic navigation etc.SLAM based on visual features is a kind of low cost solution.Besides,SLAM based on visual features have plenty information for matching and recognition.But appearance of actual world is always dynamic.Many kinds of vision- based SLAM method are not robust to influence of unstable objects such as walking humans in cafeterias or shopping malls.ICGM(Incremental Center of Gravity Matching) is method which extracts static visual features from images sequence based on feature points geometric structure.SLAMs based on ICGM can exclude bad influences of dynamic objects.This proposed method is using ICGM to extract static features and combines range finderinformation.Thus,the proposed method can achieve high accuracy robotic navigation in highly dynamic environment.We run our approach in a crowed cafeteria.The result shows that by using proposed method SLAM and navigation can be achievedfast enough for real-time processing.Comparing to previous,the proposed method's precision is higher.Proposed method is more suitable for actual needs.

Simultaneous Localization and Mapping(SLAM);computer vision;robotics

TP242

A

1672-6332（2015）01-0083-06

【责任编辑：杨立衡】

2015-03-10

花罡辰（1988-），男（汉），江西抚州人，博士，主要研究方向：智能科学系统。E-mail:huagangchen1988@gmail.com