CN108351216B - 估计装置、控制方法、程序以及存储介质 - Google Patents

Abstract

本发明提供一种能够以高精度估计当前位置的估计装置。驾驶支援系统包括：安装于车辆并执行车辆驾驶支援控制的车载器1、激光雷达2、陀螺仪传感器3以及车速传感器4。车载器1从激光雷达2获取表示具有索引k的参考地标Lk与本车辆之间的位置关系的测量值z_t ^k。车载装置1基于包括在地图DB 10中的参考地标Lk的测量值z_t ^k和位置矢量m^k，通过校正根据由车速传感器4测量的移动速度和由陀螺仪传感器3测量的角速率估计出的先验估计值x^‑ _t来计算后验估计值x^_t。

Description

估计装置、控制方法、程序以及存储介质

技术领域

本发明涉及以高精度来估计当前位置的技术。

现有技术

通常，已知用于测量相距于周围对象物的距离的技术。例如，专利文献1公开了配备有激光雷达的车辆的例子，该激光雷达通过利用间歇发射的激光进行水平扫描并接收反射光(散射光)来检测对象物表面的点群。专利文献2公开了一种点搜索装置，该装置具有包括关于多个目标搜索点的经纬度信息的地图数据库。

专利文献1：日本专利申请特开2014-089691

专利文献2：日本专利申请特开2015-135695

发明内容

本发明要解决的技术问题

在自动驾驶等领域中，需要对当前位置进行精确估计，但是，那种主要基于检测车辆状态的内部传感器的输出来估计当前位置的传统方法不足以执行精确估计。相比而言，专利文献1和专利文献2都没有公开以高精度计算车辆的绝对位置的方法。

以上是本发明要解决的问题的一个示例。本发明的目的在于提供一种能够高精度地估计当前位置的估计装置。

解决技术问题的手段

本发明一方面提供了一种估计装置，包括：获取单元，配置为获取地图信息；第一获取单元，配置为获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；第一估计单元，配置为基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。

本发明另一方面提供了一种由估计装置执行的控制方法，包括：获取处理，以便获取地图信息；第一获取处理，以便获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；第一估计处理，以便基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。

本发明另一方面提供了一种用于存储由计算机执行的程序的存储媒介，该程序使计算机用作：获取单元，配置为获取地图信息；第一获取单元，配置为获取到指示位于第一范围内的对象物的距离和角度的第一信息；第一估计单元，配置为基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。

附图说明

图1示出了根据第一实施例的驾驶辅助系统的示意性配置；

图2为示出了车载器的功能配置的框图；

图3示出了在二维直角坐标上的本车位置；

图4示出了预测步骤和测量/更新步骤之间的示意性关系；

图5示出了表示本车位置估计器的功能配置的框图；

图6是示出了在假想圆周运动之前和之后的由各变量表征的本车位置的示意图；

图7示出了地标的位置与本车位置之间的关系；

图8示出了地标提取块的功能配置；

图9示出了激光雷达的扫描范围与地标的搜索范围之间的关系；

图10是表示根据第一实施例的本车位置估计器所执行的处理的步骤的流程图；

图11是表示地标提取处理的详细内容的流程图；

图12示出了根据第二实施例的驾驶辅助系统的示意性配置；

图13示出了表示根据第二实施例的本车位置估计器的功能配置的框图；

图14示出了地标的位置与本车位置之间的关系。

优选实施方式的具体描述

根据本发明的优选实施例，提供了一种估计装置，包括：获取单元，配置为获取地图信息；第一获取单元，配置为获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；第一估计单元，配置为基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。

上述估计装置包括获取单元、第一获取单元以及第一估计单元。获取单元配置为获取地图信息。第一获取单元配置为获取相距于指示位于第一范围内的对象物的距离和角度(即，移动体和位于第一范围内的特征之间的位置关系)的第一信息。第一估计单元配置为基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。根据该模式，估计装置能够使用记录在地图信息中的特征的位置信息来精确地估计移动体的位置。

在估计装置的一种模式中，估计装置还包括配置为计算第一估计位置的第二估计单元，该第一估计位置是移动体的当前位置的估计位置，其中第一估计单元基于第一估计位置以及第一信息与第二信息之间的差来估计移动体的位置，该第二信息表示对象物与第一估计位置之间的位置关系。根据该模式，估计装置能够基于由第二估计单元估计的第一估计位置以及第一信息与第二信息之间的差来执行对移动体位置的精确估计。

在估计装置的另一模式中，第二估计单元至少基于在预定时刻之前估计的移动体位置来计算第一估计位置。因此，估计装置通过考虑在预定时刻之前估计的移动体位置，能够适当地估计出移动体的当前位置。

在估计装置的又一种模式中，估计装置还包括第二获取单元，其配置为获取移动体的控制信息，其中，第二估计单元基于在预定时刻之前估计的位置以及移动体的控制信息来计算第一估计位置。根据该模式，估计装置能够以高精度和低计算负担从预定时刻之前估计的移动体位置来计算第一估计位置。

在估计装置的又一种模式中，交替执行预测步骤和更新步骤，其中在该预测步骤中，第二估计单元计算第一估计位置，在该更新步骤中，第一估计单元基于第一信息和第二信息之间的差来校正在上一次预测步骤中计算出的第一估计位置。并且在预测步骤中，第二估计单元基于在此预测步骤之前的上一次更新步骤中校正的第一估计位置来计算当前时刻的第一估计位置。根据该模式，通过交替地执行更新步骤和预测步骤，估计装置能够校正先前计算的第一估计位置，从而以高精度和低计算负担确定当前时刻的第一估计位置。

在估计装置的又一种模式中，第一获取单元从测量装置中获取第一信息，该测量装置包括发射单元、光接收单元以及输出单元。发光单元发射激光并同时改变激光的出射方向，光接收单元接收由对象物反射的激光，输出单元基于由光接收单元输出的光接收信号、由光接收单元接收的激光的出射方向以及激光的响应延迟时间来输出第一信息。根据该模式，第一获取单元能够适当地生成并输出指示相距于位于第一范围内的对象物的距离和角度的第一信息。附加地，根据该模式，由于目标测量对象物是记录在地图信息中的三维特征，所以即使在各种情况下，例如夜间以及路面上的隔间线(白线)被雪遮住的情况，都能够通过测量相距于目标特征的距离和方位来适当地生成第一信息。

在估计装置的又一种模式中，所述对象物是人造物体。因此，与对象物是自然物体的情况相比，第一获取单元能够稳定地获取第一信息。

在估计装置的又一种模式中，所述对象物是周期性设置的人造物体。因此，估计装置可以周期性地估计移动体的位置。

根据本发明另一优选实施例，提供了一种由估计装置执行的控制方法，包括：获取处理，以便获取地图信息；第一获取处理，以便获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；第一估计处理，以便基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。通过执行上述控制方法，估计装置能够在使用关于记录在地图信息上的特征的位置信息的同时来精确地估计移动体的位置。

根据本发明另一优选实施例，提供了一种由计算机执行的程序，该程序使计算机用作：获取单元，配置为获取地图信息；第一获取单元，配置为获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；第一估计单元，配置为基于包括在地图信息和第一信息中的对象物的位置信息来估计移动体的位置。通过执行该程序，计算机能够在使用关于记录在地图信息上的特征的位置信息的同时来精确地估计移动体的位置。优选地，该程序可通过存储在存储介质中的状态来处置。

实施方式

下文将参照附图对本发明的优选实施例进行描述。

<第一实施例>

(1)示意性配置

图1示出了根据第一实施例的驾驶辅助系统的示意性配置。图1所示的驾驶辅助系统包括：安装在车辆上并执行对车辆的驾驶辅助控制的车载器1、激光雷达(光探测和测距，或激光照明式探测和测距)2、陀螺仪传感器3和车速传感器4。

车载器1电连接于激光雷达2、陀螺仪传感器3和车速传感器4。基于来自它们的输出，车载器1估计安装了该车载器1的车辆的位置(称为“本车位置”)。然后，基于本车位置的估计结果，车载器1对车辆进行自主驾驶控制，使车辆沿着经确定的路线行驶。车载器1存储有地图数据库(DB)10，该地图数据库中记录了道路信息以及关于地标的信息(称为“地标信息”)，其中，每个地标都位于道路上或道路周围，并且被用作标记。地标信息是指其中分配给每个地标的每个索引至少与每个地标的位置信息相关联的信息。基于地标信息，车载器1缩小 (限制)了通过激光雷达2搜索地标的搜索范围，并且通过相对激光雷达2的输出交叉检查地标信息来估计本车位置。以下，将作为用于车载器1估计本车位置的标记(基准)的地标称为“参考地标Lk”，并且参考地标Lk的索引由“k”表示。参考地标Lk是根据本发明的“对象物”的示例。

例如，可以作为参考地标Lk的候选地标为沿着道路周期性布置的特征，例如千米柱、百米柱、轮廓标、交通基础设施(例如标牌、方向指示牌和交通信号灯)、电线杆和路灯。优选地，上述地标是用于稳定测量的人造材料，并且更优选地是周期性地提供用于本车位置的周期性校正的特征。应该注意的是，这些特征的间隔不必严格地确定为恒定的时间段，并且这些特征只需要像电线杆和路灯那样具有某种程度的周期性。此外，这些特征的间隔可以根据行驶区域而不同。

激光雷达2通过相对于水平方向和垂直方向在预定角度范围(视场角)内发射脉冲激光束而离散地测量相距于外部对象物的距离，从而产生指示外部对象物的位置的三维点群信息。在本情形中，激光雷达2包括：发射单元，用于在改变激光的输出方向的同时发射激光；光接收单元，用于接收所发射的激光的反射光 (散射光)；以及输出单元，用于基于由光接收单元输出的光接收信号而输出扫描数据。基于由光接收单元接收的激光的出射方向和基于上述光接收信号指定的激光的响应延迟时间来产生扫描数据。根据本发明实施例的激光雷达2指向车辆的行驶方向，从而至少扫描车辆的前方。激光雷达2、陀螺仪传感器3和车速传感器4三者中的每一个都将其输出数据提供给车载器1。车载器1是根据本发明的“估计装置”的示例，激光雷达2是根据本发明的“测量装置”的示例。

图2是表示车载器1的功能配置的框图。车载器1主要包括接口11、存储单元12、输入单元14、控制单元15和输出单元16。这些元件通过总线相互连接。

接口11从包括了激光雷达2、陀螺仪传感器3和车速传感器4的传感器获取输出数据，并将输出数据提供给控制单元15。

存储单元12存储将要由控制单元15执行的程序以及控制单元15执行预定处理所需的信息。根据本发明实施例，存储单元12存储包括有地标信息的地图 DB 10。要注意的是，地图DB 10可以周期性地更新。在这种情况下，例如，控制单元15经由通信单元从存储地图信息的服务器装置接收关于本车位置区域的部分地图信息，然后用部分地图信息更新地图DB 10。

输入单元14接受用于指定路线搜索的目的地的输入和用于开启或关闭自主驾驶的输入。输出单元16包括在控制单元15的控制下进行输出的显示器和/或扬声器。

控制单元15包括用于执行程序的CPU，并且控制整个车载器1。根据本发明实施例，控制单元15包括本车位置估计器17和自主驾驶控制器18。

本车位置估计器17基于由激光雷达2测量的相距于参考地标Lk的距离和角度的测量值以及从地图DB 10提取的参考地标Lk的位置信息，来校正基于来自陀螺仪传感器3和车速传感器4的输出数据而估计的本车位置。在这种情况下，根据基于贝叶斯估计的状态估计方法，本车位置估计器17交替执行预测步骤和测量/更新步骤。在预测步骤中，本车位置估计器17根据来自陀螺仪传感器3和车辆速度传感器4的输出数据来估计本车位置，并且在测量/更新步骤中，本车位置估计器17校正在上一时刻的预测步骤中计算出的本车位置的估计值。在第一实施例中，示例性描述将针对使用扩展卡尔曼滤波器作为基于贝叶斯估计的状态估计方法的一个示例。本车位置估计器17是根据本发明的“获取单元”、“第一获取单元”、“第一估计单元”、“第二估计单元”、“第二获取单元”以及执行程序的计算机的示例。

