CN108700423B - 车载装置及推定方法 - Google Patents

车载装置及推定方法 Download PDF

Info

Publication number
CN108700423B
CN108700423B CN201780014609.6A CN201780014609A CN108700423B CN 108700423 B CN108700423 B CN 108700423B CN 201780014609 A CN201780014609 A CN 201780014609A CN 108700423 B CN108700423 B CN 108700423B
Authority
CN
China
Prior art keywords
vehicle
error
state
value
estimated
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201780014609.6A
Other languages
English (en)
Other versions
CN108700423A (zh
Inventor
冈田和弘
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Faurecia Clarion Electronics Co Ltd
Original Assignee
Clarion Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Clarion Co Ltd filed Critical Clarion Co Ltd
Publication of CN108700423A publication Critical patent/CN108700423A/zh
Application granted granted Critical
Publication of CN108700423B publication Critical patent/CN108700423B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Hardware Design (AREA)
  • Mathematical Physics (AREA)
  • Navigation (AREA)

Abstract

本发明的目的在于能够通过卡尔曼滤波器高精度地推定车辆的状态。导航装置(1)具有：基于来自传感器的输出而观测与车辆的变动相关的观测量的观测部(21)；和通过卡尔曼滤波器来推定表示车辆状态的状态量的推定部(22)，推定部(22)计算出车辆的状态量的预测值，通过作为与观测量具有微积分关系的状态量的误差而输入了观测量的误差的卡尔曼滤波器，计算出预测值的误差协方差矩阵，基于计算出的预测值和预测值的误差协方差矩阵，通过卡尔曼滤波器来计算出车辆的状态量的推定值和推定值的误差协方差矩阵。

Description

车载装置及推定方法
技术领域
本发明涉及车载装置及推定方法。
背景技术
作为本技术领域的背景技术，具有日本特开2000-55678号公报(专利文献1)。在专利文献1中，记载了“具有卡尔曼滤波器(误差推定部15)，该卡尔曼滤波器将基于移动距离、方位变化量而由相对轨迹运算部13、绝对位置运算部14计算出的推定导航数据(车速、绝对方位、绝对位置)、与来自GPS接收机8的GPS测位数据(速度、方位、位置)之差作为观测值(其中该移动距离、方位变化量是根据车速传感器4、陀螺仪6(gyro)中的检测值而由移动距离运算部11、方位变化量运算部12计算出的)，作为该状态量之一，设定从陀螺仪输出向角速度转换的转换增益的误差(增益误差)。根据通过该卡尔曼滤波器求出的增益误差的推定值，对使用陀螺仪输出及转换增益而计算出的方位变化量进行修正”。
现有技术文献
专利文献
专利文献1：日本特开2000-55678号公报
发明内容
然而，根据基于车速脉冲和来自陀螺仪等传感器的输出而观测的速度和角速度等观测量求出的移动距离和方位变化量存在由于时间推移而误差累积的情况。在专利文献1中，关于通过考虑了累积的误差的卡尔曼滤波器来推定车辆状态的技术没有公开，而无法通过卡尔曼滤波器高精度地推定车辆的状态。
于是，本发明的目的在于能够通过卡尔曼滤波器高精度地推定车辆的状态。
为了实现上述目的，车载装置是搭载于车辆的车载装置，其特征在于，具有：观测部，其基于来自传感器的输出，观测与上述车辆的变动相关的观测量；和控制部，其通过卡尔曼滤波器来推定表示上述车辆的状态的状态量，上述控制部计算出上述车辆的上述状态量的预测值，通过作为与上述观测量具有微积分关系的上述状态量的误差而输入了上述观测量的误差的上述卡尔曼滤波器，计算出上述预测值的误差，基于计算出的上述预测值和上述预测值的误差，通过上述卡尔曼滤波器计算出上述车辆的上述状态量的推定值和上述推定值的误差。
发明效果
根据本发明，能够通过卡尔曼滤波器高精度地推定车辆的状态。
附图说明
图1是表示导航装置的结构的框图。
图2是表示导航装置的动作的流程图。
图3是表示车辆状态推定处理中的推定部的动作的流程图。
图4是用于说明基于推定出的车辆状态的地图匹配(map-matching)的处理的图。
具体实施方式
图1是表示导航装置1(车载装置)的结构的框图。
导航装置1是搭载于车辆的车载装置，遵照搭乘于车辆的用户的操作，来执行地图显示、地图中车辆的当前位置显示、到达目的地为止的路径搜索、路径引导等。此外，导航装置1可以固定于车辆的仪表板等，也可以相对于车辆而可装拆。
如图1所示，导航装置1具有：控制部2、存储部3、触摸面板4、GPS接收部5、车速传感器6(传感器)、陀螺仪传感器7(角速度传感器、传感器)和加速度传感器8(传感器)。
控制部2具有CPU(Central Processing Unit，中央处理器)、ROM(Read OnlyMemory，只读存储器)、RAM(Random Access Memory，随机存取存储器)、其他控制电路等，控制导航装置1的各部分。控制部2通过执行ROM或存储部3等中所存储的控制程序，作为后述的观测部21及推定部22而发挥功能。
存储部3具有硬盘、EEPROM(Electrically Erasable Programmable read onlymemory，带电可擦可编程只读存储器)等非易失性存储器，能够重写地存储数据。存储部3除了存储由控制部2执行的控制程序以外，还存储地图数据3a。地图数据3a具有：与表示交叉点或道路网上的其他连接点的节点相关的节点信息、与表示节点与节点之间的道路区间的链路相关的链路信息、地图的显示所涉及的信息等。链路信息按每条链路而至少包含与链路的位置相关的信息、和与链路的方位相关的信息。
触摸面板4具有显示面板4a和触摸传感器4b。显示面板4a由液晶显示屏或EL(Electro Luminescent，电致发光)显示屏等构成，在控制部2的控制下，将各种信息显示到显示面板4a上。触摸传感器4b重叠地配置在显示面板4a上，检测用户的触摸操作，将表示被触摸操作了的位置的信号输出到控制部2。控制部2基于来自触摸传感器4b的输入，执行与触摸操作相对应的处理。
GPS接收部5经由GPS天线5a接收来自GPS卫星的GPS电波，根据叠加于GPS电波的GPS信号，通过运算获取车辆的位置、和车辆行进方向的方位(以下表述成“车辆方位”)。GPS接收部5将表示车辆位置的信息及表示车辆方位的信息输出到控制部2。
车速传感器6检测车辆的车速，将表示检测出的车速的信号输出到控制部2。
陀螺仪传感器7例如由振动陀螺仪构成，检测基于车辆转弯产生的角速度。陀螺仪传感器7将表示检测出的角速度的信号输出到控制部2。
加速度传感器8检测作用于车辆的加速度(例如车辆相对于行进方向的倾斜)。加速度传感器8将表示检测出的加速度的信号输出到控制部2。
如图1所示，控制部2具有观测部21和推定部22。
观测部21基于从车速传感器6、陀螺仪传感器7及加速度传感器8输出的信号，来观测与车辆的变动相关的观测量。在本实施方式中，观测部21基于从车速传感器6输出的表示车速的信号，观测车辆的速度(观测量)来作为观测量。观测部21通过规定的运算，根据表示车速的信号，来计算出车辆的速度和车辆的速度的误差。该计算出的车辆的速度的误差是在根据该速度求出的移动距离中累积的误差。
另外，观测部21基于从陀螺仪传感器7输出的表示角速度的信号，观测基于车辆转弯产生的角速度(观测量)来作为观测量。观测部21通过规定的运算，根据表示角速度的信号，来计算出车辆的角速度和车辆的角速度的误差。该计算出的车辆的角速度的误差是在根据该角速度求出的方位变化量中累积的误差。
另外，观测部21基于从加速度传感器8输出的表示加速度(观测量)的信号，观测车辆的加速度来作为观测量。观测部21通过规定的运算，根据表示加速度的信号，来计算出车辆的加速度和车辆的加速度的误差。该计算出的车辆的加速度的误差是在根据该加速度求出的速度中累积的误差。
另外，观测部21基于从GPS接收部5输出的表示车辆的位置的信息和表示车辆的方位的信息，观测车辆的位置和车辆的方位。
推定部22基于由观测部21观测的车辆的速度、车辆的加速度、车辆的角速度、车辆的位置以及车辆的方位，通过卡尔曼滤波器来推定表示车辆的状态的状态量。在本实施方式中，推定部22将车辆的位置、车辆的方位、车辆的速度以及车辆的角速度作为车辆的状态量来进行推定。
控制部2基于由推定部22推定出的车辆的状态量，来执行成为地图匹配的对象的链路的评价，这将在后叙述。
此外，由推定部22推定的车辆的速度相当于车辆的状态量。另外，由观测部21观测的车辆的速度相当于观测量。同样地，由推定部22推定的车辆的角速度相当于车辆的状态量。另外，由观测部21观测的车辆的角速度相当于观测量。
在此，说明基于卡尔曼滤波器对基本的车辆的状态量的推定。
在本实施方式中，通过卡尔曼滤波器推定的车辆的状态量是车辆的位置、车辆的方位、车辆的速度及车辆的角速度。以下示出推定的车辆的各状态量。
x：车辆位置的x坐标
y：车辆位置的y坐标
θ：车辆的方位
v：车辆的速度
ω：车辆的角速度
在此，若将用矢量表示车辆的状态量的状态矢量设为(x，y，θ，v，ω)，则针对车辆的状态量的状态方程式以式(1)表示。
式1
Figure BDA0001784850240000051
下标的k表示时刻。例如，式(1)的左边即(xk+1、yk+1、θk+1、vk+1、ωk+1)表示时刻k+1的车辆的状态量。在式(1)中，右边的第2项即qk是系统噪声(平均0，具有误差协方差矩阵即Qk的正态分布N(0，Qk))。误差协方差矩阵是指方差与协方差的矩阵。
在本实施方式中，观测部21作为观测对象而观测车辆的速度、车辆的角速度、车辆的加速度、车辆的位置以及车辆的方位。如上所述，观测部21针对车辆的速度，基于来自车速传感器6的输出而进行观测。另外，观测部21针对车辆的角速度，基于来自陀螺仪传感器7的输出而进行观测。另外，观测部21针对车辆的加速度，基于来自加速度传感器8的输出而进行观测。另外，观测部21针对车辆的位置以及车辆的方位，基于来自GPS接收部5的输出而进行观测。以下示出由观测部21观测的观测对象。此外，以下作为观测对象而例示车辆的速度、车辆的角速度、车辆的位置以及车辆的方位。
vPLS：基于来自车速传感器6的输出而观测的车辆的速度
ωGYR：基于来自陀螺仪传感器7的输出而观测的车辆的角速度
xGPS：基于来自GPS接收部5的输出而观测的车辆位置的x坐标
yGPS：基于来自GPS接收部5的输出而观测的车辆位置的y坐标
θGPS：基于来自GPS接收部5的输出而观测的车辆的方位
在此，若将用矢量表示上述观测量的观测矢量设为(vPLS、ωGYR、xGPS、yGPS、θGPS)，则针对观测对象的观测方程式以式(2)表示。式2
Figure BDA0001784850240000061
在式(2)中，rk是观测噪声(平均0，具有作为误差协方差矩阵的Rk的正态分布N(0，Rk))。
以下，针对卡尔曼滤波器，分为预测车辆的状态量的预测处理、和推定车辆的状态量的推定处理来进行说明。
此外，在以下的说明中，标注有下标k+1|k的值表示基于到时刻k为止的信息而预测出的时刻k+1的预测值。另外，标注有下标k|k-1的值表示基于到时刻k-1为止的信息而预测出的时刻k的预测值。另外，标注有下标k+1|k+1的值表示基于到时刻k+1为止的信息而推定出的时刻k+1的推定值。另外，标注有下标k|k的值表示基于到时刻k为止的信息而推定出的时刻k的推定值。另外，标注有下标k-1|k-1的值表示基于到时刻k-1为止的信息而推定出的时刻k-1的推定值。
＜预测处理＞
在卡尔曼滤波器中预测处理是计算出车辆的状态量的预测值(以下表述为“车辆状态预测值”)和车辆状态预测值的误差协方差矩阵的处理。基于式(3)计算出车辆状态预测值。此外，误差协方差矩阵的计算表示误差协方差矩阵的各元素的值的计算。
式3
Figure BDA0001784850240000071
在式(3)中，示出基于到时刻k为止的信息而预测出的时刻k+1的车辆状态预测值的计算。如式(3)的右边所示，该车辆状态预测值是根据基于到时刻k为止的信息而推定出的时刻k的车辆的状态量的推定值来计算出的。例如，表示车辆位置的x坐标的预测值的xk+1|k是根据基于到时刻k为止的信息而推定出的时刻k的、车辆位置的x坐标(xk|k)的推定值、车辆的方位(θk|k)的推定值、和车辆的速度(vk|k)的推定值来计算出的。此外，在式(3)中，T表示观测部21基于来自车速传感器6、陀螺仪传感器7以及加速度传感器8的输出而对观测量进行观测的间隔。
此外，在计算基于到时刻k-1为止的信息而预测出的时刻k的车辆状态预测值的情况下，能够通过在式(3)中逐步递减时刻的算式来进行计算。也就是说，基于到时刻k-1为止的信息而预测出的时刻k的车辆状态预测值是根据基于到时刻k-1为止的信息而推定出的时刻k-1的车辆的状态量的推定值来计算出的。
车辆状态预测值的误差协方差矩阵是基于式(4)计算出的。在本实施方式中，误差协方差矩阵是与车辆的状态量相关的方差及协方差的矩阵。方差是误差的平方。也就是说，车辆的状态量的方差是车辆的状态量的误差的平方。因此，计算车辆状态预测值的误差协方差矩阵相当于计算车辆状态预测值的误差。
式4
P(k+1|k)F(k)P(k|k)FT (k)+Q…(4)
在式(4)中，P表示误差协方差矩阵。式(4)的左边表示基于到时刻k为止的信息而预测出的时刻k+1的误差协方差矩阵。根据基于到时刻k为止的信息而推定出的时刻k的误差协方差矩阵来计算出式(4)的左边所示的误差协方差矩阵。此外，F表示根据式(1)的状态方程式求出的雅可比矩阵(Jacobi matrix)。另外，F中的上标的T表示转置矩阵。另外，Q表示系统噪声的误差协方差矩阵。
此外，在计算基于到时刻k-1为止的信息而预测出的时刻k的车辆状态预测值的误差协方差矩阵的情况下，能够通过在式(4)中逐步递减P的下标时刻的算式来进行计算。也就是说，基于到时刻k-1为止的信息而预测出的时刻k的车辆状态预测值的误差协方差矩阵是根据基于到时刻k-1为止的信息而推定出的时刻k-1的车辆的状态量的推定值的误差协方差矩阵来计算出的。
像这样，在预测处理中，例如在预测时刻k的车辆的状态量的情况下，计算出基于到时刻k-1为止的信息而计算出的时刻k的车辆状态预测值、和基于到时刻k-1为止的信息而计算出的时刻k的车辆状态预测值的误差协方差矩阵。也就是说，预测处理中预测车辆的状态量的概率分布。
＜推定处理＞
接下来，说明推定处理。
在卡尔曼滤波器中推定处理是如下处理：基于通过预测处理计算出的车辆状态预测值、以及车辆状态预测值的误差协方差矩阵，计算出车辆的状态量的推定值(以下表述为“车辆状态推定值”)和车辆状态推定值的误差协方差矩阵。
在推定处理中，通过式(5)计算出观测残差。观测残差是指观测对象的值与根据车辆状态预测值计算出的观测对象所对应的值之间的误差。
式5
Figure BDA0001784850240000091
在式(5)中，左边是用矢量表示观测残差的观测残差矢量。另外，在式(5)中，右边的第1项是作为观测部21的观测对象的观测对象矢量。另外，在式(5)中，右边的第2项是对于在预测处理中预测出的车辆状态预测值乘以根据观测方程式求出的作为观测矩阵的H的项。
使用式(5)所示的观测残差，通过式(6)计算出车辆状态推定值。
式6
Figure BDA0001784850240000101
在式(6)中，示出基于到时刻k+1为止的信息而预测出的时刻k+1的车辆状态推定值。如式(6)的右边所示，根据观测残差，对基于到时刻k为止的信息而推定出的时刻k+1的车辆状态预测值进行修正，从而计算出车辆状态推定值。此外，在计算基于到时刻k为止的信息而预测出的时刻k的车辆状态推定值的情况下，根据观测残差对基于到时刻k-1为止的信息而预测出的时刻k的车辆状态预测值进行修正，从而进行计算。
如上所述，式(6)表示使用观测残差对车辆状态预测值进行修正而计算出车辆状态推定值的算式。如式(6)所示，作为以观测残差对车辆状态预测值进行修正时的修正系数而使用Kk。Kk被称为卡尔曼增益，以式(7)表示。
式7
Kk＝P(k+1|k)HT(Rk+HP(k+1|k)HT)-1…(7)
在式(7)中，Rk是观测噪声的误差协方差矩阵。另外，上标“-1”表示逆矩阵。式(7)所示的卡尔曼增益Kk是根据基于到时刻k为止的信息得到的时刻k+1的车辆状态预测值的误差协方差矩阵而计算出的。
在式(6)中，卡尔曼增益Kk是在计算车辆状态推定值时决定是重视车辆状态预测值来进行计算、还是重视由观测部21观测的观测对象的值来进行计算的系数。
例如，在基于来自车速传感器6、陀螺仪传感器7以及GPS接收部5的输出得到的观测对象的值的误差足够小的情况下，由于该误差足够小，所以期望车辆状态推定值成为观测对象的值。这是因为车辆状态推定值成为误差足够小的值、即成为精度高的值。也就是说，在作为误差协方差矩阵的Rk的值足够小的情况下，期望车辆状态推定值成为观测对象的值。在此，若将Rk的值足够小的情况下的卡尔曼增益Kk设为Kk＝H-1并赋予式(6)，则式(6)的右边的第1项会消失。也就是说，车辆状态推定值成为误差足够小的观测对象的值。
另外，例如在车辆状态预测值的误差比基于来自车速传感器6、陀螺仪传感器7以及GPS接收部5的输出得到的观测对象的误差足够小的情况下，期望车辆状态推定值成为车辆状态预测值。这是因为车辆状态推定值成为误差比观测对象的误差足够小的值、即成为精度高的值。也就是说，在车辆状态预测值的误差协方差矩阵比Rk的值足够小的情况下，期望车辆状态推定值成为车辆状态预测值。在此，作为车辆状态预测值的误差协方差矩阵比Rk的值足够小的情况下的卡尔曼增益Kk，若将Kk＝0赋予式(6)，则车辆状态推定值成为车辆状态预测值。
像这样，卡尔曼增益Kk是如下系数：根据作为观测噪声的误差协方差矩阵的Rk、以及车辆状态预测值的误差协方差矩阵，以车辆状态推定值成为恰当的值的方式进行设定。也就是说，在观测噪声的误差协方差矩阵为观测对象的误差协方差矩阵的情况下，若准确地预测了车辆状态预测值的误差协方差矩阵，则根据基于来自车速传感器6、陀螺仪传感器7以及GPS接收部5的输出得到的观测对象的误差协方差矩阵，以车辆状态推定值成为恰当的值的方式设定卡尔曼增益Kk。
通过式(8)来计算出车辆状态推定值的误差协方差矩阵。如上所述，在本实施方式中，误差协方差矩阵是与车辆的状态量相关的方差及协方差的矩阵。车辆的状态量的方差是车辆的状态量的误差的平方。因此，计算车辆状态推定值的误差协方差矩阵相当于计算车辆状态推定值的误差。
式8
Pk+1|k+1＝(I-KkH)Pk+1|k…(8)
在式(8)中，P与式(4)同样地，表示误差协方差矩阵。另外，在式(8)中，I表示单位矩阵。式(8)的左边表示基于到时刻k+1为止的信息而推定出的时刻k+1的误差协方差矩阵。根据基于到时刻k为止的信息而预测出的时刻k+1的误差协方差矩阵来计算出式(8)的左边所示的误差协方差矩阵。
此外，在计算基于到时刻k为止的信息而预测出的时刻k的车辆状态推定值的误差协方差矩阵的情况下，能够通过在式(8)中逐步递减P的下标时刻的算式来进行计算。也就是说，通过基于到时刻k-1为止的信息而推定出的时刻k的车辆状态预测值的误差协方差矩阵，来计算出基于到时刻k为止的信息而预测出的时刻k的车辆状态推定值的误差协方差矩阵。
式(8)是对车辆状态预测值的误差协方差矩阵乘以(I-KkH)来计算出车辆状态推定值的误差协方差矩阵式的算式。如式(8)所示，车辆状态推定值的误差协方差矩阵依赖于卡尔曼增益Kk的值。
例如，若作为观测噪声的误差足够小的情况下的卡尔曼增益Kk而将Kk＝H-1赋予式(8)，则车辆状态推定值的误差协方差矩阵成为零矩阵。这表明所推定出的车辆状态推定值的误差足够小。
另外，例如，作为车辆状态预测值的误差比观测噪声的误差足够小的情况下的卡尔曼增益的值而赋予Kk＝0，由此车辆状态推定值的误差协方差矩阵变为车辆状态预测值的误差协方差矩阵。这表明车辆状态推定值的误差是误差足够小的车辆状态预测值的误差。
像这样，在式(8)中，根据观测噪声的误差协方差矩阵、和车辆状态预测值的误差协方差矩阵，以使得车辆状态推定值的误差协方差矩阵变恰当的方式设定卡尔曼增益Kk。也就是说，基于观测噪声的精度和车辆状态预测值的精度，以使得车辆状态推定值的误差协方差矩阵变恰当的方式设定卡尔曼增益Kk。
以上，在推定处理中，例如，在推定时刻k的车辆的状态量的情况下，计算基于到时刻k为止的信息而计算出的时刻k的车辆状态推定值、和基于到时刻k为止的信息而计算出的时刻k的车辆状态推定值的误差协方差矩阵。也就是说，推定处理基于在预测处理中预测出的车辆的状态量的概率分布来推定车辆的状态量的概率分布。
通过以上计算，推定部22通过卡尔曼滤波器来计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵，由此推定车辆的状态。
如上所述，卡尔曼增益Kk是恰当地设定车辆状态推定值和车辆状态推定值的误差协方差矩阵的系数。也就是说，车辆状态的推定精度依赖于卡尔曼增益Kk。如上所述，卡尔曼增益Kk是根据观测噪声和车辆状态预测值的误差协方差矩阵来恰当地设定车辆状态推定值和车辆状态推定值的误差协方差矩阵的系数。因此，为了恰当地计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵，需要准确地计算出车辆状态预测值的误差协方差矩阵。但是，在卡尔曼滤波器中，会假定系统噪声及观测噪声是白噪声。也就是说，在卡尔曼滤波器中将误差作为噪声处理的情况下，前提在于误差的平均值为零。因此，考虑平均值不为零的误差、即累积的误差而存在无法计算出车辆状态预测值的误差协方差矩阵的情况，在该情况下，不会准确地计算出卡尔曼增益Kk。这表明无法高精度地计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵，即无法高精度地推定车辆的状态。
于是，本实施方式的推定部22通过以下的卡尔曼滤波器来推定车辆的状态。
以下，通过推定车辆的状态时的导航装置1的动作，来说明由推定部22进行的车辆状态的推定。
图2是表示导航装置1的动作的流程图。
导航装置1的观测部21基于从车速传感器6、陀螺仪传感器7及加速度传感器8输出的信号，观测车辆的速度、车辆的角速度以及车辆的加速度(步骤SA1)。每当车速传感器6、陀螺仪传感器7以及加速度传感器8输出信号时，观测部21观测车辆的速度、车辆的角速度以及车辆的加速度。也就是说，观测部21观测它们的间隔是与车速传感器6、陀螺仪传感器7及加速度传感器8进行检测的间隔相同的间隔。
接着，观测部21基于GPS接收部5的输出，观测车辆的位置以及车辆的方位(步骤SA2)。每当从GPS接收部5输出表示车辆的位置的信息和表示车辆的方位的信息时，观测部21观测车辆的位置及车辆的方位。也就是说，观测部21观测车辆的位置及车辆的方位的间隔成为GPS接收部5接收GPS电波的间隔。
接着，导航装置1的推定部22基于由观测部21观测到的观测对象，执行推定车辆的状态的车辆状态推定处理(步骤SA3)。
图3是表示车辆状态推定处理中的推定部22的动作的流程图。
推定部22执行预测处理(步骤SB1)。如上所述，预测处理是计算出车辆状态预测值以及车辆状态预测值的误差协方差矩阵的处理。通过式(3)计算出车辆状态预测值。通过式(9)计算出车辆状态预测值的误差协方差矩阵。
式9
Pk+1|k＝FkPk|kFT k+Qk+2·Ck…(9)
在式(9)中，Fk是根据式(1)的状态方程式求出的雅可比矩阵，以式(10)表示。另外，Qk是系统噪声的误差协方差矩阵，以式(11)表示。另外，Ck是上次推定出的车辆状态推定值的误差与观测部21基于来自车速传感器6及陀螺仪传感器7的输出而观测的观测量的误差的协方差矩阵，以式(12)表示。Qk+2·Ck是系统噪声。
式10
Figure BDA0001784850240000141
式11
Figure BDA0001784850240000151
式12
Figure BDA0001784850240000152
在式(12)中，σk vPLS表示观测部21计算出的车辆的速度的误差。另外，σk ωGYR表示观测部21计算出的车辆的角速度的误差。如上所述，σk vPLS是随着时间推移而在根据该速度求出的移动距离中累积的误差。另外，σk ωGYR与σk vPLS同样地，是随着时间而在根据该角速度求出的方位变化量中累积的误差。
如式(12)所示，在计算车辆状态预测值的误差协方差矩阵的式(9)中，基于来自车速传感器6的输出而由观测部21计算出的车辆的速度的误差作为车辆的位置的误差而被输入。即，将σk vPLS作为cos(θk|k)σk vPLST、以及sin(θk|k)σk vPLST输入。另外，基于来自陀螺仪传感器7的输出而由观测部21计算出的车辆的角速度的误差作为车辆的方位的误差而被输入。即，将σk ωGYR作为σk ωGYRT输入。
像这样，通过针对随着时间推移而累积的误差即σk vPLS和σk ωGYR而将σk vPLS作为车辆的位置的误差输入、将σk ωGYR作为车辆的方位的误差输入的卡尔曼滤波器，推定部22计算出车辆状态预测值的误差协方差矩阵。
在此，说明在卡尔曼滤波器中将σk vPLS作为车辆的位置的误差输入、将σk ωGYR作为车辆的方位的误差输入的情况。以下详细叙述在卡尔曼滤波器中将σk vPLS作为车辆的位置的误差输入的情况。
在此为便于说明，将车辆的状态量设为车辆位置的x坐标和车辆的速度来例示，说明在卡尔曼滤波器中将σk vPLS作为车辆的位置的误差输入的情况。在该例中，车辆位置的x坐标根据车辆的速度来确定。在该例中，观测对象是车辆的速度。
因此，针对车辆的状态量的状态方程式以式(13)表示。另外，针对观测对象的观测方程式以式(14)表示。
式13
Figure BDA0001784850240000161
式14
Figure BDA0001784850240000162
基于式(13)及式(14)，将基于到时刻k为止的信息而推定出的时刻k的车辆状态推定值的误差协方差矩阵基于式(4)、式(6)及式(8)计算出。在此，在计算车辆状态推定值的误差协方差矩阵时，使P(k|k)为式(15)。P(k|k)是基于到时刻k为止的信息而推定出的时刻k的车辆状态推定值的误差协方差矩阵。
式15
Figure BDA0001784850240000163
在式(15)中，p11 (k|k)表示车辆的位置的误差方差。另外p22 (k|k)表示车辆的速度的误差方差。另外，p12 (k|k)表示车辆的位置与车辆的速度的误差协方差。
另外，在计算车辆状态推定值的误差协方差矩阵时，使F、Q、H及R分别为式(16)～式(19)。在此，F是根据状态方程式求出的雅可比矩阵。另外，Q是系统噪声的方差矩阵。另外，H是根据观测方程式求出的雅可比矩阵。另外，R是观测噪声的方差矩阵。由于观测量是车辆的速度，所以rk是车辆的速度的误差。也就是说，rk 2表示由观测部21观测的车辆的速度的误差方差。
式16
Figure BDA0001784850240000171
式17
Figure BDA0001784850240000172
式18
H＝[0 1]…(18)
式19
R(k)＝[rk 2]…(19)
若基于式(15)～式(19)来计算车辆状态推定值的误差协方差矩阵、即P(k|k)，则车辆状态推定值的误差协方差矩阵以式(20)表示。
式20
Figure BDA0001784850240000173
因此，车辆的位置的推定值的误差方差以式(21)表示。
式21
Figure BDA0001784850240000174
式(21)若以作为共同因数的p11 (k|k-1)来加括号，则变形成式(22)。p11 (k|k-1)是基于到时刻k-1为止的信息得到的时刻k的车辆的位置的预测值的误差方差。
式22
Figure BDA0001784850240000181
如式(22)所示，作为车辆的位置的推定值的误差方差的p11 (k|k)是通过对作为车辆的位置的预测值的误差方差的p11 (k|k-1)乘以式(23)而求出的。
式23
Figure BDA0001784850240000182
如式(22)所示，式(23)是被乘以作为车辆的位置的预测值的方差的p11 (k|k-1)的系数。该系数若除去分母中包含车辆的速度的误差(rk)这一因素，则与式(24)一致。
式24
Figure BDA0001784850240000183
式(24)所示的系数是“1-(车辆的位置的误差与车辆的速度的误差的相关系数)2”。这表明式(24)所示的系数是使用了最小二乘法的线性回归模型的非确定系数。也就是说，车辆的位置的误差与车辆的速度的误差的相关性越小则该系数成为越大的值，车辆的位置的预测值的方差成为越大的值。另一方面，该相关性越大则该系数成为越小的值，车辆的位置的预测值的方差成为越小的值。另外，式(22)表示车辆的位置的推定值的方差相对于车辆的速度的误差而呈指数函数地减小。
因此，表示车辆的位置的推定值的误差方差是基于车辆的位置的误差与车辆的速度的误差的相关性而确定的。另外，也表示车辆的位置的推定值的误差不会因车辆的速度的误差而累积。也就是说，为了使作为车辆的位置的推定值的误差方差的p11 (k|k)随着车辆的速度的误差的累积而累积，需要减小车辆的位置的误差与车辆的速度的误差的相关性。也就是说，为了使p11 (k|k)累积，在卡尔曼滤波器中，需要减小车辆的位置的误差与车辆的速度的误差的相关性。具体地说，关于在移动距离累积的速度的误差，不是在卡尔曼滤波器中输入车辆的速度的误差来推定车辆的位置的误差，而是需要在卡尔曼滤波器中输入车辆的速度的误差来作为车辆的位置的误差，从而推定车辆的位置的误差。
因此，如式(12)所示，关于在移动距离累积的速度的误差，将车辆的速度的误差作为与车辆的速度具有微积分关系的车辆的位置的误差输入到卡尔曼滤波器。由此，能够仅与在移动距离累积的速度的误差相应地减小车辆的速度的误差与车辆的位置的误差的相关性，从而能够将累积的车辆的速度的误差作为累积的车辆的位置的误差来计算出车辆的位置的预测值的误差协方差矩阵。也就是说，能够计算出考虑了累积的车辆的速度的误差的、车辆的位置的预测值的误差协方差矩阵。
但是，在作为车辆的位置的误差而将车辆的速度的误差输入到卡尔曼滤波器时，无法直接使用式(4)。
在式(4)中，右边的第1项是通过根据状态方程式求出的雅可比矩阵将上次推定出的车辆状态推定值的误差协方差矩阵转换成这次的车辆的状态量的误差协方差矩阵。另一方面，右边的第2项是系统噪声的误差协方差矩阵。也就是说，式(4)是通过对将上次的车辆状态推定值的误差协方差矩阵转换成这次的车辆状态推定值的误差协方差矩阵而得到的算式加上系统噪声的误差协方差矩阵来计算出车辆状态预测值的误差协方差矩阵的算式。在此式(4)使用了式(25)所示的命题。
式25
Var(X+Y)＝Var(X)+Var(Y)…(25)
式(25)是表示在随机变量X和随机变量Y相互独立的情况下针对方差Var()成立的算式。也就是说，在卡尔曼滤波器中计算车辆状态预测值的误差协方差矩阵时使用的式(4)的前提在于，车辆的状态量的误差协方差矩阵和系统噪声的误差协方差矩阵相互独立。但是，在将系统噪声的误差协方差矩阵作为累积的误差的误差协方差矩阵的情况下，在式(4)中，无法准确地计算出车辆状态预测值的误差协方差矩阵。这是因为车辆的状态量的误差协方差矩阵、和作为车辆的状态量而输入的累积的误差的误差协方差矩阵不是独立的。例如，由于将车辆的位置的误差确定为随着时间推移而累积的速度的误差，所以车辆的位置的误差和车辆的速度的误差是随着一方变化而另一方也会变化的变量、即相互不独立的变量。因此，需要使用在随机变量X和随机变量Y不独立的情况下成立的命题。该命题通过式(26)表示。
式26
Var(X+Y)＝Var(X)+Var(Y)+2Cov(X，Y)…(26)
在式(26)中，Cov()表示协方差。因此，若与式(24)相比，则式(26)输入了随机变量X与随机变量Y的协方差。
通过使用遵照式(26)的式(9)，而能够消除车辆的状态量的误差协方差矩阵与累积的误差的误差协方差矩阵的独立性，准确地计算出车辆状态预测值的误差协方差矩阵。
以上说明了将车辆的速度的误差作为车辆的位置的误差输入到卡尔曼滤波器的情况。针对将角速度的误差作为方位的误差输入到卡尔曼滤波器的情况的说明，也能够同样地说明。
如以上说明那样，作为与观测量具有微积分的关系的状态量的误差而输入了观测量的误差，另外通过观测量的误差与车辆的状态量的误差没有独立性的算式，能够计算出考虑了累积的误差的、车辆状态预测值的误差协方差矩阵。
返回到图3所示的流程图的说明，推定部22在执行预测处理时，会执行推定处理。推定部22基于式(5)～式(8)，计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵。
如上所述，卡尔曼增益Kk是根据观测噪声的误差协方差矩阵、和车辆预测值的误差协方差矩阵来恰当地设定车辆状态推定值和车辆状态推定值的误差协方差矩阵的系数。在预测处理中，推定部22能够计算出考虑了累积的误差的车辆状态预测值的误差协方差矩阵，因此能够准确地计算出卡尔曼增益Kk。因此，推定部22能够高精度地计算出车辆状态推定值和车辆状态推定值的误差。
另外，如式(8)所示，车辆状态推定值的误差协方差矩阵是对车辆状态预测值的误差协方差矩阵乘以(I-KkH)而计算出的。因此，由于车辆状态预测值的误差协方差矩阵是考虑了累积的误差的误差协方差矩阵，所以计算出的车辆状态推定值的误差协方差矩阵也是考虑了累积的误差的误差协方差矩阵。
像这样，推定部22能够计算出考虑了累积的误差的车辆状态预测值的误差协方差矩阵，所以能够准确地计算出卡尔曼增益Kk，从而能够通过卡尔曼滤波器高精度地推定车辆的状态。
另外，由于是通过卡尔曼滤波器来推定车辆的状态，所以与基于将误差作为推定对象的卡尔曼滤波器来推定车辆的状态的构成相比，无需相对于误差另外推定车辆的状态，从而能够容易推定考虑了累积的误差的车辆的状态。
此外，在作为状态量而推定车辆的速度的情况下，推定部22也可以构成为，基于将车辆的加速度的误差作为车辆的速度的误差而输入的卡尔曼滤波器来计算出车辆的速度的预测值的误差协方差矩阵，推定车辆的速度。由此，由于会计算出考虑了累积的加速度的误差的车辆的速度的预测值的误差协方差矩阵，所以能够准确地计算出卡尔曼增益Kk，从而能够通过卡尔曼滤波器高精度地推定车辆的速度。
如上所述，推定部22构成为，基于由GPS接收部5输出的表示车辆的位置的信息和表示车辆的方位的信息，并考虑由观测部21观测到的车辆的位置和车辆的方位，来计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵，从而推定车辆的状态。但是，并不限定于构成为由推定部22考虑由观测部21观测的车辆的位置和车辆的方位来推定车辆的状态。即，推定部22也可以不基于来自GPS接收部5的输出地推定车辆的状态。
例如，在车辆处于无法通过GPS接收部5接收GPS电波的环境的情况下，推定部22根据没有基于来自GPS接收部5的输出得到的车辆的位置及车辆的方位的成分的观测方程式来推定车辆的状态量。
如式(12)所示，推定部22在计算车辆状态预测值的误差协方差矩阵时，基于从车速传感器6及陀螺仪传感器7输出的信号的间隔，计算出车辆状态预测值的误差协方差矩阵。另外，如式(3)所示，推定部22基于上次推定出的车辆状态推定值和该间隔来计算出车辆状态预测值。因此，推定部22在计算车辆状态预测值、以及车辆状态预测值的误差协方差矩阵时，不依赖于GPS接收部5的接收间隔。因此，推定部22能够通过在式(5)中依据没有基于来自GPS接收部5的输出得到观测对象的成分的算式，而计算出车辆状态推定值和车辆状态推定值的误差协方差矩阵。
也就是说，推定部22能够不依赖于GPS接收部5的GPS电波的接收间隔地推定车辆的状态。这表明推定部22是基于车速传感器6、陀螺仪传感器7及加速度传感器8的检测间隔来推定车辆的状态的。因此，推定部22在车速传感器6、陀螺仪传感器7以及加速度传感器8的检测间隔比GPS接收部5的接收间隔短的情况下起到以下效果。即，构成为基于该接收间隔并通过卡尔曼滤波器推定车辆的状态，由此能够以高频率执行车辆状态的推定，另外，能够不依赖于GPS接收部5的接收环境地推定车辆的状态。
如上所述，导航装置1的控制部2基于推定出的车辆状态，执行成为地图匹配对象的链路的评价。
图4是为了说明基于推定出的车辆状态进行的地图匹配的处理而示出了地图上的道路R1和道路R2的图。在图4中，道路R1是朝向方向Y1延伸的道路。另外，道路R2是朝向不与方向Y1并行的方向Y2延伸的道路。
在图4中，链路L1是与道路R1相对应的链路。另外，链路L2是与道路R2相对应的链路。
另外，在图4中，标记α是表示由推定部22推定出的车辆的位置的标记。以下在使用了图4的说明中，假设由推定部22推定出车辆位于位置M1、且正在朝向行进方向X1行驶。如图4所示，位置M1是道路R1上的位置，是从道路R1的宽度方向的中心朝向方向Y1而向右侧偏离的位置。
在道路R1、道路R2与推定出的车辆的位置之间的关系为图4所示的关系的情况下，在进行地图匹配时，首先，控制部2参照地图数据3a，获取成为地图匹配的候选的链路即地图匹配候选链路。控制部2作为地图匹配候选链路而获取位于距推定出的车辆的位置预先设定的规定范围内、且车辆与链路之间的方位的误差在规定范围内的一条或多条链路。在图4的例子中，链路L1以及链路L2是位于距推定出的车辆的位置即位置M1规定范围内、另外车辆与链路之间的方位的误差在规定范围内的链路。因此，控制部2获取链路L1以及链路L2来作为地图匹配候选链路。
接着，控制部2分别针对获取到的地图匹配候选链路，计算出对链路进行评价的评价量。
评价量是基于推定出的车辆的位置与地图匹配候选链路的位置之间的误差、以及推定出的车辆的方位与地图匹配候选链路的方位之间的误差并通过下述所示的式(27)计算出的值。
τ＝δx2/Δx2+δy2/Δy2+δθ2/Δθ2…(27)
在此，推定出的车辆的位置与地图匹配候选链路的位置之间的误差是指下述交点与推定出的车辆的位置在x轴方向以及y轴方向上的差，所述交点是指在从推定出的车辆的位置向地图匹配候选链路引出垂线的情况下的垂线与地图匹配候选链路之间的交点。在式(27)中，δx表示该交点的x坐标与推定出的车辆的位置的x坐标之差。另外，6y表示该交点的y坐标与推定出的车辆的位置的y坐标之差。
在图4的例子中，推定出的车辆的位置与链路L1之间的位置误差是指下述交点MM1与位置M1在x轴方向以及y轴方向上的差，所述交点MM1是从位置M1相对于链路L1引出的垂线S1与链路L1之间的交点。另外，在图4中，推定出的车辆的位置与链路L2之间的位置误差是指下述交点MM2与位置M1在x轴方向以及y轴方向上的差，所述交点MM2是从位置M1相对于链路L2引出的垂线S2与链路L2之间的交点。
另外，推定出的车辆的方位与地图匹配候选链路的方位之间的误差是指推定出的车辆的方位所对应的角度与地图匹配候选链路的方位所对应的角度之差。
如上所述，车辆的方位表示车辆的行进方向的方位。在图4的例子中，推定出的车辆的方位是行进方向X1的方位。另外，与车辆的方位相对应的角度表示以向东的方向为基准而在向东的方向与车辆的方位之间的逆时针旋转的相离角度。在图4的例子中，在假想直线K1为沿东西方向延伸的假想直线的情况下，与推定出的车辆的方位相对应的角度为角度θ1。
另外，链路的方位表示链路延伸的方向的方位。链路延伸的方向表示沿着链路的两个方向中的、在与链路相对应的道路中车辆能够行驶的方向所对应的方向。另外，与链路的方位相对应的角度表示以向东的方向为基准而在向东的方向与链路的方位之间的逆时针旋转的相离角度。
在图4的例子中，链路L1的方位是方向Z1的方位。另外，在假想直线K2为沿东西方向延伸的假想直线的情况下，与链路L1的方位相对应的角度为角度θ2。另外，在图4的例子中，链路L2的方位是方向Z2的方位。另外，在假想直线K3为沿东西方向延伸的假想直线的情况下，与链路L2的方位相对应的角度为角度θ3。
在图4的例子中，车辆与链路L1的方位误差是角度θ1与角度θ2之差。另外，车辆与链路L2的方位误差是角度θ1与角度θ3之差。也就是说，在图4的例子中，在式(27)中，δθ表示角度θ1与角度θ2之差，或者表示角度θ1与角度θ3之差。
如式(27)所示，评价量τ通过如下这些项的和而计算出，这些项为：δx的平方的值除以Δx的平方的值的项、δy的平方的值除以Δy的平方的值的项、δθ的平方的值除以Δθ的平方的值的项。Δx表示由推定部22推定出的车辆的x轴方向的位置的误差。也就是说，Δx2表示由推定部22推定出的车辆的x轴方向的位置的方差。另外，Δy表示由推定部22推定出的车辆的y轴方向的位置的误差。也就是说，Δy2表示由推定部22推定出的车辆的y轴方向的位置的方差。另外，Δθ表示由推定部22推定出的车辆的方位的误差。也就是说，Δθ2表示由推定部22推定出的车辆的方位的方差。Δx2、Δy2以及Δθ2是由推定部22推定出的车辆状态推定值的误差协方差矩阵所包含的方差。像这样，评价量是通过如下这些值的和而计算出的值，这些值是：通过推定出的车辆的位置的误差对车辆的位置与链路的位置之间的误差进行了无量纲化(dimensionless)后的值、和通过推定出的车辆的位置的误差对车辆的方位与链路的方位之间的误差进行了无量纲化后的值。
在图4的例子中，控制部2计算出作为地图匹配候选链路而获取到的链路L1的评价量和链路L2的评价量。
在将链路L1的评价量设为τ1、将位置M1与交点MM1的x坐标之差设为δx1、将位置M1与交点MM1的y坐标之差设为δy1、将角度θ1与角度θ2之差设为δθ1的情况下，评价量τ1以下述所示的式(28)表示。
τ1＝δx12/Δx2+δy12/Δy2+δθ12/Δθ2…(28)
另一方面，在将链路L2的评价量设为τ2、将位置M1与交点MM2的x坐标之差设为δx2、将位置M1与交点MM2的y坐标之差设为δy2、将角度θ1与角度θ3之差设为δθ2的情况下，评价量τ2以下述所示的式(29)表示。
τ2＝δx22/Δx2+δy22/Δy2+δθ22/Δθ2…(29)
在图4的例子中，距离I1比距离I2小。即，位置M1与交点MM2中的x坐标及y坐标之差比位置M1与交点MM2中的x坐标及y坐标之差小。另外，在图4的例子中，角度θ1与角度θ2之差比角度θ1与角度θ3之差小。因此，链路L1的评价量即τ1的值比链路L2的评价量即τ2的值小。因此，控制部2将评价量小的链路L1确定为与车辆的当前位置建立对应的链路。
像这样，控制部2基于推定出的车辆的状态，执行成为地图匹配的对象的链路的评价。如上所述，控制部2在评价链路时，基于由推定部22推定出的车辆状态推定值的误差协方差矩阵所包含的方差，来评价链路。该误差协方差矩阵是基于考虑累积的误差而计算出的车辆状态预测值的误差协方差矩阵来计算出的矩阵，是包含高精度地计算出的误差的矩阵。因此，控制部2通过将该误差协方差矩阵所包含的方差用于链路的评价，而能够准确地评价链路。
如以上所说明那样，导航装置1(车载装置)具有观测与车辆的变动相关的观测量的观测部21、和通过卡尔曼滤波器来推定表示车辆的状态的状态量的推定部22。推定部22计算出车辆的状态量的预测值，通过作为与观测量具有微积分关系的状态量的误差而输入了观测量的误差的卡尔曼滤波器，计算出预测值的误差协方差矩阵(误差)，基于计算出的预测值和预测值的误差协方差矩阵，通过卡尔曼滤波器计算出车辆的状态量的推定值和推定值的误差协方差矩阵。
由此，通过将观测量的误差作为具有微积分的关系的状态量的误差输入，而能够减小观测量的误差与状态量的误差的相关性，从而考虑累积的观测量的误差而计算出车辆状态预测值的误差协方差矩阵，因此能够准确地计算出卡尔曼增益Kk，从而能够通过卡尔曼滤波器高精度地推定车辆的状态。
另外，推定部22通过输入了上次计算出的车辆状态推定值的误差、与作为车辆的状态量的误差而输入的观测量的误差的协方差的卡尔曼滤波器，计算出预测值的误差协方差矩阵。
如上所述，式(4)使用了以状态量的误差协方差矩阵和系统噪声的误差协方差矩阵相互独立为前提的命题。但是，在将系统噪声的误差协方差矩阵作为累积的误差的误差协方差矩阵的情况下，车辆的状态量的误差协方差矩阵和累积的误差的误差协方差矩阵不是独立的。于是，通过使用遵照式(26)的式(9)、即输入了上次推定出的车辆状态推定值的误差与作为状态量的误差而输入的观测量的误差的协方差的算式，而能够消除车辆的状态量的误差协方差矩阵与累积的误差的误差协方差矩阵的独立性，从而准确地计算出车辆状态预测值的误差协方差矩阵。
另外，观测部21基于来自检测车辆的速度的车速传感器6的输出，观测车辆的速度。推定部22通过作为车辆的位置的误差而输入了车辆的速度的误差的卡尔曼滤波器，计算出预测值的误差协方差矩阵。
由此，通过减小车辆的速度的误差与车辆的位置的误差的相关性，而能够考虑累积的车辆的速度的误差，从而通过卡尔曼滤波器高精度地推定车辆的位置。
另外，观测部21基于来自检测车辆的角速度的陀螺仪传感器7(角速度传感器)的输出，观测车辆的角速度。推定部22通过作为车辆的方位的误差而输入了车辆的角速度的误差的卡尔曼滤波器，计算出预测值的误差协方差矩阵。
由此，通过减小车辆的角速度的误差与车辆的方位的误差的相关性，而能够考虑累积的车辆的角速度的误差，从而能够通过卡尔曼滤波器来高精度地推定车辆的方位。
观测部21基于来自检测车辆的加速度的加速度传感器8的输出，观测车辆的加速度。推定部22通过作为车辆的速度的误差而输入了车辆的加速度的误差的卡尔曼滤波器，计算出预测值的误差协方差矩阵。
由此，通过减小车辆的加速度的误差与车辆的速度的误差的相关性，而能够考虑累积的车辆的加速度的误差，从而能够通过卡尔曼滤波器高精度地推定车辆的速度。
上述实施方式原则上只是例示了本发明的一个方式，在不脱离本发明的主旨的范围内能够任意变形及应用。
例如，为了容易理解本申请发明，图1是根据主要的处理内容来对导航装置1的功能结构进行分类而示出的概略图，导航装置1的结构也能够根据处理内容而分类成更多的结构要素。另外，也能够以一个结构要素执行更多的处理的方式进行分类。
另外，例如，为了容易理解控制部2的处理，根据主要的处理内容对图2以及图3的流程图的处理单位进行了分割，但并不会根据处理单位的分割方法和名称来限定本发明。也可以根据处理内容将控制部2的处理分割为更多的处理单位。另外，也可以以一个处理单位包含更多的处理的方式进行分割。
另外，例如在上述实施方式中，作为导航装置1而例示了车载型，但导航装置1的形态是任意的，例如也可以是行人携带的便携式装置。
附图标记说明
1 导航装置(车载装置)
6 车速传感器(传感器)
7 陀螺仪传感器(传感器)
8 加速度传感器(传感器)
21 观测部
22 推定部

Claims (4)

1.一种车载装置，搭载于车辆，所述车载装置的特征在于，具有：
观测部，其基于来自传感器的输出而观测与所述车辆的速度、角速度及加速度的变动相关的观测量，并计算所述车辆的速度的误差、车辆的角速度的误差及车辆的加速度的误差；和
推定部，其使用卡尔曼滤波器，通过下述式(1)表示的状态方程式，推定所述车辆的位置的x坐标x及y坐标y、方位θ、速度v、以及角速度ω来作为表示所述车辆的状态的状态量，
Figure FDA0003272187660000011
所述推定部计算出所述车辆的所述状态量的预测值，
通过将对某时刻下的作为所述观测量的误差的所述车辆的速度的误差、所述车辆的角速度的误差及所述车辆的加速度的误差以时间积分得到的值输入到所述卡尔曼滤波器中的下述式(2)，计算出所述预测值的误差Pk+1|k，并根据所计算出的所述预测值的误差计算出卡尔曼增益，
Pk+1|k＝FkPk|kFT k+Qk+2·Ck...(2)
在所述式(2)中，Fk是根据关于表示所述车辆的状态的状态量的状态方程式求出的雅可比矩阵，Pk|k是基于到时刻k为止的信息而推定出的时刻k的误差协方差矩阵，F表示根据关于车辆的状态量的所述式(1)的状态方程式求出的雅可比矩阵，F中的上标的T表示转置矩阵，Qk表示系统噪声的误差协方差矩阵，Ck是上次推定出的车辆状态推定值的误差与所述观测部观测的观测量的误差的协方差矩阵，Qk+2·Ck是系统噪声，
基于计算出的所述卡尔曼增益和所述预测值，通过所述卡尔曼滤波器来计算出所述车辆的所述状态量的推定值和所述推定值的误差。
2.如权利要求1所述的车载装置，其特征在于，
所述推定部通过输入了上次计算出的所述状态量的所述推定值的误差与作为所述车辆的位置的误差的所述车辆的速度的误差、作为所述车辆的方位的误差的所述车辆的角速度的误差、以及作为所述车辆的速度的误差的所述车辆的加速度的误差的协方差的所述卡尔曼滤波器，计算出所述预测值的误差。
3.一种推定方法，通过卡尔曼滤波器推定表示车辆的状态的状态量，所述推定方法的特征在于，
计算出所述车辆的所述状态量的预测值，
对与基于来自传感器的输出而观测到的所述车辆的速度、角速度及加速度的变动相关的观测量的误差进行观测，计算出所述车辆的速度和所述车辆的速度的误差、所述车辆的角速度和所述车辆的角速度的误差、以及所述车辆的加速度和所述车辆的加速度的误差，使用卡尔曼滤波器，通过下述式(1)表示的状态方程式，推定所述车辆的位置的x坐标x及y坐标y、方位θ、速度v、以及角速度ω来作为表示所述车辆的状态的状态量，
Figure FDA0003272187660000031
将对观测到的与所述车辆的速度、角速度及加速度的变动相关的观测量的误差、即某时刻下的作为所述观测量的误差的所述车辆的速度的误差、所述车辆的角速度的误差及所述车辆的加速度的误差以时间积分得到的值输入到所述卡尔曼滤波器中的下述式(2)，由此计算出所述预测值的误差Pk+1|k，并根据所计算出的所述预测值的误差计算出卡尔曼增益，
Pk+1|k＝FkPk|kFT k+Qk+2·Ck...(2)
在所述式(2)中，Fk是根据关于表示所述车辆的状态的状态量的状态方程式求出的雅可比矩阵，Pk|k是基于到时刻k为止的信息而推定出的时刻k的误差协方差矩阵，F表示根据关于车辆的状态量的所述式(1)的状态方程式求出的雅可比矩阵，F中的上标的T表示转置矩阵，Qk表示系统噪声的误差协方差矩阵，Ck是上次推定出的车辆状态推定值的误差与所述观测部观测的观测量的误差的协方差矩阵，Qk+2·Ck是系统噪声，
基于计算出的所述卡尔曼增益和所述预测值，通过所述卡尔曼滤波器来计算出所述车辆的所述状态量的推定值和所述推定值的误差。
4.如权利要求3所述的推定方法，其特征在于，
通过输入了上次计算出的所述状态量的所述推定值的误差与作为所述车辆的位置的误差的所述车辆的速度的误差、作为所述车辆的方位的误差的所述车辆的角速度的误差、以及作为所述车辆的速度的误差的所述车辆的加速度的误差的协方差的所述卡尔曼滤波器，计算出所述预测值的误差。
CN201780014609.6A 2016-03-01 2017-02-08 车载装置及推定方法 Active CN108700423B (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2016038740A JP6677533B2 (ja) 2016-03-01 2016-03-01 車載装置、及び、推定方法
JP2016-038740 2016-03-01
PCT/JP2017/004510 WO2017150106A1 (ja) 2016-03-01 2017-02-08 車載装置、及び、推定方法

Publications (2)

Publication Number Publication Date
CN108700423A CN108700423A (zh) 2018-10-23
CN108700423B true CN108700423B (zh) 2022-02-01

Family

ID=59743868

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201780014609.6A Active CN108700423B (zh) 2016-03-01 2017-02-08 车载装置及推定方法

Country Status (5)

Country Link
US (1) US11036231B2 (zh)
EP (1) EP3425338A4 (zh)
JP (1) JP6677533B2 (zh)
CN (1) CN108700423B (zh)
WO (1) WO2017150106A1 (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11136040B2 (en) * 2018-05-21 2021-10-05 Deere & Company IMU based traction control for wheeled or tracked machine
JP6581276B1 (ja) * 2018-10-18 2019-09-25 株式会社ショーワ 状態量推定装置、制御装置、および状態量推定方法
WO2020157844A1 (ja) * 2019-01-30 2020-08-06 三菱電機株式会社 計測装置、計測方法及び計測プログラム
CN113631883B (zh) * 2019-04-04 2024-04-30 三菱电机株式会社 车辆定位装置
JP7272910B2 (ja) * 2019-09-03 2023-05-12 株式会社ゼンリンデータコム 車載装置、状態推定方法及びプログラム
CN110729982B (zh) * 2019-09-30 2023-03-10 中国船舶重工集团公司第七0七研究所 一种基于矩阵稀疏性的Kalman滤波算法优化的方法
CN111881955B (zh) * 2020-07-15 2023-07-04 北京经纬恒润科技股份有限公司 多源传感器信息融合方法及装置
EP4060614A1 (en) * 2021-03-19 2022-09-21 Aptiv Technologies Limited Method for determining noise statistics of object sensors
CN113436442B (zh) * 2021-06-29 2022-04-08 西安电子科技大学 一种利用多地磁传感器的车速估计方法
CN113792265B (zh) * 2021-09-10 2024-09-17 中国第一汽车股份有限公司 一种坡度估计方法、装置、电子设备以及存储介质
CN114018250B (zh) * 2021-10-18 2024-05-03 杭州鸿泉物联网技术股份有限公司 惯性导航方法、电子设备、存储介质和计算机程序产品
CN114689079A (zh) * 2022-03-22 2022-07-01 南斗六星系统集成有限公司 一种车辆状态判断方法、装置、设备及可读存储介质
CN115859039B (zh) * 2023-03-01 2023-05-23 南京信息工程大学 一种车辆状态估计方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102410837A (zh) * 2011-07-29 2012-04-11 江苏大学 一种车辆组合定位导航系统及方法
CN102519470A (zh) * 2011-12-09 2012-06-27 南京航空航天大学 多级嵌入式组合导航系统及导航方法
JP2013129289A (ja) * 2011-12-21 2013-07-04 Toyota Motor Corp 車両状態量推定装置、車両用操舵制御装置
WO2015162873A1 (ja) * 2014-04-25 2015-10-29 日本電気株式会社 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000055678A (ja) 1998-08-04 2000-02-25 Denso Corp 車両用現在位置検出装置
JP4145451B2 (ja) * 1999-12-17 2008-09-03 古野電気株式会社 ハイブリッド航法およびその装置
JP4229141B2 (ja) * 2006-06-19 2009-02-25 トヨタ自動車株式会社 車両状態量推定装置及びその装置を用いた車両操舵制御装置
JP5011904B2 (ja) * 2006-09-19 2012-08-29 三菱電機株式会社 追尾方法及びその装置
JP4781300B2 (ja) * 2007-03-01 2011-09-28 アルパイン株式会社 位置検出装置および位置検出方法
JP2010117148A (ja) * 2008-11-11 2010-05-27 Seiko Epson Corp 位置算出方法及び位置算出装置
US8224519B2 (en) 2009-07-24 2012-07-17 Harley-Davidson Motor Company Group, LLC Vehicle calibration using data collected during normal operating conditions
JP5071533B2 (ja) * 2010-05-19 2012-11-14 株式会社デンソー 車両用現在位置検出装置
JP5610847B2 (ja) * 2010-05-26 2014-10-22 三菱電機株式会社 角速度推定装置及びコンピュータプログラム及び角速度推定方法
JP6143474B2 (ja) * 2013-01-24 2017-06-07 クラリオン株式会社 位置検出装置およびプログラム
DK3136488T3 (da) * 2013-07-02 2020-01-06 Asahi Chemical Ind Elektrolytopløsning og fremgangsmåder til fremstilling af elektrolytmembran, elektrodekatalysatorlag, membran-elektrodeanordning og brændselscelle

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102410837A (zh) * 2011-07-29 2012-04-11 江苏大学 一种车辆组合定位导航系统及方法
CN102519470A (zh) * 2011-12-09 2012-06-27 南京航空航天大学 多级嵌入式组合导航系统及导航方法
JP2013129289A (ja) * 2011-12-21 2013-07-04 Toyota Motor Corp 車両状態量推定装置、車両用操舵制御装置
WO2015162873A1 (ja) * 2014-04-25 2015-10-29 日本電気株式会社 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法

Also Published As

Publication number Publication date
WO2017150106A1 (ja) 2017-09-08
US11036231B2 (en) 2021-06-15
CN108700423A (zh) 2018-10-23
US20190041863A1 (en) 2019-02-07
JP2017156186A (ja) 2017-09-07
EP3425338A1 (en) 2019-01-09
EP3425338A4 (en) 2019-10-30
JP6677533B2 (ja) 2020-04-08

Similar Documents

Publication Publication Date Title
CN108700423B (zh) 车载装置及推定方法
US10620320B2 (en) Position estimation system and estimation method
EP2045577B1 (en) Positioning device, and navigation system
EP2664894B1 (en) Navigation apparatus
Jo et al. GPS-bias correction for precise localization of autonomous vehicles
WO2019188745A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
CN102914785B (zh) 基于卫星定位数据和车辆传感器数据的车辆导航
JP5419665B2 (ja) 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム
CN106558219B (zh) 车辆轨迹预测方法及装置
JP7272910B2 (ja) 車載装置、状態推定方法及びプログラム
US9423486B2 (en) Position calculating method and position calculating device
JP2004239643A (ja) ハイブリッド航法装置
WO2018062291A1 (ja) 他車線監視装置
JP2024105508A (ja) 出力装置、制御方法、プログラム及び記憶媒体
WO2018212301A1 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2007333652A (ja) 測位装置、ナビゲーションシステム
JP4884109B2 (ja) 移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法
JP2014089047A (ja) 測位装置、測位方法、および測位プログラム
JP5569276B2 (ja) 車両用現在位置検出装置
TWI587155B (zh) Navigation and Location Method and System Using Genetic Algorithm
JP6413816B2 (ja) 漫然運転判定装置
CN113432599A (zh) 一种载具导航方法
Roysdon et al. Enhanced State Estimation for Wheeled Vehicles
CN118226771A (zh) 用于控制自动化车辆的方法、控制设备和存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant