自主移动装置以及自主移动方法与流程

本发明涉及自主移动装置、自主移动方法以及程序。

背景技术：

根据用途而自主地移动的自主移动装置正在逐渐普及。例如，已知为了进行屋内的清洁而自主地移动的自主移动装置。一般来说，这样的自主移动装置需要进行实际空间的地图的制作、和实际空间内的自身设备位置的估算。

作为用于制作实际空间的地图的方法，例如已知SLAM(SimultaneousLocalization And Mapping，即时定位与地图构建)法。使用了单眼摄像机的SLAM技术的基本原理在非专利文献1中有记载，进行如下处理，即，通过从摄像机拍摄的运动图像的多个帧，追踪同一特征点，来交替地估算自身设备的三维位置(摄像机位置)和特征点的三维位置(该特征点的三维位置集合而构成地图的信息)。

该方法由于交替地估算自身设备位置和特征点的三维位置，因此会产生累积误差，从而存在如下情况，即，即使在进行环状的移动等而返回到原来的位置的情况下，也不能识别出返回到原来的位置。作为修正这样的累积误差的方法，存在闭环(loop closing)处理(环路解决处理)这样的技术，在专利文献1等中得到了使用。若自身设备检测出环路(若检测出在所拍摄的帧中包含过去从相同位置拍摄到的相同的特征点)，则将过去的该位置视为正确的位置，进行对从此处起到当前位置为止的路径中的位置进行修正的闭环处理。

在先技术文献

非专利文献

非专利文献1：Andrew J.Davison，“Real-Time Simultaneous Localizationand Mapping with a Single Camera”，Proceedings of the 9th IEEEInternational Conference on Computer Vision Volume 2，2003，pp.1403-1410

非专利文献2：Richard Hartley，Andrew Zisserman，“Multiple ViewGeometry in Computer Vision”，Second Edition，Cambridge.University Press，March 2004，chapter 9

专利文献

专利文献1：JP特开2004-276168

技术实现要素：

发明要解决的课题

在现有技术中，即使在返回到过去通过的位置的情况下，若利用摄像机的拍摄方向较大不同，则也不能检测出同一特征点而不进行闭环处理。因此，存在如下问题，即，累积误差得不到修正，即使返回到同一点也不能识别出返回到了同一点。例如，在摄像机被固定而使视野受限的自主移动装置进行了图13所示那样的移动的情况下，由影线示出的两个位置非常接近，但由于这些位置上的拍摄方向不同，因此不能检测出同一特征点，从而不能进行闭环处理。

本发明为了解决上述问题而作，其目的在于，提供一种通过增加进行闭环处理的频度从而能够减少自身设备位置等的累积误差的自主移动装置等。

用于解决课题的手段

为了达成上述目的，本发明的自主移动装置具备摄像部、驱动部、控制部和存储部，所述自主移动装置的特征在于，

所述存储部，

将所述摄像部拍摄到的图像的信息与拍摄时的自身设备的位置的信息一起进行存储，

并存储使用所存储的多个所述图像的信息而制作的地图，

所述控制部，

使所述驱动部进行使自身设备进行旋转的动作，同时使所述摄像部拍摄图像，

并从所述存储部中检索与所拍摄的所述图像的类似度成为规定的类似度以上的图像，

在发现了成为所述规定的类似度以上的图像的情况下，基于与该图像对应的位置的信息对存储在所述存储部中的地图进行校正。

发明效果

根据本发明，进行闭环处理的频度增加，能够减少自身设备位置等的累积误差。

附图说明

图1是表示第一实施方式所涉及的自主移动装置的外观的图。

图2是表示第一实施方式所涉及的自主移动装置的构成的图。

图3是表示第一实施方式所涉及的自主移动控制处理整体的流程图的图。

图4是表示第一实施方式所涉及的自主移动控制处理当中的自身设备位置估算线程的处理的流程图的图。

图5是表示第一实施方式所涉及的自主移动控制处理当中的地图制作线程的处理的流程图的图。

图6是表示第一实施方式所涉及的自主移动控制处理当中的闭环线程的处理的流程图的图。

图7是表示第一实施方式所涉及的自主移动控制处理当中的旋转拍摄处理的流程图的图。

图8是表示第二实施方式所涉及的自主移动装置的构成的图。

图9是表示第二实施方式所涉及的自主移动控制处理当中的自身设备位置估算线程的处理的流程图的图。

图10是说明共同可视图(co-visibility graph，コビジビリテイ一グラフ)的图。

图11是表示第四实施方式所涉及的自主移动控制处理当中的自身设备位置估算线程的处理的流程图的图。

图12是表示第四实施方式所涉及的自主移动控制处理当中的闭环线程的处理的流程图的图。

图13是说明不能进行闭环处理的移动例的图。

图14是说明通过第一实施方式所涉及的旋转拍摄处理从而能够进行闭环处理的图。

图15是说明通过第三实施方式所涉及的旋转拍摄处理从而能够进行闭环处理的图。

图16是表示将控制部等的一部分设置于外部的服务器的构成的一例的图。

符号说明

100、100’…自主移动装置，10、50…控制部，11…旋转拍摄部，12…地图制作部，13…位置估算部，14…类似图像检索部，15…地图校正部，16…距离判定部，20、60…存储部，21…图像存储部，22…地图存储部，23…位置历史记录存储部，30…传感器部，31…障碍物传感器，41…摄像部，42…驱动部，43…输入部，44、71…通信部，45…电源，200…服务器

具体实施方式

以下，参照图1，对本发明的实施方式所涉及的自主移动装置进行说明。自主移动装置100根据用途而自主地移动。该用途，例如是警备监视用、屋内清洁用、宠物用、玩具用等。

自主移动装置100在外观上具备摄像部41、驱动部42。

摄像部41具备单眼的摄像装置(摄像机)。摄像部41例如以30fps取得图像(帧)。自主移动装置100基于由摄像部41依次取得的图像，实时地识别自身设备位置和周围环境，同时进行自主移动。

驱动部42是独立2轮驱动型，是具备车轮和电动机的移动单元。自主移动装置100能够通过2个车轮的同一方向驱动来进行前后的平行移动(平移移动)，能够通过2个车轮的反向驱动来进行原地的旋转(朝向变更)，并能够通过改变2个车轮各自的速度的驱动来进行旋转移动(平移+旋转(朝向变更)移动)。此外，各个车轮具备旋转编码器，通过旋转编码器来测量车轮的转速，并通过利用车轮的直径、车轮间的距离等几何学关系而能够计算平移移动量以及旋转量。例如，若将车轮的直径设为D，并将转速设为R(由旋转编码器测定)，则该车轮的接地部分的平移移动量成为π·D·R。此外，若将车轮的直径设为D，将车轮间的距离设为I，将右车轮的转速设为R_R，并将左车轮的转速设为R_L，则朝向变更的旋转量(若以右旋转为正)成为360°×D×(R_L-R_R)/(2×I)。通过将该平移移动量、旋转量依次进行相加，从而驱动部42作为所谓的里程计(Odometry)而发挥作用，能够测量自身设备位置(以移动开始时的位置以及朝向为基准的位置以及朝向)。

从里程计得到的自身设备的位置以及朝向的精度由于车轮的摩耗、打滑等而成为低精度的情况较多。特别是由于误差逐渐累积的关系，精度随着时间经过而恶化。

另外，也可以取代车轮而具备履带，还可以通过具备多条(例如两条)腿并用腿行走来进行移动。在这些情况下，也能够基于两个履带的活动、腿的活动，与车轮的情况同样地测量自身设备的位置、朝向。

如图2所示，自主移动装置100除了具备摄像部41、驱动部42以外，还具备控制部10、存储部20、传感器部30、输入部43、通信部44、电源45。

控制部10由CPU(Central Processing Unit，中央处理单元)等构成，通过执行存储在存储部20中的程序，从而实现后述的各部(旋转拍摄部11、地图制作部12、位置估算部13、类似图像检索部14、地图校正部15)的功能。

存储部20由ROM(Read Only Memory，只读存储器)、RAM(RandomAccess Memory，随机存取存储器)等构成，包含图像存储部21、地图存储部22。在ROM中，存储有控制部10的CPU所执行的程序(例如，后述的SLAM法的运算、自主移动控制处理所涉及的程序)、执行程序上预先需要的数据。在RAM中，存储在程序执行中被生成或者变更的数据。

在图像存储部21中，存储摄像部41所拍摄的图像。但是，为了节约存储容量，可以不存储所拍摄的所有的图像，此外也可以不存储图像本身，而存储图像的特征量。关于重要的图像(后述的关键帧)，与图像的信息一起，存储拍摄该图像时的自身设备位置(自身设备的位置以及朝向)的信息。

在地图存储部22中，存储由地图制作部12基于后述的SLAM法、来自障碍物传感器31的信息而制作的地图(特征点、障碍物的三维位置的信息)。

作为传感器部30而具备障碍物传感器31。障碍物传感器31是探测行进过程中的障碍物的传感器，例如是红外线传感器、超声波传感器。另外，也可以不搭载独立的障碍物传感器31，而使用摄像部41来探测障碍物。此外，还可以具备对碰撞到其他物体进行探测的碰撞传感器(bumpersensor)(未图示)。

作为输入部43，具备用于操作自主移动装置100的操作按钮。操作按钮，例如，包含电源按钮、模式切换按钮(对清洁模式、宠物模式等进行切换)、初始化按钮(重新进行地图的制作)等。作为输入部43，也可以具备进行声音的输入的麦克风(未图示)、和识别对自主移动装置100的操作指示的声音的声音识别部。

通信部44是用于与外部装置进行通信的模块，在与外部装置进行无线通信的情况下是包含天线的无线模块。例如，通信部44是用于进行基于Bluetooth(注册商标)的近距离无线通信的无线模块。在通信部44中，进行自主移动装置100与外部的数据授受等。

电源45是使自主移动装置100进行动作的电源，一般来说是内置的充电电池，但也可以是太阳能电池，还可以是从地面利用无线进行电力供给的系统。在电源45是充电电池的情况下，自主移动装置100通过与充电站(基座)进行对接而被充电。

接着，对控制部10的功能进行说明。控制部10包含旋转拍摄部11、地图制作部12、位置估算部13、类似图像检索部14、地图校正部15，进行后述的SLAM法的运算、自主移动装置100的移动指示等。此外，控制部10适应多线程功能，能够并行进行多个线程(不同的处理的流程)。

旋转拍摄部11基于规定条件，由驱动部42使自身设备进行旋转，同时由摄像部41拍摄图像，所拍摄的图像与后述的位置估算部13所估算出的自身设备的位置以及朝向的信息一起存储到图像存储部21中。关于自身设备的位置以及朝向，也可以不使用由位置估算部13估算出的值而是使用从上述的里程计取得的值。

地图制作部12将基于存储在图像存储部21中的图像的信息以及该图像拍摄时的自身设备的位置以及朝向的信息来使用SLAM法而估算出的特征点的三维位置(Map点)、基于由障碍物传感器31探测到障碍物时的自身设备的位置以及朝向的信息而得到的该障碍物的三维位置等，作为地图的信息而存储到地图存储部22中。

位置估算部13基于后述的SLAM法，作为视觉里程计，估算自身设备的位置以及朝向。

类似图像检索部14对摄像部41所拍摄的图像的特征量与存储在图像存储部21中的图像的特征量进行比较，检索类似度为规定的类似度(基准类似度)以上的图像。

地图校正部15在由类似图像检索部14发现了类似度为基准类似度以上的图像时，基于对该图像进行了拍摄的位置以及朝向的信息，对存储在地图存储部22中的地图的信息进行校正。

图3是自主移动装置100的主流程。控制部10首先在清除初始化完成标志(表示自身设备位置估算用的初始化处理是否完成的变量)以及旋转拍摄标志(表示是否为旋转拍摄处理中的变量。以下也称为“RF”。)之后起动自身设备位置估算线程(步骤S101)，接着起动如下各线程，即，地图制作线程(步骤S102)、闭环线程(步骤S103)。通过自身设备位置估算线程、地图制作线程进行动作，从而基于SLAM法，开始地图的信息以及视觉里程计(使用地图和图像而估算出的自身设备位置的信息)的生成。然后，控制部10判定是否动作结束(步骤S104)，若动作结束(步骤S104：“是”)则结束动作，若动作并未结束(步骤S104：“否”)则地图制作部12进行地图的信息的制作/更新(步骤S105)。

接下来控制部10判定旋转拍摄标志RF是否为0(步骤S106)。旋转拍摄标志RF是表示作为本实施方式的特征性的功能的旋转拍摄处理的状态的标志，0表示“并非旋转拍摄处理中”，1表示“旋转拍摄处理中(旋转一圈前的阶段)”，2表示“旋转拍摄处理的最终阶段”。若RF为0(步骤S106：“是”)，则前进到步骤S107，为了进行自主移动而对驱动部42指示所希望的动作(步骤S107)，并返回至步骤S104。若RF不为0(步骤S106：“否”)，则由于当前是旋转拍摄处理中，因此为了将移动处理交给旋转拍摄处理，不进行主流程中的移动处理而返回至步骤S104。另外，关于旋转拍摄处理的详细情况在后面叙述。

通过该主流程的处理，从而能够基于地图的信息自主地进行移动，同时适当更新地图的信息。作为典型的示例，自主移动装置100首先若在置于充电站的状态下接通电源45，则借助障碍物传感器31，在家中的各房间到处移动，由障碍物传感器31来确定墙壁等障碍物位置，能够制作包含障碍物位置的地图的信息。若将地图制作一定程度，则能够知道虽然还没有地图的信息但可以认为能够移动的区域，在该区域自主地进行移动等，也能够促进更广范围的地图的制作。然后，若制作了能够移动的大致整个区域的地图的信息，则能够进行利用了地图的信息的高效的移动动作。例如能够实现无论从房间的什么位置都以最短路径返回到充电站，或者高效地进行房间的清洁。

关于在自主移动装置100的主流程(图3)的步骤S101起动的自身设备位置估算线程，使用图4来进行说明。该线程是如下处理，即，位置估算部13首先进行初始化处理，之后持续进行自身设备位置估算(使用由摄像部41取得的图像通过视觉里程计来估算自身设备位置)。