参考地图DB 10，自主驾驶控制器18基于所确定的路线和由本车位置估计器 17估计的本车位置来执行车辆的自主驾驶控制。基于所确定的路线，自主驾驶控制器18确定目标轨道，并且通过向车辆发送指导信号来控制车辆的位置，以使得由本车位置估计器17估计的本车位置与目标轨迹之间的间隙在预定宽度内。

(2)用扩展卡尔曼滤波器估计本车位置

接下来，示例性描述将针对由本车位置估计器17执行的本车位置的估计处理。

(2-1)基本说明

在下文中，将解释由本车位置估计器17执行的处理的基本前提。以下，本车位置由状态变化矢量“x＝(x,y,θ)”表示。此外，将“-”添加到在预测步骤中暂时估得的估计值的字符的顶部，并且将“^”添加到在测量/更新步骤中更精确估得并更新后的估计值的字符的顶部。为了便于说明，在说明书中将在顶部添加“-”或“^”的字符表示为“A^”或“A^-”(“A”代表字符)。

图3示出了二维直角坐标上的状态变化矢量x。如图3所示，在二维x-y直角坐标中定义的平面上的本车位置由坐标“(x,y)”表示，本车方位由“θ”表示。这里，该方位θ被定义为车辆的行驶方向与x轴之间的角。例如，坐标“(x,y)”表示由经度和纬度的组合表示的绝对位置。

图4示出了预测步骤与测量/更新步骤之间的示意性关系。如上所述，根据如卡尔曼滤波器这样的基于贝叶斯估计的状态估计方法，通过交替执行预测步骤和测量/更新步骤的两步处理法顺序地执行对估计值的计算和对估计值的更新。因此，根据本发明实施例，通过重复预测步骤和测量/更新步骤，顺序地执行对状态变化矢量x的估计值的计算和对估计值的更新。以下，将作为计算标的的参考时刻(即当前时刻)处的状态变化矢量表示为“x^- _t”或“x^_t”。

在预测步骤中，通过将车辆的移动速度“v”和角速率“ω”(以下统称为“控制值u_t＝(v_t,ω_t)^T”)应用于在上一次的测量/更新步骤中计算出的在时刻t-1处的状态变化矢量x^_t-1，本车位置估计器17计算在时刻t处的本车位置的估计值(称为“先验估计值”)x^- _t。控制值u_t是根据本发明的“控制信息”的示例，并且先验估计值x^- _t和后验估计值x^_t都是根据本发明的“第一估计位置”的示例。同时，本车位置估计器17根据在上一次的测量/更新步骤中在时刻t-1处计算的协方差矩阵“Σ^- _t-1”，来计算对应于先验估计值x^- _t的误差分布的协方差矩阵(称为“先验协方差矩阵”)“Σ^- _t”。

在测量/更新步骤中，本车位置估计器17获取由激光雷达2测量的参考地标 Lk的测量值“z_t”，并获取通过对激光雷达2的测量过程进行建模进而从先验估计值x^- _t计算出的上述参考地标Lk的估计测量值“z^_t”。如下文所述，测量值z_t是表示由激光雷达2在时刻t处测量的参考地标Lk的距离和扫描角度的二维矢量。然后，如下面的等式(1)所示，本车位置估计器17将测量值z_t与估计测量值z^_t之差乘以在另一计算中确定的卡尔曼增益“Kt”，然后加上先验估计值x^- _t。由此，本车位置估计器17计算处更新后的状态变化矢量(称为“后验估计值”) x^_t。

[第一等式]

在测量/更新步骤中，和在预测步骤中一样，本车位置估计器17根据先验协方差矩阵Σ^- _t计算对应于后验估计值x^的误差分布的协方差矩阵(称为“后验协方差矩阵”)“Σ^_t”。

(2-2)过程概述

图5示出了表示本车位置估计器17的功能配置的框图。本车位置估计器17 主要包括状态转换模型块20、协方差计算块21、地标提取块22、测量模型块23、协方差计算块24、卡尔曼增益计算块25、协方差更新块26、操作块31至33，以及单位延迟块34、35。

基于控制值u_t＝(v_t,ω_t)^T，状态转换模型(速度作用模型)块20根据在时刻 t-1处的后验估计值x^_t-1计算先验估计值x^- _t＝(x^- _t,y^- _t,θ^- _t)^T。图6是表示在参考时刻t处的先验估计值“x^- _t”，“y^- _t”，“θ^- _t”与在时刻t-1处的后验估计值“x^_t-1”，“y^_t-1”，“θ^_t-1”的关系的示意图。根据图6所示的几何关系，先验估计值x^- _t＝ (x^- _t，y^- _t，θ^- _t)^T由下面的等式(2)表示。符号“Δt”表示时刻t与时刻t-1之间的时间差。

[第二等式]

因此，状态转换模型块20通过使用等式(2)根据在时刻t-1处的后验估计值x^_t-1和控制值u_t＝(v_t,ω_t)^T来计算先验估计值x^- _t。

协方差计算块21通过使用矩阵“Rt”和雅可比矩阵“Gt”，基于以下等式(3) 来计算先验协方差矩阵Σ^- _t，其中矩阵Rt表示转换成状态变化矢量(x,y,θ)的三维坐标系的控制值u_t的误差分布，并且雅可比矩阵Gt则是通过围绕后验估计值x^_t-1将由等式(2)所表示的状态转换模型线性化来获取。

[第三等式]

这里，雅可比矩阵Gt由以下等式(4)表示。

[第四等式]

基于激光雷达2的输出和先验估计值x^- _t，地标提取块22从地图DB 10中提取参考地标Lk在x-y坐标系上的位置矢量“m^k＝(m_k,x,m_k,y)”。地标提取块22 向操作块31提供由激光雷达2测量并且与提取的具有索引k的参考地标Lk相对应的测量值“z_t ^k＝(r_t ^k,φ_t ^k)^T”。这里，测量值z_t ^k是其元素为距离“r_t ^k”和扫描角“φ_t ^k”的矢量值，其中距离r_t ^k表示相距于具有索引k的地标的距离，扫描角φ_t ^k表示车辆的行驶方向与具有索引k的地标之间的角度。测量值z_t ^k是根据本发明的“第一信息”的示例。稍后将描述由地标提取块22指定参考地标Lk的方法。

