CN105593776A - 车辆位置姿势角推定装置及车辆位置姿势角推定方法 - Google Patents

Abstract

本发明的车辆位置姿势角推定装置，使用质点过滤器设定规定范围的质点的存在分布范围，在所设定的存在分布范围内散布质点，根据拍摄了车辆的周围环境的图像推定车辆的位置及姿势角，当车辆的速度变高，将所设定的存在分布范围向车辆的车宽方向扩展。

Description

车辆位置姿势角推定装置及车辆位置姿势角推定方法

技术领域

本发明涉及使用质点过滤器(particlefilter)推定车辆的位置及姿势角的车辆位置姿势角推定装置及其方法。

背景技术

作为使用质点过滤器计算移动体的位置和姿势角的技术，目前公开有专利文献1。该技术中，在根据里程(odometry)而计算出的移动体的位置和姿势角的附近散布多个质点。而且，将这些质点中与搭载于移动体的激光传感器的测定值最吻合的质点(particle)作为该移动体的位置和姿势角的真实值进行计算。

此时，散布质点的存在分布范围通过时时刻刻计算使用质点过滤器计算出的位置和姿势角如何进行修正，并根据该修正量的履历进行设定。

现有技术文献

专利文献

专利文献1：(日本)特开2010－224755号公报

但是，在上述的现有技术中，考虑到向车辆的行进方向的误差，但向车宽方向的误差未充分考虑。特别是，在车速高时，如果车辆转弯，则在车辆上产生向车宽方向的大的移动，因此，向车宽方向的误差增大，但目前这种情况的对应未能充分进行。因此，存在对质点的存在分布范围的设定产生延迟，不能适当地进行设定的问题点。

发明内容

于是，本发明是鉴于上述的情况而提案的，其目的在于，提供一种即使在车辆的速度变高而在车辆上产生向车宽方向的移动的情况下，也能够适当地设定质点的存在分布范围的车辆位置姿势角推定装置及其方法。

为解决上述的课题，本发明一方式的车辆位置姿势角推定装置及其方法，使用质点过滤器设定规定范围的质点的存在分布范围。而且，在所设定的存在分布范围内散布质点，根据拍摄车辆的周围环境的图像推定车辆的位置及姿势角，当车辆的速度变高时，将所设定的存在分布范围向车辆的车宽方向扩展。

附图说明

图1是表示具备本发明第一实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；

图2是表示本发明第一实施方式的车辆位置姿势角推定装置的车辆的位置姿势角推定处理的处理顺序的流程图；

图3是示意性地表示定速圆转弯中的车辆的转弯中心和各轮胎的侧滑角的图；

图4是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的前后方向扩展的方法的图；

图5是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的车宽方向扩展的方法的图；

图6是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的偏航角方向扩展的方法的图；

图7是表示具备本发明第二实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；

图8是用于说明车辆转舵时的轮胎横力和偏航力矩的图；

图9是用于说明根据本发明第二实施方式的车辆位置姿势角推定装置推定的偏航率将质点的存在分布范围向车辆的车宽方向及偏航角方向扩展的方法的图；

图10是表示具备本发明第三实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；

图11是用于说明根据本发明第三实施方式的车辆位置姿势角推定装置推定的转向操舵角将质点的存在分布范围向车辆的前后方向、车宽方向及偏航角方向扩展的方法的图；

图12是用于说明本发明的车辆位置姿势角推定装置的质点的散布方法的图。

具体实施方式

以下，参照附图说明应用了本发明的第一～第三实施方式。

[第一实施方式]

[车辆位置姿势角推定系统的构成]

图1是表示搭载有本实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图。如图1所示，本实施方式的车辆位置姿势角推定系统具备ECU1、摄像机2、三维地图数据库3、车辆传感器群4。

在此，ECU1为由ROM、RAM、运算电路等构成的电子控制单元，具备本实施方式的车辆位置姿势角推定装置10。此外，ECU1也可以兼用于其它控制的ECU。

摄像机2使用例如CCD等固体摄像元件，本实施方式中，以其光轴水平且以可拍摄车辆前方的方式设置在车辆的前保险杠上。拍摄到的图像被送入ECU1。

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

车辆传感器群4具备GPS接收机41、加速传感器42、转向传感器43、制动传感器44、车速传感器45、加速度传感器46、车轮速传感器47、偏航率传感器48。车辆传感器群4与ECU1连接，将由各传感器41～48检测出的各种检测值向ECU1供给。ECU1通过使用车辆传感器群4的输出值，计算车出辆的大概位置，或计算出表示单位时间内车辆行进的移动量的里程(odometry)。

