CN104854637A - 移动物体位置姿态角推定装置及移动物体位置姿态角推定方法 - Google Patents

Abstract

本发明的移动物体位置姿态角推定装置(10)，获取利用鱼眼镜头拍摄移动物体的周围环境得到的鱼眼镜头图像，针对鱼眼镜头图像的规定区域获取畸变小的针孔图像。并且，对移动物体的行进方向上的行驶路的弯曲度进行检测，在检测出的弯曲度小于规定值的情况下，选择针孔图像，在弯曲度大于或等于规定值的情况下，选择鱼眼镜头图像。然后，进行所选择的图像的边缘信息和三维地图数据之间的匹配，推定移动物体的位置及姿态角。

Description

移动物体位置姿态角推定装置及移动物体位置姿态角推定方法

技术领域

本发明涉及一种推定移动物体的位置及姿态角的移动物体位置姿态角推定装置及其方法。

背景技术

当前，已经开发了通过对三维地图信息和鱼眼镜头摄像机的拍摄图像进行匹配而计算移动物体的自身位置的技术，专利文献1公开了该技术。

在该技术中，利用设置于车辆上的鱼眼镜头摄像机对信号机进行拍摄，根据拍摄得到的信号机图像计算相对于车辆的仰角和水平角，在此基础上根据信号机的发光位置坐标和高度，确定车辆的当前位置。

专利文献1：日本特开2006－10328号公报

发明内容

然而，在上述现有技术中，存在如下问题点，即，为了推定车辆的自身位置，需要信号机，在没有信号机的交叉路口、单行道等，无法对自身位置进行推定。

因此，本发明就是鉴于上述情况而提出的，其目的在于提供一种移动物体位置姿态角推定装置以及移动物体位置姿态角推定方法，其即使不存在像信号机这样的特定的装置也能够对移动物体的位置及姿态角进行推定。

为了解决上述课题，本发明获取利用鱼眼镜头拍摄移动物体的周围环境得到的鱼眼镜头图像，针对鱼眼镜头图像的规定区域获取畸变小的针孔图像。并且，对移动物体的行进方向上的行驶路的弯曲度进行检测，在检测出的弯曲度小于规定值的情况下，选择针孔图像，在弯曲度大于或等于规定值的情况下，选择鱼眼镜头图像。如果以上述方式选择图像，则进行选择出的图像的边缘信息和从三维地图数据提取出的虚拟图像的边缘信息之间的匹配，推定移动物体的位置及姿态角。

附图说明

图1是表示具备应用了本发明的第1实施方式所涉及的移动物体位置姿态角推定装置的移动物体位置姿态角推定系统的结构的框图。

图2是表示由应用了本发明的第1实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理顺序的流程图。

图3是用于说明鱼眼镜头摄像机的视场角和针孔图像的视场角的图。

图4是表示由应用了本发明的第1实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角计算处理的处理顺序的流程图。

图5是表示由应用了本发明的第1实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角计算处理的处理顺序的流程图。

图6是表示由应用了本发明的第2实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理顺序的流程图。

图7是表示由应用了本发明的第3实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理流顺序的流程图。

图8是表示由应用了本发明的第4实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理顺序的流程图。

图9是表示由应用了本发明的第5实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理顺序的流程图。

图10是表示由应用了本发明的第6实施方式所涉及的移动物体位置姿态角推定装置进行的移动物体位置姿态角推定处理的处理顺序的流程图。

图11是表示具备应用了本发明的第7实施方式所涉及的移动物体位置姿态角推定装置的移动物体位置姿态角推定系统的结构的框图。

具体实施方式

以下，参照附图对应用了本发明的第1～第7实施方式进行说明。

[第1实施方式]

[移动物体位置姿态角推定装置的结构]

图1是表示搭载有本实施方式所涉及的移动物体位置姿态角推定装置的移动物体位置姿态角推定系统的结构的框图。如图1所示，本实施方式所涉及的移动物体位置姿态角推定系统具备ECU1、鱼眼镜头摄像机2、三维地图数据库3、车辆传感器组4以及导航系统5。此外，在本实施方式中，对将移动物体位置姿态角推定装置应用于车辆上的情况进行说明。

这里，ECU1是由ROM、RAM、运算电路等构成的电子控制单元，具备本实施方式所涉及的移动物体位置姿态角推定装置10。此外，ECU1也可以兼用作用于其他控制的ECU。

鱼眼镜头摄像机2例如是利用了CCD等固体拍摄元件的摄像机，在本实施方式中，以光轴水平、且能够拍摄车辆前方的方式设置于车辆的前保险杠上。所拍摄的鱼眼镜头图像发送至ECU1。另外，鱼眼镜头可以是视场角约为170[deg]且等距投影方式的通常的镜头。

三维地图数据库3例如存储有包含路面显示在内的周围环境的边缘等的三维位置信息。在本实施方式中，至少记录有表示道路端部的划分线、路缘石等的位置和其朝向的三维信息，此外除了白线、停止线、人行横道、路面标记等路面显示之外还包含建筑物等构造物的边缘信息。此外，作为三维地图数据库3的数据格式，道路端部等的各地图信息定义为边缘的集合体。在边缘为较长的直线的情况下，例如每隔1m分隔，因此不存在特别长的边缘。在直线的情况下，各边缘带有表示直线的两端点的三维位置信息。在曲线的情况下，各边缘带有表示曲线的两端点和中央点的三维位置信息。

车辆传感器组4具备GPS接收器41、加速传感器42、转向传感器43、制动传感器44、车速传感器45、加速度传感器46、车轮速度传感器47以及三轴陀螺仪传感器48。车辆传感器组4与ECU1连接，将利用各传感器41～48检测到的各种检测值供给至ECU1。ECU1通过利用车辆传感器组4的输出值而计算车辆的大概位置，或者计算表示在单位时间内车辆行进的移动量的里程。

导航系统5可以是与搭载于通常的车辆上的导航系统相同的系统，具备数据库，该数据库储存有利用链路和节点记录的道路地图信息。并且，针对由车辆的驾驶者的操作所设定的目的地，计算将驾驶时间、距离等优化后的路线，计算出的路线向驾驶者通知，并发送至ECU1。

