CN105866812B - 一种车辆组合定位算法 - Google Patents

Abstract

本发明公开了一种新型车辆组合定位算法，采用联邦卡尔曼滤波数据融合技术，实现路侧设备定位(RSU)、卫星定位(GNSS)、航位推算(DR)三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β₁；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β₂；DR定位子系统采用扩展卡尔曼滤波器作为局部滤波器，记为LF3，对应的信息分配系数为β₃，主滤波器做信息融合；利用残差χ²检测和冗余硬件检测构建了两级故障检测方法，能同时检测出子系统的硬故障和软故障；同时根据定位子系统的定位精度因子来动态调整组合系统的信息分配系数，使系统能够快速适应环境的变化，提高车辆定位的精度与可靠性。

Description

一种车辆组合定位算法

技术领域

本发明涉及车辆定位的研究领域，特别涉及一种车辆组合定位算法。

背景技术

卫星定位具有高精度、低成本、易用性等特点，只要能接收到卫星信号的地方，卫星定位接收机都可以解算出车辆坐标，而且误差不会累积。但是在城市复杂环境中，由于卫星信号会被建筑物遮挡，卫星定位精度下降。卫星定位系统单独使用时，系统定位可靠性较低。航位推算(DR)定位系统是常用的车辆自主定位方法，只要给定车辆初始位置，就可以利用车辆的行驶速度和行进方向来推算出车辆的当前位置，在短时间内能提供高精度的定位服务。如果DR定位系统的初始位置无法得到周期性的更新或校准，由于其误差累积特性，定位可靠性下降。RSU定位方法随着车联自组织网络(VANET)的发展得到了应用，利用车辆与RSU的通信，可以不依赖卫星的情况下为车辆提供定位，在密集的建筑群中部署RSU可以实现车辆定位。定位精度与车辆所在位置的RSU部署密集程度和无线信号是否被遮挡有关，车辆接收到的RSU的定位信号越多且遮挡越少，系统定位精度越高。在城市中心高楼密集区域或者地下停车场部署RSU可以有效地解决卫星定位失效区域车辆定位问题。以上三种系统各有优劣，面对复杂的城市环境，单一系统都无法满足现在车联网应用的定位需求，利用信息融合技术提高定位系统的可靠性和精度，将是解决车辆定位的可行方法。

如果这三个系统采用集中式卡尔曼滤波来实现信息融合，将会产生两个问题：第一、计算负担重。集中式滤波器对处理器的计算能力要求较高，车载嵌入式处理器无法满足计算的实时性要求；第二、容错性能差，不利于故障诊断。GNSS定位系统在有卫星信号的情况下实现定位，而基于RSU的定位系统则刚好相反，在卫星定位失效区域部署RSU为车辆提供定位服务。当任一子系统发生故障，如果不能快速地检测出来，组合定位系统就会被故障子系统污染，使输出的位置信息不可靠。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种车辆组合定位算法，采用联邦卡尔曼滤波(FKF)组合定位算法，实现车辆的高精度定位。

为了达到上述目的，本发明采用以下技术方案：

本发明一种车辆组合定位算法，采用联邦卡尔曼滤波算法，实现RSU定位、GNSS定位、DR定位三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β₁；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β₂；DR定位系统采用扩展卡尔曼滤波作为局部滤波器，记为LF3，对应的信息分配系数为β₃，主滤波器做信息融合，具体包括下述步骤：

S1、根据汽车运动学建立系统状态方程与观测方程，所述观测方程包括RSU观测方程、GNSS观测方程以及DR观测方程；

S2、根据LF1、LF2和LF3对三个子系统进行时间更新和量测更新；

S3、在信息融合过程中进行故障检测，利用两级故障检测技术，检测出故障子系统并实现隔离；

S4、信息融合与信息分配，系统在信息分配过程中，根据定位子系统的定位精度因子来动态调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构。

作为优选的技术方案，在步骤S4中，所述两级故障检测的方法：首先各子系统利用残差χ²检测法去检测出现硬故障的子系统，然后利用冗余硬件检测法去检测系统软故障，另外，为了能校验车载定位系统本身的精确度，还提出了基于车辆与路侧设备通信V2R的位置参照检测法，让系统可以及时发现因车载设备自身问题而导致定位不准确的原因。

作为优选的技术方案，所述残差χ²检测法具体如下：

由观测值的预测值与实测值Z_k之差构造检验统计量的方法称为残差χ²检验法，残差定义为：

当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生故障时，r(k)的期望是不为零的值，根据这个特性，通过检测残差r(k)均值就可以确定被检测系统是否发生了故障，由公式可知，残差向量r(k)也可以被认为均值为零的高斯白噪声过程，其协方差S(k)为：

构造以下故障检测函数：

D(k)＝r^T(k)S^-1(k)r(k)

