CN104204726B - 移动物体位置姿态估计装置和移动物体位置姿态估计方法 - Google Patents

移动物体位置姿态估计装置和移动物体位置姿态估计方法 Download PDF

Info

Publication number
CN104204726B
CN104204726B CN201380013066.8A CN201380013066A CN104204726B CN 104204726 B CN104204726 B CN 104204726B CN 201380013066 A CN201380013066 A CN 201380013066A CN 104204726 B CN104204726 B CN 104204726B
Authority
CN
China
Prior art keywords
mentioned
likelihood score
attitude angle
virtual
attitude
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
CN201380013066.8A
Other languages
English (en)
Other versions
CN104204726A (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.)
Nissan Motor Co Ltd
Original Assignee
Nissan 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 Nissan Motor Co Ltd filed Critical Nissan Motor Co Ltd
Publication of CN104204726A publication Critical patent/CN104204726A/zh
Application granted granted Critical
Publication of CN104204726B publication Critical patent/CN104204726B/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
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
    • B60R1/00—Optical viewing arrangements; Real-time viewing arrangements for drivers or passengers using optical image capturing systems, e.g. cameras or video systems specially adapted for use in or on vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S3/00—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received
    • G01S3/78—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received using electromagnetic waves other than radio waves
    • G01S3/782—Systems for determining direction or deviation from predetermined direction
    • G01S3/785—Systems for determining direction or deviation from predetermined direction using adjustment of orientation of directivity characteristics of a detector or detector system to give a desired condition of signal derived from that detector or detector system
    • G01S3/786—Systems for determining direction or deviation from predetermined direction using adjustment of orientation of directivity characteristics of a detector or detector system to give a desired condition of signal derived from that detector or detector system the desired condition being maintained automatically
    • G01S3/7864—T.V. type tracking systems
    • 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
    • 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
    • H04N7/00—Television systems
    • H04N7/18—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast
    • H04N7/181—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast for receiving images from a plurality of remote sources
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N7/00—Television systems
    • H04N7/18—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Multimedia (AREA)
  • Computer Graphics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Electromagnetism (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Mechanical Engineering (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)
  • Processing Or Creating Images (AREA)

Abstract

移动物体位置姿态估计装置具备摄像部(2)、对比图像获取部、似然度设定部(12)以及移动物体位置姿态估计部(13)。摄像部(2)对移动物体周边进行拍摄来获取摄像图像。对比图像获取部获取从预定的位置和姿态角观察时的对比图像。似然度设定部(12)将摄像图像与对比图像进行比较，在摄像图像内的远处位置像素与对比图像内的远处位置像素一致的情况下将对比图像的姿态角似然度设定得高，在摄像图像内的近处位置像素与对比图像内的近处位置像素一致的情况下将对比图像的位置似然度设定得高。移动物体位置姿态估计部(13)根据姿态角似然度被设定得高的对比图像的姿态角来估计移动物体的姿态角，根据位置似然度被设定得高的对比图像的位置来估计移动物体的位置。

Description

移动物体位置姿态估计装置和移动物体位置姿态估计方法
技术领域
本发明涉及一种对移动物体的位置和姿态角进行估计的移动物体位置姿态估计装置和移动物体位置姿态估计方法。
背景技术
作为将三维地图与照相机的摄像图像进行比较来计算移动物体的位置的技术，例如已知有下述的专利文献1所记载的技术。在该专利文献1中，首先，制作从由车辆所具备的车载照相机获得的实际的影像中抽出边缘得到的边缘图像。制作将以三维的方式记录周围环境的边缘的位置、形状得到的三维地图按照车载照相机的位置和姿态投影得到的虚拟图像。然后，调整车载照相机的位置和姿态角以使边缘图像与虚拟图像一致。由此，估计出车载照相机在三维空间中的位置和姿态角。
专利文献1：日本特开2009-199572号公报
发明内容
然而，在专利文献1中，在虽然实际的影像与虚拟图像一致但该一致的场所距车载照相机较远的情况下，有可能车载照相机的位置的误差大。相反地，在一致的位置距车载照相机近的情况下，有可能车载照相机的姿态角的误差大。
因此，本发明是鉴于上述的实际情况而提出的，其目的在于，提供一种能够高精度地估计移动物体的位置和姿态角的移动物体位置姿态估计装置和移动物体位置姿态估计方法。
本发明的第一方式所涉及的移动物体位置姿态估计装置对移动物体的位置和姿态角进行估计，该移动物体位置姿态估计装置具备摄像部、对比图像获取部、似然度设定部以及移动物体位置姿态估计部。摄像部对移动物体周边进行拍摄来获取摄像图像。对比图像获取部获取从预定的位置和姿态角观察时的对比图像。似然度设定部将由摄像部获取到的摄像图像与由对比图像获取部获取到的对比图像进行比较，在摄像图像内的远处位置像素与对比图像内的远处位置像素一致的情况下，将对比图像的姿态角似然度设定得高，在摄像图像内的近处位置像素与对比图像内的近处位置像素一致的情况下，将对比图像的位置似然度设定得高。移动物体位置姿态估计部根据由似然度设定部将姿态角似然度设定得高的对比图像的姿态角来估计移动物体的姿态角，根据由似然度设定部将位置似然度设定得高的对比图像的位置来估计移动物体的位置。
本发明的第二方式所涉及的移动物体位置姿态估计方法是对移动物体的位置和姿态角进行估计的移动物体位置姿态估计方法。在移动物体位置姿态估计方法中，将对移动物体周边进行拍摄的摄像图像与从预定的位置和姿态角观察时的对比图像进行比较。在摄像图像内的远处位置像素与对比图像内的远处位置像素一致的情况下，将对比图像的姿态角似然度设定得高。在摄像图像内的近处位置像素与对比图像内的近处位置像素一致的情况下，将对比图像的位置似然度设定得高。根据将姿态角似然度设定得高的对比图像的姿态角来估计移动物体的姿态角。根据将位置似然度设定得高的对比图像的位置来估计移动物体的位置。
附图说明
图1是表示本发明的第一实施方式所涉及的移动物体位置姿态估计装置的结构的框图。
图2的(a)表示由摄像部获取到的摄像图像，图2的(b)表示从图2的(a)的摄像图像中抽出边缘得到的边缘图像，图2的(c)是表示由虚拟图像获取部获取到的虚拟图像，图2的(d)是表示图2的(c)中的虚拟位置向右方向偏移了的情况下的虚拟图像的俯视图。
图3是表示本发明的第一实施方式所涉及的移动物体位置姿态估计装置的动作过程的一例的流程图。
图4是用于说明使粒子(候选点)移动的动作的俯视图。
图5是表示距车辆的距离与位置似然度like_p的关系的一例的曲线图。
图6是表示距车辆的距离与姿态角似然度like_a的关系的一例的曲线图。
图7是表示本发明的第二实施方式所涉及的移动物体位置姿态估计装置的动作过程的一例的流程图。
具体实施方式
下面，参照附图说明本发明的实施方式。
(第一实施方式)
参照图1说明本发明的第一实施方式所涉及的移动物体位置姿态估计装置的结构。第一实施方式所涉及的移动物体位置姿态估计装置具备ECU(Engine Control Unit：发动机控制单元)1、照相机(摄像部的一例)2、三维地图数据库3以及车辆传感器组4。在车辆传感器组4中包括GPS接收机41、油门踏板传感器42、转向传感器(steering sensor)43、制动传感器(brake sensor)44、车速传感器45、加速度传感器46、车轮速度传感器47以及偏行率传感器等其它传感器48。此外，ECU 1实际由ROM、RAM、运算电路等构成。ECU 1通过按照保存在ROM中的移动物体位置姿态估计用的计算机程序来执行处理，由此作为虚拟图像获取部11(对比图像获取部的一例)、似然度设定部12、移动物体位置姿态估计部13发挥功能。
照相机2例如使用CCD等固体摄像元件。下面说明移动物体是车辆的情况。照相机2例如以能够拍摄车辆前方的方向(姿态角)设置在车辆的前端部分(位置)处。照相机2每隔规定时间拍摄车辆周边来获取摄像图像。获取到的摄像图像被供给到ECU 1。
三维地图数据库3例如存储有包含路面显示的周围环境的边缘等的三维位置信息。在本实施方式中，在三维地图数据库3中，作为三维位置信息，除了白线、停止线、人行横道、路面标志等路面显示以外，还存储有路边石、建筑物等构造物的边缘信息。这些三维位置信息被定义为边缘的集合体。在边缘为长的直线的情况下，将边缘例如一米一米地进行切割，因此不存在极长的边缘。在直线的情况下，各边缘具有表示直线的两端点的三维位置信息。在曲线的情况下，各边缘具有表示曲线的两端点和中央点的三维位置信息。
车辆传感器组4连接在ECU 1上。车辆传感器组4向ECU 1供给由各传感器41～48检测出的各种传感器值。ECU 1通过使用车辆传感器组4的输出值来分别计算车辆的大致位置以及表示车辆在单位时间内前进的移动量的里程。
ECU 1是使用由照相机2拍摄到的摄像图像和三维地图数据库3中存储的三维位置信息来进行车辆的位置和姿态角的估计的电子控制部件。此外，ECU 1也可以兼用作用于进行其它控制的ECU。
特别地，移动物体位置姿态估计装置将由照相机2拍摄到的摄像图像与从预定的位置和姿态角观察时的对比图像进行比较，来估计车辆的位置和姿态角。此外，在实施方式中，作为“从预定的位置和姿态角观察时的对比图像”的一例，使用“将三维地图数据变换为从虚拟位置和虚拟姿态角拍摄到的图像所得到的虚拟图像”。在此，假设得到如图2的(a)所示那样的摄像图像、得到如图2的(b)那样的边缘图像。另一方面，假设将三维位置信息以照相机2的位置和姿态角投影得到的虚拟图像如图2的(c)那样。当将图2的(b)的摄像图像与图2的(c)的虚拟图像进行比较时，远处位置(A)和近处位置(B)都一致，因此能够估计为生成虚拟图像的虚拟位置和虚拟姿态角相当于本车辆的位置和姿态角。但是，在虚拟位置向右方向偏移了的情况下，虚拟图像成为如图2的(d)所示那样。在这种情况下，当将图2的(a)的摄像图像与图2的(d)的摄像图像进行比较时，虽然远处位置(A)一致，但是近处位置(B)大幅地偏移。相反地，使图2的(c)的虚拟图像的虚拟姿态角偏移(未图示)，当与图2的(a)的摄像图像进行比较时，虽然近处位置(B)一致，但是远处位置(A)大幅偏移。
着眼于这样的现象，移动物体位置姿态估计装置在摄像图像内的近处位置像素与虚拟图像内的近处位置像素一致的情况下，判断为虚拟图像的虚拟位置可能正确。相反地，移动物体位置姿态估计装置在摄像图像内的远处位置像素与虚拟图像内的远处位置像素一致的情况下，判断为虚拟图像的虚拟姿态角可能正确。
下面，参照图3所示的位置姿态估计算法说明移动物体位置姿态估计装置的动作。此外，在本实施方式中，设为对车辆的3自由度的位置(前后方向、横方向、上下方向)以及3自由度的姿态角(倾倒、俯仰、横摆)进行估计。另外，图3所示的位置姿态估计算法由ECU 1例如以100msec左右的间隔连续地进行。
首先，在步骤S1中，ECU 1获取由照相机2拍摄到的影像，基于该影像中包含的摄像图像计算边缘图像。本实施方式中的边缘是指像素的亮度尖锐地变化的位置。作为边缘检测方法，例如能够使用Canny法。不限于此，边缘检测方法除此之外还可以使用微分边缘检测等各种方法。
另外，期望ECU 1从照相机2的摄像图像中抽出边缘的亮度变化的方向、边缘附近的色彩等。由此，在后述的步骤S5和步骤S6中，也可以还使用在三维地图数据库3中也已事先记录的这些边缘以外的信息设定位置似然度、姿态角似然度来估计车辆的位置和姿态角。
在接下来的步骤S2中，ECU 1基于从车辆传感器组4获得的传感器值，计算相对于通过前一周期的位置姿态估计算法计算出的车辆的位置的移动量、即里程。此外，在开始位置姿态估计算法后最初的周期的情况下，将里程计算为零。
ECU 1使用从车辆传感器组4获得的各种传感器值，计算车辆在单位时间内前进的移动量、即里程。作为该里程计算方法，例如基于在将车辆运动限定在平面上之后由各车轮的车轮速度传感器和转向传感器检测出的传感器值计算车速和横摆率，并计算单位时间内的移动量和旋转量即可。ECU 1可以用车速、GPS接收机41的定位值的差分来代替车轮速度，也可以用转轮角来代替偏行率传感器。此外，里程的计算方法可以想到各种计算方法，但是只要能够计算里程，就可以使用任意的方法。
在接下来的步骤S3中，ECU 1(对比图像获取部)获取从预定的位置和姿态角观察时的对比图像。具体地说，作为对比图像获取部的一例的虚拟图像获取部11基于在步骤S2中计算出的里程来计算多个虚拟(预测)位置和虚拟(预测)姿态角的候选。多个虚拟位置和虚拟姿态角的候选是本车位置和姿态角的候选。此时，虚拟图像获取部11从在前一周期的步骤S6中估计出的车辆位置移动与在本次的步骤S2中计算出的里程相应的量。虚拟图像获取部11在移动后的车辆位置附近计算多个虚拟位置和虚拟姿态角的候选。其中，在开始位置姿态估计算法后最初的周期的情况下，ECU 1不具有前次的车辆位置信息。因此，虚拟图像获取部11将来自车辆传感器组4中包括的GPS接收机41的数据设为初始位置信息。或者，虚拟图像获取部11也可以事先存储在前次停车时最后计算出的车辆位置和姿态角，将其作为初始位置和姿态角信息。
此时，虚拟图像获取部11将因车辆传感器组4的测量误差、通信迟滞产生的里程的误差、关于里程所考虑不到的车辆的动态特性考虑在内，生成多个有可能取得车辆的位置、姿态角的真值的多个虚拟位置和虚拟姿态角的候选。关于该虚拟位置和虚拟姿态角的候选，针对位置和姿态角的6自由度的参数分别设定误差的上下限，在该误差的上下限的范围内使用随机数表等随机地设定。
此外，在本实施方式中，制作500个虚拟位置和虚拟姿态角的候选。另外，针对位置和姿态角的6自由度的参数的误差的上下限按照前后方向、横方向、上下方向、倾倒、俯仰、横摆的顺序设为±0.05[m]、±0.05[m]、±0.05[m]、±0.5[deg]、±0.5[deg]、±0.5[deg]。关于制作该虚拟位置和虚拟姿态角的候选的个数、针对位置和姿态角的6自由度的参数的误差的上下限，期望对车辆的驾驶状态、路面的状况进行检测或估计，从而适当地变更。例如，在产生了急转弯、打滑等的情况下，平面方向(前后方向、横方向、横摆)的误差变大的可能性高，因此期望使这三个参数的误差的上下限变大且增加虚拟位置和虚拟姿态角的候选的制作个数。
在步骤S3中，虚拟图像获取部11也可以使用所谓的粒子过滤器来设定多个虚拟位置和虚拟姿态角的候选。此时，虚拟图像获取部11使在前一周期的步骤S6中生成的多个虚拟位置和虚拟姿态角的候选、即各粒子(候选点)的位置和姿态角移动与里程相应的量。具体地说，如图4所示，使在前一周期估计出的车辆V(t1)的位置和姿态角的粒子P以及周围的粒子P1～P5移动与里程相应的量。其结果，虚拟图像获取部11设定用于估计新的车辆V(t2)的位置和姿态角的粒子P10～15。由此，虚拟图像获取部11计算本次的多个虚拟位置和虚拟姿态角的候选。即，将各粒子的位置和姿态角设为多个虚拟位置和虚拟姿态角的候选。此外，更为优选的是，将因车辆传感器组4的测量误差、通信迟滞产生的里程的误差、关于里程所考虑不到的车辆的动态特性考虑在内，使各粒子的位置信息和姿态角信息移动与里程相应的量，之后，如上述那样在针对位置和姿态角的6自由度的参数的误差的上下限的范围内使用随机数表等使误差随机地变化。
但是，在开始位置姿态估计算法后最初的周期的情况下，各粒子不具有位置信息和姿态角信息。因此，也可以将车辆传感器组4中包括的GPS接收机41的检测数据设为初始位置信息。或者，也可以基于在前次停车时最后估计出的车辆位置来设定各粒子的位置信息和姿态角信息。在本实施方式中，在最初的周期的情况下，基于在前次停车时最后估计出的车辆位置来针对位置和姿态角的6自由度的参数设定误差的上下限。然后，在该误差的上下限的范围内使用随机数表等随机地设定各粒子的位置信息和姿态角。在本实施方式中，在最初的周期的情况下制作500个粒子。另外，针对各粒子的6自由度的参数的误差的上下限按照前后方向、横方向、上下方向、倾倒、俯仰、横摆的顺序设为±0.05[m]、±0.05[m]、±0.05[m]、±0.5[deg]、±0.5[deg]、±0.5[deg]。
在接下来的步骤S4中，虚拟图像获取部11针对在步骤S3中制作的多个虚拟位置和虚拟姿态角的候选分别制作虚拟图像(投影图像)。此时，虚拟图像获取部11例如将三维地图数据库3中存储的边缘等的三维位置信息变换成为从虚拟位置和虚拟姿态角拍摄到的照相机图像，来制作虚拟图像。该虚拟图像是用于评价虚拟位置和虚拟姿态角的各候选是否与本车辆实际的位置和姿态角相符的评价用图像。在向虚拟图像的变换处理中，需要表示照相机2的位置的外部参数和照相机2的内部参数。关于外部参数，只要通过预先测量车辆位置(例如中心位置)至照相机2的相对位置，基于虚拟位置和虚拟姿态角的候选计算即可。另外，关于内部参数，只要预先进行校准即可。
此外，更为优选的是，在能够从在步骤S1中由照相机2拍摄到的摄像图像中抽出边缘的亮度变化的方向、边缘附近的色彩等的情况下，也能够将它们的三维位置信息事先记录到三维地图数据库3中，制作虚拟图像。
在步骤S5中，ECU 1(似然度设定部12)针对在步骤S3中设定的多个虚拟位置和虚拟姿态角的候选，分别将在步骤S1中制作出的边缘图像与在步骤S4中制作出的虚拟图像进行比较。似然度设定部12根据比较的结果，针对每个虚拟位置和虚拟姿态角的候选设定位置似然度和姿态角似然度。位置似然度是表示虚拟位置的候选相对于车辆实际的位置的正确度有多少的指标。姿态角似然度是表示虚拟姿态角的候选相对于车辆实际的姿态角的正确度有多少的指标。虚拟图像与边缘图像的一致度越高，似然度设定部12将位置似然度或姿态角似然度设定得越高。此外，稍后记述求位置似然度或姿态角似然度的方法。
似然度设定部12将摄像图像与虚拟图像进行比较，在摄像图像内的远处位置像素与虚拟图像内的远处位置像素一致的情况下，将虚拟图像的姿态角似然度设定得高。似然度设定部12将摄像图像与虚拟图像进行比较，在摄像图像内的近处位置像素与虚拟图像内的近处位置像素一致的情况下，将虚拟图像的位置似然度设定得高。远处位置像素和近处位置像素例如也可以根据摄像图像和虚拟图像的位置来设定。例如，也可以将相对于摄像图像和虚拟图像内的纵方向上的中间位置向下规定幅度的下方向的范围设定为远处位置像素存在的图像区域。另外，也可以将相对于摄像图像和虚拟图像内的纵方向上的底部位置向上规定幅度的上方向的范围设定为近处位置像素存在的图像区域。在求将三维地图投影于照相机2中时的虚拟图像时，由于假定了照相机2的位置，因此能够求出该距离。
具体地说，似然度设定部12判断虚拟图像上与边缘图像上的边缘是否一致、即在虚拟图像上的边缘所存在的像素坐标位置是否存在边缘图像上的边缘。似然度设定部12在虚拟图像上与边缘图像上的边缘一致的情况下，关于该一致的边缘，参照三维地图数据库3求出该一致的边缘在三维空间上的位置。然后，计算求出了位置似然度的虚拟位置和虚拟姿态角的候选的位置信息与该一致的边缘部的距离L(单位：m)，将该距离L的倒数设为位置似然度like_p(单位：无)。此外，似然度设定部12在虚拟图像上与边缘图像上的边缘不一致的情况下，对位置似然度like_p设定0。
似然度设定部12针对虚拟图像上的所有像素实施该处理。似然度设定部12在距车辆近的部分处边缘图像与虚拟图像一致的情况下，对该像素设定较大的位置似然度like_p。相反地，ECU 1在距车辆较远的部分处边缘图像与虚拟图像一致的情况下，对该像素设定较小的位置似然度like_p。似然度设定部12将所有像素的位置似然度like_p的总和设为关于虚拟图像的位置似然度LIKE_P(单位：无)。
似然度设定部12在求位置似然度like_p时，也可以如图5所示那样与距车辆的距离相应地设置上限值。似然度设定部12在规定距离Lthp以上的近处边缘图像(摄像图像)的像素与虚拟图像的像素一致的情况下，将位置似然度like_p设定为规定的上限值lthp。或者，也可以距车辆的距离越近，似然度设定部12使似然度like_p的增加幅度越少。此外，在本实施方式中，将距离Lthp设定为1.0[m]。
具体地说，似然度设定部12预先准备记述了距车辆的距离与位置似然度like_p的关系的位置似然度设定映射表。ECU 1参照位置似然度设定映射表，根据边缘图像与虚拟图像一致的距车辆的距离来设定位置似然度like_p。
由此，即使在针对距车辆(照相机2)的距离极近的部分抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小位置估计误差。
ECU 1还求姿态角似然度。似然度设定部12在虚拟图像上与边缘图像上的边缘部一致的情况下，与求出位置似然度LIKE_P时同样地，求出一致的像素距车辆的距离L(单位：m)。似然度设定部12将距车辆的距离L除以10得到的值设为姿态角似然度like_a(单位：无)。此外，在虚拟图像上与边缘图像上的边缘部不一致的情况下，对姿态角似然度like_a设定0。
似然度设定部12针对虚拟图像上的所有像素实施设定姿态角似然度like_a的处理。似然度设定部12在距车辆较远的部分处边缘图像与虚拟图像一致的情况下，针对该像素设定较大的姿态角似然度like_a。相反地，似然度设定部12在距车辆较近的部分处边缘图像与虚拟图像一致的情况下，针对该像素设定较小的姿态角似然度like_a。似然度设定部12将所有像素的姿态角似然度like_a的总和设为关于虚拟图像的姿态角似然度LIKE_A(单位：无)。
似然度设定部12在求姿态角似然度like_a时，也可以如图6所示那样与距车辆的距离相应地设置上限值。ECU 1在规定距离Ltha以上的远处边缘图像(摄像图像)的像素与虚拟图像的像素一致的情况下，将姿态角似然度like_a设定为规定的上限值ltha。或者，也可以距车辆的距离越远，ECU 1使姿态角似然度like_a的增加幅度越少。此外，在本实施方式中，将距离Ltha设定为30.0[m]。
具体地说，似然度设定部12预先准备记述了距车辆的距离与姿态角似然度like_a的关系的姿态角似然度设定映射表。ECU 1参照姿态角似然度设定映射表，根据边缘图像与虚拟图像一致的距车辆的距离来设定姿态角似然度like_a。
由此，即使在针对距车辆(照相机2)的距离极远的部分抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小姿态角估计误差。
似然度设定部12关于各虚拟图像求出位置似然度LIKE_P和姿态角似然度LIKE_A。即，关于虚拟位置和虚拟姿态角的各候选计算位置似然度LIKE_P和姿态角似然度LIKE_A。似然度设定部12使用关于多个虚拟位置和虚拟姿态角的候选的所有结果进行归一化使得位置似然度LIKE_P、姿态角似然度LIKE_A各自的合计值为1。
在接下来的步骤S6中，移动物体位置姿态估计部13使用在步骤S5中求出了位置似然度LIKE_P和姿态角似然度LIKE_A的多个虚拟位置和虚拟姿态角的候选来计算车辆的最终的位置和姿态角。移动物体位置姿态估计部13根据姿态角似然度LIKE_A被设定得高的虚拟图像的姿态角来估计车辆实际的姿态角。移动物体位置姿态估计部13根据位置似然度LIKE_P被设定得高的虚拟图像的位置来估计车辆实际的位置。
此时，移动物体位置姿态估计部13例如也可以将位置似然度LIKE_P被设定得最高的虚拟图像的虚拟位置和虚拟姿态角计算为车辆实际的位置和姿态角。或者，移动物体位置姿态估计部13也可以将姿态角似然度LIKE_A被设定得最高的虚拟图像的虚拟位置和虚拟姿态角计算为车辆实际的位置和姿态角。或者，移动物体位置姿态估计部13也可以将位置似然度LIKE_P与姿态角似然度LIKE_A之和最高的虚拟图像的虚拟位置和虚拟姿态角计算为车辆实际的位置和姿态角。或者，移动物体位置姿态估计部13也可以针对虚拟位置和虚拟姿态角的各候选进行与各虚拟图像的位置似然度LIKE_P相应的加权，将加权后的虚拟位置和虚拟姿态角的平均值计算为车辆实际的位置和姿态角。或者，移动物体位置姿态估计部13也可以针对虚拟位置和虚拟姿态角的各候选进行与各虚拟图像的姿态角似然度LIKE_A相应的加权，将加权后的虚拟位置和虚拟姿态角的平均值计算为车辆实际的位置和姿态角。或者，移动物体位置姿态估计部13也可以针对虚拟位置和虚拟姿态角的各候选进行与各虚拟图像的位置似然度LIKE_P和姿态角似然度LIKE_A之和相应的加权，将加权后的虚拟位置和虚拟姿态角的平均值计算为车辆实际的位置和姿态角。
在步骤S3中使用粒子过滤器的情况下，首先，针对各粒子设定与位置似然度LIKE_P和姿态角似然度LIKE_A各自相应的权重。然后，移动物体位置姿态估计部13将位置似然度LIKE_P最高的粒子的虚拟位置设为车辆实际的位置。或者，移动物体位置姿态估计部13也可以针对多个粒子的虚拟位置进行与位置似然度LIKE_P相应的加权，将加权后的多个粒子的虚拟位置的平均值计算为车辆实际的位置。或者，移动物体位置姿态估计部13也可以将姿态角似然度LIKE_A最高的粒子的虚拟姿态角计算为车辆实际的姿态角。或者，移动物体位置姿态估计部13也可以针对多个粒子的虚拟姿态角进行与姿态角似然度LIKE_A相应的加权，将加权后的多个粒子的虚拟姿态角的平均值计算为车辆实际的姿态角。
并且，虚拟图像获取部11根据位置似然度LIKE_P和姿态角似然度LIKE_A进行各粒子的重采样。即，根据多个虚拟图像的姿态角似然度以及多个虚拟图像的位置似然度来重新设定多个虚拟位置和虚拟姿态角。
具体地说，虚拟图像获取部11以位置似然度LIKE_P与姿态角似然度LIKE_A之和最高的粒子为中心进行各粒子的重采样。或者，虚拟图像获取部11将各粒子的位置信息与姿态角信息暂时分离，关于只有位置信息的粒子，根据位置似然度LIKE_P进行重采样，关于只有姿态角信息的粒子，根据姿态角似然度LIKE_A进行重采样。之后，虚拟图像获取部11将只有位置信息的粒子的位置信息和只有姿态角信息的粒子的姿态角信息随机地组合，也可以重新构成新的具有位置信息和姿态角信息的粒子。
ECU 1通过重复进行如上所述的步骤S1至步骤S6，能够依次计算车辆的位置和姿态角。
如以上详细说明的那样，根据作为本实施方式示出的移动物体位置姿态估计装置，将摄像图像与虚拟图像进行比较，在摄像图像内的远处位置像素与虚拟图像内的远处位置像素一致的情况下，将虚拟图像的姿态角似然度设定得高。另一方面，在摄像图像内的近处位置像素与虚拟图像内的近处位置像素一致的情况下将虚拟图像的位置似然度设定得高。然后，移动物体位置姿态估计装置根据将姿态角似然度设定得高的虚拟图像的虚拟姿态角来估计移动物体的实际的姿态角。另一方面，根据将位置似然度设定得高的虚拟图像的虚拟位置来估计移动物体的实际的位置。
根据移动物体位置姿态估计装置，针对位置和姿态角分别设定似然度，根据到实际的影像与虚拟影像一致的场所的距离，能够分别调整位置和姿态角来进行估计。具体地说，在距照相机2远的场所中摄像图像与虚拟图像一致的情况下，虚拟姿态角虽然接近真值但有可能虚拟位置的误差大。因此，能够将姿态角似然度设定得更大，但是不将位置似然度设定得那么大。另一方面，在距照相机2近的场所中摄像图像与虚拟图像一致的情况下，将位置似然度设定得更大，相反地不将姿态角似然度设定得那么大。
在摄像图像与虚拟图像一致的场所距照相机2远的情况下，存在虚拟位置的误差大的情况。相反地，在摄像图像与虚拟图像一致的场所距照相机2近的情况下，存在虚拟姿态角的误差大的情况。根据该移动物体位置姿态估计装置，能够高精度地估计移动物体的位置和姿态角。
另外，在规定距离以上的远处摄像图像的像素与虚拟图像的像素一致的情况下，设定虚拟图像的姿态角似然度以使姿态角似然度的增加幅度减少或使其成为规定的上限值。由此，根据移动物体位置姿态估计装置，能够避免在一致的影像中包含有距照相机2的距离极远的部分时根据该远处部分中的一致度来决定姿态角似然度。因此，即使在从距照相机2的距离极远的部分中抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小姿态角的估计误差。
并且，在规定距离以上的近处摄像图像的像素与虚拟图像的像素一致的情况下，设定虚拟图像的位置似然度以使位置似然度的增加幅度减少或者使其成为规定的上限值。由此，避免在距照相机2一定距离以内的两个影像一致的情况下使位置似然度过大。因而，根据移动物体位置姿态估计装置，即使在从距照相机2的距离极近的部分抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小位置的估计误差。
并且，根据移动物体位置姿态估计装置，设定多个粒子(候选点)来设定关于各粒子的位置似然度LIKE_P和姿态角似然度LIKE_A。根据关于各粒子的位置似然度LIKE_P和姿态角似然度LIKE_A，求出车辆的位置和姿态角。然后，能够根据位置似然度LIKE_P和姿态角似然度LIKE_A对粒子进行重采样。
在此，在粒子过滤器中，在估计的次数设为n的情况下，由于将估计精度设为a倍，因此分散的粒子的个数在理论上也需要以a的n次方倍地增加(確率ロボティクス·第4章3節((著)Sebastian Thrun/Wolfram Burgard/Dieter Fox,(訳)上田隆一,(発行)(株)毎日コミュニケーションズ))。在现有技术中，例如在同时对三维中的位置信息和姿态角信息进行估计的情况下，其次数为6次。因此，如果想要将估计精度升高至2倍，则运算时间成为64倍，如果想要将估计精度升高至3倍，则运算时间增加到729倍。
与此相对地，根据本实施方式所涉及的移动物体位置姿态估计装置，能够将位置信息和姿态角信息分开进行处理。因而，在理论上想要将估计精度升高至2倍的情况下，运算时间为23×2＝16倍，在想要将估计精度升高至3倍的情况下，运算时间为33×2＝54倍，因此能够大幅地减轻运算负荷。
(第二实施方式)
在本发明的第二实施方式中，说明针对车辆的位置和姿态角分别使用不同的虚拟图像进行估计的移动物体位置姿态估计装置和移动物体位置姿态估计方法。
具体地说，在第一实施方式中，设定多个由虚拟位置和虚拟姿态角的组合构成的候选点(粒子)，根据关于各候选点获取到的虚拟图像，估计出车辆的位置和姿态角。与此相对地，在第二实施方式中，基于多个虚拟位置获取车辆附近区域的虚拟图像，使用车辆附近区域的虚拟图像来估计车辆的位置。然后，基于多个虚拟姿态角获取车辆远处区域的虚拟图像，使用车辆远处区域的虚拟图像来估计车辆的姿态角。也就是说，在车辆的位置和姿态角的估计中使用不同的虚拟图像的点不同。
此外，第二实施方式所涉及的移动物体位置姿态估计装置的硬件结构与图1所示的结构相同，省略说明。但是，ECU 1的软件结构、即虚拟图像获取部11、似然度设定部12、移动物体位置姿态估计部13如下面所示那样是不同的。
接着，参照图7所示的位置姿态估计算法说明移动物体位置姿态估计装置的动作。此外，在本实施方式中，设为对车辆的3自由度的位置(前后方向、横方向、上下方向)以及3自由度的姿态角(倾倒、俯仰、横摆)进行估计。另外，图7所示的位置姿态估计算法由ECU 1例如以100msec左右的间隔连续地进行。
步骤S1～S2的处理与参照图3说明的步骤S1～S2相同，省略说明。
在步骤S2之后进入步骤S13，ECU 1(虚拟图像获取部11)根据在步骤S2中计算出的里程来计算具有6自由度的初始预测位置和初始预测姿态角。具体地说，虚拟图像获取部11将从在前一周期的步骤S16中估计出的车辆的位置移动了与在本次的步骤S2中计算出的里程相应的量得到的位置设定为初始预测位置。其中，在开始位置姿态估计算法后最初的周期的情况下，ECU1不具有前次的车辆位置信息。因此，虚拟图像获取部11将来自车辆传感器组4中包括的GPS接收机41的数据设为初始预测位置。或者，虚拟图像获取部11也可以事先存储在前次停车时最后计算出的车辆位置和姿态角，设为初始预测位置和初始预测姿态角。
进入步骤S14，虚拟图像获取部11在步骤S13中计算出的初始预测位置及其附近设定多个虚拟位置。此时，虚拟图像获取部11将因车辆传感器组4的测量误差、通信迟滞产生的里程的误差、关于里程所考虑不到的车辆的动态特性考虑在内，生成有可能取得车辆的位置的真值的多个虚拟位置。关于虚拟位置，针对位置所包含的3自由度的参数分别设定误差的上下限，在该误差的上下限的范围内以固定间隔设定。在本实施方式中，针对位置所包含的3自由度的参数的误差的上下限按前后方向、横方向、上下方向的顺序设为±0.2[m]、±0.1[m]、±0.05[m]，分别以0.01[m]的间隔设定虚拟位置。因而，制作40×20×10＝8000的虚拟位置。关于误差的上下限和规定间隔，期望对车辆的驾驶状态、路面的状况进行检测或估计，从而适当地变更。例如在产生了急转弯、打滑等的情况下，平面方向(前后方向、横方向)的误差变大的可能性高。因此，优选的是，使三个参数的误差的上下限变大。
进入步骤S15，虚拟图像获取部11使用评价点投影方法，在步骤S14中设定的各虚拟位置制作车辆附近的区域的虚拟图像。其中，步骤S15中的虚拟图像的制作是为了估计车辆1的位置。因此，仅将三维地图数据库3中存储的边缘等的三维位置信息中的位于车辆1附近的信息利用评价点投影手段变换为从虚拟位置和虚拟姿态角拍摄到的虚拟图像。在本实施方式中，仅对位于距各虚拟位置的距离3m以内的边缘等的三维位置信息进行投影来制作虚拟图像。此外，作为在步骤S15中使用的虚拟姿态角，只要使用在前一周期的步骤S19中估计出的车辆的姿态角、或者在步骤S13中求出的初始预测姿态角即可。
进入步骤S16，似然度设定部12将在步骤S15中制作出的车辆附近的区域的各虚拟图像与摄像图像进行比较。具体地说，关于在步骤S15中制作出的各个虚拟图像，将虚拟图像中包含的边缘与由摄像图像中包含的边缘构成的边缘图像进行比较，计算虚拟图像内的边缘与边缘图像内的边缘的一致度。例如对在车辆附近的虚拟图像与边缘图像之间边缘一致的像素数、即在边缘所存在的虚拟图像上的像素坐标位置存在边缘图像上的边缘的像素数进行计数来作为一致度。然后，虚拟图像内的边缘与摄像图像内的边缘的一致度越高，似然度设定部12将位置似然度设定得越高。
接着，移动物体位置姿态估计部13根据由似然度设定部12将位置似然度设定得高的虚拟图像的虚拟位置来估计车辆的位置。例如，移动物体位置姿态估计部13将在步骤S15中制作出的多个虚拟图像中的一致的像素数最多的虚拟图像的虚拟位置计算为车辆1实际的位置。或者，移动物体位置姿态估计部13也可以针对各虚拟位置进行与各虚拟图像的位置似然度相应的加权，将加权后的虚拟位置的平均值计算为车辆实际的位置。
这样，通过步骤S14～S16的处理，来基于多个虚拟位置获取车辆附近区域的虚拟图像，能够使用车辆附近区域的虚拟图像来估计车辆的位置。
接着，进入步骤S17，虚拟图像获取部11参照在步骤S13中计算出的初始预测姿态角来设定多个虚拟姿态角。此时，虚拟图像获取部11将因车辆传感器组4的测量误差、通信迟滞产生的里程的误差、关于里程所考虑不到的车辆的动态特性考虑在内，生成有可能取得车辆的姿态角的真值的多个虚拟姿态角。关于虚拟姿态角，针对姿态角所包含的3自由度的参数分别设定误差的上下限，在该误差的上下限的范围内以规定间隔设定。在本实施方式中，针对姿态角所包含的3自由度的参数的误差的上下限按倾倒、俯仰、横摆的顺序设为±0.5[deg]、±0.5[deg]、±0.5[deg]，分别以0.05[deg]的间隔设定虚拟姿态角。因而，制作20×20×20＝8000的虚拟位置。与步骤S14的位置的情况同样地，关于误差的上下限和规定间隔，期望对车辆的驾驶状态、路面的状况进行检测或估计，从而适当地变更。例如在产生了急转弯、打滑等的情况下，横摆角的误差变大的可能性高。因此，优选的是，使横摆角的误差的上下限变大、或者在如爬坡那样的情况下使俯仰角的误差的上下限变大。
进入步骤S18，虚拟图像获取部11利用评价点投影方法，针对在步骤S17中设定的各虚拟姿态角制作距车辆远的区域的虚拟图像。其中，步骤S18中的虚拟图像的制作是为了估计车辆1的姿态角。因此，仅将三维地图数据库3中所存储的边缘等的三维位置信息中的位于距车辆1远处的信息利用评价点投影手段变换为从虚拟位置和虚拟姿态角拍摄到的虚拟图像。在本实施方式中，仅对位于距在步骤S13中计算出的初始预测位置或在步骤S16中计算出的车辆1的位置的距离20m以远的边缘等的三维位置信息进行投影来制作虚拟图像。此外，作为在步骤S18中使用的虚拟位置，只要使用在前一周期的步骤S19中估计出的车辆的位置、或者在步骤S13中求出的初始预测位置即可。此外，即使投影到极远处，投影的处理也花费时间，而且照相机2的分辨率也无法区分边缘是否一致。因此，在本实施方式中，不对距离50m以外的边缘等的三维位置信息进行投影。
进入步骤S19，似然度设定部12将在步骤S18中制作出的距车辆远的区域的各虚拟图像与摄像图像进行比较。具体地说，关于在步骤S18中制作出的各个虚拟图像，将虚拟图像中包含的边缘与由摄像图像中包含的边缘构成的边缘图像进行比较，计算虚拟图像内的边缘与边缘图像内的边缘的一致度。例如，对在车辆远处的虚拟图像与边缘图像之间边缘一致的像素数、即在边缘所存在的虚拟图像上的像素坐标位置存在边缘图像上的边缘的像素数进行计数来作为一致度。而且，虚拟图像内的边缘与摄像图像内的边缘的一致度越高，似然度设定部12将姿态角似然度设定得越高。
接着，移动物体位置姿态估计部13根据由似然度设定部12将姿态角似然度设定得高的虚拟图像的虚拟姿态角来估计车辆的姿态角。例如，移动物体位置姿态估计部13将在步骤S18中制作出的多个虚拟图像中的一致的像素数最多的虚拟图像的虚拟姿态角计算为车辆1实际的姿态角。或者，移动物体位置姿态估计部13也可以针对各虚拟姿态角进行与各虚拟图像的姿态角似然度相应的加权，将加权后的虚拟姿态角的平均值计算为车辆实际的姿态角。
这样，通过步骤S17～S19的处理，来基于多个虚拟姿态角获取车辆远处区域的虚拟图像，能够使用车辆远处区域的虚拟图像来估计车辆的姿态角。
ECU 1通过重复进行如上所述的步骤S1～步骤S19的处理，能够依次计算车辆的位置和姿态角。
如以上说明的那样，在本实施方式中，针对车辆的位置和姿态角分别使用不同的虚拟图像进行估计。具体地说，基于多个虚拟位置获取车辆附近区域的虚拟图像，使用车辆附近区域的虚拟图像来估计车辆的位置。然后，基于多个虚拟姿态角获取车辆远处区域的虚拟图像，使用车辆远处区域的虚拟图像来估计车辆的姿态角。由此，针对位置和姿态角设定不同的似然度，根据到实际的影像与虚拟影像一致的场所的距离，能够分别调整位置和姿态角来进行估计。因此，能够高精度地估计移动物体的位置和姿态角。
另外，在规定距离以上的远处的摄像图像的像素与虚拟图像的像素一致的情况下，也可以设定虚拟图像的姿态角似然度以使姿态角似然度的增加幅度减少或者使其成为规定的上限值。由此，根据移动物体位置姿态估计装置，能够避免在一致的影像中包含有距照相机2的距离极远的部分时根据该远处部分中的一致度来决定姿态角似然度。因此，即使在从距照相机2的距离极远的部分中抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小姿态角的估计误差。
并且，在规定距离以上的近处摄像图像的像素与虚拟图像的像素一致的情况下，设定虚拟图像的位置似然度以使位置似然度的增加幅度减少或者使其成为规定的上限值。由此，避免在距照相机2一定距离以内的两个影像一致的情况下使位置似然度过大。因而，根据移动物体位置姿态估计装置，即使在从距照相机2的距离极近的部分抽出的边缘中存在噪声、误差，也能够抑制其影响，从而能够减小位置的估计误差。
以上按实施方式说明了本发明的内容，但是本发明不限定于这些记载，能够进行各种变形、改进，这对于本领域技术人员来说是显而易见的。
在上述的实施方式中将车辆作为例子，但是只要是搭载有至少一台以上的照相机的移动物体，则也能够应用于飞机、船舶等。
另外，在上述的实施方式中求出了车辆的6自由度的位置(前后方向、横方向、上下方向)以及姿态角(倾倒、俯仰、横摆)，但是不限于此。在例如对不具有悬架等的在工厂等中使用的无人搬运车等那样3自由度的位置(前后方向、横方向)和姿态角(横摆)进行估计的情况下，也能够应用本实施方式。具体地说，只要是这样的车辆，上下方向的位置以及倾倒和俯仰之类的姿态角就是固定的，因此只要预先测量出这些参数、或者参照三维地图数据库3求出即可。
此外，在第一和第二实施方式中，作为获取“从预定的位置和姿态角观察时的对比图像的对比图像获取部”，例示了“将三维地图数据变换为从虚拟位置和虚拟姿态角拍摄到的图像来获取虚拟图像的虚拟图像获取部11”。但是，“对比图像获取部”不限定于此。例如，对比图像获取部也可以获取照相机2过去拍摄到的摄像图像来作为对比图像。在这种情况下，移动物体位置姿态估计部13根据拍摄到由似然度设定部12将姿态角似然度设定得高的对比图像时的车辆的姿态角来估计车辆的姿态角、根据拍摄到由似然度设定部12将位置似然度设定得高的对比图像时的车辆的位置来估计车辆的位置即可。由此，针对位置和姿态角分别设定似然度，根据到当前的摄像影像与过去的摄像图像(对比图像)一致的场所的距离，能够分别调整位置和姿态角来进行估计。因此，能够高精度地估计移动物体的位置和姿态角。
日本特愿2012-049372号(申请日：2012年3月6日)的全部内容在此处被引用。
产业上的可利用性
根据本实施方式所涉及的移动物体位置姿态估计装置和移动物体位置姿态估计方法，在远处位置处摄像图像与虚拟图像一致的情况下，使姿态角似然度变高，在近处位置处摄像图像与虚拟图像一致的情况下，使位置似然度变高，因此能够根据位置误差大的远处的一致来设定姿态角似然度，根据姿态角误差大的近处的一致来设定位置似然度。由此，能够高精度地估计移动物体的位置和姿态角。因此，本发明具有产业上的可利用性。
附图标记说明
1：ECU；2：照相机(摄像部)；3：三维地图数据库；11：虚拟图像获取部(对比图像获取部)；12：似然度设定部；13：移动物体位置姿态估计部。

Claims (7)

1.一种移动物体位置姿态估计装置，对移动物体的位置和姿态角进行估计，该移动物体位置姿态估计装置的特征在于，具有：
摄像部，其对上述移动物体周边进行拍摄来获取摄像图像；
对比图像获取部，其获取从预定的位置和姿态角观察时的对比图像；
似然度设定部，其将由上述摄像部获取到的摄像图像与由上述对比图像获取部获取到的对比图像进行比较，在上述摄像图像内的远处位置像素与上述对比图像内的远处位置像素一致的情况下，将上述对比图像的姿态角似然度设定得高，在上述摄像图像内的近处位置像素与上述对比图像内的近处位置像素一致的情况下，将上述对比图像的位置似然度设定得高；以及
移动物体位置姿态估计部，其根据由上述似然度设定部将上述姿态角似然度设定得高的上述对比图像的姿态角来估计上述移动物体的姿态角，根据由上述似然度设定部将上述位置似然度设定得高的上述对比图像的位置来估计上述移动物体的位置。
2.根据权利要求1所述的移动物体位置姿态估计装置，其特征在于，
上述对比图像获取部是将三维地图数据变换为从虚拟位置和虚拟姿态角拍摄到的图像来获取虚拟图像的虚拟图像获取部，
上述似然度设定部将由上述摄像部获取到的摄像图像与由上述虚拟图像获取部获取到的虚拟图像进行比较，在上述摄像图像内的远处位置像素与上述虚拟图像内的远处位置像素一致的情况下，将上述虚拟图像的姿态角似然度设定得高，在上述摄像图像内的近处位置像素与上述虚拟图像内的近处位置像素一致的情况下，将上述虚拟图像的位置似然度设定得高，
上述移动物体位置姿态估计部根据由上述似然度设定部将上述姿态角似然度设定得高的上述虚拟图像的虚拟姿态角来估计上述移动物体的姿态角，根据由上述似然度设定部将上述位置似然度设定得高的上述虚拟图像的虚拟位置来估计上述移动物体的位置。
3.根据权利要求1或2所述的移动物体位置姿态估计装置，其特征在于，
上述似然度设定部在上述摄像图像和上述对比图像内的远处位置像素中的、规定距离以上的远处的上述摄像图像的像素与上述对比图像的像素一致的情况下，设定上述对比图像的姿态角似然度以使似然度的增加幅度减少或者使似然度的增加幅度成为规定的上限值。
4.根据权利要求1或2所述的移动物体位置姿态估计装置，其特征在于，
上述似然度设定部在上述摄像图像和上述对比图像内的近处位置像素中的、规定距离以上的近处的上述摄像图像的像素与上述对比图像的像素一致的情况下，设定上述对比图像的位置似然度以使似然度的增加幅度减少或者使似然度的增加幅度成为规定的上限值。
5.根据权利要求2所述的移动物体位置姿态估计装置，其特征在于，
上述虚拟图像获取部设定多个设定了虚拟位置和虚拟姿态角的候选点，针对各上述候选点获取虚拟图像，上述似然度设定部将各虚拟图像与摄像图像进行比较来设定姿态角似然度和位置似然度，上述移动物体位置姿态估计部根据多个虚拟图像的姿态角似然度来估计上述移动物体的姿态角，根据多个虚拟图像的位置似然度来估计上述移动物体的位置，
上述虚拟图像获取部根据多个虚拟图像的姿态角似然度以及多个虚拟图像的位置似然度，重新设定多个上述候选点。
6.根据权利要求1所述的移动物体位置姿态估计装置，其特征在于，
上述对比图像获取部是将三维地图数据变换为从虚拟位置和虚拟姿态角拍摄到的图像来获取虚拟图像的虚拟图像获取部，
上述对比图像获取部设定上述移动物体的多个虚拟位置，针对各上述虚拟位置获取离上述移动物体近的区域的上述对比图像，
上述似然度设定部将离上述移动物体近的区域的各对比图像与上述摄像图像进行比较，上述对比图像内的边缘与上述摄像图像内的边缘的一致度越高，则将上述位置似然度设定得越高，
上述移动物体位置姿态估计部根据由上述似然度设定部将上述位置似然度设定得高的上述对比图像的位置，来估计上述移动物体的位置，
上述对比图像获取部设定上述移动物体的多个虚拟姿态角，针对各上述虚拟姿态角获取离上述移动物体远的区域的上述对比图像，
上述似然度设定部将离上述移动物体远的区域的各对比图像与上述摄像图像进行比较，上述对比图像内的边缘与上述摄像图像内的边缘的一致度越高，则将上述姿态角似然度设定得越高，
上述移动物体位置姿态估计部根据由上述似然度设定部将上述姿态角似然度设定得高的上述对比图像的姿态角，来估计上述移动物体的姿态角。
7.一种移动物体位置姿态估计方法，对移动物体的位置和姿态角进行估计，该移动物体位置姿态估计方法的特征在于，包括以下步骤：
将对上述移动物体周边进行拍摄得到的摄像图像与从预定的位置和姿态角观察时的对比图像进行比较；
在上述摄像图像内的远处位置像素与上述对比图像内的远处位置像素一致的情况下，将上述对比图像的姿态角似然度设定得高，在上述摄像图像内的近处位置像素与上述对比图像内的近处位置像素一致的情况下，将上述对比图像的位置似然度设定得高；以及
根据上述姿态角似然度被设定得高的上述对比图像的姿态角来估计上述移动物体的姿态角，根据上述位置似然度被设定得高的上述对比图像的位置来估计上述移动物体的位置。
CN201380013066.8A 2012-03-06 2013-02-28 移动物体位置姿态估计装置和移动物体位置姿态估计方法 Active CN104204726B (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2012049372 2012-03-06
JP2012-049372 2012-03-06
PCT/JP2013/055470 WO2013133129A1 (ja) 2012-03-06 2013-02-28 移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法

Publications (2)

Publication Number Publication Date
CN104204726A CN104204726A (zh) 2014-12-10
CN104204726B true CN104204726B (zh) 2017-05-31

Family

ID=49116610

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201380013066.8A Active CN104204726B (zh) 2012-03-06 2013-02-28 移动物体位置姿态估计装置和移动物体位置姿态估计方法

Country Status (5)

Country Link
US (1) US9797981B2 (zh)
EP (1) EP2824425B1 (zh)
JP (1) JP5804185B2 (zh)
CN (1) CN104204726B (zh)
WO (1) WO2013133129A1 (zh)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2621480C1 (ru) * 2013-11-13 2017-06-06 Ниссан Мотор Ко., Лтд. Устройство оценки положения движущегося тела и способ оценки положения движущегося тела
US20150226573A1 (en) * 2014-02-11 2015-08-13 Qualcomm Incorporated Pedometer integrated pedestrian positioning
JP6222353B2 (ja) * 2014-05-20 2017-11-08 日産自動車株式会社 物標検出装置及び物標検出方法
KR101784183B1 (ko) * 2014-06-17 2017-10-11 주식회사 유진로봇 ADoG 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
KR102232517B1 (ko) 2014-09-15 2021-03-26 삼성전자주식회사 이미지 촬영 방법 및 이미지 촬영 장치
CN106488143B (zh) * 2015-08-26 2019-08-16 刘进 一种生成视频数据、标记视频中物体的方法、系统及拍摄装置
DE202015008708U1 (de) * 2015-12-18 2017-03-21 GM Global Technology Operations LLC (n. d. Ges. d. Staates Delaware) Fahrzeugpositioniersystem
CN105809701B (zh) * 2016-03-25 2019-03-08 成都易瞳科技有限公司 全景视频姿态标定方法
JP6758160B2 (ja) * 2016-11-10 2020-09-23 株式会社デンソーアイティーラボラトリ 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム
JP6959032B2 (ja) * 2017-05-17 2021-11-02 株式会社Ｓｏｋｅｎ 位置推定装置、移動装置
CN108335329B (zh) * 2017-12-06 2021-09-10 腾讯科技（深圳）有限公司 应用于飞行器中的位置检测方法和装置、飞行器
JP6839677B2 (ja) * 2018-03-26 2021-03-10 康二 蓮井 移動距離測定装置、移動距離測定方法、及び移動距離測定プログラム
JP7190261B2 (ja) 2018-04-27 2022-12-15 日立Astemo株式会社 位置推定装置
JP7010778B2 (ja) * 2018-06-29 2022-01-26 国立大学法人東海国立大学機構 観測位置推定装置、その推定方法、及びプログラム
CN108871314B (zh) * 2018-07-18 2021-08-17 江苏实景信息科技有限公司 一种定位定姿方法及装置
CN109003305B (zh) * 2018-07-18 2021-07-20 江苏实景信息科技有限公司 一种定位定姿方法及装置
JP7204612B2 (ja) * 2019-08-06 2023-01-16 株式会社東芝 位置姿勢推定装置、位置姿勢推定方法及びプログラム

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101078632A (zh) * 2006-05-26 2007-11-28 富士通株式会社 移动机器人及其控制方法和程序

Family Cites Families (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB8925196D0 (en) * 1989-11-08 1990-05-30 Smiths Industries Plc Navigation systems
US5422828A (en) * 1991-12-18 1995-06-06 Choate; William C. Method and system for image-sequence-based target tracking and range estimation
US5638116A (en) * 1993-09-08 1997-06-10 Sumitomo Electric Industries, Ltd. Object recognition apparatus and method
JP3833786B2 (ja) * 1997-08-04 2006-10-18 富士重工業株式会社 移動体の３次元自己位置認識装置
JP3052286B2 (ja) * 1997-08-28 2000-06-12 防衛庁技術研究本部長 飛行システムおよび航空機用擬似視界形成装置
JP2001344597A (ja) * 2000-05-30 2001-12-14 Fuji Heavy Ind Ltd 融合視界装置
US6690883B2 (en) * 2001-12-14 2004-02-10 Koninklijke Philips Electronics N.V. Self-annotating camera
JP2005326168A (ja) * 2004-05-12 2005-11-24 Fuji Photo Film Co Ltd 運転支援システム、車両、および運転支援方法
JP3846494B2 (ja) * 2004-07-13 2006-11-15 日産自動車株式会社 移動障害物検出装置
WO2007027847A2 (en) * 2005-09-01 2007-03-08 Geosim Systems Ltd. System and method for cost-effective, high-fidelity 3d-modeling of large-scale urban environments
JP4870546B2 (ja) * 2006-12-27 2012-02-08 株式会社岩根研究所 レイヤー生成・選択機能を備えたｃｖタグ映像表示装置
DE102006062061B4 (de) * 2006-12-29 2010-06-10 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. Vorrichtung, Verfahren und Computerprogramm zum Bestimmen einer Position basierend auf einem Kamerabild von einer Kamera
JP5183071B2 (ja) * 2007-01-22 2013-04-17 任天堂株式会社 表示制御装置および表示制御プログラム
US8050458B2 (en) * 2007-06-18 2011-11-01 Honda Elesys Co., Ltd. Frontal view imaging and control device installed on movable object
JP5227065B2 (ja) 2008-01-25 2013-07-03 株式会社岩根研究所 三次元機械地図、三次元機械地図生成装置、ナビゲーション装置及び自動運転装置
US7826666B2 (en) * 2008-02-27 2010-11-02 Honeywell International Inc. Methods and apparatus for runway segmentation using sensor analysis
US8269842B2 (en) * 2008-06-11 2012-09-18 Nokia Corporation Camera gestures for user interface control
GB0818561D0 (en) * 2008-10-09 2008-11-19 Isis Innovation Visual tracking of objects in images, and segmentation of images
US8427536B2 (en) * 2009-11-19 2013-04-23 Qualcomm Incorporated Orientation determination of a mobile station using side and top view images
US9232128B2 (en) 2011-03-28 2016-01-05 Nec Corporation Image capture position and image capture direction estimation device, image capture device, image capture position and image capture direction estimation method and program

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101078632A (zh) * 2006-05-26 2007-11-28 富士通株式会社 移动机器人及其控制方法和程序

Also Published As

Publication number Publication date
EP2824425B1 (en) 2017-05-17
CN104204726A (zh) 2014-12-10
JPWO2013133129A1 (ja) 2015-07-30
US20150015702A1 (en) 2015-01-15
US9797981B2 (en) 2017-10-24
JP5804185B2 (ja) 2015-11-04
EP2824425A1 (en) 2015-01-14
WO2013133129A1 (ja) 2013-09-12
EP2824425A4 (en) 2015-05-27

Similar Documents

Publication Publication Date Title
CN104204726B (zh) 移动物体位置姿态估计装置和移动物体位置姿态估计方法
JP6930600B2 (ja) 車両位置推定装置および車両制御装置
US10354151B2 (en) Method of detecting obstacle around vehicle
CN105593776B (zh) 车辆位置姿势角推定装置及车辆位置姿势角推定方法
JP6233345B2 (ja) 路面勾配検出装置
CN110745140B (zh) 一种基于连续图像约束位姿估计的车辆换道预警方法
CN104854637B (zh) 移动物体位置姿态角推定装置及移动物体位置姿态角推定方法
US10508923B2 (en) Vehicle position estimation device, vehicle position estimation method
CN104951817B (zh) 边界检测装置和边界检测方法
CN102222236B (zh) 图像处理系统及位置测量系统
US10267640B2 (en) Vehicle position estimation device, vehicle position estimation method
CN107798699A (zh) 用立体图像进行深度图估计
JP5966747B2 (ja) 車両走行制御装置及びその方法
CN107424116A (zh) 基于侧环视相机的泊车位检测方法
CN107750364A (zh) 使用稳定的坐标系的道路垂直轮廓检测
CN104236478A (zh) 一种基于视觉的车辆外廓尺寸自动测量系统及方法
JP6201148B2 (ja) キャリブレーション装置、キャリブレーション方法、キャリブレーション機能を備えた移動体搭載用カメラ及びプログラム
CN104916163A (zh) 泊车位检测方法
CN105934774A (zh) 物体检测装置、物体检测方法以及移动机器人
JP5310027B2 (ja) 車線認識装置、及び車線認識方法
WO2016020718A1 (en) Method and apparatus for determining the dynamic state of a vehicle
JP2015194397A (ja) 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラムならびに車両位置検出システム
JP2019066302A (ja) 対象物推定装置
CN110659548A (zh) 车辆及其目标检测方法、装置
JP2007011994A (ja) 道路認識装置

Legal Events

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