自身位置计算装置以及自身位置计算方法

自身位置计算装置以及自身位置计算方法
【专利摘要】自身位置计算装置包括：投光器(11)，对车辆周围的路面(31)投光图案光(32a)；以及摄像机(12)，拍摄包含被投光了图案光(32a)的区域的车辆周围的路面(31)，获取图像(38)，由图像中的图案光的位置计算车辆(10)对于路面(31)的姿态角，根据由图像检测到的路面上的多个特征点的时间变化，计算车辆的姿态变化量。通过对车辆的初始位置以及姿态角不断加上姿态变化量，计算车辆的当前位置以及姿态角。在多个特征点的检测状态不满足第1基准的情况下，将此时的车辆的当前位置以及由图案光的位置算出的车辆的姿态角设定为车辆的初始位置以及姿态角，开始姿态变化量的相加。
【专利说明】
自身位置计算装置以及自身位置计算方法
技术领域
[0001 ]本发明涉及自身位置计算装置以及自身位置计算方法。
【背景技术】
[0002] 已知获取由安装在车辆上的摄像机拍摄的车辆近旁的图像，根据图像的变化求车 辆的移动量的技术(参照专利文献1)。在专利文献1中，为了在车辆的低速且微妙的移动中 也高精度地求出移动量，从图像中检测特征点，求出在图像上特征点的位置，由特征点的移 动方向以及移动距离(移动量)求出车辆的移动量。
[0003] 现有技术文献
[0004] 专利文献
[0005] 专利文献1:日本专利特开2008 -175717号公报

【发明内容】

[0006] 但是，在特征点的检测状态差的情况下，难以由特征点的位置高精度地求车辆的 移动量。
[0007] 本发明鉴于上述课题而完成，其目的是提供可以与特征点的检测状态无关，高精 度且稳定地估计车辆的当前位置的自身位置计算装置以及自身位置计算方法。
[0008] 本发明的一个方式的自身位置计算装置包括:投光器，对车辆周围的路面投光图 案光;拍摄包含被投光了图案光的区域的车辆周围的路面，获取图像的摄像单元。自身位置 计算装置由摄像单元获取的图像中的图案光的位置，计算车辆对于路面的姿态角，根据由 图像检测到的路面上的多个特征点的时间变化，计算车辆的姿态变化量。然后，通过对车辆 的初始位置以及姿态角加上姿态变化量，计算车辆的当前位置以及姿态角。自身位置计算 装置在多个特征点的检测状态不满足第1基准，较差的状态的情况下，将此时的车辆的当前 位置以及由图案光的位置算出的车辆的姿态角设定为车辆的初始位置以及姿态角，开始姿 态变化量的相加。
【附图说明】
[0009] 图1是表示第1实施方式的自身位置计算装置的整体结构的方框图。
[0010] 图2是表示投光器11以及摄像机12对车辆10的安装例子的外观图。
[0011] 图3(a)是表示由投光器11和摄像机12之间的基线长Lb和各亮点光在图像上的坐 标(UpL)，计算被照射了各亮点光的路面31上的位置的状况的图，图3(b)是表示根据从与 照射了图案光32a的区域不同的其它区域33检测到的特征点的时间变化，求摄像机12的移 动方向34的状况的示意图。
[0012] 图4(a)以及图4 (b)是表示对于由摄像机12获取的图像实施了二值化处理的图案 光32a的图像的图，图4(a)表示图案光32a整体，图4(b)扩大表示一个亮点光S P，图4(c)是表 示由图案光提取单元21提取的各亮点光\的重心的位置凡的图。
[0013] 图5是用于说明计算距离以及姿态角的变化量的方法的示意图。
[0014] 图6 (a)表示在时刻t获取的第1帧（图像)38的一例。图6 (b)表示在从时刻t经过了 时间A t的时刻(t+ Δ t)获取的第2帧38'。
[0015]图7是表示与在前后帧之间取得对应关系的特征点的数对应的特征点I的检测状 态的判断结果的曲线图。
[0016] 图8是表示使用了图1的自身位置计算装置的自身位置计算方法的一例的流程图。
[0017] 图9是表示图8的步骤S09的详细的步骤的流程图。
[0018] 图10(a)以及图10(b)是表示车辆10的摇摆角以及车宽方向移动量的估计误差的 例子的曲线图。
[0019] 图11是表示第2实施方式的自身位置计算装置的整体结构的方框图。
[0020]图12是表示第3实施方式的自身位置计算装置的整体结构的方框图。
【具体实施方式】
[0021]参照附图，说明实施方式。对附图的记载中同一部分附加同一标号，省略说明。 [0022](第1实施方式）
[0023] (硬件结构）
[0024] 首先，参照图1，说明第1实施方式的自身位置计算装置的硬件结构。自身位置计算 装置包括:投光器11;摄像机12;以及引擎控制单元(ECU)13。投光器11被安装在车辆上，对 车辆周围的路面投光图案光。摄像机12是被安装在车辆上，拍摄包含被投光了图案光的区 域的车辆周围的路面而获取图像的摄像单元的一个例子。ECU13是控制投光器11，并且执行 由摄像机12获取的图像估计车辆的移动量的一连串的信息处理循环的控制单元的一个例 子。
[0025] 摄像机12是使用了固体摄像元件，例如CCD以及CMOS的数字摄像机，获取可进行图 像处理的数字图像。摄像机12的摄像对象是车辆周围的路面，在车辆周围的路面中，包含车 辆的前部、后部、侧部、车辆底部的路面。例如，如图2所示，摄像机12可以安装在车辆10的前 部，具体地说安装在前保险杠上。
[0026] 调整摄像机12所设置的高度以及朝向，并且，自动调整摄像机12具有的镜头的焦 距以及光圈，以便可以拍摄车辆10的前方的路面31上的特征点（纹理）以及由投光器11投光 的图案光32b。摄像机12每隔规定的时间间隔反复进行摄像，获取一连串的图像(帧)群。由 摄像机12获取的图像数据被转发至ECU13，存储在ECU13具有的存储器中。
[0027]如图2所示，投光器11朝向摄像机12的摄像范围内的路面31，投光具有包含正方形 或长方形的栅格像的规定形状的图案光32b。摄像机12拍摄通过路面31照射的图案光。投光 器11例如具有激光指示器以及衍射光栅。通过以衍射光栅衍射从激光指示器射出的激光， 如图2~图4所示，投光器11生成由栅格像或者排列为矩阵状的多个亮点光S P构成的图案光 (3213,32&)。在图3以及图4所示的例子中，生成5\7的亮点光5 [)构成的图案光32&。
[0028]返回图1，ECU13由具有CPU、存储器、以及输入输出单元的微控制器构成，通过执行 预先安装的计算机程序，构成自身位置计算装置具有的多个信息处理单元。ECU13对每图像 (帧)反复执行根据由摄像机12获取的图像来计算车辆的当前位置的一连串的信息处理循 环。E⑶13也可以与车辆10有关的其它控制中使用的E⑶兼用。
[0029] 在多个信息处理单元中包括：图案光提取单元21;姿态角计算单元22;姿态变化量 计算单元24;自身位置计算单元26;图案光控制单元27;检测状态判断单元30;以及计算状 态判断单元35。在姿态变化量计算单元24中包含特征点检测单元23。
[0030] 图案光提取单元21从存储器读入由摄像机12获取的图像，从图像提取图案光的位 置。如图3(a)所示，例如，投光器11向路面31投光被排列为矩阵状的多个亮点光构成的图案 光32a，用摄像机12检测被路面31反射的图案光32a。如图4(a)以及图4(b)所示，图案光提取 单元21通过对由摄像机12获取的图像实施二值化处理，仅提取亮点光心的图像。如图4(c) 所示，图案光提取单元21通过计算各亮点光S P的重心的位置He，即亮点光&在图像上的坐标 (山，％)，提取图案光32a的位置。坐标在以摄像机12的摄像元件的像素为单位，5X7的亮点 光心的情况下，"j"为1以上35以下的自然数。亮点光心的图像上的坐标(UpL)作为表示图 案光32a的位置的数据，存储在存储器中。
[0031] 姿态角计算单元22从存储器读入表示图案光32a的位置的数据，根据由摄像机12 获取的图像中的图案光32a的位置，计算车辆10对于路面31的距离以及姿态角。例如，如图3 (a)所示，根据投光器11与摄像机12之间的基线长Lb和各亮点光在图像上的坐标(山，％)，使 用三角测量的原理，计算照射了各亮点光的路面31上的位置，作为对于摄像机12的相对位 置。然后，姿态角计算单元22根据各亮点光对于摄像机12的相对位置，计算投光了图案光 32a的路面31的平面式，即，摄像机12对于路面31的距离以及姿态角(法线向量）。而且，由于 摄像机12对于车辆10的安装位置以及摄像方向已知，所以在实施方式中，作为车辆10对于 路面31的距离以及姿态角的一个例子，计算摄像机12对于路面31的距离以及姿态角。以后， 将摄像机12对于路面31的距离以及姿态角简称为"距离以及姿态角"。由姿态角计算单元22 算出的距离以及姿态角被存储在存储器中。
[0032]具体地说，由于摄像机12以及投光器11被分别固定在车辆10上，所以图案光32a的 照射方向、摄像机12和投光器11的距离(基线长Lb)是已知的。因此，姿态角计算单元22使用 三角测量的原理，可以从各亮点光在图像上的坐标(山，％)求被照射了各亮点光的路面31上 的位置，作为对于摄像机12的相对位置(XpYpZj)。
[0033] 而且，各亮点光对于摄像机12的相对位置(?，乃，ZJ不存在于同一平面上的情况 较多。这是因为，与路面31上表露出的沥青的凹凸相应，各亮点光的相对位置产生变化。因 此，也可以使用最小二乘法，求与各亮点光的距离误差的平方和最小的平面式。
[0034] 特征点检测单元23从存储器读入由摄像机12获取的图像，根据从存储器读入的图 像，检测路面31上的特征点。特征点检测单元23为了检测路面31上的特征点，例如，可以使 用 "D · G · Lowe，"Distinctive Image Features from Scale-In variant Keypoints，'' Int. J.Comput.Vis.，vol ·60，ηο·2，ρρ·91-110，Νον·200"，或者，"金澤靖，金谷健一， 匕° 二一夕匕、^ (7) fc (6) 画像特徴点抽出，"信学誌，νο 1 · 87，no · 12，ρρ · 1043-1048， Dec.2004"中记载的方法。
[0035]具体地说，特征点检测单元23例如使用哈里斯(Harris)运算符或者SUSAN算符，检 测物体的顶点那样与周围相比亮度值较大地变化的点作为特征点。或者，特征点检测单元 23也可以使用SIFT (Scale-Invariant Feature Transform，尺度不变特征变换)特征量，检 测在其周围亮度值按照某个规则性变化的点作为特征点。然后，特征点检测单元23对从一 个图像检测到的特征点的总数N进行计数，对各特征点附加识别号（i(lfiSN))。各特征点 在图像上的位置(Un被存储在ECU13内的存储器中。图6(a)以及图6(b)表示从摄像机12 获取的图像检测到的特征点Te的例子。各特征点在图像上的位置(UnVd被存储在存储器 中。
[0036] 而且，实施方式中，路面31上的特征点主要设想大小为lcm以上2cm以下的沥青混 合物的颗粒。为了检测该特征点，摄像机12的分辨率为VGA(约30万像素）。而且，摄像机12对 于路面31的距离约为70cm。进而，摄像机12的摄像方向从水平面约45度（deg)向路面31倾 斜。而且，将摄像机12获取的图像转发至ECU13时的亮度数值为0~255(0:最暗，255:最亮） 的范围内。
[0037]姿态变化量计算单元24从存储器读入在每个固定的信息处理循环中拍摄的帧中、 前次帧中包含的多个特征点在图像上的位置(UnVO,进而，从存储器读入本次帧中包含的 多个特征点在图像上的位置(UnVO。然后，根据多个特征点在图像上的位置变化，求车辆的 姿态变化量。这里，所谓"车辆的姿态变化量"包含对于路面31的"距离以及姿态角"的变化 量，以及路面上的"车辆(摄像机12)的移动量"双方。以下，说明距离以及姿态角的变化量以 及车辆的移动量的计算方法。
[0038]图6(a)是表示在时刻t获取的第1帧（图像)38的一例。如图5或者图6(a)所示，考虑 在第1帧38中，例如分别计算三个特征点Tel、Te2、Te3的相对位置(Xi，Yi，Zi)的情况。在该情 况下，可以将由特征点Te 1、Te2、Te3确定的平面G视为路面。由此，姿态变化量计算单元24可 以根据相对位置(XnYnZO，求摄像机12对于路面(平面G)的距离以及姿态角(法线向量）。 进而，姿态变化量计算单元24可以通过已知的摄像机模型，求各特征点T el、Te2、Te3之间的距 离（1 i，12，13)以及连结各个特征点Tel、Te2、T e3的直线所成的角度。图5的摄像机12表示第1帧 中的摄像机的位置。
[0039]而且，作为表示对于摄像机12的相对位置的3维坐标(Xi，Yi，Zi)，将摄像机12的摄 像方向设定为Z轴，在以摄像方向为法线并且包含摄像机12的平面内，设定相互正交的X轴 以及Y轴。另一方面，作为图像38上的坐标，将水平方向以及垂直方向分别设定为V轴以及U 轴。
[0040] 图6 (b)表示在从时刻t经过了时间△ t的时刻（t+ △ t)获取的第2帧。图5的摄像机 12'表示拍摄第2帧38'时的摄像机的位置。如图5或者图6(b)所示，在第2帧38'中，摄像机 12'拍摄特征点1' (31丄2、1\33，特征点检测单元23检测特征点1'(31丄 2丄3。在该情况下，姿态变 化量计算单元24不仅可以由时刻t的各特征点Tel、Te2、Te3的相对位置(Xi，Yi，Zi )、各特征点 在第2帧38'上的位和摄像机12的摄像机模型，计算时间At的摄像机12的移动 量（AL)，还可以计算距离以及姿态角的变化量。例如，通过解以下的（1)~(4)式构成的联 立方程式，姿态变化量计算单元24可以计算摄像机12(车辆）的移动量（AL)以及距离以及 姿态角的变化量。而且，（1)式是摄像机12作为没有失真和光轴偏差的理想的针孔摄像机而 模型化的式子，λ?是常数，f是焦点距离。摄像机模型的参数只要预先校准即可。
[0041]
[0042] (xi-X2)2+(yi-y2)2+(zi-Z2)2 = li2 · · · (2)
[0043] ( X3-X2 )2+ (y312 )2+ ( Z3-Z2 )2 = 122…（3 )
[0044] (xi-X3)2+(yi-y3)2+(zi-Z3)2 = l32 · · · (4)
[0045] 图3(b)示意地表示在摄像机12的摄像范围中，从与被照射了图案光32a的区域不 同的其它区域33检测到的特征点的时间变化，求摄像机12的移动方向34的状况。而且，在图 6(a)以及图6(b)中，将表示各特征点T e的位置的变化方向以及变化量的向量Dte重叠在图像 中表示。姿态变化量计算单元24不仅可以计算时间At中的摄像机12的移动量（AL)，还可 以同时计算距离以及姿态角的变化量。由此，姿态变化量计算单元24考虑距离以及姿态角 的变化量，可以高精度地计算6自由度的移动量（△ L)。即，即使由于车辆10的旋转或加减速 导致的摇摆运动或者俯仰运动，距离或姿态角产生变化，也可以抑制移动量（AL)的估计误 差。
[0046] 而且，姿态变化量计算单元24也可以不使用全部算出相对位置的特征点，而根据 特征点之间的位置关系选定最佳的特征点。作为选定方法，例如，可以使用对极几何(对极 极线几何，R · I · Hart ley : "A linear method for reconstruction from lines and points，"Proc·5th International Conference on Computer Vi sion , Cambridge , Massachusetts,pp.882-887(1995))〇
[0047] 为了在前后帧之间将特征点相对应，例如可以预先将检测到的特征点的周边的小 区域的图像记录在存储器中，由亮度或颜色信息的类似度进行判断。具体地说，ECU13在存 储器中记录以检测到的特征点为中心的相当于5X5(水平X垂直)像素的图像。例如，若亮 度信息在20像素以上、收敛在误差1%以下，则姿态变化量计算单元24判断为在前后帧之间 取得对应关系的特征点。
[0048]这样，在从以后的定时获取的图像38'也检测到算出了相对位置(XnYnZO的特征 点Tel、Te2、Te3的情况下，姿态变化量计算单元24可以根据路面上的多个特征点的时间变 化，计算"车辆的姿态变化量"。
[0049]自身位置计算单元26根据由姿态变化量计算单元24算出的"距离以及姿态角的变 化量"计算距离以及姿态角。进而，由姿态变化量计算单元24算出"车辆的移动量"计算车辆 的当前位置。
[0050] 具体地说，在设定由姿态角计算单元22(图1参照)算出的距离以及姿态角作为起 点的情况下，通过对该起点（距离以及姿态角），逐次加上姿态变化量计算单元24中算出的 各帧的每一个的距离以及姿态角的变化量(进行积分运算），将距离以及姿态角更新为最新 的数值。而且，设定由姿态角计算单元22算出了距离以及姿态角时的车辆位置作为起点（车 辆的初始位置），通过从该初始位置逐次加上车辆的移动量(进行积分运算），计算车辆的当 前位置。例如，通过设定与地图上的位置核对后的起点(车辆的初始位置），可以逐次计算地 图上的车辆的当前位置。
[0051] 这样，只要可以继续检测在前后帧间取得对应关系的3点以上的特征点，则可以通 过继续加上距离以及姿态角的变化量的处理(积分运算），不使用图案光32a地继续将距离 或姿态角更新为最新的数值。其中，在最初的信息处理循环中，也可以使用利用图案光32a 算出的距离以及姿态角，或者规定的初始距离以及初始姿态角。即，成为积分运算的起点的 距离以及姿态角既可以使用图案光32a计算，或者，也可以使用规定的初始值。优选规定的 初始距离以及初始姿态角是至少考虑了至车辆10的乘员以及装载物的距离以及姿态角。例 如，在车辆10的点火开关为接通状态，并且档位从泊车档移动到了其它的档位时，投光图案 光32a，并使用从图案光32a算出的距离以及姿态角作为规定的初始位置以及初始姿态角即 可。由此，可以求出在没有发生车辆10的旋转或加减速导致的摇摆（roll)运动或者俯仰 (pitch)运动时的距离和姿态角。
[0052] 而且，在实施方式中，通过计算距离以及姿态角的变化量，并且逐次加上距离以及 姿态角的变化量，将距离以及姿态角更新为最新的数值。但是，也可以仅将摄像机12对于路 面31的姿态角作为其变化量的计算以及更新的对象。在该情况下，只要摄像机12对于路面 31的距离假定为固定即可。由此，可以考虑姿态角的变化量，抑制移动量（AL)的估计误差， 同时轻减E⑶13的运算负载，并且提高运算速度。
[0053]检测状态判断单元30判断特征点检测单元23的特征点Te的检测状态是否为不满 足第1基准的较差的状态。例如，隧道内的混凝土路面那样，在路面上沥青混合物的颗粒等 的花纹或凹凸较少的情况下，可从图像检测的特征点变少。在该情况下，在前后帧间取得对 应关系的特征点的连续性检测变得困难，距离以及姿态角的更新精度降低。
[0054]因此，例如，在算出对于摄像机12的相对位置的特征点中，在从之后的信息处理循 环中获取的图像中也检测出的特征点的数为规定的阈值(例如，三个）以下的情况下，检测 状态判断单元30判断为特征点的检测状态不满足第1基准，为较差的状态。即，如图7所示， 在前后帧间不能检测出取得对应的4点以上的特征点的情况下，检测状态判断单元30判断 为特征点T e的检测状态不满足第1基准，为较差的状态。而且，如图6所示，为了求距离和姿 态角的变化量，需要在前后帧间取得对应的至少三个特征点。这是因为，为了确定平面G而 需要三个特征点。由于为了提高估计精度而需要更多的特征点，所以优选规定的阈值设为 4、或者5以上。
[0055]在检测状态判断单元30判断为特征点的检测状态满足第1基准的情况下，自身位 置计算单元26维持积分运算的起点。另一方面，在检测状态判断单元30判断为特征点的检 测状态不满足第1基准，为较差的状态的情况下，自身位置计算单元26在相同的信息处理循 环中，将姿态角计算单元22(参照图1)算出的距离以及姿态角，以及那时的车辆位置设定为 新的起点(车辆的姿态角以及初始位置），从该起点起开始车辆的姿态变化量的相加。
[0056]而且，在第1实施方式中，检测状态判断单元30根据在前后帧间取得对应关系的特 征点的数，判断了特征点的检测状态，但是也可以根据从一个图像检测出的特征点的总数 N，判断特征点的检测状态。即，也可以在在特征点的总数N为规定的阈值(例如9)以下的情 况下，判断为特征点的检测状态不满足第1基准，为较差的状态。只要考虑在检测出的特征 点中存在不能对应的特征点的情况，将规定的阈值(3)的3倍的数(9)设定为阈值即可。 [0057]计算状态判断单元35判断姿态角计算单元22的距离以及姿态角的计算状态是否 不满足第2基准，为较差的状态。例如，路面31上的台阶被照射了图案光的情况下，因为路面 31上的台阶比沥青的凹凸大，所以距离以及姿态角的计算精度极端地降低。在特征点的检 测状态比第1基准差，并且距离以及姿态角的计算状态不满足第2基准，为较差的状态的情 况下，高精度地检测距离以及姿态角，以及其变化量的手段就没有了。
[0058]因此，例如，在通过姿态角计算单元22算出的距离以及姿态角的标准偏差大于规 定的阈值的情况下，计算状态判断单元35判断为姿态角计算单元22的距离以及姿态角的计 算状态不满足第2基准，为较差的状态。而且，在35个亮点光中检测出的亮点光的数不足3点 的情况下，由于在原理上不能求出路面31的平面式，所以判断为姿态角计算单元22的距离 以及姿态角的计算状态不满足第2基准，为较差的状态。在使用最小二乘法求出平面式的情 况下，也可以在与各亮点光的距离误差的最大值的绝对值为某个阈值(例如0.05m)以上的 情况下，判断为姿态角计算单元22的距离以及姿态角的计算状态不满足第2基准，为较差的 状态。
[0059] 在检测状态判断单元30判断为特征点的检测状态不满足第1基准，为较差的状态， 并且在计算状态判断单元35判断为姿态角计算单元22的距离以及姿态角的计算状态不满 足第2基准，为较差的状态的情况下，自身位置计算单元26使用前一个信息处理循环中的距 离以及姿态角，以及车辆的当前位置作为积分运算的起点。由此，可以抑制车辆的移动量的 计算误差。
[0060] 图案光控制单元27控制投光器11的图案光32a的投光。例如，在车辆10的点火开关 为接通状态，与自身位置计算装置起动同时，图案光控制单元27开始图案光32a的投光。之 后，图案光控制单元27连续投光图案光32a，直至自身位置计算装置停止。或者，也可以隔开 规定的时间间隔，反复投光的接通/关断。或者，也可以只有在检测状态判断单元30判定为 特征点I的检测状态不满足第1基准，为较差的状态的情况下，暂时地投光图案光32a。
[0061 ](信息处理循环）
[0062]接着，作为由摄像机12获取的图像38估计车辆10的移动量的自身位置计算方法的 一例，参照图8以及图9说明通过ECU13反复执行的信息处理循环。图8的流程图所示的信息 处理循环，在车辆10的点火开关为接通状态，自身位置计算装置起动的同时开始，直至自身 位置计算装置停止，被反复执行。
[0063]在图8的步骤S01中，图案光控制单元27控制投光器11，对车辆周围的路面31投光 图案光32a。在图8的流程图中，说明连续投光图案光32a的例子。
[0064]进至步骤S03，ECU13控制摄像机12,拍摄包含被投光了图案光32a的区域的车辆周 围的路面31，获取图像38』〇]13将由摄像机12获取的图像数据存储在存储器中。
[0065]而且，E⑶13可以自动控制摄像机12的光圈。也可以由前一个信息处理循环中获取 的图像38的平均亮度反馈控制摄像机12的光圈，以使其成为亮度值的最大值和最小值的中 间值。而且，因为投光了图案光32a的区域亮度值高，所以也可以从除去了提取了图案光32a 的部分的区域，求出平均亮度值。
[0066]进至步骤S05,首先，图案光提取单元21从存储器读入由摄像机12获取的图像38， 如图4(c)所示，从图像38提取图案光32a的位置。图案光提取单元21将作为表示图案光32a 的位置的数据而算出的亮点光&在图像上的坐标(山，^存储在存储器中。
[0067]在步骤S05中，姿态角计算单元22从存储器读入表示图案光32a的位置的数据，由 图案光32a的位置计算距离以及姿态角，存储在存储器中。
[0068] 进至步骤S07，ECU13从图像38检测特征点，提取在前后的信息处理循环之间取得 对应关系的特征点，由特征点在图像上的位置(UnVO计算距离以及姿态角的变化量，以及 车辆的移动量。
[0069]具体地说，首先，特征点检测单元23从存储器读入由摄像机12获取的图像38,从图 像38检测路面31上的特征点，将各特征点在图像上的位置(Ui，Vi)存储在存储器中。姿态变 化量计算单元24从存储器读入各特征点在图像上的位置(UnVO,由距离以及姿态角和特征 点在图像上的位置(Un，计算特征点对于摄像机12的相对位置(H A)。而且，姿态变 化量计算单元24使用在前一个信息处理循环的步骤S09中设定的距离以及姿态角。姿态变 化量计算单元24将特征点对于摄像机12的相对位置(Xi，Yi，Zi)存储在存储器中。
[0070] 然后，姿态变化量计算单元24从存储器读入特征点在图像上的位置(&，￥〇，&& 在前一个信息处理循环的步骤S07中算出的特征点的相对位置(Χη YnZO。姿态变化量计算 单元24使用在前后的信息处理循环之间取得对应关系的特征点的相对位置(ΧηΥηΖΟ以及 图像上的位置(UnVO,计算距离以及姿态角的变化量。进而，由前一次信息处理循环中的特 征点的相对位置(Xi，Yi，Zi)和本次信息处理循环中的特征点的相对位置(Xi，Yi，Zi)，计算车 辆的移动量。步骤S07中算出的"距离以及姿态角的变化量"以及"车辆的移动量"，在步骤 S11的处理中被使用。
[0071] 进至步骤S09，ECU13根据特征点的检测状态，以及图案光的距离以及姿态角的计 算状态，设定积分运算的起点。细节参照图9,如后所述。
[0072] 进至步骤S11，自身位置计算单元26由步骤S09的处理中设定的积分运算的起点、 以及步骤S07的处理中算出的车辆的移动量，计算车辆的当前位置。
[0073]这样，本实施方式的自身位置计算装置通过反复执行上述的一连串的信息处理循 环，累计车辆10的移动量，可以计算车辆10的当前位置。
[0074] 参照图9的流程图，说明图8的步骤S09的详细的步骤。在步骤S900中，ECU13判断本 次信息处理循环是否为初次。然后，在为初次的情况下，即，在没有前一次信息处理循环的 数据的情况下，将处理进至步骤S905,在不是初次的情况下，将处理进至步骤S901。
[0075] 在步骤S901中，检测状态判断单元30判断是否特征点检测单元23的特征点Te的检 测状态不满足第1基准，为较差的状态。在判断为较差的情况下（步骤S901中"是"），进至步 骤S903,在判断为未变差的情况下（步骤S901中"否"），进至步骤S909。
[0076]在步骤S909中，E⑶13维持当前设定的积分运算的起点。
[0077] 在步骤S903中，计算状态判断单元35判断是否姿态角计算单元22的距离以及姿态 角的计算状态不满足第2基准，为较差的状态。例如，判断在距离以及姿态角的计算上是否 成功。在判断为成功的情况下(步骤S903中"是"），进至步骤S905,在判断为不成功的情况下 (步骤S903中"否"），进至步骤S907。
[0078]在步骤S905中，ECU13将车辆的当前位置设定作为起点，进而，将在相同的信息处 理循环的步骤S05中算出的距离以及姿态角设定作为积分运算的起点。开始以该距离以及 姿态角作为起点的新的积分运算。而且，以车辆的当前位置作为起点，重新开始车辆的移动 量的积分运算。
[0079]在步骤S907中，E⑶13设定车辆的当前位置作为起点，进而，设定在前一次信息处 理循环中采用的距离以及姿态角作为积分运算的起点。以该距离以及姿态角作为起点开始 新的积分运算。而且，以车辆的当前位置作为起点，重新开始车辆的移动量的积分运算。之 后，将处理进至图8的步骤S11。
[0080]如以上说明的那样，按照第1实施方式，得到以下的作用效果。
[0081 ]因为在特征点的检测状态差时，姿态变化量计算单元24的距离以及姿态角的计算 精度降低，所以车辆10的移动量的估计误差变大。图10(a)以及图10(b)是表示车辆10的摇 摆角(姿态角的一例）以及移动量(车宽方向）的估计误差的例子的曲线图，表示车辆10在无 倾斜的平坦的路面上以固定的速度直行的情况的、摇摆角的估计值以及移动量的计算值的 时间变化。图中的"Pi"以及"p2"与特征点的检测状态无关，表示维持移动量算出的起点，以 及距离以及姿态角的积分运算的起点的比较例。"Qi"以及"Q 2"表示真值。因为车辆10在直 行，所以摇摆角以及车宽方向的移动量的真值(Qi，Q2)仍为0。但是，在时刻t~t2期间，在特 征点的检测状态差的混凝土路面行驶，所以在路面的平面式上产生误差，在摇摆角上出现 估计误差。摇摆角的误差被反映作为车宽方向的移动量的误差。比较例根据特征点的检测 状态，不重置移动量算出的起点，以及摇摆角的积分运算的起点。因此，车辆10的移动量的 误差扩大。
[0082] 按照第1实施方式，在特征点的检测状态不满足第1基准，为较差的状态的情况下， 可以将由图案光32a算出的误差小的距离以及姿态角作为起点，重新开始积分运算。由此， 可以高精度并且稳定地估计车辆10的当前位置。
[0083] 而且，在特征点的检测状态差的路面中，例如，包含成为特征点的对象的花纹或凹 凸少的路面。因为花纹或凹凸少的路面的平坦性高，所以由图案光32a算出的距离以及姿态 角中包含的干扰成分少。另一方面，在特征点的检测状态好的路面31中，因为由于沥青的凹 凸，图案光的反射位置不稳定，所以由图案光32a算出的距离以及姿态角包含较多的干扰成 分。按照第1实施方式，根据特征点的检测状态，适当地分开使用利用了特征点的距离以及 姿态角的更新、和利用了图案光32a的距离以及姿态角的计算，可以高精度并且稳定地求出 距离以及姿态角。
[0084]进而，存在由于路面31的照度急剧地变化，图像38的对比度降低，特征点的检测状 态差的情况。按照第1实施方式，通过直接判断特征点的检测状态，不需要另外设置照度计 等传感器，成本被轻减，有助于装置的轻量化。
[0085]在通过姿态变化量计算单元24取得了对应的特征点的数为规定的阈值以下的情 况下，检测状态判断单元30判断为特征点的检测状态不满足第1基准，为较差的状态(步骤 S901中"是"）。在取得对应的特征点的数减少时，距离以及姿态角的更新精度降低。在该情 况下，通过将移动量算出的起点重置为由图案光32a算出的距离以及姿态角，可以抑制距离 以及姿态角的更新精度、以及车辆10的移动量的计算精度的降低。
[0086]在特征点的检测状态比第1基准差（S901中"是"），并且距离以及姿态角的计算状 态不满足第2基准，为较差的状态的情况下(步骤S903中"否"），将移动量算出的起点重置为 前一个信息处理循环中的距离以及姿态角。即，使用检测状态判断单元30判断为多个特征 点的检测状态不满足第1基准，为较差的状态之前的自身位置计算单元26算出的距离以及 姿态角。由此，可以抑制车辆10的移动量的估计误差。
[0087](第1变形例）
[0088]检测状态判断单元30也可以取代特征点的数而根据特征点的分布，判断特征点I 的检测状态。例如，也可以在由图像38检测的特征点的密度为规定的阈值密度以下的情况 下，检测状态判断单元30判断为特征点的检测状态不满足第1基准，为较差的状态。如果特 征点的密度降低，则特征点的总数N也减少，所以取得对应关系的特征点的数也减少。或者， 也可以在从将图像38均等地分割的各区域未检测到特征点的情况下，检测状态判断单元30 判断为特征点的检测状态不满足第1基准，为较差的状态。例如，只要判断是否由将图像38 进行4等分而从各区域检测到即可。
[0089] 通过检测状态判断单元30根据由图像38检测到的特征点的密度判断特征点的检 测状态，可以抑制距离以及姿态角的更新精度、以及车辆10的移动量的计算精度的降低。
[0090] 而且，进而如果检测到的特征点的密度低，则特征点间的距离远，由特征点构成的 平面与路面31偏离的可能性高，难以高精度地计算距离以及姿态角。这是因为，一般来说， 铺装路面为了排水而设置倾斜(倾斜），所以不平坦。而且，所说的可检测的特征点的密度低 的情况，如上述那样，也有路面31的凹凸或花纹少的情况。由此，在该情况下，从图案光32a 算出的距离以及姿态角中包含的干扰成分少。由此，根据特征点的检测状态，适当地分开使 用利用了特征点的距离以及姿态角的更新、以及利用了图案光32a的距离以及姿态角的计 算，可以高精度地并且稳定地求距离以及姿态角。
[0091] (第2变形例）
[0092]进而，检测状态判断单元30也可以根据由摄像机12获取的图像38的亮度平均值， 判断特征点的检测状态。例如，在图像38的亮度平均值大于上限值Bmax，或者小于下限值Bmin 的情况下，检测状态判断单元30也可以判断为特征点的检测状态不满足第1基准，为较差的 状态。即，在很亮或者很暗的地方判断为特征点的检测状态差。例如，作为上限值B max以及下 限值Bmin，分别设定200、50即可。
[0093](第3变形例）
[0094] 在图9的步骤S907中，E⑶13也可以取代在前一次信息处理循环中采用的距离以及 姿态角，将规定的初始距离以及初始姿态角设定作为积分运算的起点。详细地说，在检测状 态判断单元30判断为特征点的检测状态不满足第1基准，为较差的状态，并且计算状态判断 单元35判断为姿态角计算单元22的距离以及姿态角的计算状态与第2基准相比也不满足， 为较差的状态的情况下，自身位置计算单元26也可以设定至少考虑了至车辆10的乘员以及 装载物的规定的初始距离以及初始姿态角，作为积分运算的起点。例如，可以使用起动了自 身位置计算装置紧后的信息处理循环的步骤S05中算出的距离以及姿态角。由此，可以将不 发生车辆10的旋转或加减速导致的摇摆运动或者俯仰运动时的距离或姿态角作为起点，进 行距离以及姿态角的更新以及移动量的计算。
[0095](第2实施方式）
[0096]如图11所示，与图1相比，第2实施方式的自身位置计算装置在ECU13还构成平面计 算单元36这一点有所不同。平面计算单元36由多个特征点的相对位置(XnY^ZD计算包含 图6所示的特征点的平面G。在平面计算单元36进行的平面G的计算精度为规定的 阈值精度以下的情况下，检测状态判断单元30判断为特征点的检测状态不满足第1基准，为 较差的状态。其它的结构与第1实施方式相同，省略说明。
[0097]例如，平面计算单元36算出相对位置(XnYnZO的特征点在一条直线上的情况下， 判断为平面G的计算精度为规定的阈值精度以下。具体地说，使用霍夫变换计算各特征点的 倾斜和离平面G的中心的距离的分布。在检测出的特征点中的90%的特征点在从某个角度 起一5度(deg)以上、+5度(deg)以下的范围、并且离平面G的中心的距离为一 10像素以上、+ 10像素以下的范围内的情况下，判断为特征点在一条直线上。而且，进行该霍夫变换时的坐 标，使用图像上的位置(UuVO。
[0098]在存在路面31上涂画的、包含人行横道以及分区线的道路标识时，有检测到的特 征点偏向确定的带状或者线状分布的情况。在该情况下，特征点排列在一条直线上的可能 性高，特征点形成的平面G的计算精度降低。这不能从特征点的数目或密度来判断。按照第2 实施方式，可以根据特征点形成的平面G的计算精度，适当地判断特征点的检测状态。
[0099](第3实施方式）
[0100]如图12所示，与图1相比，在作为检测车辆周围的照度的变化量的照度检测单元的 一例，第3实施方式的自身位置计算装置还包括照度传感器37这一点上有所不同。在由照度 传感器37检测到的照度的变化量为规定的阈值照度以上的情况下，检测状态判断单元30判 断为特征点的检测状态不满足第1基准，为较差的状态。其它的结构与第1实施方式相同，省 略说明。
[0101] 在摄像机12的光圈以及ISO灵敏度的自动调整不能追随车辆周围的照度的急剧变 化的情况下，在由摄像机12获取的图像中，产生全白或者赤褐色的现象。在该情况下，难以 从图像检测特征点，特征点的检测状态恶化。因此，将照度传感器37与摄像机12以及投光器 11同样地安装在车辆的前方，根据照度传感器37检测到的照度变化量，判断特征点的检测 状态。事先检知不能通过摄像机12检测特征点的情况，由对路面31投光的图案光32a计算距 离和姿态角。由此，可以适当地判断特征点的检测状态。
[0102] 如上述那样记载了本发明的第1至第3实施方式，成为本公开的一部分的论述以及 附图不应理解为限定本发明。由本公开，本领域的技术人员当然明白各种代替实施的方式、 实施例以及运用技术。
[0103]而且，图2表示了在车辆10的前面安装了摄像机12和投光器11的例子，但是也可以 朝向车辆10的侧方、后方、正下方设置。而且，在本实施方式中作为车辆10的一个例子，在图 2中图示了四轮的乘用汽车，但能够适用于全部摩托车、货车、或者例如运送建筑机械的特 殊车辆等，能够拍摄道路的路面或者墙面上的特征点的移动体(车辆）。
[0104] 标号说明
[0105] 10 车辆
[0106] 11投光器
[0107] 12摄像机(摄像单元）
[0108] 21图案光提取单元
[0109] 22姿态角计算单元
[0110] 23特征点检测单元
[0111] 24姿态变化量计算单元
[0112] 26自身位置计算单元
[0113] 30检测状态判断单元
[0114] 31 路面
[0115] 32a，32b 图案光
[0116] 35计算状态判断单元
[0117] 36平面计算单元
[0118] 37照度传感器(照度检测单元）
[0119] Te特征点
【主权项】
1. 一种自身位置计算装置，其特征在于，包括： 投光器，对车辆周围的路面投光图案光； 摄像单元，安装在所述车辆上，拍摄包含了被投光所述图案光的区域的车辆周围的路 面，获取图像； 姿态角计算单元，由通过所述摄像单元获取的图像中的所述图案光的位置，计算车辆 对于所述路面的姿态角； 姿态变化量计算单元，根据由通过所述摄像单元获取的图像检测到的所述路面上的多 个特征点的时间变化，计算所述车辆的姿态变化量； 自身位置计算单元，通过在所述车辆的初始位置以及姿态角上不断加上所述姿态变化 量，计算所述车辆的当前位置以及姿态角；以及 检测状态判断单元，判断所述姿态角变化量计算单元中的所述多个特征点的检测状 态， 在所述多个特征点的检测状态不满足第1基准的情况下，所述自身位置计算单元将此 时的所述车辆的当前位置以及所述姿态角计算单元的车辆的姿态角，设定为所述车辆的初 始位置以及姿态角，开始所述姿态变化量的相加。2. 如权利要求1所述的自身位置计算装置，其特征在于， 在所述姿态变化量计算单元中的所述多个特征点的数为规定的阈值以下的情况下，所 述检测状态判断单元判断为不满足第1基准。3. 如权利要求1所述的自身位置计算装置，其特征在于， 在由所述图像检测到的所述特征点的密度为规定的阈值密度以下的情况下，所述检测 状态判断单元判断为不满足第1基准。4. 如权利要求1所述的自身位置计算装置，其特征在于， 还包括:平面计算单元，由从所述图像检测到的所述多个特征点的相对位置，计算包含 所述多个特征点的平面， 在所述平面计算单元的所述平面的计算精度为规定的阈值精度以下的情况下，所述检 测状态判断单元判断为不满足第1基准。5. 如权利要求1所述的自身位置计算装置，其特征在于， 还包括:照度检测单元，检测车辆周围的照度的变化量， 在由所述照度检测单元检测到的所述照度的变化量为规定的阈值照度以上的情况下， 所述检测状态判断单元判断为不满足第1基准。6. 如权利要求1至5的任意一项所述的自身位置计算装置，其特征在于， 还包括:计算状态判断单元，判断所述姿态角计算单元的所述姿态角的计算状态， 在所述检测状态判断单元判断为所述多个特征点的检测状态不满足第1基准，并且所 述计算状态判断单元判断为所述姿态角的计算状态不满足第2基准的情况下，所述自身位 置计算单元对于至少考虑了至所述车辆的乘员以及装载物的规定的初始姿态角，开始所述 姿态变化量的相加。7. 如权利要求1至5的任意一项所述的自身位置计算装置，其特征在于， 还包括:计算状态判断单元，判断所述姿态角计算单元的所述姿态角的计算状态， 在所述检测状态判断单元判断为所述多个特征点的检测状态不满足第1基准，并且所 述计算状态判断单元判断为所述姿态角的计算状态不满足第2基准的情况下，所述自身位 置计算单元对于所述检测状态判断单元判断为所述多个特征点的检测状态不满足第1基准 之前算出的姿态角，开始所述姿态变化量的相加。8. -种自身位置计算方法，其特征在于，包括： 对车辆周围的路面投光图案光的步骤； 拍摄包含被投光了所述图案光的区域的车辆周围的路面，获取图像的步骤； 由所述图像中的所述图案光的位置，计算车辆对于所述路面的姿态角的步骤； 根据由所述图像检测到的所述路面上的多个特征点的时间变化，计算所述车辆的姿态 变化量的姿态角变化量计算步骤； 通过对所述车辆的初始位置以及姿态角，不断加上所述姿态变化量，计算所述车辆的 当前位置以及姿态角的自身位置计算步骤;以及 判断所述姿态角变化量计算步骤中的所述多个特征点的检测状态的步骤， 在所述自身位置计算步骤中，在所述多个特征点的检测状态不满足第1基准的情况下， 将此时的所述车辆的当前位置以及由所述图案光的位置算出的车辆的姿态角，设定为所述 车辆的初始位置以及姿态角，开始所述姿态变化量的相加。
【文档编号】G01C21/28GK105981086SQ201480074969
【公开日】2016年9月28日
【申请日】2014年2月24日
【发明人】山口郎, 山口一郎, 西内秀和
【申请人】日产自动车株式会社