测量模型块23通过具有索引k的参考地标Lk的位置矢量m^k和先验估计值 x^- _t来计算测量值z_t ^k的估计值“z^_t ^k＝(r^_t ^k,φ_t ^k)”，以将其提供给操作块31。这里的符号“r^_t ^k”表示从先验估计值x^- _t的位置到具有索引k的地标的距离，而符号“φ^_t ^k”表示针对先验估计值x^- _t而言相距于具有索引k的地标的扫描角度。图7 示出了参考地标Lk的位置与本车位置之间的关系。根据图7所示的几何关系，在图7中，距离r^_t ^k由下面的等式(5)表示。

[第五等式]

此外，基于图7所示的关系，满足下列等式(6)。

[第六等式]

因此，扫描角φ^t^k表示为以下等式(7)。

[第七等式]

因此，参照等式(5)和(7)，测量模型块23计算对应于测量值z_t ^k的估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T，并将其提供给操作块31。估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T是根据本发明的“第二信息”的示例。

此外，测量模型块23计算雅可比矩阵“H_t ^k”，其中雅可比矩阵“H_t ^k”通过围绕先验估计值x^- _t将由等式(5)和(7)所表示的测量模型线性化而获取。雅可比矩阵H_t ^k由以下等式(8)表示。

[第八等式]

测量模型块23将雅可比矩阵H_t ^k分别提供给协方差计算块24、卡尔曼增益计算块25和协方差更新块26。

基于以下等式(9)，协方差计算块24计算为了计算卡尔曼增益K_t ^k所需的协方差矩阵“S_t ^k”。

[第九等式]

协方差计算块24将计算出的协方差矩阵S_t ^k提供给卡尔曼增益计算块25。

卡尔曼增益计算块25基于以下等式(10)计算卡尔曼增益K_t ^k。

[第十等式]

参考以下使用了单位矩阵“I”的等式(11)，协方差更新块26基于从协方差计算块21提供的先验协方差矩阵Σ^- _t、从测量模型块23提供的雅可比矩阵H_t ^k和从卡尔曼增益计算块25提供的卡尔曼增益K_t ^k，来计算后验协方差矩阵Σ^_t。

[第十一等式]

操作块31计算从地标提取块22提供的测量值z_t ^k与从测量模型块23提供的估计值z^_t ^k之间的差(即，“z_t ^k-z^_t ^k”)。操作块32将由操作块31计算出的值乘以从卡尔曼增益计算块25提供的卡尔曼增益K_t ^k。如以下等式(12)所示，操作块33通过将由操作块32计算出的值加上先验估计值x^- _t来计算后验估计值x^_t。

[第十二等式]

如上所述，本车位置估计器17能够通过顺序地重复预测步骤和测量/更新步骤来精确地执行状态估计。除了扩展卡尔曼滤波器之外，开发用于执行贝叶斯估计的各种滤波器均可用作要在这些步骤中使用的状态估计滤波器。例如，可以使用无迹卡尔曼滤波器和/或粒子滤波器来代替扩展卡尔曼滤波器。

(2-3)地标提取块的详细说明

图8示出了地标提取块22的功能配置。如图8所示，地标提取块22包括搜索候选选择块41、估计测量值计算块42、搜索范围缩小块43和提取块44。

基于先验估计值x^- _t，搜索候选选择框41识别待由激光雷达2扫描的范围(称为“扫描范围Rsc”)，并且从地图DB 10中选择位于所识别的扫描范围Rsc内的地标。在这种情况下，搜索候选选择块41将位于与位置(x^- _t,y^- _t)相距激光雷达2 的最大测距距离之内且位于方位θ^-t的正或负90度角的边界内的区域确定为扫描范围Rsc。然后，搜索候选选择块41从地图DB 10中提取与索引k对应的位置矢量m^k＝(m_k,x,m_k,y)。扫描范围Rsc是根据本发明的“第一范围”的示例。

估计测量值计算块42基于先验估计值x^- _t和从地图DB 10提取的具有索引k 的位置矢量m^k来计算对应于测量值z_t ^k的估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T。具体而言，估计测量值计算块42基于上述等式(5)和(7)计算估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T。

搜索范围缩小块43是如此确定扫描范围Rsc的范围(称为“搜索范围Rtag”) 的，即：扫描角度与角度φ^_t ^k之间的差在预定搜索角宽度“Δ_φ”之内，并且测距距离与距离r^_t ^k之间的差在搜索距离宽度“Δr”之内。考虑到测量值z_t ^k和估计值 z^_t ^k之间的假定误差范围，通过实验性尝试来分别预先确定搜索角宽度Δ_φ和搜索距离宽度Δ_r。上述的误差范围取决于计算估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T(参见等式(5) 和(7))所需的先验估计值x^- _t＝(x^- _t,y^- _t,θ^- _t)^T的估计精度以及输出对应于测量值 z_t ^k的扫描数据的激光雷达2的精度。因此，优选地，通过考虑先验估计值x^- _t的估计精度和激光雷达2的精度中的至少一个，来确定搜索角宽度Δ_φ和搜索距离宽度Δ_r。具体而言，先验估计值x^- _t的估计精度越高，则搜索角宽度Δ_φ和搜索距离宽度Δ_r越短。另外，激光雷达2的精度越高，搜索角宽度Δ_φ和搜索距离宽度Δ_r越短。

基于由搜索范围缩小块43确定的搜索范围Rtag，提取块44从激光雷达2的时刻t处的全部扫描数据中提取与参考标记Lk的点群对应的测量值z_t ^k。具体而言，提取块44确定是否存在满足以下等式(13)和(14)的测量值z_t ⁱ(“i”是由激光雷达2在一个扫描周期发射的每个光束的索引)。

[第十三等式]

