§4.1 车辆轨迹实时校正改进算法简介
§4.1.1 车辆轨迹实时校正改进算法产生思路
在第三章中我们介绍了几种现有的基于矢量地图的车辆轨迹实时校正算法，这几种校正算法根据采用的矢量地图的精度和存储的数据量，分别采用了不同的校正算法。这些校正算法给了我们很多有益的提示：如把车辆轨迹的实时校正归结为车辆行驶道路的选择，可以采用多信息融合的方法判断车辆行驶的道路，利用道路网络的拓扑结构选择车辆行驶的道路等等。
从我国现有的矢量地图的精度考虑，本章研究的车辆轨迹实时校正算法在矢量地图的精度稍低，存储的节点、弧和路的信息较少的情况下进行的。在充分的分析了文献[11]提出的车辆轨迹实时校正算法后发现，该算法简单易行，道路选择准确性较高。只是存在两点问题：
1> 在矢量地图精度稍低时，通过把车辆轨迹校正到最近弧的最近点上，只能校正车辆轨迹的垂直于道路方向的误差，不能校正车辆轨迹沿道路方向的误差。
2> 在车辆转弯后寻找车辆当前行驶的道路，没有利用道路的方向信息辅助判断。
在此基础上本章提出了一种改进的车辆轨迹校正算法，来提高车辆轨迹实时校正精度。该算法在车辆初始化道路选择，车辆转弯和实时消除车辆轨迹误差方面作了有效的改进。
下面以图4.1 为例，对车辆轨迹实时校正算法的内容和需要解决的问题作一个简要的分析。
图4.1 利用地图拓扑结构选择车辆行驶的道路
通常情况下车辆定位系统每秒钟更新一次车辆位置。现在假设我们已经知道车辆在开始时刻在弧 lc0c4 上行驶，方向从c 0 到c 4 ，那么车辆位置 g0 应该校正到弧lc0c4 上。
在第一秒和第二秒时的GPS 接收机接收到的车辆位置 g1 和g 2 也只可能校正到弧 l c0c4上。
在k(k >> 2) 时刻，车辆位置在大节点c4 附近，这时仅仅根据车辆的位置难以判断车辆是已经转弯到弧 l c4c5上还是继续沿着弧lc4c6 行驶；但是我们至少可以确定车辆在k 时刻不会突然跳到弧l c0c1 或l c0c3 上行驶。
通过上面的分析，我们可以清楚的指出一个实用、有效的车辆轨迹实时校正算法需要解决以下几个问题：
1> 初始时刻的车辆行驶道路判断。在上面的分析中我们假定已经知道车辆开始在弧lc0c4 上，但是在实际车辆轨迹实时校正中是不能这样作假设的。必须对初始时刻的车辆行驶道路加以判断。
2> 车辆轨迹的实时校正。车辆当前行驶的道路确定之后，就需要在导航屏幕上实时的显示车辆位置了。由于定位误差、地图误差等误差因素的存在，在大多数情况下，接收到的车辆位置G = {g1 g2, ,gk , , } 都不会显示在车辆行驶的道路上。为了在导航屏幕上准确显示出车辆当前的位置，必须利用车辆的位置G = {g1 g2, ,gk , , } 对P 进行实时校正。
3> 车辆在路口处的转弯判断。城市中大量的道路交叉口是对车辆导航系统导航效果的考验：车辆究竟在道路交叉口处左转弯行驶、右转弯行驶还是径直前进；如何及时的判断出车辆转向的道路，这都是车辆轨
迹实时校正算法中着重要解决的问题。
§4.1.2 车辆轨迹实时校正改进算法简介
在4.1.1 小节中，我们详细分析了车辆轨迹实时校正面临的问题，针对这几个问题我们设计了一种实用的车辆轨迹实时校正算法。现在将该算法中几个重点问题的解决思路描述如下：
1> 初始时刻的车辆行驶道路判断。利用道路拓扑结构信息对车辆轨迹进行校正，对初始轨迹点的校正要求比较高。当初始轨迹点校正到错误的弧上时，一系列的轨迹点都会校正到错误的弧上。为了提高车辆轨迹实时校正的准确性，我们在观测了车辆定位系统接收到的车辆位置序列G 若干个周期以后，选择离最近的弧作为当前弧。
2> 轨迹校正。在确定了当前弧以后，采用了带道路约束条件的卡尔曼滤波算法，对车辆轨迹进行实时校正，并实时地输出车辆校正后的轨迹P .
3> 车辆转弯判断：如果车辆在道路大节点c j 转弯，通过车辆位置序列G和车辆速度方向信息判断车辆转弯时刻t k ，把k t 时刻车辆轨迹点校正到j c ，从而消除车辆轨迹的实时误差，提高车辆轨迹校正的精度。在本章后文中，将会对车辆轨迹实时校正的各个环节详加分析，并且给出车辆轨迹实时校正算法步骤。
§4.2 初始时刻的车辆行驶道路判断
初始时刻的车辆行驶道路判断方法有很多种。我们可以利用第三章提到的最近弧匹配算法，把车辆轨迹点 p1 校正到距离g1 最近的弧上，并以此最近弧作为车辆初始时行驶的弧l cicj 。如图4.3 所示，如果根据接收到的车辆位置 g1 距离弧 lc0c1 和弧 l c0c2的距离来进行判断，从而把车辆轨迹P 校正到距离最近的弧上，比较容易发生误判。
因此我们考虑把弧匹配初始化的时间“拉”长一点，通过对若干采样时间的接收到的车辆的位置G 的观察判断出车辆初始行驶的弧。为此我们对点到弧的距离和线到弧的距离给出定义，并以图4.2 为例进行说明。
定义1：点到弧的距离
当点到弧的投影在弧以外时，接收到的车辆位置g k 到弧lcicj 的距离定义为点到最近节点的距离Dk （图4.2 中上图）。当点到弧的投影在弧上时，点g k 到弧l cicj 的距离定义为投影距离D k （图4.2中下图）。
定义2：线到弧的距离
车辆位置序列{g1 g2, ,gn } 1 2 n g g L g 组成的线到弧cicj l 的距离S ，为车辆位置点g1 g2, ,gn 到弧的距离之和，即
图4.2 点到弧的距离定义示意图
图4.3 弧匹配初始化算法示意图
在如图4.3 所示的道路网络中，在初始化车辆位置{{g1 g2, ,gn , , , } 的匹配弧时，搜索矢量地图中的所有距离{ {g1 g2, , , ,gn , , } 在一定范围内的弧，作为被选弧，在图4.3 中{g1 g2, gn} 备选道路弧lc0c1 、l c0c2 、 lc0c3 、lc0c4 。
计算n 个采样时刻的接收到的车辆位置{g1 g2, ,gn } 到弧l c0c1 、l c0c2 、lc0c3 、l c0c4 的距离Si ,i = 1,2,3,4 ，选择Si 最小的弧l c 0ci 为车辆行驶的当前弧。采样时间n 的大小是根据矢量地图的精度来选择的。如果矢量地图的精度比较高，我们可以把初始化的时间选的短一些。
§4.3 弧上的车辆轨迹校正
在2.3.4 小节中定义了弧的ID 作为弧的唯一标识，通过弧的ID 可以在矢量地图中访问该弧的节点。在本章中我们沿用了弧的ID 这一概念，并以此来标识唯一的弧。
在确定了车辆当前行驶的弧 l cicj以后，根据该弧的ID 我们可以从矢量地图中得到弧lcicj 的节点ci ( xci , yci) 和c j (xcj ,ycj ) 。利用弧的节点的信息，把接收到的车辆位置G 校正到P ，把校正后的车辆轨迹P 输出到屏幕上显示给用户。
§4.3.1 卡尔曼滤波器建模
卡尔曼滤波器可以从被噪声干扰污染的数据中得到真实状态得最优估计值，它通过测量结果对估计结果进行不断的修正来得到最优估计。文献[33][34][35]等指出可以利用卡尔曼滤波器对车辆轨迹动态滤波。
本节在建模时采用在3.1 小节定义的坐标系，以东方为x 轴正方向，北方为y 轴正方向。
建立时域范围内的车辆运动状态变量
X (t) = ( px , py ,v x ,v y,a x , a y ,e x ,ey ) T ，px ,,v x ,,a x , ,e x , 和 py v y a y ey 分别是车辆在两个坐标轴上的位置、速度、加速度及轨迹误差。
轨迹误差是在3.1 小节中定义的各种误差的综合误差e ， e = egps + emap+ er其中egps 可视为一阶马尔柯夫过程[35]，在同一条弧上认为 emap 、 e r为常量，因此综合误差e 可视为一阶马尔柯夫过程，则有：
.
.
§4.3.2 带道路约束条件的卡尔曼滤波
在文献[36]中论述了一种利用带道路约束条件的卡尔曼滤波算法来提高车辆定位的精度。根据当前弧l cicj 的ID 可以从矢量地图中读取节点ci ( xci , yci) 和c j (xcj ,ycj ) 的位置，计算弧的直线方程，从而利用该直线方程作为约束条件，来校正车辆轨迹。
因此本节在卡尔曼滤波算法中引入了道路约束条件，并且给出带道路约束条件的卡尔曼滤波算法递推公式。
车辆行驶的弧l cicj 的直线方程描述为：
其中d i 为弧lcicj 直线方程的斜率， bi 截距。
显然车辆在这个弧上行驶时，车辆校正后的轨迹(p k , pkx ,pky ) 应满足方程(4.4)，即弧对车辆的行驶起到了一个约束的作用，成为系统模型的约束条件。
文献[36]中给出了带道路约束条件的卡尔曼滤波算法的推导公式，在此直接给出递推方程如下：
§4.4 节点处车辆行驶弧的判断
在4.3 小节中，我们利用以车辆当前行驶的弧的直线方程作为约束条件的卡尔曼滤波器对车辆轨迹进行实时校正。因此在车辆行驶到不同的弧时都需要转换弧约束条件。
本节把弧约束条件的转换分为两种情况考虑：小节点处约束条件的转换大节点处约束条件的转换。
§4.4.1 小节点处弧约束条件转换
矢量地图的道路存储结构中道路通常是由多个弧连接而成，这些弧顺序相连，除端点外的弧交点都是该道路内部的小节点。不同的弧具有不同的直线方程，也就形成了不同的约束条件。
小节点处的道路约束条件转换比较简单，因为小节点仅仅连接了两条弧，车辆只能从一条弧行驶到另外一条弧，没有弧的选择问题。我们只要判断车辆位置已经在下一条弧上，改变卡尔曼滤波的弧约束条件即可。
图4.4 小节点约束条件转换示意图
如果pk 在弧l c2c3 以外，认为车辆轨迹从k +1时刻在弧l c3c4 行驶。这时的道路约束条件为弧l c3c4 的直线方程。
§4.4.2 大节点约束条件转换
大节点连接多条弧，因此大节点处的弧约束条件的转换就比较复杂。
在大节点处弧约束条件的转换，实际上是对车辆转弯的判断。利用GPS 接收机提供的位置信息和车辆行驶方向的信息，可以对车辆的转弯加以判断。
在文献[32]中提出了一种利用车辆的位置信息的车辆转弯算法，定义阈值MaxR 。对图4.5 所示的车辆位置序列{g0 , g 1 , g2} 作转弯判断：
图4.5 位置法判断车辆转弯
以g0 点为圆心MaxR 为半径的圆只与弧lc0c1 相交，则 p 0校正到弧 l c0c1上； 与以轨迹点 g1 , g 2 为圆心的圆相交的道路多于一个，则对车辆位置 g1 , g2 不做处理，在节点 c0 处等待；与以车辆位置 g 3为圆心的圆相交的弧只有lc0c2 ，判断g3 车辆已经转弯应该校正到l c0c2 上。可以看出，这是一种滞后的车辆转弯判断算法，会造成车辆轨迹在每一个大节点处等待。
由于矢量地图的误差和GPS 接收机定位误差的影响，仅仅利用车辆位置信息判断车辆是否转弯，误差会比较大。在实际应用中的表现就是车辆在交叉路口等待或“跳跃”。
高质量的GPS 接收机在良好的环境中使用只有千分之几至百分之几的距离变化率误差，所以根据车辆的x 方向速度和y 方向速度计算出来的车辆速度方向比较准确。通过对大量实验数据的分析，在两条弧的方向差值超过40.o 时，依据车辆速度方向就能十分准确判断出车辆行驶的弧。在城市中，大部分的道路为十字路口，很少有五条以上的交叉路口，因此两条道路的方向差比较大。在这种情况下，以车辆的速度方向为依据的车辆转弯判断方法比以车辆位置为仅仅依据的车辆转弯判断方法要准确的多。
下面以图4.6 为例说明利用车辆速度方向对车辆转弯进行判断。
图4.6 简单的矢量地图的道路网络示意图
在大节点处判断了车辆行驶的当前弧的ID 以后，在对车辆轨迹校正时的道路约束条件为当前弧的直线方程。
§4.4.3 弧约束条件转换后的卡尔曼滤波器的参数
卡尔曼滤波器的参数纪录了车辆历史轨迹的信息，在弧约束条件不改变的情况下能充分利用车辆轨迹的历史信息提高滤波精度。但是转换约束条件后，参数中保留的车辆历史轨迹的信息使卡尔曼滤波器不能快速的跟踪转换约束条件的轨迹，造成校正后的轨迹误差较大。因此，我们在转换弧约束条件的同时对滤波器的参数重置，使滤波器能够快速跟踪新的车辆轨迹。
§4.5 车辆转弯时轨迹误差实时校正
文献[11]中的车辆轨迹校正始终把车辆轨迹校正到最近弧的最近点上，只能校正车辆垂直于道路方向的误差，对车辆沿道路方向的误差不能校正。在国外一些成功的车辆导航系统中，路标传感器技术在消除车辆轨迹误差方面得到了很好的应用。所谓路标传感器是指在路边树立的无线电信标，它不断发射一种高频、低功率信号，当车载系统在有效范围内接收到该信号后就在其矢量地图数据库中寻找该信标的位置，然后把车辆的位置校正到该信标所在的位置，从而达到了校正车辆轨迹的误差的目的。
目前我国的城市中还没有安装类似的装置。但是我们考虑，在车辆转弯时刻k 可以得到车辆位置点 gk 和大节点 ci 之间的位置差，通过把pk 校正到大节点ci ，也能达到消除车辆轨迹的沿道路误差的作用。
在此必须指出的是，以车辆位置为依据的的转弯判断方法计算出来的转弯时刻事实上是大大滞后于车辆转弯的实际时间的。如果把该时刻的车辆位置校正到大节点处，会带来较大的误差。如果车辆行驶的速度为10m/s 左右，那么几秒钟的滞后就会造成几十米以上的误差，这样就达不到校正沿道路方向误差的目的。而本文上一节提出的以车辆速度方向为依据的转弯判断方法得出的车辆转弯时刻k 的准确度大大提高，这时把pk 校正到大节点ci ，才真正能够起到消除车辆轨迹的沿道路误差的作用。
§4.6 车辆轨迹实时校正改进算法步骤
§4.7 车辆轨迹实时校正改进算法实验
在本节中的所有的实验均采用X 坐标为经度和Y 坐标为纬度的坐标系，GPS接收机的采样时间为1s。
实验中，利用中国科学技术大学GPS 实验室开发的矢量地图编辑工具MapAdjustHF，从本实验室开发的矢量地图上直接读取节点和弧的数据。
§4.7.1 弧匹配初始化实验
该实验中车辆的行驶轨迹实验数据取自2004 年6 月23 号采集到的车辆在合肥市部分道路行驶记录的轨迹一部分。实验中所采用的GPS 接收机为美国GARMIN 公司生产的eTrex-Legend GPS 导航仪。
我们按照矢量地图中道路网络的组织形式利用Matlab 软件绘出道路网络，如图4.8 所示。图中节点的数据是从图4.7 中实际道路网络中直接读取的。图4.7 以及本论文所有的实验图中的X 坐标是车辆轨迹经度坐标，Y 坐标是车辆轨迹纬度坐标，单位为‘分’。
设定弧匹配初始化的时间n 为10 个采样周期。为了方便后文的叙述，我们在图4.8 中用A、B、C、D 分别代表图4.7 中的弧，接收到的车辆位置G 是曲线2，从图中可以很直观的看到车辆位置G 应该匹配到弧A 上。但是在车辆刚刚开始行驶时，我们却很难判断出车辆真实行驶的弧，因此利用本文的弧匹配初始化算法选择车辆初始时刻应该匹配的弧。在图4.9、图4.10、图4.11 和图4.12 中，校正后的轨迹3 是车辆位置序列2 分别在弧A、B、C、D 上的投影点。下面对这些投影点分别分析：车辆位置序列2 到弧A、B 的投影在弧上。按照4.2.1 小节中定义的线到弧的距离，车辆位置2 到弧A、B 的距离是车辆位置序列2 的最初10 个采样时刻的车辆位置点到校正后的轨迹3 的点距离之和。
车辆位置序列2 在弧C、D 的投影点在弧C、D 以外。所以，车辆位置序列2 到弧C、D 的距离是车辆位置序列2 最初10 个采样时刻的车辆位置点分别到弧C、D 的最近的节点的距离之和。
经计算得到前10 个采样时间的车辆轨迹到弧A、B、C、D 的距离依次为0.1724 、1.8849 、2.4530 、2.4530。显然车辆轨迹车辆位置序列2 到弧A 的距离最近，因此可以判断车辆行驶在弧A 上。
图4.7 弧匹配初始化算法实验地图
图4.8 弧匹配初始化算法实验轨迹图
图4.9 弧匹配初始化算法局部放大示意图1
图4.10 弧匹配初始化算法局部放大示意图2
图4.11 弧匹配初始化算法局部放大示意图3
图4.12 弧匹配初始化算法局部放大示意图4
从该实验的结果中我们可以看到弧匹配初始化算法可以迅速的在车辆轨迹校正开始时准确的寻找到车辆当前行驶的弧。特别是弧A 和弧C 的直线方程非常相近的情况下也能判断出车辆行驶的弧，为了做出比较，实验中特地对求出车辆位置序列2 到弧A、B、C、D 的投影点和初始10 个采样时刻的车辆位置点的距离之和，分别为0.1724、1.8849、0.8406、1.8265。可以看到车辆位置序列2 到弧A、B 的距离是相同的；但是车辆位置序列2 到弧C、D 由2.4530、2.4530 的距离分别减少到0.8406、 1.8265。可见4.2 小节中定义2 的线到弧的距离对车辆行驶方向比较敏感，能较好的利用车辆当前的行驶方向区分弧。
§4.7.2 弧约束条件转换实验
该实验中坐标和数据来源和4.7.1 小节相同。
我们按照矢量地图中道路数据组织的形式在matlab 程序中绘出道路网络，如图4.14 中的弧A、B、C、D 所示。图中节点的数据是利用GPS 实验室开发的地图编辑工具从图4.13 中的实际矢量地图中直接读取的。
按照上述参数设置，对这段轨迹用带道路约束条件的卡尔曼滤波对车辆轨迹进行校正。校正结果如图4.15 所示。可以看到弧A、B、C、D 明显变宽，这是由于校正算法处理后的车辆轨迹几乎和道路重合引起的。图4.16 是对图4.15的局部放大。从图4.16 中可以看到与弧B、C、D 几乎重合的校正后的轨迹3。图4.16 中黑色圆圈出来的点，是转换道路约束条件时刻的车辆位置 gk 及其对应的车辆轨迹p k 。在该时刻，也对卡尔曼滤波器的参数重置，可以看到经过道路约束条件的转换，卡尔曼滤波器仍能很快的跟踪车辆轨迹。
图4.13 节点处弧约束条件转换实验道路地图
图4.14 节点处弧约束条件转换滤波轨迹图
图4.15 节点处弧约束条件转换滤波结果图
图4.16 节点处弧约束条件转换滤波结果局部放大图
§4.7.3 车辆轨迹误差实时校正实验
在弧约束条件转换的实验中，我们按照矢量地图中道路数据组织的形式在matlab 程序中绘出道路网络，如图4.18 中的弧A、B、C、D、E 所示。图中节点的数据是利用GPS 实验室开发的地图编辑工具从图4.17 中的实际矢量地图中直接读取的。
图4.17 车辆轨迹实时误差校正实验道路地图
在图4.18 中的点序列2 是接收到的车辆位置，用车辆轨迹校正算法对车辆轨迹处理，车辆从行驶的方向为ABCDE。
图4.19 所示为采用该算法处理后的车辆的轨迹效果图，可以看到图4.19 中的道路明显变宽，这是由于校正算法处理后的车辆轨迹3 几乎和弧A、B、C、D、E 重合。
图4.18 车辆轨迹实时误差校正实验轨迹
图4.19 车辆轨迹实时误差校正实验校正结构
图4.20 路口节点CR1 获得实时误差
图4.21 误差实时校正后路口CR2 处的轨迹
图4.22 误差实时校正后路口CR3 和CR4 处的轨迹
图4.20 中，点划线2 是接收到的车辆位置，点序列3 是经过校正算法处理过的车辆轨迹，A、B 是弧，图中黑色圆圈出的点是判断出的车辆已经转弯的时刻。可以看出在大节点CR1 处车辆轨迹的误差较大，以至于车辆在CR1 处超过弧A 后，又跳到弧B 上，这是由于矢量地图的非线性误差过大。把转弯时刻的车辆位置校正到大节点CR1 处消除车辆轨迹的实时误差。
图4.21 和图4.22 中是在节点CR1 处消除误差后的校正轨迹。图4.21 和图4.22 中，点划线2 是接收到的车辆位置，点序列3 是经过校正算法处理过的车辆轨迹，图中黑色圆圈出的点也是利用转弯判断算法判断出的车辆已经转弯的时刻。
在图4.21 和图4.22 中可以看到在路口节点CR2、CR3 和CR4 处车辆轨迹的精度有了很大的提高，带道路约束条件的卡尔曼滤波不但很好的消除了车辆轨迹垂直道路方向的综合误差，并且在车辆转弯时消除路口处的实时综合误差（包括垂直道路方向和沿道路方向的误差），避免了车辆轨迹误差的累积，提高了车辆轨迹校正的精度。
