CN111664857B - 自己位置估算装置、自己位置估算方法及记录介质 - Google Patents

自己位置估算装置、自己位置估算方法及记录介质 Download PDF

Info

Publication number
CN111664857B
CN111664857B CN202010084076.2A CN202010084076A CN111664857B CN 111664857 B CN111664857 B CN 111664857B CN 202010084076 A CN202010084076 A CN 202010084076A CN 111664857 B CN111664857 B CN 111664857B
Authority
CN
China
Prior art keywords
key frame
map
estimating
position estimation
posture
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010084076.2A
Other languages
English (en)
Other versions
CN111664857A (zh
Inventor
北岛克将
木内裕介
藤田匠
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Mitsubishi Heavy Industries Ltd
Original Assignee
Mitsubishi Heavy Industries 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 Mitsubishi Heavy Industries Ltd filed Critical Mitsubishi Heavy Industries Ltd
Publication of CN111664857A publication Critical patent/CN111664857A/zh
Application granted granted Critical
Publication of CN111664857B publication Critical patent/CN111664857B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B66—HOISTING; LIFTING; HAULING
    • B66F—HOISTING, LIFTING, HAULING OR PUSHING, NOT OTHERWISE PROVIDED FOR, e.g. DEVICES WHICH APPLY A LIFTING OR PUSHING FORCE DIRECTLY TO THE SURFACE OF A LOAD
    • B66F9/00—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes
    • B66F9/06—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes movable, with their loads, on wheels or the like, e.g. fork-lift trucks
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B66—HOISTING; LIFTING; HAULING
    • B66F—HOISTING, LIFTING, HAULING OR PUSHING, NOT OTHERWISE PROVIDED FOR, e.g. DEVICES WHICH APPLY A LIFTING OR PUSHING FORCE DIRECTLY TO THE SURFACE OF A LOAD
    • B66F9/00—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes
    • B66F9/06—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes movable, with their loads, on wheels or the like, e.g. fork-lift trucks
    • B66F9/075—Constructional features or details
    • B66F9/0755—Position control; Position detectors
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/14—Transformations for image registration, e.g. adjusting or mapping for alignment of images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • 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/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Structural Engineering (AREA)
  • Multimedia (AREA)
  • Civil Engineering (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Mechanical Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Analysis (AREA)

Abstract

目的是提供一种能够实现使用照相机的自己位置估算处理的稳定化的自己位置估算装置等。具备：取得部，从搭载在移动体上的照相机取得视野图像；估算部，基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图，进行估算前述世界坐标上的前述移动体的位置及姿势的自己位置估算处理；以及登录部，将前述视野图像作为用来向前述地图追加该视野图像上包含的新的特征点的关键帧而登录。前述登录部在前述移动体的移动满足关于该移动体的位置及姿势的至少其中一个而预先规定的条件的情况下，将前述视野图像作为前述关键帧而登录。

Description

自己位置估算装置、自己位置估算方法及记录介质
技术领域
本发明涉及自己位置估算装置、自己位置估算方法及记录介质。
背景技术
已知有高精度地检测移动体的位置及姿势，利用该位置及姿势的检测结果进行移动体的操舵控制的技术。
在专利文献1中，公开了一种基于来自相互不同的传感器的输入而估算自己位置的技术。
作为用来高精度地检测移动体的位置及姿势的技术，已知有LiDAR(激光探测与测距(Laser Detection and Ranging))，但要求搭载昂贵的传感器(激光扫描器)。另一方面，关于使用比较便宜的传感器即照相机的自己位置估算技术也推进了开发。
在非专利文献1中，记载了以使用照相机的自己位置估算技术即ORB－SLAM(ORB－同时定位映射(Simultaneous Localization and Mapping))为基础、仅在地图制作时援用LiDAR、在实际运用时仅用照相机进行高精度的自己位置估算的方法。
现有技术文献
专利文献
专利文献1：日本特开2012－248032号公报
非专利文献
非专利文献1：Adi Sujiwo，Eijiro Takeuchi，Luis Yoichi Morales，NaokiAkai，Yoshiki Ninomiya and Masato Edahiro，“Localization Based on MultipleVisual－Metric Maps”in Proc of IEEE International Conference on MultisensorFusion and Integration for Intelligent Systems，2017.
发明内容
发明要解决的课题
在基于ORB－SLAM的地图制作处理中，制作记录有多个特征点的世界坐标上的位置及观测到这些特征点的视野图像(帧)即多个关键帧的地图。在地图中，还记录通过基于ORB－SLAM的自己位置估算处理估算出的各关键帧取得时的照相机的位置及姿势。
在基于ORB－SLAM的自己位置估算处理中，通过将由从照相机取得的视野图像(帧)观测到的特征点与记录在地图上的特征点对齐(匹配)，来估算该视野图像取得时的照相机的世界坐标上的位置及姿势。此时，对于从照相机取得的视野图像，确定特征点的位置关系最近的关键帧(当前关键帧)，通过以该当前关键帧为线索，能够实现自己位置估算处理的高速化。
已知在通过从照相机取得的视野图像观测到的特征点与记录在地图上的特征点的对齐数较少(即，在所取得的视野图像中几乎没有拍摄到记录于地图上的特征点)的情况下等，自己位置估算处理失败，迷失(迷路)了自己的位置及姿势。在ORB－SLAM中，要求不迷失自己的位置及姿势而进行稳定的自己位置估算处理。
本发明的目的是提供一种能够实现使用照相机的自己位置估算处理的稳定化的自己位置估算装置、自己位置估算方法及记录介质。
用来解决课题的手段
根据本发明的第1技术方案，自己位置估算装置具备：取得部，从搭载在移动体上的照相机取得视野图像；估算部，基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图，进行估算前述世界坐标上的前述移动体的位置及姿势的自己位置估算处理；以及登录部，将前述视野图像作为用来向前述地图追加该视野图像上包含的新的特征点的关键帧而登录。前述登录部在前述移动体的移动满足关于该移动体的位置及姿势的至少其中一个而预先规定的条件的情况下，将前述视野图像作为前述关键帧而登录。
此外，根据本发明的第2技术方案，前述登录部在前述移动体的位置与关联于该位置的1个前述关键帧的距离离开了规定的距离阈值以上的情况下，将前述视野图像作为关键帧而登录。
此外，根据本发明的第3技术方案，前述登录部在移动中的前述移动体的角速度成为规定的角速度阈值以上的情况下，将前述视野图像作为关键帧而登录。
此外，根据本发明的第4技术方案，前述登录部基于防删除标志的有无，进行对于已登录的多个前述关键帧的剔除处理。对于作为多个前述关键帧之一的对象关键帧，基于比该对象关键帧更前1个被登录的关键帧和比该对象关键帧更后1个被登录的关键帧的位置及姿势的至少其中一个的关系，判定是否对该对象关键帧赋予前述防删除标志。
此外，根据本发明的第5技术方案，上述自己位置估算装置还具备将前述地图中的一部分区域读出到存储器上的读出部。
此外，根据本发明的第6技术方案，前述读出部将前述地图中的以前述移动体的位置为基准的前述一部分区域读出到存储器上。
此外，根据本发明的第7技术方案，前述读出部将前述地图中的与前述移动体的宽度方向相比在前述移动体的前后方向上更宽的前述一部分区域读出到存储器上。
此外，根据本发明的第8技术方案，前述读出部将前述地图中的与特定的行驶路线对应的前述一部分区域读出到存储器上。
此外，根据本发明的第9技术方案，上述自己位置估算装置具备在前述自己位置估算处理失败的情况下进行探索前述关键帧的再定位的处理部。前述处理部基于前述移动体的硬件的制约条件而确定探索范围，并且探索该探索范围中包含的关键帧。
此外，根据本发明的第10技术方案，前述处理部基于由前述自己位置估算处理估算出的位置和前述移动体的最大速度，将能够取为该移动体的当前的位置的范围确定为前述探索范围。
此外，根据本发明的第11技术方案，前述处理部基于由前述自己位置估算处理估算出的姿势和前述移动体的最大角速度，将能够取为该移动体的当前的姿势的范围确定为前述探索范围。
此外，根据本发明的第12技术方案，前述处理部将以由前述自己位置估算处理估算出的位置为基准的、可看作由里程计得到的估算位置的误差的范围确定为前述探索范围。
此外，根据本发明的第13技术方案，前述处理部将以由前述自己位置估算处理估算出的位置为基准的、可看作由陀螺仪传感器得到的估算姿势的误差的范围确定为前述探索范围。
此外，根据本发明的第14技术方案，前述估算部对由前述自己位置估算处理估算出的位置及姿势进行修正，以使前述世界坐标上的前述移动体的位置及姿势的轨迹平滑化。
此外，根据本发明的第15技术方案，前述估算部使用移动平均滤波器，对由前述自己位置估算处理估算出的位置及姿势进行修正。
此外，根据本发明的第16技术方案，自己位置估算方法具有：从搭载在移动体上的照相机取得视野图像的步骤；基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图、进行估算前述世界坐标上的前述移动体的位置及姿势的自己位置估算处理的步骤；以及将前述视野图像作为用来将该视野图像上包含的新的特征点追加到前述地图上的关键帧而登录的步骤。在作为前述关键帧而登录的步骤中，在前述移动体的移动满足关于该移动体的位置及姿势的至少其中一个而规定的条件的情况下，将前述视野图像作为前述关键帧而登录。
此外，根据本发明的第17技术方案，是一种计算机可读取的记录介质，存储有程序，所述程序使自己位置估算装置的计算机执行：从搭载在移动体上的照相机取得视野图像的步骤；基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图、进行估算前述世界坐标上的前述移动体的位置及姿势的自己位置估算处理的步骤；以及将前述视野图像作为用来将该视野图像上包含的新的特征点追加到前述地图上的关键帧而登录的步骤。在作为前述关键帧而登录的步骤中，在前述移动体的移动满足关于该移动体的位置及姿势的至少其中一个而规定的条件的情况下，将前述视野图像作为前述关键帧而登录。
发明效果
根据上述的发明的各技术方案，能够实现使用照相机的自己位置估算处理的稳定化。
附图说明
图1是表示有关第1实施方式的叉式起重机的整体结构的图。
图2是表示有关第1实施方式的自己位置估算装置的功能结构的图。
图3是表示有关第1实施方式的关键帧表的例子的图。
图4是表示有关第1实施方式的地图的例子的图。
图5是说明有关第1实施方式的变换矩阵的图。
图6是表示有关第1实施方式的自己位置估算装置的地图制作时的处理流程的图。
图7是表示有关第1实施方式的自己位置估算装置的实际运用时的处理流程的图。
图8是表示有关第1实施方式的变形例的自己位置估算装置的地图制作时的处理流程的图。
图9是说明有关第2实施方式的地图读出部的处理的图。
图10是说明有关第2实施方式的变形例的地图读出部的处理的图。
图11是表示有关第3实施方式的再定位(relocalization)处理部的处理流程的图。
图12是说明有关第3实施方式的再定位处理部的处理的图。
图13是说明有关第4实施方式的自己位置估算处理部的功能的图。
具体实施方式
<第1实施方式>
以下，参照图1～图7对有关第1实施方式的自己位置估算装置及具备它的移动体进行说明。
(叉式起重机的整体结构)
图1是表示有关第1实施方式的叉式起重机的整体结构的图。
图1所示的叉式起重机1是移动体的一形态，是沿着预先设定的路线自律行驶到目标地点的无人叉式起重机。
叉式起重机1具备自己位置估算装置10、照相机11、轮胎旋转角传感器12、舵角传感器13和陀螺仪传感器14。
自己位置估算装置10进行基于ORB－SLAM的自己位置估算处理。具体而言，基于从照相机11取得的图像(以下，也表述为视野图像)，对叉式起重机1的位置及姿势进行估算。关于自己位置估算装置10的详细的结构及功能在后面叙述。
照相机11是能够取得运动图像摄影、即能够连续地取得视野图像的摄影装置。视野图像的1个1个相当于构成运动图像的“帧”。假设有关本实施方式的照相机11是“单眼”而进行说明，但在其他实施方式中也可以是“复眼”。
轮胎旋转角传感器12是能够检测叉式起重机1的轮胎旋转角的传感器。
舵角传感器13是能够检测轮胎的舵角的传感器。通过将由上述的轮胎旋转角传感器12得到的轮胎旋转角的检测结果与由舵角传感器13得到的舵角的检测结果组合，能够确定叉式起重机1从某个位置移动后的相对位置。
陀螺仪传感器14是能够检测叉式起重机1的旋转角速度的传感器。
此外，在有关本实施方式的叉式起重机1上，在叉式起重机1的实际运用开始前进行的地图制作时搭载激光扫描器(在图1中没有图示)。该激光扫描器在叉式起重机1的实际运用时(一边进行使用已制作的地图的基于ORB－SLAM的自己位置估算处理一边行驶时)被拆卸。
该激光扫描器是被用于LiDAR的传感器，是能够基于遍及大范围被扫掠的激光的反射而对与配置在激光扫描器的周围的构造物的间隔而进行测距的传感器。通过利用由激光扫描器得到的遍及大范围的测距的SLAM即LiDAR－SLAM，能够以比基于ORB－SLAM的自己位置估算处理更高的精度对叉式起重机1的位置及姿势进行估算。
此外，有关本实施方式的叉式起重机1具备根据自己位置估算装置10输出的基于ORB－SLAM的自己位置估算处理的结果进行操舵控制的操舵控制装置(在图1中没有图示)。该操舵控制装置例如通过基于由自己位置估算装置10估算出的叉式起重机1的位置及姿势的反馈控制进行操舵控制。
(叉式起重机的功能结构)
图2是表示有关第1实施方式的自己位置估算装置的功能结构的图。
如图2所示，自己位置估算装置10具备CPU100、存储器101、存储设备102和通信接口103。
CPU100是负责自己位置估算装置10的动作整体的处理器，通过按照预先准备的程序进行动作而发挥各种功能。
存储器101是例如DRAM等所谓的主存储装置，是为了CPU100按照程序以高速进行动作而需要的存储区域。
存储设备102是例如HDD、SSD等所谓的辅助存储装置，预先记录有在自己位置估算装置10的处理中需要的信息。
通信接口103是用来与照相机11、轮胎旋转角传感器12、舵角传感器13、陀螺仪传感器14等进行通信的连接接口。
对CPU100的功能详细地进行说明。
CPU100通过按照预先准备的程序进行动作，发挥作为视野图像取得部1000(取得部)、自己位置估算处理部1001(估算部)、关键帧登录部1002(登录部)、地图读出部1003(读出部)及再定位处理部1004(处理部)的功能。
视野图像取得部1000从搭载在叉式起重机1上的照相机11，依次取得作为运动图像的各帧的视野图像。
自己位置估算处理部1001进行基于ORB－SLAM的自己位置估算处理。具体而言，自己位置估算处理部1001基于从照相机11取得的视野图像上的特征点的位置、和记录有世界坐标上的多个特征点的位置的地图M，对世界坐标上的叉式起重机1(照相机11)的位置及姿势进行估算。
关键帧登录部1002在地图制作时，将从照相机11取得的视野图像中的满足特定的条件者作为关键帧而登录。关键帧被选定为用来将由该视野图像(帧)观测到的特征点追加到地图M上的帧。
地图读出部1003将地图M读出到存储器101上。
再定位处理部1004在自己位置估算处理部1001的基于ORB－SLAM的自己位置估算处理失败的情况下进行再定位。所述的再定位，是探索是否存在与在当前时点取得的视野图像相似的关键帧而实现复原的处理。
(关键帧表的例子)
图3是表示有关第1实施方式的关键帧表的例子的图。
关键帧登录部1002在地图制作时，将满足规定的条件的视野图像作为关键帧而登录到关键帧表中。
如图3所示，关键帧表将“关键帧号码”、“视野图像”、“图像上的特征点的位置”、“对应的地图上的特征点”、“移动体的位置、姿势”及“变换矩阵”建立关联而记录。
“关键帧号码”是为了确定关键帧而分配的识别号码。
“视野图像”是视野图像本身的图像数据。另外，在其他实施方式中并不限定于记录视野图像的图像数据的形态，也可以是以容量比图像数据小且适合于测量图像的类似度的其他形式记录的形态。例如，也可以是被称作BoW(Bag of Words)的特征向量的统计信息。
“图像上的特征点的位置”是由视野图像观测到的特征点的图像上的位置(由像素坐标(px，py)确定的位置)。根据图3所示的关键帧表，例如根据关键帧号码“n”的关键帧(视野图像)观测特征点q1－n，q2－n，q3－n，…，分别在视野图像上的位置(px，py)上被确定。
“对应的地图上的特征点”表示与由关键帧观测到的特征点对应的地图M上的特征点。根据图3所示的关键帧表，例如，由关键帧号码“n”的关键帧观测到的特征点q1－n，q2－n，q3－n，…与被标绘到地图M的世界坐标上的特征点Q2，Q4，Q5，…分别对应。地图M上的各特征点Q2，Q4，Q5，…分别由世界坐标值(X，Y，Z)规定。
“移动体的位置、姿势”记录取得了各关键帧时的叉式起重机1(照相机11)的地图M上的位置及姿势。在图3所示的关键帧表中，例如记录取得了关键帧号码“n”的关键帧时的叉式起重机1的位置Pn(X，Y，Z)及姿势An(θx，θy，θz)。另外，如后述那样，这里记录的叉式起重机1的位置Pn(X，Y，Z)及姿势An(θx，θy，θz)不是记录通过基于ORB－SLAM的自己位置估算处理估算出的位置及姿势，而是记录由基于LiDAR－SLAM的自己位置估算处理估算出的位置及姿势。因而，作为比基于ORB－SLAM的自己位置估算处理的结果精度更高的结果得到。在以下的说明中，将记录在该项中的位置及姿势作为叉式起重机1的位置及姿势的真值来处置。
“变换矩阵”是用来将基于ORB－SLAM的自己位置估算处理的结果变换为真值的变换矩阵。将变换矩阵与各关键帧建立关联而记录。在图3所示的关键帧表中，例如记录有与关键帧号码“n”的关键帧建立关联的变换矩阵Tn。该变换矩阵Tn是进行了计算以使对取得了关键帧号码“n”的关键帧时的叉式起重机1的基于ORB－SLAM的自己位置估算处理的结果从真值的偏差(不一致)进行修正的变换矩阵。
(地图的例子)
图4是表示有关第1实施方式的地图的例子的图。
地图M基于关键帧表(图3)被制作，作为图4所示那样的以世界坐标为基准的各种信息组被记录。
在地图制作时，自己位置估算装置10将在行驶路线R的行驶中从照相机11取得的视野图像中的满足规定的条件者作为关键帧KF而登录至关键帧表(图3)。这里，行驶路线R例如是计划在叉式起重机1的实际运用时行驶的行驶路线。
如图4所示，在地图M上，记录各关键帧KF取得时的叉式起重机1的位置及姿势的真值即位置P(X，Y，Z)及姿势A(θx，θy，θz)、和由各关键帧KF观测的特征点Q的世界坐标上的位置(X，Y，Z)。如在图3中说明那样，将在世界坐标上标绘的各特征点Q与观测到该特征点Q的关键帧KF建立关联。另外，图4所示的构造物G是映入到关键帧KF中的构造物。对于各特征点Q，基于映入到关键帧KF中的构造物G等进行观测。
(变换矩阵)
图5是说明有关第1实施方式的变换矩阵的图。
图3所示的关键帧表的“变换矩阵”(Tn－1，Tn，Tn+1，…)如以下这样求出。
在图5中，考虑将在某个时点取得的视野图像作为关键帧KFn－1而登录至关键帧表(图3)的情况下的处理。在此情况下，自己位置估算装置10通过基于制作中的地图M的ORB－SLAM，对该视野图像取得时的叉式起重机1的位置及姿势进行估算。在图5中，将该估算结果设为位置Pn－1’、姿势An－1’。另一方面，自己位置估算装置10能够经由在地图制作时搭载的激光扫描器，取得关键帧KFn－1取得时的叉式起重机1的位置及姿势的真值。在图5中，将叉式起重机1的位置及姿势的真值设为位置Pn－1、姿势An－1。自己位置估算装置10在对关键帧KFn－1进行登录时，计算用来将位置Pn－1’、姿势An－1’映像到作为真值的位置Pn－1、姿势An－1的变换矩阵Tn－1，将其与关键帧KFn－1建立关联而记录至关键帧表。
自己位置估算装置10每当登录新的关键帧KFn，KFn+1，…，就计算与各关键帧KFn，KFn+1，…对应的变换矩阵Tn，Tn+1，…，记录至关键帧表。
(地图制作时的处理流程)
图6是有关第1实施方式的自己位置估算装置的地图制作时的处理流程的图。
图6所示的处理流程在地图制作时的叉式起重机1的行驶中被反复执行。在该时点，制作途中的地图M已经被读入到存储器101上。
自己位置估算装置10的视野图像取得部1000从照相机11取得视野图像(步骤S00)。
自己位置估算装置10的自己位置估算处理部1001根据在步骤S00中取得的视野图像和制作途中的地图M，进行基于ORB－SLAM的自己位置估算处理。这里，自己位置估算处理部1001通过将由在步骤S00中取得的视野图像观测到的特征点与记录在地图M上的特征点对齐(匹配)，对该视野图像取得时的照相机的世界坐标上的位置及姿势进行估算。此时，自己位置估算处理部1001对于在步骤S00中取得的视野图像，确定特征点的位置关系最近的1个关键帧(当前关键帧)。并且，自己位置估算处理部1001一边参照与该当前关键帧以及和该当前关键帧在时间序列上为前后的其他关键帧建立关联的地图上的特征点，一边检索与由视野图像观测到的特征点对齐者。通过这样做，能够实现基于ORB－SLAM的自己位置估算处理的高速化。
自己位置估算处理部1001判定基于ORB－SLAM的自己位置估算处理是否成功了(步骤S02)。
在基于ORB－SLAM的自己位置估算处理成功的情况下(步骤S02；是)，前进至是否应将在步骤S00中取得的视野图像作为关键帧而登录的判定处理(步骤S05～步骤S07)。关于该处理的详细情况在后面叙述。
在基于ORB－SLAM的自己位置估算处理失败的情况下(步骤S02；否)，自己位置估算装置10的再定位处理部1004进行再定位(步骤S03)。有关本实施方式的再定位处理部1004以登录在关键帧表中的全部关键帧为对象进行探索。
再定位处理部1004判定步骤S03的再定位是否成功(步骤S04)。在再定位失败的情况下(步骤S04；否)，再定位处理部1004回到步骤S04，接着执行再定位。在通过再定位估算出位置及姿势(成功)的情况下(步骤S04；是)，自己位置估算装置10前进至上述判定处理(步骤S05～步骤S07)。
自己位置估算装置10的关键帧登录部1002通过以下说明的步骤S05～步骤S07的判定处理，判定是否将在步骤S00中取得的视野图像作为关键帧而登录。
具体而言，首先，关键帧登录部1002对与由在步骤S00中取得的视野图像观测到的特征点对齐的地图M上的特征点进行检索。接着，关键帧登录部1002判定在由在步骤S00中取得的视野图像观测到的特征点中是否观测到了规定数量以上的新的特征点(没有记录在地图M上的特征点)(步骤S05)。
在由步骤S00取得的视野图像中观测到规定数量以上的新的特征点的情况下(步骤S05；是)，关键帧登录部1002将在步骤S00中取得的视野图像作为关键帧而登录(步骤S08)。
在由步骤S00取得的视野图像中没有观测到规定数量以上的新的特征点的情况下(步骤S05；否)，接着，关键帧登录部1002判断由步骤S01的基于ORB－SLAM的自己位置估算处理(或步骤S03的再定位的结果)估算出的位置是否从当前关键帧的位置离开了规定的距离阈值以上(步骤S06)。
在通过基于ORB－SLAM的自己位置估算处理估算出的位置从当前关键帧的位置离开了规定的距离阈值以上的情况下(步骤S06；是)，关键帧登录部1002将在步骤S00中取得的视野图像作为关键帧而登录(步骤S08)。
在通过基于ORB－SLAM的自己位置估算处理估算出的位置没有从当前关键帧的位置离开规定的距离阈值以上的情况下(步骤S06；否)，接着，关键帧登录部1002参照由陀螺仪传感器14得到的角速度的检测结果，判定视野图像取得时的叉式起重机1的角速度是否为规定的角速度阈值以上(步骤S07)。
在视野图像取得时的叉式起重机1的角速度为规定的角速度阈值以上的情况下(步骤S07；是)，关键帧登录部1002将在步骤S00中取得的视野图像作为关键帧而登录(步骤S08)。
在视野图像取得时的叉式起重机1的角速度不为规定的角速度阈值以上的情况下(步骤S07；否)，关键帧登录部1002不将在步骤S00中取得的视野图像作为关键帧而登录，转移至下个阶段的处理流程。
在步骤S08中，关键帧登录部1002不是将步骤S01的基于ORB－SLAM的自己位置估算结果、而是将基于LiDAR－SLAM的自己位置估算结果(真值)记录至关键帧表。进而，关键帧登录部1002使用基于LiDAR－SLAM的自己位置估算结果和步骤S01的基于ORB－SLAM的自己位置估算结果来计算变换矩阵，将该变换矩阵记录至关键帧表。
此外，关键帧登录部1002在作为关键帧的登录后，还实施聚束(bundle)处理等的后处理。
(实际运用时的处理流程)
图7是表示有关第1实施方式的自己位置估算装置的实际运用时的处理流程的图。
在地图完成后的实际运用时，自己位置估算装置10通过以下所示的步骤Sa0～步骤Sa3的流程，进行基于ORB－SLAM的自己位置估算处理。
首先，视野图像取得部1000从照相机11取得视野图像Fa(步骤Sa0)。
接着，自己位置估算处理部1001基于由视野图像Fa观测到的特征点和已制作的地图M，进行基于ORB－SLAM的自己位置估算处理(步骤Sa1)。将在该处理中估算出的叉式起重机1的位置及姿势分别设为位置Pa’、姿势Aa’。
接着，自己位置估算处理部1001在登录于关键帧表中的关键帧中，确定特征点的位置关系与在步骤Sa0中取得的视野图像Fa最近的关键帧即当前关键帧(步骤Sa2)。假设此处确定的当前关键帧是关键帧号码“n”的关键帧(关键帧KFn)。
接着，自己位置估算装置10对由ORB－SLAM估算出的位置Pa’、姿势Aa’，乘以与当前关键帧(关键帧KFn)建立关联的变换矩阵Tn，运算最终的位置Pa及姿势Aa。
(作用、效果)
如以上这样，有关第1实施方式的自己位置估算装置10的特征在于，例如如图6的步骤S06、步骤S07的处理那样，在叉式起重机1的移动满足了关于该叉式起重机1的位置及姿势的至少其中一个而预先规定的条件的情况下，将视野图像作为关键帧而登录。
这里，对关于不进行图6所示的步骤S06及步骤S07的判定处理而仅进行步骤S05的判定处理的情况下发生的课题进行说明。
即，步骤S05的判定处理基于在视野图像中观测到规定数以上没有被记录到地图M上的新的特征点，判定是否将该视野图像作为关键帧而登录。在此情况下，例如在向照相机11朝向的方向直线前进的情况下等，与前次的关键帧相比，新的特征点不易被观测到，所以关键帧的间隔容易变得稀疏。如果关键帧的间隔变得稀疏，则记录在地图M上的特征点的数量也减少，所以可以想到容易迷路。
此外，已知由于在叉式起重机1的转弯移动时在短时间中视野较大地变化，所以容易迷路。因此，为了实现稳定的自己位置估算，特别希望在叉式起重机1的转弯位置周边以充分密的间隔对关键帧进行登录。
进而，如果关键帧的间隔变得稀疏，则与当前关键帧的距离远离，所以基于与各关键帧建立关联的变换矩阵的自己位置估算处理的修正精度也下降。因而，从提高基于变换矩阵的修正精度的观点，也优选的是以适当的密度登录关键帧。
根据有关第1实施方式的自己位置估算装置10，在叉式起重机1的移动满足关于该叉式起重机1的位置及姿势的至少其中一个而预先规定的条件的情况下，将视野图像作为关键帧而登录。
通过这样做，例如在关键帧容易变得稀疏的直线前进时、或应使关键帧更密的转弯时等，发生了与条件对应的特定的移动形态的情况下，与以往相比更容易登录关键帧。因而，以与叉式起重机1的移动对应的适当的间隔将关键帧插入，能够实现使用照相机的自己位置估算处理的稳定化。
此外，根据有关第1实施方式的自己位置估算装置10，作为关于叉式起重机1的位置而预先规定的条件的一形态，在叉式起重机1的位置与关联于该位置的1个关键帧(当前关键帧)的距离离开规定的距离阈值以上的情况下，将视野图像作为关键帧而登录(图6的步骤S06)。
通过这样做，特别即使是在直线前进时等处于难以观测到新的特征点的状况，也以距离判定阈值以下的间隔登录关键帧，能够抑制关键帧变得稀疏。
此外，根据有关第1实施方式的自己位置估算装置10，作为关于叉式起重机1的姿势而预先规定的条件的一形态，在移动中的叉式起重机1的角速度成为规定的角速度阈值以上的情况下，将视野图像作为关键帧而登录(图6的步骤S07)。
通过这样做，在转弯角速度成为规定的角速度阈值以上的情况下，相应地关键帧被较密地登录，能够抑制在转弯时迷路。
另外，上述步骤S05～步骤S07的判定处理的顺序可以适当替换，在其他实施方式中，并不限定于图6所示的顺序。此外，在其他实施方式中，不需要进行步骤S05～步骤S07的全部，通过实施步骤S06及步骤S07的至少某一方，能够起到上述效果。
(第1实施方式的变形例)
在基于ORB－SLAM的地图制作时，在包含在某个关键帧中的特征点被充分多的其他关键帧观测到的情况下，从数据量削减的观点，有进行对该关键帧进行删除的剔除处理的情况。为了防止通过该剔除处理而关键帧的密度降低，有关本实施方式的变形例的自己位置估算装置10在地图制作时也可以进行图8所示那样的处理。
(地图制作时的处理流程)
图8是表示有关第1实施方式的变形例的自己位置估算装置的地图制作时的处理流程的图。
在地图制作时，关键帧登录部1002登录第n个关键帧(步骤S10)。此时，关键帧登录部1002对于第n个关键帧，作为指针而分配“latest”，并且对该第n个关键帧赋予防删除标志。另外，在该时点，对于第n－1个关键帧，作为指示该关键帧的指针而分配“prev1”，对于第n－2个关键帧，作为指示该关键帧的指针而分配“prev2”。此外，对于第n－1个关键帧及第n－2个关键帧都分别赋予了防删除标志。
接着，关键帧登录部1002判定由“prev2”指示的关键帧的位置与由“latest”指示的关键帧的位置的距离是否是规定距离以上(步骤S11)。
在由“prev2”指示的关键帧的位置与由“latest”指示的关键帧的位置的距离不到规定距离的情况下(步骤S11；否)，接着，关键帧登录部1002判定由“prev2”指示的关键帧的姿势与由“latest”指示的关键帧的姿势的角度是否是规定角度以上(步骤S12)。
在由“prev2”指示的关键帧的姿势与由“latest”指示的关键帧的姿势的角度不到规定角度的情况下(步骤S12；否)，接着，关键帧登录部1002判定当前关键帧是否是由“prev1”指示的关键帧(步骤S13)。
在当前关键帧是由“prev1”指示的关键帧的情况下(步骤S13；是)，由于“prev2”与“latest”之间的距离及角度的差异较小，此外，关系与“latest”的关键帧较近的当前关键帧是“prev1”，所以由“prev1”指示的关键帧即使被间隔剔除，带来较大影响(发生迷路等)的可能性也较低。
因而，在满足上述3个条件的全部的情况下(步骤S11；否，步骤S12；否，并且，步骤S13；是)，自己位置估算装置10将在“prev1”的关键帧中带有的防删除标志删除，将由“latest”指示的关键帧的指针替换为“prev1”(步骤S14)。然后，关键帧登录部1002登录下个关键帧，对该关键帧分配“latest”的指针(步骤S15)。
另一方面，在上述3个条件的任1个都没有被满足的情况下(步骤S11；是，步骤S12；是，或者，步骤S13；否)，当由“prev1”表示的关键帧被间隔剔除时，“prev2”的关键帧与“latest”的关键帧的距离(或角度)的间隔较大地分开，可以想到有可能带来较大的影响(发生迷路等)。所以，在此情况下，关键帧登录部1002不删除“prev1”的防删除标志，将由“latest”指示的关键帧的指针替换为“prev1”，将由“prev1”指示的关键帧的指针替换为“prev2”(步骤S16)。
然后，关键帧登录部1002登录下个关键帧，对该关键帧分配“latest”的指针(步骤S17)。
关键帧登录部1002反复执行步骤S11～步骤S17的处理，直到地图制作的处理结束(步骤S18)。
如以上这样，有关第1实施方式的变形例的关键帧登录部1002基于防删除标志的有无，进行对于已登录的多个关键帧的剔除处理。并且，关键帧登录部1002对于进行是否赋予防删除标志的判断的对象关键帧(prev1)，基于比该对象关键帧更前1个被登录的关键帧(prev2)与比该对象关键帧更后1个被登录的关键帧(latest)的位置及姿势的关系，判定是否对该对象关键帧赋予防删除标志。
通过这样做，由于对有可能给关键帧密度带来较大的影响的关键帧设定防删除标志，所以在进行了剔除处理之后，也能够确保与叉式起重机1的位置、姿势对应的适当的关键帧的密度。
另外，上述步骤S11～步骤S13的判定处理的顺序可以适当替换，在其他实施方式中并不限定于图8所示的顺序。此外，在其他实施方式中，不需要进行步骤S11～步骤S13的全部，通过实施步骤S11～步骤S13的至少某一方，能够起到上述效果。
<第2实施方式>
以下，参照图9、图10对有关第2实施方式的自己位置估算装置及具备它的移动体进行说明。
(地图读出部的处理)
图9是说明有关第2实施方式的地图读出部的处理的图。
有关第1实施方式的地图读出部1003在实际运用时，将地图M的全部加载(读出)到存储器101上，但有关第2实施方式的地图读出部1003在将地图M的一部分区域读出到存储器上这一点与第1实施方式不同。
有关第2实施方式的地图读出部1003在实际运用时，仅将属于以通过基于ORB－SLAM的自己位置估算处理估算出的叉式起重机1的位置为基准的规定的区域(读出区域MR)的关键帧加载到存储器101上。在图9中，地图读出部1003每当叉式起重机1的位置移动，就规定以此时的位置(基于摄影图像Fc而估算的位置)为基准的新的读出区域MR，将属于该读出区域MR的关键帧KFa加载到存储器101上。并且，地图读出部1003将不属于该读出区域MR的关键帧KFb从存储器101上卸载。
通过这样做，由于仅与叉式起重机1的当前位置对应的、自己位置估算处理所需要的最小限度的关键帧被加载到存储器101上，所以能够在维持自己位置估算处理的精度的同时节约存储器101的使用区域。
此外，有关第2实施方式的读出区域MR如图9所示，呈与叉式起重机1的宽度方向(图9所示的例子中是Y轴方向)相比在叉式起重机1的前后方向(在图9所示的例子中是X轴方向)上变宽那样的形状(图9所示那样的、在叉式起重机1的前后方向上将扇形的形状相对于以叉式起重机的当前位置为中心的圆形组合的形状)。这里，叉式起重机1在前后方向上有可能在短时间中到达，而通过做成上述那样的形状，在前后方向上较多的特征点被加载。因而，在叉式起重机1沿前后方向移动的情况下，能够防止在地图M上对齐的特征点不足的情况。
另外，在其他实施方式中，读出区域MR不需要限定于上述那样的形状，也可以是简单地被规定为以叉式起重机1的位置为基准(中心)的圆状或矩形状的形态。
(第2实施方式的变形例)
图10是说明有关第2实施方式的地图读出部的处理的图。
有关第2实施方式的变形例的地图读出部1003在实际运用时，仅将属于与特定的行驶路线对应的读出区域MR的关键帧加载到存储器101上。在图10中，读出区域MR与行驶路线R对应而被规定。在叉式起重机1计划行驶的行驶路线(例如行驶路线R)被变更的情况下，地图读出部1003将读出区域MR变更为与变更后的行驶路线(例如路线R’)对应者。地图读出部1003与第2实施方式同样，将属于读出区域MR的关键帧KFa加载到存储器101上，将不属于该读出区域MR的关键帧KFb从存储器101上卸载。
通过这样做，由于仅与叉式起重机1的特定的行驶路线对应的、自己位置估算处理所需要的最小限度的关键帧被加载到存储器101上，所以能够在维持自己位置估算处理的精度的同时节约存储器101的使用区域。
另外，也可以是特定的行驶路线根据叉式起重机1的目的地或任务的种类而设定的形态。例如，在将装入有生产的制品的箱设置于托盘之上的生产线的周边，有(1)将空的托盘向生产线输送，(2)将搭载有制品的托盘从生产线向制品保管仓库搬运并临时放置的任务。地图读出部1003在开始(1)的任务时，仅加载与从托盘放置场所到生产线的行驶路线对应的关键帧。并且，在转移到(2)的任务的情况下，地图读出部1003将在(1)中加载的关键帧卸载，并且新加载与从生产线到仓库的行驶路线对应的关键帧。
<第3实施方式>
以下，参照图11、图12对有关第3实施方式的自己位置估算装置及具备它的移动体进行说明。
(再定位处理部的处理)
图11是表示有关第3实施方式的再定位处理部的处理流程的图。
图12是说明有关第3实施方式的再定位处理部的处理的图。
有关第3实施方式的再定位处理部1004基于叉式起重机1的硬件的制约条件来确定探索关键帧的范围(以下，也单记作探索范围)，并且探索包含在该探索范围中的关键帧。
图11所示的处理流程表示在地图制作时或实际运用时、当发生了自己位置估算处理的失败(迷路)时有关第3实施方式的再定位处理部1004执行的再定位的处理的流程。
如图11所示，自己位置估算装置10的再定位处理部1004首先将关键帧的探索范围清空(步骤S30)。
接着，再定位处理部1004选择登录在关键帧表(图3)中的全部关键帧中的某1个(步骤S31)。
接着，再定位处理部1004判定在步骤S31中选择的关键帧是否存在于以在迷路要发生前估算出的叉式起重机的位置为基准、满足叉式起重机1的最大速度的制约的范围中(步骤S32)。
在所选择的关键帧存在于满足叉式起重机1的最大速度的制约的范围中的情况下(步骤S32；是)，接着，再定位处理部1004判定所选择的关键帧是否存在于以在迷路要发生前估算出的叉式起重机的位置为基准、满足叉式起重机1的最大角速度的制约的范围中(步骤S33)。
在所选择的关键帧存在于满足叉式起重机1的最大角速度的制约的范围中的情况下(步骤S33；是)，接着，再定位处理部1004判定所选择的关键帧的位置是否与由里程计得到的估算位置充分接近(步骤S34)。这里，所述的由里程计得到的估算位置，是利用由搭载在叉式起重机1上的轮胎旋转角传感器12(图1)及舵角传感器13(图1)得到的检测结果估算的叉式起重机1的位置。再定位处理部1004例如以在迷路要发生前由ORB－SLAM估算出的叉式起重机的位置为基准，估算基于由轮胎旋转角传感器12(图1)及舵角传感器13(图1)得到的检测结果的自己位置。
在所选择的关键帧的位置与由里程计得到的估算位置充分接近的情况下(步骤S34；是)，接着，再定位处理部1004判定所选择的关键帧的姿势是否与由陀螺仪传感器14(图1)得到的估算姿势充分接近(步骤S35)。所述的由陀螺仪传感器14得到的估算姿势，例如是通过对由陀螺仪传感器14检测到的角速度进行积分而求出的叉式起重机1的估算姿势。
在所选择的关键帧的位置与由陀螺仪传感器14得到的估算姿势充分接近的情况下(步骤S34；是)，再定位处理部1004将所选择的关键帧追加到探索范围中(步骤S36)。
另一方面，在上述步骤S32～步骤S35中的任1个都是否的情况下，再定位处理部1004不将所选择的关键帧追加到探索范围中而前进至下个步骤。
再定位处理部1004对登录在关键帧表中的全部关键帧进行选择，判定是否进行了步骤S32～步骤S36的处理(步骤S37)。
在没有对全部关键帧进行选择的情况下(步骤S37；否)，再定位处理部1004选择下个关键帧(步骤S38)，对于该关键帧进行步骤S32～步骤S36的处理。
在对全部关键帧进行了选择的情况下(步骤S37；是)，再定位处理部1004仅以追加在探索范围中的关键帧为对象，进行再定位(步骤S39)。
图12所示的探索范围TR是全部满足上述步骤S32～步骤S38的判定处理的区域。即，再定位处理部1004通过进行上述步骤S32～步骤S38的判定处理，以在迷路要发生前由ORB－SLAM估算出的位置Pc及姿势Ac为基准，将属于由硬件上的制约规定的探索范围TR的关键帧KFd作为再定位的探索对象，不将不属于探索范围TR的关键帧KFe作为探索对象。
如以上这样，有关第3实施方式的再定位处理部1004使用由自己位置估算处理最后估算出的位置和作为叉式起重机1的硬件上的制约之一的最大速度，将能够取为该叉式起重机1的当前的位置的范围确定为关键帧的探索范围。
此外，有关第3实施方式的再定位处理部1004使用由自己位置估算处理最后估算出的位置和作为叉式起重机1的硬件上的制约之一的最大角速度，将能够取为该叉式起重机的当前的姿势的范围确定为关键帧的探索范围。
此外，有关第3实施方式的再定位处理部1004将距由里程计(轮胎旋转角传感器12及舵角传感器13)得到的估算位置充分近的范围(即，可看作由里程计得到的估算位置的误差的范围)确定为关键帧的探索范围。
此外，有关第3实施方式的再定位处理部1004将距由陀螺仪传感器14得到的估算姿势充分近的范围(即，可看作由陀螺仪传感器14得到的估算姿势的误差的范围)确定为关键帧的探索范围。
通过这样做，在叉式起重机1的硬件的制约上，能够将明显偏离的位置及姿势的关键帧从再定位的探索对象排除，所以能够实现再定位的处理的高效率化、高速化。
另外，上述步骤S32～步骤S35的判定处理的顺序可以适当替换，在其他实施方式中并不限定于图11所示的顺序。此外，在其他实施方式中，不需要进行步骤S32～步骤S35的全部，即使仅实施步骤S32～步骤S35的至少某1个，也能够起到上述效果。
<第4实施方式>
以下，参照图13对有关第4实施方式的自己位置估算装置及具备它的移动体进行说明。
(自己位置估算处理部的处理)
图13是说明有关第4实施方式的自己位置估算处理部的功能的图。
有关第4实施方式的自己位置估算处理部1001对由自己位置估算处理估算的位置及姿势进行修正，以使叉式起重机1的位置及姿势的轨迹平滑化。
这里，在图7中，说明了自己位置估算处理部1001通过对基于ORB－SLAM的自己位置估算处理的结果(位置Pa’、姿势Aa’)乘以与当前关键帧(关键帧KFn)关联的变换矩阵Tn，能够运算出精度更高的位置Pa、姿势Aa。
在此情况下，可能发生以下这样的技术问题。即，关联于关键帧KFn的变换矩阵Tn和关联于关键帧KFn+1的变换矩阵Tn+1是相互不同的变换矩阵。在叉式起重机1的移动中，用于自己位置估算处理的当前关键帧以某个位置为边界，例如从关键帧KFn切换为关键帧KFn+1。如果这样，则随着当前关键帧的切换，用于与真值的不一致的修正的变换矩阵从变换矩阵Tn切换为变换矩阵Tn+1。在此情况下，例如如图13所示，在由变换矩阵Tn修正后的位置及姿势与由变换矩阵Tn+1修正后的位置及姿势之间发生“跳跃”，可能成为非连续的轨迹。
所以，有关本实施方式的自己位置估算处理部1001对于由自己位置估算处理估算出的位置及姿势进行规定的平滑化处理。自己位置估算处理部1001将施以该平滑化处理而得到的位置及姿势作为叉式起重机1的位置及姿势来输出。该平滑化处理例如使用移动平均滤波器等安装。
通过这样做，估算出的自己位置描绘连续的轨迹。由此，在基于位置及姿势的估算结果进行自动操舵的叉式起重机1中，能够防止舵角的急剧的变化。
(各实施方式的变形例)
以上，对有关第1～第4实施方式的自己位置估算装置10及具备它的移动体(叉式起重机1)详细地进行了说明，但自己位置估算装置10及移动体的具体的形态并不限定于上述形态，能够在不脱离主旨的范围内加以各种设计变更等。
例如，能够应用自己位置估算装置10的移动体并不限定于叉式起重机，对于无人飞机(无人机)等也能够应用。在此情况下，也可以是自己位置估算装置10以来自搭载在无人飞机上的加速度传感器或陀螺仪传感器的检测值为输入，在该检测值满足预先规定的条件的情况下，将当前的视野图像作为关键帧而登录。
上述的各实施方式中，上述自己位置估算装置10的各种处理的过程被以程序的形式存储在计算机可读取的记录介质中，通过计算机将该程序读出并执行，进行上述各种处理。此外，所述的计算机可读取的记录介质，是指磁盘、光磁盘、CD－ROM、DVD－ROM、半导体存储器等。此外，也可以将该计算机程序通过通信线路向计算机分发，接受到该分发的计算机执行该程序。
上述程序也可以是用来实现上述功能的一部分的。进而，虽然能够将上述功能通过与已经记录在计算机系统中的程序的组合来实现，但也可以是所谓的差文件(差程序)。
如以上这样，说明了有关本发明的几个实施方式，但这些全部实施方式是作为例子提示的，不是要限定发明的范围。这些实施方式能够以其他各种各样的形态来实施，能够在不脱离发明的主旨的范围中进行各种省略、替换、变更。这些实施方式及其变形包含在发明的范围或主旨中，同样包含在权利要求书所记载的发明和其等价的范围中。
标号说明
1 叉式起重机(移动体)
10 自己位置估算装置
100 CPU
1000 视野图像取得部(取得部)
1001 自己位置估算处理部(估算部)
1002 关键帧登录部(登录部)
1003 地图读出部(读出部)
1004 再定位处理部(处理部)
101 存储器
102 存储设备
103 通信接口
11 照相机
12 轮胎旋转角传感器
13 舵角传感器
14 陀螺仪传感器
M 地图

Claims (16)

1.一种自己位置估算装置，其中，
具备：
取得部，从搭载在移动体上的照相机取得视野图像；
估算部，基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图，进行对前述世界坐标上的前述移动体的位置及姿势进行估算的自己位置估算处理；以及
登录部，将前述视野图像作为用来向前述地图追加该视野图像上包含的新的特征点的关键帧而登录，
前述登录部在移动中的前述移动体的角速度成为规定的角速度阈值以上的情况下，将前述视野图像作为关键帧而登录，前述移动体的角速度是由陀螺仪传感器检测得到的角速度。
2.如权利要求1所述的自己位置估算装置，其中，
前述登录部在前述移动体的位置与关联于该位置的1个前述关键帧的距离离开了规定的距离阈值以上的情况下，将前述视野图像作为关键帧而登录。
3.如权利要求1或权利要求2所述的自己位置估算装置，其中，
前述登录部基于防删除标志的有无，进行对于已登录的多个前述关键帧的剔除处理；
对于作为多个前述关键帧之一的对象关键帧，基于比该对象关键帧更前1个被登录的关键帧和比该对象关键帧更后1个被登录的关键帧的位置及姿势的至少其中一个的关系，判定是否对该对象关键帧赋予前述防删除标志。
4.如权利要求1或权利要求2所述的自己位置估算装置，其中，
还具备将前述地图中的一部分区域读出到存储器上的读出部。
5.如权利要求4所述的自己位置估算装置，其中，
前述读出部将前述地图中的以前述移动体的位置为基准的前述一部分区域读出到存储器上。
6.如权利要求5所述的自己位置估算装置，其中，
前述读出部将前述地图中的与前述移动体的宽度方向相比在前述移动体的前后方向上更宽的前述一部分区域读出到存储器上。
7.如权利要求4所述的自己位置估算装置，其中，
前述读出部将前述地图中的与特定的行驶路线对应的前述一部分区域读出到存储器上。
8.如权利要求1或权利要求2所述的自己位置估算装置，其中，
具备在前述自己位置估算处理失败的情况下进行对前述关键帧进行探索的再定位的处理部；
前述处理部基于前述移动体的硬件的制约条件而确定探索范围，并且探索该探索范围中包含的关键帧。
9.如权利要求8所述的自己位置估算装置，其中，
前述处理部基于由前述自己位置估算处理估算出的位置和前述移动体的最大速度，将能够取为该移动体的当前的位置的范围确定为前述探索范围。
10.如权利要求8所述的自己位置估算装置，其中，
前述处理部基于由前述自己位置估算处理估算出的姿势和前述移动体的最大角速度，将能够取为该移动体的当前的姿势的范围确定为前述探索范围。
11.如权利要求8所述的自己位置估算装置，其中，
前述处理部将以由前述自己位置估算处理估算出的位置为基准的、可看作由里程计得到的估算位置的误差的范围确定为前述探索范围。
12.如权利要求8所述的自己位置估算装置，其中，
前述处理部将以由前述自己位置估算处理估算出的姿势为基准的、可看作由陀螺仪传感器得到的估算姿势的误差的范围确定为前述探索范围。
13.如权利要求1或权利要求2所述的自己位置估算装置，其中，
前述估算部对由前述自己位置估算处理估算出的位置及姿势进行修正，以使前述世界坐标上的前述移动体的位置及姿势的轨迹平滑化。
14.如权利要求13所述的自己位置估算装置，其中，
前述估算部使用移动平均滤波器，对由前述自己位置估算处理估算出的位置及姿势进行修正。
15.一种自己位置估算方法，其中，
具有：
从搭载在移动体上的照相机取得视野图像的步骤；
基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图、进行对前述世界坐标上的前述移动体的位置及姿势进行估算的自己位置估算处理的步骤；以及
将前述视野图像作为用来将该视野图像上包含的新的特征点追加到前述地图上的关键帧而登录的步骤，
在作为前述关键帧而登录的步骤中，在移动中的前述移动体的角速度成为规定的角速度阈值以上的情况下，将前述视野图像作为关键帧而登录，前述移动体的角速度是由陀螺仪传感器检测得到的角速度。
16.一种计算机可读取的记录介质，存储有程序，所述程序使自己位置估算装置的计算机执行：
从搭载在移动体上的照相机取得视野图像的步骤；
基于前述视野图像上的特征点的位置和记录有世界坐标上的多个特征点的位置的地图，进行对前述世界坐标上的前述移动体的位置及姿势进行估算的自己位置估算处理的步骤；以及
将前述视野图像作为用来将该视野图像上包含的新的特征点追加到前述地图上的关键帧而登录的步骤，
在作为前述关键帧而登录的步骤中，在移动中的前述移动体的角速度成为规定的角速度阈值以上的情况下，将前述视野图像作为关键帧而登录，前述移动体的角速度是由陀螺仪传感器检测得到的角速度。
CN202010084076.2A 2019-03-07 2020-02-10 自己位置估算装置、自己位置估算方法及记录介质 Active CN111664857B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2019041991A JP7220591B2 (ja) 2019-03-07 2019-03-07 自己位置推定装置、自己位置推定方法及びプログラム
JP2019-041991 2019-03-07

Publications (2)

Publication Number Publication Date
CN111664857A CN111664857A (zh) 2020-09-15
CN111664857B true CN111664857B (zh) 2023-09-19

Family

ID=72146736

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010084076.2A Active CN111664857B (zh) 2019-03-07 2020-02-10 自己位置估算装置、自己位置估算方法及记录介质

Country Status (4)

Country Link
US (1) US11216973B2 (zh)
JP (1) JP7220591B2 (zh)
CN (1) CN111664857B (zh)
DE (1) DE102020103252A1 (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102020126401A1 (de) 2020-10-08 2022-04-14 Jungheinrich Aktiengesellschaft Verfahren zur Bestimmung von Abstandswerten zu einem Flurförderzeug sowie ein solches
KR102484772B1 (ko) * 2020-11-03 2023-01-05 네이버랩스 주식회사 맵 생성 방법 및 이를 이용한 이미지 기반 측위 시스템
KR102525281B1 (ko) * 2020-11-12 2023-04-25 네이버랩스 주식회사 이미지 기반 측위 방법 및 시스템
CN113052882B (zh) * 2021-03-26 2023-11-24 上海商汤智能科技有限公司 图像配准方法及相关装置、电子设备、存储介质
JP2023114238A (ja) * 2022-02-04 2023-08-17 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置および情報処理方法

Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0461577A2 (en) * 1990-06-11 1991-12-18 Hitachi, Ltd. Method and apparatus for generating object motion path, method of setting object display attribute, and computer graphics system
JPH07264471A (ja) * 1994-03-19 1995-10-13 Sony Corp 特殊効果装置
US5563786A (en) * 1994-02-16 1996-10-08 Fuji Jukogyo Kabushiki Kaisha Autonomous running control system for vehicle and the method thereof
JP2009210269A (ja) * 2008-02-29 2009-09-17 Sanyo Electric Co Ltd ナビゲーション装置及び該装置における地点登録方法
JP2012185619A (ja) * 2011-03-04 2012-09-27 Toyota Motor Corp 移動体マップ装置、その処理方法及びプログラム
JP2013020345A (ja) * 2011-07-08 2013-01-31 Hitachi Industrial Equipment Systems Co Ltd 移動体の位置・姿勢推定システム
KR20140090807A (ko) * 2013-01-10 2014-07-18 주식회사 한터기술 운행열차의 위치안내 중 음영구간을 해소하는 방법과 이를 적용한 열차위치 안내시스템
CN104395932A (zh) * 2012-06-29 2015-03-04 三菱电机株式会社 对准数据的方法
CN104854637A (zh) * 2012-12-12 2015-08-19 日产自动车株式会社 移动物体位置姿态角推定装置及移动物体位置姿态角推定方法
CN105744164A (zh) * 2016-02-24 2016-07-06 惠州Tcl移动通信有限公司 一种移动终端拍照方法及系统
CN106373141A (zh) * 2016-09-14 2017-02-01 上海航天控制技术研究所 空间慢旋碎片相对运动角度和角速度的跟踪系统和跟踪方法
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN108090958A (zh) * 2017-12-06 2018-05-29 上海阅面网络科技有限公司 一种机器人同步定位和地图构建方法和系统
CN108520559A (zh) * 2018-04-04 2018-09-11 西安因诺航空科技有限公司 一种基于双目视觉的无人机定位导航的方法
JP2018156538A (ja) * 2017-03-21 2018-10-04 カシオ計算機株式会社 自律移動装置、画像処理方法及びプログラム
JP2018173882A (ja) * 2017-03-31 2018-11-08 富士通株式会社 情報処理装置、方法、及びプログラム

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5776332B2 (ja) 2011-05-27 2015-09-09 富士通株式会社 地図処理方法及びプログラム、並びにロボットシステム
WO2017038012A1 (ja) 2015-08-28 2017-03-09 パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカ 地図生成方法、自己位置推定方法、ロボットシステム、およびロボット
JP6849330B2 (ja) 2015-08-28 2021-03-24 パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカＰａｎａｓｏｎｉｃ Ｉｎｔｅｌｌｅｃｔｕａｌ Ｐｒｏｐｅｒｔｙ Ｃｏｒｐｏｒａｔｉｏｎ ｏｆ Ａｍｅｒｉｃａ 地図生成方法、自己位置推定方法、ロボットシステム、およびロボット
US9964955B2 (en) * 2016-08-04 2018-05-08 Canvas Technology, Inc. System and methods of determining a geometric pose of a camera based on spatial and visual mapping
US20200158517A1 (en) * 2017-01-19 2020-05-21 Mindmaze Holding Sa System, methods, device and apparatuses for preforming simultaneous localization and mapping
JP6760114B2 (ja) * 2017-01-31 2020-09-23 富士通株式会社 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
JP6842039B2 (ja) * 2017-03-02 2021-03-17 富士通株式会社 カメラ位置姿勢推定装置、方法およびプログラム
JP6762913B2 (ja) * 2017-07-11 2020-09-30 キヤノン株式会社 情報処理装置、情報処理方法
JP7257737B2 (ja) * 2017-09-05 2023-04-14 ソニーグループ株式会社 情報処理装置、自己位置推定方法、及び、プログラム
WO2019082670A1 (ja) 2017-10-25 2019-05-02 ソニー株式会社 情報処理装置、情報処理方法、プログラム、及び、移動体
JP2019149621A (ja) * 2018-02-26 2019-09-05 ソニー株式会社 情報処理装置、情報処理方法、およびプログラム
CN109556596A (zh) * 2018-10-19 2019-04-02 北京极智嘉科技有限公司 基于地面纹理图像的导航方法、装置、设备及存储介质

Patent Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0461577A2 (en) * 1990-06-11 1991-12-18 Hitachi, Ltd. Method and apparatus for generating object motion path, method of setting object display attribute, and computer graphics system
US5563786A (en) * 1994-02-16 1996-10-08 Fuji Jukogyo Kabushiki Kaisha Autonomous running control system for vehicle and the method thereof
JPH07264471A (ja) * 1994-03-19 1995-10-13 Sony Corp 特殊効果装置
JP2009210269A (ja) * 2008-02-29 2009-09-17 Sanyo Electric Co Ltd ナビゲーション装置及び該装置における地点登録方法
JP2012185619A (ja) * 2011-03-04 2012-09-27 Toyota Motor Corp 移動体マップ装置、その処理方法及びプログラム
JP2013020345A (ja) * 2011-07-08 2013-01-31 Hitachi Industrial Equipment Systems Co Ltd 移動体の位置・姿勢推定システム
CN104395932A (zh) * 2012-06-29 2015-03-04 三菱电机株式会社 对准数据的方法
CN104854637A (zh) * 2012-12-12 2015-08-19 日产自动车株式会社 移动物体位置姿态角推定装置及移动物体位置姿态角推定方法
KR20140090807A (ko) * 2013-01-10 2014-07-18 주식회사 한터기술 운행열차의 위치안내 중 음영구간을 해소하는 방법과 이를 적용한 열차위치 안내시스템
CN105744164A (zh) * 2016-02-24 2016-07-06 惠州Tcl移动通信有限公司 一种移动终端拍照方法及系统
CN106373141A (zh) * 2016-09-14 2017-02-01 上海航天控制技术研究所 空间慢旋碎片相对运动角度和角速度的跟踪系统和跟踪方法
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
JP2018156538A (ja) * 2017-03-21 2018-10-04 カシオ計算機株式会社 自律移動装置、画像処理方法及びプログラム
JP2018173882A (ja) * 2017-03-31 2018-11-08 富士通株式会社 情報処理装置、方法、及びプログラム
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法
CN108090958A (zh) * 2017-12-06 2018-05-29 上海阅面网络科技有限公司 一种机器人同步定位和地图构建方法和系统
CN108520559A (zh) * 2018-04-04 2018-09-11 西安因诺航空科技有限公司 一种基于双目视觉的无人机定位导航的方法

Also Published As

Publication number Publication date
JP7220591B2 (ja) 2023-02-10
CN111664857A (zh) 2020-09-15
DE102020103252A1 (de) 2020-09-10
US11216973B2 (en) 2022-01-04
JP2020144710A (ja) 2020-09-10
US20200286257A1 (en) 2020-09-10

Similar Documents

Publication Publication Date Title
CN111664857B (zh) 自己位置估算装置、自己位置估算方法及记录介质
CN112179330B (zh) 移动设备的位姿确定方法及装置
CA2952355C (en) Adaptive mapping with spatial summaries of sensor data
US7696894B2 (en) Method for determining a relative position of a mobile unit by comparing scans of an environment and mobile unit
EP2460629B1 (en) Control method for localization and navigation of mobile robot and mobile robot using same
WO2018142900A1 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
US20020051572A1 (en) Device, method, and computer-readable medium for detecting changes in objects in images and their features
JP2014034251A (ja) 車両走行制御装置及びその方法
US11768503B2 (en) Position estimation device, control device, industrial vehicle, logistics support system, position estimation method, and program
WO2020189154A1 (ja) 物品検出装置、物品検出方法、及び産業車両
WO2022016320A1 (zh) 地图更新方法、装置、计算机设备和存储介质
EP4150517A1 (en) Identifying elements in an environment
CN109857122A (zh) 仓库运输车辆的路径控制方法、装置和仓库运输系统
CN114911226A (zh) 搬运车行驶控制方法、装置及搬运车
EP3598388B1 (en) Method and apparatus for visual odometry, and non-transitory computer-readable recording medium
JP7482453B2 (ja) 測位装置及び移動体
CN113841101A (zh) 为了在移动式机器人的自主导航情况下使用而创建环境地图的方法
JP7144491B2 (ja) フォークリフト、位置推定方法、及びプログラム
CN117897348A (zh) 货架定位方法、货架对接方法以及装置、设备、介质
JPH1139464A (ja) 車両用画像処理装置
CN111256689B (zh) 一种机器人定位方法、机器人和存储介质
KR102624644B1 (ko) 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법
US20230375697A1 (en) System and Method for Support Structure Detection
US20210318690A1 (en) Positioning device
JP2022138036A (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