forall i:

[第十四等式]

然后，当存在可满足式(13)和(14)的组合(r_t ⁱ,φ_t ⁱ)形式的测量值z_t ⁱ时，提取块44确定由搜索候选选择块41选择的地标实际上是存在的。然后，提取块44 将具有索引k的位置矢量m^k提供给测量模型块23，同时将满足等式(13)和(14) 的组合(r_t ⁱ,φ_t ⁱ)形式的测量值z_t ⁱ提供给操作块31作为与参考标记Lk的点群对应的测量值z_t ^k。

优选地，在这种情况下，为了确定要提供给操作块31的测量值z_t ^k，提取块 44可以进一步执行从包括在搜索范围Rtag中的扫描数据中选择对应于参考标记 Lk的扫描数据的处理。

例如，提取块44通过从地图DB 10获取由搜索候选选择块41选择的地标的形状信息，以及通过执行将该形状信息与搜索范围Rtag内的扫描数据的点群所构成的三维形状进行匹配的处理，来选择与参考标记Lk的点群对应的测量值z_t ^k。在另一示例中，提取块44通过指定与搜索范围Rtag内的扫描数据相对应的接收光的强度并且通过将该强度与预设阈值信息进行比较，来选择与参考标记Lk的点群对应的测量值z_t ^k。在选择了多个测量值z_t ^k的情况下，提取块44可以将多个选择的测量值z_t ^k中的任何一个提供给操作块31，或者将从多个选择的测量值z_t ^k中通过统计处理选出或计算出的代表值提供给操作块31。

图9示出了扫描范围Rsc和搜索范围Rtag之间的关系。在根据图9的示例中，扫描范围Rsc是半圆形区域，其位于距车辆的前方方向的右侧90度角或左侧90 度角的边界内并且处于距车辆的最大测距距离内。应当注意的是，图9中的箭头 70指示扫描方向。

在根据图9的例子中，搜索候选选择块41首先基于先验估计值x^- _t来指定扫描范围Rsc，并且将位于扫描范围Rsc中的地标指定为参考地标Lk以选择对应于参考地标Lk的位置矢量m^k。然后，估计测量值计算块42根据参考标记Lk的位置矢量m^k和先验估计值x^-t来计算与测量值z_t ^k相对应的估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T。搜索范围缩小块43基于等式(13)和(14)来确定搜索范围Rtag。在所确定的搜索范围Rtag内存在扫描数据的情况下，提取块44将位置矢量m^k提供给测量模型块23，同时将基于包括在搜索范围Rtag中的扫描数据的测量值z_t ^k提供给操作块31。注意，在确定的搜索范围Rtag内没有扫描数据的情况下，本车位置估计器17确定无法由激光雷达2检测到参考地标Lk，并且将先验估计值x^- _t设置为后验估计值x^_t，同时将先验协方差矩阵Σ^- _t设置为后验协方差矩阵Σ^_t。

(3)处理流程

(3-1)流程概述

图10是根据第一实施例由本车位置估计器17执行的处理过程的流程图。本车位置估计器17重复执行图10中的流程图。

首先，本车位置估计器17获取时刻t-1处的后验估计值x^_t-1和后验协方差矩阵Σ^_t-1，同时从陀螺仪传感器3和车速传感器4获取时刻t处的控制值u_t(步骤 S101)。接下来，本车位置估计器17的状态转换模型块20基于等式(2)计算先验估计值x^- _t(步骤S102)。然后，本车位置估计器17的协方差计算块21基于等式(3)计算先验协方差矩阵Σ^- _t(步骤S103)。

接着，参照地图DB 10，本车位置估计器17的地标提取块22执行将在后续描述的图11所示的地标提取处理(步骤S104)。然后，本车位置估计器17确定本车位置估计器17是否可以在地图DB 10上记录的地标的位置矢量m^k和激光雷达2的扫描数据之间建立对应关系(步骤S105)。在这种情况下，本车位置估计器17基于图11所示的地标提取处理来确定是否设置能表示成功建立上述对应关系的预定标旗。

然后，当上述对应关系建立时(步骤S105；是)，协方差计算块24基于上述等式(9)计算协方差矩阵S_t ^k(步骤S106)。接下来，卡尔曼增益计算块25 基于上述等式(10)计算卡尔曼增益K_t ^k(步骤S107)。然后，根据等式(12)，操作块33计算从地标提取块22提供的测量值z_t ^k和从测量模型块23提供的估计值z^_t ^k之间的差(即“z_t ^k-z^_t ^k”)，并且将此差值乘以卡尔曼增益K_t ^k，再与先验估计值x^- _t相加，从而计算后验估计值x^_t(步骤S108)。根据等式(11)，协方差更新块26基于从协方差计算块21提供的先验协方差矩阵Σ^- _t、从测量模型块 23提供的雅可比矩阵H_t ^k和从卡尔曼增益计算块25提供的卡尔曼增益K_t ^k来计算后验协方差矩阵Σ^_t(步骤S109)。然后，单位延迟块34将后验估计值x^_t设置为后验估计值x^_t-1并将其提供给状态转换模型块20，并且单位延迟块35将后验协方差矩阵Σ^_t设置为后验协方差矩阵Σ^_t-1并将其提供给协方差计算块21.

相反，在不能建立上述对应关系的情况下(步骤S105：否)，本车位置估计器17将先验估计值x^- _t设定为后验估计值x^_t，同时将先验协方差矩阵Σ^- _t设定为后验协方差矩阵Σ^_t(步骤S110)。

(3-2)地标提取处理

图11是表示图10的步骤S104中的地标提取处理的详细内容的流程图。

首先，地标提取块22的搜索范围缩小块43获取激光雷达2的扫描数据(步骤S201)。这里假定激光雷达2在一次扫描周期发射“n”个光束并同时改变光束的出射角，并且假定激光雷达2通过测量与每个光束对应的每个反射光的接收光强度和响应时间来输出测量值z_t ¹到z_t ⁿ。