位置估算部13，首先，判定是否动作结束(步骤S201)。若动作结束(步骤S201：“是”)则结束动作，若动作并未结束(步骤S201：“是”)，则判定是否已经初始化完成(步骤S202)。若初始化完成(步骤S202：“是”)则进行步骤S220以后的自身设备位置估算处理，若尚未初始化完成(步骤S202：“否”)则前进到步骤S203进行初始化处理。首先对初始化处理进行说明。

在初始化处理中，位置估算部13首先将-1设定给帧计数器N(步骤S203)，由摄像部41取得图像(步骤S204)。图像例如能够以30fps取得(所取得的图像也被称为帧)。接着，从所取得的图像内取得2D特征点(步骤S205)。所谓2D特征点，是图像中的边缘部分等图像内的特征性的部分，能够使用SIFT(Scale-Invariant Future Transform，尺度不变特征转换)、SURF(Speed-Up Robust Features，加速稳健特征)等算法来取得。另外，为了取得2D特征点，也可以使用其他算法。

若所取得的2D特征点的个数较少，则无法进行利用后述的Two-viewStructure from Motion法(双视图运动恢复结构法)的计算，因此位置估算部13在步骤S206中，将2D特征点的取得数与基准值(例如10个)进行比较，在小于基准值的情况下(步骤S206：“否”)返回至步骤S204，反复进行图像的取得和2D特征点的取得，直到得到基准值以上的2D特征点数为止。另外，在此时点，虽然还未能制作地图的信息，但例如在上述的典型的示例中，由于借助障碍物传感器31，在家中的各房间开始到处移动，因此只要在该初始化处理中反复进行图像取得和2D特征点取得，便会一边移动一边反复进行图像取得，所以能够取得各种各样的图像，能够期待终归能取得2D特征点数较多的图像。

在2D特征点的取得数为基准值以上的情况下(步骤S206：“是”)，位置估算部13使帧计数器N递增(步骤S207)。然后，判定帧计数器N是否为0(步骤S208)。若帧计数器N为0(步骤S208：“是”)则仅仅才取得一个图像，所以为了取得第2张图像而返回至步骤S204。另外虽然在图4的流程图中并未记载，但取得第1张图像时的自身设备的位置和取得第2张图像时的自身设备的位置在一定程度上远离，会提高通过之后的处理而估算的姿势的精度。因此，在从步骤S208返回至步骤S204时，也可以追加如下处理，即，进行等待，直到通过主流程(图3)的步骤S107中的给驱动部42的动作指示从而基于里程计的平移距离成为规定距离(例如1m)以上为止。

若帧计数器N不为0(步骤S208：“否”)则可知已经取得了两个图像，因此位置估算部13在这两个图像间取得2D特征点的对应(实际环境上的同一点存在于各个图像中，能提取其对应)(步骤S209)。这里由于若特征点的对应数小于5，则不能进行后述的两个图像间的姿势的估算，因此位置估算部13判定特征点的对应数是否小于5(步骤S210)。若小于5(步骤S210：“是”)则为了重新取得初始图像，返回至步骤S203。若特征点的对应数为5点以上(步骤S210：“否”)，则通过使用Two-viewStructure from Motion法，从而能够估算两个图像间的姿势(取得各个图像的位置的差分(平移向量t)以及朝向的差分(旋转矩阵R))(步骤S211)。

该估算具体而言根据对应的特征点来求取基础矩阵E，通过将基础矩阵E分解为平移向量t和旋转矩阵R来得到，但其详情在非专利文献2中已经作了说明，在此不再赘述。另外，在此得到的平移向量t(若设想在三维空间内移动，则以取得最初的图像的位置为原点，具有X、Y、Z这3个要素)的各要素的值与实际环境上的值不同(Two-view Structure fromMotion法不能得到实际环境上的值本身，而得到与实际环境相似的空间上的值。)，因此将它们视为SLAM空间上的值，以下使用SLAM空间上的坐标(SLAM坐标)来进行说明。

若求出两个图像间的姿势(平移向量t以及旋转矩阵R)，则其值成为以最初的图像为基准(以取得最初的图像的位置为SLAM坐标的原点，平移向量为0向量，旋转矩阵为单位矩阵I。)的情况下的第二张图像的姿势(取得第二个图像时的自身设备的位置(平移向量t)以及朝向(旋转矩阵R))。在此，在求出两个图像各自的姿势(是该图像(帧)拍摄时的自身设备的位置(平移向量t)以及朝向(旋转矩阵R)，也称为帧姿势)的情况下，基于以下的思想，由地图制作部12来求取在这两个图像间能取得对应的2D特征点(对应特征点)在SLAM坐标中的3D位置(步骤S212)。

若将2D特征点在图像中的坐标(帧坐标：已知)设为(u，v)，并将该2D特征点在SLAM坐标中的3D位置(未知)设为(X，Y，Z)，则将它们用齐次坐标表示时的它们的关系，使用透视投影矩阵P由下述的算式(1)来表示。在此，“～”记号表示“除以非零的常数倍后相等”(即，相等或成为常数(非零)倍)，“’”记号表示“转置”。

(u v 1)’～P(X Y Z 1)’…(1)

在上述的算式(1)中，P为3×4的矩阵，根据表示摄像机的内部参数的3×3的矩阵A、和表示该图像的姿势(帧姿势)的外部参数R以及t由以下的算式(2)来表示。在此，(R|t)表示在旋转矩阵R的右侧排列平移列向量t而得到的矩阵。

P＝A(R|t)…(2)

在上述的算式(2)中，R以及t如上所述作为该帧姿势来求取。此外，摄像机的内部参数A由焦点距离和摄像元件尺寸来决定，因此只要决定摄像部41便成为常数。

若假设在两个图像间取得了对应的2D特征点当中的一个映射于第一个图像的帧坐标(u₁，v₁)和第二个图像的帧坐标(u₂，v₂)，则能够得到以下的算式(3)以及算式(4)。在此，I表示单位矩阵，0表示零向量，(L|r)表示在矩阵L的右侧排列列向量r而得到的矩阵。

(u₁ v₁ 1)’～A(I|0)(X Y Z 1)’…(3)

(u₂ v₂ 1)’～A(R|t)(X Y Z 1)’…(4)

在上述的算式(3)以及算式(4)中，由于能够形成关于u₁、v₁、u₂、v₂各自的算式，因此算式得到4个而未知数为X、Y、Z这3个，因此能够求取X、Y、Z，这成为该2D特征点在SLAM坐标中的3D位置。另外，由于算式的个数多于未知数的个数，因此有可能存在例如以u₁、v₁、u₂求取的X、Y、Z与以u₁、v₁、v₂求取的X、Y、Z不同的情况。这样的情况下，成为过剩条件的联立一次方程式，一般而言无解，但地图制作部12使用最小平方法，来求取似乎最可靠的X、Y、Z。

若求出2D特征点在SLAM坐标中的3D位置(X，Y，Z)，则由地图制作部12将其作为Map点而登记到Map点数据库(也称为Map点DB(Database)，被保存在地图存储部22中)中(步骤S213)。作为登记到Map点数据库中的要素，至少需要“2D特征点在SLAM坐标中的3D位置即X、Y、Z”、和“该2D特征点的特征量”(例如通过SIFT等而得到的特征量)。此外，若将“时间戳”(后述的关键帧计数器NKF(表示当前的关键帧编号的变量)在向Map点数据库的登记时间点的值等)追加到向Map点数据库的登记要素中，则在对Map点数据库进行编辑(返回到过去的状态等)时会很便利。

然后，地图制作部12判定是否已经将在两个图像间取得了对应的2D特征点(对应特征点)全部登记到Map点数据库中(步骤S214)，若尚未完成全部登记(步骤S214：“否”)则返回至步骤S212，若全部登记完成(步骤S214：“是”)则前进至步骤S215。

接下来位置估算部13将NKF(表示关键帧(是指成为后续的线程中的处理对象的图像)的计数器的变量)初始化为0(步骤S215)，将第二个图像作为关键帧而登记到帧数据库(也称为帧DB(Database)，被保存在图像存储部21中)(步骤S216)。

登记到帧数据库中的要素是：“关键帧编号”(登记时间点上的关键帧计数器NKF的值)、“姿势”(该图像拍摄时的自身设备在SLAM坐标内的位置(平移向量t)以及朝向(旋转矩阵R))、“所提取出的全部的2D特征点”、“全部的2D特征点当中作为Map点而已知3D位置的点”、“关键帧自身的特征”，但也可以除了这些以外还登记“由里程计测量出的实际环境上的姿势”(根据实际环境中的基于驱动部42的移动距离而求取的自身设备的位置以及朝向)。

上述中，所谓“关键帧自身的特征”，是用于使求取关键帧间的图像类似度的处理变得高效的数据，通常使用图像中的2D特征点的直方图等为好，但也可以将图像自身设为“关键帧自身的特征”。此外，“由里程计测量出的实际环境上的姿势”，也能够通过平移向量t和旋转矩阵R来表示，但通常，由于该自主移动装置100在二维平面上活动，因此也可以简化为二维数据，作为以移动开始时的位置(原点)以及朝向为基准的二维坐标(X，Y)以及朝向φ来表示。

接着，位置估算部13为了使地图制作线程知道生成了关键帧的情况，在地图制作线程的关键帧队列(队列(Queue)成为先入先出的数据结构)中，设置关键帧计数器NKF(步骤S217)。

以上，自身设备位置估算线程的初始化处理完成，因此位置估算部13设置初始化完成标志，并为了得到SLAM坐标与实际环境坐标的比例(scale)对应，通过将基于里程计的平移距离(以实际环境下的坐标来求取)除以通过上述处理而估算出的SLAM坐标下的平移距离d，来求取比例S(步骤S218)。

然后，控制部10为了定期性地使旋转拍摄部11进行动作，从计时器(未图示)取得当前时刻并设置到RTIME(用于设定开始旋转拍摄处理的时刻的变量)(步骤S219)。然后，经由步骤S201、步骤S202，前进到初始化完成的情况下的处理即步骤S220。

说明初始化完成的情况下的处理。该处理是自身设备位置估算线程的通常时的处理，是位置估算部13依次估算当前的自身设备的位置以及朝向(在SLAM坐标内的平移向量t和旋转矩阵R)的处理。

首先，控制部10判定当前时刻是否从RTIME起经过了规定时间(DTIME(设定了进行旋转拍摄处理的时间间隔的变量)：例如10分钟)(是否满足用于进行旋转拍摄的条件)(步骤S220)。若尚未经过(步骤S220：“是”)则前进到步骤S221。若已经过(步骤S220：“是”)则进行旋转拍摄处理(步骤S233)后返回至步骤S201。

若当前时刻从RTIME起尚未经过规定时间(DTIME)，则位置估算部13由摄像部41拍摄图像(步骤S221)，使帧计数器N递增(步骤S222)。然后，取得所拍摄的图像中包含的2D特征点(步骤S223)。接着，从登记到帧数据库中的以前的关键帧(例如关键帧编号为NKF的图像)的信息中，取得该图像的信息中所包含的2D特征点当中的3D位置已知(成为了登记到Map点数据库中的Map点)的2D特征点，提取在与当前拍摄的图像之间能取得对应的2D特征点(对应特征点)(步骤S224)。

然后，位置估算部13判定对应特征点的个数是否小于规定个数(例如10。以下称为“基准对应特征点数”。)(步骤S225)，由于在小于基准对应特征点数的情况下(步骤S225：“是”)利用SLAM法估算的姿势的精度较差，因此不进行位置的估算而返回至步骤S220。在此，也可以不是立刻返回至步骤S220，而是返回至步骤S224，从登记到帧数据库中的关键帧当中检索对应特征点的个数为基准对应特征点数以上的关键帧。在该情况下，在登记到帧数据库中的关键帧当中未找到对应特征点的个数为基准对应特征点数以上的关键帧的情况下返回至步骤S220。

位置估算部13若能够提取基准对应特征点数以上的对应特征点(步骤S225：“否”)，则从Map点数据库中取得对应特征点各自的3D位置(X_i，Y_i，Z_i)(步骤S226)。若将当前拍摄到的图像中包含的对应特征点的帧坐标设为(u_i，v_i)，将该对应特征点的3D位置设为(X_i，Y_i，Z_i)(i取从1到对应特征点数的值)，则将各对应特征点的3D位置(X_i，Y_i，Z_i)通过以下的算式(5)而投影到帧坐标系的值(ux_i，vx_i)与帧坐标(u_i，v_i)理想而言应该是一致的。

(ux_i vx_i 1)’～A(R|t)(X_i Y_i Z_i 1)’…(5)

实际上在(X_i，Y_i，Z_i)中以及在(u_i，v_i)中都包含有误差，因此(ux_i，vx_i)和(u_i，v_i)一致的情况很少存在。而且，未知数仅为R和t(三维空间的情况下分别为三维，3+3＝6是未知数的个数)，而数学式存在对应特征点的个数的2倍(对于一个对应特征点，存在与帧坐标u、v分别对应的算式)，因此成为过剩条件的联立一次方程式，如上所述通过最小平方法来求取。具体而言，位置估算部13求取使以下的算式(6)的成本函数E1成为最小的姿势(平移向量t以及旋转矩阵R)。这成为通过SLAM法而求取的SLAM坐标下的自身设备的姿势(由平移向量t以及旋转矩阵R表示的自身设备的位置以及朝向)。以此方式，位置估算部13估算自身设备的姿势(步骤S227)。

【数学式1】

由于求取到SLAM坐标下的当前的自身设备的姿势(平移向量t以及旋转矩阵R)，因此位置估算部13通过将其乘以比例S，来求取VO(视觉里程计)(步骤S228)。VO能够作为实际环境下的自身设备的位置以及朝向来利用。

接着，位置估算部13判定从对登记到帧DB中的前一个关键帧(关键帧编号为NKF的图像)进行拍摄时的自身设备的位置起是否移动了规定距离(例如1m。以下称为“基准平移距离”。)以上(步骤S229)，若移动了基准平移距离以上(步骤S229：“是”)则使关键帧计数器NKF递增后(步骤S230)，将当前帧作为关键帧而登记到帧DB中(步骤S231)。若仅移动不足基准平移距离(步骤S229：“否”)则返回至步骤S201。

在此，与基准平移距离进行比较的自身设备的移动距离既可以从里程计取得从前一个关键帧到当前帧的平移距离(两帧的平移向量之差的向量的绝对值(要素的平方和的平方根))，也可以从上述的VO(视觉里程计)求取。登记到帧DB中的内容如上所述是：“关键帧编号”、“姿势”、“所提取出的全部的2D特征点”、“全部的2D特征点当中作为Map点而已知3D位置的点”、“关键帧自身的特征”。

然后，位置估算部13为了使地图制作线程知道产生了新的关键帧，在地图制作线程的关键帧队列中设置关键帧计数器NKF(步骤S232)。然后，返回至步骤S201。另外，关键帧计数器NKF、比例S、Map点DB、帧DB存储在存储部20中，使得能够跨越线程来参照值。

接下来，关于在自主移动装置100的主流程(图3)的步骤S102起动的地图制作线程，使用图5来进行说明。该线程由地图制作部12计算关键帧中的对应特征点的3D位置，来制作地图的信息(Map点DB)。

首先，地图制作部12判定是否动作结束(步骤S301)。若动作结束(步骤S301：“是”)则结束，若动作并未结束(步骤S301：“否”)，则判定关键帧队列是否为空(步骤S302)。若关键帧队列为空(步骤S302：“是”)则返回至步骤S301，若不为空(步骤S302：“否”)，则从关键帧队列中取出数据设定给MKF(表示在地图制作线程中处理的关键帧的关键帧编号的变量)(步骤S303)。地图制作部12判定MKF是否大于0(步骤S304)，在MKF为0的情况下(步骤S304：“否”)返回至步骤S301，等待数据进入到关键帧队列。在MKF为1以上的情况下(步骤S304：“是”)，进入到以下的处理。

地图制作部12参照帧DB，提取在前关键帧(关键帧编号为MKF-1的关键帧)的2D特征点与当前关键帧(关键帧编号为MKF的关键帧)的2D特征点之间能取得对应的2D特征点(对应特征点)(步骤S305)。由于在帧DB中还登记有各关键帧的姿势(平移向量t和旋转矩阵R)，因此能够通过与自身设备位置估算线程的初始化时的处理时同样的方法来计算对应特征点的3D位置。地图制作部12将计算出了3D位置的对应特征点作为Map点而登记到Map点DB中(步骤S306)。地图制作部12对于帧DB也针对本次计算出了3D位置的2D特征点来登记3D位置(步骤S307)。

另外，在地图制作部12提取出的对应特征点已经登记在Map点DB中的情况下，可以跳过3D位置计算而进入到针对下一个对应特征点(Map点DB中未登记的对应特征点)的处理，也可以重新进行3D位置计算，对Map点DB中已经登记的3D位置、帧DB中的对应特征点对应的3D位置进行更新。

接着，地图制作部12判定关键帧队列是否为空(步骤S308)。若为空(步骤S308：“是”)，则针对全部关键帧的姿势和全部Map点的3D位置，进行捆绑调整处理，提高精度后(步骤S309)前进至步骤S310。若关键帧队列不为空(步骤S308：“否”)则前进至步骤S310。接着地图制作部12将MKF设置到闭环线程的关键帧队列(步骤S310)，返回至步骤S301。

另外，所谓捆绑调整处理，是对摄像机姿势(关键帧姿势)和Map点的3D位置同时进行估算的非线性最优化法，进行在使Map点投影在关键帧上时产生的误差最小那样的最优化。

通过进行该捆绑调整的处理，从而能够提高关键帧姿势和Map点的3D位置的精度。但是，即使不进行该处理，仅是不能实现精度提高，也并不会发生特别问题。因此，即使在没有其他处理的状态时(例如，关键帧队列为空的状态)，也不必每次都进行该处理。

此外，若进行捆绑调整的处理，则有时会发现投影在关键帧上时的误差大于规定值的Map点。关于这样的误差较大的Map点，由于会给SLAM估算造成不良影响，因此可以从Map点DB以及帧DB中进行删除，或者设立用于识别是误差较大的需要注意的Map点的标志。另外，捆绑调整的处理在本实施方式中为可选的处理，所以在此省略处理的详细情况。

接下来关于在自主移动装置100的主流程(图3)的步骤S103起动的闭环线程，使用图6来进行说明。在该线程中控制部10连续不断地检查是否能够进行闭环处理，在能够进行的情况下进行闭环处理。另外，所谓闭环处理，是指在识别出已返回到以前来到过的相同场所的情况时，使用以前位于该相同场所时的姿势的值和当前的姿势的值的偏差，对从以前来到时到当前为止的轨迹中的关键帧、关联的Map点的3D位置进行修正。

首先，控制部10判定是否动作结束(步骤S401)。若动作结束(步骤S401：“是”)则结束。若动作并未结束(步骤S401：“否”)则判定关键帧队列是否为空(步骤S402)。若关键帧队列为空(步骤S402：“是”)则返回至步骤S401，若关键帧队列不为空(步骤S402：“否”)，则从关键帧队列中取出数据并设定给LKF(表示在闭环线程中处理的关键帧的关键帧编号的变量)(步骤S403)。接着控制部10判定LKF是否大于1(步骤S404)。在LKF为0或1的情况下(步骤S404：“否”)返回至步骤S401，等待数据进入到关键帧队列中。然后，在LKF为2以上的情况下(步骤S404：“是”)，进行以下的处理。

类似图像检索部14参照帧DB，从帧DB中检索当前关键帧(关键帧编号为LKF的关键帧)与“关键帧自身的特征”的类似度为规定的类似度(例如0.9。以下称为“基准图像类似度”。)以上的关键帧(步骤S405)。在此，关于该类似度，在由特征向量表示了图像(关键帧)的特征的情况下，能够将两个图像的特征向量的绝对值(要素的平方和的平方根)归一化为1而得到的值彼此的内积设为该两个图像的类似度。此外，也可以将两个图像的特征向量(将绝对值归一化为1而得到的值)的距离(各要素之差的平方和的平方根)的倒数设为类似度。

控制部10判定由类似图像检索部14是否发现了“关键帧自身的特征”的类似度为基准图像类似度以上的关键帧(步骤S406)，若未发现(步骤S406：“否”)则返回到步骤S401，若发现(步骤S406：“是”)，则地图校正部15对从所发现的关键帧到当前关键帧的轨迹中的关键帧的姿势、和轨迹中的关键帧所包含的Map点的3D位置进行修正(步骤S407)。例如，地图校正部15将当前关键帧的姿势修正为与所发现的关键帧的姿势相同的姿势。然后，使用所发现的关键帧的姿势与当前关键帧的姿势的差分，对从所发现的关键帧到当前关键帧的轨迹中的各关键帧的姿势施加线性校正。进而关于这些各关键帧所包含的Map点的3D位置也根据各关键帧的姿势的校正量来进行修正。

然后，控制部10返回至步骤S401。

接下来关于在自主移动装置100的主流程(图3)的步骤S101起动的自身设备位置估算线程的流程图(图4)的步骤S233的处理内容即旋转拍摄处理，使用图7来进行说明。该处理如下：由驱动部42使自身设备进行旋转的同时，由摄像部41拍摄图像，并将所拍摄的图像与自身设备的位置以及朝向的信息一起存储到图像存储部21中。通过进行该旋转拍摄处理，从而如图14所示，位置和拍摄方向都相近的点上的关键帧增加。由于这样的关键帧包含很多相同的特征点，因此能够进行闭环处理，通过定期性地进行该旋转拍摄处理，从而能够定期性地闭环。

