车载导航系统的高精度定位算法

1、车载导航系统的高精度定位算法马海波1,黄跃峰1,滕寿威1摘要：本文提出了一种基于联合卡尔曼滤波理论和地图匹配技术的高精度车载导航 系统定位方法。该方法一方面对联合卡尔曼滤波器的结构进行简化，简化后的联合滤波 器能够较好的将全球定位系统（GPS）与航位推算系统（DR）获取的空间信息进行融 合，不仅较大程度减小滤波计算量，而且避免了子滤波器间误差的互相干扰，提高了空 间定位精度。另一方面提出了根据行车方向与位置匹配行车道路的技术，该技术不仅具 有较好的行车道路匹配效果，而且能够对各种行车异常情况进行处理。实验证明，本文 提出的方法能够较好的满足车载导航系统对空间定位方法实时性及高精度的要求。关键词

2、：车载导航系统，联合卡尔曼滤波，信息融合，地图匹配，GPS/DR（1北京超图软件股份有限公司导航事业部，北京市海淀区西三旗建材城西路太伟科研楼B座3层 邮编；100096）1.概述近几年，国内车载导航系统的发展如火如荼，基于全球定位系统（Global Position System, GPS）定 位的导航系统几乎成了豪华轿车的标准配置。由于巨大的市场潜力和不可估量的发展前景，各汽车 厂商和GIS企业纷纷投入大量人力进行车载导航系统的软硬件开发。作为车载导航系统的基础和核 心，车辆定位获得了更多学者的关注和热情。本文首先结合车辆定位方式，阐述了全球定位系统和航位推算系统（Dead Reckoni

3、ng, DR）相 结合的组合定位方式的必要性和优越性，尤其从价格和精度方面考虑，GPS/DR将是未来车辆定位 的最理想方式。无论是独立GPS定位，还是组合GPS/DR方式定位，得到可靠的、精确的实时车辆位置是车辆 定位的根本目的。结合实际应用，城市车辆的高精度定位算法包括两个部分：1）针对定位传感器的 滤波与信息融合，2）结合电子地图的道路匹配算法。提高传感器定位精度的方法是降低甚至去除系统定位的随机误差。本文提出了一种简化结构的 联合卡尔曼滤波器。采取独立定位方式时，该滤波器的作用相当于卡尔曼滤波，能去除定位系统的 噪声干扰，获取更加有效的位置信息；对于GPS/DR组合定位方式，该滤波器有效

4、地实现了两者的 信息融合，并产生最优的位置信息，更进一步，结合-GPS精度因子，自适应地调整信息融合时各子 系统的权重，从而提高组合定位系统的抗干扰能力，保证定位的稳定性。通过实验也进一步验证了 该滤波算法的优越性。本文介绍了一种高精度的综合匹配算法，以实现车辆位置和GIS电子地图道路的完全匹配。该 算法不仅确保了路口以及平行路况下的匹配准确性，而且具有很好的实时性。大量的真实路测实验 也充分验证了这种匹配算法在效率、精度和稳定性上的优越特性和实用性。2.车辆定位技术相关研究2.1车辆定位方式概述车辆定位技术可以大致划分为卫星定位、独立定位、地面无线电定位等三类。卫星定位包括全球定位系统(Gl

5、obal Position System, GPS) 1全球卫星定位系统(GLONASS) ，两者功能一致，只是信号分割体制不同，GLONASS接受技术比较复杂，因而民用比较少见。独立定位技术包括惯性导航(INS)和航位推算(DR) 3，其显著的特点是定位快速，且不 受外部环境的影响和干扰，但缺点是其单独定位伴随着误差累积，而且INS的解算比较复杂。 地面无线电定位包括地面通信网(GSM、CDMA、FM等)34和信标5。目前，地面无线 电定位技术在航海和航空领域获得了广泛的应用，例如Loran-C和DECCA 3,4，短距离信标的典型 应用是早期美国的电子路径引导系统ERGS5，这两种技术在城

6、市车载导航中难以普及，主要是环 境干扰、信号衰落和多径效应等的限制。2.2 GPS/DR组合是最理想的车辆定位方式实际上，还没有一种独立的定位技术能够实现高精度、稳定的车辆定位回。目前，GPS由于具 有高精度、全天候、高效率、多功能、操作简便等诸多优势无可争议地成为了现在最为广泛使用的 定位手段，但也有一些因素，如多路径效应、信号遮挡、信号丢失和弱观测环境等，会造成GPS定 位精度的下降甚至无法工作。差分GPS技术(DGPS) 1-3也仅仅只能消除其中的多路径效应和星历等 误差，因而采用其他定位方式辅助GPS进行组合定位就显得尤为重要。GPS和DR分别具有长时间绝对定位和短时间相对定位的稳定性

7、；当GPS信号丢失而无法定位 时，DR系统可继续定位输出，而GPS和DR的信息融合不仅可以提高定位的精度，而且，可以消 除DR的误差累积问题，两者优势互补。而且出于精度和价格两方面的考虑，GPS/DR组合也是城 市车辆比较合理的组合定位方式。3.GPS/DR组合系统的信息融合3.1联合卡尔曼滤波器实现信息融合的原理近30年以来，卡尔曼滤波(KF)4-8已经被认为是一种标准的滤波与信息融合方法7,8，并且 被广泛应用于车辆定位中4,5。但是，如果直接将卡尔曼滤波应用于GPS/DR组合定位系统，即采用 卡尔曼滤波集中处理所有传感器的量测数据，不仅滤波器的阶次高、计算量大；且系统不具有容错 能力，一

8、旦某个传感器发生故障，将导致整个组合定位系统无法正常工作。基于此，本文采用了一种简化的联合卡尔曼滤波器，以实现GPS/DR组合系统的滤波与信息融 合。参照通用联邦卡尔曼滤波器9J0的结构，去除其中的参考系统，且让主滤波器无信息分配，这样 就得到如图1所示的简化了的联合卡尔曼滤波器，其显著特点是主系统的计算量最小，而且总体系 统前向滤波速度最快，系统设计最佳。图1图1联合卡尔曼滤波结构其基本原理是：两个局部传感器分别处理GPS和DR系统的定位数据。两者的状态估计输送到 主滤波器进行最优融合并产生高精度的全局估计，然后按照信息分配系数对局部滤波器进行状态反 馈重置，从而使局部滤波器的精度也获得提高

9、。一旦某个子系统发生故障，只需要调整信息分配系 数便可实现另一个子系统独立定位。简化的联合卡尔曼滤波的主滤波器部分不进行滤波处理，只对来自不同传感器的定位数据完成 数据的综合。这种分散式滤波结构，不仅没有降低组合系统的定位精度，而且计算量小，稳定性高， 且避免了误差的“污染”时，具有很强的容错能力，因而更适用于车载GPS/DR组合定位系统的信息融 合。3.2 GPS与DR子系统的局部滤波3.2.1 GPS系统模型与局部滤波系统的坐标系取为地理坐标系(即东北天坐标系)，将车辆东北向的位置分量(e，n)，单位m ；速度分量(v , v )，单位m/s ；加速度分量(。，a )，单位m / s2；作