接下来，搜索候选选择块41从地图DB 10中选择位于激光雷达2的扫描范围Rsc中的地标的位置矢量m^k(步骤S202)。在这种情况下，搜索候选选择块 41将位于相距方位θ^- _t的正或负90度角边界之内且位于与位置(x^- _t,y^- _t)相距激光雷达2的最大测距距离之内的区域指定为扫描范围Rsc。然后，搜索候选选择块41 从地图DB 10中提取能指示出指定区域中的位置的位置矢量m^k。

之后，在假定位置矢量m^k是从先验估计值x^- _t测得的情况下，估计测量值计算块42计算对应于测量值z_t ^k的估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T(步骤S203)。具体而言，估计测量值计算块42基于上述等式(5)和(7)计算估计值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T。

接下来，搜索范围缩小块43确定能满足等式(13)的扫描角φ_t ⁱ(i＝1至n) (步骤S204)。提取块44确定在多个测量值z_t ⁱ中是否存在任何扫描角φ_t ⁱ满足等式(13)且距离r_t ⁱ在等式(14)所示的范围之内的测量值z_t ⁱ(步骤S205)。当测量值z_t ⁱ中存在那一扫描角φ_t ⁱ满足等式(13)且距离r_t ⁱ在等式(14)所示的范围之内的测量值z_t ⁱ时(步骤S205；是)，提取块44进一步执行从位于搜索范围内的测量值z_t ⁱ中选择出对应于参考标记Lk的那一测量值的处理(例如，已知的形状/特征提取处理或通过使用接收光强度的处理)，并提取所选择的测量值作为测量值z_t ^k(步骤S206)。然后，提取块44输出位置矢量m^k和测量值z_t ^k(测量值z_t ^k是与位置矢量m^k对应的扫描数据)，并设置表示成功建立对应关系的标旗 (步骤S207)。在上述图10的步骤S105的判定处理时刻参照该标旗。

相反，当测量值z_t ⁱ中不存在那个扫描角φ_t ⁱ满足等式(13)且距离r_t ⁱ在等式 (14)所示的范围之内的测量值z_t ⁱ时(步骤S205；否)，提取块44不设置表示成功建立对应关系的标旗(步骤S208)。

[第二实施例]

图12是根据第二实施例的驾驶辅助系统的框图。在根据图12的示例中，车载器1电连接于方位传感器5。方位传感器5的示例包括地磁传感器、罗盘和GPS 罗盘。方位传感器5向车载器1提供与车辆的行驶方向对应的方位的信息。与第一实施例相同，车载器1具有如图2的配置，并且基于记录在地图DB 10上的地标信息以及来自激光雷达2、陀螺仪传感器3、车速传感器4和方位传感器5的各种输出数据来估计本车位置。在下文中，与第一实施例相同的部件将采用与第一实施例相同的附图标记来表示，并且将省略其说明。

图13示出了根据第二实施例的车载器1的本车位置估计器17A的示意性配置。在根据图13的示例中，本车位置估计器17A包括地标提取块22和位置估计块28。在下文中，由方位传感器5输出的在参考时刻t处的车辆方位由“θ_t”表示，并且由位置估计模块28输出的在参考时刻t处的本车位置的估计值由“x^- _t＝ (x^- _t,y^- _t)”表示。

地标提取块22包括搜索候选选择块41、估计测量值计算块42、搜索范围缩小块43和提取块44，并且输出参考标记Lk的位置矢量m^k和测量值z_t ^k，其中测量值z_t ^k是由激光雷达2输出的参考标记Lk的扫描数据。

具体地，根据第一实施例的搜索候选选择块41基于从状态转换模块20提供的先验估计值x^- _t来指定扫描范围Rsc。相反，根据第二实施例的搜索候选选择块 41首先基于图6所示的几何关系以与根据第一实施例的状态转换模块20相同的方式，来计算本车位置在x-y坐标系上的瞬时估计值。具体而言，参照等式(2)，搜索候选选择块41基于上一时刻由位置估计块28估计的估计值x^- _t-1、上一时刻测量的方位θ_t-1和控制值ut＝(v_t,ω_t)^T来计算本车位置在x-y坐标系上的瞬时估计值。然后，搜索候选选择块41基于本车位置在x-y坐标系上的瞬时估计值和方位θ_t来指定扫描范围Rsc。具体而言，搜索候选选择块41确定出位于方位θ_t正或负 90度角边界之内且位于与计算出的本车位置的x-y坐标相距激光雷达2的最大测距距离之内的区域。然后，搜索候选选择块41从地图DB 10提取能指示出指定区域内的位置的地标位置矢量m^k。

参考等式(5)和(7)，估计测量值计算块42基于由搜索候选选择块41计算出的本车位置在x-y坐标系上的瞬时估计值(相当于第一实施例的先验估计值 x^- _t)和从地图DB 10提取的位置矢量m^k，来计算对应于测量值z_t ^k的估计值z^_t ^k＝ (r^_t ^k,φ^_t ^k)^T。注意，在参考等式(5)和等式(7)时，估计测量值计算块42使用由搜索候选选择块42计算的本车位置在x-y坐标系上的瞬时估计值而不是先验估计值x^- _t＝(x^- _t,y^- _t)，并使用方位θ_t而不是方位θ^- _t。然后，搜索范围缩小块43确定由等式(13)和等式(14)表示的搜索范围Rtag。当存在具有能满足等式(13) 和等式(14)的组合(r_t ⁱ,φ_t ⁱ)的测量值z^t _i(此处为z^t _k)时，提取块44将测量值及其位置矢量m^k提供给位置估计块28。优选地，在这种情况下，与第一实施例相同，提取块44还可以执行从包括在搜索范围Rtag中的扫描数据中选择与参考标记Lk对应的扫描数据的处理，以确定要提供的测量值z^t _k。

位置估计块28基于从提取块44提供的位置矢量m^k和测量值z^_t ^k＝(r^_t ^k,φ^_t ^k)^T以及由方位传感器5输出的方位θ_t来计算本车位置的估计值x^- _t＝(x^- _t,y^- _t)。具体而言，在绝对值|θ_t+φ_t ^k|小于90度的时刻处，位置估计块28基于以下等式(15)和 (16)来计算估计值x^- _t＝(x^- _t,y^- _t)。

