CN117590441B - 一种完好性保护水平计算方法及相关设备 - Google Patents

一种完好性保护水平计算方法及相关设备 Download PDF

Info

Publication number
CN117590441B
CN117590441B CN202410067050.5A CN202410067050A CN117590441B CN 117590441 B CN117590441 B CN 117590441B CN 202410067050 A CN202410067050 A CN 202410067050A CN 117590441 B CN117590441 B CN 117590441B
Authority
CN
China
Prior art keywords
gnss
protection level
navigation system
integrated navigation
state
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
CN202410067050.5A
Other languages
English (en)
Other versions
CN117590441A (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.)
Shanghai Daoyuan Intelligent System Co ltd
Guangzhou Asensing Technology Co Ltd
Original Assignee
Shanghai Daoyuan Intelligent System Co ltd
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 Shanghai Daoyuan Intelligent System Co ltd, Guangzhou Asensing Technology Co Ltd filed Critical Shanghai Daoyuan Intelligent System Co ltd
Priority to CN202410067050.5A priority Critical patent/CN117590441B/zh
Publication of CN117590441A publication Critical patent/CN117590441A/zh
Application granted granted Critical
Publication of CN117590441B publication Critical patent/CN117590441B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/396—Determining accuracy or reliability of position or pseudorange measurements
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/23—Testing, monitoring, correcting or calibrating of receiver elements
    • 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
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Hardware Design (AREA)
  • Mathematical Physics (AREA)
  • Navigation (AREA)

Abstract

本申请提供一种完好性保护水平计算方法及相关设备，涉及组合导航技术领域。本申请通过将GNSS/INS组合导航系统在当前时刻的实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到该组合导航系统在当前时刻的目标系统误差状态估计向量，同时检测该组合导航系统在当前时刻的载体移动状态及系统运行状态，接着调用与载体移动状态及系统运行状态适配的保护水平计算策略，基于该组合导航系统在当前时刻的前一时刻的历史完好性保护水平和目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到该组合导航系统在当前时刻的期望完好性保护水平，从而确保最终输出的完好性保护水平尽可能地反映真实导航定位误差。

Description

