CN111738047A - 自身位置推测方法 - Google Patents

自身位置推测方法 Download PDF

Info

Publication number
CN111738047A
CN111738047A CN202010112109.XA CN202010112109A CN111738047A CN 111738047 A CN111738047 A CN 111738047A CN 202010112109 A CN202010112109 A CN 202010112109A CN 111738047 A CN111738047 A CN 111738047A
Authority
CN
China
Prior art keywords
self
algorithm
processing unit
estimated
estimation processing
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.)
Pending
Application number
CN202010112109.XA
Other languages
English (en)
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.)
Honda Motor Co Ltd
Original Assignee
Honda Motor 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 Honda Motor Co Ltd filed Critical Honda Motor Co Ltd
Publication of CN111738047A publication Critical patent/CN111738047A/zh
Pending legal-status Critical Current

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/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/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on 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/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
    • 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/20—Instruments for performing navigational calculations
    • 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
    • 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/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Multimedia (AREA)
  • Software Systems (AREA)
  • Biomedical Technology (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Health & Medical Sciences (AREA)
  • Electromagnetism (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Optics & Photonics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种自身位置推测方法可，其一边适当地修正由SLAM的算法所得的自身位置的推测值，一边适当地进行移动体的自身位置。自身位置推测方法根据利用多个算法分别推测的自身位置，确定移动体(1)的实际的自身位置。多个算法包含SLAM的算法(12)、及与SLAM不同的算法(11)。利用修正处理部(16)，间歇地执行对应于通过SLAM以外的算法所获得的自身位置的推测值与已确定的自身位置中的任意一个自身位置，修正通过SLAM的算法所获得的自身位置的推测值。

Description

自身位置推测方法
技术领域
本发明涉及一种推测移动体的自身位置的方法。
背景技术
从先前以来，已提出有利用多个传感器的检测信息来推测移动机器人等移动体的自身位置的各种各样的技术。例如，在专利文献1中，提出有利用编码器或陀螺仪等内部传感器的检测信息，及测距传感器、红外线(Infrared，IR)传感器、超声波传感器、摄像机等外部传感器的检测信息来推测移动体的自身位置的技术。
而且，从先前以来，利用一个以上的传感器的检测信息来推测移动体的自身位置的算法也提出有各种各样的算法。
[现有技术文献]
[专利文献]
专利文献1：日本专利特开2007-149088号公报
发明内容
发明所要解决的问题
另外，在根据多个传感器的检测信息，利用多个算法分别推测自身位置，并根据所述多个自身位置的推测值来确定移动体的自身位置的情况下，作为所述多个算法中的一个算法，可采用即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)的算法。
但是，SLAM的算法以如下方式构成：在将移动体的初始位置作为基准的局部坐标系中，依次算出所述移动体的移动量(位置变化量)，并对所述移动量进行累计，由此推测相对于初始位置的移动体的移动位置。因此，在推测对移动体的移动环境任意地设定的全局坐标系中的移动体的自身位置的情况下，存在因在全局坐标系中看到的移动体的初始位置的设定值的误差、或依次算出的移动量的误差等，而导致通过SLAM的算法所获得的移动体的自身位置的误差累计式地增大的情况。进而，若使通过SLAM的算法所获得的移动体的自身位置反映来确定移动体的自身位置，则存在所述已确定的自身位置的可靠性受损的担忧。
本发明是鉴于所述背景而成者，其目的在于提供一种在利用包含SLAM的算法的多个算法来推测移动体的自身位置的方法中，可一边适当地修正由SLAM的算法所得的自身位置的推测值，一边适当地进行移动体的自身位置的推测(确定)的方法。
解决问题的技术手段
为了达成所述目的，本发明的自身位置推测方法根据多个传感器的检测信息，通过分别使用所述多个传感器中的一个以上的传感器的检测信息的多个算法的各算法来推测移动体的自身位置，并根据通过所述多个算法的各算法所获得的所述自身位置的推测值，确定所述移动体的自身位置，其中：
所述多个算法包含作为SLAM(Simultaneous Localization and Mapping)的算法的第一算法、及作为与所述SLAM不同的算法的第二算法，且
所述自身位置推测方法包括修正步骤，所述修正步骤对应于作为通过所述第二算法所获得的所述自身位置的推测值的第二自身位置推测值、及作为所述已确定的自身位置的自身位置确定值中的任意一个自身位置，间歇地执行作为通过所述第一算法所获得的所述自身位置的推测值的第一自身位置推测值的修正(第一发明)。
另外，在本发明中，只要事先无特别说明，则移动体的“自身位置”是指在对移动体的移动环境任意地设定的全局坐标系中看到的自身位置。而且，所述“自身位置”并不限定于通常的含义中的移动体的位置(坐标位置)，也可以包含所述移动体的姿势(方向)。
根据所述第一发明，由于包括所述修正步骤，因此对应于所述一个自身位置(所述第二自身位置推测值或所述自身位置确定值)，间歇地(例如，每经过规定时间)执行通过作为SLAM的算法的第一算法所获得的第一自身位置推测值的修正。由此，防止第一自身位置推测值的误差累计式地扩大。进而，可使用可靠性高的第一自身位置推测值，确定移动体的自身位置(获得自身位置确定值)。
因此，根据第一发明，可一边适当地修正由SLAM的算法所得的自身位置的推测值，一边适当地进行移动体的自身位置的推测(确定)。
在所述第一发明的所述修正步骤中，优选根据规定期间内的所述第二自身位置推测值及所述自身位置确定值中的所述一个自身位置的时间序列、及所述规定期间内的所述第一自身位置推测值的时间序列，决定用于修正所述第一自身位置推测值的参数，以使由所述第一自身位置推测值的时间序列表示的轨道接近由所述一个自身位置的时间序列表示的轨道，第一并使用所述已决定的参数来修正所述规定期间以后的所述第一自身位置推测值(第二发明)。
据此，可决定可靠性高的参数作为用于修正所述第一自身位置推测值的参数。进而，使用所述已决定的参数来修正所述规定期间以后的所述第一自身位置推测值，由此可适宜地修正所述第一自身位置推测值。
在所述第一发明或第二发明中，所述多个传感器可包含拍摄所述移动体的外部的摄像机。在此情况下，优选所述第一算法是以如下方式构成的视觉(Visual)_SLAM的算法，即为了推测所述移动体的自身位置，依次执行从所述摄像机的拍摄图像中检测特征点，进行所述经检测的特征点与从比所述拍摄图像更早的拍摄图像中检测到的特征点的匹配的处理(第三发明)。
据此，由于第一算法是Visua1_SLAM的算法，因此可利用所述第一算法适宜地进行自身位置的推测。
而且，在所述第一发明～第三发明中，所述多个传感器可包含测定所述移动体至外部物体为止的距离的测距传感器。在此情况下，优选所述第二算法是使用由所述测距传感器所得的测距数据与粒子滤波器，推测所述移动体的自身位置的算法(第四发明)。
据此，在移动体周围存在其他移动物体的状况等下，通过所述第二算法，可获得可靠性高(换言之，所述准确性高)的第二自身位置推测值。进而，可将通过所述第二算法所获得的第二自身位置推测值作为基础，获得可靠性高的自身位置确定值。
因此，可适宜地提高对应于所述一个自身位置(所述第二自身位置推测值或所述自身位置确定值)来修正所述第一自身位置推测值的可靠性。
在所述第一发明～第四发明中，优选还包括如下的步骤：在所述多个算法的各算法中，将通过各算法的推测处理所获得的一个以上的与由各算法所推测的所述自身位置的准确性相关联的状态量输入学习完毕的神经网络，根据所述已被输入的状态量，通过所述神经网络来决定所述多个算法的各算法的权重系数，且
将利用所述已决定的权重系数将由所述多个算法的各算法所推测的所述自身位置合成所获得的自身位置确定为所述移动体的自身位置确定值(第五发明)。
决定所述多个算法的各算法的权重系数的神经网络在各算法中，被输入通过各算法的推测处理(推测移动体的自身位置的处理)所获得的一个以上的状态量，根据这些状态量来决定各算法的权重系数。
在此情况下，各算法的状态量是与由各算法所推测的移动体的自身位置的准确性相关联的状态量，因此各算法的所述状态量变成反映此算法的推测处理在求出移动体的自身位置的可靠性高的推测值方面，以何种程度适当地进行的程度者。
因此，通过所述神经网络的事先学习，所述神经网络能够以如下方式对应于被输入的各算法的状态量的整体来决定各算法的权重系数，所述方式是在可比其他算法更适当地进行利用此算法的自身位置的推测处理的状况下，与各算法对应的权重系数相对地变高，且在难以比其他算法更适当地进行利用此算法的自身位置的推测处理的状况下，与各算法对应的权重系数相对地变低。
而且，由所述神经网络所决定的各算法的权重系数是取决于各算法的所述状态量，而非由各算法所推测的移动体的自身位置的精度来决定者，因此所述权重系数变成难以受到移动体的移动环境的场所的影响者。
并且，根据第五发明，将利用如所述那样决定的权重系数将由所述多个算法的各算法所推测的所述自身位置合成所获得的位置确定为所述移动体的自身位置确定值。
由此，根据第五发明，能够以难以受到移动体的移动环境的影响的形态进行移动体的自身位置的推测。
若进行补充，则在第五发明中，当所述第一算法为所述第三发明中的算法(Visual_SLAM的算法)时，优选与所述第一算法相关的所述状态量例如包含表示从所述拍摄图像中检测到的特征点的总数之中，通过所述匹配而建立了对应的特征点的个数的比例的状态量。由此，所述状态量变成适合作为与由第一算法所推测的移动体的自身位置的准确性相关联的状态量者。
而且，在第五发明中，当所述第二算法为所述第四发明中的算法时，优选与所述第二算法相关的所述状态量包含所述第二算法推测所述移动体的自身位置的过程中生成的协方差。由此，所述状态量变成适合作为与由第二算法所推测的移动体的自身位置的准确性相关联的状态量者。
附图说明
图1是表示本发明的实施方式中的移动体的概略构成的图。
图2是表示由图1中所示的自身位置推测处理装置所进行的处理的框图。
图3A至图3C是用于说明图2中所示的修正处理部的处理的图。
符号的说明
1：移动体
5：测程法用传感器
6：激光测距仪(测距传感器)
7：摄像机(传感器)
具体实施方式
以下，参照图1～图3A-图3C对本发明的一实施方式进行说明。参照图1，本实施方式中的移动体1例如为车轮型的移动体，具有经由多个车轮2而支撑在其移动环境的地面上的机体3。并且，移动体1通过省略图示的电动马达等致动器来对一个以上的车轮2进行旋转驱动、或进行一个以上的车轮2的旋转驱动与操舵(方向转换)，由此在地面上移动。
在所述移动体1搭载有用于推测自身位置的多个传感器。例如，在移动体1搭载有检测一个以上的车轮2的旋转角的角度传感器等测程法用的传感器5、测定移动体1至存在于周围的外部物体为止的距离的作为测距传感器的激光测距仪6(以后，称为LRF(LaserRange Finder)6)、以及拍摄存在于移动体1的上方的天花板的摄像机7。另外，测程法用的传感器5除了检测车轮2的旋转角的角度传感器以外，可包含检测车轮2中的操舵轮的操舵角的传感器。而且，在本实施方式中，摄像机7是立体摄像机。
在移动体1搭载有利用这些传感器的检测信息进行自身位置推测的自身位置推测处理装置10。所述自身位置推测处理装置10例如包括包含微型计算机、存储器、接口电路等的一个以上的电子电路单元。
并且，自身位置推测处理装置10如图2的框图所示，包括第一自身位置推测处理部11、第二自身位置推测处理部12、权重系数学习处理部14、自身位置确定处理部15、以及修正处理部16作为通过已安装的硬件构成或程序(软件构成)的两者或一者来实现的功能。另外，自身位置推测处理装置10并不限定于所述功能，例如也可以包含进行移动体1的移动控制等的功能。
第一自身位置推测处理部11是执行利用LRF6的检测信息与测程法用传感器5的检测信息来推测移动体1的自身位置的处理的功能部，第二自身位置推测处理部12是依次执行利用摄像机7的检测信息来推测移动体1的自身位置的功能部。
另外，在本实施方式中，第一自身位置推测处理部11及第二自身位置推测处理部12分别推测的移动体1的自身位置更详细而言，是在对移动体1的移动环境任意地设定的全局坐标系中看到的移动体1的位置与姿势(方向)的组合。第一自身位置推测处理部11推测的移动体1的自身位置相当于本发明中的第二自身位置推测值，第二自身位置推测处理部12推测的移动体1的自身位置相当于本发明中的第一自身位置推测值。在以后的说明中，有时将分别由第一自身位置推测处理部11及第二自身位置推测处理部12推测的自身位置称为推测位置姿势。
而且，权重系数学习处理部14是对应于分别从第一自身位置推测处理部11及第二自身位置推测处理部12输入的后述的状态量A、状态量B，输出分别与第一自身位置推测处理部11及第二自身位置推测处理部12对应的权重系数(详细而言，针对分别从第一自身位置推测处理部11及第二自身位置推测处理部12输出的移动体1的推测位置姿势的权重系数)的功能部，自身位置确定处理部15是对应于从权重系数学习处理部14输出的权重系数，将分别从第一自身位置推测处理部11及第二自身位置推测处理部12输出的移动体1的推测位置姿势合成，由此确定移动体1的自身位置(确定移动体1的推测位置姿势)的功能部。所述自身位置确定处理部15确定的移动体1的自身位置相当于本发明中的自身位置确定值。
而且，修正处理部16是适当生成用于修正第二自身位置推测处理部12推测的自身位置(推测位置姿势)的修正用参数，并将其输入第二自身位置推测处理部12的功能部。
以下，将自身位置推测处理装置10的整体的处理与所述各功能部的详细情况一并进行说明。
第一自身位置推测处理部11及第二自身位置推测处理部12的处理如以下所说明的那样执行。RF6及测程法用传感器5各自的检测信息被输入第一自身位置推测处理部11，并且移动体1的移动环境的地图数据(例如，二维的占据栅格地图等)被输入第一自身位置推测处理部11。在此情况下，地图数据事先存储保持在自身位置推测处理装置10的存储器、或从外部的服务器等随时下载。
并且，第一自身位置推测处理部11根据LRF6的检测信息(移动体1周围的外部物体的测距数据)、测程法用传感器5的检测信息、及地图数据，通过自身位置推测用的规定的算法来依次推测移动体1的自身位置(依次决定推测位置姿势)。
在本实施方式中，第一自身位置推测处理部11将利用粒子滤波器进行自身位置推测的算法，例如自适应蒙特卡罗定位(Adaptive Monte Carlo Localization，AMCL)这一公知的方法的算法用作自身位置推测用的算法。所述算法相当于本发明中的第二算法。
在所述AMCL的算法中，通过如下的处理来依次推测移动体1的自身位置(在全局坐标系中看到的自身位置)。即，首先对应于移动体1的初始位置及初始姿势(方向)，在移动体1的附近生成粒子滤波器中的多个粒子(颗粒)。另外，移动体1的初始位置及初始姿势例如在移动体1的移动开始时或即将开始之前，利用LRF6的检测数据(移动体1周围的外部物体的测距数据)与地图来确定。
并且，对应于根据测程法用传感器5的检测信息所推测的移动体1的移动速度(并进速度及角速度)，推测各粒子的新的移动位置。接下来，通过LRF6来获取移动体1周围的外部物体的测距数据，根据所述测距数据与地图数据，算出各粒子的新的移动位置的似然值。并且，通过重采样处理，使似然值低的粒子消失，进而，在似然值高的粒子的附近生成新的粒子。
在AMCL的算法中，根据通过依次执行如以上那样的处理所获得的似然值高的粒子的位置的时间序列，依次推测移动体1的自身位置(依次决定移动体1的推测位置姿势)。但是，在本实施方式中，由AMCL的算法所推测的移动体1的自身位置(推测位置姿势)中的姿势是偏摆方向(环绕上下方向的轴的方向)上的移动体1的姿势，不包含横摇方向(环绕移动体1的前后方向的轴的方向)及俯仰方向(环绕移动体1的左右方向的轴的方向)的姿势。
而且，在AMCL的算法中，在移动体1的自身位置的推测处理的执行过程中，生成作为与推测位置姿势的准确性相关联的状态量的协方差(详细而言，方差与协方差)。并且，第一自身位置推测处理部11依次输出如所述那样由AMCL的算法所推测的移动体1的自身位置(推测位置姿势)、及作为所述推测时的时刻的推测时刻，并且输出作为与所述推测位置姿势的准确性相关联的状态量(以后，称为状态量1)的所述协方差(方差与协方差)。
若进行补充，则在产生了由AMCL的算法所决定的移动体1的推测位置姿势脱离规定的范围、或所述推测位置姿势的变化量(各推测处理周期的变化量)脱离规定的范围等异常的情况下，第一自身位置推测处理部11对应于由自身位置确定处理部15如后述那样确定的移动体1的自身位置，对推测位置姿势进行修正。例如，以使移动体1的推测位置姿势与自身位置确定处理部15之前已确定的自身位置一致或接近的方式进行修正。
作为摄像机7的检测信息的拍摄图像(移动体1上方的天花板的拍摄图像)被输入第二自身位置推测处理部12。另外，在本实施方式中，摄像机7是立体摄像机，因此被输入第二自身位置推测处理部12的摄像机7的检测信息(拍摄图像)是立体图像(一对拍摄图像)。但是，摄像机7并不限定于立体摄像机，也可以是单眼摄像机。
并且，第二自身位置推测处理部12根据已被输入的拍摄图像，通过自身位置推测用的规定的算法来依次推测移动体1的自身位置(依次决定推测位置姿势)。
在本实施方式中，第二自身位置推测处理部12将进行从已被输入的拍摄图像中的特征点的检测、及所述特征点与从过去输入的拍摄图像中检测到的特征点的匹配的算法用作自身位置推测用的算法。更具体而言，第二自身位置推测处理部12将例如作为Visual_SLAM的一方法的定向二进制简单描述符(Oriented FAST and Rotated BRIEF，ORB)_SLAM这一公知的方法的算法用作所述算法。所述算法相当于本发明中的第一算法。
在所述ORB_SLAM的算法中，通过如下的处理来依次推测移动体1的自身位置。即，在设定移动体1的初始位置姿势(在全局坐标系中看到的移动体1的初始位置及初始姿势的组)后，将由摄像机7所得的拍摄图像(立体图像)依次输入第二自身位置推测处理部12，并且对已被输入的各拍摄图像依次执行从所述拍摄图像中检测ORB特征点的处理。另外，关于第二自身位置推测处理部12的处理中的移动体1的初始位置姿势，例如在移动体1的移动开始时，在所述移动开始时或即将开始之前设定所述第一自身位置推测处理部11已求出的移动体1的推测位置姿势。但是，所述初始位置姿势通过修正处理部16的后述的处理来适当地重置(再次设定)。
并且，将从新输入的拍摄图像(时刻t的拍摄图像)中检测到的ORB特征点与从前一个输入的过去的拍摄图像(时刻t－Δt的拍摄图像)中检测到的ORB特征点加以比较，并进行ORB特征点彼此的匹配。在所述匹配中，判定特征量的差的大小(绝对值)变成规定的阈值以下的ORB特征点彼此为相互相同的ORB特征点(相互对应的ORB特征点)。另外，Δt是拍摄图像的采样周期。
并且，根据所述匹配，推测所述两个时刻t、t－Δt之间的期间内的移动体1的移动量及移动方向，进而，根据所述移动量及移动方向的推测值、与时刻t－Δt处的移动体1的推测位置姿势，决定时刻t处的移动体1的推测位置姿势。
更详细而言，将两个时刻t、t－Δt之间的期间内的移动体1的移动量及移动方向作为在利用ORB_SLAM的算法的自身位置的推测处理用的局部坐标系(将初始位置作为原点的局部坐标系)中看到的移动量及移动方向来推测。并且，将时刻t、时刻t－Δt之间的期间内的移动体1的移动量及移动方向的推测值坐标变换成在全局坐标系中看到的移动量及移动方向，将所述坐标变换后的移动量及移动方向加入时刻t－Δt处的移动体1的推测位置姿势(全局坐标系中的推测位置姿势)，由此求出时刻t处的移动体1的推测位置姿势(全局坐标系中的推测位置姿势)。在此情况下，对应于由在全局坐标系中看到的移动体1的初始位置姿势所规定的所述局部坐标系与全局坐标系的相对的位置及姿势关系，决定所述坐标变换。
在ORB_SLAM的算法中，基本上依次执行如以上那样的处理，由此依次推测移动体1的自身位置(依次决定移动体1的推测位置姿势)。在此情况下，在通过ORB_SLAM的算法所求出的推测位置姿势中的姿势中，不仅包含偏摆方向的姿势，也包含横摇方向及俯仰方向的姿势。
另外，在本实施方式中，第二自身位置推测处理部12在从所述修正处理部16提供了由修正处理部16如后述那样决定的修正参数的情况下，对应于所述修正用参数来修正如所述那样推测的自身位置(推测位置姿势)。此修正处理将后述。
而且，在ORB_SLAM的算法中，在移动体1的自身位置的推测处理的执行过程中，生成作为与推测位置姿势的准确性相关联的状态量的移动体1的推测速度·角速度(并进速度及角速度的推测值)、经检测的ORB特征点的个数(总数)及匹配个数(通过所述匹配而判定为相互相同的ORB特征点的个数)、误差函数的误差值(所述匹配中的特征量的差)、以及推测状态(移动体1的推测速度·角速度的推测精度的高低程度)。
并且，第二自身位置推测处理部12依次输出已推测的移动体1的自身位置(推测位置姿势)与作为所述推测时的时刻的推测时刻，并且输出作为与所述推测位置姿势的准确性相关联的状态量(以后，称为状态量2)的移动体1的推测速度·角速度(并进速度及角速度的推测值)、经检测的ORB特征点的个数及匹配个数、误差函数的误差值(所述匹配中的特征量的差)、以及推测状态(移动体1的推测速度·角速度的推测精度的高低程度)。另外，也可以输出匹配个数对于ORB特征点的个数(总数)的比例来代替ORB特征点的个数及匹配个数。
从第一自身位置推测处理部11及第二自身位置推测处理部12分别输出的所述状态量1、状态量2被输入权重系数学习处理部14。在本实施方式中，所述权重系数学习处理部14包含神经网络。并且，所述神经网络事先实施了将包含所述状态量1、状态量2的特征向量用作多模态信息的多模态学习。
更详细而言，所述多模态学习是根据包含各种各样的状态量1、状态量2的特征向量，生成如下的权重系数(与从第一自身位置推测处理部11及第二自身位置推测处理部12输出的推测位置姿势分别对应的权重系数)的学习，所述权重系数相当于利用第一自身位置推测处理部11及第二自身位置推测处理部12各自的算法的移动体1的自身位置的推测处理与其他算法相比，以何种程度适当地进行的程度。
权重系数学习处理部14的神经网络事先实施了此种多模态学习。因此，从第一自身位置推测处理部11及第二自身位置推测处理部12分别输入了状态量1、状态量2的权重系数学习处理部14根据包含所述状态量B、状态量A的特征向量，通过神经网络来生成与第一自身位置推测处理部11及第二自身位置推测处理部12分别对应的权重系数并将其输出。
在此情况下，所述神经网络基本上以第一自身位置推测处理部11及第二自身位置推测处理部12各自输出的移动体1的推测位置姿势各自的准确性越高，权重系数变得越大的值的方式，生成与第一自身位置推测处理部11及第二自身位置推测处理部12分别对应的权重系数并将其输出。
例如，在由第一自身位置推测处理部11所进行的移动体1的自身位置的推测处理比第二自身位置推测处理部12更适当地进行的状况下，以与第一自身位置推测处理部11对应的权重系数变成比与第二自身位置推测处理部12对应的权重系数大的值的方式，生成各自的权重系数。
而且，例如，在由第二自身位置推测处理部12所进行的移动体1的自身位置的推测处理比第一自身位置推测处理部11更适当地进行的状况下，以与第二自身位置推测处理部14对应的权重系数变成比与第一自身位置推测处理部11对应的权重系数大的值的方式，生成各自的权重系数。
从权重系数学习处理部14输出的与第一自身位置推测处理部11及第二自身位置推测处理部12分别对应的权重系数被输入自身位置确定处理部15。而且，从第一自身位置推测处理部11及第二自身位置推测处理部12分别朝所述自身位置确定处理部15输入移动体1的推测位置姿势及推测时刻。
并且，自身位置确定处理部15将对应于已被输入的权重系数，将第一自身位置推测处理部11及第二自身位置推测处理部12各自的相互同一时刻的推测位置姿势合成而成的位置姿势确定为移动体1的自身位置。即，将使第一自身位置推测处理部11及第二自身位置推测处理部12各自的推测位置姿势乘以与两处理部对应的权重系数所得者的总和确定为移动体1的自身位置。另外，此时，各权重系数的和为1。而且，在本实施方式中，自身位置确定处理部15确定的移动体1的自身位置中的姿势是偏摆方向上的移动体1的姿势，不包含横摇方向及俯仰方向的姿势。
在本实施方式中，自身位置确定处理部15如以上所说明的那样依次确定移动体1的自身位置。并且，自身位置确定处理部15输出已确定的自身位置和与其对应的推测时刻的组合。
本实施方式的自身位置推测处理装置10如所述那样一边执行第一自身位置推测处理部11及第二自身位置推测处理部12、权重系数学习处理部14以及自身位置确定处理部15的处理，一边进一步执行修正处理部16的处理。另外，在以后的说明中，有时将第一自身位置推测处理部11已推测的自身位置(推测位置姿势)称为第一推测位置姿势，将第二自身位置推测处理部12已推测的自身位置(推测位置姿势)称为第二推测位置姿势，将自身位置确定处理部15已确定的自身位置称为确定位置姿势。
从第一自身位置推测处理部11输出的第一推测位置姿势与推测时刻的组合、从第二自身位置推测处理部12输出的第二推测位置姿势与推测时刻的组合、以及从自身位置确定处理部15输出的确定位置姿势与推测时刻的组合被输入修正处理部16。并且，修正处理部16将已被输入的第一推测位置姿势、第二推测位置姿势及确定位置姿势的数据与对应于这些姿势的推测时刻一同按时间序列存储保持在未图示的存储器。在此情况下，存储保持在存储器的数据是在从最新的数据至规定时间前为止的期间内已被输入修正处理部16的数据。
并且，修正处理部16首先每当被输入第二推测位置姿势时，均判断由第二自身位置推测处理部12所进行的推测处理是否正常地进行。此处，在本实施方式中，移动体1在具有天花板的建筑物内的地面上移动，因此所述移动体1的姿势在横摇方向(环绕移动体1的前后方向的轴的方向)、或俯仰方向(环绕移动体1的左右方向的轴的方向)上不怎么变化。因此，例如当在横摇方向或俯仰方向上，第二自身位置推测处理部12在最新的处理周期的时刻新决定的第二推测位置姿势中的姿势从在其前一个处理周期的时刻所决定的第二推测位置姿势中的姿势，进行了比规定量大的变化时，修正处理部16判断由第二自身位置推测处理部12所进行的移动体1的自身位置的推测处理未正常地进行。
并且，在此情况下，修正处理部16指示第二自身位置推测处理部12将在与最新的第二推测位置姿势对应的推测时刻或与其大致一致的时刻已从第一自身位置推测处理部11输出的第一推测位置姿势，作为第二自身位置推测处理部12的处理中的移动体1的新的初始位置姿势来再次设定，并且将所述第一推测位置姿势输入第一自身位置推测处理部12。
此时，第二自身位置推测处理部12将已被输入的第一推测位置姿势作为移动体1的新的初始位置姿势来再次设定，进而，再次设定第二自身位置推测处理部12的推测处理中的局部坐标系。并且，第二自身位置推测处理部12以后利用经再次设定的局部坐标系，执行移动体1的自身位置的推测处理。
而且，在第二推测位置姿势中的姿势的朝横摇方向及俯仰方向的变化止于规定量以下的情况下，修正处理部16判断由第二自身位置推测处理部12所进行的移动体1的自身位置的推测处理正常地进行。
在此情况下，修正处理部16例如每经过规定期间(进而，间歇地)，执行以下所说明的修正参数的决定处理。另外，所述规定期间例如为从所述期间的开始时至第二自身位置推测处理部12获取的摄像机7的拍摄图像的采样数(帧数)达到规定数为止的期间。
在所述修正参数的决定处理中，修正处理部16在移动体1开始移动后，经过了第一次的规定期间时，从未图示的存储器提取在所述第一次的规定期间内已从第二自身位置推测处理部12输出的第二推测位置姿势的时间序列数据、及已从第一自身位置推测处理部11输出的第一推测位置姿势的时间序列数据。而且，当经过了第二次以后的规定期间时，从未图示的存储器提取在各次的规定期间内已从第一自身位置推测处理部11输出的第一推测位置姿势的时间序列数据、及已从自身位置确定处理部15输出的确定位置姿势的时间序列数据。另外，关于第二次以后的规定期间的起点的时机，当如所述那样再次设定了第二自身位置推测处理部12中的移动体1的初始位置姿势时，将所述再次设定时设为新的规定期间的起点的时机。
此处，第二自身位置推测处理部12的推测处理是从移动体1的初始位置姿势，对依次求出的移动体1的移动量及移动方向进行累计的处理。因此，存在因移动体1的初始位置姿势(在全局坐标系中看到的初始位置姿势)的误差、或第二自身位置推测处理部12依次求出的移动体1的移动量及移动方向的推测误差等，而导致所述第二自身位置推测处理部12依次求出的移动体1的第二推测位置姿势的误差累计式地增大的情况。
进而，存在由第二推测位置姿势的时间序列数据表示的移动体1的移动轨道(以后，称为移动轨道Lb)相对于由第一推测位置姿势的时间序列数据、或确定位置姿势的时间序列数据表示的移动体1的移动轨道(以后，称为移动轨道La)，如图3A中例示的那样背离的情况。而且，在本实施方式中，自身位置确定处理部15如所述那样求出移动体1的确定位置姿势，因此存在移动体1刚开始移动后，所述自身位置确定处理部15求出的确定位置姿势缺乏稳定性的可能性。
因此，修正处理部16在移动体1刚开始移动后的第一次的规定期间的经过时，使用在所述规定期间内已从第二自身位置推测处理部12输出的第二推测位置姿势的时间序列数据、及已从第一自身位置推测处理部11输出的第一推测位置姿势的时间序列数据，决定使由第二推测位置姿势的时间序列数据表示的移动体1的移动轨道Lb以接近由第一推测位置姿势的时间序列数据表示的移动体1的移动轨道La的方式旋转变换的旋转变换矩阵R。所述旋转变换矩阵R例如使用作为公知的点群处理库的点云库(Point Cloud Library，PCL)来决定。图3B所示的移动轨道Lb'表示通过旋转变换矩阵R来将移动轨道Lb旋转变换后的移动轨道。
进而，修正处理部16决定并进变换操作量T(例如并进移动向量)，所述并进变换操作量T使通过旋转变换矩阵R来将由第二推测位置姿势的时间序列数据表示的移动体1的移动轨道Lb旋转变换而成的移动轨道Lb'的终端如图3C中例示的那样，以与由第一推测位置姿势的时间序列数据表示的移动体1的移动轨道La的终端一致的方式并进移动。所述并进变换操作量T根据旋转变换后的移动轨道Lb'的终端的位置(全局坐标系中的位置)、及移动轨道L2的移动体1的移动轨道的终端的位置(全局坐标系中的位置)来求出。图3C中所示的移动轨道Lb”表示通过并进变换操作量T来将移动轨道Lb'并进变换后的移动轨道。
在第二次以后的各次的规定期间的经过时，修正处理部16使用已从自身位置确定处理部15输出的确定位置姿势的时间序列数据来代替第一推测位置姿势的时间序列数据，与所述同样地决定旋转变换矩阵R及并进变换操作量T。
在本实施方式中，修正处理部16基本上每经过规定期间，将如所述那样决定的旋转变换矩阵R及并进变换操作量T作为用于修正第二推测位置姿势的修正参数输出至第二自身位置推测处理部12。但是，在本实施方式中，第一自身位置推测处理部11求出的推测位置姿势的姿势、及自身位置确定处理部15确定的确定位置姿势的姿势是偏摆方向的姿势，因此旋转变换矩阵R理想的是使第二推测位置姿势朝偏摆方向或大致偏摆方向旋转者。
因此，在本实施方式中，修正处理部16仅在如所述那样决定的旋转变换矩阵R中的与偏摆方向的旋转变换相关的成分(具体而言，三行三列的成分)为十分接近“1”的值的情况(详细而言，所述成分与“1”的偏差的绝对值比零附近的规定的阈值小的情况)下，将旋转变换矩阵R及并进变换操作量T作为修正参数输出至第二自身位置推测处理部12，并且对第二自身位置推测处理部12指示利用所述修正参数的第二推测位置姿势修正。
此时，第二自身位置推测处理部12在以后的推测处理中，利用已从修正处理部16输入的修正参数(旋转变换矩阵R及并进变换操作量T)，对如所述那样通过ORB_SLAM的算法所求出的第二推测位置姿势进行修正。即，第二自身位置推测处理部12利用旋转变换矩阵R将通过ORB_SLAM的算法所求出的第二推测位置姿势旋转变换，并且利用并进变换操作量T将通过ORB_SLAM的算法所求出的第二推测位置姿势并进变换，由此修正所述第二推测位置姿势。并且，第二自身位置推测处理部12输出所述修正后的第二推测位置姿势。
而且，修正处理部16在旋转变换矩阵R中的与偏摆方向的旋转变换相关的成分并非十分接近“1”的值的情况(所述成分与“1”的偏差的绝对值比零附近的规定的阈值大的情况)下，不对第二自身位置推测处理部12指示第二推测位置姿势的修正，并且不输出旋转变换矩阵R及并进变换操作量T。此时，第二自身位置推测处理部12不修正通过ORB_SLAM的算法所求出的第二推测位置姿势而输出。
在本实施方式中，如以上所说明的那样执行自身位置推测处理装置10的处理。在此情况下，对应于修正处理部16每经过规定期间(间歇地)，使用第一自身位置推测处理部11输出的第一推测位置姿势、或自身位置确定处理部15输出的确定位置姿势所决定的修正参数(旋转变换矩阵R及并进变换操作量T)，对利用作为VISUAL_SLAM的一例的ORB_SLAM的算法来推测移动体1的自身位置的第二自身位置推测处理部12求出的第二推测位置姿势进行修正。由此，防止因第二自身位置推测处理部12的推测处理中的移动体1的初始位置姿势的误差、或通过所述推测处理来依次求出的移动体1的移动量及移动方向的误差，而导致第二推测位置姿势的误差累计式地扩大。进而，能够以高鲁棒性实现维持自身位置确定处理部15输出的确定位置姿势的可靠性。
而且，以使由第二自身位置推测处理部12输出的第二推测位置姿势的时间序列数据表示的移动轨道Lb接近由第一自身位置推测处理部11输出的第一推测位置姿势、或由自身位置确定处理部15输出的确定位置姿势的时间序列数据表示的移动轨道La的方式，决定作为修正参数的旋转变换矩阵R及并进变换操作量T。因此，可决定作为修正参数适当的旋转变换矩阵R及并进变换操作量T。
而且，在实施方式中，利用由权重系数学习处理部14对应于第一自身位置推测处理部11及第二自身位置推测处理部12各自的状态量1、状态量2所决定的权重系数，将分别由第一自身位置推测处理部11及第二自身位置推测处理部12所推测的自身位置(第一推测位置姿势及第二推测位置姿势)合成，由此确定移动体1的自身位置。
并且，在此情况下，权重系数学习处理部14是将第一自身位置推测处理部11及第二自身位置推测处理部12各自的状态量1、状态量2作为输入信息者，因此可在移动体1的各种各样的移动环境中，事先学习对应于状态量1、状态量2应输出的权重系数。
因此，自身位置推测处理装置10可在移动体1的各种各样的移动环境中，以高可靠性推测(确定)移动体1的自身位置。
另外，本发明并不限定于以上所说明的实施方式，也可以采用其他实施方式。以下，例示几个其他实施方式。
在所述实施方式中，修正处理部16在第二次以后的各规定期间的经过时，使用已从自身位置确定处理部15输出的确定位置姿势的时间序列数据来决定修正参数(旋转变换矩阵R及并进变换操作量T)。但是，也可以在第二次以后的各规定期间的经过时，使用已从第一自身位置推测处理部11输出的第一推测位置姿势的时间序列数据来决定修正参数(旋转变换矩阵R及并进变换操作量T)。
或者，例如也可以根据从第一自身位置推测处理部11输出的状态量1、或权重系数学习处理部14决定的权重系数，判断已从第一自身位置推测处理部11输出的第一推测位置姿势的可靠性或准确性的高低，并对应于其判断结果，选定将第一推测位置姿势的时间序列数据与确定位置姿势的时间序列数据的哪一个用于决定修正参数。更具体而言，也可以在判断第一推测位置姿势的可靠性或准确性高的情况下，将第一推测位置姿势的时间序列数据用于决定修正参数，在其以外的情况下，将确定位置姿势的时间序列数据用于决定修正参数。
而且，在所述实施方式中，通过第一自身位置推测处理部11所求出的第一推测位置姿势中的姿势、及通过自身位置确定处理部15所求出的确定位置姿势中的姿势仅为偏摆方向的姿势，但也可以包含横摇方向的姿势及俯仰方向的姿势。
而且，第二自身位置推测处理部12的推测处理用的算法并不限定于ORB_SLAM，也可以是其他SLAM的算法。
而且，在所述实施方式中，作为自身位置推测处理部，例示了包括的两个自身位置推测处理部(第一自身位置推测处理部11及第二自身位置推测处理部12)者，但例如也可以代替第一自身位置推测处理部11、或除第一自身位置推测处理部11及第二自身位置推测处理部12以外，包括可利用其他算法来推测移动体1的自身位置的其他自身位置推测处理部。作为所述其他自身位置推测处理部，例如可采用：利用可搭载在移动体1的磁传感器、所述测程法用传感器5、及移动体1的移动环境的磁状态的地图来推测移动体1的自身位置的自身位置推测处理部。作为此情况下的自身位置的推测处理的算法，可使用使用与第一自身位置推测处理部11相同的算法(AMCL的算法)。
另外，当自身位置推测处理装置包括三个以上的自身位置推测处理部时，只要利用与所述实施方式相同的方法，通过权重系数学习处理部14来决定与各自身位置推测处理部对应的权重系数，并使用这些权重系数，将由各自身位置推测处理部所推测的自身位置合成，由此确定移动体1的自身位置即可。
而且，在所述实施方式中，作为权重系数学习处理部14，例示了进行多模态学习的神经网络。但是，作为权重系数学习处理部14，例如也可以使用长短期记忆网络(Longshort-term memory，LSTM)等循环神经网络(Recurrent Neural Network，RNN)等。
而且，在所述实施方式中，作为与第二自身位置推测处理部12相关的状态量2，使用移动体1的推测速度·角速度(并进速度及角速度的推测值)、经检测的ORB特征点的个数及匹配个数、误差函数的误差值(所述匹配中的特征量的差)、以及推测状态。但是，也可以省略这些状态量中的一部分的状态量，例如移动体1的推测速度·角速度、误差函数的误差值、推测状态等。
而且，根据由多个自身位置推测处理部分别推测的自身位置，确定移动体1的实际的自身位置的方法并不限定于所述实施方式中所说明的方法。例如，也可以根据移动体1的动作状态、或周围环境的状态，将由从多个自身位置推测处理部中选定的一个自身位置推测处理部所推测的自身位置直接为确定移动体1的实际的自身位置。
而且，在所述实施方式中，作为测距传感器，使用了LRF6。但是，例如也可以使用超声波式的测距传感器等来代替LRF6。
而且，在所述实施方式中，作为移动体，例示了车轮型的移动体1。但是，本发明的适用对象的移动体并不限定于车轮型的移动体，例如也可以是脚式的移动体、倒立摆型的移动体等。

Claims (5)

1.一种自身位置推测方法，其根据多个传感器的检测信息，通过分别使用所述多个传感器中的一个以上的传感器的检测信息的多个算法的各算法来推测移动体的自身位置，并根据通过所述多个算法的各算法所获得的所述自身位置的推测值，确定所述移动体的自身位置，其特征在于，
所述多个算法包含作为即时定位与地图构建的算法的第一算法、及作为与所述即时定位与地图构建不同的算法的第二算法，且所述自身位置推测方法包括：
修正步骤，对应于作为通过所述第二算法所获得的所述自身位置的推测值的第二自身位置推测值、及作为已确定的所述自身位置的自身位置确定值中的任意一个自身位置，间歇地执行作为通过所述第一算法所获得的所述自身位置的推测值的第一自身位置推测值的修正。
2.根据权利要求1所述的自身位置推测方法，其特征在于，
在所述修正步骤中，根据规定期间内的所述第二自身位置推测值及所述自身位置确定值中的所述一个自身位置的时间序列、及所述规定期间内的所述第一自身位置推测值的时间序列，决定用于修正所述第一自身位置推测值的参数，以使由所述第一自身位置推测值的时间序列表示的轨道接近由所述一个自身位置的时间序列表示的轨道，第一并使用已决定的所述参数来修正所述规定期间以后的所述第一自身位置推测值。
3.根据权利要求1或2所述的自身位置推测方法，其特征在于，
所述多个传感器包含拍摄所述移动体的外部的摄像机，
所述第一算法是以如下方式构成的视觉_即时定位与地图构建的算法，即为了推测所述移动体的自身位置，依次执行从所述摄像机的拍摄图像中检测特征点，进行经检测的所述特征点与从比所述拍摄图像更早的拍摄图像中检测到的特征点的匹配的处理。
4.根据权利要求1至3中任一项所述的自身位置推测方法，其特征在于，
所述多个传感器包含测定所述移动体至外部物体为止的距离的测距传感器，
所述第二算法是使用由所述测距传感器所得的测距数据与粒子滤波器，推测所述移动体的自身位置的算法。
5.根据权利要求1至4中任一项所述的自身位置推测方法，其特征在于，
还包括如下的步骤：在所述多个算法的各算法中，将通过各算法的推测处理所获得的一个以上的与由各算法所推测的所述自身位置的准确性相关联的状态量输入学习完毕的神经网络，根据已被输入的所述状态量，通过所述神经网络来决定所述多个算法的各算法的权重系数，
将利用已决定的所述权重系数将由所述多个算法的各算法所推测的所述自身位置合成所获得的自身位置确定为所述移动体的自身位置确定值。
CN202010112109.XA 2019-03-25 2020-02-24 自身位置推测方法 Pending CN111738047A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2019-057026 2019-03-25
JP2019057026A JP7336223B2 (ja) 2019-03-25 2019-03-25 自己位置推定方法

Publications (1)

Publication Number Publication Date
CN111738047A true CN111738047A (zh) 2020-10-02

Family

ID=72605828

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010112109.XA Pending CN111738047A (zh) 2019-03-25 2020-02-24 自身位置推测方法

Country Status (3)

Country Link
US (1) US11662742B2 (zh)
JP (1) JP7336223B2 (zh)
CN (1) CN111738047A (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2022042630A (ja) * 2020-09-03 2022-03-15 本田技研工業株式会社 自己位置推定方法
CN112950712B (zh) * 2021-02-25 2023-03-24 深圳市慧鲤科技有限公司 定位方法及装置、电子设备和存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014115247A (ja) * 2012-12-12 2014-06-26 Honda Motor Co Ltd 車両位置推定装置およびセンサ情報統合装置
JP2014211862A (ja) * 2013-04-02 2014-11-13 パナソニック株式会社 自律移動装置及び自律移動装置の自己位置推定方法
US20150213617A1 (en) * 2014-01-24 2015-07-30 Samsung Techwin Co., Ltd. Method and apparatus for estimating position
JP2015201734A (ja) * 2014-04-07 2015-11-12 キヤノン株式会社 画像処理装置、画像処理装置の制御方法およびプログラム
WO2016185659A1 (ja) * 2015-05-15 2016-11-24 株式会社デンソー 移動体位置検出装置、移動体位置検出方法
JP2017072492A (ja) * 2015-10-08 2017-04-13 国立大学法人東京工業大学 計測システム
WO2017130397A1 (ja) * 2016-01-29 2017-08-03 富士通株式会社 位置推定装置、位置推定方法および位置推定プログラム
US20180297207A1 (en) * 2017-04-14 2018-10-18 TwoAntz, Inc. Visual positioning and navigation device and method thereof

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE19859169A1 (de) * 1997-12-19 1999-06-24 Univ Carnegie Mellon Lernsystem und Verfahren zur Optimierung der Steuerung autonomer Erdbewegungsmaschinen
KR100834761B1 (ko) 2005-11-23 2008-06-05 삼성전자주식회사 이동 로봇의 자기 위치 인식 방법 및 장치
JP5370122B2 (ja) * 2009-12-17 2013-12-18 富士通株式会社 移動体位置推定装置及び移動体位置推定方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014115247A (ja) * 2012-12-12 2014-06-26 Honda Motor Co Ltd 車両位置推定装置およびセンサ情報統合装置
JP2014211862A (ja) * 2013-04-02 2014-11-13 パナソニック株式会社 自律移動装置及び自律移動装置の自己位置推定方法
US20150213617A1 (en) * 2014-01-24 2015-07-30 Samsung Techwin Co., Ltd. Method and apparatus for estimating position
JP2015201734A (ja) * 2014-04-07 2015-11-12 キヤノン株式会社 画像処理装置、画像処理装置の制御方法およびプログラム
WO2016185659A1 (ja) * 2015-05-15 2016-11-24 株式会社デンソー 移動体位置検出装置、移動体位置検出方法
JP2017072492A (ja) * 2015-10-08 2017-04-13 国立大学法人東京工業大学 計測システム
WO2017130397A1 (ja) * 2016-01-29 2017-08-03 富士通株式会社 位置推定装置、位置推定方法および位置推定プログラム
US20180297207A1 (en) * 2017-04-14 2018-10-18 TwoAntz, Inc. Visual positioning and navigation device and method thereof

Also Published As

Publication number Publication date
US11662742B2 (en) 2023-05-30
JP2020160594A (ja) 2020-10-01
JP7336223B2 (ja) 2023-08-31
US20200310435A1 (en) 2020-10-01

Similar Documents

Publication Publication Date Title
CN111052183B (zh) 利用事件相机的视觉惯性里程计
CN106780608B (zh) 位姿信息估计方法、装置和可移动设备
KR102016551B1 (ko) 위치 추정 장치 및 방법
KR102440358B1 (ko) 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법
JP7034264B2 (ja) 自己位置推定方法
US20130116823A1 (en) Mobile apparatus and walking robot
CN112230242A (zh) 位姿估计系统和方法
JP2008006519A (ja) ロボット装置及びロボット装置の制御方法
EP1548534A2 (en) Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program
JP2016517981A (ja) 基準方向に対する移動要素の角度偏差を推定する方法
CN111738047A (zh) 自身位置推测方法
Nobre et al. Drift-correcting self-calibration for visual-inertial SLAM
WO2021157116A1 (ja) 位置測定装置
Hachemi et al. Fault diagnosis and reconfiguration for mobile robot localization based on multi-sensors data fusion
Dhédin et al. Visual-inertial and leg odometry fusion for dynamic locomotion
JP2002046087A (ja) ３次元位置計測方法及び計測装置並びにロボット制御装置
Fuchs et al. Advanced 3-D trailer pose estimation for articulated vehicles
US20220066466A1 (en) Self-position estimation method
WO2018230560A1 (ja) 位置検出装置および位置検出方法
WO2023162017A1 (ja) 位置姿勢推定装置、位置姿勢推定システム、およびセンサ設置方法
KR100575108B1 (ko) 비전 센서를 이용하여 다수 비행체를 도킹시키는 방법
WO2022179047A1 (zh) 一种状态信息估计方法及装置
Kim et al. A robotic pan and tilt 3-d target tracking system by data fusion of vision, encoder, accelerometer, and gyroscope measurements
Unterholzner et al. Vision-based online-calibration of inertial gaze stabilization
JP5056436B2 (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