CN115291253B - 一种基于残差检测的车辆定位完好性监测方法及系统 - Google Patents
一种基于残差检测的车辆定位完好性监测方法及系统 Download PDFInfo
- Publication number
- CN115291253B CN115291253B CN202210921597.8A CN202210921597A CN115291253B CN 115291253 B CN115291253 B CN 115291253B CN 202210921597 A CN202210921597 A CN 202210921597A CN 115291253 B CN115291253 B CN 115291253B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- measurement
- value
- positioning result
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000012544 monitoring process Methods 0.000 title claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 85
- 238000012360 testing method Methods 0.000 claims abstract description 53
- 238000001914 filtration Methods 0.000 claims abstract description 49
- 238000004364 calculation method Methods 0.000 claims abstract description 11
- 230000002159 abnormal effect Effects 0.000 claims description 35
- 239000011159 matrix material Substances 0.000 claims description 29
- 230000004927 fusion Effects 0.000 claims description 13
- 230000008030 elimination Effects 0.000 claims description 9
- 238000003379 elimination reaction Methods 0.000 claims description 9
- 238000007476 Maximum Likelihood Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 238000000546 chi-square test Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000013215 result calculation Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 10
- 230000002708 enhancing effect Effects 0.000 abstract description 2
- 230000000007 visual effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/20—Integrity monitoring, fault detection or fault isolation of space segment
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/35—Constructional details or hardware or software details of the signal processing chain
- G01S19/37—Hardware or software details of the signal processing chain
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Signal Processing (AREA)
- Computer Security & Cryptography (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
一种基于残差检测的车辆定位完好性监测方法及系统，涉及车辆定位技术领域；所述方法基于改进的滤波残差计算方法构造了一个精确的检验统计量，再根据卡方检验法构造的检测门限值来判断定位信息的置信度，提高了原有完好性检测算法的准确度，解决了原有基于卡尔曼滤波器的接收器自主完好性检测算法具有较高误报率的问题。本发明为了补偿全球导航卫星系统限制并增强定位完好性，利用扩展卡尔曼滤波融合了除卫星外的多个传感器的测量信息，通过重构测量模型并建立视域，以窗口化的形式处理测量信息，以及提出一种简化的保护等级计算方法，有效降低了算法的计算量并进一步增强了定位完好性。
Description
技术领域
本发明涉及车辆定位技术领域，尤其涉及一种基于残差检测的车辆定位完好性监测方法及系统。
背景技术
基于全球导航卫星系统(如GPS、北斗)的定位和导航服务的安全性在许多应用中都非常重要。为了提供出色的服务，定位解决方案必须满足所需的精度和完好性或服务可用性性能。对于城市车辆应用，定位完好性是一个关键问题，由于多路径效应、有限的卫星可见性和传感器融合的困难，该问题非常具有挑战性。任何故障都会在融合滤波器中递归传播，并导致位置误差(Position Error，PE)的增加。
现有的评估定位完好性的方法是接收器自主完好性监测(ReceiverAutonomousIntegrity Monitoring，RAIM)，也可以理解为故障检测与排除(Fault DetectionandExclusion，FDE)。接收器自主完好性监测算法比较冗余信号之间的平滑伪距测量值，以确保它们都是一致的。它通常运行基于卡尔曼滤波器(Kalman Filter，KF)或其变体的错误测量拒绝机制。递归过滤算法简单高效，但由于模型不准确而不够可靠。因此，基于卡尔曼滤波器的接收器自主完好性监测方案通常具有很高的误报率，故研究一个更为精准的完好性监测方法非常重要。为了补偿全球导航卫星系统限制并增强定位完好性，接收器自主完好性监测还可以与其他传感模式集成，例如惯性测量单元、雷达/激光雷达、相机、机会信号(例如，蜂窝、无线电、数字电视)等，但许多基于多传感器集成的方案都导致了很高的计算量和算法的复杂度。
此外，定位完好性的一个关键衡量标准是最小保护级别(Protection Level，PL)，它是通过基于位置误差的累积密度函数的错误表征来计算的。用于保护级别计算的传统算法通常需要位置误差的精确分布和最坏情况的确定，以适应所有可能的偏差和故障。由于确定这两个问题的结果并不简单，因此很难生成具有给定完好性风险的保护级别，这很容易导致定位完好性判断的失误。
发明内容
为解决现有技术的不足，本发明提供一种基于残差检测的车辆定位完好性监测方法及系统，根据城市环境中的车辆多传感器融合定位结果提供改进的故障检测与排除和保护级别计算框架，该框架包括信号采集模块、滤波模块、故障检测与排除模块、保护级别计算模块及报警模块，通过基于滤波器的残差进行完好性评估。
为实现上述目的，本发明采用的技术方案为：
第一方面，本发明提供一种基于残差检测的车辆定位完好性监测方法，包括以下步骤：
S1：采集当前时刻车载惯性传感器的车辆运动测量数据、磁强计的车辆姿态测量数据及卫星的车辆位置及速度测量数据，建立系统状态方程模型及测量模型，通过扩展卡尔曼滤波进行系统状态变量估计；
S2：根据第一步中所获得的基于卡尔曼滤波所产生的滤波信息，重建测量模型，通过滤波残差计算检验统计量；
S3：采用卡方检测法计算障检测门限值，利用所述检验统计量与故障检测门限值进行比较以判断定位结果的置信度，保留可信的定位结果，将不可信的定位结果舍弃，采用扩展卡尔曼滤波得到排除异常值后的定位结果，从而实现故障检测与排除；
S4：根据第三步所述排除异常值后的定位结果，计算保护等级，判断排除异常值后的定位结果的可靠性，若定位结果可靠则不发出警报，若定位结果不可靠则发出警报。
所述S1具体包括以下步骤：
S1.1，建立系统状态空间模型，所述系统状态空间模型包括状态方程模型和测量方程模型：
所述状态方程模型，如式(1)所示：
X＝[ψ,δV,δ,γ,ζ]T (1)
其中Ψ,V,δ,γ和ζ，如式(2)所示：
其中，ψ表示车辆姿态误差变量，ψE,ψN,ψU分别表示东向、北向和天向车辆姿态误差，δV表示车辆速度误差变量，δVE,δVN,δVU分别表示东向、北向和天向车辆速度误差，δ表示车辆的位置误差，δL、δλ和δh分别表示车辆所在位置的维度、精度和高程的误差，γ表示加速度计在机体坐标系下的偏置，γbx,、γby、γbz分别为加速度计在机体坐标系下X、Y、Z方向的偏置，ζ表示陀螺仪在机体坐标系下的偏移，ζbx,、ζby,、ζbz分别为陀螺仪在机体坐标系下X、Y、Z方向的漂移；
所述测量方程模型，如式(3)所示：
其中，Pstate、Pgps分别表示车载惯性传感器及卫星测量的位置数据，vstate、vgps分别表示车载惯性传感器及卫星测量的速度数据，测量变量Z为车载惯性传感器测量与卫星测量的位置及速度差值组成的矩阵向量；
S1.2：读入车载惯性传感器及磁强计测量信息，利用所构建的状态方程模型进行扩展卡尔曼滤波的预测步，从而得到当前时刻系统状态变量的先验估计；
S1.3：读入当前卫星的测量信息，利用所构建的测量方程模型进行扩展卡尔曼滤波的更新步，从而对先验估计的系统状态变量进行修正，得到系统状态变量的后验估计结果。
所述S2具体包括以下步骤：
S2.1：利用所述滤波信息重建测量模型，如式(4)所示：
其中，k表示时刻，表示系统状态变量的先验估计，/>表示系统状态变量的后验估计，Hk为观测矩阵，Zk表示卫星测量信息，I为单位阵，vk、vx均为噪声；
将测量变量以及系统状态变量先验估计更新测量变量，则重建后的测量模型简化如式(5)所示：
其中，Yk为重建的测量变量、C为重建的观测矩阵、vzx为重建的观测噪声变量，同时假定声变量为零均值高斯白噪声，所述零均值高斯白噪声对应的协方差为Vk，如式(6)所示：
使用W＝Vk -1作为权值矩阵，根据最小二乘理论得到全新的系统状态变量的后验状态估计值，如式(7)所示：
S2.2：设定视域N，推算当前时刻的状态变量递推估计值及测量变量估计值，如式(8)所示:
其中，代表从k-N+1到k历元的递推估计值，k-N历元的/>与N个状态转移矩阵F连乘得到/>
由此计算k时刻系统测量变量估计值，如式(9)所示：
S2.3：利用所述测量变量估计值与卫星测量信息，计算滤波残差，由此生成检验统计量：
根据测量变量估计值及卫星测量信息生成滤波残差，如式(10)所示：
N个历元的残差协方差阵Ai，如式(11)所示：
根据信息分配原则，对式(11)做平均处理，如式(12)所示：
其中，Ai -1表示Ai的逆；窗口内各历元残差分配权值矩阵λi，如式(13)所示：
对N个历元的所有残差进行加权求和得到ηave，进行残差量的更新，如式(14)所示：
基于最大似然估计理论构造检验统计量S，如式(15)所示：
所述S3具体包括以下步骤：
S3.1：给定误警概率PFA，计算故障检测门限值TD，如式(16)所示：
其中，n为可见卫星数，表示自由度为n+1的卡方分布的概率密度函数；
S3.2：对比检验统计量与检测门限值的关系，若所述检验统计量小于或等于故障检测门限值，则判定定位结果可信并根据步骤一的后验状态变量估计值生成对应的车辆定位结果；
S3.3：若所述检验统计量大于所述故障检测门限值，则判定定位结果不可信，即确定定位结果存在异常值，并利用扩展卡尔曼滤波将存在异常值的定位结果排除。
所述S4具体包括以下步骤：
利用排除异常值后的定位结果计算保护等级，并与预先设定的警报极限做比较，判断是否发出警报，如式(17)所示：
其中，|X^k,k-N+1-X|为位置误差(PE)，X为排除异常值后的定位结果，PL为保护级别，AL为警报极限，Prisk为完好性风险；
根据需求给定完好性风险值Prisk，当位置误差、保护级别、警报极限满足式(17)时，排除异常值后的定位结果是可靠的，其他情况均为不可靠，需要系统发出警报。
所述保护级别的计算方法为：
计算故障监测阈值Tq，如式(18)所示：
其中，Q-1为零均值单位正态分布函数的尾部概率的倒数；wq为加权值，σq是从滤波结果的协方差阵P中提取的，如式(19)、式(20)所示：
P＝UVkUT (19)
其中，U表示位置误差，UT为U的转置；
保护级别PL，如式(21)所示：
PL＝Tq+Kmdσ (21)
σ为定位结果X的标准差，Kmd为与完好性风险有关的可调因子。
第二方面，本发明提供一种基于残差检测的车辆定位完好性监测系统，用于实现所述基于残差检测的车辆定位完好性监测方法，所述系统包含信号采集模块、滤波模块、残差生成模块、故障检测与排除模块以及报警模块；
所述系统通过布置在车载设备预设处和/或终端装置，所述终端装置包括单片型微型计算机、手机；所述车载设备包括车载电子控制单元。
信号采集模块将传感器检测到的卫星信号、惯性测量信号和车载传感器信号处理成滤波模块所需要的输入信号；卫星信号包括卫星所测车辆位置信号、车辆速度信号；惯性测量信号包括车辆加速度信号、车辆姿态信号；所述车载传感器信号包括车辆位置信号、车辆速度信号；
滤波模块将输入的信号进行数据融合；所述数据融合采用卡尔曼滤波进行，包括预测步和更新步；所述预测步采用由惯性测量信号和车载传感器信号信息构成的状态方程模型，进行状态变量的一步预测，所述更新步采用由卫星信号信息构成的测量方程模型，修正预测步的状态变量，完成数据融合；
所述残差生成模块将滤波模块中的测量方程模型重建，并计算出基于残差的检验统计量；所述检验统计量生成模块由预设最近视域N和残差生成器构成；所述残差生成器利用重建滤波模块中的测量方程模型，生成递推估计值，进而生成测量变量估计值，通过所述测量变量估计值与卫星测量信息做差得到残差值，利用所述残差值在视域N内的加权平均值得到新的残差值，并根据最大似然估计理论构造检验统计量。
所述故障检测与排除模块通过对检验统计量生成模块生成的检验统计量进行卡方检验检测定位结果是否存在超过预期的异常值，舍弃存在超过预期的异常值的定位结果，保留可信的定位结果；所述卡方检验通过计算一个检测门限值来与所述检验统计量进行比较，若检验统计量大于检测门限值，则判定定位结果存在超过预期的异常值；若检验统计量小于或等于检测门限值，则判定定位结果可信。
所述报警模块通过故障与排除模块排除异常值后的定位结果计算与检验统计量生成模块所述递推估计值做差计算相应的保护等级值，并将保护等级与预设的警报极限进行比较，若超出警报极限则对系统发出警报，反之不发出警报信号。
有益技术效果
1、本发明提出的一种基于残差检测的车辆定位完好性监测方法，基于改进的滤波残差计算方法构造了一个检验统计量，再根据卡方检验法构造的检测门限值来判断定位信息的置信度，提高了原有完好性检测算法的准确度，解决了原有基于卡尔曼滤波器的接收器自主完好性检测算法具有较高误报率的问题。
2、本发明为了补偿全球导航卫星系统限制并增强定位完好性，利用扩展卡尔曼滤波融合了除卫星外的多个传感器的测量信息，通过重构测量模型并建立视域，以窗口化的形式处理测量信息，以及提出一种简化的保护等级计算方法，有效降低了算法的计算量并进一步增强了定位完好性。
附图说明
图1为本发明实施例提供的一种基于残差检测的车辆定位完好性监测方法的流程示意图；
图2为本发明实施例提供的残差生成策略示意图；
图3为本发明实施例提供的一种基于残差检测的车辆定位完好性监测系统结构图；
图4为本发明实施例提供的信号采集模块工作流程示意图；
图5为本发明实施例提供的滤波模块工作流程示意图；
图6为本发明实施例提供的残差生成模块工作流程示意图；
图7为本发明实施例提供的故障检测与排除模块工作流程示意图；
图8为本发明实施例提供的报警模块工作流程示意图。
具体实施方式
下面将结合附图，对本申请中的技术方案进行进一步详细描述。
本实施例提供一种基于残差检测的车辆定位完好性监测方法及系统，根据城市环境中的车辆多传感器融合定位结果提供改进的故障检测与排除和保护级别计算框架，该框架包括信号采集模块、滤波模块、故障检测与排除模块、保护级别计算模块及报警模块，通过基于滤波器的残差进行完好性评估。
第一方面，本实施例提供一种基于残差检测的车辆定位完好性监测方法，如图1所示，包括以下步骤：
S1：采集当前时刻车载惯性传感器的车辆运动测量数据、磁强计的车辆姿态测量数据及卫星的车辆位置及速度测量数据，建立系统状态方程模型及测量模型，通过扩展卡尔曼滤波进行系统状态变量估计；
本实施例中，所述车载惯性传感器采用霍尼韦尔公司的微惯性导航系统CROSSBOWMEMS-INS(XBOW)；所述磁强计采用霍尼韦尔公司的三轴磁强计HMC5883L；所述卫星信号采集装置采用GNSS接收机SPANOEM4；
包括以下步骤：
S1.1，建立系统状态空间模型，所述系统状态空间模型包括状态方程模型和测量方程模型：
所述状态方程模型，如式(1)所示：
X＝[ψ,δV,δ,γ,ζ]T (1)
其中Ψ,V,δ,γ和ζ表示，如式(2)所示：
其中，ψ表示车辆姿态误差变量，ψE,ψN,ψU分别表示东向、北向和天向车辆姿态误差，δV表示车辆速度误差变量，δVE,δVN,δVU分别表示东向、北向和天向车辆速度误差，δ表示车辆的位置误差，δL、δλ和δh分别表示车辆所在位置的维度、精度和高程的误差，γ表示加速度计在机体坐标系下的偏置，γbx,、γby、γbz分别为加速度计在机体坐标系下X、Y、Z方向的偏置，ζ表示陀螺仪在机体坐标系下的偏移，ζbx,、ζby,、ζbz分别为陀螺仪在机体坐标系下X、Y、Z方向的漂移；
所述测量方程模型，如式(3)所示：
其中，Pstate、Pgps分别表示MEMS-INS及GPS接收机测量的位置数据，vstate、vgps分别表示MEMS-INS及GPS接收机测量的速度数据，测量变量Z为车载惯性传感器测量与卫星测量的位置及速度差值组成的矩阵向量；
S1.2：读入MEMS-INS及磁强计测量信息，利用所构建的状态方程模型进行扩展卡尔曼滤波的预测步，从而得到当前时刻系统状态变量的先验估计；
所述预测步为计算先验状态变量与状态转移雅可比矩阵来评估预测协方差，表示为：
其中，表示状态变量的先验估计，Pk,k-1是先验估计协方差矩阵，Pk-1是后验估计误差协方差矩阵，Qk-1是过程噪声协方差矩阵，Fk为fk经过线性化处理的状态矩阵；
S1.3：读入当前GPS接收机的测量信息，利用所构建的测量方程模型进行扩展卡尔曼滤波的更新步，从而对先验估计的系统状态变量进行修正，得到系统状态变量的后验估计结果。
所述更新步为计算观测雅可比矩阵和卡尔曼滤波增益，表示为：
Pk＝Pk,k-1-KkHkPk,k-1 (8)
表示状态变量的后验估计，Rk为观测噪声协方差矩阵，Hk为hk经过线性化处理的观测矩阵，Kk为卡尔曼增益，Zk表示测量变量；
S2：根据第一步中所获得的基于卡尔曼滤波所产生的滤波信息，重建测量模型，通过滤波残差计算检验统计量；
包括以下步骤：
S2.1：利用所述滤波信息重建测量模型，如式(9)所示：
其中，表示系统状态变量的先验估计，/>表示系统状态变量的后验估计，Hk为观测矩阵，Zk表示测量变量，I为单位阵，vk、vx均为噪声；
将测量变量以及系统状态变量先验估计更新测量变量，则重建后的测量模型简化如式(10)所示：
其中，Yk为重建的测量变量、C为重建的观测矩阵、Vzx为重建的观测噪声变量，同时假定声变量为零均值高斯白噪声，所述零均值高斯白噪声对应的协方差为Vk，如式(12)所示：
使用W＝Vk -1作为权值矩阵，根据最小二乘理论得到全新的系统状态变量的后验状态估计值，如式(13)所示：
利用原始扩展卡尔曼滤波预测步骤进行一步预测后，再使用上式进行状态估计，这样做的好处是在完好性监测中有效地考虑了时间连通性和系统传播所产生的完好性风险，为接下来的窗口化提供了预处理过程。
S2.2：设定视域N，以窗口化的形式推算当前时刻的状态变量递推估计值及测量变量估计值，如式(14)所示
其中，代表从k-N+1到k历元的递推估计值，k-N历元的/>与N个状态转移矩阵F连乘得到/>
由此计算k时刻系统测量变量估计值，如式(15)所示：
S2.3：利用所述测量变量估计值与卫星测量信息，计算滤波残差，由此生成检验统计量：
根据图2所示残差生成策略，利用测量变量估计值及卫星测量信息生成滤波残差，如式(16)所示：
N个历元的残差协方差阵Ai，如式(17)所示：
根据信息分配原则，对式(17)做平均处理，如式(18)所示：
则窗口内各历元残差分配权值矩阵λi，如式(19)所示：
对N个历元的所有残差进行加权求和得到ηave，进行残差量的更新，，如式(20)所示：
基于最大似然估计理论构造检验统计量S，如式(21)所示：
S3：采用卡方检测法计算障检测门限值，利用所述检验统计量与故障检测门限值进行比较以判断定位结果的置信度，保留可信的定位结果，将不可信的定位结果舍弃，采用扩展卡尔曼滤波得到排除异常值后的定位结果，从而实现故障检测与排除；包括以下步骤：
S3.1：给定一个误警概率PFA，计算故障检测门限值TD，如式(22)所示：
其中，n为可见卫星数，表示自由度为n+1的卡方分布的概率密度函数；
S3.2：对比第二步所得出的检验统计量与检测门限值的关系，若所述检验统计量小于或等于故障检测门限值，则判定定位结果可信并根据步骤一的后验状态变量估计值生成对应的车辆定位结果；
S3.3：若所述检验统计量大于所述故障检测门限值，则判定定位结果不可信，即确定定位结果存在异常值，并利用扩展卡尔曼滤波将存在异常值的定位结果排除。
定位结果可信与否的判断原理是若定位结果不存在异常值，检验统计量服从自由度为n+1的中心卡方分布χ2；如果存在异常值，则检验统计量服从非中心卡方分布：
S4：根据第三步所述排除异常值后的定位结果，计算保护等级，判断排除异常值后的定位结果的可靠性，若定位结果可靠则不发出警报，若定位结果不可靠则发出警报，包括以下步骤：
利用排除异常值后的定位结果计算保护等级，并与预先设定的警报极限做比较，判断是否发出警报，如式(24)所示：
其中，|X^k,k-N+1-X|为位置误差(PE)，X为排除异常值后的定位结果，PL为保护级别，AL为警报极限，Prisk为完好性风险；
根据需求给定完好性风险值Prisk，当位置误差、保护级别、警报极限满足式(24)时，排除异常值后的定位结果是可靠的，其他情况均为不可靠，需要系统发出警报。
所述保护级别的计算方法为：
计算故障监测阈值Tq，如式(25)所示：
其中，Q-1为零均值单位正态分布函数的尾部概率的倒数；wq为加权值，σq是从滤波结果的协方差阵P中提取的，如式(26)、式(27)所示：
P＝UVkUT (26)
其中，U为位置误差；
保护级别PL为，如式(28)所示：
PL＝Tq+Kmdσ (28)
σ为定位结果X的标准差，Kmd为与完好性风险有关的可调因子。
第二方面，本发明提供一种基于残差检测的车辆定位完好性监测系统，用于实现所述基于残差检测的车辆定位完好性监测方法，如图3所示，所述系统包括信号采集模块、滤波模块、残差生成模块、故障检测与排除模块以及报警模块；
所述系统通过布置在车载设备预设处和/或终端装置，所述终端装置包括单片型微型计算机、手机；所述车载设备包括车载电子控制单元。
如图4所示，信号采集模块将传感器检测到的卫星信号、惯性测量信号和车载传感器信号处理成滤波模块所需要的输入信号；卫星信号包括卫星所测车辆位置信号、车辆速度信号；惯性测量信号包括车辆加速度信号、车辆姿态信号；所述车载传感器信号包括车辆位置信号、车辆速度信号；
如图5所示，滤波模块将输入的信号进行数据融合；所述数据融合采用卡尔曼滤波进行，包括预测步和更新步；所述预测步采用由惯性测量信号和车载传感器信号信息构成的状态方程模型，进行状态变量的一步预测，所述更新步采用由卫星信号信息构成的测量方程模型，修正预测步的状态变量，完成数据融合；
如图6所示，所述残差生成模块将滤波模块中的测量方程模型重建，并计算出基于残差的检验统计量；所述检验统计量生成模块由预设最近视域N和残差生成器构成；所述残差生成器利用重建滤波模块中的测量方程模型，生成递推估计值，进而生成测量变量估计值，通过所述测量变量估计值与卫星测量信息做差得到残差值，利用所述残差值在视域N内的加权平均值得到新的残差值，并根据最大似然估计理论构造检验统计量。
如图7所示，所述故障检测与排除模块通过对检验统计量生成模块生成的检验统计量进行卡方检验检测定位结果是否存在超过预期的异常值，舍弃存在超过预期的异常值的定位结果，保留可信的定位结果；所述卡方检验通过计算一个检测门限值来与所述检验统计量进行比较，若检验统计量大于检测门限值，则判定定位结果存在超过预期的异常值；若检验统计量小于或等于检测门限值，则判定定位结果可信。
如图8所示，所述报警模块通过故障与排除模块排除异常值后的定位结果计算与检验统计量生成模块所述递推估计值做差计算相应的保护等级值，并将保护等级与预设的警报极限进行比较，若超出警报极限则对系统发出警报，反之不发出警报信号。
Claims (1)
1.一种基于残差检测的车辆定位完好性监测方法，其特征在于：包括以下步骤：
S1：采集当前时刻车载惯性传感器的车辆运动测量数据、磁强计的车辆姿态测量数据及卫星的车辆位置及速度测量数据，建立系统状态方程模型及测量模型，通过扩展卡尔曼滤波进行系统状态变量估计；
S1.1，建立系统状态空间模型，所述系统状态空间模型包括状态方程模型和测量方程模型：
所述状态方程模型，如式(1)所示：
X＝[ψ,δV,δ,γ,ζ]T (1)
其中Ψ,V,δ,γ和ζ，如式(2)所示：
其中，ψ表示车辆姿态误差变量，ψE,ψN,ψU分别表示东向、北向和天向车辆姿态误差，δV表示车辆速度误差变量，δVE,δVN,δVU分别表示东向、北向和天向车辆速度误差，δ表示车辆的位置误差，δL、δλ和δh分别表示车辆所在位置的维度、精度和高程的误差，γ表示加速度计在机体坐标系下的偏置，γbx,、γby、γbz分别为加速度计在机体坐标系下X、Y、Z方向的偏置，ζ表示陀螺仪在机体坐标系下的偏移，ζbx,、ζby,、ζbz分别为陀螺仪在机体坐标系下X、Y、Z方向的漂移；
所述测量方程模型，如式(3)所示：
其中，Pstate、Pgps分别表示车载惯性传感器及卫星测量的位置数据，vstate、vgps分别表示车载惯性传感器及卫星测量的速度数据，测量变量Z为车载惯性传感器测量与卫星测量的位置及速度差值组成的矩阵向量；
S1.2：读入车载惯性传感器及磁强计测量信息，利用所构建的状态方程模型进行扩展卡尔曼滤波的预测步，从而得到当前时刻系统状态变量的先验估计；
S1.3：读入当前卫星的测量信息，利用所构建的测量方程模型进行扩展卡尔曼滤波的更新步，从而对先验估计的系统状态变量进行修正，得到系统状态变量的后验估计结果；
S2：根据第一步中所获得的基于卡尔曼滤波所产生的滤波信息，重建测量模型，通过滤波残差计算检验统计量；
S2.1：利用所述滤波信息重建测量模型，如式(4)所示：
其中，k表示时刻，表示系统状态变量的先验估计，/>表示系统状态变量的后验估计，Hk为观测矩阵，Zk表示卫星测量信息，I为单位阵，vk、vx均为噪声；
将测量变量以及系统状态变量先验估计更新测量变量，则重建后的测量模型简化如式(5)所示：
其中，Yk为重建的测量变量、C为重建的观测矩阵、vzx为重建的观测噪声变量，同时假定声变量为零均值高斯白噪声，所述零均值高斯白噪声对应的协方差为Vk，如式(6)所示：
使用W＝Vk -1作为权值矩阵，根据最小二乘理论得到全新的系统状态变量的后验状态估计值，如式(7)所示：
S2.2：设定视域N，推算当前时刻的状态变量递推估计值及测量变量估计值，如式(8)所示:
其中，代表从k-N+1到k历元的递推估计值，k-N历元的/>与N个状态转移矩阵F连乘得到/>
由此计算k时刻系统测量变量估计值，如式(9)所示：
S2.3：利用所述测量变量估计值与卫星测量信息，计算滤波残差，由此生成检验统计量：
根据测量变量估计值及卫星测量信息生成滤波残差，如式(10)所示：
N个历元的残差协方差阵Ai，如式(11)所示：
根据信息分配原则，对式(11)做平均处理，如式(12)所示：
其中，Ai -1表示Ai的逆；窗口内各历元残差分配权值矩阵λi，如式(13)所示：
对N个历元的所有残差进行加权求和得到ηave，进行残差量的更新，如式(14)所示：
基于最大似然估计理论构造检验统计量S，如式(15)所示：
S3：采用卡方检测法计算障检测门限值，利用所述检验统计量与故障检测门限值进行比较以判断定位结果的置信度，保留可信的定位结果，将不可信的定位结果舍弃，采用扩展卡尔曼滤波得到排除异常值后的定位结果，从而实现故障检测与排除；
S3.1：给定误警概率PFA，计算故障检测门限值TD，如式(16)所示：
其中，n为可见卫星数，表示自由度为n+1的卡方分布的概率密度函数；
S3.2：对比检验统计量与检测门限值的关系，若所述检验统计量小于或等于故障检测门限值，则判定定位结果可信并根据步骤一的后验状态变量估计值生成对应的车辆定位结果；
S3.3：若所述检验统计量大于所述故障检测门限值，则判定定位结果不可信，即确定定位结果存在异常值，并利用扩展卡尔曼滤波将存在异常值的定位结果排除；
S4：根据第三步所述排除异常值后的定位结果，计算保护等级，判断排除异常值后的定位结果的可靠性，若定位结果可靠则不发出警报，若定位结果不可靠则发出警报；
利用排除异常值后的定位结果计算保护等级，并与预先设定的警报极限做比较，判断是否发出警报，如式(17)所示：
其中，|X^k,k-N+1-X|为位置误差(PE)，X为排除异常值后的定位结果，PL为保护级别，AL为警报极限，Prisk为完好性风险；
根据需求给定完好性风险值Prisk，当位置误差、保护级别、警报极限满足式(17)时，排除异常值后的定位结果是可靠的，其他情况均为不可靠，需要系统发出警报；
所述保护级别的计算方法为：
计算故障监测阈值Tq，如式(18)所示：
其中，Q-1为零均值单位正态分布函数的尾部概率的倒数；wq为加权值，σq是从滤波结果的协方差阵P中提取的，如式(19)、式(20)所示：
P＝UVkUT (19)
其中，U表示位置误差，UT为U的转置；
保护级别PL，如式(21)所示：
PL＝Tq+Kmdσ (21)
σ为定位结果X的标准差，Kmd为与完好性风险有关的可调因子；
所述的基于残差检测的车辆定位完好性监测方法，通过一种基于残差检测的车辆定位完好性监测系统实现，其特征在于：
所述系统包括信号采集模块、滤波模块、残差生成模块、故障检测与排除模块以及报警模块；通过布置在车载设备预设处和/或终端装置，所述终端装置包括单片型微型计算机、手机；所述车载设备包括车载电子控制单元；
信号采集模块将传感器检测到的卫星信号、惯性测量信号和车载传感器信号处理为滤波模块的输入信号；
滤波模块将输入的信号进行数据融合；所述数据融合采用卡尔曼滤波进行，包括预测步和更新步；
所述预测步采用由惯性测量信号和车载传感器信号信息构成的状态方程模型，进行状态变量的一步预测；
所述更新步采用由卫星信号信息构成的测量方程模型，修正预测步的状态变量，完成数据融合；
所述检验统计量生成模块将滤波模块中的测量方程模型重建，并计算出基于残差的检验统计量；
所述故障检测与排除模块通过对检验统计量生成模块生成的检验统计量进行卡方检验检测定位结果是否存在超过预期的异常值，舍弃存在超过预期的异常值的定位结果，保留可信的定位结果；所述报警模块通过故障与排除模块排除异常值后的定位结果计算与检验统计量生成模块所述递推估计值做差计算相应的保护等级值，并将保护等级与预设的警报极限进行比较，若超出警报极限则对系统发出警报，反之不发出警报信号；
所述卫星信号包括卫星所测车辆位置信号、车辆速度信号；惯性测量信号包括车辆加速度信号、车辆姿态信号；所述车载传感器信号包括车辆位置信号、车辆速度信号；
所述卡方检验通过计算一个检测门限值来与所述检验统计量进行比较，若检验统计量大于检测门限值，则判定定位结果存在超过预期的异常值；若检验统计量小于或等于检测门限值，则判定定位结果可信；
所述检验统计量生成模块由预设最近视域N和残差生成器构成；所述残差生成器利用重建滤波模块中的测量方程模型，生成递推估计值，进而生成测量变量估计值，通过所述测量变量估计值与卫星测量信息做差得到残差值，利用所述残差值在视域N内的加权平均值得到新的残差值，并根据最大似然估计理论构造检验统计量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210921597.8A CN115291253B (zh) | 2022-08-02 | 2022-08-02 | 一种基于残差检测的车辆定位完好性监测方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210921597.8A CN115291253B (zh) | 2022-08-02 | 2022-08-02 | 一种基于残差检测的车辆定位完好性监测方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115291253A CN115291253A (zh) | 2022-11-04 |
| CN115291253B true CN115291253B (zh) | 2023-12-05 |
Family
ID=83826794
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210921597.8A Active CN115291253B (zh) | 2022-08-02 | 2022-08-02 | 一种基于残差检测的车辆定位完好性监测方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115291253B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116736339B (zh) * | 2023-08-11 | 2023-11-03 | 浙江中裕通信技术有限公司 | 一种面向禁航区管控的北斗自主监测及预警方法 |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104808220A (zh) * | 2015-04-02 | 2015-07-29 | 北京交通大学 | 一种基于无线信息交互的车辆定位完好性监测方法 |
| CN109490916A (zh) * | 2019-01-21 | 2019-03-19 | 南京航空航天大学 | 一种gnss接收机自主完好性监测方法 |
| CN110007317A (zh) * | 2019-04-10 | 2019-07-12 | 南京航空航天大学 | 一种选星优化的高级接收机自主完好性监测方法 |
| CN110687557A (zh) * | 2019-09-24 | 2020-01-14 | 北京航空航天大学 | 高级接收机自主完好性监测保护级优化方法和设备 |
| CN110941000A (zh) * | 2019-12-18 | 2020-03-31 | 哈尔滨工程大学 | 一种精密单点定位完好性监测方法 |
| CN111323793A (zh) * | 2020-03-30 | 2020-06-23 | 中国矿业大学 | 一种gnss伪距单点定位状态域完好性监测方法 |
| CN113009520A (zh) * | 2021-02-26 | 2021-06-22 | 北京遥测技术研究所 | 一种卫星导航矢量跟踪环的完好性检测方法 |
| CN113670337A (zh) * | 2021-09-03 | 2021-11-19 | 东南大学 | 一种用于gnss/ins组合导航卫星缓变故障检测方法 |
| CN113984054A (zh) * | 2021-09-17 | 2022-01-28 | 兰州交通大学 | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 |
| CN114167469A (zh) * | 2021-10-29 | 2022-03-11 | 北京邮电大学 | 一种基于5g/gnss组合的车辆导航信息监测方法和装置 |
| CN114415224A (zh) * | 2022-01-25 | 2022-04-29 | 东北大学 | 一种复杂受限环境中车辆融合定位系统及方法 |
| CN114545454A (zh) * | 2022-02-15 | 2022-05-27 | 南京航空航天大学 | 一种面向自动驾驶的融合导航系统完好性监测方法 |
| CN114706110A (zh) * | 2022-01-17 | 2022-07-05 | 北京交通大学 | 一种基于车路协同的车辆的卫星动态定位方法和系统 |
| CN114721017A (zh) * | 2022-03-04 | 2022-07-08 | 北京理工大学 | 一种gnss/ins组合导航自主完好性监测方法 |
-
2022
- 2022-08-02 CN CN202210921597.8A patent/CN115291253B/zh active Active
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104808220A (zh) * | 2015-04-02 | 2015-07-29 | 北京交通大学 | 一种基于无线信息交互的车辆定位完好性监测方法 |
| CN109490916A (zh) * | 2019-01-21 | 2019-03-19 | 南京航空航天大学 | 一种gnss接收机自主完好性监测方法 |
| CN110007317A (zh) * | 2019-04-10 | 2019-07-12 | 南京航空航天大学 | 一种选星优化的高级接收机自主完好性监测方法 |
| CN110687557A (zh) * | 2019-09-24 | 2020-01-14 | 北京航空航天大学 | 高级接收机自主完好性监测保护级优化方法和设备 |
| CN110941000A (zh) * | 2019-12-18 | 2020-03-31 | 哈尔滨工程大学 | 一种精密单点定位完好性监测方法 |
| CN111323793A (zh) * | 2020-03-30 | 2020-06-23 | 中国矿业大学 | 一种gnss伪距单点定位状态域完好性监测方法 |
| CN113009520A (zh) * | 2021-02-26 | 2021-06-22 | 北京遥测技术研究所 | 一种卫星导航矢量跟踪环的完好性检测方法 |
| CN113670337A (zh) * | 2021-09-03 | 2021-11-19 | 东南大学 | 一种用于gnss/ins组合导航卫星缓变故障检测方法 |
| CN113984054A (zh) * | 2021-09-17 | 2022-01-28 | 兰州交通大学 | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 |
| CN114167469A (zh) * | 2021-10-29 | 2022-03-11 | 北京邮电大学 | 一种基于5g/gnss组合的车辆导航信息监测方法和装置 |
| CN114706110A (zh) * | 2022-01-17 | 2022-07-05 | 北京交通大学 | 一种基于车路协同的车辆的卫星动态定位方法和系统 |
| CN114415224A (zh) * | 2022-01-25 | 2022-04-29 | 东北大学 | 一种复杂受限环境中车辆融合定位系统及方法 |
| CN114545454A (zh) * | 2022-02-15 | 2022-05-27 | 南京航空航天大学 | 一种面向自动驾驶的融合导航系统完好性监测方法 |
| CN114721017A (zh) * | 2022-03-04 | 2022-07-08 | 北京理工大学 | 一种gnss/ins组合导航自主完好性监测方法 |
Non-Patent Citations (2)
| Title |
|---|
| 刘金山.基于MEMS-INS/GPS/磁强计的车载安全智能监测技术研究.《中国优秀硕士学位论文全文数据库 信息科技辑》.2018,(第3期),2.6MEMS-INS/GPS/磁强计组合导航滤波器设计，4.3基于Kalman滤波的RAIM算法. * |
| 基于MEMS-INS/GPS/磁强计的车载安全智能监测技术研究;刘金山;《中国优秀硕士学位论文全文数据库 信息科技辑》(第3期);2.6MEMS-INS/GPS/磁强计组合导航滤波器设计，4.3基于Kalman滤波的RAIM算法 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115291253A (zh) | 2022-11-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US7711482B2 (en) | Hybrid INS/GNSS system with integrity monitoring and method for integrity monitoring | |
| US10018729B2 (en) | Selected aspects of advanced receiver autonomous integrity monitoring application to kalman filter based navigation filter | |
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| US10234292B2 (en) | Positioning apparatus and global navigation satellite system, method of detecting satellite signals | |
| US8773303B2 (en) | Position tracking device and method | |
| EP3043148B1 (en) | Heading for a hybrid navigation solution based on magnetically calibrated measurements | |
| EP3901664A1 (en) | Systems and methods for inertial measurement unit aided detection and exclusion against spoofing attacks | |
| CN112507281B (zh) | 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法 | |
| CN111044051B (zh) | 一种复合翼无人机容错组合导航方法 | |
| EP2685214B1 (en) | Multiple truth reference system and method | |
| CN115291253B (zh) | 一种基于残差检测的车辆定位完好性监测方法及系统 | |
| JP2012233800A (ja) | マルチセンサ判定装置及びプログラム | |
| CN111024067B (zh) | 一种信息处理方法、装置、设备及计算机存储介质 | |
| US11662472B2 (en) | Integrity monitoring of odometry measurements within a navigation system | |
| US20230341563A1 (en) | System and method for computing positioning protection levels | |
| CN110906929A (zh) | 一种卫星异常数据识别方法及装置 | |
| EP3901650A1 (en) | Integrity monitoring of odometry measurements within a navigation system | |
| CN115096309A (zh) | 融合定位方法、装置、电子设备及存储介质 | |
| CN113819911A (zh) | Gnss失锁下基于自适应容错滤波的导航方法 | |
| CN117310767A (zh) | 组合导航方法 | |
| US11860287B2 (en) | System and method for detecting outliers in GNSS observations | |
| EP3882665A1 (en) | Implementing single differences within a solution separation framework | |
| EP3882572A1 (en) | Single-difference based pre-filter of measurements for use in solution separation framework | |
| CN117739962A (zh) | 组合导航方法、装置、电子设备及可读存储介质 | |
| CN116089767A (zh) | 滤波信息补偿方法、装置、电子设备及介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |