随着导航辅助定位手段的不断更新，组合导航的精度得到了不断的提高，尤其是以GPS为代表的辅助导航方法，极大地满足了各类用户的导航定位需求。但是基于卫星的导航手段，受到定位源信号质量的影响，其自主性和抗干扰性都大打折扣，特别是在高大建筑群、立交桥或者涵洞等位置，信号容易受到遮蔽或者干扰，其定位精度往往不够理想^[1]。与之相比，以轨迹匹配为基础的辅助导航方法，在抗外界干扰和自主性上有着明显的优势，在组合导航中得到了广泛应用^[2]。其基本思想是利用具有较高精度的导航电子地图或者路网数据库中的道路数据，与车辆行驶过程中传感器测量的轨迹数据进行匹配，并由此确定车辆相对于道路的位置信息，从而抑制组合导航系统的误差，在一定程度上提高了定位精度。此外，以惯性导航为主的组合导航系统还能提供航向角、速度以及里程信息，给轨迹匹配算法以更多的优化空间^[3]。

现有地图匹配算法以模式识别理论作为基础，根据几何形状特征，通过比较道路特征与轨迹特征的相似性，以获得相似性最大的道路作为匹配结果。例如最常用的直接投影算法，其原理清晰，逻辑简单，且实时性较高^[4-6]。但算法实施过程中需要进行大量的距离运算，增加了系统内存负荷；尤其在遇到道路形状复杂或者交叉口等特殊路段时，其匹配准确率相对较低^[7-9]。此外，算法过于依赖对投影距离的判断，而车辆的其他轨迹参数信息如航向角、行驶里程等都没有得到充分利用，造成了有效信息的浪费^[10]。文献[11]提出一种基于权重因子的算法，根据车辆轨迹的参数，按照一定的评价标准获取最后的匹配结果，进一步提高了匹配算法的置信度。但权重系数的确定大多根据经验值，缺乏科学的选择标准^[12-13]。因此，在遇到评价权值相似或者相近的情况时，匹配结果容易出现跳变，从而导致误匹配。文献[14]在直接投影算法的基础上做了许多有效的改进工作，提出了一种基于整体形状特征的匹配思想，一定程度上降低误匹配的概率。但算法对定位轨迹的连通性重视不够，在遇到较为复杂的道路段时，匹配结果仍不够理想，不能完全满足组合导航的定位需求^[15-17]。

虽然轨迹匹配算法能够有效地解决大部分的道路匹配问题，然而算法需要获取完整的道路形状特征才能进行匹配分析，其实时性不够理想^[18]。此外，在遇到例如交叉口、立交桥或者匝道等特殊路况时，容易发生误匹配，直接影响了导航的定位结果。考虑到车辆在道路网中的行驶轨迹应该是一条连续路径，其匹配结果也应该是一条连续路径^[19]。而矢量道路一般以离散点集组成，其定位坐标和拓扑关系上有很高的准确度，因此在研究匹配算法时，应该充分利用道路中的拓扑结构和轨迹点之间的关系^[20-21]，正如Bemstein^[22]指出的那样，越注意网络的拓扑结构，匹配的结果越好。鉴于上述思想，本文以道路拓扑结构为基础，提出了一种基于道路追踪的矢量道路匹配算法，根据不同道路拓扑结构的变化，进入不同的匹配状态，进行实时匹配修正。本文进行了实地跑车和仿真实验，并与文献[14]的匹配以及基于形状的匹配算法^[23]进行了比较，实验结果表明加入拓扑追踪算法后，能提高有效特征点的匹配数量和准确度，针对于常规算法难以匹配的复杂道路情况^[24]，也有着良好的匹配效果。

道路追踪就是从车辆轨迹初始点开始，找到道路相应的追踪点，并将车辆轨迹一直确定在矢量地图某一对应道路上，如果车辆行驶过程中遇到转弯特征、交叉口等情况，追踪点需要根据具体的航向角、里程变化情况，判断车辆当前的行驶道路是否发生了改变。如果改变则重新记录当前道路ID，并继续追踪，直到下一次出现变化为止。

所以新算法必须解决的问题是：如何找到轨迹初始点的道路对应点，并以确定道路为基础，结合路网拓扑结构，监控车辆行驶状态的改变情况，以确定其匹配的道路。

根据车辆行驶的实际状况及地图匹配算法的主要特点，本文给出车辆在追踪匹配定位过程中的4种不同状态，并针对每种状态各自的特点和定位要求，采取了不同的处理方法，这4种状态包括：

初始化状态 (initialization) 负责搜索车辆轨迹点对应的道路，采用最近距离原则搜索相应道路，并通过垂直投影算法确定轨迹点在道路中的对应点以及相应的行进方向；

追踪状态 (tracing) 根据经过初始化后轨迹点在道路的对应点或者完成一次与道路匹配修正的轨迹点开始追踪，计算当前点到下一个路口的距离，判断是否到达路口区域，判断过程中如果发现道路特征则进行特征匹配；

路口状态 (intersection) 对轨迹点进入路口区域的运动情况进行判断，按照入口的方向依次分析到出口轨迹点的角度变化量和运动趋势，从而确定正确的出口道路；

搜索状态 (searching road) 当算法无法追踪到轨迹点的运动情况和对应的道路点时，重新进行基于形状匹配，找到对应道路后进入初始化状态。

图 1表示了4种状态间的转换关系以及相应状态下所采用匹配方法。

本文采用矢量道路数据是通过高精度GPS-RTK测量并通过事后差分，而后进行道路采集点数据抽稀、转弯形状特征点提取^[18]、道路曲线拟合和中心线生成等预处理步骤后，获取的表示道路中心线的一系列顺序坐标点集合。拓扑结构是指根据拓扑关系进行空间数据的组织方式，具体到道路网络中，主要是记录和表达各条道路和对应结点之间的相互连通和关联的情况。为了满足道路追踪匹配算法的要求，需要建立点、线之间的空间拓扑关系，重新构建矢量道路数据的存储结构，并建立符合路网实际情况的数据库和表结构。为了更好地理解矢量道路网结构，本文定义矢量道路中的结点、路段的概念如下：

结点：道路的起止点或相交道路的交叉点，用于表示道路的交叉路口或道路的起止情况，串联整个道路网络结构。

路段：两个结点之间的一段道路，由一系列的有序坐标点集合组成，包括表示直线道路的一般路段点和表示弯曲道路的形状路段点，用于表示分段的道路，具有正向、反向和双向通行三种方向属性。

为了生成便于道路追踪匹配的网络拓扑结构，本文首先定义结点数据 (表 1)、路段数据以 (表 2) 及路口数据的数据结构，然后根据相应的数据结构完成矢量道路的拓扑结构生成。

表 2中基础属性数据中的nType表示是当前路段中的路段点类型，可分为一般路段点 (直线路段) 和路段形状点 (弯曲路段)。路段形状点是指路段中具有一定弯曲特征的有序点集合，即实际行车中通过的一个曲率较大的弯曲路段，例如道路转弯等突出的部分。形状点可将路段划分为多个形状点组成的子路段，用于道路和车辆轨迹点的匹配计算。形状点相对一般路段点采样间隔更为稠密，可以通过预处理后道路采样点的密集程度进行划分提取。弯曲路段开口方向是指该路段曲率最大一点处切线的法线方向与正北方向的逆时针夹角，用于甄别和区分不同的弯曲路段。

路口是一个比较特殊的对象要素，它是道路网的交汇点，是行人、车辆的汇合、转向、分流的地方。表现在数据结构上，则包含一定区域内的相交弧段 (道路) 及其交点，主要记录了从进入路口到离开路口过程中车辆可能行驶的所有道路。路口通常指的是多道交叉口，即3条或者3条以上路段 (单车道或多车道) 平面相交形成的一个结点，但有时也存在多条道路的中心线在交汇处并没有形成交点的情况，但若存在交叉口的交汇作用，仍要作为一个路口结点进行处理。环岛型交叉口 (如图 2(d)所示) 作为一种比较特殊交叉口，其交叉结点坐标实际并存在，需要通过几何计算才能获取，在环岛内部车辆必须单行，一般按照环岛逆时针旋转行驶。此外，还存在一些平面视觉上的“立体相交”的虚结点或“伪路口”(如上下层立交桥，如图 2(e)所示) 以及道路延长线相交的伪结点 (如图 2(f)所示) 等，这些都不能作为路口结点的拓扑结构。

因此，本文设计的路口结构如下：

typedef struct _CrossData

{

  int nID；

  int nRoadCount；//所有通行路径的个数

  int nRoadIDCount[40]；//每条通行路径包含的道路ID数组

  double ^*x，^*y；//通行路径包含道路点坐标集合

  double pInDirection[40]；//每个通行路径的入口转弯角

  double pOutDirection[40]；//每个通行路径的出口弯角

  double diffAngle[40]；//角变化量

  double disCross[40]；//路径长度

    double right；//路口右边界

double left；//路口左边界

double top；//路口上边界

double bottom；//路口下边界

  }CrossData

根据国家《城市规划定额指标暂行规定》以及实地测量^[25]，考虑车道数量及其隔离带、绿化带的宽度，车辆在路口完成转弯行为的过程中，其路口范围一般在边长为100 m左右矩形区域内，所以本文设定路口区域为以道路交叉口点为中心的100 m正方形边框内包含的各种点和路段要素。

在矢量道路追踪匹配过程中，由于只考虑道路网信息，因此只需要建立结点与路段的拓扑关联关系。本文采用图形拓扑结构组织道路网，该拓扑结构适合于存储，并有效地改善了运行效率。以十字和丁字路口为例，建立简单道路网的拓扑结构如图 3所示，路段用弧段表示，路段的两个端点用结点表示，结点编号用于规定结点的惟一标识，弧段编码用于规定弧段的惟一标识，路口用一个包含路段和结点的区域来表示。

建立拓扑关系步骤如下：

1) 建立结点ID列表和路段的ID列表。根据图 2分别建立结点ID列表和路段ID列表，如表 3，表 4所示。

路口结点可直接根据前文提出的100 m外接矩形范围进行相应区域内路段点提取，以便后续路口匹配算法的实施。

需要说明的是要考虑图 2中的各条路段通行方向，如果是单行路段只在表 4中添加一条记录即可，如果是双向通行的路段，还需要将该路段的反向加入到表中，以区分于正向通行的路段作为新的路段。表 4中表示的是各路段均为单行通行。

2) 建立结点-路段拓扑关系表。在图 2的道路网络中，根据表 3、表 4建立的结点和路段列表，依次读取结点和道路数据信息，并进行相同ID结点的合并，即可获得结点-路段的拓扑关系，如表 5所示。

通过建立矢量道路的数据结构和拓扑关系，不仅反映出了矢量道路网络中各结点、路段要素之间的相互关系，可将道路数据的全局搜索变成局部搜索，大幅提高了搜索效率，为后续匹配算法及其数据处理提供了良好的数据基础。

矢量道路拓扑追踪匹配算法的实施要基于两个必要前提：1) 车辆必须行驶在道路上；2) 矢量道路数据的精度要高于车载惯性导航的精度。由于本文矢量道路数据是通过GPS-RTK获取，其精度质量能够满足匹配要求。因此，本文假设在道路追踪匹配过程，车辆始终行驶在道路上。

初始化需要有一个前提条件，即组合导航的初始偏差不大 (保持在10 m以内)。当组合导航系统发来一个车辆轨迹点时，算法接收该点信息$\left( l,b,ang,O,t \right)$，其中$l,b$为当前点的经纬度，$ang$为当前点的航向角，$O$为惯导里程计读数，$t$为系统时间 (以100 ms接收一个点为基准)。根据该点位置开始判断轨迹点在道路的初始位置，利用轨迹点到道路的投影方法找到相应道路。确定道路后，根据轨迹点航向角和道路方位角判断出行驶方向，继续跟踪该初始点，如果接下来行驶的20 m或者2 s内，都处于同一条道路，则判定该点是初始化对应的追踪点，开始转入追踪状态，否则重新进行初始化。如图 3所示，$P$点在两条道路上的投影点为${{Q}_{1}},{{Q}_{2}}$，若${{d}_{1}}$和${{d}_{2}}$距离都满足10 m以内，且${{d}_{1}}>{{d}_{2}}$，所以选${{Q}_{2}}$为追踪初始点。此时道路如果是单向通行则可以直接进入追踪状态，反之需要根据轨迹点的航向角情况进行方向判断，图 4的中按照轨迹点的追踪方向应为右侧。

如果没有找到相应的道路或者当前初始化道路为相似的平行道路，初始化状态就会转为“搜索状态”，“搜索状态”即1 km内没有找到合适的道路，即采用搜索状态进行形状特征匹配，以匹配的结果作为起点重新初始化。

当系统进入追踪状态时，需要完成两个工作：一是判断进入其他状态的约束条件；二是发现轨迹转弯或者其他形状特征后，将其与对应路段形状点集合进行匹配，完成误差修正。在1.1节提到“追踪状态”可转换为“路口状态”或“搜索状态”，因此首先需要进行状态转换条件的判断。

1) 进入路口状态。组合导航系统除了提供轨迹坐标外，还能提供较为精确的航向角和里程信息，所以可采用距离的方法对是否到达路口进行判断。根据道路的拓扑关系，可以获得当前追踪点到达下一路口区域边界的道路距离，记为${{d}_{1}}$，此时车辆传感器发来的里程计上有一个里程读数，记为${{O}_{1}}$。随着不断接收系统发来的轨迹点，道路追踪点也沿着道路方向前进，里程计读数不断增加，到达路口区域边界时记为${{O}_{2}}$，那么该段轨迹长度可表示为${{d}_{2}}={{O}_{2}}-{{O}_{1}}$。比较${{d}_{1}},{{d}_{2}}$距离的大小，则有

$ \Delta d=\left( {{d}_{2}}\pm \frac{{{d}_{2}}}{1\ 000} \right)-{{d}_{1}} $

(1)

式中，$\Delta d$是追踪点到路口距离与里程计的差值，实际系统设定的里程计的误差为1/1 000，因此需要加入误差估计。

由于矢量道路数据一般是按照中心线进行测量的，在考虑到驾驶员驾驶习惯、车辆避让等因素，所以里程计记录的轨迹距离通常大于矢量道路的测量距离，故有

$ \left\{ \begin{align} &进入路口状态\ \ \ \Delta d\ge 0 \\ &继续追踪状态\ \ \ \Delta d < 0 \\ \end{align} \right. $

2) 进入搜索状态。若追踪点到达的路口只关联了一条道路，则认为追踪到了一条断头路，将当前状态更改为搜索状态。

3) 发现轨迹形状特征。当轨迹点在追踪状态下，发现一个轨迹特征时，需要寻找与该轨迹特征对应的道路特征进行分析判断。首先，记追踪点为$Q$，其当前所在道路为$L$，那么设追踪点在$L$上的初始化点为$P$，根据系统里程计的读数可获取从$P$点到$Q$点的车辆实际行驶距离${{d}_{3}}$。此时，搜索道路$L$上从$P$点沿道路行进方向至距$P$点${{d}_{3}}$距离范围内的所有道路特征和路口，并将它们提取出来存储在集合$\mathit{\boldsymbol{F}}$中。

集合$\mathit{\boldsymbol{F}}$储存的特征情况各种情况和处理方法如表 6所示，表 6中的匹配方法详见2.5节“匹配修正”。

追踪过程如图 5所示，红色点$Q$表示当前追踪点的位置，继续接收轨迹点发现了一个轨迹弯曲特征${{P}_{i}}\left( i=1,2,3,\cdots ,n \right)$(图 5中的黑色实心点串)，在相应距离内只出现了一个弯曲路段$F$(图 5中的红色弯曲弧段)，因此可以直接将轨迹特征${{P}_{i}}\left( i=1,2,3,\cdots ,n \right)$与道路特征进行匹配。同时计算出$Q$点到下一个路口$N$边界的距离${{d}_{1}}$，并记录下来；随着沿道路的追踪继续进行，当追踪点移动到${{Q}_{0}}$时，查看$Q$点到${{Q}_{0}}$点的里程计之差${{d}_{2}}$，通过比较${{d}_{1}}$和${{d}_{2}}$的限差，判断是否进入路口状态。如果限差满足条件，则认为进入路口区域，反之继续追踪计算。

路口一般关联着至少3条道路，当追踪点进入路口时，系统开始缓存矢量道路点，并且保证只缓存路口区域内100 m范围内的矢量道路点，当接收到车辆轨迹点后，需要对轨迹点的航向角变化做出累积判断，以判断轨迹整体航向趋势和相应的道路方位角的关系，从而确定车辆通过路口到达的下一条道路。

针对航向角的累积判断，本文选取一定距离范围内的角度变化值作为阈值，例如将10 m。

20 m或30 m范围内角度变化进行逐一判断，首先计算当前轨迹位置的航向角相较前一轨迹位置的航向角的变化值$\Delta \phi $，则有