由残差序列的统计特性知，D(k)服从自由度为n的χ²分布，n为状态向量X的维数，如果设定故障检测阀值T_D，那么可以根据下述公式检测系统故障，阀值T_D决定了故障检测的性能：

作为优选的技术方案，所述冗余硬件检测法具体为：

采用低成本冗余硬件检测，通过增加加速度计对比各子系统所测量的车辆速度的一致性来实现故障的检测与隔离。5、根据权利要求2所述的车辆组合定位算法，其特征在于，所述位置参照检测法具体为：

传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统，V2R通信的发展，在某些特定区域，如停车场，车辆可以通过后台数据库查询到自己所处的精确位置坐标，利用所查询到的坐标信息与实时定位的坐标相对比，以此来判断车辆定位系统的准确性。

作为优选的技术方案，在步骤S4中，自适应的信息分配系数调整方法以各子系统的定位精度因子为主要参考，动态调整信息分配系数，提高联邦滤波器的性能，实现组合导航。

作为优选的技术方案，三个子系统在设定初始参数后，经过时间与量测更新后，得到了进一步预测值，根据残差χ²检验算法对各子系统进行第一级硬故障检测，剩余无故障子系统再使用冗余硬件故障检测法，对比找出有故障的系统进行故障隔离，对于余下的无故障子系统，联邦卡尔曼滤波组合算法根据子系统定位精度因子动态调整信息系分配数，具体如下：

(1)有故障子系统的信息分配：

有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种情况下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系统输出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论；

第一、基于RSU的定位子系统出现故障，β₁＝0，在开阔区域，一般没有部署或少量部署无法提供定位功能，那么基于RSU定位子系统将会出现故障，系统只剩下GNSS和DR子系统，构成了GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的PDOP参数来确定，PDOP通常用来衡量定位精度的好坏，取值范围从0.5到99.9，系数如下述公式所示：

β₃＝1-β₂

第二、GNSS出现故障，β₂＝0，当车辆进入到隧道或者在高楼密集的城市中心区域，卫星信号被遮挡，GNSS将会出现故障，一般情况下，在这些特殊区域部署RSU基站，为车辆提供定位服务，此时RSU/DR工作，信息分配系数将按照车载DSRC设备计算的的定位精度因子DOP的值来确定，信息分配系数如下述公式所示：

β₃＝1-β₁

第三、DR出现故障时，根据冗余硬件实现伪DR系统功能，1)车载里程仪的速度测量出现异常，取车速计的速度值代替；2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位置信息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题，如果DR子系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模块。

作为优选的技术方案，当无故障子系统的信息分配时：

不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作，设计基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题，GNSS不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之间，存在GNSS和RSU定位系统两个系统都能正常工作的区域，因此，信息分配系数根据子系统定位的质量，表现为GNSS的PDOP和RSU的定位精度因子，来调整β_i的值，一般情况下，如果GNSS的PDOP小于等于2或RSU的DOP大于等于0.9时，将以这两个系统的定位信息为主，分情形讨论；

1)当GNSS的PDOP<＝2且RSU的DOP<0.9时，β₂＝0.99，β₁＝β₃＝(1-β₂)/2；

2)当GNSS的PDOP>2且RSU的DOP>＝0.9时，β₁＝DOP，β₂＝β₃＝(1-β₁)/2；

3)其他情况，β₃＝1-β₁-β₂；

其中，

系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自适应子系统定位精度的变化。

本发明与现有技术相比，具有如下优点和有益效果：

本发明利用联邦卡尔曼滤波算法实现了基于RSU的定位子系统，GNSS定位子系统和DR子系统的信息融合；利用残差χ²检测法和冗余硬件检测法构建了两级故障检测方法，能同时检测出系统的硬故障和软故障；根据子系统的定位质量因子来动态调整组合系统的信息分配系数，使系统能够适应环境的变化。与其他组合定位系统具有如下优势：

(1)环境适应能力较强，可靠性高。国内大城市越来越繁华，越来越密集，卫星定位在这样的环境下无法工作，组合定位算法可以适应，加上可靠的故障检测和自适应的信息分配系数调整方法，系统可靠性更高。

(2)具有车-车、车-路通信能力。在停车场、隧道内也可以通信，弥补了3G/4G网络的缺陷。

附图说明

图1是多模卫星定位系统定位原理图；

图2是RSU定位原理与坐标转换；

图3是RSU协同定位示意图；

图4是RSU/GNSS/DRS组合定位系统框架图；

图5是位置参照检测示意图；

图6是多级故障检测与信息分配流程图；

图7是测试场景示意图；

图8(a)–图8(b)是GNSS定位结果；

图9(a)–图9(b)是基于RSU的子系统定位结果；

图10(a)–图10(b)是基于RSU/GNSS/DR组合定位结果与对比图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

本发明采用联邦卡尔曼滤波(FKF)组合定位算法，实现车辆的高精度定位。首先介绍三个子系统的定位原理；然后详细讨论RSU/GNSS/DR组合定位算法和两级故障检测方法，并提出了自适应信息分配系数动态调整方法；最后利用跑车实验验证了所提出的组合定位算法的可行性。

1、车辆组合定位系统概述

GPS是建成最早也是应用最广泛的全球定位系统。BDS是我国自行建设的卫星系统，随着新一代的导航卫星的发射，标志着BDS迈入了全球部署阶段，GLONASS虽然建设早，但是由于俄罗斯的财力，逐渐被BDS所超越。Galileo是最精确的卫星定位系统，在设计初就考虑与GPS的兼容问题，但是进展最慢。单一系统已经不能满足高精度的定位需求，支持多个卫星定位系统的多模卫星接收机由于其高性价比逐渐成为GNSS的主流。多模GNSS定位精度比单一卫星定位系统有较大的提高，但是由于卫星信号容易被遮挡，DR系统仍然是其重要的辅助部分。有了DR系统的辅助，GNSS/DR组合定位可以解决短时间内卫星信号被遮挡环境下的定位，如果GNSS长时间无法获取自身位置而导致DR系统无法重置其初始位置，由于误差累积会导致系统总体定位精度大幅下降，如在香港的部分街区，系统长时间捕捉不到足够的卫星信号。基于RSU的地面定位系统在类似这样的环境下将可以为GNSS/DR系统提供很好的补充，因此RSU/GNSS/DR组合定位应运而生。

1.1、GNSS定位原理

随着中国北斗系统逐步投入使用，BDS与GPS组合定位成为研究热点。采用支持BDS和GPS导航系统一体化SOC芯片，多系统的融合不是对单模定位结果做选择，而是同时利用多个系统的卫星播发的信息，提取相关原始测量值，并将原始测量单元做数据融合从而解算出定位位置，如图1所示。因此多模工作模式不是定位结果的数据融合，而是多卫星定位系统的原始测量单元的数据融合过程。总而言之，多模卫星定位是卫星接收机发展的趋势。

1.2、航位推算原理

航位推算(DR)系统是车辆定位导航中采用的一种比较经典的完全自主的导航系统，在二维的空间上给车辆进行定位。假定汽车的初始点(x₀,y₀)和初始航向角θ₀，通过车载里程仪、角速率陀螺实时测量车辆的行驶距离和航向角的变化，推算出车辆的位置。

具体算法如下：

车辆在t_k时刻的位置同样可以表示为：

其中，(x₀,y₀)是车辆的初始位置，d_i和θ_i分别是车辆从t_i时刻到t_i+1时刻的位移矢量的长度和绝对航向。假定采样周期是恒定的(如1秒1次)，那么公式(1.3)可以表示为：

从以上讨论可知，航位推算是一个基于初始位置逐步累加的过程，需要实时得到一段时间内的距离变化量d_i和航向角θ_i，还必须确定初始位置。因此，不同时刻的测量误差和计算误差都会累积起来，需要周期性地重置初始位置。总之，单一的DR系统不适合用于长时间的定位。

1.3、基于RSU的定位原理与RSU自定位

利用时间间隔测量模块测量无线信号从车辆到RSU的飞行时间，再乘以无线电波的传播速度，可以得到车辆与RSU之间的距离。

当n≥3时，利用EKF就可以解算出车辆的坐标，然后完成坐标变换，如图2所示。

根据算法要求RSU的坐标需提前确定，所以要解决RSU自身的定位问题。如果RSU坐标不准确，就无法为车辆提供位置参考。因此，部署RSU时，在RSU中存储其自身的位置信息(如经纬度)，利用经纬度推算出UTM坐标(E,N,Zone,Hemi)。RSU设备带有GNSS定位模块可以自定位，可以解决开阔区域的RSU自定位问题。如果部署在城市中心区或地下停车场时，RSU的自身位置需要在部署时利用协同定位方法确定自身位置然后存储在RSU中，如图3所示。

在地下停车场附近可以接收到GNSS信号的地方部署RSU₅、RSU₆、RSU₇，然后利用OSS-TWR测距算法和扩展卡尔曼滤波算法，可以解算出RSU₃的坐标，最后再依次解算出其他3个RSU的坐标，经过多次迭代，RSU_1-4的坐标将会更精确。

1.4、FKF辅助的RSU/GNSS/DR组合定位算法

