CN114729811A - 位置推断装置、车辆、位置推断方法及位置推断程序 - Google Patents

Abstract

本发明是具有拍摄周围实景的n个相机(20a～20d)的移动体的位置推断装置，其具有：推断部(13)，对n个相机(20a～20d)中的每一个，基于从相机图像中提取的实景中的特征点在相机图像中的位置和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的相机的候选位置；验证部(14)，以该候选位置为基准，将在地图数据中与地图空间中的位置建立关联地存储的实景中的特征点群向n个相机(20a～20d)各自的相机图像投影，并基于投影到n个相机(20a～20d)各自的相机图像中的特征点群与从n个相机(20a～20d)各自的相机图像中提取到的特征点群的一致度，计算n个相机各自的候选位置的准确度。

Description

位置推断装置、车辆、位置推断方法及位置推断程序

技术领域

本发明涉及位置推断装置、车辆、位置推断方法及位置推断程序。

背景技术

以往，已知有一种位置推断装置(也称为“自身位置推断装置”)，其搭载于车辆或机器人等移动体，使用该移动体具有的相机，推断该移动体的位置及姿势(例如，参照非专利文献1及非专利文献2)。

一般而言，在这种位置推断装置中，进行以下处理：参照事先制作的、存储有实景(是指移动体周围的可用相机拍摄的风景。以下相同)中存在的物体的特征点(也称为“地标”)的三维位置的地图数据，使映射到相机图像中的特征点与地图数据中的特征点建立对应关系，由此，推断相机的位置及姿势(即，移动体的位置及姿势)。

现有技术文献

非专利文献

非专利文献1：Mikael Persson et al.“Lambda Twist：An Accurate FastRobust Perspective Three Point(P3P)Solver.”，ECCV 2018，pp 334-349，published in2018，http：//openaccess.thecvf.com/content_ECCV_2018/papers/Mikael_Persson_Lambda_Twist_An_ECCV_2018_paper.pdf

非专利文献2：Gim Hee Lee et al.“Minimal Solutions for Pose Estimationof a Multi-Camera System”，Robotics Research pp 521-538，published in2016，https：//inf.ethz.ch/personal/pomarc/pubs/LeeISRR13.pdf

发明内容

本发明的目的在于，提供能够以较小的运算负荷提高移动体的位置及姿势的推断精度的位置推断装置、车辆、位置推断方法及位置推断程序。

解决问题的方案

本发明的一个形态是一种位置推断装置，是具有对周围的实景进行拍摄的n个相机的移动体的位置推断装置，其中，n为2以上的整数，该位置推断装置具备：

推断部，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

验证部，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

所述推断部针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

所述验证部计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

另外，在另一方面，本发明是具备所述位置推断装置的车辆。

另外，在另一方面，本发明是一种位置推断方法，是具有对周围的实景进行拍摄的n个相机的移动体的位置推断方法，其中，n为2以上的整数，该位置推断方法包括以下处理：

第一处理，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

第二处理，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

在所述第一处理中，针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

在所述第二处理中，计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

另外，在另一方面，本发明是一种位置推断程序，是使计算机推断具有对周围的实景进行拍摄的n个相机的移动体的位置的位置推断程序，其中，n为2以上的整数，该位置推断程序包括以下处理：

第一处理，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

第二处理，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

在所述第一处理中，针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

在所述第二处理中，计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

附图说明

图1是表示一个实施方式的车辆的结构的一例的图，

图2是表示一个实施方式的车辆所搭载的四个相机的搭载位置的一例的图，

图3是表示一个实施方式的位置推断装置的硬件结构的一例的图，

图4是表示一个实施方式的位置推断装置中预先存储的地图数据的一例的图，

图5是表示一个实施方式的位置推断装置的结构的一例的图，

图6是表示一个实施方式的第一特征点提取部提取到的特征点的一例的图，

图7是对一个实施方式的第一推断部的处理进行说明的图，

图8是对一个实施方式的第一验证部的处理进行说明的图，

图9是对一个实施方式的第一验证部的处理进行说明的图，

图10是表示一个实施方式的位置推断装置的动作的一例的流程图，

图11是示意性地表示图10的步骤Sa、Sb的循环处理的图，

图12是表示变形例的位置推断装置的动作的一例的流程图。

具体实施方式

以往，在这种位置推断装置中，如非专利文献1那样，采用以下方法：从映射到单一的相机的相机图像中的多个特征点中提取3个特征点，基于该3个特征点在相机图像的拍摄面内的位置、和地图数据中存储的该3个的特征点的三维位置，计算相机的候选位置及候选姿势。在这样的方法中，一边改变从相机图像提取的特征点一边进行反复运算，从而计算出相机的位置及姿势的最优解(也称为RANSAC(Random Sample Consensus，随机抽样一致算法))。

在能够以比较小的运算负荷推断移动体的位置及姿势这一点上，这样的现有技术是有用的。但是，这样的现有技术中，存在以下问题：由于遮挡(是指如下状态，即，位于跟前的物体遮蔽位于背后的物体而使其不能被看到的状态)等的影响，使得映射到相机图像中的特征点的分布与存储于地图数据的特征点的分布相差较大的情况下，推断精度变差。

从这样的背景考虑，例如，研究了如非专利文献2那样，使用多个相机来提高针对遮挡的鲁棒性的方法。然而，在这样的方法中，通常，需要同时进行多个相机各自的相机图像中的3D-2D的几何运算，因此运算量庞大(例如，需要求解8次多项式)。此外，在这样运算量庞大的情况下，尤其是在车载环境等运算性能有限的环境中，位置推断的运算跟不上移动体的移动速度，实质上的推断精度变差。

根据本发明的位置推断装置，能够实现已解决了上述问题的、移动体的位置推断及姿势推断。

此外，下面，为了方便说明，将包含相机或移动体的“位置”及“姿势(即、朝向)”这两个概念的概念称为“位置”。

下面，参照附图对本发明的优选的实施方式详细地进行说明。此外，在本说明书及附图中，对具有实质上相同的功能的构成要素，赋予相同的附图标记，省略重复的说明。

[车辆的结构]

下面，参照图1～图4对一个实施方式的位置推断装置的概要结构的一例进行说明。本实施方式的位置推断装置搭载于车辆，对车辆的位置进行推断。此外，下面，为了方便说明，将包含相机或移动体的“位置”及“姿势(即、朝向)”这两个概念的概念称为“位置”。

图1是表示本实施方式的车辆A的结构的一例的图。图2是表示本实施方式的车辆A所搭载的四个相机20a、20b、20c、20d的搭载位置的一例的图。

车辆A具备：位置推断装置10、四个相机20a、20b、20c、20d(以下，也称为“第一相机20a”、“第二相机20b”、“第三相机20c”及“第四相机20d”)、车辆ECU30以及车辆驱动装置40。

第一相机至第四相机20a～20d例如是拍摄车辆A周围的实景的一般的可见光相机，对自身具有的摄像元件所产生的图像信号进行AD转换(模拟/数字转换)，产生相机图像的图像数据(以下，称为“相机图像数据”)D1、D2、D3、D4。其中，相机图像数据D1、D2、D3、D4在时刻上同步。而且，第一相机至第四相机20a～20d将自身所产生的相机图像数据向位置推断装置10输出。此外，第一相机至第四相机20a～20d例如构成为连续地执行拍摄，并能够以动态图像格式产生相机图像数据。

第一相机至第四相机20a～20d以拍摄互不相同的区域的方式配置。具体地，第一相机20a配置于车辆A的前侧的面即前面，拍摄车辆A的前方区域。第二相机20b配置于车辆A的右侧后视镜，拍摄车辆A的右方区域。

第三相机20c配置于车辆A的后侧的面即后面，拍摄车辆A的后方区域。

第四相机20d配置于车辆A的左侧后视镜，拍摄车辆A的左方区域。

位置推断装置10基于第一相机至第四相机20a～20d的相机图像数据，推断车辆A的位置(例如，世界坐标系中的车辆A的三维位置、以及车辆A的朝向)。而且，位置推断装置10将车辆A的位置的信息向车辆ECU30发送。

图3是表示本实施方式的位置推断装置10的硬件结构的一例的图。

图4是表示本实施方式的位置推断装置10中预先存储的地图数据Dm的一例的图。此外，图4中，将地图数据Dm中存储的实景中的多个特征点Q的地图空间中的位置用俯瞰图表示。

位置推断装置10是具备CPU(Central Processing Unit，中央处理单元)101、ROM(Read Only Memory，只读存储器)102、RAM(Random Access Memory，随机存取存储器)103、外部存储装置(例如，闪存)104、及通信接口105等作为主要组件的计算机。

例如，CPU101通过参照ROM102、RAM103及外部存储装置104等中存储的控制程序(例如，位置推断程序Dp)、各种数据(例如，地图数据Dm、相机搭载位置数据Dt)来实现位置推断装置10的后述的各功能。

在位置推断装置10的外部存储装置104中，除了保存有执行后述的车辆A的位置推断的位置推断程序Dp以外，还保存有地图数据Dm及相机搭载位置数据Dt。

地图数据Dm中，针对在广域(包含车辆A周围的区域)得到的实景中的多个特征点中的每一个，预先将地图空间中的三维位置与从制作地图数据Dm时拍摄到的相机图像中得到的特征量建立关联地存储。作为地图数据Dm而被存储的特征点，例如是在实景中可成为目标的物体(例如，建筑物、标识、或广告牌等)的、能够从其相机图像得到特征性的图像的模式的部分(例如，拐角部)。另外，作为实景中的特征点，也可以使用预先设置的标记的特征点。此外，例如以利用识别编号可单独识别的方式存储地图数据Dm的多个特征点。