移动物体位置姿态角推定装置10是进行拍摄移动物体的周围环境得到的图像和三维地图数据之间的匹配而推定移动物体的位置及姿态角的装置。移动物体位置姿态角推定装置10通过执行储存于ROM中的移动物体位置姿态角推定用的程序，而作为鱼眼镜头图像获取部12、针孔图像获取部14、图像选择部16以及位置姿态角推定部18进行动作。

鱼眼镜头图像获取部12从鱼眼镜头摄像机2获取鱼眼镜头图像，该鱼眼镜头图像是利用鱼眼镜头对移动物体的周围环境进行拍摄而得到的。在鱼眼镜头图像中，拍摄有移动物体的周围环境，作为用于推定本车辆的位置及姿态角所需的路面信息，至少拍摄有表示道路端部的划分线、路缘石。另外，除此之外，也可以拍摄有白线、停止线、人行横道、路面标记等的路面显示。

针孔图像获取部14针对鱼眼镜头图像的规定区域获取畸变较小的针孔图像。特别是，在本实施方式中，通过对利用鱼眼镜头摄像机2拍摄得到的鱼眼镜头图像的规定区域进行畸变校正，而获取畸变较小的针孔图像。在针孔图像中，与鱼眼镜头图像相同地，拍摄有移动物体的周围环境，作为用于推定本车辆的位置及姿态角所需的路面信息，至少拍摄有表示道路端部的划分线、路缘石。另外，除此之外，也可以拍摄有白线、停止线、人行横道、路面标记等的路面显示。

图像选择部16对移动物体的行进方向上的行驶路的弯曲度进行检测，在检测出的弯曲度小于规定值的情况下，选择针孔图像，在弯曲度大于或等于规定值的情况下，选择鱼眼镜头图像。这里，行驶路的弯曲度是表示行驶路以何种程度弯曲的指标，如果行驶路较大程度地弯曲，则弯曲度的值变大，如果行驶路未弯曲，则弯曲度的值变小。

特别是，在本实施方式中，对移动物体的行进方向上的转弯半径进行检测，在检测出的转弯半径大于或等于规定值的情况下，判定为弯曲度小于规定值而选择针孔图像。另一方面，在转弯半径小于规定值的情况下，判定为弯曲度大于或等于规定值而选择鱼眼镜头图像。

位置姿态角推定部18基于由图像选择部16选择的图像而进行与三维地图数据的匹配，推定移动物体的位置及姿态角。特别是，进行所选择的图像的边缘信息和从三维地图数据提取出的虚拟图像的边缘信息之间的匹配，推定移动物体的位置及姿态角。此时，在虚拟图像中包含有所推定的移动物体的虚拟位置和虚拟姿态角。另外，在与三维地图数据的匹配中，基于在所选择的图像中拍摄有的周围环境中的特别是表示道路端部的路面信息而进行匹配。

此外，在本实施方式中，对应用于车辆上的情况进行了说明，但只要是移动物体即可，也可以应用于飞机、船舶等。在应用于飞机、船舶的情况下，作为周围环境，可以取代与道路相关的路面信息而是通过对地形、建筑物进行匹配，推定移动物体的位置和姿态角。

[移动物体的位置姿态角推定处理的顺序]

下面参照图2的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序进行说明。此外，在本实施方式中，对作为车辆的位置信息的前后方向、横向、上下方向、作为姿态角信息的侧倾、俯仰、偏转合计6个自由度的位置和姿态角进行推定。另外，在以下所说明的移动物体的位置姿态角推定处理，例如以100msec左右的间隔连续地进行。

如图2所示，首先在步骤S10中，利用鱼眼镜头摄像机2拍摄得到的影像发送至ECU1，鱼眼镜头图像获取部12获取鱼眼镜头图像。

在步骤S20中，针孔图像获取部14对利用鱼眼镜头摄像机2拍摄得到的影像的规定区域进行畸变校正，由此计算并获得畸变较小的针孔图像。由于鱼眼镜头摄像机2的鱼眼镜头为等距投影方式，因此在本实施方式中，以鱼眼镜头的虚拟球面模型进行考虑，以在虚拟球面和光轴的交点处相接的平面屏幕像上再现的方式，校正畸变而计算针孔图像。另外，如图3所示，该针孔图像是在鱼眼镜头摄像机2的视场角170[deg]中，将视场角40[deg]的区域作为规定区域进行畸变校正而计算。如果以上述方式计算出的针孔图像不被障碍物等遮挡，则在大部分的行驶状态中能够拍摄本车辆前方的左右的道路端部。

然后，在步骤S30中，图像选择部16对移动物体即本车辆的行进方向上的转弯半径R[m]进行检测。此时，在转弯半径发生变化的情况下，将最小的转弯半径作为转弯半径R进行检测。并且，在检测出的转弯半径R大于或等于规定值Rth[m]的情况下，判定为弯曲度小于规定值而进入步骤S50，选择针孔图像。另一方面，在转弯半径R小于规定值Rth[m]的情况下，判定为弯曲度大于或等于规定值而进入步骤S40，选择鱼眼镜头图像。另外，也可以利用方向指示信号的ON、OFF而选择图像。此外，在本实施方式中，作为规定值Rth设定为30m。

这里，作为转弯半径的检测方法，可以利用以下的方法进行。例如，利用车辆传感器组4的转向传感器43对本车辆的驾驶者的转向操作进行检测，乘以本车辆的转向机构的转向传动比而求出前轮的转向角。并且，通过利用阿卡曼转向几何、线形两轮模型(《自動車の運動と制御》第3章，安部正人，山海堂)等的车辆模型，能够计算转弯半径。另外，根据由导航系统5计算出的本车辆的行驶路线，参照三维地图数据库3，预先计算在行驶路线上的转弯半径而记录至ECU1。并且，也可以根据在上1个循环所计算出的本车辆的位置和姿态角，求出当前的转弯半径R。

另一方面，在步骤S30中，不仅与转弯半径相对应地选择图像，也可以仅在判定为无法在针孔图像中拍摄本车辆的行进方向上的左右的道路端部的情况下，进入步骤S40而选择鱼眼镜头图像。

在该情况下，首先参照三维地图数据库3对行驶路线上的左右的道路端部的间隔即道路的宽度进行计算。并且，参照该宽度、转弯半径R、鱼眼镜头摄像机2的安装位置以及在上1个循环的处理中计算出的本车辆的位置和姿态角。根据这些信息，图像选择部16对能否从当前的本车位置在针孔图像上拍摄到本车辆的行进方向上的左右的道路端部进行判定，在判定为无法拍摄的情况下，进入步骤S40，选择鱼眼镜头图像。由此，在能够拍摄道路端部的情况下，选择针孔图像，因此，选择针孔图像的情况增加，能够更可靠且高速地进行移动物体的位置及姿态角的推定。

如上所述，如果在步骤S30中判定出转弯半径，则在步骤S40中，图像选择部16作为在后述的步骤S60的处理中使用的图像而选择鱼眼镜头图像，进入步骤S60。在转弯半径较小的情况下，即，在如急弯道、交叉路口处的右转/左转这样的弯曲度较大的情况下，为了计算本车辆的位置和姿态角而所需的划分线、路缘石等道路端部，在针孔图像中拍摄不到的可能性较高。因此，选择可视角较大的鱼眼镜头图像而可靠地检测道路端部，能够推定移动物体的位置及姿态角。

另一方面，在步骤S50中，图像选择部16作为在后述的步骤S60的处理中使用的图像而选择针孔图像，进入步骤S60。在转弯半径较大的情况下，即，在如直线路、缓弯道、交叉路口直行这样的弯曲度较小的情况下，即使在可视角较窄的针孔图像中，拍摄到道路端部的可能性较高。因此，即使利用针孔图像，也能够推定移动物体的位置及姿态角，另外针孔图像的畸变较小，因此能够高速且高精度地推定移动物体的位置及姿态角。

然后，在步骤S60中，位置姿态角推定部18利用在步骤S40或步骤S50中选择出的图像，进行与三维地图数据的匹配，计算并推定本车辆的位置和姿态角。具体而言，在步骤S40中选择了鱼眼镜头图像的情况下，利用图4的流程图计算移动物体的位置和姿态角，在步骤S50中选择了针孔图像的情况下，利用图5的流程图计算移动物体的位置和姿态角。这样，如果推定出移动物体的位置和姿态角，则结束本实施方式所涉及的移动物体的位置姿态角推定处理。

以下，参照图4、5的流程图对步骤S60中的移动物体的位置姿态角计算处理的顺序进行说明。

[选择了鱼眼镜头图像的情况下的移动物体的位置姿态角计算处理的顺序]

首先，参照图4的流程图，对在步骤S40中选择了鱼眼镜头图像的情况下的移动物体的位置姿态角计算处理的顺序进行说明。

在步骤S110中，位置姿态角推定部18根据鱼眼镜头图像计算边缘图像。本实施方式的边缘是指像素的亮度敏锐地变化的部位。作为边缘的检测方法，例如能够利用Canny法。除此之外也可以使用微分边缘检测等各种方法。

另外，位置姿态角推定部18优选从鱼眼镜头图像中也针对边缘的亮度变化的方向、边缘附近的颜色等进行提取。由此，在后述的步骤S150及步骤S160中，也利用记录于三维地图数据库3中的除了这些边缘以外的信息，设定似然度，能够计算本车辆的位置及姿态角。

然后，在步骤S120中，位置姿态角推定部18基于从车辆传感器组4得到的传感器值，对从在上1个循环的步骤S120中计算出的时刻至当前为止的车辆的移动量即里程进行计算。此外，在开始程序而进行最初的循环的情况下，将里程计算为零。

位置姿态角推定部18利用从车辆传感器组4得到的各种传感器值，对车辆在单位时间行进的移动量即里程进行计算。作为该里程的计算方法，例如，在将车辆运动限定在平面上的基础上，根据各车轮的车轮速度和偏航率，计算单位时间内的移动量和旋转量即可。另外，也可以用车速或GPS接收器41的测位值的差值代替车轮速度，也可以用转向操纵角代替偏航率。此外，里程的计算方法能够想到多种计算方法，只要能够计算里程则可以采用任意方法。

然后，在步骤S130中，位置姿态角推定部18使车辆位置从在上1个循环的步骤S160中推定出的车辆位置移动与在此次的步骤S120中计算出的里程相应的量。并且，在移动后的车辆位置的附近，计算多个虚拟位置及虚拟姿态角的候补。虚拟位置及虚拟姿态角的候补是本车位置及姿态角的候补。但是，在将程序开始而进行最初的循环的情况下，由于不存在上一次的车辆位置信息，因此将包含于车辆传感器组4中的来自GPS接收器41的数据作为初始位置信息。另外，也可以预先存储在上一次停车时最后计算出的车辆位置及姿态角，作为初始位置及姿态角信息。

此时，位置姿态角推定部18考虑由于车辆传感器组4的测定误差、通信延迟而产生的里程的误差、和在里程中无法考虑的车辆的动态特性，生成多个具有成为车辆的位置和姿态角的真值的可能性的虚拟位置及虚拟姿态角的候补。该虚拟位置及虚拟姿态角的候补是以下述方式设定的，即，针对位置及姿态角的6个自由度的参数分别设定各自误差的上下限，在该误差的上下限的范围内利用随机数表等随机地设定。

此外，在本实施方式中，生成500个虚拟位置及虚拟姿态角的候补。另外，位置及姿态角的6个自由度的参数中的误差的上下限按照前后方向、横向、上下方向、侧倾、俯仰、偏转的顺序设为±0.05[m]、±0.05[m]、±0.05[m]、±0.5[deg]、±0.5[deg]、±0.5[deg]。生成该虚拟位置及虚拟姿态角的候补的数量、位置及姿态角的6个自由度的参数中的误差的上下限，优选检测或者推定车辆的运行状态、路面的状况而进行适当变更。例如，在发生急转弯、滑移等的情况下，由于平面方向(前后方向、横向、偏转)的误差变大的可能性较高，因此优选增大该3个参数的误差的上下限，并且增加虚拟位置及虚拟姿态角的候补的生成数量。

