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

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

背景技术：

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

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

在先技术文献

非专利文献

非专利文献1：andrewj.davison，“real-timesimultaneouslocalizationandmappingwithasinglecamera”，proceedingsofthe9thieeeinternationalconferenceoncomputervisionvolume2，2003，pp.1403-1410

非专利文献2：richardhartley，andrewzisserman，“multipleviewgeometryincomputervision”，secondedition，cambridge.universitypress，march2004，chapter9

专利文献

专利文献1：jp特开2015-146091号公报

专利文献2：jp特开2014-186694号公报

技术实现要素：

发明要解决的课题

对于使用了这样的图像的自身设备位置估算而言，在摄像机拍摄的方向发生较大变化的情况下、或在所拍摄到的图像中特征点较少的情况下，存在如下这样的问题，即，无法获得与过去的图像的共同特征点，自身设备位置的估算失败。

此外，在专利文献1中，公开了如下的自身位置估算方法：基于移动量来估算自身位置的候补点，在地图上决定多个测定点，基于距离映射将从所述测定点到壁障的距离设为评价距离，在第1评价工序中，基于评价距离来评价一致的程度，在第2评价工序中，将从外界传感器朝向测定点的方向的单位向量即测定方向向量与测定点的壁障方向向量的内积作为评价函数来进行评价，将在第1评价工序中一致较大、并且在所述第2评价工序中评价函数较大的候补点估算为自身位置。

此外，在专利文献2中，公开了一种自主移动式无人输送车，其特征在于，在起动时，基于存储在停止主要原因存储单元中的停止主要原因，来判断是否能够从当前的自身位置继续进行自主移动，在判断为能够进行自主移动的情况下，基于存储在自身位置存储单元中的自身位置开始自主移动，在判断为不能进行自主移动的情况下，保持停止状态。

但是，对于专利文献1所记载的技术而言，虽然能够提高自身位置估算的精度，但存在自身设备位置的估算失败的情况下，不能恢复这样的课题。

此外，对于专利文献2所记载的技术而言，仍然存在判断为不能进行自主移动的情况下，不能恢复这样的课题。

因此，本发明为了解决上述问题而作，其目的在于提供一种能够从不能进行自身设备位置估算的状态快速地恢复的自主移动装置等。

用于解决课题的手段

本发明的自主移动装置具备摄像部、图像存储部、控制部和驱动部，其中，

所述图像存储部存储所述摄像部所拍摄到的多个图像的信息，

所述控制部，

对所述驱动部进行驱动处理使得基于所述摄像部所拍摄到的图像的信息来自主地移动，

使用存储在所述图像存储部中的多个图像的信息来估算自身设备位置，

判定所述自身设备位置的估算结果是否唯一确定，

在判定为所述估算结果不唯一确定的情况下，控制所述驱动处理使得移动到所述自身设备位置的估算结果唯一确定的位置。

发明效果

根据本发明，能够从不能进行自身设备位置估算的状态快速地恢复

附图说明

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

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

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

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

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

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

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

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

图9是表示第二实施方式所涉及的自主移动控制处理当中的移动处理控制的处理的流程图的图。

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

图11是表示第三实施方式所涉及的自主移动控制处理当中的移动处理控制的处理的流程图的图。

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

符号说明

100…自主移动装置，10、50…控制部，11…移动处理部，12…地图制作部，13…位置估算部，14…估算判定部，15…移动处理控制部，16…旋转拍摄部，20、60…存储部，21…图像存储部，22…地图存储部，23…位置变化历史记录存储部，24…位置历史记录存储部，25…特征点存储部，30…传感器部，31…加速度传感器，32…角速度传感器，33…障碍物传感器，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，将右车轮的转速设为rr，并将左车轮的转速设为rl，则朝向变更的旋转量(若以右旋转为正)成为360°×d×(rl-rr)/(2×i)。通过将该平移移动量、旋转量依次进行相加，从而驱动部42作为所谓的里程计(odometry)而发挥作用，能够测量自身设备位置(以移动开始时的位置以及朝向为基准的位置以及朝向)。