地图数据Dm中存储的特征点在地图空间(是指地图数据Dm的由三维坐标系表达的空间。以下相同)中的三维位置由三维的正交坐标系(X、Y、Z)表示。此外，例如也可以使这些(X、Y、Z)与纬度、经度、高度等实际空间中的坐标建立对应关系。通过这样做，使得地图空间与实际空间具有相同含义。此外，特征点在地图空间中的三维位置例如是通过使用了事先在多个位置拍摄到的相机图像的测定(例如，利用三角测量法的原理的测定)、使用了LIDAR(Light Detection and Ranging，激光探测与测量)的测定、或使用了立体相机的测定而得到的位置。

作为地图数据Dm中存储的特征点的特征量，除了使用相机图像上的亮度、浓度以外，还使用SIFT(Scale Invariant Feature Transform，尺度不变特征转换)特征量、或SURF(Speeded Up Robust Features，加速稳健特征)特征量等。此外，地图数据Dm中存储的特征点的特征量数据可以是，即使是针对同一三维位置的特征点，也按拍摄该特征点时的相机的各个拍摄位置、拍摄方向而分别存储该特征点的特征量数据。另外，地图数据Dm中存储的特征点的特征量数据也可以与具有该特征点的物体的图像建立关联地存储。

在相机搭载位置数据Dt中，存储第一相机至第四相机20a～20d的相互间的位置关系(例如，相机间的距离上的关系、以及相机的朝向的相互关系)。即，第一相机至第四相机20a～20d各自的位置能够通过确定其中一个相机的位置来计算出。

另外，相机搭载位置数据Dt中，也存储有第一相机至第四相机20a～20d各自的位置与车辆A的规定位置(例如，重心点)之间的位置关系，以能够根据第一相机至第四相机20a～20d各自的位置确定车辆A。

车辆ECU(Electronic Control Unit，电子控制单元)30是进行车辆驱动装置40的控制的电子控制单元。车辆ECU30例如一边参照由位置推断装置10推断的车辆A的位置，一边对车辆驱动装置40的各部(例如，驱动马达的输出、离合器的接合及断开、自动变速器的变速级、以及转向装置的转向角)进行自动控制，以使得车辆A的行驶状态成为最佳。

车辆驱动装置40是使车辆A行驶的驱动部，其例如包括驱动马达、自动变速器、动力传递机构、制动机构和转向装置等而构成。此外，本实施方式的车辆驱动装置40通过车辆ECU30进行动作控制。

此外，位置推断装置10、第一相机至第四相机20a～20d、车辆ECU30及车辆驱动装置40经由车载网络(例如，依据CAN(控制器局域网总线，Controller Area Network)通信协议的通信网络)相互连接，并能够相互发送和接收所需的数据和控制信号。

[位置推断装置的详细结构]

接着，参照图5～图9对本实施方式的位置推断装置10的详细结构进行说明。

图5是表示本实施方式的位置推断装置10的结构的一例的图。

位置推断装置10具备获得部11、特征点提取部12、推断部13、验证部14和判定部15。

获得部11从搭载于车辆A的第一相机至第四相机20a～20d分别获得相机图像数据D1～D4。具体地，获得部11具有从第一相机20a获得相机图像数据D1的第一获得部11a、从第二相机20b获得相机图像数据D2的第二获得部11b、从第三相机20c获得相机图像数据D3的第三获得部11c和从第四相机20d获得相机图像数据D4的第四获得部11d。此外，第一获得部至第四获得部11a～11d获得的相机图像数据D1～D4是在同一时刻产生的数据。

特征点提取部12从相机图像数据D1～D4各自的相机图像中提取实景中的特征点。具体地，特征点提取部12具有从第一相机20a的相机图像中提取实景中的特征点的第一特征点提取部12a、从第二相机20b的相机图像中提取实景中的特征点的第二特征点提取部12b、从第三相机20c的相机图像中提取实景中的特征点的第三特征点提取部12c和从第四相机20d的相机图像中提取实景中的特征点的第四特征点提取部12d。此外，第一特征点提取部至第四特征点提取部12a～12d可以由分开而设置的四个处理器实现，也可以由一个处理器将处理时间进行时间分割来实现。

图6是表示本实施方式的第一特征点提取部12a所提取的特征点的一例的图。图6示出由第一相机20a产生的相机图像的一例，映射到该相机图像中的物体的拐角部等被作为特征点R提取。

第一特征点提取部至第四特征点提取部12a～12d从相机图像中提取特征点的方法可以是公知的任意的方法。第一特征点提取部至第四特征点提取部12a～12d例如使用SIFT法、Harris法、FAST(Features From Accelerated Segment Test，基于加速分割测试的特征)法、或已学习的CNN(Convolutional Neural Network，卷积神经网络)等从相机图像中提取特征点。

此外，从第一相机至第四相机20a～20d各自的相机图像中提取到的特征点的数据D1a～D4a例如包含特征点在相机图像中的二维坐标、以及该特征点的特征量信息。

推断部13计算第一相机至第四相机20a～20d各自所在的位置的候选。具体地，推断部13具有：基于第一相机20a的相机图像的特征点数据D1a及地图数据Dm计算第一相机20a的候选位置(以下，也称为“第一候选位置”)的第一推断部13a、基于第二相机20b的相机图像的特征点数据D2a及地图数据Dm计算第二相机20b的候选位置(以下，也称为“第二候选位置”)的第二推断部13b、基于第三相机20c的相机图像的特征点数据D3a及地图数据Dm计算第三相机20c的候选位置(以下，也称为“第三候选位置”)的第三推断部13c、和基于第四相机20d的相机图像的特征点数据D4a及地图数据Dm计算第四相机20d的候选位置(以下，也称为“第四候选位置”)的第四推断部13d。此外，推断部13也可以是，代替在与第一相机至第四相机20a～20d分别对应的第一推断部13a～第四推断部13d中计算各相机的候选位置，而构成为将推断部13的处理时间进行时间分割来计算各相机的候选位置。

图7是对本实施方式的第一推断部13a的处理进行说明的图。图7中的点R1、R2、R3表示从第一相机20a的相机图像中提取到的3个特征点，点Q1、Q2、Q3表示地图数据Dm中存储的特征点R1、R2、R3的地图空间中的三维位置。另外，点P1表示第一相机20a的候选位置。另外，RP1表示第一相机20a的拍摄面。

第一推断部13a首先使用模式匹配、特征量搜索等，将从第一相机20a的相机图像中提取到的特征点与地图数据Dm中存储的特征点进行对照。然后，第一推断部13a在如下的特征点中──即，从第一相机20a的相机图像中提取到的特征点中的、已完成了与地图数据Dm中存储的特征点的对照的特征点中──随机地选择数个(例如，3个～6个)特征点，并基于该数个特征点在相机图像内的位置(例如，图7的点R1、R2、R3)、和地图数据Dm中所存储的该数个特征点在地图空间中的三维位置(例如，图7的点Q1、Q2、Q3)，计算地图空间中的第一相机20a的第一候选位置。这时，第一推断部13a例如使用Lambda Twist等公知的方法(例如，参照非专利文献1)求解PnP问题，由此计算第一相机20a的第一候选位置。

此外，第一推断部13a也可以在进行从第一相机20a的相机图像中提取到的特征点与地图数据Dm所存储的特征点的对照时，例如，以根据GPS(Global Positioning System，全球定位系统)信号推断的车辆A的当前位置、或前一帧中计算出的车辆A的位置为基准，将地图数据Dm所存储的特征点中的、作为与从第一相机20a的相机图像中提取的特征点进行对照的对象的特征点的范围缩小。

优选地，将第一推断部13a计算第一相机20a的第一候选位置时使用的特征点的个数设为3个。由此，能够减轻计算第一候选位置时的运算负荷。

另外，优选地，第一推断部13a反复改变从第一相机20a的相机图像中提取到的全部特征点中的用于计算第一候选位置的特征点来计算多个第一候选位置，以计算出精度更高的第一候选位置。此外，在由第一推断部13a计算出了多个第一候选位置的情况下，由后述的第一验证部14a计算多个第一候选位置各自的准确度。

第二推断部13b、第三推断部13c及第四推断部13d分别通过与第一推断部13a同样的方法，计算第二相机20b的第二候选位置、第三相机20c的第三候选位置及第四相机20d的第四候选位置。

此外，例如，利用世界坐标系中的三维位置(X坐标、Y坐标、Z坐标)及该相机的拍摄方向(滚动、俯仰、偏航)表示第一相机至第四相机20a～20d各自的候选位置。

由第一推断部13a计算出的第一相机20a的第一候选位置的数据D1b被发送到第一验证部14a。另外，由第二推断部13b计算出的第二相机20b的第二候选位置的数据D2b被发送到第二验证部14b。另外，由第三推断部13c计算出的第三相机20c的第三候选位置的数据D3b被发送到第三验证部14c。另外，由第四推断部13d计算出的第四相机20d的第四候选位置的数据D4b被发送到第四验证部14d。

验证部14计算推断部13中计算出的第一相机至第四相机20a～20d各自的候选位置的准确度。具体地，验证部14具有：计算第一相机20a的第一候选位置的准确度的第一验证部14a、计算第二相机20b的第二候选位置的准确度的第二验证部14b、计算第三相机20c的第三候选位置的准确度的第三验证部14c、和计算第四相机20d的第四候选位置的准确度的第四验证部14d。此外，除了向第一验证部至第四验证部14a～14d输入候选位置的数据(D1b～D4b中的一个)，还输入从第一相机至第四相机20a～20d各自的相机图像中提取到的特征点的数据D1a、D2a、D3a、D4a、地图数据Dm、以及相机搭载位置数据Dt。此外，第一验证部至第四验证部13a～13d可以由单独设置的四个处理器实现，也可以由一个处理器将处理时间进行时间分割来实现。

图8、图9是对本实施方式的第一验证部14a的处理进行说明的图。

图9示出从第二相机20b的相机图像中提取到的特征点R、和将地图数据Dm中所存储的特征点投影到第二相机20b的相机图像而得的投影点R’的一例。

第一验证部14a以第一相机20a的第一候选位置为基准，向第一相机至第四相机20a～20d各自的相机图像，投影地图数据Dm中所存储的特征点群，基于已被投影到第一相机至第四相机20a～20d各自的相机图像中的该特征点群与从第一相机至第四相机20a～20d各自的相机图像中提取到的该特征点群的一致度，计算第一相机20a的第一候选位置的准确度。

第一验证部14a进行的处理的详细内容如下。

首先，第一验证部14a例如在假定第一相机20a存在于第一候选位置的情况下，根据相机搭载位置数据Dt中预先存储的第一相机20a与第二相机20b的位置关系，计算第二相机20b的虚拟位置(图8的点P2)。此外，例如，基于相机搭载位置数据Dt中预先存储的第一相机20a与第二相机20b的位置关系，相对于第一相机20a的第一候选位置进行旋转移动及平行移动的运算处理，由此计算出第二相机20b的虚拟位置。

接着，第一验证部14a例如以第二相机20b的虚拟位置为基准，将地图数据Dm中预先存储的特征点群的各特征点(图8的点Q4、Q5、Q6)投影到第二相机20b的相机图像(表示拍摄面。以下相同)(图8的PR2)中，并计算第二相机20b的相机图像内的该特征点的投影位置(图8的点R4’、R5’、R6’)。这时，第一验证部14a例如将地图数据Dm中预先存储的特征点群中的能够投影的全部的特征点投影到第二相机20b的相机图像中，并计算它们的投影位置。

接着，第一验证部14a将已被投影到第二相机20b的相机图像的地图数据Dm所存储的特征点(图8的点Q4、Q5、Q6)，与从第二相机20b的相机图像中提取的特征点(图8的点R4、R5、R6)进行对照。该对照处理与公知的方法相同，例如，使用特征量匹配处理等。

接着，第一验证部14a针对地图数据Dm中预先存储的特征点群中的已与从第二相机20b的相机图像中提取的特征点进行了对照的特征点，计算实际位置(图8的点R4、R5、R6的位置)与投影位置(图8的点R4’、R5’、R6’的位置)的重投影误差(即，投影位置与实际位置之间的距离)。图8中，点R4与点R4’之间的距离、点R5与点R5’之间的距离、以及点R6与点R6’之间的距离分别相当于重投影误差。

接着，第一验证部14a对地图数据Dm中预先存储的特征点群中的、与从第二相机20b的相机图像中提取的特征点之间的重投影误差为阈值以下的特征点(以下，称为“命中点”)的数量进行计数。即，第一验证部14a将命中点的个数作为如下的一致度来掌握，该一致度是已被投影到第二相机20b的相机图像的地图数据Dm中预先存储的特征点群与从第二相机20b的相机图像提取的特征点群的一致度。

图9中，从第二相机20b的相机图像中提取了15个特征点，但是，在第一验证部14a的处理中，例如，仅将15个特征点中的、与地图数据Dm中预先存储的特征点进行了对照且重投影误差为阈值以下的特征点作为命中点，对其个数进行计数。

而且，第一验证部14a通过同样的方法，除了从第二相机20b的相机图像提取命中点以外，还从第一相机20a的相机图像、第三相机20c的相机图像、以及第四相机20d的相机图像提取命中点，并对该命中点的数量进行计数。

即，第一验证部14a以第一相机20a的第一候选位置为基准，向第一相机20a的相机图像投影地图数据Dm中所存储的特征点群，对投影到第一相机20a的相机图像中的该特征点群中的、与从第一相机20a的相机图像中提取到的特征点之间的重投影误差为阈值以下的特征点的数量进行计数。另外，第一验证部14a以第一相机20a的第一候选位置为基准，向第三相机20c的相机图像投影地图数据Dm中所存储的特征点群，对投影到第三相机20c的相机图像中的该特征点群中的、与从第三相机20c的相机图像中提取到的特征点之间的重投影误差为阈值以下的特征点的数量进行计数。另外，第一验证部14a以第一相机20a的第一候选位置为基准，向第四相机20d的相机图像投影地图数据Dm中所存储的特征点群，对投影到第四相机20d的相机图像中的该特征点群中的、与从第四相机20d的相机图像中提取到的特征点之间的重投影误差为阈值以下的特征点的数量进行计数。

接着，第一验证部14a对从第一相机至第四相机20a～20d各自的相机图像中提取到的命中点的个数进行合计，将该合计数作为第一相机20a的第一候选位置的准确度。

第二验证部14b、第三验证部14c及第四验证部14d分别通过与第一验证部14a同样的方法，计算第二相机20b的第二候选位置的准确度、第三相机20c的第三候选位置的准确度、以及第四相机20d的第四候选位置的准确度。

判定部15获得表示由第一验证部14a计算出的第一候选位置的准确度的数据D1c、表示由第二验证部14b计算出的第二候选位置的准确度的数据D2c、表示由第三验证部14c计算出的第三候选位置的准确度的数据D3c、以及表示由第四验证部14d计算出的第四候选位置的准确度的数据D4c。而且，判定部15采用第一候选位置至第四候选位置中的准确度最大的候选位置作为最可信赖的位置。

进而，判定部15以第一候选位置至第四候选位置中的准确度最大的候选位置为基准，判断地图空间中的车辆A的位置。此外，这时，判定部15例如基于相机搭载位置数据Dt中预先存储的、车辆A的重心点与准确度最大的候选位置的相机的位置关系，来推断车辆A的位置。

此外，在构成为第一推断部至第四推断部13a～13d各自反复运算候选位置的情况下，为了规定反复运算的结束条件，在判定部15中，也可以设置准确度(即，命中点的个数)的阈值(参照后述的变形例)。

本实施方式的位置推断装置10通过这样的推断方法，即使是在第一相机至第四相机20a～20d中的某一个中，由于遮挡等的影响而出现了该相机的相机图像的特征点的分布与地图数据Dm中所存储的特征点的分布相差较大的状况的情况下，也能够高精度地推断车辆A的位置。

例如，在第一相机20a的相机图像由于遮挡的影响而与地图数据Dm相差较大的情况下，通常，如下的情形较多，即，在从第一相机20a的相机图像中提取的特征点中，只有距离第一相机20a较远处的特征点能与地图数据Dm中所存储的特征点对照。这样的特征点位置精度较低，在以这样的特征点为基准，推断第一相机20a的位置的情况下，第一相机20a的位置(即，车辆A的位置)的精度也会变差。

关于这一点，在本实施方式的位置推断装置10中，能够使用从第一相机至第四相机20a～20d分别提取的特征点中的、位置精度较高的适当的特征点，来推断车辆A的位置，其结果，能够使车辆A的位置推断的精度也提高。

[位置推断装置的动作]

图10是表示本实施方式的位置推断装置10的动作的一例的流程图。在此，示出本实施方式的位置推断装置10的各功能通过程序实现的形态。图11是示意性地表示图10的步骤Sa、Sb的循环处理的图。

步骤S101中，首先，位置推断装置10从第一相机至第四相机20a～20d各自的相机图像中提取特征点。

步骤S102中，位置推断装置10将从第i个相机(表示第一相机至第四相机20a～20d中一个相机。以下相同)的相机图像中提取的特征点(例如，3个)与地图数据Dm的特征点进行对照，基于这些，计算第i个相机的候选位置。

步骤S103中，位置推断装置10基于第i个相机的候选位置和相机搭载位置数据Dt，计算第一相机至第四相机20a～20d中的、除第i个相机以外的其他相机的虚拟位置。

步骤S104中，位置推断装置10向第一相机至第四相机20a～20d各自的相机图像，投影地图数据Dm中所存储的特征点群。而且，位置推断装置10将投影到第一相机至第四相机20a～20d各自的相机图像中的该特征点群的各特征点、与从第一相机至第四相机20a～20d各自的相机图像中提取到的特征点进行对照后，针对这些特征点群的各特征点计算重投影误差。

步骤S105中，位置推断装置10基于步骤S104中计算出的重投影误差，将在第一相机至第四相机20a～20d各自的相机图像中提取到的特征点中的重投影误差为阈值以下的特征点决定为命中点，对在第一相机至第四相机20a～20d各自的相机图像中提取到的命中点的合计数进行计数。

