CN107194970B - 自主移动装置、自主移动方法以及程序存储介质 - Google Patents

自主移动装置、自主移动方法以及程序存储介质 Download PDF

Info

Publication number
CN107194970B
CN107194970B CN201710040227.2A CN201710040227A CN107194970B CN 107194970 B CN107194970 B CN 107194970B CN 201710040227 A CN201710040227 A CN 201710040227A CN 107194970 B CN107194970 B CN 107194970B
Authority
CN
China
Prior art keywords
feature point
unit
movement
acquired
destination
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710040227.2A
Other languages
English (en)
Other versions
CN107194970A (zh
Inventor
前野泰士
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Casio Computer Co Ltd
Original Assignee
Casio Computer Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Casio Computer Co Ltd filed Critical Casio Computer Co Ltd
Publication of CN107194970A publication Critical patent/CN107194970A/zh
Application granted granted Critical
Publication of CN107194970B publication Critical patent/CN107194970B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/0088—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20076—Probabilistic image processing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Artificial Intelligence (AREA)
  • Medical Informatics (AREA)
  • Health & Medical Sciences (AREA)
  • Game Theory and Decision Science (AREA)
  • Business, Economics & Management (AREA)
  • Evolutionary Computation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供自主移动装置、自主移动方法以及程序存储介质。即使不用检测环也能使误差减少。自主移动装置(100)的移动方向速度决定部(16)基于摄像部所拍摄的多个图像的信息来决定自主的移动的方向以及速度。特征点取得部(11)从摄像部所拍摄的图像取得特征点。地图存储部(22)存储特征点取得部所取得的特征点的位置。特征点对应数取得部(14)取得特征点取得部所取得的特征点与在地图存储部存储位置的特征点的对应数。移动方向速度变更部(15)，在特征点对应数取得部所取得的特征点对应数阈值以下的情况下，为了使特征点对应数增加，变更移动方向速度决定部(16)所决定的移动方向以及移动速度。

Description

自主移动装置、自主移动方法以及程序存储介质
技术领域
本发明涉及自主移动装置、自主移动方法以及程序存储介质。
背景技术
根据用途而自主进行移动的自主移动装置不断普及。例如已知为了室内的清扫而自主进行移动的自主移动装置。一般，这样的自主移动装置需要进行实空间的地图的作成和实空间内的自机位置的估计。
作为用于作成实空间的地图的手法，例如已知SLAM(Simultaneous LocalizationAnd Mapping，即时定位与地图构建)法。利用单镜头摄像机的SLAM技术的基本的原理记载于非专利文献1以及非专利文献2中，通过从摄像机所拍摄的动态图像的多帧追踪同一特征点，来进行交替估计自机的三维位置(摄像机位置)和特征点的三维位置(其汇集而构成地图的信息)的处理。
该方法由于交替估计自机位置和特征点的三维位置，因此会发生累积误差，有时在进行环状的移动等而回到初始的位置的情况下也不能认识到已经回到了初始的位置的情况。作为修正这样的累积误差的方法，有合环处理(环解决处理)这样的技术，在非专利文献3公开。
在该技术中，若自机检测到环(检测到拍摄的帧与过去拍摄帧类似的情况)，则将过去的该位置视作正确的位置，来修正从那里起到当前位置的路径中的位置，由此进行减少误差的合环处理。
现有技术文献
非专利文献
非专利文献1：Andrew J.Davison，“Real-Time Simultaneous Localization andMapping with a Single Camera”，Proceedings of the 9th IEEE InternationalConference on Computer Vision Volume 2，2003，pP.1403-1410
非专利文献2：Richard Hartley，Andrew Zisserman，“Multiple View Geometryin ComputerVision”，Second Edition，Cambridge.
University Press，March 2004，chapter 9
非专利文献3：奈良先端科学技術大学院大学情報科学研究科准教授佐藤智和、“SSII2015チユ一トリアル特徴点追跡による勤画像からの逐次三次元復元とその応用-座標系の基礎から応用事例·最新研究勤向まで-”、[online]、2015年6月1O日、画像センシングシンポジウムチュㅡトリアル講演会、[2016年2月22日检索]、因特网＜URL：http：//yokoya.naist.jp/paper/datas/1407/SSII％E3％83％81％E3％83％A5％E3％83％BC％E3％83％88％E3％83％AA％E3％82％A2％E3％83％AB％E4％BD％90％E8％97％A4.pdf>
但在现有技术中，由于不检测环就不能进行合环处理，因此使误差减少的机会受到限制。
发明内容
本发明为了解决上述问题而提出，目的在于，提供不检测环也能减少误差的自主移动装置等。
为了达成上述目的，本发明的自主移动装置特征在于，具备：基于摄像部所拍摄的多个图像的信息来对自主的移动进行控制的移动控制部；从所述摄像部所拍摄的图像取得特征点的特征点取得部；存储所述特征点取得部所取得的特征点的位置的地图存储部；和取得所述特征点取得部所取得的特征点与在所述地图存储部存储有位置的特征点的对应数即特征点对应数的特征点对应数取得部，所述移动控制部按照所述特征点对应数取得部所取得的特征点对应数来对移动进行控制。
发明的效果
根据本发明，不用检测环也能减少误差。
附图说明
图1是表示实施方式所涉及的自主移动装置的外观的图。
图2是表示实施方式所涉及的自主移动装置的构成的图。
图3是表示实施方式所涉及的自主移动控制处理整体的流程图的图。
图4A是表示实施方式所涉及的自主移动控制处理中的自机位置估计线程的处理的第1个流程图的图。
图4B是表示实施方式所涉及的自主移动控制处理中的自机位置估计线程的处理的第2个流程图的图。
图5是表示实施方式所涉及的自主移动控制处理中的地图作成线程的处理的流程图的图。
图6是表示实施方式所涉及的自主移动控制处理中的合环线程的处理的流程图的图。
图7是表示实施方式所涉及的自主移动控制处理中的移动控制的流程图的图。
图8是表示实施方式所涉及的自主移动装置的移动的具体例的图。
图9是表示将控制部等的一部分设置在外部的服务器的构成的一例的图。
标号的说明
100 自主移动装置
10、50 控制部
11 特征点取得部
12 地图作成部
13 位置估计部
14 特征点对应数取得部
15 移动方向速度变更部
16 移动方向速度决定部
20、60 存储部
21 图像存储部
22 地图存储部
23 特征点对应数存储部
24 移动履历存储部
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，将右车轮的旋转数设为RR，将左车轮的旋转数设为RL，则朝向变更的旋转量成为(若将右旋转设为)360°×D×(RL-RR)/(2×I)。通过将该并进移动量、旋转量逐次加在一起，驱动部42作为所谓的里程计发挥功能，能测量自机位置(以移动开始时的位置以及朝向为基准的位置以及朝向)。
另外，也可以取代车轮而具备履带，也可以具备多个(例如2条)腿而用腿行走，由此进行移动。在这些情况下，也能基于2个履带的活动、腿的活动，与车轮的情况同样地测量自机的位置、朝向。
如图2所示那样，自主移动装置100除了具备摄像部41、驱动部42以外，还具备控制部10、存储部20、传感器部30、输入部43、通信部44、电源45。
控制部10由CPU(Central Processing Unit，中央处理器)等构成，通过执行存储于存储部20的程序来实现后述的各部(特征点取得部11、地图作成部12、位置估计部13、特征点对应数取得部14、移动方向速度变更部15、移动方向速度决定部16)的功能。另外，控制部10具备计时器(未图示)，能对经过时间进行计数。
存储部20由ROM(Read Only Memory，只读存储器)、RAM(Random Access Memory，随机存取存储器)等构成，包含图像存储部21、地图存储部22、特征点对应数存储部23、移动履历存储部24。在ROM中存储控制部10的CPU所执行的程序(例如后述的SLAM法的运算或自主移动控制处理所涉及的程序)、执行程序上所需的数据。在RAM中存储在程序执行中作成或变更的数据。
在图像存储部21中存储摄像部41所拍摄的图像。其中为了存储容量的节约，可以不存储拍摄的全部图像，另外也可以不是存储图像自身，而是存储图像的特征量。关于重要的图像(后述的关键帧)，和图像的信息一起存储拍摄该图像时的自机位置(自机的位置以及朝向)的信息。
在地图存储部22中存储基于后述的SLAM法或来自障碍物传感器31的信息由地图作成部12作成的地图(特征点或障碍物的三维位置的信息)。
在特征点对应数存储部23中，在后述的自机位置估计线程中存储摄像部41所拍摄的图像的特征点与存储于地图存储部22的Map点的对应数。
在移动履历存储部24中存储移动方向以及移动距离的履历。也可以取代存储移动方向以及移动距离而存储自机的三维位置以及朝向的履历。移动履历存储部24成为后进先出的堆栈的存储部。在此，存储的履历的数量相当于移动履历存储部24的堆栈指针的值。
作为传感器部30而具备障碍物传感器31。障碍物传感器31是探测行驶中的障碍物的传感器，例如是红外线传感器、超声波传感器。另外，也可以不搭载独立的障碍物传感器31，而是使用摄像部41探测障碍物。另外，也可以具备探测与其他物体碰撞的保险杆传感器(未图示)、检测动作的角速度传感器或加速度传感器(均未图示)等。
作为输入部43，具备用于操作自主移动装置100的操作按钮。操作按钮例如包含电源按钮、模式切换按钮(切换清扫模式、宠物模式等)、初始化按钮(重新进行地图的作成)等。作为输入部43，可以具备进行声音的输入的麦克风(未图示)和认识给自主移动装置100的操作指示的声音的声音认识部。
通信部44是用于与外部装置进行通信的模块，在与外部装置进行无线通信的情况下是包含天线的无线模块。例如通信部44是用于进行基于Bluetooth(注册商标)的近距离无线通信的无线模块。在通信部44中，进行自主移动装置100与外部的数据的收发。在对自主移动装置100指示目的地时，经由该通信部44来传递目的地的信息。
电源45是使自主移动装置100动作的电源，一般是内置的充电电池，但也可以是太阳能电池，也可以是从地面无线进行电力提供的系统。在电源45是充电电池的情况下，通过自主移动装置100与充电站(基地)对接来进行充电。
接下来说明控制部10的功能。控制部10包含特征点取得部11、地图作成部12、位置估计部13、特征点对应数取得部14、移动方向速度变更部15、移动方向速度决定部16，进行后述的SLAM法的运算、自主移动装置100的移动控制等。另外，控制部10与多线程功能对应，能并行推进多个线程(不同的处理的流程)。
特征点取得部11取得摄像部41所拍摄的图像中的2D特征点。所谓2D特征点，是图像中的边缘部分等图像内的特征性的部分，能使用SIFT(Scale-Invariant FutureTransform，尺度不变特征转换)或SURF(Speed-Up Robust Features，加速鲁棒特征)等算法取得。
地图作成部12将基于存储于图像存储部21的图像的信息和该图像拍摄时的自机的位置以及朝向的信息使用SLAM法估计出的特征点(Map点)的三维位置、基于由障碍物传感器31探测到障碍物时自机的位置以及朝向的信息而得到的该障碍物的三维位置等作为地图的信息存储在地图存储部22中。
位置估计部13基于后述的SLAM法，作为视觉里程计来估计自机的位置以及朝向。
特征点对应数取得部14，取得特征点取得部11所取得的特征点与存储于地图存储部22的Map点的对应数。
移动方向速度变更部15基于由特征点对应数取得部14取得的特征点对应数来降低自机的移动速度或停止，或者变更移动方向。由此增加了特征点对应数。
移动方向速度决定部16决定自机的移动速度、移动方向。为了基于在此决定的移动速度、移动方向进行移动，控制部10对驱动部42进行控制。另外，移动方向速度变更部15以及移动方向速度决定部16相当于移动控制部。
参考图3来说明主移动装置100的自主移动控制处理的主流程。控制部10首先对移动状态变量设置初始值即「通常」，对前次履历数设置初始值即0(步骤S101)。移动状态变量和前次履历数都是在步骤S107的移动控制中使用的变量，详细在移动控制的说明中叙述。接下来，控制部10启动自机位置估计线程(步骤S102)。然后启动地图作成线程(步骤S103)。接下来启动合环线程(步骤S104)。通过自机位置估计线程、地图作成线程动作，基于SLAM法开始地图的信息以及视觉里程计(使用地图和图像而估计的自机位置的信息)的生成。之后，控制部10判定是否动作结束(步骤S105)，若是动作结束(步骤S105；“是”)，则结束动作，若不是动作结束(步骤S105；“否”)，则利用障碍物传感器31和视觉里程计进行地图的更新(步骤S106)。
接下来，控制部10为了进行自主移动而进行对驱动部42发出动作指示的移动控制(步骤S107)，回到步骤S105。在此的自主移动还有为了作成地图而在房间中到处活动这样的移动的情况，还有接受到向目的地的移动指示的情况等向指示的目的地的沿着最短路径的移动的情况。另外，还有通过不指示明确的目的地而指示任务(地图的再作成、清扫、宠物等)而时刻基于周围的状况和所指示的任务来设定目的地的情况。
通过该主流程的处理，能基于地图的信息自主进行移动并适宜更新地图的信息。作为典型的示例，自主移动装置100若在最初置于充电站的状态下接通电源，则凭借障碍物传感器31在家的各房间到处移动，通过障碍物传感器31来确定墙壁等的障碍物位置，能作成包含障碍物位置的地图的信息。若某种程度作成地图，则能获知还没有地图的信息但认为能移动的区域，还能自主移动到该区域等来促使更大范围的地图的作成。然后，若作成能移动的几乎全域的地图的信息，就能进行利用地图的信息的有效率的移动动作。例如不管从房间的哪个位置都能以最短路径返回充电站，或者能有效率地进行房间的清扫。
参考图4A以及图4B来说明在自主移动装置100的主流程(图3)的步骤S102启动的自机位置估计线程。该线程是位置估计部13最初进行初始化处理、之后持续自机位置估计(使用由摄像部41取得的图像并用视觉里程计估计自机位置)的处理。
位置估计部13首先对估计状态变量设置「初始化中」(步骤S201)。估计状态变量表示该时间点的自机位置估计处理的状态，取「初始化中」、「追踪成功」、「追踪失败」这3个值。接下来判定是否是动作结束(步骤S202)。若是动作结束(步骤S202；“是”)，则结束，若不是动作结束(步骤S202；“否”)，则判定估计状态变量是否是「初始化中」(步骤S203)。若不是初始化中(步骤S203；“否”)，则进行步骤S221以后的自机位置估计处理，若是初始化中(步骤S203；“是”)，则前进到步骤S204来进行初始化处理。首先说明初始化处理。
在初始化处理中，位置估计部13首先对帧计数器N设置-1(步骤S204)，在摄像部41取得图像(步骤S205)。图像例如能以30fps取得(取得的图像也被称作帧)。接下来，特征点取得部11取得在摄像部41取得的图像中所含的2D特征点(步骤S206)。
由于若取得的2D特征点的个数少，就不能用后述的Two-view Structure fromMotion(双视图运动恢复结构)法进行计算，因此位置估计部13将2D特征点的取得数与基准值(例如10个)进行比较(步骤S207)，在不足基准值的情况下(步骤S207；“否”)返回步骤S205，直到取得基准值以上的2D特征点数为止，重复图像的取得和2D特征点的取得。另外，虽然在该时间点尚未作成地图的信息，但例如在上述的典型的示例中，由于凭借障碍物传感器31开始在家的各房间到处移动，因此只要在该初始化处理中重复图像取得和2D特征点取得，就会一边移动一边重复图像取得。因此能取得各种图像，总之能期待能取得2D特征点数多的图像。
在2D特征点的取得数为基准值以上的情况下(步骤S207；“是”)，位置估计部13使帧计数器N递增(步骤S208)。然后判定帧计数器N是否是0(步骤S209)。若帧计数器N是0(步骤S209；“是”)，则意味着刚刚仅取得1副图像，因此为了取得第2副图像而返回步骤S205。另外虽然未在图4A的流程图中记载，但取得第1副图像时的自机的位置与取得第2副图像时的自机的位置有某种程度的分离有助于在这以后的处理提升估计的姿态的精度。为此也可以在从步骤S209返回步骤S205时，在等待通过主流程(图3)的步骤S107中的移动控制而以里程计得到的并进距离成为给定的距离(例如1m)以上之后再返回步骤S205。
若帧计数器N不是0(步骤S209；“否”)，则意味着取得2个图像，因此位置估计部13在这2个图像间取得2D特征点的对应(实际环境上的同一点存在于各个图像中，取该对应)(步骤S210)。在此，由于若特征点的对应数不足5就不能进行后述的2个图像间的姿态的估计，因此位置估计部13判定特征点的对应数是否不足5(步骤S211)。如不足5(步骤S211；“是”)，则为了重新取得初始图像而返回步骤S204。若特征点的对应数为5点以上(步骤S211；“否”)，则能使用后述的Two-view Structure from Motion(双视图运动恢复结构)法来估计2个图像间的姿态(取得各个图像的位置的差分(并进矢量t)以及朝向的差分(旋转矩阵R))(步骤S212)。
该Two-view Structure from Motion(双视图运动恢复结构)法的姿态的估计，具体从对应的特征点求取基本矩阵E，将基本矩阵E分解为并进矢量t和旋转矩阵R，由此进行(详细参考非专利文献2)。另外，由于在此得到的并进矢量t(若设想在三维空间内移动，则以取得最初的图像的位置为原点具有X、Y、Z这3个要素)的各要素的值与实际环境上的值不同(在Two-view Structure from Motion(双视图运动恢复结构)法中不能得到实际环境上的值自身，得到与实际环境相似的空间上的值)，因此将它们视作SLAM空间上的值，以下使用SLAM空间上的坐标(SLAM坐标)来进行说明。
若求得2个图像间的姿态(并进矢量t以及旋转矩阵R)，则该值成为以最初的图像为基准(以取得最初的图像的位置为SLAM坐标的原点、并进矢量是0矢量、旋转矩阵为单位矩阵I)的情况下的第2副图像的姿态(取得第2个图像时的自机的位置(并进矢量t)以及朝向(旋转矩阵R))。在此，在求取2个图像各自的姿态(该图像(帧)拍摄时的自机的位置(并进矢量t)以及朝向(旋转矩阵R)，也称作帧姿态)的情况下，基于以下的思路由地图作成部12求取在这2个图像间取对应的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其就成为常数。
若在2个图像间取对应的2D特征点当中的一者映现在第1个图像的帧坐标(u1，v1)和第2个图像的帧坐标(u2，v2)，则能做出以下的式(3)以及式(4)。在此，I表征单位矩阵，0表征零矢量，(L|r)表征在矩阵L的右边排列了列矢量r的矩阵。
(u1 v1 1)’～A(I|0)(X Y Z 1)’...(3)
(u2 v2 1)’～A(R|t)(X Y Z 1)’...(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。
若求得2D特征点的SLAM坐标中的3D位置(X，Y，Z)，则将其作为Map点，由地图作成部12登录到Map点数据库(也称作Map点DB(Database，数据库)，容纳在地图存储部22中)(步骤S214)。作为登录在Map点数据库的要素，至少需要「2D特征点的SLAM坐标中的3D位置即X、Y、Z」和「该2D特征点的特征量」(例如以SIFT等得到的特征量)。
然后，地图作成部12判定是否将在2个图像间取对应的2D特征点(对应特征点)的全部登录在Map点数据库(步骤S215)，若尚未全部登录(步骤S215；“否”)，则返回步骤S213，若全部登录了(步骤S215；“是”)，则前进到步骤S216。
然后，位置估计部13将NKF(表征关键帧(是指成为之后紧接的线程中的处理对象的图像)的计数器的变量)初始化为0(步骤S216)，将第2个图像作为关键帧登录到帧数据库(也称作帧DB，容纳在图像存储部21)(步骤S217)。
登录在帧数据库的要素是「关键帧编号」(在登录时间点的关键帧计数器NKF的值)、「姿态」(该图像拍摄时的自机在SLAM坐标内的位置(并进矢量t)以及朝向(旋转矩阵R))、「提取的全部2D特征点」、「在全部2D特征点中作为Map点而3D位置已知的点」、「关键帧自身的特征」，但除此以外也可以还登录「以里程计测量的实际环境上的姿态」(基于实际环境中的驱动部42所实现的移动距离求得的自机的位置以及朝向)。
上述中，所谓「关键帧自身的特征」，是用于使求取关键帧间的图像类似度的处理效率化的数据，通常可以使用图像中的2D特征点的直方图等，但也可以将图像自身作为「关键帧自身的特征」。另外，「以里程计测量的实际环境上的姿态」还能以并进矢量t和旋转矩阵R表征，然而通常来说，由于本自主移动装置100在二维平面上上活动，因此可以简化为二维数据，表征为以移动开始时的位置(原点)以及朝向为基准的二维坐标(X，Y)以及朝向
Figure GDA0002637938370000121
接下来，位置估计部13为了使地图作成线程获知生成了关键帧这一情况，对地图作成线程的关键帧队列(队列(Queue)成为先进先出的数据结构)设置关键帧计数器NKF(步骤S218)。
以上完成自机位置估计线程的初始化处理，位置估计部13对估计状态变量设置「追踪成功」(步骤S219)，为了获得SLAM坐标与实际环境坐标的标量对应，将里程计测量的并进距离(在实际环境中的坐标求取)除以上述的处理中估计出的在SLAM坐标的并进距离d，由此求取标量S(步骤S220)。
然后经由步骤S202、步骤S203，前进到初始化完毕的情况下的处理的步骤S221。
对初始化完毕的情况下的处理进行说明。该处理是自机位置估计线程的通常时的处理，是位置估计部13逐次估计当前的自机的位置以及朝向(SLAM坐标内的并进矢量t和旋转矩阵R)的处理。
首先，位置估计部13用摄像部41拍摄图像(步骤S221)，使帧计数器N递增(步骤S222)。然后，特征点取得部11取得用摄像部41拍摄的图像中所含的2D特征点(步骤S223)。
接下来，位置估计部13判定估计状态变量是否是「追踪成功」(步骤S224)。若是「追踪成功」(步骤S224；“是”)，则特征点对应数取得部14从登录在帧数据库的1个之前的关键帧(关键帧编号为NKF的图像)的信息取得该图像的信息中所含的2D特征点当中3D位置为已知的(成为登录在Map点数据库的Map点)2D特征点，取得与这当中在步骤S223取得的特征点取对应的2D特征点(对应特征点)的个数(特征点对应数)，存储在特征点对应数存储部23(步骤S225)。
若估计状态变量的值不是「追踪成功」(步骤S224；“否”)，则特征点对应数取得部14从登录在帧数据库的2个以上之前的关键帧(关键帧编号不足NKF的图像)中取得Map点与在步骤S223取得的特征点的对应数成为最大的关键帧、和其特征点对应数，存储在特征点对应数存储部23(步骤S226)。
然后，位置估计部13判定特征点对应数是否大于阈值(例如30，以下称作「基准对应特征点数」)(步骤S227)，在为基准对应特征点数以下的情况下(步骤S227；“否”)，由于用SLAM法估计的姿态的精度变差，因此对估计状态变量设置「追踪失败」(步骤S230)，不进行位置的估计而返回步骤S202。
在特征点对应数大于基准对应特征点数的情况下(步骤S227；“是”)，则由特征点对应数取得部14取得与Map点取对应的在步骤S223取得的特征点(对应特征点)的个数(特征点对应数)，将其存储在特征点对应数存储部23(步骤S228)，其中该Map点包含于在步骤S225或步骤S226取得特征点对应数的关键帧的近旁的关键帧(与取得特征点对应数的关键帧存在给定的比例以上(例如30％以上)特征点的重叠的关键帧)中。
然后，位置估计部13判定特征点对应数是否大于阈值(例如50，以下称作「第2基准对应特征点数」)(步骤S229)，若为第2基准对应特征点数以下(步骤S229；“否”)，则对估计状态变量设置「追踪失败」(步骤S230)，不进行位置的估计而返回步骤S202。若特征点对应数大于第2基准对应特征点数(步骤S229；“是”)，则前进到步骤S231。
在步骤S231，位置估计部13对估计状态变量设置追踪成功(步骤S231)。位置估计部13从Map点数据库取得在步骤S228取得的对应特征点各自的3D位置(Xi，Yi，Zi)，使用其与在步骤S221取得的图像所含的对应特征点的对应关系，来估计当前的自机的姿态(以并进矢量t以及旋转矩阵R表征的自机的位置以及朝向)(步骤S232)。
对该自机的姿态的估计方法进行补足说明。若将在步骤S221取得的图像中所含的对应特征点的帧坐标设为(ui，vi)，将其对应特征点的3D位置设为(Xi，Yi，Zi)(i取1到特征点对应数的值)，在将各对应特征点的3D位置(Xi，Yi，Zi)通过以下的式(5)投影到帧坐标系得到的值(uxi，vxi)和帧坐标(ui，vi)在理想上应当一致。
(uxi vxi 1)’～A(R|t)(Xi Yi Zi 1)’...(5)
但由于实际上不管在(Xi，Yi，Zi)还是在(ui，vi)都包含误差，因此(uxi，vxi)和(ui，vi)难得一致。并且未知数仅是R和t(三维空间中分别为三维，3+3＝6是未知数的个数)，数式却存在对应特征点的个数的2倍(相对于对应特征点的1个而存在帧坐标的u、v各自对应的算式)，因此成为过剩条件的联立一次方程式，如上述那样用最小二乘法来求取。具体地，位置估计部13求取使以下的式(6)的成本函数E1最小化的姿态(并进矢量t以及旋转矩阵R)。其成为用SLAM法求得的SLAM坐标中的自机的姿态(以并进矢量t以及旋转矩阵R表征的自机的位置以及朝向)。如此，位置估计部13补给自机的姿态(步骤S232)。
【数学式1】
Figure GDA0002637938370000141
由于求得SLAM坐标中的前的自机的姿态(并进矢量t以及旋转矩阵R)，因此位置估计部13通过将其与标量S相乘来求取VO(视觉里程计)(步骤S233)。VO能作为实际环境中的自机的位置以及朝向而利用。
接下来，位置估计部13根据拍摄即将登录在帧DB之前的关键帧(关键帧编号为NKF的图像)时的自机的位置来判定是否移动了给定的距离(例如1m，以下称作「基准并进距离」)以上(步骤S234)，在若移动了给定的距离以上(步骤S234；“是”)，则使关键帧计数器NKF递增(步骤S235)，然后将当前帧作为关键帧登录在帧DB(步骤S236)。若仅移动了不足基准并进距离(步骤S234；“否”)，则返回步骤S202。
在此，与基准并进距离进行比较的自机的移动距离，可以从里程计取得从之前紧挨的关键帧到当前帧的并进距离(两帧的并进矢量的差的矢量的绝对值(要素的平方和的平方根))，也可以根据上述的VO求取。登录在帧DB的内容如上述那样是「关键帧编号」、「姿态」、「提取的全部2D特征点」、「在全部2D特征点中作为Map点而3D位置已知的点」、「关键帧自身的特征」。
然后，位置估计部13为了使地图作成线程获知产生了新的关键帧，对地图作成线程的关键帧队列设置关键帧计数器NKF(步骤S237)。然后返回步骤S202。另外，关键帧计数器NKF、标量S、Map点DB、帧DB、特征点对应数存储在存储部20，以能跨线程对值进行参考。
接下来，参考图5来说明在自主移动装置100的主流程(图3)的步骤S103启动的地图作成线程。在该线程中，地图作成部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位置进行光束平差处理(bundle adjustment)，来谋求精度提升(步骤S309)。然后，若进行光束平差处理的结果是找到误差大的Map点，则将其从Map点DB删除(步骤S310)，前进到步骤S311。若关键帧队列不是空(步骤S308；“否”)，则不进行任何处理地前进到步骤S311。
在步骤S311，地图作成部12对合环线程的关键帧队列设置MKF(步骤S311)，返回步骤S301。
另外，所谓光束平差处理，是同时估计摄像机姿态(关键帧姿态)和Map点的3D位置的非线性最优化法，是使在将Map点投影在关键帧上时产生的误差成为最小的进行最优化的处理。
通过进行该光束平差的处理，能谋求关键帧姿态和Map点的3D位置的精度提升。但不进行该处理也只是不能实现精度提升，并不会产生特别问题。因此在没有其他处理的状态时(例如关键帧队列是空的状态)也不需要每次都进行该处理。
另外，若进行光束平差的处理，则有时会找到投影在关键帧上时的误差大于给定的值的Map点。对于这样误差大的Map点，由于会给SLAM估计带来不良影响，因此在步骤S310从Map点DB中删除。另外，也可以不是从Map点DB中删除，而是在Map点DB中设立用于识别是需要注意误差大的Map点的标记。
接下来，参考图6来说明在主移动装置100的主流程(图3)的步骤S104启动的合环线程。在该线程中，控制部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的关键帧)与「关键帧自身的特征」的类似度成为给定的类似度(例如0.9，以下称作「基准图像类似度」)以上的关键帧(步骤S405)。在此，关于该类似度，在以特征矢量表征图像(关键帧)的特征的情况下，能将使2个图像的特征矢量的绝对值(要素的平方和的平方根)归一化为1的参数彼此的内积设为该2个图像的类似度。另外可以将2个图像的特征矢量(使绝对值归一化为1的参数)的距离(各要素的差的平方和的平方根)的倒数设为类似度。
控制部10判定是否发现「关键帧自身的特征」的类似度成为基准图像类似度以上的关键帧(步骤S406)，若未发现(步骤S406；“否”)，则返回步骤S401，若发现了(步骤S406；“是”)，则修正从发现的关键帧到当前关键帧的轨迹中的关键帧的姿态和轨迹中的关键帧所含的Map点的3D位置(步骤S407)。例如，控制部10将当前关键帧的姿态修正为与发现的关键帧的姿态相同的姿态。然后，使用发现的关键帧的姿态与当前关键帧的姿态的差分，对从发现的关键帧到当前关键帧的轨迹中的各关键帧的姿态线性地加进补正。进而，对于这些各关键帧中所含的Map点的3D位置，也按照各关键帧的姿态的补正量进行修正。然后返回步骤S401。
接下来说明自主移动装置100的主流程(图3)的步骤S107的处理即移动控制。其中，虽然基本上是为了自主向目的地移动而对驱动部42发出动作指示，但若特征点对应数较少，则通过降低速度或停止，或者变更移动方向来进行使特征点对应数增加的处理。
另外，由于在移动控制的处理中使用图3的步骤S101中简单说明的移动状态变量和前次履历数这2个变量，因此进行补足说明。移动状态变量具有「通常」、「低速度」、「停止」、「返回」的任一者的值，用在移动速度或移动方向的变更中。设置存储于移动履历存储部24的移动履历的个数，在后述的「返回」的移动后在决定是否设定另外的目的地时使用前次履历数。
接下来参考图7来说明移动控制。首先，移动方向速度变更部15判定移动状态变量的值是否是「返回」(步骤S501)。若移动状态变量的值是「返回」(步骤S501；“是”)，则前进到步骤S519。步骤S519以后的处理在之后叙述。
若移动状态变量的值不是「返回」(步骤S501；“否”)，则移动方向速度变更部15判定是否目的地是未设定或者是否抵达了目的地(步骤S502)。若目的地是未设定或抵达了目的地(步骤S502；“是”)，则控制部10设定目的地(步骤S503)，前进到步骤S504。在步骤S503，控制部10相当于目的地设定部。若设定了目的地且尚未抵达目的地(步骤S502；“否”)，则前进到步骤S504。
在步骤S504，移动方向速度决定部16基于当前位置、目的地以及存储于地图存储部22的地图的信息来决定移动方向(步骤S504)。
接下来，移动方向速度变更部15判定存储于特征点对应数存储部23的特征点对应数是否大于第1移动变更阈值(例如100)(步骤S505)。若大于第1移动变更阈值(步骤S505；“是”)，则对移动状态变量设置「通常」(步骤S506)，前进到步骤S509。
若存储于特征点对应数存储部23的特征点对应数为第1移动变更阈值以下(步骤S505；“否”)，则移动方向速度变更部15判定特征点对应数是否大于第2移动变更阈值(例如30)(步骤S507)。若大于第2移动变更阈值(步骤S507；“是”)，则对移动状态变量设置「低速度」(步骤S508)，前进到步骤S509。另外，第1移动变更阈值和第2移动变更阈值都能设定为数十以上的任意的值，但第1移动变更阈值设定为大于第2移动变更阈值的值。
在步骤S509，移动方向速度决定部16将在步骤S504决定的移动方向、和从移动状态变量的值所表示的速度得到的移动距离存储到移动履历存储部24。另外，也可以取代移动方向以及移动距离，而将在自机位置估计线程取得的自机的姿态(位置(并进矢量t)以及朝向(旋转矩阵R))存储到移动履历存储部24。
然后，移动方向速度决定部16基于在步骤S504决定的移动方向、和移动状态变量的值所表示的速度来驱动驱动部42(步骤S510)，结束处理。
若存储于特征点对应数存储部23的特征点对应数为第2变更阈值以下(步骤S507；“否”)，则移动方向速度变更部15判定为移动状态变量的值是「停止」(步骤S511)。若移动状态变量的值不是「停止」(步骤S511；“否”)，则对移动状态变量设置「停止」(步骤S512)，开始控制部10所具备的计时器的计数(步骤S513)，结束。
若移动状态变量的值是「停止」(步骤S511；“是”)，则移动方向速度变更部15判定在步骤S513起始的计时器的值是否大于基准停止时间(例如10秒)(步骤S514)。若计时器的值为基准停止时间以下(步骤S514；“否”)，则结束。
若计时器的值大于基准停止时间(步骤S514；“是”)，则移动方向速度变更部15对动作状态变量设置「返回」(步骤S515)，判定在该时间点存储于移动履历存储部24的履历的个数是否大于前次履历数(步骤S516)。若存储于移动履历存储部24的履历的个数大于前次履历数(步骤S516；“是”)，则结束。
若存储于移动履历存储部24的履历的个数为前次履历数以下(步骤S516；“否”)，则控制部10对前次履历数设置存储于移动履历存储部24的履历的个数(步骤S517)，设定另外目的地(步骤S518)。在步骤S518，控制部10相当于目的地变更部。在步骤S518之后结束。
在步骤S501中动作状态变量是「返回」的情况下(步骤S501；“是”)，移动方向速度决定部16将与最后存储于移动履历存储部24的移动方向相反的方向决定为下一移动时的移动方向，将最后存储于移动履历存储部24的移动距离决定为下一移动时的移动距离(步骤S519)。在存储于移动履历存储部24的信息是自机的位置以及朝向的情况下，移动方向速度决定部16将用于从当前位置返回到最后存储于移动履历存储部24的自机的位置以及朝向的移动方向以及移动距离决定为下一移动时的移动方向以及移动距离(步骤S519)。
接下来，移动方向速度变更部15为了判定是否返回了给定的数量的履历，判定从前次履历数减去在该时间点存储于移动履历存储部24的履历的个数的数量是否为给定返回数(例如10)以上(步骤S520)。若不足给定返回数(步骤S520；“否”)，则由于还在继续「返回」的移动，因此就这样前进到步骤S522。
若为给定返回数以上(步骤S520；“是”)，则为了使「返回」的移动结束而对动作状态变量设置「通常」(步骤S521)，前进到步骤S522。
在步骤S522，移动方向速度变更部15删除最后存储于移动履历存储部24的移动履历(步骤S522)。然后，移动方向速度决定部16基于在步骤S504决定的移动方向、和移动状态变量的值所表示的速度，来驱动驱动部42(步骤S510)，结束处理。
步骤S503中的目的地的设定例如由通信部44取得目的地的信息来设定。这时，通信部44相当于目的地取得部。另外，步骤S518中的另外目的地的设定例如也通过通信部44取得另外目的地的信息来设定。
参考图8，以具体例来说明通过让以上的处理作为主流程(图3)所示的步骤S107的移动控制处理来反复执行数次从而自主移动装置100停止或返回的样子。若特征点对应数大于第2移动变更阈值，则向目的地持续移动(图8的1.步骤S506或步骤S508)。若特征点对应数为第2移动变更阈值以下，则停止(图8的2.步骤S512)。
由于在停止的期间通过在后台工作的地图作成线程来持续进行向Map点数据库的Map点的登录，因此，若由此特征点对应数变得大于第2移动变更阈值，则移动又重开(在步骤S514；以“否”结束后，这以后的移动控制的步骤S506或步骤S508)。但若即使停止基准停止时间，特征点对应数也维持在第2移动变更阈值以下，则返回给定返回数(图8的3.步骤S515、步骤S520)。另外，在图8中将给定返回数设为2。
若返回给定返回数，则再度向目的地移动(图8的4.)。若在再度向着目的地的过程中特征点对应数大于第2移动变更阈值的状态持续，则就这样持续移动(图8的5.步骤S506或步骤S508)。但在相比于前次前进的履历没有行进的情况下，即使要移动到该目的地，也会因特征点对应数少而中途而多次返回，因此变更目的地(图8的5’.步骤S518)，返回给定返回数(图8的6.步骤S520)。在返回给定返回数后，向变更的另外目的地移动(图8的7.步骤S506或步骤S508)。
通过进行以上那样的移动控制，能将能进行自机位置估计线程的自机位置的估计的状态保持尽可能长的时间。另外，也可以在图7的步骤S508变更在步骤S504决定的移动方向。该变更既可以变更为随机的方向，也可以由自主移动装置100当场转动360度并拍摄图像，将移动方向变更到拍摄的图像中特征点多的图像的方向。另外，在图7的步骤S518设定的「另外目的地」在存在多个目的地的情况下，依次变更为第1候补→第2候补→第3候补即可。另外，在对相同的目的地存在多个路径的情况下，也可以不改变目的地而改变路径。另外，在没有特别目的地而自由移动的情况下，也可以随机设定目的地。
在没有特别目的地的情况下，期望向特征点多的方向移动。作为这样的移动方法，例如能举出如下方法：自主移动装置100当场转动360度并每给定的角度就拍摄成为目的地候补的方向的图像，将拍摄的图像中特征点最多的图像的方向作为新的目的地，朝向该方向移动。在此，控制部10由于对每个给定的角度选定目的地候补，因此相当于目的地候补选定部。然后，特征点取得部11由于从目的地候补的方向的图像取得特征点，因此相当于目的地候补特征点取得部。通过进行这样的处理，相比于随机移动，能向特征点对应数变多的方向移动的可能性变高，易于恢复到能进行自机位置的估计的状态。
另外，作为特征点对应数变少的原因，除了在周围的景色自身特征点少以外，还能举出周围暗而曝光时间变长，在用摄像部41拍摄的图像中出现抖动。在图7的处理中，由于若特征点对应数变少就降低速度或停止，因此成为让拍摄图像中的抖动变少的移动控制。关于这点，也可以在未在特征点对应数取得部14取得特征点对应数的状态下，也由明亮度传感器(未图示)或摄像部41取得周围的明亮度，若比给定的明亮度要暗，则降低速度或停止，由此能取得抖动少的图像。
另外，本发明的自主移动装置100的各功能还能通过通常的PC(PersonalComputer，个人计算机)等计算机实施。具体地，在上述实施方式中，说明为自主移动装置100进行的自主移动控制处理的程序预先存储在存储部20的ROM中。但也可以将程序存放在软盘、CD-ROM(Compact Disc Read Only Memory，只读光盘)、DVD(Digital VersatileDisc，数字多功能盘)以及MO(Magneto-Optical Disc，光磁盘)等计算机可读的记录介质来分发，通过将该程序读入到计算机进行安装来实现上述的各功能，构成这样的计算机。
另外，该计算机既可以内置于自主移动装置100，也可以与自主移动装置100分开存在。即，也可以如图9所示那样将控制部、存储部设置在外部的服务器200。通过云计算技术，自主移动装置100的控制部10将在摄像部41、传感器部30等取得的数据经由通信部44发送给外部的服务器200，在使该服务器200进行运算处理后，经由通信部44接收服务器200进行的运算结果，来控制驱动部42等。在图9中，服务器200的控制部50成为具备特征点取得部11、地图作成部12、位置估计部13、特征点对应数取得部14、移动方向速度变更部15以及移动方向速度决定部16的构成，但在服务器200的控制部50和自主移动装置100的控制部10的任一者具备各部即可。关于是服务器200的控制部50具备各部还是自主移动装置100的控制部10具备各部，则是任意。
以上说明了本发明优选的实施方式，但本发明并不限定于相关的特定的实施方式，在本发明中包含权利要求书所记载的发明和其等同的范围。

Claims (13)

1.一种自主移动装置，具备：
基于摄像部所拍摄的多个图像的信息来对自主的移动进行控制的移动控制部；
从所述摄像部所拍摄的图像取得特征点的特征点取得部；
存储所述特征点取得部所取得的特征点的位置的地图存储部；
取得所述特征点取得部所取得的特征点与在所述地图存储部存储有位置的特征点的对应数即特征点对应数的特征点对应数取得部；和
位置估计部，基于所述地图存储部存储的特征点的位置对自主移动装置的位置进行估计，
所述位置估计部，基于所述特征点对应数取得部取得的特征点对应数判定自主移动装置的位置的估计状态处于追踪成功状态还是追踪失败状态，
所述特征点对应数取得部，若所述判定为追踪成功状态则参照1个之前的关键帧取得特征点对应数，若为追踪失败状态则参照2个以上之前的关键帧取得特征点对应数，
所述移动控制部按照所述特征点对应数取得部所取得的特征点对应数来对移动进行控制。
2.根据权利要求1所述的自主移动装置，其中，
所述移动控制部在所述特征点对应数取得部所取得的特征点对应数为第1阈值以下的情况下变更移动方向。
3.根据权利要求1所述的自主移动装置，其中
所述移动控制部在所述特征点对应数取得部所取得的特征点对应数为第1阈值以下的情况下使移动速度降低。
4.根据权利要求3所述的自主移动装置，其中，
所述移动控制部在所述特征点对应数取得部所取得的特征点对应数为小于所述第1阈值的第2阈值以下的情况下，在给定的时间内使所述移动速度为0。
5.根据权利要求4所述的自主移动装置，其中，
所述自主移动装置还具备存储过去的移动履历的移动履历存储部，
在从使所述移动速度为0起经过所述给定的时间后所述特征点对应数取得部所取得的特征点对应数仍为所述第2阈值以下的情况下，所述移动控制部基于存储于所述移动履历存储部的移动履历来变更移动方向，以按给定的次数返回到过去的位置。
6.根据权利要求5所述的自主移动装置，其中，
所述自主移动装置还具备取得移动的目的地的目的地取得部，
所述移动控制部在变更移动方向以按所述给定的次数返回到过去的位置后，变更移动方向以朝向所述目的地取得部所取得的目的地进行移动。
7.根据权利要求6所述的自主移动装置，其中，
所述自主移动装置还具备变更所述目的地取得部所取得的目的地的目的地变更部，
在所述移动控制部变更移动方向以返回到所述过去的位置后变更移动方向以朝向所述目的地取得部所取得的目的地进行移动之后，在从返回到所述过去的位置以前的位置向前行进之前，在所述特征点对应数取得部所取得的特征点对应数为所述第2阈值以下的情况下，所述目的地变更部变更所述目的地。
8.根据权利要求7所述的自主移动装置，其中，
所述自主移动装置还具备选定多个目的地候补的目的地候补选定部；和
从所述目的地候补选定部所选定的目的地候补的方向的图像取得特征点的目的地候补特征点取得部，
所述目的地变更部将所述目的地变更为所述目的地候补选定部所选定的多个目的地候补中由所述目的地候补特征点取得部取得的特征点的数量成为最大的目的地候补。
9.一种自主移动方法，是用于基于摄像部所拍摄的多个图像的信息让自主移动装置自主进行移动的自主移动方法，包括：
从所述摄像部所拍摄的图像取得特征点的特征点取得步骤；
取得所述特征点取得步骤所取得的特征点与在地图存储部存储有位置的特征点的对应数即特征点对应数的特征点对应数取得步骤，其中所述地图存储部存储所述特征点取得步骤所取得的特征点的位置；
位置估计步骤，基于所述地图存储部存储的特征点的位置对自主移动装置的位置进行估计；和
移动控制步骤，按照所述特征点对应数取得步骤中取得的特征点对应数来对移动进行控制，
所述位置估计步骤，进一步基于所述特征点对应数取得步骤取得的特征点对应数判定自主移动装置的位置的估计状态处于追踪成功状态还是追踪失败状态，
所述特征点对应数取得步骤，包含：若所述判定为追踪成功状态则参照1个之前的关键帧取得特征点对应数的第1取得步骤、和若所述判定为追踪失败状态则参照2个以上之前的关键帧取得特征点对应数的第2取得步骤。
10.根据权利要求9所述的自主移动方法，其中，
若所述图像特征点与所述估计特征点的对应数成为第1阈值以下，则使移动速度降低。
11.根据权利要求10所述的自主移动方法，其中，
若所述图像特征点与所述估计特征点的对应数成为小于所述第1阈值的第2阈值以下，则停止给定的时间。
12.根据权利要求11所述的自主移动方法，其中，
在即使停止给定的时间所述图像特征点与所述估计特征点的对应数仍为所述第2阈值以下的情况下，返回过去的位置。
13.一种记录介质，记录用于使基于摄像部所拍摄的多个图像的信息进行自主移动的自主移动装置的计算机执行以下步骤的程序：
从所述摄像部所拍摄的图像取得特征点的特征点取得步骤；
取得所述特征点取得步骤所取得的特征点与在地图存储部存储有位置的特征点的对应数即特征点对应数的特征点对应数取得步骤，其中所述地图存储部存储所述特征点取得步骤所取得的特征点的位置；
位置估计步骤，基于所述地图存储部存储的特征点的位置对自主移动装置的位置进行估计；和
移动控制步骤，按照所述特征点对应数取得步骤中取得的特征点对应数来对移动进行控制，
所述位置估计步骤，进一步基于所述特征点对应数取得步骤取得的特征点对应数判定自主移动装置的位置的估计状态处于追踪成功状态还是追踪失败状态，
所述特征点对应数取得步骤，包含：若所述判定为追踪成功状态则参照1个之前的关键帧取得特征点对应数的第1取得步骤、和若所述判定为追踪失败状态则参照2个以上之前的关键帧取得特征点对应数的第2取得步骤。
CN201710040227.2A 2016-03-14 2017-01-18 自主移动装置、自主移动方法以及程序存储介质 Active CN107194970B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2016049350A JP6187623B1 (ja) 2016-03-14 2016-03-14 自律移動装置、自律移動方法及びプログラム
JP2016-049350 2016-03-14

Publications (2)

Publication Number Publication Date
CN107194970A CN107194970A (zh) 2017-09-22
CN107194970B true CN107194970B (zh) 2020-11-03

Family

ID=59720317

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710040227.2A Active CN107194970B (zh) 2016-03-14 2017-01-18 自主移动装置、自主移动方法以及程序存储介质

Country Status (3)

Country Link
US (1) US10296002B2 (zh)
JP (1) JP6187623B1 (zh)
CN (1) CN107194970B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10990829B2 (en) * 2017-04-28 2021-04-27 Micro Focus Llc Stitching maps generated using simultaneous localization and mapping
WO2019053986A1 (ja) * 2017-09-14 2019-03-21 ソニー株式会社 自己位置推定装置と自己位置推定方法とプログラムおよび移動体装置
US11874666B2 (en) * 2018-04-20 2024-01-16 Honda Motor Co., Ltd. Self-location estimation method
KR102070283B1 (ko) 2018-05-16 2020-01-28 엘지전자 주식회사 청소기 및 그 제어방법
CN109213154A (zh) * 2018-08-10 2019-01-15 远形时空科技（北京）有限公司 一种基于Slam定位方法、装置、电子设备及计算机存储介质
CN109579847B (zh) * 2018-12-13 2022-08-16 歌尔股份有限公司 同步定位与地图构建中关键帧提取方法、装置和智能设备
JP2021189822A (ja) 2020-06-01 2021-12-13 オリンパス株式会社 画像処理システム、及び、画像処理方法、並びに、画像処理プログラム
KR20220054104A (ko) * 2020-10-23 2022-05-02 삼성전자주식회사 전자 장치 및 그 제어 방법
JP2022181736A (ja) * 2021-05-27 2022-12-08 キヤノン株式会社 情報処理装置、移動装置及びコンピュータプログラム

Family Cites Families (41)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3176701B2 (ja) 1992-04-15 2001-06-18 本田技研工業株式会社 移動体の現在位置認識処理装置
JP4060974B2 (ja) 1999-02-25 2008-03-12 株式会社ザナヴィ・インフォマティクス 経路誘導装置
JP4175165B2 (ja) * 2002-04-17 2008-11-05 松下電工株式会社 自律移動装置
JP4352148B2 (ja) 2003-03-14 2009-10-28 独立行政法人科学技術振興機構 移動ロボット用地図作成システム
US7835820B2 (en) * 2005-10-11 2010-11-16 Vanderbilt University System and method for image mapping and visual attention
US8965578B2 (en) * 2006-07-05 2015-02-24 Battelle Energy Alliance, Llc Real time explosive hazard information sensing, processing, and communication for autonomous operation
US8355818B2 (en) * 2009-09-03 2013-01-15 Battelle Energy Alliance, Llc Robots, systems, and methods for hazard evaluation and visualization
JP2008059218A (ja) * 2006-08-30 2008-03-13 Fujitsu Ltd 自律走行ロボットの自己位置回復方法
KR100855469B1 (ko) * 2006-09-13 2008-09-01 삼성전자주식회사 이동 로봇의 자세 추정 장치 및 방법
JP4856510B2 (ja) 2006-10-02 2012-01-18 本田技研工業株式会社 移動ロボット
GB2448470B (en) * 2007-04-20 2012-04-04 Ultra Global Ltd Vehicle guidance system
KR101337534B1 (ko) * 2007-07-24 2013-12-06 삼성전자주식회사 이동 로봇의 위치 인식 장치 및 방법
US20090096686A1 (en) 2007-10-11 2009-04-16 Nec Laboratories America, Inc. Autonomous mobile wireless antenna systems
JP2009169845A (ja) 2008-01-18 2009-07-30 Toyota Motor Corp 自律移動ロボット及び地図更新方法
JP5270184B2 (ja) 2008-02-13 2013-08-21 古野電気株式会社 衛星航法／推測航法統合測位装置
JP2009193240A (ja) 2008-02-13 2009-08-27 Toyota Motor Corp 移動ロボット及び環境地図の生成方法
JP5310285B2 (ja) * 2009-06-12 2013-10-09 日産自動車株式会社 自己位置推定装置及び自己位置推定方法
KR20100138725A (ko) * 2009-06-25 2010-12-31 삼성전자주식회사 가상 세계 처리 장치 및 방법
JP2011043419A (ja) * 2009-08-21 2011-03-03 Sony Corp 情報処理装置、および情報処理方法、並びにプログラム
JP5444952B2 (ja) 2009-08-28 2014-03-19 富士通株式会社 センサフュージョンによる地図の自動生成、およびそのように自動生成された地図を用いて移動体の移動をするための、装置、方法、ならびにプログラム
JP5315289B2 (ja) 2010-04-12 2013-10-16 トヨタ自動車株式会社 オペレーティングシステム及びオペレーティング方法
US20120230550A1 (en) 2011-03-10 2012-09-13 Jacob Kraut Method and Apparatus for Generating a Map from Landmarks
JP5222971B2 (ja) 2011-03-31 2013-06-26 富士ソフト株式会社 歩行ロボット装置及びその制御プログラム
GB2509445A (en) * 2011-09-14 2014-07-02 Smart Ship Holdings Ltd Allocating an area to a vehicle
JP5821526B2 (ja) * 2011-10-27 2015-11-24 ソニー株式会社 画像処理装置、画像処理方法及びプログラム
EP2776216B1 (en) 2011-11-11 2022-08-31 iRobot Corporation Robot apparautus and control method for resuming operation following a pause.
JP5429901B2 (ja) 2012-02-08 2014-02-26 富士ソフト株式会社 ロボット及び情報処理装置のプログラム
JP6223976B2 (ja) * 2012-07-23 2017-11-01 富士通株式会社 表示制御プログラム、表示制御方法及び表示制御装置
US9739215B2 (en) 2013-03-15 2017-08-22 Ford Global Technologies, Llc Intrusive EGR monitor for a hybrid vehicle
JP6051993B2 (ja) 2013-03-25 2016-12-27 村田機械株式会社 自律移動式無人搬送車、及び、自律移動式無人搬送システム
US9037396B2 (en) * 2013-05-23 2015-05-19 Irobot Corporation Simultaneous localization and mapping for a mobile robot
JP6027554B2 (ja) 2014-01-21 2016-11-16 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置、情報処理システム、ブロックシステム、および情報処理方法
JP6191484B2 (ja) 2014-02-03 2017-09-06 トヨタ自動車株式会社 移動ロボットの自己位置推定方法
TWI507664B (zh) 2014-03-13 2015-11-11 Pixart Imaging Inc 光學編碼器及其運作方法
KR101629649B1 (ko) * 2014-09-30 2016-06-13 엘지전자 주식회사 로봇 청소기 및 로봇 청소기의 제어방법
US9519289B2 (en) 2014-11-26 2016-12-13 Irobot Corporation Systems and methods for performing simultaneous localization and mapping using machine vision systems
US9569851B2 (en) 2014-12-23 2017-02-14 Ricoh Co., Ltd. Sequencing products recognized in a shelf image
JP6789624B2 (ja) * 2015-11-20 2020-11-25 キヤノン株式会社 情報処理装置、情報処理方法
GB2545171B (en) * 2015-12-03 2020-08-19 Sony Interactive Entertainment Inc Light source identification apparatus and method
JP6323439B2 (ja) * 2015-12-17 2018-05-16 カシオ計算機株式会社 自律移動装置、自律移動方法及びプログラム
US9889859B2 (en) * 2015-12-21 2018-02-13 Intel Corporation Dynamic sensor range in advanced driver assistance systems

Also Published As

Publication number Publication date
CN107194970A (zh) 2017-09-22
US20170261985A1 (en) 2017-09-14
JP6187623B1 (ja) 2017-08-30
JP2017167601A (ja) 2017-09-21
US10296002B2 (en) 2019-05-21

Similar Documents

Publication Publication Date Title
CN107194970B (zh) 自主移动装置、自主移动方法以及程序存储介质
CN106896807B (zh) 自主移动装置、自主移动方法以及记录介质
US11385062B2 (en) Map creation method for mobile robot and path planning method based on the map
JP6724439B2 (ja) 自律移動装置、自律移動システム、自律移動方法及びプログラム
CN107053166B (zh) 自主移动装置、自主移动方法以及存储介质
US11340610B2 (en) Autonomous target following method and device
CN106780608B (zh) 位姿信息估计方法、装置和可移动设备
US10108199B2 (en) Autonomous movement device, autonomous movement method and non-transitory recording medium
US20180161986A1 (en) System and method for semantic simultaneous localization and mapping of static and dynamic objects
US9329598B2 (en) Simultaneous localization and mapping for a mobile robot
US10921820B2 (en) Movable object and control method thereof
US20130216098A1 (en) Map generation apparatus, map generation method, moving method for moving body, and robot apparatus
CN110238838B (zh) 自主移动装置、自主移动方法以及存储介质
JP6638753B2 (ja) 自律移動装置、自律移動方法及びプログラム
US11926038B2 (en) Information processing apparatus and information processing method
JP7354528B2 (ja) 自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラム
JP2018156538A (ja) 自律移動装置、画像処理方法及びプログラム
Botterill Visual navigation for mobile robots using the bag-of-words algorithm
JP7067130B2 (ja) 自律移動装置、メモリ整理方法及びプログラム
JP7327596B2 (ja) 自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラム
Pal et al. Evolution of simultaneous localization and mapping framework for autonomous robotics—a comprehensive review
CN115700507B (zh) 地图更新方法及其装置
JP2020057187A (ja) 自律移動装置、自律移動方法及びプログラム

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant