一种基于路径追踪的车载导航定位轨迹匹配方法
【专利摘要】一种基于路径追踪的车载导航定位轨迹匹配方法，其特征在于在导航过程中，根据接收到的航向角和里程数据提取导航轨迹特征段，并与已提取的路网数据中与导航轨迹特段征邻近的待匹配的路段进行匹配，得到与导航轨迹特征段匹配的路段，根据匹配结果计算导航的定位误差，然后进行修正，导航轨迹特征段是转弯路段的导航轨迹特征段。本发明与已有技术相比，具有能实时精确定位行驶中的车辆位置，进而有效提升导航精度的优点。
【专利说明】
一种基于路径追踪的车载导航定位轨迹匹配方法
[0001 ] 技术领域:
本发明涉及一种导航定位技术。
[0002]【背景技术】:
惯性导航系统能够完全依靠其自身设备自主地完成导航任务，不与外界发生任何光、电联系，并且工作条件不受环境因素限制，是一种自主性强、全天候、全参数的导航系统。但是惯性导航系统的导航误差随时间积累而发散，因此必须将惯性导航系统与其它辅助导航系统进行组合，从而补偿惯性导航系统的导航误差。
[0003]目前常用的组合导航系统包括惯性/卫星、惯性/地磁、惯性/地形匹配等组合导航系统。车辆运动过程中卫星信息解算可能出现延迟，在不同时间段卫星辅助导航精度也存在差异，而地磁、地形本身的测量精度就不高，因此组合导航的精度有限。
[0004]目前，地图匹配技术随着GPS应用的普及有了很大的发展。地图匹配技术主要解决由卫星定位设备获取的位置信息如何与相应的电子地图进行匹配，找到定位点在电子地图上的相应位置。其主要任务是确定当前定位点对应电子地图上的哪条道路以及在该道路上的具体位置。
[0005]
【发明内容】
:
本发明的发明目的在于提供一种能实时精确定位行驶中的车辆位置，进而有效提升导航精度的行驶中的车辆定位方法。
[0006]本发明是这样实现的，在导航过程中，根据接收到的航向角和里程数据提取导航轨迹特征段，并与已提取的路网数据中与导航轨迹特征段邻近的待匹配的路段进行匹配，得到与导航轨迹特征段匹配的路段，根据匹配结果计算导航的定位误差，然后进行修正，导航轨迹特征段是转弯路段的导航轨迹特征段。
[0007]直线路段的轨迹特征只能获得法向的定位误差，而不能获得径向的定位误差，而转弯路段的轨迹特征既能获得法向的定位误差，同时，也能获得径向的定位误差，从而提升了误差修正的精确度。
[0008]本发明与已有技术相比，由于采用导航轨迹特征与精确的路网数据进行匹配来修正误差，因此，具有能实时精确定位行驶中的车辆位置，进而有效提升导航精度的优点。
[0009]【附图说明】:
图1为本发明的路径匹配流程图；
图2为路径追踪状态选择图；
图3为初始状态处理流程图；
图4为为追踪状态流程图；
图5为特征提取与路径匹配流程图；
图6为导航轨迹特征段覆盖的空间范围图。
[0010]【具体实施方式】:
现结合附图和实施例对本发明做进一步详细描述:
整个路径匹配过程如图1所示。[0011 ]如图2所示，将路径追踪状态划分为初始状态，追踪状态、路口状态和搜索状态。其定义为:
初始状态:从系统启动到行驶一定距离以内这段区间内，还未转换为追踪状态。
[0012]追踪状态:在两个路口之间，已经确定车辆在哪几段道路上行驶的状态。
[0013]路口状态:车辆行驶的距离路口一定范围以内，且还无法确定车辆经过路口后走哪条道路的状态。
[0014]搜索状态:不能确定车辆处于以上三种状态中的任意一种时的状态。
[0015]如图3所示，在接收到卫星导航定位数据时，在开始导航时，先进行初始状态处理，以确定目标车辆所处的路段，其过程是:
步骤1:计算从卫星获得的当前点的定位位置与路网(车载的电子地图)中所有道路的最短距离(方法为常见的点与折线的距离计算)，并比较该距离的大小，如果路网中有道路小于设定的值(如10米，10米为经验值，可根据实际情况替换为其它值)，则将该道路记录为当前道路，并记录确定过程车辆所行驶的里程(因为计算确定过程是需要时间的，这段时间内，车辆是在运动着的)。
[0016]步骤2:判断当前道路与上次记录的道路是否相同，如果不同，则此次的路段确定为首次，然后重复步骤I，，若相同，将此次确定过程车辆所行驶的里程累计到上次的确定过程车辆所行驶的里程，若累加的里程大于设定值(如20米，20米为经验值，可根据实际情况替换为其它值)，则将初始状态转换为追踪状态。
[0017]在步骤I中如果在确定目标车辆所处的路段前，车辆所行驶的里程大于设定的里程，则进入搜索状态处理，即重新开始初始状态处理;若从卫星获得的当前点的定位位置与路网中所有道路的最短距离都没有小于设定的值的，则进入搜索状态处理，即重新开始初始状态处理。
[0018]然后是追踪状态，其过程如图4所示。
[0019]若在追踪状态过程中，进入路口时，则进行路口状态处理，
路口参数的计算:
路口参数主要包括:进入路口的路径，离开路口的路径，进入路口的航向角，离开路口的航向角。进入路口的道路即路口算法中获取的最后一条道路;离开路口的路径即以路口点为起点的所有道路。相应的航向角即为相应道路的航向角。
[0020]如图5所示，在导航过程中，进行特征提取与路径匹配，包括:
I)自主导航系统发送定位数据的记录
自主导航定位数据是根据导航系统测量的车辆运动信息(包括惯性导航平台，捷联惯组等)计算产生的。定位数据包括经度、玮度和高程。另外为了提取自主导航轨迹的特征，还需要记录自主导航平台发送的航向角信息，接收自主导航定位数据的具体方法可以通过串口通讯，直接集成等方法实现。
[0021 ])导航轨迹特征段的计算与提取
惯性导航的误差可以分解为北向误差和东向误差，也可以分解为沿道路行进方向的径向误差和垂直道路行进方向的法向误差。如果对直线特征进行匹配，只能发现垂直于道路行进方向的法向误差，而无法得到沿道路行进方向的径向误差，因此需要在航向具有一定程度变化的路段进行匹配，另一方面为了提高匹配的计算效率，只考虑对车载自主导航系统的定位误差进行平移变换，忽略旋转变换及累积误差，因此参与路径匹配的导航轨迹的特征段应该满足下列条件:
(I)特征段内轨迹的航向角变化必须大于预设的阈值，即从特征段开始到特征段结束，航向角变化大于一定的角度(角度的大小需要根据实际的自主导航系统进行确定，本发明经试验取值45°)。
[0022](2)特征段转弯的角度不要过于平缓，即从特征段开始到特征段结束，航向特征段转弯过程的航向角速率速度大于一定的阈值(航向角速率速度阈值根据实际的自主导航系统进行数据确定，本发明经试验定为4°/s)，主要是因为当特征段航向角速度太小时，转弯过于平缓，为了满足条件(I)，提取的特征段就会过长，在较长的特征段中位置累积误差比较大，而本发明将位置误差作为固定值，因此将不适合本发明。
[0023]上述条件对导航轨迹特征段进行了限定，但是在实际的自主导航轨迹中还会出现满足上述条件但不属于轨迹特征的情况需要进行剔除，主要分为一下两种:
(I)车辆紧急避让，车辆在行驶过程中紧急避让障碍物产生的轨迹变化。
[0024](2)自主导航平台自身的随机误差产生的轨迹变化。
[0025]根据当前位置点的航向角信息，将车辆的运动状态分为三类:直行、顺时针转弯和逆时针转弯，并分别用0、_1、1表示，如果当前的行驶状态发生改变，则表明发现了一个特征;对直线特征不进行处理，对顺时针转弯和逆时针转弯的特征进行提取。
[0026]在特征提取过程中为避免自主导航数据的随机误差或者紧急避让产生的行驶状态改变对特征提取的影响，设置有子状态处理:
子状态处理:当状态发生改变时并不立即进行特征提取，而是将这种改变作为当前状态的一种子状态。
[0027]当追踪的行驶状态发生改变时将进行一个子状态处理。为了避免提取的特征段过长而无法满足算法的要求，在提取过程中根据时间间隔对特征提取过程进行控制，在规定的时间段内行驶航向角的变化量小于一定值时结束当前特征的跟踪，开始新的跟踪。
[0028]导航轨迹特征段的提取流程如下:
当开始导航时，开始新的跟踪，将开始新的跟踪时接收的有航向角变化的轨迹点的信息记录为当前导航轨迹特征段的轨迹点；
接收下一个轨迹点信息时，若该下一个轨迹点信息的航向角变化与前一个轨迹点信息的航向角变化相反，则将该下一个轨迹点信息记录为子导航轨迹特征段的轨迹点，否则，该下一个轨迹点信息记录为当前导航轨迹特征段的轨迹点；
若所接收的数个子导航轨迹特征段的轨迹点所形成的轨迹特征段的转弯角度大于设定的角度(如大于15°或者大于当前跟踪状态转弯角度的1/4)，则将构成子导航轨迹特征段的轨迹点轨迹点信息记录为当前导航轨迹特征段的轨迹点；
若在设定的时间内所有的当前导航轨迹特征段的轨迹点所形成的轨迹段的转弯角度大于设定的角度，则该轨迹段就是导航轨迹特征段；
若在设定的时间段内行驶航向角的变化量小于设定值时结束当前特征的跟踪，并开始新的跟踪。
[0029])自主导航轨迹与路网数据的匹配方法
步骤1:计算导航轨迹特征段覆盖的空间范围，即计算轨迹特征段的最小外接矩形(最小外接矩形即能够包含该特征段的最小矩形，其提取方法为行业共知)，在该矩形的基础上向外各扩充五分之一或三分之一(如图6所示，图中为向外各扩充四分之一)作为导航轨迹特征段覆盖的空间范围；
步骤2:将导航轨迹特征段覆盖范围内的道路作为备选特征路段(若是在已知道路上进行自主导航轨迹与路网数据的匹配，如在追踪状态下，则只需要将追踪点往前及往后设定一定距离后的该已知道路的路段作为备选特征路段即可)；
步骤3:计算导航轨迹特征段上位置点沿前进方向的法向量在备选特征路段上的投影点，并计算导航轨迹特征段的轨迹点与投影点之间的距离，如果无法投影到备选特征路段上，认为该轨迹点与投影点之间的距离为其它轨迹点与投影点之间的距离的均值；
步骤4:计算所有距离的均值作为平移量对轨迹点进行平移；
步骤5:计算所有轨迹点到投影点的均方差，将本次均方差与上次均方差进行比较，如果两者之差小于0.0l米则执行步骤6，否则，如果本步迭代超过15次，则匹配失败，结束本次匹配;否则将平移后的导航轨迹特征段作为初始特征段，执行步骤I;
步骤6:当满足两次均方差之差小于0.01米时，匹配成功;将所有的平移量进行累加，作为导航轨迹特征段与匹配道路之间的位置误差；
步骤7:计算最后一次平移后所有轨迹点沿前进方向法方向在道路上的投影点，并计算轨迹点与平移点之间的均方差作为匹配相似度的度量。
【主权项】
1.一种基于路径追踪的车载导航定位轨迹匹配方法，其特征在于在导航过程中，根据接收到的航向角和里程数据提取导航轨迹特征段，并与已提取的路网数据中与导航轨迹特段征邻近的待匹配的路段进行匹配，得到与导航轨迹特征段匹配的路段，根据匹配结果计算导航的定位误差，然后进行修正，导航轨迹特征段是转弯路段的导航轨迹特征段。2.根据权利要求1所述的基于路径追踪的车载导航定位轨迹匹配方法，其特征在于导航轨迹特征段的提取流程如下: 当开始导航时，开始新的跟踪，将开始新的跟踪时接收的有航向角变化的轨迹点的信息记录为当前导航轨迹特征段的轨迹点； 接收下一个轨迹点信息时，若该下一个轨迹点信息的航向角变化与前一个轨迹点信息的航向角变化相反，则将该下一个轨迹点信息记录为子导航轨迹特征段的轨迹点，否则，该下一个轨迹点信息记录为当前导航轨迹特征段的轨迹点； 若所接收的数个子导航轨迹特征段的轨迹点所形成的轨迹特征段的转弯角度大于设定的角度，则将构成子导航轨迹特征段的轨迹点轨迹点信息记录为当前导航轨迹特征段的轨迹点； 若在设定的时间内所有的当前导航轨迹特征段的轨迹点所形成的轨迹段的转弯角度大于设定的角度，则该轨迹段就是导航轨迹特征段； 若在设定的时间段内行驶航向角的变化量小于设定值时结束当前特征的跟踪，并开始新的跟踪。3.根据权利要求2所述的基于路径追踪的车载导航定位轨迹匹配方法，其特征在于匹配过程是: 步骤1:计算导航轨迹特征段覆盖的空间范围，即计算轨迹特征段的最小外接矩形，在该矩形的基础上向外各扩充五分之一或三分之一作为导航轨迹特征段覆盖的空间范围；步骤2:将导航轨迹特征段覆盖范围内的道路作为备选特征路段； 步骤3:计算导航轨迹特征段上位置点沿前进方向的法向量在备选特征路段上的投影点，并计算导航轨迹特征段的轨迹点与投影点之间的距离，如果无法投影到备选特征路段上，认为该轨迹点与投影点之间的距离为其它轨迹点与投影点之间的距离的均值； 步骤4:计算所有距离的均值作为平移量对轨迹点进行平移； 步骤5:计算所有轨迹点到投影点的均方差，将本次均方差与上次均方差进行比较，如果两者之差小于0.0l米则执行步骤6，否则，如果本步迭代超过15次，则匹配失败，结束本次匹配;否则将平移后的导航轨迹特征段作为初始特征段，执行步骤I; 步骤6:当满足两次均方差之差小于0.01米时，匹配成功;将所有的平移量进行累加，作为导航轨迹特征段与匹配道路之间的位置误差； 步骤7:计算最后一次平移后所有轨迹点沿前进方向法方向在道路上的投影点，并计算轨迹点与平移点之间的均方差作为匹配相似度的度量。
【文档编号】G01C21/34GK105841708SQ201610149503
【公开日】2016年8月10日
【申请日】2016年3月16日
【发明人】张彩霞, 王向东
【申请人】佛山科学技术学院