步骤S106中，位置推断装置10对步骤S105中计算出的命中点的合计数是否比当前保持的最有力候选位置的命中点的合计数大进行判定。而且，在步骤S105中计算出的命中点的合计数比当前保持的最有力候选位置的命中点的合计数大的情况下(S106：“是”)，使处理向步骤S107推进，在步骤S105中计算出的命中点的合计数为当前保持的最有力候选位置的命中点的合计数以下的情况下(S106：“否”)，返回到步骤S102，对下一个相机(第i+1个的相机)执行处理。

步骤S107中，位置推断装置10将步骤S102中计算出的候选位置设定为最有力候选位置后，返回到步骤S102，对下一个相机(第i+1个的相机)执行处理。

位置推断装置10在循环处理Sa及循环处理Sb中反复执行这样的步骤S102～步骤S107的处理。在此，循环处理Sb是用于在第一相机至第四相机20a～20d中切换作为处理对象的相机(即，作为计算候选位置，并计算该候选位置的准确度的对象的相机)的循环。另外，循环处理Sa是用于切换计算第一相机至第四相机20a～20d各自的候选位置时所使用的特征点的循环。此外，图10的流程图中变量i是表示第一相机至第四相机20a～20d中的作为处理对象的相机的变量(在此为，1～4的整数)，变量N是表示计算一个候选位置时所使用的特征点的切换次数的变量(在此为，1～N(N例如为50)的整数)。

具体地，如图11所示，位置推断装置10反复执行以下步骤：使用第一相机20a的相机图像计算第一相机20a的第一候选位置的步骤Sb1、使用第一相机至第四相机20a～20d的相机图像验证第一候选位置的准确度的步骤Sb2、使用第二相机20b的相机图像计算第二相机20b的第二候选位置的步骤Sb3、使用第一相机至第四相机20a～20d的相机图像验证第二候选位置的准确度的步骤Sb4、使用第三相机20c的相机图像计算第三相机20c的第三候选位置的步骤Sb5、使用第一相机至第四相机20a～20d的相机图像验证第三候选位置的准确度的步骤Sb6、使用第四相机20d的相机图像计算第四相机20d的第四候选位置的步骤Sb7、和使用第一相机至第四相机20a～20d的相机图像验证第四候选位置的准确度的步骤Sb8。

本实施方式的位置推断装置10通过以上那样的处理计算出位置精度最高的相机(在此为，第一相机至第四相机20a～20d中某一个)的候选位置。而且，位置推断装置10使用该相机的候选位置推断车辆A的位置。

[效果]

如上所述，本实施方式的位置推断装置10具备：

推断部13，基于从n个相机中第k个相机的相机图像中提取的实景中的特征点在相机图像中的位置、和在地图数据Dm中预先存储的该特征点在地图空间中的位置，计算地图空间中的第k个相机的候选位置，其中，n为2以上的整数，k为1～n的整数；以及

验证部14，以第k个相机的候选位置为基准，将在地图数据Dm中与地图空间中的位置建立关联地存储的实景中的特征点群，向n个相机各自的相机图像投影，并基于投影到n个相机各自的相机图像中的该特征点群与从n个相机各自的相机图像中提取到的该特征点群的一致度，计算第k个相机的所述候选位置的准确度，

推断部13针对n个相机中的第1个相机到第n个相机，分别计算候选位置，

验证部14计算n个相机中第1个相机到第n个相机各自的候选位置的准确度，

使用n个相机中第1个相机到第n个相机各自的候选位置的准确度中准确度最高的候选位置，推断移动体(例如，车辆A)的位置。

由此，即使是在移动体(例如，车辆A)具有的多个相机20a～20d中的某一个中由于遮挡等的影响而出现了该相机的相机图像与地图数据(即、在地图数据中所存储的特征点的分布)相差较大的状况的情况下，也能够高精度地推断该移动体的位置。

特别地，本实施方式的位置推断装置10在以下方面是有用的，即，使用多个相机而不必如非专利文献2那样进行复杂的运算，能够以较小的运算量高精度地推断移动体。由此，即使是在如车载环境那样运算量被限制且移动体的移动速度快时，也能够实时地推断该移动体的位置。

(变形例)

图12是表示变形例的位置推断装置的动作的一例的流程图。在步骤S107之后增加了步骤S108的处理这一点上，图12的流程图与图10的流程图不同。

上述实施方式构成为，为了搜索位置精度尽可能高的候选位置，将循环处理Sa执行一定以上的次数。但是，从缩短推断移动体(例如，车辆A)的位置的时间的观点考虑，优选地，使循环处理Sa的次数尽可能小。

本变形例的流程图中，从这样的观点考虑，在步骤S108，增加了对步骤S105中计算出的命中点的合计数(即、最有力候选的命中点的合计数)是否比阈值大进行判定的处理。而且，在步骤S105中计算出的命中点的合计数比阈值大的情况下(S108：“是”)，结束图12的流程图，在步骤S105中计算出的命中点的合计数为阈值以下的情况下(S108：“否”)，保持原样，继续进行循环处理Sa、Sb。

由此，能够在确保移动体的位置的推断精度的同时，尽可能地缩短到推断出该移动体的位置为止的运算时间。

(其他实施方式)

本发明不限于上述实施方式，可以考虑各种各样的变形形态。

例如，上述实施方式中，作为搭载于车辆A的相机的一例，示出了4个相机，但是，搭载于车辆A的相机的个数只要是2个以上即可，可任意设定。另外，相机的拍摄区域也可以为车辆A的前方、后方、或全方位，多个相机的拍摄区域也可以彼此重复。另外，搭载于车辆A的相机可以是被固定的，也可以是可动的。

另外，上述实施方式中，作为应用位置推断装置10的对象的移动体的一例，示出了车辆A，但是，移动体的种类是任意的。应用位置推断装置10的对象的移动体也可以是机器人或无人机。

另外，上述实施方式中，示出了通过基于CPU101的处理实现位置推断装置10的各功能的形态，但是，也可以代替基于CPU101的处理，或与其共同地，通过基于DSP(DigitalSignal Processor，数字信号处理器)或专用硬件电路(例如，ASIC(Application-specificintegrated circuit，专用集成电路)或FPGA(Field Programmable Gate Array，现场可编程门阵列))的处理，来实现位置推断装置10的各功能的一部分或全部。

以上，对本发明的具体例详细地进行了说明，但是，这些不过是示例，不对权利要求进行限定。在权利要求记载的技术中包含对以上示例的具体例进行各种各样的变形、变更后的内容。

在2019年11月22日提出的日本专利申请特愿2019-211243中包含的说明书、附图、以及摘要的公开内容全部引用于本申请。

工业实用性

根据本发明的位置推断装置，能够以较小的运算负荷提高移动体的位置及姿势的推断精度。

附图标记说明

A 车辆

10 位置推断装置

11 获得部

12 特征点提取部

13 推断部

14 验证部

15 判定部

20a、20b、20c、20d 相机

30 车辆ECU

40 车辆驱动装置

Dm 地图数据

Dt 相机搭载位置数据

Claims (8)

1.一种位置推断装置，是具有对周围的实景进行拍摄的n个相机的移动体的位置推断装置，其中，n为2以上的整数，该位置推断装置具备：

推断部，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

验证部，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

所述推断部针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

所述验证部计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

2.如权利要求1所述的位置推断装置，其中，

所述验证部计算所述特征点群中的重投影误差为阈值以下的特征点的个数，作为所述第k个相机的所述候选位置的所述准确度。

3.如权利要求1所述的位置推断装置，其中，

所述移动体是车辆。

4.如权利要求1所述的位置推断装置，其中，

所述n个相机拍摄所述实景的互不相同的区域。

5.如权利要求1所述的位置推断装置，其中，

所述推断部通过改变从所述第k个相机的相机图像中提取的多个特征点中的用于计算所述候选位置的所述特征点，来计算所述第k个相机的多个所述候选位置，

所述验证部针对所述第k个相机的多个所述候选位置中的每一个候选位置，计算所述准确度，

以所述n个相机中第1个相机到第n个相机各自的多个所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

6.一种车辆，具备权利要求1所述的位置推断装置。

7.一种位置推断方法，是具有对周围的实景进行拍摄的n个相机的移动体的位置推断方法，其中，n为2以上的整数，该位置推断方法包括以下处理：

第一处理，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

第二处理，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

在所述第一处理中，针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

在所述第二处理中，计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

8.一种位置推断程序，是使计算机推断具有对周围的实景进行拍摄的n个相机的移动体的位置的位置推断程序，其中，n为2以上的整数，该位置推断程序包括以下处理：

第一处理，基于从所述n个相机中第k个相机的相机图像中提取的所述实景中的特征点在相机图像中的位置、和在地图数据中预先存储的所述特征点在地图空间中的位置，计算地图空间中的所述第k个相机的候选位置，其中，k为1～n的整数；以及

第二处理，以所述第k个相机的所述候选位置为基准，将在所述地图数据中与地图空间中的位置建立关联地存储的所述实景中的特征点群，向所述n个相机各自的相机图像投影，并基于投影到所述n个相机各自的相机图像中的所述特征点群与从所述n个相机各自的相机图像中提取到的所述特征点群的一致度，计算所述第k个相机的所述候选位置的准确度，

在所述第一处理中，针对所述n个相机中的第1个相机到第n个相机，分别计算所述候选位置，

在所述第二处理中，计算所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度，

以所述n个相机中第1个相机到第n个相机各自的所述候选位置的所述准确度中准确度最高的所述候选位置为基准，推断所述移动体的位置。

Images