车辆位置姿势角推定装置10是用于将拍摄车辆的周围环境得到的图像和三维地图数据进行匹配来推定车辆的位置及姿势角的装置。而且，通过执行特定的程序，作为边缘图像计算部12、里程计算部14、车速检测部15、质点存在分布范围设定部16、质点散布部18、投影图像生成部20、相似度计算部22及位置姿势角推定部24而进行动作。

边缘图像计算部12由摄像机2取得拍摄车辆的周围环境而得到的图像，根据该图像检测边缘，并计算边缘图像。在由摄像机2拍摄到的图像中，作为用于推定自车辆的位置及姿势角所需的路面信息，至少拍摄表示道路端部的区划线或路牙。另外，也可以拍摄其它白线、停止线、人行横道、路面标识等路面显示。

里程计算部14使用由车辆传感器群4得到的各种传感器值计算出车辆的每单位时间的移动量即里程。

车速检测部15通过取得由车速传感器45测量出的传感器值，检测车辆的速度。

质点存在分布范围设定部16在仅移动由里程计算部14计算出的里程量的位置和姿势角的附近设定规定范围的质点的存在分布范围，根据车辆的行驶状态修正质点的存在分布范围。具体而言，如图12所示，使1循环前推定的车辆V(t1)的位置及姿势角的质点P和周围的质点P1～P5移动里程量，设定质点的存在分布范围进行修正。本实施方式中，作为车辆的行驶状态，当速度变高时，进行使质点的存在分布范围在车辆的前后方向、车宽方向及偏航角方向扩展的修正。

质点散布部18在由质点存在分布范围设定部16设定的质点的存在分布范围内散布质点。如图12所示，质点散布部18为推定新的车辆V(t2)的位置及姿势角而设定质点P10～15。

投影图像生成部20对于由质点散布部18散布的质点分别生成投影图像。例如，将存储于三维地图数据库3的边缘等三维位置信息以成为根据各质点的位置及姿势角用摄像机拍摄的图像的方式进行投影变换，生成投影图像。

相似度计算部22将由投影图像生成部20生成的投影图像和由边缘图像计算部12计算出的边缘图像进行比较，对质点分别计算出相似度(尤度likelihood)。该相似度是表示各质点的位置及姿势角相对于实际的车辆的位置及姿势角相似多少的指标，以投影图像和边缘图像的一致度越高，相似度越高的方式进行设定。

位置姿势角推定部24基于由相似度计算部22计算出的相似度来推定车辆的位置及姿势角。例如，将相似度最高的质点的位置及姿势角作为车辆的实际的位置及姿势角的推定结果来进行计算。

此外，本实施方式中，求出车辆的6自由度的位置(前后方向、车宽方向、上下方向)及姿势角(侧倾、俯仰、偏航)。但是，例如如不具备悬架等的工厂等所使用的无人输送车，即使在推定3自由度的位置(前后方向、横方向)及姿势角(偏航)的情况下，也可以应用本技术。具体而言，在这样的车辆中，由于上下方向的位置和姿势角的侧倾及俯仰固定，所以只要预先测量这些参数并利用即可。

[车辆的位置姿势角推定处理的顺序]

其次，参照图2的流程图说明本实施方式的车辆的位置姿势角推定处理的顺序。此外，本实施方式中，作为车辆的位置信息推定前后方向、车宽方向、上下方向，作为姿势角信息推定侧倾、俯仰、偏航的合计6自由度的位置和姿势角。但是，侧倾是以车辆的前后方向为轴的旋转方向，俯仰是以车辆的车宽方向为轴的旋转方向，偏航是以车辆的上下方向为轴的旋转方向(参照图12)。

另外，以下说明的车辆的位置姿势角推定处理例如以100msec左右的间隔连续进行。

如图2所示，首先，在步骤S110中，边缘图像计算部12根据摄像机2的图像计算出边缘图像。本实施方式中的边缘是指像素的辉度锐利地变化的部位。作为边缘的检测方法，例如可使用Canny法。除此之外，也可以使用微分边缘检测等各种手法。

另外，边缘图像计算部12期望从摄像机2的图像中也提取边缘的辉度变化的方向或边缘附近的彩色等。由此，在后述的步骤S160及步骤S170中，也利用记录于三维地图数据库3的这些边缘以外的信息来设定相似度，可以计算自车辆的位置及姿势角。

其次，在步骤S120中，里程计算部14基于由车辆传感器群4得到的传感器值，计算在1循环前的步骤S120计算出的时刻至目前为止的车辆的移动量即里程。此外，在开始程序而成为最初的循环的情况下，将里程作为零而计算。

里程计算部14作为里程的计算方法，在将车辆运动限定于平面上后，根据左右轮的车轮速传感器47的编码值之差计算出偏航角方向的转弯量(转弯角)。而且，前后方向和车宽方向的移动量通过根据各车轮的车轮速传感器47的编码值求出平均的移动量，相对于该移动量取偏航角方向的转弯角的余弦和正弦而求出。另外，也可以根据各车轮的车轮速和偏航率计算每单位时间的移动量和旋转量。而且，可以用车速或GPS接收机41的测位值的差值来代用车轮速，也可以用操舵角来代用偏航率。此外，里程的计算方法考虑各种计算方法，但只要能够计算出里程，则也可以使用任何方法。

特别是，为更正确地计算车宽方向或偏航角方向的移动量，也可以在例如车速约10km/h以下的极低速下根据阿卡曼转向几何(汽车的运动和控制第三章、安部正人著、山海堂发行)求出里程。另外，也可以在其以上的车速下使用可考虑轮胎的侧滑角的线形2轮模型的运动方程式(汽车的运动和控制第三章3.2.1节P.56(3.12)及(3.13)式、安部正人著、山海堂发行)进行计算。但是，在使用线形2轮模型的运动方程式的情况下，需要预先测定各车轮的轮胎参数(转弯参数)。

其次，在步骤S130中，质点存在分布范围设定部16使在1循环前的步骤S170推定的各质点的位置及姿势角移动在本次的步骤S120计算出的里程量。但是，在开始程序而成为开始的循环的情况下，由于没有上次的车辆位置信息，所以将来自包含于车辆传感器群4的GPS接收机41的数据作为初期位置信息。另外，也可以存储在上次的停车时最后计算出的车辆位置及姿势角，设为初期位置及姿势角信息。

而且，在移动了里程量的车辆的位置和姿势角的附近，考虑车辆的动特性或行驶状态而设定质点的存在分布范围。此时，在本实施方式中，如果考虑车辆的行驶状态而速度变高时，则使质点的存在分布范围在车辆的前后方向、车宽方向及偏航角方向扩展。

在此，参照图3，说明扩展质点的存在分布范围的理由。图3是示意性表示定速圆转弯中的车辆的转弯中心和各轮胎的侧滑角的图，图3(a)是极低速下的恒定圆转弯的情况，图3(b)是离心力的作用下的恒定圆转弯的情况。

如图3(a)所示，在极低速下由于未对车辆作用离心力，所以车辆沿轮胎的方向行进，向车宽方向的移动几乎不产生，因此，不易产生向车宽方向的里程的误差。

但是，如图3(b)所示，当车速变高时，则对车辆作用离心力，因此，对各车轮设定侧滑角δ，在车辆上产生向车宽方向的运动。因此，因路面状态或移动体的个体差等原因而产生的里程的误差与极低速时相比容易产生。即，作为车辆模型的特性，如果车速变高，则转弯中心Os变化，特别是车辆的前后方向的位置或车宽方向的位置、偏航角方向的姿势角容易产生误差。

因此，在车速变高时，如果使前后方向或车宽方向、偏航角方向上的质点的存在分布范围比里程扩大地扩展，则因产生误差而不能设定适当的质点的存在分布范围。

于是，在本实施方式中，当速度变高时，则使质点的存在分布范围向车辆的前后方向、车宽方向及偏航角方向扩展。

以下，参照图4～图6具体说明扩展质点的存在分布范围的方法。

首先，对将质点的存在分布范围向车辆的前后方向扩展的情况进行说明。如图4(a)所示，在由车速检测部15检测到的车速低于阈值Vlgth(km/h)时，从移动了里程量的位置向车辆的前后方向在±Rlg_min(m)的范围设定存在分布范围。而且，如果车速为阈值Vlgth以上，则在车辆的前后方向在±Rlg_max(m)的范围设定存在分布范围，扩展存在分布范围。

这是因为，如图3中所说明，如果车速变高，则车辆的转弯中心向车辆前方移动，因此，因路面状态或移动体的个体差等原因而产生的向车辆的前后方向的误差增大。

另外，如图4(b)所示，也可以根据车速使前后方向的存在分布范围连续地变化。

