CN110850863A - 自主移动装置、自主移动方法以及存储介质 - Google Patents
自主移动装置、自主移动方法以及存储介质 Download PDFInfo
- Publication number
- CN110850863A CN110850863A CN201910686631.6A CN201910686631A CN110850863A CN 110850863 A CN110850863 A CN 110850863A CN 201910686631 A CN201910686631 A CN 201910686631A CN 110850863 A CN110850863 A CN 110850863A
- Authority
- CN
- China
- Prior art keywords
- map
- posture
- environment
- autonomous moving
- moving apparatus
- 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.)
- Granted
Links
- 238000003860 storage Methods 0.000 title claims abstract description 101
- 238000000034 method Methods 0.000 title claims description 195
- 230000008859 change Effects 0.000 claims abstract description 40
- 239000011159 matrix material Substances 0.000 claims description 180
- 238000006243 chemical reaction Methods 0.000 claims description 57
- 238000010606 normalization Methods 0.000 claims description 52
- 230000009466 transformation Effects 0.000 claims description 37
- 239000013598 vector Substances 0.000 claims description 19
- 238000005286 illumination Methods 0.000 claims description 10
- 238000009434 installation Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 abstract description 91
- 230000007613 environmental effect Effects 0.000 abstract description 22
- 238000003384 imaging method Methods 0.000 abstract description 21
- 230000036544 posture Effects 0.000 description 195
- 230000008569 process Effects 0.000 description 180
- 230000004048 modification Effects 0.000 description 25
- 238000012986 modification Methods 0.000 description 25
- 230000033001 locomotion Effects 0.000 description 21
- 230000006870 function Effects 0.000 description 17
- 238000012937 correction Methods 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 11
- 238000000605 extraction Methods 0.000 description 9
- 238000004891 communication Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 101001096365 Homo sapiens Replication factor C subunit 2 Proteins 0.000 description 5
- 102100037851 Replication factor C subunit 2 Human genes 0.000 description 5
- 230000010391 action planning Effects 0.000 description 5
- 238000013507 mapping Methods 0.000 description 5
- 238000007726 management method Methods 0.000 description 3
- 102100036865 Solute carrier family 52, riboflavin transporter, member 3 Human genes 0.000 description 2
- 101710102466 Solute carrier family 52, riboflavin transporter, member 3 Proteins 0.000 description 2
- 230000009471 action Effects 0.000 description 2
- 238000004140 cleaning Methods 0.000 description 2
- 230000007958 sleep Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000005674 electromagnetic induction Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
-
- 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/0242—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using non-visible light signals, e.g. IR or UV signals
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3863—Structures of map data
- G01C21/387—Organisation of map data, e.g. version management or database structures
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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/0251—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 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- 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/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning 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/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
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Databases & Information Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明能够进行针对环境变化进行鲁棒性自身位置估计。自主移动装置(100)使用通过摄像部(33)拍摄到的图像来进行环境地图的制作以及位置的估计。自主移动装置(100)具备控制部(10)和存储部(20)。控制部(10)根据周围环境的变化制作多个环境地图,将制作出的多个环境地图进行标准化并以能够统一处理的方式保存于存储部(20),使用标准化后的环境地图来估计位置。
Description
本申请要求基于2018年7月31日提交的日本专利申请号2018-144206的优先权,并且本申请涵盖了该基础申请的全部内容。
技术领域
本发明涉及自主移动装置、自主移动方法以及存储介质。
背景技术
如自动清扫室内的吸尘器机器人那样的、制作环境地图并自主地移动的自主移动装置正在普及。这样的自主移动装置通常进行vSLAM(Visual Simultaneous LocalizationAnd Mapping,基于视觉的即时定位与地图绘制)处理,所述vSLAM使用相机同时进行自身位置估计和环境地图制作。在vSLAM处理中,基于由相机拍摄的图像中包含的特征点来进行自身位置估计和环境地图制作,因此自身位置估计的精度或所制作的环境地图的内容受到照明等环境的较大影响。因此,如果在与制作环境地图时的照明等环境不同的环境中利用该环境地图来进行自身位置的估计,则位置估计的性能会大幅恶化。作为解决该问题的技术,在国际公开第2016/016955号中记载了一种自主移动装置,该自主移动装置通过以周围环境的界标配置为依据估计自身位置,能够进行抑制了干扰影响的自身位置估计。
发明内容
在国际公开第2016/016955号中记载的自主移动装置选择不受照明变化等干扰因素(环境的变化)影响的界标,控制相机以追踪所选择的界标,并且以由这样的追踪控制的相机拍摄的界标的配置为依据来估计自身位置,因此能够针对环境变化进行鲁棒性自身位置估计。但是,不受环境变化影响的界标是指地板的白线等具有明确形状的界标(标志),在周围没有这样明确的界标的环境中,自主移动装置的动作有可能变得不稳定。
如果每当照明环境改变时制作另外的环境地图,并将这样制作的多个环境地图根据各个照明环境分别使用,则解决了上述问题。然而,通过vSLAM处理制作出的多个环境地图的坐标系在每个环境地图中成为不同的坐标系,不能被统一处理。这是因为,一般在通过vSLAM处理制作的环境地图的坐标系中,vSLAM处理的初始化结束时的自主移动装置的位置成为原点,并根据vSLAM处理的初始化结束时的自主移动装置的朝向来决定坐标轴。因此,在以往的自主移动装置中,由于不能统一地处理根据环境变化制作的多个环境地图,因此在能够针对环境变化进行鲁棒性自身位置估计的技术中存在改善的余地。
本发明是为了解决上述问题而完成的,其目的是提供通过使多个环境地图能够被统一处理,由此能够针对环境变化进行鲁棒性自身位置估计的自主移动装置、自主移动方法以及存储介质。
为了实现上述目的,本发明的自主移动装置使用通过摄像部拍摄的图像进行环境地图的制作以及位置的估计,其中,
具备控制部和存储部,
所述控制部根据周围环境的变化制作多个环境地图,
对所述制作出的多个环境地图进行标准化并以能够统一地处理的方式保存于所述存储部,
使用所述标准化的环境地图来估计位置。
根据本发明,可以实现针对环境变化进行鲁棒性自身位置估计。
附图说明
图1是表示本发明的实施方式1的自主移动装置的外观的图。
图2是表示实施方式1的充电器的外观的图。
图3是说明实施方式1的充电器发送的反馈信号的图。
图4是表示实施方式1的自主移动装置的功能结构的图。
图5是表示实施方式1的自主移动装置所制作的环境地图的数据结构的图。
图6是实施方式1的自主移动装置的启动时处理的流程图。
图7是实施方式1的自主移动装置的自身位置估计线程(thread)的流程图。
图8是实施方式1的自主移动装置的环境地图提取处理的流程图。
图9是实施方式1的自主移动装置的Relocalization(重新定位)处理的流程图。
图10是实施方式1的自主移动装置的姿势估计处理的流程图。
图11是实施方式1的自主移动装置的环境地图保存处理的流程图。
图12是实施方式1的自主移动装置的环境地图标准化处理的流程图。
图13是实施方式1的自主移动装置的姿势变换矩阵的误差修正处理的流程图。
图14是实施方式1的自主移动装置的ICP处理的流程图。
图15是本发明的实施方式1的变形例1的自主移动装置的环境地图保存处理的流程图。
图16是表示本发明的实施方式2的自主移动装置制作的环境地图的数据结构的图。
图17是实施方式2的自主移动装置的环境地图保存处理的流程图。
图18是实施方式2的自主移动装置的环境地图标准化处理的流程图。
具体实施方式
以下,参照图表对本发明的实施方式的自主移动装置进行说明。另外,对图中相同或相当的部分标注相同的附图标记。
(实施方式1)
本发明的实施方式的自主移动装置是一边制作地图(环境地图),一边根据用途自主地移动的装置。该用途是指例如警备监视用、室内扫除用、宠物用、玩具用等。
如图1所示,实施方式1的自主移动装置100在外观上具备反馈信号接收部31(31a、31b)、驱动部32(32a、32b)、摄像部33、充电连接部35。此外,虽然在图1中未示出,但自主移动装置100也可以具备对存在于周围的物体(障碍物)进行检测的障碍物传感器。此外,如图2所示,用于对自主移动装置100的电池进行充电的充电器200在外观上具备反馈信号发送部51(51a、51b)、电力供给部52。
自主移动装置100的充电连接部35与充电器200的电力供给部52连接,由此自主移动装置100能够从充电器200接受电力的供给,对自主移动装置100内置的电池进行充电。另外,充电连接部35以及电力供给部52分别是用于相互连接的连接端子。自主移动装置100通过驱动部32移动到充电器200上,由此这些连结端子连接。该连接可以通过充电连接部35与电力供给部52接触来进行连接,也可以通过充电连接部35与电力供给部52接近来利用电磁感应等进行连接。
摄像部33具备能够对从自主移动装置100的前方到上部的大范围进行拍摄的广角透镜。因此,摄像部33能够拍摄可判别天花板的电灯是否点亮的图像。而且,自主移动装置100能够利用摄像部33拍摄到的图像来进行单眼的SLAM(Simultaneous Localization AndMapping,即时定位与地图绘制)处理。
自主移动装置100的反馈信号接收部31是用于接收从充电器200发送的反馈信号(红外线信标(beacon))的设备。面向自主移动装置100的正面,在左侧具备反馈信号接收部31a,在右侧具备反馈信号接收部31b,共计具备两个反馈信号接收部31。此外,充电器200的反馈信号发送部51是用于将反馈信号发送到自主移动装置100的设备。面向充电器200的正面,在右侧具备反馈信号发送部51a,在左侧具备反馈信号发送部51b。而且,从反馈信号发送部51a发送的反馈信号和从反馈信号发送部51b发送的反馈信号为不同的信号。因此,反馈信号接收部31能够判别分别是从左侧和右侧中的哪一侧的反馈信号发送部51接收到反馈信号。
图3表示从充电器200的反馈信号发送部51发送的反馈信号的左右各自的可接收范围53(53a、53b)的一例。当自主移动装置100的反馈信号接收部31进入可接收范围53a的内部时,能够接收从反馈信号发送部51a发送的反馈信号。而且,当自主移动装置100的反馈信号接收部31进入可接收范围53b的内部时,能够接收从反馈信号发送部51b发送的反馈信号。因此,自主移动装置100能够在进入可接收范围53的内部时掌握充电器200存在的方向。然后,自主移动装置100通过一边调整朝向以使反馈信号接收部31a接收来自反馈信号发送部51a的反馈信号,反馈信号接收部31b接收来自反馈信号发送部51b的反馈信号,从而能够移动到充电器200上。当自主移动装置100移动到充电器200上时,充电连接部35与电力供给部52连接,能够对内置于自主移动装置100的电池进行充电。
驱动部32是独立双轮驱动型,是具备车轮和电动机的移动单元。自主移动装置100能够通过两个车轮的相同方向驱动来进行前后的平行移动(并行移动),通过两个车轮的反向驱动来进行原地的旋转(朝向变更),通过改变了两个车轮各自的速度的驱动来进行回旋移动(并行+旋转(朝向变更)移动)。另外,在各个车轮上具备旋转编码器,通过旋转编码器测量车轮的转速,通过利用车轮的直径或车轮间的距离等几何学关系,能够计算并行移动量以及旋转量。例如,如果将车轮的直径设为D、转速设为R(由旋转编码器测定),则该车轮的接地部分处的并行移动量为π×D×R。另外,如果将车轮的直径设为D、车轮间的距离设为I、右车轮的转速设为RR、左车轮的转速设为RL,则朝向变更的旋转量(如果将右旋转设为正)为360°×D×(RL-RR)/(2×I)。通过逐次相加该并行移动量以及旋转量,驱动部32作为所谓的测距仪(机械测距仪)发挥功能,能够测量自身位置(以移动开始时的位置以及朝向为基准的自主移动装置100的位置以及朝向)。
如图4所示,自主移动装置100除了反馈信号接收部31(31a、31b)、驱动部32(32a、32b)、摄像部33、充电连接部35之外,还具备控制部10、存储部20、通信部34。
控制部10由CPU(Central Processing Unit)等构成,通过执行存储于存储部20的程序,实现后述的各部(环境信息取得部11、地图制作部12、地图标准化部13、自身位置估计部14、行动计划部15、移动控制部16)的功能。另外,控制部10具备时钟(未图示),能够进行当前日期时间的取得或经过时间的计数。
存储部20由ROM(Read Only Memory)、RAM(Random Access Memory)等构成,ROM的一部分或者全部由可电改写的存储器(闪存等)构成。存储部20在功能上包含地图存储部21和地图保存部22。在ROM中存储有控制部10的CPU执行的程序以及在执行程序的基础上预先需要的数据。在RAM中存储有在程序执行过程中制作或变更的数据。
地图存储部21存储有地图制作部12基于摄像部33拍摄的图像的信息通过SLAM处理制作出的环境地图。如图5所示,环境地图包含地图ID(Identifier)、环境信息、关键帧信息组以及MapPoint(地图点)信息组。地图ID是用于唯一地识别环境地图的ID。环境信息是周围的明亮度等被认为对基于SLAM处理的位置估计造成影响的周围环境的信息。在此,在取得了关键帧信息组以及MapPoint信息组时的环境信息被记录。
此外,关键帧是指在SLAM处理中摄像部33拍摄到的图像(帧)中用于估计三维(3D)位置的帧。另外,MapPoint是指在SLAM处理中,能够估计在环境地图(三维空间)内的3D位置的坐标的特征点的3D坐标点(三维空间内的点)。
然后,如图5所示,关键帧信息包含:作为用于唯一地识别该关键帧的ID的关键帧ID、拍摄到关键帧时的摄像部33(自主移动装置100)在环境地图(三维空间)内的3D姿势(位置以及朝向)、作为关键帧中包含的特征点的信息的特征点信息(通常,由于一个关键帧中包含多个特征点,所以成为特征点信息组。)。
其中,为了使旋转和并行的计算变得容易,3D姿势由(4×4的)齐次坐标形式的姿势矩阵表示,该姿势矩阵通过一个矩阵表示了表示朝向的(3×3的)旋转矩阵和表示位置(3D坐标)的(3×1的)位置矩阵(位置向量)。因此,后述的姿势变换矩阵也是包含旋转矩阵和位置矩阵的(以齐次坐标形式表示的4×4的)矩阵。此外,旋转矩阵包括记述作用于旋转矩阵的右侧的向量的右手坐标系和记述作用于旋转矩阵的左侧的向量的左手坐标系,但在本实施方式中使用右手坐标系的旋转矩阵。
关键帧中包含的特征点是指关键帧(图像)中的边缘部分或角部分等图像内的特征性部分的点。能够使用例如SIFT(Scale-Invariant Feature Transform,尺度不变特征变换)、SURF(Speeded Up Robust Features,加速鲁棒特征)、FAST(Features fromAccelerated Segment Test,加速分割测试特征)等算法来检测特征点。而且,如图5所示,作为特征点信息,包含该特征点的关键帧内的2D坐标、该特征点的特征量,以及如果该特征点在环境地图内的3D坐标点估计完毕之后还包含与该特征点对应的MapPoint的ID。如果还未估计出该特征点的3D坐标,则在“对应的MapPoint的ID”中储存表示还未估计3D坐标的特别的ID(例如0)。另外,作为特征点的特征量,例如能够使用ORB(Oriented FAST andRotated BRIEF)特征。
另外,如图5所示,MapPoint信息包含用于唯一地识别该MapPoint的ID即MapPointID和该MapPoint在环境地图内的3D坐标。因此,通过基于特征点信息中包含的“对应的MapPoint的ID”来参照MapPoint信息,能够取得该特征点在环境地图内的3D坐标。
另外,环境信息是电灯的开/关、周围的明亮度、时刻等被认为对自主移动装置100的位置估计造成影响的周围环境的信息。作为环境信息,重要的信息主要是与明亮度相关的信息(照明的开/关、窗帘的开闭、来自窗户的日光的条件(早晨和傍晚、天气)等),但也可以将人的数量、家具的配置等包含在环境信息中。另外,气温、湿度、气压等不直接影响位置估计,但如果因这些而房间的配置、人的出入发生变化,则会对位置估计造成影响,因此也可以将这些信息包含于环境信息。另外,在本实施例中,使用点亮的电灯的数量作为环境信息。这是因为,例如,能够求出作为拍摄了天花板的图像中亮度高的区域(具有规定的亮度基准值以上的明亮度的区域)的数量。
地图保存部22保存环境地图,以使其在自主移动装置100的电源断开后也不会消失,所述环境地图存在于存储部20的可电改写的ROM(闪存等)内,且在地图存储部21中存储。
通信部34是包含用于与充电器200或其他外部装置进行无线通信的天线的无线模块。例如,通信部34是用于进行基于Bluetooth(注册商标)的近距离无线通信的无线模块。通过使用通信部34,自主移动装置100能够与外部装置进行数据的交接等。
接着,说明自主移动装置100的控制部10的功能结构。控制部10实现环境信息取得部11、地图制作部12、地图标准化部13、自身位置估计部14、行动计划部15、移动控制部16的功能,进行环境地图的制作、本机的姿势(位置以及朝向)的估计、移动控制等。此外,控制部10与多线程功能对应,能够并行地执行多个线程(不同的处理流程)。
环境信息取得部11基于摄像部33拍摄到的图像数据,取得点亮的电灯的数量作为表示周围环境的环境信息。然而,这仅是环境信息的一例,在想要使用其他信息作为环境信息的情况下,环境信息取得部11取得电灯的开/关、明亮度信息等想要用作环境信息的信息。另外,在想要使用的环境信息中包含日期时间信息的情况下,环境信息取得部11从控制部10所具备的时钟取得当前日期时间。
地图制作部12使用摄像部33所拍摄的图像数据通过SLAM处理来制作图5所示的环境地图的数据,并写入地图存储部21,所述环境地图包含关键帧信息组和MapPoint信息组。
地图标准化部13通过后述的地图标准化处理,对地图制作部12制作出的环境地图的数据进行标准化。
自身位置估计部14使用摄像部33拍摄到的图像数据和在地图存储部21中存储的环境地图的数据,通过SLAM处理,估计自主移动装置100在环境地图的坐标系中的姿势(位置以及朝向)。另外,由于每次记载为“姿势(位置以及朝向)”是繁杂的,在本说明书以及所要保护的范围中,即使在仅记载为“位置”的情况下,也不仅包含位置还包含朝向。即,作为表示“姿势(位置以及朝向)”的词语,有时使用“位置”。特别地,“自身位置的估计”意味着本机(自主移动装置100)的姿势(位置以及朝向)的估计。
行动计划部15基于在地图存储部21中存储的环境地图以及动作模式设定移动目的地以及路线。此外,动作模式定义了自主移动装置100的行动样式。自主移动装置100例如具有随机移动的“自由散步模式”、逐渐扩大地图的制作范围的“地图制作模式”、移动到根据后述的主线程等指定的场所的“移动目的地指定模式”等多个动作模式。动作模式例如初始值是地图制作模式,可以预先设定变化的条件,如在制作了某种程度的地图后(例如在地图制作模式中经过10分钟后)成为自由散步模式,在电池余量变少后成为将充电器200的位置指定为移动目的地的移动目的地指定模式,也可以通过来自外部(用户、主线程等)的指示来设定。当行动计划部15设定路径时,基于地图制作部12制作出的环境地图来设定从自主移动装置100的当前位置到移动目的地的路径。
移动控制部16控制驱动部32,以使自主移动装置100沿着行动计划部15所设定的路径移动。
以上,说明了自主移动装置100的功能结构。接着,说明由自主移动装置100启动的各种处理。自主移动装置100在电源断开时与充电器200连接而被充电。而且,当电源被接通时,在与充电器200连接的位置处执行后述的启动时处理,并行地开始执行以主线程为首的各种线程,进行与自主移动装置100的用途相应的处理。参照图6说明自主移动装置100启动时的处理。图6是当自主移动装置100启动时执行的启动时处理的流程图。
首先,自主移动装置100的控制部10启动主线程(步骤S101)。主线程是从在下一步骤中启动的自身位置估计线程接收自主移动装置100的当前的环境地图内的姿势(位置以及朝向)的信息,进行与用途相应的处理(例如室内清扫用的处理)的线程。
接着,控制部10启动用于SLAM处理的各种线程(步骤S102)。用于SLAM处理的各种线程是指估计自主移动装置100的姿势(位置以及朝向)的自身位置估计线程、制作环境地图的地图制作线程、进行循环闭环处理的循环闭环线程等。此外,循环闭环处理是指,在识别出自主移动装置100返回到以前曾经来到的某相同场所的情况下,使用以前在该相同场所时的自主移动装置100的姿势与当前的姿势之间的偏差,修正从以前来到时到当前为止的移动轨迹中的关键帧(的3D姿势)或关联的MapPoint(的3D坐标)的处理。
接着,控制部10启动移动线程(步骤S103),结束启动时处理。移动线程是进行如下处理的线程:接受来自主线程的移动命令,移动控制部16控制驱动部32使自主移动装置100移动。然后,在启动时处理结束之后,自主移动装置100由通过该启动时处理启动的各线程控制。
参照图7说明用于SLAM处理的线程中的自身位置估计线程。自身位置估计线程是从地图保存部22所保存的环境地图中选择适合于当前环境的环境地图,并使用该选择出的环境地图进行追踪处理(自身位置估计处理)的线程。
首先,控制部10判定在地图保存部22中是否保存有环境地图(步骤S201)。如果在地图保存部22中未保存有环境地图(步骤S201:否),则控制部10从初始状态就开始SLAM处理,将变量MODE设置为TRACKING(步骤S202),前进到步骤S212。此外,变量MODE是表示自主移动装置100当前是处于可估计自身位置的状态(TRACKING状态)还是处于不可估计自身位置的状态(LOST状态)的变量。此外,步骤S202中的“从初始状态开始SLAM处理”意味着从清除了环境地图的状态开始移动同时通过摄像部33取得两个图像,基于这两个图像之间的特征点的对应,开始自身位置的估计和环境地图制作。如果两个图像之间的特征点的对应数量为五个以上,则能够通过使用Two-view Structure from Motion法(运动双视图结构法)来估计两个图像之间的姿势(取得各个图像的位置的差分(并行向量t)以及朝向的差分(旋转矩阵R)),因此,通过由摄像部33连续地取得图像直到取得五个以上的特征点的对应,控制部10能够估计任两个图像之间的姿势。然后,能够认为该估计出的姿势是以取得了最初的图像的位置为原点的情况下的、取得了第二个图像时的自身位置(位置以及朝向),控制部10随后能够将估计出的姿势或特征点的信息作为环境地图的信息写入地图存储部21,并且根据环境地图中包含的特征点和通过摄像部33取得的图像中的特征点来估计自身位置。这是SLAM处理。
如果在地图保存部22中保存有环境地图(步骤S201:是),则控制部10进行环境地图提取处理,所述环境地图提取处理是从在地图保存部22中保存的环境地图中提取适合于当前环境的可能性高的环境地图的处理(步骤S203)。关于环境地图提取处理的详细情况将后述。
然后,控制部10在当前的自身位置不明的状态下,进行利用环境地图估计自身位置的处理即Relocalization处理(步骤S204)。关于Relocalization处理的详细情况将后述。
然后,控制部10判定Relocalization处理是否成功(步骤S205)。如果Relocalization处理失败(步骤S205:否),则控制部10判定是否有来自主线程或用户的结束指示(步骤S206)。如果有结束指示(步骤S206:是),则结束自身位置估计线程。如果没有结束指示(步骤S206:否),则向移动线程发出移动指示并移动(步骤S207)。另外,步骤S207中的移动是用于使Relocalization处理的最初取得的图像变化的移动,例如在自主移动装置100通过并行动作的其他线程的处理而移动的情况下,不需要在步骤S207中重新移动。
然后,控制部10判定步骤S204中的Relocalization处理是否持续失败规定时间以上(步骤S208)。该判定例如导入记录Relocalization处理失败的变量RFC和记录变量RFC成为1时的时刻的变量RFT,并如下进行。如果Relocalization处理成功,则将变量RFC以及变量RFT初始化为0,如果失败,则对变量RFC加上1。如果变量RFC成为1,则将此时的时刻记录于变量RFT。然后,在步骤S208中,判定当前时刻是否从变量RFT中记录的时刻起经过了规定时间(例如5分钟)。
如果Relocalization处理持续失败规定时间以上(步骤S208:是),则进入步骤S202,从SLAM处理的初始化(以及地图存储部21的清除)开始重新进行处理。如果虽然Relocalization处理失败但还没有经过规定时间(步骤S208:否),则返回步骤S204再次进行Relocalization处理。另外,作为步骤S208中的判定,也可以代替规定时间而判定是否持续失败规定次数(例如5次)以上。该判定能够通过上述的变量RFC的值是否达到规定次数以上来进行。
另一方面,如果Relocalization处理成功(步骤S205:是),则控制部10将Relocalization处理成功时选择的环境地图读入到地图存储部21,并在随后的自身位置估计中利用(步骤S209)。然后,控制部10将变量MODE设置为TRACKING(步骤S210),并前进到步骤S212。
在步骤S212中,通过基于SLAM的追踪处理来估计自身位置。在该追踪处理中,首先,从摄像部33拍摄的图像数据中提取特征点,并且利用特征量取得提取出的特征点与环境地图中包含的3D坐标估计完毕的关键帧的特征点之间的对应。如果取得了对应的特征点(对应特征点)的数量在可追踪基准数(例如10)以上,则控制部10能够根据对应特征点的图像中的2D坐标和环境地图内的3D坐标的关系,估计自身位置。该情况为追踪成功。在对应特征点的数量小于可追踪基准数的情况下,即使估计自身位置,误差也会变大,因此控制部10判断为追踪失败,不进行自身位置的估计。
控制部10在追踪处理之后,判定追踪处理是否成功(步骤S213)。如果追踪处理成功(步骤S213:是),则控制部10将在追踪处理中取得的自身位置发送给主线程(步骤S214)。然后,控制部10休眠规定时间(例如10秒)(步骤S215)。
另一方面,如果追踪处理失败(步骤S213:否),则控制部10将变量MODE设置为LOST(步骤S221),对主线程发送自身位置取得失败的意思(步骤S222),前进到步骤S215而休眠规定时间。
随后,控制部10判定是否有来自主线程或用户的结束指示(步骤S216)。如果有结束指示(步骤S216:是),则结束自身位置估计线程。如果没有结束指示(步骤S216:否),则进行将环境地图标准化并保存于地图保存部22的处理即环境地图保存处理(步骤S217)。关于环境地图保存处理的详细情况将后述。
接着,控制部10判定在变量MODE中设置的值是否为LOST(步骤S211)。如果在变量MODE中设置的值不是LOST(步骤S211:否),则当前处于TRACKING,因此前进到步骤S212,进行追踪处理。
如果在变量MODE中设置的值为LOST(步骤S211:是),则控制部10使用当前利用的(读入到地图存储部21的)环境地图进行Relocalization处理(步骤S218),并判定该Relocalization处理是否成功(步骤S219)。如果Relocalization处理成功(步骤S219:是),则控制部10将变量MODE设置为TRACKING(步骤S220),并前进到步骤S214。如果Relocalization处理失败(步骤S219:否),则前进到步骤S222。
以上,说明了自身位置估计线程的处理。接着,参照图8说明在自身位置估计线程(图7)的步骤S203中执行的环境地图提取处理。该处理是为了从在地图保存部22中保存的多个环境地图中提取适合于当前环境的可能性高的环境地图而提取包含与当前环境信息一致或相似的环境信息的环境地图的处理。
首先,控制部10通过摄像部33拍摄图像(步骤S301)。然后,通过求出图像中的亮度高的区域(具有规定的亮度基准值以上的明亮度的区域)的数量,来检测点亮的电灯的数量(步骤S302)。
然后,控制部10从在地图保存部22中保存的多个环境地图中提取规定的候选数量(N个)的、环境信息(点亮电灯数)相同或相似的环境地图(步骤S303),结束环境地图提取处理。在提取环境地图时,按照环境地图中附加的环境信息与当前的环境信息相似的顺序提取N个。提取出的N个环境地图是将来利用的环境地图的候选,因此被称为候选环境地图。另外,可以将N设定为例如5等任意数,但在地图保存部22中保存的环境地图的个数少的情况下,也可以存在只能提取小于N的个数的候选环境地图的情况。
通过以上说明的环境地图提取处理,从在地图保存部22保存的多个环境地图中提取N个环境信息与当前的环境信息一致或相似的候选环境地图。接着,参照图9说明自身位置估计线程(图7)的步骤S204中执行的Relocalization处理。Relocalization处理是指,在当前的自身位置不明的状态下,利用环境地图来估计自身位置的处理。
首先,控制部10通过摄像部33拍摄而取得图像(步骤S401)。然后,检测图像中的特征点,算出检测出的各特征点的特征量(步骤S402)。特征点的检测方法或使用的特征量是任意的,例如,控制部10能够使用FAST作为特征点的检测方法,使用ORB作为特征点的特征量。
接着,控制部10判定是否对在之前进行的环境地图提取处理中提取出的所有N个候选环境地图确认了特征点的对应(步骤S403)。如果对所有候选环境地图确认了特征点对应(步骤S403:是),则Relocalization处理失败(步骤S404),结束Relocalization处理。
如果还剩余仍未确认特征点对应的候选环境地图(步骤S403:否),则控制部10从该剩余的候选环境地图中按顺序选择一个(步骤S405)。然后,控制部10基于在步骤S401中取得的图像和在步骤S405中选择的环境地图,进行姿势估计处理(步骤S406)。如后所述,姿势估计处理成为取估计的姿势拍摄的图像A、用于姿势估计的环境地图B以及表示是否为Relocation处理的标记变量isReloc这三个自变量的子程序,在此,将标记变量isReloc设置为true并且调用姿势估计处理。关于姿势估计处理的详细情况将后述。
然后,控制部10判定姿势估计处理是否成功(步骤S407)。如果姿势估计处理失败(步骤S407:否),则返回步骤S403。如果姿势估计处理成功(步骤S407:是),则Relocalization处理设为成功(步骤S408),结束Relocalization处理。
通过以上说明的Relocalization处理,自主移动装置100基于通过摄像部33拍摄到的图像,能够选择能够估计本机的姿势(位置以及朝向)的环境地图。
接着,参照图10说明在上述的Relocalization处理(图9)的步骤S406中调用的姿势估计处理。如上所述,由于该姿势估计处理取三个自变量,因此以下分别设为图像A、环境地图B、标记变量isReloc进行说明。
首先,控制部10从环境地图B的关键帧信息组中检索与图像A相似的相似关键帧(步骤S501)。该相似关键帧的检索方法是任意的,例如,如果用特征量的直方图将环境地图B内的所有关键帧分类,通过图像A的特征量的直方图和直方图之间的相似度进行相似检索,则能够高速检索。
然后,控制部10通过特征量在步骤S501中检索到的相似关键帧的特征点中3D坐标估计完毕的特征点与图像A的特征点之间取得对应。例如,在相似关键帧内的某个特征点(3D坐标估计完毕)的特征量和图像A内的某特征点的特征量的相似度高于规定的基准相似度的情况下,判断为这两个特征点是取得了对应的特征点(对应特征点)。然后,控制部10求出该对应特征点的数量(步骤S502)。
然后,控制部10判定对应特征点的数量是否大于3(步骤S503)。如果对应特征点的数量为3以下(步骤S503:否),则作为姿势估计失败(步骤S504),结束姿势估计处理。此外,在此可以不立即将姿势估计作为失败,而是也可以返回到步骤S501,从与图像A相似的关键帧中检索上次未检索到的(即相似度程度在第二位以下的)相似关键帧。这是因为可能存在虽然相似度不高但对应特征点的数量多的情况。在这种情况下,即使处理返回到步骤S501规定的次数(例如三次)并且以其他相似关键帧求出对应特征点的数量,但如果为3以下(步骤S503:否),则姿势估计为失败(步骤S504),结束姿势估计处理。
另一方面,如果对应特征点的数量大于3(步骤S503:是),则前进到步骤S505。在图像A内的特征点中的四个以上与相似关键帧内的特征点(3D坐标估计完毕)取得了对应的情况下,能够估计取得图像A时的自主移动装置100的姿势(位置以及朝向)作为PnP问题(Perspective-n-Point Problem)。
在步骤S505中,控制部10解决该PnP问题并估计自主移动装置100的姿势,使用该估计出的姿势,算出相似关键帧内的特征点(不管3D坐标是否估计完毕)的2D坐标与对应于该特征点的图像A内的特征点的2D坐标之间的误差(对应误差)。然后,如果该对应误差为基准误差T以下,则判断该特征点的位置匹配,控制部10求出这种匹配的对应特征点的数量(匹配数)(步骤S505)。
然后,控制部10判定标记变量isReloc是否为true(步骤S506)。如果标记变量isReloc为true(步骤S506:是),则控制部10判定匹配数是否大于匹配基准值K(例如50)(步骤S507)。如果匹配数为匹配基准值K以下(步骤S507:否),则作为姿势估计失败(步骤S504),结束姿势估计处理。如果匹配数大于匹配基准值K(步骤S507:是),则控制部10通过将在步骤S505中估计出的姿势与相似关键帧的3D姿势相加来估计姿势,将估计出的结果设置为矩阵Pa(步骤S508)。然后,作为姿势估计成功(步骤S509),结束姿势估计处理。
另一方面,在步骤S506中,如果标记变量isReloc不是true(步骤S506:否),则控制部10判定匹配数是否大于匹配基准值K(例如10)×0.8(步骤S510)。如果匹配数为匹配基准值K×0.8以下(步骤S510:否),则作为姿势估计失败(步骤S504),结束姿势估计处理。如果匹配数大于匹配基准值K×0.8(步骤S510:是),则控制部10通过将在步骤S505中估计出的姿势与相似关键帧的3D姿势相加来估计姿势,将估计出的结果设置为矩阵Pa(步骤S508)。然后,作为姿势估计成功(步骤S509),结束姿势估计处理。
通过以上说明的姿势估计处理,在变量isReloc=true时,自主移动装置100能够估计本机的姿势(位置以及朝向),在变量isReloc=false时,能够允许若干误差地估计拍摄了最初的自变量(图像A)时的自主移动装置100的姿势。另外,在上述的步骤S510中,将匹配数与匹配基准值K×0.8进行比较,但该0.8这样的数值只不过是一例。但是,如果该数值过小,则误差会增加,因此优选设定在1至0.5左右之间。
此外,在图9的Relocalization处理中,是最终选择候选环境地图中最初姿势估计成功的环境地图的处理,但该处理也只不过是一例。例如,在姿势估计处理(图10)中,也可以将在步骤S505中求出的匹配数设为返回值,在Relocalization处理中,针对所有的候选环境地图求出匹配数,最终选择匹配数最大的环境地图。此外,也可以在姿势估计处理(图10)的步骤S505中求出匹配数时,针对各候选环境地图还存储特征点的坐标位置的误差,而在Relocalization处理中,从所有的候选环境地图中最终选择误差最小的环境地图。
接着,参照图11说明在自身位置估计线程(图7)的步骤S217中执行的环境地图保存处理。该处理是每隔规定的时间(例如1小时)将存储在地图存储部21中的地图保存于地图保存部22的处理。
首先,控制部10判定从上次将环境地图保存于地图保存部22起是否经过了规定时间(例如1小时)(步骤S601)。如果没有经过规定时间(步骤S601:否),则结束环境地图保存处理。如果经过了规定时间(步骤S601:是),则控制部10通过摄像部33拍摄图像(步骤S602)。然后,控制部10通过对图像中的亮度高的区域的数量进行计数,来取得点亮的电灯的数量(步骤S603)。此外,图像中的亮度高的区域具体地是指具有规定的亮度基准值以上的明亮度的区域。摄像部33具备能够对从前方至上部的大范围进行拍摄的广角透镜,因此,天花板包含在摄像范围内,能够拍摄可判别天花板的电灯的数量的图像。在步骤S603中,控制部10作为环境信息取得部11发挥功能。
然后,控制部10将取得的点亮电灯数作为环境信息写入地图存储部21(步骤S604)。接着,控制部判定一个以上的环境地图是否保存于地图保存部22(步骤S605)。
如果在地图保存部22中保存有一个以上的环境地图(步骤S605:是),则控制部10从地图保存部22中保存的环境地图中选择用于标准化的基准的环境地图(以下称为“基准地图”。)(步骤S606)。例如,选择最初保存于地图保存部22的环境地图设为“基准地图”。然后,地图标准化部13进行环境地图标准化处理,所述环境地图标准化处理以在步骤S606中选择的“基准地图”为基准,对地图存储部21中存储的环境地图进行标准化(步骤S607)。在此,标准化是指将环境地图的坐标轴、原点以及标度与“基准地图”相匹配。如后所述,环境地图标准化处理是取进行标准化的环境地图(以下称为“对象地图”。)和作为标准化的基准的“基准地图”这两个自变量的子程序。环境地图标准化处理的详细情况将后述。
然后,控制部10判定环境地图标准化处理是否成功(步骤S608)。如果环境地图标准化处理失败(步骤S608:否),则控制部10清除地图存储部21,从初始状态开始SLAM处理,将变量MODE设置为TRACKING(步骤S610),结束环境地图保存处理。如果环境地图标准化处理成功(步骤S608:是),则控制部10将在地图存储部21中存储的标准化的环境地图(“对象地图”)保存于地图保存部22(步骤S609)。然后,结束环境地图保存处理。
另一方面,如果在地图保存部22中没有保存任何环境地图(步骤S605:否),则控制部10将地图存储部21中存储的环境地图直接(不需要进行标准化处理)保存于地图保存部22(步骤S609)。然后,结束环境地图保存处理。
接着,参照图12说明在环境地图保存处理(图11)的步骤S607中调用的环境地图标准化处理。如上所述,该环境地图标准化处理取进行标准化的环境地图以及作为标准化的基准的环境地图这两个自变量,因此,以下,以进行标准化的环境地图为“对象地图”,以作为标准化的基准的环境地图为“基准地图”来进行说明。
首先,控制部10将0代入作业用变量n以及m(步骤S701)。变量m是用作在按顺序地逐个处理“对象地图”中包含的关键帧时的索引的变量。变量n是为了对在“对象地图”中包含的关键帧中,使用”基准地图”估计该关键帧的姿势(在拍摄该关键帧时的自主移动装置100的姿势)的成功数进行计数而使用的变量。
接着,控制部10对变量m加上1(步骤S702)。然后,控制部10判定对于“对象地图”中包含的所有关键帧,处理(后述的估计关键帧的姿势的处理)是否结束(步骤S703)。如果对于“对象地图”中包含的所有关键帧的处理未结束(步骤S703:否),则控制部10对于“对象地图”的第m个关键帧,基于“基准地图”进行姿势估计处理(步骤S704)。如上所述,姿势估计处理是取图像A、环境地图B以及标记变量isReloc这三个自变量的子程序,在此,分别在图像A中设置“对象地图”的第m个关键帧,在环境地图B中设置“基准地图”,在标记变量isReloc中设置false,调用姿势估计处理。
然后,控制部10判断在步骤S704中调用的姿势估计处理在姿势估计中是否成功(步骤S705)。如果姿势估计不成功(步骤S705:否),则返回步骤S702。如果姿势估计成功(步骤S705:是),则保存姿势估计结果等(步骤S706)。具体而言,将基于“基准地图”对“对象地图”的第m个关键帧进行姿势估计的结果即矩阵Pa代入到排列变量PA[n],将“对象地图”的第m个关键帧的3D姿势代入到排列变量PX[n]。然后,对变量n加上1(步骤S707),返回步骤S702。
另一方面,如果对于“对象地图”中包含的所有关键帧结束了处理(步骤S703:是),则控制部10判定变量n是否为0(步骤S708)。如果变量n为0(步骤S708:是),则对于“对象地图”中包含的所有关键帧基于“基准地图”的姿势估计失败,在这种情况下,由于不能用“基准地图”对“对象地图”进行标准化,作为环境地图的标准化失败(步骤S709),结束环境地图标准化处理。在这种情况下,控制部10废弃“对象地图”,使得以后不利用。
如果变量n不为0(步骤S708:否),则控制部10算出从“对象地图”看到的“基准地图”的标度S(从“对象地图”向“基准地图”的标度S)(步骤S710)。具体而言,用以下的式(1)求出标度S。
S=sel(std(pos(PA[]))/std(pos(PX[])))…(1)
在此,pos()是从姿势矩阵组中包含的各姿势矩阵选出位置矩阵并且返回由选出的位置矩阵构成的位置矩阵组的函数。位置矩阵组是排列了多个(n个)由x、y、z这三个元素构成的位置矩阵(列向量)的矩阵组。此外,std()是返回标准偏差的函数,该标准偏差是对于作为自变量的矩阵组中包含的n个位置矩阵的三个元素即x、y、z中的每一个根据n个值求出的。通过std(pos(PA[])/std(pos(PX[])),对于三个元素(x、y、z)的每一个求出根据PA求得的标准偏差与根据PX求得的标准偏差之比。而且,sel()是选择在对于这三个元素的每一个求出的三个比内的最大值的函数。即,标度S成为在作为位置矩阵的三个元素的x、y、z的各标准偏差之比内,标准偏差之比最大的值。
但是,式(1)是假设自主移动装置100在平面上行驶的情况下的式。在自主移动装置100能够在三维空间中自由移动的情况下,需要在姿势矩阵组PA和姿势矩阵组PX中改变朝向,因此,例如,能够取出姿势矩阵组PA的旋转矩阵部分RA和姿势矩阵组PX的旋转矩阵部分RX,通过以下的式(2)来求出标度S。在此,tr()是取转置矩阵的函数(在旋转矩阵中,由于逆矩阵是转置矩阵,所以在式(2)中为了求逆矩阵而求出转置矩阵)。另外,RA是与在pos(PA[])中取出的位置矩阵对应的旋转矩阵,RX是与在pos(PX[])中取出的位置矩阵对应的旋转矩阵。
S=sel(std(pos(PA[]))/std(RA·tr(RX)·pos(PX[])))…(2)
接着,控制部10从姿势矩阵组PX和姿势矩阵组PA算出姿势变换矩阵组PA’(步骤S711)。具体而言,由于姿势矩阵组PA表示“基准地图”的坐标内的、“对象地图”的各关键帧的3D姿势,所以对于“对象地图”的各关键帧的3D姿势(姿势矩阵组PX),通过以下的式(3)的计算,能够求出从进行了标度修正的“对象地图”向“基准地图”的姿势变换矩阵组PA’.此外,在式(3)中,inv()表示求出逆矩阵的函数。
PA’=PA·inv(S·PX)…(3)
由于姿势矩阵组PA以及姿势矩阵组PX中的每一个都包含n个姿势矩阵,所以通过对各姿势矩阵组中包含的n个各姿势矩阵中的每一个进行式(3)的计算,得到包含n个姿势变换矩阵的姿势变换矩阵组PA’.但是,由于姿势变换矩阵组PA’所包含的姿势变换矩阵在步骤S704中作为isReloc=false调用姿势估计处理,因此认为姿势估计处理(图10)的步骤S510的判定中的阈值被设定得较低,包含较多的误差。
因此,控制部10进行姿势变换矩阵的误差修正处理(步骤S712)。该误差修正处理的详细情况将后述。然后,控制部10根据进行了误差修正处理的姿势变换矩阵P和标度S,对“对象地图”中包含的关键帧信息组以及MapPoint信息组进行变换(步骤S713)。具体而言,将“对象地图”中包含的关键帧信息的3D姿势PX0用以下的式(4)变换为标准化的3D姿势PS,将各MapPoint信息的3D坐标MX0用以下的式(5)变换为标准化的3D坐标MS。
PS=P·S·PX0…(4)
MS=P·S·MX0…(5)
然后,作为环境地图的标准化成功(步骤S714),结束环境地图标准化处理。在上述处理中通过计算求出标度S,但在通过机械测距仪等得知绝对标度的情况下,也可以在SLAM的初始化时、环境地图的保存时等使标度匹配。如果在多个环境地图之间标度匹配,则不需要环境地图标准化处理(图12)的步骤S710中的算出标度S的处理,作为标度S=1进行处理即可。
接着,参照图13说明在环境地图标准化处理(图12)的步骤S712中执行的姿势变换矩阵的误差修正处理。
首先,控制部10算出姿势变换矩阵组PA’中包含的n个姿势变换矩阵的中值,并设为姿势变换矩阵P0(步骤S731)。如上所述,姿势变换矩阵由于以齐次坐标形式表示,因此由旋转矩阵和位置矩阵构成。在算出姿势变换矩阵的中值时,只要分别求出旋转矩阵的中值和位置矩阵的中值即可。由于位置矩阵是线性的,因此位置矩阵的中值只要求出位置矩阵的每个元素(x、y、z的每一个)的中值即可。由于旋转矩阵为非线性,因此为了容易地算出旋转矩阵的中值,进行如下处理:将旋转矩阵变换为四元数(Quaternions)并投影到线性空间,在线性空间内求出中值,然后将四元数变换为旋转矩阵并返回到原来的非线性空间。
例如,通过用Exponential Map(指数映射)来处理旋转矩阵,能够将旋转矩阵处理作为线性空间处理。具体而言,首先,将旋转矩阵变换为四元数q。关于从旋转矩阵向四元数的变换以及从四元数向旋转矩阵的变换由于是公知技术,因此省略此处的说明。由于四元数q具有w、x、y、z这四个元素(q=w+xi+yj+zk),因此能够考虑以这四个元素为四轴的四维空间中的以原点为中心的半径为1的超球体(w2+x2+y2+z2=1)。将该超球体的接触空间称为Exponential Map。Exponential Map是与四维空间上的超球体接触的三维空间,是线性空间。
如果用q.w+q.x·i+q.y·j+q.z·k来表示四元数q,则从四元数q向ExponentialMap(expmap)的变换例如能够利用下面的式(6)以及式(7)来表示。在此,acos()表示逆余弦函数,sin()表示正弦函数。
θ0=acos(q.w)…(6)
expmap=θ0/sin(θ0)·[q.x,q.y,q.z]…(7)
根据将姿势变换矩阵组PA’所包含的n个姿势变换矩阵的各旋转矩阵部分变换为四元数q而得到的矩阵,通过式(6)以及式(7),能够算出n个expmap。在此算出的expmap具有x、y、z这三个元素,所以在这三个元素的每一个取中值,设为expmap’。于是,通过以下的式(8)、式(9)以及式(10),能够返回到四元数q’(=q’.w+q’.x·i+q’.y·j+q’.z·k)。在此norm()表示返回欧几里德模量的函数,cos()表示余弦函数。
θ1=norm(expmap’)…(8)
q’.w=cos(θ1)…(9)
[q’.x,q’.y,q’.z]=sin(θ1)·expmap’/θ1…(10)
然后,将四元数q’变换为旋转矩阵,并将中值与已算出的位置矩阵合在一起设为姿势变换矩阵P0。另外,在上述例子中,在算出姿势变换矩阵P0时,使用了n个姿势变换矩阵的中值,但是使用中值只不过是一例,例如也可以使用平均值算出姿势变换矩阵P0。或者,也可以使用RANSAC(Random Sample Consensus,随机抽样一致算法)来选择支持的关键帧多的姿势变换矩阵(姿势接近的姿势变换矩阵),或者选择支持的MapPoint多的姿势变换矩阵(位置接近的姿势变换矩阵)。
上述“选择支持的关键帧多的姿势变换矩阵(姿势接近的姿势变换矩阵)”是指“选择姿势变换矩阵组PA’包含的n个姿势变换矩阵中的如下的姿势变换矩阵:在变换“对象地图”的关键帧的3D姿势时,与“基准地图”的关键帧的3D姿势一致(或者规定的基准以上相似)的关键帧的数量比规定的阈值多的姿势变换矩阵”。在此,在选择多个姿势变换矩阵的情况下,可以取所选择的姿势变换矩阵的中值或平均值,也可以选择上述那样的(一致或规定的基准以上相似的)关键帧的数量最多的姿势变换矩阵。
另外,上述的“选择支持的MapPoint多的姿势变换矩阵(位置接近的姿势变换矩阵)”是指“选择姿势变换矩阵组PA’包含的n个姿势变换矩阵中如下的姿势变换矩阵:在变换“对象地图”的MapPoint的3D坐标时,与“基准地图”的MapPoint的3D坐标一致(或者两点间的距离为规定的基准距离以下)的MapPoint的数量比规定的阈值多的姿势变换矩阵”。在此,在选择多个姿势变换矩阵的情况下,可以取所选择的姿势变换矩阵的中值或平均值,也可以选择上述那样的(两点间的距离为规定的基准距离以下)的MapPoint的数量最多的姿势变换矩阵。
然后,控制部10判定n(姿势变换矩阵组PA’中包含的姿势变换矩阵的数量)是否大于规定的阈值N1(例如,设定为“对象地图”中包含的关键帧的数的30%左右的值)(步骤S732)。如果n大于N1(步骤S732:是),则判断为在姿势变换矩阵P0中包含的误差小,将姿势变换矩阵P0作为姿势变换矩阵P(步骤S733),结束误差修正处理,前进到环境地图标准化处理(图12)的步骤S713。
如果n为N1以下(步骤S732:否),则为了使姿势变换矩阵P0的误差变小,还利用MapPoint。因此,控制部10将用姿势变换矩阵P0以及标度S将“对象地图”中包含的各MapPoint的3D坐标变换为“基准地图”的坐标的点组设为M1(步骤S734)。具体而言,如果用M0表示“对象地图”的MapPoint组(的各3D坐标),则根据姿势变换矩阵P0以及标度S,通过以下的式(11)算出M1。由于M0包含多个MapPoint的3D坐标,因此M1成为包含与M0相同数量的点的3D坐标的点组。
M1=P0·S·M0…(11)
然后,如果用MB表示“基准地图”的MapPoint组(的各3D坐标),则控制部10使用ICP(Iterative Closest Point,迭代最近点)处理算出表示从点组M1向点组MB的姿势变化的姿势变换矩阵P1(步骤S735)。ICP处理的概要将后述。
然后,控制部10通过式(12)根据姿势变换矩阵P0和姿势变换矩阵P1算出姿势变换矩阵P(步骤S736),结束误差修正处理,前进到环境地图标准化处理(图12)的步骤S713。
P=P1·P0…(12)
接着,参照图14说明在误差修正处理(图13)的步骤S735中执行的ICP处理。ICP处理取两个点组作为自变量,因此分别设为点组T0以及点组T1。如果在误差修正处理(图13)的步骤S735中调用ICP处理,则分别将点组M1代入点组T0,将点组MB代入点组T1,从而开始处理。此外,一般而言,ICP处理存在初始值依赖性高的问题,但如上所述,赋予利用姿势变换矩阵P0对“对象地图”的MapPoint组进行了变换的点组M1作为初始值,因此初始值中包含的误差变小,能够避免初始值依赖性问题。
首先,控制部10分别将最大循环数(例如10)代入作业用变量L,将0代入作业用变量ct,作为从应该求出的点组T0向点组T1的姿势变换矩阵P01的初始值,代入旋转矩阵部分为单位矩阵且位置矩阵部分为0向量的齐次坐标形式的矩阵(步骤S751)。
然后,控制部10在点组T0的各点中,根据点组T1求出距离最短的点并取得对应(步骤S752)。接着,控制部10基于点组T0和点组T1的对应,算出从点组T0向点组T1的姿势变换矩阵,设为姿势变换矩阵Ptmp(步骤S753)。
然后,控制部10将姿势变换矩阵P01以及点组T0更新为分别用姿势变换矩阵Ptmp变换后的矩阵,并对作业用变量ct加上1(步骤S754)。然后,控制部10判定步骤S754中的姿势变换矩阵P01的更新量(利用姿势变换矩阵Ptmp对姿势变换矩阵P01进行变换的前后的差值)是否为规定的既定值以下(步骤S755)。
如果姿势变换矩阵P01的更新量为既定值以下(步骤S755:是),则判断为姿势变换矩阵P01收敛,控制部10将姿势变换矩阵P01代入作为ICP处理的返回值的姿势变换矩阵P1(步骤S756),结束ICP处理。
如果姿势变换矩阵P01的更新量超过既定值(步骤S755:否),则控制部10判定作业用变量ct是否小于最大循环数L(步骤S757)。然后,如果作业用变量ct小于最大循环数L(步骤S757:是),则返回步骤S752,重复进行姿势变换矩阵P01的更新。
如果作业用变量ct为最大循环数L以上(步骤S757:否),则判断为即使在此以上反复更新也不能充分收敛,控制部10将姿势变换矩阵P01代入作为ICP处理的返回值的姿势变换矩阵P1(步骤S756),结束ICP处理。
通过以上说明的姿势变换矩阵的误差修正处理以及ICP处理,能够减小姿势变换矩阵P的误差,环境地图标准化处理能够使用误差小的姿势变换矩阵P,用“基准地图”对“对象地图”进行标准化。
然后,通过上述的环境地图标准化处理,在地图保存部22中,每隔规定时间将附加了环境信息的环境地图的数据用“基准地图”标准化后保存。因此,自主移动装置100能够统一地处理多个环境地图,因此能够进行对于环境的变化而言鲁棒性的自身位置估计。
此外,在上述的姿势变换矩阵的误差修正处理(图13)中,在步骤S732中,控制部10判定n是否大于规定的阈值N1,如果n为N1以下,则还利用MapPoint进行了使姿势变换矩阵P0的误差减小的处理,但也可以不进行该处理。即,也可以将通过从包含n个姿势变换矩阵的姿势变换矩阵组PA’求出中值等而求出的姿势变换矩阵P0设为最终的姿势变换矩阵P。
(实施方式1的变形例1)
在实施方式1中,当在环境地图保存处理(图11)的步骤S606中选择“基准地图”(用作标准化的基准的环境地图)时,举出了选择将最初保存于地图保存部22的环境地图作为“基准地图”的例子。但是,“基准地图”的选择方法并不限于此。作为其他方法,例如,还考虑在地图保存部22中保存的环境地图中,选择规模大的环境地图作为“基准地图”的方法。在此,规模大意味着环境地图中包含的关键帧的数量和MapPoint的数量多。对这样选择规模大的环境地图作为“基准地图”的实施方式1的变形例1进行说明。
在实施方式1中,由于将最初保存于地图保存部22的环境地图作为“基准地图”使用,新制作出的环境地图也全部与最初保存的环境地图(“基准地图”)和坐标轴、原点以及标度匹配地保存。即,确定了一个“基准地图”,就不发生变化。与此相对,在实施方式1的变形例1中,如果在地图保存部22中存储比过去保存的环境地图规模大的环境地图,则在之后的环境地图保存处理中选择的“基准地图”成为与在其之前的“基准地图”不同的环境地图。即,存在“基准地图”发生变化的可能性。
因此,如图15所示,实施方式1的变形例1的环境地图保存处理导入表示当前的“基准地图”的地图ID的变量KMAP,如果“基准地图”发生变化,则进行用新的“基准地图”重新将地图保存部22中保存的其他环境地图进行标准化的处理。此外,作为使用变量KMAP的事先准备,在启动时处理(图6)的步骤S102的最初,进行初始化变量KMAP(例如设置0作为不存在的地图ID)的处理。
实施方式1的变形例1的环境地图保存处理(图15)成为在实施方式1的环境地图保存处理(图11)中追加了从步骤S621到步骤S624的处理的内容,因此对所追加的各步骤进行说明。
在步骤S621中,控制部10从地图保存部22中保存的环境地图中选择规模大的环境地图作为“基准地图”。例如,选择将环境地图中包含的关键帧的数量和MapPoint的数量之和最大的环境地图作为“基准地图”。在此,也可以不是通过单纯的和的最大值来选择“基准地图”,而是选择将关键帧的数量和MapPoint的数量分别乘以权重后取和的值最大的环境地图作为“基准地图”。
在步骤S622中,控制部10判定变量KMAP中储存的值是否与步骤S621中选择的“基准地图”的地图ID相同。如果相同(步骤S622:是),则前进到步骤S609。在这种情况下,地图保存部22中保存的环境地图已经被“基准地图”进行标准化。
如果变量KMAP中储存的值与“基准地图”的地图ID不同(步骤S622:否),则控制部10分别用“基准地图”将地图保存部22中保存的环境地图(“基准地图”以外的所有环境地图)进行标准化,并重新保存于地图保存部22(步骤S623)。然后,控制部10在变量KMAP中设置“基准地图”的地图ID(步骤S624),前进到步骤S609。
在以上说明的实施方式1的变形例1的环境地图保存处理中,由于选择规模大的环境地图作为“基准地图”,在“基准地图”中包含各种关键帧或MapPoint的可能性变高。因此,例如,包含在不易受照明影响的场所(例如桌子下方等)的关键帧或MapPoint多的可能性变高,能够期待环境地图的标准化变得容易成功。因此,由于能够对更多的环境地图进行标准化,可以实现对于环境的变化更鲁棒性的自身位置估计。
(实施方式1的变形例2)
在上述的实施方式1中,如果Relocalization处理持续失败规定时间以上,则进行SLAM处理的初始化(以及地图存储部21的清除)。然而,SLAM处理的初始化(以及地图存储部21的清除)的定时不限于该定时。例如,在环境地图保存处理(图11)中,也可以将在地图存储部21中存储的环境地图保存于地图保存部之后进行SLAM处理的初始化(以及地图存储部21的清除)。对这样的实施方式1的变形例2进行说明。
在实施方式1的变形例2中,在环境地图保存处理(图11)的步骤S609的处理之后,前进到步骤S610,在步骤S610的处理之后,结束环境地图保存处理。
通过进行上述的处理,在实施方式1的变形例2中,每当将环境地图保存于地图保存部22时,根据初始状态制作新的环境地图,因此,能够将反映最近的环境信息的环境地图保存于地图保存部22。因此,在地图保存部22中保存有针对各种环境信息的环境地图,在重新选择环境地图时,能够提高能够选择更适合于当前环境的环境地图的可能性。
(实施方式1的变形例3)
此外,作为SLAM处理的初始化(以及地图存储部21的清除)的定时还考虑上述以外的定时。例如,在自身位置估计线程(图7)中,也可以在当前环境地图上的Relocalization处理(步骤S218)经过一定时间后仍未成功的情况下,进行SLAM处理的初始化(以及地图存储部21的清除)。对这样的实施方式1的变形例3进行说明。
在实施方式1的变形例3中,导入对Relocalization处理的失败进行计数的变量RFC2,如果自身位置估计线程(图7)的当前环境地图中的Relocalization处理(步骤S218)成功(步骤S219:是),则使变量RFC2为0,如果失败(步骤S219:否),则对变量RFC2加1。然后,如果变量RFC2超过规定的值(例如5),则进行SLAM处理的初始化(以及地图存储部21的清除),并且从步骤S202开始重新进行处理。或者,如果将变量RFC2成为1时的时刻存储于变量RFT2,即使从RFT2开始经过规定的时间(例如5分钟)在当前环境地图中的Relocalization处理持续失败,则进行SLAM处理的初始化(以及地图存储部21的清除),并且从步骤S202开始重新进行处理。
通过进行上述的处理,实施方式1的变形例3能够防止陷入到无论当前环境地图中的Relocalization处理经过多长时间也不成功的状况。
(实施方式1的变形例4)
此外,作为SLAM处理的初始化的定时,例如,在自身位置估计线程(图7)开始时在地图保存部22中存储的环境地图的数量为规定的环境地图基准数以下的情况下,可以进行SLAM处理的初始化(以及地图存储部21的清除)。对这样的实施方式1的变形例4进行说明。
在实施方式1的变形例4中,在自身位置估计线程(图7)的步骤S201中,不判定在地图保存部22中是否保存有环境地图,而是判定是否保存有环境地图基准数(例如五个)以上的环境地图。然后,如果保存有小于环境地图基准数的环境地图(步骤S201:否),则从初始状态开始SLAM。此外,在在Relocalization(步骤S204)失败的情况下,也可以不是返回步骤S204,而是返回步骤S203,从多个环境地图中提取适合于当前环境的可能性高的环境地图。此外,在当前环境地图中的Relocalization(步骤S218)无论几次都失败的情况下,也可以返回步骤S203,从多个环境地图中提取适合于当前环境的可能性高的环境地图。此外,在步骤S204或步骤S218中Relocalization失败的情况下,也可以返回步骤S202,进行SLAM处理的初始化(以及地图存储部21的清除),从重新开始制作环境地图处进行处理。
通过进行上述的处理,实施方式1的变形例4在将某程度的数量的环境地图保存于地图保存部22之前,制作不同的环境地图,因此各种环境下的环境地图保存于地图保存部22可能性提高。然后,如果在Relocalization处理失败的情况下从环境地图提取(步骤S203)开始重新进行处理,则能够提取适合于当前环境的环境地图的可能性提高,其结果是,能够针对环境变化进行鲁棒性的自身位置估计。
(实施方式1的变形例5)
在上述的实施方式中,在在环境地图保存处理(图11)中的环境地图标准化处理中失败的情况下(步骤S608:否),废弃在地图存储部21中存储的环境地图,以后不再使用。然而,例如,在在“基准地图”包含许多异常值的情况下,考虑环境地图标准化处理几乎失败。因此,在标准化的失败持续的情况下,判断为“基准地图”存在异常,控制部10可以从地图保存部22删除“基准地图”。对这样在环境地图的标准化不成功的情况下从地图保存部22删除“基准地图”的实施方式1的变形例5进行说明。
在实施方式1的变形例5中,导入用于对在环境地图保存处理(图11)中标准化连续失败的次数计数的变量FC。而且,如果标准化成功(步骤S608:是),则控制部10将变量FC设为0,前进到步骤S609。另一方面,如果标准化失败(步骤S608:否),则控制部10对变量FC加上1,如果FC的值超过规定的值(例如5),则从地图保存部22中删除“基准地图”。然后,从在地图保存部22中保存的剩余的环境地图中选择一个(例如,规模最大的环境地图)作为“基准地图”,重新标准化在地图保存部22中保存的其他环境地图。然后,尝试用新的“基准地图”对在地图存储部21中存储的环境地图的标准化。如果使用新的“基准地图”对在地图存储部21中存储的环境地图的标准化成功,则将变量FC设为0,前进到步骤S609。
在即使新的“基准地图”而标准化也失败的情况下,由于有可能不是“基准地图”而是在地图存储部21中存储的环境地图存在问题,因此将变量FC设为1,前进到步骤S610。而且之后也是,如果环境地图的标准化持续失败,FC的值超过规定的值(例如5),则从地图保存部22中删除“基准地图”,重复上述的处理。
通过进行以上的处理,在实施方式1的变形例5中,在“基准地图”存在问题的情况下,能够防止陷入环境地图的标准化始终不成功情况。
(实施方式2)
在实施方式1中,能够将在地图保存部22中保存的任意的环境地图设为作为标准化的基准的环境地图(“基准地图”),但算出进行标准化时的姿势变换矩阵P需要计算成本。因此,说明通过导入在对各环境地图进行标准化时的成为基准的姿势(基准姿势),能够抑制计算成本并进行标准化的实施方式2。
实施方式2的自主移动装置101具有与实施方式1的自主移动装置100相同的外观以及结构。但是,如图16所示,在地图存储部21中存储的环境地图成为在实施方式1的环境地图中追加了基准姿势信息的数据结构。基准姿势信息由基准姿势和变换完毕矩阵构成。基准姿势是指在对环境地图进行标准化时设为基准的姿势,通常在规定的场所(例如充电器200的设置场所)处的自动移动装置101的姿势被登记为基准姿势。此外,变换完毕矩阵是指在对环境地图进行标准化时使用的姿势变换矩阵。
而且,基准姿势和变换完毕矩阵都由包含3×3旋转矩阵和3×1位置矩阵的4×4齐次坐标形式的姿势矩阵表示。在环境地图生成开始时(初始化时),基准姿势被初始化为元素全部为0的矩阵(0矩阵),变换完毕矩阵被初始化为旋转矩阵部分为单位矩阵、位置矩阵部分为0矩阵。
另外,自主移动装置101能够使驱动部32作为机械测距仪发挥功能,控制部10能够通过机械测距仪取得移动距离。而且,地图制作部12使用机械测距仪以用米系统表示环境地图中包含的关键帧的3D姿势、MapPoint的3D坐标等的单位(标度)的方式来制作环境地图。也就是说,定义环境地图的长度的单位(标度),以使在环境地图上的单位长度1在对应的实际的3D空间中成为1米。因此,自身位置估计部14估计的自主移动装置101的姿势的单位(标度)也以米系统表示。
在由自主移动装置101启动的各种处理中,环境地图保存处理以及环境地图标准化处理与由自主移动装置100启动的该处理有不同的部分,因此说明这些处理。首先,参照图17说明环境地图保存处理。但是,自主移动装置101的环境地图保存处理与自主移动装置100的环境地图保存处理(图11)共同的部分较多,因此以不同的部分为中心进行说明。
在自主移动装置101的环境地图保存处理中,在步骤S604之后,控制部10判定基准姿势是否登记于在地图存储部21中存储的环境地图(步骤S631)。如果基准姿势为0矩阵,则由于基准姿势未被登记(步骤S631:否),控制部10进行使自主移动装置101返回充电器200的设置场所的控制(步骤S632)。
然后,在返回到充电器200的设置场所之后,控制部10将在充电器200的位置处的自主移动装置101的姿势作为基准姿势写入地图存储部21(步骤S633)。另外,在充电器200的位置处的自主移动装置101的姿势可以通过SLAM处理来算出,也可以通过机械测距仪来求出。
随后的处理与自主移动装置100的环境地图保存处理(图11)基本上相同。但是,如后所述,由于自主移动装置101的环境地图标准化处理(图18)不会标准化失败(始终成功),自主移动装置100的环境地图保存处理(图11)的步骤S608以及步骤S610的处理是不需要的。
接着,参照图18说明自主移动装置101的环境地图标准化处理。该环境地图标准化处理与实施方式1同样地,由于取进行标准化的环境地图以及作为标准化的基准的环境地图这两个自变量,以下说明将进行标准化的环境地图设为“对象地图”,将作为标准化的基准的环境地图设为“基准地图”。
首先,控制部10将“基准地图”的基准姿势代入储存基准姿势的变量SB(步骤S771)。接着,控制部10将“对象地图”的基准姿势代入储存基准姿势的变量SX(步骤S772)。接着,控制部10将“对象地图”的变换完毕矩阵代入到储存变换完毕矩阵的变量P0(步骤S773)。
然后,控制部10基于“基准地图”的基准姿势SB、“对象地图”的基准姿势SX以及“对象地图”的变换完毕矩阵P0,算出从“对象地图”向“基准地图”的姿势变换矩阵P(步骤S774)。具体而言,通过以下的式(13)求出姿势变换矩阵P。在此,inv()是求出逆矩阵的函数。
P=SB·inv(SX)·inv(P0)…(13)
接着,控制部10通过姿势变换矩阵P对“对象地图“中包含的关键帧信息组以及MapPoint信息组进行变换(步骤S775)。具体而言,将“对象地图”中包含的各关键帧信息的3D姿势PX0使用以下的式(14)变换为标准化的3D姿势PS,将各MapPoint信息的3D坐标MX0使用以下的式(15)变换为标准化的3D坐标MS。
PS=P·PX0…(14)
MS=P·MX0…(15)
然后,控制部10将姿势变换矩阵P作为“对象地图”的变换完毕矩阵写入地图存储部21(步骤S776),结束环境地图标准化处理。
上述的处理是自主移动装置101通过机械测距仪使所有的环境地图的标度设为一致而省略了标度S的计算的处理。但是,在标度有可能根据环境地图而变动的情况下,也可以通过与实施方式1同样的处理,通过计算求出标度S。但是,在这种情况下,当使用上述的式(14)或式(15)计算时,需要进一步乘以标度S。
另外,在上述的处理中,将充电器200的位置处的自主移动装置101的姿势作为基准姿势,但这只不过是基准姿势的一例。例如,也可以将在电视机的正面从电视机离开规定的距离的位置处的自主移动装置101的姿势作为基准姿势。在这种情况下,根据通过摄像部33拍摄的图像,通过一般的图像识别来识别电视机的正面。并且,例如,在房间中任意移动时,能够将在拍摄到的图像中的特定的位置识别为特定的大小的电视时的自主移动装置101的姿势设为基准姿势。
以上说明的实施方式2的自主移动装置101利用基准姿势进行环境地图的标准化(使坐标系一致),因此需要在保存环境地图之前取得基准姿势。然而,在实施方式2中,由于仅使用基准姿势来进行矩阵的运算就能够算出用于标准化的姿势变换矩阵,因此不存在标准化失败的风险,具有能够抑制计算成本并且可靠地标准化的优点。
此外,在上述的实施方式中,环境地图保存处理每隔规定的时间(例如1小时)将在地图存储部21中存储的地图保存于地图保存部22,但该“每隔规定的时间”这样的判定只不过是一例。例如,也可以采用“周围的明亮度变化了规定的量以上”这样的判定。在这种情况下,环境地图保存处理(图11、15、17)的步骤S601中的判定成为“环境信息是否从上次地图保存时的环境信息变化了规定的量以上的判定”。
另外,在上述的实施方式中,将点亮的电灯数作为环境信息,但这只不过是环境信息的一例。例如,作为环境信息,也可以采用由“电灯的点亮的有无”和“周围的明亮度”构成的二维向量值。在这种情况下,环境信息的二维向量的最初的值(天花板的电灯的开启或关闭)设为如果电灯开启则为1、如果关闭则为0这二值,环境信息的二维向量的第二个值(周围的明亮度)设为将摄像部33拍摄到的图像中包含的所有像素值的平均值或中值即可。
而且,在环境地图提取处理(图8)中,也可以在通过图像拍摄得到的环境信息的二维向量和分别附加于在地图保存部22中保存的环境地图的环境信息的二维向量之间求出相似度,按照相似度的降序提取N个候选环境地图。此外,例如,二维向量之间的相似度能够通过将向量的每一个的范数标准化为1然后取内积来得到。
此外,在使用更多信息作为环境信息的情况下,也能够通过增加表示环境信息的向量的维数来应对。而且,即使向量的维数增加,也能够通过根据将成为相似度计算的对象的两个向量的范数标准化为1取内积来算出环境信息的相似度。此外,尽管在上述的例子中示出了由向量表示环境信息的例子,但是环境信息不需要必须由向量表示,也可以采用其他数据结构。
另外,在上述的实施方式中,作为环境信息说明了照明变化(照明变动)的影响,但是照明变动特别是指照明方向、照明位置改变。具体而言,是电灯的开/关、从窗户进入的阳光由于太阳的位置而改变、百叶窗的打开/关闭等。
由于考虑到如果产生照明变动则拍摄到的图像的特征量变化,对自主移动装置100、101的位置估计造成影响,在上述的实施方式中,作为环境信息,以照明的变化为例进行了说明。但是,环境信息不限于此。在环境信息中能够包含成为特征点的结构物的位置变化等。例如,在仓库中物品的位置或量周期性地改变很大的情况下,环境信息中可以包含物品的位置或量。
在这种情况下,控制部10也可以从通过摄像部33拍摄的图像中,通过一般的图像识别来取得物品的位置或量的信息。另外,控制部10也可以经由通信部34与进行仓库的库存管理的外部的库存管理系统等通信,从该库存管理系统等取得物品的位置或量的信息。如果在环境信息中包含物品的量,则在仓库中装满物品时和仓库中物品少时,环境信息不同,控制部10分别制作其他的环境地图,因此对于物品的量的变化能够进行鲁棒性的自身位置估计。
另外,上述的实施方式以及变形例能够适当地组合。例如,通过将实施方式1的变形例2、变形例3以及变形例4进行组合,在地图保存部22中对各种环境的下的环境地图进行标准化并保存,在Relocalization处理失败时,能够从该各种环境地图中提取适合于当前环境且标准化后的环境地图,因此对于环境的变化能够进行更鲁棒性的自身位置估计。
此外,自主移动装置100、101的各功能也能够通过通常的PC(Personal Computer,个人计算机)等计算机来实施。具体而言,在上述实施方式中,说明了将自主移动装置100、101进行的自主移动控制处理的程序预先存储于存储部20的ROM的情况。但是,也可以将程序储存于软盘、CD-ROM(Compact Disc Read Only Memory)、DVD(Digital VersatileDisc)、MO(Magneto-Optical Disc)、存储卡、USB(Universal Serial Bus)存储器等计算机可读取的记录介质中进行分发,通过将该程序读入计算机并安装,构成能够实现上述各功能的计算机。
以上,对本发明的优选实施方式进行了说明,但本发明并不限定于的特定的实施方式,本发明包含权利要求书所记载的发明及其等同的范围。
Claims (16)
1.一种自主移动装置,其特征在于,
具备控制部和存储部,
所述控制部使用拍摄到的图像,根据周围环境的变化制作多个环境地图,
将所述制作出的多个环境地图进行标准化并以能够统一处理的方式保存于所述存储部,
使用所述标准化后的环境地图来估计所述自主移动装置的位置。
2.如权利要求1所述的自主移动装置,其特征在于,
所述控制部从所述多个环境地图中选择作为用于所述标准化的基准的环境地图的基准地图,
基于所述基准地图,将所述多个环境地图中的对象地图进行标准化,所述对象地图是作为以所述基准地图进行标准化的对象的环境地图。
3.如权利要求2所述的自主移动装置,其特征在于,
所述环境地图包含关键帧的信息和MapPoint即地图点的信息,所述关键帧是由所述摄像部拍摄的图像中的用于位置的估计的图像,所述MapPoint是由所述摄像部拍摄的图像中包含的作为特征性点的特征点之中、被估计出三维位置的坐标的特征点,并且,作为所述关键帧的信息,包含所述关键帧被拍摄时的所述自主移动装置的姿势的信息和所述关键帧中包含的所述特征点的信息,
所述控制部通过将所述对象地图中包含的所述姿势的信息以及所述MapPoint的信息变换为所述基准地图的坐标系中的值,对所述对象地图进行标准化。
4.如权利要求3所述的自主移动装置,其特征在于,
所述控制部基于所述关键帧被拍摄时的所述自主移动装置的姿势,算出从所述对象地图向所述基准地图的姿势变换矩阵,所述关键帧被拍摄时的所述自主移动装置的姿势是通过所述对象地图中包含的所述关键帧和与所述关键帧相似的所述基准地图中包含的关键帧的对应而估计出的,
通过所述姿势变换矩阵对所述环境地图进行标准化。
5.如权利要求4所述的自主移动装置,其特征在于,
所述控制部通过使用所述对象地图中包含的多个所述关键帧的每一个来算出所述姿势变换矩阵,算出由多个姿势变换矩阵构成的姿势变换矩阵组,
通过使用所述姿势变换矩阵组中包含的多个姿势变换矩阵来算出使误差减小的一个姿势变换矩阵。
6.如权利要求5所述的自主移动装置,其特征在于,
所述控制部通过算出所述姿势变换矩阵组中包含的多个姿势变换矩阵的中值,算出所述一个姿势变换矩阵。
7.如权利要求6所述的自主移动装置,其特征在于,
所述控制部在算出所述多个姿势变换矩阵的中值时,将从所述多个姿势变换矩阵的每一个取出的旋转矩阵变换为四元数,将所述四元数投影到线性空间内并在线性空间内算出中值。
8.如权利要求5所述的自主移动装置,其特征在于,
如果所述姿势变换矩阵组中包含的姿势变换矩阵的数量为规定的阈值以下,则所述控制部使用所述环境地图中包含的所述MapPoint,减小所述一个姿势变换矩阵的误差。
9.如权利要求4所述的自主移动装置,其特征在于,
所述控制部在不能算出从所述对象地图向所述基准地图的姿势变换矩阵的情况下,删除所述对象地图,开始新的环境地图的制作。
10.如权利要求3所述的自主移动装置,其特征在于,
所述环境地图还包含作为标准化时成为基准的姿势的基准姿势的信息,
所述控制部基于所述基准姿势,算出从所述对象地图向所述基准地图的姿势变换矩阵,
根据所述姿势变换矩阵,将所述对象地图进行标准化。
11.如权利要求10所述的自主移动装置,其特征在于,
所述控制部在将所述对象地图进行标准化之前,判定所述基准姿势是否登记于所述对象地图,如果所述基准姿势未登记,则使所述自主移动装置移动至登记基准姿势的规定场所,在所述规定场所将所述基准姿势登记于所述对象地图。
12.如权利要求11所述的自主移动装置,其特征在于,
所述登记基准姿势的规定场所是充电器的设置场所。
13.如权利要求3所述的自主移动装置,其特征在于,
所述控制部,基于从所述对象地图所包含的姿势的信息取出的位置向量的各元素的标准偏差与从所述基准地图中包含的姿势的信息取出的位置向量的各元素的标准偏差之比,算出从所述对象地图向所述基准地图的长度之比即标度,
还使用所述标度将所述环境地图进行标准化。
14.如权利要求1至13中任一项所述的自主移动装置,其特征在于,
所述周围环境的变化是照明的变化。
15.一种自主移动装置的自主移动方法,其特征在于,
使用拍摄到的图像,根据周围环境的变化制作多个环境地图,
将所述制作出的多个环境地图进行标准化并以能够统一处理的方式保存于存储部,
通过使用所述标准化后的环境地图来估计所述自主移动装置的位置。
16.一种存储程序的非暂时性存储介质,其特征在于,
所述程序使自主移动装置的计算机执行如下处理:
使用拍摄到的图像,根据周围环境的变化制作多个环境地图,
将所述制作出的多个环境地图进行标准化并以能够统一处理的方式保存于存储部,
使用所述标准化后的环境地图来估计所述自主移动装置的位置。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018-144206 | 2018-07-31 | ||
JP2018144206A JP7139762B2 (ja) | 2018-07-31 | 2018-07-31 | 自律移動装置、自律移動方法及びプログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110850863A true CN110850863A (zh) | 2020-02-28 |
CN110850863B CN110850863B (zh) | 2023-05-23 |
Family
ID=69227444
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910686631.6A Active CN110850863B (zh) | 2018-07-31 | 2019-07-29 | 自主移动装置、自主移动方法以及存储介质 |
Country Status (3)
Country | Link |
---|---|
US (1) | US20200042010A1 (zh) |
JP (1) | JP7139762B2 (zh) |
CN (1) | CN110850863B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6893200B2 (ja) * | 2018-10-16 | 2021-06-23 | 株式会社きんでん | 自走制御プログラム、および、自走式作業装置 |
CN110110604A (zh) * | 2019-04-10 | 2019-08-09 | 东软集团股份有限公司 | 目标对象检测方法、装置、可读存储介质及电子设备 |
CN112148742A (zh) * | 2019-06-28 | 2020-12-29 | Oppo广东移动通信有限公司 | 地图更新方法及装置、终端、存储介质 |
CN115210668A (zh) * | 2020-03-13 | 2022-10-18 | 千叶工业大学 | 自己位置推定装置 |
US20230245341A1 (en) * | 2020-06-19 | 2023-08-03 | Nec Corporation | Positioning device, estimation method, and non-transitory computer-readable medium |
EP3929613A1 (en) * | 2020-06-22 | 2021-12-29 | Carnegie Robotics, LLC | A method for navigating a movable device along an inclined surface |
CN112819782B (zh) * | 2021-01-29 | 2023-03-24 | 菱王电梯有限公司 | 一种自学习的轿厢视觉分析方法 |
WO2022172831A1 (ja) * | 2021-02-10 | 2022-08-18 | 株式会社Preferred Networks | 情報処理装置 |
US11521332B1 (en) | 2021-06-29 | 2022-12-06 | Midea Group Co., Ltd. | Method and apparatus for optimization of a monocular visual-inertial localization system |
US11756231B2 (en) * | 2021-06-29 | 2023-09-12 | Midea Group Co., Ltd. | Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system |
DE102021130631A1 (de) * | 2021-11-23 | 2023-05-25 | Still Gesellschaft Mit Beschränkter Haftung | Verfahren und System zur Navigation eines Flurförderzeugs |
CN114131599B (zh) * | 2021-11-30 | 2023-08-29 | 珠海格力电器股份有限公司 | 一种机器人编程控制方法、装置、存储介质及示教器 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2006337192A (ja) * | 2005-06-02 | 2006-12-14 | Nec Fielding Ltd | 道案内システムおよび道案内システム用プログラム |
JP2007322138A (ja) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | 移動装置及び移動装置の自己位置推定方法 |
CN102129290A (zh) * | 2010-01-12 | 2011-07-20 | 索尼公司 | 图像处理设备、对象选择方法和程序 |
CN102194367A (zh) * | 2010-03-09 | 2011-09-21 | 索尼公司 | 信息处理设备、地图更新方法、程序以及信息处理系统 |
JP2014186693A (ja) * | 2013-03-25 | 2014-10-02 | Murata Mach Ltd | 自律移動式無人搬送車 |
CN104756155A (zh) * | 2012-10-31 | 2015-07-01 | 高通股份有限公司 | 合并多个地图以用于基于计算机视觉的跟踪的系统及方法 |
JP2016103269A (ja) * | 2014-11-12 | 2016-06-02 | 村田機械株式会社 | 移動量推定装置、自律移動体、及び移動量の推定方法 |
CN106896807A (zh) * | 2015-12-17 | 2017-06-27 | 卡西欧计算机株式会社 | 自主移动装置以及自主移动方法 |
JP2017167625A (ja) * | 2016-03-14 | 2017-09-21 | カシオ計算機株式会社 | 自律移動装置、自律移動システム、自律移動方法及びプログラム |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5959899B2 (ja) * | 2012-03-30 | 2016-08-02 | 本田技研工業株式会社 | 接触状態推定装置 |
JP6445808B2 (ja) * | 2014-08-26 | 2018-12-26 | 三菱重工業株式会社 | 画像表示システム |
-
2018
- 2018-07-31 JP JP2018144206A patent/JP7139762B2/ja active Active
-
2019
- 2019-07-29 CN CN201910686631.6A patent/CN110850863B/zh active Active
- 2019-07-30 US US16/525,946 patent/US20200042010A1/en not_active Abandoned
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2006337192A (ja) * | 2005-06-02 | 2006-12-14 | Nec Fielding Ltd | 道案内システムおよび道案内システム用プログラム |
JP2007322138A (ja) * | 2006-05-30 | 2007-12-13 | Toyota Motor Corp | 移動装置及び移動装置の自己位置推定方法 |
CN102129290A (zh) * | 2010-01-12 | 2011-07-20 | 索尼公司 | 图像处理设备、对象选择方法和程序 |
CN102194367A (zh) * | 2010-03-09 | 2011-09-21 | 索尼公司 | 信息处理设备、地图更新方法、程序以及信息处理系统 |
CN104756155A (zh) * | 2012-10-31 | 2015-07-01 | 高通股份有限公司 | 合并多个地图以用于基于计算机视觉的跟踪的系统及方法 |
JP2014186693A (ja) * | 2013-03-25 | 2014-10-02 | Murata Mach Ltd | 自律移動式無人搬送車 |
JP2016103269A (ja) * | 2014-11-12 | 2016-06-02 | 村田機械株式会社 | 移動量推定装置、自律移動体、及び移動量の推定方法 |
CN106896807A (zh) * | 2015-12-17 | 2017-06-27 | 卡西欧计算机株式会社 | 自主移动装置以及自主移动方法 |
JP2017167625A (ja) * | 2016-03-14 | 2017-09-21 | カシオ計算機株式会社 | 自律移動装置、自律移動システム、自律移動方法及びプログラム |
Also Published As
Publication number | Publication date |
---|---|
CN110850863B (zh) | 2023-05-23 |
JP7139762B2 (ja) | 2022-09-21 |
JP2020021257A (ja) | 2020-02-06 |
US20200042010A1 (en) | 2020-02-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110850863B (zh) | 自主移动装置、自主移动方法以及存储介质 | |
JP7150773B2 (ja) | 移動ロボット及びその制御方法 | |
US11561549B2 (en) | Method of controlling mobile robot | |
US9798957B2 (en) | Robot cleaner and control method thereof | |
WO2020223974A1 (zh) | 更新地图的方法及移动机器人 | |
EP3349087B1 (en) | Moving robot | |
KR101538775B1 (ko) | 전방 영상을 이용한 위치 인식 장치 및 방법 | |
US11119501B2 (en) | Moving robot and control method for the same | |
CN106896807B (zh) | 自主移动装置、自主移动方法以及记录介质 | |
KR101618030B1 (ko) | 이동 로봇의 위치 인식 및 주행 제어 방법과 이를 이용한 이동 로봇 | |
CN110238838B (zh) | 自主移动装置、自主移动方法以及存储介质 | |
KR20180134683A (ko) | 인공지능 이동 로봇의 제어 방법 | |
CN107194970B (zh) | 自主移动装置、自主移动方法以及程序存储介质 | |
US11348276B2 (en) | Mobile robot control method | |
JP2020053028A (ja) | 物体追跡システム | |
CN111220148A (zh) | 移动机器人的定位方法、系统、装置及移动机器人 | |
US11709499B2 (en) | Controlling method for artificial intelligence moving robot | |
CN111630346B (zh) | 基于图像和无线电词语的移动设备的改进定位 | |
JP7306546B2 (ja) | 自律移動装置、自律移動方法及びプログラム | |
JP5626011B2 (ja) | プログラム及び画像処理装置 | |
JP2024043795A (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 |