[第十五等式]

[第十六等式]

相反，当绝对值|θ_t+φ_t ^k|大于90度的时刻，位置估计块28基于以下等式(17) 和上述等式(16)来计算估计值x^- _t＝(x^- _t,y^- _t)。

[第十七等式]

此外，当在绝对值|θ_t+φ_t ^k|等于90度的时刻，位置估计块28基于以下等式(18) 和上述等式(16)来计算估计值x^- _t＝(x^- _t,y^- _t)。

[第十八等式]

这里，将给出关于等式(15)至(18)的推导方法的描述。

首先，由于距离r_t ^k和扫描角φ_t ^k具有上述图7中所示的与距离r^_t ^k和扫描角φ^_t ^k一样的几何关系，所以如上述等式(5)和(6)所示，距离r_t ^k和扫描角φ_t ^k都与的估计值x^- _t＝(x^- _t,y^- _t)和方位θ^- _t有关系。以下等式(19)对应于等式(6)的变形，以下等式(20)对应于等式(5)的变形。

[第十九等式]

[第二十等式]

然后，通过将等式(19)代入等式(20)中，可以获得下面的等式(21)。

[第二十一等式]

由于等式(21)包括符号“±”，因此有必要认为等式(21)取决于估计值 x^- _t的x坐标值“x^- _t”与位置矢量m^k的x坐标值“m_k，x”之间的大小关系。图14 (A)至图14(C)示出了估计值x^- _t的x坐标值“x^- _t”与位置矢量m^k的x坐标值“m_k，x”之间的大小关系。具体地，图14A示出了在位置矢量m^k的x坐标值“m_k，x”大于估计值x^- _t的x坐标值“x^- _t”的情况下的大小关系。图14A示出了位置矢量m^k的x坐标值“m_k，x”大于估计值x^- _t的x坐标值“x^- _t”的情况。图14B示出了位置矢量m^k的x坐标值“m_k，x”等于估计值x^- _t的x坐标值“x^- _t”的情况。图 14C示出了位置矢量m^k的x坐标值“m_k，x”小于估计值x^- _t的x坐标值“x^- _t”的情况。

如图14A所示，“θ_t+φ_t ^k”等于或小于90度角，则满足不等式“x^- _t<m_k，x”。相反，如图14B所示，“θ_t+φ_t ^k”等于90度角，则满足等式“x^- _t＝m_k，x”。此外，如图14C所示，“θ_t+φ_t ^k”大于90度角，则满足不等式“x^- _t>m_k，x”。

因此，提供以下关系，其中包括“θ_t+φ_t ^k”为负的情况。

(a)|θ_t+φ_t ^k|<90°→x^- _t<m_k，x

(b)|θ_t+φ_t ^k|＝90°→x^- _t＝m_k，x

(c)|θ_t+φ_t ^k|>90°→x^- _t>m_k，x

在上述(a)的情况下，由于等式(21)的左边为正，所以等式右边的符号也是“+”，由此得出上述等式(15)。在上述(b)的情况下，满足“x^- _t＝m_k，x”的关系。在上述(c)的情况下，由于等式(21)的左边为负，所以等式右边的符号也是“-”，由此得出上述等式(17)。注意，通过变换等式(19)而将估计值 x^- _t的y坐标值“y^- _t”表示为等式(16)。

如上所述，根据第二实施例，通过使用从方位传感器5获取的方位θ_t，本车位置估计器17A能够从记录在地图DB 10上的地标的位置矢量m^k和测量值z^_t ^k＝ (r^_t ^k,φ^_t ^k)^T适当地计算出本车位置的估计值x^- _t＝(x^- _t,y^- _t)。

在本车位置估计器17A不能建立在地图DB 10上记录的地标的位置矢量m^k和激光雷达2的扫描数据之间的对应关系的情况下，本车位置估计器17A将由搜索候选选择块41计算出的本车位置的x-y坐标的瞬时估计值设置为本车位置的估计值x^- _t＝(x^- _t,y^- _t)。即，在这种情况下，本车位置的估计值x^- _t＝(x^- _t,y^- _t)是基于由位置估计块28在上一时刻估计的本车位置的估计值x^- _t-1＝(x^- _t-1,y^- _t-1)、由车速传感器 4和陀螺仪传感器3在参考时刻t处测量的控制值u_t和在上一时刻测量的方位θ_t-1计算出的估计值。

<变型例>

在下文中，将给出对第一和第二实施例的优选变型的描述。

(第一变型例)

根据第一和第二实施例的搜索候选选择框41可以从地图DB 10中提取位于扫描范围Rsc中的多个地标的位置矢量，从而与激光雷达2的扫描数据进行交叉检查。

通常，在车辆行驶时，由于在本车辆的前方或侧面存在诸如其它车辆的障碍物，所以会出现这种情况(即，堵塞)：无法从激光雷达2获得从地图DB 10选择的参考地标Lk的扫描数据。在上述考虑事项中，根据该变型例，搜索候选选择块41从地图DB 10中提取位于扫描范围Rsc中的多个地标的位置矢量。相应地，即使在任何一个地标发生遮挡时，搜索候选选择块41都能够基于其他没有遮挡的地标来执行本车位置估计。因此，能够高概率地探测用于本车位置估计所需的参考标记Lk。注意，在能够提取到多个地标并且其全部被使用的情况下，可以多次执行测量/更新步骤(即，可以通过使用多个地标来校正先验估计值)。在这种情况下，能够以统计学方式来增加本车位置的估计精度。

将参照图11补充说明上述变型。在步骤S202中，当地标提取块22参考地图DB 10确定了存在多个位于扫描范围Rsc中的地标时，地标提取块22从地图 DB 10中选择多于一个地标的位置矢量。然后，在步骤S203中，测量模型块23 计算所选位置矢量的估计测量值。然后，在步骤S204和步骤S205中，地标提取块22针对每个计算出的估计测量值，根据等式(13)和(14)来确定每个搜索范围Rtag，并试图在每个搜索范围Rtag中提取与每个所选地标相匹配的扫描数据。当成功执行提取处理时，地标提取块22在步骤S207中输出与匹配的扫描数据对应的测量值和位置矢量。

将参照图10补充描述当能够提取到多个地标并且其全部被使用时的处理。在这种情况下，在步骤S104中，在地标提取处理中提取多个地标，并且在步骤 S105中作出正面判定“是”。将针对提取到的每一个地标，执行与步骤S106至 S109对应的以下一系列处理，并且基于与步骤S207中关于每个地标的输出相对应的数据，以提取到的地标数量为次数地重复执行上述一系列处理。

(第二变型例)

根据第二实施例的本车位置估计器17A能够基于陀螺仪传感器3的输出来估计方位θ_t，而不是基于方位传感器5的输出来指定方位θ_t。

在这种情况下，以与图6和等式(2)类似的方式，本车位置估计器17A通过将值(ω_tΔt)相加到上一时刻计算的方位θ_t-1来计算参考时刻处的方位θ_t的估计值，其中本车位置估计器17A通过将角速度ω乘以时刻t-1和时刻t之间的持续时间Δt来计算值(ω_tΔt)。即使根据该示例，也可以与第二实施例一样，在不使用例如扩展卡尔曼滤波器的任何贝叶斯估计的情况下适当地计算本车位置的估计值x^- _t。

(第三变型例)

根据以上说明，通过激光雷达2获得的具有索引k的地标的测量值z_t ^k是这样一种矢量值：当假定车辆的前行方向为扫描角φ_t ^k的0度角的情况下，该矢量值的元素为具有索引k的地标的距离“r_t ^k”和扫描角“φ_t ^k”。然而，存在以三维空间方式输出坐标值的激光雷达产品，其中将相距于对象物的距离和角度转换到三维空间中。例如，扫描二维空间的激光雷达以笛卡尔坐标的形式(例如x^- _t ^k＝r_t ^k cosφ_t ^k，y_t ^k＝r_t ^ksinφ_t ^k)来输出数据，其中笛卡尔坐标中使用了距离r_t ^k和角度φ_t ^k。例如，根据本发明的上述方法也可以应用于那些在将坐标值x_t ^k和y_t ^k简单地转换为距离r_t ^k和角度φ_t ^k之后输出这种形式的数据的激光雷达产品。

(变型例4)

替代车载器1的存储单元12存储地图DB 10的配置，服务器装置可以存储地图DB10。在这种情况下，车载器1通过通信单元与服务器装置进行通信来获取必要的地标信息。

附图标记

1 车载器

2 激光雷达

3 陀螺仪传感器

4 车速传感器

5 方位传感器

10 地图DB

Claims (9)

1.一种估计装置，包括：

获取单元，其配置为获取地图信息；

第一获取单元，其配置为获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；

第一估计单元，其配置为基于包括在所述地图信息中的所述对象物的位置信息和所述第一信息来估计移动体的位置；以及

第二估计单元，其配置为计算第一估计位置，所述第一估计位置是所述移动体的当前位置的估计位置，

其中，所述第一估计单元基于所述第一估计位置以及所述第一信息与第二信息之间的差来估计所述移动体的位置，所述第二信息指示所述对象物的位置信息所示的位置与所述第一估计位置之间的位置关系。

2.根据权利要求1所述的估计装置，其中，

所述第二估计单元至少基于在预定时刻之前的所述移动体的估计位置来计算所述第一估计位置。

3.根据权利要求2所述的估计装置，其中，还包括：

第二获取单元，其配置为获取所述移动体的控制信息，

其中，所述第二估计单元基于在预定时刻之前的估计位置和所述移动体的控制信息来计算所述第一估计位置。

4.根据权利要求2或3所述的估计装置，其中，

交替执行预测步骤和更新步骤，其中在所述预测步骤中所述第二估计单元计算所述第一估计位置，在所述更新步骤中所述第一估计单元基于所述第一信息和所述第二信息之间的差来校正上一次预测步骤中计算出的第一估计位置，并且

其中，在所述预测步骤中，所述第二估计单元基于在所述预测步骤之前的上一次更新步骤中校正的第一估计位置来计算与当前时刻对应的所述第一估计位置。

5.根据权利要求1至3中任一项所述的估计装置，其中，

所述第一获取单元从测量装置获取所述第一信息，所述测量装置包括发射单元、光接收单元和输出单元，所述发射单元发射激光并同时改变所述激光的出射方向，所述光接收单元接收由所述对象物反射的所述激光，所述输出单元基于由所述光接收单元输出的光接收信号、与由所述光接收单元接收的所述激光对应的所述出射方向以及所述激光的响应延迟时间来输出所述第一信息。

6.根据权利要求1至3中任一项所述的估计装置，其中，

所述对象物是人造物体。

7.根据权利要求6所述的估计装置，其中，

所述对象物是周期性设置的人造物体。

8.一种由估计装置执行的控制方法，包括：

获取处理，以便获取地图信息；

第一获取处理，以便获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；

第一估计处理，以便基于包括在所述地图信息中的所述对象物的位置信息和所述第一信息来估计移动体的位置；以及

第二估计处理，以便计算第一估计位置，所述第一估计位置是所述移动体的当前位置的估计位置，

其中，在所述第一估计处理中，基于所述第一估计位置以及所述第一信息与第二信息之间的差来估计所述移动体的位置，所述第二信息指示所述对象物的位置信息所示的位置与所述第一估计位置之间的位置关系。

9.一种用于存储由计算机执行的程序的存储媒介，所述程序使计算机用作：

获取单元，其配置为获取地图信息；

第一获取单元，其配置为获取指示相距于位于第一范围内的对象物的距离和角度的第一信息；

第一估计单元，其配置为基于包括在所述地图信息中的所述对象物的位置信息和所述第一信息来估计移动体的位置；以及

第二估计单元，其配置为计算第一估计位置，所述第一估计位置是所述移动体的当前位置的估计位置，

其中，所述第一估计单元基于所述第一估计位置以及所述第一信息与第二信息之间的差来估计所述移动体的位置，所述第二信息指示所述对象物的位置信息所示的位置与所述第一估计位置之间的位置关系。

Images