一种完好性保护水平计算方法及相关设备
技术领域
本申请涉及组合导航技术领域，具体而言，涉及一种完好性保护水平计算方法及相关设备。
背景技术
随着科学技术的不断发展，自动驾驶技术在各大行业的应用越发广泛，而对自动驾驶技术来说，高精度导航定位技术属于不可或缺的重要组成部分，其中GNSS（GlobalNavigation Satellite System，全球卫星导航定位系统）导航系统和INS（InertialNavigation System，惯性导航系统）导航系统具备良好的互补性，属于应用最广泛的一种组合导航定位技术，能够为用户提供连续可靠的定位信息。
在GNSS/INS组合导航系统的实际使用过程中，通常需要GNSS/INS组合导航系统在输出导航定位结果的同时，输出与导航定位结果匹配的完好性保护水平（ProtectionLevel，PL），以通过输出的完好性保护水平量化反映输出的导航定位结果的可靠程度，其中当完好性保护水平超过预设的告警门限（Alert Limit, AL），对应组合导航系统将会发出告警信息来提醒用户当前导航定位结果不可用。而对完好性保护水平来说，理论上其应该真实反映导航系统定位误差（Position Error，PE），但因算法本身无法计算出真实定位误差大小，因此通常会出现定位误差大于告警门限但输出的完好性保护水平小于告警门限的现象，出现完好性风险（Integrity Risk，IR），极易在自动驾驶过程中发生风险。因此，如何提供一种可有效表征导航定位误差的完好性保护水平计算方案，便是当今组合导航技术领域中的一项亟需解决的重要问题。
发明内容
有鉴于此，本申请的目的在于提供一种完好性保护水平计算方法及装置、GNSS/INS组合导航系统和可读存储介质，能够利用组合导航卡尔曼滤波原理估计出可表征导航定位误差变化状况的系统误差状态向量，并结合组合导航系统的实际移动状态和实际运行状态进行保护水平快速计算，确保最终输出的完好性保护水平尽可能地反映真实导航定位误差，提高保护水平的数据可靠性和计算效率，同时确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统。
为了实现上述目的，本申请实施例采用的技术方案如下：
第一方面，本申请提供一种完好性保护水平计算方法，应用于GNSS/INS组合导航系统，所述方法包括：
获取所述GNSS/INS组合导航系统在当前时刻的实际导航参数，并将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量；
根据所述实际导航参数，检测所述GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态；
调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平。
在可选的实施方式中，所述将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量的步骤，包括：
调用所述卡尔曼滤波器根据所述实际导航参数，计算所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵和系统噪声驱动矩阵，以及所述GNSS/INS组合导航系统在当前时刻的观测矩阵、观测噪声向量和观测噪声协方差矩阵；
根据所述状态转移矩阵、所述系统噪声驱动矩阵、所述观测矩阵、所述观测噪声向量、所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态向量和历史系统噪声向量，计算所述GNSS/INS组合导航系统在当前时刻的目标观测向量；
根据所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态估计向量、历史系统误差状态协方差矩阵和历史系统噪声协方差矩阵，基于所述状态转移矩阵和所述系统噪声驱动矩阵进行卡尔曼滤波预测，得到所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量和预测系统误差状态协方差矩阵；
根据所述目标观测向量、所述观测矩阵和所述观测噪声协方差矩阵，对所述系统误差状态预测向量和预测系统误差状态协方差矩阵进行卡尔曼滤波观测更新处理，得到所述目标系统误差状态估计向量。
在可选的实施方式中，所述目标观测向量的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标观测向量。
在可选的实施方式中，所述系统误差状态预测向量和所述预测系统误差状态协方差矩阵的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态估计向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的预测系统误差状态协方差矩阵。
在可选的实施方式中，所述卡尔曼滤波观测更新处理的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统在当前时刻的滤波增益矩阵，用于表示所述GNSS/INS组合导航系统在当前时刻的预测系统误差状态协方差矩阵，用于表示所述GNSS/INS组合导航系统在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标观测向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量，/>用于表示单位矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态协方差矩阵。
在可选的实施方式中，所述调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平的步骤，包括：
检测所述载体移动状态是否处于正常移动状态，并检测所述系统运行状态是处于GNSS失效状态还是处于GNSS有效状态；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS有效状态的情况下，获取GNSS导航系统在当前时刻的位置观测误差，并对所述历史完好性保护水平、所述位置观测误差和所述系统位置误差估值进行数据分析，得到所述期望完好性保护水平；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS失效状态的情况下，获取INS导航系统在当前时刻的位置误差发散变量，并根据所述位置误差发散变量、所述历史完好性保护水平和所述系统位置误差估值计算所述期望完好性保护水平。
在可选的实施方式中，所述对所述历史完好性保护水平、所述位置观测误差和所述系统位置误差估值进行数据分析，得到所述期望完好性保护水平的步骤，包括：
检测所述历史完好性保护水平是否小于所述位置观测误差；
若检测到所述历史完好性保护水平小于所述位置观测误差，则对所述系统位置误差估值的绝对值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平；
若检测到所述历史完好性保护水平大于或等于所述位置观测误差，则对所述历史完好性保护水平与所述系统位置误差估值的绝对值进行减法运算，得到所述期望完好性保护水平。
在可选的实施方式中，所述根据所述位置误差发散变量、所述历史完好性保护水平和所述系统位置误差估值计算所述期望完好性保护水平的步骤，包括：
对所述位置误差发散变量与所述系统位置误差估值的绝对值进行减法运算，得到定位误差估值；
对所述定位误差估值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平。
在可选的实施方式中，所述调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平的步骤，还包括：
在检测到所述载体移动状态不处于正常移动状态的情况下，将所述历史完好性保护水平作为所述期望完好性保护水平。
第二方面，本申请提供一种完好性保护水平计算装置，应用于GNSS/INS组合导航系统，所述装置包括：
系统误差估计模块，用于获取所述GNSS/INS组合导航系统在当前时刻的实际导航参数，并将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量；
系统状况检测模块，根据所述实际导航参数，检测所述GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态；
保护水平计算模块，用于调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平。
第三方面，本申请提供一种GNSS/INS组合导航系统，包括主控单元、GNSS导航系统、INS导航系统和卡尔曼滤波器；
所述主控单元与所述GNSS导航系统、所述INS导航系统和所述卡尔曼滤波器同时通信连接，其中所述主控单元存储有计算机程序，并可执行所述计算机程序，来实现前述实施方式中任意一项所述完好性保护水平计算方法。
第四方面，本申请提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被GNSS/INS组合导航系统执行时，实现前述实施方式中任意一项所述的完好性保护水平计算方法。
在此情况下，本申请实施例的有益效果可以包括以下内容：
本申请通过将GNSS/INS组合导航系统在当前时刻的实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量，同时检测GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态，接着调用与载体移动状态及系统运行状态适配的保护水平计算策略，基于GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到GNSS/INS组合导航系统在当前时刻的期望完好性保护水平，从而利用组合导航卡尔曼滤波原理估计出可表征导航定位误差变化状况的系统误差状态向量，并结合组合导航系统的实际移动状态和实际运行状态进行保护水平快速计算，确保最终输出的完好性保护水平尽可能地反映真实导航定位误差，提高保护水平的数据可靠性和计算效率，同时确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的GNSS/INS组合导航系统的系统组成示意图；
图2为本申请实施例提供的完好性保护水平计算方法的流程示意图；
图3为图2中的步骤S210包括的子步骤的流程示意图；
图4为图2中的步骤S230包括的子步骤的流程示意图之一；
图5为图2中的步骤S230包括的子步骤的流程示意图之二；
图6为本申请实施例提供的完好性保护水平计算装置的组成示意图。
图标：10-GNSS/INS组合导航系统；11-GNSS导航系统；12-INS导航系统；13-主控单元；14-卡尔曼滤波器；15-零速修正单元；16-轮速检测单元；17-NHC约束单元；100-完好性保护水平计算装置；110-系统误差估计模块；120-系统状况检测模块；130-保护水平计算模块。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本申请的描述中，可以理解的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
在本申请的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
目前完好性保护水平计算方案通常采用如下几种形式实现：
（1）基于机器学习模型训练的计算方案：筛选与完好性保护水平相关的关键特征，用机器学习模型训练来构建起特征量和完好性保护水平之间的映射关系，使完好性保护水平能够最佳反映导航定位误差，在降低完好性风险的同时提高可用率，满足客户需求。值得注意的是，该方案需要投入大量重复的模型参数进行模型训练，整个模型训练周期较长，同时最终训练出的机器学习模型实质仅适配于对应模型参数所采用的GNSS/INS组合导航系统部署方式，难以得到一套适用于不同部署方式的GNSS/INS组合导航系统的模型。
（2）基于卡尔曼滤波的原始状态协方差矩阵做调整的计算方案：利用原始状态协方差矩阵能够反映估计状态精度的特性，在原始状态协方差矩阵的基础上，根据其他相关特征做一定调整转换得到更加合理的完好性保护水平（PL）值。值得注意的是，该方案类似于机器学习，存在调参工作量大，普适性不高的问题。
（3）基于卡尔曼滤波相关矩阵构建推导模型的计算方案：推导了误差状态模型在卡尔曼滤波情况下的组合导航系统保护级公式，可在滤波过程中通过一系列矩阵运算得到最终的PL值。值得注意的是，该方案存在矩阵计算较复杂且计算量巨大的问题，无法在组合导航系统的实际使用过程中及时且快速地输出合适的PL值。
（4）基于误差核心分布的计算方案：通过统计不同定位误差值各自对应的分布概率，并遍历所有的概率值，计算出不同概率值各自对应的完好性保护水平和实际定位结果，最后按照不同用户需求输出合适的完好性保护水平和具体定位结果。值得注意的是，该方案需要获取足够多的先验数据，来确保统计出误差分布概率足够可靠，因误差分布具有随机性，通常会随环境、硬件等因素变化而发生变化，难以一概而论，致使最终输出的完好性保护水平在不同部署方式的组合导航系统的实际使用过程中的数据可靠性容易出现较大波动，实质无法有效反映真实导航定位误差。
在此情况下，为解决上述问题，本申请实施例通过提供一种完好性保护水平计算方案，以利用组合导航卡尔曼滤波原理估计出可表征导航定位误差变化状况的系统误差状态向量，并结合组合导航系统的实际移动状态和实际运行状态通过常规的逻辑递推关系进行保护水平计算，来确保最终输出的完好性保护水平尽可能地反映真实导航定位误差，提高保护水平的数据可靠性，同时通过利用逻辑递推关系计算保护水平的方式，有效提升保护水平的计算效率，并确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统实现完好性保护水平的快速且可靠输出效果。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。
请参照图1，图1是本申请实施例提供的GNSS/INS组合导航系统10的系统组成示意图。在本申请实施例中，所述GNSS/INS组合导航系统10可以包括GNSS导航系统11、INS导航系统12、主控单元13、卡尔曼滤波器14、零速修正单元15、轮速检测单元16及NHC（Non-Holonomic Constraint，非完整性约束）约束单元17。其中，所述GNSS导航系统11、所述INS导航系统12、所述主控单元13、所述卡尔曼滤波器14、所述零速修正单元15、所述轮速检测单元16及所述NHC约束单元17各个元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，所述GNSS导航系统11、所述INS导航系统12、所述主控单元13、所述卡尔曼滤波器14、所述零速修正单元15、所述轮速检测单元16及所述NHC约束单元17这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。
在本实施例中，所述GNSS导航系统11可通过卫星定位系统技术快速、准确地提供所述GNSS/INS组合导航系统10所在载体（例如，汽车）的速度信息、位置信息、时间信息等导航参数；所述INS导航系统12可通过包括陀螺仪和加速度计的惯性测量单元（InertialMeasurement Unit，IMU）检测载体运动速度、载体运动姿态、载体运动位置、载体姿态变化角速度、载体运动加速度、载体姿态变化加速度等导航参数。
在本实施例中，所述零速修正单元15用于在所述载体处于静止状态且载体速度为零时，利用所述INS导航系统12的解算速度作为系统速度误差的观测量，对其他类型的误差量进行修正，以改善静止状态下的组合导航结果。
在本实施例中，所述轮速检测单元16用于检测所述载体的实际轮速；所述NHC约束单元17用于在所述载体移动过程中，假定载体不出现侧滑、漂移、弹跳等突发性动作时的侧向速度和垂向速度分别为零，并构建虚拟观测量来对所述载体进行运动约束，以确保所述载体避免出现移动事故。
在本实施例中，所述主控单元13可通过获取所述GNSS导航系统11、所述INS导航系统12、所述零速修正单元15、所述轮速检测单元16及所述NHC约束单元17各自在实际使用过程中的导航参数，并基于存储的完好性保护水平计算装置100调用所述卡尔曼滤波器14基于组合导航卡尔曼滤波原理估计出可表征导航定位误差变化状况的系统误差状态向量，并结合组合导航系统的实际移动状态和实际运行状态通过常规的逻辑递推关系进行保护水平计算，来确保最终输出的完好性保护水平尽可能地反映真实导航定位误差，提高保护水平的数据可靠性，同时通过利用逻辑递推关系计算保护水平的方式，有效提升保护水平的计算效率，并确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统实现完好性保护水平的快速且可靠输出效果。其中，所述实际移动状态可以包括暂停移动状态和正常移动状态，其中所述暂停移动状态用于表示所述载体静止，所述正常移动状态用于表示所述载体正常移动；所述实际运行状态可以包括GNSS有效状态和GNSS失效状态，其中所述GNSS有效状态用于表示所述GNSS导航系统11无丢帧现象，所述GNSS失效状态用于表示所述GNSS导航系统11因所述载体经过强磁场区域、遮挡区域或隧道区域等区域出现丢帧现象。
可以理解的是，图1的系统组成图仅为所述GNSS/INS组合导航系统10的一种组成示意图，所述GNSS/INS组合导航系统10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
在本申请中，为确保所述GNSS/INS组合导航系统10能够最终输出的完好性保护水平尽可能地反映真实导航定位误差，并有效提高完好性保护水平的数据可靠性和计算效率，同时提升完好性保护水平计算方案的方案普适性，本申请实施例通过提供一种完好性保护水平计算方法实现前述目的。下面对本申请提供的完好性保护水平计算方法进行详细描述。
请参照图2，图2是本申请实施例提供的完好性保护水平计算方法的流程示意图。在本申请实施例中，所述完好性保护水平计算方法可以包括步骤S210~步骤S230。
步骤S210，获取GNSS/INS组合导航系统在当前时刻的实际导航参数，并将实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量。
在本实施例中，所述实际导航参数可以包括所述GNSS导航系统11、所述INS导航系统12、所述零速修正单元15、所述轮速检测单元16及所述NHC约束单元17各自在实际使用过程中的导航参数，其中所述GNSS导航系统11的导航参数可以包括所述GNSS导航系统11检测到的载体速度信息、载体位置信息及时间信息等，所述INS导航系统12的导航参数可以包括所述INS导航系统12检测到的载体运动速度、载体运动姿态、载体运动位置、载体姿态变化角速度、载体运动加速度、载体姿态变化加速度等，所述轮速检测单元16的导航参数可以是载体轮速信息，所述零速修正单元15的导航参数可以是所述零速修正单元15输出的误差修正信息，所述NHC约束单元17的导航参数可以是所述NHC约束单元17输出的NHC约束信息。
当所述主控单元13得到所述GNSS/INS组合导航系统10在当前时刻的实际导航参数后，会将与当前时刻对应的实际导航参数输入到所述卡尔曼滤波器14，使所述卡尔曼滤波器14基于组合导航卡尔曼滤波原理和与当前时刻对应的实际导航参数进行系统误差状态估计，得到可表征所述GNSS/INS组合导航系统10在当前时刻的导航定位误差变化状况的目标系统误差状态估计向量。
此外，所述主控单元13在得到所述目标系统误差状态估计向量后，会基于所述目标系统误差状态估计向量在导航定位校正环节对所述GNSS/INS组合导航系统10的导航定位结果进行修正，以确保修正后的导航定位结果具备更强的可靠性。
可选地，请参照图3，图3是图2中的步骤S210包括的子步骤的流程示意图。在本实施例中，所述步骤S210包括的步骤“将实际导航参数输入到卡尔曼滤波器14中进行系统误差状态估计，得到GNSS/INS组合导航系统10在当前时刻的目标系统误差状态估计向量”的步骤可以包括子步骤S211~子步骤S214，以确保最终估计出的目标系统误差状态估计向量能够有效表征所述GNSS/INS组合导航系统10在当前时刻的导航定位误差变化状况。
子步骤S211，调用卡尔曼滤波器根据实际导航参数，计算GNSS/INS组合导航系统从前一时刻到当前时刻的状态转移矩阵和系统噪声驱动矩阵，以及GNSS/INS组合导航系统在当前时刻的观测矩阵、观测噪声向量和观测噪声协方差矩阵。
在本实施例中，所述卡尔曼滤波器14可在获取到与当前时刻对应的实际导航参数后，从数据存储单元处获取所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史导航参数，并通过将所述实际导航参数与所述历史导航参数进行数据比对的方式，对应生成所述GNSS/INS组合导航系统10从所述前一时刻到当前时刻的状态转移矩阵和系统噪声驱动矩阵，以及所述GNSS/INS组合导航系统10在当前时刻的观测矩阵、观测噪声向量和观测噪声协方差矩阵。
子步骤S212，根据状态转移矩阵、系统噪声驱动矩阵、观测矩阵、观测噪声向量、GNSS/INS组合导航系统在前一时刻的历史系统误差状态向量和历史系统噪声向量，计算GNSS/INS组合导航系统在当前时刻的目标观测向量。
在本实施例中，所述卡尔曼滤波器14可通过调用与系统误差状态相关的系统状态方程和观测方程，对所述状态转移矩阵、所述系统噪声驱动矩阵、所述观测矩阵、所述观测噪声向量、所述GNSS/INS组合导航系统10在前一时刻的历史系统误差状态向量和历史系统噪声向量进行方程求解处理，得到所述GNSS/INS组合导航系统10在当前时刻的目标观测向量。此时所述目标观测向量的计算过程可采用前述系统状态方程和前述观测方程进行表达，此时前述系统状态方程和前述观测方程可采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统10在当前时刻的目标系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统10从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统10从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史系统噪声向量，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的观测矩阵，用于表示所述GNSS/INS组合导航系统10在当前时刻的观测噪声向量，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的目标观测向量。
子步骤S213，根据GNSS/INS组合导航系统在前一时刻的历史系统误差状态估计向量、历史系统误差状态协方差矩阵和历史系统噪声协方差矩阵，基于状态转移矩阵和系统噪声驱动矩阵进行卡尔曼滤波预测，得到GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量和预测系统误差状态协方差矩阵。
其中，所述系统误差状态预测向量和所述预测系统误差状态协方差矩阵的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统10在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统10从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史系统误差状态估计向量，/>用于表示所述GNSS/INS组合导航系统10从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史系统误差状态协方差矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史系统噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的预测系统误差状态协方差矩阵。
子步骤S214，根据目标观测向量、观测矩阵和观测噪声协方差矩阵，对系统误差状态预测向量和预测系统误差状态协方差矩阵进行卡尔曼滤波观测更新处理，得到目标系统误差状态估计向量。
其中，所述卡尔曼滤波观测更新处理的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统10在当前时刻的滤波增益矩阵，用于表示所述GNSS/INS组合导航系统10在当前时刻的预测系统误差状态协方差矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的观测噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的目标观测向量，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的目标系统误差状态估计向量，/>用于表示单位矩阵，/>用于表示所述GNSS/INS组合导航系统10在当前时刻的目标系统误差状态协方差矩阵。
由此，本申请可通过执行上述子步骤S211~子步骤S214，确保最终估计出的目标系统误差状态估计向量能够有效表征所述GNSS/INS组合导航系统10在当前时刻的导航定位误差变化状况。
步骤S220，根据实际导航参数，检测GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态。
在本实施例中，所述主控单元13可通过对所述INS导航系统12在当前时刻的导航参数进行载体静态检测，以确定当前时刻的载体移动状态是暂停移动状态还是正常移动状态；所述主控单元13可通过判断所述GNSS导航系统11在当前时刻的导航参数是否为空，来确定所述GNSS导航系统11当前是否丢帧，以确定当前时刻的系统运行状态是GNSS失效状态还是GNSS有效状态。
步骤S230，调用与载体移动状态及系统运行状态适配的保护水平计算策略，基于GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到GNSS/INS组合导航系统在当前时刻的期望完好性保护水平。
在本实施例中，所述主控单元13可以预先存储有所述GNSS/INS组合导航系统10在不同载体移动状态和/或不同系统运行状态下的保护水平计算策略，其中每种保护水平计算策略单独对应一种涉及保护水平计算操作的逻辑递推关系，每种保护水平计算策略可由组合导航系统研发人员根据保护水平精度要求进行不同的配置，每种保护水平计算策略所表征的逻辑递推关系可以涉及除所述历史完好性保护水平、所述系统位置误差估值和所述期望完好性保护水平以外的其他参数，例如保护水平门限值、所述GNSS导航系统11在当前时刻的位置观测误差、观测误差阈值、所述INS导航系统12在当前时刻的位置误差发散变量、保护水平输出最小值等。
因此，当所述主控单元13得到所述GNSS/INS组合导航系统10在当前时刻的载体移动状态及系统运行状态后，会从存储的多种保护水平计算策略中筛选出与所述载体移动状态及系统运行状态适配的目标保护水平计算策略，并从所述GNSS/INS组合导航系统10在当前时刻的标系统误差状态估计向量中提取出与载体位置相关的系统位置误差估值，而后基于所述系统位置误差估值和所述GNSS/INS组合导航系统10在当前时刻的前一时刻的历史完好性保护水平，按照所述目标保护水平计算策略进行逻辑递推计算，以确保计算出的与当前时刻对应的期望完好性保护水平能够尽可能地反映真实导航定位误差，从而有效提高完好性保护水平的数据可靠性和计算效率，同时确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统实现完好性保护水平的快速且可靠输出效果。
可以理解的是，为确保最终计算出的期望完好性保护水平的数据可靠性，所述GNSS/INS组合导航系统10在系统初始化完成后输出的起始完好性保护水平应该维持在相对准确的状态。其中，所述起始完好性保护水平可以在所述INS导航系统12对准初始化完成时赋值，所述起始完好性保护水平通常来源于所述GNSS导航系统11的定位结果噪声，其中可在所述INS导航系统12对准初始化过程中，通过限制所述GNSS导航系统11的数据质量，来将所述起始完好性保护水平控制到小数值的状态，使完好性保护水平计算精度被控制在厘米级精度。
由此，本申请可通过执行上述步骤S210~步骤S230，确保最终输出的完好性保护水平能够尽可能地反映真实导航定位误差，并有效提高完好性保护水平的数据可靠性和计算效率，同时提升完好性保护水平计算方案的方案普适性。
可选地，请参照图4，图4是图2中的步骤S230包括的子步骤的流程示意图之一。在本申请实施例中，为确保计算出的所述期望完好性保护水平符合保护水平输出规范，且能够尽可能地反映真实导航定位误差，本申请实施例针对不同载体移动状态和/或不同系统运行状态分别配置有合适的通用型保护水平计算策略，此时所述步骤S230可以包括子步骤S231~子步骤S233。
子步骤S231，检测载体移动状态是否处于正常移动状态，并检测系统运行状态是处于GNSS失效状态还是处于GNSS有效状态。
子步骤S232，在检测到载体移动状态处于正常移动状态，且系统运行状态处于GNSS有效状态的情况下，获取GNSS导航系统在当前时刻的位置观测误差，并对历史完好性保护水平、位置观测误差和系统位置误差估值进行数据分析，得到期望完好性保护水平。
其中，与所述正常移动状态和GNSS有效状态同时匹配的通用型保护水平计算策略可以至少涉及所述GNSS导航系统11在当前时刻的位置观测误差，则所述主控单元13可从所述卡尔曼滤波器14计算出的与当前时刻对应的观测噪声协方差矩阵中提取出所述位置观测误差，而后按照前述通用型保护水平计算策略对所述位置观测误差（采用进行表示）、所述历史完好性保护水平（采用/>进行表示）和所述系统位置误差估值（采用/>进行表示）进行数据分析，以得到符合保护水平输出规范的期望完好性保护水平（采用/>进行表示）。
可选地，与所述正常移动状态和GNSS有效状态同时匹配的通用型保护水平计算策略可以涉及保护水平门限值和观测误差阈值，对应通用型保护水平计算策略可以是“判断所述历史完好性保护水平与所述保护水平门限值之差的绝对值是否小于预设数值，并判断所述位置观测误差是否大于所述观测误差阈值；在所述历史完好性保护水平与所述保护水平门限值之差的绝对值小于预设数值，和/或所述位置观测误差大于所述观测误差阈值的情况下，直接将所述历史完好性保护水平作为所述期望完好性保护水平；在所述历史完好性保护水平与所述保护水平门限值之差的绝对值大于或等于预设数值，且所述位置观测误差小于或等于所述观测误差阈值的情况下，若所述历史完好性保护水平小于所述位置观测误差，则采用公式‘’计算所述期望完好性保护水平，否则采用公式‘’计算所述期望完好性保护水平”。
可选地，与所述正常移动状态和GNSS有效状态同时匹配的通用型保护水平计算策略可以涉及保护水平输出最小值，对应通用型保护水平计算策略可以是“检测所述历史完好性保护水平是否小于所述位置观测误差；在检测到所述历史完好性保护水平小于所述位置观测误差的情况下，采用公式‘’计算得到待输出完整性保护水平（采用/>进行表示），接着判断该待输出完整性保护水平是否小于所述保护水平输出最小值，并判定该待输出完整性保护水平小于所述保护水平输出最小值时，将所述保护水平输出最小值作为所述期望完好性保护水平，否则将该待输出完整性保护水平作为所述期望完好性保护水平；在检测到所述历史完好性保护水平大于或等于所述位置观测误差的情况下，采用公式‘/>’计算得到待输出完整性保护水平，接着判断该待输出完整性保护水平是否小于所述保护水平输出最小值，并判定该待输出完整性保护水平小于所述保护水平输出最小值时，将所述保护水平输出最小值作为所述期望完好性保护水平，否则将该待输出完整性保护水平作为所述期望完好性保护水平”。
可选地，与所述正常移动状态和GNSS有效状态同时匹配的通用型保护水平计算策略可以仅涉及位置观测误差，对应通用型保护水平计算策略可以是“检测所述历史完好性保护水平是否小于所述位置观测误差；在检测到所述历史完好性保护水平小于所述位置观测误差的情况下，采用公式‘’计算所述期望完好性保护水平，否则采用公式‘/>’计算所述期望完好性保护水平”，此时所述子步骤S232包括的步骤“对历史完好性保护水平、位置观测误差和系统位置误差估值进行数据分析，得到期望完好性保护水平”可以包括：
检测所述历史完好性保护水平是否小于所述位置观测误差；
若检测到所述历史完好性保护水平小于所述位置观测误差，则对所述系统位置误差估值的绝对值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平；
若检测到所述历史完好性保护水平大于或等于所述位置观测误差，则对所述历史完好性保护水平与所述系统位置误差估值的绝对值进行减法运算，得到所述期望完好性保护水平。
可以理解的是，为确保最终计算出的期望完好性保护水平的数据可靠性，所述GNSS/INS组合导航系统10在系统初始化完成后输出的起始位置观测误差应该维持在相对准确的状态。其中，所述起始位置观测误差可基于所述GNSS导航系统11单独计算出的相对准确合理的完好性输出结果，以及所述INS导航系统12的数据质量控制策略进行配置得到。
子步骤S233，在检测到载体移动状态处于正常移动状态，且系统运行状态处于GNSS失效状态的情况下，获取INS导航系统在当前时刻的位置误差发散变量，并根据位置误差发散变量、历史完好性保护水平和系统位置误差估值计算期望完好性保护水平。
其中，与所述正常移动状态和GNSS失效状态同时匹配的通用型保护水平计算策略可以至少涉及所述INS导航系统12在当前时刻的位置误差发散变量（采用进行表示），其中所述位置误差发散变量的具体数值与所述INS导航系统12包括的IMU性能、发散持续时长等因素实时相关，则所述主控单元13可按照前述通用型保护水平计算策略对所述位置误差发散变量、所述历史完好性保护水平（采用/>进行表示）和所述系统位置误差估值（采用进行表示）进行逻辑递推计算，以得到符合保护水平输出规范的期望完好性保护水平（采用/>进行表示）。
可选地，与所述正常移动状态和GNSS失效状态同时匹配的通用型保护水平计算策略可以涉及保护水平输出最小值，对应通用型保护水平计算策略可以是“采用公式‘’计算得到待输出完整性保护水平（采用/>进行表示），接着判断该待输出完整性保护水平是否小于所述保护水平输出最小值，并判定该待输出完整性保护水平小于所述保护水平输出最小值时，将所述保护水平输出最小值作为所述期望完好性保护水平，否则将该待输出完整性保护水平作为所述期望完好性保护水平”。
可选地，与所述正常移动状态和GNSS失效状态同时匹配的通用型保护水平计算策略可以仅涉及位置误差发散变量，对应通用型保护水平计算策略可以是“采用公式‘’计算所述期望完好性保护水平”，此时所述子步骤S233包括的步骤“根据位置误差发散变量、历史完好性保护水平和系统位置误差估值计算期望完好性保护水平”可以包括：
对所述位置误差发散变量与所述系统位置误差估值的绝对值进行减法运算，得到定位误差估值；
对所述定位误差估值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平。
其中，所述定位误差估值采用公式“”计算得到。
可选地，请参照图5，图5是图2中的步骤S230包括的子步骤的流程示意图之二。在本实施例中，与图4所述的步骤S230相比，图5所示的步骤S230还可以包括子步骤S234。
子步骤S234，在检测到载体移动状态不处于正常移动状态的情况下，将历史完好性保护水平作为期望完好性保护水平。
其中，当检测到所述载体移动状态不处于正常移动状态，即表明所述载体移动状态属于暂停移动状态，此时与所述暂停移动状态匹配的通用型保护水平计算策略可以基于位置保持机制进行配置，对应通用型保护水平计算策略可以是“直接将所述历史完好性保护水平作为所述期望完好性保护水平（即‘’）”。
由此，本申请可通过上述子步骤S231~子步骤S234，针对不同载体移动状态和/或不同系统运行状态分别配置有合适的通用型保护水平计算策略，以确保最终计算出的期望完好性保护水平符合保护水平输出规范，且能够尽可能地反映真实导航定位误差。
在本申请中，为确保上述GNSS/INS组合导航系统10所包括的主控单元13能够执行实现上述任意一种完好性保护水平计算方法，本申请通过对以软件或固件的形式存储于所述主控单元13内的完好性保护水平计算装置100进行功能模块划分的方式，使所述主控单元13能够通过运行所述完好性保护水平计算装置100所包括的软件功能模块或计算机程序执行上述完好性保护水平计算方法。下面对本申请提供的完好性保护水平计算装置100的具体组成进行相应描述。
请参照图6，图6是本申请实施例提供的完好性保护水平计算装置100的组成示意图。在本申请实施例中，所述完好性保护水平计算装置100可以包括系统误差估计模块110、系统状况检测模块120及保护水平计算模块130。
系统误差估计模块110，用于获取GNSS/INS组合导航系统在当前时刻的实际导航参数，并将实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量。
系统状况检测模块120，根据实际导航参数，检测GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态。
保护水平计算模块130，用于调用与载体移动状态及系统运行状态适配的保护水平计算策略，基于GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到GNSS/INS组合导航系统在当前时刻的期望完好性保护水平。
需要说明的是，本申请实施例所提供的完好性保护水平计算装置100，其基本原理及产生的技术效果与前述的完好性保护水平计算方法相同。为简要描述，本实施例部分未提及之处，可参考上述的针对完好性保护水平计算方法的描述内容。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。本申请提供的各项功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个可读存储介质中，包括若干指令用以使得上述GNSS/INS组合导航系统10或者安装有不同部署方式的GNSS/INS组合导航系统10的可移动载体（例如，汽车）执行本申请各个实施例记载方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
综上所述，在本申请实施例提供的一种完好性保护水平计算方法及装置、GNSS/INS组合导航系统和可读存储介质中，本申请通过将GNSS/INS组合导航系统在当前时刻的实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量，同时检测GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态，接着调用与载体移动状态及系统运行状态适配的保护水平计算策略，基于GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到GNSS/INS组合导航系统在当前时刻的期望完好性保护水平，从而利用组合导航卡尔曼滤波原理估计出可表征导航定位误差变化状况的系统误差状态向量，并结合组合导航系统的实际移动状态和实际运行状态进行保护水平快速计算，确保最终输出的完好性保护水平尽可能地反映真实导航定位误差，提高保护水平的数据可靠性和计算效率，同时确保本申请提供的保护水平计算方案能够适用于部署方式不同的GNSS/INS组合导航系统实现完好性保护水平的快速且可靠输出效果。
以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应当以权利要求的保护范围为准。

Claims (7)

1.一种完好性保护水平计算方法，其特征在于，应用于GNSS/INS组合导航系统，所述方法包括：
获取所述GNSS/INS组合导航系统在当前时刻的实际导航参数，并将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量；
根据所述实际导航参数，检测所述GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态；
调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平；
其中，所述将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量的步骤，包括：
调用所述卡尔曼滤波器根据所述实际导航参数，计算所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵和系统噪声驱动矩阵，以及所述GNSS/INS组合导航系统在当前时刻的观测矩阵、观测噪声向量和观测噪声协方差矩阵；
根据所述状态转移矩阵、所述系统噪声驱动矩阵、所述观测矩阵、所述观测噪声向量、所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态向量和历史系统噪声向量，计算所述GNSS/INS组合导航系统在当前时刻的目标观测向量；所述目标观测向量的计算过程采用进行表示，其中/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标观测向量；
根据所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态估计向量、历史系统误差状态协方差矩阵和历史系统噪声协方差矩阵，基于所述状态转移矩阵和所述系统噪声驱动矩阵进行卡尔曼滤波预测，得到所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量和预测系统误差状态协方差矩阵；
根据所述目标观测向量、所述观测矩阵和所述观测噪声协方差矩阵，对所述系统误差状态预测向量和预测系统误差状态协方差矩阵进行卡尔曼滤波观测更新处理，得到所述目标系统误差状态估计向量；
同时，所述调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平的步骤，包括：
检测所述载体移动状态是否处于正常移动状态，并检测所述系统运行状态是处于GNSS失效状态还是处于GNSS有效状态；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS有效状态的情况下，获取GNSS导航系统在当前时刻的位置观测误差，并对所述历史完好性保护水平、所述位置观测误差和所述系统位置误差估值进行数据分析，得到所述期望完好性保护水平；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS失效状态的情况下，获取INS导航系统在当前时刻的位置误差发散变量，并根据所述位置误差发散变量、所述历史完好性保护水平和所述系统位置误差估值计算所述期望完好性保护水平。
2.根据权利要求1所述的方法，其特征在于，所述系统误差状态预测向量和所述预测系统误差状态协方差矩阵的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态估计向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的预测系统误差状态协方差矩阵。
3.根据权利要求1所述的方法，其特征在于，所述卡尔曼滤波观测更新处理的计算过程采用如下式子进行表示：
；
其中，用于表示所述GNSS/INS组合导航系统在当前时刻的滤波增益矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的预测系统误差状态协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测噪声协方差矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标观测向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量，/>用于表示单位矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态协方差矩阵。
4.根据权利要求1-3中任意一项所述的方法，其特征在于，所述对所述历史完好性保护水平、所述位置观测误差和所述系统位置误差估值进行数据分析，得到所述期望完好性保护水平的步骤，包括：
检测所述历史完好性保护水平是否小于所述位置观测误差；
若检测到所述历史完好性保护水平小于所述位置观测误差，则对所述系统位置误差估值的绝对值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平；
若检测到所述历史完好性保护水平大于或等于所述位置观测误差，则对所述历史完好性保护水平与所述系统位置误差估值的绝对值进行减法运算，得到所述期望完好性保护水平。
5.根据权利要求1-3中任意一项所述的方法，其特征在于，所述根据所述位置误差发散变量、所述历史完好性保护水平和所述系统位置误差估值计算所述期望完好性保护水平的步骤，包括：
对所述位置误差发散变量与所述系统位置误差估值的绝对值进行减法运算，得到定位误差估值；
对所述定位误差估值和所述历史完好性保护水平进行加法运算，得到所述期望完好性保护水平。
6.根据权利要求1-3中任意一项所述的方法，其特征在于，所述调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平的步骤，还包括：
在检测到所述载体移动状态不处于正常移动状态的情况下，将所述历史完好性保护水平作为所述期望完好性保护水平。
7.一种完好性保护水平计算装置，其特征在于，应用于GNSS/INS组合导航系统，所述装置包括：
系统误差估计模块，用于获取所述GNSS/INS组合导航系统在当前时刻的实际导航参数，并将所述实际导航参数输入到卡尔曼滤波器中进行系统误差状态估计，得到所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态估计向量；
系统状况检测模块，根据所述实际导航参数，检测所述GNSS/INS组合导航系统在当前时刻的载体移动状态及系统运行状态；
保护水平计算模块，用于调用与所述载体移动状态及所述系统运行状态适配的保护水平计算策略，基于所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史完好性保护水平和所述目标系统误差状态估计向量包括的系统位置误差估值进行保护水平计算，得到所述GNSS/INS组合导航系统在当前时刻的期望完好性保护水平；
其中，所述系统误差估计模块具体用于：
调用所述卡尔曼滤波器根据所述实际导航参数，计算所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵和系统噪声驱动矩阵，以及所述GNSS/INS组合导航系统在当前时刻的观测矩阵、观测噪声向量和观测噪声协方差矩阵；
根据所述状态转移矩阵、所述系统噪声驱动矩阵、所述观测矩阵、所述观测噪声向量、所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态向量和历史系统噪声向量，计算所述GNSS/INS组合导航系统在当前时刻的目标观测向量；所述目标观测向量的计算过程采用进行表示，其中/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的状态转移矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统误差状态向量，/>用于表示所述GNSS/INS组合导航系统从所述前一时刻到当前时刻的系统噪声驱动矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的前一时刻的历史系统噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测矩阵，/>用于表示所述GNSS/INS组合导航系统在当前时刻的观测噪声向量，/>用于表示所述GNSS/INS组合导航系统在当前时刻的目标观测向量；
根据所述GNSS/INS组合导航系统在所述前一时刻的历史系统误差状态估计向量、历史系统误差状态协方差矩阵和历史系统噪声协方差矩阵，基于所述状态转移矩阵和所述系统噪声驱动矩阵进行卡尔曼滤波预测，得到所述GNSS/INS组合导航系统在当前时刻的系统误差状态预测向量和预测系统误差状态协方差矩阵；
根据所述目标观测向量、所述观测矩阵和所述观测噪声协方差矩阵，对所述系统误差状态预测向量和预测系统误差状态协方差矩阵进行卡尔曼滤波观测更新处理，得到所述目标系统误差状态估计向量；
同时，所述保护水平计算模块具体用于：
检测所述载体移动状态是否处于正常移动状态，并检测所述系统运行状态是处于GNSS失效状态还是处于GNSS有效状态；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS有效状态的情况下，获取GNSS导航系统在当前时刻的位置观测误差，并对所述历史完好性保护水平、所述位置观测误差和所述系统位置误差估值进行数据分析，得到所述期望完好性保护水平；
在检测到所述载体移动状态处于正常移动状态，且所述系统运行状态处于GNSS失效状态的情况下，获取INS导航系统在当前时刻的位置误差发散变量，并根据所述位置误差发散变量、所述历史完好性保护水平和所述系统位置误差估值计算所述期望完好性保护水平。
CN202410067050.5A 2024-01-16 2024-01-16 一种完好性保护水平计算方法及相关设备 Active CN117590441B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410067050.5A CN117590441B (zh) 2024-01-16 2024-01-16 一种完好性保护水平计算方法及相关设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410067050.5A CN117590441B (zh) 2024-01-16 2024-01-16 一种完好性保护水平计算方法及相关设备

Publications (2)

Publication Number Publication Date
CN117590441A CN117590441A (zh) 2024-02-23
CN117590441B true CN117590441B (zh) 2024-04-30

Family

ID=89920398

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410067050.5A Active CN117590441B (zh) 2024-01-16 2024-01-16 一种完好性保护水平计算方法及相关设备

Country Status (1)

Country Link
CN (1) CN117590441B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806339A (zh) * 2016-05-14 2016-07-27 中卫物联成都科技有限公司 一种基于gnss、ins和守时系统的组合导航方法和设备
WO2018059532A1 (zh) * 2016-09-30 2018-04-05 华为技术有限公司 观测时滞系统的组合导航数据解算方法、装置及导航设备
KR102113593B1 (ko) * 2019-04-30 2020-05-21 한국과학기술원 칼만필터의 상태 예측단계에 사용되는 센서 고장에 대한 항법 보호수준 산출 방법
CN115265594A (zh) * 2022-07-18 2022-11-01 北京航空航天大学 多源pnt信息弹性融合导航多级自主完好性监测方法和系统
CN117146806A (zh) * 2023-06-07 2023-12-01 武汉大学 一种位姿、安装角的矩阵李群估计方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806339A (zh) * 2016-05-14 2016-07-27 中卫物联成都科技有限公司 一种基于gnss、ins和守时系统的组合导航方法和设备
WO2018059532A1 (zh) * 2016-09-30 2018-04-05 华为技术有限公司 观测时滞系统的组合导航数据解算方法、装置及导航设备
KR102113593B1 (ko) * 2019-04-30 2020-05-21 한국과학기술원 칼만필터의 상태 예측단계에 사용되는 센서 고장에 대한 항법 보호수준 산출 방법
CN115265594A (zh) * 2022-07-18 2022-11-01 北京航空航天大学 多源pnt信息弹性融合导航多级自主完好性监测方法和系统
CN117146806A (zh) * 2023-06-07 2023-12-01 武汉大学 一种位姿、安装角的矩阵李群估计方法及系统

