一种新型车辆组合定位算法

一种新型车辆组合定位算法
【专利摘要】本发明公开了一种新型车辆组合定位算法，采用联邦卡尔曼滤波数据融合技术，实现路侧设备定位(RSU)、卫星定位(GNSS)、航位推算(DR)三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β1；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β2；DR定位子系统采用扩展卡尔曼滤波器作为局部滤波器，记为LF3，对应的信息分配系数为β3，主滤波器做信息融合；利用残差χ2检测和冗余硬件检测构建了两级故障检测方法，能同时检测出子系统的硬故障和软故障；同时根据定位子系统的定位精度因子来动态调整组合系统的信息分配系数，使系统能够快速适应环境的变化，提高车辆定位的精度与可靠性。
【专利说明】
一种新型车辆组合定位算法
技术领域
[0001 ]本发明涉及车辆定位的研究领域，特别涉及一种新型车辆组合定位算法。
【背景技术】
[0002]卫星定位具有高精度、低成本、易用性等特点，只要能接收到卫星信号的地方，卫 星定位接收机都可以解算出车辆坐标，而且误差不会累积。但是在城市复杂环境中，由于卫 星信号会被建筑物遮挡，卫星定位精度下降。卫星定位系统单独使用时，系统定位可靠性较 低。航位推算(DR)定位系统是常用的车辆自主定位方法，只要给定车辆初始位置，就可以利 用车辆的行驶速度和行进方向来推算出车辆的当前位置，在短时间内能提供高精度的定位 服务。如果DR定位系统的初始位置无法得到周期性的更新或校准，由于其误差累积特性，定 位可靠性下降。RSU定位方法随着车联自组织网络(VANET)的发展得到了应用，利用车辆与 RSU的通信，可以不依赖卫星的情况下为车辆提供定位，在密集的建筑群中部署RSU可以实 现车辆定位。定位精度与车辆所在位置的RSU部署密集程度和无线信号是否被遮挡有关，车 辆接收到的RSU的定位信号越多且遮挡越少，系统定位精度越高。在城市中心高楼密集区域 或者地下停车场部署RSU可以有效地解决卫星定位失效区域车辆定位问题。以上三种系统 各有优劣，面对复杂的城市环境，单一系统都无法满足现在车联网应用的定位需求，利用信 息融合技术提高定位系统的可靠性和精度，将是解决车辆定位的可行方法。
[0003]如果这三个系统采用集中式卡尔曼滤波来实现信息融合，将会产生两个问题:第 一、计算负担重。集中式滤波器对处理器的计算能力要求较高，车载嵌入式处理器无法满足 计算的实时性要求;第二、容错性能差，不利于故障诊断。GNSS定位系统在有卫星信号的情 况下实现定位，而基于RSU的定位系统则刚好相反，在卫星定位失效区域部署RSU为车辆提 供定位服务。当任一子系统发生故障，如果不能快速地检测出来，组合定位系统就会被故障 子系统污染，使输出的位置信息不可靠。

【发明内容】

[0004] 本发明的主要目的在于克服现有技术的缺点与不足，提供一种新型车辆组合定位 算法，采用联邦卡尔曼滤波(FKF)组合定位算法，实现车辆的高精度定位。
[0005] 为了达到上述目的，本发明采用以下技术方案：
[0006] 本发明一种新型车辆组合定位算法，采用联邦卡尔曼滤波算法，实现R S U定位、 GNSS定位、DR定位三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系 统的局部滤波器，记为LF1，对应的信息分配系数是i3 1;GNSS定位子系统也采用线性卡尔曼 滤波器作为局部滤波器，记为LF2,对应的信息分配系数为i32;DR定位系统采用扩展卡尔曼 滤波作为局部滤波器，记为LF3，对应的信息分配系数为β 3，主滤波器做信息融合，具体包括 下述步骤：
[0007] S1、根据汽车运动学建立系统状态方程与观测方程，所述观测方程包括RSU观测方 程、GNSS观测方程以及DR观测方程；
[0008] S2、根据LF1、LF2和LF3对三个子系统进行时间更新和量测更新；
[0009] S3、在信息融合过程中进行故障检测，利用两级故障检测技术，检测出故障子系统 并实现隔离；
[0010] S4、信息融合与信息分配，系统在信息分配过程中，根据定位子系统的定位精度因 子来动态调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构。
[0011] 作为优选的技术方案，在步骤S4中，所述两级故障检测的方法:首先各子系统利用 残差X2检测法去检测出现硬故障的子系统，然后利用冗余硬件检测法去检测系统软故障， 另外，为了能校验车载定位系统本身的精确度，还提出了基于车辆与路侧设备通信V2R的位 置参照检测法，让系统可以及时发现因车载设备自身问题而导致定位不准确的原因。
[0012] 作为优选的技术方案，所述残差X2检测法具体如下：
[0013] 由观测值的预测值之^与实测值Zk之差构造检验统计量的方法称为残差X2检验 法，残差定义为：
[0014] r(k) = Zt -Zkfk_x = 'Zk-HkXkjk
[0015] 当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生 故障时，r(k)的期望是不为零的值，根据这个特性，通过检测残差r(k)均值就可以确定被检 测系统是否发生了故障，由公式=-之^ = ? -可知，残差向量r(k)也可以 被认为均值为零的高斯白噪声过程，其协方差S(k)为：
[0016] S⑷= 丨",?
[0017] 构造以下故障检测函数：
[0018] D(k)=rT(k)S_1(k)r(k)
[0019] 由残差序列的统计特性知，D(k)服从自由度为η的X2分布，η为状态向量X的维数， 如果设定故障检测阀值T D，那么可以根据下述公式检测系统故障，阀值TD决定了故障检测的 性能：
[0021] 作为优选的技术方案，所述冗余硬件检测法具体为：
[0022] 采用低成本冗余硬件检测，通过增加加速度计对比各子系统所测量的车辆速度的 一致性来实现故障的检测与隔离。5、根据权利要求2所述的新型车辆组合定位算法，其特征 在于，所述位置参照检测法具体为：
[0023]传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统，V2R通信的 发展，在某些特定区域，如停车场，车辆可以通过后台数据库查询到自己所处的精确位置坐 标，利用所查询到的坐标信息与实时定位的坐标相对比，以此来判断车辆定位系统的准确 性。
[0024] 作为优选的技术方案，在步骤S4中，自适应的信息分配系数调整方法以各子系统 的定位精度因子为主要参考，动态调整信息分配系数，提高联邦滤波器的性能，实现组合导 航。
[0025] 作为优选的技术方案，三个子系统在设定初始参数后，经过时间与量测更新后，得 到了进一步预测值，根据残差X2检验算法对各子系统进行第一级硬故障检测，剩余无故障 子系统再使用冗余硬件故障检测法，对比找出有故障的系统进行故障隔离，对于余下的无 故障子系统，联邦卡尔曼滤波组合算法根据子系统定位精度因子动态调整信息系分配数， 具体如下：
[0026] (1)有故障子系统的信息分配：
[0027] 有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种 情况下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系 统输出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论；
[0028]第一、基于RSU的定位子系统出现故障，= 0，在开阔区域，一般没有部署或少量 部署无法提供定位功能，那么基于RSU定位子系统将会出现故障，系统只剩下GNSS和DR子系 统，构成了 GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的Η)0Ρ参数来确定， ro〇P通常用来衡量定位精度的好坏，取值范围从0.5到99.9，系数如下述公式所示：
[0031]第二、GNSS出现故障，β2 = 0,当车辆进入到隧道或者在高楼密集的城市中心区域， 卫星信号被遮挡，GNSS将会出现故障，一般情况下，在这些特殊区域部署RSU基站，为车辆提 供定位服务，此时RSU/DR工作，信息分配系数将按照车载DSRC设备计算的的定位精度因子 D0P的值来确定，信息分配系数如下述公式所示：
[0033] β3=1-βι
[0034] 第三、DR出现故障时，根据冗余硬件实现伪DR系统功能，1)车载里程仪的速度测量 出现异常，取车速计的速度值代替；2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位 置信息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题，如果 DR子系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模 块。
[0035] 作为优选的技术方案，当无故障子系统的信息分配时：
[0036]不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作， 设计基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题， GNSS不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之 间，存在GNSS和RSU定位系统两个系统都能正常工作的区域，因此，信息分配系数根据子系 统定位的质量，表现为GNSS的roop和RSU的定位精度因子，来调整&的值，一般情况下，如果 GNSS的PD0P小于等于2或RSU的D0P大于等于0.9时，将以这两个系统的定位信息为主，分情 形讨论；
[0037] 1)当GNSS的 PD0P〈 = 2且RSU的 D0P〈0 · 9时，& = 0 · 99，βι = β3 =(卜&) /2;
[0038] 2)当GNSS的 PD0P>2 且 RSU 的 D0P> = 0 · 9时，βι = D0P，β2 = fo = α _βι) /2;
[0041] 系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自适应子系统 定位精度的变化。
[0042] 本发明与现有技术相比，具有如下优点和有益效果：
[0043]本发明利用联邦卡尔曼滤波算法实现了基于RSU的定位子系统，GNSS定位子系统 和DR子系统的信息融合;利用残差X2检测法和冗余硬件检测法构建了两级故障检测方法， 能同时检测出系统的硬故障和软故障;根据子系统的定位质量因子来动态调整组合系统的 信息分配系数，使系统能够适应环境的变化。与其他组合定位系统具有如下优势：
[0044] (1)环境适应能力较强，可靠性高。国内大城市越来越繁华，越来越密集，卫星定位 在这样的环境下无法工作，组合定位算法可以适应，加上可靠的故障检测和自适应的信息 分配系数调整方法，系统可靠性更高。
[0045] (2)具有车-车、车-路通信能力。在停车场、隧道内也可以通信，弥补了3G/4G网络 的缺陷。
【附图说明】
[0046]图1是多模卫星定位系统定位原理图；
[0047]图2是RSU定位原理与坐标转换；
[0048]图3是RSU协同定位示意图；
[0049]图4是RSU/GNSS/DRS组合定位系统框架图；
[0050]图5是位置参照检测示意图；
[0051 ]图6是多级故障检测与信息分配流程图；
[0052]图7是测试场景示意图；
[0053] 图8(a)-图8(b)是GNSS定位结果；
[0054]图9(a)-图9(b)是基于RSU的子系统定位结果；
[0055] 图10(a)-图10(b)是基于RSU/GNSS/DR组合定位结果与对比图。
【具体实施方式】
[0056]下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限 于此。
[0057] 实施例
[0058]本发明采用联邦卡尔曼滤波(FKF)组合定位算法，实现车辆的高精度定位。首先介 绍三个子系统的定位原理;然后详细讨论RSU/GNSS/DR组合定位算法和两级故障检测方法， 并提出了自适应信息分配系数动态调整方法;最后利用跑车实验验证了所提出的组合定位 算法的可行性。
[0059] 1、车辆组合定位系统概述
[0060] GPS是建成最早也是应用最广泛的全球定位系统。BDS是我国自行建设的卫星系 统，随着新一代的导航卫星的发射，标志着BDS迈入了全球部署阶段，GLONASS虽然建设早， 但是由于俄罗斯的财力，逐渐被BDS所超越。Galileo是最精确的卫星定位系统，在设计初就 考虑与GPS的兼容问题，但是进展最慢。单一系统已经不能满足高精度的定位需求，支持多 个卫星定位系统的多模卫星接收机由于其高性价比逐渐成为GNSS的主流。多模GNSS定位精 度比单一卫星定位系统有较大的提高，但是由于卫星信号容易被遮挡，DR系统仍然是其重 要的辅助部分。有了 DR系统的辅助，GNSS/DR组合定位可以解决短时间内卫星信号被遮挡环 境下的定位，如果GNSS长时间无法获取自身位置而导致DR系统无法重置其初始位置，由于 误差累积会导致系统总体定位精度大幅下降，如在香港的部分街区，系统长时间捕捉不到 足够的卫星信号。基于RSU的地面定位系统在类似这样的环境下将可以为GNSS/DR系统提供 很好的补充，因此RSU/GNSS/DR组合定位应运而生。
[0061 ] 1.UGNSS 定位原理
[0062]随着中国北斗系统逐步投入使用，BDS与GPS组合定位成为研究热点。采用支持BDS 和GPS导航系统一体化S0C芯片，多系统的融合不是对单模定位结果做选择，而是同时利用 多个系统的卫星播发的信息，提取相关原始测量值，并将原始测量单元做数据融合从而解 算出定位位置，如图1所示。因此多模工作模式不是定位结果的数据融合，而是多卫星定位 系统的原始测量单元的数据融合过程。总而言之，多模卫星定位是卫星接收机发展的趋势。 [0063] 1.2、航位推算原理
[0064]航位推算(DR)系统是车辆定位导航中采用的一种比较经典的完全自主的导航系 统，在二维的空间上给车辆进行定位。假定汽车的初始点(xq，y〇)和初始航向角θ〇，通过车载 里程仪、角速率陀螺实时测量车辆的行驶距离和航向角的变化，推算出车辆的位置。
[0065] 具体算法如下：
[0066] (U) ν, = vn + 4sm^
[0067] 车辆在tk时刻的位置同样可以表示为：
[0069]其中，（XQ，yQ)是车辆的初始位置，di和0汾别是车辆从ti时刻到t i+1时刻的位移矢 量的长度和绝对航向。假定采样周期是恒定的(如1秒1次），那么公式(1.3)可以表示为：
[0071]从以上讨论可知，航位推算是一个基于初始位置逐步累加的过程，需要实时得到 一段时间内的距离变化量CU和航向角0,，还必须确定初始位置。因此，不同时刻的测量误差 和计算误差都会累积起来，需要周期性地重置初始位置。总之，单一的DR系统不适合用于长 时间的定位。
[0072] 1.3、基于RSU的定位原理与RSU自定位
[0073]利用时间间隔测量模块测量无线信号从车辆到RSU的飞行时间，再乘以无线电波 的传播速度，可以得到车辆与RSU之间的距离。
[0075] 当η彡3时，利用EKF就可以解算出车辆的坐标，然后完成坐标变换，如图2所示。
[0076] 根据算法要求RSU的坐标需提前确定，所以要解决RSU自身的定位问题。如果RSU坐 标不准确，就无法为车辆提供位置参考。因此，部署RSU时，在RSU中存储其自身的位置信息 (如经炜度(:《α:))，利用经炜度推算出UTM坐标化九2〇1^，他11^)。1?1]设备带有6吧3定位模块 可以自定位，可以解决开阔区域的RSU自定位问题。如果部署在城市中心区或地下停车场 时，RSU的自身位置需要在部署时利用协同定位方法确定自身位置然后存储在RSU中，如图3 所示。
[0077]在地下停车场附近可以接收到GNS S信号的地方部署RSU5、RSU6、RSU7，然后利用 0SS-TWR测距算法和扩展卡尔曼滤波算法，可以解算出RSU3的坐标，最后再依次解算出其他 3个RSU的坐标，经过多次迭代，RSUh的坐标将会更精确。
[0078] 1 · 4、FKF辅助的RSU/GNSS/DR组合定位算法
[0079] 面对复杂的城市环境，需要多种定位技术融合才能实现高精度、高可靠的车辆定 位。集中式卡尔曼滤波组合算法由于计算量大、故障分离难等问题，在多维度的信息融合中 略有不足。本发明采用联邦卡尔曼滤波算法，实现基于RSU的定位、卫星定位、航位推算三者 的信息融合，为车辆提供准确的车辆位置服务。基于RSU的定位系统采用线性卡尔曼滤波器 作为局部滤波器，记为LF1，对应的信息分配系数是i3 1;GNSS定位系统也采用线性卡尔曼滤 波器作为局部滤波器，记为LF2,对应的信息分配系数为fe;DR定位系统采用扩展卡尔曼滤 波作为局部滤波器器，记为LF3，对应的信息分配系数为β 3;主滤波器做信息融合，所设计的 RSU/GNSS/DR组合定位系统框架如图4所示。
[0080] 2.、系统状态方程与观测方程
[0081] 采用"当前"统计模型来描述车辆加速度的统计分布，当车辆正以某一加速度机动 时，它在下一瞬时的加速度取值范围是有限的，而且只能在当前加速度的邻域内。根据上述 讨论可知
[0083]其中，ai(t)为机动加速度，其方差为σ2，Ι·(?)为机动加速度"当前"均值，其中a在每 一采样周期内为常数。a(t)为均值为零的有色加速度噪声，w(t)是均值为零的高斯白噪声， τ为机动加速度变化率的相关时间常数。
[0084]取组合定位整体状态变量为X = [ Xe，Ve，ae，Xn，Vn，an]T，其中Xe，Χη分别是车辆东向 和北向的位置分量(单位:m)，V(3，Vn分别是车辆东向和北向的速度分量(单位:!11/ 8)，~，&11分 别是车辆东向和北向的加速度分量(单位:m/s2)。系统的状态方程写成矩阵形式为：
[0088] '，'是均值为0,方差分别为σ^，<的高斯白噪声&分别是车辆东向 和北向机动加速度变化率的相关时间常数;％，瓦分别为车辆东向和北向机动加速度分量 的"当前"均值。
[0089] 设采样周期为T，将系统连续的状态方程离散化，得到离散的系统状态方程为
[0090] X(k) = ?(k/k-l)X(k/k-l)+U(k)+ff(k) (1.8)
[0091 ]其中，X(k) = [xe(k)，ve(k)，ae(k)，xn(k)，v n(k)，an(k) ]τ [0092] Φ (k/k-1) =diag[ 0e(k/k~l), ?n(k/k-l)] (1.9)
[0097]其中，
[0101] (l)RSU观测方程
[0102] 对于基于RSU的定位子系统，取系统状态变量X1 = X，系统状态方程与总体系统状 态方程相同。其观测量为子系统定位输出的车辆东向和北向位置坐标分量e〇bs，n〇b s (单位： m)，观测方程可表示为：
[0103] Zi(k)=Hi(k)Xi(k)+Vi(k) (1.11)
[0104] 其中，
[0108] we(k)和wn(k)是均值为0,方差分别为af , 的高斯白噪声序列，表示在RSU定位 设备在东向和北向的位置测量噪声，量测噪声协方差矩阵Λ(幻= , )。
[0109] (2)GNSS 观测方程
[0110] 对于GNSS定位子系统与RSU定位系统类似，取系统状态变量X2 = X，与总体系统方 程相同。其观测量为GNSS接收机输出的车辆东向和北向位置坐标分量6。^，11^，观测方程可 表示为：
[0111] Z2(k)=H2(k)X2(k)+V2(k) (1.12)
[0112] 其中，
[0116] We3(kWPwn(k)是卫星定位接收机在东向和北向的噪声误差，其均值为0,方差分别 为#，σ Μ2的高斯白噪声序列，量测噪声协方差矩阵：<)。
[0117] (3)DR观测方程
[0118] 航位推算系统的状态变量为X3 = X，子系统状态方程与总体系统状态方程相同，其 观测量取角速度陀螺仪的输出ω和里程表输出的车辆在采用周期Τ时间内行进的距离s，即 系统的观测方程可表示为：
[0119] Z3(t)=h3(t，X3(t))+V 3(t) (1.13)
[0120] 其中，
[0124] 假定标定系数价为1，εω为陀螺的漂移，近似为(0, <)的高斯白噪声，es为里程表输 出量s的观测噪声，近似为(0, of)的高斯白噪声，量测噪声协方差矩阵：/?α)=&轨σΚ)。
[0125] 将观测方程离散化，得到系统离散的观测方程为
[0126] Z3(k)=h3[k，X3(k)]+V 3(k) (1.14)
[0127] 局部滤波器采用扩展卡尔曼滤波，将h3[X3(k)]在Μ -I)附近展开为泰勒级数， 忽略二次以上的高次项，可得观测方程
[0137] 3.子系统的时间更新与量测更新
[0138] LF1和LF2子系统时间更新与量测更新方程
[0139] XAk / k -1) = Φ(Α: /k -\)Χ^ -1) + U{k -1) (1.16)
[0144] 状态转移矩阵:把加速度的一步预测看做"当前"加速度的均值，即
[0145] 元⑷二次(人"人·-ft A⑷二太(人"々-I)
[0146] 公式(1.20)可以简化为
[0147] (k / = Φ(^ Ik-\)X\{k-1) (1.21)
[0148] 其中，Φ (k/k-1) =diag[ ΦΘ(Τ)，Φη(Τ)]，即
[0150]系统噪声协方差矩阵:Q(k_l)为系统噪声协方差矩阵Q的离散化矩阵
[0153]由于采用了扩展卡尔曼滤波作为LF3的局部滤波器，其推导方程与LF1和LF2略有 不同，如公式（1.22)所示，其中，系统噪声协方差矩阵Q(k-l)和状态转移矩阵?(k/k-l)与 LF1和LF2相同。
[0155] 4、全局信息融合与信息分配
[0156] 全局信息融合：
[0158] 信息分配：
[0160] 其中，&+β2+β3 = 1。
[0161] 5、两级故障检测与信息分配新方法
[0162] 车联网应用对定位的容错性和可靠性提出了更高的要求，故障检测与故障隔离是 提高系统可靠性的有效途径。残差X2检验法被认为是比较可靠的检测方法，在检测系统硬 故障具有灵敏度高，可靠性好的特点，但是对软故障检测的有效性下降。
[0163] 超大集成电路技术的进步和成本的下降，为组合定位系统的冗余硬件容错设计提 供了可能性，本系统增加一个加速度计AD)(L345测量车辆速度，对系统的故障进行检测。残 差X 2故障检测和冗余硬件故障检测为组合定位算法构建了一个两级故障检测新方法。
[0164] 另外，现有的车载定位设备暂时还没有方法去检测和校正自身设备的定位准确 性，基于V2R通信，本发明提出了一个操作简单的基于位置参照的检测方法，用于校验车载 设备GNSS定位精度和基于RSU的定位精度，有利于及早发现车载定位设备本身的问题。
[0165] 组合定位系统利用两级故障检测技术，可以把系统软硬故障都检测出来。系统在 信息融合时，合理调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统 的重构。本发明提出了一种新的自适应系数调整方法，构建具有高容错性的联邦组合定位 算法。
[0166] 5.1、两级故障检测方法
[0167] 本系统采用两级故障检测方法，首先各子系统利用残差X2检测法去检测出现硬故 障的子系统，然后利用冗余硬件检测法去检测系统软故障。另外，为了能校验车载定位系统 本身的精确度，本发明提出了基于V2R通信的位置参照检测法，让系统可以及时发现因车载 设备自身问题而导致定位不准确的原因。
[0168] (1)残差义2检测法
[0169] 由观测值的预测值与实测值Zk之差构造检验统计量的方法称为残差X2检验 法。残差定义为：
[0170] r{k) = Z, -tklk^ = Zk (1.25)
[0171] 当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生 故障时，r(k)的期望是不为零的值。根据这个特性，通过检测残差r(k)均值就可以确定被检 测系统是否发生了故障。由公式（1.25)可知，残差向量r(k)也可以被认为均值为零的高斯 白噪声过程，其协方差S(k)为：
[0172] S(k)^HkPk.k ,//； +Rk ( 1.26 )
[0173] 构造以下故障检测函数：
[0174] D(k)=rT(k)S-Hk)r(k) (1.27)
[0175] 由残差序列的统计特性知，D(k)服从自由度为η(状态向量X的维数）的x2分布，如 果设定故障检测阀值Td，那么可以根据公式（1.32)检测系统故障，阀值T D决定了故障检测的 性能。
[0177] (2)冗余硬件检测法
[0178] 采用低成本冗余硬件检测，通过增加ADXL345 (市场价格7元人民币，对总体成本影 响很小)对比各子系统所测量的车辆速度的一致性来实现故障的检测与隔离。本系统中里 程仪、加速度计、基于RSU的定位系统、卫星导航这四个系统可以测量一个周期内的车辆速 度，按照少数服从多数的原则来实现检测与隔离。冗余硬件检测法可以有效解决软故障检 测问题。
[0179] (3)位置参照检测法
[0180]传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统。V2R通信的 发展，在某些特定区域，车辆可以通过后台查询到自己所处的位置，如图5所示。
[0181 ]在停车位，车辆通过与RSU (咪表)设备通信自动完成停车交费，在这个过程中，RSU 可以向车辆发送本车位的精确位置信息，车辆利用这个信息与车载定位系统提供的位置信 息的一致性来判断车载定位系统是否准确。新带来的问题就是现有的停车位位置不可知。 事实上，随着I〇V的不断推进，全市的停车位会纳入城市智能停车场管理系统进行统一管理 和查询，任何一个停车位都有精确的位置信息，并且清楚显示在GIS地图上。
[0182] 综上所述，由于车载里程仪、加速度计等检测设备性能下降或外部环境如温湿度 的影响，子系统的可能发生故障，这些故障既有突变的硬故障也有缓慢变化的软故障。残差 X 2检验法由于计算相对简单，对子系统硬故障的检验非常有效，因此，在组合导航系统中应 用广泛。基于硬件冗余的检测方法，可以有效地检测出系统的软故障，但是会增加系统的成 本。采用成本低的硬件进行部分冗余，可以兼顾成本和性能，提高系统可靠性。利用位置信 息已知的停车位给车辆定位系统提供检测参照，是车联网发展到现在的附加功能，它可以 检测出定位系统中存在的一些利用自身无法确定的故障，相对前面两种来说，检测结果更 客观。检测结果虽然无法直接用于组合定位算法中，如参与信息分配，但是可以用于定位设 备维护，或给DR子系统设置初始位置等。
[0183] 5.2、自适应的信息分配系数调整方法
[0184] 最早Carlson提出的固定比例的信息分配原则，考虑到复杂城市环境中各个子滤 波器性能和位置估计准确度在不断地变化，因此引入动态自适应信息分配的方法，以各子 系统的定位精度为主要参考，动态调整信息分配系数队，改变联邦滤波器的性能，实现组合 导航，主要工作流程如图6所示。
[0185] 三个子系统在设定初始参数后，经过时间与量测更新后，得到了进一步预测值，根 据残差X2检验算法对各子系统进行第一级硬故障检测。剩余无故障子系统再使用冗余硬件 故障检测法，对比找出有故障的系统进行故障隔离。对于余下的无故障子系统，联邦卡尔曼 滤波组合算法根据子系统定位精度因子动态调整信息系分配数，具体如下：
[0186] (1)有故障子系统的信息分配
[0187] 有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种 情况下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系 统输出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论。
[0188] 第一、基于RSU的定位子系统出现故障(fo = 0)。在开阔区域，一般没有部署或少量 部署无法提供定位功能，那么基于RSU定位子系统将会出现故障。系统只剩下GNSS和DR子系 统，构成了 GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的Η)ΟΡ参数来确定， ro〇P通常用来衡量定位精度的好坏，取值范围从0.5到99.9，系数如公式（1.29)所示。
[0190] β3=1-β2
[0191] 第二、GNSS出现故障（β2 = 0)。当车辆进入到隧道或者在高楼密集的城市中心区 域，卫星信号被遮挡，GNSS将会出现故障。一般情况下，在这些特殊区域部署RSU基站，为车 辆提供定位服务，此时RSU/DR工作。信息分配系数将按照车载DSRC设备计算的的定位精度 因子(D0P)的值来确定，信息分配系数如公式(1.30)所示。
[0193] β3=1-βι
[0194] 第三、DR出现故障时，根据冗余硬件实现伪DR系统功能。1)车载里程仪的速度测量 出现异常，取车速计的速度值代替；2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位 置信息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题。如果 DR子系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模 块。
[0195] (2)无故障子系统的信息分配
[0196] 不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作。 设计基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题， GNSS不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之 间，存在GNSS和RSU定位系统两个系统都能正常工作的区域。因此，信息分配系数根据子系 统定位的质量，表现为GNSS的roop和RSU的定位精度因子，来调整&的值。一般情况下，如果 GNSS的PD0P小于等于2或RSU的D0P大于等于0.9时，将以这两个系统的定位信息为主，分情 形讨论。
[0197] 1)当GNSS的 PD0P〈 = 2且RSU的 D0P〈0 · 9时，& = 0 · 99，βι = β3 =(卜&) /2;
[0198] 2)当GNSS的 PD0P>2 且 RSU 的 D0P> = 0 · 9时，βι = D0P，β2 = fo = α _βι) /2;
[0199] 3)其他情况，爲=/</2,爲二戽/2
[0201] 综上所述，系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自 适应子系统定位精度的变化，提高了系统的可靠性。
[0202] 6、实验与结果分析
[0203] 车联网基于位置服务的应用很多，除了传统的车辆导航服务外，还出现了很多新 的应用：市区车位预约、自动寻位泊车、广告推送、特种车辆实时监控。以特种车辆实时监控 为例，传统方法只依靠GPS接收机来获取车辆位置，然后通过3G/4G网络把车辆的位置(包含 车辆状态信息)发送到监控中心，监控端通过GIS系统辅助可以对车辆进行实时跟踪及轨迹 回放。但是弊端是进入城市中心区域或稍长的隧道，就无法对车辆进行定位。基于RSU/ GNSS/DR组合定位算法的定位解决方案，可以在城市中不同的环境中为车辆提供定位服务， 还可以通过V2R通信把车辆当前的状态实时传送到监控端。
[0204]如图7所示，区域1没有部署RSU，在区域1中主要测试多模GNSS子系统在开阔区域 的定位精度，在区域2部署了 7个RSU，测试在GNSS失效的情况下，RSU/DR定位的效果，区域3 也没有部署RSU，测试在楼群之间组合定位RSU子系统出现故障时GNSS/DR的定位效果。 [0205] 6.1、车辆定位及结果分析
[0206]根据预设路线，按照GNSS单一系统定位测试、基于RSU的定位系统的定位测试， RSU/GNSS/DR组合系统的定位测试的次序，分别进行跑车实验，跑车测试流程如下：
[0207]设备初始化化：包括设置多模GNSS接收机串口号（C0M6)与波特率（4800 )，设置 GNSS数据存储目录，设置基于RSU定位子系统车载DSRC接收机信道(CH182)与数据存储目 录，设置DR子系统的初始位置。
[0208]时间校准:校准GNSS与DSRC接收机的时间基准，使两者时间保持同步；
[0209]定位数据获取:被测试车辆以20公里每小时的速度按土黄色轨迹匀速行驶，按次 序进行车辆定位测试，获取定位数据。
[0210]坐标变换:GNSS输出是经炜度坐标格式，DSRC定位数据是UTM坐标格式。由于子系 统的坐标标准不同，为了更好的解算出车辆坐标，需要实现坐标变换。
[0211]信息融合:使用信息融合算法，利用三个定位子系统的定位输出，解算出车辆坐 标。
[0212] 轨迹显示:把定位系统输出的数据生成KML文件，然后在Google Earth显示出车辆 行驶轨迹。
[0213] 根据以上流程进行跑车实验，测试结果如下：
[0214] (l)GNSS定位结果与分析
[0215] 采用多模卫星定位芯片XN647-8，可以同时支持BDS和GPS，在开阔区域可以捕捉到 8-9颗定位卫星，根据定位模块的输出数据可知，PD0P在开阔区域基本保持在2左右，定位精 度相对较高。根据输出数据统计分析，在区域1，GNSS定位精度（以圆径向误差(2DRMS)来衡 量)达到在2米，如图8(a)-图8(b)所示。
[0216]当车辆进入到区域2时，由于滤波算法的作用，开始十米左右定位还比较正常，但 是进入隧道之后，定位精度骤然下降，从点A到点B这个区间，没有定位结果输出，卫星信号 被完全遮挡。
[0217]当车辆进入到区域3后，由于楼群的影响，从点C到点D，GNSS接收机虽然有车辆位 置信息输出，但是定位精度不高。GNSS接收机输出的H)0D基本上都大于5，定位精度下降。 [0218] (2)基于RSU的定位结果与分析
[0219] 区域2部署了 7个RSU，为了不影响区域1和区域3个定位测试，在测试过程中调整了 RSU的发射功率，使其信号作用范围限定在区域2。车辆从点E行驶到点F，定位精度(2DRMS) 达到2.5米。其他区域由于没有信号，因此无法定位。测试结果如图9(a)-图9(b)所示。
[0220] (3)RSU/GNSS/DR定位结果与分析，如图10(a)-图10(b)所示：
[0221] 从图10(b)可以知道组合定位在区域1与GNSS定位大致相同，差异较小。但是进入 到区域2之后，GNSS子系统被判定为故障子系统，从组合定位系统隔离，在RSU/DR作用下，取 得了较好的定位结果。对比图9(b)，在区域2中，有DR子系统参与定位后，定位精度有一定的 提高，波动幅度减小。在区域3，从点C到点D超出了 RSU通信网络的覆盖范围，RSU子系统被判 定为故障子系统，GNSS/DR作用下，由于DR的初始位置比较精确，前半段定位精度较高，但由 于DR系统的误差累积，后半段被故障检测算法判定为故障系统且被隔离，在这种情况下，只 有GNSS子系统正常工作。
[0222] 从以上定位结果可以看出，组合定位系统达到了设计的目的，满足了车辆在复杂 城市环境下的定位需求，定位精度已经达到道路级的精度要求。
[0223] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的 限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化， 均应为等效的置换方式，都包含在本发明的保护范围之内。
【主权项】
1. 一种新型车辆组合定位算法，其特征在于，采用联邦卡尔曼滤波算法，实现RSU定位、 GNSS定位、DR定位三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系 统的局部滤波器，记为LF1，对应的信息分配系数是i3 1;GNSS定位子系统也采用线性卡尔曼 滤波器作为局部滤波器，记为LF2,对应的信息分配系数为fe;DR定位系统采用扩展卡尔曼 滤波作为局部滤波器，记为LF3，对应的信息分配系数为β 3，主滤波器做信息融合，具体包括 下述步骤： 51、 根据汽车运动学建立系统状态方程与观测方程，所述观测方程包括RSU观测方程、 GNSS观测方程以及DR观测方程； 52、 根据LF1、LF2和LF3对三个子系统进行时间更新和量测更新； 53、 在信息融合过程中进行故障检测，利用两级故障检测技术，检测出故障子系统并实 现隔离； 54、 信息融合与信息分配，系统在信息分配过程中，根据定位子系统的定位精度因子来 动态调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构。2. 根据权利要求1所述的新型车辆组合定位算法，其特征在于，在步骤S4中，所述两级 故障检测的方法:首先各子系统利用残差X 2检测法去检测出现硬故障的子系统，然后利用 冗余硬件检测法去检测系统软故障，另外，为了能校验车载定位系统本身的精确度，还提出 了基于车辆与路侧设备通信V2R的位置参照检测法，让系统可以及时发现因车载设备自身 问题而导致定位不准确的原因。3. 根据权利要求2所述的新型车辆组合定位算法，其特征在于，所述残差X2检测法具体 如下： 由观测值的预测值与实测值Zk之差构造检验统计量的方法称为残差X2检验法，残 差定义为：当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生故障 时，r(k)的期望是不为零的值，根据这个特性，通过检测残差r(k)均值就可以确定被检测系 统是否发生了故障，由公式4々）=Ζ& -= ZA 可知，残差向量r(k)也可以被认 为均值为零的高斯白噪声过程，其协方差S(k)为：= 构造以下故障检测函数： D(k)=rT(k)S_1(k)r(k) 由残差序列的统计特性知，D(k)服从自由度为η的X2分布，η为状态向量X的维数，如果设 定故障检测阀值TD，那么可以根据下述公式检测系统故障，阀值TD决定了故障检测的性能：4. 根据权利要求2所述的新型车辆组合定位算法，其特征在于，所述冗余硬件检测法具 体为： 采用低成本冗余硬件检测，通过增加加速度计对比各子系统所测量的车辆速度的一致 性来实现故障的检测与隔离。5. 根据权利要求2所述的新型车辆组合定位算法，其特征在于，所述位置参照检测法具 体为： 传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统，V2R通信的发 展，在某些特定区域，如停车场，车辆可以通过后台数据库查询到自己所处的精确位置坐 标，利用所查询到的坐标信息与实时定位的坐标相对比，以此来判断车辆定位系统的准确 性。6. 根据权利要求1所述的新型车辆组合定位算法，其特征在于，在步骤S4中，自适应的 信息分配系数调整方法以各子系统的定位精度因子为主要参考，动态调整信息分配系数， 提高联邦滤波器的性能，实现组合导航。7. 根据权利要求1所述的新型车辆组合定位算法，其特征在于，三个子系统在设定初始 参数后，经过时间与量测更新后，得到了进一步预测值，根据残差X 2检验算法对各子系统进 行第一级硬故障检测，剩余无故障子系统再使用冗余硬件故障检测法，对比找出有故障的 系统进行故障隔离，对于余下的无故障子系统，联邦卡尔曼滤波组合算法根据子系统定位 精度因子动态调整信息系分配数，具体如下： (1)有故障子系统的信息分配： 有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种情况 下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系统输 出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论； 第一、基于RSU的定位子系统出现故障，fo = 0,在开阔区域，一般没有部署或少量部署无 法提供定位功能，那么基于RSU定位子系统将会出现故障，系统只剩下GNSS和DR子系统，构 成了 GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的H)OP参数来确定，PDOP通 常用来衡量定位精度的好坏，取值范围从〇. 5到99.9，系数如下述公式所示：第二、GNSS出现故障，β2 = 0,当车辆进入到隧道或者在高楼密集的城市中心区域，卫星 信号被遮挡，GNSS将会出现故障，一般情况下，在这些特殊区域部署RSU基站，为车辆提供定 位服务，此时RSU/DR工作，信息分配系数将按照车载DSRC设备计算的的定位精度因子D0P的 值来确定，信息分配系数如下述公式所示：第三、DR出现故障时，根据冗余硬件实现伪DR系统功能，1)车载里程仪的速度测量出现 异常，取车速计的速度值代替;2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位置信 息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题，如果DR子 系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模块。8.根据权利要求1所述的新型车辆组合定位算法，其特征在于，当无故障子系统的信息 分配时： 不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作，设计 基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题，GNSS 不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之间， 存在GNSS和RSU定位系统两个系统都能正常工作的区域，因此，信息分配系数根据子系统定 位的质量，表现为GNSS的H)OP和RSU的定位精度因子，来调整队的值，一般情况下，如果GNSS 的PD0P小于等于2或RSU的D0P大于等于0.9时，将以这两个系统的定位信息为主，分情形讨 论； 1) 当GNSS的PD0P〈 = 2且RSU的D0P〈0 · 9时，β2 = 0 · 99，βι = β3 = (1_&) /2; 2) 当GNSS的PD0P>2 且 RSU的D0P> = 0 · 9时，βι = DOP，β2 = fo = (1_βι) /2; 3) 其他情况，々=片 / 2. /J2 = /J; / 2 ,β3 = 1-^-022; 其中：系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自适应子系统定位 精度的变化。
【文档编号】G01S19/47GK105866812SQ201610178772
【公开日】2016年8月17日
【申请日】2016年3月24日
【发明人】刘建圻, 闫荷花, 邹才凤, 张严林
【申请人】广东机电职业技术学院