自己位置计算装置及自己位置计算方法

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

【发明内容】

[0007] 本发明是鉴于上述课题而创立的，其目的在于，提供一种自己位置计算装置及自 己位置计算方法，无论特征点的检测状态如何，均能够高精度且稳定地推定车辆的当前位 置。
[0008] 本发明的一方式的自己位置(本车辆位置local location)计算装置具备：投光 器，其向车辆周围的路面投射图案光;摄像部，其拍摄包含投射图案光的区域在内的车辆周 围的路面并取得图像。自己位置计算装置根据由摄像部取得的图像中的图案光的位置，计 算出车辆相对于路面的姿势角，并基于根据图像检测出的路面上的多个特征点的时间变 化，计算出车辆的姿势变化量。而且，通过在车辆的初始位置及姿势角加上姿势变化量，计 算出车辆的当前位置及姿势角。自己位置计算装置在多个特征点的检测状态不满足第一基 准而差的状态的情况下，投射图案光。
【附图说明】
[0009] 图1是表示实施方式的自己位置计算装置的整体构成的块图；
[0010] 图2是表示投光器11及摄像机12向车辆10的搭载例的外观图；
[0011] 图3(a)是表示根据投光器11和摄像机12之间的基线长度Lb、各光点的图像上的坐 标(UpVj)，计算出照射各光点的路面31上的位置的情形的图，图3(b)是表示根据从与照射 图案光32a的区域不同的另一区域33检测出的特征点的时间变化，求出摄像机12的移动方 向34的情形的不意图；
[0012 ]图4 (a)及图4 (b)是表示对由摄像机12取得的图像实施了二值化处理的图案光32a 的图像的图，图4(a)表不图案光32a整体，图4(b)表不扩大一个光点Sp，图4(c)是表不由图 案光抽取部21抽取的各光点S p的重心的位置凡的图；
[0013] 图5是用于说明计算距离及姿势角的变化量及摄像机12的移动量（AL)的方法的 示意图；
[0014] 图6(a)表示在时刻t取得的第一帧（图像)38的一例，图6(b)表示在从时刻t经过时 间At的时刻(t+At)取得的第二帧38' ；
[0015]图7是表示使用了图1的自己位置计算装置的自己位置计算方法的一例的流程图；
[0016] 图8是表示图7的步骤S05的详细顺序的流程图；
[0017] 图9是表示按照图8的流程执行的信息处理的一例的图，图9(a)表示姿势角计算部 22是否根据图案光32a计算出距离及姿势角，图9(b)表示图案光投射实施标志的状态，图9 (c)表示取得对应的特征点的数量；
[0018] 图10是表示图7的步骤S09的详细顺序的流程图；
[0019] 图11(a)及图11(b)是表示车辆10的侧滚角及车宽方向移动量的推定误差的例的 图；
[0020] 图12是表示第二实施方式的步骤S05的详细顺序的流程图；
[0021] 图13是表示按照图12的流程执行的信息处理的一例的图，图13(a)表示姿势角计 算部22是否根据图案光32a计算出距离及姿势角，图13(b)表示图案光投射实施标志的状 态，图13 (c)表示取得对应的特征点的数量；
[0022]图14是表示第三实施方式的步骤S05的详细顺序的流程图；
[0023]图15是表示按照图14的流程执行的信息处理的一例的图，图15(a)表示姿势角计 算部22是否根据图案光32a计算出距离及姿势角，图15(b)表示图案光投射实施标志的状 态，图15 (c)表示取得对应的特征点的数量；
[0024]图16是表示第四实施方式的步骤S05的详细顺序的流程图；
[0025]图17是表示按照图16的流程执行的信息处理的一例的图，图17(a)表示姿势角计 算部22是否根据图案光32a计算出距离及姿势角，图17(b)表示图案光投射实施标志的状 态，图17 (c)表示取得对应的特征点的数量。
【具体实施方式】
[0026]参照【附图说明】实施方式。附图的记载中，对相同部分标注相同的符号并省略说明。 [0027](第一实施方式）
[0028](硬件构成）
[0029]首先，参照图1说明第一实施方式的自己位置计算装置的硬件构成。自己位置计算 装置具备:投光器11、摄像机12、发动机控制单元(ECU)13。投光器11搭载于车辆上，向车辆 周围的路面投射图案光。摄像机12是搭载于车辆上而拍摄包含投射图案光的区域在内的车 辆周围的路面并取得图像的摄像部的一例。ECU13是控制投光器11且执行根据由摄像机12 取得的图像推定车辆的移动量的一连串的信息处理循环的控制部的一例。
[0030] 摄像机12是使用了固体摄像元件、例如CCD及CMOS的数字摄像机，取得可以进行图 像处理的数字图像。摄像机12的拍摄对象是车辆周围的路面，车辆周围的路面包含车辆的 前部、后部、侧部、车辆底部的路面。例如，如图2所示，摄像机12可以搭载于车辆10的前部， 具体而言，可以搭载于前保险杠上。
[0031] 调整设置摄像机12的高度及方向且自动调整摄像机12所具备的透镜的焦点及光 圈，以可拍摄车辆10的前方路面31上的特征点（纹理)及由投光器11投射的图案光32b。摄像 机12每隔规定的时间间隔进行重复拍摄，取得一连串的图像(帧)组。由摄像机12取得的图 像数据向E⑶13传送，并存储于E⑶13所具备的存储器。
[0032]如图2所示，投光器11向摄像机12的拍摄范围内的路面31，投射具有包含正方形或 长方形的光栅图像的规定形状的图案光32b。摄像机12拍摄由路面31照射的图案光。投光器 11具备例如激光指示器及衍射光栅。将从激光指示器射出的激光利用衍射光栅进行衍射， 由此，如图2~图4所示，投光器11生成光栅图像或由行列状地排列的多个光点心构成的图 案光(32b、32a)。图3及图4所示的例子中，生成由5X7的光点S p构成的图案光32a。
[0033] 返回图1，ECU13由具备CPU、存储器及输入输出部的微型控制器构成，通过执行预 先安装的计算机程序，构成自己位置计算装置具备的多个信息处理部。ECU13对每个图像 (帧）重复执行根据由摄像机12取得的图像计算出车辆的当前位置的一连串的信息处理循 环。E⑶13也可以兼用作车辆10的其它控制所使用的E⑶。
[0034]多个信息处理部包含：图案光抽取部21、姿势角计算部22、姿势变化量计算部24、 自己位置计算部26、图案光控制部27、检测状态判断部30、计算状态判断部35。姿势变化量 计算部24包含特征点检测部23。
[0035] 图案光抽取部21从存储器读取由摄像机12取得的图像，根据图像抽取图案光的位 置。如图3(a)所示，例如，投光器11向路面31投射由行列状地排列的多个光点构成的图案光 32a，并利用摄像机12检测被路面31反射的图案光32a。图案光抽取部21通过对由摄像机12 取得的图像实施二值化处理，如图4(a)及图4(b)所示，仅抽取光点S p的图像。如图4(c)所 示，图案光抽取部21通过计算出各光点Sp的重心的位置He，即光点心的图像上的坐标(Uj、 V j)，而抽取图案光32a的位置。坐标在以摄像机12的摄像元件的像素为单位且5X7的光点 Sp的情况下，"j"为1以上且35以下的自然数。光点&的图像上的坐标㈨、％)作为表示图案 光32a的位置的数据存储于存储器。
[0036] 姿势角计算部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计算出的距离 及姿势角存储于存储器。
[0037] 具体而言，摄像机12及投光器11分别固定于车辆10上，因此，图案光32a的照射方 向、摄像机12和投光器11的距离(基线长度Lb)已知。因此，姿势角计算部22可以使用三角测 量的原理，将从各光点的图像上的坐标(UpV j)照射各光点的路面31上的位置作为相对于摄 像机12的相对位置(Xj，Yj，Z j)而求出。
[0038]此外，各光点相对于摄像机12的相对位置(?，乃，Zj)大多情况下不存在于同一平 面上。这是由于，根据在路面31上露出的沥青的凹凸不同，各光点的相对位置进行改变。因 此，也可以使用最小二乘法，求出与各光点的距离误差的平方和成为最小那样的平面方程 式。
[0039]特征点检测部23从存储器读取由摄像机12取得的图像，并根据从存储器读取的图 像，检测路面31上的特征点。特征点检测部23为了检测路面31上的特征点，可以使用例如 uD.G.Lowe, uDistinctive Image Features from Scale-Invariant Keypoints /' Int · J. Comput .Vis.，vo 1.60,否.2, pp .91-110,否 ν· 200"，或，"金泽靖，金谷健 一，匕。 二一夕匕'、：％^(7)化^)(7)画像(7)特徴点抽出（用于计算机图像的图像的特征点抽取)"信学 志，vol · 87，否· 12，pp · 1043-1048，Dec · 2004" 所记载的方法。
[0040]具体而言，特征点检测部23使用例如哈里斯(Harris)运算符或SUSAN操作符，以物 体的顶点那样辉度值比周围大幅变化的点为特征点进行检测。或者，特征点检测部23也可 以使用SIFT(Scale_Invariant Feature Transform)特征量，以根据在其周围辉度值以一 定规则性变化的点为特征点进行检测。而且，特征点检测部23对由1个图像检测的特征点的 总数N进行计数，并对各特征点标注识别编号（i(l<i<N))。各特征点的图像上的位置(仏、 V 1)存储于ECU13内的存储器。图6(a)及图6(b)表示根据由摄像机12取得的图像检测出的特 征点Te的例子。各特征点的图像上的位置(U 1J1)存储于存储器中。
[0041]此外，实施方式中，路面31上的特征点主要假定大小为Icm以上且2cm以下的沥青 混合物的粒子。为了检测该特征点，摄像机12的分辨率为VGA(约30万像素）。另外，摄像机12 相对于路面31的距离大致为70cm。另外，摄像机12的拍摄方向从水平面向路面31倾斜约 45deg。另外，将由摄像机12取得的图像向ECU13传送时的辉度数值为0~255(0:最暗，255: 最壳)的范围内。
[0042]姿势变化量计算部24从存储器读取按照一定的信息处理循环拍摄的帧中、上一次 帧所包含的多个特征点的图像上的位置(UhV1),且从存储器读取此次帧所包含的多个特征 点的图像上的位置(UhV 1)13而且，基于多个特征点的图像上的位置变化，求出车辆的姿势变 化量。在此，"车辆的姿势变化量"包含相对于路面31的"距离及姿势角"的变化量及在路面 上的"车辆(摄像机12)的移动量"双方。以下，说明距离及姿势角的变化量及车辆的移动量 的计算方法。
[0043] 图6(a)表示在时刻t取得的第一帧（图像)38的一例。如图5或图6(a)所示，考虑在 第一帧38分别计算出例如3个特征点T el、Te2、Te3的相对位置(XiUi)的情况。在该情况下， 可以将由特征点T el、Te2、Te3特定的平面G看作路面。因此，姿势变化量计算部24可以根据相 对位置(X 1J1)，求出摄像机12相对于路面(平面G)的距离及姿势角(法向矢量）。另外，姿 势变化量计算部24可以根据已知的摄像机型号，求出各特征点T el、Te2、Te3之间的距离（h、 12、1 3)及连结各个特征点Te3l Je32Je33的直线构成的角度。图5的摄像机12表示第一帧时的摄 像机的位置。
[0044] 此外，作为表示相对于摄像机12的相对位置的三维坐标(Xi、Yi、Zi )，将摄像机12的 拍摄方向设定成Z轴，将拍摄方向设为法线且在包含摄像机12的平面内设定相互正交的X轴 及Y轴。另一方面，作为图像38上的坐标，将水平方向及垂直方向分别设定成V轴及U轴。
[0045] 图6 (b)表示在从时刻t经由时间△ t的时刻（t+ △ t)取得的第二帧。图5的摄像机 12'表示拍摄第二帧38'时的摄像机的位置。如图5或图6(b)所示，在第二帧38'，摄像机12' 拍摄特征点T e3l Je32Je33,特征点检测部23检测出特征点Te3l Je32Je33t3在该情况下，姿势变化量 计算部24根据时刻t时的各特征点Tel、Te2、Te3的相对位置(Xi、Yi、Zi )、各特征点的第二帧38 ' 上的位和摄像机12的摄像机型号，不仅可以计算出时间At时的摄像机12(车 辆)的移动量（AL)，而且还可以计算出距离及姿势角的变化量。例如，通过解开以下的由式 (1)~(4)构成的联合方程式，姿势变化量计算部24可以计算出摄像机12的移动量（AL)及 距离及姿势角的变化量。此外，式（1)是摄像机12作为没有失真或没有光轴偏离的理想性的 针孔摄像机而模拟化的式子，Ai为常数，f为焦点距离。摄像机型号的参数只要预先进行校 准即可。
[0046]
[0047]
[0048]
[0049]
[0050] 图3(b)示意性地表示摄像机12的拍摄范围中，根据与照射图案光32a的区域不同 的其它区域33检测出的特征点的时间变化，求出摄像机12的移动方向34的情形。另外，图6 (a)及图6 (b)中，将表示各特征点Te的位置的变化方向及变化量的矢量Dte与图像重叠表示。 姿势变化量计算部24不仅可以计算出时间△ t时的摄像机12的移动量（△ L)，而且还可以同 时计算出距离及姿势角的变化量。因此，姿势变化量计算部24考虑距离及姿势角的变化量， 可以高精度地计算出6个自由度的移动量（△ L)。即，即使距离或姿势角根据车辆10的转向 或加减速产生的侧滚运动或俯仰运动不同而变化，也可以抑制移动量（△ U的推定误差。 [0051 ]此外，姿势变化量计算部24也可以不使用计算出相对位置的全部特征点，而基于 特征点彼此的位置关系选定最佳的特征点。作为选定方法，例如可以使用对极几何(对极线 I.Hartley:uA linear method for reconstruction from lines and points," Proc.5th International Conference on Computer Vision,Cambridge,Massachusetts, pp.882-887(1995))〇
[0052]就在前后帧间使特征点对应而言，只要将例如检测出的特征点周边的小区域的图 像记录于存储器，并根据辉度或色信息的类似度进行判断即可。具体而言，ECU13将以检测 出的特征点为中心的5 X 5(水平X垂直)像素的图像记录于存储器中。姿势变化量计算部24 如果将例如辉度信息收纳于20像素以上且误差1 %以下，则判断为在前后帧间取得对应关 系的特征点。
[0053]这样，从在后面的时刻取得的图像38'也检测到计算出相对位置(X1 JnZ1)的特征 点Tel、Te2、Te3时，姿势变化量计算部24可以基于路面上的多个特征点的时间变化，计算出 "车辆的姿势变化量"。
[0054]自己位置计算部26根据由姿势变化量计算部24计算出的"距离及姿势角的变化 量"计算出距离及姿势角。另外，根据由姿势变化量计算部24计算出的"车辆的移动量"计算 出车辆的当前位置。
[0055]具体而言，在将由姿势角计算部22(参照图1)计算出的距离及姿势角设定为起点 的情况下，对该起点(距离及姿势角)逐次加上(进行积分运算）由姿势变化量计算部24计算 出的各个帧的距离及姿势角的变化量，由此，将距离及姿势角更新成最新的数值。另外，将 由姿势角计算部22计算出距离及姿势角时的车辆位置设定为起点（车辆的初始位置），且从 该初始位置逐次加上(进行积分运算)车辆的移动量，由此，计算出车辆的当前位置。例如， 通过设定与地图上的位置核对的起点（车辆的初始位置），可以逐次计算出地图上的车辆的 当前位置。
[0056]这样，如果可以继续检测在前后帧间取得对应关系的3点以上的特征点，则通过继 续加上距离及姿势角的变化量的处理(积分运算），不使用图案光32a，也可以将距离或姿势 角继续更新成最新的数值。其中，在最初的信息处理循环中，也可以使用通过使用图案光 32a计算出的距离及姿势角或规定的初始距离及初始姿势角。即，成为积分运算的起点的距 离及姿势角也可以使用图案光32a计算出，或也可以使用规定的初始值。规定的初始距离及 初始姿势角优选为至少考虑了车辆10的乘坐人员及搭载物的距离及姿势角。例如，在车辆 10的点火开关为接通状态，且移动位置从停车场移动至另一位置时，只要投射图案光32a， 且将根据图案光32a计算出的距离及姿势角用作规定的初始距离及初始姿势角即可。由此， 可以求出不产生车辆10的转向或加减速引起的滚动运动或俯仰运动时的距离及姿势角。 [0057] 此外，实施方式中，通过计算出距离及姿势角的变化量，且逐次加上距离及姿势角 的变化量，将距离及姿势角更新成最新的数值。但是，也可以仅将摄像机12相对于路面31的 姿势角设为其变化量而计算出并作为更新的对象。在该情况下，摄像机12相对于路面31的 距离只要假定为一定即可。由此，也可以考虑姿势角的变化量，抑制移动量（AL)的推定误 差，同时减轻E⑶13的运算负荷，且提高运算速度。
[0058]检测状态判断部30判断特征点检测部23进行的特征点Te的检测状态是否不满足 第一基准而是差的状态。例如，如隧道内的混凝土路面那样，在路面上沥青混合物的颗粒等 的状况或凹凸较少的情况下，从图像可检测的特征点变少。在该情况下，在前后帧间取得对 应关系的特征点的继续检测变得困难，距离及姿势角的更新精度降低。
[0059] 因此，检测状态判断部30在例如计算出相对于摄像机12的相对位置的特征点中、 从在后面的信息处理循环中取得的图像也可检测出的特征点的数量不是规定的阈值（例 如，4个）以上的情况下，判断为多个特征点的检测状态不满足第一基准，而是差的状态。即， 在不能检测前后帧间取得对应的4个以上的特征点的情况下，检测状态判断部30判断为特 征点T e的检测状态不满足第一基准，而是差的状态。此外，如图6所示，为了求出距离及姿势 角的变化量，需要在前后帧间取得对应的至少3个特征点。这是由于，特定平面G需要3个特 征点。提高推定精度需要更多的特征点，因此，规定的阈值优选为4或5以上。
[0060] 图案光控制部27基于检测状态判断部30的判断结果，控制投光器11进行的图案光 32a的投射。在检测状态判断部30判断为多个特征点的检测状态满足第一基准而不是差的 状态的情况下，图案光控制部27将图案光32a熄灭。另一方面，在检测状态判断部30判断为 多个特征点的检测状态不满足第一基准而是差的状态的情况下，图案光控制部27投射图案 光32a。具体地，图案光控制部27基于检测状态判断部30的判断结果，切换图案光投射实施 标志的0N/0FF，并对投光器1发送图案光投射实施标志。投光器1基于图案光投射实施标志， 在下面的信息处理循环中，将图案光32a进行投射或熄灭。
[0061] 另外，检测状态判断部30判断特征点检测部23进行的特征点Te3的检测状态是否不 满足比第一基准更低的第二基准而是差的状态。例如，在不能检测前后帧间取得对应的3个 以上的特征点的情况下，检测状态判断部30判断为特征点Te的检测状态不满足第二基准而 是差的状态。
[0062]在检测状态判断部30判断为多个特征点的检测状态满足第二基准且不是差的状 态的情况下，姿势角计算部22不能根据图案光32a的位置计算出距离及姿势角。另一方面， 在检测状态判断部30判断为多个特征点的检测状态不满足第二基准而是差的状态的情况 下，不能根据特征点T e3特定平面G，因此，姿势角计算部22根据图案光32a的位置计算出距离 及姿势角。
[0063]自己位置计算部26在检测状态判断部30判断为多个特征点的检测状态满足第二 基准的情况下，维持积分运算的起点。另一方面，在检测状态判断部30判断为多个特征点的 检测状态不满足第二基准而是差的状态的情况下，自己位置计算部26在相同的信息处理循 环中，将姿势角计算部22(参照图1)计算出的距离及姿势角及此时的车辆位置设定成新的 起点(车辆的姿势角及初始位置），并从该起点起开始车辆的姿势变化量的加算。
[0064]此外，第一实施方式中，检测状态判断部30基于在前后帧间取得对应关系的特征 点的数量，判断多个特征点的检测状态，但也可以基于从1个图像检测出的特征点的总数N， 判断多个特征点的检测状态。即，在从1个图像检测出的特征点的总数N为规定的阈值(例如 9)以下的情况下，也可以判断为多个特征点的检测状态差。只要考虑检测出的特征点中有 未取得对应的特征点，且将规定的阈值(4)的3倍的数(12)设定成阈值即可。
[0065]计算状态判断部35判断姿势角计算部22计算距离及姿势角的计算状态是否不满 足第三基准而是差的状态。例如，在图案光照射至路面31上的台阶的情况下，路面31上的台 阶比沥青的凹凸大，因此，距离及姿势角的计算精度极低。在多个特征点的检测状态不满足 第二基准而是差的状态，且距离及姿势角的计算状态不满足第三基准而是差的状态的情况 下，就没有高精度地检测距离及姿势角及其变化量的手段。
[0066] 于是，计算状态判断部35在例如由姿势角计算部22计算出的距离及姿势角的标准 偏差比规定的阈值大的情况下，判断为由姿势角计算部22计算距离及姿势角的计算状态不 满足第三基准而是差的状态。另外，在35个的光点中不能检测出的光点的数量低于3点的情 况下，原则上不能求出路面31的平面方程式，因此，判断为由姿势角计算部22计算距离及姿 势角的计算状态不满足第三基准而是差的状态。在使用最小二乘法求出平面方程式时，与 各光点的距离误差的最大值的绝对值成为某个阈值(例如0.05m)以上的情况下，也可以判 断为由姿势角计算部22计算距离及姿势角的计算状态不满足第三基准而是差的状态。
[0067] 在检测状态判断部30判断为多个特征点的检测状态不满足第二基准而是差的状 态，且计算状态判断部35判断为由姿势角计算部22计算距离及姿势角的计算状态不满足第 三基准而是差的状态的情况下，自己位置计算部26使用之前的信息处理循环中的距离及姿 势角及车辆的当前位置作为积分运算的起点。由此，可抑制车辆的移动量的计算误差。 [0068](信息处理循环）
[0069]接着，作为根据由摄像机12取得的图像38推定车辆10的移动量的自己位置计算方 法的一例，参照图7说明由ECU13重复执行的信息处理循环。图7的流程图所示的信息处理循 环在车辆10的点火开关成为接通状态且自己位置计算装置启动的同时开始，并重复执行到 自己位置计算装置停止。
[0070]图7的步骤SOl中，投光器11将图案光32a投射或熄灭。具体而言，投光器11在上一 次的信息处理循环的步骤S05,基于从图案光控制部27发送的图案光投射实施标志的ON/ OFF，切换图案光32a的投光/熄灭。参照图8对图案光投射实施标志进行后述。
[0071]进入步骤S03，ECU13控制摄像机12,拍摄包含投射图案光32a的区域的车辆周围的 路面31，而取得图像38 ACU13将由摄像机12取得的图像数据存储于存储器中。
[0072]此外，E⑶13可自动控制摄像机12的光圈。也可以根据之前的信息处理循环中取得 的图像38的平均辉度，对摄像机12的光圈进行反馈控制，以成为辉度值的最大值和最小值 的中间值。另外，投射图案光32a的区域的辉度值较高，因此，也可以从除抽取了图案光32a 的部分以外的区域，求出平均辉度值。
[0073]进入步骤S05,检测状态判断部30判断多个特征点的检测状态。图案光控制部27基 于检测状态判断部30的判断结果切换图案光投射实施标志，并发送至投光器11。而且，姿势 角计算部22基于检测状态判断部30的判断结果计算出或不计算出距离及姿势角。步骤S05 的详情参照图8及图9进行后述。
[0074]进入步骤S07，ECU13根据图像38检测特征点，且抽取前后的信息处理循环之间取 得对应的特征点，并根据特征点的图像上的位置OJ1J1)计算出距离及姿势角的变化量及车 辆的移动量。
[0075]具体而言，首先，特征点检测部23从存储器读取由摄像机12取得的图像38,且从图 像38检测路面31上的特征点，并将各特征点的图像上的位置(Ui、V1)存储于存储器。姿势变 化量计算部24从存储器读取各特征点的图像上的位置(Ui、V 1),并根据距离及姿势角和特征 点的图像上的位置OJ1J1)计算出特征点相对于摄像机12的相对位置(HZ 1)。此外，姿势 变化量计算部24使用之前的信息处理循环的步骤S09中设定的起点（距离及姿势角）。姿势 变化量计算部24将特征点相对于摄像机12的相对位置(X i、Yi、Zi)存储于存储器中。
[0076]而且，姿势变化量计算部24从存储器读取特征点的图像上的位置(U1J1)和之前的 信息处理循环的步骤S07中计算出的特征点的相对位置(Xi、Yi、Zi)。姿势变化量计算部24使 用前后的信息处理循环之间取得对应的特征点的相对位置(X 1 JnZ1)及图像上的位置(仏、 V1),计算出距离及姿势角的变化量。另外，根据上一次的信息处理循环中的特征点的相对 位置(X 1U1)和此次的信息处理循环中的特征点的相对位置(HZ1),计算出车辆的移 动量。步骤S07中计算出的"距离及姿势角的变化量"及"车辆的移动量"在步骤Sll的处理中 使用。
[0077]进入步骤S09，ECU13根据多个特征点的检测状态及图案光的距离及姿势角的计算 状态，设定积分运算的起点。详情参照图10进行后述。
[0078] 进入步骤Sll，自己位置计算部26根据步骤S09的处理中设定的积分运算的起点、 及步骤S07的处理中计算出的"距离及姿势角的变化量"及"车辆的移动量"，计算出距离及 姿势角、及车辆的当前位置。
[0079]这样，本实施方式的自己位置计算装置通过重复执行上述的一连串的信息处理循 环并累计车辆10的移动量，可以计算出车辆10的当前位置。
[0080]参照图8的流程，说明图7的步骤S05的详细顺序。首先，在步骤S501中，检测状态判 断部30判断是否检测有4个以上的前后帧间取得对应的特征点。在检测有4个以上特征点的 情况下（S501中，是），根据特征点的时间变化可计算出距离及姿势角的变化量。于是，进入 步骤S505,图案光控制部27将图案光投射实施标志设定成OFF状态，且发送至投光器11。然 后，不根据图案光32a的位置计算出距离及姿势角，并进入图7的步骤S07。
[0081]另一方面，在未检测4个以上特征点的情况下（S501中，否），判断为多个特征点的 检测状态不满足第一基准而是差的状态，因此，进入步骤S503。而且，图案光控制部27将图 案光投射实施标志设定成ON状态，并发送至投光器11。然后，进入步骤S507。
[0082]步骤S507中，检测状态判断部30判断是否检测有3个以上的前后帧间取得对应的 特征点。在检测有3个以上特征点的情况下（S507中，是），根据特征点的时间变化可计算出 距离及姿势角的变化量。因此，不根据图案光32a的位置计算出距离及姿势角，进入图7的步 骤 S07。
[0083]另一方面，在未检测有3个以上特征点的情况下（S507中，否），不能根据特征点的 时间变化计算出距离及姿势角的变化量。因此，进入步骤S509,姿势角计算部22根据图案光 32a的位置计算出距离及姿势角。然后，进入图7的步骤S07。
[0084]步骤S509中，首先，图案光抽取部21从存储器读取由摄像机12取得的图像38,如图 4(c)所示，从图像38抽取图案光32a的位置。图案光抽取部21将作为表示图案光32a的位置 的数据计算出的光点&的图像上的坐标(山、％)存储于存储器中。
[0085]姿势角计算部22从存储器读取表示图案光32a的位置的数据，根据图案光32a的位 置计算出距离及姿势角，并存储于存储器中。
[0086]参照图9说明按照图8的流程执行的信息处理的一例。图9(a)表示姿势角计算部22 是否根据图案光32a计算出距离及姿势角，图9(b)表示图案光投射实施标志的状态，图9(c) 表示取得对应的特征点的数量。图9 (a)~（c)的横轴表示时间的经过，t0、11、t2、......表示 信息处理循环（以后，均简称为"循环"）。
[0087]循环t0~t3中，取得对应的特征点的数量为4个以上，因此，图案光投射实施标志 为OFF状态，"三维测量实施标志"也为OFF状态。如果"三维测量实施标志"为OFF状态，则姿 势角计算部22不根据图案光32a计算出距离及姿势角。循环t4~t6中，取得对应的特征点的 数量成为3个时，图8的步骤S501判断为否，步骤S503中图案光投射实施标志成为ON状态，但 在步骤S507中判断为是，因此，三维测量实施标志仍为OFF状态。循环t7~tlO中取得对应的 特征点的数量成为4个以上时，图案光投射实施标志成为OFF状态。循环112~114中，取得对 应的特征点的数量成为低于3个，因此，步骤S501及S507中均判断为否，因此，图案光投射实 施标志及三维测量实施标志成为ON状态，实施利用姿势角计算部22进行距离及姿势角的计 算。
[0088] 参照图10的流程说明图7的步骤S09的详细顺序。步骤S900中，E⑶13判断此次的信 息处理循环是否为初次。而且，在为初次的情况下，即在没有上一次的信息处理循环的数据 的情况下，将处理进入步骤S905，在不是初次的情况下，将处理进入步骤S901。
[0089]步骤S901中，检测状态判断部30判断由特征点检测部23检测的特征点Te的检测状 态是否不满足第二基准而是差的状态。即，如与图8的步骤S507相同，判断取得对应的特征 点Te的数是否为3个以上。在判断为不是3个以上的情况下（步骤S901中为是），进入步骤 S903,在判断为3个以上的情况下(步骤S901中为否），进入步骤S909。
[0090] 步骤S909中，E⑶13维持当前设定的积分运算的起点。
[0091] 步骤S903中，计算状态判断部35判断由姿势角计算部22计算距离及姿势角的计算 状态是否不满足第三基准而是差的状态。例如在相同的信息处理循环的步骤S509中判断距 离及姿势角的计算是否成功。在判断为成功的情况下(步骤S903中为是），进入步骤S905,在 判断为不成功的情况下(步骤S903中为否），进入步骤S907。
[0092] 步骤S905中，E⑶13将步骤S509中由姿势角计算部22计算出的距离及姿势角、及此 时的车辆的当前位置设定为积分运算的起点。以该距离及姿势角为起点开始新的积分运 算。另外，以车辆的当前位置为起点开始新的积分运算。
[0093]步骤S907中，E⑶13将上一次的信息处理循环中采用的距离及姿势角、及上一次的 信息处理循环中的车辆的当前位置设定为积分运算的起点。以该距离及姿势角为起点开始 新的积分运算。另外，以车辆的当前位置为起点开始新的积分运算。然后，使处理进入图8的 步骤Sll。
[0094]如以上说明，根据第一实施方式，可得到以下的作用效果。
[0095]当多个特征点的检测状态差时，由姿势变化量计算部24计算出的距离及姿势角的 计算精度降低，因此，车辆10的移动量的推定误差变大。图11(a)及图11(b)是表示车辆10的 侧滚角(姿势角的一例)及移动量(车宽方向）的推定误差的例子的图，表示车辆10在无倾斜 的平坦的路面上以一定的速度直行时的、侧滚角的推定值及移动量的计算值的时间变化。 图中的 uP1 "及"p2"表示不管多个特征点的检测状态如何，均维持移动量计算的起点及距离 及姿势角的积分运算的起点的比较例。"Qi"及"Q 2"表示真实值。车辆10进行直行，因此，侧滚 角及车宽方向的移动量的真实值(Qi、Q2)为保持为0的状态。但是，在时刻tl~t2之间，由于 在多个特征点的检测状态差的混凝土路面行驶，因此，路面的平面方程式中产生误差，侧滚 角中出现推定误差。侧滚角的误差会作为车宽方向的移动量的误差反映。比较例中，根据多 个特征点的检测状态，不能将移动量计算的起点及侧滚角的积分运算的起点重置至根据图 案光32a计算出的距离及姿势角、及此时的车辆的当前位置。因此，车辆10的移动量的误差 会扩大。
[0096]根据第一实施方式，在检测状态判断部30判断为多个特征点的检测状态不满足第 一基准而是差的状态的情况下(步骤S501中为否），图案光控制部27投射图案光32a。由此， 可以根据图案光32a的位置计算出距离及姿势角。因此，以根据图案光32a计算出的误差较 小的距离及姿势角为起点，可重新开始积分运算。因此，可以高精度且稳定地推定车辆10的 当前位置。
[0097]另外，多个特征点的检测状态变差的路面包含例如成为特征点的对象的状况或凹 凸较少的路面。状况或凹凸较少的路面的平坦性较高，因此，根据图案光32a计算出的距离 及姿势角所包含的噪声成分较少。另一方面，在多个特征点的检测状态良好的路面31，由于 沥青的凹凸，图案光32a的反射位置不稳定，因此，根据图案光32a计算出的距离及姿势角包 含较多的噪声成分。根据第一实施方式，可以根据多个特征点的检测状态，适当地分为使用 了特征点的距离及姿势角的更新和使用了图案光32a的距离及姿势角的计算，并高精度且 稳定地求出距离及姿势角。
[0098]另外，由于路面31的照度的急剧变化，有时图像38的对比度降低，且多个特征点的 检测状态变差。根据第一实施方式，通过直接判断多个特征点的检测状态，不需要额外设置 照度计等传感器，可减少成本，且有助于装置的轻量化。
[0099]根据第一实施方式，在检测状态判断部30判断为多个特征点的检测状态满足第一 基准而不是差的状态的情况下(步骤S501中为是），图案光控制部27停止图案光32a的投射。 仅在多个特征点的检测状态不满足第一基准而是差的状态的情况下可以投射图案光32a， 因此，可以仅在需要的情况下投射图案光32a，并抑制投光所需要的耗电量。
[0100] 图7的步骤S05中的图案光32a的投光及熄灭（步骤S503、S505)的判断在下面的信 息处理步骤的步骤SOl中反映。因此，为了使用图案光32a计算出距离及姿势角，需要预先在 之前的循环中将图案光投射实施标志切换成ON状态。于是，第一实施方式中，如果是多个特 征点的检测状态不满足第一基准而是差的状态，则将图案光投射实施标志切换成ON状态 (S05)。在其下面的循环中，投射图案光32a(S01)。而且，在相同的循环中，在多个特征点的 检测状态不满足第二基准而是差的状态的情况下(S507中为否），姿势角计算部22可以根据 图案光32a的位置计算出距离及姿势角。这样，通过使用两个基准(第一基准，第二基准)判 断多个特征点的检测状态，可以适当地控制图案光32a的投光时间及距离及姿势角的计算 时间。
[0101] (第二实施方式）
[0102] 第二实施方式中，说明根据从开始图案光32a的投射起的经过时间控制使用了图 案光32a的距离及姿势角的计算时间的例子。第二实施方式的自己位置计算装置的硬件构 成如图1所示，因此，省略说明。另外，由ECU13重复执行的信息处理循环除了步骤S05之外， 也如图7所示那样，因此，省略说明。后述的第三及第四实施方式也同样。
[0103] 参照图12说明第二实施方式的步骤S05的详细顺序。图12所示的流程中，代替图8 的步骤S507,而实施步骤S511。其它步骤与图8相同，而省略说明。ECU13测量从开始图案光 32a的投射起的经过时间。
[0104]具体而言，E⑶13在步骤S503中测量从将图案光投射实施标志从OFF状态切换至ON 状态起的经过时间。步骤S511中，ECU13判断从切换图案光投射实施标志起是否经过第一规 定时间。如果未经过第一规定时间（S511中为否），则不根据图案光32a的位置计算出距离及 姿势角，而进入步骤S07。另一方面，如果经过第一规定时间（S511中为是），则进入步骤 S509,根据图案光32a的位置计算出距离及姿势角，然后，进入步骤S07。
[0105] 此外，第一规定时间例如图13所示，可以设定两次的信息处理循环(Dl)。即，将图 案光投射实施标志从OFF状态切换至ON状态的信息处理循环（til)及下一次的信息处理循 环(tl2)中，不计算出距离及姿势角(在S511中为否），或者在下一次(第二次)的信息处理循 环(tl3)中，首次计算出距离及姿势角(S511中为是，S509)。
[0106] 如之前叙述，为了使用图案光32a计算出距离及姿势角，需要预先在之前的循环中 将图案光投射实施标志切换成ON状态。第二实施方式中，图案光控制部27从开始图案光32a 的投射起经过第一规定时间(Dl)之后，姿势角计算部22开始根据图案光32a的位置计算出 距离及姿势角。由此，可以适当地控制图案光32a的投光时间及距离及姿势角的计算时间。
[0107] (第三实施方式）
[0108] 第三实施方式中，说明如下例子，即，在开始图案光32a的投射后，即使判断为多个 特征点的检测状态满足第一基准且不是差的状态，也将图案光32a的投射从该判断起继续 到经过第二规定时间为止。
[0109] 在检测状态判断部30判断为多个特征点的检测状态是不比第一基准差的情况下， 图案光控制部27在从检测状态判断部30判断起经过第二规定时间之后，停止图案光32a的 投射。
[0110] 参照图14说明第三实施方式的步骤S05的详细顺序。相对于图8,图14所示的流程 还具备步骤S513及步骤S515。其它步骤与图8相同而省略说明。
[0111] ECU13测量从判断为取得对应的特征点的数量为4个以上起的经过时间。在判断为 取得对应的特征点的数量为4个以上的情况下（S501中为是），进入步骤S513，ECUl 3判断为 取得对应的特征点的数量为4个以上后，判断是否经过第二规定时间。如果经过第二规定时 间（S513中为是），则进入步骤S505,图案光控制部27将图案光投射实施标志从ON状态向OFF 状态切换。另一方面，如果未经过第二规定时间（S513中为否），则进入步骤S515,图案光控 制部27将图案光投射实施标志维持成ON状态。
[0112] 此外，第二规定时间例如图15所示，可以设定两次的信息处理循环(D2)。即，取得 对应的特征点的数量成为4个以上的信息处理循环(t6、tl4)及下一次的信息处理循环(t7、 tl5)中将图案光投射实施标志维持成ON状态(S513中为否，S515)，且在下一次(第二次）的 信息处理循环（t8、tl6)中，首次将图案光投射实施标志切换成OFF状态（S513中为是， S505)〇
[0113] 由此，如果暂时开始图案光32a的投射，则之后即使改善多个特征点的检测状态， 图案光32a也不立即熄灭。即，图案光32a的投光状态中可以具有滞后(滞后效应），因此，可 以抑制图案光32a的闪烁，且减轻闪烁引起的烦恼。
[0114](第四实施方式）
[0115] 第四实施方式中，与第三实施方式对称地说明图案光32a的熄灭状态中具有滞后 的例子。在使图案光32a熄灭后，即使多个特征点的检测状态不满足第一基准而变差，从该 判断起图案光控制部27也将图案光32a的熄灭继续到经过第三规定时间。在检测状态判断 部30判断为多个特征点的检测状态不满足第一基准而是差的状态的情况下，图案光控制部 27在从检测状态判断部30判断起经过第三规定时间之后，开始图案光32a的投光(投射）。
[0116] 参照图16说明与第四实施方式相关的步骤S05的详细顺序。相对于图8,图16所示 的流程还具备步骤S517。另外，步骤S501中，判断取得对应的特征点的数量是否为5个以上。 即，在取得对应的特征点的数量不是5个以上的情况下，检测状态判断部30判断为多个特征 点的检测状态不满足第一基准而是差的状态。其它步骤与图8相同并省略说明。
[0117] ECU13测量从判断为取得对应的特征点的数量不是5个以上起的经过时间。在判断 为取得对应的特征点的数量不是5个以上的情况下（S501中为否），进入步骤S517，ECU13判 断为取得对应的特征点的数量不是5个以上后，判断是否经过第三规定时间。如果经过第三 规定时间（S517中为是），则进入步骤S503,图案光控制部27将图案光投射实施标志切换成 ON状态。另一方面，如果未经过第三规定时间（S517中为否），则进入步骤S505,图案光控制 部27将图案光投射实施标志维持成OFF状态。
[0118]此外，第三规定时间可以如例如图17所示设定两次的信息处理循环(D3)。即，取得 对应的特征点的数量低于5个的信息处理循环(t2、t9)及下一次的信息处理循环（t3、tl0) 中，将图案光投射实施标志维持成OFF状态(S517中为否，S505)，且在下一次(第二次）的信 息处理循环(t4、tll)中，首次将图案光投射实施标志切换成ON状态(S517中为是，S503)。
[0119]由此，如果暂时停止图案光32a的投射，则之后，即使多个特征点的检测状态恶化， 也立即不开始图案光32a的投射。即，图案光32a的熄灭状态中可以具有滞后(滞后效应），因 此，可以抑制图案光32a的闪烁，并减少闪烁产生的烦恼。
[0120] 如上所述，记载了本发明的第一~第四实施方式，但不应理解为形成该公开的一 部分的论述及附图限定于本发明。根据本公开内容，对于本领域技术人员来说，可知有各种 代替实施方式、实施例及运用技术。
[0121] 图2表示将摄像机12和投光器11安装于车辆10的前面的例子，但也可以朝向车辆 10的侧方、后方、正下方设置。另外，本实施方式中，作为车辆10的一例，图2中表示了四轮的 乘用车，但可以适用于摩托车、货车、或例如搬运工程机械的特殊车辆等、可拍摄道路的路 面或壁面上的特征点的全部移动体(车辆）。
[0122] 符号说明
[0123] 10 车辆
[0124] 11投光器
[0125] 12摄像机(摄像部）
[0126] 21图案光抽取部
[0127] 22姿势角计算部
[0128] 23特征点检测部
[0129] 24姿势变化量计算部
[0130] 26自己位置计算部
[0131] 27图案光控制部
[0132] 30检测状态判断部
[0133] 31 路面
[0134] 32a、32b 图案光
[0135] 35计算状态判断部
[0136] Te特征点
【主权项】
1. 一种自己位置计算装置，其特征在于，具备： 投光器，其向车辆周围的路面投射图案光； 摄像部，其搭载于所述车辆，拍摄包含投射图案光的区域在内的车辆周围的路面并取 得图像； 姿势角计算部，其根据由所述摄像部取得的图像中的所述图案光的位置，计算出车辆 相对于所述路面的姿势角； 姿势变化量计算部，其基于根据由所述摄像部取得的图像检测出的所述路面上的多个 特征点的时间变化，计算出所述车辆的姿势变化量； 自己位置计算部，其通过在所述车辆的初始位置及姿势角加上所述姿势变化量，计算 出所述车辆的当前位置及姿势角； 检测状态判断部，其判断所述姿势变化量计算部中的所述多个特征点的检测状态； 图案光控制部，其控制所述投光器进行的图案光的投射， 在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下， 所述图案光控制部投射图案光。2. 如权利要求1所述的自己位置计算装置，其特征在于， 在所述检测状态判断部判断为所述多个特征点的检测状态满足第一基准的情况下，所 述图案光控制部停止图案光的投射。3. 如权利要求1或2所述的自己位置计算装置，其特征在于， 在所述检测状态判断部判断为所述多个特征点的检测状态不满足比所述第一基准低 的第二基准的情况下，所述姿势角计算部根据所述图案光的位置计算出车辆相对于所述路 面的姿势角。4. 如权利要求1或2所述的自己位置计算装置，其特征在于， 从所述图案光控制部开始图案光的投射起，经过第一规定时间之后，所述姿势角计算 部根据所述图案光的位置开始计算出车辆相对于所述路面的姿势角。5. 如权利要求1~4中任一项所述的自己位置计算装置，其特征在于， 在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下， 所述图案光控制部在从所述检测状态判断部进行判断起经过第二规定时间之后，停止图案 光的投射。6. 如权利要求1~4中任一项所述的自己位置计算装置，其特征在于， 在所述检测状态判断部判断为所述多个特征点的检测状态不满足第一基准的情况下， 所述图案光控制部在从所述检测状态判断部进行判断起经过第三规定时间之后，开始图案 光的投射。7. -种自己位置计算方法， 向车辆周围的路面投射图案光， 拍摄包含投射所述图案光的区域在内的车辆周围的路面，并取得图像， 根据所述图像中的所述图案光的位置，计算出车辆相对于所述路面的姿势角， 基于根据所述图像检测出的所述路面上的多个特征点的时间变化，计算出所述车辆的 姿势变化量， 通过在所述车辆的初始位置及姿势角加上所述姿势变化量，计算出所述车辆的当前位 置及姿势角， 其特征在于， 在判断为所述多个特征点的检测状态不满足第一基准的情况下，投射所述图案光。
【文档编号】G08G1/16GK106062849SQ201480076204
【公开日】2016年10月26日
【申请日】2014年2月24日
【发明人】山口郎, 山口一郎, 西内秀和
【申请人】日产自动车株式会社