CN107305123B - 位置推断装置以及推断方法 - Google Patents
位置推断装置以及推断方法 Download PDFInfo
- Publication number
- CN107305123B CN107305123B CN201710254076.0A CN201710254076A CN107305123B CN 107305123 B CN107305123 B CN 107305123B CN 201710254076 A CN201710254076 A CN 201710254076A CN 107305123 B CN107305123 B CN 107305123B
- Authority
- CN
- China
- Prior art keywords
- error
- gps
- vehicle
- observation
- influence
- 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
Links
Images
Classifications
-
- 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/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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明提供位置推断装置以及推断方法,目的在于,能够通过卡尔曼滤波器高精度地推断与当前所在地相关的状态。导航装置具有:接收GPS信号的GPS接收部;对观测量进行观测的观测部,观测量含有基于接收到的GPS信号的GPS车辆位置;推断部,基于观测部观测的观测量和卡尔曼滤波器,推断与当前所在地相关的状态量,推断部计算状态量的预测值和预测值的误差;基于预测值、预测值的误差和观测部观测的观测量的误差,计算状态量的推断值和推断值的误差;在计算推断值和推断值的误差时,对GPS车辆位置的误差赋予基于从第一定时到第二定时为止的期间的加权,在第二定时,在第一定时接收到的GPS信号不再反映到观测部对GPS车辆位置的观测中。
Description
技术领域
本发明涉及位置推断装置以及推断方法。
背景技术
作为本技术领域的背景技术,有JP特开2012-215491号公报(专利文献1)。在专利文献1中有这样的记载:“在具有GPS(Global Positioning System:全球定位系统)单元3和INS(注册商标,Inertial Navigation System:惯性导航系统)单元5的位置计算装置1中,位置计算精度判断部6判断位置计算精度。影响程度设定部7基于由位置计算精度判断部6判断出的位置计算精度,设定GPS计测结果相对于INS计测结果的影响程度。耦合处理部9基于由影响程度设定部7设定的影响程度,对GPS计测结果与INS计测结果进行耦合处理,来计算移动体的位置。”。
现有技术文献
专利文献
专利文献1:JP特开2012-215491号公报
发明内容
发明要解决的问题
通常,基于卡尔曼滤波器来推断与位置、方位等当前所在地相关的状态时,前提在于,基于传感器、GPS信号等观测到的位置、方位等的观测量的误差是误差平均值为零的白噪声性的误差。但是,基于GPS信号观测到的位置的误差,有时是误差平均值不为零的有色噪声性的误差。在专利文献1中,没有考虑到基于GPS信号观测到的位置的误差是有色噪声性的误差这一情况,无法通过卡尔曼滤波器高精度地推断与当前所在地相关的状态。
因此,本发明的目的在于,能够通过卡尔曼滤波器高精度地推断与当前所在地相关的状态。
用于解决问题的方法
为了达到上述目的,位置推断装置的特征在于,具有:GPS接收部,其接收GPS信号;观测部,其对观测量进行观测,所述观测量含有基于由所述GPS接收部接收到的所述GPS信号的GPS测位位置;以及推断部,其基于由所述观测部观测的所述观测量和卡尔曼滤波器,推断与当前所在地相关的状态量,所述推断部计算所述状态量的预测值和所述预测值的误差;所述推断部基于所述预测值、所述预测值的误差、由所述观测部观测的所述观测量的误差,计算所述状态量的推断值和所述推断值的误差;所述推断部在计算所述推断值和所述推断值的误差时,对所述GPS测位位置的误差赋予基于从第一定时到第二定时为止的期间的加权,在所述第二定时,在所述第一定时接收到的所述GPS信号不再反映到基于所述观测部对所述GPS测位位置的观测中。
发明效果
根据本发明,能够通过卡尔曼滤波器高精度地推断与当前所在地相关的状态。
附图说明
图1是表示导航装置的结构的框图。
图2是表示导航装置的动作的流程图。
图3是表示车辆状态推断处理中的推断部的动作的流程图。
图4是用于说明相关时间常数的图。
图 5是用于说明基于所推断的车辆状态进行的地图匹配(map-matching)的处理的图。
附图标记说明
1 导航装置(位置推断装置、车载装置)
5 GPS接收部
21 观测部
22 推断部
具体实施方式
图1是表示导航装置1(位置推断装置、车载装置)的结构的框图。
导航装置1是搭载于车辆的车载装置,按照搭乘于车辆的用户的操作,来执行地图显示、地图中车辆的当前位置显示、到达目的地为止的路径搜索、路径引导等。此外,导航装置1可以固定在车辆的仪表板等上,也可以相对于车辆而可装拆。
如图1所示,导航装置1具有:控制部2、存储部3、触摸面板4、GPS接收部5、车速传感器6、陀螺仪传感器7、加速度传感器8。
控制部2具有CPU、ROM、RAM、其它控制回路等,用于控制导航装置1的各部分。控制部2通过执行存储在ROM、存储部3等中的控制程序,来作为后述的观测部21以及推断部22发挥功能。
存储部3具有硬盘、EEPROM等非易失性存储器,以可改写的方式存储数据。存储部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接收部5基于接收到的GPS信号,针对规定数以上的卫星,测定车辆与GPS卫星间的距离以及距离的变化率,由此至少计算出车辆位置(GPS测位位置)与车辆的行进方向的方位(以下,表现为“车辆方位”)。另外,GPS接收部5在计算车辆位置以及车辆方位时,在考虑接收到的GPS信号和前次计算出的车辆位置以及车辆方位的基础上,来进行计算。在以下的说明中,将基于GPS信号的车辆位置表现为GPS车辆位置。GPS接收部5在计算GPS车辆位置和车辆方位时,计算GPS车辆位置的误差和车辆方位的误差。GPS接收部5将表示GPS车辆位置的信息、表示GPS车辆位置的误差的信息、表示车辆方位的信息以及表示车辆方位的误差的信息,输出至控制部2。
车速传感器6检测车辆的车速,将表示检测出的车速的信号输出至控制部2。
陀螺仪传感器7例如由振动陀螺仪构成,检查车辆转弯的角速度。陀螺仪传感器7将表示检测出的角速度的信号输出至控制部2。
加速度传感器8检查作用于车辆上的加速度(例如,车辆相对于行进方向的倾斜)。加速度传感器8将表示检测出的加速度的信号输出至控制部2。
如图1所示,控制部2具有观测部21和推断部22。
观测部21基于从车速传感器6、陀螺仪传感器7以及加速度传感器8输出的信号和从GPS接收部5输出的信息,来观测与车辆的变动相关的观测量。
观测部21基于从车速传感器6输出的表示车速的信号,观测车辆速度来作为观测量。观测部21通过规定的运算,根据表示车速的信号,来计算车辆速度和车辆速度的误差。
另外,观测部21基于从陀螺仪传感器7输出的表示角速度的信号,观测车辆转弯的角速度来作为观测量。观测部21通过规定的运算,根据表示角速度的信号,来计算车辆的角速度和车辆的角速度的误差。
另外,观测部21基于从加速度传感器8输出的表示加速度的信号,观测车辆的加速度来作为观测量。观测部21通过规定的运算,根据表示加速度的信号,来计算车辆的加速度和车辆的加速度的误差。
另外,观测部21基于从GPS接收部5输出的表示GPS车辆位置的信息和表示车辆方位的信息,观测GPS车辆位置和车辆方位来作为观测量。
推断部22基于由观测部21观测的车辆速度、车辆的加速度、车辆的角速度、车辆位置以及车辆方位,通过卡尔曼滤波器,推断表示与车辆当前所在地相关的状态的状态量。在本实施方式中,推断部22将车辆速度、车辆的角速度、车辆位置以及车辆方位作为车辆的状态量进行推断。
控制部2基于由推断部22推断出的车辆的状态量,来执行成为地图匹配的对象的链路的评价,这将在后叙述。
此外,由推断部22推断的车辆速度相当于车辆的状态量。另外,由观测部21观测的车辆速度,相当于观测量。同样地,由推断部22推断的车辆的角速度,相当于车辆的状态量。另外,由观测部21观测的车辆的角速度,相当于观测量。
在此,说明基于卡尔曼滤波器的基本的车辆状态量的推断。
在本实施方式中,通过卡尔曼滤波器推断的车辆的状态量是车辆位置、车辆方位、车辆速度以及车辆的角速度。以下,示出通过卡尔曼滤波器推断的车辆的各状态量。
x:车辆位置的x坐标
y:车辆位置的y坐标
θ:车辆方位
v:车辆速度
ω:车辆的角速度
在此,如果将用矢量表示车辆的状态量的状态矢量设为(x,y,θ,v,ω),则基于车辆的状态量的状态方程式如式(1)所示。
式1
下标k+1以及k表示时刻。例如,式(1)左边的(xk+1,yk+1,θk+1,vk+1,ωk+1)表示时刻k+1的车辆的状态量。在式(1)中,右边的第2项即qk是系统噪声(平均0,具有误差协方差矩阵即Qk的正态分布N(0,Qk))。误差协方差矩阵是指方差与协方差的矩阵。
如前述,观测部21观测车辆速度、车辆的角速度、车辆的加速度、GPS车辆位置以及车辆方位,来作为观测量。如前述,观测部21针对车辆速度,基于来自车速传感器6的输出进行观测。另外,观测部21针对车辆的角速度,基于来自陀螺仪传感器7的输出进行观测。另外,观测部21针对车辆的加速度,基于来自加速度传感器8的输出进行观测。另外,观测部21基于来自GPS接收部5的输出,对GPS车辆位置以及车辆方位进行观测。以下,示出由观测部21观测的观测量。此外,以下作为观测量而例示出车辆速度、车辆的角速度、GPS车辆位置以及车辆方位。
vPLS:基于来自车速传感器6的输出而观测出的车辆速度
ωGYR:基于来自陀螺仪传感器7的输出而观测出的车辆的角速度
xGPS:基于来自GPS接收部5的输出而观测出的GPS车辆位置的x坐标
yGPS:基于来自GPS接收部5的输出而观测出的GPS车辆位置的y坐标
θGPS:基于来自GPS接收部5的输出而观测出的车辆方位
在此,如果将用矢量表示上述观测量的观测矢量设为(vPLS,ωGYR,xGPS,yGPS,θGPS),则针对观测量的观测方程式如式(2)所示。
式2
在式(2)中,rk是观测噪声(平均0,具有误差协方差矩阵即Rk的正态分布N(0,Rk))。
以下,针对卡尔曼滤波器,分为预测车辆的状态量的预测处理和推断车辆的状态量的推断处理来进行说明。
此外,在以下的说明中,标注有下标k+1|k的值,表示基于时刻k为止的信息而预测出的时刻k+1的预测值。另外,标注有下标k+1|k+1的值,表示基于时刻k+1为止的信息而推断出的时刻k+1的推断值。另外,标注有下标k|k的值,表示基于时刻k为止的信息而推断出的时刻k的推断值。
<预测处理>
在卡尔曼滤波器中,预测处理是对车辆的状态量的预测值(以下,表现为“车辆状态预测值”)、车辆状态预测值的误差协方差矩阵(预测值的误差)进行计算的处理。基于式(3)来计算出车辆状态预测值。此外,误差协方差矩阵的计算是表示误差协方差矩阵的各元素的值的计算。
式3
在式(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+1的车辆的状态量的情况下,计算基于时刻k为止的信息而计算出的时刻k+1的车辆状态预测值、基于时刻k为止的信息而计算出的时刻k+1的车辆状态预测值的误差协方差矩阵。即,预测处理用于预测车辆的状态量的概率分布。
<推断处理>
接着,说明推断处理。
在卡尔曼滤波器中,推断处理是如下的处理:基于通过预测处理计算出的车辆状态预测值以及车辆状态预测值的误差协方差矩阵,计算车辆的状态量的推断值(以下,表现为“车辆状态推断值”)和车辆状态推断值的误差协方差矩阵(推断值的误差)。
在推断处理中,通过式(5)计算出观测残差。观测残差是指,观测量与根据车辆状态预测值计算出的观测量所对应的值之间的差。
式5
在式(5)中,左边是用矢量表示观测残差的观测残差矢量。另外,在式(5)中,右边的第1项是观测部21在时刻k观测到的观测量的观测矢量。另外,在式(5)中,右边的第2项是对于在预测处理中预测出的车辆状态预测值乘以根据观测方程式求出的表示观测矩阵的“H”的项。
利用式(5)所示的观测残差,通过式(6)来计算车辆状态推断值。
式6
在式(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是观测噪声的误差协方差矩阵。在本实施方式中,观测噪声的误差协方差矩阵是指,表示观测量的误差的误差协方差矩阵。以下,将该误差协方差矩阵表现为观测量的误差协方差矩阵。如前述,是误差二次方的方差与协方差的矩阵。因此,Rk是观测量的误差二次方的方差和协方差的矩阵。即,观测量的误差协方差矩阵相当于观测量的误差。此外,在式(7)中,上标“-1”表示逆矩阵。
式(7)所示的卡尔曼增益Kk根据基于时刻k为止的信息得到的时刻k+1的车辆状态预测值的误差协方差矩阵(Pk+1|k)、以及在时刻k观测出的观测量的误差协方差矩阵(Rk)而计算出。
该卡尔曼增益Kk是如下的参数:在式(6)中,在计算车辆状态推断值时,决定是重视车辆状态预测值来进行计算,还是重视由观测部21观测的观测量来进行计算的参数。
例如,在基于来自车速传感器6、陀螺仪传感器7以及GPS接收部5的输出得到的观测量的误差比车辆状态预测值的误差足够小的情况下,由于观测量的误差足够小,因此优选车辆状态推断值成为观测量。即,在观测量的误差协方差矩阵即Rk的值比车辆状态预测值的误差协方差矩阵即Pk+1|k的值足够小的情况下,作为车辆状态预测值的式(6)的左边优选成为作为观测量的式(5)的右边第1项。这是由于,车辆状态预测值成为误差足够小的值,换言之,车辆状态预测值成为精度高的值。在此,当将Rk的值足够小的情况下的卡尔曼增益Kk设为Kk=H-1并赋予式(6)时,基于与式(5)之间的关系,则式(6)的右边的第1项会消失。即,车辆状态推断值成为误差足够小的观测量。
另外,例如,在车辆状态预测值的误差比基于来自车速传感器6、陀螺仪传感器7以及GPS接收部5的输出得到的观测量的误差足够小的情况下,优选车辆状态推断值成为车辆状态预测值。即,在车辆状态预测值的误差协方差矩阵即Pk+1|k的值比观测量的误差协方差矩阵即Rk的值足够小的情况下,优选作为车辆状态预测值的式(6)的左边成为作为车辆状态预测值的式(6)的右边第1项。这是由于,车辆状态推断值的误差比观测量的误差足够小,换言之,车辆状态推断值成为精度高的值。在此,作为车辆状态预测值的误差协方差矩阵比Rk的值足够小的情况下的卡尔曼增益Kk,将Kk=0赋予(6),则车辆状态推断值成为车辆状态预测值。
这样,卡尔曼增益Kk是这样的系数:根据观测量的误差协方差矩阵即Rk以及车辆状态预测值的误差协方差矩阵,以使得车辆状态推断值成为恰当的值的方式进行设定的系数。即,卡尔曼增益Kk是这样的系数:基于由观测部21观测的观测量的误差协方差矩阵与车辆状态预测值的误差协方差矩阵,以使得车辆状态推断值成为重视观测量与车辆状态预测值中的误差协方差矩阵小、即误差小的一方的值方式进行设定的系数。另外,就卡尔曼增益Kk而言,如果能够正确预测车辆状态预测值的误差协方差矩阵,换言之,在能够正确地计算该误差协方差矩阵的情况下,根据基于来自车速传感器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=H-1作为观测量的误差即Rk足够小的情况下的卡尔曼增益Kk来赋予式(8),则车辆状态推断值的误差协方差矩阵成为零矩阵。这表明,所推断出的车辆状态推断值的误差足够小。
另外,例如,赋予Kk=0来作为车辆状态预测值的误差比观测量的误差足够小的情况下的卡尔曼增益的值,从而使得车辆状态推断值的误差协方差矩阵变为车辆状态预测值的误差协方差矩阵。这表明,车辆状态推断值的误差是比观测量的误差足够小的车辆状态预测值的误差。
这样,在式(8)中,根据观测量的误差协方差矩阵和车辆状态预测值的误差协方差矩阵,以使得车辆状态推断值的误差协方差矩阵变恰当的方式设定卡尔曼增益Kk。即,基于观测量的精度和车辆状态预测值的精度,以使得车辆状态推断值的误差协方差矩阵变恰当的方式设定卡尔曼增益Kk。
如上述,在推断处理中,例如,在推断时刻k+1的车辆的状态量的情况下,计算基于时刻k+1为止的信息而计算出的时刻k+1的车辆状态推断值、以及基于时刻k+1为止的信息而计算出的时刻k+1的车辆状态推断值的误差协方差矩阵。即,在推断处理中,基于在预测处理中预测出的车辆的状态量的概率分布,来推断车辆的状态量的概率分布。
通过以上计算,推断部22利用卡尔曼滤波器来计算车辆状态推断值和车辆状态推断值的误差协方差矩阵,从而推断车辆状态。
如前述,卡尔曼增益Kk是恰当地设定车辆状态推断值和车辆状态推断值的误差协方差矩阵的系数。即,车辆状态的推断精度依赖于卡尔曼增益Kk。如前述,卡尔曼增益Kk是根据观测量的误差协方差矩阵和车辆状态预测值的误差协方差矩阵来恰当地设定车辆状态推断值与车辆状态推断值的误差协方差矩阵的系数。
然而,在卡尔曼滤波器中,前提在于,系统噪声以及观测噪声是白噪声。即,在卡尔曼滤波器中将误差作为噪声处理的情况下,前提在于,该误差是误差平均值为零的白噪声性的误差。因此,前提在于,在卡尔曼增益Kk的计算中采用的观测量的误差是白噪声性的误差。但是,在由观测部21观测的观测量中,至少GPS车辆位置的误差有时会因多路传播、GPS卫星的卫星配置等的影响而导致显现为误差平均值不为零的有色噪声性的误差。该情况下,由于观测量的误差是有色噪声性的误差,因此无法正确地计算卡尔曼增益Kk。这表明,无法高精度地计算车辆状态推断值与车辆状态推断值的误差协方差矩阵,即,无法高精度地推断车辆的状态。
因此,如下所示,本实施方式的推断部22设定GPS车辆位置的误差,基于卡尔曼滤波器来推断车辆状态。
以下,通过推断车辆状态时的导航装置1的动作,来说明由推断部22进行的车辆状态的推断。
图2是示出导航装置1的动作的流程图。
导航装置1的观测部21,基于从车速传感器6、陀螺仪传感器7以及加速度传感器8输出的信号,观测车辆速度、车辆的角速度以及车辆的加速度(步骤SA1)。每当车速传感器6、陀螺仪传感器7以及加速度传感器8输出信号时,观测部21观测车辆速度、车辆的角速度以及车辆的加速度。即,观测部21观测它们的间隔,是与车速传感器6、陀螺仪传感器7以及加速度传感器8进行检测的间隔相同的间隔。
接下来,观测部21基于GPS接收部5的输出,观测GPS车辆位置以及车辆方位(步骤SA2)。每当从GPS接收部5输出表示GPS车辆位置的信息和表示车辆方位的信息时,观测部21观测GPS车辆位置以及车辆方位。即,观测部21观测GPS车辆位置以及车辆方位的间隔,成为GPS接收部5接收GPS信号的间隔。
接下来,导航装置1的推断部22基于由观测部21观测到的观测量,执行推断车辆状态的车辆状态推断处理(步骤SA3)。
图3是表示车辆状态推断处理中的推断部22的动作的流程图。
推断部22执行预测处理(步骤SB1)。如前述,预测处理是计算车辆状态预测值以及车辆状态预测值的误差协方差矩阵的处理。通过式(3)来计算车辆状态预测值。通过式(4)来计算车辆状态预测值的误差协方差矩阵。
接下来,推断部22执行推断处理(步骤SB2)。推断部22在推断处理中,基于式(7)来计算卡尔曼增益Kk,基于计算出的卡尔曼增益Kk、式(5)、式(6)以及式(8),计算车辆状态推断值和车辆状态推断值的误差协方差矩阵。
推断部22在基于式(7)来计算卡尔曼增益Kk时,如式(9)所示那样设定观测量的误差协方差矩阵所包含的元素中的、GPS车辆位置的误差。此外,在以下的说明中,为了方便,将对观测量的误差协方差矩阵即Rk输入的GPS车辆位置的误差,表现为GPS车辆位置的观测误差;将观测部21基于来自GPS接收部5的输出而观测的GPS车辆位置的误差,表现为GPS车辆位置的测位误差。
式9
在式(9)中,GPS车辆位置的观测误差表现为,对GPS车辆位置的测位误差乘以的值。这表明,推断部22对GPS车辆位置的测位误差赋予基于τ的的加权,将赋予了加权的GPS车辆位置的测位误差设定为GPS车辆位置的观测误差。
在式(9)中,τ表示相关时间常数。相关时间常数表示如下期间,即,从某定时(第一定时)起到在该某定时接收到的GPS信号不反映到由观测部21观测的GPS车辆位置的观测中且与该某定时不同的其它定时(第二定时)为止的期间。在此,详细叙述相关时间常数。
图4是用于说明相关时间常数的图。
在图4中,例示出车辆在沿方向YZ延伸的直线TS上朝方向YZ1行驶的情况。
假设在定时T1(第一定时),发生了与GPS信号的接收相关的影响(以下,表现为“GPS接收影响”)(第一影响)。GPS接收影响例如可以举出多路传播的影响、因GPS卫星的卫星配置而导致的GPS信号的接收强度下降的影响等。
在图4中,将定时T1的车辆实际位置设为位置MT1。如前述,在图4中,由于例示出车辆在直线TS上行驶的情况,因此,位置MT1位于直线TS上。
另外,在图4中,将在定时T1由观测部21观测的GPS车辆位置设为位置NT1。如图4所示,位置NT1位于与直线TS同样地在方向YZ上延伸的假想直线KT上。假想直线KT是指,因在定时T1发生的GPS接收影响而导致实际的车辆位置与GPS车辆位置之间产生了误差(以下,表现为“相离误差”)的情况下,由观测部21观测的GPS车辆位置所处的假想的直线。在本实施方式中,假想直线KT是与直线TS并行的直线。
这样,在图4中例示出这样的情况:由于在定时T1发生了GPS接收影响,观测部21将与实际车辆位置即位置MT1相距距离l1的位置NT1,观测为GPS车辆位置。即,在图4中示出了这样的情况:在定时T1,因GPS接收影响,导致由观测部21观测的GPS车辆位置与实际的车辆位置之间,产生了距离l1的相离误差。
因产生GPS接收影响而导致的GPS车辆位置的测位误差,是与没有GPS接收影响时的GPS车辆位置的测位误差相比更大的误差。在图4的情况下,因在定时T1发生的GPS接收影响,导致在定时T1由观测部21观测的GPS车辆位置的测位误差即位置NT1的误差比没有该GPS接收影响时的GPS车辆位置的测位误差大。
在此,以下举一例说明因GPS接收影响导致GPS车辆位置的测位误差成为大误差的情形。
GPS接收部5基于接收到的GPS信号所含的GPS卫星的位置、发送GPS信号的时刻等信息、接收到GPS信号的时刻等,测定GPS卫星与车辆的距离(以下,表现为“疑似距离”)。GPS接收部5基于针对多个GPS卫星而得到的疑似距离来进行规定的运算,从而计算GPS车辆位置。在此,假设作为GPS接收影响而产生了多路传播的影响。多路传播是指,基于从GPS卫星直接到来的直接波和被建筑物等反射的反射波的多个路径接收GPS信号的现象。由于这样从多个路径接收GPS信号,导致在直接波的GPS信号与反射波的GPS信号中,从GPS卫星到车辆的到达时间是不同的。因此,基于反射波的GPS信号或基于由直接波与反射波合成的合成波的GPS信号等计算出的疑似距离,与基于直接波的GPS信号计算出的疑似距离相比,前者会在疑似距离上产生大的误差。因此,因针对疑似距离而产生大误差,导致由观测部21观测的GPS车辆位置的测位误差比没有多路传播的影响的GPS车辆位置的测位误差大。
回到图4的说明中,在定时T2,假设在定时T1所发生的GPS接收影响持续。在图4中,将定时T2的实际的车辆位置设为位置MT2。与位置MT1同样地,位置MT2也位于直线TS上。另外,在图4中,将在定时T2由观测部21观测的GPS车辆位置设为位置NT2。
在图4中例示出如下情况:在定时T2,因在定时T1发生的GPS接收影响,观测部21将与实际的车辆位置即位置MT2相距距离l2的位置NT2,观测为GPS车辆位置。即,图4中示出如下情况:在定时T2,因在定时T1发生的GPS接收影响,在由观测部21观测的GPS车辆位置与实际的车辆位置之间,产生了距离l2的相离误差。
如前述,GPS接收部5在考虑前次计算出的GPS车辆位置的基础上,来计算基于接收到的GPS信号的GPS车辆位置。因此,在图4中,GPS接收部5在计算定时T2的GPS车辆位置即位置NT2时,在考虑前次计算出的位置、即在定时T1计算出的GPS车辆位置即位置NT1的基础上来进行计算。在图4中,示出了位置NT2位于假想直线KT上的情形。这样,通过考虑前次计算出的位置,GPS接收部5能够抑制GPS车辆位置的变化相对于实际的车辆位置的变化而偏离的情况。即,在图4的情况下,当车辆在直线TS上朝方向YZ1行驶的情况下,通过考虑前次计算出的位置,以使得GPS车辆位置在假想直线KT上朝方向YZ1移动的方式,计算GPS车辆位置。由此,GPS接收部5以如下方式来计算GPS车辆位置,即,与实际的车辆位置在直线TS上朝方向YZ1变化相对应地,使GPS车辆位置在假想直线KT上朝方向YZ1变化。在图4中,例示出位置NT1与位置NT2位于假想直线KT上的情况,因此,相离误差即距离l1与距离l2为相同值。
这样,由于考虑前次计算出的位置,因此,GPS接收部5计算基于在定时T1接收到的GPS信号而计算出的GPS车辆位置,即,反映了位置NT1的GPS车辆位置。即,观测部21在定时T2观测反映了在定时T1接收到的GPS信号的GPS车辆位置,即,位置NT2。
在图4的定时T2由观测部21观测的GPS车辆位置的测位误差,即,位置NT2的误差,因在定时T1发生的GPS接收影响,导致比没有该GPS接收影响时的GPS车辆位置的测位误差大。
在此,在位置NT1的误差与位置NT2的误差之间存在相关关系。这是由于,位置NT1的误差以及位置NT2的误差是比没有GPS接收影响时的误差大的误差,因而包含了基于在定时T1发生的GPS接收影响导致的误差。具体地说,位置NT1的误差以及位置NT2的误差,包含基于相同GPS接收影响而导致的误差,因此互相成为与该GPS接收影响对应的误差。因此,位置NT1的误差与位置NT2的误差之间存在相关关系。这样,由于位置NT1的误差与位置NT2的误差之间存在相关关系,因此它们是具有相关性的误差。具有相关性的误差,是误差平均值不为零的误差。因此,位置NT1的误差以及位置NT2的误差是误差平均值不为零的误差,即,有色噪声性的误差。
在定时T3,也假设在定时T1发生的GPS接收影响仍然持续。在图4中,将定时T3的实际的车辆位置设为位置MT3。与位置MT1以及位置MT2同样地,位置MT3也位于直线TS上。另外,在图4中,将在定时T3由观测部21观测的GPS车辆位置设为位置NT3。
在图4中例示出如下情况:在定时T3,因在定时T1发生的GPS接收影响,观测部21将与实际的车辆位置即位置MT3相距距离l3的位置NT3,观测为GPS车辆位置。即,在图4中例示出如下情况:在定时T3,因在定时T1发生的GPS接收影响,导致在由观测部21观测的GPS车辆位置与实际的车辆位置之间,产生了距离l3的相离误差。
GPS接收部5在计算定时T3的GPS车辆位置即位置NT3时,考虑在定时T2计算出的GPS车辆位置的基础上进行计算。在定时T2计算出的GPS车辆位置,是在考虑了位置NT1的基础上而计算出来的。因此,GPS接收部5在定时T3计算反映了基于在定时T1接收到的GPS信号而计算出的GPS车辆位置的GPS车辆位置。即,观测部21在定时T3观测反映了在定时T1接收到的GPS信号的GPS车辆位置。
另外,由于考虑前次计算出的GPS车辆位置,因此GPS接收部5计算出抑制了GPS车辆位置的变化相对于实际车辆位置变化的偏离的位置NT3。即,在定时T3存在与定时T1相同的GPS接收影响,因此,GPS接收部5以使位置NT3位于假想直线KT上的方式来计算该位置NT3。在图4中,例示出位置NT3位于假想直线KT上的情况,因此,相离误差即距离l3,是与距离l1及距离l2相同的值。
在图4的定时T3,由观测部21观测的GPS车辆位置的测位误差、即位置NT3的误差,因在定时T1发生的GPS接收影响,是比没有GPS接收影响时的误差大的误差。
在此,位置NT2的误差与位置NT3的误差之间存在相关关系。这是由于,位置NT2的误差以及位置NT3的误差是比没有GPS接收影响时的误差大的误差,因此,其包含了基于在定时T1发生的GPS接收影响而导致的误差。具体地说,位置NT2的误差以及位置NT3的误差,包含基于在定时T1发生的GPS接收影响而导致的误差,因此相互成为与该GPS接收影响对应的误差,从而存在相关关系。这样,位置NT1的误差以及位置NT2的误差由于存在相关关系,因此是具有相关性的误差。如前述,具有相关性的误差,是误差平均值不为零的误差。因此,位置NT1的误差以及位置NT2的误差,是误差平均值不为零的误差,即,是有色噪声性的误差。
在定时T4,也假设在定时T1发生的GPS接收影响仍然持续。在图4中,将定时T4的实际的车辆位置设为位置MT4。另外,在图4中,将在定时T4由观测部21观测的GPS车辆位置设为位置NT4。
在图4中例示出这样的情况:在定时T4,因在定时T1发生的GPS接收影响,观测部21将与实际的车辆位置即位置MT4相距距离l4的位置NT4观测为GPS车辆位置。即,在图4中示出了如下情况,在定时T4,因在定时T1发生的GPS接收影响,导致从实际的车辆位置相对于由观测部21观测的GPS车辆位置而产生了距离l4的相离误差。
GPS接收部5在计算定时T4的GPS车辆位置即位置NT4时,考虑在定时T3计算出的GPS车辆位置的基础上来进行计算。在定时T3计算出的GPS车辆位置,是在考虑了位置NT2的基础上而计算出来的。因此,GPS接收部5在定时T4计算出反映了基于在定时T1接收到的GPS信号而计算出的GPS车辆位置的GPS车辆位置。即,观测部21在定时T4观测反映了在定时T1接收到的GPS信号的GPS车辆位置。
另外,由于考虑前次计算出的GPS车辆位置,因此GPS接收部5能够计算出抑制了GPS车辆位置的变化相对于实际车辆位置变化的偏离的位置NT4。即,在定时T4存在与定时T1相同的GPS接收影响,因此,GPS接收部5以使位置NT4位于假想直线KT上的方式来计算该位置NT4。在图4中,例示出位置NT4位于假想直线KT上的情况,因此,相离误差即距离l4,是与距离l1、距离l2及距离l3相同的值。
在图4的定时T4,由观测部21观测的GPS车辆位置的误差、即位置NT4的误差,因在定时T1发生的GPS接收影响,是比没有GPS接收影响时的GPS车辆位置的测位误差大的误差。
在此,位置NT3的误差与位置NT4的误差之间存在相关关系。这是由于,位置NT3的误差以及位置NT4的误差是比没有GPS接收影响时的误差大的误差,因此,其包含了基于在定时T1发生的GPS接收影响而导致的误差。具体地说,位置NT3的误差以及位置NT4的误差,包含基于在定时T1发生的GPS接收影响而导致的误差,因此相互成为与该GPS接收影响对应的误差,从而存在相关关系。这样,位置NT3的误差以及位置NT4的误差由于存在相关关系,因此是具有相关性的误差。因此,位置NT3的误差以及位置NT4的误差,是误差平均值不为零的误差,即,是有色噪声性的误差。
如到此为止说明的那样,在图4中,示出了包括从定时T1到定时T4各自的GPS车辆位置的测位误差均包含基于在定时T1发生的GPS接收影响导致的误差的情况。因此,在图4中,从位置NT1到位置NT4各自的误差互相存在相关关系。
在定时T5(第二定时),假设发生了与在定时T1发生的GPS接收影响不同的GPS接收影响(第二影响)。
在图4中,将在定时T5的实际的车辆位置设为位置MT5。另外,在图4中,将在定时T5由观测部21观测的GPS车辆位置设为位置NT5。即,因基于在定时T5发生的GPS信号的影响,观测部21将与实际的车辆位置即位置MT5相距距离l5的位置NT5,观测为GPS车辆位置。这表明,因在定时T5发生的GPS接收影响,导致从实际的车辆位置相对于观测部21观测的GPS车辆位置存在距离l5的相离误差。
如图4所示,在定时T5的相离误差,与从定时T1到定时T4各自的相离误差都不同。在图4中,例示出表示相离误差的距离l5比距离l1到距离l4中的任意一个相离误差都短的情况。即,在图4中例示出这样的情况:基于在定时T5发生的GPS接收影响而导致的相离误差,比基于在定时T1发生的GPS接收影响导致的相离误差小。这样在定时T5的相离误差与其它相离误差不同的原因在于,在定时T5发生了与在定时T1发生的GPS接收影响不同的GPS接收影响。
如前述,GPS接收部5在考虑前次计算出的GPS车辆位置的基础上,来计算基于接收到的GPS信号的GPS车辆位置。因此,GPS接收部5在计算定时T5的GPS车辆位置即位置NT5时,考虑在定时T4计算出的GPS车辆位置的基础上来进行计算。在定时T4计算出的GPS车辆位置,是在考虑了位置NT3的基础上而计算出来的,是反映了在定时T1接收到的GPS信号而计算出来的。但是,能够将在定时T5计算出的GPS车辆位置,视作没有反映在定时T1接收到的GPS信号的GPS车辆位置。这是由于,如图4所示,在定时T5观测到的GPS车辆位置,不位于假想直线KT上。这样,在定时T5不位于假想直线KT上的原因在于,在定时T5发生了与在定时T1发生的GPS接收影响不同的GPS接收影响,其与定时T1到定时T4发生的相离误差都不同。因此,能够将在定时T5计算出的GPS车辆位置,视作没有反映基于在定时T1接收到的GPS信号而计算出的GPS车辆位置的GPS车辆位置。即,这相当于,观测部21在定时T5观测没有反映出在定时T1接收到的GPS信号的GPS车辆位置。
在图4的定时T5由观测部21观测的GPS车辆位置的测位误差,即,位置NT5的误差,因在定时T5发生的GPS接收影响,是比没有该GPS接收影响时的GPS车辆位置的测位误差大的误差。另外,由于GPS接收影响不同,因此,在定时T5由观测部21观测的GPS车辆位置的测位误差,是与定时T1到定时T4中的各个定时的GPS车辆位置的测位误差不同的误差。因此,位置NT5的误差,与从位置NT1到位置NT4各自的误差之间不存在相关关系。
在图4中,相关时间常数表示以下的期间。即,在图4中,相关时间常数是指,从定时T1(第一定时)开始,到在定时T1接收到的GPS信号不再反映到由观测部21进行的GPS车辆位置的观测中的定时T5(第二定时)为止的期间。更为具体地说,在图4中,相关时间常数是指如下期间:从在定时T1发生GPS接收影响(第一影响)而导致针对所观测的GPS车辆位置发生基于该GPS接收影响的相离误差起,到发生了与该GPS接收影响不同的GPS接收影响(第二影响)而导致针对GPS车辆位置发生基于该不同的GPS接收影响的相离误差为止的期间。
此外,在图4中,对于相关时间常数,不限于上述期间。例如,也可以是这样的期间:从发生GPS接收影响,基于该GPS接收影响导致在实际的车辆位置与GPS车辆位置之间发生相离误差起,到该相离误差消除为止的期间。即,在图4的情况下,该期间是指,从定时T1起,到因在定时T1发生的GPS接收影响导致的相离误差消除为止的期间。在此,消除是指,相离距离变为接近零的值,即,实际的车辆位置与GPS车辆位置之间的误差消失,而不是指相离距离变为因定时T5的GPS接收影响导致的相离距离。
该相关时间常数基于事先的测试或模拟等计算出,并存储在存储部等中。在本实施方式中,将针对山路或高速公路之下设置的车道等的各种车道计算出的多个上述期间中的最长期间,作为相关时间常数存储在存储部等中。
如前述,在计算卡尔曼增益Kk时,在观测量的误差协方差矩阵所含的元素中,按式(9)所示那样设定GPS车辆位置的观测误差。即,推断部22将对GPS车辆位置的测位误差乘以而得的结果,设定为GPS车辆位置的观测误差。
按如下方式计算式(9)。
若GPS车辆位置的误差按照作为相关时间常数的τ以及作为GPS车辆位置的测位误差的σ的一次马尔可夫过程(Markov process),则GPS车辆位置的误差的自相关函数如式(10)所示。自相关函数在本实施方式的情况下是指,表示GPS车辆位置的误差相对于时刻的自相关的函数。
式10
C(t)=σet/τ…(10)
式(10)表示:GPS车辆位置的误差的自相关函数是基于相关时间常数的τ和GPS车辆位置的测位误差的σ的指数函数。如式(10)所示,时刻t越大,则GPS车辆位置的误差的自相关性越大。
在用式(10)表示GPS车辆位置的误差的自相关函数的情况下,基于维纳-辛钦定理(Wiener‐Khintchine's theorem)以及功率谱(Power Spectral)与方差之间的关系,求出以下所示的式(11)。
式11
在式(11)中,Err(t)表示GPS车辆位置的误差。另外,在式(11)中,σt表示时刻t的GPS车辆位置的测位误差。在式(11)中,将式(11)的左边移至右边,将式(11)的右边第1项移至左边,用τ来进行整理,从而能够变形为式(12)。
式12
在式(12)中,右边第1项的Err(t)/dt表示GPS车辆位置的误差的变化量。即,在式(12)中,示出GPS车辆位置的误差由如下两值的和表示:对GPS车辆位置的误差的变化量乘以“-τ”而得到的值、对时刻t的GPS车辆位置的测位误差乘以而得到的值。
在此,根据如下理由,能够将式(12)的右边第1项的Err(t)/dt视作零。
GPS车辆位置的误差的变化量是难以进行观测或推断等的量。因此,认为GPS车辆位置的误差的变化量的期待值为零。另外,通过GPS接收部5的运算,将GPS车辆位置计算为误差没有变动的位置,即,误差不具有相对于误差的变化量的位置。因此,能够认为GPS车辆位置的误差的变化量为零。如上所述,能够将GPS车辆位置的误差的变化量视为零。
根据以上理由,如果将GPS车辆位置的误差的变化量设为零,则式(12)的右边第1项消失,如式(13)所示。
式13
式(13)示出:通过对时刻t的GPS车辆位置的测位误差乘以来表示时刻t的GPS车辆位置的误差。在此,式(13)的右边相当于式(9)的右边,是GPS车辆位置的观测误差。即,GPS车辆位置的观测误差通过对GPS车辆的测位误差赋予了基于相关时间常数即τ的的加权来表示。
推断部22在计算卡尔曼增益Kk时,针对观测量的误差协方差矩阵所含的元素中的GPS车辆位置,基于式(13)来设定GPS车辆位置的观测误差。即,推断部22对由观测部21观测到的GPS车辆位置的测位误差,赋予的加权,将赋予了加权的GPS车辆位置的测位误差作为GPS车辆位置的观测误差,输入至卡尔曼增益Kk的观测量的误差协方差矩阵。更为具体地说,推断部22针对观测量的误差协方差矩阵所含的元素中的GPS车辆位置,如式(14)以及式(15)所示那样设定GPS车辆位置的观测误差,并将其输入至卡尔曼增益Kk的观测量的误差协方差矩阵。
式14
在式(14)中,下标“k”表示时刻。另外,上标“xGPS”以及“gx”表示x轴元素。即,式(14)的右边表示时刻k的x轴上的GPS车辆位置的观测误差。另外,式(14)的左边表示对乘以时刻k的x轴上的GPS车辆位置的测位误差。即,GPS车辆位置的观测误差的x轴元素被设定为,对GPS车辆位置的测位误差的x轴元素赋予的加权。
式15
在式(15)中,下标“k”表示时刻。另外,上标“yGPS”以及“gy”表示y轴元素。即,式(15)的右边表示时刻k的y轴上的GPS车辆位置的观测误差。另外,式(15)的左边表示对乘以时刻k的y轴上的GPS车辆位置的测位误差。即,GPS车辆位置的观测误差的y轴元素被设定为,对GPS车辆位置的测位误差的y轴元素赋予的加权。
赋予的加权的目的在于,在τ所示的期间内,对作为有色噪声性的误差的GPS车辆位置的测位误差汇总作为一个GPS车辆位置的测位误差(以下,表现为“集GPS车辆位置测位误差”)而由卡尔曼滤波器处理。如前述,τ是相关时间常数,是在某定时接收到的GPS信号不再反映到由观测部21进行的GPS车辆位置的观测中且与该某定时不同的其它定时为止的期间。并且,τ是包括基于在某定时发生的GPS接收影响而导致的有色噪声性的误差即GPS车辆位置的测位误差在内的期间。推断部22通过赋予该的加权,从而在τ期间内对有色噪声性的误差即GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差而由卡尔曼滤波器处理,来计算卡尔曼增益Kk。
能够将集GPS车辆位置测位误差视作白噪声性的误差。这是由于,在集GPS车辆位置测位误差和在τ期间内除了作为有色噪声性的误差的GPS车辆位置的测位误差之外的其它GPS车辆位置的测位误差之间,不存在相关关系。例如,在图4的情况下,对从定时T1到定时T4为止的GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差的情况下,由于GPS接收影响不同,因此该集GPS车辆位置测位误差与定时T5的GPS车辆位置的测位误差之间没有相关关系。另外,集GPS车辆位置的测位误差,与定时T1的前次定时的GPS车辆位置的测位误差之间也没有相关关系。在图4中,例示出在定时T1发生了GPS接收影响的情况。因此,即使在定时T1的前次定时存在GPS接收影响,该GPS接收影响也与在定时T1发生的GPS接收影响不同。因此,集GPS车辆位置测位误差与定时T1的前次定时的GPS车辆位置的测位误差之间也没有相关关系。这样,通过在τ期间内将作为有色噪声性的误差的GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差,从而能够将该集GPS车辆位置测位误差视作白噪声性的误差。
另外,集GPS车辆位置测位误差,是包括在τ期间内表示有色噪声性的误差的全部GPS车辆位置的测位误差在内的GPS车辆位置的测位误差。例如,在图4中,在将从定时T1到定时T4的GPS车辆位置的测位误差、即从位置NT1到位置NT4为止的各个误差设为集GPS车辆位置测位误差的情况下,集GPS车辆位置测位误差包括从位置NT1到位置NT4为止的各个误差。因此,由于将集GPS车辆位置测位误差视作白噪声性的误差,因此集GPS车辆位置测位误差所含的GPS车辆位置的测位误差分别相当于白噪声性的误差的一部分。这表明,通过对在τ期间内表示有色噪声性的误差的GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差,将在τ期间内表示有色噪声性的误差的GPS车辆位置的测位误差作为白噪声性的误差的一部分来由卡尔曼滤波器进行处理。即,将在τ期间内表示有色噪声性的误差的GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差而由卡尔曼滤波器进行处理,从而能够将在τ期间内表示有色噪声性的误差的GPS车辆位置的测位误差视作白噪声性的误差。
例如,在图4中,在将从定时T1到定时T4的GPS车辆位置的测位误差、即从位置NT1到位置NT4为止的各个误差设为集GPS车辆位置测位误差的情况下,在卡尔曼滤波器中将赋予了的从位置NT1到位置NT4为止的各个误差作为白噪声性的误差的一部分处理。这样,将从位置NT1到位置NT4为止的各个误差汇总地作为集GPS车辆位置测位误差而由卡尔曼滤波器进行处理,从而能够将从位置NT1到位置NT4为止的各个误差视作白噪声性的误差。
如前述,赋予的加权的目的在于,在τ所示的期间内,将作为有色噪声性的误差的GPS车辆位置的测位误差汇总地作为集GPS车辆位置测位误差来由卡尔曼滤波器进行处理。因此,赋予了的加权的GPS车辆位置的测位误差,即式(13)的左边能够被视作集GPS车辆位置测位误差的一部分且白噪声性的误差。即,能够将如式(13)所示那样设定的GPS车辆位置的观测误差视作白噪声性的误差。由此,通过如式(13)所示那样设定GPS车辆位置的观测误差,能够恰当地对以白噪声性的误差为前提的观测量的误差协方差矩阵输入GPS车辆位置的观测误差,推断部22能够正确地计算卡尔曼增益Kk。并且,推断部22由于能够正确地计算卡尔曼增益Kk,因此能够高精度地推断与车辆当前所在地相关的状态。
另外,通过如式(13)那样设定GPS车辆位置的观测误差,推断部22每当接收GPS信号时,能够高精度地推断车辆当前所在地的状态。如前述,在卡尔曼滤波器中,观测量的误差协方差矩阵所含的误差是以白噪声性的误差为前提的。因此,在发生了GPS接收影响的情况下,每当接收GPS信号时,如果直接将GPS车辆位置的测位误差作为GPS车辆位置的观测误差输入至卡尔曼滤波器,则会导致车辆当前所在地的状态的推断精度下降。与此相对,τ期间结束后,考虑直接将GPS车辆位置的测位误差作为GPS车辆位置的观测误差输入至卡尔曼滤波器,并且推断车辆当前所在地的状态。但是,这样一来,担心会导致与车辆当前所在地相关的状态的推断频度下降,另外,推断精度也下降。在此,通过如式(13)那样设定GPS车辆位置的观测误差,即使τ期间的GPS车辆位置的测位误差是有色噪声性的误差,在卡尔曼滤波器中也能够将其作为白噪声性的误差来处理。因此,通过如式(13)那样设定GPS车辆位置的观测误差,推断部22每当接收GPS信号时,能够高精度地推断车辆当前所在地的状态。
在图4中,τ所示的期间是如下的期间:从在定时T1发生GPS接收影响而导致针对所观测的GPS车辆位置发生基于该GPS接收影响的相离误差起,到发生了与该GPS接收影响不同的GPS接收影响而导致针对GPS车辆位置发生基于该不同的GPS接收影响的相离误差为止的期间。推断部22通过赋予基于该期间的加权能够在该期间内将作为有色噪声性的误差的GPS车辆位置的测位误差,设定为可视作白噪声性的误差的GPS车辆位置的观测误差。因此,推断部22通过将所设定的GPS车辆位置的观测误差输入观测量的误差协方差矩阵(Rk),能够正确地计算卡尔曼增益Kk。因此,推断部22能够高精度地推断该期间内的与车辆当前所在地相关的状态。
另外,在图4中,τ所示的期间,也可以是从发生GPS接收影响而导致针对GPS车辆位置发生了基于该GPS接收影响的相离误差起,到该相离误差消除为止的期间。即使是这样的期间,推断部22通过赋予基于期间的加权也能够在这样的期间内将作为有色噪声性的误差的GPS车辆位置的测位误差,设定为可视作白噪声性的误差的GPS车辆位置的观测误差。因此,推断部22通过将所设定的GPS车辆位置的观测误差输入至观测量的误差协方差矩阵(Rk),能够正确地计算卡尔曼增益Kk。因此,推断部22能够高精度地推断如下期间内的与车辆当前所在地相关的状态:从发生GPS接收影响而导致针对GPS车辆位置发生了基于该GPS接收影响的相离误差起到该相离误差消除为止的期间。
此外,在导航装置1是能够判断有无GPS接收影响的结构的情况下,也可以在判断为有GPS接收影响时,如式(16)以及式(17)那样设定GPS车辆位置的观测误差。
式16
VARrx,表示时刻k的GPS车辆位置的x轴元素与在预测处理中预想的车辆位置(车辆状态预测值中的车辆位置)的x轴元素的方差。即,在式(16)中,对式(14)加上该方差来设定GPS车辆位置的观测误差。
式17
VARry,表示时刻k的GPS车辆位置的y轴元素与在预测处理中预想的车辆位置(车辆状态预测值的车辆位置)的y轴元素的方差。即,在式(17)中,对式(15)加上该方差来设定GPS车辆位置的观测误差。
这样,构成为能够判断有无GPS接收影响,在有GPS接收影响的情况下,以式(16)以及式(17)所示那样设定GPS车辆位置的观测误差。由此,通过加上GPS车辆位置与在预测处理中预测的车辆位置的方差,使GPS车辆位置的观测误差含有基于这些位置的方差,因此能够更加正确地计算GPS车辆位置的观测误差。因此,能够更加正确地计算卡尔曼增益Kk,推断部22能够高精度地推断与车辆当前所在地相关的状态。
此外,式(16)以及式(17)是在构成为能够判断有无GPS接收影响且有GPS接收影响的情况下,设定GPS车辆位置的观测误差的式子。但是,也可以与有无GPS接收影响无关地采用式(16)以及式(17)。这是由于,在没有GPS接收影响的情况下,认为式(16)的VARrx与式(17)的VARry近似于0。因此,也可以与有无GPS接收影响无关地,在设定GPS车辆位置的观测误差时采用式(16)以及式(17)。该情况下也能够起到上述效果。
如前述,导航装置1的控制部2基于推断出的车辆状态,执行成为地图匹配对象的链路的评价。
图5是用于说明基于推断出的车辆状态进行的地图匹配的处理且示出了地图上的道路R1、道路R2的图。在图5中,道路R1是朝向方向Y1延伸的道路。另外,道路R2是朝向不与方向Y1并行的方向Y2延伸的道路。
在图5中,链路L1是与道路R1对应的链路。另外,链路L2是与道路R2对应的链路。
另外,在图5中,标记α是表示由推断部22推断出的车辆位置的标记。以下,在用图5进行的说明中,假设由推断部22推断出车辆位于位置M1并且正在朝向行进方向X1行驶。如图5所示,位置M1是位于道路R1上的位置,是从道路R1的宽度方向的中心朝向方向Y1而向右侧偏离的位置。
道路R1、道路R2以及推断出的车辆位置之间的关系为图5所示的关系的情况下,在地图匹配时,首先,控制部2参照地图数据3a,取得成为地图匹配的候选的链路即地图匹配候选链路。控制部2取得如下的一个或多个链路来作为地图匹配候选链路,所述一个或多个链路位于根据推断出的车辆位置预先设定的规定的范围内,并且车辆与链路之间的方位误差在规定范围内。在图5的例子中,链路L1以及链路L2是相对于推断出的车辆位置即位置M1位于规定范围内,并且车辆与链路之间的方位误差在规定范围内的链路。因此,控制部2取得链路L1以及链路L2来作为地图匹配候选链路。
接下来,控制部2分别针对所取得的地图匹配候选链路,计算用于评价链路的评价量。
评价量是基于推断出的车辆位置与地图匹配候选链路的位置之间的误差、以及推断出的车辆方位与地图匹配候选链路的方位之间的误差,通过以下所示的式(18)计算出的值。
τ=δx2/Δx2+δy2/Δy2+δθ2/Δθ2···(18)
在此,推断出的车辆位置与地图匹配候选链路的位置之间的误差是指,下述交点与推断出的车辆位置在x轴方向以及y轴方向上的差,所述交点是指,在从推断出的车辆位置向地图匹配候选链路引出垂线时垂线与地图匹配候选链路之间的交点。在式(18)中,δx表示该交点的x坐标与推断出的车辆位置的x坐标的差。另外,δy表示该交点的y坐标与推断出的车辆位置的y坐标的差。
在图5的例子中,推断出的车辆位置与链路L1之间的位置误差是指,交点MM1与位置M1在x轴方向以及y轴方向上的差,所述交点MM1是从位置M1向链路L1引出的垂线S1与链路L1之间的交点。另外,在图5中,推断出的车辆位置与链路L2之间的位置误差是指,交点MM2与位置M1在x轴方向以及y轴方向上的差,所述交点MM2是从位置M1向链路L2引出的垂线S2与链路L2之间的交点。
另外,推断出的车辆方位与地图匹配候选链路的方位之间的误差是指,推断出的车辆方位所对应的角度与地图匹配候选链路的方位所对应的角度之间的差。
如前述,车辆方位是指,车辆的行进方向的方位。在图5的例子中,推断出的车辆方位是行进方向X1的方位。另外,与车辆方位对应的角度是指,以向东的方向为基准,在向东的方向与车辆方位之间的逆时针旋转的相离角度。在图5的例子中,在假想直线K1为在东西方向上延伸的假想直线的情况下,与推断出的车辆方位对应的角度为角度θ1。
另外,链路的方位是指,链路延伸的方向的方位。链路延伸的方向是指,在沿着链路的两个方向中在与链路对应的道路上车辆能够行驶的方向所对应的方向。另外,与链路的方位对应的角度是指,以向东的方向为基准,在向东的方向与链路的方位之间的逆时针旋转的相离角度。
在图5的例子中,链路L1的方位是方向Z1的方位。另外,在假想直线K2为在东西方向上延伸的假想直线的情况下,与链路L1的方位对应的角度是角度θ2。另外,在图5的例子中,链路L2的方位是方向Z2的方位。另外,在假想直线K3是在东西方向上延伸的假想直线的情况下,与链路L2的方位对应的角度是角度θ3。
在图5的例子中,车辆与链路L1的方位误差,是角度θ1与角度θ2的差。另外,车辆与链路L2的方位误差,是角度θ1与角度θ3的差。即,在图5的例子中,在式(18)中,δθ表示角度θ1与角度θ2的差,或者表示角度θ1与角度θ3的差。
如式(18)所示,评价量τ通过如下这些项的和而计算出,这些项为:δx的平方的值除以Δx的平方的值的项、δy的平方的值除以Δy的平方的值的项、δθ的平方的值除以Δθ的平方的值的项。Δx表示推断部22推断出的车辆的x轴方向的位置的误差。即,Δx2表示推断部22推断出的车辆的x轴方向的位置的方差。另外,Δy表示推断部22推断出的车辆的y轴方向的位置的误差。即,Δy2表示推断部22推断出的车辆的y轴方向的位置的方差。另外,Δθ表示推断部22推断出的车辆方位的误差。即,Δθ2表示推断部22推断出的车辆方位的方差。Δx2、Δy2以及Δθ2是推断部22推断出的车辆状态推断值的误差协方差矩阵所含的方差。这样,评价量是通过如下这些值的和而计算出的值,这些值是:通过推断出的车辆位置的误差对车辆位置与链路位置之间的误差进行了无量纲化(dimensionless)后的值、通过推断出的车辆位置的误差对车辆方位与链路方位之间的误差进行了无量纲化后的值。
在图5的例子中,控制部2计算作为地图匹配候选链路而取得的链路L1的评价量与链路L2的评价量。
将链路L1的评价量设为τ1,将位置M1与交点MM1的x坐标的差设为δx1,将位置M1与交点MM1的y坐标的差设为δy1,将角度θ1与角度θ2的差设为δθ1,在此情况下,评价量τ1如以下所示的式(19)所示。
τ1=δx12/Δx2+δy12/Δy2+δθ12/Δθ2···(19)
另一方面,将链路L2的评价量设为τ2,将位置M1与交点MM2的x坐标的差设为δx2,将位置M1与交点MM2的y坐标的差设为δy2,将角度θ1与角度θ3的差设为δθ2,在此情况下,评价量τ2如以下所示的式(20)所示。
τ2=δx22/Δx2+δy22/Δy2+δθ22/Δθ2···(20)
在图5的例子中,距离l1比距离l2小。即,位置M1与交点MM2的x坐标以及y坐标的差,比位置M1与交点MM2的x坐标以及y坐标的差小。另外,在图5的例子中,角度θ1与角度θ2的差,比角度θ1与角度θ3的差小。因此,链路L1的评价量即τ1的值,比链路L2的评价量即τ2的值小。因此,控制部2将评价量小的链路L1,决定为与车辆的当前位置对应关联的链路。
这样,控制部2基于推断出的车辆的状态,评价成为地图匹配对象的链路。如上述,控制部2在评价链路时,基于由推断部22推断出的车辆状态推断值的误差协方差矩阵所含的方差,来评价链路。该误差协方差矩阵,是基于输入了GPS车辆位置的观测误差来作为白噪声性的误差的卡尔曼增益Kk而计算出的矩阵,是含有高精度地计算出的误差的矩阵。因此,控制部2通过将该误差协方差矩阵所含的方差用于链路的评价中,从而能够正确地评价链路。
如以上说明的那样,导航装置1(位置推断装置、车载装置)具有:GPS接收部5,其接收GPS信号;观测部21,其对观测量进行观测,该观测量含有基于由GPS接收部5接收到的GPS信号的GPS车辆位置(GPS测位位置);推断部22,其基于由观测部21观测的观测量与卡尔曼滤波器,推断与车辆当前所在地相关的状态量。推断部22计算车辆状态预测值(状态量的预测值)与车辆状态预测值的误差协方差矩阵(预测值的误差)。另外,推断部22基于车辆状态预测值、车辆状态预测值的误差协方差矩阵、由观测部21观测的观测量的误差协方差矩阵(观测量的误差),来计算车辆状态推断值(状态量的推断值)、车辆状态推断值的误差协方差矩阵(推断值的误差)。并且,推断部22在计算车辆状态推断值与车辆状态推断值的误差协方差矩阵时,对GPS车辆位置的测位误差赋予基于以下期间的加权,该以下期间是指,从某定时(第一定时)起到与该某定时不同的其它定时(第二定时)为止的期间,在该其它定时(第二定时),在该某定时接收到的GPS信号不再被反映到基于观测部21对所述GPS车辆位置的观测中。
通过对GPS车辆位置的测位误差赋予基于该期间的加权,将GPS车辆位置的测位误差设定为可视作白噪声性的误差的GPS车辆位置的观测误差。因此,能够针对以白噪声性的误差为前提的观测量的误差协方差矩阵,恰当地输入GPS车辆位置的观测误差,从而推断部22能够正确地计算卡尔曼增益Kk。并且,由于推断部22能够正确地计算卡尔曼增益Kk,因此能够高精度地推断与车辆当前所在地相关的状态。
另外,推断部22在计算车辆状态推断值与车辆状态推断值的误差协方差矩阵时,对GPS车辆位置的测位误差赋予基于下述期间的加权,该期间是指,从针对GPS车辆位置发生了基于在某定时发生的GPS接收影响(第一影响)的误差(相离误差)起,到针对GPS车辆位置发生了基于在与某定时不同的其它定时发生的与上述GPS接收影响不同的GPS接收影响(第二影响)的误差(相离误差)为止的期间。
在图4的情况下,该期间为,从在定时T1发生GPS接收影响而导致针对所观测的GPS车辆位置发生了基于该GPS接收影响的相离误差起,到在定时T5发生与该GPS接收影响不同的GPS接收影响而导致针对GPS车辆位置发生了基于该不同的GPS接收影响的相离误差为止的期间。推断部22在计算卡尔曼增益Kk时,通过赋予基于该期间的加权将在该期间内作为有色噪声性的误差的GPS车辆位置的测位误差,作为可视作白噪声性的误差的GPS车辆位置的观测误差而输入至观测量的误差协方差矩阵(Rk),来计算卡尔曼增益Kk。因此,推断部22能够高精度地推断该期间内的与车辆当前所在地相关的状态。
另外,推断部22在计算车辆状态推断值与车辆状态推断值的误差协方差矩阵时,对GPS车辆位置的误差赋予基于如下期间的加权,该期间是指,从针对GPS车辆位置发生了基于GPS接收影响的误差(相离误差)起到该误差消除为止的期间。
如前述,τ所示的期间,可以是从发生GPS接收影响而导致针对GPS车辆位置发生了基于该GPS接收影响的相离误差起,到该相离误差消除为止的期间。即使是这样的期间,推断部22也能够在该期间内将作为有色噪声性的误差的GPS车辆位置的测位误差设为可视作白噪声性的误差的GPS车辆位置的观测误差而输入至观测量的误差协方差矩阵(Rk),来计算卡尔曼增益Kk。因此,推断部22能够高精度地推断该期间内的与车辆当前所在地相关的状态。
上述实施方式只不过为本发明一个方式的例示,在不脱离本发明思想的范围内能够进行任意变形及应用。
例如,为了容易理解本发明,图1为根据主要处理内容来对导航装置1的功能结构进行分类示出的概略图,但也能够根据处理内容来将导航装置1的结构分类为更多的结构要素。另外,也可以分类为一个结构要素执行更多处理。
另外,例如,为了容易理解控制部2的处理,根据主要处理内容对图2以及图3的流程图的处理单位进行了分割,但本发明并不受限于处理单位的分割方式及名称。也可以根据处理内容将控制部2的处理分为更多的处理单位。另外,也可以分为一个处理单位执行更多处理。
另外,例如,在上述实施方式中,例示出将位置推断装置作为搭载于车辆的车载装置的导航装置1的例子,但位置推断装置的形态可以是任意的,例如也可以是行人携带的便携式装置。
Claims (8)
1.一种位置推断装置,具有:
速度传感器,其检测移动体的速度,所述移动体为车辆或行人;
陀螺仪传感器,其检测所述移动体转弯的角速度;
GPS接收部,其接收GPS信号;
观测部,其在每规定的观测间隔,基于表示从所述速度传感器输出的速度的信号、表示从所述陀螺仪传感器输出的角速度的信号、以及由所述GPS接收部接收到的所述GPS信号,对所述移动体的速度、角速度、位置和方位角进行观测而作为观测量;以及
推断部,其在每所述观测间隔,基于前一次观测间隔下的所述移动体的位置的预测值、以及前一次观测间隔下的所述移动体的速度、角速度、位置和方位的观测量,计算出此次观测间隔下的所述移动体的位置的预测值,并对此次观测间隔下的所述移动体的位置的预测值加上修正值,来计算出此次观测间隔下的所述移动体的位置的推断值,其中,所述修正值是对前一次观测间隔下的所述移动体的位置的预测值与观测量之差乘以基于所述移动体的位置的预测值的误差与观测量的误差的卡尔曼增益而得到的值,
所述位置推断装置的特征在于,
所述推断部在发生了与所述GPS接收部的所述GPS信号的接收相关的影响的情况下,对所述移动体的位置的观测量的误差进行基于设想所述影响所持续的时间的加权,来设定所述卡尔曼增益,
所述卡尔曼增益通过下式(1)来设定,所述推断部对误差协方差矩阵Rk所包含的元素中的、基于所述观测部的位置的观测量的误差进行基于时间常数的加权,
Kk=P(k+1|k)HT(Rk+HP(k+1|k)HT)-1...(1)
其中,Kk是卡尔曼系数,P(k+1|k)是基于时刻k为止的观测间隔的信息得到的时刻k+1的观测检测下的车辆状态预测值的误差协方差矩阵,HT是通过观测方程式求出的观测矩阵,Rk是在时刻k的观测间隔观测出的观测量的误差协方差矩阵。
2.如权利要求1所述的位置推断装置,其特征在于,
所述推断部在计算所述推断值和所述推断值的误差时,对GPS测位位置的误差赋予基于从针对所述GPS测位位置发生了基于第一影响的误差起、到针对所述GPS测位位置发生了基于第二影响的误差为止的期间的加权,所述第一影响是与所述GPS信号的接收相关的影响,所述第二影响是与所述第一影响不同的影响。
3.如权利要求1所述的位置推断装置,其特征在于,
所述推断部在计算所述推断值和所述推断值的误差时,对GPS测位位置的误差赋予基于从针对所述GPS测位位置发生了基于与所述GPS信号的接收相关的影响的误差起、到与所述GPS信号的接收相关的影响消除为止的期间的加权。
4.如权利要求1~3中任意一项所述的位置推断装置,其特征在于,
所述位置推断装置是搭载于车辆的车载装置,推断与所述车辆的当前所在地相关的所述状态量。
5.一种推断方法,推断与当前所在地相关的状态量,其特征在于,
在每规定的观测间隔,基于表示速度的信号、表示角速度的信号、以及GPS信号,对移动体的速度、角速度、位置和方位角进行观测而作为观测量,所述移动体为车辆或行人,
在每所述观测间隔,基于前一次观测间隔下的所述移动体的位置的预测值、以及前一次观测间隔下的所述移动体的速度、角速度、位置和方位的观测量,计算出此次观测间隔下的所述移动体的位置的预测值,并对此次观测间隔下的所述移动体的位置的预测值加上修正值,来计算出此次观测间隔下的所述移动体的位置的推断值,其中,所述修正值是对前一次观测间隔下的所述移动体的位置的预测值与观测量之差乘以基于所述移动体的位置的预测值的误差和所述移动体的位置的观测量的误差的卡尔曼增益而得到的值,
在发生了与所述GPS信号的接收相关的影响的情况下,对所述移动体的位置的观测量的误差进行基于设想所述影响所持续的时间的加权,来设定所述卡尔曼增益,
所述卡尔曼增益通过下式(1)来设定,所述推断部对误差协方差矩阵Rk所包含的元素中的、基于所述观测部的位置的观测量的误差进行基于时间常数的加权,
Kk=P(k+1|k)HT(Rk+HP(k+1|k)HT)-1...(1)
其中,Kk是卡尔曼系数,P(k+1|k)是基于时刻k为止的观测间隔的信息得到的时刻k+1的观测检测下的车辆状态预测值的误差协方差矩阵,HT是通过观测方程式求出的观测矩阵,Rk是在时刻k的观测间隔观测出的观测量的误差协方差矩阵。
6.如权利要求5所述的推断方法,其特征在于,
在计算所述推断值和所述推断值的误差时,对GPS测位位置的误差赋予基于从针对所述GPS测位位置发生了基于第一影响的误差起、到针对所述GPS测位位置发生了基于第二影响的误差为止的期间的加权,所述第一影响是与所述GPS信号的接收相关的影响,所述第二影响是与所述第一影响不同的影响。
7.如权利要求5所述的推断方法,其特征在于,
在计算所述推断值和所述推断值的误差时,对GPS测位位置的误差赋予基于从针对所述GPS测位位置发生了基于与所述GPS信号的接收相关的影响的误差起、到与所述GPS信号的接收相关的影响消除为止的期间的加权。
8.如权利要求5~7中任意一项所述的推断方法,其特征在于,
推断与车辆当前所在地相关的状态量。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016-083340 | 2016-04-19 | ||
JP2016083340A JP6689659B2 (ja) | 2016-04-19 | 2016-04-19 | 位置推定装置、及び、推定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107305123A CN107305123A (zh) | 2017-10-31 |
CN107305123B true CN107305123B (zh) | 2020-10-13 |
Family
ID=58578900
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710254076.0A Active CN107305123B (zh) | 2016-04-19 | 2017-04-18 | 位置推断装置以及推断方法 |
Country Status (4)
Country | Link |
---|---|
US (1) | US10620320B2 (zh) |
EP (1) | EP3236289B1 (zh) |
JP (1) | JP6689659B2 (zh) |
CN (1) | CN107305123B (zh) |
Families Citing this family (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11349589B2 (en) | 2017-08-04 | 2022-05-31 | Metrom Rail, Llc | Methods and systems for decentralized rail signaling and positive train control |
CN111278688B (zh) * | 2017-11-03 | 2022-09-20 | 黑拉有限责任两合公司 | 驱动车辆的部件的方法、系统、计算机程序产品和计算机可读介质 |
WO2019143006A1 (ko) * | 2018-01-17 | 2019-07-25 | 한국항공대학교산학협력단 | 다중 gnss를 활용한 위치영역 hatch 필터 기반 위치 추정 방법 및 장치 |
CN109190811B (zh) * | 2018-08-20 | 2021-10-26 | 浙江工业大学 | 一种基于自适应扩展卡尔曼滤波的车辆速度跟踪方法 |
CN109275121B (zh) * | 2018-08-20 | 2021-08-03 | 浙江工业大学 | 一种基于自适应扩展卡尔曼滤波的车辆轨迹跟踪方法 |
JP7272910B2 (ja) * | 2019-09-03 | 2023-05-12 | 株式会社ゼンリンデータコム | 車載装置、状態推定方法及びプログラム |
JP7320755B2 (ja) * | 2020-01-21 | 2023-08-04 | パナソニックIpマネジメント株式会社 | 車両シミュレーションシステム、車両シミュレーション方法およびコンピュータプログラム |
CN111624626B (zh) * | 2020-04-29 | 2022-06-14 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 卫星导航系统欺骗干扰信号抑制方法 |
CN112590806B (zh) * | 2020-11-30 | 2022-08-30 | 上海欧菲智能车联科技有限公司 | 基于卡尔曼滤波的车辆信息处理方法、装置、设备和介质 |
CN112229418B (zh) * | 2020-12-11 | 2021-04-09 | 北京晶众智慧交通科技股份有限公司 | 基于obd盒子的车辆定位系统 |
CN112665593B (zh) * | 2020-12-17 | 2024-01-26 | 北京经纬恒润科技股份有限公司 | 一种车辆定位方法及装置 |
EP4060614A1 (en) * | 2021-03-19 | 2022-09-21 | Aptiv Technologies Limited | Method for determining noise statistics of object sensors |
US11644579B2 (en) * | 2021-03-30 | 2023-05-09 | Mitsubishi Electric Research Laboratories, Inc. | Probabilistic state tracking with multi-head measurement model |
CN114034298B (zh) * | 2021-11-04 | 2023-11-03 | 吉林大学 | 一种基于可重构智能表面的车辆定位方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102192745A (zh) * | 2010-02-24 | 2011-09-21 | 歌乐株式会社 | 位置推测装置及位置推测方法 |
WO2012137415A1 (ja) * | 2011-04-01 | 2012-10-11 | セイコーエプソン株式会社 | 位置算出方法及び位置算出装置 |
CN103492837A (zh) * | 2011-04-11 | 2014-01-01 | 歌乐株式会社 | 位置计算方法和位置计算装置 |
Family Cites Families (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE69420418T2 (de) * | 1994-11-29 | 2000-05-25 | Zanavy Informatics Kk | Navigationssystem mit Umschaltung, wenn ein Radiosignal nicht empfangen werden kann |
JP4145451B2 (ja) * | 1999-12-17 | 2008-09-03 | 古野電気株式会社 | ハイブリッド航法およびその装置 |
US6408245B1 (en) | 2000-08-03 | 2002-06-18 | American Gnc Corporation | Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit |
JP2002181926A (ja) * | 2000-12-15 | 2002-06-26 | Furuno Electric Co Ltd | 目標追尾方法およびその装置 |
JP5028851B2 (ja) * | 2006-04-24 | 2012-09-19 | 株式会社デンソー | 道路情報検出装置及びプログラム |
JP4229141B2 (ja) * | 2006-06-19 | 2009-02-25 | トヨタ自動車株式会社 | 車両状態量推定装置及びその装置を用いた車両操舵制御装置 |
US8600660B2 (en) | 2006-09-29 | 2013-12-03 | Honeywell International Inc. | Multipath modeling for deep integration |
JP5398120B2 (ja) * | 2007-03-22 | 2014-01-29 | 古野電気株式会社 | Gps複合航法装置 |
JP2010008095A (ja) | 2008-06-24 | 2010-01-14 | Aisin Aw Co Ltd | 自車位置補正システム及びプログラム |
US9116002B2 (en) * | 2009-08-27 | 2015-08-25 | Apple Inc. | Context determination to assist location determination accuracy |
CN101853243A (zh) * | 2010-04-01 | 2010-10-06 | 西北工业大学 | 系统模型未知的自适应卡尔曼滤波方法 |
JP5664059B2 (ja) * | 2010-09-17 | 2015-02-04 | 株式会社デンソー | 車両用軌跡推定装置 |
EP2555017B1 (en) * | 2011-08-03 | 2017-10-04 | Harman Becker Automotive Systems GmbH | Vehicle navigation on the basis of satellite positioning data and vehicle sensor data |
JP2015087227A (ja) * | 2013-10-30 | 2015-05-07 | セイコーエプソン株式会社 | 測位方法及び測位装置 |
JP6164054B2 (ja) | 2013-11-08 | 2017-07-19 | 富士通株式会社 | 情報処理装置、コンパイル方法およびコンパイラプログラム |
WO2017154131A1 (ja) * | 2016-03-09 | 2017-09-14 | 三菱電機株式会社 | 測位装置および測位方法 |
-
2016
- 2016-04-19 JP JP2016083340A patent/JP6689659B2/ja active Active
-
2017
- 2017-03-06 US US15/450,427 patent/US10620320B2/en active Active
- 2017-04-18 CN CN201710254076.0A patent/CN107305123B/zh active Active
- 2017-04-19 EP EP17167060.7A patent/EP3236289B1/en active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102192745A (zh) * | 2010-02-24 | 2011-09-21 | 歌乐株式会社 | 位置推测装置及位置推测方法 |
WO2012137415A1 (ja) * | 2011-04-01 | 2012-10-11 | セイコーエプソン株式会社 | 位置算出方法及び位置算出装置 |
CN103492837A (zh) * | 2011-04-11 | 2014-01-01 | 歌乐株式会社 | 位置计算方法和位置计算装置 |
Also Published As
Publication number | Publication date |
---|---|
JP2017194302A (ja) | 2017-10-26 |
US20170299729A1 (en) | 2017-10-19 |
JP6689659B2 (ja) | 2020-04-28 |
EP3236289A1 (en) | 2017-10-25 |
EP3236289B1 (en) | 2021-10-20 |
CN107305123A (zh) | 2017-10-31 |
US10620320B2 (en) | 2020-04-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107305123B (zh) | 位置推断装置以及推断方法 | |
CN108700423B (zh) | 车载装置及推定方法 | |
AU2009200190B2 (en) | Methods and systems for underwater navigation | |
JP5855249B2 (ja) | 測位装置 | |
US7987047B2 (en) | Navigation equipment | |
US11015957B2 (en) | Navigation system | |
US9423486B2 (en) | Position calculating method and position calculating device | |
EP3312634B1 (en) | Positioning apparatus | |
KR20120080489A (ko) | 항법 알고리즘을 이용한 위치 정보 검출 장치 및 방법 | |
JP2011209268A (ja) | 位置推定装置及びプログラム | |
US20140104101A1 (en) | Position calculating method and position calculating device | |
KR102172145B1 (ko) | 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치 | |
JP7272910B2 (ja) | 車載装置、状態推定方法及びプログラム | |
EP3798683A1 (en) | System and method for gnss position fix estimation using two or more antennas | |
JP2001264409A (ja) | カーナビゲーション装置におけるマルチパス判定方法 | |
KR101502721B1 (ko) | 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치 | |
JP2012098185A (ja) | 方位角推定装置及びプログラム | |
JP3705187B2 (ja) | ナビゲーション装置 | |
JP2007010554A (ja) | 測位装置 | |
KR20160129147A (ko) | 차량의 위치정보 산출 장치 및 방법 | |
KR100596637B1 (ko) | 기준점들의 가중치를 이용한 실시간 gis 업데이트 방법 | |
JP2023019895A (ja) | 受信信号の到来方向と衛星方位が不一致となる衛星の検出方法および検出装置ならびにgnss受信機 |
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 |