面对复杂的城市环境，需要多种定位技术融合才能实现高精度、高可靠的车辆定位。集中式卡尔曼滤波组合算法由于计算量大、故障分离难等问题，在多维度的信息融合中略有不足。本发明采用联邦卡尔曼滤波算法，实现基于RSU的定位、卫星定位、航位推算三者的信息融合，为车辆提供准确的车辆位置服务。基于RSU的定位系统采用线性卡尔曼滤波器作为局部滤波器，记为LF1，对应的信息分配系数是β₁；GNSS定位系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β₂；DR定位系统采用扩展卡尔曼滤波作为局部滤波器器，记为LF3，对应的信息分配系数为β₃；主滤波器做信息融合，所设计的RSU/GNSS/DR组合定位系统框架如图4所示。

2.、系统状态方程与观测方程

采用“当前”统计模型来描述车辆加速度的统计分布，当车辆正以某一加速度机动时，它在下一瞬时的加速度取值范围是有限的，而且只能在当前加速度的邻域内。根据上述讨论可知

其中，a₁(t)为机动加速度，其方差为σ²，为机动加速度“当前”均值，其中在每一采样周期内为常数。a(t)为均值为零的有色加速度噪声，w(t)是均值为零的高斯白噪声，τ为机动加速度变化率的相关时间常数。

取组合定位整体状态变量为X＝[x_e,v_e,a_e,x_n,v_n,a_n]^T，其中x_e，x_n分别是车辆东向和北向的位置分量(单位：m)，v_e，v_n分别是车辆东向和北向的速度分量(单位：m/s)，a_e，a_n分别是车辆东向和北向的加速度分量(单位：m/s²)。系统的状态方程写成矩阵形式为：

其中，

是均值为0，方差分别为的高斯白噪声，分别是车辆东向和北向机动加速度变化率的相关时间常数；分别为车辆东向和北向机动加速度分量的“当前”均值。

设采样周期为T，将系统连续的状态方程离散化，得到离散的系统状态方程为

X(k)＝Φ(k/k-1)X(k/k-1)+U(k)+W(k) (1.8)

其中，X(k)＝[x_e(k),v_e(k),a_e(k),x_n(k),v_n(k),a_n(k)]^T

Φ(k/k-1)＝diag[Φ_e(k/k-1),Φ_n(k/k-1)] (1.9)

设则Φ_e(k/k-1)和Φ_n(k/k-1)为

U(k)＝[u₁ u₂ u₃ u₄ u₅ u₆]^T (1.10)

其中，

(1)RSU观测方程

对于基于RSU的定位子系统，取系统状态变量X₁＝X，系统状态方程与总体系统状态方程相同。其观测量为子系统定位输出的车辆东向和北向位置坐标分量e_obs，n_obs(单位：m)，观测方程可表示为：

Z₁(k)＝H₁(k)X₁(k)+V₁(k) (1.11)

其中，

w_e(k)和w_n(k)是均值为0，方差分别为的高斯白噪声序列，表示在RSU定位设备在东向和北向的位置测量噪声，量测噪声协方差矩阵

(2)GNSS观测方程

对于GNSS定位子系统与RSU定位系统类似，取系统状态变量X₂＝X，与总体系统方程相同。其观测量为GNSS接收机输出的车辆东向和北向位置坐标分量e_obs，n_obs，观测方程可表示为：

Z₂(k)＝H₂(k)X₂(k)+V₂(k) (1.12)

其中，

w_e(k)和w_n(k)是卫星定位接收机在东向和北向的噪声误差，其均值为0，方差分别为的高斯白噪声序列，量测噪声协方差矩阵：

(3)DR观测方程

航位推算系统的状态变量为X₃＝X，子系统状态方程与总体系统状态方程相同，其观测量取角速度陀螺仪的输出ω和里程表输出的车辆在采用周期T时间内行进的距离s，即系统的观测方程可表示为：

Z₃(t)＝h₃(t,X₃(t))+V₃(t) (1.13)

其中，

化简后为，

假定标定系数为1，ε_ω为陀螺的漂移，近似为的高斯白噪声，ε_s为里程表输出量s的观测噪声，近似为的高斯白噪声，量测噪声协方差矩阵：

将观测方程离散化，得到系统离散的观测方程为

Z₃(k)＝h₃[k,X₃(k)]+V₃(k) (1.14)

局部滤波器采用扩展卡尔曼滤波，将h₃[X₃(k)]在附近展开为泰勒级数，忽略二次以上的高次项，可得观测方程

其中，

3.子系统的时间更新与量测更新

LF1和LF2子系统时间更新与量测更新方程

P_i(k/k+1)＝Φ(k/k-1)P_i(k-1)Φ^T(k/k-1)+Q(k-1) (1.17)

K_i(k)＝P_i(k/k-1)H_i ^T(k)[H_i(k)P_i(k/k-1)H_i ^T(k)+R_i(k)]^-1 (1.18)

P_i(k)＝[1-K_i(k)H_i(k)]P_i(k/k-1) (1.20)

状态转移矩阵：把加速度的一步预测看做“当前”加速度的均值，即

