CN115825999B - 一种滤波器状态监测方法、装置、电子设备及存储介质 - Google Patents

一种滤波器状态监测方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN115825999B
CN115825999B CN202310152864.4A CN202310152864A CN115825999B CN 115825999 B CN115825999 B CN 115825999B CN 202310152864 A CN202310152864 A CN 202310152864A CN 115825999 B CN115825999 B CN 115825999B
Authority
CN
China
Prior art keywords
data
determining
vehicle
speed
satellite
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
Application number
CN202310152864.4A
Other languages
English (en)
Other versions
CN115825999A (zh
Inventor
姜东凯
施垒
蒋欢
韩雷晋
焦旭
冯世伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Asensing Technology Co Ltd
Original Assignee
Guangzhou Asensing Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangzhou Asensing Technology Co Ltd filed Critical Guangzhou Asensing Technology Co Ltd
Priority to CN202310152864.4A priority Critical patent/CN115825999B/zh
Publication of CN115825999A publication Critical patent/CN115825999A/zh
Application granted granted Critical
Publication of CN115825999B publication Critical patent/CN115825999B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Navigation (AREA)

Abstract

本发明的实施例提供了一种滤波器状态监测方法、装置、电子设备及存储介质，涉及自动驾驶领域，方法包括：确定车辆和卫星的原始数据和输出数据，基于原始数据和输出数据确定滤波器的一级稳定性，在一级稳定性为稳定状态或者收敛状态时，对车辆和/或卫星的运行参数进行监测，基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性，以基于二级稳定性对滤波器进行处理。基于原始数据和输出数据对滤波器的稳定性进行多次检测，可以提高确定滤波器稳定性的准确性。基于原始数据和输出数据对车辆和/或卫星的不同运行参数进行监测，避免依赖滤波器自身精度对滤波器的稳定性进行监测，进而提高自动驾驶的稳定性和精度。

Description

一种滤波器状态监测方法、装置、电子设备及存储介质
技术领域
本发明涉及自动驾驶技术领域，具体而言，涉及一种滤波器状态监测方法、装置、电子设备及存储介质。
背景技术
目前对于卡尔曼滤波器的监测通常为：对量测估计误差、系统噪声协方差、理论量测估计方差等预设一定的条件，如果满足条件则认为是稳定的，否则为发散。
因此，对卡尔曼滤波状态判断不管是基于一步判定、或者是过程判断都过度依赖卡尔曼滤波器的内符合精度，使得卡尔曼滤波器监测结果不够准确。
发明内容
本发明的目的在于提供一种滤波器状态监测方法，能够避免依赖滤波器自身精度对滤波器的稳定性进行监测，提高自动驾驶的稳定性和精度。
为了实现上述目的，本申请实施例采用的技术方案如下：
第一方面，本申请实施例提供了一种滤波器状态监测方法，所述方法包括：
确定车辆和卫星的原始数据，其中，所述车辆和所述卫星进行信息交互，以实现所述车辆的自动驾驶；
确定滤波器对所述原始数据进行处理后的输出数据；
基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性；
在所述一级稳定性为稳定状态或者收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测；
基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理。
在可选的实施方式中，所述基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性的步骤，包括：
获取第一预设时间段内的第一原始数据和第一输出数据；
计算所述第一原始数据的第一标准差、第一P值方差以及第一协方差；
计算所述第一输出数据的第二标准差、第二P值方差以及第二协方差；
在所述第一标准差和所述第二标准差均小于第一阈值，且所述第一P值方差和第二P值方差分别小于所述第一协方差与第二阈值的乘积和第二协方差与所述第二阈值的乘积时，确定所述原始数据中的GNSS数据；
判断所述GNSS数据是否可靠；
在所述GNSS数据可靠时，确定第二预设时间段内所述原始数据中的GNSS数据的第一卫星导航位置信息集与所述输出数据中的第一组合位置集，其中，所述第一卫星导航位置信息集中各卫星导航位置信息与所述第一组合位置集中各组合位置一一对应；
针对每个所述卫星导航位置信息，计算该卫星导航位置信息与对应的组合位置的水平差值的绝对值；
针对每个所述水平差值的绝对值，确定该水平差值的绝对值是否小于第三阈值；
在各所述水平差值均小于所述第三阈值时，确定所述滤波器的一级稳定性为收敛状态。
在可选的实施方式中，所述方法还包括：
在所述第一标准差和所述第二标准差任一大于第一阈值，或者所述第一P值方差大于所述第一协方差所述第二阈值的乘积或者所述第二P值方差大于所述第二协方差与所述第二阈值的乘积时，确定所述滤波器的一级稳定性为未收敛。
在可选的实施方式中，所述方法还包括：
在所述GNSS数据不可靠时，确定第二预设时间段内所述输出数据中的GNSS数据；
计算所述输出数据中GNSS数据的第三标准差；
确定所述输出数据中GNSS数据的解状态；
在所述第三标准差小于第四阈值，或者所述输出数据中GNSS数据的解状态为固定解时，确定所述滤波器的一级稳定性为收敛状态。
在可选的实施方式中，所述判断所述GNSS数据是否可靠的步骤，包括：
确定所述GNSS数据是否有效；
在所述GNSS数据有效的情况下，获取所述GNSS数据中当前时刻的速度数据；
将所述速度数据与第一预设速度进行比较；
在所述速度数据大于所述第一预设速度时，确定所述GNSS数据中当前时刻的收星数据；
确定所述收星数据的第一数量和当前时刻的上一时刻的收星数据的第二数量；
计算所述第一数量和所述第二数量的第一差值；
在所述第一数量大于第五阈值或者所述第一差值小于第六阈值时，判定所述GNSS数据可靠。
在可选的实施方式中，所述方法还包括：
在所述速度数据小于所述第一预设速度时，判定所述GNSS数据不可靠；
在所述第一数量小于所述第五阈值或者所述第一差值大于所述第六阈值时，判定所述GNSS数据不可靠。
在可选的实施方式中，所述在所述一级稳定性为稳定状态或者收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测的步骤，包括：
在所述一级稳定性为稳定状态或者收敛状态时，基于所述原始数据和输出数据对车辆和/或卫星的位置参数、速度参数、姿态参数以及里程参数进行监测；
所述基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理的步骤，包括：
在所述位置参数、速度参数、姿态参数以及里程参数均处于正常状态时，则确定所述滤波器的二级稳定性为收敛状态；
在所述位置参数、速度参数、姿态参数或者里程参数中任意之一处于异常状态时，则确定所述滤波器的二级稳定性为发散状态。
在可选的实施方式中，所述方法还包括：
在所述滤波器的二级稳定性为发散状态时，对所述滤波器进行重置操作。
在可选的实施方式中，所述基于所述原始数据和输出数据对车辆和/或卫星的位置参数进行监测的步骤，包括：
确定第二预设时间段内的所述原始数据中的第二卫星导航位置信息集和所述输出数据中的第二组合位置集，其中，所述第二卫星导航位置信息集中各卫星导航位置信息与所述第二组合位置集中各组合位置一一对应；
针对每个所述卫星导航位置信息，计算该卫星导航位置信息对应组合位置的第二差值；
在所述第二差值大于第七阈值时，确定所述卫星的位置参数异常。
在可选的实施方式中，所述方法还包括：
在所述第二差值小于第七阈值时，确定所述卫星的位置参数正常。
在可选的实施方式中，所述基于所述原始数据和输出数据对车辆和/或卫星的速度参数进行监测的步骤，包括：
确定当前时刻所述输出数据中所述车辆的第一组合定位速度；
从所述原始数据中确定当前时刻所述车辆的四轮的轮速；
将所述第一组合定位速度与第二预设速度进行比较；
在所述第一组合定位速度小于所述第二预设速度时，将所述车辆的四轮的轮速中最大轮速和最小轮速剔除，得到第一原始数据集；
确定补偿轮速系数；
基于所述补偿轮速系数和所述第一原始数据集中的轮速，计算所述车辆速度；
计算所述车辆速度与所述第一组合定位速度的第三差值的第一绝对值；
在所述第一绝对值小于第八阈值时，确定所述车辆的速度参数正常。
在可选的实施方式中，所述方法还包括：
在所述第一组合定位速度大于所述第二预设速度时，获取第二预设时间段内的第二组合定位速度集，其中，所述第二组合定位速度集中包含所述第二预设时间段内的多个第二组合定位速度；
将所述第二组合定位速度集中各第二组合定位速度与所述第二预设速度进行比较；
在各所述第二组合定位速度均大于或者等于所述第二预设速度时，确定所述车辆的速度参数异常。
在可选的实施方式中，所述基于所述原始数据和输出数据对车辆和/或卫星的姿态参数进行监测的步骤，包括：
确定当前时刻，所述输出数据中的所述车辆的第一俯仰角和第一翻滚角；
在所述第一俯仰角小于第一预设角度，且在所述第一翻滚角小于所述第一预设角度时，确定所述输出数据中所述车辆的第一偏航角和上一时刻的第二偏航角；
确定当前时刻，所述原始数据中IMU的Z轴的航向和上一时刻IMU的Z轴的航向；
基于所述第一偏航角和所述第二偏航角得到第一航向变化；
基于所述IMU的Z轴的航向和上一时刻IMU的Z轴的航向得到第二航向变化；
计算所述第一航向变化与所述第二航向行变化的差值的第二绝对值；
在所述第一航向变化大于第二预设角度且所述第二绝对值小于第三预设角度时，确定所述原始数据中的GNSS是否可靠；
若是，则从所述原始数据中确定GNSS的航向角度；
计算所述第一航向变化与所述GNSS的航向角度的差值的第三绝对值；
在所述第三绝对值小于第三预设角度时，确定所述车辆姿态参数正常。
在可选的实施方式中，所述方法还包括：
在所述第一俯仰角大于第一预设角度，且在所述第一翻滚角大于所述第一预设角度时，获取第二预设时间段内的第一俯仰角集和第一翻滚角集；
分别确定所述第一俯仰角集中的各第二俯仰角是否均大于所述第一预设角度；
且分别确定所述第一翻滚角集中的各第二翻滚角是否均大于所述第一预设角度；
若是，则确定所述车辆的姿态参数异常。
在可选的实施方式中，所述基于所述原始数据和输出数据对车辆和/或卫星的里程参数进行监测的步骤，包括：
从所述原始数据中确定里程计积分和里程计均速度；
从所述输出数据中确定组合导航结果距离积分；
计算所述里程计积分与所述组合导航结果距离积分的差值的第四绝对值；
判断所述里程计均速度是否小于第一预设速度，或者所述第四绝对值大于第九阈值与所述里程计积分的乘积；
若否，则确定所述里程计均速度是否大于或者等于第一预设速度，或者所述第四绝对值是否大于第十阈值与所述里程计积分的乘积；
若否，则确定所述车辆和/或卫星的里程参数正常。
第二方面，本申请实施例提供了一种滤波器状态监测装置，所述装置包括：
第一确定模块，用于确定车辆和卫星的原始数据，其中，所述车辆和所述卫星进行信息交互，以实现所述车辆的自动驾驶；
第二确定模块，用于确定滤波器对所述原始数据进行处理后的输出数据；
第三确定模块，用于基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性；
检测模块，用于在所述一级稳定性为稳定状态或者收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测；
第四确定模块，用于基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理。
第三方面，本申请实施例提供了一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述滤波器状态监测方法的步骤。
第四方面，本申请实施例提供了一种存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现所述滤波器状态监测方法的步骤。
本申请具有以下有益效果：
本申请通过确定车辆和卫星的原始数据，确定滤波器对原始数据进行处理后的输出数据，基于原始数据和输出数据确定滤波器的一级稳定性，在一级稳定性为稳定状态或者收敛状态时，基于原始数据和输出数据对车辆和/或卫星的运行参数进行监测，基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性，以基于二级稳定性对滤波器进行处理。基于原始数据和输出数据对滤波器的稳定性进行多次检测，可以提高确定滤波器稳定性的准确性。基于原始数据和输出数据对车辆和/或卫星的不同运行参数进行监测，避免依赖滤波器自身精度对滤波器的稳定性进行监测，进而提高自动驾驶的稳定性和精度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本发明实施例提供的电子设备的方框示意图；
图2为本发明实施例提供的一种滤波器状态监测方法的流程示意图之一；
图3为本发明实施例提供的一种滤波器状态监测方法的流程示意图之二；
图4为本发明实施例提供的一种滤波器状态监测方法的流程示意图之三；
图5为本发明实施例提供的一种滤波器状态监测方法的流程示意图之四；
图6为本发明实施例提供的一种滤波器状态监测方法的流程示意图之五；
图7为本发明实施例提供的一种滤波器状态监测方法的流程示意图之六；
图8为本发明实施例提供的一种滤波器状态监测方法的流程示意图之七；
图9为本发明实施例提供的一种滤波器状态监测方法的流程示意图之八；
图10为本发明实施例提供的一种滤波器状态监测方法的流程示意图之九；
图11为本发明实施例提供的一种滤波器状态监测装置的结构框图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本发明的描述中，需要说明的是，若出现术语“上”、“下”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，若出现术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
在本申请的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
经过发明人大量研究发现，目前对于卡尔曼滤波器的监测通常为：对量测估计误差、系统噪声协方差、理论量测估计方差等预设一定的条件，如果满足条件则认为是稳定的，否则为发散。
因此，对卡尔曼滤波状态判断不管是基于一步判定、或者是过程判断都过度依赖卡尔曼滤波器的内符合精度，使得卡尔曼滤波器监测结果不够准确。
有鉴于对上述问题的发现，本实施例提供了一种滤波器状态监测方法、装置、电子设备及存储介质，能够确定车辆和卫星的原始数据，确定滤波器对原始数据进行处理后的输出数据，基于原始数据和输出数据确定滤波器的一级稳定性，在一级稳定性为稳定状态或者收敛状态时，基于原始数据和输出数据对车辆和/或卫星的运行参数进行监测，基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性，以基于二级稳定性对滤波器进行处理。基于原始数据和输出数据对滤波器的稳定性进行多次检测，可以提高确定滤波器稳定性的准确性。基于原始数据和输出数据对车辆和/或卫星的不同运行参数进行监测，避免依赖滤波器自身精度对滤波器的稳定性进行监测，进而提高自动驾驶的稳定性和精度，下面对本实施例提供的方案进行详细阐述。
本实施例提供一种可以对滤波器状态进行监测的电子设备。在一种可能的实现方式中，所述电子设备可以为用户终端，例如，电子设备可以是，但不限于，服务器、智能手机、个人电脑(PersonalComputer，PC)、平板电脑、个人数字助理(PersonalDigitalAssistant，PDA)、移动上网设备(Mobile InternetDevice，MID)等。
请参照图1，图1是本申请实施例提供的电子设备100的结构示意图。所述电子设备100还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
所述电子设备100包括滤波器状态监测装置110、存储器120及处理器130。
所述存储器120及处理器130各元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述滤波器状态监测装置110包括至少一个可以软件或固件(firmware)的形式存储于所述存储器120中或固化在所述电子设备100的操作系统(operatingsystem，OS)中的软件功能模块。所述处理器130用于执行所述存储器120中存储的可执行模块，例如所述滤波器状态监测装置110所包括的软件功能模块及计算机程序等。
其中，所述存储器120可以是，但不限于，随机存取存储器(RandomAccessMemory，RAM)，只读存储器(Read OnlyMemory，ROM)，可编程只读存储器(ProgrammableRead-OnlyMemory，PROM)，可擦除只读存储器(ErasableProgrammableRead-Only Memory，EPROM)，电可擦除只读存储器(ElectricErasable ProgrammableRead-Only Memory，EEPROM)等。其中，存储器120用于存储程序，所述处理器130在接收到执行指令后，执行所述程序。
请参照图2，图2为应用于图1的电子设备100的一种滤波器状态监测方法的流程图，以下将方法包括各个步骤进行详细阐述。
步骤201：确定车辆和卫星的原始数据。
其中，车辆和卫星进行信息交互，以实现车辆的自动驾驶。
步骤202：确定滤波器对原始数据进行处理后的输出数据。
步骤203：基于原始数据和输出数据确定滤波器的一级稳定性。
步骤204：在一级稳定性为稳定状态或者收敛状态时，基于原始数据和输出数据对车辆和/或卫星的运行参数进行监测。
步骤205：基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性，以基于二级稳定性对滤波器进行处理。
需要说明的是，车辆的原始数据和卫星的原始数据可以包括：GNSS（GlobalNavigation Satellite System，全球导航卫星系统）数据、IMU（InertialMeasurement Unit，惯性测量单元）数据以及里程计数据。
将车辆和/或卫星的GNSS数据、IMU数据以及里程计数据输入至滤波器进行处理，滤波器对上述数据处理后得到原始数据的输出数据。
基于车辆和/或卫星的GNSS数据、IMU数据以及里程计数据确定当前滤波器的一级稳定性，在滤波器的一级稳定性为发散时，则直接将滤波器进行重置，在滤波器的一级稳定性为收敛或者稳定时，进一步对滤波器的稳定性进行监测，即对车辆和/或卫星的运行参数进行监测，并基于车辆和/或卫星运行参数的监测结果最终确定滤波器的二级稳定性。
在滤波器的二级稳定性为收敛或者稳定状态时，不对滤波器进行处理，在滤波器的二级稳定性为发散状态时，对滤波器进行重置操作。
需要说明的是，滤波器的一级稳定性或者二级稳定性包括发散状态、稳定状态以及收敛状态，发散状态指示滤波的实际误差远超过滤波误差的允许范围。稳定状态指示滤波的实际误差小于滤波误差的允许范围或超于一定范围但短时间随着正确观测量的进入可以恢复。收敛状态指示滤波器输出结果的误差小于滤波误差的允许范围。
为了进一步提高确定滤波器的稳定性，将车辆和/或卫星的原始数据和输出数据进行处理，基于处理后的原始数据和输出数据，确定滤波器的一级稳定性和二级稳定性。
对车辆和/或卫星的原始数据和输出数据进行处理的方式有多种，示例性的，获取原始数据和输出数据小于10HZ的原始子数据和输出子数据。基于原始子数据和第一输出数据确定滤波器的一级稳定性，在滤波器的一级稳定性为稳定状态或收敛状态时，基于原始子数据和输出子数据对车辆和/或卫星的运行参数进行监测，并基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性。
基于原始数据和输出数据确定滤波器的一级稳定性的实现方式有多种，在一种实现方式中，如图3所示，包括以下步骤：
步骤203-1：获取第一预设时间段内的第一原始数据和第一输出数据。
步骤203-2：计算第一原始数据的第一标准差、第一P值方差以及第一协方差。
步骤203-3：计算第一输出数据的第二标准差、第二P值方差以及第二协方差。
步骤203-4：在第一标准差和第二标准差均大于第一阈值，且第一P值方差和第二P值方差分别大于第一协方差与第二阈值的乘积和第二协方差与第二阈值的乘积时，确定原始数据中的GNSS数据。
步骤203-5：判断GNSS数据是否可靠。
步骤203-6：在GNSS数据可靠时，确定第二预设时间段内原始数据中的GNSS数据的第一卫星导航位置信息集与输出数据中的第一组合位置集。
其中，第一卫星导航位置信息集中各卫星导航位置信息与第一组合位置集中各组合位置一一对应。
步骤203-7：针对每个卫星导航位置信息，计算该卫星导航位置信息与对应的组合位置的水平差值的绝对值。
步骤203-8：针对每个水平差值的绝对值，确定该水平差值的绝对值是否小于第三阈值。
步骤203-9：在各水平差值均小于第三阈值时，确定滤波器的一级稳定性为收敛状态。
在一示例中，以某一时刻为起始点，获取以起始点为基准连续2s的第一原始数和第一输出数据，分别确定第一原始数据的第一标准差和第一P值方差以及某个观测量的P值自身通大数据计算的第一协方差，确定第一输出数据的第二标准差、第二P值方差以及某个观测量的P值自身通过大数据计算的第二协方差，将第一标准差和第二标准差与第一阈值比较，其中，第一阈值可以设置为1.0，即将第一标准差和第二标准差分别与1.0比较，在第一标准差和第二标准差均小于1.0时，计算第一协方差与第二阈值的第一乘积，计算第二协方差与第二阈值的第二乘积，其中，第二阈值可以设置为2.5，将第一P值与第一乘积比较，将第二P值与第二乘积比较，在第一P值小于第一乘积，且第二P值小于第二乘积时，执行下一操作，即获取原始数据中的GNSS数据。
需要说明的是，本申请实施例对于第一预设时间段、第一阈值以及第二阈值不作具体限制。
在另一示例中，在原始数据和输出数据包括GNSS数据、IMU数据以及里程计数据时，以某一时刻为起始点，获取以起始点为基准的第一预设时间段内的原始数据中的第一GNSS数据、第一IMU数据以及第一里程计数据，获取以起始点为基准的第一预设时间段内的输出数据中的第二GNSS数据、第二IMU数据以及第二里程计数据，分别确定第一GNSS数据、第二GNSS数据、第一IMU数据、第二IMU数据、第一里程计数据以及第二里程计数据的标准差、P值以及协方差，并分别各标准差与第一阈值比较，在各第一标准差均小于第一阈值时，则分别将各协方差与第二阈值相乘，得到各个乘积，针对每个P值，将该P值与对应的乘积比较，在各P值均小于对应的乘积时，则执行下一操作，获取原始数据中的GNSS数据。
在第一标准差和第二标准差任一大于第一阈值，或者第一P值方差大于所述第一协方差第二阈值的乘积或者第二P值方差大于第二协方差与第二阈值的乘积时，确定滤波器的一级稳定性为未收敛，执行基于原始数据和输出数据对车辆和/或卫星的运行参数进行监测，基于车辆和/或卫星的运行参数的监测结果，确定滤波器的二级稳定性。
在GNSS数据可靠时，以某一时刻为起始点，获取以起始点为基准连续5s内的原始数据中的第一卫星导航位置集和输出数据中第一组合位置集，分别计算每个第一卫星导航位置与对应的组合位置的水平差值的绝对值，将各水平差值的绝对值与第三阈值比较，其中，第三阈值可以为0.25，即将各水平差值的绝对值与0.25比较，在各水平差值的绝对值均小于0.25时，确定滤波器的一级稳定性为收敛状态，此时无需对滤波器进行处理。
需要说明的是，第二预设时间段可以设置为5s、6s、7s等，第三阈值可以设置为0.25、0.26、0.27，本申请实施例对此不做具体限制。
在GNSS数据不可靠时，确定滤波器的一级稳定性的另一种实现方式，如图4所示，包括以下步骤：
步骤301：在GNSS数据不可靠时，确定第二预设时间段内输出数据中的GNSS数据。
步骤302：计算输出数据中GNSS数据的第三标准差。
步骤303：确定输出数据中GNSS数据的解状态。
步骤304：在第三标准差大于第四阈值，或者输出数据中GNSS数据的解状态为固定解时，确定滤波器的一级稳定性为收敛状态。
在一示例中，确定第二预设时间段内的GNSS数据包括A、B以及C，计算A、B以及C的第三标准差，将第三标准差与第四阈值比较，其中，第四阈值可以设置为0.3，在第三标准差小于第四阈值，或者，确定输出数据的GNSS数据的解状态，在解状态为固定解时，确定滤波器的一级稳定性为收敛状态。
在另一示例中，在第三标准差大于第四阈值，确定滤波器的一级稳定性为发散状态。在第三标准差小于第四阈值，且解状态不为固定解时，确定滤波器的一级稳定性为发散状态。
确定GNSS数据是否可靠的实现方式有多种，在一种实现方式中，如图5，包括以下步骤：
步骤401：确定GNSS数据是否有效。
步骤402：在GNSS数据有效的情况下，获取GNSS数据中当前时刻的速度数据。
步骤403：将速度数据与第一预设速度进行比较。
步骤404：在速度数据大于第一预设速度时，确定GNSS数据中当前时刻的收星数据。
步骤405：确定收星数据的第一数量和当前时刻的上一时刻的收星数据的第二数量。
步骤406：计算第一数量和第二数量的第一差值。
步骤407：在第一数量大于第五阈值或者第一差值小于第六阈值时，判定GNSS数据可靠。
确定GNSS数据是否有效的实现方式有多种，在一种实现方式中，确定每个GNSS数据是否异常，将各GNSS数据与GNSS数据阈值范围比较，在确定某个GNSS数据超出GNSS数据阈值范围时，确定该GNSS数据无效。或者确定每个GNSS数据的时间帧与获取时间段的时间帧是否相同，若相同，则确定GNSS数据有效，若不同，则确定GNSS数据无效，在GNSS数据无效时，确定GNSS数据不可靠。
在一示例中，在GNSS数据有效的情况下，获取GNSS数据中当前时刻的车辆的速度数据，即获取从GNSS数据中获取当前时刻车辆的车速，在车辆的车速小于第一预设速度时，确定GNSS数据不可靠。在车辆的车速大于第一预设速度时，确定当前时刻的收星数据，收星数据包括收星数的第一数量和上一时刻的收星数的第二数量，基于第一数量和第二数量计算收星数变化量，在当前时刻的收星数的第一数量大于第五阈值时，确定GNSS数据可靠，或者在收星数变化量小于第六阈值时，确定GNSS数据可靠。
在当前时刻的收星数的第一数量小于第五阈值，或者在收星数变化量大于第六阈值时，确定GNSS数据不可靠。
在另一示例中，在GNSS数据有效时，获取第二预设时间段内的车辆的速度数据集合，在速度数据集合中各车辆的车速均大于第一预设速度时，确定第二预设时间段内的收星数据集合，确定收星数据集合中各收星数据的收星数的各第一数量，在确定各第一数量均大于第五阈值时，则确定GNSS数据可靠，若任一第一数量小于第五阈值，则确定GNSS数据不可靠。确定收星数据集合中各第一数量对应的上一时刻的第二数量，基于各第一数量和各第二数量，确定各收星数变化，在各收星数变化均小于第六阈值，确定GNSS数据可靠。
在另一示例中，在各第一数量任意之一小于第五阈值，或者各收星数变化任意之一大于第六阈值，确定GNSS数据不可靠。
需要说明的是，第一预设速度可以设置为5m/s、6m/s、7m/s，第五阈值可以设置为20、21或者22，第六阈值可以设置为5、6或者7，本领域技术人员对,第一预设速度、第五阈值以及第六阈值不作具体限制。
基于原始数据和输出数据对车辆和/或卫星的卫星运行参数进行监测的实现方式有多种，在一种实现方式中，如图6所示，包括以下步骤：
步骤204-1：在一级稳定性为稳定状态或者收敛状态时，基于原始数据和输出数据对车辆和/或卫星的位置参数、速度参数、姿态参数以及里程参数进行监测。
步骤205-1：在位置参数、速度参数、姿态参数以及里程参数均处于正常状态时，则确定滤波器的二级稳定性为收敛状态。
步骤205-2：在位置参数、速度参数、姿态参数或者里程参数中任意之一处于异常状态时，则确定滤波器的二级稳定性为发散状态。
在基于原始数据和输出数据判定滤波器的一级稳定性为稳定状态或者收敛状态时，对滤波器的二级稳定性进行判定，具体判定方式基于原始数据和输出数据对车辆和/或卫星的位置参数、速度参数、姿态参数一级里程参数进行监测结果，对滤波器的二级稳定性进行判定。
在基于原始数据和输出数据确定车辆和/或卫星的位置参数、速度参数、姿态参数以及里程参数均未出现异常，即均处于正常状态时，判定滤波器的二级稳定性为收敛状态。
在一示例中，可以为位置参数、速度参数、姿态参数以及里程参数的监测顺序进行设置，基于位置参数、速度参数、姿态参数以及里程参数的监测顺序进行监测，在监测到某一运行参数异常时，则直接停止对后续运行参数的监测，并判定滤波器的二级稳定性为发散状态，并对滤波器进行重置操作。
在另一示例中，同时对位置参数、速度参数、姿态参数以及里程参数进行监测，在监测到任一运行参数异常时，则判定滤波器的二级稳定性为发散状态。
基于原始数据和输出数据对车辆和/或卫星的卫星位置参数进行监测的实现方式有多种，在一种实现方式中，如图7所示，包括以下步骤：
步骤501：确定第二预设时间段内的原始数据中的第二卫星导航位置信息集和输出数据中的第二组合位置集。
其中，第二卫星导航位置信息集中各卫星导航位置信息与第二组合位置集中各组合位置一一对应。
步骤502：针对每个卫星导航位置信息，计算该卫星导航位置信息对应组合位置的第二差值。
步骤503：在第二差值大于第七阈值时，确定卫星的位置参数异常。
需要说明的是，第二预设时间段可以为5s或者10s，在第二预设时间段为5s时，从原始数据中确定5s内的第二卫星导航位置信息集，从输出数据中获取5s内的第二组合位置集，其中，第二卫星导航位置信息集中的卫星导航位置信息表征在数据未进行处理的直接从GNSS数据、IMU数据以及里程计数据中直接获取的卫星的位置或者车辆的位置，组合位置集中各组合位置表征基于GNSS数据、IMU数据以及里程计数据综合处理后，卫星的位置和车辆的位置。其中，每个卫星导航位置信息均对应一个组合位置。
示例性的，当获取的第二卫星导航位置信息集{A,B,C}，第二组合位置集为{a,b,c}，其中，A与a存在对应关系，B与b存在对应关系，C与c存在对应关系。
分别计算A与a的第二差值E、B与b的第二差值F，C与c的第二差值G，分别将E与第七阈值比较、F与第七阈值比较、G与第七阈值比较，在E、F以及G均大于第七阈值时，确定车辆和/或卫星的位置参数异常。
在另一示例中，在第二差值小于第七阈值时，确定卫星的位置参数正常。
需要说明的是，第七阈值可以设置为1000m、1500m、2000m等，本申请实施例对此不做具体限制。
基于原始数据和输出数据对车辆和/或卫星的卫星速度参数进行监测的实现方式有多种，在一种实现方式中，如图8所示，包括以下步骤：
步骤601：确定当前时刻输出数据中车辆的第一组合定位速度。
步骤602：从原始数据中确定当前时刻车辆的四轮的轮速。
步骤603：将第一组合定位速度与第二预设速度进行比较。
步骤604：在第一组合定位速度小于第二预设速度时，将车辆的四轮的轮速中最大轮速和最小轮速剔除，得到第一原始数据集。
步骤605：确定补偿轮速系数。
步骤606：基于补偿轮速系数和第一原始数据集中的轮速，计算车辆速度。
步骤607：计算车辆速度与第一组合定位速度的第三差值的第一绝对值。
步骤608：在第一绝对值小于第八阈值时，确定车辆的速度参数正常。
在一示例中，从输出数据中确定车辆当前时刻的第一组合定位速度，从输出数据中确定车辆的四轮的轮速，其中，车辆的四个轮对应有轮速。将当前时刻的第一组合定位速度与第二预设速度比较，以判定车辆的第一组合定位速度是否异常，需要说明的是，第二预设速度可以设置为70m/s。在第一组合定位速度大于第二预设速度时，判定车辆的速度参数异常。
在第一定位组合速度小于第二预设速度时，判定输出数据中的第一组合定位速度和原始数据中的轮速之间的差异，从而对车辆的速度参数的状态进行判定。
示例性的，车辆的四轮的轮速分别为第一轮速、第二轮速、第三轮速以及第四轮速，将第一轮速、第二轮速、第三轮速以及第四轮速中的最大轮速和最小轮速剔除，如第一轮速为最大轮速，第二轮速为最小轮速，剔除后得到包含第三轮速和第四轮速的第一原始数据集。确定补偿轮速系数，并基于补偿轮速系数、第三轮速以及第四轮速计算车辆速度。计算输出数据中获取的第一组合定位速度与车辆速度的第三差值的第一绝对值，从而确定原始数据中确定的车辆车速与输出数据中的车辆速度的差异，依据差异对车辆的速度参数进行监测。
将第一绝对值与第八阈值比较，在第一绝对值小于第八阈值时，确定车辆的速度参数正常。在第一绝对值大于或者等于第八阈值时，确定车辆的速度参数异常。
需要说明的是，第八阈值可以设置为15m/s、20m/s或者25m/s，本申请实施例对此不做具体限制。
在另一示例中，为了提高对车辆的速度参数监测的准确性，在第一组合定位速度大于第二预设速度时，获取第二预设时间段内的第二组合定位速度集，其中，第二组合定位速度集中包含第二预设时间段内的多个第二组合定位速度，将第二组合定位速度集中各第二组合定位速度与第二预设速度进行比较，在各第二组合定位速度均大于或者等于第二预设速度时，确定车辆的速度参数异常。
基于原始数据和输出数据对车辆和/或卫星的卫星姿态参数进行监测的实现方式有多种，在一种实现方式中，如图9所示，包括以下步骤：
步骤701：确定当前时刻，输出数据中的车辆的第一俯仰角和第一翻滚角。
步骤702：在第一俯仰角小于第一预设角度，且在第一翻滚角小于第一预设角度时，确定输出数据中车辆的第一偏航角和上一时刻的第二偏航角。
步骤703：确定当前时刻，原始数据中IMU的Z轴的航向和上一时刻IMU的Z轴的航向。
步骤704：基于第一偏航角和第二偏航角得到第一航向变化。
步骤705：基于IMU的Z轴的航向和上一时刻IMU的Z轴的航向得到第二航向变化。
步骤706：计算第一航向变化与第二航向变化的差值的第二绝对值。
步骤707：在第一航向变化大于第二预设角度或者第二绝对值小于第三预设角度时，确定原始数据中的GNSS是否可靠。
步骤708：若是，则从原始数据中确定GNSS的航向角度。
步骤709：计算第一航向变化与GNSS的航向角度的差值的第三绝对值。
步骤710：在第三绝对值小于第三预设角度时，确定车辆姿态参数正常。
从输出数据中确定当前时刻车辆的第一俯仰角和第一翻滚角，实际上，在车辆上安装有组合定位装置，获取组合定位装置的第一俯仰角和第一翻滚角，即为获取当前时刻车辆的第一俯仰角和第一翻滚角。
将第一俯仰角与第一预设角度比较，将第一翻滚角与第二预设角度比较，在第一俯仰角大于或者等于第一预设角度且第一翻滚角大于或者等于第二预设角度，判定车辆的姿态参数异常。
示例性的，在第一俯仰角大于第一预设角度，且在第一翻滚角大于第一预设角度时，获取第二预设时间段内的第一俯仰角集和第一翻滚角集，分别确定第一俯仰角集中的各第二俯仰角是否均大于第一预设角度，且分别确定第一翻滚角集中的各第二翻滚角是否均大于第一预设角度，若是，则确定车辆的姿态参数异常。
需要说明的是，第一预设角度和第二预设角度可以设置为15°，本申请实施例对此不做具体限制。
在第一俯仰角小于第一预设角度，且第一翻滚角小于第二预设角度的情况下，进一步对车辆的姿态参数进行监测。
示例性的，确定输出数据中车辆的第一偏航角和上一时刻的第二偏航角，从原始数据中确定当前时刻IMU的Z轴的航向和上一时刻IMU的Z轴的航向，从而确定输出数据中的第一航向变化和原始数据中的第二航向变化，基于输出数据的航向变化和原始数据中的航向变化，对车辆的姿态参数进行监测，在输出数据和原始数据确定的航向变化差异较大时，确定车辆的姿态参数异常，在输出数据和原始数据确定的航向变化差异在允许范围内时，确定车辆的姿态参数正常。
具体地，在第一航向变化大于第二预设角度且第二绝对值小于第三预设角度时，确定原始数据中的GNSS是否可靠，在GNSS数据可靠时，获取GNSS的航向角度，计算第一航向变化与GNSS的航向角度的差值的第三绝对值，在第三绝对值小于第三预设角度时，确定车辆的姿态参数正常。
在GNSS数据不可靠时，确定车辆的姿态参数异常。
在第三绝对值大于或者等于第三预设角度时，确定车辆的姿态参数异常。
在一示例中，在第一航向变化小于第二预设角度，或者第一航向变化和第二航向变化的差值的第二绝对值大于第三预设角度时，确定车辆的姿态参数异常。在另一示例中，为了提高对车辆的姿态参数监测的准确性，在当前时刻的第一航向变化小于第二预设角度，或者当前时刻的第一航向变化和第二航向变化的差值的第二绝对值大于第三预设角度时，获取第二预设时间段内的多个第一航向变化和第二航向变化，在各第一航向变化均小于第二预设角度，或者各第二绝对值均大于第三预设角度，确定车辆的姿态参数异常。
需要说明的是，第三预设角度可以设置为4°、5°、6°等，本申请实施例对此不做具体限制。
基于原始数据和输出数据对车辆和/或卫星的卫星里程参数进行监测的实现方式有多种，在一种实现方式中，如图10所示，包括以下步骤：
步骤801：从原始数据中确定里程计积分和里程计均速度。
步骤802：从输出数据中确定组合导航结果距离积分。
步骤803：计算里程计积分与组合导航结果距离积分的差值的第四绝对值。
步骤804：判断里程计均速度是否小于第一预设速度，或者第四绝对值大于第九阈值与里程计积分的乘积。
步骤805：若否，则确定里程计均速度是否大于或者等于第一预设速度，或者第四绝对值是否大于第十阈值与里程计积分的乘积。
步骤806：若否，则确定车辆和/或卫星的里程参数正常。
在一示例中，里程计均速度小于第一预设速度时，确定车辆和/或卫星的里程参数异常。或者里程计积分与组合导航结果距离积分的差值的第四绝对值大于第九阈值与里程计积分的乘积时，确定车辆和/或卫星的里程参数异常。
在另一示例中，当里程计均速度小于第一预设速度，或者里程计积分与组合导航结果距离积分的差值的第四绝对值大于第九阈值与里程计积分的乘积时，获取第二预设时间段内多个里程计均速度和多个里程计积分以及组合导航结果距离积分，在各里程计均速度均小于第一预设速度，或者各第四绝对值均大于第九阈值与里程计积分的乘积，则确定车辆和/或卫星的里程参数异常。
在判断里程计均速度是否小于第一预设速度，或者第四绝对值大于第九阈值与里程计积分的乘积的判断结果为否，确定里程计均速度是否大于或者等于第一预设速度，或者第四绝对值是否大于第十阈值与里程计积分的乘积，若否，则确定车辆和/或卫星的里程参数正常。
在另一示例中，为了提高对车辆和/或卫星的里程参数监测的准确性，在当前里程计均速度大于或者等于第一预设速度，或者第四绝对值大于第九阈值与里程计积分的乘积时，获取第二预设时间段内的多个里程计均速度和多个里程计积分以及组合导航结果距离积分，在各里程计均速度和多个里程计积分以及组合导航结果距离积分，在各里程计均速度大于或者等于第一预设速度时，或者各第四绝对值均大于第十阈值与里程计积分的乘积，确定车辆和/或卫星的里程参数异常。
需要说明的是，第九阈值可以设置为0.5，第十阈值可以设置为0.2。
请参照图11，本申请实施例还提供了一种应用于图1所述电子设备100的滤波器状态监测装置110，所述滤波器状态监测装置110包括：
第一确定模块111，用于确定车辆和卫星的原始数据，其中，所述车辆和所述卫星进行信息交互，以实现所述车辆的自动驾驶；
第二确定模块112，用于确定滤波器对所述原始数据进行处理后的输出数据；
第三确定模块113，用于基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性；
检测模块114，用于在所述一级稳定性为稳定状态或者收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测；
第四确定模块115，用于基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理。
本申请还提供一种电子设备100，电子设备100包括处理器130以及存储器120。存储器120存储有计算机可执行指令，计算机可执行指令被处理器130执行时，实现该滤波器状态监测方法。
本申请实施例还提供一种计算机可读存储介质，存储介质存储有计算机程序，计算机程序被处理器130执行时，实现该滤波器状态监测方法。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random AccessMemory)、磁碟或者光盘等各种可以存储程序代码的介质。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

Claims (17)

1.一种滤波器状态监测方法，其特征在于，所述方法包括：
确定车辆和卫星的原始数据，其中，所述车辆和所述卫星进行信息交互，以实现所述车辆的自动驾驶，所述原始数据包括GNSS数据、IMU数据以及里程计数据；
确定滤波器对所述原始数据进行处理后的输出数据；
基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性；
在所述一级稳定性为收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测；
基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理；其中，所述基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性的步骤，包括：
获取第一预设时间段内的第一原始数据和第一输出数据；
计算所述第一原始数据的第一标准差、第一P值方差以及第一协方差；
计算所述第一输出数据的第二标准差、第二P值方差以及第二协方差；
在所述第一标准差和所述第二标准差均小于第一阈值，且所述第一P值方差和第二P值方差分别小于所述第一协方差与第二阈值的乘积和第二协方差与所述第二阈值的乘积时，确定所述原始数据中的GNSS数据；
判断所述GNSS数据是否可靠；
在所述GNSS数据可靠时，确定第二预设时间段内所述原始数据中的GNSS数据的第一卫星导航位置信息集与所述输出数据中的第一组合位置集，其中，所述第一卫星导航位置信息集中各卫星导航位置信息与所述第一组合位置集中各组合位置一一对应；
针对每个所述卫星导航位置信息，计算该卫星导航位置信息与对应的组合位置的水平差值的绝对值；
针对每个所述水平差值的绝对值，确定该水平差值的绝对值是否小于第三阈值；
在各所述水平差值均小于所述第三阈值时，确定所述滤波器的一级稳定性为收敛状态。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
在所述第一标准差和所述第二标准差任一大于第一阈值，或者所述第一P值方差大于所述第一协方差所述第二阈值的乘积或者所述第二P值方差大于所述第二协方差与所述第二阈值的乘积时，确定所述滤波器的一级稳定性为未收敛。
3.根据权利要求1所述的方法，其特征在于，所述方法还包括：
在所述GNSS数据不可靠时，确定第二预设时间段内所述输出数据中的GNSS数据；
计算所述输出数据中GNSS数据的第三标准差；
确定所述输出数据中GNSS数据的解状态；
在所述第三标准差小于第四阈值，或者所述输出数据中GNSS数据的解状态为固定解时，确定所述滤波器的一级稳定性为收敛状态。
4.根据权利要求1所述的方法，其特征在于，所述判断所述GNSS数据是否可靠的步骤，包括：
确定所述GNSS数据是否有效；
在所述GNSS数据有效的情况下，获取所述GNSS数据中当前时刻的速度数据；
将所述速度数据与第一预设速度进行比较；
在所述速度数据大于所述第一预设速度时，确定所述GNSS数据中当前时刻的收星数据；
确定所述收星数据的第一数量和当前时刻的上一时刻的收星数据的第二数量；
计算所述第一数量和所述第二数量的第一差值；
在所述第一数量大于第五阈值或者所述第一差值小于第六阈值时，判定所述GNSS数据可靠。
5.根据权利要求4所述的方法，其特征在于，所述方法还包括：
在所述速度数据小于所述第一预设速度时，判定所述GNSS数据不可靠；
在所述第一数量小于所述第五阈值或者所述第一差值大于所述第六阈值时，判定所述GNSS数据不可靠。
6.根据权利要求1所述的方法，其特征在于，所述在所述一级稳定性为收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测的步骤，包括：
在所述一级稳定性为收敛状态时，基于所述原始数据和输出数据对车辆和/或卫星的位置参数、速度参数、姿态参数以及里程参数进行监测；
所述基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理的步骤，包括：
在所述位置参数、速度参数、姿态参数以及里程参数均处于正常状态时，则确定所述滤波器的二级稳定性为收敛状态；
在所述位置参数、速度参数、姿态参数或者里程参数中任意之一处于异常状态时，则确定所述滤波器的二级稳定性为发散状态。
7.根据权利要求6所述的方法，其特征在于，所述方法还包括：
在所述滤波器的二级稳定性为发散状态时，对所述滤波器进行重置操作。
8.根据权利要求6所述的方法，其特征在于，所述基于所述原始数据和输出数据对车辆和/或卫星的位置参数进行监测的步骤，包括：
确定第二预设时间段内的所述原始数据中的第二卫星导航位置信息集和所述输出数据中的第二组合位置集，其中，所述第二卫星导航位置信息集中各卫星导航位置信息与所述第二组合位置集中各组合位置一一对应；
针对每个所述卫星导航位置信息，计算该卫星导航位置信息对应组合位置的第二差值；
在所述第二差值大于第七阈值时，确定所述卫星的位置参数异常。
9.根据权利要求8所述的方法，其特征在于，所述方法还包括：
在所述第二差值小于第七阈值时，确定所述卫星的位置参数正常。
10.根据权利要求6所述的方法，其特征在于，所述基于所述原始数据和输出数据对车辆和/或卫星的速度参数进行监测的步骤，包括：
确定当前时刻所述输出数据中所述车辆的第一组合定位速度；
从所述原始数据中确定当前时刻所述车辆的四轮的轮速；
将所述第一组合定位速度与第二预设速度进行比较；
在所述第一组合定位速度小于所述第二预设速度时，将所述车辆的四轮的轮速中最大轮速和最小轮速剔除，得到第一原始数据集；
确定补偿轮速系数；
基于所述补偿轮速系数和所述第一原始数据集中的轮速，计算所述车辆速度；
计算所述车辆速度与所述第一组合定位速度的第三差值的第一绝对值；
在所述第一绝对值小于第八阈值时，确定所述车辆的速度参数正常。
11.根据权利要求10所述的方法，其特征在于，所述方法还包括：
在所述第一组合定位速度大于所述第二预设速度时，获取第二预设时间段内的第二组合定位速度集，其中，所述第二组合定位速度集中包含所述第二预设时间段内的多个第二组合定位速度；
将所述第二组合定位速度集中各第二组合定位速度与所述第二预设速度进行比较；
在各所述第二组合定位速度均大于或者等于所述第二预设速度时，确定所述车辆的速度参数异常。
12.根据权利要求6所述的方法，其特征在于，所述基于所述原始数据和输出数据对车辆和/或卫星的姿态参数进行监测的步骤，包括：
确定当前时刻，所述输出数据中的所述车辆的第一俯仰角和第一翻滚角；
在所述第一俯仰角小于第一预设角度，且在所述第一翻滚角小于所述第一预设角度时，确定所述输出数据中所述车辆的第一偏航角和上一时刻的第二偏航角；
确定当前时刻，所述原始数据中IMU的Z轴的航向和上一时刻IMU的Z轴的航向；
基于所述第一偏航角和所述第二偏航角得到第一航向变化；
基于所述IMU的Z轴的航向和上一时刻IMU的Z轴的航向得到第二航向变化；
计算所述第一航向变化与所述第二航向变化的差值的第二绝对值；
在所述第一航向变化大于第二预设角度且所述第二绝对值小于第三预设角度时，确定所述原始数据中的GNSS是否可靠；
若是，则从所述原始数据中确定GNSS的航向角度；
计算所述第一航向变化与所述GNSS的航向角度的差值的第三绝对值；
在所述第三绝对值小于第三预设角度时，确定所述车辆的姿态参数正常。
13.根据权利要求12所述的方法，其特征在于，所述方法还包括：
在所述第一俯仰角大于第一预设角度，且在所述第一翻滚角大于所述第一预设角度时，获取第二预设时间段内的第一俯仰角集和第一翻滚角集；
分别确定所述第一俯仰角集中的各第二俯仰角是否均大于所述第一预设角度；
且分别确定所述第一翻滚角集中的各第二翻滚角是否均大于所述第一预设角度；
若是，则确定所述车辆的姿态参数异常。
14.根据权利要求6所述的方法，其特征在于，所述基于所述原始数据和输出数据对车辆和/或卫星的里程参数进行监测的步骤，包括：
从所述原始数据中确定里程计积分和里程计均速度；
从所述输出数据中确定组合导航结果距离积分；
计算所述里程计积分与所述组合导航结果距离积分的差值的第四绝对值；
判断所述里程计均速度是否小于第一预设速度，或者所述第四绝对值大于第九阈值与所述里程计积分的乘积；
若否，则确定所述里程计均速度是否大于或者等于第一预设速度，或者所述第四绝对值是否大于第十阈值与所述里程计积分的乘积；
若否，则确定所述车辆和/或卫星的里程参数正常。
15.一种滤波器状态监测装置，其特征在于，所述装置包括：
第一确定模块，用于确定车辆和卫星的原始数据，其中，所述车辆和所述卫星进行信息交互，以实现所述车辆的自动驾驶，所述原始数据包括GNSS数据、IMU数据以及里程计数据；
第二确定模块，用于确定滤波器对所述原始数据进行处理后的输出数据；
第三确定模块，用于基于所述原始数据和所述输出数据确定所述滤波器的一级稳定性；
检测模块，用于在所述一级稳定性为收敛状态时，基于所述原始数据和所述输出数据对所述车辆和/或所述卫星的运行参数进行监测；
第四确定模块，用于基于所述车辆和/或所述卫星的运行参数的监测结果，确定所述滤波器的二级稳定性，以基于所述二级稳定性对所述滤波器进行处理；
所述第三确定模块还用于：获取第一预设时间段内的第一原始数据和第一输出数据；
计算所述第一原始数据的第一标准差、第一P值方差以及第一协方差；
计算所述第一输出数据的第二标准差、第二P值方差以及第二协方差；
在所述第一标准差和所述第二标准差均小于第一阈值，且所述第一P值方差和第二P值方差分别小于所述第一协方差与第二阈值的乘积和第二协方差与所述第二阈值的乘积时，确定所述原始数据中的GNSS数据；
判断所述GNSS数据是否可靠；
在所述GNSS数据可靠时，确定第二预设时间段内所述原始数据中的GNSS数据的第一卫星导航位置信息集与所述输出数据中的第一组合位置集，其中，所述第一卫星导航位置信息集中各卫星导航位置信息与所述第一组合位置集中各组合位置一一对应；
针对每个所述卫星导航位置信息，计算该卫星导航位置信息与对应的组合位置的水平差值的绝对值；
针对每个所述水平差值的绝对值，确定该水平差值的绝对值是否小于第三阈值；
在各所述水平差值均小于所述第三阈值时，确定所述滤波器的一级稳定性为收敛状态。
16.一种电子设备，其特征在于，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-14任一项所述方法的步骤。
17.一种存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-14中任一项所述方法的步骤。
CN202310152864.4A 2023-02-22 2023-02-22 一种滤波器状态监测方法、装置、电子设备及存储介质 Active CN115825999B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310152864.4A CN115825999B (zh) 2023-02-22 2023-02-22 一种滤波器状态监测方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310152864.4A CN115825999B (zh) 2023-02-22 2023-02-22 一种滤波器状态监测方法、装置、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN115825999A CN115825999A (zh) 2023-03-21
CN115825999B true CN115825999B (zh) 2023-05-02