此外，在本实施方式中，例如将Vlgth、Rlg_min、Rlg_{_}max分别设为20[km/h]、0.5[m]、1.0[m]。其中，Vlgth是向车辆的前后方向的误差增大的速度。另外，Rlg_min是向车辆的前后方向的误差少的低速下的质点的存在分布范围，预先通过实验或模拟求出适当的值而进行设定。Rlg_max是向车辆的前后方向的误差增大的高速下的质点的存在分布范围，预先通过实验或模拟求出适当的值而进行设定。另一方面，前后方向以外的质点的存在分布范围如下设定。即，根据移动了里程量的位置和姿势角，在车宽方向、上下方向、侧倾、俯仰、偏航的各方向分别设为±0.5[m]、±0.1[m]、±0.5[deg]、±0.5[deg]、±3.0[deg]。

其次，对使质点的存在分布范围向车辆的车宽方向扩展的情况进行说明。如图5(a)所示，在车速低于阈值Vltth(km/h)时，从移动了里程量的位置向车辆的车宽方向在±Rlt_min(m)的范围设定存在分布范围。而且，如果车速为阈值Vltth以上，则在车辆的车宽方向在±Rlt_max(m)的范围设定存在分布范围，扩展存在分布范围。

这是因为，如图3中所说明，如果车速变高，则在转舵时产生在极低速未出现的车辆的车宽方向的运动，因此，因路面状态或移动体的个体差等原因而产生的向车辆的车宽方向的误差增大。

另外，如图5(b)所示，根据车速使车宽方向的存在分布范围连续地变化。

此外，在本实施方式中，例如将Vltth、Rlt_min、Rlt_max分别设为20[km/h]、0.2[m]、0.5[m]。其中，Vltth是对车辆作用离心力而使各车轮带有侧滑角，在车辆上产生向车宽方向的运动的速度。另外，Rlt_min是在车辆上未产生向车宽方向的运动的低速下的质点的存在分布范围，预先通过实验或模拟求出适当的值并进行设定。Rlt_max是在车辆上产生向车宽方向的运动这样的高速下的质点的存在分布范围，预先通过实验或模拟求出适当的值并进行设定。

其次，对使质点的存在分布范围向偏航角方向扩展的情况进行说明。如图6(a)所示，在车速低于阈值Vywth(km/h)时，从移动了里程量的姿势角向车辆的偏航角方向，在±Ryw_min(rad)的范围设定存在分布范围。而且，如果车速为阈值Vywth以上，则在车辆的偏航角方向，在±Ryw_max(rad)的范围设定存在分布范围，扩展存在分布范围。

这是因为，如图3中所说明，如果车速变高，则车辆的转弯中心向车辆前方移动，因此，因路面状态或移动体的个体差等原因而产生的向车辆的偏航角方向的误差增大。

另外，如图6(b)所示，也可以根据车速，使偏航角方向的存在分布范围连续地变化。

此外，在本实施方式中，例如将Vywth、Ryw_min、Ryw_max分别设为10[km/h]、0.02[rad]、0.05[rad]。其中，Vywth是对车辆作用离心力而使各车轮带有侧滑角，在车辆上产生向偏航角方向的运动的速度。另外，Ryw_min是在车辆上未产生向偏航角方向的运动的低速下的质点的存在分布范围，预先通过实验或模拟求出适当的值进行设定。Ryw_max是在车辆上产生向偏航角方向的运动这样的高速下的质点的存在分布范围，预先通过实验或模拟求出适当的值进行设定。

其次，在步骤S140中，质点散布部18在由步骤S130设定的存在分布范围内散布质点。作为散布方法，相对于规定质点的位置及姿势角的6自由度的参数，分别在由步骤S130所设定的范围(上下限)内使用随机数表等随机进行设定。另外，本实施方式中，总是生成500个质点。

此外，也可以使用专利文献1中公开的技术散布质点。该情况下，计算出在步骤S130中移动了里程量的车辆的位置和姿势角根据通过步骤S170中计算出的位置和姿势角修正了多少，只要根据该修正量设定散布质点的平均和分布进行散布即可。此时，散布质点的范围为步骤S130中设定的存在分布范围。另外，对于存在分布范围，也使用专利文献1所公开的技术求出，采用与步骤S130中设定的存在分布范围的不同的情况(OR)。而且，对于散布质点的数量，也可以使用专利文献1中公开的技术并根据步骤S130中设定的存在分布范围而动态地决定。

