专利名称：基于汽车状态转移的多假设地图匹配方法
基于汽车状态转移的多假设地图匹配方法
技 术领域本发明涉及地图匹配领域，具体是一种利用汽车状态转换信息来限制假设节点生成的多假设地图匹配方法，本方法可用来匹配汽车走过的路径。
背景技术：
地图匹配算法将GPS(Global Positioning System,全球定位系统)/ INSdnertial Navigation System，惯性导航系统)数据与电子地图数据进行匹配，以找出汽车在电子地图中的位置。地图匹配算法主要应用在汽车导航、记录汽车运行轨迹等方面。从20世纪末到现在，已经有了很多地图匹配算法，包括基于位置点匹配的地图匹配算法、基于面积法准则的地图匹配算法、�：呒惴�、曲线拟合算法等。这些算法在电子地图导航方面取得了非�：玫男Ч欢诩锹计翟诵泄旒５挠τ弥校床灰欢芄坏玫酵玫男Ч�。电子地图导航对算法的实时性和精确性要求很高，对匹配结果的连续性要求不是很高。而记录汽车运行轨迹的应用中，除了对算法的实时性和精确性要求很高外，还要求匹配结果具有较好的连续性。现有的算法在下列情况下并不能够满足道路计费系统的要求1、在道路比较密集的区域，现有算法可能会匹配错误；2、如果地图中有很短的路径，则这些路径可能会被算法忽略；3、在GPS信号不好的区域，如高楼区、隧道等，现有算法匹配出错的可能性会很大。
发明内容
(一)要解决的技术问题本发明要解决的技术问题是如何准确地记录汽车所走过的连续路径，同时在 GPS信号不好、道路密集的区域获得较好的地图匹配结果，以使地图匹配算法能更好地应用于汽车计费系统中。( 二 )技术方案为解决上述技术问题，本发明提供了一种基于汽车状态转移的多假设地图匹配方法，包括以下步骤Si、获取全球定位系统GPS数据和惯性导航系统INS数据；S2、根据GPS数据和INS数据定义一颗多假设树，所述多假设树用于记录汽车走过的所有路径，其有一个根节点，所述根节点代表汽车的起始点；S3、在汽车每获得一次GPS数据时，在多假设树中生成新的假设节点；S4、根据所述GPS数据和INS数据计算所述假设节点的权重，所述权重表示该假设节点包含的信息与真实信息的相似性；S5、根据假设节点的权重删除一部分假设节点；S6、选择权重最大的假设节点作为最佳假设节点。然后重复步骤Sl S5，每一轮可获得一个最佳假设节点，该最佳假设节点所在的分支记录了汽车走过的路径轨迹。
其中，在步骤S2中定义多假设树时，假设每个所述假设节点至少包含如下信息 假设节点产生的时间；汽车当前状态，该状态有四种初始、停止、加速和运行，汽车当前状态通过INS数据计算获得；汽车当前可能所在的路径link;汽车在link上运行的方向；汽车在At时间内转过的角度，其通过INS数据获得；汽车在At时间内走过的里程，其通过INS数据获得；汽车当前的方向，其通过GPS数据和INS数据获得；汽车在link上走过的里程；假设节点的权重；假设节点的活跃性，代表汽车当前状态的假设节点称为活跃节点，代表汽车历史状态的假设节点称为非活跃节点。其中，在步骤S2中以根节点为父节点，对每一条汽车可能所在的路径，生成两个路径相同但方向不同的假设节点，步骤具体包括S21，判断GPS数据的有效性，若GPS数据有效则执行如下的步骤，否则不进行初始化；S22，计算汽车当前状态；S23，以GPS坐标为圆心，以GPS的误差范围R为半径画一个圆，找到所有与该圆有交集的路径，这些路径都是汽车当前可能所在的路径link ；S24，针对每一条可能的路径link，都以根节点为父节点，生成两个代表该路径的假设节点，两个假设节点的方向相反，代表该路径的两个方向，这两个都可能是汽车当前状态，然后设置假设节点的其它信息；S25，把所述父节点设置为非活跃节点，把所生成的假设节点设置为活跃节点。其中，当第一次获得GPS数据时，根据所述GPS数据的有效标志位、精度以及卫星数量判断该数据是否有效，当再次获得GPS数据时，所述GPS数据的有效标志位、精度、卫星数量以及GPS数据与Δ t前的GPS数据的距离差距来判断该数据是否有效。其中，若以假设节点j为父节点生成新的假设节点，则步骤S3中生成新的假设节点的步骤具体为根据所述假设节点j的状态和汽车当前状态生成新的假设节点。其中，步骤S4中计算所述权重的步骤如下假设Gn、An、On分别表示在t = η时的GPS坐标、汽车偏转角度和汽车走过的里程数，对任意一条t = η时的汽车当前可能所在的路径h，用Xk表示在t = k时假设节点的位置和汽车方向信息，则路径h的权重计算公式如下P(h\Gx,Ax,Ox,---,Gn,An,On)
一…“ I Xn)P(An'°n I UJ Pih \Π 4 Π . Π Λ Π Λ= PiXnIX^PiGn, An,Οη)’ HI)= A1 P(G"1 xjJIa" f ” 'f"-1 人)· PiK ι G1,4，O1,..., G“, An_x, O“)P (* I **)表示在“**”条件下计算“*”的条件概率。
其中，步骤S5具体为将所有活跃的假设节点，按权重从大到小进行排序；假设只允许出现MaxActiveHypos个活跃的假设节点，则取权重最大的 MaxActiveHypos个活跃的假设节点，而把其他节点删掉；假设多假设树最多只保存MaxHypoLength步的结果，则从每个活跃节点开始往回遍历，把超过MaxHypoLength步的假设节点删除掉。(三)有益效果本发明的方法把汽车可能走过的所有路径都记录在一个树型数据结构上，将树根为汽车的起始点，汽车每获得一次GPS数据，都更新一次多假设树，在树中添加新的叶子节点(即假设节点)。树中每一分枝都代表着汽车可能走过的一段道路序列，从而通过保证路径的连续性而解决了短路径被遗漏的问题；通过同时保存所有的可能性，而保证了即使在GPS偶尔出现较大误差，仍能在以后纠正因这个较大误差而产生的错误；另外，为了克服 GPS信号不好的问题，在方法中引入了 INS数据在GPS较准确时使用GPS数据，在GPS数据不准确时使用INS数据对汽车位置进行估计。
图1为本发明定义的多假设树的结构；图2是本发明的方法流程图；图3是汽车状态转换图；图4示出了产生新的假设节点时，汽车状态的转换过程；图5中a)为本发明实施例中所使用的道路示意图；b)示出了本发明实施例生成的多假设树；图6示出了本发明实施例的实验结果图。
具体实施例方式下面结合附图和实施例，对本发明的具体实施方式
作进一步详细说明。以下实施例用于说明本发明，但不用来限制本发明的范围。如图2所示，本发明的方法包括以下步骤Si、获取全球定位系统GPS数据和惯性导航系统INS数据；S2、根据GPS数据和INS数据(以下简写为GPS/INS数据)定义一颗多假设树，所述多假设树的定义如下多假设树是一棵树(如图1所示，图1中，rl r7表示路径，dl d7表示汽车在本路径上的行驶方向)；该树只有一个根节点(root节点，即rl和dl表示的节点)，代表汽车的起始点；除了根节点外，其余所有节点都是因GPS/INS数据而产生的假设节点。每一个假设节点至少包含的信息如下parent 指向父节点的指针。beginTime 假设节点产生的时间。type:汽车当前的运行状态。汽车可能的状 态有四个initial (初始)；stopping (停止)；accelerating (力口速)；running (运行)。汽车当前的状态可通过INS数据计算获得。该汽车当前状态、汽车可能所在路径、汽车在此路径上的方向，共同组成了该假设节点的状态。link:汽车当前可能所在的路径。direction 汽车在当前link上运行的方向。turnAngle 汽车在At时间内转过的角度。通过INS数据获得。runDis 汽车在At时间内走过的里程。通过INS数据获得。finalBear 汽车当前的方向。通过GPS/INS数据获得。disOnLink 汽车在本link上走过的里程。weight 假设节点的权重。所述权重表示该假设节点包含的信息与真实信息的相似性。active:假设节点的活跃性。代表汽车当前状态的假设节点称为活跃节点，代表汽车历史状态的假设节点为非活跃节点。当汽车接收到GPS/INS数据时，只有活跃节点才能够生成新的假设节点。任何一个假设节点及其所有子节点，又构成一棵多假设树。在获得第一个有效的GPS数据前，是无法得到汽车当前的绝对位置的。于是，多假设树的初始化，是以一个有效的GPS数据为前提的。在获得第一个GPS数据之前，多假设树一直处于未初始化状态(initial，只有root节点)。在步骤S2中以根节点为父节点，生成两个假设节点的步骤具体包括S21，判断GPS数据的有效性，若GPS数据有效则执行如下的步骤，否则不进行初始化；S22，计算汽车当前状态；S23，以GPS坐标为圆心，以GPS的误差范围R为半径画一个圆，找到所有与该圆有交集的路径，这些路径都是汽车当前可能所在的路径link ；S24，针对每一条可能的路径link，都以根节点为父节点，生成两个代表该路径的假设节点，两个假设节点的方向相反，代表该路径的两个方向，这两个都可能是汽车当前状态，然后设置假设节点的其它信息；S25，把所述父节点设置为非活跃节点，把所生成的假设节点设置为活跃节点。影响GPS数据有效性的因素有很多。在GPS数据里有三个参数直接影响了 GPS数据的可靠性，它们是GPS数据的有效标志位、GPS数据的精度(PD0P)、卫星数量。当GPS数据的有效标志位标记此GPS数据无效时，可以认为GPS当前无信号；在GPS数据的有效标志标记此数据有效时，GPS数据中的PDOP值给出了此GPS数据的精度；当卫星数量小于4 时，我们可以认为此GPS数据不可靠。所以，只有当GPS数据标志位有效，而且GPS数据精度大于一定阈值，而且卫星数量不小于4时，我们才认为此GPS数据是有效的。在第一次获得GPS数据时，我们只能按照S211的标准来判断GPS数据的有效性此外，为了保证算法的可靠性，在第二次及以后获得GPS数据时，还引入了另外一个判断GPS数据有效性的因素。由于汽车运行的速度是有限的，汽车在△1时间内走过的里程也是有限的。所以，如果已经认为Δ 1时间前的GPS数据是有效的，而且当前的GPS数据与At前的GPS数据的距离差距大于一定阈值时，则可以认为当前的GPS数据无效。否则，可以认为此GPS数据是有效的。现在图3解释如何判断汽车的运行状态汽车可能的运行状态如下initial 初始状态。当汽车刚启动时， 认为汽车处于本状态。stopping 停止状态。汽车在Δ t时间内的速度和偏转角均小于一定阈值时，认为汽车处于stopping状态。running 运行状态。汽车在Δ t时间内的速度大于一定阈值，偏转角小于一定阈值时，认为汽车处于running状态。accelerating 加速状态。当汽车不处于上面三种状态时，认为汽车正处于加速状态。判断汽车运行状态的方式如下判断GPS数据的有效性。如果GPS数据无效，且多假设树还未初始化，则说明无须计算汽车状态。根据汽车的INS数据，计算汽车的速度、偏转方向和里程。如果汽车的速度小于停止速度阈值，且汽车偏转角度小于停止角度阈值时，设置汽车状态为stopping状态。如果汽车的程度大于运行速度阈值，且汽车偏转角度小于运行角度阈值时，设置汽车状态为running状态。其它情况下，设置汽车状态为accelerating状态。S3、在汽车每获得一次GPS数据时，在多假设树中生成新的假设节点；若以假设节点j为父节点生成新的假设节点，则步骤S3中生成新的假设节点的步骤具体为根据所述假设节点j的状态和汽车当前状态生成新的假设节点(即所选假设节点的子节点)。图4中，(a)表示汽车的前一状态；(b)表示汽车从stopping状态转换为 stopping ； (c) ^TjkhK stopping 转换为 accelerating ； (d) @示从 accelerating 转换为 stopping ； (e)表示从 accelerating 转换为 accelerating ； (f) @示从 running 转换为 stopping ； (g)表示从 running 转换为 accelerating ； (h)表示从 stopping 转换为 running ； (i)表示从 accelerating 转换为 running ； (j)表示从 runing 转换为 rurming。如图4所示，生成所述新的假设节点的方式如下非活跃节点不产生新的假设节点。如图4(b)，如果所选假设节点中的汽车状态为stopping，且汽车当前状态为 stopping，则无须产生新的节点。如图4(c)，如果所选假设节点中的汽车状态为stopping，且汽车当前状态为 accelerating，则为所选节点生成两个子节点，一子节点与父节点的方向相同，另一子节点与父节点的方向相反。如图4(d)，如果所选假设节点中的汽车状态为accelerating，且汽车当前状态为 stopping，则为所选节点生成一个与父节点方向相同的子节点。设置父节点为非活跃节点。如图4(e)，如果所选假设节点中的汽车状态为accelerating，且汽车当前状态为 accelerating，且汽车在本条路上走过的路程小于该道路的长度，则不产生新的节点。否贝U，找到与该道路末端相连的所有道路，针对每一条道路，生成一个新的子假设节点。
如图4(f)，如果所选假设节点中的汽车状态为running，且汽车当前状态为 stopping，则为所选假设节点生成一个与父节点方向相同的子节点。设置父节点为非活跃节点。如图4(g)，如果 所选假设节点中的汽车状态为running，且汽车当前状态为 accelerating，则为所选假设节点生成一个与父节点方向相同的子节点。设置父节点为非活跃节点。如图4(h)，如果所选假设节点中的汽车状态为stopping，且汽车当前状态为 running，则为所选假设节点生成两个子节点，一子节点与父节点的方向相同，另一子节点与父节点的方向相反。设置父节点为非活跃节点。如图4(i)，如果所选假设节点中的汽车状态为accelerating，且汽车当前状态为 running，则为所选假设节点生成两个子节点，一子节点与父节点的方向相同，另一子节点与父节点的方向相反。设置父节点为非活跃节点。如图4(j)，如果所选假设节点中的汽车状态为running，且汽车当前状态为 running，且汽车在本道路上走过的路程小于该道路的长度，则不产生新的节点。否则，找到与本道路末端相连且与本道路夹角小于一定阈值的所有道路，针对每一条道路，生成一个新的子假设节点。S4、根据所述GPS数据和INS数据计算所述假设节点的权重，所述权重表示该假设节点包含的信息与真实信息的相似性使用GPS/INS数据来计算假设节点的权重。假设Gn、An、0n分别表示在t = η时的 GPS坐标、汽车偏转角度和汽车走过的里程数，对任意一条t = η时的汽车当前可能所在的路径h，用Xk表示在t = k时假设节点的位置和汽车方向信息，则路径h的权重计算公式如下PihlG^Al,Ou--^GniAn,On)
Γ01101 P(GJXn)PjAn,Οη\Χη^,Χη)
torn]=人户丨1 U") ·Ρ("。1 G1,A1,01,..,Gn,,A^OnJ
尸(Λη IUρ(*|**)表示在“**”条件下计算“*”的条件概率。上面的计算公式仅为最基本的计算公式。除此之外，我们还需要考虑更多的情况Α.当某一个假设节点代表汽车在一条单行线上逆向行驶时，我们要对这个假设节点的权重进行很大的惩罚。此惩罚具体为将权重减去一个很大的固定值 OneWayPun i shment。B.当某一条假设通过INS数据估计的汽车方向与路径的方向相差很大时，要对这个假设节点的权重进行较大的惩罚。此惩罚具体为将权重减去一个数，这个数等于两夹角的差的平方乘以一系数K1。C.当某一条假设认为汽车已经运行到新的路径，但通过INS累加得到的在原来路径上跑过的距离远远小于原来路径的总长度时，我们要对这个假设节点的权重进行较小的惩罚。此惩罚具体为将权重减去一个数，这个数等于根据INS数据得到的未走完的路径长度相对于总长度的比例的平方乘以一系数K2。S5、根据假设节点的权重删除一部分假设节点
将所有活跃的假设节点，按权重从大到小进行排序。假设只允许出现MaxActiveHypos个活跃的假设节点，则取权重最大的 MaxActiveHypos个活跃的假设节点，而把其他节点删掉；假设多假设树最多只保存MaxHypoLength步的结果，则从每个活跃节点开始往回遍历，把超过MaxHypoLength步的假设节点删除掉。MaxActiveHypos和MaxHypoLength均为正整数，所述分支上的每一个假设节点代表一步。S6、选择权重最大的假设节点作为最佳假设节点，然后重复步骤Sl S5，每一轮可获得一个最佳假设节点，该最佳假设节点所在的分支记录了汽车走过的路径轨迹。我们可用此最佳假设节点，与权重第二大的假设节点来计算最佳假设节点的置信度，计算公式如下:belief = l~exp (secondweight-biggestweight)，其中，secondweight 表不所有假设节点中权重第二大的假设节点的权重，biggestweight表示权重最大的假设节点的权重。下面举例说明本发明的方法。如图5所示，假设有两条四条道路(即上面提到的路径)组成一个十字路口，这四条道路分别是0A、0B、0C、0D，交点为0点。图5的a)中坐标的tl_t5时刻上所画的点代表汽车在tl-t5时刻时汽车所在的位置。下面将描述如何从tl开始，不断地根据GPS数据更新多假设树。每一个多假设树的节点都会包含道路的编号，以及汽车在这条道路上走的方向。 假定(A0)表示汽车在AO这条道路上走，而且方向是由A向0的方向行驶。如果汽车是从 0向A行驶的话，则用(OA)表示本节点。步骤如下假设在tl时刻，汽车走在AO这条道路上，方向从A驶向0，汽车的状态为运行。在t2时刻时，汽车的状态变成了加速。根据图4的汽车状态转换表，当汽车从运行状态转换到加速状态时，汽车只生成一个子节点，而且方向不变。在t3时刻时，汽车的状态变成了停止(比如汽车在等红灯)。根据汽车状态转换表，当汽车从加速状态转换到停止状态时，汽车只生成一个子节点，且方向不变。在t4时刻时，汽车的状态变成了加速。根据汽车状态转换表得知，当汽车从停止状态变成加速状态时，将在各个方向上生成相应的假设节点。在t5时刻时，汽车的状态变成了运行。根据汽车状态转换表得知，当汽车从加速状态变成运行状态时，将在各个方向上生成相应的假设节点。但由于本实施例中，汽车已经走过了十字路口，所以，只在汽车原来的道路和方向上生成了一个运行状态的假设节点。如图6所示，为实验中，在真实的电子地图上执行本发明的方法所得到的匹配路径图，图中的地图所示为北京国贸桥，匹配路径附近的粗线段为GPS数据，从图中可以看出本发明的方法能够很好地保证道路匹配的准确性和连续性。以上实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。
权利要求
1.一种基于汽车状态转移的多假设地图匹配方法，其特征在于，包括以下步骤S1、获取全球定位系统GPS数据和惯性导航系统INS数据；S2、根据GPS数据和INS数据定义一颗多假设树，所述多假设树用于记录汽车走过的所有路径，其有一个根节点，所述根节点代表汽车的起始点；S3、在汽车每获得一次GPS数据时，在多假设树中生成新的假设节点；S4、根据所述GPS数据和INS数据计算所述假设节点的权重，所述权重表示该假设节点包含的信息与真实信息的相似性；S5、根据假设节点的权重删除一部分假设节点；S6、选择权重最大的假设节点作为最佳假设节点；然后重复步骤Sl S5，每一轮获得一个最佳假设节点，该最佳假设节点所在的分支记录了汽车走过的路径轨迹。
2.如权利要求1所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，在步骤S2中定义多假设树时，假设每个所述假设节点至少包含如下信息假设节点产生的时间；汽车当前状态，该状态有四种初始、停止、加速和运行，汽车当前状态通过INS数据计算获得；汽车当前可能所在的路径link ；汽车在link上运行的方向；汽车在At时间内转过的角度，其通过INS数据获得；汽车在At时间内走过的里程，其通过INS数据获得；汽车当前的方向，其通过GPS数据和INS数据获得；汽车在link上走过的里程；假设节点的权重；假设节点的活跃性，代表汽车当前状态的假设节点称为活跃节点，代表汽车历史状态的假设节点称为非活跃节点。
3.如权利要求2所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，在步骤S2中以根节点为父节点，对每一条汽车可能所在的路径，生成两个路径相同但方向不同的假设节点，步骤具体包括S21，判断GPS数据的有效性，若GPS数据有效则执行如下的步骤，否则不进行初始化； S22，计算汽车当前状态；S23，以GPS坐标为圆心，以GPS的误差范围R为半径画一个圆，找到所有与该圆有交集的路径，这些路径都是汽车当前可能所在的路径link ；S24，针对每一条可能的路径link，都以根节点为父节点，生成两个代表该路径的假设节点，两个假设节点的方向相反，代表该路径的两个方向，这两个都可能是汽车当前状态， 然后设置假设节点的其它信息；S25，把所述父节点设置为非活跃节点，把所生成的假设节点设置为活跃节点。
4.如权利要求3所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，当第一次获得GPS数据时，根据所述GPS数据的有效标志位、精度以及卫星数量判断该数据是否有效，当再次获得GPS数据时，所述GPS数据的有效标志位、精度、卫星数量以及GPS数据与Δ t前的GPS数据的距离差距来判断该数据是否有效。
5.如权利要求4所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，若以假设节点j为父节点生成新的假设节点，则步骤S3中生成新的假设节点的步骤具体为根据所述假设节点j中表示的汽车状态和汽车当前状态生成新的假设节点。
6.如权利要求5所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，步骤 S4中计算所述权重的步骤如下假设Gn、An、On分别表示在t = η时的GPS坐标、汽车偏转角度和汽车走过的里程数，对任意一条t = η时的汽车当前可能所在的路径h，用Xk表示在t = k时假设节点的位置和汽车方向信息，则路径h的权重计算公式如下
7.如权利要求6所述的基于汽车状态转移的多假设地图匹配方法，其特征在于，步骤 S5具体为将所有活跃的假设节点，按权重从大到小进行排序；假设只允许出现MaxActiveHypos个活跃的假设节点，则取权重最大的 MaxActiveHypos个活跃的假设节点，而把其他节点删掉；假设多假设树最多只保存MaxHypoLength步的结果，则从每个活跃的假设节点开始往回遍历，把超过MaxHypoLength步的假设节点删除掉。
全文摘要
本发明公开了一种基于汽车状态转移的多假设地图匹配方法，包括以下步骤S1、获取全球定位系统GPS数据和惯性导航系统INS数据；S2、根据GPS数据和INS数据定义一颗多假设树，所述多假设树用于记录汽车走过的所有路径，其有一个根节点，所述根节点代表汽车的起始点；S3、在汽车每获得一次GPS数据时，在多假设树中生成新的假设节点；S4、根据所述GPS数据和INS数据计算所述假设节点的权重，所述权重表示该假设节点包含的信息与真实信息的相似性；S5、根据假设节点的权重删除一部分假设节点；S6、选择权重最大的假设节点作为最佳的假设节点。本发明能记录汽车所走过的连续路径，在GPS信号不太好，或者道路密集的区域，能够获得较好的匹配结果。
文档编号G01C21/30GK102175253SQ20101062251
公开日2011年9月7日 申请日期2010年12月28日 优先权日2010年12月28日
发明者刘松, 刘澍, 戴良光, 王道顺, 石宗英, 董琳, 贺志宏, 赵明国, 高达 申请人:清华大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