从里程计得到的自身设备的位置以及朝向的精度由于车轮的摩耗、打滑等而成为低精度的情况较多。特别是由于误差逐渐累积的关系，精度随着时间经过而恶化。不过，关于里程计的旋转分量(朝向的信息)，通过使用后述的角速度传感器信息，能够提高精度。此外，通过使用检测地磁来确定方位的方位传感器(未图示)，也能够与来自里程计的取得值无关地取得利用了地磁的绝对朝向的信息。

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

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

控制部10由cpu(centralprocessingunit，中央处理单元)等构成，通过执行存储在存储部20中的程序，从而实现后述的各部(移动处理部11，地图制作部12，位置估算部13，估算判定部14，移动处理控制部15)的功能。

存储部20由rom(readonlymemory，只读存储器)、ram(randomaccessmemory，随机存取存储器)等构成，包含图像存储部21、地图存储部22、位置变化历史记录存储部23、位置历史记录存储部24。在rom中，存储有控制部10的cpu所执行的程序(例如，后述的slam法的运算、自主移动控制处理所涉及的程序)、执行程序上预先需要的数据。在ram中，存储在程序执行中被生成或者变更的数据。

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

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

在位置变化历史记录存储部23中，存储由移动处理部11使自身设备的位置怎样发生了变化的历史记录。

在位置历史记录存储部24中，存储由位置估算部13估算出的自身设备位置的历史记录。

作为传感器部30，具备加速度传感器31、角速度传感器32、障碍物传感器33。加速度传感器31是测量xyz方向(3轴)的加速度的传感器。通过对这些值进行时间积分，从而能够求取xyz的各方向的速度，进而通过对这些速度进行时间积分，从而能够求取xyz的各方向的位置的变化量(移动距离)(但是由于对传感器值进行二次积分，因而累积误差变得相当大)。

角速度传感器32是测量角速度(每单位时间的角度移动量)的传感器。已知与基于车轮的转速来求取自身设备的朝向相比，使用角速度传感器32来求取朝向更能提高精度。障碍物传感器33是探测行进过程中的障碍物的传感器，例如是红外线传感器、超声波传感器。另外，也可以不搭载独立的障碍物传感器33，使用摄像部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来使自身设备移动。

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

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

估算判定部14判定位置估算部13是否能够基于当前已取得的信息来估算自身设备位置。更详细而言，若已取得估算自身设备位置所需的信息，并且基于这些信息的自身设备位置的估算结果唯一确定则判定为能够估算。反之，在未能取得估算自身设备位置所需的信息或者即使能够取得但是基于这些信息的自身设备位置的估算结果不定(产生多个估算候补等)的情况下，判定为不能估算。

移动处理控制部15在估算判定部14判定为不能估算的情况下，控制移动处理部11以使自身设备位置自主地移动到能够估算的位置。

图3是自主移动装置100的主流程。控制部10起动如下各线程，即，自身设备位置估算线程(步骤s101)、地图制作线程(步骤s102)、闭环线程(步骤s103)。通过自身设备位置估算线程、地图制作线程进行动作，从而基于slam法，开始地图的信息以及视觉里程计(使用地图和图像而估算出的自身设备位置的信息)的生成。然后，控制部10将位置变化历史记录计数器dc(表示向位置变化历史记录存储部23的位置变化历史记录的登记数的变量)进行清零(步骤s104)。然后，控制部10判定是否动作结束(步骤s105)，若动作结束(步骤s105：“是”)则结束动作，若动作未结束(步骤s105：“否”)则地图制作部12进行地图的信息的制作/更新(步骤s106)。

接着，控制部10判定估算状态变量es(表示是否为能够进行位置估算的状态的变量)的值是否为“不能进行位置估算”(步骤s107)。若是不能进行位置估算(步骤s107：“是”)，则进行用于自主地移动到能够进行位置估算的位置的处理即“移动处理控制”之后(步骤s110)返回至步骤s105。该“移动处理控制”实现了本实施方式的特征性的功能，关于其详情在后面叙述。

若估算状态变量es的值不是“不能进行位置估算”(步骤s107：“否”)则使位置变化历史记录计数器dc递增(步骤s108)，为了自主移动而由移动处理部11向驱动部42指示所希望的动作，并且将伴随该动作的位置的变化量与时间戳(使用当前时刻等能够与位置历史记录存储部24的时间戳进行比较的数据)一起登记到位置变化历史记录存储部23的第dc个项目(步骤s109)，返回至步骤s105。在该步骤s109中的位置的变化量的登记处理中，移动处理部11相当于位置变化历史记录登记部。

在此，位置的变化量，是从在步骤s109中移动处理部11向驱动部42发出指示之前的位置起，到驱动部42按照指示进行动作从而自主移动装置100移动之后的位置为止的(步骤s109中的移动处理的一次动作的)变化量。该变化量既可以使用里程计信息来取得，也可以使用加速度传感器31或角速度传感器32的值来取得。通常使用里程计信息更能够取得高精度的位置变化量，但在通过人的手而被移动的情况等下，里程计信息没有用处(在该情况下加速度传感器31或角速度传感器32的值有用)。因此，期望通过使用里程计信息和传感器信息这两者，从而通过传感器信息来补偿在车轮打滑或者通过人的手而被移动的情况等下产生的里程计信息的误差。

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

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

首先使用图4来说明自身设备位置估算线程的初始化处理。位置估算部13将“初始化中”设置给估算状态变量es，并清除位置历史记录登记计数器hc(表示向位置历史记录存储部24的位置历史记录的登记数的变量)和位置历史记录检索计数器sc(从位置历史记录存储部24取得位置历史记录时使用的变量)(步骤s201)。接着将-1设置给帧计数器n(表示摄像部41的图像取得数的变量)(步骤s202)。接下来位置估算部13判定是否动作结束(步骤s203)。若动作结束(步骤s203：“是”)则结束，若动作未结束(步骤s203：“否”)，则由摄像部41取得图像(步骤s204)。图像例如能够以30fps取得(所取得的图像也被称为帧)。

接着，从所取得的图像内取得2d特征点(步骤s205)。所谓2d特征点，是图像中的边缘部分等图像内的特征性的部分，能够使用sift(scale-invariantfuturetransform，尺度不变特征转换)、surf(speed-uprobustfeatures，加速稳健特征)等算法来取得。另外，为了取得2d特征点，也可以使用其他算法。

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

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

若帧计数器n不为0(步骤s208：“否”)则可知已经取得了两个图像，因此位置估算部13在这两个图像间取得2d特征点的对应(实际环境上的同一点存在于各个图像中，能取得其对应)(步骤s209)。在该步骤s209中，位置估算部13相当于对应特征点取得部。由于若在此取得的特征点的对应数小于5，则不能进行后述的两个图像间的姿势的估算，因此估算判定部14判定特征点的对应数是否小于5(步骤s210)。若小于5(步骤s210：“是”)则为了重新取得初始图像，返回至步骤s202。若特征点的对应数为5点以上(步骤s210：“否”)，则通过使用two-viewstructurefrommotion法，从而能够估算两个图像间的姿势(取得各个图像的位置的差分(平移向量t)以及朝向的差分(旋转矩阵r))(步骤s211)。

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

另外，即使特征点的对应数为5点以上，也存在两个图像间的姿势的估算值不唯一确定的情况(存在条件不足而不确定的情况、即使想要通过最小平方法来唯一确定但似乎可靠的估算值不唯一确定而成为多个的情况等)。在位置估算部13估算两个图像间的姿势(取得各个图像的位置的差分(平移向量t)以及朝向的差分(旋转矩阵r))时，在估算值唯一确定的情况下，估算判定部14判定为能够估算，在估算值不唯一确定的情况下，估算判定部14判定为不能估算(步骤s212)。

在估算判定部14判定为不能估算的情况下(步骤s212：“否”)，返回至步骤s202，从初始图像的取得开始重新进行处理。在估算判定部14判定为能够估算的情况下(步骤s212：“是”)，也就是说已经求出两个图像间的姿势(平移向量t以及旋转矩阵r)，因此前进至步骤s213。

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

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

(uv1)’～p(xyz1)’…(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特征点当中的一个映射于第一个图像的帧坐标(u1，v1)和第二个图像的帧坐标(u2，v2)，则能够得到以下的算式(3)以及算式(4)。在此，i表示单位矩阵，0表示零向量，(l|r)表示在矩阵l的右侧排列列向量r而得到的矩阵。

(u1v11)’～a(i|0)(xyz1)’…(3)

(u2v21)’～a(r|t)(xyz1)’…(4)

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

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

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

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

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

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

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

然后，位置估算部13为了得到slam坐标与实际环境坐标的比例(scale)对应，通过将基于里程计的平移距离(以实际环境下的坐标来求取)除以通过上述处理而估算的slam坐标下的平移距离d，来求取比例s(步骤s219)。

以上，自身设备位置估算线程的初始化处理完成，因此进入到图5的自身设备位置估算(主处理)(步骤s220)。

接下来使用图5对自身设备位置估算线程的主处理进行说明。首先，位置估算部13将“能够进行位置估算”设置给估算状态变量es(步骤s231)。这是因为在初始化处理中，实现了自身设备位置的估算，在此时点可以说是处于能够进行位置估算的状态。接下来位置估算部13判定是否动作结束(步骤s232)。若动作结束(步骤s232：“是”)则结束，若动作未结束(步骤s232：“否”)，则由摄像部41取得图像(步骤s233)，并使帧计数器n递增(步骤s234)。

接着，位置估算部13取得所拍摄到的图像中包含的2d特征点(步骤s235)。然后，从登记在帧数据库中的以前的关键帧(例如关键帧编号为nkf的图像)的信息中，取得该图像的信息中所包含的2d特征点当中的3d位置已知(成为登记在map点数据库中的map点)的2d特征点，提取与当前拍摄到的图像之间能取得对应的2d特征点(对应特征点)(步骤s236)。

然后，估算判定部14判定对应特征点的个数是否小于规定值(例如5)(步骤s237)，在小于规定值的情况下(步骤s237：“是”)，不能实现似乎可靠的自身设备位置的估算，因此前进至步骤s247。在此，也可以不是立刻前进至步骤s247，而是返回至步骤s236，从登记在帧数据库中的关键帧当中检索对应特征点的个数为规定值以上的关键帧。在该情况下，在登记在帧数据库中的关键帧当中未找到对应特征点的个数为规定值以上的关键帧的情况下前进至步骤s247。

若能够提取规定值以上的对应特征点(步骤s237：“否”)，则位置估算部13从map点数据库中取得对应特征点各自的3d位置(xi，yi，zi)(步骤s238)。若将当前拍摄到的图像中包含的对应特征点的帧坐标设为(ui，vi)，将该对应特征点的3d位置设为(xi，yi，zi)(i取从1到对应特征点数的值)，则将各对应特征点的3d位置(xi，yi，zi)通过以下的算式(5)而投影到帧坐标系的值(uxi，vxi)与帧坐标(ui，vi)理想而言应该是一致的。

(uxivxi1)’～a(r|t)(xiyizi1)’…(5)

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

【数学式1】

在该自身设备位置的估算中，存在自身设备的姿势的估算值不唯一确定的情况(存在条件不足而不确定的情况(例如两个图像中的对应特征点数小于5点的情况)、即使想要通过最小平方法来唯一确定但似乎可靠的估算值也不唯一确定而成为多个的情况(例如在算式(6)中，最小值大于规定值的情况，或极小值存在多个并且将最小值(最小的极小值)除以第二小的极小值而得到的值大于规定值的情况)等)。在位置估算部13估算自身设备的姿势(平移向量t以及旋转矩阵r)时，在估算值唯一确定的情况下，估算判定部14判定为能够估算，在估算值不唯一确定的情况下，估算判定部14判定为不能估算(步骤s240)。

在估算判定部14判定为不能估算的情况下(步骤s240：“否”)，前进至步骤s247。在估算判定部14判定为能够估算的情况下(步骤s240：“是”)，也就是说已经求出自身设备的姿势(平移向量t以及旋转矩阵r)，因此前进至步骤s241。

由于求出slam坐标下的当前的自身设备的姿势(平移向量t以及旋转矩阵r)，因此控制部10使位置历史记录登记计数器hc递增，在位置历史记录存储部24的第hc个项目中，与时间戳(使用当前时刻等能够与位置变化历史记录存储部23的时间戳进行比较的数据)一起登记当前的自身设备的姿势(平移向量t以及旋转矩阵r)，并将位置历史记录登记计数器hc的值设置给位置历史记录检索计数器sc(步骤s241)。在该步骤s241中，控制部10相当于位置历史记录登记部。然后，位置估算部13通过对当前的自身设备的姿势乘以比例s，来求取vo(视觉里程计)(步骤s242)。vo能够作为实际环境下的自身设备的位置以及朝向来利用。

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

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

然后，位置估算部13为了使地图制作线程知道产生了新的关键帧，在地图制作线程的关键帧队列中设置关键帧计数器nkf(步骤s246)。然后，返回至步骤s232。另外，关键帧计数器nkf、比例s、map点db、帧db、位置历史记录存储部24、位置变化历史记录存储部23、位置历史记录登记计数器hc、位置历史记录检索计数器sc、位置变化历史记录计数器dc都存储在存储部20中，使得能够跨越线程来参照值。

接下来对步骤s247以后进行说明。这是不能进行自身设备位置的估算时的处理。首先，位置估算部13对估算状态变量es设置“不能进行位置估算”(步骤s247)。接着位置估算部13判定是否动作结束(步骤s248)。若动作结束(步骤s248：“是”)则结束，若动作未结束(步骤s248：“否”)，则由摄像部41取得图像(步骤s249)。

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

控制部10判定是否能够从帧db中发现与取得图像的类似度成为基准图像类似度以上的关键帧(步骤s251)。若未能发现(步骤s251：“否”)则返回至步骤s248。若能够发现(步骤s251：“是”)则分别对取得图像的2d特征点和所发现的类似图像的2d特征点进行取得(步骤s252)，并在这两个图像间取得2d特征点的对应(步骤s253)。

然后，通过与步骤s239同样的方法来估算自身设备的姿势(步骤s254)。此时，估算判定部14与步骤s240同样地判定是否能够估算(步骤s255)，若不能估算(步骤s255：“否”)则返回至步骤s248，若能够估算(步骤s255：“是”)，也就是说能够进行自身设备位置的估算，所以前进至步骤s231。

这样，自身设备位置估算线程的初始化结束后，在估算状态变量es中会设置“能够进行位置估算”或者“不能进行位置估算”中的任意一者，因此在图3所示的主流程中，若es的值为“能够进行位置估算”，则自主移动装置100进行所希望的动作，若es的值为“不能进行位置估算”，则能够求取能够进行位置估算那样的位置来进行移动处理控制。

接着，关于在自主移动装置100的主流程(图3)的步骤s102起动的地图制作线程，使用图6来进行说明。该线程由地图制作部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起动的闭环线程，使用图7来进行说明。在该线程中控制部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：“是”)，进行以下的处理。

控制部10参照帧db，从帧db中检索与当前关键帧(关键帧编号为lkf的关键帧)之间的“关键帧自身的特征”的类似度为基准图像类似度以上的关键帧(步骤s405)。

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

接下来关于自主移动装置100的主流程(图3)的步骤s110的处理即移动处理控制，使用图8来进行说明。首先，控制部10判定位置历史记录检索计数器sc的值是否为0(步骤s501)。若sc为0，则意味着针对位置历史记录存储部24中存储的全部位置已经尝试了回退处理(returnprocess)但并未变为能够进行位置估算的状态，在该情况下(步骤s501：“是”)为了在未能进行位置的估算的状态下进行自主动作而进行与主流程(图3)的步骤s108、s109同样的动作(步骤s502、s503)，并返回至主流程(图3)的步骤s105。

若sc不为0(步骤s501：“否”)，则取得存储在位置历史记录存储部24中的第sc个信息(自身设备的姿势(位置)的信息以及当时的时刻)(步骤s504)。所取得的信息当中，自身设备的姿势(位置)的信息保存在hp(对从位置历史记录存储部24中取得的自身设备的位置的信息进行保存的变量)中，时刻保存在ht(对从位置历史记录存储部24中取得的时刻进行保存的变量)中。然后，判定位置变化历史记录计数器dc的值是否为0(步骤s505)。若dc为0则意味着虽然尝试了位置变化历史记录存储部23中存储的全部的位置的变化但并未变为能够进行位置估算的状态，在该情况下(步骤s505：“是”)为了在未能进行位置的估算的状态下进行自主动作而进行与主流程(图3)的步骤s108、s109同样的动作(步骤s502、s503)，并返回至主流程(图3)的步骤s105。

若dc不为0(步骤s505：“否”)，则取得存储在位置变化历史记录存储部23中的第dc个信息(位置变化量以及当时的时刻)(步骤s506)。所取得的信息当中，位置变化量保存在dp(对从位置变化历史记录存储部23中取得的位置变化量进行保存的变量)中，时刻保存在dt(对从位置变化历史记录存储部23中取得的时刻进行保存的变量)中。然后，判定从位置历史记录存储部24中取得的时刻ht与从位置变化历史记录存储部23中取得的时刻dt相比是否为过去的时刻(是否为较小的值)(步骤s507)。

若ht与dt相比为过去(步骤s507：“是”)，则自身设备还未完全返回到从位置历史记录存储部24中取得的过去的hp的位置，所以前进至步骤s508。若ht和dt为相同时刻或dt一方为过去(步骤s507：“否”)则为了进行移动处理控制中的最后的移动处理而前进至步骤s510。

在步骤s508中，基于登记在位置变化历史记录存储部23中的位置变化量dp，由移动处理控制部15使移动处理部11进行消除该变化量的移动处理(步骤s508)。例如若dp为“向前方前进了10cm”，则此处的移动处理成为“向后方退回10cm”的处理。然后，使位置变化历史记录计数器dc递减(步骤s509)并返回至步骤s505。

步骤s510也与步骤s508同样地，基于登记在位置变化历史记录存储部23中的位置变化量dp，由移动处理控制部15使移动处理部11进行消除该变化量的移动处理(步骤s510)。通过该移动处理，从而(如果存储在位置变化历史记录存储部23中的信息、移动处理部11的移动距离没有误差)自主移动装置100应该会返回到过去实现了位置估算的位置(由hp所示的位置)，所以为了正确地进行自身设备位置估算线程中的自身设备位置估算，该最后的移动处理期望降低速度来进行。特别是在步骤s507的判定时若ht与dt的值相等，则通过该移动应该会返回到hp所示的位置，所以也可以在步骤s510与步骤s511之间(为了等待自身设备位置估算线程中的自身设备位置估算的进行)追加规定时间不移动而停止的处理。

接着，使位置变化历史记录计数器dc递减(步骤s511)，并使位置历史记录检索计数器sc递减(步骤s512)，返回至主流程的步骤s105。使位置历史记录检索计数器sc递减，是为了在即使通过本次的移动处理控制也未能在自身设备位置估算线程中实现自身设备位置估算的情况下，在下次的移动处理控制中，进一步返回到过去的位置来尝试自身设备位置估算。但是，若在到下次为止的期间自身设备位置估算成功，则在图5的步骤s241中位置历史记录登记计数器hc的值被设置到位置历史记录检索计数器sc。通过进行这样的移动处理控制，从而能够返回到过去实现了自身设备位置估算的位置，能够进行自身设备位置估算。

接下来，作为与上述不同的移动处理控制，对进行图9的流程图所示的移动处理控制的第二实施方式进行说明。第二实施方式除了移动处理控制的流程图以外都与第一实施方式相同。

首先，控制部10判定位置历史记录检索计数器sc的值是否为0(步骤s601)。若sc为0，则意味着针对位置历史记录存储部24中存储的全部位置已经尝试了回退处理但并未变为能够进行位置估算的状态，在该情况下(步骤s601：“是”)为了在未能进行位置的估算的状态下进行自主动作而进行与主流程(图3)的步骤s108、s109同样的动作(步骤s602、s603)，并返回至主流程(图3)的步骤s105。

若sc不为0(步骤s601：“否”)，则取得存储在位置历史记录存储部24中的第sc个信息(自身设备的姿势(平移向量t以及旋转矩阵r)的信息hp以及当时的时刻ht)(步骤s604)。然后，将位置变化历史记录计数器dc的值设置给位置变化历史记录工作计数器wc(在从位置变化历史记录存储部23中取得位置变化的历史记录时使用的变量)，并对位置变化历史记录堆栈(保存位置变化量的堆栈)进行初始化(步骤s605)。另外，堆栈为先入后出的数据结构。

然后，控制部10判定位置变化历史记录工作计数器wc是否为0(步骤s606)。若wc为0则意味着虽然尝试了位置变化历史记录存储部23中存储的全部的位置的变化但并未变为能够进行位置估算的状态，在该情况下(步骤s606：“是”)为了在未能进行位置的估算的状态下进行自主动作而进行与主流程(图3)的步骤s108、s109同样的动作(步骤s602、s603)，并返回至主流程(图3)的步骤s105。

若wc不为0(步骤s606：“否”)，则控制部10取得存储在位置变化历史记录存储部23中的第wc个信息(位置变化量dp以及当时的时刻dt)(步骤s607)。然后，判定从位置历史记录存储部24中取得的时刻ht与从位置变化历史记录存储部23中取得的时刻dt相比是否为过去的时刻(是否为较小的值)(步骤s608)。

若ht与dt相比为过去(步骤s608：“是”)，则自身设备还未完全返回到从位置历史记录存储部24中取得的过去的hp的位置，所以前进至步骤s609。若ht和dt为相同时刻或dt一方为过去(步骤s608：“否”)则为了根据位置变化历史记录来累计估算自身设备的位置而前进至步骤s611。

在步骤s609中，控制部10将位置变化量dp的值压入到位置变化历史记录堆栈中。然后，使位置变化历史记录工作计数器wc递减(步骤s610)并返回至步骤s606。由此，到目前为止的位置变化都被积累到位置变化历史记录堆栈中。

在步骤s611中，控制部10将从位置历史记录存储部24中取得的过去的hp的姿势(平移向量t以及旋转矩阵r)作为初始值而设置给mp(为了求取自身设备位置的估算值而使用的变量)，进行从位置变化历史记录堆栈中取出数据(dp)并加到mp上的处理，直到位置变化历史记录堆栈成为空为止。由此，在mp中，能够得到通过从过去的能够进行自身设备位置估算的状态时的位置hp起对位置变化历史记录dp进行累计而得到的当前的自身设备位置的估算值。在该处理中，控制部10相当于累计位置估算部。

接着，移动处理控制部15控制移动处理部11，使得从当前的自身设备位置的估算值mp，移动到过去的自身设备位置hp(步骤s612)。具体而言，进行控制，使得进行从hp的平移向量中减去mp的平移向量的量的平移移动，并进行对hp的旋转矩阵乘以mp的旋转矩阵的逆矩阵而得到的旋转矩阵所表示的旋转量的旋转动作。通过该移动处理控制，从而(如果存储在位置变化历史记录存储部23中的信息、移动处理部11的移动距离没有误差)自主移动装置100应该会返回到过去实现了位置估算的位置(由hp所示的位置)，所以也可以在步骤s612与步骤s613之间(为了等待自身设备位置估算线程中的自身设备位置估算的进行)追加规定时间不移动而停止的处理。

接着，使位置历史记录检索计数器sc递减(步骤s613)，并返回至主流程的步骤s105。使位置历史记录检索计数器sc递减，是为了在即使通过本次的移动处理控制也未能在自身设备位置估算线程中实现自身设备位置估算的情况下，在下次的移动处理控制中，进一步返回到过去的位置来尝试自身设备位置估算。但是，若在到下次为止的期间自身设备位置估算成功，则在图5的步骤s241中位置历史记录登记计数器hc的值被设置到位置历史记录检索计数器sc。通过进行这样的移动处理控制，从而能够返回到过去实现了自身设备位置估算的位置，能够进行自身设备位置估算。

第二实施方式若与第一实施方式相比较，则针对位置变化历史记录存储部23中所存储的位置变化并不是一个一个进行移动处理，而是汇总来进行移动处理，因此存在如下这样的优点，即，移动处理控制时的驱动部42的误差不容易积累。

至此说明的两个实施方式虽然都进行了返回到过去实现了自身设备位置估算的位置的处理，但是它们都存在如下这样的问题，即，若存储在位置变化历史记录存储部23中的信息或基于移动处理部11的移动距离不正确则不能返回到过去的正确位置。

因此，对不需要位置变化历史记录存储部23、位置历史记录存储部24的第三实施方式进行说明。图10中示出第三实施方式的构成，若与第一或第二实施方式相比较，则在以下方面不同，即，在控制部10中存在旋转拍摄部16，以及在存储部20中不存在位置变化历史记录存储部23、位置历史记录存储部24而存在特征点存储部25。

关于图3～图7的各流程，仅是省略与位置变化历史记录计数器dc、位置变化历史记录存储部23、位置历史记录登记计数器hc、位置历史记录检索计数器sc相关的处理，其余与第三实施方式的处理流程相同。移动处理控制的处理流程与第一、第二实施方式较大不同，因此以下使用图11来进行说明。