$ \Delta \phi =\left\{ \begin{align} &\psi-{{\psi }_{-1}}\ \ \ \ -\text{ }\!\!\pi\!\!\text{ }\le \psi-{{\psi }_{-1}}\le \text{ }\!\!\pi\!\!\text{ } \\ &\psi -{{\psi }_{-1}}+2\text{ }\!\!\pi\!\!\text{ }\ \ \psi -{{\psi }_{-1}}< -\text{ }\!\!\pi\!\!\text{ } \\ &\psi -{{\psi }_{-1}}-2\text{ }\!\!\pi\!\!\text{ }\ \ \psi -{{\psi }_{-1}}>-\text{ }\!\!\pi\!\!\text{ } \\ \end{align} \right. $

(2)

式中，$\psi $表示前一轨迹点航向角，${{\psi }_{-1}}$表示当前轨迹点的航向角。

对航向角的变化值进行累加，则

$ \phi =\phi +\Delta \phi $

(3)

式中，$\phi $为航向角变化总和，表示该段范围内航向角的变化程度。

最后再与该路口各个道路的方位角${{\theta }_{i}}\left( i=1,2,3,\cdots ,n \right)$ 作比较，其中$n$的值是该路口包含的道路数量，则

$ \Delta =\left\{ \begin{align} &0\ \ \left| \phi-{{\theta }_{i}} \right|>\delta \\ &1\ \ \left| \phi-{{\theta }_{i}} \right| < \delta \\ \end{align} \right. $

(4)

式中，$\Delta $表示轨迹点是否追踪到道路$i$上，1表示满足限差进入道路$i$，反之不进入。$\delta $表示该范围内轨迹累积航向角与道路方位角的限差。当时$\Delta =1$，轨迹点追踪进入该道路，并以道路交叉节点作为新的追踪点，返回追踪状态；若限差未满足要求，增大距离范围继续判断，直到满足限差要求后，重新转换为“追踪状态”。如出现没有满足限差的出口道路，或者不止一条出口道路，则认为无法确定出口道路，进入“搜索状态”。

如图 6所示，黑色点串${{P}_{i}}\left( i=1,2,3,\cdots ,n \right)$表示当前车辆的轨迹点，红色点$Q$表示轨迹当前在道路上的追踪点，此时已经进入到路口区域，需要进行路口分析判断。$N$表示路口结点，$dis$表示距离阈值，表示路口匹配判断的范围大小，${{L}_{1}}$，${{L}_{2}}$表示路口分支的两条道路，${{\theta }_{1}},{{\theta }_{2}}$表示两条路的道路方位角，计算在${{L}_{\text{i}}}$上$dis$范围内轨迹点航向角的累计变化量$\phi $，并与道路方位角${{\theta }_{i}}$进行比较。图 6中${{L}_{1}}$道路的角度满足限差要求，因此认为车辆通过路口进入了道路${{L}_{1}}$。

判断完成交叉口的道路出口后，将路口中对应道路点提取出来，需要对轨迹点和交叉口相应道路进行匹配修正，完成匹配修正后，以$N$点 (交叉口节点) 作为新的起点继续进行追踪状态。

当算法在运行过程中无法继续追踪时，进入到搜索状态，通过发现一段车辆轨迹点集合的转弯曲线特征，按照弯曲形状相似性进行匹配。

首先，根据车辆轨迹弯曲特征，可以确定轨迹特征所在区域的最小外接矩形，由于惯导误差的存在，需要将轨迹弯曲特征适当外扩，按照误差椭圆计算公式^[26]，获得的搜索待匹配道路区域如图 7所示，图中虚线框为搜索区域，${{x}_{m}}$和${{y}_{m}}$表示经纬方向的外扩范围大小。

其次，搜索该区域内所有具有弯曲形状点特征的路段，将每个搜索到的弯曲路段的开口方向 (open-direction) 进行一次粗匹配，剔除明显不能匹配的路段。如图 8所示，${{\alpha }_{P}}$、${{\alpha }_{Q}}$分别表示车辆轨迹弯曲和相应弯曲路段的开口方向，$\Delta \alpha =\left| {{\alpha }_{P}}-{{\alpha }_{Q}} \right|$∣表示开口的限差，不满足限差要求的弯曲路段将被剔除。

最后，按照文献[14]中基于ICCP迭代匹配的方法进行匹配，详见2.5节，直到方差满足匹配需求或者迭代次数达到上限时，将最后平移结果作为匹配结果。若出现多个特征可以匹配时，取方差最小值 (即匹配度最好的道路作为匹配结果)，当匹配道路有且只有一条道路时，完成道路搜索，然后返回初始化状态进行重新追踪。

当算法进入追踪状态追踪到弯曲路段时、在路口状态判断道路出口时以及完成搜索状态粗匹配后，需要将车辆惯导的轨迹点和相应储存的矢量路段点进行匹配，以修正轨迹点存在的误差，从而提高导航精度。不同于GPS轨迹，惯导轨迹数据具有非突变性的特点，不会出现跳变、丢失等情况，且与预存的道路路段整体形状呈现出更好的相似性。但由于惯导随时间产生累积误差，使其与预存矢量路段的偏离越来越大。因此不能通过一般的直接投影算法进行匹配修正。本文借鉴笔者在文献[14]中基于ICCP的迭代匹配方法，由于匹配是在具有转弯特征的路段进行的，所以惯导的旋转误差相对较小，只考虑平移误差，其关键步骤如下：

1) 针对当前发现的车辆轨迹弯曲轨迹点集合找到其对应的弯曲路段点集合，根据当前车辆轨迹点航向角法线方向来搜索最近点，确定对应弯曲道路上的最近点，如图 9所示。

2) 计算每个轨迹点到其对应弯曲路段最近点的经纬度方向平移量，并计算该段轨迹的整体平均平移量

$ \left\{ \begin{align} &\overline{dx}=\sum\limits_{i=1}^{n}{\frac{d{{x}_{i}}}{n}} \\ &\overline{dy}=\sum\limits_{i=1}^{n}{\frac{d{{y}_{i}}}{n}} \\ \end{align} \right. $

(5)

式中，$\overline{dx}$和$\overline{dy}$分别表示经度和纬度方向的平均平移量，$d{{x}_{i}}$和$d{{y}_{i}}$分别表示每个轨迹点到其对应弯曲路段最近点的经纬度方向平移量，$i=1,2,3,\cdots ,n$。

3) 按照平均平移量对轨迹点进行整体平移，重复步骤1) 2)，直到轨迹误差满足导航要求的基本误差或者迭代次数超过阈值时，匹配结束。

考虑到计算机的内存要求，本文取迭代平移30次为迭代上限，超过30次，则匹配修正结束；此外，当轨迹点的逼近程度基本与道路点吻合时，即经度或者纬度偏差小于导航允许的基本误差范围内，也认为匹配成功，此时有

$ \sqrt{\overline{d{{x}^{2}}}+\overline{d{{y}^{2}}}} < {{d}_{\max }} $

(6)

式中，${{d}_{\max }}$为组合导航的要求的最大误差，参照一般导航的误差要求设置为9.09×10^-5度 (大约10 m)。

如图 10所示，经过满足迭代条件的平移匹配修正，轨迹点与道路点基本吻合，达到了导航匹配的要求。

根据上述算法基本原理的说明，本文算法的整体流程如图 11所示，具体为：

1) 根据导航轨迹初始点开始进入初始化状态，如果找到跟踪点和相应确定道路，进入追踪状态，转入步骤3)；否则进入搜索状态，转入步骤2)；

2) 按照形状相似性进行匹配，找到当前轨迹的对应道路，重新进行初始化，转入步骤1)；

3) 计算初始追踪点到下一路口的道路距离，根据轨迹点里程信息计算相对里程，当里程满足$dis\pm dis/1\ 000$，进入路口状态，转入步骤5)；反之，转入步骤4)；

4) 继续接收导航轨迹点，并进行特征判断，若发现特征则搜索相应道路特征，当有且只有一条对应特征，则进行匹配；如没有发现特征，则继续追踪，并转入步骤3)；

5) 在路口中，根据拓扑关系找到关联道路，计算轨迹点航向角的总变化量，当航向角总变化量与后续行驶道路方位角满足阈值时，则找到出口道路，追踪道路发生改变，并进行路口匹配，转入步骤6)，如无法判断出口，则转入步骤2)；

6) 以路口交叉节点作为新的追踪点，沿出口道路继续追踪，判断是否遍历所有轨迹点，如果没有，转入步骤3)；否则，结束跳出本算法流程。

为了验证追踪算法划分轨迹特征的准确性和实际效果，本文对北京西五环阜石路地区以及密云地区约130 km的路段进行了实地跑车并根据跑车轨迹进行匹配仿真实验。实验采集的数据为惯性导航实测轨迹数据，其中平均车速为60 km/h，惯性导航中陀螺常值漂移量为0.05°/h，随机漂移量为0.01°/h，加速度计的初始零偏均值取为10^-4 g，随机零偏值为0.5×10^-4 g，东向、北向和方位失准角的初始值取为1°。算法每100 ms接收一次导航轨迹数据，进行道路追踪匹配。

仿真所采用的矢量道路数据是通过GPS-RTK差分获取的txt格式矢量道路点集合，包括2014年测量的北京市“密云地区”路段 (图 12(a)) 和“西五环阜石路”路段 (图 12(b))。

在应用追踪算法进行匹配修正后的效果如图 13所示。图 13中蓝色弧段表示的是组合导航系统发回的车辆轨迹数据，黑色弧段表示的是矢量道路数据，深蓝色弯曲弧段表示具有弯曲特征的路段，粉色是采用文献[18]算法提取到轨迹弯曲特征，红色弧段为经过本文算法匹配修正过后的车辆修正轨迹。算法分别针对弯曲路段、丁字路口、以及复杂路口等具体路段进行了轨迹误差匹配修正。实验结果表明，本文算法能实时快速地完成导航轨迹数据点的道路追踪和匹配修正，车辆通过相应路段后，追踪点能直接对应到相应道路上，在路口和弯曲路段处都完成了匹配修正，保证了导航的准确性。

路口状态是追踪算法的一个关键步骤，它决定着追踪状态的转换成功与否。因此在进行路口状态出口判断时，路口的范围大小直接影响轨迹点的匹配结果，所以距离阈值$dis$的大小设定是匹配结果的重要因素。考虑到道路规范中车道宽度基本一致，距离阈值过小则无法获得到完整的路口有效道路点，达不到轨迹匹配的效果；而距离阈值过大会包含一些路口以外的道路点，容易出现误匹配，所以本文根据设定不同的距离阈值来分析确定理想的距离阈值。

对匹配结果进行定量评价，评论指标为：

准确率＝正确匹配的特征点数量/导航轨迹的总特征点数量；

漏匹配率＝遗漏匹配的特征点数量/导航轨迹的总特征点数量；

误匹配率＝错误匹配的特征点数量/导航轨迹的总特征点数量。

根据上述评价指标，本文利用北京五环晋元桥地区的道路数据进行了仿真实验，针对不同距离阈值对于组合导航匹配结果的影响进行了控制变量分析，其匹配结果如表 7所示。结果表明，当$dis$=10时，路口范围过小，路口道路点不够完整，虽然匹配较为准确，但容易出现漏匹配；当$dis$=30时，路口范围过大，一些路口外道路也参与了匹配，所以误匹配概率升高；当$dis$=20时，漏匹配率与误匹配率都能降低到一个合理的值，轨迹匹配准确率达到最高。

最后，本文对实验区域内选取的5条道路分别利用文献[14]算法和本文算法进行了轨迹点匹配实验，并对得到的车辆轨迹匹配结果进行了统计，如表 8所示。实验统计结果表明，本文算法能较好地完成特征匹配，其准确率达到了90.2 %，相比较于文献[14]和基于形状特征的匹配算法^[23]的匹配准确率，有了较大的提高，能满足组合导航轨迹匹配的基本要求。

与文献[14]算法和形状匹配算法^[23]相比，本文通过采用矢量道路的拓扑结构在各种状态进行轨迹匹配修正，具有较好的实时性，实时性检测根据3种算法的匹配速度的实时性比较。

任意选取一条包含有至少一个路口和一个弯曲的矢量路段作为测试路段，表 9为不同路段算法匹配时间的测试结果，其中本文算法在弯曲道路和路口情况下在实时性方面具有明显的优势，但由于转弯处车辆轨迹特征的发现提取会造成一定程度延迟，从而引起部分匹配点的丢失。表 9数据显示，与形状算法相比，本文所提出算法匹配速度提高了4~8倍，仅仅少匹配了20 %左右的轨迹点，由于算法每100 ms接收一次导航轨迹数据，因此实时性基本满足导航误差修正的要求。

本文根据矢量道路的拓扑结构特点，提出了一种利用拓扑追踪的矢量道路匹配算法，有效地解决了道路交叉口等路段的误匹配问题，用于组合导航的轨迹匹配修正定位，得到以下结论：

1) 算法采用道路数据的拓扑信息，对矢量道路数据和车辆轨迹进行追踪匹配，适用于卫星信号盲区等无信号条件下的组合系统辅助导航，弥补传统基于卫星的组合导航在自主性、抗干扰性的不足；

2) 算法通过4种状态的处理，实现对车辆行驶轨迹的匹配和误差修正，从而提高组合导航的定位精度；

3) 采用拓扑追踪的匹配算法，可以大大提高组合导航误差修正的实时性，消除因复杂路口 (超过3条道路的交叉口) 等路段情况造成的误匹配，同时搜索状态的设定可以迅速从失去定位的状态下重新定位；

4) 路口距离阈值$dis$的设定，决定了路口状态匹配的准确率，设定时可根据实地情况选取相应距离值，实验表明，当$dis$为20 m时，路口匹配准确率最佳；

5) 利用真实道路验证了所提出算法的性能。实验结果表明，本文所提出的道路追踪算法在交叉口路段上的匹配准确度优于文献[14]和传统基于形状的算法，匹配速度有了较大的提高，匹配结果较为理想，能够满足组合导航匹配工作的要求。