车辆定位装置的制作方法

1.本发明涉及搭载于车辆，对车辆的位置进行定位的车辆定位装置，特别涉及不易受到来自卫星的电波状态的影响的车辆定位装置。


背景技术：

2.以往，提出了使用基于gnss(global navigation satellite system，全球导航卫星系统)的卫星定位结果，针对惯性定位的定位结果校正误差被偏置的惯性传感器的输出，提高车辆的定位精度的车辆定位装置。这样的技术通过在卫星可补充时间段对惯性传感器的偏置误差进行定位，能够提高惯性定位的精度，当无法捕捉卫星例如在屋内、隧道内存在车辆的情况下也能够维持定位精度。例如，在专利文献1的段落[0007]中，公开了其目的在于除了卫星定位之外，还使用屋内用的室内gps(global positioning system，全球定位系统)，不论是在屋内还是在屋外，都能够得到无缝且精度高的定位技术。
[0003]
现有技术文献
[0004]
专利文献
[0005]
专利文献1：日本特开2016
‑
170124号公报


技术实现要素：

[0006]
在以往的车辆定位技术中，在无法捕捉卫星的屋内也能够继续定位，但另一方面，存在即使补充卫星，在卫星定位信号显著劣化的情形下，也无法维持定位精度这样的课题。作为这样的情形，可举出来自卫星的电波在大厦等构造物反射，容易产生通过多个路径到达gnss天线的多路径的城区以及由于树木而来自卫星的电波强度下降的山区等中的定位。
7.本发明是为了解决如上所述的问题而完成的，其目的在于提供维持定位精度，并将能够继续定位的状况扩大的车辆定位装置。
[0008]
本发明提供一种车辆定位装置，搭载于车辆，对所述车辆的位置进行定位，其中，所述车辆定位装置连接于输出卫星定位数据的第1传感器以及检测所述车辆的状态量并作为状态量数据而输出的第2传感器，并且连接于检测地面物体并输出所述地面物体与所述车辆的相对关系的数据的第3传感器以及检测道路线形并输出道路线形数据的第4传感器中的至少一方，所述车辆定位装置具备：观测值处理部，接受所述卫星定位数据、所述相对关系的数据以及所述道路线形数据中的至少一方，将这些数据合并而生成实际观测值；传感器校正部，连接于所述第2传感器，校正所述状态量数据中包含的传感器误差；惯性定位部，使用由所述传感器校正部校正后的校正后传感器值进行惯性定位，并作为惯性定位结果而输出；观测值预测部，至少使用所述惯性定位结果预测观测值，并作为预测观测值而输出；滤波器，使用所述预测观测值以及所述实际观测值进行定位运算，并作为定位结果而输出，并且推测所述传感器误差，并作为传感器校正量而输出，所述滤波器将所述传感器校正量反馈给所述传感器校正部，所述传感器校正部使用所述传感器校正量校正所述传感器误差。
[0009]
根据本发明的车辆定位装置，在容易产生多路径的状况下，也能够维持定位精度，能够将能够继续定位的状况扩大。
附图说明
[0010]
图1是示出本发明的实施方式1的车辆定位装置的结构的功能框图。
[0011]
图2是说明多路径环境下的定位的图。
[0012]
图3是说明数字地图的静态信息的图。
[0013]
图4是说明车辆导航坐标系的图。
[0014]
图5是说明本发明的实施方式1的车辆定位装置的整体的处理流程的流程图。
[0015]
图6是说明与地面物体的相对位置关系的图。
[0016]
图7是示出本发明的实施方式2的车辆定位装置的结构的功能框图。
[0017]
图8是说明本发明的实施方式2的车辆定位装置的整体的处理流程的流程图。
[0018]
图9是说明道路线形的图。
[0019]
图10是说明道路线形的计算处理的流程图。
[0020]
图11是说明道路线形的概念图。
[0021]
图12是示出本发明的实施方式3的车辆定位装置的结构的功能框图。
[0022]
图13是示出实现本发明的实施方式1至3的车辆定位装置的硬件结构的图。
[0023]
图14是示出实现本发明的实施方式1至3的车辆定位装置的硬件结构的图。
具体实施方式
[0024]
<首先>
[0025]
在以下说明的实施方式中，地面物体包括道路标识、信号灯、电柱等，道路信息包括与道路有关的节点数据、行车线宽度、坡度数据等，自主传感器是指检测搭载实施方式的车辆定位装置的车辆自身的状态量的传感器，包括车速计、加速度计、角速度计等，外界传感器是指检测与存在搭载实施方式的车辆定位装置的车辆的环境有关的信息的传感器，包括激光测距仪、摄像机、雷达，道路线形是指道路的形状，在道路线形数据中，包含是怎样的直线与曲线的组合、坡度是何种程度等的信息。
[0026]
<实施方式1>
[0027]
<装置结构：概略>
[0028]
图1是示出本发明的实施方式1的车辆定位装置20的结构的功能框图。如图1所示，gnss传感器1(第1传感器)以及地面物体检测部3(第3传感器)作为外界传感器而连接于车辆定位装置20，检测车辆周边的外界信息。另外，地面物体信息存储部4以及自主传感器6(第2传感器)连接于车辆定位装置20。
[0029]
车辆定位装置20具备观测值处理部12、滤波器13、传感器校正部14、惯性定位部15以及观测值预测部16。
[0030]
观测值处理部12具有如下功能：接受从gnss传感器1输出的卫星观测数据以及从地面物体检测部3输出的与地面物体的相对关系的数据，运算为了进行定位运算以及自主传感器6输出的状态量数据的校正量的推测而所需的实际观测值，输出到滤波器13。
[0031]
传感器校正部14具有如下功能：校正自主传感器6的标度因子(scale factor)误
差以及偏置误差，并作为校正后传感器值而输出到惯性定位部15。
[0032]
惯性定位部15具有如下功能：使用从传感器校正部14输入的校正后传感器值，进行作为车辆的定位结果的位置、姿势、速度等的惯性定位运算，将其结果作为惯性定位结果而输出到观测值预测部16。
[0033]
观测值预测部16具有如下功能：使用从惯性定位部15输入的惯性定位结果以及从地面物体信息存储部4输入的地面物体信息，运算为了进行定位运算以及自主传感器输出的状态量数据的校正量的推测而所需的预测观测值，输出到滤波器13。
[0034]
滤波器13具有如下功能：使用从观测值处理部12输入的实际观测值以及由观测值预测部16预测出的预测观测值，推测定位运算以及自主传感器6的传感器误差。由滤波器13推测出的传感器误差作为传感器校正量而反馈给传感器校正部14。另外，滤波器13中的定位运算的结果作为定位结果而成为车辆定位装置20的输出。此外，在无法从外界传感器得到数据的情况下，滤波器13将从惯性定位部15输出的惯性定位结果作为车辆定位装置20的输出。
[0035]
另外，在滤波器13中，在从外界传感器得到数据的定时推测传感器校正量，但该值被保持，所以在无法从外界传感器得到数据的情况下，也能够进行传感器校正，维持定位精度。
[0036]
根据具有以上说明的结构的车辆定位装置20，在从gnss传感器1以及地面物体检测部3中的任意方得到数据的情况下，能够进行定位运算和传感器校正量的运算，能够维持定位精度。进而，在从gnss传感器1以及地面物体检测部3这两方得到数据的情况下，能够输出可靠性更高的定位结果。
[0037]
另外，利用滤波器13将使用来自gnss传感器1以及地面物体检测部3的数据而得到的实际观测值与使用来自自主传感器6的数据而得到的预测观测值进行融合，所以不需要如选择实际观测值以及预测观测值的一方那样的切换作业。因此，能够抑制如在切换时定位结果成为不连续那样的定位跳跃等的不好的现象。
[0038]
另外，通过使用多个外界传感器，能够进行如下处理，即，在任意的外界传感器输出了异常值的情况下，在概率上判定出异常值，不将异常值用于定位运算。其结果，能够维持定位运算的可靠性。
[0039]
另外，一般而言，外界传感器的传感器值的更新周期是低的周期，一般为10hz左右，所以在要求高的周期下的定位的情况下，仅通过外界传感器无法满足要求。另一方面，自主传感器是以高的周期，一般以100hz左右得到传感器值，能够按照该周期执行惯性定位。因此，在车辆定位装置20中，与仅使用外界传感器中的传感器值的情况相比，还能够得到能够以高的周期获取定位结果这样的效果。
[0040]
图2是示意地示出在能够捕捉卫星的情况下卫星定位精度下降的例子的图。如图2所示，在城区及其近郊，在多个大厦bd林立的场所，有时同时接收来自卫星st的电波在大厦bd等构造物反射而到达车辆ov的反射电波wr和直接到达车辆ov的直达电波ws。当在这样的状况下进行卫星定位时，一般而言，产生定位精度显著下降，无法维持卫星定位的定位精度这样的问题。在作为现有技术而说明的日本特开2016
‑
170124号公报所公开的技术中，难以解决该问题。
[0041]
另一方面，根据车辆定位装置20，在如图2那样的状况下，也通过使用与地面物体
的相对关系的数据来能够维持定位精度。即，不仅使用gnss传感器1，还将区划线cl以及地面物体fe等车辆ov能够观测的信息进行合并，能够将能够定位的状况扩大。
[0042]
作为使用了地面物体信息的定位装置的例子，可举出日本特开2005
‑
265494号公报所公开的技术。在上述文献中提出的车辆位置推测装置中，由车载摄像机检测地面物体以及白线，进行定位。然而，在检测不到地面物体以及白线的时间段，仅使用车速计来推测位置，所以无法应对曲线形状的道路线形，另外根据地面物体以及白线的检测而推测不出传感器误差，所以车速计具有误差的情况下，传感器的误差被累积，在结果上招致定位精度的下降。
[0043]
另一方面，在车辆定位装置20中，在能够观测基于外界传感器的外界信息的时间段推测传感器误差，在无法观测外界信息的时间段，利用在能够观测外界信息的时间段推测出的传感器误差，从而能够维持惯性定位的精度。
[0044]
近年来，在各国进行发布用于车辆的自动驾驶的高精细且静态以及动态的信息的数字地图的整理，在日本称为动态地图。作为其参考文献，可举出自动行驶中的动态地图整理(系统/控制/信息，vol.60,no.11pp.463－468,2016)。
[0045]
在上述参考文献中，说明了关于动态地图发布怎样的信息的概念。在图3中，示出了作为上述参考文献所记载的动态地图中的静态信息而发布的地面物体的信息的一个例子。在图3中，作为静态信息，编入有道口、道路标识、护栏、电柱、信号灯等在道路上实际存在的实际存在地面物体的绝对位置信息以及道路中央坐标等地图信息。另外，作为根据实际存在地面物体制作的虚拟地面物体，编入有车道链路、行车线链路、行车线链路上的节点、行车线区域等信息。
[0046]
此外，车道意味着其边界为墙壁、护栏、路缘石等在物理上被限制的边界线和隧道、洞门等存在高度限制的地面物体。另外，行车线的边界线被通行带的区划线(白线等)限制。
[0047]
在车辆定位装置20中，作为地面物体信息存储部4的数据库，能够应用动态地图。
[0048]
关于包含动态地图的数字地图的发布，在世界各国推进研究，在国际标准化机构(iso)等中推进标准化，所以在世界各国标准性地被利用。
[0049]
<装置结构：详细内容>
[0050]
详细地说明车辆定位装置20各自的功能块。
[0051]
<gnss传感器>
[0052]
gnss天线1a连接于gnss传感器1。gnss传感器1利用gnss天线1接收来自在卫星轨道上转圈的定位卫星的定位信号，对接收到的定位信号进行信号处理，从而能够获取来自卫星的各种观测数据。
[0053]
在gnss传感器1中具有如下功能的gnss传感器，该功能为：除了根据输出的设定在gnss传感器1内进行定位运算而得到的定位运算结果之外，还将进行定位运算之前的gnss的观测数据作为定位原始数据而输出。在定位原始数据中包含模拟距离观测值、多普勒观测值、载波相位观测值，关于这些观测值，能够针对卫星发布的每个频域(例如l1频段、l2频段、l5频段等)得到。
[0054]
作为定位卫星，除了有美国的gps之外，还有俄罗斯的glonass(global navigation satellite system，全球导航卫星系统)、欧州的galileo、日本的qzss
(quasi－zenith satellite system，准天顶卫星系统)，中国的北斗(beidou)、印度的navic(navigation indian constellation，导航印度星座)等，本发明的实施方式1～3的车辆定位装置能够应用于这些的全部。
[0055]
gnss传感器1中的定位运算能够进行单独定位、dgps(differential gps，差分gps)定位、rtk(real time kinematic，实时动态)定位、网络型rtk定位等定位方式中的任意定位方式。另外，通常，gnss传感器1能够输出定位运算结果的可靠度信息。
[0056]
单独定位是指使用从4个以上的定位卫星接收到的模拟距离观测值来进行定位的卫星定位方式的一种。
[0057]
dgps定位是指通过使用能够从静止卫星型卫星导航增强系统(sbas：satellite－based augmentation system)、电子基准点以及私有的固定站生成的卫星定位误差增强数据来进行定位运算从而能够得到精度比单独定位高的卫星定位结果的定位方式。
[0058]
rtk定位是指将电子基准点以及私有的固定站的卫星原始数据转送到移动站，去除基站附近的卫星定位误差主要原因来能够进行高精度的卫星定位的定位方式。在rtk定位中，在用被称为模糊度的整数变量高的可靠度求出的情况下，能够以cm级的精度进行定位。此时的定位解被称为fix(固定)解，在未求出模糊度的情况下，输出float(浮动)解。
[0059]
网络型rtk定位是指使用网络获取与基站设置相当的卫星定位数据，进行高精度定位的定位方式。
[0060]
在gnss传感器1中，除了使用纬度、经度、高度等绝对位置信息之外，还使用多普勒观测值和卫星导航数据，能够运算gnss天线1a相对于地球的3轴方向例如北、东、铅垂方向的绝对速度。能够使用该绝对速度信息，检测gnss天线1a移动的方向即方位。此外，能够应用于本发明的实施方式1～3的车辆定位装置的卫星定位方式除了包括上述方式以外，还包括所有的卫星定位方式。
[0061]
<地面物体检测部>
[0062]
地面物体检测部3具有如下功能：使用安装于车辆的周边识别摄像机以及还被称为激光测距仪的lidar(light detection and ranging，光探测和测距)、雷达等检测地面物体，作为地面物体检测结果而输出存在于车辆周边的地面物体与车辆的相对关系以及地面物体的显示内容。
[0063]
地面物体与车辆的相对关系是指，表示在车辆导航坐标系中从车辆的导航中心(例如，后轴中央)起的与和地图数据库的坐标信息取得对应的地面物体代表点(点)的相对的距离关系以及相对速度关系等。
[0064]
车辆导航坐标系是指，例如，如图4所示，使用如下坐标系的情形较多：以车辆ov的导航中心，在此以后轴中央为原点，以车辆的前方向为xb轴，以左方向为yb轴，以与xb轴、yb轴成为右旋的法则的关系的方式取zb轴。
[0065]
进而，地面物体检测部3参照保持有与地面物体有关的地面物体信息的地面物体信息存储部4的地面物体数据库，与地面物体检测部3的检测结果进行组合，从而能够输出与和地面物体与车辆的相对关系关联起来的地面物体有关的信息。
[0066]
与地面物体有关的地面物体信息是指电柱、道路标识、护栏等地面物体类别、绝对位置坐标、显示内容。即，地面物体检测部3能够同时输出直至在车辆坐标系中定义的地面物体为止的距离、速度、地面物体的类别、绝对位置坐标、显示内容等。
[0067]
作为外界传感器的gnss传感器1以及地面物体检测部3各自的检测对象不同，所以各自能够检测的状况不同。例如，在gnss传感器1中在隧道等无法补充卫星的状况下无法得到观测值。另外，关于地面物体检测部3，在没有地面物体的区间，无法输出检测结果。
[0068]
<观测值处理部>
[0069]
观测值处理部12具有如下功能：将从gnss传感器1得到的观测值与从地面物体检测部3得到的地面物体与车辆的相对关系合并地处理，作为实际观测值而传送到滤波器13。
[0070]
<传感器校正部>
[0071]
传感器校正部14具有使用由滤波器13推测出的传感器校正量来校正从自主传感器6得到的传感器数据的功能。
[0072]
<自主传感器>
[0073]
自主传感器6包括测量车辆的车速的车速计、测量车辆的加速度以及角速度的惯性测量装置(imu：inertial measurement unit)、测量车辆的转向角度的转向角计等用于对车辆的位置、速度、姿势进行定位的传感器。
[0074]
车速计安装于车辆所具有的车轮，具有使用来自检测车轮的转速的脉冲传感器的输出变换为车辆的车速的功能。
[0075]
imu设置于车辆的屋顶或室内，具有检测车辆坐标系中的加速度、角速度的功能。在imu中编入有mems(micro electric mechanical system，微机电系统)以及光纤陀螺仪(fiber optic gyroscope)等的装置等被销售。
[0076]
<惯性定位部>
[0077]
惯性定位部15具有如下功能：使用由传感器校正部14校正后的自主传感器6的传感器值和车辆的运动模型，根据车辆的车速、加速度、角速度传感器的累计值对车辆的位置以及速度、姿势进行定位。
[0078]
在惯性定位部中，根据车辆的运动模型对从自主传感器6时时刻刻获取的传感器值进行累计，从而进行定位。通常，在自主传感器6的传感器值中包含标度因子误差以及偏置误差等，这时时刻刻被累计，所以存在惯性定位的精度随着时间而劣化这样的问题。
[0079]
另一方面，在本发明的实施方式1～3的车辆定位装置中，利用滤波器推测该传感器误差，作为传感器校正量而校正自主传感器6的传感器值，所以能够提高惯性定位的精度。
[0080]
<观测值预测部>
[0081]
观测值预测部16具有如下功能：针对由观测值处理部12运算处理后的实际观测值，使用惯性定位结果以及车辆周边的地面物体信息来运算预测观测值，输出到滤波器13。
[0082]
<滤波器>
[0083]
滤波器13具有如下功能：通过使用从观测值处理部12得到的实际观测值和从观测值预测部16得到的预测观测值，推测在概率上最可能的状态量，从而推测位置、速度、姿势等的定位运算、进而自主传感器6所具有的标度因子误差、偏置误差等传感器误差。
[0084]
在此，状态量是指车辆的3维方向的位置、速度、姿势以及自主传感器6的传感器误差等。由滤波器13推测出的定位运算结果作为定位结果而从车辆定位装置20输出，传感器误差作为传感器校正量而输入到传感器校正部14。
[0085]
<动作>
[0086]
接下来，使用图5所示的流程图来说明车辆定位装置20的整体的处理流程。当车辆定位装置20开始定位时，首先，获取惯性定位的初始值以及在观测值预测部16中利用的当前的惯性定位结果(步骤s100)。
[0087]
此外，在车辆定位装置20的电源刚刚接通之后等无法获取当前的惯性定位结果的情况下，能够从gnss传感器1获取估算的定位结果而利用，或者作为惯性定位的初始值而使用预先决定的值。
[0088]
接下来，车辆定位装置20判定从gnss传感器1、地面物体检测部3等外界传感器是否得到数据(步骤s101)。外界传感器各自的检测对象不同，但在得到传感器值的传感器有1个的情况(是的情况)下，进入到步骤s102，在不是的情况(否的情况)下，进入到步骤s131。
[0089]
车辆定位装置20在无法得到来自外界传感器的数据的情况下，在步骤s131中，将在之后说明的步骤s113的惯性定位运算中求出的惯性定位结果作为车辆定位装置20的定位结果而输出。
[0090]
在步骤s102中，观测值处理部12将由外界传感器得到的传感器值以在接下来的步骤s103中滤波器13能够利用的方式进行处理，并作为实际观测值而输出。
[0091]
在此，说明针对由观测值处理部12中的gnss传感器1以及地面物体检测部3各自得到的观测值的处理。
[0092]
<针对gnss传感器中的观测值的处理>
[0093]
在本实施方式1中，gnss传感器1输出gnss天线1a的相位中心处的纬度、经度、高度以及方位的坐标信息以及各自的可靠度。通常，从gnss传感器1按照在由nmea(national marine electronics association，国家海洋电子协会)定义的协议发送，但输出规格根据制造商而不同，所以在观测值处理部12中，对于由gnss传感器1得到的传感器值，关于纬度、经度、方位变换为deg或者rad等单位系，关于高度变换为m(米)等单位系。
[0094]
<针对地面物体检测部中的观测值的处理>
[0095]
在本实施方式1中，从地面物体检测部3得到地面物体的代表点的绝对位置坐标例如纬度、经度、高度信息以及车辆导航中心与地面物体代表点的相对位置关系。表达相对位置关系的方法各种各样，但在此，如图6所示，设为从车辆ov的导航中心至地面物体代表点为止的相对距离以及相对方位。
[0096]
图6的xyz表达绝对坐标系，在该情况下，以地球的重心为原点，表示作为固定于地球的坐标系的ecef坐标系(earth centered earth fixed(地心地固)坐标)。图6的坐标(xi，yi，zi)以及坐标(xb，yb，zb)分别表示地面物体fe的绝对位置坐标以及车辆ov的导航中心的绝对位置坐标。此外，zi未图示。另外，图6的轴n、轴e、轴d表示以车辆ov的导航中心为原点而在北方、东方、铅垂方向上取坐标轴的坐标系，被称为ned坐标系。此外，图的ψ表示以北为基准而将顺时针方向设为正的车辆方位。
[0097]
在地面物体检测部3中的输出相对于地面物体fe是坐标(xi，yi，zi)的情况下，相对距离ρ
m,i
和相对方位θ
m,i
分别通过以下的数式(1)以及(2)求出。
[0098]
[数学式1]
[0099][0100]
[数学式2]
[0101][0102]
这些信息与作为地面物体代表点的坐标的坐标(xi，yi，zi)一起输出。此外，在此仅说明了相对距离和相对方位，但作为地面物体检测部3的输出，能够输出相对速度信息，所以也可以将相对速度信息用作观测值。另外，本次仅使用了相对方位，但相对俯仰角(pitch angle)以及相对倾斜角(roll angle)等也根据坐标(xi，yi，zi)求出，所以还能够将其作为观测值而追加。在该情况下，还能够进行车辆的姿势角等的定位，所以能够进行更高级的定位。在此，俯仰角以及倾斜角分别是与图4所示的绕yb轴以及绕xb轴的角度有关的名称。
[0103]
在此，返回到图5的流程图的说明，说明步骤s111以下的流程。
[0104]
车辆定位装置20与步骤s101的判定并行地从自主传感器6获取传感器值(步骤s111)。即，从imu获取加速度、角速度，从车速计获取车速信息等。
[0105]
<自主传感器的传感器值的校正>
[0106]
接下来，在传感器校正部14中，校正从自主传感器6获取到的传感器值(步骤s112)。以下，说明作为自主传感器6使用车速计以及车辆的偏航轴方向的角速度(以下，称为偏航率)传感器，使用如用以下的数式(3)以及数式(4)表示那样的传感器误差模型来校正的情况。
[0107]
[数学式3]
[0108]
v＝(l s
v
)v
t
ꢀꢀꢀ…
(3)
[0109]
v：车速的传感器值
[0110]
v
t
：车速的真值
[0111]
s
v
：车速的标度因子
[0112]
[数学式4]
[0113]
γ＝(l s
γ
)(γ
t
b
γ
)
ꢀꢀꢀ…
(4)
[0114]
γ：偏航率传感器值
[0115]
γ
t
：偏航率真值
[0116]
s
γ
：偏航率的标度因子
[0117]
b
γ
：偏航率传感器的偏置
[0118]
数式(3)是对车速的真值v
t
乘以车速的标度因子s
v
的模型，数式(4)是对偏航率真值γ
t
重叠偏航率传感器的偏置b
γ
，并乘以偏航率的标度因子s
γ
的模型。
[0119]
之后进行说明，在该例子中，在滤波器13中，s
v
，s
γ
以及b
γ
各自的推测值s
ve
，s
γe
以及b
γe
被推测为传感器误差。传感器校正部14使用该传感器误差的推测值，通过以下的数式(5)以及(6)来校正自主传感器6的传感器值。
[0120]
[数学式5]
[0121][0122]
[数学式6]
[0123][0124]
在数式(5)以及(6)中，v
e
以及γ
e
分别是校正后的车速以及偏航率。此外，以上说明的传感器误差模型是一个例子，也可以利用其它传感器误差模型。
[0125]
<惯性定位部中的处理>
[0126]
在此，返回到图5的流程图的说明，进行步骤s113的处理。即，使用校正后的传感器值和车辆的运动模型，在惯性定位部15中进行惯性定位运算。作为惯性定位运算的具体的运算方法，设为车辆在大致平面内移动而进行模型化。此外，以下，用遵循grs80(geodetic reference system 1980)的椭圆体的导航坐标系来表达。首先，对如用以下的数式(7)表示那样的状态变量进行定义。
[0127]
[数学式7]
[0128][0129]
数式(7)中的y
d
表示将与惯性定位有关的状态变量汇总而成的与惯性定位有关的状态矢量。另外，λ
d
表示在惯性定位运算中得到的纬度，表示在惯性定位运算中得到的经度，h
d
表示在惯性定位运算中得到的椭圆体高，ψ
d
表示在惯性定位运算中得到的方位。
[0130]
该状态变量是通过如用以下的数式(8)表示那样的运动模型进行模型化的。
[0131]
[数学式8]
[0132][0133]
λ
d
：基于惯性定位的纬度[rad]
[0134]
基于惯性定位的经度[rad]
[0135]
h
d
：基于惯性定位的椭圆体高[m]
[0136]
ψ
d
：基于惯性定位的方位(以北为基准的顺时针方向)[rad]
[0137]
v：车速[m/sec]
[0138]
γ：偏航率[rad/sec]
[0139]
a：赤道半径(＝6378137.0[m])
[0140]
fe：地球扁率＝(＝1/298.257223563)
[0141]
数式(8)中的y
d
.表示对与惯性定位有关的状态矢量进行时间微分而得到的矢量。另外，g(y
d
，u)表示将y
d
、u作为输入的非线性函数，u是将输入变量v、γ汇总而成的输入矢量，表示u＝[vγ]
t
。
[0142]
另外，数式(8)中的n表示卯酉半径，m表示子午半径，分别通过以下的数式(9)以及(10)来定义。
[0143]
[数学式9]
[0144][0145]
e：
[0146]
[数学式10]
[0147][0148]
通过将校正后的传感器值代入到数式(8)，时时刻刻进行积分，从而能够得到惯性定位结果。关于积分的方法，经常利用龙格－库塔法(runge－kutta method)等手法。此外，惯性导航的纬度、经度、高度等坐标设为车辆的导航中心的坐标。
[0149]
在此，返回到图5的流程图的说明，进行步骤s114的处理。即，使用通过惯性定位得到的信息，在观测值预测部16中计算预测观测值。以下，说明gnss传感器1的观测值预测、地面物体检测部3的观测值预测。
[0150]
<观测值预测部中的处理>
[0151]
<针对gnss传感器的观测值的预测>
[0152]
由gnss传感器1得到的观测值是gnss天线1a的纬度、经度、高度等坐标信息。以下，将gnss传感器1的观测值设为(λ
m
，h
m
，ψ
m
)。另一方面，关于惯性定位结果也能够得到这些坐标信息，但惯性定位结果是车辆的导航中心的坐标，所以使用从车辆导航中心至gnss天线1a的位置为止的偏移量，预测gnss传感器1的观测值。即，当将从用车辆的导航坐标系表达的车辆导航中心至gnss天线1a为止的偏移量设为(δx，δy，δz)时，预测出的gnss传感器1的观测值(λ
p
，h
p
，ψ
p
)能够根据坐标变换函数c(y
d
，v)，如以下的数式(11)那样从惯性定位值y
d
(λ
d
，h
d
，ψ
d
)和偏移量v(δx，δy，δz)求出。
[0153]
[数学式11]
[0154][0155]
<针对地面物体检测部的观测值的预测>
[0156]
由地面物体检测部3得到的观测值是车辆与地面物体代表点的距离以及方位角。如果使用惯性定位结果和地面物体代表点的绝对位置坐标，则能够预测这些观测值。即，当将车辆与地面物体fe的代表点(xi，yi，zi)的距离的预测值以及相对角分别设为ρ
p，i
以及θ
p，i
时，能够分别通过以下的数式(12)以及(13)求出。
[0157]
[数学式12]
[0158][0159]
[数学式13]
[0160]
θ
p，i
＝atan(e1/n1)
‑
ψ
d
ꢀꢀꢀ…
(13)
[0161]
在此，数式(12)的(x
d
，y
d
，z
d
)是用ecef坐标系表达了作为惯性定位结果的λ
d
，
h
d
的值，能够通过以下的数式(14)来求出。
[0162]
[数学式14]
[0163][0164]
另外，数式(13)的e
i
以及n
i
是用ned坐标系表达了地面物体代表点的值，能够通过以下的数式(15)来求出。
[0165]
[数学式15]
[0166][0167]
<滤波器中的处理>
[0168]
在此，返回到图5的流程图的说明，说明步骤s103的处理。在滤波器13中，使用在步骤s102中得到的实际观测值以及在步骤s114中得到的预测观测值，进行滤波器定位运算和自主传感器6的误差的推测(步骤s103)。
[0169]
首先，将作为推测对象的变量作为纬度、经度、高度、方位、车速标度因子、偏航率标度因子、偏航率偏置，对用以下的数式(16)表示的状态矢量进行定义。
[0170]
[数学式16]
[0171][0172]
设为车速的标度因子s
v
以及偏航率的标度因子s
γ
微小时，从数式(5)以及(6)，车速的真值v
t
以及偏航率的真值γ
t
分别能够通过以下的数式(17)以及(18)近似。
[0173]
[数学式17]
[0174]
v
t
＝(l
‑
s
v
)v
ꢀꢀꢀ…
(17)
[0175]
[数学式18]
[0176]
γ
t
＝(l
‑
s
γ
)γ
‑
b
γ
ꢀꢀꢀ…
(18)
[0177]
用以下的数式(19)、(20)以及(21)表示车速的标度因子s
v
、偏航率的标度因子s
γ
以及偏航率传感器的偏置b
γ
的动态的模型。即，在根据当前的状态来预测接下来的状态的1次马尔可夫过程中进行驱动。
[0178]
[数学式19]
[0179]
s
v
.＝(
‑
s
v
w
sv
)/τ
sγ
ꢀꢀꢀ…
(19)
[0180]
w
sv
：车速标度因子的过程噪音[
‑
]
[0181]
τ
sv
：车速标度因子模型参数值[sec]
[0182]
[数学式20]
[0183]
s
γ
.＝(
‑
s
γ
w
sγ
)τ
sγ
…
(20)
[0184]
w
sγ
：偏航率标度因子的过程噪音[
‑
]
[0185]
τ
sγ
：偏航率标度因子模型参数值[sec]
[0186]
[数学式21]
[0187]
b
γ
.＝(
‑
b
γ
w
bγ
)/τ
bγ
…
(21)
[0188]
w
bγ
：偏航率偏置的过程噪音[rad/sec]
[0189]
τ
bγ
：偏航率偏置模型参数值[sec]
[0190]
在数式(19)～(21)中，s
v
.是s
v
的时间微分，s
γ
.是s
γ
的时间微分，b
γ
.是b
γ
的时间微分。另外，车速标度因子的过程噪音w
sv
是与车速标度因子的时间变迁有关的噪音，偏航率标度因子的过程噪音w
sγ
是与偏航率标度因子的时间变迁有关的噪音，偏航率偏置的过程噪音w
bγ
是与偏航率偏置的时间变迁有关的噪音。
[0191]
将数式(19)～(21)进行汇总时，状态方程能够用以下的数式(22)表示。
[0192]
[数学式22]
[0193][0194]
在数式(22)中，x.表示对状态矢量x进行时间微分而得到的矢量。另外，u是能够用以下的数式(23)表示的输入矢量。
[0195]
[数学式23]
[0196]
u＝[vγ]
t
ꢀꢀꢀ…
(23)
[0197]
通过将数式(22)作为状态方程，将数式(11)作为基于gnss传感器1的观测方程，将数式(12)以及(13)作为基于地面物体检测的观测方程来推测状态矢量x，能够推测定位运算以及自主传感器6的误差。
[0198]
数式(22)的状态方程以及数式(11)～(13)的观测方程关于状态矢量是非线性的，所以为了推测定位运算、自主传感器6的误差，需要应用非线性的状态推测。作为非线性的状态推测的手法，能够应用被称为粒子滤波器或者序贯蒙特卡罗法的粒子滤波器以及扩展卡尔曼滤波器等公知的手法。这些手法是推测在概率上最可能的状态的手法，在状态推测问题中经常利用。
[0199]
以下，说明使用了扩展卡尔曼滤波器的手法。在卡尔曼滤波器中，在系统所附带的噪声遵循高斯分布这样的假定之下进行状态矢量的推测，但与粒子滤波器相比，计算负荷小，运算电路小也可以，所以在安装上是有利的。
[0200]
<基于扩展卡尔曼滤波器的状态推测>
[0201]
当在状态矢量的事先推测值x
b
的周围，对数式(22)进行1次泰勒展开时，能够用以下的数式(24)表示。
[0202]
[数学式24]
[0203]
δx.＝f
a
δx w
ꢀꢀꢀ…
(24)
[0204]
在数式(24)中，w是过程噪音，δx是能够用以下的数式(25)表示的误差状态矢量。
[0205]
[数学式25]
[0206]
δx：＝x
‑
x
b
ꢀꢀꢀ…
(25)
[0207]
另外，在数式(24)中，fa能够用以下的数式(26)表示。
[0208]
[数学式26]
[0209][0210]
如以下的数式(27)那样表示基于gnss传感器1的观测方程z
gnss
。
[0211]
[数学式27]
[0212][0213]
如以下的数式(28)那样表示基于地面物体i的观测方程z
land，i
。
[0214]
[数学式28]
[0215]
z
land，i
＝[ρ
p，i θ
p，i
]
t
ꢀꢀꢀ…
(28)
[0216]
设为能够同时观测n个地面物体时，如以下的数式(29)那样表示将i＝1～n个地面物体观测值进行汇总而得到的观测方程z
land
。
[0217]
[数学式29]
[0218]
z
land
＝[ρ
p，1 θ
p，1 ρ
p，2 θ
p，2
...ρ
p，n θ
p，n
]
t
ꢀꢀꢀ…
(29)
[0219]
关于在各种状况下观测到的观测值，将它们进行汇总而得到的值设为输出矢量z。输出矢量z在能够得到观测值的状况下，其内容不同。以下，说明各状况下的z。
[0220]
<仅gnss传感器能够观测的情况>
[0221]
在地面物体不存在于车辆周边，基于gnss传感器1的观测值良好的情况下，输出矢量z如以下的数式(30)那样表示。
[0222]
[数学式30]
[0223][0224]
<仅地面物体检测部能够观测的情况>
[0225]
在市区等基于gnss传感器1的观测值的可靠度低的情况，且检测到地面物体的情况下，输出矢量z如以下的数式(31)那样表示。
[0226]
[数学式31]
[0227][0228]
<能够用gnss传感器和地面物体检测部观测的情况>
[0229]
在gnss传感器1的观测值良好，且检测到地面物体的情况下，输出矢量z如以下的数式(32)那样表示。
[0230]
[数学式32]
[0231][0232]
此外，输出矢量z能够表达成状态矢量x和u的函数，在上述状况中，全部能够写成如以下的数式(33)那样。
[0233]
[数学式33]
[0234]
z＝h0(x，u)
ꢀꢀꢀ…
(33)
[0235]
通过如数式(32)那样融合地利用观测方程，能够以更高的可靠性进行定位运算以及自主传感器的误差推测。进而，无需如数式(32)那样切换观测方程，通过融合地使用，能够抑制定位跳跃等不好的现象。
[0236]
当在状态矢量的事先推测值x
b
的周围对数式(33)进行泰勒展开时，能够用以下的数式(34)以及(35)表示。
[0237]
[数学式34]
[0238][0239]
[数学式35]
[0240]
δz＝hδx
ꢀꢀꢀ…
(35)
[0241]
在数式(34)中，输出矢量z成为用先前所示的数式(33)表示的观测方程。
[0242]
另外，在数式(35)中h是将观测方程关于状态矢量x进行1次泰勒展开并将事先推测值x
b
作为x代入的矩阵，用以下的数式(36)表示。
[0243]
[数学式36]
[0244][0245]
此外，矩阵h能够以解析方式求出，或者使用数值微分来计算。
[0246]
当按照自主传感器6的采样时间δt将数式(24)以及数式(35)离散化，将离散时刻设为k时，分别成为以下的数式(37)以及数式(38)。
[0247]
[数学式37]
[0248]
δx
k
＝fδx
k
‑1 w
k
ꢀꢀꢀ…
(37)
[0249]
[数学式38]
[0250]
δz
k
＝hδx
k
v
k
ꢀꢀꢀ…
(38)
[0251]
在数式(37)以及数式(38)中，f是与和时刻k有关的误差状态矢量δx
k
有关的状态转移矩阵，用f＝(1 f
a
·
dt)表示，用w
k
＝w
·
δt表示。v
k
是与各观测值对应的传感器噪声。过程噪声w以及传感器噪声v
k
是卡尔曼滤波器的参数，能够使用事先的测定值等来设定。
[0252]
如果使用数式(37)以及数式(38)，应用卡尔曼滤波器的处理算法，则能够求出离散时刻k下的误差状态矢量的推测值δx
e，k
。
[0253]
<时间发展处理>
[0254]
时间发展处理是指自主传感器6每隔采样时间执行的处理。使用时刻k下的惯性定位结果y
d，k
以及自主传感器误差e
sensor，k
，用以下的数式(39)表示时刻k下的状态矢量的事先推测值x
b，k
。
[0255]
[数学式39]
[0256][0257]
当将时刻k下的误差状态矢量的事先推测值设为δx
b，k
，将误差协方差矩阵设为p
k
(n
×
n矩阵)，将事先误差协方差矩阵设为p
b，k
(n
×
n矩阵)时，事先推测值δx
b，k
以及事先误差协方差矩阵p
b，k
分别作为以下的数式(40)以及数式(41)而进行时间发展处理。
[0258]
[数学式40]
[0259]
δx
b，k
＝fδx
b，k
‑1ꢀꢀꢀ…
(40)
[0260]
[数学式41]
[0261]
p
b，k
＝fp
k
‑
l
f
t
q
ꢀꢀꢀ…
(41)
[0262]
在数式(41)中，q是将w
k
的方差作为对角分量的过程噪声的协方差矩阵(n
×
n矩阵)。在电源刚刚接通之后等，需要误差协方差矩阵的初始值，但作为初始值，使用满足0以上的任意的标量值α和n
×
n的单位矩阵i
n
×
n
而用以下的数式(42)表示的p
k
‑1等被经常利用。另外，作为δx
b，k
的初始值，利用将δx
b，k
的要素全部设为0的矢量。
[0263]
[数学式42]
[0264]
p
k
‑1＝α
·
i
n
×
n
ꢀꢀꢀ…
(42)
[0265]
<观测更新处理>
[0266]
在得到基于外界传感器的观测值的时刻，进行按照以下的数式(43)、(44)以及(45)定义的观测更新处理。
[0267]
[数学式43]
[0268]
g
k
＝p
b，k
h
t
(hp
b，k
h
t
r)
‑1ꢀꢀꢀ…
(43)
[0269]
[数学式44]
[0270]
δx
e，k
＝δx
b，k
g
k
(δz
k
‑
hδx
b，k
)
ꢀꢀꢀ…
(44)
[0271]
[数学式45]
[0272]
p
k
＝(i
n
×
n
‑
g
k
h)p
b，k
ꢀꢀꢀ…
(45)
[0273]
在数式(43)～(45)中，δx
e，k
是误差状态矢量的推测值，r是传感器噪声的协方差矩阵(p
×
p矩阵)，g
k
是卡尔曼增益。
[0274]
另外，δz
k
是将z
m，k
设为时刻k下的实际观测值，将z
p，k
设为预测观测值而用以下的数式(46)表示的矢量。
[0275]
[数学式46]
[0276]
δz
k
＝z
m，k
‑
z
p，k
ꢀꢀꢀ…
(46)
[0277]
如果这样做，则由于求出时刻k下的误差状态矢量的推测值δx
e，k
，所以状态矢量x
k
的推测值x
e，k
能够作为以下的数式(47)而求出。
[0278]
[数学式47]
[0279]
x
e，k
＝x
b，k
δx
e，k
ꢀꢀꢀ…
(47
[0280]
在此，p
k
表示与状态矢量的真值与推测值之差有关的分布，如果使用该值，则能够判定外界传感器的异常值。例如，抽取针对p
k
的纬度、经度的要素，进行固有值解析结果，从而能够求出被称为误差椭圆的椭圆，构成如下等的舍弃机构：如果gnss传感器1的传感器值进入到误差椭圆的范围内，则该值用作观测值，在未进入的情况下，不用作观测值。
[0281]
在使用了粒子滤波器的情况下，也能够构成同样的舍弃机构，通过舍弃异常值，能够进行可靠性更高的推测。
[0282]
与gnss传感器1对应的传感器噪声的协方差矩阵r的要素优选根据gnss传感器1的定位状态例如单独定位、dgps、float解以及fix解来变化。
[0283]
与地面物体观测值对应的传感器噪声的协方差矩阵r的要素有时根据地面物体检测部3的性能，针对每个地面物体类别而不同，所以优选针对每个地面物体类别而变化。
[0284]
在此，返回到图5的流程图的说明，说明步骤s104的处理。将在步骤s103中得到的状态矢量的推测值x
e，k
设为状态矢量x
e
，如以下的数式(48)那样定义。
[0285]
[数学式48]
[0286][0287]
在数式(48)中，λ
e
、h
e
以及ψ
e
分别是纬度、经度、高度以及方位的推测值，s
ve
、s
γe
以及b
γe
是车速标度因子、偏航率标度因子、偏航率偏置的推测值。
[0288]
当设为时，作为车辆定位装置20的输出的定位结果y
out
用以下的数式(49)表示。
[0289]
[数学式49]
[0290]
y
out
＝y
e
ꢀꢀꢀ…
(49)
[0291]
另外，自主传感器误差e
sensor
用以下的数式(50)表示，输入到传感器校正部14。
[0292]
[数学式50]
[0293]
e
sensor
＝[s
ve s
γe b
γe
]
t
ꢀꢀꢀ…
(50)
[0294]
通过将具有以上说明的结构的车辆定位装置20搭载于车辆，从而在容易产生多路径的城区及其近郊，也能够维持定位精度，能够将能够继续定位的状况扩大。
[0295]
此外，在本实施方式中，基于gnss传感器1的观测值利用由纬度、经度、高度、方位等gnss传感器1定位运算出的结果，但根据gnss传感器1，有时还能够输出模拟距离观测值、多普勒观测值、载波相位观测值等原始数据，在该情况下，能够将它们中的任意一个或者多个或者全部用作观测值。在该情况下，需要对状态变量追加gnss传感器1的接收机时刻的漂移等，还实施关于接收机时刻的误差推测，但在可见卫星数为少数(例如仅1个)的情况下，也能够生成基于gnss传感器1的观测值。这样的技术被称为紧耦合，通过如本实施方式那样与地面物体检测部3的观测值进行组合，能够进行精度更高的定位。
[0296]
另外，在本实施方式中，作为惯性定位的模型而使用了大致在平面移动的模型，但当作为自主传感器6而使用3轴的加速度以及角速度时，能够构建车辆的倾斜角以及俯仰角变动的惯性定位模型，能够进行更高精度的定位以及姿势推测。
[0297]
另外，在本实施方式中，作为地面物体观测值，将相对距离和相对角度用作观测值，但根据地面物体检测的处理，有时还考虑输出与地面物体的相对速度。在该情况下，对地面物体的观测值追加相对速度，所以能够进行精度更高的定位。
[0298]
另外，在本实施方式中，示出了基于扩展卡尔曼滤波器的推测方法，但在系统所附带的噪声不是高斯分布的情况下，应用粒子滤波器，从而能够以高的精度进行定位以及传感器误差的推测。
[0299]
另外，作为车辆定位装置20的输出而仅采用定位结果，但还能够输出校正后的传感器值。
[0300]
<实施方式2>
[0301]
在以上说明的实施方式1中，使用地面物体的观测值而进行了定位，但也可以在容易产生多路径的城区及其近郊，使用道路的线形信息来进行定位，从而进行定位运算以及传感器校正量的推测。
[0302]
图7是示出本发明的实施方式2的车辆定位装置20a的结构的功能框图。此外，在图
7中，关于与使用图1说明的车辆定位装置20相同的结构附加相同的符号，省略重复的说明。
[0303]
如图7所示，在车辆定位装置20a中，构成为代替图1所示的车辆定位装置20的地面物体检测部3而从道路线形检测部2(第4传感器)接受道路线形数据。
[0304]
道路线形检测部2具有使用安装于车辆的摄像机等来检测道路线形而输出检测结果的功能。通常，摄像机安装于车辆前方，通过图像处理来检测车辆前方的左右的区划线，作为道路线形数据而输入到观测值处理部12。
[0305]
道路线形计算部11具有如下功能：使用从道路信息存储部5得到的道路信息来计算车辆附近的道路线形，作为道路线形信息而输出到观测值预测部16。
[0306]
<动作>
[0307]
接下来，使用图8所示的流程图，说明车辆定位装置20a的整体的处理流程。此外，在图8中，除了追加有步骤s121以及s122以外，关于与使用图5说明的流程图相同的处理附加相同的符号，省略重复的说明。
[0308]
当车辆定位装置20a开始定位时，首先，获取惯性定位的初始值以及在观测值预测部16中利用的当前的惯性定位结果(步骤s100)。
[0309]
接下来，车辆定位装置20a判定从gnss传感器1以及道路线形检测部2等外界传感器是否得到数据(步骤s101)。外界传感器各自的检测对象不同，但在得到传感器值的传感器有1个的情况(是的情况)下，进入到步骤s102，在不是的情况(否的情况)下，进入到步骤s131。
[0310]
车辆定位装置20a在无法得到来自外界传感器的数据的情况下，在步骤s131中，将在步骤s113的惯性定位运算中求出的惯性定位结果作为车辆定位装置20a的定位结果而输出。
[0311]
在步骤s102中，观测值处理部12将由外界传感器得到的传感器值以在接下来的步骤s103中滤波器13能够利用的方式进行处理，并作为实际观测值而输出。
[0312]
在此，说明针对由观测值处理部12中的道路线形检测部2得到的观测值的处理。此外，关于针对由gnss传感器1得到的观测值的处理，与实施方式1相同，所以省略说明。
[0313]
<针对道路线形检测部中的观测值的处理>
[0314]
在道路线形检测部2中，能够从安装于车辆前方的摄像机检测车辆的左右方向的区划线。在道路线形检测部2中，作为与左右的区划线有关的检测结果，输出3次函数的系数，右区划线以及左区划线在车辆坐标系中分别用以下的数式(51)以及(52)表示。
[0315]
[数学式51]
[0316]
yb＝c
3r
·
xb3 c
2r
·
xb2 c
1r
·
xb1 c
0r
ꢀꢀꢀ…
(51)
[0317]
[数学式52]
[0318]
yb＝c
3l
·
xb3 c
2l
·
xb2 c
1l
·
xb1 c
0l
ꢀꢀꢀ…
(52)
[0319]
在本实施方式中，将行车线中央的道路线形用作代表值。因而，在观测值处理部12中，关于道路线形计算结果，进行用以下的数式(53)表示的处理。
[0320]
[数学式53]
[0321]
yb＝c
3c
·
xb3 c
2c
·
xb3 c
1c
·
xb1 c
0c
ꢀꢀꢀ…
(53)
[0322]
在数式(53)中，c
3c
＝(c
3r
c
3l
)/2，c
2c
＝(c
2r
c
2l
)/2，c
1c
＝(c
1r
c
1l
)/2，c
0c
＝(c
0r
c
0l
)/2。
[0323]
在本实施方式中，作为由道路线形检测部2检测的观测值，设为xb＝0的点处的横向位置偏差y
m
，偏差角θ
m
，曲率κ
m
。然后，分别通过以下的数式(54)、(55)、(56)，从行车线中央的道路线形求出。
[0324]
[数学式54]
[0325]
y
m
＝c
0c
ꢀꢀꢀ…
(54)
[0326]
[数学式55]
[0327]
θ
m
＝atan(c
1c
)
ꢀꢀꢀ…
(55)
[0328]
[数学式56]
[0329]
κ
m
＝2c
2c
ꢀꢀꢀ…
(56)
[0330]
图9是说明道路线形的图，图示出横向位置偏差y
m
，偏差角θ
m
，曲率κ
m
的定义，用车辆导航坐标系以及ned坐标系表示车辆ov的位置。另外，示意地示出了用数式(51)表示的右区划线以及用数式(52)表示的左区划线。
[0331]
<道路线形计算部中的处理>
[0332]
在此，返回到图8的流程图的说明，说明步骤s121。车辆定位装置20a与步骤s101以及步骤s111并行地从道路信息存储部5获取车辆周边的道路信息(步骤s121)。道路信息存储部5如图3所示，存储有与地图数据被整理的区间中的道路上的实际存在地面物体以及虚拟地面物体有关的信息，作为与虚拟地面物体有关的信息，存储有车道链路、行车线链路、行车线链路上的节点、斜线区域等信息。除此之外，有时还包括行车线的行车线宽度、纵、横坡度等信息。
[0333]
当在步骤s121中获取到道路信息之后，进入到步骤s122。在步骤s122中，在道路线形计算部11中，计算之后在观测值预测部16中利用的车辆周边的道路线形信息。在本实施方式中，以下说明行车线中央的绝对位置坐标作为地图节点而保持于道路信息存储部5，使用该信息来计算道路线形的方法。
[0334]
图10是示出步骤s122中的处理的流程图。以下，使用图10来说明步骤s122处理。
[0335]
首先，在步骤s201中，从道路信息存储部5获取地图节点数据。在此，作为地图节点数据，车辆所处的行车线中央的绝对位置坐标作为纬度、经度、高度而得到。以下，将车辆周边的多个点的地图节点数据称为地图点群。
[0336]
接下来，在步骤s202中，关于在步骤s201中得到的地图点群实施函数近似。具体而言，首先，将地图点群表达成经过从开始位置的节点至结束位置的节点为止的各节点的线，从得到的线求出道路线形。即，使用如开始位置节点处的s为s＝0、结束点节点处的s为s＝l那样的线长参数s，将道路点群表示为道路线形。各节点位置处的s能够从地图节点数据的纬度及经度及高度求出，所以能够通过函数近似等插补处理，将地图点群的各节点处的纬度及经度及高度等绝对位置坐标表达为使用了s的线。作为函数近似的方法，可举出使用了贝塞尔曲线、样条曲线等的方法。在本实施方式中，说明以3次多项式近似的手法。
[0337]
图11示出地图点群和函数近似的道路线形的概念图。当函数近似的地图点群即道路中央的绝对位置坐标在ecef坐标(x，y，z)中用函数l
x
(s)、l
y
(s)、l
z
(s)表达时，能够分别如以下的数式(57)、(58)、(59)那样用3次多项式表达。
[0338]
[数学式57]
[0339]
l
x
(s)：x＝c
3x
s3 c
2x
s2 c
1x
s c
0x
ꢀꢀꢀ…
(57)
[0340]
[数学式58]
[0341]
l
y
(s)：y＝c
3y
s3 c
2y
s2 c
1y
s c
0y
ꢀꢀꢀ…
(58)
[0342]
[数学式59]
[0343]
l
z
(s)：z＝c
3z
s3 c
2z
s2 c
1z
s c
0z
ꢀꢀꢀ…
(59)
[0344]
此外，作为计算道路线形的其它方法，有根据行车线链路以及节点信息来计算道路线形的方法等，例如能够使用日本特开2005
‑
313803号公报所公开的技术。
[0345]
接下来，在步骤s203中，作为在步骤s202中得到的道路线形，将3次近似系数作为计算结果而输出。
[0346]
在步骤s122中计算出的道路线形的信息被提供到观测值预测部16而进行步骤s114的处理。即，在步骤s114中，使用由惯性定位部15得到的惯性定位结果和由道路线形计算部11得到的道路线形信息来计算预测观测值。此外，针对gnss传感器1的观测值的预测与实施方式1相同，所以以下说明针对道路线形检测部2的观测值的预测。
[0347]
<观测值预测部中的处理>
[0348]
<针对道路线形检测部的观测值的预测>
[0349]
由道路线形检测部2得到的观测值是横向位置偏差、偏差角、曲率。能够根据惯性定位结果和由道路线形计算部11得到的车辆周边的道路线形，预测横向位置偏差、偏差角、曲率。具体而言，如图9所示，使用坐标变换，将由道路线形检测部2得到的道路线形(l
x
(s)，l
y
(s)，l
z
(s))变换为用车辆导航坐标系表达的道路线形(l
nx
(s)，l
ny
(s)，l
nz
(s))。即，表达成xb＝l
nx
(s)，yb＝l
ny
(s)，zb＝l
nz
(s)。进而，以解析性或数值性地计算成为xb＝0的s。当将此时的s设为sc时，预测出的横向位置偏差、偏差角、曲率分别如以下的数式(60)、(61)、(62)那样求出。这些能够以解析性地求出，或者使用数值计算来求出。
[0350]
[数学式60]
[0351]
y
p
＝l
ny
|
s＝sc
[0352]
…
(60)
[0353]
[数学式61]
[0354][0355]
[数学式62]
[0356][0357]
<滤波器中的处理>
[0358]
在此，返回到图8的流程图的说明，说明步骤s103的处理。在滤波器13中，使用在步骤s102中得到的实际观测值以及在步骤s114中得到的预测观测值，进行滤波器定位运算和自主传感器6的误差的推测。与实施方式1不同的点在于观测方程。
[0359]
如以下的数式(63)那样表示基于道路线形的观测方程z
road
。
[0360]
[数学式63]
[0361]
z
road
＝[y
p θ
p κ
p
]
t
ꢀꢀꢀ…
(63)
[0362]
<仅gnss传感器能够观测的情况>
[0363]
无法检测道路线形，基于gnss传感器1的观测值良好的情况下，输出矢量z如以下的数式(64)那样表示。
[0364]
[数学式64]
[0365][0366]
<仅道路线形检测部能够观测的情况>
[0367]
在市区等基于gnss传感器1的观测值的可靠度低的情况，且在检测到道路线形的情况下，输出矢量z如以下的数式(65)那样表示。
[0368]
[数学式65]
[0369][0370]
<能够用gnss传感器和道路线形检测部观测的情况>
[0371]
在gnss传感器1的观测值良好，且检测到道路线形的情况下，输出矢量z如以下的数式(66)那样表示。
[0372]
[数学式66]
[0373][0374]
在各状况下，如果能够进行与实施方式1同样的处理，则能够推测定位运算和自主传感器的校正量。
[0375]
另外，在上述中，作为道路线形的观测值，使用了xb＝0的点，但xb也可以选择任意的点。进而，也可以不仅如xb＝0那样利用一点，而利用多个点。例如，也可以同时使用xb＝0的点和xb＝10的点等。在该情况下，与道路线形有关的观测值的数量增加，所以能够进行精度更高的定位。
[0376]
另外，将基于道路线形的观测值设为横向位置偏差、偏差角、曲率，但在道路线形的观测值以更高次的次数进行了曲线近似的情况下，能够将曲率变化等也作为观测值而追加。在该情况下，观测值的数量增加，所以能够进行精度更高的定位。
[0377]
如以上说明，使用道路线形检测部2和道路信息存储部5，从而能够进行定位运算以及传感器校正量的推测。根据这样的结构，与使用了地面物体的情况同样地，能够将能够继续定位的状况扩大。
[0378]
<实施方式3>
[0379]
也可以将以上说明的实施方式1以及实施方式2进行组合，使用地面物体的观测值以及道路的线性信息来进行定位。
[0380]
<装置结构>
[0381]
图12是示出本发明的实施方式3的车辆定位装置20b的结构的功能框图。此外，在图12中，关于与使用图1以及图7分别说明的车辆定位装置20以及20a相同的结构附加相同的符号，省略重复的说明。
[0382]
如图12所示，在车辆定位装置20b中，构成为从道路线形检测部2接受道路线形数据，另外，从地面物体检测部3接受地面物体信息相对关系的数据。
[0383]
<动作>
[0384]
车辆定位装置20b的整体的处理流程与图8相同，所以使用图8，仅说明与实施方式1以及2不同的点。
[0385]
当车辆定位装置20b开始定位时，首先，获取惯性定位的初始值以及在观测值预测部16中利用的当前的惯性定位结果(步骤s100)。
[0386]
接下来，车辆定位装置20b判定从gnss传感器1、道路线形检测部2以及地面物体检测部3等外界传感器是否得到数据(步骤s101)。外界传感器各自的检测对象不同，但在得到传感器值的传感器有1个的情况(是的情况)下，进入到步骤s102，在不是的情况(否的情况)下，进入到步骤s131。
[0387]
车辆定位装置20b在无法得到来自外界传感器的数据的情况下，在步骤s131中，将在步骤s113的惯性定位运算中求出的惯性定位结果作为车辆定位装置20b的定位结果而输出。
[0388]
在步骤s102中，观测值处理部12将由外界传感器得到的传感器值以在接下来的步骤s103中滤波器13能够利用的方式进行处理，并作为实际观测值而输出。
[0389]
在此，关于观测值处理部12中的针对由gnss传感器1、道路线形检测部2以及地面物体检测部3各自得到的观测值的处理，与实施方式1以及2相同，所以省略说明。
[0390]
<滤波器中的处理>
[0391]
在此，返回到图8的流程图的说明，说明步骤s103的处理。在滤波器13中，使用在步骤s102中得到的实际观测值以及在步骤s114中得到的预测观测值，滤波器进行定位运算和自主传感器6的误差的推测。与实施方式1以及2不同的点在于观测方程。滤波器13中的观测方程能够根据各状况，以如下那样表示。
[0392]
<仅gnss传感器能够观测的情况>
[0393]
在地面物体不存在于车辆周边，无法检测道路线形的情况下，在仅基于gnss传感器1的观测值良好的情况下，输出矢量z如以下的数式(67)那样表示。
[0394]
[数学式67]
[0395][0396]
<仅地面物体检测部能够观测的情况>
[0397]
在市区等基于gnss传感器1的观测值的可靠度低，无法检测道路线形的情况，仅能够检测地面物体的情况下，输出矢量z如以下的数式(68)那样表示。
[0398]
[数学式68]
[0399][0400]
<仅道路线形检测部能够观测的情况>
[0401]
在地面物体不存在于车辆周边，市区等基于gnss传感器1的观测值的可靠度低的情况，且仅检测到道路线形的情况下，输出矢量z如以下的数式(69)那样表示。
[0402]
[数学式69]
[0403][0404]
<能够用gnss传感器和地面物体检测部观测的情况>
[0405]
在无法检测道路线形，gnss传感器1的观测值良好，且检测到地面物体的情况下，输出矢量z如以下的数式(70)那样表示。
[0406]
[数学式70]
[0407][0408]
<能够用gnss传感器和道路线形检测部观测的情况>
[0409]
在地面物体不存在于车辆周边，gnss传感器1的观测值良好，且检测到道路线形的情况下，输出矢量z如以下的数式(71)那样表示。
[0410]
[数学式71]
[0411][0412]
<能够用地面物体检测部和道路线形检测部观测的情况>
[0413]
在市区等基于gnss传感器1的观测值的可靠度低的情况，且检测到道路线形以及地面物体的情况下，输出矢量z如以下的数式(72)那样表示。
[0414]
[数学式72]
[0415][0416]
<能够用gnss传感器、道路线形检测部以及地面物体检测部观测的情况>
[0417]
在gnss传感器1的观测值良好，且检测到道路线形以及地面物体的情况下，输出矢量z如以下的数式(73)那样表示。
[0418]
[数学式73]
[0419][0420]
通过使用与上述各状况对应的观测方程，能够在滤波器13中进行滤波器定位运算和自主传感器6的误差的推测。
[0421]
如以上说明，通过使用道路线形检测部2、地面物体检测部3、地面物体信息存储部4以及道路信息存储部5，能够进行定位运算以及传感器校正量的推测。根据这样的结构，能够实现能够继续定位的状况的扩大和抑制定位跳跃等的车辆定位装置。
[0422]
<硬件结构>
[0423]
此外，以上说明的实施方式1～3的车辆定位装置20、20a以及20b的各构成要素能够使用计算机来构成，通过由计算机执行程序而实现。即，车辆定位装置20～20b例如通过图13所示的处理电路50来实现。作为处理电路50，应用cpu、dsp(digital signal processor，数字信号处理器)等处理器，通过执行保存于存储装置的程序，从而实现各部分的功能。
[0424]
此外，作为处理电路50，也可以应用专用的硬件。在处理电路50是专用的硬件的情况下，作为处理电路50，例如单一电路、复合电路、程序化的处理器、并联程序化的处理器、asic(application specific integrated circuit，专用集成电路)、fpga(field－programmable gate array，现场可编程门阵列)、或者将这些组合而成的电路等符合。
[0425]
车辆定位装置20～20b既可以是构成要素各自的功能通过单独的处理电路来实现，也可以是它们的功能汇总地通过1个处理电路来实现。
[0426]
另外，在图14中，示出了处理电路50使用处理器而构成的情况下的硬件结构。在该
情况下，车辆定位装置20～20b的各部分的功能通过与软件等(软件、固件或者软件和固件)的组合来实现。软件等作为程序而记述，保存于存储器52。作为处理电路50发挥功能的处理器51通过读出存储于存储器52(存储装置)的程序而执行，从而实现各部分的功能。即，可以说该程序使计算机执行车辆定位装置20～20b的构成要素的动作的次序以及方法。
[0427]
在此，存储器52例如也可以是ram、rom、闪存存储器、eprom(erasable programmable read only memory，可擦可编程只读存储器)，eeprom(electrically erasable programmable read only memory，电可擦可编程只读存储器)等非易失性或者易失性的半导体存储器、hdd(hard disk drive，硬盘驱动器)、磁盘、柔性盘、光盘、压缩盘、迷你盘、dvd(digital versatile disc，数字多功能盘)及其驱动器装置等或者今后使用的所有的存储介质。
[0428]
以上说明了车辆定位装置20～20b的各构成要素的功能通过硬件以及软件等中的任意一方来实现的结构。但是并不限于此，也可以构成为通过专用的硬件来实现车辆定位装置20～20b的一部分的构成要素，通过软件等来实现另一部分的构成要素。例如，关于一部分的构成要素，通过作为专用的硬件的处理电路50来实现其功能，关于另一部分的构成要素，作为处理器51的处理电路50读出保存于存储器52的程序而执行，从而能够实现其功能。
[0429]
如以上那样，车辆定位装置20～20b能够通过硬件、软件等或者这些的组合来实现上述各功能。
[0430]
详细地说明了本发明，但上述说明在所有的方面是例示，本发明并不限定于此。被理解为未被例示的无数的变形例不脱离本发明的范围而能够设想。
[0431]
此外，本发明能够在其发明的范围内，对各实施方式自由地进行组合，或者对各实施方式适当地进行变形、省略。