其次，在步骤S150中，投影图像生成部20对于步骤S140中散布的多个质点分别生成投影图像(假想图像)。此时，将例如存储于三维地图数据库3的边缘等三维位置信息投影变换为各预测位置及姿势角候补的摄像机图像，生成评价用的投影图像。投影于该投影图像的评价点群在后述的步骤S160中与步骤S110中计算出的边缘图像上的边缘进行比较。

另外，在投影变换中，需要表示摄像机2的位置的外部参数、和摄像机2的内部参数。外部参数只要通过预先测量车辆到摄像机2的相对位置并根据预测位置及姿势角候补进行计算即可。另外，内部参数只要预先进行校准即可。

此外，在步骤S110中从摄像机图像提取边缘的辉度变化的方向或边缘附近的彩色等的情况下，期望使用这些生成投影图像。

其次，在步骤S160中，相似度计算部22对于步骤S140中散布的多个质点分别比较步骤S110中计算出的边缘图像、和步骤S150中生成的投影图像。而且，基于比较的结果对作为预测位置及姿势角候补的质点分别计算相似度。该相似度是表示各预测位置及姿势角候补与实际的车辆的位置及姿势角相似多少的指标。相似度计算部22以投影图像和边缘图像的一致度越高，相似度越高的方式进行设定。以下说明该相似度的求取方法之一例。

首先，在投影图像上特定像素位置，判定在该像素位置是否存在边缘。而且，判定在成为与投影图像相同的像素位置的边缘图像上的位置是否存在边缘，在边缘的有无一致的情况下，作为相似度(likelihood)(单位：无)设定1，在不一致的情况下设定0。对全评价点进行这种处理，将作为其总和的一致评价点的数作为相似度。而且，对所有的预测位置及姿势角候补即各质点计算了相似度后，以各相似度的合计值为1的方式标准化。此外，相似度的计算方法除此之外还考虑许多计算方法，所以也可以使用这些方法。

其次，在步骤S170中，位置姿势角推定部24使用步骤S160中计算出的具有相似度信息的多个预测位置及姿势角候补，计算出最终的车辆的位置及姿势角。例如，位置姿势角推定部24将相似度最高的预测位置及姿势角候补作为车辆的实际的位置及姿势角进行计算。另外，也可以使用各预测位置及姿势角候补的相似度求出预测位置及姿势角的加权平均，将该值作为最终的车辆的位置及姿势角。如果这样计算出车辆的位置和姿势角的推定结果，则结束本实施方式的车辆的位置姿势角推定处理。

此外，在本实施方式中，将由车载的摄像机2拍摄的图像和三维地图数据库3进行匹配，推定车辆的位置和姿势角，但如专利文献1所公开，也可以使用激光传感器的测定值来推定车辆的位置和姿势角。

该情况下，三维地图数据库3具备例如由激光传感器可测定距车辆的距离或方位角的电桩等柱状障害物的位置信息、有关道路周边的建筑物或围墙等构造物的位置或形状的信息作为占有网格地图。

该占有网格地图以网格状的细微的单元分割环境，并附加各单元是否占据障害物的占有概率，由此来表现地图(概率机器人(確率ロボティクス)·第9章、每日通讯(毎日コミュニケーションズ)发行)。

而且，在图2的流程图的步骤S150中，在各质点的位置及姿势角有车辆的情况下，计算存储于三维地图数据库3中的各障害物或构造物如何配置，并将其投影于占有网格地图。

而且，在图2的流程图的步骤S160中，对于各质点，对将存储于三维地图数据库3的被各障害物或构造物占据的单元在占有网格地图上可以由激光传感器检测到几个进行计数，求出相似度。

这样，也可以变更图2的流程图的步骤S150及S160的处理，或者使用概率机器人(確率ロボティクス)·第8章(每日通讯(毎日コミュニケーションズ))所记载的方法来推定位置或姿势角。

[第一实施方式的效果]

如以上详细说明，在本实施方式的车辆位置姿势角推定装置中，如果车辆的速度变高，则将质点的存在分布范围向车辆的车宽方向扩展。由此，即使在车辆的速度变高，在车辆上产生了向车宽方向的移动的情况下，也能够适当地设定质点的存在分布范围，因此，可以正确地推定车辆的位置及姿势角。

另外，在本实施方式的车辆位置姿势角推定装置中，如果车辆的速度变高，则将质点的存在分布范围向车辆的偏航角方向扩展。由此，即使在车辆的速度变高而在车辆上产生了向偏航角方向的移动的情况下，也能够适当地设定质点的存在分布范围，因此，能够正确地推定车辆的位置及姿势角。

[第二实施方式]

