CN116635891A - 位置/姿态估计装置、位置/姿态估计方法及程序 - Google Patents

位置/姿态估计装置、位置/姿态估计方法及程序 Download PDF

Info

Publication number
CN116635891A
CN116635891A CN202080107829.5A CN202080107829A CN116635891A CN 116635891 A CN116635891 A CN 116635891A CN 202080107829 A CN202080107829 A CN 202080107829A CN 116635891 A CN116635891 A CN 116635891A
Authority
CN
China
Prior art keywords
orientation
unit
absolute
posture
frame
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
CN202080107829.5A
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.)
Mitsubishi Electric Corp
Original Assignee
Mitsubishi Electric Corp
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 Mitsubishi Electric Corp filed Critical Mitsubishi Electric Corp
Publication of CN116635891A publication Critical patent/CN116635891A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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/3844—Data obtained from position sensors only, e.g. from inertial navigation
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
    • 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/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/761—Proximity, similarity or dissimilarity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/20—Scenes; Scene-specific elements in augmented reality scenes
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N23/00—Cameras or camera modules comprising electronic image sensors; Control thereof
    • H04N23/60—Control of cameras or camera modules
    • 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/20084—Artificial neural networks [ANN]
    • 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/30244—Camera pose
    • 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/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Geometry (AREA)
  • Medical Informatics (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Computer Graphics (AREA)
  • Signal Processing (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)
  • Navigation (AREA)
  • Indexing, Searching, Synchronizing, And The Amount Of Synchronization Travel Of Record Carriers (AREA)
  • Image Processing (AREA)

Abstract

位置/姿态估计装置(101)具有：数据库读入部(15)，其从数据库中读入三维地图的数据；帧选择部(16)，其进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；相对位置/姿态取得部(17)，其进行取得关于由所述帧选择部选择出的多个帧的多个相对位置/姿态的处理；绝对位置/姿态计算部(18)，其进行取得关于由所述帧选择部选择出的多个帧的多个绝对位置/姿态的处理；以及绝对位置/姿态综合部(19)，其对由所述相对位置/姿态取得部(17)取得的相对位置/姿态和由所述绝对位置/姿态计算部(18)取得的绝对位置/姿态进行综合，取得最终的绝对位置/姿态。

Description

位置/姿态估计装置、位置/姿态估计方法及程序
技术领域
本公开涉及位置/姿态估计装置、位置/姿态估计方法及程序。
背景技术
在应用了Augmented Reality(AR：增强现实)的装置和应用了Automated GuidedVehicle(AGV：自动导引车)的机器人等中，提出了通过组合相对位置/姿态的计算结果与绝对位置/姿态的计算结果而提高位置/姿态的计算(即，估计)精度的方法。例如参照专利文献1。
相对位置/姿态的计算是从某个位置/姿态起的相对的移动量的计算，是按照每个固定周期(通常是短周期)而重复执行的处理。例如，在相对位置/姿态的计算中，使用以下方法等：基于由摄像头拍摄到的图像即摄像头图像或者由距离传感器检测到的距离而求出移动量的Simultaneous Localization And Mapping(SLAM：即时定位与地图构建)、使用综合了陀螺仪传感器或加速度传感器等的Inertial Measurement Unit(IMU：惯性测量单元)的方法、或者基于车轮的转速而求出移动量的自主导航法。在这些方法中，在每次计算相对位置/姿态时都加上了误差，因此，存在当进行长距离移动时累计的误差变大的问题。因此，进行以下处理：组合相对位置/姿态的计算结果和绝对位置/姿态的计算结果，定期地去除通过相对位置/姿态的计算而累计的误差。
绝对位置/姿态的计算使用预先准备的三维地图而进行，例如，在三维地图所表示的物体前执行。绝对位置/姿态的计算例如使用三维地图和摄像头图像而进行。
图1是示出在携带应用了AR的终端111的用户112进行了移动的情况下、终端111利用相对位置/姿态的计算和绝对位置/姿态的计算来估计位置/姿态的例子的图。终端111例如是平板终端或使用了Head Mounted Display(HMD：头戴式显示器)的终端等。在该情况下，终端111一边执行相对位置/姿态计算(1)一边进行移动，使用在三维地图所表示的物体113前执行的绝对位置/姿态计算(2)的结果来去除累计的误差，之后，一边执行相对位置/姿态计算(3)一边进行移动。由此，即便离开三维地图所表示的物体113，也能够高精度地估计位置/姿态，其结果是，能够在终端的画面所显示的实际图像上的适当位置持续地重叠显示AR内容。
图2是示出在应用了AGV的机器人121进行了移动的情况下、机器人121利用相对位置/姿态的计算和绝对位置/姿态的计算来估计位置/姿态的例子的图。在该情况下，机器人121一边执行相对位置/姿态计算(4)一边进行移动，使用在三维地图所表示的物体123前执行的绝对位置/姿态计算(5)的结果来去除累计的误差，之后，一边执行相对位置/姿态计算(6)一边进行移动。由此，即便离开三维地图所表示的物体123，机器人121也能够高精度地估计自己的位置/姿态，其结果是，能够准确地到达作为目标的位置。
现有技术文献
专利文献
专利文献1：日本特开2019-160147号公报
发明内容
发明要解决的问题
但是，以往的绝对位置/姿态的估计仅使用1帧(即，1个图像帧)而进行，因此，存在计算精度(即，精度的估计)依赖于1帧所包含的被摄体而大幅变动这样的问题。
例如，在使用摄像头图像的情况下，绝对位置/姿态的计算精度有时依赖于被摄体的图案而变动。即，在被摄体的图案具有特征的情况下，能够高精度地进行绝对位置/姿态的计算，但在被摄体的图案是条纹或边框等重复图案的情况下、或者在被摄体是纯白壁等那样的没有图案的物体的情况下，绝对位置/姿态的计算的精度下降。
此外，在使用利用激光或红外线等得到的距被摄体的距离信息来计算绝对位置/姿态的情况下，绝对位置/姿态的计算的精度依赖于被摄体的形状大幅变动。即，在被摄体的形状具有特征的情况下，能够高精度地进行绝对位置/姿态的计算，但在被摄体的形状不具有特征的情况下，绝对位置/姿态的计算的精度下降。
本公开是为了解决上述的问题而完成的，其目的在于，提供一种能够提高位置/姿态的估计的精度的位置/姿态估计装置、位置/姿态估计方法以及程序。
用于解决问题的手段
本公开的位置/姿态估计装置的特征在于，具有：数据库读入部，其从数据库中读入三维地图的数据；帧选择部，其进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；相对位置/姿态取得部，其进行取得关于由所述帧选择部选择出的多个帧的多个相对位置/姿态的处理；绝对位置/姿态计算部，其进行取得关于由所述帧选择部选择出的多个帧的多个绝对位置/姿态的处理；以及绝对位置/姿态综合部，其对由所述相对位置/姿态取得部取得的相对位置/姿态和由所述绝对位置/姿态计算部取得的绝对位置/姿态进行综合，取得最终的绝对位置/姿态。
本公开的位置/姿态估计方法是由位置/姿态估计装置执行的方法，其特征在于，具有以下步骤：从位置数据库中读入三维地图的数据；进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；进行取得关于选择出的多个所述帧的多个相对位置/姿态的处理；进行取得关于选择出的多个所述帧的多个绝对位置/姿态的处理；以及对取得的所述相对位置/姿态和取得的所述绝对位置/姿态进行综合，取得最终的绝对位置/姿态。
发明的效果
根据本公开的装置、方法或程序，能够提高位置/姿态的估计的精度。
附图说明
图1是示出在携带应用了AR的终端的用户进行了移动的情况下、终端利用相对位置/姿态的计算和绝对位置/姿态的计算来估计位置/姿态的例子的图。
图2是示出在应用了AGV的机器人进行了移动的情况下、机器人利用相对位置/姿态的计算和绝对位置/姿态的计算来估计位置/姿态的例子的图。
图3是示出实施方式1的位置/姿态估计装置和包含该位置/姿态估计装置的位置/姿态估计系统的硬件结构的例子的图。
图4是示出图3所示的位置/姿态估计装置的硬件结构的例子的图。
图5是概要地示出实施方式1的三维地图制作装置的结构的功能框图。
图6是示出对图像附加随机图案的处理的一例的图。
图7是示出将三维地图与平面地图对位并进行登记的处理的图。
图8是概要地示出实施方式1的位置/姿态估计装置的结构的功能框图。
图9是示出由实施方式1的三维地图制作装置进行的用于制作三维地图的处理的例子的流程图。
图10是示出由实施方式1的位置/姿态估计装置进行的用于估计位置/姿态的处理的例子的流程图。
图11是示出由实施方式1的位置/姿态估计装置进行的用于估计位置/姿态的处理的其他例的流程图。
图12是概要地示出实施方式2的三维地图制作装置的结构的功能框图。
图13是示出实施方式2的三维地图制作装置所使用的方差的计算方法的图。
图14是概要地示出实施方式2的位置/姿态估计装置的结构的功能框图。
图15是示出由实施方式2的三维地图制作装置进行的用于制作三维地图的处理的例子的流程图。
图16是示出由实施方式2的位置/姿态估计装置进行的用于估计位置/姿态的处理的例子的流程图。
图17是概要地示出实施方式3的位置/姿态估计装置的结构的功能框图。
图18是示出由实施方式3的位置/姿态估计装置进行的用于估计位置/姿态的处理的例子的流程图。
图19是概要地示出实施方式4的位置/姿态估计装置的结构的功能框图。
图20是示出由实施方式4的位置/姿态估计装置进行的用于估计位置/姿态的处理的其他例的流程图。
具体实施方式
以下，参照附图对实施方式的位置/姿态估计装置、位置/姿态估计方法以及程序进行说明。以下的实施方式只不过例子，能够适当组合实施方式以及适当变更实施方式。另外，在本申请中，“位置/姿态”是指位置和姿态。“位置”例如是指搭载了摄像头的终端或机器人的位置。“姿态”例如是指摄像头的拍摄方向或距离传感器的测定方向。
《1》实施方式1
《1-1》结构
《1-1-1》概要
在实施方式1中，针对使用了由摄像头拍摄到的图像即摄像头图像(即，摄像图像)的绝对位置/姿态的计算精度的提高进行叙述。作为使用摄像头图像来估计位置/姿态的方法，已知有第1估计方法。例如，参照非专利文献1。
非专利文献1：Paul-Edouard Sarlin，外3名著，“From Coarse to Fine：RobustHierarchical Localization at Large Scale”，2019IEEE/CVF Conference on ComputerVision and Pattern Recognition.
在第1估计方法中，使用直接的匹配。在第1估计方法中，基于图像的局部特征的集合，直接计算摄像头的位置/姿态。
此外，作为使用摄像头图像估计位置/姿态的方法，已知有第2估计方法。在第2估计方法中，通过Convolutional Neural Network(CNN：卷积神经网络)根据图像来估计摄像头的位置/姿态。例如参照非专利文献2。
非专利文献2：Samarth Brahmbhatt，外4名著，“Geometry-Aware Learning ofMaps for Camera Localization”，2018IEEE/CVF Conference on Computer Vision andPattern Recognition.
在第2估计方法中，使用2个阶段的匹配。在第2估计方法中，准备视点不同的多个图像，首先，从多个图像中确定与通过拍摄得到的摄像头图像最相似的图像作为相似图像，接着，基于从相似图像中提取出的局部特征与从摄像头图像中提取出的局部特征的对应关系，求出摄像头的位置/姿态。在实施方式1中，使用基于第2估计方法的方法。
图3是示出实施方式1的位置/姿态估计装置101和包含该位置/姿态估计装置101的位置/姿态估计系统100的硬件结构的例子的图。实施方式1的位置/姿态估计装置101具有计算机，该计算机是执行用于估计位置/姿态的计算的计算机。在图3的例子中，实施方式1的位置/姿态估计系统100具有位置/姿态估计装置101、存储于存储装置的三维地图数据库(三维地图DB)102、距离传感器103、作为摄像装置的摄像头104、以及液晶显示装置等显示器105。此外，在图3的例子中，位置/姿态估计系统100具有陀螺仪传感器106、加速度传感器107、以及地磁传感器108。具有陀螺仪传感器106、加速度传感器107及地磁传感器108的装置也称为“IMU”。位置/姿态估计装置101与图3所示的其他结构例如通过网络而连接。三维地图DB102也可以是位置/姿态估计装置101的一部分。
三维地图DB102包含在进行绝对位置/姿态的计算时使用的、预先准备的三维地图信息。三维地图DB102无需是实施方式1的位置/姿态估计系统100的一部分，也可以是外部的存储装置所存储的信息。此外，三维地图DB102也可以由位置/姿态估计装置101制作。在该情况下，位置/姿态估计装置101具有作为三维地图制作装置的功能。即，实施方式1的三维地图制作装置是位置/姿态估计装置101的一部分。但是，实施方式1的三维地图制作装置也可以是与位置/姿态估计装置101不同的装置。
距离传感器103是使用红外线或激光等对距离进行计测的设备。摄像头104是取得摄像头图像的设备。位置/姿态估计系统100也可以仅具有摄像头104和距离传感器103中的一方。
显示器105是在将AR内容重叠显示于摄像头图像的情况下需要的显示设备。位置/姿态估计系统100也可以不具备显示器105。
陀螺仪传感器106、加速度传感器107以及地磁传感器108构成IMU，该IMU是用于通过自主导航法进行相对位置/姿态的计算的设备。但是，在不通过自主导航法进行相对位置/姿态的计算的情况下，也可以不具备IMU。此外，也可以仅具备陀螺仪传感器106、加速度传感器107以及地磁传感器108中的1个或2个。另外，与位置/姿态估计装置101连接的设备可以是图3所示的设备的一部分，或者也可以具备图3中未示出的其他设备。
图4是示出位置/姿态估计装置101的硬件结构的例子的图。位置/姿态估计装置101具有作为信息处理部的CPU(Central Processing Unit：中央处理单元)1011、作为存储装置的存储器1012、以及接口1013。三维地图DB102、距离传感器103、摄像头104、显示器105、陀螺仪传感器106、加速度传感器107以及地磁传感器108经由接口1013和数据总线而与CPU1011连接。
位置/姿态估计装置101的各功能由处理电路实现。处理电路可以是专用的硬件，也可以是执行作为存储于存储器1012的软件的程序(例如，位置/姿态估计程序)的CPU1011。CPU1011也可以是处理装置、运算装置、微处理器、微型计算机、处理器、以及DSP(Digital Signal Processor：数字信号处理器)中的任意一方。
在处理电路是专用的硬件的情况下，处理电路例如是单一电路、复合电路、程序化的处理器、并行程序化的处理器、ASIC(Application Specific Integrated Circuit：专用集成电路)、FPGA(Field-Programmable Gate Array：现场可编程门阵列)、或者将它们中的任意硬件组合而得到的硬件。
在处理电路是CPU1011的情况下，位置/姿态估计装置101的功能通过软件、固件、或者软件与固件的组合而实现。软件和固件以程序的形式记述，并存储在存储器1012中。处理电路通过读出并执行存储器1012所存储的程序而实现各部的功能。即，位置/姿态估计装置101在由处理电路执行处理时，执行实施方式1的位置/姿态估计方法。
这里，存储器1012例如是RAM(Random Access Memory：随机存取存储器)、ROM(Read Only Memory：只读存储器)、闪存、EPROM(Erasable Programmable ROM：可擦可编程只读存储器)、EEPROM(Electrically Erasable Programmable ROM：电可擦可编程只读存储器)等非易失性或易失性的半导体存储器、或者磁盘、光盘、高密度盘、DVD(DigitalVersatile Disc：数字通用光盘)等中的任意一方。
另外，也可以是，由专用的硬件实现位置/姿态估计装置101的一部分，由软件或固件实现一部分。这样，处理电路能够通过硬件、软件、固件、或者它们中的任意组合来实现各功能。
《1-1-2》三维地图制作装置
图5是概要地示出实施方式1的三维地图制作装置的结构的功能框图。图5所示的三维地图制作装置是能够执行实施方式1的三维地图制作方法的装置。另外，在以下的说明中，对三维地图制作装置为位置/姿态估计装置101的一部分(即，地图制作登记部)的例子进行说明。但是，三维地图制作装置也可以是与位置/姿态估计装置101不同的装置。在该情况下，三维地图制作装置的硬件结构与图4所示的硬件结构相同。
如图5所示，实施方式1的三维地图制作装置具有关键帧检测部10、关键帧位置/姿态计算部11、位置/姿态方差计算部12、对应关系登记部13、以及数据库保存部(DB保存部)14。这些结构使用由摄像头104(图3)拍摄到的摄像头图像、由距离传感器103(图3)取得的距离信息、以及由IMU(图3)取得的传感器值而构筑三维地图。实施方式1中所需最低限度的数据是摄像头图像。即便在不具备距离信息或IMU的情况下，也能够制作三维地图。
关键帧检测部10执行以下处理：检测在摄像头104的位置移动了预先决定的平行移动量的阈值以上的情况下或者在摄像头104的姿态移动了(即，旋转了)预先决定的旋转量的阈值以上的情况下取得的图像(例如，彩色图像)以及检测到的距离信息，作为关键帧。
关键帧位置/姿态计算部11执行以下处理：通过SLAM等这样的使用了图像的相对位置/姿态的计算方法，计算拍摄了由关键帧检测部10检测到的关键帧的摄像头104或距离传感器103的位置/姿态。关键帧检测部10和关键帧位置/姿态计算部11执行与以往的SLAM技术的情况的处理(例如，非专利文献3所记载的处理)同样的处理。
非专利文献3：Raul Mur-Artal，外1名著，“ORB-SLAM2：An Open-Source SLAMSystem for Monocular，Stereo，and RGB-D Cameras”，IEEE Transactions on Robotics，Vol.33，No.5，2017年10月.
位置/姿态方差计算部12执行针对由关键帧检测部10检测到的关键帧分别求出位置/姿态的方差的处理。作为位置/姿态的方差的计算方法，例如，具有以下所示的第1计算方法和第2计算方法。
第1计算方法是对图像附加噪声(即，随机图案)的方法。图6是示出对关键帧的图像附加随机图案的方法的一例的图。在第1计算方法中，执行多次对关键帧的图像附加随机图案而计算位置/姿态的处理，基于得到的位置/姿态的多个计算结果来求出位置/姿态的方差。
第2计算方法是使用随机数的方法。在第2计算方法中，例如，在绝对位置/姿态的计算的处理中，以排除偏离值为目的而使用Random Sample Consensus(RANSAC：随机样本一致性)。RANSAC的主要目的是排除观测出的数据中的从用于计算绝对位置/姿态的数据偏离的偏离值的数据。但是，在RANSAC中，随机地选择计算对象的样本，计算结果每次成为不同的值，因此，能够将RANSAC用于计算位置/姿态的方差。
[数式1]
和/>
分别表示位置的方差(σtk 2)和姿态的方差(σRk 2)，分别通过式(1)和式(2)来计算。在式(1)和式(2)中，N是正整数，表示在求出方差时使用的试验次数。此外，k表示正整数。
[数式2]
tn和Rn
分别表示在第n次试验中求出的绝对的位置(tn)和绝对的姿态(Rn)。另外，n是1以上且N以下的整数。
[数式3]
μt和μR
分别表示位置的平均(μt)和姿态的平均(μR)，分别通过式(3)和式(4)来计算。
[数式4]
图7是示出将三维地图与平面地图对位而进行登记的处理的图。图7的平面地图是描绘了表示设备的配置的布局的平面布局。在平面地图中，以虚线的矩形示出应配置设备的区域即设置区域。对应关系登记部13进行定义与其他三维地图或整体地图之间的相关性的处理。如图7所示，在平面地图的布局上登记了三维地图(实线的矩形所示。)时，得到三维地图间的位置关系、以及制作中的三维智头与已经构筑的三维地图之间的对应关系。通过将三维地图与平面地图对位而进行登记，从而定义整体地图与三维地图的匹配性或者多个三维地图间的位置关系。
数据库保存部14将通过上述的方法制作出的三维地图(即，例如，通过SLAM得到的每个关键帧的数据)保存于三维地图DB102(图3)。在三维地图DB102中，与关键帧的个数对应地保存了按照每个关键帧得到的位置/姿态、摄像头图像、距离信息(即，距离图像)以及局部特征的点云。
《1-1-3》位置/姿态估计装置
图8是概要地示出实施方式1的位置/姿态估计装置101的结构的功能框图。位置/姿态估计装置101进行基于以不同视点拍摄到的多个传感器数据来计算位置/姿态的处理。这里，传感器数据包含摄像头图像、距离传感器的检测数据。如图8所示，位置/姿态估计装置101具有数据库读入部15、帧选择部16、相对位置/姿态取得部17、绝对位置/姿态计算部18、以及绝对位置/姿态综合部19。
数据库读入部15进行读入存放在DB保存部14所存储的数据库中的三维地图(即，预先准备的三维地图)的处理。
帧选择部16进行如下处理：从以多个不同视点拍摄到的摄像头图像的帧中选择用于计算位置/姿态的帧。作为帧选择方法，例如具有第1选择方法、第2选择方法以及第3选择方法。
在第1选择方法中，使用相对移动量。在第1选择方法中，基于通过计算相对位置/姿态得到的相对位置的变化成为预先决定的位置的变化的阈值以上这样的条件以及相对姿态的变化成为预先决定的姿态的变化的阈值以上这样的条件，判定是否选择帧(关键帧)。例如，选择满足通过计算相对位置/姿态得到的相对位置的变化成为预先决定的位置的变化的阈值以上这样的条件和相对姿态的变化成为预先决定的姿态的变化的阈值以上这样的条件中的至少一方的关键帧。
在第2选择方法中，使用在时间上不同的帧。在第2选择方法中，选择在时间上相邻的视点的帧、或者在时间上以阈值以上的时间间隔拍摄到的帧(关键帧)。
在第3选择方法中，使用方差。在第3选择方法中，基于在制作三维地图时求出的方差来选择要使用的数据。例如，选择方差比预先决定的方差的阈值小的视点的帧(关键帧)。
相对位置/姿态取得部17进行取得与帧对应的相对位置/姿态的处理。相对位置/姿态取得部17取得通过图1的相对位置/姿态的计算的任意方法计算相对位置/姿态而得到的结果。
绝对位置/姿态计算部18进行使用所选择的多个帧来计算绝对位置/姿态的处理。此时，例如，使用Perspective n Points(PnP：N点透视)等来计算位置/姿态。
绝对位置/姿态综合部19进行综合多个位置/姿态的计算结果来计算最终的绝对位置/姿态的处理。作为绝对位置/姿态综合部19进行的综合方法，例如具有第1综合方法、第2综合方法以及第3综合方法。
第1综合方法使用“Winner takes all”方式。即，在第1综合方法中，采用根据方差最小的关键帧估计出的位置/姿态作为最终的结果。
第2综合方法使用加权线性和。即，在第2综合方法中，基于方差进行加权。
[数式5]
和/>
分别表示最终得到的位置和姿态，分别通过式(5)和式(6)来计算。
[数式6]
t′k和R′k
分别表示由帧选择部16得到的帧中的第k个帧的位置(t′k)和第k个帧的姿态(R′k)。
[数式7]
和/>
分别表示针对第k个帧的位置的权重(wtk)和针对第k个帧的姿态的权重(wRk)，分别通过式(7)和式(8)来计算。
针对第k个帧的位置的权重wtk和针对第k个帧的姿态的权重wRk是使用位置的方差σtk 2和姿态的方差σRk 2来计算的，该位置的方差σtk 2和姿态的方差σRk 2是在位置/姿态的计算中使用的关键帧的方差。此外，也可以使用能够视为与位置的方差σtk 2及姿态的方差σRk 2等效的统计指标的标准偏差、即位置的标准偏差σtk和姿态的标准偏差σRk来计算权重。在使用标准偏差的情况下，针对第k个帧的位置的权重wtk和针对第k个帧的姿态的权重wRk分别通过式(9)和式(10)来计算。
[数式8]
在式(5)和式(6)中，关于第k个帧的位置t′k和第k个帧的姿态R′k，并不是直接输入由绝对位置/姿态计算部18计算出的位置/姿态。使用从绝对位置/姿态计算部18计算出的位置/姿态移动到任意的帧时的位置/姿态。例如，在由帧选择部16选择K个(K是正整数)帧并与第K个帧配合地进行综合的情况下，第k个(k是正整数)帧的位置t′k和第k个帧的姿态R′k由式(11)表示。在式(11)中，第k个帧的位置tk和第k个帧的姿态Rk是由绝对位置/姿态计算部18求出的位置/姿态。
[数式9]
Pk→K和tk→K
分别表示从第k个帧向第K个帧的相对姿态的移动量和相对位置的移动量，从由相对位置/姿态取得部17得到的位置/姿态中导出。如果使用式(11)，则能够求出第K个帧的绝对位置/姿态。
[数式10]
第3综合方法是通过非线性优化而求出绝对位置/姿态的方法。例如，如式(12)所示，以再投影误差成为最小的方式求出第k个帧的位置tk和第k个帧的姿态Rk。
[数式11]
L
表示摄像头的内部参数(L)。
[数式12]
pki和p′ki
分别表示匹配的局部特征的三维的位置和图像上的点。
Nk表示第k个帧的匹配的局部特征对的数量。wk是与第k个帧对应的权重，使用权重wtk和wRk中的任意权重、或者将它们综合而得到的权重。
当通过最速下降法等非线性优化方法求解式(12)时，能够得到绝对位置/姿态。
[数式13]
《1-2》动作
《1-2-1》三维地图的生成
图9是示出由实施方式1的三维地图制作装置进行的用于制作三维地图的处理的例子的流程图。如图9所示，关键帧检测部10和关键帧位置/姿态计算部11执行三维地图的制作(步骤S101)。例如使用SLAM，一边检测关键帧一边执行三维地图的制作。
位置/姿态方差计算部12按照每个关键帧(步骤S102)来计算位置/姿态的方差(步骤S103)。接着，对应关系登记部13如图8所示那样进行对应关系的登记(步骤S104)。对应关系登记部13例如执行在平面地图上登记三维地图并定义与整体地图或其他三维地图之间的位置关系的处理。数据库保存部14进行将通过上述处理制作出的地图保存于三维地图DB102的处理(步骤S105)。
《1-2-2》位置/姿态的估计
图10是示出由实施方式1的位置/姿态估计装置101进行的用于估计位置/姿态的处理的例子的流程图。数据库读入部15从DB保存部14读入三维地图作为数据(步骤S111)。帧选择部16选择帧(步骤S112)。所选择的帧是基于预先决定的规则进行处理的帧。
相对位置/姿态取得部17按照所选择的每个帧(步骤S113)，进行取得与帧对应的相对位置/姿态的处理(步骤S114)。绝对位置/姿态计算部18基于所选择的帧的数据，计算绝对位置/姿态(步骤S115)。当所选择的帧的数量达到预先决定的个数时，处理进入综合处理。
绝对位置/姿态综合部19基于方差对绝对位置/姿态的结果进行综合(步骤S116)。
图11是示出由实施方式1的位置/姿态估计装置进行的用于估计位置/姿态的处理的其他例的流程图。数据库读入部15从数据库保存部14中读入三维地图作为数据(步骤S121)。帧选择部16判断是否选择帧(步骤S122、S123)，在选择帧的情况下，选择帧。所选择的帧是基于预先决定的规则进行处理的帧。
相对位置/姿态取得部17进行取得与所选择的帧对应的相对位置/姿态的处理(步骤S124)。绝对位置/姿态计算部18针对所选择的各数据，计算绝对位置/姿态(步骤S125)。帧选择部16判断是否完成了充分的帧检测(步骤S126)，在完成了充分的帧检测的情况下进行综合处理，在未完成充分的帧检测的情况下，将处理返回到步骤S122。“完成了充分的帧检测”例如是指，进行了预先决定的个数的帧检测时、得到预先决定的数量的绝对位置/姿态时等。
在完成了充分的帧检测的情况下，绝对位置/姿态综合部19基于方差对绝对位置/姿态的结果进行综合(步骤S116)。
《1-3》效果
如以上说明的那样，根据实施方式1的位置/姿态估计装置或位置姿态估计方法，基于使用多个图像得到的位置/姿态的数据进行位置/姿态的估计，因此，能够提高位置/姿态的估计的精度。
此外，不使用根据位置/姿态的计算结果的方差大的图像求出的位置/姿态进行综合处理，或者减小关于根据位置/姿态的计算结果的方差大的图像求出的位置/姿态的权重而进行综合处理，由此，能够计算精度高的绝对位置/姿态。
此外，即便在图像内的被摄体缺乏特征的情况下，也能够实现绝对坐标系中的高精度的自己的位置/姿态的估计，并且，能够将运算量抑制到能够进行实时处理的范围。
《2》实施方式2
《2-1》结构
《2-1-1》
在实施方式1中，说明了作为使用摄像头图像的绝对位置/姿态的计算方法，具有使用直接匹配的第1计算方法和使用2个阶段的匹配的第2计算方法。此外，在实施方式1中，说明了使用第2计算方法的例子。在实施方式2中，说明以下方法：对通过使用摄像头图像的直接匹配方法求出的位置/姿态进行综合的方法、以及使用LiDAR(Light Detection andRanging：激光雷达)等激光传感器的位置/姿态综合方法。
实施方式2的位置/姿态估计装置和位置姿态估计系统的硬件结构与在实施方式1(图4和图5)中说明的硬件结构相同。因此，在说明实施方式2时，也参照图4和图5。
《2-1-2》三维地图制作装置
图12是概要地示出实施方式2的三维地图制作装置的结构的功能框图。图12所示的三维地图制作装置是能够执行实施方式2的三维地图制作方法的装置。另外，在以下的说明中，说明三维地图制作装置是位置/姿态估计装置的一部分(即，地图制作登记部)的情况，但三维地图制作装置也可以是与位置/姿态估计装置不同的装置。
如图12所示，实施方式2的三维地图制作装置具有三维地图制作部21、位置/姿态方差计算部22、对应关系登记部23、以及数据库保存部(DB保存部)24。
在实施方式1中，按照每个关键帧对三维数据进行了管理，但在实施方式2中，将点云作为三维地图进行管理。例如，三维地图制作部21在使用图像的情况下，将从图像中得到的局部特征及其位置制作为三维地图。在使用LiDAR等激光传感器的情况下，将观测到的点云(仅位置)制作为三维地图。图12的三维地图制作部21进行制作上述的三维地图数据的处理。
图13是示出实施方式2的三维地图制作装置所使用的方差的计算方法的图。图13示出在应用了AGV的机器人131进行了移动的情况下、机器人利用相对位置/姿态的计算和绝对位置/姿态的计算来估计位置/姿态的例子。机器人131计算方差，但是，方差的计算方法与实施方式1的情况下的计算方法不同。在图13中，针对由椭圆包围的每个区域(例如，区域#1、#2、#3)的方差σ1、σ2、σ3的计算进行说明。
在具体的方差的计算中，从制作三维地图时观测到的数据中，按照每个区域，各取得(采样)任意个数的数据。针对采样得到的数据，多次通过不同的方法施加噪声而进行绝对位置/姿态的估计。要求出的方差是每个特定区域的位置/姿态的方差。施加噪声的方法与实施方式1同样，在图像的情况下是附加随机图案的方法。在LiDAR的情况下也同样，通过对局部的区域附加随机图案而施加噪声。这里，“随机图案”也包含用于去除该区域中的数据的图案。
对应关系登记部23通过与实施方式1的对应关系登记部13同样的方法来定义与整体地图或其他三维地图之间的相关性。
DB保存部24将三维地图和每个区域的方差保存于数据库。
《2-1-3》位置/姿态估计装置
图14是概要地示出实施方式2的位置/姿态估计装置的结构的功能框图。图14所示的位置/姿态估计装置能够执行实施方式2的位置/姿态估计方法的装置。位置/姿态估计装置具有数据库读入部25、帧选择部26、相对位置/姿态取得部27、绝对位置/姿态计算部28以及绝对位置/姿态综合部29。
数据库读入部25进行读入存储在数据库中的三维地图数据的处理。
帧选择部26与实施方式1的帧选择部16同样地选择帧，或者从预先划分的多个区域中以不重复的方式选择帧。例如，如图13所示，针对划分为3个区域#1、#2、#3而计算方差σ1、σ2、σ3并进行管理的例子进行说明。针对位置/姿态的计算结果是区域#1、#2、#3中分别包含的帧的个数为M个的例子进行说明。
相对位置/姿态取得部27通过与实施方式1中的相对位置/姿态取得部17相同的方法而取得相对位置/姿态。
绝对位置/姿态计算部28在图像的情况下，使用直接匹配图像的局部特征进行位置/姿态的计算的方法(例如参照非专利文献4)等来计算绝对位置/姿态。在使用利用了LiDAR的距离传感器的数据的情况下，通过进行三维地图与由距离传感器观测到的形状信息的匹配来计算位置/姿态。
非专利文献4：Torsten Sattler，外2名著，“Efficient&Effective PrioritizedMatching for Large-Scale Image-Based Localization”，IEEE Transactions onPattern Analysis and Machine Intelligence，Vol.39，No.9，2017年9月.
绝对位置/姿态综合部29通过与实施方式1中的绝对位置/姿态综合部19同样的方法对多个位置/姿态进行综合。绝对位置/姿态综合部29基于按照每个区域而设定的方差求出最终的位置/姿态。
《2-2》动作
《2-2-1》三维地图的制作
图15是示出由实施方式2的三维地图制作装置进行的用于制作三维地图的处理的例子的流程图。三维地图制作部21和位置/姿态方差计算部22制作三维地图，在使用图像的情况下，制作表示局部特征及其位置的地图，在使用LiDAR的情况下，制作表示点云(仅位置)的地图(步骤S201)。对应关系登记部23进行作为与三维地图制作部21对应的处理的对应关系的登记(步骤S202)。数据库保存部24进行将对应关系保存于数据库的处理(步骤S203)。
《2-2-2》位置/姿态的估计
图16是示出由实施方式2的位置/姿态估计装置进行的用于估计位置/姿态的处理的例子的流程图。数据库读入部25进行从数据库中读入数据的处理(步骤S211)。相对位置/姿态取得部27取得相对位置/姿态，绝对位置/姿态计算部28进行绝对位置/姿态的计算(步骤S212～S214)。
帧选择部判断是否需要进行帧选择(步骤S215)，在需要的情况下，判断是否完成了充分的帧检测(步骤S216)。在完成了充分的帧检测的情况下，绝对位置/姿态综合部29进行绝对位置/姿态的综合(步骤S217)。
《2-3》效果
如以上说明的那样，根据实施方式2的位置/姿态估计装置或位置姿态估计方法，在使用图像且使用直接的匹配方法的情况下，或者在使用由LiDAR观测到的形状来计算绝对位置/姿态的例子中，由于使用了多个帧，因此能够提高位置/姿态的估计的精度。
《3》实施方式3
《3-1》结构
《3-1-1》
在AR中使用的终端或机器人有时在设备内管理相对位置/姿态。在AR的情况下，通过将以绝对位置/姿态定义的内容的位置转换成由终端管理的相对位置/姿态的坐标系，从而将内容重叠显示在图像上。在机器人的情况下也同样，在以绝对位置定义了机器人的目的地的情况下，需要将该位置转换成由机器人管理的相对位置/姿态的坐标系。
实施方式3对以下方法进行叙述：使用多个帧高精度地计算外部参数，该外部参数是从绝对位置/姿态的坐标系转向相对位置/姿态的坐标系转换的矩阵。这里，以实施方式1为基础，来叙述使用多个帧高精度地求出外部参数的方法。实施方式3的结构也可以与实施方式2或4组合。
实施方式2的位置/姿态估计装置和位置姿态估计系统的硬件结构与实施方式1(图4和图5)中说明的硬件结构相同。因此，在说明实施方式2时，也参照图4和图5。
《3-1-2》三维地图制作装置
实施方式2的三维地图制作装置与实施方式1的三维地图制作装置相同。
《3-1-3》位置/姿态估计装置
图17是概要地示出实施方式3的位置/姿态估计装置的结构的功能框图。图17所示的位置/姿态估计装置是能够执行实施方式3的位置/姿态估计方法的装置。位置/姿态估计装置具有数据库读入部35、帧选择部36、相对位置/姿态取得部37、绝对位置/姿态计算部38、外部参数计算部38a、以及作为绝对位置/姿态综合部的外部参数综合部39。
实施方式3与实施方式1的不同点在于，具有按照所选择的每个帧来计算外部参数的外部参数计算部38a、以及对多个外部参数进行综合的外部参数综合部39。关于这些以外的处理，实施方式3的处理与实施方式1的处理相同。
外部参数计算部38a假定基于相同的帧进行了相对位置/姿态的计算和绝对位置/姿态的计算来计算外部参数。此时，外部参数通过式(13)来计算。
[数式14]
和/>
表示使用第k个帧求出的绝对位置和绝对姿态。
[数式15]
和/>
表示第k个帧的相对位置和相对姿态。
[数式16]
t′k和R′k
表示第k个帧的外部参数(t′k和R′k)，通过式(13)来计算。
[数式17]
外部参数综合部39的处理与实施方式1的绝对位置/姿态综合部19(图8)大致相同。因此，外部参数综合部39也称为绝对位置/姿态综合部。在实施方式1中，对多个绝对位置/姿态进行了综合，但在实施方式3中，对外部参数进行综合。具体而言，使用通过式(13)求出的t′k和R′k进行综合。例如，在通过加权线性和进行综合的情况下，将式(13)的t′k、R′k代入到式(5)和式(6)进行综合。
《3-2》动作
《3-2-1》三维地图的制作
三维地图制作的处理流程与实施方式1相同，因此省略。叙述对外部参数进行综合的方法的处理流程。
《3-2-2》位置/姿态的估计
图18是示出由实施方式3的位置/姿态估计装置进行的用于估计位置/姿态的处理的例子的流程图。数据库读入部35进行从数据库中读入数据的处理(步骤S301)。相对位置/姿态取得部37取得装置位置/姿态(步骤S302、S303)。绝对位置/姿态计算部38进行绝对位置/姿态的计算(步骤S304)。外部参数计算部38a计算外部参数(步骤S305)。
帧选择部36判断是否需要帧选择(步骤S306)，在需要的情况下，判断是否完成了充分的帧检测(步骤S307)。在完成了充分的帧检测的情况下，外部参数综合部39进行绝对位置/姿态的综合(步骤S308)。
《3-3》效果
如以上说明的那样，根据实施方式3的位置/姿态估计装置或位置姿态估计方法，能够高精度地求出从绝对位置/姿态的坐标系向相对位置/姿态的坐标系的转换矩阵，因此，能够在应用于AR的终端高精度地显示内容。此外，在应用于机器人的情况下，能够高精度地求出机器人的目的地。
《4》实施方式4
《4-1》结构
《4-1-1》
在实施方式4中，叙述对实施方式1中的位置/姿态估计方法加入使用了多个帧的错误处理的方式。在使用了图像的绝对位置/姿态的计算中，在被摄体中特征性的图案较少的情况下，所输出的位置/姿态的值可能包含较大的误差。为了排除这样的结果，实施方式4的位置/姿态估计装置执行错误处理。
实施方式4的位置/姿态估计装置和位置姿态估计系统的硬件结构基本上与实施方式1(图4和图5)中说明的硬件结构相同。因此，在说明实施方式4时，也参照图4和图5。
《4-1-2》三维地图制作装置
实施方式4的三维地图制作装置的结构与实施方式1的结构相同。
《4-1-3》位置/姿态估计装置
图19是概要地示出实施方式4的位置/姿态估计装置的结构的功能框图。图19所示的位置/姿态估计装置是能够执行实施方式4的位置/姿态估计方法的装置。实施方式4的位置/姿态估计装置在追加了错误处理部48a这一点与实施方式1不同。
如图19所示，实施方式4的位置/姿态估计装置具有数据库读入部45、帧选择部46、相对位置/姿态取得部47、绝对位置/姿态计算部48、错误处理部48a、以及绝对位置/姿态综合部49。数据库读入部45、帧选择部46、相对位置/姿态取得部47、绝对位置/姿态计算部48以及绝对位置/姿态综合部49与图8所示的数据库读入部15、帧选择部16、相对位置/姿态取得部17、绝对位置/姿态计算部18以及绝对位置/姿态综合部19相同。
错误处理部48a进行错误处理。在错误处理中，对通过多个帧而求出的绝对位置/姿态的多个计算结果进行比较，由此，从综合处理中排除误差比预先决定的阈值大的绝对位置/姿态的计算结果。式(14)和式(15)示出错误处理的一例。第j个(j为K以下的正整数。)帧是在由帧选择部46选择出的K个帧中满足j≠k的帧。通过多个帧而求出的绝对位置/姿态的多个计算结果应该指示相同的位置/姿态。因此，错误处理部48a将通过某个帧而求出的绝对位置/姿态的计算结果与通过其他帧而求出的绝对位置/姿态的计算结果进行比较。错误处理部48a在计算出的位置的差比预先决定的阈值th_t大的情况(即，满足式(14)的情况)下、或者在计算出的姿态的差比预先决定的阈值th_r大的情况(即，满足式(15)的情况)下，排除该帧的计算结果。
[数式18]
||t′k-t′j||＞th_t (14)
《4-2》动作
《4-2-1》三维地图的制作
实施方式4的三维地图制作装置的动作与实施方式1的动作相同。
《4-2-2》位置/姿态的估计
图20是示出由实施方式4的位置/姿态估计装置进行的用于估计位置/姿态的处理的其他例的流程图。图20所示的位置/姿态估计装置的动作在追加了错误处理(步骤S406a)这一点与图11所示的实施方式1的位置/姿态估计装置的动作不同。图20所示的步骤S401～S406的处理及步骤S407的处理与图11所示的步骤S101～S107的处理相同。
《4-3》效果
如以上说明的那样，根据实施方式4的位置/姿态估计装置或位置姿态估计方法，进行了错误处理，因此，能够实现耐环境性比实施方式1的情况高的绝对位置/姿态的估计(即，针对各种环境能够实现绝对位置/姿态较高的估计精度)。
附图标记说明
10关键帧检测部，11关键帧位置/姿态计算部，12、22位置/姿态方差计算部，13、23对应关系登记部，14、24DB保存部，15、25、35、45数据库读入部，16、26、36、46帧选择部，17、27、37、47相对位置/姿态取得部，18、28、38、48绝对位置/姿态计算部，19、29、49绝对位置/姿态综合部，21三维地图制作部，38a外部参数计算部，39外部参数综合部(绝对位置/姿态综合部)，48a错误处理部，100位置/姿态估计系统，101位置/姿态估计装置，102三维地图DB，103距离传感器，104摄像头，105显示器，106陀螺仪传感器，107加速度传感器，108地磁传感器。

Claims (13)

1.一种位置/姿态估计装置，其具有：
数据库读入部，其从数据库中读入三维地图的数据；
帧选择部，其进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；
相对位置/姿态取得部，其进行取得关于由所述帧选择部选择出的多个帧的多个相对位置/姿态的处理；
绝对位置/姿态计算部，其进行取得关于由所述帧选择部选择出的多个帧的多个绝对位置/姿态的处理；以及
绝对位置/姿态综合部，其对由所述相对位置/姿态取得部取得的相对位置/姿态和由所述绝对位置/姿态计算部取得的绝对位置/姿态进行综合，取得最终的绝对位置/姿态。
2.根据权利要求1所述的位置/姿态估计装置，其中，
所述位置/姿态估计装置还具有地图制作登记部，
所述地图制作登记部具有：
关键帧检测部，其从由摄像头拍摄到的摄像头图像中检测关键帧；
关键帧位置/姿态计算部，其计算拍摄了所述关键帧的所述摄像头的位置和姿态；
位置/姿态方差计算部，其执行按照每个所述关键帧计算位置/姿态的方差的处理；
对应关系登记部，其进行将三维地图与平面地图对位而制作登记数据的处理；以及
数据库保存，其进行将所述登记数据保存于所述数据库的处理。
3.根据权利要求2所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部基于按照每个所述关键帧计算出的所述位置/姿态的方差，对所述多个绝对位置/姿态进行综合。
4.根据权利要求3所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部采用根据所述关键帧中的所述方差最小的关键帧估计出的位置/姿态作为所述最终的绝对位置/姿态。
5.根据权利要求3所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部按照每个所述关键帧计算基于所述方差的权重，基于使用所述权重的加权线性和，对所述多个绝对位置/姿态进行综合。
6.根据权利要求2所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部使用非线性优化对所述多个绝对位置/姿态进行综合。
7.根据权利要求1所述的位置/姿态估计装置，其中，
所述位置/姿态估计装置还具有地图制作登记部，
所述地图制作登记部具有：
三维地图制作部，其根据由摄像头拍摄到的摄像头图像或者由距离传感器测定出的距离信息，按照每个局部的区域来制作三维地图；
位置/姿态方差计算部，其执行根据所述摄像头图像或所述距离信息并按照每个所述区域来计算位置/姿态的方差的处理；
对应关系登记部，其进行将三维地图与平面地图对位而制作登记数据的处理；以及
数据库保存，其进行将所述登记数据保存于所述数据库的处理。
8.根据权利要求2所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部基于按照每个所述关键帧而计算出的每个所述区域的位置/姿态的方差，对所述多个绝对位置/姿态进行综合。
9.根据权利要求2所述的位置/姿态估计装置，其中，
所述位置/姿态估计装置还具有外部参数计算部，该外部参数计算部按照每个所述关键帧来计算外部参数，
所述绝对位置/姿态综合部对由所述外部参数计算部计算出的多个外部参数进行综合，从而对所述多个绝对位置/姿态进行综合。
10.根据权利要求9所述的位置/姿态估计装置，其中，
所述绝对位置/姿态综合部基于按照每个所述关键帧计算出的所述位置/姿态的方差，对所述多个外部参数进行综合。
11.根据权利要求1至10中的任意一项所述的位置/姿态估计装置，其中，
所述位置/姿态估计装置还具有错误处理部，该错误处理部在按照每个所述关键帧计算出的绝对位置/姿态的误差比预先决定的阈值大的情况下，不将误差比所述阈值大的所述绝对位置/姿态的计算结果用于综合处理。
12.一种位置/姿态估计方法，其是由位置/姿态估计装置执行的方法，其中，
所述位置/姿态估计方法具有以下步骤：
从位置数据库中读入三维地图的数据；
进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；
进行取得关于选择出的多个所述帧的多个相对位置/姿态的处理；
进行取得关于选择出的多个所述帧的多个绝对位置/姿态的处理；以及
对取得的所述相对位置/姿态和取得的所述绝对位置/姿态进行综合，取得最终的绝对位置/姿态。
13.一种程序，其中，
所述程序使计算机执行以下步骤：
从位置数据库中读入三维地图的数据；
进行从以多个不同视点拍摄到的图像的帧中选择用于计算位置/姿态的帧的处理；
进行取得关于选择出的多个所述帧的多个相对位置/姿态的处理；
进行取得关于选择出的多个所述帧的多个绝对位置/姿态的处理；以及
对取得的所述相对位置/姿态和取得的所述绝对位置/姿态进行综合，取得最终的绝对位置/姿态。
CN202080107829.5A 2020-12-18 2020-12-18 位置/姿态估计装置、位置/姿态估计方法及程序 Pending CN116635891A (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2020/047417 WO2022130618A1 (ja) 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム

Publications (1)

Publication Number Publication Date
CN116635891A true CN116635891A (zh) 2023-08-22

Family

ID=82059339

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202080107829.5A Pending CN116635891A (zh) 2020-12-18 2020-12-18 位置/姿态估计装置、位置/姿态估计方法及程序

Country Status (7)

Country Link
US (1) US20230260149A1 (zh)
JP (1) JP7258250B2 (zh)
KR (1) KR20230087613A (zh)
CN (1) CN116635891A (zh)
DE (1) DE112020007700T5 (zh)
TW (1) TWI817124B (zh)
WO (1) WO2022130618A1 (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150246654A1 (en) * 2012-01-13 2015-09-03 Pulse Function F6 Ltd Telematics system with 3d intertial sensors
WO2018142580A1 (ja) * 2017-02-03 2018-08-09 三菱電機株式会社 表示制御装置および表示制御方法
JP6842039B2 (ja) * 2017-03-02 2021-03-17 富士通株式会社 カメラ位置姿勢推定装置、方法およびプログラム
WO2019098002A1 (ja) * 2017-11-20 2019-05-23 ソニー株式会社 情報処理装置、情報処理方法、プログラム、及び移動体
JP7077691B2 (ja) 2018-03-16 2022-05-31 トヨタ自動車株式会社 自己位置検出装置
EP3904992B1 (en) * 2018-12-28 2024-01-10 Panasonic Intellectual Property Management Co., Ltd. Positioning apparatus and moving body

Also Published As

Publication number Publication date
US20230260149A1 (en) 2023-08-17
TW202226003A (zh) 2022-07-01
TWI817124B (zh) 2023-10-01
JPWO2022130618A1 (zh) 2022-06-23
WO2022130618A1 (ja) 2022-06-23
KR20230087613A (ko) 2023-06-16
JP7258250B2 (ja) 2023-04-14
DE112020007700T5 (de) 2023-08-03

Similar Documents

Publication Publication Date Title
US9965870B2 (en) Camera calibration method using a calibration target
US10260862B2 (en) Pose estimation using sensors
US11830216B2 (en) Information processing apparatus, information processing method, and storage medium
CN108052103B (zh) 基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法
WO2018177159A1 (zh) 运动物体的位置确定方法及系统
US10895458B2 (en) Method, apparatus, and system for determining a movement of a mobile platform
EP3460715B1 (en) Template creation apparatus, object recognition processing apparatus, template creation method, and program
US10451403B2 (en) Structure-based camera pose estimation system
US11062475B2 (en) Location estimating apparatus and method, learning apparatus and method, and computer program products
CN107025663A (zh) 视觉系统中用于3d点云匹配的杂波评分系统及方法
JP2007322138A (ja) 移動装置及び移動装置の自己位置推定方法
WO2018142533A1 (ja) 位置姿勢推定装置および位置姿勢推定方法
JP6410231B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
CN113643380A (zh) 一种基于单目相机视觉标靶定位的机械臂引导方法
CN108447092B (zh) 视觉定位标识物的方法及装置
Kokovkina et al. The algorithm of EKF-SLAM using laser scanning system and fisheye camera
CN108564626B (zh) 用于确定安装于采集实体的相机之间的相对姿态角的方法和装置
RU2583756C2 (ru) Способ определения местоположения на основе сигнатур изображений городской застройки в видимом и инфракрасном диапазонах
JP7258250B2 (ja) 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム
CN113628284B (zh) 位姿标定数据集生成方法、装置、系统、电子设备及介质
JP2011174891A (ja) 位置姿勢計測装置、位置姿勢計測方法、及びプログラム
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
JP2017182564A (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
CN114719759B (zh) 一种基于slam算法和图像实例分割技术的物体表面周长和面积测量方法
CN113034538A (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备

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