公式(1.20)可以简化为

其中，Φ(k/k-1)＝diag[Φ_e(T),Φ_n(T)]，即

系统噪声协方差矩阵：Q(k-1)为系统噪声协方差矩阵Q的离散化矩阵

其中，

由于采用了扩展卡尔曼滤波作为LF3的局部滤波器，其推导方程与LF1和LF2略有不同，如公式(1.22)所示，其中，系统噪声协方差矩阵Q(k-1)和状态转移矩阵Φ(k/k-1)与LF1和LF2相同。

4、全局信息融合与信息分配

全局信息融合：

信息分配：

其中，β₁+β₂+β₃＝1。

5、两级故障检测与信息分配新方法

车联网应用对定位的容错性和可靠性提出了更高的要求，故障检测与故障隔离是提高系统可靠性的有效途径。残差χ²检验法被认为是比较可靠的检测方法，在检测系统硬故障具有灵敏度高，可靠性好的特点，但是对软故障检测的有效性下降。

超大集成电路技术的进步和成本的下降，为组合定位系统的冗余硬件容错设计提供了可能性，本系统增加一个加速度计ADXL345测量车辆速度，对系统的故障进行检测。残差χ²故障检测和冗余硬件故障检测为组合定位算法构建了一个两级故障检测新方法。

另外，现有的车载定位设备暂时还没有方法去检测和校正自身设备的定位准确性，基于V2R通信，本发明提出了一个操作简单的基于位置参照的检测方法，用于校验车载设备GNSS定位精度和基于RSU的定位精度，有利于及早发现车载定位设备本身的问题。

组合定位系统利用两级故障检测技术，可以把系统软硬故障都检测出来。系统在信息融合时，合理调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构。本发明提出了一种新的自适应系数调整方法，构建具有高容错性的联邦组合定位算法。

5.1、两级故障检测方法

本系统采用两级故障检测方法，首先各子系统利用残差χ²检测法去检测出现硬故障的子系统，然后利用冗余硬件检测法去检测系统软故障。另外，为了能校验车载定位系统本身的精确度，本发明提出了基于V2R通信的位置参照检测法，让系统可以及时发现因车载设备自身问题而导致定位不准确的原因。

(1)残差χ²检测法

由观测值的预测值与实测值Z_k之差构造检验统计量的方法称为残差χ²检验法。残差定义为：

当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生故障时，r(k)的期望是不为零的值。根据这个特性，通过检测残差r(k)均值就可以确定被检测系统是否发生了故障。由公式(1.25)可知，残差向量r(k)也可以被认为均值为零的高斯白噪声过程，其协方差S(k)为：

构造以下故障检测函数：

D(k)＝r^T(k)S^-1(k)r(k) (1.27)

由残差序列的统计特性知，D(k)服从自由度为n(状态向量X的维数)的χ²分布，如果设定故障检测阀值T_D，那么可以根据公式(1.32)检测系统故障，阀值T_D决定了故障检测的性能。

(2)冗余硬件检测法

采用低成本冗余硬件检测，通过增加ADXL345(市场价格7元人民币，对总体成本影响很小)对比各子系统所测量的车辆速度的一致性来实现故障的检测与隔离。本系统中里程仪、加速度计、基于RSU的定位系统、卫星导航这四个系统可以测量一个周期内的车辆速度，按照少数服从多数的原则来实现检测与隔离。冗余硬件检测法可以有效解决软故障检测问题。

(3)位置参照检测法

传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统。V2R通信的发展，在某些特定区域，车辆可以通过后台查询到自己所处的位置，如图5所示。

在停车位，车辆通过与RSU(咪表)设备通信自动完成停车交费，在这个过程中，RSU可以向车辆发送本车位的精确位置信息，车辆利用这个信息与车载定位系统提供的位置信息的一致性来判断车载定位系统是否准确。新带来的问题就是现有的停车位位置不可知。事实上，随着IoV的不断推进，全市的停车位会纳入城市智能停车场管理系统进行统一管理和查询，任何一个停车位都有精确的位置信息，并且清楚显示在GIS地图上。

综上所述，由于车载里程仪、加速度计等检测设备性能下降或外部环境如温湿度的影响，子系统的可能发生故障，这些故障既有突变的硬故障也有缓慢变化的软故障。残差χ²检验法由于计算相对简单，对子系统硬故障的检验非常有效，因此，在组合导航系统中应用广泛。基于硬件冗余的检测方法，可以有效地检测出系统的软故障，但是会增加系统的成本。采用成本低的硬件进行部分冗余，可以兼顾成本和性能，提高系统可靠性。利用位置信息已知的停车位给车辆定位系统提供检测参照，是车联网发展到现在的附加功能，它可以检测出定位系统中存在的一些利用自身无法确定的故障，相对前面两种来说，检测结果更客观。检测结果虽然无法直接用于组合定位算法中，如参与信息分配，但是可以用于定位设备维护，或给DR子系统设置初始位置等。

