CN107615201A - 自身位置估计装置及自身位置估计方法 - Google Patents

Abstract

本发明的自身位置估计装置检测在移动体的周围存在的地标的地标位置而检测移动体的移动量，使检测到的地标位置移动移动量并将其作为地标位置数据进行累积。而且，获取包含地图上存在的地标的地标位置的地图信息，将基于移动体的直至当前位置为止的移动履历而设定的规定范围的地标位置数据和地图信息中包含的地标位置进行对照，估计移动体的自身位置。

Description

自身位置估计装置及自身位置估计方法

技术领域

本发明涉及估计移动体的自身位置的自身位置估计装置及其方法。

背景技术

正在开发使用搭载于移动体上的摄像头或者激光测距仪来传感移动体的周围，将其结果和地图进行匹配，求地图上的位置或者姿态角的自身位置估计技术，目前公开有专利文献1。

在专利文献1的自身位置估计技术中，将通过车轮速度脉冲或者陀螺仪等计算出的移动量和传感的结果的过去的履历进行累积并连结，进行调节，以使该结果和地图一致，从而估计移动体的位置和姿态角。另外，在专利文献1的自身位置估计技术中，通过总是使用固定距离的传感结果的履历，排除移动体的移动速度的影响，稳定地进行自身位置的估计。

现有技术文献

专利文献

专利文献1：日本特开2008-250906号公报

发明内容

但是，在上述的现有自身位置估计技术中，使用了固定距离的传感结果的履历，所以在移动量的检测误差增大的状况下，存在传感结果和地图之间的背离增大，难以进行自身位置的估计的问题。

因此，本发明是鉴于上述的实际情况而提出，其目的在于，提供即使在移动量的检测误差增大的状况下，也能够高精度且稳定地估计自身位置的自身位置估计装置及其方法。

为了解决上述的课题，本发明一方式的自身位置估计装置及其方法，检测在移动体的周围存在的地标的地标位置而检测移动体的移动量。而且，使检测到的地标位置移动移动量并将其作为地标位置数据进行累积，获取包含地图上存在的物体的地标位置的地图信息。之后，将基于移动体的直至当前位置为止的移动履历所设定的规定范围的地标位置数据和地图信息中包含的地标位置进行对照，估计移动体的自身位置。

附图说明

图1是表示具备本发明第一实施方式的自身位置估计装置的自身位置估计系统的结构的框图。

图2是表示激光测距仪和摄像头对车辆的搭载位置的图。

图3是表示本发明第一实施方式的自身位置估计装置进行的自身位置估计处理的处理步骤的流程图。

图4是用于说明本发明第一实施方式的自身位置估计装置中采用的坐标系的图。

图5是用于说明本发明第一实施方式的自身位置估计装置的激光测距仪的检测方法的图。

图6是用于说明本发明第一实施方式的自身位置估计装置的摄像头的白线的检测方法的图。

图7是表示本发明第一实施方式的自身位置估计装置进行的地标的检测结果的图。

图8是用于说明本发明第一实施方式的自身位置估计装置进行的移动量的检测误差的估计方法的图。

图9是用于说明本发明第一实施方式的自身位置估计装置的提取范围的设定方法的图。

图10是用于说明由本发明第一实施方式的自身位置估计装置获取的链路节点信息的图。

图11是用于说明本发明第一实施方式的变形例5的自身位置估计装置的提取范围的设定方法的图。

图12是用于说明现有自身位置估计技术的图。

图13是用于说明现有自身位置估计技术的图。

图14是用于说明现有自身位置估计技术的图。

图15是用于说明本发明第一实施方式的自身位置估计装置的提取范围的设定方法的图。

图16是表示本发明第二实施方式的自身位置估计装置进行的自身位置估计处理的处理步骤的流程图。

具体实施方式

以下，参照附图说明适用了本发明的第一实施方式及第二实施方式。

[第一实施方式]

[自身位置估计系统的结构]

图1是表示具备本实施方式的自身位置估计装置的自身位置估计系统的结构的框图。如图1所示，本实施方式的自身位置估计系统包括ECU1、摄像头2、三维地图数据库3、车辆传感器群4、激光测距仪5。

这里，ECU1是由ROM、RAM、运算电路等构成的电子控制单元，在ROM中存储有实现本实施方式的自身位置估计装置10的程序。此外，ECU1也可以兼作其它控制使用的ECU。

摄像头2(2a、2b)使用CCD等固体摄像元件，例如，如图2所示设置于车辆的左右的车门后视镜上，并朝向可拍摄车辆下方的路面的方向。拍摄到的图像被发送到ECU1。

三维地图数据库3是存储包含地图上存在的地标的地标位置的地图信息的存储单元，例如存储有包含道路标识的周围环境的三维位置信息。于地图信息中所记录的地标(landmark)被注册在地图中，除车道线、停车线、人行横道、路面标记等路面标识之外，还包含路缘石等路面上的构造物或建筑物等。白线等各地图信息被被定义为边缘的集合体。在边缘为长的直线的情况下，例如，按每1m(米)进行划分，因此，不存在极长的边缘。在直线的情况下，各边缘具有表示直线的两端点的三维位置信息。在弯道的情况下，各边缘具有表示弯道的两端点和中央点的三维位置信息。另外，三维地图数据库3存储有一般的导航系统所具有的节点链路信息，所以，能够通过ECU1进行直至目的地为止的路径导引或者记录过去的行驶路径的处理。

车辆传感器群4包括GPS接收机41、加速传感器42、转向传感器43、制动传感器44、车速传感器45、加速度传感器46、车轮速度传感器47、横向偏摆率传感器48。车辆传感器群4连接到ECU1，将由各传感器41～48检测到的各种检测值供给到ECU1。ECU1通过使用车辆传感器群4的输出值，计算车辆的大概位置、或者计算表示每单位时间车辆前进的移动量的里程数。

如图2所示，安装激光测距仪5(5a、5b)，以能够扫描车身的左右方向。激光测距仪5分别在从正下方向到水平方向的90[deg]的扫描范围内，以0.1[deg]刻度且与行进方向垂直地照射900束×2＝1800束激光。由此，能够检测到路面或障碍物的距离εi[m](i＝1～1800)和反射波的强度δi(i＝1～1800、0≤δi≤255)。

自身位置估计装置10作为执行自身位置估计处理的控制单元起作用，通过将车辆的周围存在的物体的地标位置和存储于三维地图数据库3的地图信息进行匹配，估计车辆的位置及姿态角。自身位置估计装置10通过执行存储在ROM中的自身位置估计用的程序，作为地标位置检测单元12、移动量检测单元14、地标位置累积单元16、地图信息获取单元18及自身位置估计单元20进行动作。

地标位置检测单元12根据激光测距仪5的扫描结果，检测在车辆的周围存在的地标的地标位置。另外，地标位置检测单元12也可以根据摄像头2的图像，检测地标位置，也可以使用摄像头2和激光测距仪5双方检测地标位置。

移动量检测单元14基于来自搭载于车辆上的车辆传感器群4的信息，检测车辆的移动量即里程数。

地标位置累积单元16使在以前执行的各控制周期中由地标位置检测单元12检测到的地标位置移动由移动量检测单元14检测到的移动量的量，并作为地标位置数据进行累积。

地图信息获取单元18从三维地图数据库3获取地图信息，获取的地图信息中包含地图上存在的物体的地标位置。

自身位置估计单元20在本实施方式中，基于车辆的行驶履历(相当于移动体的移动履历)的过去的移动量变化，估计由移动量检测单元14检测到的移动量的检测误差。然后，自身位置估计单元20基于从车辆的行驶履历的过去的移动量变化所估计出的移动量的检测误差，设定从地标位置累积单元16提取地标位置数据时的规定范围(后述的提取范围A)，提取基于地标位置累积单元16中累积的地标位置数据之中的车辆的行驶履历设定的规定范围(提取范围A)内包含的地标位置数据，将所提取的地标位置数据和地图信息中包含的地标位置进行对照，估计车辆的自身位置。

此外，在本实施方式中，说明适用于车辆的情况，但也可以适用于飞机或船舶等移动体。在适用于飞机或船舶的情况下，作为周围环境，取代有关道路标识的信息而与地形或建筑物进行匹配，由此能够估计移动体的自身位置。

[自身位置估计处理的步骤]

接着，参照图3的流程图，说明本实施方式的自身位置估计处理的步骤。此外，在本实施方式中，在估计车辆的自身位置时，使用图4所示的两个坐标系。即，是以地图信息的原点为中心的绝对坐标系和以车辆的后轮车轴中心为原点的相对空间坐标系。绝对坐标系中，将地图信息的原点设为原点O、将东西方向设为X轴、将南北方向设为Y轴，将垂直上方设为Z轴。在该绝对坐标系中，车辆朝向的方位角(平摆角(yaw))θ[rad]用将东方向(X轴方向)设为0时的逆时针的角度来表示。另外，相对空间坐标系将车辆的后轮车轴中心设为原点o，将车辆的前后方向设为x轴，将车宽方向设为y轴，将垂直上方设为z轴。

此外，在本实施方式中，作为估计的车辆的自身位置，估计东西方向(X轴方向)的位置(X坐标[m])、南北方向(Y轴方向)的位置(Y坐标[m])，作为姿态角信息，估计车辆的方位角θ(平摆角[rad])的合计3自由度的位置和姿态角。但是，不仅可以求得3自由度的位置和姿态角，而且可以求得6自由度的空间上的位置和姿态角。另外，以下说明的自身位置估计处理例如以100msec左右的间隔连续地进行。

如图3所示，首先，在步骤S10中，地标位置检测单元12检测在车辆的周围存在的地标的地标位置。具体而言，地标位置检测单元12获取激光测距仪5的扫描结果，提取车道线或停车线等路面标识或路缘石、房屋等构造物的地标位置。在此提取的地标位置是相对于相对空间坐标系的车辆的相对平面位置坐标(xj(t)、yj(t))[m]。再者，xj、yj的j与所提取的路面标识或路缘石的数目相等。另外，t表示当前的时刻(周期)。