其次，参照附图对本发明第二实施方式的车辆位置姿势角推定装置进行说明。

[车辆位置姿势角推定系统的构成]

图7是表示搭载有本实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图。如图7所示，本实施方式的车辆位置姿势角推定装置10还具备偏航率检测部75，这与第一实施方式不同。此外，其它构成由于与第一实施方式相同，所以标注同一符号并省略详细的说明。

偏航率检测部75通过取得由偏航率传感器48测量的传感器值，检测车辆的偏航角方向的变化率即偏航率。

[车辆的位置姿势角推定处理的顺序]

本实施方式的车辆的位置姿势角推定处理中，图2的步骤S130中执行的质点的存在分布范围的设定方法与第一实施方式不同。在第一实施方式中，根据车辆的速度设定质点的存在分布范围，但在本实施方式中，根据车辆的偏航率设定质点的存在分布范围，在这一点上不同。

在图2的步骤S130中，质点存在分布范围设定部16使1循环前的各质点的位置及姿势角移动里程量。而且，在移动的车辆的位置和姿势角的附近设定质点的存在分布范围，在本实施方式中，如果车辆的偏航率变高，则将质点的存在分布范围向车辆的车宽方向及偏航角方向扩展。

在此，参照图8，对在车辆的偏航率变高的情况下将质点的存在分布范围扩展的理由。图8是表示车辆转舵时的轮胎横力和偏航力矩的图，图8(a)是直行行驶的情况，图8(b)是使前轮转舵之后的情况，图8(c)是恒定圆转弯的情况。

如图8(a)所示，在将车辆的方向盘固定于中立位置，车辆向行进方向M直行时，如图3(b)中所说明，以使各车轮带有侧滑角δ那样的高的车速操作方向盘进行转舵。于是，如图8(b)所示，转舵之后，首先仅使前轮带侧滑角δ，在前轮上产生轮胎横力Ff。而且，通过该轮胎横力Ff产生偏航力矩Yf，开始转弯。在该状态下主要是偏航角方向的运动，因此，成为在偏航角方向的姿势角上容易出现误差的状态。

而且，如果维持图8(b)的转舵状态，则如图8(c)所示，前轮的轮胎横力Ff产生的偏航力矩Yf和后轮的轮胎横力Fr产生的偏航力矩Yr取得平衡，偏航率不增加，成为恒定圆转弯状态。在该状态下，在车辆的偏航角方向的运动的基础上，车宽方向的加速度增大，成为在车宽方向的位置上也容易出现误差的状态。

因此，在通过车辆转舵而偏航率变高时，如果不将车辆的车宽方向及偏航角方向上的质点的存在分布范围扩大扩展，则因产生误差而不能设定适宜的质点的存在分布范围。

因此，本实施方式中，如果偏航率变高，则将质点的存在分布范围向车辆的车宽方向及偏航角方向扩展。

以下，参照图9对扩展质点的存在分布范围的方法进行具体说明。

如图9(a)所示，在由偏航率检测部75检测到偏航率不足阈值γth(rad/s)时，从移动了里程量的位置向车宽方向在±Rltr_min(m)的范围设定存在分布范围。另外，从移动了里程量的姿势角向偏航角方向在±Rywr_min(rad)的范围设定存在分布范围。而且，如果偏航率为阈值γth以上，则在车宽方向在±Rltr_max(m)的范围设定存在分布范围，在偏航角方向在±Rywr_max(rad)的范围设定存在分布范围，扩展存在分布范围。

这是因为，如图8中所说明，如果偏航率或车宽方向的加速度变高，则在各车轮上产生侧滑角，在根据里程计算出的位置或姿势角容易产生误差。因此，在本实施方式中，考虑到因路面状态或移动体的个体差等原因而产生的车宽方向或偏航角方向的误差增大，将存在分布范围扩展设定。

另外，如图9(b)所示，也可以根据偏航率使存在分布范围连续地变化。

此外，在本实施方式中，例如将γth、Rltr_min、Rltr_max、Rywr_min、Rywr_max分别设为0.15[rad/s]、0.2[m]、0.5[m]、0.02[rad]、0.05[rad]。其中，γth是通过车辆的转舵而产生向车辆的偏航角方向或车宽方向的运动的偏航率。另外，Rltr_min、Rywr_min是在车辆上未产生向车宽方向或偏航角方向的运动的低的偏航率下的质点的存在分布范围，预先通过实验或模拟求出适当的值而进行设定。Rltr_max、Rywr_max是在车辆上产生向车宽方向或偏航角方向的运动这样高的偏航率下的质点的存在分布范围，预先通过实验或模拟求出适当的值而进行设定。