Family

ID=85522137

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310152864.4A Active CN115825999B (zh) 2023-02-22 2023-02-22 一种滤波器状态监测方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN115825999B (zh)

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806338A (zh) * 2016-03-17 2016-07-27 孙红星 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法
CN105824039A (zh) * 2016-03-17 2016-08-03 孙红星 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法
CN105973243A (zh) * 2016-07-26 2016-09-28 广州飞歌汽车音响有限公司 一种车载惯性导航系统
US10809388B1 (en) * 2019-05-01 2020-10-20 Swift Navigation, Inc. Systems and methods for high-integrity satellite positioning
CN112987560A (zh) * 2021-04-19 2021-06-18 长沙智能驾驶研究院有限公司 滤波器控制方法、装置、设备及计算机存储介质
CN113126642A (zh) * 2021-04-26 2021-07-16 广东华南计算技术研究所 基于多mems惯性传感器的偏航角测量系统、方法和飞控系统
CN113534222A (zh) * 2020-04-17 2021-10-22 宝马股份公司 用于车辆定位的方法、用于车辆定位的设备和车辆
CN113805214A (zh) * 2021-08-24 2021-12-17 广东汇天航空航天科技有限公司 组合导航方法、装置、可行驶设备及计算机存储介质
WO2022007437A1 (zh) * 2020-07-04 2022-01-13 华为技术有限公司 传感器安装偏差角的标定方法、组合定位系统和车辆
WO2022037340A1 (zh) * 2020-08-20 2022-02-24 华为技术有限公司 故障检测方法、装置及系统
CN114199236A (zh) * 2021-11-29 2022-03-18 北京百度网讯科技有限公司 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN115047496A (zh) * 2022-04-14 2022-09-13 东南大学 一种用于gnss/ins组合导航卫星同步多故障检测方法
CN115096346A (zh) * 2022-06-17 2022-09-23 安徽蔚来智驾科技有限公司 基于自动驾驶的传感器参数标定方法、装置、介质及车辆

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9791575B2 (en) * 2016-01-27 2017-10-17 Novatel Inc. GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter
US10871579B2 (en) * 2018-11-16 2020-12-22 Swift Navigation, Inc. System and method for satellite positioning

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806338A (zh) * 2016-03-17 2016-07-27 孙红星 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法
CN105824039A (zh) * 2016-03-17 2016-08-03 孙红星 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法
CN105973243A (zh) * 2016-07-26 2016-09-28 广州飞歌汽车音响有限公司 一种车载惯性导航系统
US10809388B1 (en) * 2019-05-01 2020-10-20 Swift Navigation, Inc. Systems and methods for high-integrity satellite positioning
CN113534222A (zh) * 2020-04-17 2021-10-22 宝马股份公司 用于车辆定位的方法、用于车辆定位的设备和车辆
WO2022007437A1 (zh) * 2020-07-04 2022-01-13 华为技术有限公司 传感器安装偏差角的标定方法、组合定位系统和车辆
WO2022037340A1 (zh) * 2020-08-20 2022-02-24 华为技术有限公司 故障检测方法、装置及系统
CN112987560A (zh) * 2021-04-19 2021-06-18 长沙智能驾驶研究院有限公司 滤波器控制方法、装置、设备及计算机存储介质
CN113126642A (zh) * 2021-04-26 2021-07-16 广东华南计算技术研究所 基于多mems惯性传感器的偏航角测量系统、方法和飞控系统
CN113805214A (zh) * 2021-08-24 2021-12-17 广东汇天航空航天科技有限公司 组合导航方法、装置、可行驶设备及计算机存储介质
CN114199236A (zh) * 2021-11-29 2022-03-18 北京百度网讯科技有限公司 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN115047496A (zh) * 2022-04-14 2022-09-13 东南大学 一种用于gnss/ins组合导航卫星同步多故障检测方法
CN115096346A (zh) * 2022-06-17 2022-09-23 安徽蔚来智驾科技有限公司 基于自动驾驶的传感器参数标定方法、装置、介质及车辆

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
GNSS辅助捷联惯导行进间对准自适应滤波方法;刘斌;穆荣军;赵杨杨;米长伟;崔乃刚;;中国惯性技术学报(第05期);577-582 *
组合导航滤波器的在线初值设置及收敛判定方法;吕建强;邓博炜;叶松;高晓颖;;航天控制(第03期);56-61 *