另外，在步骤S130中，位置姿态角推定部18可以利用所谓粒子滤波设定多个虚拟位置及虚拟姿态角的候补。在该情况下，位置姿态角推定部18使在上1个循环的步骤S160中所生成的多个虚拟位置及虚拟姿态角的候补即各粒子(候补点)的位置及姿态角，移动与里程对应的量，作为此次虚拟位置及虚拟姿态角的候补进行计算。即，将各粒子的位置及姿态角设为多个虚拟位置及虚拟姿态角的候补。此外，如果考虑由于车辆传感器组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]。

然后，在步骤S140中，位置姿态角推定部18针对在步骤S130中生成的多个虚拟位置及虚拟姿态角的候补分别生成投影图像(虚拟图像)。此时，例如将存储于三维地图数据库3中的边缘等三维位置信息，以成为根据虚拟位置及虚拟姿态角利用鱼眼镜头拍摄得到的摄像机图像的方式进行投影变换，生成投影图像。该投影图像是用于评价各虚拟位置及虚拟姿态角的候补是否与实际的本车辆的位置及姿态角一致的评价用图像。另外，在投影变换中，需要表示鱼眼镜头摄像机2的位置的外部参数、和鱼眼镜头摄像机2的内部参数。外部参数通过预先测量从车辆位置(例如，中心位置)至鱼眼镜头摄像机2的相对位置而根据虚拟位置及虚拟姿态角的候补计算即可。另外，内部参数预先调准即可。

此外，在步骤S110中，在从鱼眼镜头图像中针对边缘的亮度变化的方向、边缘附近的颜色等进行提取的情况下，优选利用它们生成投影图像。

然后，在步骤S150中，位置姿态角推定部18在步骤S130中设定出的多个虚拟位置及虚拟姿态角的候补中，分别对在步骤S110中生成的边缘图像和在步骤S140中生成的投影图像的边缘信息进行比较。并且，基于比较得到的结果，对各虚拟位置及虚拟姿态角的候补分别求出似然度。该似然度是表示虚拟位置及虚拟姿态角的候补在多大程度上与实际的车辆的位置及姿态角相近的指标。位置姿态角推定部18设定为，投影图像和边缘图像的一致度越高，似然度越高。以下说明该似然度的计算方法的一个例子。

首先，在投影图像上确定像素位置，判定在该像素位置是否存在边缘。并且，判定在与投影图像成为相同像素位置的边缘图像上的位置是否存在边缘，在边缘的有无一致的情况下，将似然度likelihood(单位：无)设定为1，在不一致的情况下，设定为0。通过对投影图像上的所有像素进行这样的处理，并将在所有的像素位置上设定的似然度相加，而最终作为该虚拟位置及虚拟姿态角的候补的似然度。并且，如果针对所有的虚拟位置及虚拟姿态角的候补计算似然度，则进行标准化以使得各个似然度的合计值成为1。

然后，在步骤S160中，位置姿态角推定部18利用具有在步骤S150中计算出的似然度信息的多个虚拟位置及虚拟姿态角的候补，计算最终的本车辆的位置及姿态角。例如，位置姿态角推定部18将似然度最高的虚拟位置及虚拟姿态角的候补作为车辆的实际位置及姿态角而进行计算。另外，也可以利用各虚拟位置及虚拟姿态角的候补的似然度，求出虚拟位置及虚拟姿态角的加权平均，将该值作为最终的本车辆的位置及姿态角。另外，在步骤S160中，基于各虚拟位置及虚拟姿态角的候补的似然度而进行各粒子的重新采样。如果以这样的方式计算出本车辆的位置和姿态角，则结束本实施方式所涉及的移动物体的位置姿态角计算处理。并且，计算出的本车辆的位置和姿态角作为图2的步骤S60中的推定结果而输出。

[选择了针孔图像的情况下的移动物体的位置姿态角计算处理的流程]

下面，参照图5的流程图对在图2的步骤S50中选择了针孔图像的情况下的移动物体的位置姿态角计算处理的顺序进行说明。

在步骤S210中，位置姿态角推定部18以与上述的图4的步骤S110相同的方法根据针孔图像计算边缘图像。

然后，在步骤S220中，位置姿态角推定部18对在步骤S210中计算出的边缘图像利用霍夫(Hough)变换等，对道路端部、划分线的线段存在于边缘图像上的何处进行确定。此外，在三维地图数据库3中包含有停止线、人行横道、路面标记等路面显示、建筑物等构造物的边缘信息的情况下，针对这些线段也确定它们存在于边缘图像上的何处。

然后，在步骤S230中，位置姿态角推定部18进行调整，以使得记录于三维地图DB3中的道路端部的三维位置及姿态角信息与在步骤S220中确定出的道路端部等的边缘图像上的位置匹配，而计算本车辆的位置和姿态角。

具体而言，通过针孔图像的摄像机模型而对记录于三维地图数据库3中的三维地图数据进行投影，并调整为所投影的道路端部的三维位置及姿态角与在边缘图像上确定出的道路端部匹配。并且，如果存在与在边缘图像上确定出的道路端部一致的三维地图数据的道路端部，则将该道路端部的三维位置和姿态角作为本车辆的位置及姿态角而进行计算。

此外，在步骤S230中，例如在利用来自车辆传感器组4的GPS接收器41的信息筛选本车位置以后，调整为使在边缘图像上确定的道路端部、和投影出的三维地图数据的道路端部匹配即可。

另外，作为筛选本车位置的方法，可以设为，在将上1个循环的图2的步骤S60中计算出的本车辆的位置及姿态角移动与利用图4的步骤S130的方法计算出的里程相对应的量的位置及姿态角附近，存在当前的本车辆的位置及姿态角的真值。

如果以该方式计算出本车辆的位置和姿态角，则结束本实施方式所涉及的移动物体的位置姿态角计算处理。并且，将计算出的本车辆的位置和姿态角作为图2的步骤S60中的推定结果而输出。

