CN106062849B - 自己位置计算装置及自己位置计算方法 - Google Patents

自己位置计算装置及自己位置计算方法 Download PDF

Info

Publication number
CN106062849B
CN106062849B CN201480076204.1A CN201480076204A CN106062849B CN 106062849 B CN106062849 B CN 106062849B CN 201480076204 A CN201480076204 A CN 201480076204A CN 106062849 B CN106062849 B CN 106062849B
Authority
CN
China
Prior art keywords
posture angle
vehicle
light
pattern
road surface
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
CN201480076204.1A
Other languages
English (en)
Other versions
CN106062849A (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 CN106062849A publication Critical patent/CN106062849A/zh
Application granted granted Critical
Publication of CN106062849B publication Critical patent/CN106062849B/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60Q—ARRANGEMENT OF SIGNALLING OR LIGHTING DEVICES, THE MOUNTING OR SUPPORTING THEREOF OR CIRCUITS THEREFOR, FOR VEHICLES IN GENERAL
    • B60Q1/00—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor
    • B60Q1/02—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor the devices being primarily intended to illuminate the way ahead or to illuminate other areas of way or environments
    • B60Q1/04—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor the devices being primarily intended to illuminate the way ahead or to illuminate other areas of way or environments the devices being headlights
    • B60Q1/06—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor the devices being primarily intended to illuminate the way ahead or to illuminate other areas of way or environments the devices being headlights adjustable, e.g. remotely-controlled from inside vehicle
    • B60Q1/08—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor the devices being primarily intended to illuminate the way ahead or to illuminate other areas of way or environments the devices being headlights adjustable, e.g. remotely-controlled from inside vehicle automatically
    • B60Q1/085—Arrangement of optical signalling or lighting devices, the mounting or supporting thereof or circuits therefor the devices being primarily intended to illuminate the way ahead or to illuminate other areas of way or environments the devices being headlights adjustable, e.g. remotely-controlled from inside vehicle automatically due to special conditions, e.g. adverse weather, type of road, badly illuminated road signs or potential dangers
    • 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
    • G01B11/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes
    • 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/265—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network constructional aspects of navigation devices, e.g. housings, mountings, displays
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • 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
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/10—Image acquisition
    • G06V10/12—Details of acquisition arrangements; Constructional details thereof
    • G06V10/14—Optical characteristics of the device performing the acquisition or on the illumination arrangements
    • G06V10/145—Illumination specially adapted for pattern recognition, e.g. using gratings
    • 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
    • G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60Q—ARRANGEMENT OF SIGNALLING OR LIGHTING DEVICES, THE MOUNTING OR SUPPORTING THEREOF OR CIRCUITS THEREFOR, FOR VEHICLES IN GENERAL
    • B60Q2400/00—Special features or arrangements of exterior signal lamps for vehicles
    • B60Q2400/50—Projected symbol or information, e.g. onto the road or car body
    • 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
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • Data Mining & Analysis (AREA)
  • Mechanical Engineering (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

自己位置计算装置具备：向车辆周围的路面(31)投射图案光(32a)的投光器(11)、拍摄包含投射有图案光(32a)的区域的车辆周围的路面(31)并取得图像(38)的摄像机(12)，根据由摄像机(12)取得的图像(38)中的图案光(32a)的位置，计算出车辆(10)相对于路面(31)的姿势角，基于从图像(38)检测出的路面上的多个特征点的时间变化，计算出车辆的姿势变化量。自己位置计算装置通过车辆的初始位置及姿势角加上姿势变化量，计算出车辆的当前位置及姿势角，且在多个特征点的检测状态不满足第一基准的情况下，投射图案光(32a)。

Description

自己位置计算装置及自己位置计算方法
技术领域
本发明涉及自己位置计算装置及自己位置计算方法。
背景技术
已知有如下的技术，即、取得由搭载于车辆的摄像机拍摄的车辆附近的图像，并基于图像的变化求出车辆的移动量(参照专利文献1)。专利文献1中，为了在车辆的低速且细微的移动中也高精度地求出移动量，从图像中检测特征点，求出图像上的特征点的位置，并根据特征点的移动方向及移动距离(移动量)求出车辆的移动量。
现有技术文献
专利文献
专利文献1：(日本)特开2008－175717号公报
但是，在特征点的检测状态差的情况下，难以根据特征点的位置高精度地求出车辆的移动量。
发明内容
本发明是鉴于上述课题而创立的，其目的在于，提供一种自己位置计算装置及自己位置计算方法，无论特征点的检测状态如何，均能够高精度且稳定地推定车辆的当前位置。
本发明的一方式的自己位置(本车辆位置local location)计算装置具备：投光器，其向车辆周围的路面投射图案光；摄像部，其拍摄包含投射图案光的区域在内的车辆周围的路面并取得图像。自己位置计算装置根据由摄像部取得的图像中的图案光的位置，计算出车辆相对于路面的姿势角，并基于根据图像检测出的路面上的多个特征点的时间变化，计算出车辆的姿势变化量。而且，通过在车辆的初始位置及姿势角加上姿势变化量，计算出车辆的当前位置及姿势角。自己位置计算装置在多个特征点的检测状态不满足第一基准而差的状态的情况下，投射图案光。
附图说明
图1是表示实施方式的自己位置计算装置的整体构成的块图；
图2是表示投光器11及摄像机12向车辆10的搭载例的外观图；
图3(a)是表示根据投光器11和摄像机12之间的基线长度Lb、各光点的图像上的坐标(Uj、Vj)，计算出照射各光点的路面31上的位置的情形的图，图3(b)是表示根据从与照射图案光32a的区域不同的另一区域33检测出的特征点的时间变化，求出摄像机12的移动方向34的情形的示意图；
图4(a)及图4(b)是表示对由摄像机12取得的图像实施了二值化处理的图案光32a的图像的图，图4(a)表示图案光32a整体，图4(b)表示扩大一个光点Sp，图4(c)是表示由图案光抽取部21抽取的各光点Sp的重心的位置He的图；
图5是用于说明计算距离及姿势角的变化量及摄像机12的移动量(ΔL)的方法的示意图；
图6(a)表示在时刻t取得的第一帧(图像)38的一例，图6(b)表示在从时刻t经过时间Δt的时刻(t+Δt)取得的第二帧38’；
图7是表示使用了图1的自己位置计算装置的自己位置计算方法的一例的流程图；
图8是表示图7的步骤S05的详细顺序的流程图；
图9是表示按照图8的流程执行的信息处理的一例的图，图9(a)表示姿势角计算部22是否根据图案光32a计算出距离及姿势角，图9(b)表示图案光投射实施标志的状态，图9(c)表示取得对应的特征点的数量；
图10是表示图7的步骤S09的详细顺序的流程图；
图11(a)及图11(b)是表示车辆10的侧滚角及车宽方向移动量的推定误差的例的图；
图12是表示第二实施方式的步骤S05的详细顺序的流程图；
图13是表示按照图12的流程执行的信息处理的一例的图，图13(a)表示姿势角计算部22是否根据图案光32a计算出距离及姿势角，图13(b)表示图案光投射实施标志的状态，图13(c)表示取得对应的特征点的数量；
图14是表示第三实施方式的步骤S05的详细顺序的流程图；
图15是表示按照图14的流程执行的信息处理的一例的图，图15(a)表示姿势角计算部22是否根据图案光32a计算出距离及姿势角，图15(b)表示图案光投射实施标志的状态，图15(c)表示取得对应的特征点的数量；
图16是表示第四实施方式的步骤S05的详细顺序的流程图；
图17是表示按照图16的流程执行的信息处理的一例的图，图17(a)表示姿势角计算部22是否根据图案光32a计算出距离及姿势角，图17(b)表示图案光投射实施标志的状态，图17(c)表示取得对应的特征点的数量。
具体实施方式
参照附图说明实施方式。附图的记载中，对相同部分标注相同的符号并省略说明。
(第一实施方式)
(硬件构成)
首先，参照图1说明第一实施方式的自己位置计算装置的硬件构成。自己位置计算装置具备：投光器11、摄像机12、发动机控制单元(ECU)13。投光器11搭载于车辆上，向车辆周围的路面投射图案光。摄像机12是搭载于车辆上而拍摄包含投射图案光的区域在内的车辆周围的路面并取得图像的摄像部的一例。ECU13是控制投光器11且执行根据由摄像机12取得的图像推定车辆的移动量的一连串的信息处理循环的控制部的一例。
摄像机12是使用了固体摄像元件、例如CCD及CMOS的数字摄像机，取得可以进行图像处理的数字图像。摄像机12的拍摄对象是车辆周围的路面，车辆周围的路面包含车辆的前部、后部、侧部、车辆底部的路面。例如，如图2所示，摄像机12可以搭载于车辆10的前部，具体而言，可以搭载于前保险杠上。
调整设置摄像机12的高度及方向且自动调整摄像机12所具备的透镜的焦点及光圈，以可拍摄车辆10的前方路面31上的特征点(纹理)及由投光器11投射的图案光32b。摄像机12每隔规定的时间间隔进行重复拍摄，取得一连串的图像(帧)组。由摄像机12取得的图像数据向ECU13传送，并存储于ECU13所具备的存储器。
如图2所示，投光器11向摄像机12的拍摄范围内的路面31，投射具有包含正方形或长方形的光栅图像的规定形状的图案光32b。摄像机12拍摄照射到路面31的图案光。投光器11具备例如激光指示器及衍射光栅。将从激光指示器射出的激光利用衍射光栅进行衍射，由此，如图2～图4所示，投光器11生成光栅图像或由行列状地排列的多个光点Sp构成的图案光(32b、32a)。图3及图4所示的例子中，生成由5×7的光点Sp构成的图案光32a。
返回图1，ECU13由具备CPU、存储器及输入输出部的微型控制器构成，通过执行预先安装的计算机程序，构成自己位置计算装置具备的多个信息处理部。ECU13对每个图像(帧)重复执行根据由摄像机12取得的图像计算出车辆的当前位置的一连串的信息处理循环。ECU13也可以兼用作车辆10的其它控制所使用的ECU。
多个信息处理部包含：图案光抽取部21、姿势角计算部22、姿势变化量计算部24、自己位置计算部26、图案光控制部27、检测状态判断部30、计算状态判断部35。姿势变化量计算部24包含特征点检测部23。
图案光抽取部21从存储器读取由摄像机12取得的图像，根据图像抽取图案光的位置。如图3(a)所示，例如，投光器11向路面31投射由行列状地排列的多个光点构成的图案光32a，并利用摄像机12检测被路面31反射的图案光32a。图案光抽取部21通过对由摄像机12取得的图像实施二值化处理，如图4(a)及图4(b)所示，仅抽取光点Sp的图像。如图4(c)所示，图案光抽取部21通过计算出各光点Sp的重心的位置He，即光点Sp的图像上的坐标(Uj、Vj)，而抽取图案光32a的位置。坐标在以摄像机12的摄像元件的像素为单位且5×7的光点Sp的情况下，“j”为1以上且35以下的自然数。光点Sp的图像上的坐标(Uj、Vj)作为表示图案光32a的位置的数据存储于存储器。
姿势角计算部22从存储器读取表示图案光32a的位置的数据，根据由摄像机12取得的图像中的图案光32a的位置，计算出车辆10相对于路面31的距离及姿势角。例如，如图3(a)所示，根据投光器11和摄像机12之间的基线长度Lb、各光点的图像上的坐标(Uj、Vj)，使用三角测量原理，将照射各光点的路面31上的位置作为相对于摄像机12的相对位置而计算出。而且，姿势角计算部22根据各光点相对于摄像机12的相对位置，计算出投射图案光32a的路面31的平面方程式即摄像机12相对于路面31的距离及姿势角(法向矢量)。此外，摄像机12相对于车辆10的搭载位置及拍摄方向已知，因此，实施方式中，作为车辆10相对于路面31的距离及姿势角的一例，计算出摄像机12相对于路面31的距离及姿势角。以后，将摄像机12相对于 路面31的距离及姿势角简称为“距离及姿势角”。由姿势角计算部22计算出的距离及姿势角存储于存储器。
具体而言，摄像机12及投光器11分别固定于车辆10上，因此，图案光32a的照射方向、摄像机12和投光器11的距离(基线长度Lb)已知。因此，姿势角计算部22可以使用三角测量的原理，将从各光点的图像上的坐标(Uj、Vj)照射各光点的路面31上的位置作为相对于摄像机12的相对位置(Xj，Yj，Zj)而求出。
此外，各光点相对于摄像机12的相对位置(Xj，Yj，Zj)大多情况下不存在于同一平面上。这是由于，根据在路面31上露出的沥青的凹凸不同，各光点的相对位置进行改变。因此，也可以使用最小二乘法，求出与各光点的距离误差的平方和成为最小那样的平面方程式。
特征点检测部23从存储器读取由摄像机12取得的图像，并根据从存储器读取的图像，检测路面31上的特征点。特征点检测部23为了检测路面31上的特征点，可以使用例如“D.G.Lowe，“Distinctive Image Features from Scale-Invariant Keypoints,”Int.J.Comput.Vis.,vol.60,否.2,pp.91-110,否v.200”，或，“金泽靖,金谷健一,“コンピュータビジョンのための画像の特徴点抽出(用于计算机图像的图像的特征点抽取)”信学志,vol.87,否.12,pp.1043-1048,Dec.2004”所记载的方法。
具体而言，特征点检测部23使用例如哈里斯(Harris)运算符或SUSAN操作符，以物体的顶点那样辉度值比周围大幅变化的点为特征点进行检测。或者，特征点检测部23也可以使用SIFT(Scale-Invariant Feature Transform)特征量，以根据在其周围辉度值以一定规则性变化的点为特征点进行检测。而且，特征点检测部23对由1个图像检测的特征点的总数N进行计数，并对各特征点标注识别编号(i(1≤i≤N))。各特征点的图像上的位置(Ui、Vi)存储于ECU13内的存储器。图6(a)及图6(b)表示根据由摄像机12取得的图像检测出的特征点Te的例子。各特征点的图像上的位置(Ui、Vi)存储于存储器中。
此外，实施方式中，路面31上的特征点主要假定大小为1cm以上且2cm以下的沥青混合物的粒子。为了检测该特征点，摄像机12的分辨率为VGA(约30万像素)。另外，摄像机12相对于路面31的距离大致为70cm。另外，摄像机12的拍摄方向从水平面向路面31倾斜约45deg。另外，将由摄像 机12取得的图像向ECU13传送时的辉度数值为0～255(0：最暗，255：最亮)的范围内。
姿势变化量计算部24从存储器读取按照一定的信息处理循环拍摄的帧中、上一次帧所包含的多个特征点的图像上的位置(Ui、Vi)，且从存储器读取此次帧所包含的多个特征点的图像上的位置(Ui、Vi)。而且，基于多个特征点的图像上的位置变化，求出车辆的姿势变化量。在此，“车辆的姿势变化量”包含相对于路面31的“距离及姿势角”的变化量及在路面上的“车辆(摄像机12)的移动量”双方。以下，说明距离及姿势角的变化量及车辆的移动量的计算方法。
图6(a)表示在时刻t取得的第一帧(图像)38的一例。如图5或图6(a)所示，考虑在第一帧38分别计算出例如3个特征点Te1、Te2、Te3的相对位置(Xi、Yi、Zi)的情况。在该情况下，可以将由特征点Te1、Te2、Te3特定的平面G看作路面。因此，姿势变化量计算部24可以根据相对位置(Xi、Yi、Zi)，求出摄像机12相对于路面(平面G)的距离及姿势角(法向矢量)。另外，姿势变化量计算部24可以根据已知的摄像机型号，求出各特征点Te1、Te2、Te3之间的距离(l1、l2、l3)及连结各个特征点Te1、Te2、Te3的直线构成的角度。图5的摄像机12表示第一帧时的摄像机的位置。
此外，作为表示相对于摄像机12的相对位置的三维坐标(Xi、Yi、Zi)，将摄像机12的拍摄方向设定成Z轴，将拍摄方向设为法线且在包含摄像机12的平面内设定相互正交的X轴及Y轴。另一方面，作为图像38上的坐标，将水平方向及垂直方向分别设定成V轴及U轴。
图6(b)表示在从时刻t经由时间Δt的时刻(t+Δt)取得的第二帧。图5的摄像机12’表示拍摄第二帧38’时的摄像机的位置。如图5或图6(b)所示，在第二帧38’，摄像机12’拍摄特征点Te1、Te2、Te3，特征点检测部23检测出特征点Te1、Te2、Te3。在该情况下，姿势变化量计算部24根据时刻t时的各特征点Te1、Te2、Te3的相对位置(Xi、Yi、Zi)、各特征点的第二帧38’上的位置P1(Ui、Vi)和摄像机12的摄像机型号，不仅可以计算出时间Δt时的摄像机12(车辆)的移动量(ΔL)，而且还可以计算出距离及姿势角的变化量。例如，通过解开以下的由式(1)～(4)构成的联合方程式，姿势变化量计算部24可以计算出摄像机12的移动量(ΔL)及距离及姿势角的变化量。此外，式(1)是摄像机12作为没有失真或没有光轴偏离的理想性的***而模拟化的式子，λi为常数，f为焦点距离。摄像机型号的参数只要预先进行校准即可。
图3(b)示意性地表示摄像机12的拍摄范围中，根据与照射图案光32a的区域不同的其它区域33检测出的特征点的时间变化，求出摄像机12的移动方向34的情形。另外，图6(a)及图6(b)中，将表示各特征点Te的位置的变化方向及变化量的矢量Dte与图像重叠表示。姿势变化量计算部24不仅可以计算出时间Δt时的摄像机12的移动量(ΔL)，而且还可以同时计算出距离及姿势角的变化量。因此，姿势变化量计算部24考虑距离及姿势角的变化量，可以高精度地计算出6个自由度的移动量(ΔL)。即，即使距离或姿势角根据车辆10的转向或加减速产生的侧滚运动或俯仰运动不同而变化，也可以抑制移动量(ΔL)的推定误差。
此外，姿势变化量计算部24也可以不使用计算出相对位置的全部特征点，而基于特征点彼此的位置关系选定最佳的特征点。作为选定方法，例如可以使用对极几何(对极线几何，R.I.Hartley:“A linear method for reconstruction from lines and points,”Proc.5th International Conference on Computer Vision,Cambridge,Massachusetts,pp.882-887(1995))。
就在前后帧间使特征点对应而言，只要将例如检测出的特征点周边的小区域的图像记录于存储器，并根据辉度或色信息的类似度进行判断即可。具体而言，ECU13将以检测出的特征点为中心的5×5(水平×垂直)像素的图像记录于存储器中。姿势变化量计算部24如果将例如辉度信息收纳于20像素以上且误差1％以下，则判断为在前后帧间取得对应关系的特征点。
这样，从在后面的时刻取得的图像38’也检测到计算出相对位置(Xi、Yi、 Zi)的特征点Te1、Te2、Te3时，姿势变化量计算部24可以基于路面上的多个特征点的时间变化，计算出“车辆的姿势变化量”。
自己位置计算部26根据由姿势变化量计算部24计算出的“距离及姿势角的变化量”计算出距离及姿势角。另外，根据由姿势变化量计算部24计算出的“车辆的移动量”计算出车辆的当前位置。
具体而言，在将由姿势角计算部22(参照图1)计算出的距离及姿势角设定为起点的情况下，对该起点(距离及姿势角)逐次加上(进行积分运算)由姿势变化量计算部24计算出的各个帧的距离及姿势角的变化量，由此，将距离及姿势角更新成最新的数值。另外，将由姿势角计算部22计算出距离及姿势角时的车辆位置设定为起点(车辆的初始位置)，且从该初始位置逐次加上(进行积分运算)车辆的移动量，由此，计算出车辆的当前位置。例如，通过设定与地图上的位置核对的起点(车辆的初始位置)，可以逐次计算出地图上的车辆的当前位置。
这样，如果可以继续检测在前后帧间取得对应关系的3点以上的特征点，则通过继续加上距离及姿势角的变化量的处理(积分运算)，不使用图案光32a，也可以将距离或姿势角继续更新成最新的数值。其中，在最初的信息处理循环中，也可以使用通过使用图案光32a计算出的距离及姿势角或规定的初始距离及初始姿势角。即，成为积分运算的起点的距离及姿势角也可以使用图案光32a计算出，或也可以使用规定的初始值。规定的初始距离及初始姿势角优选为至少考虑了车辆10的乘坐人员及搭载物的距离及姿势角。例如，在车辆10的点火开关为接通状态，且移动位置从停车场移动至另一位置时，只要投射图案光32a，且将根据图案光32a计算出的距离及姿势角用作规定的初始距离及初始姿势角即可。由此，可以求出不产生车辆10的转向或加减速引起的滚动运动或俯仰运动时的距离及姿势角。
此外，实施方式中，通过计算出距离及姿势角的变化量，且逐次加上距离及姿势角的变化量，将距离及姿势角更新成最新的数值。但是，也可以仅将摄像机12相对于路面31的姿势角设为其变化量而计算出并作为更新的对象。在该情况下，摄像机12相对于路面31的距离只要假定为一定即可。由此，也可以考虑姿势角的变化量，抑制移动量(ΔL)的推定误差，同时减轻ECU13的运算负荷，且提高运算速度。
检测状态判断部30判断特征点检测部23进行的特征点Te的检测状态是否不满足第一基准而是差的状态。例如，如隧道内的混凝土路面那样，在路面上沥青混合物的颗粒等的状况或凹凸较少的情况下，从图像可检测的特征点变少。在该情况下，在前后帧间取得对应关系的特征点的继续检测变得困难，距离及姿势角的更新精度降低。
因此，检测状态判断部30在例如计算出相对于摄像机12的相对位置的特征点中、从在后面的信息处理循环中取得的图像也可检测出的特征点的数量不是规定的阈值(例如，4个)以上的情况下，判断为多个特征点的检测状态不满足第一基准，而是差的状态。即，在不能检测前后帧间取得对应的4个以上的特征点的情况下，检测状态判断部30判断为特征点Te的检测状态不满足第一基准，而是差的状态。此外，如图6所示，为了求出距离及姿势角的变化量，需要在前后帧间取得对应的至少3个特征点。这是由于，特定平面G需要3个特征点。提高推定精度需要更多的特征点，因此，规定的阈值优选为4或5以上。
图案光控制部27基于检测状态判断部30的判断结果，控制投光器11进行的图案光32a的投射。在检测状态判断部30判断为多个特征点的检测状态满足第一基准而不是差的状态的情况下，图案光控制部27将图案光32a熄灭。另一方面，在检测状态判断部30判断为多个特征点的检测状态不满足第一基准而是差的状态的情况下，图案光控制部27投射图案光32a。具体地，图案光控制部27基于检测状态判断部30的判断结果，切换图案光投射实施标志的ON/OFF，并对投光器11发送图案光投射实施标志。投光器11基于图案光投射实施标志，在下面的信息处理循环中，将图案光32a进行投射或熄灭。
另外，检测状态判断部30判断特征点检测部23进行的特征点Te的检测状态是否不满足比第一基准更低的第二基准而是差的状态。例如，在不能检测前后帧间取得对应的3个以上的特征点的情况下，检测状态判断部30判断为特征点Te的检测状态不满足第二基准而是差的状态。
在检测状态判断部30判断为多个特征点的检测状态满足第二基准且不是差的状态的情况下，姿势角计算部22不能根据图案光32a的位置计算出距离及姿势角。另一方面，在检测状态判断部30判断为多个特征点的检测状态不满足第二基准而是差的状态的情况下，不能根据特征点Te特定平面G，因此，姿势角计算部22根据图案光32a的位置计算出距离及姿势角。
自己位置计算部26在检测状态判断部30判断为多个特征点的检测状态满足第二基准的情况下，维持积分运算的起点。另一方面，在检测状态判断部30判断为多个特征点的检测状态不满足第二基准而是差的状态的情况下，自己位置计算部26在相同的信息处理循环中，将姿势角计算部22(参照图1)计算出的距离及姿势角及此时的车辆位置设定成新的起点(车辆的姿势角及初始位置)，并从该起点起开始车辆的姿势变化量的加算。
此外，第一实施方式中，检测状态判断部30基于在前后帧间取得对应关系的特征点的数量，判断多个特征点的检测状态，但也可以基于从1个图像检测出的特征点的总数N，判断多个特征点的检测状态。即，在从1个图像检测出的特征点的总数N为规定的阈值(例如9)以下的情况下，也可以判断为多个特征点的检测状态差。只要考虑检测出的特征点中有未取得对应的特征点，且将规定的阈值(4)的3倍的数(12)设定成阈值即可。
计算状态判断部35判断姿势角计算部22计算距离及姿势角的计算状态是否不满足第三基准而是差的状态。例如，在图案光照射至路面31上的台阶的情况下，路面31上的台阶比沥青的凹凸大，因此，距离及姿势角的计算精度极低。在多个特征点的检测状态不满足第二基准而是差的状态，且距离及姿势角的计算状态不满足第三基准而是差的状态的情况下，就没有高精度地检测距离及姿势角及其变化量的手段。
于是，计算状态判断部35在例如由姿势角计算部22计算出的距离及姿势角的标准偏差比规定的阈值大的情况下，判断为由姿势角计算部22计算距离及姿势角的计算状态不满足第三基准而是差的状态。另外，在35个的光点中不能检测出的光点的数量低于3点的情况下，原则上不能求出路面31的平面方程式，因此，判断为由姿势角计算部22计算距离及姿势角的计算状态不满足第三基准而是差的状态。在使用最小二乘法求出平面方程式时，与各光点的距离误差的最大值的绝对值成为某个阈值(例如0.05m)以上的情况下，也可以判断为由姿势角计算部22计算距离及姿势角的计算状态不满足第三基准而是差的状态。
在检测状态判断部30判断为多个特征点的检测状态不满足第二基准而是差的状态，且计算状态判断部35判断为由姿势角计算部22计算距离及姿势角的计算状态不满足第三基准而是差的状态的情况下，自己位置计算部26使用之前的信息处理循环中的距离及姿势角及车辆的当前位置作为积分运算的起点。由此，可抑制车辆的移动量的计算误差。
(信息处理循环)
接着，作为根据由摄像机12取得的图像38推定车辆10的移动量的自己位置计算方法的一例，参照图7说明由ECU13重复执行的信息处理循环。图7的流程图所示的信息处理循环在车辆10的点火开关成为接通状态且自己位置计算装置启动的同时开始，并重复执行到自己位置计算装置停止。
图7的步骤S01中，投光器11将图案光32a投射或熄灭。具体而言，投光器11在上一次的信息处理循环的步骤S05，基于从图案光控制部27发送的图案光投射实施标志的ON/OFF，切换图案光32a的投光/熄灭。参照图8对图案光投射实施标志进行后述。
进入步骤S03，ECU13控制摄像机12，拍摄包含投射图案光32a的区域的车辆周围的路面31，而取得图像38。ECU13将由摄像机12取得的图像数据存储于存储器中。
此外，ECU13可自动控制摄像机12的光圈。也可以根据之前的信息处理循环中取得的图像38的平均辉度，对摄像机12的光圈进行反馈控制，以成为辉度值的最大值和最小值的中间值。另外，投射图案光32a的区域的辉度值较高，因此，也可以从除抽取了图案光32a的部分以外的区域，求出平均辉度值。
进入步骤S05，检测状态判断部30判断多个特征点的检测状态。图案光控制部27基于检测状态判断部30的判断结果切换图案光投射实施标志，并发送至投光器11。而且，姿势角计算部22基于检测状态判断部30的判断结果计算出或不计算出距离及姿势角。步骤S05的详情参照图8及图9进行后述。
进入步骤S07，ECU13根据图像38检测特征点，且抽取前后的信息处理循环之间取得对应的特征点，并根据特征点的图像上的位置(Ui、Vi)计算出距离及姿势角的变化量及车辆的移动量。
具体而言，首先，特征点检测部23从存储器读取由摄像机12取得的图像38，且从图像38检测路面31上的特征点，并将各特征点的图像上的位置(Ui、Vi)存储于存储器。姿势变化量计算部24从存储器读取各特征点的图像上的位置(Ui、Vi)，并根据距离及姿势角和特征点的图像上的位置(Ui、Vi)计算出特征点相对于摄像机12的相对位置(Xi、Yi、Zi)。此外，姿势变化量计算部24使用之前的信息处理循环的步骤S09中设定的起点(距离及 姿势角)。姿势变化量计算部24将特征点相对于摄像机12的相对位置(Xi、Yi、Zi)存储于存储器中。
而且，姿势变化量计算部24从存储器读取特征点的图像上的位置(Ui、Vi)和之前的信息处理循环的步骤S07中计算出的特征点的相对位置(Xi、Yi、Zi)。姿势变化量计算部24使用前后的信息处理循环之间取得对应的特征点的相对位置(Xi、Yi、Zi)及图像上的位置(Ui、Vi)，计算出距离及姿势角的变化量。另外，根据上一次的信息处理循环中的特征点的相对位置(Xi、Yi、Zi)和此次的信息处理循环中的特征点的相对位置(Xi、Yi、Zi)，计算出车辆的移动量。步骤S07中计算出的“距离及姿势角的变化量”及“车辆的移动量”在步骤S11的处理中使用。
进入步骤S09，ECU13根据多个特征点的检测状态及图案光的距离及姿势角的计算状态，设定积分运算的起点。详情参照图10进行后述。
进入步骤S11，自己位置计算部26根据步骤S09的处理中设定的积分运算的起点、及步骤S07的处理中计算出的“距离及姿势角的变化量”及“车辆的移动量”，计算出距离及姿势角、及车辆的当前位置。
这样，本实施方式的自己位置计算装置通过重复执行上述的一连串的信息处理循环并累计车辆10的移动量，可以计算出车辆10的当前位置。
参照图8的流程，说明图7的步骤S05的详细顺序。首先，在步骤S501中，检测状态判断部30判断是否检测有4个以上的前后帧间取得对应的特征点。在检测有4个以上特征点的情况下(S501中，是)，根据特征点的时间变化可计算出距离及姿势角的变化量。于是，进入步骤S505，图案光控制部27将图案光投射实施标志设定成OFF状态，且发送至投光器11。然后，不根据图案光32a的位置计算出距离及姿势角，并进入图7的步骤S07。
另一方面，在未检测4个以上特征点的情况下(S501中，否)，判断为多个特征点的检测状态不满足第一基准而是差的状态，因此，进入步骤S503。而且，图案光控制部27将图案光投射实施标志设定成ON状态，并发送至投光器11。然后，进入步骤S507。
步骤S507中，检测状态判断部30判断是否检测有3个以上的前后帧间取得对应的特征点。在检测有3个以上特征点的情况下(S507中，是)，根据特征点的时间变化可计算出距离及姿势角的变化量。因此，不根据图案光32a的位置计算出距离及姿势角，进入图7的步骤S07。
另一方面，在未检测有3个以上特征点的情况下(S507中，否)，不能根据特征点的时间变化计算出距离及姿势角的变化量。因此，进入步骤S509，姿势角计算部22根据图案光32a的位置计算出距离及姿势角。然后，进入图7的步骤S07。
步骤S509中，首先，图案光抽取部21从存储器读取由摄像机12取得的图像38，如图4(c)所示，从图像38抽取图案光32a的位置。图案光抽取部21将作为表示图案光32a的位置的数据计算出的光点Sp的图像上的坐标(Uj、Vj)存储于存储器中。
姿势角计算部22从存储器读取表示图案光32a的位置的数据，根据图案光32a的位置计算出距离及姿势角，并存储于存储器中。
参照图9说明按照图8的流程执行的信息处理的一例。图9(a)表示姿势角计算部22是否根据图案光32a计算出距离及姿势角，图9(b)表示图案光投射实施标志的状态，图9(c)表示取得对应的特征点的数量。图9(a)～(c)的横轴表示时间的经过，t0、t1、t2、……表示信息处理循环(以后，均简称为“循环”)。
循环t0～t3中，取得对应的特征点的数量为4个以上，因此，图案光投射实施标志为OFF状态，“三维测量实施标志”也为OFF状态。如果“三维测量实施标志”为OFF状态，则姿势角计算部22不根据图案光32a计算出距离及姿势角。循环t4～t6中，取得对应的特征点的数量成为3个时，图8的步骤S501判断为否，步骤S503中图案光投射实施标志成为ON状态，但在步骤S507中判断为是，因此，三维测量实施标志仍为OFF状态。循环t7～t10中取得对应的特征点的数量成为4个以上时，图案光投射实施标志成为OFF状态。循环t12～t14中，取得对应的特征点的数量成为低于3个，因此，步骤S501及S507中均判断为否，因此，图案光投射实施标志及三维测量实施标志成为ON状态，实施利用姿势角计算部22进行距离及姿势角的计算。
参照图10的流程说明图7的步骤S09的详细顺序。步骤S900中，ECU13判断此次的信息处理循环是否为初次。而且，在为初次的情况下，即在没有上一次的信息处理循环的数据的情况下，将处理进入步骤S905，在不是初次的情况下，将处理进入步骤S901。
步骤S901中，检测状态判断部30判断由特征点检测部23检测的特征点Te的检测状态是否不满足第二基准而是差的状态。即，如与图8的步骤S507 相同，判断取得对应的特征点Te的数是否为3个以上。在判断为不是3个以上的情况下(步骤S901中为是)，进入步骤S903，在判断为3个以上的情况下(步骤S901中为否)，进入步骤S909。
步骤S909中，ECU13维持当前设定的积分运算的起点。
步骤S903中，计算状态判断部35判断由姿势角计算部22计算距离及姿势角的计算状态是否不满足第三基准而是差的状态。例如在相同的信息处理循环的步骤S509中判断距离及姿势角的计算是否成功。在判断为成功的情况下(步骤S903中为是)，进入步骤S905，在判断为不成功的情况下(步骤S903中为否)，进入步骤S907。
步骤S905中，ECU13将步骤S509中由姿势角计算部22计算出的距离及姿势角、及此时的车辆的当前位置设定为积分运算的起点。以该距离及姿势角为起点开始新的积分运算。另外，以车辆的当前位置为起点开始新的积分运算。
步骤S907中，ECU13将上一次的信息处理循环中采用的距离及姿势角、及上一次的信息处理循环中的车辆的当前位置设定为积分运算的起点。以该距离及姿势角为起点开始新的积分运算。另外，以车辆的当前位置为起点开始新的积分运算。然后，使处理进入图8的步骤S11。
如以上说明，根据第一实施方式，可得到以下的作用效果。
当多个特征点的检测状态差时，由姿势变化量计算部24计算出的距离及姿势角的计算精度降低，因此，车辆10的移动量的推定误差变大。图11(a)及图11(b)是表示车辆10的侧滚角(姿势角的一例)及移动量(车宽方向)的推定误差的例子的图，表示车辆10在无倾斜的平坦的路面上以一定的速度直行时的、侧滚角的推定值及移动量的计算值的时间变化。图中的“P1”及“P2”表示不管多个特征点的检测状态如何，均维持移动量计算的起点及距离及姿势角的积分运算的起点的比较例。“Q1”及“Q2”表示真实值。车辆10进行直行，因此，侧滚角及车宽方向的移动量的真实值(Q1、Q2)为保持为0的状态。但是，在时刻t1～t2之间，由于在多个特征点的检测状态差的混凝土路面行驶，因此，路面的平面方程式中产生误差，侧滚角中出现推定误差。侧滚角的误差会作为车宽方向的移动量的误差反映。比较例中，根据多个特征点的检测状态，不能将移动量计算的起点及侧滚角的积分运算的起点重置至根据图案光32a计算出的距离及姿势角、及此时的车辆的当前位置。因此，车辆10的移动量的误差会扩大。
根据第一实施方式，在检测状态判断部30判断为多个特征点的检测状态不满足第一基准而是差的状态的情况下(步骤S501中为否)，图案光控制部27投射图案光32a。由此，可以根据图案光32a的位置计算出距离及姿势角。因此，以根据图案光32a计算出的误差较小的距离及姿势角为起点，可重新开始积分运算。因此，可以高精度且稳定地推定车辆10的当前位置。
另外，多个特征点的检测状态变差的路面包含例如成为特征点的对象的状况或凹凸较少的路面。状况或凹凸较少的路面的平坦性较高，因此，根据图案光32a计算出的距离及姿势角所包含的噪声成分较少。另一方面，在多个特征点的检测状态良好的路面31，由于沥青的凹凸，图案光32a的反射位置不稳定，因此，根据图案光32a计算出的距离及姿势角包含较多的噪声成分。根据第一实施方式，可以根据多个特征点的检测状态，适当地分为使用了特征点的距离及姿势角的更新和使用了图案光32a的距离及姿势角的计算，并高精度且稳定地求出距离及姿势角。
另外，由于路面31的照度的急剧变化，有时图像38的对比度降低，且多个特征点的检测状态变差。根据第一实施方式，通过直接判断多个特征点的检测状态，不需要额外设置照度计等传感器，可减少成本，且有助于装置的轻量化。
根据第一实施方式，在检测状态判断部30判断为多个特征点的检测状态满足第一基准而不是差的状态的情况下(步骤S501中为是)，图案光控制部27停止图案光32a的投射。仅在多个特征点的检测状态不满足第一基准而是差的状态的情况下可以投射图案光32a，因此，可以仅在需要的情况下投射图案光32a，并抑制投光所需要的耗电量。
图7的步骤S05中的图案光32a的投光及熄灭(步骤S503、S505)的判断在下面的信息处理循环的步骤S01中反映。因此，为了使用图案光32a计算出距离及姿势角，需要预先在之前的循环中将图案光投射实施标志切换成ON状态。于是，第一实施方式中，如果是多个特征点的检测状态不满足第一基准而是差的状态，则将图案光投射实施标志切换成ON状态(S05)。在其下面的循环中，投射图案光32a(S01)。而且，在相同的循环中，在多个特征点的检测状态不满足第二基准而是差的状态的情况下(S507中为否)，姿势角计算部22可以根据图案光32a的位置计算出距离及姿势角。这样，通过使用两个基准(第一基准，第二基准)判断多个特征点的检测状态，可以适当地控制图案光32a的投光时间及距离及姿势角的计算时间。
(第二实施方式)
第二实施方式中，说明根据从开始图案光32a的投射起的经过时间控制使用了图案光32a的距离及姿势角的计算时间的例子。第二实施方式的自己位置计算装置的硬件构成如图1所示，因此，省略说明。另外，由ECU13重复执行的信息处理循环除了步骤S05之外，也如图7所示那样，因此，省略说明。后述的第三及第四实施方式也同样。
参照图12说明第二实施方式的步骤S05的详细顺序。图12所示的流程中，代替图8的步骤S507，而实施步骤S511。其它步骤与图8相同，而省略说明。ECU13测量从开始图案光32a的投射起的经过时间。
具体而言，ECU13在步骤S503中测量从将图案光投射实施标志从OFF状态切换至ON状态起的经过时间。步骤S511中，ECU13判断从切换图案光投射实施标志起是否经过第一规定时间。如果未经过第一规定时间(S511中为否)，则不根据图案光32a的位置计算出距离及姿势角，而进入步骤S07。另一方面，如果经过第一规定时间(S511中为是)，则进入步骤S509，根据图案光32a的位置计算出距离及姿势角，然后，进入步骤S07。
此外，第一规定时间例如图13所示，可以设定两次的信息处理循环(D1)。即，将图案光投射实施标志从OFF状态切换至ON状态的信息处理循环(t11)及下一次的信息处理循环(t12)中，不计算出距离及姿势角(在S511中为否)，或者在下一次(第二次)的信息处理循环(t13)中，首次计算出距离及姿势角(S511中为是，S509)。
如之前叙述，为了使用图案光32a计算出距离及姿势角，需要预先在之前的循环中将图案光投射实施标志切换成ON状态。第二实施方式中，图案光控制部27从开始图案光32a的投射起经过第一规定时间(D1)之后，姿势角计算部22开始根据图案光32a的位置计算出距离及姿势角。由此，可以适当地控制图案光32a的投光时间及距离及姿势角的计算时间。
(第三实施方式)
第三实施方式中，说明如下例子，即，在开始图案光32a的投射后，即使判断为多个特征点的检测状态满足第一基准且不是差的状态，也将图案光32a的投射从该判断起继续到经过第二规定时间为止。
在检测状态判断部30判断为多个特征点的检测状态是不比第一基准差的情况下，图案光控制部27在从检测状态判断部30判断起经过第二规定时间之后，停止图案光32a的投射。
参照图14说明第三实施方式的步骤S05的详细顺序。相对于图8，图14所示的流程还具备步骤S513及步骤S515。其它步骤与图8相同而省略说明。
ECU13测量从判断为取得对应的特征点的数量为4个以上起的经过时间。在判断为取得对应的特征点的数量为4个以上的情况下(S501中为是)，进入步骤S513，ECU13判断为取得对应的特征点的数量为4个以上后，判断是否经过第二规定时间。如果经过第二规定时间(S513中为是)，则进入步骤S505，图案光控制部27将图案光投射实施标志从ON状态向OFF状态切换。另一方面，如果未经过第二规定时间(S513中为否)，则进入步骤S515，图案光控制部27将图案光投射实施标志维持成ON状态。
此外，第二规定时间例如图15所示，可以设定两次的信息处理循环(D2)。即，取得对应的特征点的数量成为4个以上的信息处理循环(t6、t14)及下一次的信息处理循环(t7、t15)中将图案光投射实施标志维持成ON状态(S513中为否，S515)，且在下一次(第二次)的信息处理循环(t8、t16)中，首次将图案光投射实施标志切换成OFF状态(S513中为是，S505)。
由此，如果暂时开始图案光32a的投射，则之后即使改善多个特征点的检测状态，图案光32a也不立即熄灭。即，图案光32a的投光状态中可以具有滞后(滞后效应)，因此，可以抑制图案光32a的闪烁，且减轻闪烁引起的烦恼。
(第四实施方式)
第四实施方式中，与第三实施方式对称地说明图案光32a的熄灭状态中具有滞后的例子。在使图案光32a熄灭后，即使多个特征点的检测状态不满足第一基准而变差，从该判断起图案光控制部27也将图案光32a的熄灭继续到经过第三规定时间。在检测状态判断部30判断为多个特征点的检测状态不满足第一基准而是差的状态的情况下，图案光控制部27在从检测状态判断部30判断起经过第三规定时间之后，开始图案光32a的投光(投射)。
参照图16说明与第四实施方式相关的步骤S05的详细顺序。相对于图8，图16所示的流程还具备步骤S517。另外，步骤S501中，判断取得对应的特征点的数量是否为5个以上。即，在取得对应的特征点的数量不是5个以上 的情况下，检测状态判断部30判断为多个特征点的检测状态不满足第一基准而是差的状态。其它步骤与图8相同并省略说明。
ECU13测量从判断为取得对应的特征点的数量不是5个以上起的经过时间。在判断为取得对应的特征点的数量不是5个以上的情况下(S501中为否)，进入步骤S517，ECU13判断为取得对应的特征点的数量不是5个以上后，判断是否经过第三规定时间。如果经过第三规定时间(S517中为是)，则进入步骤S503，图案光控制部27将图案光投射实施标志切换成ON状态。另一方面，如果未经过第三规定时间(S517中为否)，则进入步骤S505，图案光控制部27将图案光投射实施标志维持成OFF状态。
此外，第三规定时间可以如例如图17所示设定两次的信息处理循环(D3)。即，取得对应的特征点的数量低于5个的信息处理循环(t2、t9)及下一次的信息处理循环(t3、t10)中，将图案光投射实施标志维持成OFF状态(S517中为否，S505)，且在下一次(第二次)的信息处理循环(t4、t11)中，首次将图案光投射实施标志切换成ON状态(S517中为是，S503)。
由此，如果暂时停止图案光32a的投射，则之后，即使多个特征点的检测状态恶化，也立即不开始图案光32a的投射。即，图案光32a的熄灭状态中可以具有滞后(滞后效应)，因此，可以抑制图案光32a的闪烁，并减少闪烁产生的烦恼。
如上所述，记载了本发明的第一～第四实施方式，但不应理解为形成该公开的一部分的论述及附图限定于本发明。根据本公开内容，对于本领域技术人员来说，可知有各种代替实施方式、实施例及运用技术。
图2表示将摄像机12和投光器11安装于车辆10的前面的例子，但也可以朝向车辆10的侧方、后方、正下方设置。另外，本实施方式中，作为车辆10的一例，图2中表示了四轮的乘用车，但可以适用于摩托车、货车、或例如搬运工程机械的特殊车辆等、可拍摄道路的路面或壁面上的特征点的全部移动体(车辆)。
符号说明
10 车辆
11 投光器
12 摄像机(摄像部)
21 图案光抽取部
22 姿势角计算部
23 特征点检测部
24 姿势变化量计算部
26 自己位置计算部
27 图案光控制部
30 检测状态判断部
31 路面
32a、32b 图案光
35 计算状态判断部
Te 特征点

Claims (7)

1.一种自己位置计算装置，其特征在于，具备：
投光器，其向车辆周围的路面投射图案光；
摄像部，其搭载于所述车辆，拍摄包含投射图案光的区域在内的车辆周围的路面并取得图像；
姿势角计算部，其根据由所述摄像部取得的图像中的所述图案光的位置，计算出车辆相对于所述路面的姿势角；
姿势变化量计算部，其基于根据由所述摄像部取得的图像检测出的所述路面上的多个特征点的时间变化，计算出所述车辆的姿势变化量；
自己位置计算部，其通过在所述车辆的初始位置及姿势角加上所述姿势变化量，计算出所述车辆的当前位置及姿势角；
检测状态判断部，其判断所述姿势变化量计算部中的所述多个特征点的检测状态；
图案光控制部，其控制所述投光器进行的图案光的投射，
在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下，所述图案光控制部投射图案光。
2.如权利要求1所述的自己位置计算装置，其特征在于，
在所述检测状态判断部判断为所述多个特征点的检测状态满足第一基准的情况下，所述图案光控制部停止图案光的投射。
3.如权利要求1或2所述的自己位置计算装置，其特征在于，
在所述检测状态判断部判断为所述多个特征点的检测状态不满足比所述第一基准低的第二基准的情况下，所述姿势角计算部根据所述图案光的位置计算出车辆相对于所述路面的姿势角。
4.如权利要求1或2所述的自己位置计算装置，其特征在于，
从所述图案光控制部开始图案光的投射起，经过第一规定时间之后，所述姿势角计算部根据所述图案光的位置开始计算出车辆相对于所述路面的姿势角。
5.如权利要求1或2所述的自己位置计算装置，其特征在于，
在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下，所述图案光控制部在从所述检测状态判断部进行判断起经过第二规定时间之后，停止图案光的投射。
6.如权利要求1或2所述的自己位置计算装置，其特征在于，
在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下，所述图案光控制部在从所述检测状态判断部进行判断起经过第三规定时间之后，开始图案光的投射。
7.一种自己位置计算方法，
从搭载于车辆的投光器向车辆周围的路面投射图案光，
利用搭载于所述车辆的摄像部，拍摄包含投射所述图案光的区域在内的车辆周围的路面，并取得图像，
所述车辆的控制部根据所述图像中的所述图案光的位置，计算出车辆相对于所述路面的姿势角，
所述控制部基于根据所述图像检测出的所述路面上的多个特征点的时间变化，计算出所述车辆的姿势变化量，
所述控制部通过在所述车辆的初始位置及姿势角加上所述姿势变化量，计算出所述车辆的当前位置及姿势角，
其特征在于，
所述控制部在判断为所述多个特征点的检测状态不满足第一基准的情况下，从所述投光器投射所述图案光。
CN201480076204.1A 2014-02-24 2014-02-24 自己位置计算装置及自己位置计算方法 Active CN106062849B (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2014/054311 WO2015125297A1 (ja) 2014-02-24 2014-02-24 自己位置算出装置及び自己位置算出方法

Publications (2)

Publication Number Publication Date
CN106062849A CN106062849A (zh) 2016-10-26
CN106062849B true CN106062849B (zh) 2017-07-04

Family

ID=53877830

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201480076204.1A Active CN106062849B (zh) 2014-02-24 2014-02-24 自己位置计算装置及自己位置计算方法

Country Status (8)

Country Link
US (1) US10043080B2 (zh)
EP (1) EP3113148B1 (zh)
JP (1) JP6237875B2 (zh)
CN (1) CN106062849B (zh)
BR (1) BR112016019519B1 (zh)
MX (1) MX349228B (zh)
RU (1) RU2628035C1 (zh)
WO (1) WO2015125297A1 (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3113147B1 (en) * 2014-02-24 2018-11-28 Nissan Motor Co., Ltd Self-location calculating device and self-location calculating method
JP6299319B2 (ja) * 2014-03-25 2018-03-28 日産自動車株式会社 自己位置算出装置及び自己位置算出方法
JP6601352B2 (ja) * 2016-09-15 2019-11-06 株式会社デンソー 車両姿勢推定装置
CN108115671B (zh) * 2016-11-26 2021-04-20 沈阳新松机器人自动化股份有限公司 基于3d视觉传感器的双臂机器人控制方法及***
US11181368B2 (en) * 2018-08-20 2021-11-23 Simmonds Precision Products, Inc. Visibility range sensing
EP3623180B1 (en) * 2018-09-11 2021-05-05 Connaught Electronics Ltd. A method and system for determining a state of a trailer being towed by a vehicle
DE112019005747T5 (de) * 2018-11-15 2021-08-19 Panasonic Intellectual Property Management Co., Ltd. Kamerasystem und Fahrzeug
CN109947886B (zh) * 2019-03-19 2023-01-10 腾讯科技（深圳）有限公司 图像处理方法、装置、电子设备及存储介质
DE102019123647A1 (de) * 2019-09-04 2021-03-04 Still Gesellschaft Mit Beschränkter Haftung Verfahren zur Lokalisierung eines Fahrzeugs sowie Fahrzeug zur Durchführung des Verfahrens
CN111678488B (zh) * 2020-06-17 2022-04-15 深圳地平线机器人科技有限公司 测距方法、装置、计算机可读存储介质及电子设备

Family Cites Families (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5155775A (en) * 1988-10-13 1992-10-13 Brown C David Structured illumination autonomous machine vision system
JP2901112B2 (ja) * 1991-09-19 1999-06-07 矢崎総業株式会社 車両周辺監視装置
JPH06325298A (ja) 1993-05-13 1994-11-25 Yazaki Corp 車両周辺監視装置
RU2247921C2 (ru) * 2002-06-26 2005-03-10 Анцыгин Александр Витальевич Способ ориентирования на местности и устройство для его осуществления
JP2004198211A (ja) 2002-12-18 2004-07-15 Aisin Seiki Co Ltd 移動体周辺監視装置
JP2004198212A (ja) 2002-12-18 2004-07-15 Aisin Seiki Co Ltd 移動体周辺監視装置
CN101163940B (zh) * 2005-04-25 2013-07-24 株式会社吉奥技术研究所 摄影位置分析方法
CN101288105B (zh) 2005-10-11 2016-05-25 苹果公司 用于物体重现的方法和***
JP5130638B2 (ja) * 2006-03-22 2013-01-30 日産自動車株式会社 回避操作算出装置、回避制御装置、各装置を備える車両、回避操作算出方法および回避制御方法
JP5151050B2 (ja) * 2006-03-23 2013-02-27 日産自動車株式会社 車両用環境認識装置及び車両用環境認識方法
JP4914726B2 (ja) 2007-01-19 2012-04-11 クラリオン株式会社 現在位置算出装置、現在位置算出方法
JP5251419B2 (ja) 2008-10-22 2013-07-31 日産自動車株式会社 距離計測装置および距離計測方法
US9014848B2 (en) 2010-05-20 2015-04-21 Irobot Corporation Mobile robot system
US8692980B2 (en) * 2010-11-01 2014-04-08 Advanced Scientific Concepts, Inc. Flash LADAR collision avoidance system
EP2656156B1 (en) * 2010-12-22 2017-09-06 Philips Lighting Holding B.V. Vehicle positioning and guidance system
EP2722646B1 (en) 2011-06-14 2021-06-09 Nissan Motor Co., Ltd. Distance measurement device and environment map generation apparatus
JP2013147114A (ja) * 2012-01-18 2013-08-01 Toyota Motor Corp 周辺環境取得装置およびサスペンション制御装置
JP5992184B2 (ja) 2012-03-09 2016-09-14 株式会社トプコン 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
JP5857224B2 (ja) * 2012-03-30 2016-02-10 パナソニックＩｐマネジメント株式会社 駐車支援装置、及び駐車支援方法
US9446652B2 (en) * 2012-08-02 2016-09-20 Toyota Jidosha Kabushiki Kaisha Road surface state obtaining device and suspension system

Also Published As

Publication number Publication date
BR112016019519B1 (pt) 2022-07-26
MX2016010743A (es) 2016-10-26
CN106062849A (zh) 2016-10-26
EP3113148A4 (en) 2017-03-29
EP3113148B1 (en) 2019-01-09
US20170011268A1 (en) 2017-01-12
BR112016019519A2 (zh) 2017-08-15
EP3113148A1 (en) 2017-01-04
US10043080B2 (en) 2018-08-07
JPWO2015125297A1 (ja) 2017-03-30
WO2015125297A1 (ja) 2015-08-27
JP6237875B2 (ja) 2017-11-29
RU2628035C1 (ru) 2017-08-14
MX349228B (es) 2017-07-19

Similar Documents

Publication Publication Date Title
CN106062849B (zh) 自己位置计算装置及自己位置计算方法
CN105981086B (zh) 自身位置计算装置以及自身位置计算方法
JP6237876B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6187671B2 (ja) 自己位置算出装置及び自己位置算出方法
CN106030680B (zh) 自身位置计算装置以及自身位置计算方法
JP6176387B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6398218B2 (ja) 自己位置算出装置及び自己位置算出方法
JP2016183890A (ja) 自己位置算出装置及び自己位置算出方法
JP6369897B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6299319B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6492974B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6369898B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6459701B2 (ja) 自己位置算出装置及び自己位置算出方法
JP6398219B2 (ja) 自己位置算出装置及び自己位置算出方法
JP2024031615A (ja) 路面推定システム及びデータ処理装置
CN116092050A (zh) 机动车障碍物检测方法、装置及计算机可读存储介质
JP2018170663A (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