路面标识的相对平面位置坐标的求取方法为，当照射的激光的反射波的强度δi为阈值δth以上时，则判断为激光照射到路面标识，根据照射了激光的点相对自车的距离εi和角度求相对平面位置坐标。这样求取的理由是因为，因为路面标识部分含有大量反射材料，所以与路面的沥青相比，可更强地反射激光。另外，阈值δth预先通过实验等而求得，存储在ECU1中即可，在本实施方式中，对阈值8th设定120。

另外，关于路缘石或建筑物等构造物的地标位置的求取方法，根据直到各激光照射到的部位为止的距离εi和角度，计算并求各照射点的相对平面位置坐标(xj(t)、yj(t))。具体而言，例如，在图5(a)所示的状况中，当从激光测距仪5照射激光时，激光如线53所示对包含路缘石51的路面进行照射。当检测该照射的激光的点群时，则如图5(b)所示，检测到大幅变化的部位55。因此，在检测到的点群中，使用霍夫(Hough)变换等线段检测方法，提取点群的斜度大幅变化的部位，计算该部位的相对平面位置坐标(xj(t)、yj(t))。即使在路面上的构造物为建筑物的情况下，只要提取点群的斜度大幅变化的部位，则同样能够求相对平面位置坐标(xj(t)、yj(t))。

另外，在步骤S10中，地标位置检测单元12也可以获取摄像头2的图像，从摄像头2的图像提取车道线等道路标识的相对平面位置坐标(xj(t)、yj(t))。参照图6说明从摄像头2的图像提取道路标识的相对平面位置坐标的方法。图6(a)表示拍摄车辆的左下方的摄像头2a的图像60a、和拍摄车辆的右下方的摄像头2b的图像60b。在这些图像60a、60b中，地标位置检测单元12对区域61a、61b进行白线的检测。例如，对区域61a、61b进行2值化处理并提取亮度值高的范围63，如图6(b)所示，检测亮度值高的部分65。然后，如图6(c)所示，计算亮度值高的部分65的重心位置67，根据摄像头2的内部参数(摄像头型号)和外部参数(摄像头2的车辆安装位置)，求重心位置67的相对平面位置坐标(xj(t)、yj(t))。

再者，在车辆的发动机起动之后、或者在车辆为电动汽车的情况下驱动用的电源刚成为“ON”之后，就不再累积激光测距仪5的扫描结果。因此，在本实施方式中，在车辆的发动机成为“OFF”、或者车辆为电动汽车的情况下驱动用的电源成为“OFF”时，从当前位置提取后述的提取范围A[m]的范围内的扫描结果，将其记录在ECU1的存储器或记录介质中。而且，在车辆的发动机起动、或者在车辆为电动汽车的情况下驱动用的电源成为“ON”的瞬间，读取所记录的扫描结果。

另外，步骤S10中提取的车道线或路缘石、建筑物等的相对平面位置坐标(xj(t)、yj(t))，通过执行后述的处理，被作为当前时刻t的相对平面位置坐标持续记录，直至车辆的发动机成为“OFF”为止。另外，在车辆为电动汽车的情况下，持续记录，直至驱动用的电源成为“OFF”为止。

接着，在步骤S20中，移动量检测单元14基于从车辆传感器群4获得的传感器信息，检测从1周期前至当前的时刻t为止的车辆的移动量即里程数。该里程数是车辆单位时间内前进的移动量。例如，在本实施方式中，对于平摆角的移动量，将从横向偏摆率传感器48获取的偏摆率γ[rad/s]乘以ECU1的运算周期100msec，求平摆角的变化量Δθ(t)[rad]。另外，关于并行方向的移动量，对从车速传感器45获取的车速V[m/s]乘以ECU1的运算周期100msec，求并行方向的移动量ΔL(t)[m]。进而，在计算里程数时，也可以通过测量车辆的各车轮的轮胎参数，使用2轮模型等，估计并算出车身的侧滑角或车身的侧滑。

在步骤S30中，地标位置累积单元16使由地标位置检测单元12检测到的地标位置移动由移动量检测单元14检测到的移动量，并将其作为地标位置数据进行累积。若具体地进行说明，则使在以前的各周期中在步骤S10获取的车道线或路缘石等地标的相对平面位置坐标(xj(t)、yj(t))移动了在步骤S20中获取的里程数的量。换言之，使用里程数信息将由激光测距仪5或摄像头2的任一方或双方在过去获取的车道线或路缘石等的地标位置变换成以当前的车辆的后轮车轴中心为原点的相对空间坐标系。

由此，例如，如图7(b)所示，能够根据激光测距仪5的扫描结果指定地标的位置。图7(b)是表示从图7(a)所示的上空观察到的车辆在小道上以箭头71的路径行驶时累积的地标位置数据的图。即，图7(b)是表示使在步骤S10中由激光测距仪5检测到的地标的位置信息移动了在步骤S20中检测到的移动量并进行累积的结果的一个例子。图7(b)中，路面的反射强度高的点、例如与车道线或停车线、人行横道的位置一起表示有激光的检测结果大幅变化的路缘石的位置。

在步骤S40中，自身位置估计单元20基于车辆的行驶履历的过去的移动量变化，估计车辆的移动量的检测误差，基于估计出的移动量的检测误差，设定从地标位置累积单元16提取地标位置数据时的规定范围(提取范围A)。特别是，在本实施方式中，根据车辆的行驶履历的过去的移动量变化估计移动量的检测误差，作为行驶履历的过去的移动量变化的具体例子，估计车辆的当前位置和规定时间前的位置、或根据追溯了规定距离的位置处的过去的移动量变化估计移动量的检测误差。

如图8所示，自身位置估计单元20将距离行驶履历一个周期前的由步骤S50算出的车辆的自身位置80(X(t)、Y(t)、θ(t))和比当前靠前时间T[s]的车辆的自身位置82(X(t-T)、Y(t-T)、θ(t-T))的移动量变化进行比较。但是，这里所说的自身位置是绝对坐标系的东西方向的位置(X坐标[m])、南北方向的位置(Y坐标[m])、作为姿态角信息将东方向设为0时的逆时针的方位角θ(平摆角[rad])。

而且，图8中，从行驶履历靠前时间T[s]的车辆的自身位置82，从当前的自身位置80的车辆的方向观察，存在沿车宽方向偏离绝对值Δy[m]的过去的移动量变化。在该车宽方向的过去的移动量变化即偏离量的绝对值Δy[m]为阈值yth[m]以上的情况下，自身位置估计单元20可估计为移动量的检测误差大。另一方面，在车宽方向的过去的移动量变化即偏离量的绝对值Δy[m]比阈值yth[m]小的情况下，自身位置估计单元20可估计为移动量的检测误差小。

如图8所示，车辆沿车宽方向移动情况，可以估计为进行里程数的误差容易累积的移动量变化大的转弯或车道变更、右/左转、弯路的行驶，因此，可以估计为移动量的检测误差大。

而且，自身位置估计单元20在根据行驶履历的过去的移动量变化估计为移动量的检测误差小的情况下，在后述的步骤S50中增大提取地标位置数据时的提取范围A[m]，例如设定为200m。另一方面，自身位置估计单元20在根据行驶履历的过去的移动量变化估计为移动量的检测误差大的情况下，减小提取范围A[m]，例如设定为100m。另外，如图9所示，也可以根据行驶履历，与过去的移动量变化即偏离量Δy增大的情况相应地使提取范围A以连续减小的方式进行变化。即，进行设定，使得随着根据行驶履历的过去的移动量变化而估计的移动量的检测误差增大，提取范围A变小。这样，在根据行驶履历的过去的移动量变化估计为移动量的检测误差大的情况下，通过减小提取范围A，使里程数的误差的累积减少。

再者，阈值yth只要设定为例如50m(米)即可。另外，提取范围A的减少幅度是预先通过实验或模拟验证步骤S50的匹配状态，设定最佳的值的幅度，除了从200m减少至100m之外，也可以设定为其它值。

另外，不仅使用车宽方向的移动量，而且还使用车辆通过的十字路口的数作为行驶履历，也可以估计过去的移动量变化，估计移动量的检测误差的大小。该情况下，首先将提取范围A设定为200m，根据在三维地图数据库3中注册的链路节点信息，对在提取范围A中通过十字路口的次数进行计数作为行驶履历。

而且，在作为行驶履历通过十字路口的次数低于3次的情况下，过去的移动量变化小，所以估计为移动量的检测误差减小，将提取范围A仍设定为200m。另一方面，在作为行驶履历通过十字路口的次数为3次以上的情况下，过去的移动量变化大，所以估计为移动量的检测误差增大，将提取范围A设定为100m。另外，除此之外，探索提取范围A中的地图上的最大坡度，例如在作为行驶履历包含最大坡度为4％以上的区间的情况下，过去的移动量变化大，所以估计为移动量的检测误差增大，也可以将提取范围A设定为100m。

这样，作为行驶履历，除车辆的当前位置和规定时间前的位置之外，还可以参照车辆的行驶路径上的地图信息来估计过去的移动量变化，估计移动量的检测误差。由此，在行驶路径中包含伴随右转/左转或者停止、起步的十字路口或急坡度的区间等移动量变化大、里程数的误差容易累积的点的情况下，可以减小提取范围A，使里程数的误差的累积减少。

在步骤S50中，自身位置估计单元20提取累积于地标位置累积单元16的地标位置数据之中在步骤S40中设定的提取范围A内包含的地标位置数据。而且，将提取到的地标位置数据和地图信息中包含的地标位置进行对照，估计车辆的自身位置。

具体而言，自身位置估计单元20从步骤S30中累积的车道线或者路缘石等地标位置数据之中提取位于在步骤S40中设定的提取范围A的范围内的地标位置数据。此时，地标位置数据从当前时刻t追溯并累计在步骤S20算出的移动量ΔL(t)，提取该累计值直至超过提取范围A为止。

而且，将提取的地标位置数据和三维地图数据库3中存储的地图信息的地标位置进行匹配，估计车辆在绝对坐标系中的自身位置。即，估计由车辆的东西方向的位置(X坐标)、南北方向的位置(Y坐标)、方位角(平摆角θ)构成的合计3自由度的位置和姿态角。如果这样估计出自身位置，则本实施方式的自身位置估计处理结束。

再者，步骤S50中的匹配使用ICP(Iterative Closest Point；迭代最近点)算法。此时，三维地图数据库3的地图信息中包含的地标位置之中的例如车道线，以其两端的端点为评价点进行匹配。另外，越接近车辆(摄像头2)，地标位置数据越不受到里程数的误差的影响，所以对车辆的附近进行直线增补，使评价点的数增加，对车辆的远方使评价点的数减少。

[变形例1]

作为本实施方式的变形例1，在步骤S40中根据车辆的行驶履历估计移动量的检测误差时，作为行驶履历的具体例子，车辆的右转/左转次数越多，过去的移动量变化越大，所以估计为移动量的检测误差大，缩小提取范围A。该情况下，首先将提取范围A设定为200m，根据在三维地图数据库3中注册的链路节点信息，计算在提取范围A的范围内，车辆在十字路口进行右转/左转的次数。

再者，如图10所示，链路节点信息中记录有分支的链路(箭头)和节点(圆圈标志)，各链路中被给予可判断直行、右转、左转、汇流、分支那样的属性信息。因此，通过检测车辆通过了哪个链路并参照该链路节点信息，能够获得车辆的右转/左转信息。

而且，作为行驶履历，在十字路口进行右/左转的次数低于2次的情况下，过去的移动量变化小，因此，估计为移动量的检测误差小，增大提取范围A，例如仍设定为200m。另一方面，作为行驶履历，在十字路口进行右转/左转的次数为2次以上的情况下，过去的移动量变化大，估计为移动量的检测误差大，减小提取范围A，例如设定为100m。此时，也可以使提取范围A根据右转/左转的次数而连续地变化。

这样，在进行里程数的误差容易累积的右转/左转的情况下，减小提取范围A，使里程数的误差的累积减少。

[变形例2]

作为变形例2，作为步骤S40的行驶履历的具体例子，车辆的车道变更的次数越多，过去的移动量变化越大，因此，估计为移动量的检测误差大，缩小提取范围A。该情况下，首先将提取范围A设定为200m，根据在三维地图数据库3中注册的链路节点信息，计算在提取范围A的范围内车辆进行车道变更的次数。如图10所示，在链路节点信息中，对于存在多个车道的公路，对每个车道个别地设定链路信息，所以只要参照链路节点信息就能够计算车道变更的次数。

而且，作为行驶履历，在车道变更的次数为0次的情况下，过去的移动量变化小，所以估计为移动量的检测误差小，增大提取范围A，例如仍设定为200m。另一方面，作为行驶履历，在车道变更的次数为1次以上的情况下，过去的移动量变化大，所以估计为移动量的检测误差大，减小提取范围A，例如设定为100m。

这样，在进行了里程数的误差容易累积的车道变更的情况下，减小提取范围A，使里程数的误差的累积减少。

[变形例3]

作为变形例3，作为步骤S40的行驶履历的具体例子，车辆的分流或汇流的次数越多，过去的移动量变化越大，因此，估计为移动量的检测误差大，减小提取范围A。该情况下，首先将提取范围A设定为200m，根据在三维地图数据库3中注册的链路节点信息，计算在提取范围A的范围内车辆分流或汇流的次数。关于分流或汇流的判断，如图10所示，只要参照赋予链路节点信息的各链路的属性信息即可。图10中未图示汇流、分支的链路，但因为对各链路赋予了可判断直行、右转、左转、汇流、分支那样的属性信息，所以只要参照该属性信息就能够判断车辆进行分流或汇流。

而且，作为行驶履历，在分流或汇流的次数为0次的情况下，过去的移动量变化小，所以估计为移动量的检测误差小，增大提取范围A，例如仍设定为200m。另一方面，作为行驶履历，在分流或汇流的次数为1次以上的情况下，过去的移动量变化大，所以估计为移动量的检测误差大，减小提取范围A，例如设定为180m。

这样，在进行里程数的误差容易累积的分流或汇流的情况下，减小提取范围A，使里程数的误差的累积减少。

[变形例4]

作为变形例4，作为步骤S40的行驶履历的具体例子，车辆行驶的弯道的曲率半径越小，过去的移动量变化越大，所以估计为移动量的检测误差大，缩小提取范围A。该情况下，首先将提取范围A设定为200m，根据在三维地图数据库3中注册的链路节点信息，检测在提取范围A的范围内车辆行驶的弯道的曲率半径。曲率半径被记录在链路节点信息的各链路中，所以只要特定车辆行驶的链路就能够检测曲率半径。

而且，作为行驶履历，在车辆行驶的弯道的曲率半径大于50m的情况下，因为过去的移动量变化小，所以估计为移动量的检测误差小，增大提取范围A，例如仍设定为200m。另一方面，作为行驶履历，在车辆行驶的弯道的曲率半径为50m以下的情况下，过去的移动量变化大，所以估计为移动量的检测误差大，减小提取范围A，例如设定为100m。此时，也可以使提取范围A根据曲率半径而连续地变化。

这样，车辆在里程数的误差容易累积的曲率半径小的弯道行驶的情况下，减小提取范围A，使里程数的误差的累积减少。

[变形例5]

作为变形例5，也可以使用上述的实施方式和变形例1～4的多个行驶履历的形式估计过去的移动量变化，估计移动量的检测误差，设定提取范围A。该情况下，如图11所示，对假定为行驶履历的过去的移动量变化即偏离量Δy或右转/左转次数等过去的移动量变化大的每一误差要素分别设定提取范围A的减少量ΔA，在各误差要素中估计为检测误差大的情况下，要分别从提取范围A减少各减少量ΔA。例如，图11中，在行驶履历中偏离量Δy为50m以上、十字路口通过次数为3次以上、右/左转次数为2次以上的情况下，将减少量ΔA各自设定为20m，在分流或汇流、车道变更的次数为1次以上的情况下，将减少量ΔA设定为10m。另外，在行驶履历中车辆行驶的弯道的曲率半径为50m以下的情况下，将减少量ΔA设定为50m。而且，在估计为移动量的检测误差大的误差要素存在多个的情况下，从预先设定的200m减少这些减少量ΔA之和而设定提取范围A。例如，在行驶履历中右转/左转次数为2次以上且车道变更有1次的情况下，从200m减少各减少量20m和10m之和即30m，将提取范围A设定为170m。此时，如果减少量ΔA之和过大、提取范围A过小，则难以进行步骤S50中所执行的匹配，所以将提取范围A的最小值设定为例如100m。另外，就减少幅度ΔA而言，预先通过实验或模拟而验证步骤S50的匹配，并设定最佳的值。

[第一实施方式的效果]

接着，说明本实施方式的自身位置估计装置的效果。首先，对现有的自身位置估计技术进行说明，在现有技术中，通过将移动量和传感结果的履历进行累积并连结，估计了自身位置。例如，图12(a)是将拍摄车辆左右的方向向下的鱼眼摄像头的图像进行俯瞰变换后的图。如图12(a)所示，当前的俯瞰图像121较小，但如图12(b)所示，如果在当前的俯瞰图像121中追加将过去的移动量和传感结果(俯瞰图像)的履历进行累积并连结的部分123，则能够获得过去的一定区间的俯瞰图像。因此，即使存在驻车车辆等障碍物，也可以使用过去的传感结果估计自身位置。

此时，在现有自身位置估计技术中，通过总是使用固定距离的传感结果的履历而排除移动体的移动速度的影响，以便稳定地进行自身位置的估计。这是因为，使用了固定时间的传感结果的履历的情况下，当移动体极低速或者停止时，则变得与仅利用当前的传感结果的情况相同。另外，当移动速度为高速时，连结传感结果的履历所获得的区域扩大，用于调节的运算负荷增大，以使与地图信息一致。

但是，在上述的现有自身位置估计技术中，因为使用了固定距离的传感结果的履历，所以在移动量的检测误差增大的状况中，存在传感结果和地图信息之间的背离增大而自身位置的估计变得困难这样的问题。

例如，如图13所示，对车辆的行驶履历以恒定速度直行的情况131和反复进行包含加减速的右/左转的情况133中连结相同距离的传感结果的情况进行比较。

图14是表示该比较结果的图，图中的黑色圆圈表示由搭载在车辆上的激光测距仪每时每刻检测到的白线边缘的位置，白色圆圈表示按照现有自身位置估计技术连结过去的传感结果的结果。

如图14(a)所示，在车辆的行驶履历以一定速度直行的情况下，车辆的过去的移动量变化小，因此，移动量的检测误差减小，检测到的白线边缘(黑色圆圈)和所连结的传感结果(白色圆圈)之间的背离减小。

与此相反，如图14(b)所示，在车辆的行驶履历中车辆反复进行包含加减速的右转/左转的情况下，过去的移动量变化大，其结果是，移动量的检测误差增大，所以随着从当前向过去追溯，检测到的白线边缘(黑色圆圈)和所连结的传感结果(白色圆圈)之间的背离增大。在这样的情况下，考虑通过将移动量的检测所使用的车辆模型制成更严格的模型来减小移动量的检测误差的对策。但是，如果以车辆为例，则乘员或者搭载燃料的变化所导致的车身的质量变化或路面的摩擦系数的变化难以严格地模型化，难以解决上述的问题。

因此，在本实施方式的自身位置估计装置中，根据车辆的行驶履历来估计移动量的检测误差，与估计的检测误差增大的情况相应地减小从累积的地标位置数据中提取的数据的范围。例如，如图15所示，在移动量的检测误差小的直行的情况下，扩大提取范围151，在反复进行检测误差大的右转/左转的情况下，缩小提取范围153。

这样，在本实施方式的自身位置估计装置中，基于行驶履历设定地标位置数据的规定范围(提取范围A)，将从规定范围(提取范围A)提取的地标位置数据和地图信息中包含的地标位置进行对照，估计车辆的自身位置。由此，在行驶履历中车辆反复进行右转/左转那样的情况下，因为过去的移动量变化大，所以检测误差一样大，能够缩小规定范围(提取范围A)，在与地图信息的背离非常小的范围，将地标位置数据和地图信息的地标位置进行对照。另外，在行驶履历中车辆以定速直行那样的情况下，因为过去的移动量变化小，所以检测误差小，能够扩大规定范围(提取范围A)，将更多的地标位置数据和地图信息的地标位置进行对照。因此，基于行驶履历，不仅是移动量的检测误差小的状况，即使是检测误差大那样的状况，也能够高精度且稳定地估计自身位置。

另外，在本实施方式的自身位置估计装置中，行驶履历中车辆的右转/左转次数越多，越缩小规定范围。在十字路口的右转/左转不仅是车辆的转弯，而且在其前后还伴有加减速，过去的移动量变化大，所以车身的前后方向上的移动量的检测误差增大。因此，在移动量的检测误差容易累积的右转/左转次数多的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

进而，在本实施方式的自身位置估计装置中，行驶履历中车辆的车道变更的次数越多，越缩小规定范围。在车辆中进行车道变更时，过去的移动量变化大，产生非线性的车身的侧滑运动，所以难以高精度地估计移动量，移动量的检测误差增大。因此，在移动量的检测误差容易累积的车道变更的次数多的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

另外，在本实施方式的自身位置估计装置中，行驶履历中车辆的分流或汇流的次数越多，越缩小规定范围。在车辆的分流或汇流中，车道的变更或者转弯之类的过去的移动量变化大，产生如移动量的检测误差扩大那样的举动。因此，在移动量的检测误差容易累积的分流或汇流的次数多的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

进而，在本实施方式的自身位置估计装置中，行驶履历中车辆行驶的弯道的曲率半径越小，越缩小规定范围。当以高车速在急的弯道行驶时，与车道变更相同，过去的移动量变化大，产生非线性的车身的侧滑运动，所以，难以高精度地估计移动量。因此，在移动量的检测误差容易累积的曲率半径小的弯道行驶的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

[第二实施方式]

接着，参照附图说明本发明第二实施方式的自身位置估计装置。再者，本实施方式的自身位置估计系统的结构与第一实施方式相同，所以省略详细的说明。

[自身位置估计处理的步骤]

参照图16的流程图说明本实施方式的自身位置估计处理的步骤。第一实施方式中，在步骤S40中，根据车辆的行驶履历估计移动量的检测误差。但是，在本实施方式中，在步骤S140中关注行驶履历中的车辆举动，设定规定范围(提取范围A)的情况与第一实施方式不同。此外，步骤S10～S30和步骤S50与图3的第一实施方式相同，因此省略详细的说明。

在步骤S140中，自身位置估计单元20根据行驶履历估计车辆的移动量的检测误差，基于估计出的移动量的检测误差，设定从地标位置累积单元16提取地标位置数据时的规定范围(提取范围A)。特别是，在本实施方式中，根据行驶履历中的过去的移动量变化即车辆举动来估计移动量的检测误差，作为车辆举动的具体例子，根据车辆的转弯量来估计移动量的检测误差。

自身位置估计单元20根据行驶履历计算在一个周期前的步骤S50中算出的车辆的姿态角θ(t)和比当前靠前时间T[s]的车辆的姿态角θ(t-T)之差，求作为移动量变化的转弯量dθ[rad]。而且，在该过去的移动量变化即转弯量dθ的绝对值为阈值dθth以上的情况下，自身位置估计单元20可估计为移动量的检测误差大。另一方面，在过去的移动量变化即转弯量dθ的绝对值比阈值dθth小的情况下，自身位置估计单元20可估计为移动量的检测误差小。此外，阈值dθth例如设定为即可。

这样，车辆的移动量变化即转弯量大的情况可估计为进行里程数的误差容易累积的转弯或车道变更、弯路的行驶，所以能够估计移动量的检测误差大。

而且，自身位置估计单元20在根据行驶履历的过去的移动量变化而估计为移动量的检测误差小的情况下，在步骤S50，增大提取地标位置数据时的提取范围A[m]，例如设定为200m。另一方面，自身位置估计单元20在根据行驶履历的过去的移动量变化而估计为移动量的检测误差大的情况下，减小提取范围A[m]，例如设定为100m。另外，也可以与行驶履历的过去的移动量变化即转弯量增大相应地，使提取范围A以连续地减小的方式进行变化。即，以相应于根据行驶履历的过去的移动量变化而估计的移动量的检测误差增大来减小提取范围A的方式进行设定。

这样，在车辆的转弯量大且里程数的误差容易累积的情况下，减小提取范围A，以使里程数的误差的累积减少。

另外，也可以不按转弯量dθ，而是对从行驶履历的当前的时刻t至时间T[s]之前的每时每刻的平摆角的变化量Δθ(t)的绝对值进行积分，求转弯量dθ。该情况下，即使是在障碍赛行驶等中姿态角表观上还原而实际的转弯量增大的情况也能够检测。

另外，也可以不检测转弯量的积分值，而是检测提取范围A的转弯速度(偏摆率)的绝对值的最大值γabs[rad/s]，在该γabs为阈值γth[rad/s]以上时，估计为移动量的检测误差大。阈值γth例如设定为即可。

[变形例6]

另外，作为变形例6，作为步骤S140的车辆举动的具体例子，车辆的车速变化越大，估计移动量的检测误差越大，越缩小提取范围A。该情况下，首先将提取范围A设定为200m，检测测量车辆的前后方向的加速度的加速度传感器46的测量值α[m/s²]的绝对值的最大值αabs[m/s²]。

而且，作为行驶履历，在该αabs小于阈值αth[m/s²]的情况下，估计为移动量的检测误差小，增大提取范围A，例如仍设定为200m。另一方面，作为行驶履历，在αabs为阈值αth以上的情况下，估计为移动量的检测误差增大，减小提取范围A，例如设定为100m。阈值αth例如设定为0.2[m/s²]即可。此时，也可以根据αabs使提取范围A连续地变化。

