专利名称：定位装置及导航系统的制作方法
技术领域：
本发明涉及一种用于检测移动物体位置的定位装置及导航系统， 尤其涉及一种通过在无线电导航获得的定位位置上累积自主传感器测 得的信息，同时对地图数据进行规范从而估计位置的定位装置及导航 系统。
背景技术：
在导航系统中，依据从GPS (Global Positioning System)卫星 发出的无线电波检测出车辆的位置，用车速传感器及陀螺传感器等累 积行驶距离及行驶方向，高精度地估计车辆的当前位置。
但是，在无法收到GPS卫星发出的无线电波的状态下，包含在自 主导航定位之中的误差将随时间的推移而被放大，因而位置的精度会 逐渐下降。因此，提出了各种方法来对通过自主导^i全测出的车辆位 置进行补正。
例如，有利用导航系统的地图数据，对通过自主导航所定位的位 置进行补正的地图匹配方法。由于地图数据是将节点(例如，交叉路 口 )间用连接线(例如，道路)进行结合而成，因此通过将自主导航 所检测的位置与地图数据相对应，从而对车辆的位置进行补正以使车 辆能够在道路上行驶。
但是，地图数据的精度并不太高，通过地图匹配进行的补正通常 不能获得足够的精度。因此，现公开的技术有，例如从地图数据中选
择与通过自主导航所定位的位置、方向最为符合的位置及方位的道路， 并根据该道路而对位置、方向进行补正的方法。(例如，参照专利文献
专利文献l:特开2002-213979号公报
发明内容
但是，如上文所述， 一般市售的导航系统的地图数据精度不是很 高，此外，由于将道路网用连接交叉路口 (节点)的直线状的连接线 来表现，因此有时会与实际道路的形状不一致。
图1 (a)表示了地图数据的连接线信息与实际道路形状偏离的一 个示例。如图1 (a)中所示，实际的道路有緩慢的弯曲，但是在地图 数据中，将其用两条连接线来表现，因此出现了实际道路与连接线信 息不相符的位置。例如，对于"车辆的实际位置"，当车辆正在行驶， 并且能够捕捉到GPS卫星信号的状态下，可将"车辆的实际位置"作 为车辆的位置进行识别；但是在不能捕捉到GPS卫星信号的状态下用 直线状的连接线进行了地图匹配时，车辆的实际位置与定位的位置之 间就会有误差L。
图1 (b)表示了通过测量对误差L的程度进行了调查的地图数据 精度的一个示例。在图1 (b)中，横轴表示测量点(定位的识别序号)， 纵轴表示实际的道路形状与地图数据间的比较差值。如图1 (b)所示， 地图数据在交叉路口 (节点)附近与实际道路间的误差较小，根据地 点不同，有时与实际道路有6m左右的差距。因此，在不能捕捉到GPS 卫星信号的状态下，对地图数据进行地图匹配从而对位置进行补正时， 存在高精度定位比较困难的问题。
另一方面，为了获得高精度的地图数据，需要道路的正确测量及 连接线的高精细化，因而4艮难对所有道路实现上述要求。
因此，本发明是鉴于上述问题点而进行的。鉴于上述课题，其目 的在于，提供一种即使在GPS卫星信号捕捉困难的状态下，也能够使 用现有的地图数据实现高精度定位的定位装置、导航系统。
根据本发明的一种观点，能够提供一种定位装置，其通过无线电
导航定位单元检测移动物体的位置，其特征在于，具有第1自主传 感器，用于检测所述移动物体的动作信息；位置检测单元，用于将第1 自主传感器检测的检测信息累积至所述无线电导航定位单元检测的定 位结果，检测所述移动物体的估计位置；地图数据定位单元，用于根 据所述位置检测单元;^测的所述估计位置并参考地图数据存储单元， 检测从对应于所述估计位置的连接线开始的预定距离的地图数据估计 位置；最优位置估计单元，用于将所述估计位置及所述地图数据估计 位置应用于卡尔曼滤波器，估计所述移动物体的所述位置。
根据本发明的其他观点，能够提供一种使用了下述定位装置的导 航系统。 一种定位装置，其通过无线电导航定位单元检测移动物体的 位置，其特征在于，具有第1自主传感器，用于检测所述移动物体 的动作信息；位置检测单元，用于将第1自主传感器检测的检测信息 累积至所述无线电导航定位单元检测的定位结果，检测所述移动物体 的估计位置；地图数据定位单元，用于根据所述位置检测单元检测的 所述估计位置并参考地图数据存储单元，检测从对应于所述估计位置 的连接线开始的预定距离的地图数据估计位置；最优位置估计单元， 用于将所述估计位置及所述地图数据估计位置应用于卡尔曼滤波器， 估计所述移动物体的所述位置。通过本发明，能够提供一种即使在GPS卫星信号捕捉困难的状态 下，也能够使用现有的地图数据实现高精度定位的定位装置及导航系统。
图1为表示地图数据的连接线信息与实际道路形状之间的偏离的 一个示例的示意图。
图2为使用了定位装置的导航系统的概要构成图。
图3为，定位装置的功能框图及卡尔曼滤波器的耦合图像的示意图。
图4为，为了与卡尔曼滤波器耦合，通过两个传感器系统检测出 的各个位置的示意图(其1)。
图5为，为了与卡尔曼滤波器耦合，通过两个传感器系统检测出 的各个位置的示意图(其2)。
图6为，用于说明卡尔曼滤波器中使用的观测方程式的示意图。
图7为，定位装置依据GPS无线电波的捕捉状况对车辆位置进行 定位的顺序的流程图。
图8为，通过定位装置进行实际定位获得的实验结果的示意图。
符号说明
1, GPS4妻收装置 2， 车速传感器3, 横摆率传感器
4, 转向角传感器 5， 地图数据库
6, 输入装置
7, 显示装置 8， 导航ECU 9， 定位装置 10, 导航系统
具体实施例方式
以下参照图纸对本发明的最优实施形态进行说明。
图2中图示了使用了本实施形态中的定位装置9的导航系统10的 概要构成图。
导4元系统10受控制导4元系统的导4元ECU (Electrical Control Unit) 8控制。导航ECU8的构成为，将运行程序的CPU、存储程序的 存储装置(硬盘、R0M)、临时存储数据及程序的RAM、数据输入及输出 的输入输出装置、NV (Non Volatile:非易失)-RAM等通过总线连接 在一起的计算机。
导航ECU8中连接有下列部件接收来自GPS (Global Positioning System)卫星的无线电波的GPS接收装置1;检测车辆速度的车速传感 器2;对围绕经过车的重心并沿车辆前后方向的轴线(侧倾轴)的转动 角速度进行检测的横摆率传感器3;检测方向盘转向角的转向角传感器 4;存储了地图数据的地图数据库(以下简称地图DB) 5;用于操作导 航系统10的输入装置6;显示地图中的当前位置的液晶或HUD( Head Up Display:抬头显示器)等显示装置7。
地图DB5中，将实际的道^^网与节点(例如，道^各与道^各的交叉 点、从交叉路口每按规定间隔分段的点等)以及连接线(连接节点与 节点的道路)相对应，构成图表状的数据库。
导航ECU8，将检测出的当前位置周边的地图从地图DB5中提取， 并按指定的缩小比例显示在设置于车厢内的显示装置7上。导航ECU8， 根据需要在地图中重叠显示车辆的当前位置。
此外，从按键式的键盘及遥控器等输入装置6输入目的地时，从 检测出的当前位置到目的地，例如用迪杰斯特拉算法(dijkstra)等 已知的路线检索法进行路线检索，将路线在地图中重叠显示的同时， 在左右转向的交叉路口前向驾驶员提示路线。
导航ECU5的CPU,通过运行存储于存储装置中的程序，实现本实 施形态中所说明的定位。在图3 (a)中图示了定位装置9的功能框图。
本实施形态中的定位装置9,能在GPS卫星信号捕捉困难及GPS定 位的可靠性降低的状况下，对车辆位置进行高精度定位。下面对其概 要进行说明，定位装置9，在通过自主导航进行的定位中，附加基于地
图DB5的定位信息，然后将通过卡尔曼滤波器进行了最优估计的位置 输出。与在自主导航获得的定位结果上进行地图匹配的情况相比，能 够提高定位精度。通过后文详述的实际定位结果可以看出，本实施形 态的位置补正，特别是对于与行进方向(道路长度方向)垂直的方向 上的位置偏离的补正，具有4艮好的效果。
此外，对于卡尔曼滤波器的应用，需要使耦合的定位信息相互独 立，因此，在本实施形态中，
A. 车速传感器2及转向角传感器4
B. 地图DB5及^f黄摆率传感器3
划分为上述两个传感器系统。另外，也可以在A传感器系统使用横摆 率传感器3、在B传感器系统使用转向角传感器4，还可以在检测车辆 方向的传感器中使用陀螺传感器。
此外，应用卡尔曼滤波器时，将由两个传感器系统获得的输出结 果通过观测方程式进行耦合，但是相对于在A传感器系统中检测出位 置及方向，在B传感器系统中检测出的是与地图DB5中地图数据的相 对距离及相对方向，二者不能直接耦合。因此，本实施形态的定位装 置9，导出了用于将从两个传感器系统获得的检测值进行耦合的观测方 程式。以下进行详细说明。
首先，GPS定位单元81是用已知的方法、依据来自GPS卫星的无 线电波对车辆的位置进行定位。GPS定位单元81，从围绕规定轨道转
动的多个GPS卫星中，选择4颗进入当前车辆位置的规定仰角内的GPS 卫星，并接收从这些GPS卫星发出的无线电波。GPS定位单元81计算 电波的到达时间，由到达时间与光速c算出距捕捉到的GPS卫星的距 离。并且，将3个GPS卫星与车辆的距离交叉的一点作为车辆的位置 进行定位。
定位装置9在接收GPS无线电波过程中，每隔规定时间对车辆位 置进行定位。并且，如果GPS无线电波被屏蔽，则进入将最后定位的 位置作为初始位置，将当时的行驶方向作为初始方向，将这些行驶距 离及方向进行累积的自主导航定位。
图4 (a)为通过自主导航(A传感器系统)检测出的位置的示意 图。图4(a)中的道路21,是用从地图DB5中提取的两个连接线22、 23构成的地图数据来表示的。设定车辆正在从连接线22向连接线23 的方向行驶，GPS无线电波在初始位置24处被屏蔽。
从GPS卫星发出的无线电波被屏蔽时，INS ( Inertial Navigation Sensor )定位单元82，通过车速传感器2检测车速，通过转向角传感 器4检测转向角，然后在初始位置24及初期方向上累积行驶距离及方 向，从而通过自主导航对位置及方向进行估计(以下称为估计位置、 估计方向)。在图4(a)中，图示了相对于车辆的实际位置25的、通 过自主导航估计得到的估计位置26。
此外，为了使用卡尔曼滤波器，需要每个传感器系统的误差分散， 而车速传感器2及转向角传感器4的误差根据速度及GPS无线电波屏 蔽时间等为已知。因此，依据上述这些条件，累积得到的估计位置26
的误差分散也为已知。在图4 (a)中，估计位置26的误差分散是用虚 线围成的椭圓来表示的。
图4 (b)为，依据INS定位单元82估计的估计位置26,使其与 地图DB (B传感器系统)5的连接线对应从而检测出的位置的示意图。 INS定位单元82估计的估计位置26被输入到地图数据定位单元83, 地图数据定位单元83将与估计位置26对应的连接线23从地图DB5中 提取。
由于车辆被估计为存在于估计位置26与连接线23的周边，因此， 地图数据定位单元83将车辆的存在位置(以下称为地图数据估计位置 27 )估计为，存在于从连接线23向估计位置26的方向隔着预定距离r (以下称为偏移量r)的线上的任意点上。
此偏移量r为，GPS屏蔽前的初始位置24与连接线22之间的距离。 即，估计为GPS被屏蔽前的初始位置24与连接线22的偏移持续存在。
此外，估计位置26和偏移量r都只是估计值，因此，地图数据估 计位置27为"隔着偏移量r的线上"的任意点。即，地图数据估计位 置27并不限定于一点。
图5 (c)为，通过横摆率传感器(B传感器系统)的检测值估计 的车辆方向的示意图。通过GPS定位单元81获得的初始方向被输入到 方向检测单元84,方向检测单元84在初始方向上累积通过横摆率传感 器3检测出的横摆率的积分值(表示方向)，从而估计车辆的方向(以
下称为累积估计方向28)。因此，与A传感器系统分别进^f亍车辆行驶方 向的估计。
此外，如上文所述，地图数据估计位置27为本身包含误差分散的 值，累积估计方向28也包含横摆率传感器3的检测误差等误差。地图 数据估计单元83依据偏移量r 、速度及GPS无线电波被屏蔽时间等， 算出地图数据估计位置27、累积估计方向28的误差分散。
用上文所述方法，通过A、 B传感器系统分别对车辆位置及方向进 行估计时，卡尔曼滤波器运算单元85输出车辆位置及方向的最优位置。 图5 (d)为，依据通过A传感器系统获得的估计位置26及估计方向、 和通过B传感器系统获得的地图数据估计位置27及累积估计方向28, 运算得到的最终估计位置29的示意图。最终估计位置29中包含表示 方向的信息。
图5 (d)中，将两个传感器系统的误差分散，用估计位置26与地 图数据估计位置27中有凸部的分散来表示，但是分散实际上有3维的 扩散。图3 (b)中图示了此分散及通过卡尔曼滤波器进行最优位置估 计的图像。卡尔曼滤波器在一个系统的状态被各自独立的进行估计时， 依据状态的概率密度分布，估计概率最高的状态(分布的积为最大的 状态)。因此，通过将两个系统的定位信息用卡尔曼滤波器进行耦合， 能够估计出存在概率最高的位置。
接下来，对卡尔曼滤波器中使用的观测方程式进行说明。
图6为用于说明卡尔曼滤波器中使用的观测方程式的示意图。如 果设定适当的坐标系(例如，Y方向朝北)，并用Xt表示车辆的实际位
置、行驶方向，则X可以用2维平面位置(x， y)及行驶方向6的函 数来表示。
Xt=(x,y，0) …(1)
此外，粗实线表示的是连接线23,连接线23相对于x轴的倾斜角 设为cc。另外，连接线23的交叉路口节点位置设为与y轴的交点(0， c),这样获取连接线23时，连接线23通过如下面所示的车道的公式 来表示。
—x.sin + y-cosa + c = 0 ... (2)
Xt与连接线23的斜率设为4) , Xt与连接线23的距离设为r。斜 率(J)为，依据横摆率传感器3的检测值，由方向检测单元84检测出的 值，距离r为GPS无线电波被屏蔽前的偏移量r。此外，在图6中，a 以逆时针旋转为正，4)以顺时针旋转为正。
因此，可以首先设立使距连接线23的距离r成为零的观测方程式。 此外，也可以设立使相对于连接线23的行驶方向的斜率6成为零的观 测方程式。
因此，将观测量S设为
S= ( cx , c, r, (J))
时，观测方程式可以用式(3)表示。此外，如式3所示的观测方程式
为2行1列的行列式。
<formula>formula see original document page 15</formula>(3)
上侧的要素表示从车道开始的距离的关系式，下侧的要素表示从 车道开始的斜率的关系式。此外，当车辆在行驶直线下侧时，上侧的 要素成为 "xsina -ycosa _c_r ，， o
关于式3的观测方程式向卡尔曼滤波器的应用，在后文进行详细
说明,
图7为表示定位装置9依据GPS无线电波的捕捉状况对车辆位置 进行定位的顺序的流程图。
定位装置9在GPS定位单元81的每个定位间隔，对GPS无线电波 是否被屏蔽进行判断(Sl )。如果GPS无线电波没有被屏蔽(Sl为否)， 则利用GPS无线电波对车辆位置进行定位(S2)。
当GPS无线电波被屏蔽时(Sl为是)，INS定位单元82依据车速 及转向角，在初始位置24及初始方向上累积行驶距离及行驶方向，从 而通过自主导航对估计位置及估计方向进行估计(S3)。此外，INS定 位单元82，根据车速传感器2及转向角传感器4的误差等，算出基于 这些而累积的估计位置26以及估计方向的分散。此外，方向检测单元84在GPS无线电波被屏蔽前的初始方向上累 积横摆率的积分值，从而检测出车辆的累积估计方向(S4)。此外，方 向检测单元84依据横摆率传感器3的检测误差等，算出累积估计方向 的分散。
此外，地图数据定位单元83依据INS定位单元82所估计的估计 位置，参照地图DB5，在与对应的连接线23相距偏移量r的线上的任 意位置，检测地图数据估计位置(S5)。
其次，卡尔曼滤波器运算单元85，将来自A传感器系统的估计位 置及估计方向、来自B传感器系统的地图数据估计位置及累积估计方 向通过卡尔曼滤波器进行耦合(S5)。
用上述方法，能够依据由两个传感器系统独立检测出的位置及方 向，输出概率最高的最终估计位置。此外，步骤S4与S5的顺序不同。
通过自主导航进行的位置估计，具有能反映车辆的行驶过程的优 点和随着时间的推移误差会增大的缺点；基于地图DB的位置估计，具 有能通过地图数据限定车辆位置的优点和其含有因地图数据的精度不 足而产生的误差的缺点。
本实施形态的定位装置9，考虑到由这两个系统估计的位置的误 差，用卡尔曼滤波器对最优位置进行估计，因而能够对双方取长补短。 因此，能够使用地图数据，对通过自主导航;险测出的位置进行高精度 的才卜正。
由于即使GPS无线电波被屏蔽也能够进行高精度定位，因此能够 在适当的时机向驾驶员提供在交叉路口等处的驾驶支持(车辆控制、 唤起注意等)。
下面，对为了将观测方程式(3)应用于卡尔曼滤波器而对观测方 程式(3)实施的操作进行说明。
首先，将观测方程式(3)线形化。为此，观测方程式(3)在时 刻t的估计位置
估计4立置&
的周围进行泰勒(Taylor)展开。 [数3]
h(Xt，§t) + J'x(Xt-Xt) + J;AS = 02xl…(4)
其中，Xt为真实位置
^为观测到的测定值 AS为测定误差 真实观'J定值S" + AS
此外，J'X,J;为观测方程式的偏微分。 [数4]
<formula>formula see original document page 18</formula>
将式(4)变形可以得到下式。
<formula>formula see original document page 18</formula>... (5)
进行标准化
<formula>formula see original document page 18</formula>... (6)
<formula>formula see original document page 18</formula>... (7)
在此，Jx，-Ws,Js分别可用下面的公式表示,
<formula>formula see original document page 18</formula>
<formula>formula see original document page 19</formula>此外，在卡尔曼滤波器中，由于状态方程式及观测方程式均以线 形模型为前提，因此将x,定义如下。<formula>formula see original document page 19</formula>
并且，如果用乘以J,的m维空间考虑，可以通过线形模型来表述。 由式(7)可得，
-WS=XS+JSAS... ( 8 ) 式(8)为线形模型。
由于观测量的测定误差AS被^假定为白色高斯分散，因此，其平均 ^f直E ( AS) =0。因此，
E(JSAS) = 0
E(JSASASTJST) = JSE(ASAST)JST=JSQSJST=Q'S... (9)
将通过自主导航算出的估计位置的平均分散，用乘以J,的m维空 间考虑时，可以表示如下。
<formula>formula see original document page 20</formula>…(10) 但是，假定平均E(AXt)二O
<formula>formula see original document page 20</formula> (11) 根据上述内容，最优估计值可用如下方法求得。 [数9]<formula>formula see original document page 20</formula>) ... (12)
但是，<formula>formula see original document page 20</formula>
为使式(12)返回X- (x， y， 6 )的空间，对其进行逆映射。 [数IO]<formula>formula see original document page 20</formula>[通过定位装置9获得的测定结果]
图8所示为，在一般道路上GPS无线电波被屏蔽的状况下，定位
装置9的定位结果。定位时间与GPS无线电波被屏蔽的时间基本相等， 约为950秒。
偏移量r相当于标准偏差10[m]、观测量S= ( a , c， r, c)))的 误差共分散行列式Qs设定如下。此外，误差共分散行列式Qs为a 、 c、 r、小的顺序。
0 0 、
0 0 100 0 0 0週y
图8 (a)所示为，相对于用其他系统进行定位的实际车道(作为 真实值)，通过自主导航检测出的估计位置及本实施形态的定位装置9 定位而得的位置的偏离大小的试验结果。在图8 (a)中，以定位时间 (行驶时间)作为横轴，累积误差平均的距离[m]用纵轴表示。
如图8 (a)所示，通过自主导航检测出的估计位置中，有相对于 真实值偏离30m以上的情况，而用本实施形态的定位装置，即使行驶 了 950秒，相对于真实值也仅有10m左右的偏离。
此外，在图8 (b)中分别图示了 I.用其他系统定位的实际车道、 II.通过自主导航检测出的估计位置、III.用本实施形态的定位装置9
00
00
00
00
定位的位置。黑圓点为交叉路口。此外，在图8 (b)中，I.用其他系
统定位的实际车道，与地图DB5中提取的车道(连接线细实线)基 本重叠。
由于在通过自主导航检测出的估计位置中，在沿连接线的车道上 有大幅度偏离的情况，因此，即使进行地图匹配，在匹配时也不可能 不包含误差。但是，本实施形态的定位装置9定位的位置，与实际车 道高精度的一致。此外，如图8 (b)所示，本实施形态的位置补正， 特别是对于与行驶方向垂直的方向上的补正具有^f艮大效果。
以上通过实施例对本发明进行了说明，但是本发明并不仅限于所 述实施例，在本发明的范围内，进行各种变形及改良均可。
本国际申请为，主张2006年7月25日提交的日本专利申请 2006-202475号的优先权的申请，日本专利申请2006-202475号的全部 内容都援引至本国际申请。
工业利用性
本发明能够适用于检测移动物体位置的定位装置及导航系统，特 别是，能够适用于将通过无线电导航获得的定位位置与通过自主传感 器测得的信息进行累积，同时基于地图数据，从而估计位置的定位装 置及导航系统。
权利要求
1、一种定位装置，其通过无线电导航定位单元检测移动物体的位置，其特征在于，具有第1自主传感器，用于检测所述移动物体的动作信息；位置检测单元，用于将第1自主传感器检测的检测信息累积至所述无线电导航定位单元检测的定位结果，检测所述移动物体的估计位置；地图数据定位单元，用于根据所述位置检测单元检测的所述估计位置并参考地图数据存储单元，检测从对应于所述估计位置的连接线开始的预定距离的地图数据估计位置；最优位置估计单元，用于将所述估计位置及所述地图数据估计位置应用于卡尔曼滤波器，估计所述移动物体的所述位置。
2、 如权利要求1所述的定位装置，其特征在于，具有第2自主传感器，用于检测所述移动物体的动作信息；方向检测单元，用于将第2自主传感器检测的检测信息累积至所 述无线电导航定位单元检测的定位结果，检测累积估计方向，所述最优位置估计单元除了将所述估计位置以及所述地图数据估 计位置应用于卡尔曼滤波器外，还将所述位置检测单元检测的估计方 向以及所述累积估计方向也应用于卡尔曼滤波器，估计所述移动物体 的所述位置。
3、 如权利要求1或权利要求2所述的定位装置，其特征在于所述第1自主传感器为转向角传感器及车速传感器，所述第2自 主传感器为横摆率传感器，所述卡尔曼滤波器的观测方程式为 [数12]<formula>formula see original document page 3</formula>":车道的斜率c:车道的截距r:从车道开始的距离 4):从车道开始的斜率。
4、 一种导航系统，其使用了权利要求1至权利要求3中任意一项所 述的定位装置。
全文摘要
一种使用无线电导航定位单元(81)检测移动物体位置的定位装置(9)，其特征在于，具有检测移动物体动作信息的第1及第2自主传感器(2)、(4)、(3)；将定位结果与通过第1自主传感器测得的信息进行累积，从而测出估计位置及估计方向的位置检测单元(82)；依据估计位置并参考地图数据存储单元(5)，测得距对应的连接线预定距离的地图数据估计位置的地图数据定位单元(83)；将定位结果与通过第2自主传感器测得的信息进行累积，从而测出累积估计方向的方向检测单元(84)；将估计位置及地图数据估计位置、与估计方向及累积估计方向适用于卡尔曼滤波器，从而估计出移动物体位置的最优位置估计单元(85)。
文档编号G09B29/00GK101360971SQ200780001589
公开日2009年2月4日 申请日期2007年6月8日 优先权日2006年7月25日
发明者小堀训成, 香川和则 申请人:丰田自动车株式会社