[第1实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，基于利用鱼眼镜头拍摄移动物体的周围环境得到的鱼眼镜头图像，或者拍摄鱼眼镜头图像的规定区域得到的针孔图像，推定移动物体的位置及姿态角。由此，即使不具备信号机这样的特定的装置，也能够根据移动物体的周围环境，推定移动物体的位置及姿态角。

另外，在本实施方式所涉及的移动物体位置姿态角推定装置中，对移动物体的行进方向上的转弯半径进行检测，在检测出的转弯半径大于或等于规定值的情况下，判定为弯曲度小于规定值而选择针孔图像。另一方面，在转弯半径小于规定值的情况下，判定为弯曲度大于或等于规定值而选择鱼眼镜头图像。由此，即使在如急弯道、交叉路口处的左右转弯这样的情况下，也能够利用可视角较大的鱼眼镜头图像对移动物体的位置及姿态角进行推定。另一方面，在如直线路、缓弯道、交叉路口处直行这样的情况下，能够利用针孔图像高速且高精度地对移动物体的位置及姿态角进行推定。

另外，在本实施方式所涉及的移动物体位置姿态角推定装置中，对鱼眼镜头图像的规定区域进行畸变校正，由此获取畸变较小的针孔图像。由此，无需设置针孔摄像机，能够获取针孔图像，因此，能够大幅削减成本。

并且，在本实施方式所涉及的移动物体位置姿态角推定装置中，仅在判定为无法在针孔图像中拍摄到移动物体的行进方向上的左右的道路端部的情况下，选择鱼眼镜头图像。由此，在能够拍摄到道路端部的情况下，能够选择针孔图像，因此，选择针孔图像的情况增加，能够更加可靠且高速地进行移动物体的位置及姿态角的推定。

[第2实施方式]

下面，参照附图对本发明的第2实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，本实施方式所涉及的移动物体位置姿态角推定装置的结构和第1实施方式相同，因此省略详细的说明。

[移动物体的位置姿态角推定处理的顺序]

参照图6的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序进行说明。在本实施方式中，与第1实施方式的不同之处在于，追加了判定本车辆的车速是否小于规定值的步骤S25。

在步骤S25中，图像选择部16基于利用车辆传感器组4的车速传感器45所获取的车速信息，判定本车辆的移动速度是否小于规定值。在本实施方式中，作为规定值，判定车速是否小于30[km/h]。并且，在车速小于30[km/h]的情况下，无论本车辆的行进方向上的转弯半径如何，都进入步骤S40而选择鱼眼镜头图像。即，不进行步骤S30的处理而进入步骤S40。另一方面，在车速大于或等于30[km/h]的情况下，进入步骤S30。

在本实施方式中，在本车辆的移动速度较低、且在高速的周期中对移动物体的位置及姿态角进行推定的必要性较低的情况下，利用鱼眼镜头图像对移动物体的位置及姿态角进行推定。

[第2实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的移动速度小于规定值的情况下，选择鱼眼镜头图像。通常在处于以低速行驶的拥堵状态、交叉路口等时，如果可视角如针孔图像那样较小，则被障碍物遮挡而无法拍摄道路端部的可能性较高。因此，在移动速度小于规定值的情况下，利用拍摄到道路端部的可能性较高的鱼眼镜头图像，由此能够可靠地对移动物体的位置及姿态角进行推定。

[第3实施方式]

下面，参照附图对本发明的第3实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，本实施方式所涉及的移动物体位置姿态角推定装置的结构与第1实施方式相同，因此省略详细的说明。另外，本实施方式不仅能够适用于第1实施方式，还能够适用于第2实施方式。

[移动物体的位置姿态角推定处理的顺序]

参照图7的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序进行说明。在本实施方式中，与第1实施方式的不同之处在于，追加了判定本车辆的俯仰角速度或者侧倾角速度是否大于或等于规定值的步骤S28。

在步骤S28中，图像选择部16判定利用车辆传感器组4的三轴陀螺仪传感器48所获取的俯仰角速度或者侧倾角速度是否大于或等于规定值。在本实施方式中，作为规定值，判定俯仰角速度或者侧倾角速度是否大于或等于3[deg/s]。并且，在俯仰角速度或者侧倾角速度大于或等于3[deg/s]的情况下，无论本车辆的行进方向上的转弯半径如何，都进入步骤S50而选择针孔图像。即，不进行步骤S30的处理而进入步骤S50。另一方面，在俯仰角速度或者侧倾角速度小于3[deg/s]的情况下，进入步骤S30。

在本实施方式中，在本车辆的俯仰角、侧倾角的时间变化率较大的情况下，例如在存在凹凸的路面上行驶这样的情况下，利用针孔图像对本车辆的位置及姿态角进行推定。在俯仰角、侧倾角的时间变化率较大的情况下，摄像机的视点振动，因此在镜头的畸变较大的鱼眼镜头图像中拍摄到的道路端部以振动的方式移动，并且其形状也总是变化。因此，与三维地图数据的匹配较难，运算负荷增大。因此，在这种情况下，利用针孔图像对移动物体的位置及姿态角进行推定。

[第3实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的俯仰角的时间变化率大于或等于规定值的情况下，选择针孔图像。由此，即使摄像机因路面的凹凸、高低差等而振动，也能够利用畸变较小的针孔图像，因此，能够更加可靠且高速地对移动物体的位置及姿态角进行推定。

另外，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的侧倾角的时间变化率大于或等于规定值的情况下，选择针孔图像。由此，即使摄像机因路面的凹凸、高低差等而振动，也能够利用畸变较小的针孔图像，因此，能够更加可靠且高速地对移动物体的位置及姿态角进行推定。

[第4实施方式]

下面，参照附图对本发明的第4实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，本实施方式所涉及的移动物体位置姿态角推定装置的结构与第1实施方式相同，因此省略详细的说明。

其中，在本实施方式的图像选择部16中，基于利用针孔图像获取部14所获取的针孔图像，判定移动物体的周围环境是否被遮挡，在被遮挡的情况下，判定为弯曲度大于或等于规定值而选择鱼眼镜头图像。另一方面，在未被遮挡的情况下，判定为弯曲度小于规定值而选择针孔图像。

[移动物体的位置姿态角推定处理的顺序]

参照图8的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序进行说明。在本实施方式中，与第1实施方式的不同之处在于，在步骤S330中判定本车辆的周围环境是否被遮挡。

如图8所示，在步骤S310、320中，执行与图2所示的步骤S10、20相同的处理而获取针孔图像。

在步骤S330中，图像选择部16基于在步骤S320中获取的针孔图像，判定移动物体即本车辆的周围环境是否被遮挡。作为判定是否被遮挡的判定方法，利用以下方法进行。

首先，求出针孔图像的所有像素的亮度平均，在该亮度平均值大于或等于阈值THmax(单位：无)、或者小于或等于阈值THmin(单位：无)的情况下，判定为被遮挡。即，其理由在于，可以认为，亮度平均极大的图像是如直接拍摄太阳光这样的过亮的图像，相反，亮度平均极小的图像是如拍摄黑暗环境这样的处于被遮挡的状态的图像。这种图像是在后述的推定移动物体的位置及姿态角时所进行的边缘提取中难以提取图像的特征点的图像。因此，难以调整为与三维地图数据一致，因此，需要扩大可视角，需要切换为鱼眼镜头图像而不是针孔图像。此外，在本实施方式中，将提取的图像记录于ECU1时的亮度数值处于0～255(0：最暗，255：最亮)的范围内，将THmax设定为240，将THmin设定为15。

另外，作为其他判定方法，可以对存在于本车辆的周围的车辆、步行者等障碍物的位置和形状进行检测，在存在该障碍物的方向的图像中，判定该障碍物的远方是否被遮挡。作为障碍物的检测方法，通过使用摄像机图像的运动立体视觉(motion stereo)、在本车辆上另外搭载激光测距机等检测障碍物即可。在本实施方式中，根据障碍物的位置和形状、以及在上一个循环中推定出的车辆的位置和姿态角，推定障碍物在针孔图像的像素中占据的百分比是多少。在此基础上，在推定为障碍物在针孔图像中占据的百分比大于或等于50％的情况下，判断为看不到远处，判定为被遮挡。

并且，作为其他判定方法，可以根据三维地图、以及在上一个循环中推定出的车辆的位置和姿态角，提取在车辆的前方道路或者地平面的斜度凸向上方的场所，由此判定有没有遮挡。在本实施方式中，判断为斜度凸向上方的基准，设为在从车辆离开的方向上每单位长度(1m)的斜度变化量大于或等于2[deg]的情况。另外，在凸向上方的场所与鱼眼镜头摄像机2的距离为10[m]以内的情况下，也判断为被遮挡。

另外，在进行上述判定方法时，在针孔图像中，在移动物体的行进方向上的左右的道路端部未被遮挡的情况下，可以判定为周围环境未被遮挡。即，假设即使针孔图像的大部分被遮挡，只要能够检测出划分线、路缘石等的道路端部，就判定为未被遮挡。例如，在只有针孔图像的上半部分、即主要不是路面的区域被遮挡的情况下，判定为未被遮挡。另外，可以对针孔图像进行二值化处理等而检测白线部分，在能够检测出白线的情况下，判定为未被遮挡。

相反，也可以是，假设即使被遮挡的部分是一部分，在划分线、路缘石等的道路端部被遮挡的情况下，判定为周围环境被遮挡。例如，在针孔图像的下半部分、即主要是拍摄路面的区域被遮挡的情况下，判定为被遮挡。另外，可以对针孔图像进行二值化处理等而进行检测白线部分的处理，在无法检测出白线的情况下，判定为被遮挡。

利用上述判定方法，图像选择部16判定本车辆的周围环境是否被遮挡，在被遮挡的情况下，判定为弯曲度大于或等于规定值而进入步骤S340。另一方面，在未被遮挡的情况下，判定为弯曲度小于规定值而进入步骤S350。

在步骤S340，图像选择部16选择鱼眼镜头图像作为在后述的步骤S360的处理中使用的图像，进入步骤S360。通常，除了行驶路的弯曲度较大的情况以外，划分线、路缘石等的道路端部容易被由其他车辆等障碍物形成的影子等的光线环境等被遮挡，如果利用鱼眼镜头图像则可视角较大，因此，拍摄到道路端部的可能性较高。因此，在判定为周围环境被遮挡的情况下，选择鱼眼镜头图像而可靠地对道路端部进行检测，推定移动物体的位置及姿态角。

在步骤S350中，图像选择部16选择针孔图像作为在后述的步骤S360的处理中使用的图像，进入步骤S60。针孔图像的畸变较小，因此，如果利用针孔图像，则能够更加高速且高精度地进行移动物体的位置及姿态角的推定。因此，在判定为周围环境未被遮挡的情况下，使用针孔图像。

如果以该方式利用图像选择部16选择鱼眼镜头图像或者针孔图像的任一方，则在步骤S360中执行与图2的步骤S60相同的处理后，使本实施方式所涉及的移动物体的位置姿态角推定处理结束。

[第4实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，判定移动物体的周围环境是否被遮挡，在被遮挡的情况下，判定为弯曲度大于或等于规定值而选择鱼眼镜头图像。另一方面，在未被遮挡的情况下，判定为弯曲度小于规定值而选择针孔图像。由此，即使在被遮挡的情况下，也能够利用可视角较大的鱼眼镜头图像对移动物体的位置及姿态角进行推定。另一方面，在未被遮挡的情况下，利用针孔图像能够高速且高精度地推定移动物体的位置及姿态角。

另外，在本实施方式所涉及的移动物体位置姿态角推定装置中，对鱼眼镜头图像的规定区域进行畸变校正，由此获取畸变较小的针孔图像。由此，无需设置针孔摄像机，能够获取针孔图像，因此能够大幅削减成本。

并且，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的行进方向上的左右的道路端部未被遮挡的情况下，判定为弯曲度小于规定值而选择针孔图像。由此，只要道路端部未被遮挡就能够选择针孔图像，因此，选择针孔图像的情况增加，能够更加可靠且高速地进行移动物体的位置及姿态角的推定。

[第5实施方式]

下面，参照附图对本发明的第5实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，本实施方式所涉及的移动物体位置姿态角推定装置的结构与第4实施方式相同，因此省略详细的说明。

[移动物体的位置姿态角推定处理的顺序]

参照图9的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序流程进行说明。在本实施方式中，与第4实施方式的不同之处在于，追加了判定本车辆的车速是否小于规定值的步骤S325。

在步骤S325中，图像选择部16基于利用车辆传感器组4的车速传感器45获取的车速信息，判定本车辆的移动速度是否小于规定值。在本实施方式中，作为规定值，判定车速是否小于30[km/h]。并且，在车速小于30[km/h]的情况下，无论在步骤S320中获取的针孔图像是否被遮挡，都进入步骤S340。即，不进行步骤S330的处理而进入步骤S340。另一方面，在车速大于或等于30[km/h]的情况下，进入步骤S330。

在本实施方式中，在本车辆的移动速度较低、且在高速的周期内对移动物体的位置及姿态角进行推定的必要性较低的情况下，利用鱼眼镜头图像推定移动物体的位置及姿态角。

[第5实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的移动速度小于规定值的情况下，选择鱼眼镜头图像。由此，通常在处于低速行驶的拥堵状态、交叉路口等时，在较多地发生被障碍物遮挡的情况下，也能够利用拍摄到道路端部的鱼眼镜头图像，因此，能够更加可靠地推定移动物体的位置及姿态角。

[第6实施方式]

下面，参照附图对本发明的第6实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，本实施方式所涉及的移动物体位置姿态角推定装置的结构与第4实施方式相同，因此省略详细的说明。另外，本实施不仅能够适用于第4实施方式，还能够适用于第5实施方式。

[移动物体的位置姿态角推定处理的流程]

参照图10的流程图对本实施方式所涉及的移动物体的位置姿态角推定处理的顺序进行说明。在本实施方式中，与第4实施方式的不同之处在于，追加了判定本车辆的俯仰角速度或者侧倾角速度是否大于或等于规定值的步骤S328。

在步骤S328中，图像选择部16判定利用车辆传感器组4的三轴陀螺仪传感器48所获取的俯仰角速度或者侧倾角速度是否大于或等于规定值。在本实施方式中，作为规定值，判定俯仰角速度或者侧倾角速度是否大于或等于3[deg/s]。并且，在俯仰角速度或者侧倾角速度大于或等于3[deg/s]的情况下，无论在步骤S320中获取的针孔图像是否被遮挡，都进入步骤S350。即，不进行步骤S330的处理而进入步骤S350。另一方面，在俯仰角速度或者侧倾角速度小于3[deg/s]的情况下，进入步骤S330。

在本实施方式中，在本车辆的俯仰角、侧倾角的时间变化率较大的情况下，例如在存在凹凸的路面上行驶这样的情况下，利用针孔图像对本车辆的位置及姿态角进行推定。在移动物体的俯仰角、侧倾角的时间变化率较大的情况下，摄像机的视点振动，因此，透镜的畸变较大而在鱼眼镜头图像中拍摄的道路端部以振动的方式移动，并且其形状也总是变化。因此，与三维地图数据的匹配较难，运算负荷增大。因此，在这种情况下，利用针孔图像对移动物体的位置及姿态角进行推定。

但是，可以在检测出左右的道路端部在针孔图像上几乎完全被遮挡的情况下，进入步骤S340而利用鱼眼镜头图像。另外，在这种情况下，可以使步骤S330以及之后的处理中止，计算在图4的步骤S120中示出的里程，对移动物体的位置及姿态角进行推定。即，在摄像机的视点振动、且摄像机被遮挡这样的情况下，可以使摄像机的图像和三维地图数据的匹配暂时中断，根据里程对移动物体的位置及姿态角进行推定。

[第6实施方式的效果]

如以上详细说明所示，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的俯仰角的时间变化率大于或等于规定值的情况下，选择针孔图像。由此，即使摄像机因路面的凹凸、高低差等而振动，也能够利用畸变较小的针孔图像，因此，能够更加可靠且高速地推定移动物体的位置及姿态角。

另外，在本实施方式所涉及的移动物体位置姿态角推定装置中，在移动物体的侧倾角的时间变化率大于或等于规定值的情况下，选择针孔图像。由此，即使摄像机因路面的凹凸、高低差等而振动，也能够利用畸变较小的针孔图像，因此，能够更加可靠且高速地推定移动物体的位置及姿态角。

[第7实施方式]

下面，参照附图对本发明的第7实施方式所涉及的移动物体位置姿态角推定装置进行说明。此外，对与第1～第6实施方式相同的结构要素标注相同的标号并省略详细的说明。另外，本实施方式能够适用于第1～第6实施方式。

[移动物体位置姿态角推定装置的结构]

图11是表示搭载有本实施方式所涉及的移动物体位置姿态角推定装置的移动物体位置姿态角推定系统的结构的框图。如图11所示，在本实施方式所涉及的移动物体位置姿态角推定系统中，与第1～第6实施方式的不同之处在于，还具备针孔摄像机81。

针孔摄像机81例如是使用CCD等的固体拍摄元件的窄角镜头摄像机，在本实施方式中，以光轴水平、且能够拍摄车辆前方的方式设置于车辆的前保险杠上。另外，针孔摄像机81的光轴和鱼眼镜头摄像机2的光轴一致，视场角约为40[deg]。

利用针孔摄像机81拍摄的针孔图像是对鱼眼镜头图像的规定区域即视场角为40[deg]的范围进行拍摄的图像，是畸变较小的图像。如果拍摄针孔图像，则从针孔摄像机81向ECU1发送该针孔图像，由针孔图像获取部14获取该针孔图像。

因此，在本实施方式中，针孔图像获取部14不对鱼眼镜头图像的规定区域进行畸变校正，而是从针孔摄像机81获取针孔图像。

[第7实施方式的效果]

如以上详细说明所示，根据本实施方式所涉及的移动物体位置姿态角推定装置，从对鱼眼镜头图像的规定区域进行拍摄的针孔摄像机获取针孔图像，因此，无需对鱼眼镜头图像进行畸变校正，能够减轻处理负荷。

此外，上述实施方式是本发明的一个例子。因此，本发明不限定于上述实施方式，即使是除了该实施方式以外的方式，只要未脱离本发明所涉及的技术思想的范围，当然能够根据设计等进行各种变更。

本申请主张基于2012年12月12日申请的日本特愿第2012－270979号以及日本特愿第2012－270980号的优先权，通过参照的方式将这些申请的内容并入本发明的说明书中。

工业实用性

根据本发明的一个方式所涉及的移动物体位置姿态角推定装置及移动物体位置姿态角推定方法，即使不具备信号机这样的特定的装置，也能够根据周围环境对移动物体的位置及姿态角进行推定。因此，本发明的一个方式所涉及的移动物体位置姿态角推定装置及移动物体位置姿态角推定方法能够在工业中使用。

标号的说明

1 ECU

2 鱼眼镜头摄像机

3 三维地图数据库

4 车辆传感器组

5 导航系统

10 移动物体位置姿态角推定装置

12 鱼眼镜头图像获取部

14 针孔图像获取部

16 图像选择部

18 位置姿态角推定部

41 GPS接收器

42 加速传感器

43 转向传感器

44 制动传感器

45 车速传感器

46 加速度传感器

47 车轮速度传感器

48 三轴陀螺仪传感器

81 针孔摄像机

Claims (12)

1.一种移动物体位置姿态角推定装置，其推定移动物体的位置及姿态角，

该移动物体位置姿态角推定装置的特征在于，具备：

鱼眼镜头图像获取部，其获取利用鱼眼镜头拍摄所述移动物体的周围环境而得到的鱼眼镜头图像；

针孔图像获取部，其针对所述鱼眼镜头图像的规定区域，获取畸变小的针孔图像；

图像选择部，其对所述移动物体的行进方向上的行驶路的弯曲度进行检测，在所述弯曲度小于规定值的情况下，选择所述针孔图像，在所述弯曲度大于或等于规定值的情况下，选择所述鱼眼镜头图像；以及

位置姿态角推定部，其进行由所述图像选择部选择出的图像的边缘信息、和从三维地图数据提取出的虚拟图像的边缘信息之间的匹配，推定所述移动物体的位置及姿态角。

2.根据权利要求1所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部对所述移动物体的行进方向上的转弯半径进行检测，在所述转弯半径大于或等于规定值的情况下，判定为所述弯曲度小于规定值而选择所述针孔图像，在所述转弯半径小于规定值的情况下，判定为所述弯曲度大于或等于规定值而选择所述鱼眼镜头图像。

3.根据权利要求2所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部，仅在判定为在所述针孔图像中无法拍摄到所述移动物体的行进方向上的左右的道路端部的情况下，选择所述鱼眼镜头图像。

4.根据权利要求1所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部，基于由所述针孔图像获取部获取的针孔图像，判定所述移动物体的周围环境是否被遮挡，在被遮挡的情况下，判定为所述弯曲度大于或等于规定值而选择所述鱼眼镜头图像，在未被遮挡的情况下，判定为所述弯曲度小于规定值而选择所述针孔图像。

5.根据权利要求4所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部，在所述移动物体的行进方向上的左右的道路端部未被遮挡的情况下，判定为所述弯曲度小于规定值而选择所述针孔图像。

6.根据权利要求1至5中任一项所述的移动物体位置姿态角推定装置，其特征在于，

所述针孔图像获取部，对利用所述鱼眼镜头图像获取部获取的鱼眼镜头图像的规定区域进行畸变校正，由此获取畸变小的针孔图像。

7.根据权利要求1至5中任一项所述的移动物体位置姿态角推定装置，其特征在于，

所述针孔图像获取部，从对所述鱼眼镜头图像的规定区域进行拍摄的针孔摄像机获取所述针孔图像。

8.根据权利要求1至7中任一项所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部在所述移动物体的移动速度小于规定值的情况下，选择所述鱼眼镜头图像。

9.根据权利要求1至8中任一项所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部在所述移动物体的俯仰角的时间变化率大于或等于规定值的情况下，选择所述针孔图像。

10.根据权利要求1至9中任一项所述的移动物体位置姿态角推定装置，其特征在于，

所述图像选择部在所述移动物体的侧倾角的时间变化率大于或等于规定值的情况下，选择所述针孔图像。

11.一种移动物体位置姿态角推定方法，其是对移动物体的位置及姿态角进行推定的移动物体位置姿态角推定装置中的移动物体位置姿态角推定方法，

该移动物体位置姿态角推定方法的特征在于，

获取利用鱼眼镜头拍摄所述移动物体的周围环境而得到的鱼眼镜头图像；

针对所述鱼眼镜头图像的规定区域获取畸变小的针孔图像；

对所述移动物体的行进方向上的行驶路的弯曲度进行检测，在所述弯曲度小于规定值的情况下，选择所述针孔图像，在所述弯曲度大于或等于规定值的情况下，选择所述鱼眼镜头图像；

进行所述选择出的图像的边缘信息、和从三维地图数据提取出的虚拟图像的边缘信息之间的匹配，推定所述移动物体的位置及姿态角。

12.一种移动物体位置姿态角推定装置，其推定移动物体的位置及姿态角，

该移动物体位置姿态角推定装置的特征在于，具备：

鱼眼镜头图像获取单元，其获取利用鱼眼镜头拍摄所述移动物体的周围环境而得到的鱼眼镜头图像；

针孔图像获取单元，其针对所述鱼眼镜头图像的规定区域获取畸变小的针孔图像；

图像选择单元，其对所述移动物体的行进方向上的行驶路的弯曲度进行检测，在所述弯曲度小于规定值的情况下，选择所述针孔图像，在所述弯曲度大于或等于规定值的情况下，选择所述鱼眼镜头图像；以及

位置姿态角推定单元，其进行由所述图像选择单元选择出的图像的边缘信息、和从三维地图数据提取出的虚拟图像的边缘信息之间的匹配，推定所述移动物体的位置及姿态角。