首先，作为旋转拍摄处理的初始化处理，旋转拍摄部11对旋转拍摄标志RF设置1(步骤S501)，并清除累积旋转角φ_S(步骤S502)。

接着，旋转拍摄部11由摄像部41拍摄图像(步骤S503)，并使帧计数器N递增(步骤S504)。然后，取得所拍摄的图像中包含的2D特征点(步骤S505)。接着，从登记到帧数据库中的以前的关键帧(例如关键帧编号为NKF的图像)的信息中，取得该图像的信息中所包含的2D特征点当中的3D位置已知的2D特征点(登记到Map点数据库中的Map点)，并提取在与当前拍摄的图像之间能取得对应的2D特征点(对应特征点)(步骤S506)。

然后，位置估算部13判定对应特征点的个数是否小于基准对应特征点数(步骤S507)，在小于基准对应特征点数的情况下(步骤S507：“是”)利用SLAM法估算的姿势的精度较差，因此不进行位置的估算而前进至步骤S514。在此，也可以不是立刻前进至步骤S514，而是返回至步骤S506，从登记到帧数据库中的关键帧当中检索对应特征点的个数为基准对应特征点数以上的关键帧。在该情况下，在登记到帧数据库中的关键帧当中未找到对应特征点的个数为基准对应特征点数以上的关键帧的情况下前进至步骤S514。

旋转拍摄部11若能够提取基准对应特征点数以上的对应特征点(步骤S507：“否”)，则从Map点数据库中取得对应特征点各自的3D位置(X_i，Y_i，Z_i)(步骤S508)，并进行与自身设备位置估算线程的步骤S227同样的计算来求取SLAM坐标下的自身设备的姿势(由平移向量t以及旋转矩阵R表示的自身设备的位置以及朝向)(步骤S509)。

然后，旋转拍摄部11通过对其乘以比例S，来求取VO(视觉里程计)(步骤S510)，并使关键帧计数器NKF递增后(步骤S511)，将当前帧作为关键帧而登记到帧DB中(步骤S512)。然后，旋转拍摄部11为了使地图制作线程知道产生了新的关键帧的情况，在地图制作线程的关键帧队列中设置关键帧计数器NKF(步骤S513)。

接着，旋转拍摄部11判定旋转拍摄标志RF是否被设置了2(步骤S514)。若被设置了2(步骤S514：“是”)则将当前时刻设置到RTIME(步骤S520)，并清除旋转拍摄标志RF之后(步骤S521)，返回至自身设备位置估算线程的S201。若RF不为2(步骤S514：“否”)则前进到步骤S515，判定累积旋转角φ_S+旋转角φ₁是否小于360°(步骤S515)。在此，旋转角φ₁是旋转拍摄处理的一次旋转动作相应的旋转角，例如为30°。φ₁期望设定摄像部41的视角的一半以下的角度。

旋转拍摄部11在累积旋转角φ_S+旋转角φ₁小于360°的情况下(步骤S515：“是”)，向驱动部42发出指示使得进行旋转角φ₁的旋转动作(步骤S516)，在累积旋转角φ_S上加上旋转角φ₁(步骤S517)之后返回至步骤S503。这里的旋转动作如果通过使驱动部42的两个车轮等速反向旋转等，从而能够不移动位置地旋转，则期望进行不移动位置的旋转动作。在只能使两个车轮向相同方向进行旋转的情况下，期望进行旋转半径尽可能小的旋转动作。

旋转拍摄部11在累积旋转角φ_S+旋转角φ₁为360°以上的情况下(步骤S515：“否”)，向驱动部42发出指示使得进行旋转角(360°-φ_S)的旋转动作(步骤S518)，并对旋转拍摄标志RF设置2之后(步骤S519)返回至步骤S503。

另外，假如通过驱动部42来进行不移动位置的旋转动作的情况下，在步骤S509中，能够在平移向量t不变这样的前提下，求取旋转矩阵R。

在以上的第一实施方式中，旋转拍摄部11成为如下处理，即，若“经过了规定时间(DTIME)”，则进行动作，但作为变形例，也可以设为“若移动规定距离(例如基准平移距离)以上”则进行动作。在该情况下，只要取代RTIME而导入Rt(用于设定上次开始旋转拍摄处理的位置(平移向量)的变量)，并且图4的步骤S219设为“Rt＝当前的平移向量t”，步骤S220设为“当前的平移向量t与Rt的距离＜规定距离”，图7的步骤S520设为“Rt＝关键帧编号NKF的平移向量t”，则每当移动规定距离时旋转拍摄部11便进行动作。

以上，在第一实施方式中，每当经过规定时间时，便定期性地，由旋转拍摄部11一边进行旋转动作一边拍摄图像，在第一实施方式的变形例中，每当移动规定距离时，便定期性地，由旋转拍摄部11一边进行旋转动作一边拍摄图像。通过该旋转拍摄部11的动作，从而能够增加成为类似图像检索部14中的检索对象的图像存储部21内的图像的信息。

作为第二实施方式，说明在对与过去访问过的位置在欧几里德距离上接近的位置进行了访问的情况下进行旋转拍摄处理的实施方式。本实施方式所涉及的自主移动装置100’如图8所示，成为如下构成，即，在第一实施方式的控制部10中追加了距离判定部16，在存储部20中追加了位置历史记录存储部23。距离判定部16判定自主移动装置100’的当前位置与过去的位置的距离是否为规定距离以上。在位置历史记录存储部23中，位置估算部13估算出的位置(平移向量t)与时间戳(关键帧计数器)一起进行存储。另外，即使不追加独立的位置历史记录存储部23，也能够认为在图像存储部21中包含位置历史记录存储部23(由于在图像存储部21内的帧DB中存储的“姿势”的信息中包含存储在位置历史记录存储部23中的“位置”(位置历史记录存储时的平移向量t)的信息)。

第二实施方式的自主移动处理整体的流程图与第一实施方式相同，在图3中示出。第二实施方式的自身设备位置估算线程的流程图在图9中示出。由于大部分与第一实施方式的自身设备位置估算线程的流程图即图4相同，因此仅说明不同的部分。

在步骤S2192中，控制部10将当前的平移向量t设置到Rt。在步骤S2202中，距离判定部16判定是否当前的平移向量t与Rt的距离为规定距离D(表示规定距离(基准距离)的变量。例如设定基准平移距离。)以上、并且在位置历史记录存储部23中存储有与当前的平移向量t达到规定的类似度(以下称为“基准位置类似度”。例如0.9。)以上一致的位置(位置历史记录存储时的平移向量t)(位置的类似度能够通过计算将进行比较的两个平移向量的绝对值(要素的平方和的平方根)归一化为1而得到的值彼此的内积来求取)。另外，在不追加独立的位置历史记录存储部23而将图像存储部21内的帧DB也作为位置历史记录存储部23来利用的情况下，判定在帧DB内的关键帧的各姿势当中是否存储有与当前的平移向量t达到基准位置类似度以上一致的位置(关键帧登记时的平移向量t)。

如果，当前的平移向量t与Rt相距D以上并且在位置历史记录存储部23(或帧DB)中存储有达到基准位置类似度以上一致的位置(步骤S2202：“是”)，则在前进到步骤S233进行旋转拍摄处理之后返回至步骤S201。若与Rt的距离小于基准距离D或在位置历史记录存储部23(或者帧DB)中未存储达到基准位置类似度以上一致的位置(步骤S2202：“否”)则前进到步骤S221。

在步骤S234中，位置估算部13将当前的位置(平移向量t)与时间戳一起登记到位置历史记录存储部23中。在进行登记时，也可以对以前存储的位置进行确认，在已经存储了相同位置(或达到基准位置类似度以上一致的位置)的情况下不进行登记。如此一来，能够节约位置历史记录存储部23的存储区域。此外，在不追加独立的位置历史记录存储部23而将图像存储部21内的帧DB也作为位置历史记录存储部23来利用的情况下，步骤S234能够省略。

第二实施方式的地图制作线程与第一实施方式相同，在图5中示出。第二实施方式的闭环线程也与第一实施方式相同，在图6中示出。第二实施方式的旋转拍摄处理与图7所示的第一实施方式大致相同，但图7的步骤S520的处理并非“RTIME＝当前时刻”而是“对Rt设置关键帧编号NKF的平移向量t”，仅这一点不同。

第二实施方式通过设为这样的处理，从而仅在来到与过去访问过的位置在欧几里德距离上接近的位置时进行旋转拍摄处理，能够选择进行闭环处理的可能性较高时来高效地进行旋转拍摄处理。另外，上述在步骤S2202的处理中，距离判定部16进行了“在位置历史记录存储部23中是否存储有与当前的平移向量t达到基准位置类似度以上一致的位置的判定”，但也可以进行“在位置历史记录存储部23中是否存储有与当前的平移向量t的距离小于规定距离(例如设定“5cm”或“基准平移距离的1/10”等与D相比相当小的值。)的位置的判定”(在以下记述的变形例中也同样)。

进而作为第二实施方式的变形例，可以考虑将图9的步骤S2202的条件追加第3个条件，使得成为“当前的平移向量t与Rt的距离为基准距离D以上，并且，在位置历史记录存储部23中存储有与当前的平移向量t达到基准位置类似度以上一致的位置，并且，在帧DB中不存在与当前关键帧达到基准图像类似度以上类似的关键帧”。

在帧DB中存在与当前关键帧达到基准图像类似度以上类似的关键帧的情况下，即使不进行旋转拍摄处理，在闭环线程中进行闭环处理的可能性也很高。因此，在该变形例中，通过在这样的情况下不进行旋转拍摄处理，从而与第二实施方式相比，能够进一步减少进行旋转拍摄处理的次数。

作为第三实施方式，说明在对与过去访问过的位置在欧几里德距离上接近、并且共同可视图距离较远的位置进行了访问的情况下进行旋转拍摄处理的实施方式。首先，对共同可视图进行说明。所谓共同可视图，如图10所示，是用线将“包含有多个共同的Map点(已取得三维位置的特征点)等的、能够直接计算相对位置关系的关键帧彼此”连结的图。在两个关键帧间沿着该线最低前进n次才能够达到的情况下，这些关键帧间的共同可视图距离成为n。对于共同可视图距离较大的关键帧彼此来说，在SLAM估算中与该距离相应的误差发生累积，因此会包含很多相对位置的误差。在第三实施方式中，如图15所示，通过在对与过去访问过的位置欧几里德距离较近、并且共同可视图距离较远的位置进行了访问的情况下进行旋转拍摄处理，能够得到位置和拍摄方向都相近的关键帧，因此会被闭环。

第三实施方式与第二实施方式共同点很多。在图9的步骤S2202的条件中以AND条件追加了“与达到基准位置类似度以上一致的位置的共同可视图距离为规定值n以上”，仅在这一点上不同，其他的处理、构成都与第二实施方式相同。

在该情况下，图9的步骤S2202的条件成为“当前的平移向量t与Rt的距离为基准距离D以上，并且，在位置历史记录存储部23中存储有与当前的平移向量t达到基准位置类似度以上一致的位置，并且，与达到基准位置类似度以上一致的位置的共同可视图距离为规定值以上”。

说明共同可视图距离的求取方法。首先，若参照帧DB来提取能够与关键帧K直接计算相对位置关系的关键帧的集合(简单来说设为“与关键帧K的图像类似度为基准图像类似度以上的关键帧的集合”即可)，则该关键帧的集合成为“与关键帧K的共同可视图距离为1的关键帧集合”。然后，若针对“与关键帧K的共同可视图距离为i的关键帧集合”中所包含的每一个关键帧，参照帧DB来提取能够与该关键帧直接计算相对位置关系的关键帧的集合(简单来说设为“与该关键帧的图像类似度为基准图像类似度以上的关键帧的集合”即可)，则该关键帧的集合成为“与关键帧K的共同可视图距离为i+1的关键帧集合”。在这些关键帧集合的提取处理中，类似图像检索部14进行相当于相对位置计算可能判定部的处理。

在关键帧K和L被提供时，为了求取K与L之间的共同可视图距离，通过上述的处理方法，首先生成“与关键帧K的共同可视图距离为1的关键帧集合”。然后，若在“与关键帧K的共同可视图距离为i的关键帧集合”中不包含L，则反复进行生成“与关键帧K的共同可视图距离为i+1的关键帧集合”的处理，直到在“与关键帧K的共同可视图距离为i+1的关键帧集合”中包含L为止。若得到包含L的关键帧集合，则此时的“i+1”成为K与L之间的共同可视图距离。在该共同可视图距离计算处理中，控制部10进行相当于共同可视图距离计算部的处理。

在步骤S2202中，作为第二个判定条件而存在“在位置历史记录存储部23中是否存储有与当前的平移向量t达到基准位置类似度以上一致的位置”这样的条件，但在该条件被满足的情况下，若参照帧DB提取该位置所对应的关键帧(在位置历史记录存储部23中登记位置时的时间戳(关键帧计数器)的值的关键帧编号的关键帧成为与该位置对应的关键帧)来作为关键帧K，则“与达到基准位置类似度以上一致的位置的共同可视图距离为规定值n以上”的判定如上所述，生成“与当前关键帧的共同可视图距离为1的关键帧集合”～“与当前关键帧的共同可视图距离为n-1的关键帧集合”，若在这些关键帧集合中不包含关键帧K则能够判定为“与达到基准位置类似度以上一致的位置的共同可视图距离为规定值n以上”。

在存在共同可视图距离较近的关键帧的情况下，可以认为没怎么累积误差，因此通过仅在共同可视图距离为规定值以上的情况下进行旋转拍摄处理，从而仅在误差累积了规定值以上的情况下进行旋转拍摄处理，能够减少进行旋转拍摄处理的次数。

在以上说明的实施方式中，由于通过该旋转拍摄处理，从而从各种朝向拍摄的图像作为关键帧而被登记到帧DB中，因此在之后的闭环线程中，容易发现类似关键帧，结果，进行地图校正的频度增加，能够减少自身设备位置等的累积误差。

到此为止，通过增加登记到帧DB中的关键帧，从而使进行闭环处理的频度增加，但作为其他的处理方法，还存在如下方法，即，在闭环线程中的步骤S405中由类似图像检索部14检索类似图像时，减小基准图像类似度(使得即使类似度并不那么大也进行闭环处理)这样的方法。