这样，在车辆产生了里程数的误差容易累积的大的车速变化的情况下，减小提取范围A，使里程数的误差的累积减少。

另外，加速度传感器46也可以使用多轴传感器，测量车辆的车宽方向、上下方向的加速度，利用该合成成分进行判断。进而，在步骤S140中，也可以根据车辆举动判定例如车辆进行在十字路口右/左转的车道变更等，用第一实施方式中说明的方法设定提取范围A。

[第二实施方式的效果]

如以上详细地说明的那样，本实施方式的自身位置估计装置中，行驶履历的过去的移动量变化即车辆的转弯量越大，估计移动量的检测误差越大，越减小规定范围(提取范围A)。当车辆转弯时，不仅转弯方向而且轮胎的打滑导致的车宽方向的移动量的检测误差也增大。因此，在基于行驶履历，移动量的检测误差容易累积的车辆的转弯量大的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

另外，在本实施方式的自身位置估计装置中，行驶履历的车辆的车速变化越大，估计移动量的检测误差越大，越减小规定范围(提取范围A)。在车辆产生了加减速的情况下，因为过去的移动量变化大，所以车身的前后方向的移动量的检测误差增大。因此，基于行驶履历，在移动量的检测误差容易累积的车速变化较大的情况下，通过减小规定范围(提取范围A)，能够减少移动量的检测误差的累积，由此，能够高精度且稳定地估计自身位置。

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

特别是，在上述的实施方式中，以车辆为例进行了说明，但本发明也可应用于飞机或船舶等，只要是搭载有至少1台摄像头或激光测距仪的任一方或双方和用于测量里程数的传感器的移动体即可。另外，在上述的实施方式中，求出车辆的3自由度的位置及姿态角，但也可以估计6自由度的位置及姿态角。

标号说明

1 ECU

2、2a、2b 摄像头

3 三维地图数据库

4 车辆传感器群

5、5a、5b 激光测距仪

10 自身位置估计装置

12 地标位置检测单元

14 移动量检测单元

16 地标位置累积单元

18 地图信息获取单元

20 自身位置估计单元

41 GPS接收机

42 加速传感器

43 转向传感器

44 制动传感器

45 车速传感器

46 加速度传感器

47 车轮速度传感器

48 横向偏摆率传感器

Claims (9)

1.一种自身位置估计装置，其估计移动体的自身位置，其特征在于，具备：

地标位置检测单元，其检测在所述移动体的周围存在的地标的地标位置；

移动量检测单元，其检测所述移动体的移动量；

地标位置累积单元，其使由所述地标位置检测单元检测到的地标位置移动由所述移动量检测单元检测到的移动量，并将其作为地标位置数据进行累积；

地图信息获取单元，其获取包含地图上存在的地标的地标位置的地图信息；

自身位置估计单元，其将基于所述移动体的直至当前位置为止的移动履历而设定的规定范围的所述地标位置数据和所述地图信息中包含的地标位置进行对照，估计所述移动体的自身位置。

2.根据权利要求1所述的自身位置估计装置，其特征在于，

所述移动履历的所述移动体的过去的移动量变化越大，所述自身位置估计单元越减小所述规定范围。

3.根据权利要求1或2所述的自身位置估计装置，其特征在于，

所述移动体是车辆，

在所述移动履历中所述车辆的右转/左转次数越多，所述自身位置估计单元越减小所述规定范围。

4.根据权利要求1～3中任一项所述的自身位置估计装置，其特征在于，

所述移动体是车辆，

在所述移动履历中所述车辆的车道变更的次数越多，所述自身位置估计单元越减小所述规定范围。

5.根据权利要求1～4中任一项所述的自身位置估计装置，其特征在于，

所述移动体是车辆，

在所述移动履历中所述车辆的分流或汇流的次数越多，所述自身位置估计单元越减小所述规定范围。

6.根据权利要求1～5中任一项所述的自身位置估计装置，其特征在于，

所述移动体是车辆，

在所述移动履历中所述车辆行驶的弯道的曲率半径越小，所述自身位置估计单元越减小所述规定范围。

7.根据权利要求1～6中任一项所述的自身位置估计装置，其特征在于，

在所述移动履历中所述移动体的转弯量越大，所述自身位置估计单元越减小所述规定范围。

8.根据权利要求1～7中任一项所述的自身位置估计装置，其特征在于，

在所述移动履历中所述移动体的移动速度变化越大，所述自身位置估计单元越减小所述规定范围。

9.一种自身位置估计方法，其估计移动体的自身位置，其特征在于，

搭载于所述移动体上的控制单元检测在所述移动体的周围存在的地标的地标位置，

所述控制单元检测所述移动体的移动量，

所述控制单元使检测到的所述地标位置移动所检测到的所述移动量并将其作为地标位置数据进行累积，

所述控制单元获取包含地图上存在的地标的地标位置的地图信息，

所述控制单元将基于所述移动体的直至当前位置为止的移动履历而设定的规定范围的所述地标位置数据和所述地图信息中包含的地标位置进行对照，估计所述移动体的自身位置。