自身位置计算装置以及自身位置计算方法与流程

本发明涉及自身位置计算装置以及自身位置计算方法。

背景技术：
已知由车辆上安装的摄像机拍摄并获取车辆近旁的图像，根据该图像的变化求车辆的移动量的技术(参照专利文献1)。在专利文献1中，为了在车辆低速且微妙地移动的情况下高精度地求得移动量，从图像中检测特征点，求该特征点的位置，由特征点的移动方向以及移动距离(移动量)求车辆的移动量。现有技术文献专利文献专利文献1:日本专利特开2008－175717号公报

技术实现要素：
但是，在上述的以往的技术中，若车辆周围的路面上存在凹凸或台阶时，则存在不能高精度地计算车辆的位置的问题。因此，本发明鉴于上述的实际情况而提出，目的是提供即使在车辆周围的路面上存在凹凸或台阶的情况下，也可以高精度地计算车辆的自身位置的自身位置计算装置以及自身位置计算方法。为了解决上述的课题，本发明的一个方式的自身位置计算装置拍摄被投光了图案光的车辆周围的路面，获取图像，从该图像中的图案光的位置，计算车辆相对路面的姿态角。而且，自身位置计算装置根据获取的图像中的路面上的多个特征点的时间变化计算车辆的姿态变化量，通过将该姿态变化量加上车辆的初始位置以及姿态角，计算车辆的当前位置以及姿态角。然后，在判定为车辆周围的路面状态变化了阈值以上的情况下，自身位置计算装置对前次的信息处理循环中算出的车辆的当前位置以及姿态角加上姿态变化量，计算车辆的当前位置以及姿态角。附图说明图1是表示第1实施方式的自身位置计算装置的整体结构的方框图。图2是表示将投光器以及摄像机安装在车辆上的方法的一个例子的外观图。图3(a)是表示使用投光器和摄像机计算亮点光在被照射的路面上的位置的状况的图，图3(b)是表示根据在与照射了图案光的区域不同的区域中检测到的特征点的时间变化，求摄像机的移动方向的状况的图。图4是表示对摄像机获取的图像实施了二值化处理后的图案光的图像的图，图4(a)是表示图案光整体的图，图4(b)是表示将一个亮点光扩大表示的图，图4(c)是表示亮点光的重心位置的图。图5是用于说明计算距离以及姿态角的变化量的方法的示意图。图6是表示在图像上检测到的特征点的图，图6(a)是表示在时刻t获取的第1帧(图像)的图，图6(b)是表示在时刻t+Δt获取的第2帧的图。图7是用于说明由图案光的位置估计路面的高度的变化量的方法的图。图8是表示第1实施方式的自身位置计算装置进行的判定路面状态的变化的处理的定时图。图9是表示第1实施方式的自身位置计算装置进行的自身位置计算处理的处理步骤的流程图。图10是表示第1实施方式的自身位置计算装置进行的图9的步骤S09的详细的处理步骤的流程图。图11是表示在车辆的滚动角以及移动量中发生了估计误差的情况下的一个例子的图。图12是表示第2实施方式的自身位置计算装置进行的判定路面状态的变化的处理的定时图。图13是表示第2实施方式的自身位置计算装置进行的图9的步骤S09的详细的处理步骤的流程图。具体实施方式以下，参照附图说明适用了本发明的第1实施方式以及第2实施方式。[第1实施方式][硬件结构]首先，参照图1，说明第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生成由栅格像或者排列为矩阵状的多个亮点光构成的图案光(32b，32a)。在图3以及图4所示的例子中，生成5×7个的亮点光构成的图案光32a。返回图1，ECU13由具有CPU、存储器、以及输入输出单元的微控制器构成，通过执行预先安装的计算机程序，构成具有作为自身位置计算装置功能的多个信息处理单元。ECU13对每图像(帧)反复执行根据由摄像机12获取的图像来计算车辆的自身位置的一连串的信息处理循环。ECU13也可以与车辆10的其它控制中使用的ECU兼用。这里，在多个信息处理单元中包括：图案光提取单元21；姿态角计算单元22；特征点检测单元23；姿态变化量计算单元24；自身位置计算单元26；图案光控制单元27；以及路面状态判定单元30。在姿态变化量计算单元24中包含特征点检测单元23。图案光提取单元21从存储器读入由摄像机12获取的图像，从该图像提取图案光的位置。如图3(a)所示，例如，投光器11向路面31投光被排列为矩阵状的多个亮点光构成的图案光32a，用摄像机12检测被路面31反射的图案光32a。如图4(a)以及图4(b)所示，图案光提取单元21通过对由摄像机12获取的图像实施二值化处理，仅提取亮点光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的距离以及姿态角，可以求得路面31和车辆10之间的距离、以及车辆10相对路面31的姿态角。具体地说，由于摄像机12以及投光器11被分别固定在车辆10上，所以图案光32a的照射方向、摄像机12和投光器11的距离(基线长Lb)是已知的。因此，姿态角计算单元22使用三角测量的原理，可以从各亮点光在图像上的坐标(Uj，Vj)求各亮点光在被照射了的路面31上的位置，作为相对摄像机12的相对位置(Xj，Yj，Zj)。以后，将摄像机12相对路面31的距离以及姿态角简称为“距离以及姿态角”。由姿态角计算单元22算出的距离以及姿态角被存储在存储器中。而且，在本实施方式中，说明在每次的信息处理循环中计算距离以及姿态角的情况。其中，如后述，在通过路面状态判定单元30判定为路面状态变化了阈值以上的情况下，姿态角计算单元22停止车辆的距离以及姿态角的计算。而且，各亮点光相对摄像机12的相对位置(Xj，Yj，Zj)不存在于同一平面上的情况较多。这是因为，与路面31上表露出的沥青的凹凸相应，各亮点光的相对位置产生变化。因此，也可以使用最小二乘法，求与各亮点光的距离误差的平方和最小的平面式。在图1所示的自身位置计算单元26中使用这样算出的距离以及姿态角的数据。特征点检测单元23从存储器读入由摄像机12获取的图像，根据从存储器读入的图像，检测路面31上的特征点。特征点检测单元23为了检测路面31上的特征点，例如，可以使用“D.G.Lowe，“DistinctiveImageFeaturesfromScale-InvariantKeypoints,”Int.J.Comput.Vis.,vol.60,no.2,pp.91-110,Nov.200”，或者，特征点检测单元23也可以使用“金澤靖,金谷健一,“コンピュータビジョンのための画像の特徴点抽出,”信学誌,vol.87,no.12,pp.1043-1048,Dec.2004”中记载的方法。具体地说，特征点检测单元23例如使用哈里斯(Harris)运算符或者SUSAN算符，检测物体的顶点那样与周围相比亮度值较大地变化的点作为特征点。或者，特征点检测单元23也可以使用SIFT(Scale-InvariantFeatureTransform，尺度不变特征变换)特征量，检测在其周围亮度值按照某个规则性变化的点作为特征点。然后，特征点检测单元23对从一个图像检测到的特征点的总数N进行计数，对各特征点附加识别号(i(1≦i≦N))。各特征点在图像上的位置(Ui，Vi)被存储在ECU13内的存储器中。图6(a)以及图6(b)表示从摄像机12获取的图像检测到的特征点Te的例子。进而，表示各特征点Te的变化方向以及变化量作为向量Dte。而且，实施方式中，路面31上的特征点主要设想大小为1cm以上2cm以下的沥青混合物的颗粒。为了检测该特征点，摄像机12的分辨率为VGA(约30万像素)。而且，摄像机12相对路面31的距离约为70cm。进而，摄像机12的摄像方向从水平面约45度(deg)向路面31倾斜。而且，将摄像机12获取的图像转发至ECU13时的亮度数值为0～255(0：最暗，255：最亮)的范围内。姿态变化量计算单元24从存储器读入在每个固定的信息处理循环中拍摄的各帧的图像之中的、前次(时刻t)的帧的图像中包含的多个特征点在图像上的位置(Ui，Vi)，进而，从存储器读入本次(时刻t+Δt)的帧的图像中包含的多个特征点在图像上的位置(Ui，Vi)。然后，姿态变化量计算单元24根据多个特征点在图像上的随时间的位置变化，求车辆的姿态变化量。这里，所谓“车辆的姿态变化量”包含车辆相对路面的“距离以及姿态角的变化量”，以及路面上的“车辆的移动量”双方。以下，说明“距离以及姿态角的变化量”以及“车辆的移动量”的计算方法。距离以及姿态角的变化量例如可以如下那样求出。图6(a)是表示在时刻t获取的第1帧(图像)38(图5)的一例。如图5及图6(a)所示，考虑在第1帧38中，例如分别计算三个特征点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表示拍摄第1帧38时的摄像机的位置。而且，在图5中，作为表示特征点相对摄像机12的相对位置的3维坐标(Xi，Yi，Zi)，将摄像机12的摄像方向设定为Z轴，在以摄像方向为法线并且包含摄像机12的平面内，设定相互正交的X轴以及Y轴。另一方面，作为图像38上的坐标，将水平方向以及垂直方向分别设定为V轴以及U轴。图6(b)表示在从时刻t经过了时间Δt的时刻(t+Δt)获取的第2帧38’。图5的摄像机12’表示拍摄第2帧38’时的摄像机的位置。如图5或者图6(b)所示，在第2帧38’中，摄像机12’拍摄特征点Te1、Te2、Te3，特征点检测单元23检测特征点Te1、Te2、Te3。在该情况下，姿态变化量计算单元24可以由时刻t的各特征点Te1～Te3的相对位置(Xi，Yi，Zi)、各特征点在第2帧38’上的位置P1(Ui，Vi)、和摄像机12的摄像机模型，计算时间Δt的摄像机12的移动量ΔL。进而，还可以计算距离以及姿态角的变化量。例如，通过解以下的(1)～(4)式构成的联立方程式，姿态变化量计算单元24可以计算摄像机12(车辆)的移动量(ΔL)以及距离以及姿态角的变化量。而且，下述的(1)式是摄像机12作为没有失真和光轴偏差的理想的针孔摄像机而模型化的式子，λi是常数，f是焦点距离。摄像机模型的参数只要预先校准即可。(x1-x2)2+(y1-y2)2+(z1-z2)2＝l12…(2)(x3-x2)2+(y3-y2)2+(z3-z2)2＝l22…(3)(x1-x3)2+(y1-y3)2+(z1-z3)2＝l32…(4)而且，姿态变化量计算单元24也可以不使用在时刻t和时刻t+Δt检测出的各图像中被算出相对位置的全部特征点，而根据特征点之间的位置关系选定最佳的特征点。作为选定方法，例如，可以使用对极几何(对极极线几何，R.I.Hartley:“Alinearmethodforreconstructionfromlinesandpoints,”Proc.5thInternationalConferenceonComputerVision,Cambridge,Massachusetts,pp.882-887(1995))。这样，在特征点检测单元23从时刻t+Δt的帧图像38’也检测到了在时刻t的帧图像38中算出了相对位置(Xi，Yi，Zi)的特征点Te1、Te2、Te3的情况下，姿态变化量计算单元24可以由路面上的多个特征点的相对位置(Xi，Yi，Zi)与特征点在图像上的位置(Ui，Vi)的时间变化，可以计算“车辆的姿态角变化量”。进而，可以计算车辆的移动量。即，只要可以继续检测在前次帧和本次帧之间取得对应关系的3点以上的特征点，则可以通过继续加上距离以及姿态角的变化量的处理(积分运算)，不使用图案光32a而继续更新距离以及姿态角。其中，在最初的信息处理循环中，也可以使用利用图案光32a算出的距离以及姿态角，或者规定的初始距离以及初始姿态角。即，成为积分运算的起点的距离以及姿态角既可以使用图案光32a计算，或者，也可以使用规定的初始值。优选规定的初始距离以及初始姿态角是至少考虑了至车辆10的乘员以及装载物的距离以及姿态角。例如，在车辆10的点火开关为接通状态，并且档位从泊车档移动到了其它的档位时，投光图案光32a，并使用从图案光32a算出的距离以及姿态角作为规定的初始位置以及初始姿态角即可。由此，可以求出在没有发生车辆10的旋转或加减速导致的摇摆(roll)运动或者俯仰(pitch)运动时的距离和姿态角。而且，为了在前后帧之间将特征点相对应，例如可以预先将检测到的特征点的周边的小区域的图像记录在存储器中，由亮度或颜色信息的类似度进行判断。具体地说，ECU13在存储器中记录以检测到的特征点为中心的相当于5×5(水平×垂直)像素的图像。例如，若亮度信息在20像素以上、收敛在误差1％以下，则姿态变化量计算单元24判断为在前后帧之间取得对应关系的特征点。然后，在上述的处理中获取的姿态变化量在后级的自身位置计算单元26中计算车辆的自身位置时被使用。自身位置计算单元26根据由姿态变化量计算单元24算出的“距离以及姿态角的变化量”计算车辆的当前的距离以及姿态角。进而，根据由姿态变化量计算单元24算出的“车辆的移动量”计算车辆的自身位置。具体地说，说明由姿态角计算单元22算出的距离以及姿态角(使用图案光算出的距离以及姿态角)被设定作为起点的情况。在该情况下，对于该起点(距离以及姿态角)，自身位置计算单元26对姿态角计算单元22算出的距离及姿态角逐次加上由姿态变化量计算单元24算出的各帧的每一帧的距离以及姿态角的变化量(进行积分运算)，将距离以及姿态角更新为最新的数值。而且，自身位置计算单元26将由姿态角计算单元22算出距离以及姿态角时的车辆位置设为起点(车辆的初始位置)，逐次加上从该初始位置起的车辆的移动量(进行积分运算)，计算车辆的自身位置。例如，通过设定与地图上的位置核对后的起点(车辆的初始位置)，可以逐次计算地图上的车辆的当前位置。因此，姿态变化量计算单元24通过求出时间Δt期间的摄像机12的移动量(ΔL)，可以计算车辆的自身位置。进而，由于还可以同时计算距离以及姿态角的变化量，所以姿态变化量计算单元24可以考虑车辆的距离以及姿态角的变化量，高精度地计算6自由度(前后，左右，上下，偏摆，俯仰，摇摆)的移动量(ΔL)。即，即使由于车辆10的旋转或加减速导致的摇摆运动或者俯仰运动，距离或姿态角变化，也可以抑制移动量(ΔL)的估计误差。而且，在本实施方式中，通过计算距离以及姿态角的变化量，更新距离以及姿态角，计算摄像机12的移动量(ΔL)，但是也可以仅将摄像机12相对路面31的姿态角作为变化量计算以及更新的对象。在该情况下，路面31和摄像机12之间的距离只要假定为固定即可。由此，可以考虑姿态角的变化量，抑制移动量(ΔL)的估计误差，减轻ECU13的运算负载，并且使运算速度提高。图案光控制单元27控制投光器11进行的图案光32a的投光。例如，在车辆10的点火开关为接通状态，自身位置计算装置起动的同时，图案光控制单元27开始图案光32a的投光。之后，在自身位置计算装置停止之前，图案光控制单元27连续地投光图案光32a。或者，也可以每隔规定的时间间隔，反复投光的接通/关断。路面状态判定单元30检测车辆周围的路面状态的变化，判定路面状态是否变化阈值以上。然后，在判定为路面状态变化在阈值以上的情况下，自身位置计算单元26将起点固定为在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角。由此，姿态角计算单元22停止计算车辆10相对路面的距离以及姿态角。而且，自身位置计算单元26对在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆的当前位置、相对路面的距离以及姿态角。这里，说明判定路面状态的变化的方法。在本实施方式中，在路面上投影35(5×7)个图案光32a的亮点光。因此，例如，在35个亮点光中，在摄像机12的图像中只能检测80％以下，即28个以下的情况下，路面状态判定单元30判定为路面的台阶和凹凸变化激烈，路面状态变化了阈值以上。这时，也可以由路面的高度的变化量估计路面状态的变化。路面的高度的变化量可以由车辆的各车轮的悬挂装置上安装的行程传感器的检测值的振动来检测。例如，在行程传感器的检测值的振动为1Hz以上的情况下，路面状态判定单元30估计为路面的台阶或凹凸变化激烈，判定为路面状态变化了阈值以上。而且，在将计测垂直方向的加速度的加速度传感器的检测值进行积分而计算垂直方向的速度，该速度的朝向的变化为1Hz以上的情况下，路面状态判定单元30也可以判定为路面的台阶或凹凸变化激烈，路面状态变化了阈值以上。进而，路面的高度的变化量也可以由摄像机12拍摄的图像中的图案光32a的位置来估计。在本实施方式中，图7所示的图案光32a被投影在路面31上。因此，将在X方向连接了图案光32a的亮点光线段71、和在Y方向连接了图案光32a的亮点光的线段73拉线。然后，在这些线段的斜率如点75所示在中途变化15度(deg)以上的情况下，路面状态判定单元30估计为路面的台阶或凹凸变化激烈，路面状态变化在阈值以上。而且，如图7所示，在相邻的亮点光的间隔d1、d2的差变化了50％以上的情况下，路面状态判定单元30也可以判定为路面状态变化了阈值以上。这样，在判定为路面状态变化了阈值以上时，自身位置计算单元26固定为在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角。因此，姿态角计算单元22停止车辆10相对路面的距离以及姿态角的计算，自身位置计算单元26对在前次的信息处理循环中算出10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆10的当前位置、相对路面的距离以及姿态角。例如，如图8所示，路面状态判定单元30监视检测到的亮点光的数，将阈值设定为对应于35个亮点光的80％的28个。在该情况下，路面状态判定单元30在检测到亮点光多于28个期间，将姿态角计算标记设定为“ON”。由此，姿态角计算单元22进行车辆10相对路面的距离以及姿态角的计算，自身位置计算单元26使用由姿态角计算单元22算出的车辆的距离以及姿态角计算当前的距离以及姿态角，通过对在前次的信息处理循环中算出的车辆10的当前位置加上车辆的移动量(继续积分运算)，计算当前的车辆的自身位置。但是，如果在时刻t1中，若亮点光的检测个数低于阈值，则自身位置计算单元26将姿态角计算标记切换至“OFF”。由此，车辆10的当前位置、相对路面的距离以及姿态角的起点被固定为在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角，姿态角计算单元22停止车辆10的距离以及姿态角的计算。因此，自身位置计算单元26对在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆的当前位置、相对路面的距离以及姿态角。此后，在时刻t2中，若亮点光的检测个数再次超过阈值，则姿态角计算标记被设定为“ON”，姿态角计算单元22再次开始车辆10的距离以及姿态角的计算。然后，自身位置计算单元26使用由姿态角计算单元22算出的车辆10的距离以及姿态角，计算车辆10的当前的距离以及姿态角。这样，本实施方式的自身位置计算装置，在路面状态较大地变化了的情况下，不使用姿态角计算单元22算出的车辆10的距离以及姿态角，而使用在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角，所以即使路面状态较大地变化，也可以高精度地并且稳定地计算车辆10的自身位置。[信息处理循环]接着，参照图8以及图9，说明由ECU13反复执行的信息处理循环。该信息处理循环是由摄像机12获取的图像38计算车辆10的自身位置的自身位置计算方法的一个例子。图9的流程图所示的信息处理循环与车辆10的点火开关成为接通状态、自身位置计算装置起动的同时开始，直至自身位置计算装置停止被反复执行。在图9的步骤S01中，图案光控制单元27控制投光器11，对车辆周围的路面31投光图案光32a。在图9的流程图中，说明连续投光图案光32a的例子。进至步骤S03，ECU13控制摄像机12，拍摄包含被投光了图案光32a的区域的车辆周围的路面31，获取图像38。ECU13将由摄像机12获取的图像数据存储在存储器中。而且，ECU13可以自动控制摄像机12的光圈。也可以由前一个信息处理循环中获取的图像38的平均亮度反馈控制摄像机12的光圈，以使其成为亮度值的最大值和最小值的中间值。而且，因为投光了图案光32a的区域亮度值高，所以也可以从除去了提取了图案光32a的部分的区域，求出平均亮度值。进至步骤S05，首先，图案光提取单元21从存储器读入由摄像机12获取的图像38，如图4(c)所示，从图像38提取图案光32a的位置。图案光提取单元21将作为表示图案光32a的位置的数据而算出的亮点光Sp在图像上的坐标(Uj，Vj)存储在存储器中。进而，在步骤S05中，姿态角计算单元22从存储器读入表示图案光32a的位置的数据，由图案光32a的位置计算车辆10相对路面31的距离以及姿态角，存储在存储器中。进至步骤S07，ECU13从图像38检测特征点，提取在前后的信息处理循环之间取得对应关系的特征点，由特征点在图像上的位置(Ui，Vi)计算距离以及姿态角的变化量，进而计算车辆的移动量。具体地说，首先，特征点检测单元23从存储器读入由摄像机12获取的图像38，从图像38检测路面31上的特征点，将各特征点在图像上的位置(Ui，Vi)存储在存储器中。姿态变化量计算单元24从存储器读入各特征点在图像上的位置(Ui，Vi)，由姿态角计算单元22算出的距离以及姿态角、和特征点在图像上的位置(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)，计算距离以及姿态角的变化量。进而，姿态变化量计算单元24由前一次的特征点的相对位置(Xi，Yi，Zi)和本次的特征点的相对位置(Xi，Yi，Zi)，计算车辆的移动量，存储在存储器中。步骤S07中算出的“距离以及姿态角的变化量”以及“车辆的移动量”，在步骤S11的处理中被使用。进至步骤S09，ECU13根据车辆周围的路面状态的变化，设定用于计算自身位置的积分运算的起点。细节参照图10，如后所述。进至步骤S11，自身位置计算单元26由步骤S09中设定的积分运算的起点、以及步骤S07的处理中算出的车辆10的距离以及姿态角的变化量，计算车辆10的自身位置。这样，本实施方式的自身位置计算装置通过反复执行上述的一连串的信息处理循环，累计车辆10的移动量，可以计算车辆10的自身位置。[步骤S09的处理]接着，参照图10的流程图，说明图9的步骤S09的详细的步骤。如图10所示，在步骤S101中，路面状态判定单元30检测车辆周围的路面状态的变化。具体地说，路面状态判定单元30一边检测图案光32a的亮点光的数，一边检测在各个车轮上安装的行程传感器的检测值的振动。而且，路面状态判定单元30也可以一边将可以计测车辆的垂直方向的加速度的加速度传感器的检测值进行积分，计算垂直方向的速度，一边检测图案光32a的位置。接着，在步骤S103中，路面状态判定单元30判定车辆周围的路面状态是否已变化了阈值以上。例如，在检测图案光32a的亮点光的数的情况下，在35个亮点光中在摄像机的图像上只能检测到28个以下的情况下，路面状态判定单元30判定为路面的台阶或凹凸变化激烈，路面状态变化了阈值以上。而且，在使用行程传感器的情况下，在检测值的振动为1Hz以上的情况下，路面状态判定单元30判定为路面状态变化了阈值以上。进而，在使用加速度传感器的情况下，将加速度传感器的检测值积分而计算垂直方向的速度，在该速度的朝向的变化为1Hz以上的情况下，路面状态判定单元30判定为路面状态变化了阈值以上。而且，在使用图案光32a的位置的情况下，在连接亮点光的线段的斜率在中途变化15度(deg)以上的情况下，路面状态判定单元30判定为路面状态变化了阈值以上。或者，在相邻的亮点光的间隔的差变化50％以上的情况下，路面状态判定单元30也可以判定为路面状态变化了阈值以上。这样，在进行车辆周围的路面状态是否变化了阈值以上的判定，路面状态判定单元30判定为变化了阈值以上的情况下(步骤S103中“是”)，进至步骤S105。另一方面，在路面状态判定单元30判定为未变化阈值以上的情况下(步骤S103中“否”)，进至步骤S107。在步骤S105中，自身位置计算单元26将车辆10的当前位置、相对路面的距离以及姿态角固定为在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角。即，作为积分运算的起点，自身位置计算单元26设定在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角。由此，姿态角计算单元22停止车辆10相对路面的距离以及姿态角的计算，自身位置计算单元26对在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆10的当前位置、相对路面的距离以及姿态角。另一方面，在步骤S107中，自身位置计算单元26设定在本次的信息处理循环的步骤S05中算出的车辆10的当前位置、相对路面的距离以及姿态角作为积分运算的基点。由此，自身位置计算单元26对在本次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算车辆10的当前位置、相对路面的距离以及姿态角。这样，在设定用于计算当前的车辆10的当前位置、相对路面的距离以及姿态角的积分运算的起点时，步骤S09的处理结束，进至图9的步骤S11。[第1实施方式的效果]如以上详细地说明的那样，在本实施方式的自身位置计算装置中，在判定为车辆周围的路面状态变化了阈值以上的情况下，对在前次的信息处理循环中算出的车辆的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆的当前位置、相对路面的距离以及姿态角。由此，即使在车辆周围的路面上存在凹凸或台阶的情况下，也可以高精度地计算车辆的自身位置。以往，在车辆周围的路面上存在凹凸或台阶时，车辆的移动量的误差变大。例如，图11表示车辆10的滚动角(姿态角的一例)以及移动量(车宽方向)的估计误差的一个例子。在图11(a)中表示车辆10在不产生滚动角的直行行驶的情况下的滚动角的计算值的时间变化，图11(b)中表示在相同的情况下的车辆10的移动量的计算值的时间变化。在图11中，“P1”以及“P2”表示按照以往的方法计算车辆的移动量的情况下的值，“Q1”以及“Q2”是实际的值即真值。因为车辆10在直行，所以滚动角以及车宽方向的移动量的真值(Q1、Q2)一直为0而不移动。但是，在时刻t1～t2的期间，车辆10在有凹凸或台阶的路面上行驶，所以在路面的平面式中发生误差，在滚动角中出现计算误差。该滚动角的误差被反映作为车宽方向的移动量的误差，并且进行积分，所以车宽方向的移动量的误差扩大。这原因是不管车辆周围的路面状态因台阶等变化而存在误差，都在每次的信息处理循环中计算距离以及姿态角。因此，在本实施方式的自身位置计算装置中，判定为车辆周围的路面状态变化了阈值以上的情况下，停止车辆10的当前位置、相对路面的距离以及姿态角的计算，将在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角用作起点。因此，即使在本次的信息处理循环中，在车辆周围的路面状态上产生了变化，也可以通过使用前次的信息处理循环的值，高精度地计算车辆10的自身位置，而没有图11的P1、P2所示的误差扩大。而且，在本实施方式的自身位置计算装置中，由路面的高度的变化量估计路面状态的变化。由此，可以确实地检测路面上存在的凹凸或台阶，所以可以高精度地计算车辆的自身位置。进而，在本实施方式的自身位置计算装置中，由图案光的位置估计路面的高度的变化量。由此，因为可以检测路面状态的变化而不使用车辆上安装的传感器，所以可以用简单的方法高精度地计算车辆的自身位置。[第2实施方式]接着，参照附图说明本发明的第2实施方式的自身位置计算装置。其中，本实施方式的自身位置计算装置的结构与图1所示的第1实施方式相同，所以省略详细的说明。在上述的第1实施方式中，在每次的信息处理循环中计算车辆10的距离以及姿态角，但是在本实施方式中，以固定周期计算车辆10的距离以及姿态角，这一点有所不同。因此，如图12所示，例如进行设定，使得以8帧等固定的周期发生周期计数脉冲，在该定时将姿态角计算标记设定为“ON”。由此，可以以固定周期计算车辆10的距离以及姿态角。其中，在发生了周期计数脉冲时车辆周围的路面状态变化了阈值以上的情况下，即在检测到的亮点光的数为阈值以下的情况下，不将姿态角计算标记设为“ON”而仍为“OFF”。例如，在时刻t1～t2的期间，由于检测到的亮点光的数为阈值以下，所以虽然发生周期计数脉冲，但是姿态角计算标记未被设定为“ON”而仍为“OFF”。由此，在车辆周围的路面上存在凹凸或台阶的情况下，路面状态判定单元30将车辆的当前位置、相对路面的距离以及姿态角固定为在前次的信息处理循环中算出的距离以及姿态角。因此，姿态角计算单元22停止车辆的当前位置、相对路面的距离以及姿态角的计算，并且自身位置计算单元26对在前次的信息处理循环中算出的车辆10的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆10的当前位置、相对路面的距离以及姿态角。[信息处理循环]本实施方式的自身位置计算装置的信息处理循环与图9所示的第1实施方式相同，所以省略详细的说明。在本实施方式中，设定图9的步骤S09的积分运算的起点的处理与第1实施方式有所不同。[步骤S09的处理]接着，参照图13的流程图，说明图9的步骤S09的详细的步骤。如图13所示，在步骤S201中，路面状态判定单元30判定是否经过了规定的周期。如图12中说明的那样，路面状态判定单元30监视是否发生周期计数脉冲，在发生的情况下判定为经过了规定的周期，进至步骤S203。另一方面，在未发生周期计数脉冲的情况下，判定为未经过规定的周期，进至步骤S207。此后的步骤S203～S209的处理与图10的步骤S101～S107的处理分别相同，所以省略详细的说明。[第2实施方式的效果]如以上详细地说明的那样，在本实施方式的自身位置计算装置中，以固定的周期进行车辆的距离以及姿态角的计算。然后，在那时判定为车辆周围的路面状态变化了阈值以上的情况下，自身位置计算装置对在前次的信息处理循环中算出的车辆的当前位置、相对路面的距离以及姿态角加上姿态变化量，计算当前的车辆的当前位置、相对路面的距离以及姿态角。由此，姿态角计算单元22可以减少计算车辆的距离以及姿态角的频度，所以可以减轻ECU13的运算负荷，并且提高运算速度。再有，上述的实施方式是本发明的一个例子。因此，本发明不限定于上述的实施方式，只要是不超出本发明的技术的思想的范围，即使是该实施方式以外的方式，当然可以根据设计等而存在各种变更。标号说明10车辆11投光器12摄像机(摄像单元)13ECU21图案光提取单元22姿态角计算单元23特征点检测单元24姿态变化量计算单元26自身位置计算单元27图案光控制单元30路面状态判定单元31路面32a，32b图案光Te特征点