Also Published As

Publication number Publication date
CN117590441A (zh) 2024-02-23

Similar Documents

Publication Publication Date Title
JP6576578B2 (ja) 車両を制御する方法、システム、及び非一時的コンピューター可読メモリ
US9726499B2 (en) Method of determining a radius of protection associated with a navigation parameter of a hybrid inertial navigation system, and associated system
EP1760431B1 (en) Inertial navigation system with a plurality of Kalman filters and vehicle equipped with such a system
Xia et al. Autonomous vehicles sideslip angle estimation: Single antenna GNSS/IMU fusion with observability analysis
US20090299494A1 (en) System and Method of Improved Kalman Filtering for Estimating the State of a Dynamic System
US20140236445A1 (en) Method for Estimating Tire Parameters for a Vehicle
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
CN111024124B (zh) 一种多传感器信息融合的组合导航故障诊断方法
EP3770554A1 (en) Apparatus and method for terrain aided navigation using inertial position
EP3171132B1 (en) Methods for attitude and heading reference system to mitigate vehicle acceleration effects
JP2022513511A (ja) 完全性範囲を特定するための方法
KR20210102970A (ko) 파라미터 추정의 무결성 범위의 적응식 결정 방법
FR3023918A1 (fr) Procede d'estimation de la vitesses d'un aeronef par rapport a l'air environnant, et systeme associe
WO2017141469A1 (ja) 位置推定装置
CN104819717A (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
US10889302B2 (en) Methods for monitoring the output performance of state estimators in navigation systems
CN117590441B (zh) 一种完好性保护水平计算方法及相关设备
KR20200115710A (ko) 딥 앙상블과 칼만필터를 통합한 방식의 차량 사이드슬립 앵글 추정 방법 및 장치
Almagbile et al. An integrated adaptive Kalman filter for improving the reliability of navigation systems
EP3855117A1 (en) Terrain referenced navigation system with generic terrain sensors for correcting an inertial navigation solution
JP7028223B2 (ja) 自己位置推定装置
CN113959433A (zh) 一种组合导航方法及装置
Berntorp et al. Offset and noise estimation of automotive-grade sensors using adaptive particle filtering
CN114076964A (zh) 用于借助于卡尔曼滤波器确定至少一个系统状态的方法
Da Lio et al. Robust and sample-efficient estimation of vehicle lateral velocity using neural networks with explainable structure informed by kinematic principles

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