首先，旋转拍摄部16对旋转拍摄标志rf(表示当前是否是旋转拍摄的处理中的变量)设置1(步骤s701)，并对累积旋转角φs进行清零(步骤s702)。接着，由摄像部41取得图像(步骤s703)。接着，从所取得的图像内取得2d特征点(步骤s704)。然后，将所取得的2d特征点的个数和累积旋转角φs登记到特征点存储部25中(步骤s705)。

接着，旋转拍摄部16判定旋转拍摄标志rf是否为2(步骤s706)。若rf为2(步骤s706：“是”)则意味着1圈旋转的拍摄完成，所以前进至步骤s712。若rf不为2(步骤s706：“否”)，则判定累积旋转角φs+旋转角φ1是否小于360°(步骤s707)。在此，旋转角φ1是基于旋转拍摄部16的一次旋转动作的旋转角，例如是30°。φ1期望设定摄像部41的视角的一半以下的角度，使得能够从周围的环境拍摄尽可能多的特征点。

旋转拍摄部16在累积旋转角φs+旋转角φ1小于360°的情况下(步骤s707：“是”)，对驱动部42发出指示以进行旋转角φ1的旋转动作(步骤s708)，在累积旋转角φs上加上旋转角φ1(步骤s709)，之后返回至步骤s703。此处的旋转动作若是能够通过使驱动部42的两个车轮反向等速旋转等而不移动位置地旋转，则期望进行不移动位置的旋转动作。在只能使两个车轮向相同方向旋转的情况下，期望进行旋转半径尽可能小的旋转动作。

旋转拍摄部16在累积旋转角φs+旋转角φ1为360°以上的情况下(步骤s707：“否”)，对驱动部42发出指示以进行旋转角(360°-φs)的旋转动作(步骤s710)，并对旋转拍摄标志rf设置2(步骤s711)，之后返回至步骤s703。

在步骤s712中，移动处理控制部15从特征点存储部25中检索登记到特征点存储部25中的2d特征点的个数最多时的累积旋转角φs，并控制移动处理部11以进行旋转角φs的旋转动作(步骤s713)。然后，移动处理控制部15控制移动处理部11以慢慢地向前方移动规定距离(例如基准平移距离的3倍)(步骤s714)。通过慢慢地移动，从而能够增加在自身设备位置估算线程中能够估算自身设备位置的定时。

然后，旋转拍摄部16将旋转拍摄标志rf清零(步骤s715)，并返回至主流程的步骤s105。通过进行这样的动作，从而向2d特征点的个数多的方向进行移动，所以自身设备位置的估算成功的可能性得到提高。另外，在本实施方式中仅关注了2d特征点的个数，但也可以不仅关注个数还关注质量，利用2d特征点的个数和质量来定义评价函数，在步骤s712中进行对该2d特征点的评价函数最高的旋转角φs进行检索的处理。

此外，也可以适当切换进行第一、第二、第三的各实施方式的移动处理控制。例如，可以考虑如下的实施方式等：首先进行第一实施方式的移动处理控制，若即便如此也不能进行自身设备位置估算，则进行第三实施方式的移动处理控制的实施方式；首先进行第二实施方式的移动处理控制，若即便如此也不能进行自身设备位置估算，则进行第三实施方式的移动处理控制的实施方式；首先进行第一实施方式的移动处理控制，若即便如此也不能进行自身设备位置估算，则进行第二实施方式的移动处理控制，若即便如此也不能进行自身设备位置估算，则进行第三实施方式的移动处理控制的实施方式。

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

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

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

另外，本发明的自主移动装置100的各功能通过通常的pc(personalcomputer，个人计算机)等计算机也能够实施。具体而言，在上述实施方式中，自主移动装置100所进行的自主移动控制处理的程序作为预先存储在存储部20的rom中的程序来进行了说明。但是，也可以将程序保存在软盘、cd-rom(compactdiscreadonlymemory，光盘只读存储器)，dvd(digitalversatiledisc，数字多用途盘)以及mo(magneto-opticaldisc，磁光盘)等计算机可读取的记录介质中来进行分发，通过将该程序读入到计算机中进行安装，从而构成能够实现上述的各功能的计算机。

此外，该计算机既可以内置于自主移动装置100，也可以与自主移动装置100分开存在。即，也可以如图12所示，将控制部、存储部设置于外部的服务器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具备各部是任意的。

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