Also Published As

Publication number Publication date
CN115825999A (zh) 2023-03-21

Similar Documents

Publication Publication Date Title
CN110031019B (zh) 一种用于自动驾驶车辆的打滑检测处理方法
CN106687764B (zh) 用于初始化传感器融合系统的方法和系统
CN110030999A (zh) 一种基于惯性导航的定位方法、装置、系统和车辆
CN110834642B (zh) 车辆跑偏识别方法、装置、车辆及存储介质
KR20140093961A (ko) 센서 시스템의 데이터의 정확성을 독립적으로 평가하기 위한 센서 시스템
CN108696368B (zh) 一种网元健康状态的检测方法及设备
CN110843766B (zh) 车辆姿态检测方法、装置、车载终端、车辆和介质
CN109000643A (zh) 导航参数获取方法、车辆急速转弯判断方法、系统、装置
CN110057381A (zh) 一种导航系统的零速修正方法及系统
US20130006528A1 (en) Navigation device and process integrating several hybrid inertial navigation systems
EP4345421A2 (en) Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle
CN112744313B (zh) 一种机器人状态估计方法、装置、可读存储介质及机器人
CN112590874A (zh) 列车轮径校正方法及装置
CN110658542B (zh) 自动驾驶汽车定位识别方法、装置、设备及存储介质
CN112752952A (zh) 惯性测量系统的校准方法、惯性测量系统和可移动平台
CN114179825A (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
Wang et al. High-performance inertial measurements using a redundant array of inexpensive gyroscopes (RAIG)
CN115825999B (zh) 一种滤波器状态监测方法、装置、电子设备及存储介质
CN114076959A (zh) 故障检测方法、装置及系统
CN115388914B (zh) 传感器的参数标定方法、装置、存储介质和电子装置
CN109827572B (zh) 一种检测车位置预测的方法及装置
CN109655057B (zh) 一种六推无人机加速器测量值的滤波优化方法及其系统
CN115900796A (zh) 定位诊断方法、装置、设备及存储介质
CN112874528B (zh) 车速确定方法、车速确定装置、车载终端及存储介质
CN111999750B (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