10、为系统的状态变量，即系统的 e ne n状态变量X = le ve ae n匕a J，将GPS接收机输出的车辆东向和北向位置坐标分量ep，n (单位m)作为外部观测量，可以建立GPS离散数学模型阳如下：(1)系统方程：X1 (k)=(k/k -1)X1 (k -1)+U(k(1)观测方程：Z(k) = H(k)X(k)+匕(k)(2)式中，中(k / k -1)为系统状态转移矩阵，U(k-1)为系统噪声；Z (k) = egps(k)为GPS系统观测量； 1 n (k)L gps H(k) = 1000 00为系统观测矩阵；10001 00V*）为系统观测噪声。该数学模型已经在GPS定位系统中

11、被广泛使用5-9,而且基于该模型的卡尔曼滤波也已经成为 了一种通用的滤波算法9，关于模型建立的详细过程、相关参数的解析以及卡尔曼滤波的推算过程， 可参见文献（4,8）。3.2.2 DR系统模型与局部滤波选取和GPS相同的状态变量，将陀螺输出的角速率（单位：rad/s）和里程仪输出的车辆在采样 周期T时间内行进的距离s （单位：m）作为DR子系统的外部观测量，构造离散数学模型W如下：（3）（4）X 2(k) = O 2(k / k -1) X 2(k -1) + U 2（3）（4）Z 2( k) = h k, X 2( k) + V2( k)式中，Z 2(k)=w (k)s(Z 2(k)=w (

12、k)s(k)为DR系统观测量;hk, X 2(k)=arctan 尝1七(k)， 邓2(k)+V2(k) en -1为系统观测矩阵;V2（k）为测量噪声阵。DR系统方程和GPS系统方程相同，但其观测方程是非线性的，一般采用扩展卡尔曼滤波器 （EKF）9进行线性化并滤波。关于线性化的过程以及线性化的DR数学模型，以及扩展卡尔曼滤波算 法，可参见文献（11,12,13）。3.2.3 GPS与DR的信息融合如果将子滤波器i的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为X、Q、P （其 中i = 1,2）；全局融合的滤波器的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为X : Q、 P；

13、则可按照如下规则来进行滤波器的信息融合与分配：Q -1 = Q-1 + Q -1Q-1 = P Q -1, Q-1 = P 2 Q -1P -1 = P -1 + P -112P -1 = P P -1, P -1 = P P -1 1122P -1X = P-1 X1 + P2-1X 2（5）式中，p ,p是信息分配系数，且满足P +p =1。1212在式（5）中，可以根据具体情况自动调整吟和P2的大小，实现信息的自适应融合。当GPS 正常工作时，取P1 = 0.5, &2 = 0.5，此时联合滤波器有较高的定位精度和容错能力；当出现卫星信 号遮挡等原因，造成GPS定位系统不能正常定位或定位

14、精度较差时，可以让&产0, &2 = 1，此时 联合滤波器的输出近似于DR子系统的输出。实时的调整吟和&2，0.992/PDOPPDOP22PDOP实时的调整吟和&2，0.992/PDOPPDOP22PDOP51/PDOP0.5 / PDOP=1&15 PDOP 1010 PDOP自适应策略增加了联合滤波器的抗干扰能力，增加了滤波算法的稳定性。高精度的地图匹配算法无论是单独GPS定位，还是GPS/DR组合定位，都需要将车辆位置和电子地图进行结合，地图 匹配的目的是将车辆位置无偏差地纠正到所在的道路上。在实际应用中，一般的车载导航系统只采 用GPS定位，甚至很多低价格的GPS定位系统都没有做卡尔

15、曼滤波处理，定位误差比较大，此时 通过地图匹配对定位点进行纠偏，便显得尤为重要。4.1匹配道路的选择地图匹配的关键就是选择匹配道路，只要确定了匹配道路，只需要将定位点向其做投影，即可 得到准确的地图匹配点。这里采用一种综合的权值因子TWS作为选择道路的依据，如式（7）所示，其考虑了车辆和各 道路的接近程度（WSD + WSp）以及车辆航向和各道路方向的一致程度WSh 0TWS = WS h + （WSD + WS）综合权值因子TWS最大的道路，即为最优匹配道路，将当前定位点直接投影到该道路即得到 准确的匹配点。各权值因子的计算方法如下所述：1）车辆航向和道路方向的符合程度如图2所示，P ,P

16、,P为车辆之前按先后顺序获取的定位点，P为当前定位点，车辆离开道 1234路1，备选匹配道路包括道路2、道路3和道路4。这里，定义&为当前车辆航向角（车向和正北方 向的夹角），定义&为道路方向和正北方向的夹角，在图2中，道路2,道路3和道路4的方向分 别是0。、90。和 180。图2航向与道路的符合程WSH = Ah cos）（8）其中，8 = 8邳=邳，if -180。Ap 180。A8 = 360。+Ap，if Ap -180。如果某条道路的ws h值越大，则车辆航向和该道路方向越接近。2）车辆和各道路的接近程度。这里从距离和角度两个方面，构造了两个权值WS Q和WS,来更全面准确地反映车

17、辆和各道路 的接近程度。如图3所示，P为当前定位点，AB为道路，C为P在该道路上的匹配点。图3图3车辆与道路的距离图4车辆与道路的角度牙(y y ) y 3 工)+ 3 y ”)小、P 到 AB 的距离 D = _123 121 =2(9)*x x )2 + (y y )21212NS。= AJD，该值越大，定位点和对象道路的距离越近。如图4所示，C为路口接点，P为当前定位点，a表示某条道路到以 的角度。W% = A, cos(a )，该值越大，定位点和对象道路越接近。WS,考虑了定位点、路口和道路线段的空间关系，在WSD的基础上，更进一步地反映了车辆 和道路的接近程度。以图4为例，P与道路1