5.2、自适应的信息分配系数调整方法

最早Carlson提出的固定比例的信息分配原则，考虑到复杂城市环境中各个子滤波器性能和位置估计准确度在不断地变化，因此引入动态自适应信息分配的方法，以各子系统的定位精度为主要参考，动态调整信息分配系数β_i，改变联邦滤波器的性能，实现组合导航，主要工作流程如图6所示。

三个子系统在设定初始参数后，经过时间与量测更新后，得到了进一步预测值，根据残差χ²检验算法对各子系统进行第一级硬故障检测。剩余无故障子系统再使用冗余硬件故障检测法，对比找出有故障的系统进行故障隔离。对于余下的无故障子系统，联邦卡尔曼滤波组合算法根据子系统定位精度因子动态调整信息系分配数，具体如下：

(1)有故障子系统的信息分配

有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种情况下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系统输出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论。

第一、基于RSU的定位子系统出现故障(β₁＝0)。在开阔区域，一般没有部署或少量部署无法提供定位功能，那么基于RSU定位子系统将会出现故障。系统只剩下GNSS和DR子系统，构成了GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的PDOP参数来确定，PDOP通常用来衡量定位精度的好坏，取值范围从0.5到99.9，系数如公式(1.29)所示。

β₃＝1-β₂

第二、GNSS出现故障(β₂＝0)。当车辆进入到隧道或者在高楼密集的城市中心区域，卫星信号被遮挡，GNSS将会出现故障。一般情况下，在这些特殊区域部署RSU基站，为车辆提供定位服务，此时RSU/DR工作。信息分配系数将按照车载DSRC设备计算的的定位精度因子(DOP)的值来确定，信息分配系数如公式(1.30)所示。

β₃＝1-β₁

第三、DR出现故障时，根据冗余硬件实现伪DR系统功能。1)车载里程仪的速度测量出现异常，取车速计的速度值代替；2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位置信息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题。如果DR子系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模块。

(2)无故障子系统的信息分配

不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作。设计基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题，GNSS不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之间，存在GNSS和RSU定位系统两个系统都能正常工作的区域。因此，信息分配系数根据子系统定位的质量，表现为GNSS的PDOP和RSU的定位精度因子，来调整β_i的值。一般情况下，如果GNSS的PDOP小于等于2或RSU的DOP大于等于0.9时，将以这两个系统的定位信息为主，分情形讨论。

1)当GNSS的PDOP<＝2且RSU的DOP<0.9时，β₂＝0.99，β₁＝β₃＝(1-β₂)/2；

2)当GNSS的PDOP>2且RSU的DOP>＝0.9时，β₁＝DOP，β₂＝β₃＝(1-β₁)/2；

3)其他情况，β₃＝1-β₁-β₂；

其中，

综上所述，系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自适应子系统定位精度的变化，提高了系统的可靠性。

6、实验与结果分析

车联网基于位置服务的应用很多，除了传统的车辆导航服务外，还出现了很多新的应用：市区车位预约、自动寻位泊车、广告推送、特种车辆实时监控。以特种车辆实时监控为例，传统方法只依靠GPS接收机来获取车辆位置，然后通过3G/4G网络把车辆的位置(包含车辆状态信息)发送到监控中心，监控端通过GIS系统辅助可以对车辆进行实时跟踪及轨迹回放。但是弊端是进入城市中心区域或稍长的隧道，就无法对车辆进行定位。基于RSU/GNSS/DR组合定位算法的定位解决方案，可以在城市中不同的环境中为车辆提供定位服务，还可以通过V2R通信把车辆当前的状态实时传送到监控端。

如图7所示，区域1没有部署RSU，在区域1中主要测试多模GNSS子系统在开阔区域的定位精度，在区域2部署了7个RSU，测试在GNSS失效的情况下，RSU/DR定位的效果，区域3也没有部署RSU，测试在楼群之间组合定位RSU子系统出现故障时GNSS/DR的定位效果。

6.1、车辆定位及结果分析

根据预设路线，按照GNSS单一系统定位测试、基于RSU的定位系统的定位测试，RSU/GNSS/DR组合系统的定位测试的次序，分别进行跑车实验，跑车测试流程如下：

设备初始化化：包括设置多模GNSS接收机串口号(COM6)与波特率(4800)，设置GNSS数据存储目录，设置基于RSU定位子系统车载DSRC接收机信道(CH182)与数据存储目录，设置DR子系统的初始位置。

时间校准：校准GNSS与DSRC接收机的时间基准，使两者时间保持同步；

定位数据获取：被测试车辆以20公里每小时的速度按土黄色轨迹匀速行驶，按次序进行车辆定位测试，获取定位数据。