而且，也可以并用本实施方式的方法和第一实施方式的方法而设定质点的存在分布范围，该情况下，只要使用各方向的上下限值的中的大的值来设定存在分布范围即可。

另外，本实施方式的质点存在分布范围设定部16如下进行控制，如果车辆的偏航率变高，则将质点的存在分布范围向车辆的偏航角方向扩展起并经过规定时间后向车辆的车宽方向扩展。例如，向偏航角方向扩展后加入时间常数0.5[s]的暂时延迟再向车宽方向扩展。

这是因为，如图8中所说明，车辆的车宽方向的位置相对于偏航角方向的姿势角延迟地变化。

这样，在将偏航角方向的存在分布范围扩展后延迟并向车宽方向扩展，因此，可以与车辆的车宽方向的位置相对于偏航角方向的姿势角延迟变化相对应，适当地设定质点的存在分布范围。

此外，车辆的偏航角方向的移动相对于驾驶者的转向操舵存在转向机构或轮胎横力带来的延迟。因此，也可以相对于转向操舵延迟扩展偏航角方向的质点的存在分布范围的时间。即，当由驾驶者转向操舵时，例如也可以在加入时间常数0.2[s]的暂时延迟后，扩展偏航角方向的质点的存在分布范围。

[第二实施方式的效果]

如以上详细说明，在本实施方式的车辆位置姿势角推定装置中，如果车辆的偏航率变高，则将质点的存在分布范围向车辆的偏航角方向扩展。由此，即使在车辆的偏航率变高，在车辆上产生了向偏航角方向的移动的情况下，也可以适当地设定质点的存在分布范围，因此，可以正确地推定车辆的位置及姿势角。

另外，在本实施方式的车辆位置姿势角推定装置中，如果车辆的偏航率变高，则将质点的存在分布范围向车辆的偏航角方向扩展起并经过规定时间后向车辆的车宽方向扩展。由此，可以与车辆的车宽方向的位置相对于偏航角方向的姿势角延迟变化相对应，适当设定质点的存在分布范围。

[第三实施方式]

其次，参照附图说明本发明第三实施方式的车辆位置姿势角推定装置。

[车辆位置姿势角推定系统的构成]

图10是表示搭载有本实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图。如图10所示，本实施方式的车辆位置姿势角推定装置10还具备操舵角检测部105，在这一点上与第一实施方式不同。此外，其它结构与第二实施方式系统，所以标注同一符号并省略详细的说明。

操舵角检测部105通过取得由转向传感器43测量的传感器值，检测车辆的操舵角。

[车辆的位置姿势角推定处理的顺序]

本实施方式的车辆的位置姿势角推定处理在图2的步骤S130中执行的质点的存在分布范围的设定方法与第一实施方式不同。在第一实施方式中，根据车辆的速度设定质点的存在分布范围，但在本实施方式中，根据车辆的转向操舵角设定质点的存在分布范围，在这一点上是不同的。

在图2的步骤S130中，质点存在分布范围设定部16使1循环前的各质点的位置及姿势角移动里程量。而且，在移动的车辆的位置和姿势角的附近设定质点的存在分布范围，如果车辆的转向操舵角增大，则将质点的存在分布范围向车辆的前后方向、车宽方向及偏航角方向扩展。

以下，参照图11具体说明扩展质点的存在分布范围的方法。

如图11(a)所示，在由操舵角检测部105检测到的转向操舵角不足阈值θth时，根据移动了里程量的位置向车辆的前后方向在±Rlgs_min的范围设定存在分布范围，在车宽方向在±Rlts_min的范围设定存在分布范围。另外，根据移动了里程量的姿势角在偏航角方向在±Ryws_min(rad)的范围设定存在分布范围。而且，如果转向操舵角成为阈值θth以上，则在车辆的前后方向在±Rlgs_max(m)的范围设定存在分布范围，在车宽方向在±Rlts_max(m)的范围设定存在分布范围并扩展存在分布范围。另外，在偏航角方向在±Ryws_max(rad)的范围设定存在分布范围，并扩展存在分布范围。

这是因为，如图8中所说明，如果偏航率或车宽方向的加速度变高，则在各车轮上产生侧滑角而在位置或姿势角上产生误差，此时，如果为进行转向的状态，则该误差进一步扩大的可能性提高。

另外，如图11(b)所示，也可以根据转向操舵角使存在分布范围连续地变化。

此外，在本实施方式中，例如将θth、Rlgs_min、Rlts_min、Ryws_min分别设为3[rad]、0.2[m]、0.2[m]、0.02[rad]。另外，将Rlgs_max、Rlts_max、Ryws_max分别设为1.0[m]、0.5[m]、0.05[rad]。其中，θth是通过车辆的转舵而产生向车辆的偏航角方向或车宽方向的运动，误差扩大的转向操舵角。另外，Rlgs_min、Rlts_min、Ryws_min是在车辆上未产生向车宽方向或偏航角方向的运动的低的转向操舵角下的质点的存在分布范围，预先通过实验或模拟求出适当的值并进行设定。Rlgs_max、Rlts_max、Ryws_max是在车辆上产生向车宽方向或偏航角方向的运动这样的高的转向操舵角下的质点的存在分布范围，预先通过实验或模拟求出适当的值并进行设定。

而且，也可以并用本实施方式的方法或第一及第二实施方式的方法而设定质点的存在分布范围，该情况下，只要使用各方向的上下限值中最大的值设定存在分布范围即可。

[第三实施方式的效果]

如以上详细说明，在本实施方式的车辆位置姿势角推定装置中，如果车辆的操舵角增大，则将存在分布范围向车辆的车宽方向和偏航角方向扩展。由此，即使在操舵角增大，在车辆上产生了向车宽方向或偏航角方向的移动的情况下，也可以适当地设定质点的存在分布范围，因此，可以正确地推定车辆的位置及姿势角。

此外，上述的实施方式是本发明的一例。因此，本发明不限于上述的实施方式，即使是该实施方式以外的方式，只要在不脱离本发明的技术思想的范围，则当然可以根据设计等进行各种变更。

本申请基于2013年8月1日申请的日本国特许申请第2013－160074号主张优先权，该申请的内容通过参照被编入到本发明的说明书中。

符号说明

1ECU

2摄像机

3三维地图数据库

4车辆传感器群

10车辆位置姿势角推定装置

12边缘(edge)图像计算部

14里程(odometry)计算部

15车速检测部

16质点存在分布范围设定部

18质点散布部

20投影图像生成部

22相似度计算部

24位置姿势角推定部

41GPS接收机

42加速传感器

43转向传感器

44制动传感器

45车速传感器

46加速度传感器

47车轮速传感器

48偏航率传感器

75偏航率检测部

105转向角检测部

Claims (6)

1.一种车辆位置姿势角推定装置，使用质点过滤器设定规定范围的质点的存在分布范围，在所设定的存在分布范围内散布质点，根据拍摄车辆的周围环境得到的图像推定车辆的位置及姿势角，其特征在于，具备：

车速检测部，其检测所述车辆的速度；

质点存在分布范围设定部，其在由所述车速检测部检测到的车辆的速度变高时，将所述存在分布范围向所述车辆的车宽方向扩展。

2.如权利要求1所述的车辆位置姿势角推定装置，其特征在于，

所述质点存在分布范围设定部在所述车辆的速度变高时，将所述存在分布范围向以所述车辆的上下方向为轴的旋转方向即偏航角方向扩展。

3.如权利要求2所述的车辆位置姿势角推定装置，其特征在于，

还具备检测所述车辆的偏航角方向的变化率即偏航率的偏航率检测部，

所述质点存在分布范围设定部在由所述偏航率检测部检测到的偏航率变高时，将所述存在分布范围向所述偏航角方向扩展。

4.如权利要求3所述的车辆位置姿势角推定装置，其特征在于，

所述质点存在分布范围设定部在所述车辆的偏航率变高时，在将所述存在分布范围向所述车辆的偏航角方向扩展起经过规定时间后，将所述存在分布范围向所述车辆的车宽方向扩展。

5.如权利要求1～4中任一项所述的车辆位置姿势角推定装置，其特征在于，

还具备检测所述车辆的转向角的转向角检测部，

所述质点存在分布范围设定部在由所述转向角检测部检测到的转向角变大时，将所述存在分布范围向所述车辆的车宽方向和偏航角方向扩展。

6.一种车辆位置姿势角推定装置的车辆位置姿势角推定方法，使用质点过滤器设定规定范围的质点的存在分布范围，在所设定的存在分布范围内散布质点，根据拍摄车辆的周围环境得到的图像推定车辆的位置及姿势角，其特征在于，

所述车辆位置姿势角推定装置检测所述车辆的速度，当所述车辆的速度变高时，将所述存在分布范围向所述车辆的车宽方向扩展。