但是，若是仅减小基准图像类似度，则即使在错误的位置(与过去访问过的场所无关的位置)也进行闭环处理的可能性增高。因此，仅在由位置估算部13估算出的当前的自身设备位置与成为类似图像检索部14中的比较对象的关键帧的位置成为基准距离以下的情况下，进行减小基准图像类似度的处理。将其作为第四实施方式来进行说明。

第四实施方式的构成与第二实施方式相同，在图8中示出。第四实施方式的自主移动处理整体的流程图与第一实施方式相同，在图3中示出。第四实施方式的自身设备位置估算线程的流程图在图11中示出。由于其大部分与第一实施方式的自身设备位置估算线程的流程图即图4相同，故仅对不同的部分进行说明。

步骤S2193进行旋转拍摄处理的规定条件的初始化。例如，控制部10对Rt设置当前的平移向量t。在第四实施方式中旋转拍摄处理并非必须，因此在不进行旋转拍摄处理的情况下，该步骤S2193能够省略。

在步骤S2203中，控制部10判定旋转拍摄处理的规定条件是否满足。若满足(步骤S2203：“是”)则前进到步骤S233进行旋转拍摄处理后返回至步骤S201。若不满足(步骤S2203：“否”)则前进到步骤S221。在第四实施方式中，在不进行旋转拍摄处理的情况下，该步骤S2203也能够省略，在该情况下不进行条件判定而进入到步骤S221。

步骤S234由于与第二实施方式的自身设备位置估算线程的流程图即图9的步骤S234相同，故省略说明。

第四实施方式的地图制作线程与第一实施方式相同，在图5中示出。第四实施方式的旋转拍摄处理与图7所示的第一实施方式大致相同，但图7的步骤S520的处理不是“RTIME＝当前时刻”而是成为“旋转拍摄处理的规定条件的初始化”，仅这一点不同。另外，在第四实施方式中不进行旋转拍摄处理的情况下，能够省略图7的整个处理。

第四实施方式的闭环线程与图6所示的第一实施方式的闭环线程有一部分不同，如图12所示，在步骤S404与步骤S405之间追加步骤S408～步骤S410的各步骤。步骤S408由控制部10判定在位置历史记录存储部23中是否保存有与当前关键帧(关键帧编号为LKF的关键帧)的位置的一致度成为基准位置类似度以上的位置(拍摄当前关键帧时的自身设备的位置与存储在位置历史记录存储部23中的位置的距离成为规定值以下的位置)。

若在位置历史记录存储部23中保存有符合条件的位置(步骤S408：“是”)则采用D1作为基准图像类似度的值(步骤S409)，若未保存(步骤S408：“否”)则采用D2作为基准图像类似度的值(步骤S410)。在此，D1和D2成为D1＜D2这样的关系。即，在来到了与以前访问过的位置接近的位置的情况下(步骤S408为“是”的情况下)，将基准图像类似度设定为比通常值(D2)小的值。通过这样，从而在步骤S405中的检索中即使类似度稍小(即使为D1)也能够发现类似图像，能够进行步骤S407中的闭环处理。

在步骤S408中判定为“是”的情况下，大多处于与以前访问过的位置接近的位置。在该状况下从帧DB中不能发现类似图像，可以认为是进行拍摄的朝向与以前不同。在这样的情况下，通过将基准图像类似度设定为比通常值小的值，从而即使进行拍摄的朝向稍有不同也能够发现类似图像，能够进行闭环处理。

该第四实施方式与第一～第三实施方式不同，即使没有旋转拍摄处理也能够实现。因此，在上述中也说明了可以省略旋转拍摄处理。但是，若结合旋转拍摄处理来执行第四实施方式，则能够更可靠地执行闭环处理。

在不进行旋转拍摄处理的第四实施方式中，作为摄像部41希望采用视角较广的摄像部。特别是若使用视角为180°以上的摄像部41(最期望的是使用采用了视角360°的鱼眼透镜的摄像部41)，则如果来到以前访问过的位置，那么无论朝向那个方向，大多都能够拍摄共同的特征点，因此即使降低图像的类似度的判定的阈值也大多能够进行闭环处理。

另外，在上述的各实施方式中，将基准平移距离的值设定为了“例如1m”，但由于根据自主移动装置自身的大小、速度、自主移动的环境、移动范围等，最佳的基准平移距离会改变，因此关于基准平移距离的设定方法进行补充。

作为基准平移距离的设定方法，可以按对从关键帧上的所有Map点(3D位置已知的2D特征点)的3D位置到自身设备位置的距离进行平均得到的值(＝到在关键帧上观察到的所有Map点的进深的平均距离)与平移距离之比来进行设定(例如，将到Map点的进深的平均距离的5％的距离设为基准平移距离等)。在此使用的“所有Map点”能够通过由控制部10参照Map点DB来提取，在该处理中控制部10进行相当于位置估算可能特征点提取部的处理。此外，也可以根据实际环境上的平移距离的大小，例如若在桌子上活动则将10cm设定为基准平移距离，若在房间来回活动则将1m设定为基准平移距离，若在屋外的广场等来回活动则将10m等设定为基准平移距离，还可以将与驱动部42的车轮的直径相应的值(例如车轮的直径的10倍)设定为基准平移距离。

在作为基准平移距离而以实际环境上的距离进行设定的情况下，将SLAM坐标下的平移距离乘以比例S来变换为实际环境上的距离之后进行比较(反之也同样，在作为基准平移距离而设定SLAM坐标下的距离的情况下，将实际环境上的距离除以比例S来变换为SLAM坐标下的距离之后进行比较)。

另外，本发明的自主移动装置100(100’)的各功能通过通常的PC(Personal Computer，个人计算机)等计算机也能够实施。具体而言，在上述实施方式中，自主移动装置100(100’)所进行的自主移动控制处理的程序作为预先存储在存储部20的ROM中的程序来进行了说明。但是，也可以将程序保存在软盘、CD-ROM(Compact Disc Read Only Memory，光盘只读存储器)，DVD(Digital Versatile Disc，数字多用途盘)以及MO(Magneto-Optical Disc，磁光盘)等计算机可读取的记录介质中来进行分发，通过将该程序读入到计算机中进行安装，从而构成能够实现上述的各功能的计算机。

此外，该计算机既可以内置于自主移动装置100，也可以与自主移动装置100分开存在。即，也可以如图16所示，将控制部、存储部设置于外部的服务器200。自主移动装置100的控制部10可以通过云计算技术，经由通信部44将由摄像部41、传感器部30等所取得的数据发送到外部的服务器200，并使该服务器200进行了运算处理之后，经由通信部44来接收服务器200所执行的运算结果，对驱动部42等进行控制。在图12中，成为如下构成，即，服务器200的控制部50具备地图制作部12、位置估算部13、类似图像检索部14以及地图校正部15，自主移动装置100的控制部10具备旋转拍摄部11，但只要在服务器200的控制部50和自主移动装置100的控制部10的任意一者中具备各部即可。关于是服务器200的控制部50具备各部还是自主移动装置100的控制部10具备各部是任意的。

以上，对本发明的优选实施方式进行了说明，但本发明并不限定于这种特定的实施方式，在本发明中，包含权利要求书所记载的发明及其均等的范围。