CN103492837A - 位置计算方法和位置计算装置 - Google Patents
位置计算方法和位置计算装置 Download PDFInfo
- Publication number
- CN103492837A CN103492837A CN201280018021.5A CN201280018021A CN103492837A CN 103492837 A CN103492837 A CN 103492837A CN 201280018021 A CN201280018021 A CN 201280018021A CN 103492837 A CN103492837 A CN 103492837A
- Authority
- CN
- China
- Prior art keywords
- link
- candidate point
- link candidate
- orientation
- probability
- 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.)
- Granted
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/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
- G01C21/30—Map- or contour-matching
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
- Instructional Devices (AREA)
Abstract
一种位置计算方法,作为使用移动体的现在位置、链路候选点位置、现在位置周边的区域所包含的多个链路的位置的误差方差、和多个链路的方位的误差方差进行的运算,根据现在位置与链路候选点位置之间的距离和移动体的移动方位与链路候选点方位的方位差,对包含至链路候选点位置的按每规定距离间隔得到的距离和方位差的多个数据进行累计,由此,按各链路候选点的每个链路计算移动体在与各链路候选点对应的道路上行驶的概率所对应的评价量,设移动体位于多个链路候选点中的概率最高的链路候选点,基于评价量计算现在位置。
Description
技术领域
本发明涉及计算移动体的位置的位置计算方法和位置计算装置。
背景技术
决定车辆的推测位置,计算由于推测位置的误差和道路地图的误差而产生的推测位置的极限误差,抽出位于推测位置的极限误差的范围的链路,并对这些链路实施地图匹配,计算车辆的位置。(参照专利文献1)
专利文献1的现有技术不仅考虑车辆的推测位置的误差而且考虑链路数据的误差地抽出链路,由此,能够根据车辆的推测位置、链路的误差,对链路进行匹配,判定是否从道路偏离(偏离)。
现有技术文献
专利文献
专利文献1:日本国特开平9-229698号公报
发明内容
发明要解决的技术问题
不能够应对由于行驶路径、GPS信号的接收状况等导致的车辆的推测位置和推测方位的误差、和因各地域、各地图制造商而不同的链路的误差,因此,导致在行驶链路的附近存在另外的链路的狭角分支、并行路等,匹配错误的链路。
用于解决问题的技术方案
本发明的第一方面提供一种位置计算方法,在该位置计算方法中,计算移动体的现在位置、移动速度和移动方位,基于现在位置、移动速度和移动方位,计算现在位置和移动方位的推测误差协方差,作为使用现在位置、移动方位、推测误差协方差、现在位置周边的区域所包含的多个链路候选点中的各链路候选点的链路候选点位置、各链路候选点的链路候选点方位、区域所包含的多个链路的位置的第一误差方差、和多个链路的方位的第二误差方差进行的运算,根据现在位置与链路候选点位置之间的距离和移动方位与链路候选点方位的方位差,对包含至链路候选点位置的按每规定距离间隔得到的距离和方位差的多个数据进行累计,由此,按各链路候选点的每个链路计算移动体在与各链路候选点对应的道路上行驶的概率所对应的评价量,令移动体位于多个链路候选点中的概率最高的链路候选点,基于评价量计算现在位置。
本发明的第二方面在第一方面的位置计算方法中,优选:第一误差方差基于距离计算,第二误差方差基于方位差计算,评价量通过对于多个数据将以下两个值之和累计而得到:将多个数据中的各数据所包含的距离的平方除以第一误差方差与现在位置的推测误差方差之和而得到的值;将多个数据中的各数据所包含的方位差的平方除以第二误差方差与移动方位的推测误差方差之和而得到的值。
本发明的第三方面在第一或者第二方面的位置计算方法中,优选:基于与现在位置和概率最高的链路候选点位置之间的距离相关的第一检测量是否大于第一阈值,判定现在位置与该链路候选点位置是否存在差,基于与移动方位和概率最高的链路候选点方位的方位差相关的第二检测量是否大于第二阈值,判定移动方位与该链路候选点方位是否存在差,当现在位置与该链路候选点位置存在差时,或者移动方位与该链路候选点方位存在差时,判定移动体偏离与概率最高的链路候选点对应的道路,并且,当现在位置与该链路候选点位置不存在差时,且移动方位与该链路候选点方位不存在差时,判定移动体在与概率最高的链路候选点对应的道路上行驶,当判定出移动体偏离与概率最高的链路候选点对应的道路时,令移动体正位于现在位置并进行计算,并且,当判定出移动体在与概率最高的链路候选点对应的道路上行驶时,令移动体正位于该链路候选点,计算现在位置。
本发明的第四方面在第三方面的位置计算方法中,优选:当判定出移动体未偏离与概率最高的链路候选点对应的道路时,根据推测误差协方差是否为第三阈值以下,判定是否计算第一误差方差,当判定出移动体未偏离与概率最高的链路候选点对应的道路时,根据推测误差协方差是否为第四阈值以下,判定是否计算第二误差方差。
本发明的第五方面在第三方面的位置计算方法中,优选:根据测位卫星的卫星位置和现在位置计算DOP值,基于现在位置的变化、移动速度的变化和移动方位的变化,判定现在位置是否为异常和移动方位是否为异常,当判定出移动体未偏离与概率最高的链路候选点对应的道路,且判定出现在位置不为异常时,根据DOP值是否为第五阈值以下,判定是否计算第一误差方差,当判定出移动体未偏离与概率最高的链路候选点对应的道路,且判定出现在位置不为异常时,根据DOP值是否为第五阈值以下,判定是否计算第二误差方差。
本发明的第六方面在第三至第五中任一方面的位置计算方法中,优选:第一检测量和第二检测量的值是将以下两个值无量纲化(量纲为1)并相加而得到的值:将现在位置与概率最高的链路候选点位置之间的距离的平方,除以现在位置和概率最高的链路候选点位置的误差协方差之和而得到的值;将移动方位与概率最高的链路候选点方位的方位差的平方,除以移动方位和概率最高的链路候选点方位的误差方差之和而得到的值。
本发明的第七方面在第三至第六中任一方面的位置计算方法中,优选:第一检测量是将现在位置与概率最高的链路候选点位置之间的距离的平方,除以现在位置和概率最高的链路候选点位置的误差协方差之和而得到的值,第二检测量是将移动方位与概率最高的链路候选点方位的方位差的平方,除以移动方位和概率最高的链路候选点方位的误差方差之和而得到的值。
本发明的第八方面在第一至第七中任一项的位置计算方法中,优选:当构成多个链路的第一链路和第二链路从分支点节点分支,且概率最高的链路候选点包含于第一链路时,在移动体通过与分支点节点对应的交差路口后,直至该链路候选点的链路候选点位置和与第二链路对应的区间道路的间隔变得比规定值大的期间,减少多个数据的数据数量。
本发明的第九方面在第一至第七中任一项的位置计算方法中,优选:在判定移动体在通过与构成多个链路的第一链路和第二链路分支的分支点节点对应的交差路口后,在包含与概率最高的链路候选点不同的链路候选点的链路所对应的道路上行驶的第一期间,减少多个数据的数据数量。
本发明的第十方面在第九方面的位置计算方法中,优选:在基于从现在位置附近的起点链路至移动体的目的地附近的终点链路为止的引导路径,对移动体进行路径引导,且包含概率最高的链路候选点的链路包含于引导路径的情况下,当判定出移动体未道路偏离,且交差路口与该链路候选点的链路候选点位置之间的距离为规定值以下时,对显示声音输出装置输出与引导路径相关的向导显示(指引显示)和向导声音,在基于引导路径对移动体进行路径引导,且包含概率最高的链路候选点的链路不包含于引导路径的情况下,当判定出移动体通过交差路口后,偏离了包含与概率最高的链路候选点不同的链路候选点的链路所对应的道路时,探索从现在位置附近的起点链路至移动体的目的地附近的终点链路的新的引导路径。
本发明的第十一方面的位置计算装置具备:移动信息计算部,其计算移动体的现在位置、移动速度和移动方位;推测误差协方差计算部,其基于现在位置、移动速度和移动方位,计算现在位置和移动方位的推测误差协方差;概率计算部,其使用现在位置、移动方位、推测误差协方差、现在位置周边的区域所包含的多个链路候选点中的各链路候选点的链路候选点位置、各链路候选点的链路候选点方位、区域所包含的多个链路的位置的第一误差方差、和多个链路的方位的第二误差方差,进行运算,通过根据现在位置与链路候选点位置之间的距离和移动方位与链路候选点方位的方位差,对包含至链路候选点位置的按每规定距离间隔得到的距离和方位差的多个数据进行累计,按各链路候选点的每个链路计算移动体在与各链路候选点对应的道路上行驶的概率所对应的评价量;和位置计算部,其设移动体位于多个链路候选点中的概率最高的链路候选点,基于评价量计算现在位置。
本发明的第十二方面在第十一方面的位置计算装置中,优选还具备误差方差计算部,其基于距离计算第一误差方差,并且基于方位差计算第二误差方差。概率计算部通过对于多个数据将以下两个值之和累计来计算评价量:将多个数据中的各数据所包含的距离的平方除以第一误差方差与现在位置的推测误差方差之和而得到的值;将多个数据中的各数据所包含的方位差的平方除以第二误差方差与移动方位的推测误差方差之和而得到的值。
本发明的第十三方面在第十一或者第十二方面的位置计算装置中,优选还具备:第一判定部,其基于将现在位置与概率最高的链路候选点的链路候选点位置之间的距离的平方,除以第一误差方差与现在位置的推测误差方差之和而得到的值是否大于第一阈值,判定现在位置与该链路候选点位置是否存在差;第二判定部,其基于将移动方位与概率最高的链路候选点的链路候选点方位的方位差,除以第二误差方差与移动方位的推测误差方差之和的平方根而得到的值是否大于第二阈值,判定移动方位与该链路候选点方位是否存在差;和第三判定部,其在现在位置与该链路候选点位置存在差时,或者移动方位与该链路候选点方位存在差时,判定移动体偏离了与概率最高的链路候选点对应的道路,并且,在现在位置与该链路候选点位置不存在差时,且移动方位与该链路候选点方位不存在差时,判定移动体在与概率最高的链路候选点对应的道路上行驶。位置计算部在由第三判定部判定出移动体偏离了与概率最高的链路候选点对应的道路时,令移动体位于现在位置并进行计算,并且,在判定出移动体在与概率最高的链路候选点对应的道路上行驶时,令移动体位于该链路候选点,计算现在位置。
本发明的第十四方面在第十一至第十三中任一方面的位置计算装置中,优选还具备:接收部,其接收从测位卫星发出的测位信号;和测量部,其测量包含移动体的速度、角速度和加速度中的至少一个的传感器数据。移动信息计算部基于测位信号和传感器数据,计算现在位置、移动速度和移动方位,推测误差协方差计算部使用测位信号和传感器数据计算推测误差协方差。
本发明的第十五方面在第十一至第十四任一方面的位置计算装置中,优选还具备设定部,其进行以下设定:当构成多个链路的第一链路和第二链路从分支点节点分支,且概率最高的链路候选点包含于第一链路时,在移动体通过与分支点节点对应的交差路口后,直至该链路候选点的链路候选点位置和与第二链路对应的区间道路的间隔变得比规定值大的期间,减少多个数据的数据数量。
发明效果
根据本发明,能够降低计算移动体的位置时的道路偏离判定的错误。
附图说明
图1是表示一实施方式的位置计算装置的结构的图。
图2是表示位置计算装置的动作的图。
图3是表示位置计算装置的地图匹配处理的动作的图。
图4是表示读取地图信息时的4个网格区域中的位置计算装置的位置的图。
图5是表示链路间的距离的图。
图6是表示位置计算装置和链路候选点的位置的图。
图7是表示位置计算装置的道路偏离判定处理的动作的图。
图8是表示位置计算装置的地图精度计算处理的动作的图。
图9是表示分支后错误匹配继续的状况的图。
图10是表示分支后从错误匹配成为正确的匹配的状况的图。
图11是表示位置计算装置的道路偏离判定处理的动作的图。
图12是表示分支后的地图匹配的状况的图。
图13是表示分支后的地图匹配的状况的图。
图14是表示位置计算装置的道路偏离判定处理的动作的图。
图15是表示位置计算装置的距离序列数据数量的设定处理的动作的图。
图16是说明刚分支后的第一链路候选点的显示的图。
图17是表示位置计算装置的显示和声音的输出处理的动作的图。
图18是说明刚分支后的第一链路候选点的显示的图。
图19是表示位置计算装置的显示和声音的输出处理的动作的图。
图20是表示位置计算装置的路径探索的实施判定处理的动作的图。
具体实施方式
图1表示本发明的一实施方式中的位置计算装置100的结构。位置计算装置100包括接收装置101、速度传感器装置102、角速度传感器装置103、加速度传感器装置104、路径探索指示装置105、地图信息存储装置106、显示声音输出装置107、地图精度存储装置108和运算装置110。
接收装置101包含天线,具有降频、模拟/数字转换、正交检波,C/A(coarse/acquisition:粗/捕获)代码生成、相关检测、解码的处理功能。GPS卫星、GLONASS卫星、模拟卫星等的测位卫星是从宇宙或地面上发出用于测位的信号的装置。使用天线接收从测位卫星发送来的信号(测位信号),检测包含测位卫星的轨道信息、发送状态的信息、电离层延迟计算参数等的导航电文,测量接收时刻、模拟距离(距卫星的距离)、多普勒频率和信号强度等的观测数据。基于接收时刻和轨道信息计算测位卫星的位置,基于测位卫星的位置和模拟距离计算接收装置101的位置(接收位置)。另外,根据基于轨道信息求出的接收时刻附近的测位卫星的位置,计算测位卫星的速度,基于测位卫星的位置和速度、以及多普勒频率,计算接收装置101的速度和方位(速度矢量、接收速度和接收方位)。
车速传感器等的速度传感器装置102在搭载有位置计算装置100的汽车等的移动体中设置在车轴等,计算与车轴的旋转对应的脉冲数,输出脉冲数。陀螺仪等的角速度传感器装置103输出与角速度对应的信号。加速度计等的加速度传感器装置104设置在与汽车等的移动体的行进方向垂直的方向上,输出与加速度对应的信号。
遥控器、触摸屏、扩音器等的路径探索指示装置105输出被输入的目的地。
由CPU(central processing unit,中央运算处理装置)和存储器等结构的运算装置110包括传感器输出校正装置111、接收异常判定装置112、测位运算装置113、地图匹配装置114、路径探索装置115、路径引导装置116、和地图精度计算装置117。
传感器输出校正装置111将来自速度传感器装置102的输出与速度传感器装置102的比例因子相乘,计算速度(传感器速度)。从来自角速度传感器装置103的输出减去角速度传感器装置103的偏压(bias),乘以角速度传感器装置103的比例因子,由此计算角速度(传感器角速度)。另外,从来自加速度传感器装置104的输出减去加速度传感器装置104的偏压,计算加速度(传感器加速度)。
接收异常判定装置112使用来自接收装置101的接收位置、接收速度和接收方位、来自传感器输出校正装置111的传感器速度、传感器角速度、传感器加速度,判定接收位置、接收速度和接收方位的异常。
测位运算装置113使用来自接收异常判定装置112的接收位置、接收速度和接收方位、它们的接收异常判定结果、来自传感器输出校正装置111的传感器速度、传感器角速度、传感器加速度,计算位置计算装置100的位置、方位和速度、位置和方位的误差的协方差。
地图匹配装置114读取在地图信息存储装置106中存储的链路信息。基于来自测位运算装置113的位置计算装置100的位置和方位、它们的协方差,计算从位置计算装置100的位置向链路垂下垂线而得到的点(链路候选点)的评价量,将在道路上行驶的具有概率最高的评价量的链路候选点设为第一链路候选点。基于节点的位置、链路方位、它们的方差、道路宽度信息、位置计算装置100的位置和方位、它们的协方差,判定位置计算装置100是否从道路偏离。
路径探索装置115基于来自地图匹配装置114的第一链路候选点的链路序号、离目的地最近的链路序号,探索将这些链路连接的多个路径,选择距离最短的路径。
路径引导装置116从地图匹配装置114取得第一链路候选点的链路序号和位置,从路径探索装置115取得引导路径的链路序号,从地图信息存储装置106取得向导显示等的信息。左右转弯的分支与第一链路候选点的位置的距离为规定的值以下时,将引导开始信号开通,将表示引导方向的引导方向信号、向导显示、引导开始信号等的引导信息发送至显示声音输出装置107。另外,在第一链路候选点的链路序号与引导路径的链路序号不同的情况下,将路径探索开始信号开通,并将路径探索开始信号发送至路径探索装置115。
地图精度计算装置117在判定为未偏离道路且推测位置的误差的方差为阈值以下的情况下,计算从第一链路候选点至推测位置的矢量,由北方的矢量成分决定正负,使用矢量的大小和正负,计算现在位置中的链路位置的误差。另外,在判定为未偏离道路且推测方位的误差的方差为阈值以下的情况下,计算推测方位与第一链路候选点的方位的差,作为现在位置中的链路方位的误差。对包含现在位置的规定区域中的链路位置及方位的误差的累计值及平方和,分别加上现在位置中的链路位置及方位的误差和平方值,对包含现在位置的规定区域中的链路位置及方位的误差的统计数加上1。使用链路位置和方位的误差的累计值、平方和及统计数,计算包含现在位置的规定区域中的链路位置和方位的误差方差。
硬盘、存储器等的地图信息存储装置106按每个规定的网格区域存储节点序号、节点座標、链路序号、链路的道路宽度、属性等的链路信息、以及向导显示等的信息。节点是道路上的点,链路是连接节点的直线。
监控器和扬声器等的显示声音输出装置107从地图匹配装置114取得位置计算装置100的位置及方位、第一链路候选点的链路序号、位置及方位、道路偏离判定结果等的地图匹配结果,从路径引导装置116取得引导开始信号和引导信息,从地图信息存储装置106读取位置计算装置的位置周边的链路信息,描绘链路以及第一链路候选点的位置和方位。另外,在引导开始信号开通的情况下,输出向导显示,用声音输出引导方向。
硬盘、存储器等的地图精度存储装置108存储有每个规定区域的与链路位置的误差和方位的误差相关的方差、统计数、累计值及平方和的累计值。
使用图2说明图1所示的位置计算装置100的工作顺序。
步骤S201中,接收装置101用天线接收从测位卫星发出的信号(测位信号),检测包含测位卫星的轨道信息、发送状态的信息、电离层延迟计算参数等的向导电文,测量接收时刻、模拟距离(距卫星的距离)、多普勒频率和信号强度等的观测数据。基于接收时刻和轨道信息计算测位卫星的位置,基于测位卫星的位置和模拟距离计算接收位置。另外,根据基于轨道信息求出的接收时刻附近的测位卫星的位置计算测位卫星的速度,基于测位卫星的位置和速度、以及多普勒频率计算接收速度和接收方位,对接收异常判定装置112发送接收位置、接收速度和接收方位。接收异常判定装置112接收这些信息。
步骤S202中,速度传感器装置102测量与移动体的车轴的旋转相伴的脉冲数,并发送至传感器输出校正装置111。传感器输出校正装置111接收速度传感器装置102的输出。角速度传感器装置103将与移动体的角速度对应的信号发送至传感器输出校正装置111。传感器输出校正装置111接收角速度传感器装置103的输出。加速度传感器装置104将与移动体的加速度对应的信号发送至传感器输出校正装置111。传感器输出校正装置111接收加速度传感器装置104的输出。
步骤S203中,传感器输出校正装置111将来自速度传感器装置102的输出与速度传感器装置102的比例因子相乘,计算传感器速度。从来自角速度传感器装置103的输出减去角速度传感器装置103的偏压,乘以角速度传感器装置103的比例因子,由此计算传感器角速度。另外,从来自加速度传感器装置104的输出减去加速度传感器装置104的偏压,由此计算传感器加速度。将计算的值发送至接收异常判定装置112和测位运算装置113。
步骤S204中,接收异常判定装置112从接收装置101接收接收位置、接收速度和接收方位,从传感器输出校正装置111接收传感器速度、传感器角速度和传感器加速度。根据传感器速度和传感器角速度计算规定时间的位置变化,在该位置变化与规定时间的接收位置的变化之差为阈值以上的情况下,判定接收位置为异常,在该位置变化与规定时间的接收位置的变化之差不为阈值以上的情况下,判定接收位置为正常。计算接收速度与传感器速度之差,该差为阈值以上的情况下,判定接收位置为异常,该差不为阈值以上的情况下,判定接收速度为正常。根据传感器角速度计算规定时间的方位变化,在该方位变化与规定时间的接收方位的变化之差为阈值以上的情况下,判定接收方位为异常,该差不为阈值以上的情况下,判定为正常。将接收异常判定结果、接收位置、接收速度和接收方位发送至测位运算装置113。
步骤S205中,测位运算装置113从接收异常判定装置112接收接收异常判定结果、接收位置、接收速度和接收方位,从传感器输出校正装置111接收传感器速度、传感器角速度、传感器加速度,执行测位处理。在测位处理的工作顺序中,当设位置计算装置的位置、行进方向的速度和加速度、方位、方位的角速度和倾角为状态量,设传感器速度、传感器角速度、传感器加速度、接收位置、接收速度和接收方位为观测量时,由式(1)表示的状态方程式(连续型)、以及由式(2)表示的观测方程式(连续型)成立。式(1)中,加速度和角速度为一阶马尔可夫过程(一阶概率分布过程)。在此,x(t)和y(t)为位置计算装置100的经度方向的位置和纬度方向的位置(现在位置),v(t)和a(t)为行进方向(移动方位)的速度(移动速度)和加速度,θ(t)和ω(t)为方位(移动方位)和方位的角速度,(t)为倾角,θp为方位(移动方位)的预测值,p为倾角的预测值,αa和αω为加速度的时间常数的倒数和角速度的时常数的倒数,σa为加速度的标准偏差、σω和σ为方位(移动方位)的角速度的标准偏差和倾角的标准偏差,w(t)为平均0,标准偏差1的白色噪声,η(t)为状态量矢量,vs(t)为传感器速度,gs(t)为传感器加速度,ωs(t)为传感器角速度、(xr(t)、yr(t)、zr(t))为接收位置,vr(t)为接收速度,θr(t)为接收方位,ε(t)为观测噪声矢量,y(t)为观测量矢量y(t),F、G以及H为矩阵。方位(移动方位)的预测值θp和倾角的预测值p能够根据由式(5)计算的预测量矢量得到。
[数1]
【数1】
[数2]
【数2】
测位运算装置113在接收位置、接收速度和接收方位为正常(有効)的情况下,使用式(3)~(7)计算位置计算装置100的推测位置、推测速度、推测加速度、推测方位、推测角速度和推测倾角。通过式(3)~(7),能够进行基于卡尔曼滤波算法的计算。在此,K(k)为增益矩阵,R为观测噪声矩阵,η(k|k)为推测量矢量,η(k|k+1)为预测量矢量,P(k|k)和P(k|k+1)为推测量矢量η(k|k)的推测误差协方差矩阵和预测量矢量η(k|k+1)的推测误差协方差矩阵,Φ(Δt、αa、αω)为状态迁移矩阵,Δt为采样间隔,Q(k)为系统噪声矩阵。
[数3]
【数3】
K(k)=P(k|k-1)HT(k)[H(k)P(k|k-1)HT(k)+R]-1 (3)
[数4]
【数4】
η(k|k)=η(k|k-1)+K(k)[y(k)-Hη(k|k-1)] (4)
[数5]
【数5】
η(k+1|k)=Φ(Δt,αa,αω)η(k|k) (5)
[数6]
【数6】
P(k|k)=P(k|k-1)-K(k)H(k)P(k|k-1) (6)
[数7]
【数7】
P(k|k-1)=Φ(Δt,α)P(k|k-1)ΦT(Δt,αa,αω)+Q(k) (7)
在此,在判定出接收位置为异常的情况下,从观测量矢量y、矩阵H和观测噪声矩阵R削除与接收位置相关的要素。在判定出接收速度为异常的情况下,从观测量矢量y、矩阵H和观测噪声矩阵R削除与接收速度相关的要素。在判定出接收方位为异常的情况下,从观测量矢量y、矩阵H和观测噪声矩阵R削除与接收方位相关的要素。另外,在没有来自接收装置101的接收位置、接收速度和接收方位的输出的情况下,从观测量矢量y、矩阵H和观测噪声矩阵R削除与接收位置、接收速度和接收方位相关的要素。
测位运算装置113将位置计算装置100的位置(现在位置)、方位(移动方位)和速度(移动速度)、位置和方位的推测误差协方差发送至地图匹配装置114和地图精度计算装置117。
步骤S206中,地图匹配装置114从地图信息存储装置106读取节点序号、节点位置、链路序号、链路的道路宽度等的信息,并且从地图精度存储装置108读取包含位置计算装置100的位置的区域中的链路的位置误差的方差和方位误差的方差。前进规定数量的链路候选点,在其规定距离的前方的链路制作链路候选点。对于全部的链路候选点,基于链路候选点的链路方位和包含位置计算装置100的位置的规定区域的链路的位置误差的方差,计算链路候选点的位置误差的协方差。将链路候选点的方位的方差设为包含位置计算装置100的位置的规定区域的链路的方位误差的方差。基于链路候选点的位置误差的协方差和包含位置计算装置100的位置的规定区域的链路的方位误差的方差、位置计算装置100的位置的协方差和方位的协方差、位置计算装置100和链路候选点的位置及方位,计算链路候选点的评价量。将这些链路候选点之中的、具有在道路上行驶的概率最高的评价量的链路候选点决定为第一链路候选点。接着,基于链路候选点的位置和方位、位置误差的协方差、方位误差的方差、道路宽度信息、位置计算装置100的位置和方位、以及位置计算装置100的位置和方位各自的协方差,判定位置计算装置100是否偏离道路。地图匹配装置114将第一链路候选点的位置、方位和链路序号、以及道路偏离判定结果发送至路径探索装置115和路径引导装置116,并且,对显示声音输出装置107发送位置计算装置100的位置和方位、以及第一链路候选点的链路序号、位置和方位和道路偏离判定结果。对于步骤S206中的处理的详细,使用图3和7在后文述说。
步骤S207中,地图精度计算装置117从测位运算装置113接收推测位置和推测方位、它们的误差的方差,从地图匹配装置114接收道路偏离的判定结果和第一链路候选点的位置和方位,并且,对于包含推测位置的规定区域中的链路位置的误差和方位的误差,分别读取统计数、累计值及平方和的累计值。在判定出未偏离道路且推测位置的误差的方差为阈值以下的情况下,计算从第一链路候选点至推测位置的矢量,利用北方的矢量成分决定正负,使用矢量的大小和正负,计算现在位置中的链路位置的误差。在此,在矢量的方向处于从东方向反时针旋转至西方向的(包含正东)范围中的情况下为正,处于从西方向逆时针旋转至东方向的(包含正西范围中)的情况下为负。对包含现在位置的规定区域中的链路位置误差的累计值及平方和的累计值加上链路位置的误差及其平方值,对包含现在位置的规定区域中的链路位置误差的统计数加上1。使用链路位置误差的累计值、平方和的累计值及统计数,计算包含现在位置的规定区域中的链路位置误差的方差。另外,在判定出未偏离道路且推测方位的误差的方差为阈值以下的情况下,将推测方位与第一链路候选点的方位之差设为现在位置中的链路方位的误差。对包含现在位置的规定区域中的链路方位的误差的累计值及平方和的累计值,分别加上现在位置中的链路方位的误差及其平方值,对包含现在位置的规定区域中的链路方位的误差的统计数加上1。使用链路方位的误差的累计值、平方和的累计值及统计数,计算包含现在位置的规定区域中的链路方位的误差方差。地图精度计算装置117将与包含推测位置的规定区域中的链路位置的误差和方位的误差的方差、统计数、累计值和平方和的累计值,存储于地图精度存储装置108。
步骤S208中,对路径探索指示装置105输入有目的地的情况下,路径探索指示装置105将所输入的目的地发送至路径探索装置115,进入步骤S211。在未输入目的地的情况下,进入步骤S209。
步骤S209中,在具有引导路径的情况下,进入步骤S210。在没有引导路径的情况下,进入步骤S213。
步骤S210中,在具有第一链路候选点的链路包含于引导路径的链路的情况下,进入步骤S212。在不被包含的情况下,进入步骤S211。
步骤S211中,路径探索装置115从地图信息存储装置106读取目的地附近的链路信息,计算从目的地至链路的距离,将最近的链路序号设为目的地的链路。从地图匹配装置114接收第一链路候选点的链路序号、位置和方位,探索从第一链路候选点的链路朝向其方位直至目的地的链路为止连接的多个路径,并将最短的距离的路径设为引导路径。路径探索装置115将引导路径上的链路序号发送至路径引导装置116。
步骤S212中,路径引导装置116从路径探索装置115接收引导路径上的链路序号,从地图匹配装置114接收第一链路候选点的位置、方位和链路序号、以及道路偏离判定结果,从地图信息存储装置106读取向导显示等的信息。道路偏离判定结果为道路行驶,即判定出未偏离道路且左右转弯的分支与第一链路候选点的位置之间的距离为规定值以下时,将引导开始信号开通,并将表示引导方向的引导方向信号、向导显示、引导开始信号等的引导信息发送至显示声音输出装置107。
步骤S213中,显示声音输出装置107从地图匹配装置114取得位置计算装置100的位置及方位、第一链路候选点的链路序号、位置及方位、以及道路偏离判定结果,从路径引导装置116取得引导开始信号和引导信息,从地图信息存储装置106读取位置计算装置100的位置周边的链路信息。在道路偏离判定结果为道路行驶的情况下,即判定出未偏离道路的情况下,描绘由第一链路候选点的位置和方位表示的汽车标志、以及第一链路候选点的位置周边的链路。在道路偏离判定结果为道路偏离的情况下,描绘由位置计算装置100的位置和方位表示的汽车标志、以及位置计算装置100的位置周边的链路。另外,在引导开始信号设为开通的情况下,输出向导显示,用声音输出引导方向。
使用图3说明图1所示的位置计算装置100的工作顺序(图2)的步骤S206。
步骤S401中,地图匹配装置114在过去从地图信息存储装置106读取过链路信息的情况下,进入步骤S402。否则,进入步骤S403。
步骤S402中,在位置计算装置100的位置601与4个网格区域602的端的距离比规定距离短的情况下,即位置计算装置100的位置601位于图4(a)所示的4个网格区域602的端附近的区域603(斜线部)的情况下,进入步骤S403。否则,进入步骤S404。
步骤S403中,地图匹配装置114读取:包含位置计算装置的位置的网格区域;和距位置计算装置100的位置601近的3个网格区域的链路信息。如图4(b)所示,在由这3个网格区域和包含位置计算装置100的位置601的网格区域构成的4个网格区域612中,位置计算装置100的位置601不包含于4个网格区域612的端附近的区域613(斜线部),进入步骤S404。
步骤S404中,在存在链路候选点的情况下,进入步骤S406。在没有链路候选点的情况下,进入步骤S405。
步骤S405中,地图匹配装置114计算包含位置计算装置100的位置的网格区域中的全部的链路与位置计算装置100的位置的距离,以距离短的顺序选择规定数量的链路,对这些链路生成链路候选点。将链路候选点的链路方位与位置计算装置的方位之差为90度以下的链路方位设定为链路候选点的方位。
步骤S406中,地图匹配装置114从测位运算装置113接收位置计算装置的位置、方位和速度、以及位置计算装置的位置和方位的误差协方差。使各链路候选点的位置向前方沿链路移动将位置计算装置100的速度与处理周期相乘而得到的行驶距离。
步骤S407中,地图匹配装置114将位置计算装置100的速度与处理周期相乘,并与前次的行驶距离相加。在行驶距离为规定距离以上的情况下,为了进行地图匹配,将行驶距离设为零,进入步骤S408。否则,进入步骤S414。
步骤S408中,地图匹配装置114对位于各链路候选点的规定距离的前方的全部链路新追加链路候选点。
步骤S409中,地图匹配装置114在前次的第一链路候选点(完成前方移动)位于从规定角度以下的分支至链路间的距离为规定距离以下的地点为止的情况下,将评价量的计算中使用的规定距离间隔的位置计算装置100的位置和方位的距离序列数据的数量设定为比通常的第一规定数小的第二规定数。将基于每个规定距离间隔的位置计算装置100的位置和方位的数据称为距离序列数据。
前次的第一链路候选点位于规定角度以下的分支之前,且与链路间的距离比规定距离大的地点相比更靠前的情况下,设定链路间的距离比规定距离大的距离序列数据的数量。但是,距离序列数据的数量比通常的第一规定数大的情况下,设定通常的第一规定数。在前次的第一链路候选点通过了比规定角度大的分支的情况下,对距离序列数据设定通常的第一规定数。图5表示从包含与链路701对应的区间道路的2车线的道路706,分支出包含与链路702对应的区间道路的1车线的道路707的状况。如图5所示,考虑到链路701所表示的道路和链路702所表示的道路的宽度,链路701与链路702间的距离703为接近彼此道路的车线的中央704与705间的距离。此外,由于中央704和705的任一方位于第一链路候选点的附近,因此,距离703可以置换为链路701和702中的不包含第一链路候选点的链路与第一链路候选点的距离。
步骤S410中,地图匹配装置114从地图精度存储装置108读取包含位置计算装置100的位置的规定区域的链路位置和方位的方差。使用式(8)~(11)计算各链路候选点的评价量T。在此,(xe,ye)为位置计算装置100的位置,θe为位置计算装置100的方位,(xl,yl)为从位置计算装置100的位置向链路垂下垂线而得到的链路候选点的位置,θl为链路候选点的方位,Σe为位置计算装置100的位置和方位的误差协方差矩阵,Σl为链路候选点的位置和方位的误差协方差矩阵,σpl和σθl为包含位置计算装置100的位置的周边区域的链路的位置误差的标准偏差和方位误差的标准偏差,m为距离序列数据的序号,n为距离序列数据的数量。其中,考虑链路803的道路宽度、和搭载有位置计算装置100的移动体的行驶方向,如图6所示,将链路候选点802的位置(xl,yl)配置在最接近与位置计算装置100的位置对应的位置801的车线的中央。如上所述,多个距离序列数据各自所包含的位置计算装置100的位置(xe,ye)的历史记录在步骤S407中按每规定距离配置为等距离间隔。式(8)中,评价量T通过将从距离序列数据的序号i=m-n+1至i=m的n个距离序列数据、即从链路候选点的位置(xl,yl)起向跟前n个(移动方向的逆方向上n个)的距离序列数据进行累计而求得。距离序列数据的序号i中的距离序列数据包含:表示从位置计算装置100的位置(xe,ye)至链路候选点的位置(xl,yl)的距离的矢量(Δx,Δy);位置计算装置100的方位θe和链路候选点的方位θl的方位差Δθ;位置计算装置和链路候选点的位置和方位的误差方差;式(8)的总和Σ内的要素的值等。总和Σ内的要素的值是以下两个值之和:对应1个距离序列数据,将位置计算装置100的位置(xe,ye)与链路候选点的位置(xl,yl)之间的距离无量纲化(量纲为1)而得到的值;将位置计算装置100的移动方位θe与链路候选点的方位θl的方位差Δθ无量纲化而得到的值。将位置计算装置100的位置(xe,ye)与链路候选点的位置(xl,yl)之间的距离无量纲化而得到的值可以通过以下方法得到:将表示该距离的矢量(Δx,Δy)的长度(距离)的平方,除以包含位置计算装置100的位置(xe,ye)的周边区域的链路的位置误差的标准偏差σpl的平方(误差方差)与位置计算装置100的位置(xe,ye)的推测误差方差之和而得到。将位置计算装置100的移动方位θe与链路候选点的方位θl的方位差Δθ无量纲化而得到的值可以通过以下方法得到:将该方位差Δθ的平方,除以上述周边区域的链路的方位误差的标准偏差σθl的平方(误差方差)与位置计算装置100的方位θe的推测误差方差之和而得到。当假定位置计算装置100和链路候选点802的位置和方位的误差协方差遵循正规分布时,评价量T遵循自由度2×n的χ平方分布。自由度2×n取决于由位置(距离)和方位构成的2种参数、和距离序列数据的数量n。因此,在对该自由度2的χ平方分布的概率密度函数代入评价量T时,位置计算装置在道路(链路)上行驶时,能够转换为根据位置计算装置100的位置及方位和链路候选点的位置及方位计算出的差所产生的概率、即考虑了位置和方位的差的存在概率。换言之,能够转换为位置计算装置100在道路(链路)上行驶的概率。
[数8]
【数8】
[数9]
【数9】
[数10]
【数10】
∑d(i)=∑e(i)+∑l(i) (10)
[数11]
【数11】
步骤S411中,地图匹配装置114从链路候选点中以评价量T小(在道路上行驶的概率高)的顺序选择规定数量的链路候选点,将它们之外的链路候选点的信息除去。这是为了降低地图匹配的运算量。
步骤S412中,地图匹配装置114从链路候选点中将评价量T最小的(在道路上行驶的概率最高的)链路候选点设为第一链路候选点。
步骤S413中,地图匹配装置114基于位置计算装置100的位置和方位、链路候选点的位置和方位、它们的协方差、以及接收装置101正在接收的相对方的卫星数量,来判定是否偏离了道路。对于步骤S413中的处理的详细内容,使用图7在后文述说。
步骤S414中,地图匹配装置114对路径探索装置115和路径引导装置116发送位置计算装置100的位置和方位、第一链路候选点的位置、方位和链路序号、以及道路偏离判定结果。
使用图7说明图1所示的位置计算装置100的工作顺序(图3)的步骤S413。
步骤S901中,如下文所示,设立本车的推测位置与链路的距离是否存在差的假设。
·解消假设Hp0:位置计算装置100的推测位置与链路的距离不存在差。
·对立假设Hp1:位置计算装置100的推测位置与链路的距离存在差。
使用式(12)~(15),将位置计算装置100的推测位置与链路的距离的平方,除以位置计算装置100的推测位置的误差协方差Σpe与链路位置的误差协方差Σpl之和,由此,进行无量纲化,计算检测量Tp。式(12)中,位置计算装置100的推测位置与链路的距离(位置的差)由位置数据Δx和Δy表示。
[数12]
【数12】
[数13]
【数13】
[数14]
【数14】
∑p=∑pe+∑pl (14)
[数15]
【数15】
当假设位置计算装置100的推测位置的误差和链路位置的误差遵循正规分布时,检测量Tp遵循自由度1的χ平方分布。自由度1取决于仅位置(距离)的1种参数。所以,决定显著性水平α,基于自由度1的χ平方分布进行针对该假设的检测时,能够如以下方式选择假设。在检测量Tp为自由度1和显著性水平α的χ平方值以下的情况下,不排除解消假设Hp0,判定本车的推测位置与链路的距离不存在差。在检测量Tp大于自由度1和显著性水平α的χ平方值的情况下,排除解消假设Hp0,判定本车的推测位置与链路的距离存在差。
步骤S902中,如下所述,设立是否位置计算装置100的推测方位与链路方位存在差的假设。
·解消假设Hθ0:位置计算装置100的推测方位与链路方位不存在差。
·对立假设Hθ1:位置计算装置100的推测方位与链路方位存在差。
使用式(16)~(18),将位置计算装置100的推测方位与链路方位的方位差,除以位置计算装置100的推测方位的误差协方差(σθe)^2与链路位置的误差协方差(σθl)^2之和的平方根,由此,进行无量纲化,计算检测量Tθ。式(16)中,位置计算装置100的推测方位与链路候选点的方位之差由Δθ表示。
[数16]
【数16】
[数17]
【数17】
Δθ=θe-θl (17)
[数18]
【数18】
在位置计算装置100的推测方位的误差和链路方位的误差遵循正规分布时,检测量Tθ遵循正规分布。所以,决定显著性水平α,基于正规分布进行针对该假设的双侧检验,能够如以下方式选择假设。检测量Tθ为显著性水平α的正规分布的值以下的情况下,不排除解消假设Hθ0,判定位置计算装置100的推测方位与链路方位不存在差。检测量Tθ大于显著性水平α的正规分布的值的情况下,排除解消假设Hθ0,判定位置计算装置100的推测方位与链路方位存在差。
步骤S903中,在满足下述的条件(a-1)~(a-3)的全部的情况下,判定移动体进入了地下停车场。
(a-1)位置计算装置100与第一链路候选点的距离为规定距离以上。
(a-2)接收装置101正在接收的相对方的卫星数为0。
(a-3)第一链路候选点的链路属性不是隧道。
步骤S903中,满足下述的条件(b-1)和(b-2)中任一条件的情况下,判定驶出了地下停车场。
(b-1)接收装置101正在接收的相对方的卫星数为1个以上。
(b-2)第一链路候选点的链路属性为隧道。
步骤S903中,上述以外的情况下,使前次的判定结果为此次的判定结果。
步骤S904中,满足下述的条件(c-1)~(c-3)中任一条件的情况下,判定为道路偏离。否则,判定为道路行驶。
(c-1)位置计算装置100的推测位置与链路的距离存在差。
(c-2)位置计算装置100的推测方位与链路方位存在差。
(c-3)位置计算装置100在地下停车场。
在步骤S905中,在步骤S904中判定为道路偏离的情况下,进入步骤S908。否则,进入步骤S414。
在步骤S906中,满足下述的条件(d-1)和(d-2)中任一条件的情况下,判定为入库中。否则,判定为出库。
(d-1)判定出位置计算装置100在地下停车场。
(d-2)位置计算装置100与第一链路候选点的距离为规定距离以上。
步骤S907中,在判定为入库中的情况下,进入步骤S908。否则,进入步骤S414。
步骤S908中,留下链路候选点的所在的位置和方位,将它们之外的過去的距离序列数据消除,将距离序列数据的数量设为1。
使用图8说明图1所示的位置计算装置100的工作顺序(图3)的步骤S207。
步骤S1001中,地图精度计算装置117从测位运算装置113接收推测位置的误差的方差,并且,从地图匹配装置114接收道路偏离的判定结果。在道路偏离的判定结果为道路行驶、即未偏离道路的判定结果,且推测位置的误差的方差为阈值以下的情况下,进入步骤S1002,否则,进入步骤S1004。
步骤S1002中,地图精度计算装置117从测位运算装置113接收推测位置,并且从地图匹配装置114接收第一链路候选点的位置。计算推测位置与第一链路候选点的距离,设为现在位置中的链路位置的误差。在此,在从第一链路候选点至推测位置的矢量的北方向成分为正的情况下,即矢量的方向处于从东方逆时针旋转至西方向的范围内(包括正东)的情况下,设链路位置的误差为正。另外,在从第一链路候选点至推测位置的矢量的北方向成分为负的情况下,即处于从西方逆时针旋转至东方向的范围内(包括正西)的情况下,设链路位置的误差为负。
步骤S1003中,地图精度计算装置117从地图精度存储装置108读取与包含现在位置的规定区域中的链路位置的误差有关的统计数、累计值和平方和的累计值。将包含现在位置的规定区域中的链路位置的误差的累计值与现在位置中的链路位置的误差相加,将包含现在位置的规定区域中的链路位置的误差的平方和的累计值与现在位置中的链路位置的误差的平方值相加,对包含现在位置的规定区域中的链路位置的误差的统计数加上1。将相加后的链路位置的误差的累计值除以相加后的链路位置的误差的统计数,由此计算链路位置的误差的平均值。将相加后的链路位置的误差的平方和的累计值,除以相加后的链路位置的误差的统计数,由此计算链路位置的误差的平方的平均值。从链路位置的误差的平方的平均值减去链路位置的误差的平均值的平方,由此计算包含现在位置的规定区域中的链路位置的误差的方差。在地图精度存储装置108中存储与包含现在位置的规定区域中的链路位置的误差相关的方差、统计数、累计值和平方和的累计值。
步骤S1004中,地图精度计算装置117从测位运算装置113接收推测方位的误差的方差。道路偏离的判定结果为道路行驶、即未偏离道路的判定结果,且推测方位的误差的方差为阈值以下的情况下,进入步骤S1004,否则,进入步骤S208。
步骤S1005中,地图精度计算装置117从测位运算装置113接收推测方位,并且从地图匹配装置114接收第一链路候选点的方位。计算推测方位与第一链路候选点的方位之差,设为现在位置中的链路方位的误差。
步骤S1006中,地图精度计算装置117从地图精度存储装置108读取包含与现在位置的规定区域中的链路方位的误差相关的统计数、累计值和平方和的累计值。将包含现在位置的规定区域中的链路方位的误差的累计值与现在位置中的链路方位的误差相加,将包含现在位置的规定区域中的链路方位的误差的平方和的累计值与现在位置中的链路方位的误差的平方值相加,对包含现在位置的规定区域中的链路方位的误差的统计数加上1。将相加后的链路方位的误差的累计值除以相加后的链路方位的误差的统计数,由此计算链路方位的误差的平均值。将相加后的链路方位的误差的平方和的累计值除以相加后的链路方位的误差的统计数,由此计算链路方位的误差的平方的平均值。从链路方位的误差的平方的平均值减去链路方位的误差的平均值的平方,由此计算包含现在位置的规定区域中的链路方位的误差的方差。在地图精度存储装置108中存储与包含现在位置的规定区域中的链路方位的误差相关的方差、统计数、累计值和平方和的累计值。
以上说明的本实施方式的位置计算装置100起到以下的作用效果。
(1)地图的链路数据的误差因各地域和各地图制造商而不同。因此,在不考虑地图误差的地图匹配、或者设定地图误差为固定值的地图匹配中,存在是否偏离道路的判定产生错误的情况。例如,在实际的链路的误差方差比假定的数值大的情况下,即使在道路上行驶,在假定的范围内不包含链路,导致错误地判定出偏离了道路的情况频繁发生。另外,在实际的链路的误差方差比假定的数值小的情况下,即使道路偏离,在假定的范围内包含链路,导致错误地与链路匹配的情况频繁发生。根据本实施方式的位置计算装置100的工作顺序(图2、图3、图7、图8),当推测位置和推测方位的误差为阈值以下时,即精度高时,基于推测位置和推测方位、以及链路位置和链路方位,计算包含现在位置的规定区域中的链路位置和方位的误差的方差。由此,即使在地图的链路误差因各地域和各地图制造商而不同的情况下,也能够使用正确的链路位置和方位的误差方差进行地图匹配,能够降低道路偏离判定的错误。
(2)基于位置计算装置100的位置与链路候选点的位置的距离、以及位置计算装置100的方位与链路候选点的方位的差,将位置计算装置100的位置、方位和链路候选点的位置、方位的距离序列数据的一致度作为评价量进行计算,选择第一链路候选点。该选择方法基于过去的路径进行计算,因此,能够防止在合流之前与错误的链路匹配。但是,根据现有技术,在图9所示的狭角分支的情况中,存在与错误的链路匹配的情况。在图9中,链路1100分支为链路1104和1105。移动体在链路1100和链路1104上行驶。由于刚从链路1100分支后的链路1104的位置和方位的误差、以及位置计算装置100的位置(移动体的检测位置)1101的位置误差的影响,在刚分支后的链路1104上行驶的移动体的检测位置与移动体未行驶的刚分支后的链路1105的距离和方位之差,有可能小于移动体的检测位置与链路1104的距离和方位之差。因此,移动体未行驶的链路1105上的错误的链路候选点被决定为第一链路候选点1103a,距离序列数据长时,由于刚分支后的移动体的检测位置1101与链路1104的距离和方位之差的影响,即使暂时行驶,也不会选择正确的链路1104的链路候选点1102。
根据本实施方式的位置计算装置100的工作顺序(图2、图3、图7、图8),在步骤S409中,地图匹配装置114在从规定角度以下的分支起链路1104与链路1105之间的距离保持为规定距离以下的期间,减少评价量的计算中所使用的距离序列数据的数量。如图9和图10所示,距离序列数据的数量C由移动体的检测位置1101的数量表示。在图9和图10中,移动体在分支前的链路1100上行驶的距离序列数据的数量C为相同数量,例如为50个,但在通过分支点后的距离序列数据的数量C与图9不同,在图10中例如从50个减少至3个。这样,使刚分支后的距离序列数据尽量不在评价量的计算中使用,由此,如图10所示,能够尽早去除刚分支后的链路1104的位置和方位的误差、移动体的检测位置1101的位置误差的影响,能够尽早决定正确的链路候选点1103b。
即,基于推测位置与链路的距离、推测方位与链路方位的差、推测位置、链路位置、以及推测方位和链路方位的误差协方差的距离序列数据来计算评价量时,在分支后减少距离序列数据的数量,使刚分支后的距离序列数据尽量不在评价量的计算中使用。由此,能够除去推测位置、链路位置、推测方位和链路方位的误差导致的刚分支后的错误匹配的评价量的影响。因此,即使在分支后选择错误的链路候选点,也能够尽早恢复至正确的链路候选点。
(3)根据图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8),在步骤S410中,将位置计算装置100的位置与链路候选点的位置的距离、位置计算装置100的方位与链路候选点的方位之差除以它们的协方差矩阵。由此,进行无量纲化(量纲为1),计算将位置和方位的要素相加而得到的评价量,用于决定第一链路候选,因此,能够从统计学的角度实现正确的距离和方位的相加,能够进行考虑了位置和方位的误差和存在概率的高精度的决定。另外,当假定位置计算装置100和链路候选点的位置和方位的误差协方差遵循正规分布时,评价量遵循自由度2×n的χ平方分布。因此,对该自由度2的χ平方分布的概率密度函数代入评价量时,当位置计算装置在道路(链路)上行驶时,能够转换为根据位置计算装置的位置和方位与链路候选点的位置和方位计算出的差所产生的概率、即考虑了位置和方位之差的存在概率。换言之,能够转换为位置计算装置在道路(链路)上行驶的概率。
(4)在使用将距离和方位单位不同的变数乘以经验的系数并相加而得到的评价量的方法中,为了提高正确的链路候选的决定精度,必须行驶多个分支,并使用在分支处收集的数据导出系数值,导致开发耗费较长时间。根据图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8),在步骤S207中计算链路的位置和方位的误差方差,在步骤S410中使用它们的误差方差,计算评价量,在步骤S412中能够决定第一链路候选点。因此,不必行驶多个分支并收集数据,能够在较短的时间内完成开发。
(5)根据图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8),在步骤S901和步骤S902中使用假想检测,判定位置计算装置100的位置和方位与链路的位置和方位是否存在差。根据这样判定,由于考虑了位置计算装置100的位置和方位的误差、链路的位置和方位的误差、位置计算装置100的存在概率,因此能够从统计学的角度正确且高精度地判定道路偏离。
(6)在基于将距离和方位单位不同的变数乘以经验的系数并相加而得到的值来判定道路偏离的方法中,为了提高道路偏离的决定精度,必须行驶多个分支,并使用在分支处收集的数据导出系数值,导致开发耗费较长时间。根据图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8),在步骤S207中计算链路的位置和方位的误差方差,在步骤S901和S902中使用它们的误差方差,计算评价量,判定推测量与第一链路候选点的位置和方位是否存在差。根据这样判定,行驶多个分支并收集数据,能够在较短的时间内完成开发。
―变形例―
(1)在图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)中,也可以替代步骤S901和步骤S902,如图11所示设置步骤S911,设立下述的假设。此外,在图11中,在步骤S911后继续的步骤S903以后的步骤中的处理与图7同样,因此省略说明。
·解消假设Hpθ0:位置计算装置100的推测位置与链路的距离、位置计算装置100的推测方位和链路方位不存在差。
·对立假设Hpθ1:位置计算装置100的推测位置与链路的距离、位置计算装置100的推测方位与链路方位存在差。
使用式(19),将位置计算装置100的推测位置与链路的距离的平方、和位置计算装置100的推测方位与链路方位的方位差的平方,各自除以表示其距离的位置数据的误差协方差、和表示其方位差的方位差数据的方差,由此进行无量纲化并相加,计算检测量Tpθ。在式(19)中,位置计算装置100的推测位置与链路的距离由位置数据的差Δx和Δy表示,位置计算装置100的推测方位与链路方位的方位差由方位差数据Δθ表示。式(8)是针对n个距离序列数据的累计式,式(19)是针对1个距离序列数据的计算式。
[数19]
【数19】
当假设位置计算装置100与链路候选点的位置和方位的误差协方差遵循正规分布时,检测量Tpθ遵循自由度2的χ平方分布。自由度2取决于位置(距离)和方位的2种参数。所以,决定显著性水平α,基于χ平方分布进行针对该假设的检测时,能够如以下那样选择假设。检测量Tpθ为显著性水平α的χ平方值以下的情况下,不排除解消假设,判定位置计算装置100的推测位置与链路的距离、位置计算装置100的推测方位与链路方位不存在差。在检测量Tpθ大于显著性水平α的χ平方值大的情况下,排除解消假设,判定位置计算装置100的推测位置与链路的距离、位置计算装置100的推测方位与链路方位存在差。由此,考虑了位置计算装置100与链路候选点的位置和方位的误差、存在概率,能够从统计学的角度正确且高精度地判定道路偏离。
(2)在图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)中,在步骤S201中,接收装置101基于多个测位卫星的位置和接收位置计算HDOP(Horizontal Dilution Position:水平精度因子),并将其发送至接收异常判定装置112。在步骤S207中,地图精度计算装置117从接收异常判定装置112接收接收位置和接收方位、它们的异常判定结果、以及HDOP,并且从地图匹配114装置接收道路偏离的判定结果和第一链路候选点的位置和方位。另外,从地图精度存储装置108分别读取与包含接收位置的规定区域中的链路位置的误差和方位的误差相关的统计数、累计值和平方和的累计值。在判定未偏离道路且判定接收位置没有异常,且HDOP为阈值以下的情况下,计算从第一链路候选点至接收位置的矢量,根据北方向的矢量成分决定正负,根据矢量的大小和正负,计算现在位置中的链路位置的误差。对包含现在位置的规定区域中的链路位置的误差的累计值和平方和的累计值,分别加上现在位置中的链路位置的误差及其平方值,对包含现在位置的规定区域中的链路位置的误差的统计数加上1。根据链路位置的误差的累计值、平方和的累计值和统计数,计算包含现在位置的规定区域中的链路位置的误差的方差。
另外,在判定出未偏离道路且判定出接收方位没有异常,并且HDOP为阈值以下的情况下,将接收方位与第一链路候选点的方位之差设为现在位置中的链路方位的误差。对包含现在位置的规定区域中的链路方位的误差的累计值和平方和的累计值,各自加上现在位置中的链路方位的误差及其平方值,对包含现在位置的规定区域中的链路方位的误差的统计数加上1。根据链路方位的误差的累计值、平方和的累计值和统计数,计算包含现在位置的规定区域中的链路方位的误差的方差。地图精度计算装置117将与包含现在位置的规定区域中的链路位置的误差和方位的误差相关的方差、统计数、累计值和平方和的累计值,存储于地图精度存储装置108。由此,当接收位置和接收方位的精度良好时,基于接收位置和接收方位,计算链路位置和链路方位的误差方差,因此,能够使用正确的链路位置和方位的误差方差进行地图匹配,能够正确判定道路偏离。
(3)在图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)中,可以进行以下的处理替代步骤S409。图12是表示替代步骤S409进行的以下的处理中,前次的第一链路候选点(已向前方移动)1301通过规定角度以下的分支后的样子的图。在分支点,链路1300分支为链路1303和1305,链路1303在节点1307与链路1304连接。另一方面,链路1305在节点1308与链路1306连接。第一链路候选点(完成前方移动)1301或链路候选点1302,如图12所示,处于规定角度以下的分支后的第一个链路1303或1305上的情况下,将距离序列数据的数量设定为比通常的第一规定数小的第二规定数。第一链路候选点1301或链路候选点1302处于规定角度以下的分支后的第二个链路1304或1306以后的链路上的情况下,将距离序列数据的数量设定为以通常的第一规定数为上限、以第二规定数为下限的来自第二个以后的链路的距离序列数据的数量。即,第一链路候选点1301或链路候选点1302处于规定角度以下的分支后的第二个以后的链路上时,根据来自第二个以后的链路的距离序列数据的数量,设定评价量的计算中使用的距离序列数据的数量。在来自第二个以后的链路的距离序列数据的数量比第一规定数多的情况下,将评价量的计算中所使用的距离序列数据的数量设定为第一规定数,在来自第二个以后的链路的距离序列数据的数量比第二规定数少的情况下,将评价量的计算中所使用的距离序列数据的数量设定为第二规定数。在这些以外的情况下,将评价量的计算中所使用的距离序列数据的数量设定为来自第二个以后的链路的距离序列数据的数量。由此,能够尽早消除刚分支后的链路的位置和方位的误差、以及位置计算装置100的位置误差的影响,能够尽早决定正确的链路候选点。
(4)图13是表示在替代步骤S409和步骤S413的以下的处理中前次的第一链路候选点(已向前方移动)1401通过规定角度以下的分支后的样子的图。图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)中,进行以下的处理来替代步骤S413。即,对第一链路候选点进行道路偏离判定,并且,在规定角度以下的分支后存在与第一链路候选点不同的分支后的链路上的链路候选点的情况下,对该链路候选点也进行道路偏离判定。另外,进行以下的处理来替代步骤S409。即,对前次的第一链路候选点(完成前方移动)1401通过规定角度以下的分支1400后、在分支1400分支后的一个分支链路1411上的链路候选点(以后称为分支链路候选点)1402判定出为道路行驶的情况下,将评价量的计算中所使用的距离序列数据的数量设定为比通常的第一规定数小的第二规定数。对第一链路候选点1401通过了规定角度以下的分支1400后、在分支1400分支后的另一个分支链路1412上的分支链路候选点1403判定出为道路偏离的情况下,可以将评价量的计算中所使用的距离序列数据的数量,设定为以通常的第一规定数为上限且以第二规定数为下限的、来自判定为道路偏离的位置的距离序列数据的数量。由此,能够尽早消除刚分支后的链路的位置和方位的误差、以及位置计算装置100的位置误差的影响,能够尽早决定正确的链路候选点。
具体而言,替代步骤S413和步骤S409,进行图14和图15所示的处理。使用图14,说明替代图1所示的位置计算装置100的工作顺序(图3)的步骤S413的步骤S1601~S1603、S1901~S1904和S413。
步骤S1601中,从第一链路候选点1401向在步骤S407中述说的规定距离后方存在规定角度以下的分支的情况下,判定第一链路候选点1401刚通过规定角度以下的分支,进入步骤S1603。否则,判定没有刚通过规定角度以下的分支,进入步骤S1602。
步骤S1602中,距离序列数据的数量不足第一规定数的情况下,判定存在与第一链路候选点1401不同的分支后的链路上的分支链路候选点,进入步骤S1603。否则,进入步骤S413。
步骤S1603中,确定与第一链路候选点1401的链路不同的分支后的链路,探测位于其上的分支链路候选点。
步骤S1901中,与图7的步骤S901同样,判定位置计算装置100的推测位置与分支链路的距离是否存在差(本车辆的推测位置与分支链路候选点的位置是否存在差)。
步骤S1902中,与图7的步骤S902同样,判定位置计算装置100的推测方位与分支链路候选点的链路方位是否存在差。
步骤S1903中,与图7的步骤S903同样,其中,使用分支链路候选点替代第一链路候选点,判定搭载有位置计算装置100的本车辆是否入库至地下停车场。
步骤S1904中,与图7的步骤S904同样,判定是否为道路偏离的状态。其中,使用分支链路候选点替代第一链路候选点。
步骤413中,进行图7所示的步骤S901~S908的处理,对于第一链路候选点1401进行道路偏离判定。
使用图15说明将图1所示的位置计算装置100的工作顺序(图3)的步骤S409替代的步骤S1701~S1705。
步骤S1701中,从由于步骤S406而向前方前进的第一链路候选点1401(由前次的处理中决定)起,在步骤S407所述的规定距离后方存在规定角度以下的分支的情况下,判定第一链路候选点1401刚通过规定角度以下的分支,进入步骤S1705。否则,判定没有刚通过规定角度以下的分支,进入步骤S1702。
步骤S1702中,前次处理的距离序列数据的数量不足第一规定数的情况下,进入步骤S1703。否则,进入步骤S410。
通过前次处理的步骤S1901~S1904,对分支链路候选点是否判定出为道路偏离,但是此处在步骤S1703判定。如分支链路候选点1403所示,在判定出为道路偏离的情况下,进入步骤S1704。如分支链路候选点1402所示,在判定出为道路行驶的情况下,进入步骤S1705。
步骤S1704中,对前次的距离序列数据的数量加上1。
步骤S1705中,将距离序列数据的数量设定为第二规定数。
(5)例如,根据变形例(4),在图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)之中,步骤S413中,能够在第一链路候选点通过规定角度以下的分支后,对第一链路候选点进行道路偏离判定,并且对分支链路候选点进行道路偏离判定。步骤S213中,第一链路候选点通过规定角度以下的分支后,步骤S413中进行的分支链路候选点的偏离判定结果为道路行驶的情况下,可以改变第一链路候选点的显示的颜色,使其与其它的情况不同。或者,也可以使第一链路候选点的显示闪烁。此时表示:分支后的2个链路中的任一链路为移动体有可能正在行驶的链路。在图16中,改变刚分支后的由阴影表示的第一链路候选点1503a和1503b的显示颜色,或使其闪烁。通过这样进行显示,驾驶员知道位置计算装置100正在研究分支判定中,因此,在尽管移动体位于分支链路候选点但却错误地显示第一链路候选点的情况下,能够降低驾驶员对该错误的不安。
具体而言,替代图1所示的位置计算装置100的工作顺序(图3)的步骤S413和步骤409,进行图14和图15所示的处理,并且,替代图1所示的位置计算装置100的工作顺序(图2)的步骤S213,进行图17所示的处理。使用图17说明替代步骤S213进行的步骤S1801~S1810。
步骤S1801中,显示声音输出装置107从地图匹配装置114取得位置计算装置100的位置和方位、第一链路候选点的链路序号、位置和方位、以及道路偏离判定结果,从路径引导装置116取得引导开始信号和引导信息,从地图信息存储装置106读取位置计算装置100的位置周边的链路信息。
步骤S1802中,将位置计算装置100的位置周边的链路信息描绘于画面中。
步骤S1803中,图14所示的道路偏离判定的步骤S413中,判定出第一链路候选点为道路偏离的情况下,进入步骤S1804。判定出为道路行驶的情况下,进入步骤S1805。
步骤S1804中,基于位置计算装置100的位置和方位,使用规定的颜色描绘汽车标志,进入步骤S201。
步骤S1805中,判定是否实施了分支链路候选点道路偏离判定。图14所示的道路偏离判定的步骤S1901~S1904中,对分支链路候选点实施了道路偏离判定的情况下,进入步骤S1806。在未实施的情况下,进入步骤S1808。
步骤S1806中,判定是否在分支链路候选点道路偏离判定中判定出为道路行驶。图14所示的道路偏离判定的步骤S1901~S1904中,判定出分支链路候选点为道路行驶的情况下,进入步骤S1807。对应图16中的链路候选点1502a和1502b。在判定出为道路偏离的情况下,进入步骤S1808。在图16对应链路候选点1504。
步骤S1807中,利用带有规定的颜色以外的颜色的汽车标志描绘第一链路候选点的位置和方位。或者,可以使汽车标志闪烁。在图16中,对应第一链路候选点1503a和1503b。
步骤S1808中,利用带有规定的颜色的汽车标志描绘第一链路候选点的位置和方位。在图16中,对应第一链路候选点1501。
步骤S1809中,引导开始信号开通的情况下进入步骤S1810,在断开的情况下进入步骤S201。
步骤S1810中,输出向导显示,利用声音输出引导方向。
(6)例如,根据变形例(4),在图1所示的位置计算装置100的工作顺序(图2、图3、图7、图8)之中,在步骤S413中,能够在第一链路候选点通过规定角度以下的分支后,对第一链路候选点进行道路偏离判定,并且对与第一链路候选点不同的分支的链路上的分支链路候选点进行道路偏离判定。步骤S213中,第一链路候选点通过规定角度以下的分支后,步骤S413中进行的分支链路候选点的偏离判定结果为道路行驶的情况下,可以不在第一链路候选点的位置和方位而在分支后的二个链路之间显示位置计算装置100的位置。在通过汽车标志显示位置计算装置100的位置时,将汽车标志的方位作为二个链路方位的平均值进行显示。这表示:考虑分支后的2个链路中的任一链路为移动体有可能正在行驶的链路。图18中,将刚分支后的由斜线阴影表示的汽车标志1505显示于分支后的二个链路之间,或将汽车标志1505的方位作为二个链路方位的平均值进行显示。通过这样进行显示,能够防止尽管移动体位于与第一链路候选点不同的分支链路候选点但却错误地在第一链路候选点的位置和方位显示汽车标志,消除驾驶员对第一链路候选点的错误的不安。
具体而言,替代图1所示的位置计算装置100的工作顺序(图3)的步骤S413和步骤409,进行图14和图15所示的处理,并且,替代图1所示的位置计算装置100的工作顺序(图2)的步骤S213,进行图19所示的处理。在图19的工作顺序中,替代图17的工作顺序的步骤S1801~S1810之中的步骤S1807,实施步骤S1811。对于步骤S1801~S1806以及S1808~S1810,使用图17进行了述说,因此,在此,下面仅说明替代步骤S1807新追加的步骤S1811。
步骤S1811中,在与第一链路候选点不同的分支后的链路上的分支链路候选点与第一链路候选点之间描绘汽车标志。另外,汽车标志的方位为二个链路的方位之间的值。在图18中,对于汽车标志1505。此时的第一链路候选点1506a和1506b不显示。在图18中,对标注有与图16相同的符号的图形,在图16的说明中已经述说,所以在此省略说明。
(7)在图1所示的位置计算装置100的工作顺序(图2,图3,图7,图8)中,步骤S210中,不限于具有第一链路候选点的链路包含于引导路径的链路的情况。例如,即使判定在通过分支后与第一链路候选点不同的分支后的链路上的分支链路候选点为道路行驶的情况下,也可以改变为进入步骤S212的处理。由此,能够在将因分支后的链路位置和方位的误差而错误选择的链路候选点设为第一链路候选点时,防止基于错误的链路候选点进行路径再探索。另外,考虑存在概率,在判定仅第一链路候选点为道路行驶的状态下进行路径探索,因此,不进行错误的路径的再探索的概率变高。
具体而言,替代图1所示的位置计算装置100的工作顺序(图3)的步骤S413和步骤S409,进行图14和图15所示的处理,并且,在图1所示的位置计算装置100的工作顺序(图2)中,进行在步骤S210与步骤S211之间插入有S2001和S2002的处理。对于步骤S208~S210以及S211~S213使用图2进行了述说,因此,下面使用图20仅说明新插入于步骤S210与步骤S211之间的步骤S2001和S2002。
步骤S2001中,判定是否实施了分支链路候选点道路偏离判定。图14所示的道路偏离判定的步骤S1901~S1904中,对分支链路候选点实施了道路偏离判定的情况下,进入步骤S2002。在未实施的情况下,进入步骤S211。
步骤S2002中,判定在分支链路候选点道路偏离判定中是否判定为道路行驶。在图14所示的道路偏离判定的步骤S1901~S1904中,判定分支链路候选点为道路行驶的情况下,进入步骤S212。在判定为道路偏离的情况下,进入步骤S211。
(8)图1所示的位置计算装置100中,也可以去除加速度传感器装置104。在图2所示的工作顺序中,步骤S202中,不进行将与位置计算装置100的加速度对应的信号发送至传感器输出校正装置111的处理。在步骤S203中,从来自加速度传感器装置104的输出减去加速度传感器装置104的偏压,不进行计算传感器加速度的处理。在步骤S205中,式(1)中,从状态量矢量η(t)、矩阵F、G、状态迁移矩阵Φ(Δt、αa、αω)和Q(k)削除与节距(t)相关的要素,从观测量矢量y、矩阵H和观测噪声矩阵R削除与传感器加速度gs(t)相关的要素。这样,能够削减传感器的数量,能够降低装置成本。
(9)图1所示的位置计算装置100中,也可以去除速度传感器装置102、角速度传感器装置103、加速度传感器装置104和传感器输出校正装置111。图2所示的工作顺序中,削除步骤S202和步骤S203,步骤S204中,接收异常判定装置112从接收装置101接收接收位置、接收速度和接收方位。根据接收速度和接收方位计算规定时间的位置变化,该位置变化与规定时间的接收位置的变化之差为阈值以上的情况下,判定接收位置为异常,否则,判定接收位置为正常。设接收速度和接收方位总为正常。步骤S205的测位处理中需要使推测位置和推测方位为最新,因此需要经常输入某些信息。在此,经常输入接收速度和接收方位。接收位置在被判定为正常时输入。在步骤S205中,式(1)中,从状态量矢量η(t)、矩阵F、G、状态迁移矩阵Φ(Δt、αa、αω)和Q(k)削除与节距有关的要素,并且,从观测量矢量y、矩阵H和观测噪声矩阵R削除与传感器速度vs(t)、传感器角速度ωs(t)、传感器加速度gs(t)有关的要素。这样,能够削减传感器的数量,能够降低装置成本。
(10)通过将位置计算装置100搭载于车载导航系统,即使地图的链路误差因地域、按地图制造商而不同,也能够实现可正确判定是否道路偏离的车载导航系统。另外,该车载导航系统即使在通过锐角分支后将本车与错误的链路匹配情况下,也能够尽早回归至正确的链路。
(11)图1所示的位置计算装置100中,可以按每个规定的区域,预先分别计算与链路位置的误差和方位的误差相关的方差,并将其存储于地图精度存储装置108。在来自人工卫星或者航空机的航空照片上叠加描绘道路链路,基于该照片和链路的位置的差,能够预先计算链路位置的误差方差和链路方位的误差方差。或者,实际使车辆行驶,即,使移动体移动来计算链路位置的误差和方位的误差,使用该计算结果预先计算各自的误差方差。由此,在工作顺序(图2、图3、图7)中,不需要在每次动作时执行步骤S207。在该情况下,在地图精度存储装置108中,不需要按每个规定区域存储统计数、累计值及平方和的累计值。
另外,将与链路位置的误差和方位的误差有关的方差预先存储于地图精度存储装置108之后,可以进一步执行图8中的步骤S207的处理。由此,能够基于实际的地图匹配结果,计算链路位置的误差方差和链路方位的误差方差,更新存储于地图精度存储装置108的值。
S410和S413中可以使用与按每个预先计算的规定区域的链路位置的误差和方位的误差相关的方差。由此,能够分别进行步骤S410中的链路候选点的评价量的计算、步骤S413中的道路偏离判定。由此,链路候选点的评价量、道路偏离的检测量不受到因地域和按制造商而不同的地图的链路数据的误差的影响,能够减少匹配、道路偏离判定的错误。
(12)上述实施方式以及变形例可以各自组合。
以上,说明了各种实施方式和变形例,但是本发明不限于这些内容。在本发明的技术思想的范围内想到的其它的实施方式也包含于本发明的范围内。
以下优先权技术申请的公开内容作为引用文援引于本文。
日本国专利申请2011年第87237号(2011年4月11日申请)
Claims (15)
1.一种位置计算方法,其特征在于:
计算移动体的现在位置、移动速度和移动方位,
基于所述现在位置、所述移动速度和所述移动方位,计算所述现在位置和所述移动方位的推测误差协方差,
作为使用所述现在位置、所述移动方位、所述推测误差协方差、所述现在位置周边的区域所包含的多个链路候选点中的各链路候选点的链路候选点位置、所述各链路候选点的链路候选点方位、所述区域所包含的多个链路的位置的第一误差方差、和所述多个链路的方位的第二误差方差进行的运算,根据所述现在位置与所述链路候选点位置之间的距离和所述移动方位与所述链路候选点方位的方位差,对包含至所述链路候选点位置的按每规定距离间隔得到的所述距离和所述方位差的多个数据进行累计,由此,按所述各链路候选点的每个链路计算所述移动体在与所述各链路候选点对应的道路上行驶的概率所对应的评价量,
令所述移动体位于所述多个链路候选点中的所述概率最高的链路候选点,基于所述评价量计算所述现在位置。
2.如权利要求1所述的位置计算方法,其特征在于:
所述第一误差方差基于所述距离计算,
所述第二误差方差基于所述方位差计算,
所述评价量通过对于所述多个数据将以下两个值之和累计而得到:将所述多个数据中的各数据所包含的所述距离的平方除以所述第一误差方差与所述现在位置的推测误差方差之和而得到的值;将所述多个数据中的各数据所包含的所述方位差的平方除以所述第二误差方差与所述移动方位的推测误差方差之和而得到的值。
3.如权利要求1或2所述的位置计算方法,其特征在于:
基于与所述现在位置和所述概率最高的所述链路候选点位置之间的距离相关的第一检测量是否大于第一阈值,来判定所述现在位置与该链路候选点位置是否存在差,
基于与所述移动方位和所述概率最高的所述链路候选点方位的方位差相关的第二检测量是否大于第二阈值,来判定所述移动方位与该链路候选点方位是否存在差,
当所述现在位置与该链路候选点位置存在差时,或者所述移动方位与该链路候选点方位存在差时,判定所述移动体偏离与所述概率最高的所述链路候选点对应的所述道路,并且,当所述现在位置与该链路候选点位置不存在差时,且所述移动方位与该链路候选点方位不存在差时,判定所述移动体在与所述概率最高的所述链路候选点对应的所述道路上行驶,
当判定出所述移动体偏离与所述概率最高的所述链路候选点对应的所述道路时,令所述移动体正位于所述现在位置并进行计算,并且,当判定出所述移动体在与所述概率最高的所述链路候选点对应的所述道路上行驶时,令所述移动体正位于该链路候选点,并计算所述现在位置。
4.如权利要求3所述的位置计算方法,其特征在于:
当判定出所述移动体未偏离与所述概率最高的所述链路候选点对应的所述道路时,根据所述推测误差协方差是否为第三阈值以下,来判定是否计算所述第一误差方差,
当判定出所述移动体未偏离与所述概率最高的所述链路候选点对应的所述道路时,根据所述推测误差协方差是否为第四阈值以下,来判定是否计算所述第二误差方差。
5.如权利要求3所述的位置计算方法,其特征在于:
根据所述测位卫星的卫星位置和所述现在位置计算DOP值,
基于所述现在位置的变化、所述移动速度的变化和所述移动方位的变化,来判定所述现在位置是否为异常和所述移动方位是否为异常,
当判定出所述移动体未偏离与所述概率最高的所述链路候选点对应的所述道路,且判定出所述现在位置不为异常时,根据所述DOP值是否为第五阈值以下,判定是否计算所述第一误差方差,
当判定出所述移动体未偏离与所述概率最高的所述链路候选点对应的所述道路,且判定出所述现在位置不为异常时,根据所述DOP值是否为所述第五阈值以下,判定是否计算所述第二误差方差。
6.如权利要求3~5中任一项所述的位置计算方法,其特征在于:
所述第一检测量和所述第二检测量的值是将以下两个值无量纲化(量纲为1)并相加而得到的值:将所述现在位置与所述概率最高的所述链路候选点位置之间的所述距离的平方,除以所述现在位置和所述概率最高的所述链路候选点位置的误差协方差之和而得到的值;将所述移动方位与所述概率最高的所述链路候选点方位的所述方位差的平方,除以所述移动方位和所述概率最高的所述链路候选点方位的误差方差之和而得到的值。
7.如权利要求3~6中任一项所述的位置计算方法,其特征在于:
所述第一检测量是将所述现在位置与所述概率最高的所述链路候选点位置之间的所述距离的平方,除以所述现在位置和所述概率最高的所述链路候选点位置的误差协方差之和而得到的值,
所述第二检测量是将所述移动方位与所述概率最高的所述链路候选点方位的所述方位差的平方,除以所述移动方位和所述概率最高的所述链路候选点方位的误差方差之和而得到的值。
8.如权利要求1~7中任一项所述的位置计算方法,其特征在于:
当构成所述多个链路的第一链路和第二链路从分支点节点分支,且所述概率最高的所述链路候选点包含于所述第一链路时,在所述移动体通过与所述分支点节点对应的交差路口后,直至该链路候选点的所述链路候选点位置和与所述第二链路对应的区间道路的间隔变得比规定值大的期间,减少所述多个数据的数据数量。
9.如权利要求1~7中任一项所述的位置计算方法,其特征在于:
在判定所述移动体在通过与构成所述多个链路的第一链路和第二链路分支的分支点节点对应的交差路口后,在包含与所述概率最高的所述链路候选点不同的链路候选点的链路所对应的道路上行驶的第一期间,减少所述多个数据的数据数量。
10.如权利要求9所述的位置计算方法,其特征在于:
在基于从所述现在位置附近的起点链路至所述移动体的目的地附近的终点链路为止的引导路径,对所述移动体进行路径引导,且包含所述概率最高的所述链路候选点的链路包含于所述引导路径的情况下,当判定出所述移动体未偏离所述道路,且所述交差路口与该链路候选点的所述链路候选点位置之间的距离为规定值以下时,对显示声音输出装置输出与所述引导路径相关的向导显示和向导声音,
在基于所述引导路径对所述移动体进行路径引导,且包含所述概率最高的所述链路候选点的链路不包含于所述引导路径的情况下,当判定出所述移动体通过所述交差路口,偏离了包含与所述概率最高的所述链路候选点不同的链路候选点的链路所对应的道路时,探索从所述现在位置附近的起点链路至所述移动体的目的地附近的终点链路的新的引导路径。
11.一种位置计算装置,其特征在于,具备:
移动信息计算部,其计算移动体的现在位置、移动速度和移动方位;
推测误差协方差计算部,其基于所述现在位置、所述移动速度和所述移动方位,计算所述现在位置和所述移动方位的推测误差协方差;
概率计算部,作为使用所述现在位置、所述移动方位、所述推测误差协方差、从所述现在位置包含所述现在位置的规定区域所包含的多个链路候选点中的各链路候选点的链路候选点位置、所述各链路候选点的链路候选点方位、所述区域所包含的多个链路的位置的第一误差方差、和所述多个链路的方位的第二误差方差进行的运算,根据所述现在位置与所述链路候选点位置之间的距离和所述移动方位与所述链路候选点方位的方位差,对包含至所述链路候选点位置的按每规定距离间隔得到的所述距离和所述方位差的多个数据进行累计,由此,按所述各链路候选点的每个链路计算所述移动体在与所述各链路候选点对应的道路上行驶的概率所对应的评价量;和
位置计算部,其令所述移动体位于所述多个链路候选点中的所述概率最高的链路候选点,基于所述评价量计算所述现在位置。
12.如权利要求11所述的位置计算装置,其特征在于:
还具备误差方差计算部,其基于所述距离计算所述第一误差方差,并且基于所述方位差计算所述第二误差方差,
所述概率计算部通过对于所述多个数据将以下两个值之和累计来计算所述评价量:将所述多个数据中的各数据所包含的所述距离的平方除以所述第一误差方差与所述现在位置的推测误差方差之和而得到的值;将所述多个数据中的各数据所包含的所述方位差的平方除以所述第二误差方差与所述移动方位的推测误差方差之和而得到的值。
13.如权利要求11或12所述的位置计算装置,其特征在于,还具备:
第一判定部,其基于将所述现在位置与所述概率最高的所述链路候选点的所述链路候选点位置之间的距离的平方,除以所述第一误差方差与所述现在位置的推测误差方差之和而得到的值是否大于第一阈值,来判定所述现在位置与该链路候选点位置是否存在差;
第二判定部,其基于将所述移动方位与所述概率最高的所述链路候选点的所述链路候选点方位的方位差,除以所述第二误差方差与所述移动方位的推测误差方差之和的平方根而得到的值是否大于第二阈值,来判定所述移动方位与该链路候选点方位是否存在差;和
第三判定部,其在所述现在位置与该链路候选点位置存在差时,或者所述移动方位与该链路候选点方位存在差时,判定所述移动体偏离了与所述概率最高的所述链路候选点对应的所述道路,并且,在所述现在位置与该链路候选点位置不存在差时,且所述移动方位与该链路候选点方位不存在差时,判定所述移动体在与所述概率最高的所述链路候选点对应的所述道路上行驶,
所述位置计算部在由所述第三判定部判定出所述移动体偏离了与所述概率最高的所述链路候选点对应的所述道路时,令所述移动体位于所述现在位置并进行计算,并且,在判定出所述移动体在与所述概率最高的所述链路候选点对应的所述道路上行驶时,令所述移动体位于该链路候选点,计算所述现在位置。
14.如权利要求11~13中任一项所述的位置计算装置,其特征在于,还具备:
接收部,其接收从测位卫星发出的测位信号;和
测量部,其测量包含所述移动体的速度、角速度和加速度中的至少一个的传感器数据,
所述移动信息计算部基于所述测位信号和所述传感器数据,计算所述现在位置、所述移动速度和所述移动方位,
所述推测误差协方差计算部使用所述测位信号和所述传感器数据计算所述推测误差协方差。
15.如权利要求11~14中任一项所述的位置计算装置,其特征在于:
还具备设定部,其进行以下设定:当构成所述多个链路的第一链路和第二链路从分支点节点分支,且所述概率最高的所述链路候选点包含于所述第一链路时,在所述移动体通过与所述分支点节点对应的交差路口后,直至该链路候选点的所述链路候选点位置和与所述第二链路对应的区间道路的间隔变得比规定值大的期间,减少所述多个数据的数据数量。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011087237 | 2011-04-11 | ||
JP2011-087237 | 2011-04-11 | ||
PCT/JP2012/059876 WO2012141199A1 (ja) | 2011-04-11 | 2012-04-11 | 位置算出方法及び位置算出装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103492837A true CN103492837A (zh) | 2014-01-01 |
CN103492837B CN103492837B (zh) | 2016-04-27 |
Family
ID=47009370
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201280018021.5A Active CN103492837B (zh) | 2011-04-11 | 2012-04-11 | 位置计算方法和位置计算装置 |
Country Status (5)
Country | Link |
---|---|
US (1) | US9157749B2 (zh) |
EP (1) | EP2698608B1 (zh) |
JP (1) | JP5640146B2 (zh) |
CN (1) | CN103492837B (zh) |
WO (1) | WO2012141199A1 (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103956071A (zh) * | 2014-05-20 | 2014-07-30 | 徐敬 | 一种停车场找车的方法 |
CN105526939A (zh) * | 2014-09-29 | 2016-04-27 | 高德软件有限公司 | 道路匹配方法和装置 |
CN106855415A (zh) * | 2017-01-09 | 2017-06-16 | 北京京东尚科信息技术有限公司 | 地图匹配方法和系统 |
CN107305123A (zh) * | 2016-04-19 | 2017-10-31 | 歌乐株式会社 | 位置推断装置以及推断方法 |
CN107532913A (zh) * | 2015-12-03 | 2018-01-02 | 华为技术有限公司 | 一种导航方法和用于导航的设备 |
CN107783166A (zh) * | 2016-08-29 | 2018-03-09 | 千寻位置网络有限公司 | Gps速度异常检测与修复的方法及其系统 |
CN107924631A (zh) * | 2015-08-19 | 2018-04-17 | 本田技研工业株式会社 | 车辆控制装置、车辆控制方法及车辆控制程序 |
Families Citing this family (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
ES2288036T5 (es) † | 1998-11-13 | 2017-07-06 | Immunex Corporation | ADN de TSLP humano y polipéptidos |
US9167463B2 (en) * | 2011-09-02 | 2015-10-20 | Telcordia Technologies, Inc. | Communication node operable to estimate faults in an ad hoc network and method of performing the same |
JP6054638B2 (ja) * | 2012-05-30 | 2016-12-27 | クラリオン株式会社 | 車両位置検出装置およびプログラム |
JP6236954B2 (ja) * | 2013-07-23 | 2017-11-29 | アイシン・エィ・ダブリュ株式会社 | 運転支援システム、方法およびプログラム |
CN104603821B (zh) * | 2013-08-30 | 2016-10-19 | 株式会社小松制作所 | 矿山机械的管理系统以及矿山机械的管理方法 |
KR102016551B1 (ko) * | 2014-01-24 | 2019-09-02 | 한화디펜스 주식회사 | 위치 추정 장치 및 방법 |
JP6413404B2 (ja) * | 2014-07-07 | 2018-10-31 | カシオ計算機株式会社 | 電子機器、位置推定方法及びプログラム |
JP2016138816A (ja) * | 2015-01-28 | 2016-08-04 | アルパイン株式会社 | ナビゲーション装置及びコンピュータプログラム |
US9537880B1 (en) * | 2015-08-19 | 2017-01-03 | Palantir Technologies Inc. | Anomalous network monitoring, user behavior detection and database system |
CN107228677B (zh) * | 2016-03-23 | 2019-03-26 | 腾讯科技(深圳)有限公司 | 偏航识别方法和装置 |
US10145691B2 (en) * | 2016-05-18 | 2018-12-04 | Here Global B.V. | Ambiguity map match rating |
US10641612B2 (en) | 2016-12-19 | 2020-05-05 | Alpine Electronics, Inc. | Method and apparatus for correcting current position in navigation system via human-machine interface |
EP3339807B1 (en) * | 2016-12-20 | 2024-03-13 | HERE Global B.V. | An apparatus and associated methods for determining the location of a vehicle |
DE102017208245A1 (de) * | 2017-05-16 | 2018-11-22 | Continental Automotive Gmbh | Verfahren und Vorrichtung zur zielbasierten Prädiktion dynamischer Objekte |
CN111448575B (zh) * | 2017-11-29 | 2024-03-26 | 北京嘀嘀无限科技发展有限公司 | 用于评估模型性能的系统和方法 |
KR102181755B1 (ko) * | 2018-08-31 | 2020-11-24 | 주식회사 카카오모빌리티 | 경로를 보정하는 서버, 방법 및 컴퓨터 프로그램 |
US10852746B2 (en) | 2018-12-12 | 2020-12-01 | Waymo Llc | Detecting general road weather conditions |
US10900804B2 (en) * | 2018-12-12 | 2021-01-26 | Here Global B.V. | Methods and systems for roadwork extension identification using speed funnels |
US11168989B2 (en) * | 2019-01-02 | 2021-11-09 | Here Global B.V. | Supervised point map matcher |
SG11202108564TA (en) * | 2019-03-08 | 2021-09-29 | Waymo Llc | Signaling for turns for autonomous vehicles |
JP7245084B2 (ja) * | 2019-03-15 | 2023-03-23 | 日立Astemo株式会社 | 自動運転システム |
CN110164255A (zh) * | 2019-05-28 | 2019-08-23 | 上海车轮互联网服务有限公司 | 用于路考的路线处理方法及装置 |
CN110782667A (zh) * | 2019-10-30 | 2020-02-11 | 北京百度网讯科技有限公司 | 信号灯分时段配时方法、装置、电子设备及存储介质 |
JP7380652B2 (ja) * | 2021-05-26 | 2023-11-15 | セイコーエプソン株式会社 | 報知方法、情報処理装置、表示システム |
CN113487884B (zh) * | 2021-06-15 | 2023-11-24 | 阿波罗智联(北京)科技有限公司 | 交通量的获取方法、装置及电子设备 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101360971A (zh) * | 2006-07-25 | 2009-02-04 | 丰田自动车株式会社 | 定位装置及导航系统 |
CN100489455C (zh) * | 2000-12-08 | 2009-05-20 | 松下电器产业株式会社 | 发送数字地图的位置信息的方法和用于该方法的设备 |
JP2009236517A (ja) * | 2008-03-26 | 2009-10-15 | Clarion Co Ltd | Gpsレシーバ |
EP2264404A2 (en) * | 2009-06-18 | 2010-12-22 | Clarion Co., Ltd. | Position detection apparatus and position detection program |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5436840A (en) * | 1993-03-05 | 1995-07-25 | Motorola, Inc. | Vehicle position uncertainty area correction method and apparatus therefor |
JP3012501B2 (ja) * | 1995-11-30 | 2000-02-21 | 富士通テン株式会社 | 車両位置検出装置 |
JPH09229698A (ja) | 1996-02-21 | 1997-09-05 | Sumitomo Electric Ind Ltd | 位置検出装置 |
JP3745165B2 (ja) * | 1999-06-01 | 2006-02-15 | 三菱電機株式会社 | ロケータ装置 |
US6816799B2 (en) * | 2002-08-05 | 2004-11-09 | Robert Bosch Corporation | Vehicle operating parameter determination system and method |
US7643939B2 (en) * | 2006-03-08 | 2010-01-05 | Honeywell International Inc. | Methods and systems for implementing an iterated extended Kalman filter within a navigation system |
US7865299B2 (en) * | 2006-09-14 | 2011-01-04 | Toyota Motor Engineering & Manufacturing North America, Inc. | Method and system for predicting a future position of a vehicle using numerical integration |
-
2012
- 2012-04-11 JP JP2013509938A patent/JP5640146B2/ja active Active
- 2012-04-11 US US14/111,117 patent/US9157749B2/en active Active
- 2012-04-11 WO PCT/JP2012/059876 patent/WO2012141199A1/ja active Application Filing
- 2012-04-11 CN CN201280018021.5A patent/CN103492837B/zh active Active
- 2012-04-11 EP EP12771980.5A patent/EP2698608B1/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN100489455C (zh) * | 2000-12-08 | 2009-05-20 | 松下电器产业株式会社 | 发送数字地图的位置信息的方法和用于该方法的设备 |
CN101360971A (zh) * | 2006-07-25 | 2009-02-04 | 丰田自动车株式会社 | 定位装置及导航系统 |
JP2009236517A (ja) * | 2008-03-26 | 2009-10-15 | Clarion Co Ltd | Gpsレシーバ |
EP2264404A2 (en) * | 2009-06-18 | 2010-12-22 | Clarion Co., Ltd. | Position detection apparatus and position detection program |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103956071B (zh) * | 2014-05-20 | 2015-09-09 | 徐敬 | 一种停车场找车的方法 |
CN103956071A (zh) * | 2014-05-20 | 2014-07-30 | 徐敬 | 一种停车场找车的方法 |
CN105526939A (zh) * | 2014-09-29 | 2016-04-27 | 高德软件有限公司 | 道路匹配方法和装置 |
CN105526939B (zh) * | 2014-09-29 | 2018-12-11 | 高德软件有限公司 | 道路匹配方法和装置 |
CN107924631A (zh) * | 2015-08-19 | 2018-04-17 | 本田技研工业株式会社 | 车辆控制装置、车辆控制方法及车辆控制程序 |
CN107532913A (zh) * | 2015-12-03 | 2018-01-02 | 华为技术有限公司 | 一种导航方法和用于导航的设备 |
US10823577B2 (en) | 2015-12-03 | 2020-11-03 | Huawei Technologies Co., Ltd. | Navigation method and navigation device |
CN107532913B (zh) * | 2015-12-03 | 2020-03-27 | 华为技术有限公司 | 一种导航方法和用于导航的设备 |
CN107305123A (zh) * | 2016-04-19 | 2017-10-31 | 歌乐株式会社 | 位置推断装置以及推断方法 |
CN107305123B (zh) * | 2016-04-19 | 2020-10-13 | 歌乐株式会社 | 位置推断装置以及推断方法 |
CN107783166B (zh) * | 2016-08-29 | 2020-10-16 | 千寻位置网络有限公司 | Gps速度异常检测与修复的方法及其系统 |
CN107783166A (zh) * | 2016-08-29 | 2018-03-09 | 千寻位置网络有限公司 | Gps速度异常检测与修复的方法及其系统 |
CN106855415A (zh) * | 2017-01-09 | 2017-06-16 | 北京京东尚科信息技术有限公司 | 地图匹配方法和系统 |
CN106855415B (zh) * | 2017-01-09 | 2020-03-03 | 北京京东尚科信息技术有限公司 | 地图匹配方法和系统 |
Also Published As
Publication number | Publication date |
---|---|
US20140067256A1 (en) | 2014-03-06 |
US9157749B2 (en) | 2015-10-13 |
JPWO2012141199A1 (ja) | 2014-07-28 |
JP5640146B2 (ja) | 2014-12-10 |
CN103492837B (zh) | 2016-04-27 |
EP2698608A1 (en) | 2014-02-19 |
WO2012141199A1 (ja) | 2012-10-18 |
EP2698608B1 (en) | 2015-08-26 |
EP2698608A4 (en) | 2014-10-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103492837B (zh) | 位置计算方法和位置计算装置 | |
EP2664894B1 (en) | Navigation apparatus | |
Hashemi et al. | A critical review of real-time map-matching algorithms: Current issues and future directions | |
Ochieng et al. | Map-matching in complex urban road networks | |
Taylor et al. | Road reduction filtering for GPS‐GIS navigation | |
Georgy et al. | Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration | |
CN102648394A (zh) | 惯性测量单元(imu)的空间对准确定 | |
Crespillo et al. | Bayesian GNSS/IMU tight integration for precise railway navigation on track map | |
Liu et al. | Generating enhanced intersection maps for lane level vehicle positioning based applications | |
Yoo et al. | Improvement of TERCOM aided inertial navigation system by velocity correction | |
CN106405592A (zh) | 车载北斗载波相位周跳检测与修复方法及系统 | |
JP5732377B2 (ja) | ナビゲーション装置 | |
KR100542014B1 (ko) | 복합교차로 경로 안내를 위한 차량 항법유도방법 | |
KR20090070936A (ko) | 네비게이션 시스템 및 방법, 방법 프로그램을 기록한저장매체 | |
WO2000050917A1 (en) | Vehicle navigation system with correction for selective availability | |
JP2009036651A (ja) | ナビゲーション装置、ナビゲーション方法及びナビゲーションプログラム | |
Taylor et al. | Road reduction filtering using GPS | |
Kauffman et al. | Navigation via H-field signature map correlation and INS integration | |
Ochieng et al. | Integrated positioning algorithms for transport telematics applications | |
Davidson et al. | Uninterrupted portable car navigation system using GPS, map and inertial sensors data | |
Song et al. | Enhanced Map‐Aided GPS/3D RISS Combined Positioning Strategy in Urban Canyons | |
Blazquez et al. | Performance of a new enhanced topological decision-rule map-matching algorithm for transportation applications | |
Fouque et al. | Multi-hypothesis map-matching on 3d navigable maps using raw gps measurements | |
JPS6366411A (ja) | 車両用経路案内装置 | |
Mahmoud et al. | Land vehicle navigation using low-cost integrated smartphone gnss mems and map matching technique |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |