移动体位置估计装置以及移动体位置估计方法
【技术领域】
[0001 ]本发明涉及移动体位置估计装置以及移动体位置估计方法。
【背景技术】
[0002]在专利文献I中，对每个粒子进行由摄像机的摄像图像生成的边缘图像与使用粒子滤波器由已知的三维地图生成的假想图像的边缘的匹配，通过边缘的重叠越多使似然越高，边缘的重叠越少使似然越低，由似然分布概率性地估计移动体的位置。
[0003]现有技术文献
[0004]专利文献
[0005]专利文献1:日本专利特开2010-60451号公报
【发明内容】
[0006]发明要解决的课题
[0007]但是，在上述以往技术中，由于从边缘的重叠程度计算似然，所以即使在粒子的参数接近移动体的真实的位置的情况下，当粒子的位置的参数从真实的位置仅偏移了一点时，由于该粒子的似然极端地降低，产生估计变得不稳定的问题。
[0008]本发明的目的是提供可以稳定地估计移动体的位置的移动体位置估计装置以及移动体位置估计方法。
[0009]用于解决课题的手段
[0010]在本发明中，对于每个粒子比较边缘图像和假想图像，边缘重叠的数越多则给予越高的评价值，并且，作为不重叠的边缘，在边缘图像中的边缘与假想图像中的边缘的距离即边缘间距离为规定距离以下的边缘越多，则给予越高的评价值。
[0011]发明的效果
[0012]因此，可以稳定地估计移动体的位置。
【附图说明】
[0013]图1是实施例1的移动体位置估计装置的结构图。
[0014]图2是实施例1的计算机3中的自身位置估计的控制方框图。
[0015]图3是表示实施例1的移动体位置估计装置的粒子的散布方法的说明图。
[0016]图4是表示实施例1的与像素数对应的评价校正值的计算方法的说明图。
[0017]图5是表示实施例1的与实际距离对应的评价校正值的计算方法的说明图。
[0018]图6是表示实施例1的计算机3的全体处理的流程的流程图。
[0019]图7是表示实施例1的自身位置估计处理的流程的流程图。
[0020]图8是表示实施例1的自身位置估计作用的图。
[0021]图9是表示实施例2的与边缘密度对应的评价校正值的计算方法的说明图。
[0022]I 车辆
[0023]2摄像机(摄像单元)
[0024]3计算机
[0025]4存储部件(存储单元)
[0026I11特征提取部件(边缘图像生成单元)
[0027]12初始化处理部件
[0028]13位置、姿态候选生成部件
[0029]14假想图像生成部件(假想图像生成单元)
[°03°]15评价值计算部件(评价值计算单元)
[0031]15a评价校正值计算部件
[0032]16位置、姿态估计部件(位置估计单元)
[0033]17初始化判定部件
【具体实施方式】
[0034]〔实施例1〕
[0035]图1是实施例1的移动体位置估计装置的结构图。
[0036]车辆I包括:摄像机(摄像单元)2;计算机3;以及存储部件(存储单元)4。
[0037]在车辆I的前端部，以高度h、从水平下向角度Θ安装摄像机2，拍摄车辆I的前方区域。
[0038]计算机3实施在存储部件4中存储的地图数据与由摄像机2拍摄的图像的匹配处理，估计车辆I的位置、姿态。
[0039]存储部件4记录具有车辆I的周围存在的结构物的边缘信息和位置信息的三维的地图数据。
[0040]图2是实施例1的计算机3中的自身位置估计的控制方框图。
[0041]摄像机2的镜头为一般的无失真的镜头，但是也可以例如为了拍摄宽范围而设为鱼眼镜头。在该情况下，如果将如后所述的假想图像生成部件14中的摄像机模型设为鱼眼的模型，则可以以相同的框架估计车辆的自身位置。而且，在为了在更宽范围拍摄车辆周边而安装了鱼眼镜头的情况下，也可以车载多个摄像机。
[0042]特征提取部件(边缘图像生成单元)11从摄像机2拍摄的图像提取边缘而生成边缘图像。为了提取该边缘，使用Canny法等已存在的边缘检测方法即可。当然，也可以与问题相应，用Sobel滤波器等方法检测边缘，重要的是，只要根据地图数据，在与从某个被提供的位置、姿态的参数和摄像机2的模型生成的假想图像的匹配中，可以从图像中观测足够精度的边缘即可。
[0043]初始化处理部件12进行为了估计车辆的位置、姿态而利用的粒子滤波器的初始化处理。在实施例1中，在车辆的被设想的位置、姿态的范围进行粒子群的(位置、姿态)参数的初始化。此次，由于是在受到限制的区间中估计车辆的位置、姿态的问题，所以设为表示车辆位置的3自由度(x，y，z)、以及表示车辆姿态的3自由度(yaw,pitch,roll)的合计6自由度(参照图3)。在使用地图数据估计自身位置的实施例1的情况下，为了设定初始化的范围也可以使用GPS来设定粗略的位置范围。而且，虽然这时需要设定粒子的数，但是只要与问题相应地设定适当的数即可。由于粒子滤波器自身为已知的方法，所以省略细节。
[0044]位置、姿态候选生成部件13使用预先设定的车辆的系统模型，从一时刻前的粒子群的位置、姿态参数，设定当前时刻的粒子群的参数(在刚经过初始化处理部件之后，从该被初始化的值设定当前时刻的参数)。该框架也是粒子滤波器的范畴，省略细节，但是在实施例I中，将系统模型设为随机漫步(在规定范围内随机地设想车辆运动。)。当然，也可以与问题相应，将等速直线运动等的模型作为系统模型。例如，如图3所示，使I循环前估计的车辆V(tl)的位置以及姿态角的粒子P和周围的粒子Pl?P5仅移动相当于里程计(odome try)的量，设定并校正粒子的存在分布范围。然后，为了估计新的车辆V(t2)的位置以及姿态角，设定粒子PlO?P15。
[0045]假想图像生成部件(假想图像生成单元)14参照存储部件4，使用在前述的位置、姿态候选生成部件13中对某个粒子设定的位置、姿态的参数和摄像机2的摄像机模型，生成假想图像。一般来说，车辆的位置、姿态被作为参数提供，并且摄像机2的摄像机模型已知(只要事前测量即可。或者也可以是设计值)，进而，如果存储部件4的三维数据已知，则可以将三维的地图数据变换为二维的图像(将其称为假想图像)。
[0046]在实施例1中，在如后所述的评价值计算部件15中进行假想图像生成部件14中生成的假想图像的边缘、与特征提取部件11从摄像机2拍摄的图像提取的图像中的边缘的匹配。因此，在假想图像生成部件14中，仅将对应于从特征提取部件11拍摄的图像中作为边缘提取的成分的、三维的地图数据的边缘信息投影到假想图像即可。具体地说，只要投影地图中的建筑物的边缘部分和路面上的白线等即可，不需要投影建筑物和路面的纹理(texture)、颜色等信息。
[0047]虽然记载了在前述的位置、姿态候选生成部件13中与问题相应地设定粒子的数即可，但是在假设生成100个粒子的情况下，将该处理重复100次。
[0048]评价值计算部件(评价值计算单元)15进行从特征提取部件11输出的边缘图像与从假想图像生成部件14输出的假想图像的边缘成分的匹配，评价两者的边缘的重叠程度，越重叠，计算越高的评价值(似然)e。在以往的评价值计算方法中，扫描两个图像，如果关注像素(Xi，yi)中都存在边缘，则加上评价值(Eval (Xi，yi) = I)，在不是这样的情况下不加上(Eval (xi，yi) = O)，越重叠，评价值越高地进行设定，相对于此，在实施例1中，以提高使用了粒子滤波器的位置、姿态估计方法的精度以及稳定性为目标，在计算评价值时，即使边缘不重叠，在最接近的边缘之间的距离(边缘间距离)近的(规定距离以下)情况下，加上与两边缘的接近度相应的评价校正值。评价校正值的计算通过评价值计算部件15内的评价校正值计算部件15a来进行。
[0049]评价校正值可以根据从假想图像中的边缘至边缘图像中的边缘为止的像素数(像素数)来设定。例如，对于假想图像中的边缘，设与边缘图像中的边缘偏移I或者2像素的像素(11，71)的像素数对应的评价校正值6?&1，71)=0.5，设与偏移3或者4像素的像素&1，71)的像素数对应的评价校正值