坐标变换：GNSS输出是经纬度坐标格式，DSRC定位数据是UTM坐标格式。由于子系统的坐标标准不同，为了更好的解算出车辆坐标，需要实现坐标变换。

信息融合：使用信息融合算法，利用三个定位子系统的定位输出，解算出车辆坐标。

轨迹显示：把定位系统输出的数据生成KML文件，然后在Google Earth显示出车辆行驶轨迹。

根据以上流程进行跑车实验，测试结果如下：

(1)GNSS定位结果与分析

采用多模卫星定位芯片XN647-8，可以同时支持BDS和GPS，在开阔区域可以捕捉到8-9颗定位卫星，根据定位模块的输出数据可知，PDOP在开阔区域基本保持在2左右，定位精度相对较高。根据输出数据统计分析，在区域1，GNSS定位精度(以圆径向误差(2DRMS)来衡量)达到在2米，如图8(a)-图8(b)所示。

当车辆进入到区域2时，由于滤波算法的作用，开始十米左右定位还比较正常，但是进入隧道之后，定位精度骤然下降，从点A到点B这个区间，没有定位结果输出，卫星信号被完全遮挡。

当车辆进入到区域3后，由于楼群的影响，从点C到点D，GNSS接收机虽然有车辆位置信息输出，但是定位精度不高。GNSS接收机输出的PDOD基本上都大于5，定位精度下降。

(2)基于RSU的定位结果与分析

区域2部署了7个RSU，为了不影响区域1和区域3个定位测试，在测试过程中调整了RSU的发射功率，使其信号作用范围限定在区域2。车辆从点E行驶到点F，定位精度(2DRMS)达到2.5米。其他区域由于没有信号，因此无法定位。测试结果如图9(a)-图9(b)所示。

(3)RSU/GNSS/DR定位结果与分析,如图10(a)-图10(b)所示：

从图10(b)可以知道组合定位在区域1与GNSS定位大致相同，差异较小。但是进入到区域2之后，GNSS子系统被判定为故障子系统，从组合定位系统隔离，在RSU/DR作用下，取得了较好的定位结果。对比图9(b)，在区域2中，有DR子系统参与定位后，定位精度有一定的提高，波动幅度减小。在区域3，从点C到点D超出了RSU通信网络的覆盖范围，RSU子系统被判定为故障子系统，GNSS/DR作用下，由于DR的初始位置比较精确，前半段定位精度较高，但由于DR系统的误差累积，后半段被故障检测算法判定为故障系统且被隔离，在这种情况下，只有GNSS子系统正常工作。

从以上定位结果可以看出，组合定位系统达到了设计的目的，满足了车辆在复杂城市环境下的定位需求，定位精度已经达到道路级的精度要求。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种车辆组合定位算法，其特征在于，采用联邦卡尔曼滤波算法，实现RSU定位、GNSS定位、DR定位三个定位子系统的信息融合，即采用线性卡尔曼滤波器作为RSU定位子系统的局部滤波器，记为LF1，对应的信息分配系数是β₁；GNSS定位子系统也采用线性卡尔曼滤波器作为局部滤波器，记为LF2，对应的信息分配系数为β₂；DR定位系统采用扩展卡尔曼滤波作为局部滤波器，记为LF3，对应的信息分配系数为β₃，主滤波器做信息融合，具体包括下述步骤：

S1、根据汽车运动学建立系统状态方程与观测方程，所述观测方程包括RSU观测方程、GNSS观测方程以及DR观测方程；

S2、根据LF1、LF2和LF3对三个子系统进行时间更新和量测更新；

S3、在信息融合过程中进行故障检测，利用两级故障检测技术，检测出故障子系统并实现隔离；

S4、信息融合与信息分配，系统在信息分配过程中，根据定位子系统的定位精度因子来动态调整各子系统的信息分配系数，实现故障子系统的隔离与无故障子系统的重构；

在步骤S4中，所述两级故障检测的方法：首先各子系统利用残差χ²检测法去检测出现硬故障的子系统，然后利用冗余硬件检测法去检测系统软故障，另外，为了能校验车载定位系统本身的精确度，还提出了基于车辆与路侧设备通信V2R的位置参照检测法，让系统可以及时发现因车载设备自身问题而导致定位不准确的原因。

2.根据权利要求1所述的车辆组合定位算法，其特征在于，所述残差χ²检测法具体如下：

由观测值的预测值与实测值Z_k之差构造检验统计量的方法称为残差χ²检验法，残差定义为：

当被检测系统无故障时，残差r(k)是均值为零的高斯白噪声，当被检测系统发生故障时，r(k)的期望是不为零的值，根据这个特性，通过检测残差r(k)均值就可以确定被检测系统是否发生了故障，由公式可知，残差向量r(k)也可以被认为均值为零的高斯白噪声过程，其协方差S(k)为：

构造以下故障检测函数：

D(k)＝r^T(k)S^-1(k)r(k)

由残差序列的统计特性知，D(k)服从自由度为n的χ²分布，n为状态向量X的维数，如果设定故障检测阀值T_D，那么可以根据下述公式检测系统故障，阀值T_D决定了故障检测的性能：

3.根据权利要求1所述的车辆组合定位算法，其特征在于，所述冗余硬件检测法具体为：

采用低成本冗余硬件检测，通过增加加速度计对比各子系统所测量的车辆速度的一致性来实现故障的检测与隔离。

4.根据权利要求1所述的车辆组合定位算法，其特征在于，所述位置参照检测法具体为：

传统车载定位通常接收卫星信号定位或自主定位，是一个孤立的系统，V2R通信的发展，在某些特定区域，如停车场，车辆可以通过后台数据库查询到自己所处的精确位置坐标，利用所查询到的坐标信息与实时定位的坐标相对比，以此来判断车辆定位系统的准确性。

5.根据权利要求1所述的车辆组合定位算法，其特征在于，在步骤S4中，自适应的信息分配系数调整方法以各子系统的定位精度因子为主要参考，动态调整信息分配系数，提高联邦滤波器的性能，实现组合导航。

6.根据权利要求1所述的车辆组合定位算法，其特征在于，三个子系统在设定初始参数后，经过时间与量测更新后，得到了进一步预测值，根据残差χ²检验算法对各子系统进行第一级硬故障检测，剩余无故障子系统再使用冗余硬件故障检测法，对比找出有故障的系统进行故障隔离，对于余下的无故障子系统，联邦卡尔曼滤波组合算法根据子系统定位精度因子动态调整信息系分配数，具体如下：

(1)有故障子系统的信息分配：

有故障子系统的情况一般有以下三种：1)极端情况是3个子系统都出现故障，这种情况下没有位置信息输出；2)当2个子系统出现故障时，把剩下的子系统的融合结果作为系统输出；3)只有1个子系统有故障，根据不同的故障子系统的定位精度分别进行讨论；

第一、基于RSU的定位子系统出现故障，β₁＝0，在开阔区域，一般没有部署或少量部署无法提供定位功能，那么基于RSU定位子系统将会出现故障，系统只剩下GNSS和DR子系统，构成了GNSS/DR的组合定位，信息分配系数将按照GNSS接收机给定的PDOP参数来确定，PDOP通常用来衡量定位精度的好坏，取值范围从0.5到99.9，系数如下述公式所示：

β₃＝1-β₂

第二、GNSS出现故障，β₂＝0，当车辆进入到隧道或者在高楼密集的城市中心区域，卫星信号被遮挡，GNSS将会出现故障，一般情况下，在这些特殊区域部署RSU基站，为车辆提供定位服务，此时RSU/DR工作，信息分配系数将按照车载DSRC设备计算的的定位精度因子DOP的值来确定，信息分配系数如下述公式所示：

β₃＝1-β₁

第三、DR出现故障时，根据冗余硬件实现伪DR系统功能，1)车载里程仪的速度测量出现异常，取车速计的速度值代替；2)陀螺仪的角速度的值出现故障，将采用GNSS或RSU位置信息推算出来的航向角代替，采用伪DR可以短时间解决DR子系统出现故障的问题，如果DR子系统出现硬件故障，可以采用位置参照检测法检测，及早发现问题，更换相应电路模块。

7.根据权利要求1所述的车辆组合定位算法，其特征在于，当无故障子系统的信息分配时：

不存在无故障子系统时，基于RSU的定位系统，GNSS和DR三个子系统都正常工作，设计基于RSU的定位系统的出发点是为了解决卫星定位信号被遮挡区域的车辆定位问题，GNSS不能正常工作的区域才部署RSU，但是在GNSS正常工作区域和RSU定位系统工作区域之间，存在GNSS和RSU定位系统两个系统都能正常工作的区域，因此，信息分配系数根据子系统定位的质量，表现为GNSS的PDOP和RSU的定位精度因子，来调整β_i的值，一般情况下，如果GNSS的PDOP小于等于2或RSU的DOP大于等于0.9时，将以这两个系统的定位信息为主，分情形讨论；

1)当GNSS的PDOP<＝2且RSU的DOP<0.9时，β₂＝0.99，β₁＝β₃＝(1-β₂)/2；

2)当GNSS的PDOP>2且RSU的DOP>＝0.9时，β₁＝DOP，β₂＝β₃＝(1-β₁)/2；

3)其他情况，β₃＝1-β₁-β₂；

其中，

系统根据不同的情况可以得到合理的信息分配系数，组合定位系统自适应子系统定位精度的变化。