18、和3的距离权值相等，即WS (1) = WS (3)，但WS 0 S (3),DDPP这样，(WSD+ WSp(1) (WSD+ WSp(3)，即对于道路1而言，P更接近于道路3。因此，(WSD + WSp)全面准确地反映了 P和各道路的接近程度。由于综合匹配因子tws充分考虑了车辆位置、车辆航向和道路，路口的空间关系，因此其匹 配算法相对于依靠距离或方向等传统的匹配方法14,15具有更高的精度和适应性；尤其是在定位误 差比较大的情况下，其依然能正确地识别车辆所在的道路，具有较高的抗干扰能力。4.2异常情况的处理异常情况指定位信号“漂移”、静止等。目前城市车辆普遍使用独立GPS定位，异常情 况

19、更加频繁。1）在车辆静止或极低速时，GPS定位点会不断地随机跳跃，此时保持最后静止的匹配 点作为每一静止时刻的匹配点。一般判断静止的方法是：连续3个定位点之间的距离小于定 位误差容限，即认为车辆静止了。停止静止的判断方法是：连续三个点之间的距离大于定位 误差容限，即确认车辆开始运动了。2）在定位信号“漂移”，此时保持最后有效的匹配点作为即时的匹配点。考虑到车辆的 机动加速特性，判断“漂移”的方法是：如果当前定位点和前一定位点的距离大于前一定位点 时速度的3倍，则认为定位信号“漂移”了。3）在传感器无法定位时（如GPS信号被“遮挡”），直接利用之前有效的匹配点作为即 时匹配点。4.3地图匹配的算

20、法流程地图匹配算法的实现流程如图5所示。获取定 -*位数据检查定位数是否漂移或无效值.是f保持之前匹配值- 或线性化插值否V是否静止.：一是* J或极低速？.用之前的匹配点作为 匹配结果T 否计算综合匹配权值TWS选择TWS最大的道路作为匹配-道路将定位点在匹配道路的投影点作为匹配点图5地图匹配的算法流程实验与分析5.1滤波与信息融合算法实验为了充分测试GPS/DR组合定位的优势以及联合卡尔曼滤波的效果，本文进行了车载 GPS单独定位、车载DR单独定位与车载GPS/DR组合定位的对比实验。实验环境：数据采集利用北工大自主技术与智能控制研究中心的GPS OEM（12通道， 最低追踪信号感度-17

21、5dBW）、惯性导航开发板（板载角速率陀螺ADXRS150ER和加速度计 Inna AL-M30）以及 PC104 （研华 PCM-3350, 300MHZ CPU，128MB SDRAM），操作系统 Windows CE，采样周期1s；采集轨迹为北工大校园北门至校园南门，车速大致保持在25km/h 左右。选取车辆行驶过程中的300组数据案例，在PC下利用Matlab进行滤波与信息融合算 法的对比分析。GPS与DR数学模型参数直接采用文献（1）中建模仿真的设置参数，联合卡尔曼滤波器的信息分配系数选取B1 = B 2 = 0.5。GPS/DR组合定位与GPS单独卡尔曼滤波定位的东北向位置误差比较

22、如图6中(a)和(切 所示；此外，DR独立定位的东北向位置误差如图7 中(a)和(切所示。图6图6组合定位与GPS单独定位误差比较图7 DR独立定位东北向位置误差图在图6中，独立GPS定位时，采用Kalman滤波后东北向位置最大误差分别是4.5731m 和4.5126m；对于GPS/DR的组合定位方式，采用联合卡尔曼滤波后东北向位置的最大误差 分别是2.7811m和2.8102m。通过对比可以明显看到基于联合卡尔曼滤波的GPS/DR组合定 位比单独的GPS卡尔曼滤波定位更加准确。图7为GPS信号无效情况下，DR独立定位的误差曲线图，可以看到DR定位虽然是一 个误差不断累积和发散的过程，但在一段

23、时间内(大约120s)，可以保证比较准确的定位输出 (东北向位置误差均在4m以内)，这样在GPS卫星信号被遮挡或者GPS无法正常工作的时 候，DR可以提供暂时的车辆实时定位。因此，基于联合卡尔曼滤波的GPS/DR组合导航具 有很好的定位精度、容错能力和可靠性。综上所述，联合卡尔曼滤波器不仅实现了 GPS和DR的信息最优融合，获取了高 精度的位置信息，而且实现了 GPS无效或弱GPS条件下的DR独立定位，提高了定位系统 的容错能力和稳定性。5.2地图匹配实验为了测试方便，将本文中的综合地图匹配算法移植到SNE1.1(SuperMap NavigatorEngine)的引导引擎中，利用修改引导引擎

24、后的SNE1.1在北京和天津区域做了大量的路测。实验环境:测试设备为 HP iPAQ B21 手持机，400MHZ CPU(ARM920T)，64MB SDRAM， 操作系统为Windows Mobile5.0，GPS接收器为HOLUX GPSlim236，其最低最终信号感度 为-159dBW,。匹配算法相关权值系数设置为：AD = A =10, Ah = 50；并利用SuperMap DeskonPro强大的分析功能对比分析匹配算法的精度。图8为北京市内2009年4月1日测试的部分匹配轨迹。图9图14反映了测试过程中， 各种路况条件下的匹配效果以及定位异常情况下的匹配处理。图8长距离连续匹配

25、测试部分轨迹图9路口的匹配图10路口定位点和匹配点的对比图11主辅路的匹配图12上下线分离道路的匹配图13静止的处理图14定位信号中断的处理图9图14非常直观地反映了各种情况下匹配算法的准确性。图9为SNE1.1的真实导 航界面截图，截图场景为西三旗桥路口，黑色线为定位点串构成的轨迹，高亮紫色线路为匹 配后的车辆运行轨迹，图10为经过该路口（西三旗桥）过程中的定位点串和匹配后的点串 的对比显示。可以明显地看到，在路口情况下的综合匹配算法具有很好的匹配精度。图11 和图12,反映了定位误差比较大的情况下，综合匹配算法依然能够正确去区分主辅路和上 下线分离道路。图13反映了静止时的匹配处理效果，可

26、以看到，在这一过程中，匹配规则 能够及时动态识别车辆的“由运动变静止”以及“由静止变运动”两种状态。图14中，定位信 号中断后，匹配点保持为信号有效前的点，在定位信号恢复后，匹配立刻恢复正常。表1纠偏距离的统计分析纠偏距离相关统计参数结果纠偏距离相关统计参数结果纠偏案例总数Sum纠偏案例总数Sum7962最小纠偏距离dmin最小纠偏距离dmin0.002523m3最大纠偏距离dmax46.80149m4均值E d11.276635方差D d78.57086绝对偏差均值mse7.328531d如果不考虑GPS“漂移”和失效等异常情况下的匹配处理，路测过程中，利用综合权值进 行算法匹配案例共计79

27、62次。如果定义定位点与匹配点的距离为纠偏距离d，单位m，纠 偏距离也可以理解为定位点和真实点的误差，其数学统计如表1所示。结合图8和表1，可 以看到，在定位信号不稳定，以及市内道路情况复杂等条件下，匹配算法依然能长时间保持 准确和稳定的定位纠偏。综上所述，本文采用的综合匹配算法实现了各种城市路况情形下正确行驶道路的选择和 定位点的精确匹配，而且其高精度并不是以一定的滞后时间为代价UH具有非常好的实时 性，是目前车辆定位中一种真正实用的高精度地图匹配算法。6.结论GPS/DR是一种理想的车辆组合定位方式。本文设计的联合卡尔曼滤波器，能自适应地 进行GPS/DR系统的信息融合，具有很好的精度和抗

28、干扰能力，且易于实现，真正实现了低 价格、高精度的实时车辆定位。综合地图匹配算法实现了车辆位置与GIS道路的无偏差结 合，该算法兼顾精度和稳定性。通过大量的实验验证了本文算法的优越性和可行性。对于目 前比较主流的独立GPS定位，算法同样适应，并对GIS位置服务相关其他应用同样提供了 一种非常好的定位解决方案。参考文献：张其善，吴今培，杨东凯，智能车辆定位导航系统及应用M.科学出版社，北京.2002.付梦印，邓志红，张继伟.Kalman滤波理论及其在导航系统中的应用M.北京：科学出版社，2003.李卫平，智能交通技术应用M.人民交通出版社.人民出版社，北京，2006.赵亦林，车辆定位与导航系统M.电子工业出版社，北京，1999.袁信，俞济祥，陈哲，导航系统M.航空工业出版社，北京，1993.Zickel Robert and Nahum Nebemia. GPS Aided Dead Reckoning NavigationC.Proceedings of Natural Technical Meeting,1994,pp.577-585.寇艳红，张其善，李先亮.车载GPS/D