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

2 ICGM简介

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

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

图中的(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方法用来排除不稳定的匹配。

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最后能够排除所有不稳定的特征点，而留下稳定的特征点。

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点云。我们把这样的一个点云叫做一个数据模型Mx 。我们能够连续得到一个Mx 的集合M。

通过计算两个Mx间的6D刚性变换，我们能够得到他们之间的相对3D姿态变换R。计算R的过程中我们利用RANSAC [5] 来取得最佳模型以获取最高精度。

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

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

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

3.2 基于混合地图的自动导航(导航阶段)

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

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

4 实验

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

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

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

图5为机器人里程计计算出的路径，显然地，机器人里程计计算出的路径误差会渐渐累计，导致极大的误差。图5为通过本方法学习的路径。由于环境较大，取得真实数据很难，所以较难做出精确的误差评价。但因为本方法通过检出闭环修正了路径，误差累计的问题显然被较好地解决了。

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

在导航阶段，我们选取了一段长度为12米的路径作为规划路径。此路径包含在学习路径中。我们基于上一学习阶段构建的混合地图进行导航。导航阶段每帧平均计算时间为321ms。机器人移动速度为125mm/s，用地板砖的分界线作为误差评判基准，移动阶段对于规划路径的平均偏移为60mm，最大偏移为92mm。

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

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., andHasegawaO.. Position-invariant 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.. Vision-based 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.