CN113959433A - 一种组合导航方法及装置 - Google Patents

一种组合导航方法及装置 Download PDF

Info

Publication number
CN113959433A
CN113959433A CN202111086062.5A CN202111086062A CN113959433A CN 113959433 A CN113959433 A CN 113959433A CN 202111086062 A CN202111086062 A CN 202111086062A CN 113959433 A CN113959433 A CN 113959433A
Authority
CN
China
Prior art keywords
error
navigation system
inertial navigation
current moment
kalman filter
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.)
Granted
Application number
CN202111086062.5A
Other languages
English (en)
Other versions
CN113959433B (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.)
Shenzhen Digital Power Grid Research Institute of China Southern Power Grid Co Ltd
Original Assignee
Shenzhen Digital Power Grid Research Institute of China Southern Power Grid 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 Shenzhen Digital Power Grid Research Institute of China Southern Power Grid Co Ltd filed Critical Shenzhen Digital Power Grid Research Institute of China Southern Power Grid Co Ltd
Priority to CN202111086062.5A priority Critical patent/CN113959433B/zh
Publication of CN113959433A publication Critical patent/CN113959433A/zh
Application granted granted Critical
Publication of CN113959433B publication Critical patent/CN113959433B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Computational Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computer Hardware Design (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Graphics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Operations Research (AREA)
  • Multimedia (AREA)
  • Algebra (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Databases & Information Systems (AREA)
  • Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种组合导航方法及装置，该方法应用在包括惯性导航系统和卫星定位系统的组合导航系统中，该方法包括根据惯性导航系统的误差模型和卫星定位系统的误差模型，以及惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定多维卡尔曼滤波方程；将惯性导航系统和卫星定位系统测量的观测误差确定为该卡尔曼滤波方程的量测值；通过该卡尔曼滤波方程进行迭代计算，对惯性导航系统输出的测量值进行校正，得到当前时刻的最优导航估计值。可见，本发明能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度。

Description

一种组合导航方法及装置
技术领域
本发明涉及导航技术领域，尤其涉及一种组合导航方法及装置。
背景技术
随着导航技术的不断发展，SINS/GPS(惯性/卫星组合导航系统)在航空领域得到了广泛的应用。
实际应用中，传统的SINS/GPS组合导航方法通常将SINS作为量测系统，可以得到连续变化的位置、速度、姿态等数据，GPS作为辅助系统主要是提供外部观测信息对SINS的数据进行校正，使用数据融合算法进行导航数据计算，可以在已知系统噪声统计特性的情况下实现导航系统状态的最优估计。然而实践发现，在机载使用环境中，GPS信号因为黑障等影响而导致解算的位置、速度信号存在跳变，以致无法获取更精准的外部测量信息，这会导致组合导航系统估计精度下降，导航数据的准确度降低。
因此，提供一种有效的组合导航方法以提高导航的精度和准确度的显得尤为重要。
发明内容
本发明提供了一种组合导航方法及装置，能够提供一种多维状态估计的组合导航方案，在GPS测量信号可能存在跳变时，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度，同时对惯性导航系统进行在线偏差校正，进一步提高测量精度和准确度。
为了解决上述技术问题，本发明第一方面公开了一种组合导航方法，所述组合导航方法应用在组合导航系统中，所述组合导航系统包括惯性导航系统和卫星定位系统，所述方法包括：
根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程；
根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程；
根据当前时刻所述惯性导航系统输出的第一测量数据与所述卫星定位系统输出的第二测量数据，确定所述当前时刻的观测误差；其中，所述第一测量数据和所述第二测量数据均包括位置数据和速度数据，所述观测误差包括位置误差和速度误差；
将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差；
根据所述当前时刻的最优估计误差，对所述当前时刻的第一测量数据进行校正，得到所述当前时刻的最优导航估计值，所述最优导航估计值包括最优导航位置值以及最优导航速度值。
作为一种可选的实施方式，在本发明第一方面中，所述根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程，包括：
根据所述惯性导航系统的误差模型，确定所述惯性导航系统的误差状态方程：
Figure BDA0003265584790000021
其中，
Figure BDA0003265584790000022
为所述惯性导航系统的误差状态向量，FI(t)为所述惯性导航系统的状态矩阵，XI(t)为所述惯性导航系统的误差状态量矩阵，GI(t)为所述惯性导航系统的噪声驱动矩阵，WI(t)为所述惯性导航系统的状态白噪声向量，其中XI(t)具体为：
Figure BDA0003265584790000023
其中，φE、φN、φU分别为所述惯性导航系统的三个维度方向的姿态误差，δE、δN、δU分别为所述惯性导航系统的三个维度方向的速度误差，
Figure BDA0003265584790000024
δλ、δh分别为所述惯性导航系统的三个维度方向的位置误差，εx、εy、εz分别为所述惯性导航系统的陀螺仪在三个维度方向上的误差，
Figure BDA0003265584790000025
分别为所述惯性导航系统的加速度计在三个维度方向上的误差；
根据所述卫星定位系统的误差模型，确定所述卫星定位系统的误差状态方程：
Figure BDA0003265584790000031
其中，
Figure BDA0003265584790000032
为所述惯性导航系统的误差状态向量，FS(t)为所述惯性导航系统的状态转移矩阵，XS(t)为所述惯性导航系统的误差状态量矩阵，GS(t)为所述惯性导航系统的噪声驱动矩阵，WS(t)为所述惯性导航系统的状态白噪声向量，其中XS(t)具体为：
Figure BDA0003265584790000033
其中，
Figure BDA0003265584790000034
分别为所述惯性导航系统的三个维度方向上的速度误差，
Figure BDA0003265584790000035
δλS、δhS分别为所述惯性导航系统的三个维度方向上的位置误差；
根据所述惯性导航系统的误差状态方程以及所述卫星定位系统的误差状态方程，确定卡尔曼滤波器的状态方程：
Figure BDA0003265584790000036
作为一种可选的实施方式，在本发明第一方面中，所述根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程，包括：
根据所述惯性导航系统和所述卫星定位系统分别测量的位置之差形成的位置差模型以及分别测量的速度之差形成的速度差模型，确定所述卡尔曼滤波器的量测向量：
Figure BDA0003265584790000037
其中，vEI、vNI、vUI分别为所述惯性导航系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000038
λI、hI分别为所述惯性导航系统测量得到的纬度、经度、高度，vES、vNS、vUS分别为所述卫星定位系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000039
λS、hS分别为所述卫星定位系统度测量得到的纬度、经度、高度；
根据所述卡尔曼滤波器的量测值，确定所述卡尔曼滤波器的量测方程：
Figure BDA0003265584790000041
其中，H为观测矩阵，
Figure BDA0003265584790000042
为所述卡尔曼滤波器的状态向量，V为量测白噪声向量，其中
Figure BDA0003265584790000043
具体为：
Figure BDA0003265584790000044
作为一种可选的实施方式，在本发明第一方面中，所述将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差，包括：
将所述当前时刻的观测误差输入至所述卡尔曼滤波器，根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值，所述先验估计值包括所述当前时刻的状态估计误差和所述当前时刻的协方差预估矩阵；
根据所述当前时刻的先验估计值和所述当前时刻的观测误差，对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差。
作为一种可选的实施方式，在本发明第一方面中，所述对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差，包括：
根据所述当前时刻的协方差预估矩阵，确定所述当前时刻的滤波增益；
根据所述滤波增益，分别确定所述当前时刻的先验估计值和所述当前时刻的观测误差对应的权重系数；
对所述当前时刻的先验估计值和所述当前时刻的观测误差进行加权计算，得到所述当前时刻的最优估计误差。
作为一种可选的实施方式，在本发明第一方面中，所述根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值之前，所述方法还包括：
判断所述当前时刻的观测误差是否有效；
当判断出所述当前时刻的观测误差无效时，将所述惯性导航系统输出的第一测量数据确定为所述当前时刻的最优导航估计值；
当判断出所述当前时刻的观测误差有效时，触发执行所述的根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值的操作。
作为一种可选的实施方式，在本发明第一方面中，所述将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差之后，所述方法还包括：
根据所述当前时刻的最优估计误差，对所述惯性导航系统执行偏差校正操作，所述偏差校正操作用于修正所述惯性导航系统的累积误差，以校正所述惯性导航系统在所述当前时刻的下一时刻输出的第一测量数据。
本发明第二方面公开了一种组合导航装置，所述装置包括：
第一确定模块，用于根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程；
第二确定模块，用于根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程；
第三确定模块，用于根据当前时刻所述惯性导航系统输出的第一测量数据与所述卫星定位系统输出的第二测量数据，确定所述当前时刻的观测误差；其中，所述第一测量数据和所述第二测量数据均包括位置数据和速度数据，所述观测误差包括位置误差和速度误差；
计算模块，用于将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差；
第一校正模块，用于根据所述当前时刻的最优估计误差，对所述当前时刻的第一测量数据进行校正，得到所述当前时刻的最优导航估计值，所述最优导航估计值包括最优导航位置值以及最优导航速度值。
作为一种可选的实施方式，在本发明第二方面中，所述第一确定模块，包括：
第一确定子模块，用于根据所述惯性导航系统的误差模型，确定所述惯性导航系统的误差状态方程：
Figure BDA0003265584790000051
其中，
Figure BDA0003265584790000052
为所述惯性导航系统的误差状态向量，FI(t)为所述惯性导航系统的状态矩阵，XI(t)为所述惯性导航系统的误差状态量矩阵，GI(t)为所述惯性导航系统的噪声驱动矩阵，WI(t)为所述惯性导航系统的状态白噪声向量，其中XI(t)具体为：
Figure BDA0003265584790000061
其中，φE、φN、φU分别为所述惯性导航系统的三个维度方向的姿态误差，δE、δN、δU分别为所述惯性导航系统的三个维度方向的速度误差，
Figure BDA0003265584790000062
δλ、δh分别为所述惯性导航系统的三个维度方向的位置误差，εx、εy、εz分别为所述惯性导航系统的陀螺仪在三个维度方向上的误差，
Figure BDA0003265584790000063
分别为所述惯性导航系统的加速度计在三个维度方向上的误差；
第二确定子模块，用于根据所述卫星定位系统的误差模型，确定所述卫星定位系统的误差状态方程：
Figure BDA0003265584790000064
其中，
Figure BDA0003265584790000065
为所述惯性导航系统的误差状态向量，FS(t)为所述惯性导航系统的状态转移矩阵，XS(t)为所述惯性导航系统的误差状态量矩阵，GS(t)为所述惯性导航系统的噪声驱动矩阵，WS(t)为所述惯性导航系统的状态白噪声向量，其中具体为：
Figure BDA0003265584790000066
其中，
Figure BDA0003265584790000067
分别为所述惯性导航系统的三个维度方向上的速度误差，
Figure BDA0003265584790000068
δλS、δhS分别为所述惯性导航系统的三个维度方向上的位置误差；
第三确定子模块，用于根据所述惯性导航系统的误差状态方程以及所述卫星定位系统的误差状态方程，确定卡尔曼滤波器的状态方程：
Figure BDA0003265584790000069
作为一种可选的实施方式，在本发明第二方面中，所述第二确定模块，包括：
第四确定子模块，用于根据所述惯性导航系统和所述卫星定位系统分别测量的位置之差形成的位置差模型以及分别测量的速度之差形成的速度差模型，确定所述卡尔曼滤波器的量测向量：
Figure BDA0003265584790000071
其中，vEI、vNI、vUI分别为所述惯性导航系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000072
λI、hI分别为所述惯性导航系统测量得到的纬度、经度、高度，vES、vNS、vUS分别为所述卫星定位系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000073
λS、hS分别为所述卫星定位系统度测量得到的纬度、经度、高度；
第五确定子模块，用于根据所述卡尔曼滤波器的量测值，确定所述卡尔曼滤波器的量测方程：
Figure BDA0003265584790000074
其中，H为观测矩阵，
Figure BDA0003265584790000075
为所述卡尔曼滤波器的状态向量，V为量测白噪声向量，其中
Figure BDA0003265584790000076
具体为：
Figure BDA0003265584790000077
作为一种可选的实施方式，在本发明第二方面中，所述计算模块，包括：
第一计算子模块，用于将所述当前时刻的观测误差输入至所述卡尔曼滤波器，根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值，所述先验估计值包括所述当前时刻的状态估计误差和所述当前时刻的协方差预估矩阵；
第二计算子模块，用于根据所述当前时刻的先验估计值和所述当前时刻的观测误差，对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差。
作为一种可选的实施方式，在本发明第二方面中，所述第二计算子模块对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差的具体方式为：
根据所述当前时刻的协方差预估矩阵，确定所述当前时刻的滤波增益；
根据所述滤波增益，分别确定所述当前时刻的先验估计值和所述当前时刻的观测误差对应的权重系数；
对所述当前时刻的先验估计值和所述当前时刻的观测误差进行加权计算，得到所述当前时刻的最优估计误差。
作为一种可选的实施方式，在本发明第二方面中，所述方法还包括：
判断模块，用于判断所述第三确定模块确定出的所述观测误差是否有效；
当判断出所述当前时刻的观测误差无效时，将所述惯性导航系统输出的第一测量数据确定为所述当前时刻的最优导航估计值；
当判断出所述当前时刻的观测误差有效时，触发执行所述的根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值的操作。
作为一种可选的实施方式，在本发明第二方面中，所述方法还包括：
第二校正模块，用于根据所述当前时刻的最优估计误差，对所述惯性导航系统执行偏差校正操作，所述偏差校正操作用于修正所述惯性导航系统的累积误差，以校正所述惯性导航系统在所述当前时刻的下一时刻输出的第一测量数据。
本发明第三方面公开了另一种组合导航装置，所述装置包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明第一方面公开的任意一种组合导航方法中的部分或全部步骤。
本发明第四方面公开了一种计算机存储介质，所述计算机存储介质存储有计算机指令，所述计算机指令被调用时，用于执行本发明第一方面公开的任意一种组合导航方法中的部分或全部步骤。
与现有技术相比，本发明具有以下有益效果：
本发明公开了一种组合导航方法及装置，该方法应用在包括惯性导航系统和卫星定位系统的组合导航系统中，该方法包括根据惯性导航系统的误差模型和卫星定位系统的误差模型，以及惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定多维卡尔曼滤波方程；将惯性导航系统和卫星定位系统测量的观测误差确定为该卡尔曼滤波方程的量测值；通过该卡尔曼滤波方程进行迭代计算，对惯性导航系统输出的测量值进行校正，得到当前时刻的最优导航估计值。可见，本发明能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度，同时对惯性导航系统进行在线偏差校正，进一步提高测量精度和准确度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例公开的一种组合导航方法的流程示意图；
图2是本发明实施例公开的另一种组合导航方法的流程示意图；
图3是本发明实施例公开的一种组合导航装置的结构示意图；
图4是本发明实施例公开的另一种组合导航装置的结构示意图；
图5是本发明实施例公开的又一种组合导航装置的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、装置、产品或端没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或端固有的其他步骤或单元。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
本发明公开了一种组合导航方法及装置，能够在需要对任意导航系统进行导航的应用场景中，提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度，同时对惯性导航系统进行在线偏差校正，进一步提高测量精度和准确度。以下分别进行详细说明。
实施例一
请参阅图1，图1是本发明实施例公开的一种组合导航方法的流程示意图。其中，图1所描述的方法可以应用于组合导航装置中，该组合导航装置可以是一个独立的装置(比如，具有惯性导航传感器和GPS导航传感器的导航模组)，也可以集成在任意导航设备(比如，车载导航设备、机载导航设备等)中，本发明实施例不做限定。如图1所示，该组合导航方法可以包括以下操作：
101、根据惯性导航系统的误差模型和卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程。
本发明实施例中，本发明所指的组合导航方法应用在组合导航系统中，其中组合导航系统包括惯性导航系统和卫星定位系统。通过惯性导航系统的误差模型，建立惯性导航系统的误差状态方程；通过卫星定位系统的误差模型，建立卫星定位系统的误差状态方程。进而，通过将惯性导航系统的误差状态方程和卫星定位系统的误差状态方程进行组合，建立组合导航系统的多维卡尔曼滤波器的状态方程。
102、根据惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定卡尔曼滤波器的量测方程。
本发明实施例中，组合导航系统的外部观测值设计两种，一是惯性导航系统和卫星定位系统分别测量输出的速度之差，二是惯性导航系统和卫星定位系统分别测量输出的位置之差，基于各子系统之间的速度差和位置差形成多维卡尔曼滤波器的量测值，建立组合导航系统的多维卡尔曼滤波器的量测方程。
103、根据当前时刻惯性导航系统输出的第一测量数据与卫星定位系统输出的第二测量数据，确定当前时刻的观测误差；其中，第一测量数据和第二测量数据均包括位置数据和速度数据，观测误差包括位置误差和速度误差。
本发明实施例中，组合导航系统进行测量时，首先确定出当前时刻惯性导航系统输出的位置数据和速度数据(也即上述的第一测量数据)以及卫星定位系统输出的位置数据和速度数据(也即上述的第二测量数据)，将分别输出的位置数据和速度数据进行相减，得到当前时刻测量得到的观测误差(也即上述的位置误差和速度误差)，便可得到多维卡尔曼滤波器的量测值。
104、将当前时刻的观测误差输入至卡尔曼滤波器进行滤波计算，得到当前时刻的最优估计误差。
本发明实施例中，将步骤103确定出的观测误差输入该多维卡尔曼滤波器，进行卡尔曼滤波计算，得到观测误差的估值，其中，估值的部分主要是多维的状态变量的误差值，最终确定出该观测误差对应的最优估计误差。
105、根据当前时刻的最优估计误差，对当前时刻的第一测量数据进行校正，得到当前时刻的最优导航估计值，其中，最优导航估计值包括最优导航位置值以及最优导航速度值。
本发明实施例中，根据确定出的当前时刻的最优估计误差，将最优估计误差反馈并修正惯性导航系统的输出数据，即用来修正惯性导航系统测量的位置和速度数据，得到当前时刻的最优导航位置值以及最优导航速度值。
可见，本发明实施例所描述的方法能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度。
在一个可选的实施例中，根据惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定卡尔曼滤波器的量测方程，可以包括以下操作：
根据惯性导航系统的误差模型，确定惯性导航系统的误差状态方程：
Figure BDA0003265584790000111
其中，
Figure BDA0003265584790000112
为惯性导航系统的误差状态向量，FI(t)为惯性导航系统的状态矩阵，XI(t)为惯性导航系统的误差状态量矩阵，GI(t)为惯性导航系统的噪声驱动矩阵，WI(t)为惯性导航系统的状态白噪声向量，其中XI(t)具体为：
Figure BDA0003265584790000121
其中，φE、φN、φU分别为惯性导航系统的三个维度方向的姿态误差，δE、δN、δU分别为惯性导航系统的三个维度方向的速度误差，
Figure BDA0003265584790000122
δλ、δh分别为惯性导航系统的三个维度方向的位置误差，εx、εy、εz分别为惯性导航系统的陀螺仪在三个维度方向上的误差，
Figure BDA0003265584790000123
分别为惯性导航系统的加速度计在三个维度方向上的误差；
其中，GI(t)为惯性导航系统的噪声驱动矩阵，取单位阵；
其中，FI(t)具体为：
Figure BDA0003265584790000124
在以上FI(t)中，
Figure BDA0003265584790000125
FM为陀螺仪和加速度计的误差方程对应的状态矩阵。本发明主要考虑惯性导航测量传感器加速度计和陀螺仪的系统误差为高斯白噪声，故可以记为零阵。
FN为惯性导航系统中的一个9×9阶的方阵，FN阵为惯性导航系统误差传播矩阵，包含了三轴位置、三轴姿态、三轴速度的相关误差递推关系，非零阵FN记为：
Figure BDA0003265584790000126
Figure BDA0003265584790000127
Figure BDA0003265584790000128
Figure BDA0003265584790000129
Figure BDA0003265584790000131
Figure BDA0003265584790000132
Figure BDA0003265584790000133
f(4,2)＝-fU；f(4,3)＝fN；
Figure BDA0003265584790000134
Figure BDA0003265584790000135
Figure BDA0003265584790000136
Figure BDA0003265584790000137
f(5,1)＝fU f(5,3)＝-fE；
Figure BDA0003265584790000138
Figure BDA0003265584790000139
Figure BDA00032655847900001310
Figure BDA00032655847900001311
f(6,1)＝-fN；f(6,2)＝fE；
Figure BDA00032655847900001312
Figure BDA00032655847900001313
f(9,6)＝1；
Figure BDA00032655847900001314
Figure BDA0003265584790000141
Figure BDA0003265584790000142
Figure BDA0003265584790000143
根据卫星定位系统的误差模型，确定卫星定位系统的误差状态方程：
Figure BDA0003265584790000144
其中，
Figure BDA0003265584790000145
为惯性导航系统的误差状态向量，FS(t)为惯性导航系统的状态转移矩阵，XS(t)为惯性导航系统的误差状态量矩阵，GS(t)为惯性导航系统的噪声驱动矩阵，WS(t)为惯性导航系统的状态白噪声向量，其中XS(t)具体为：
Figure BDA0003265584790000146
其中，δvSE、δvSN、δvSU分别为惯性导航系统的三个维度方向上的速度误差，
Figure BDA0003265584790000147
δλS、δhS分别为惯性导航系统的三个维度方向上的位置误差；
以下分别对卫星导航系统的速度模型和位置模型的误差相关性用一阶Markov过程来表示，记为：
Figure BDA0003265584790000148
Figure BDA0003265584790000149
其中，
Figure BDA00032655847900001410
其中，FS(t)是卫星导航系统误差状态方程的状态矩阵，记为：
Figure BDA0003265584790000151
其中，GS(t)是系统噪声驱动矩阵。
其中，WS(t)是系统速度和位置的白噪声，记为：
Figure BDA0003265584790000158
本发明实施例中，将传统15维卡尔曼滤波方程扩展到了21维，所扩展的留个维度为卫星导航系统的三轴速度、位置误差，因此将惯性导航系统的误差状态方程以及卫星定位系统的误差状态方程进行合并，最终得到组合导航系统的状态方程为：
Figure BDA0003265584790000152
简记为：
Figure BDA0003265584790000153
在另一个可选的实施例中，根据惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定卡尔曼滤波器的量测方程，可以包括如下操作：
根据惯性导航系统和卫星定位系统分别测量的位置之差形成的位置差模型以及分别测量的速度之差形成的速度差模型，确定卡尔曼滤波器的量测向量：
Figure BDA0003265584790000154
其中，vEI、vNI、vUI分别为惯性导航系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000155
λI、hI为所述惯性导航系统测量得到的纬度、经度、高度，vES、vNS、vUS分别为卫星定位系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000156
λS、hS分别为卫星定位系统度测量得到的纬度、经度、高度。
Figure BDA0003265584790000157
进一步的，由于惯性导航系统和卫星定位系统各自输出的导航参数中位置、速度存在着对应的误差，可将测量向量Z写为：
Figure BDA0003265584790000161
其中，δvE、δvN、δvU、
Figure BDA0003265584790000162
δλ、δh分别为分别为惯性导航系统中东北天速度误差和载体的纬度、经度和高度误差；δvES、δvNS、δvUS、
Figure BDA0003265584790000163
δλs、δhs分别为分别为卫星导航系统中东北天速度误差和载体的纬度、经度和高度误差。
本发明实施例中，基于21维状态估计的量测方程(亦称观测方程)记为：
Figure BDA0003265584790000164
其中，H为观测矩阵，
Figure BDA0003265584790000168
为所述卡尔曼滤波器的状态向量，V为量测白噪声向量，其中
Figure BDA0003265584790000165
具体为：
Figure BDA0003265584790000166
其中，观测矩阵H具体为：
Figure BDA0003265584790000167
在又一个可选的实施例中，将当前时刻的观测误差输入至卡尔曼滤波器进行滤波计算，得到当前时刻的最优估计误差，可以包括如下操作：
将当前时刻的观测误差输入至卡尔曼滤波器，根据当前时刻的上一时刻的最优估计误差，对卡尔曼滤波器进行时间更新操作，得到当前时刻的先验估计值，先验估计值包括当前时刻的状态估计误差和当前时刻的协方差预估矩阵；
根据当前时刻的先验估计值和当前时刻的观测误差，对卡尔曼滤波器进行量测更新操作，得到当前时刻的最优估计误差。
本发明实施例中，将当前时刻的观测误差输入至该多维卡尔曼滤波器，进行滤波计算，其中滤波计算包括两个迭代过程：一是卡尔曼滤波器进行时间更新操作，二是卡尔曼滤波器进行量测更新操作。其中，时间更新操作，用于根据初始化值以及上一时刻的最优估计误差值，得到当前时刻的先验估计值，其中先验估计值包括当前时刻的状态估计误差和当前时刻的协方差预估矩阵。其中，量测更新操作用于得到当前时刻的最优估计误差。
可见，本发明实施例所描述的方法能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度。
在该可选的实施例中，对卡尔曼滤波器进行量测更新操作，得到当前时刻的最优估计误差，可以包括如下操作：
根据当前时刻的协方差预估矩阵，确定当前时刻的滤波增益；
根据滤波增益，分别确定当前时刻的先验估计值和当前时刻的观测误差对应的权重系数；
对当前时刻的先验估计值和当前时刻的观测误差进行加权计算，得到当前时刻的最优估计误差。
本发明实施例中，根据卡尔曼滤波算法的估计步骤，获得当前时刻的协方差预估矩阵，进而根据当前时刻的协方差预估矩阵，确定出当前时刻的滤波增益。根据滤波增益，分别确定出当前时刻的先验估计值对应的权重系数以及当前时刻输入的观测误差Z对应的权重系数，对当前时刻的先验估计值和当前时刻的观测误差进行加权计算，即可得到当前时刻组合导航系统的最优估计误差值。
可见，本发明实施例所描述的方法能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度。
实施例二
请参阅图2，图2是本发明实施例公开的另一种组合导航方法的流程示意图。其中，图2所描述的方法可以应用于组合导航装置中，该组合导航装置可以是一个独立的装置(比如，具有惯性导航传感器和GPS导航传感器的导航模组)，也可以集成在任意导航设备(比如，车载导航设备、机载导航设备等)中，本发明实施例不做限定。如图2所示，该组合导航方法可以包括以下操作：
201、根据惯性导航系统的误差模型和卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程。
202、根据惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定卡尔曼滤波器的量测方程。
203、根据当前时刻惯性导航系统输出的第一测量数据与卫星定位系统输出的第二测量数据，确定当前时刻的观测误差。
204、判断当前时刻的观测误差是否有效。
本发明实施例中，可以通过多种因素判断观测误差是否有效，比如卫星导航系统测量的数值明显超过阈值，还可以通过运动状态来判断，具体可以通过卫星导航系统检测的比力的幅值和方差、角速度的幅值、速度等是否达到了相应的阈值条件，如果这些参数都达到了阈值条件，例如比力幅值处于某一阈值范围、方差小于方差阈值、角速度幅值小于幅值阈值且行车速度小于速度参考阈值，则判定所述车辆未运动，如果有任一参数没达到对应的阈值条件，则判定所述车辆运动。
205、当判断出当前时刻的观测误差无效时，将惯性导航系统输出的第一测量数据确定为当前时刻的最优导航估计值。
本发明实施例中，当判断出当前时刻的观测误差无效时，短时间内可以仅使用惯性导航系统输出的第一测量数据确定为当前时刻的最优导航估计值。
206、当判断出当前时刻的观测误差有效时，将当前时刻的观测误差输入至卡尔曼滤波器进行滤波计算，得到当前时刻的最优估计误差。
207、根据当前时刻的最优估计误差，对当前时刻的第一测量数据进行校正，得到当前时刻的最优导航估计值。
需要说明的是，对于步骤201-203、206、207的其它描述请参照实施例一中的相关描述，本发明实施例不再相似赘述。
可见，本发明实施例所描述的方法能够提供一种多维状态估计的组合导航方案，同时通过对观测误差有效性进行判断，降低组合导航系统在特定状态下的误导航的概率，提高组合导航系统的稳定性，进一步提高导航系统的测量精度和准确度。
208、根据当前时刻的最优估计误差，对惯性导航系统执行偏差校正操作，偏差校正操作用于修正惯性导航系统的累积误差，以校正惯性导航系统在当前时刻的下一时刻输出的第一测量数据。
本发明实施例中，组合导航系统还可以根据当前时刻的最优估计误差，对惯性导航系统的传感器偏差进行在线校正，其中在线校正主要用于修正惯性导航系统在不断测量过程中的累积误差。此外，在当前时刻执行完偏差校正操作之后，可以使得惯性导航系统在下一时刻的输出测量数据更加的精准。
可见，本发明实施例所描述的方法能够提供一种多维状态估计的组合导航方案，同时通过当前时刻的最优估计误差，对惯性导航系统进行在线偏差校正，进一步提高了组合导航系统的稳定性和适用性，进一步提高测量精度和准确度。
实施例三
请参阅图3，图3是本发明实施例公开的一种组合导航装置的结构示意图。其中，图3所描述的装置可以是一个独立的装置(比如，具有惯性导航传感器和GPS导航传感器的导航模组)，也可以集成在任意导航设备(比如，车载导航设备、机载导航设备等)中，本发明实施例不做限定。需要说明的是，该组合导航装置参照的是实施例一和实施例二所描述的一种组合导航方法中的步骤，详细的描述在本实施例中就不做赘述，如图3所示，该组合导航装置可以包括：
第一确定模块301，用于用于根据惯性导航系统的误差模型和卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程。
第二确定模块302，用于根据惯性导航系统和卫星定位系统之间的位置差模型以及速度差模型，确定卡尔曼滤波器的量测方程。
第三确定模块303，用于根据当前时刻惯性导航系统输出的第一测量数据与卫星定位系统输出的第二测量数据，确定当前时刻的观测误差；其中，第一测量数据和第二测量数据均包括位置数据和速度数据，观测误差包括位置误差和速度误差。
计算模块304，用于将当前时刻的观测误差输入至卡尔曼滤波器进行滤波计算，得到当前时刻的最优估计误差。
第一校正模块305，用于根据当前时刻的最优估计误差，对当前时刻的第一测量数据进行校正，得到当前时刻的最优导航估计值，最优导航估计值包括最优导航位置值以及最优导航速度值。
可见，本发明实施图所描述的装置能够提供一种多维状态估计的组合导航方案，通过将GPS的速度误差和位置误差扩展为系统的状态变量，对惯性导航系统的测量数据进行修正，提高导航系统的测量精度和准确度。
在一个可选的实施例中，如图4所示，第一确定模块301可以包括：
第一确定子模块3011，用于根据惯性导航系统的误差模型，确定惯性导航系统的误差状态方程：
Figure BDA0003265584790000201
其中，
Figure BDA0003265584790000202
为惯性导航系统的误差状态向量，FI(t)为惯性导航系统的状态矩阵，XI(t)为惯性导航系统的误差状态量矩阵，GI(t)为惯性导航系统的噪声驱动矩阵，WI(t)为惯性导航系统的状态白噪声向量，其中XI(t)具体为：
Figure BDA0003265584790000203
其中，φE、φN、φU分别为惯性导航系统的三个维度方向的姿态误差，δE、δN、δU分别为惯性导航系统的三个维度方向的速度误差，
Figure BDA0003265584790000204
δλ、δh分别为惯性导航系统的三个维度方向的位置误差，εx、εy、εz分别为惯性导航系统的陀螺仪在三个维度方向上的误差，
Figure BDA0003265584790000205
分别为惯性导航系统的加速度计在三个维度方向上的误差；
第二确定子模块3012，用于根据卫星定位系统的误差模型，确定卫星定位系统的误差状态方程：
Figure BDA0003265584790000206
其中，
Figure BDA0003265584790000207
为惯性导航系统的误差状态向量，FS(t)为惯性导航系统的状态转移矩阵，XS(t)为惯性导航系统的误差状态量矩阵，GS(t)为惯性导航系统的噪声驱动矩阵，WS(t)为惯性导航系统的状态白噪声向量，其中具体为：
Figure BDA0003265584790000208
其中，δvSE、δvSN、δvSU分别为惯性导航系统的三个维度方向上的速度误差，
Figure BDA0003265584790000209
δλS、δhS分别为惯性导航系统的三个维度方向上的位置误差；
第三确定子模块3013，用于根据惯性导航系统的误差状态方程以及卫星定位系统的误差状态方程，确定卡尔曼滤波器的状态方程：
Figure BDA00032655847900002010
在一个可选的实施例中，如图4所示，第二确定模块302，可以包括；
第四确定子模块3021，用于根据惯性导航系统和卫星定位系统分别测量的位置之差形成的位置差模型以及分别测量的速度之差形成的速度差模型，确定卡尔曼滤波器的量测向量：
Figure BDA0003265584790000211
其中，vEI、vNI、vUI分别为惯性导航系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000212
λI、hI分别为惯性导航系统测量得到的纬度、经度、高度，vES、vNS、vUS分别为卫星定位系统测量得到的三个维度方向的速度，
Figure BDA0003265584790000213
λS、hS分别为卫星定位系统度测量得到的纬度、经度、高度；
第五确定子模块3022，用于根据卡尔曼滤波器的量测值，确定卡尔曼滤波器的量测方程：
Figure BDA0003265584790000214
其中，H为观测矩阵，
Figure BDA0003265584790000215
为卡尔曼滤波器的状态向量，V为量测白噪声向量，其中
Figure BDA0003265584790000216
具体为：
Figure BDA0003265584790000217
在另一个可选的实施例中，如图4所示，计算模块304，包括：
第一计算子模块3041，用于将当前时刻的观测误差输入至所述卡尔曼滤波器，根据当前时刻的上一时刻的最优估计误差，对卡尔曼滤波器进行时间更新操作，得到当前时刻的先验估计值，所述先验估计值包括当前时刻的状态估计误差和当前时刻的协方差预估矩阵；
第二计算子模块3042，用于根据当前时刻的先验估计值和当前时刻的观测误差，对卡尔曼滤波器进行量测更新操作，得到当前时刻的最优估计误差。
在该可选的实施例中，第二计算子模块3042对卡尔曼滤波器进行量测更新操作，得到当前时刻的最优估计误差的具体方式为：
根据当前时刻的协方差预估矩阵，确定当前时刻的滤波增益；
根据滤波增益，分别确定当前时刻的先验估计值和当前时刻的观测误差对应的权重系数；
对当前时刻的先验估计值和当前时刻的观测误差进行加权计算，得到当前时刻的最优估计误差。
在又一可选的实施例中，该装置还可以包括：
判断模块306，用于判断第三确定模块303确定出的观测误差是否有效；
当判断出当前时刻的观测误差无效时，将惯性导航系统输出的第一测量数据确定为当前时刻的最优导航估计值；
当判断出当前时刻的观测误差有效时，触发执行所述的根据当前时刻的上一时刻的最优估计误差，对卡尔曼滤波器进行时间更新操作，得到当前时刻的先验估计值的操作。
可见，本发明实施图所描述的装置能够能够提供一种多维状态估计的组合导航方案，同时通过对观测误差有效性进行判断，降低组合导航系统在特定状态下的误导航的概率，提高组合导航系统的稳定性，进一步提高导航系统的测量精度和准确度。
在又一可选的实施例中，该装置还可以包括：
第二校正模块307，用于根据当前时刻的最优估计误差，对惯性导航系统执行偏差校正操作，偏差校正操作用于修正惯性导航系统的累积误差，以校正第三确定模块303中惯性导航系统在当前时刻的下一时刻输出的第一测量数据。
可见，本发明实施图所描述的装置能够提供一种多维状态估计的组合导航方案，同时通过当前时刻的最优估计误差，对惯性导航系统进行在线偏差校正，进一步提高了组合导航系统的稳定性和适用性，进一步提高测量精度和准确度。
实施例四
请参阅图5，图5是本发明实施例公开的一种组合导航装置的结构示意图。其中，图5所描述的装置可以是一个独立的装置(比如，具有惯性导航传感器和GPS导航传感器的导航模组)，也可以集成在任意导航设备(比如，车载导航设备、机载导航设备等)中，本发明实施例不做限定。如图5所示，该组合导航装置可以包括：
存储有可执行程序代码的存储器401；
与存储器401耦合的处理器402；
处理器402调用存储器402中存储的可执行程序代码，执行本发明实施例一或实施例二公开的组合导航方法中的部分或全部步骤。
实施例五
本发明实施例公开了一种计算机存储介质，该计算机存储介质存储有计算机指令，该计算机指令被调用时，用于执行本发明实施例一或实施例二公开的xx方法中的步骤。
以上所描述的装置实施例仅是示意性的，其中所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施例的具体描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中,存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(ErasableProgrammable Read Only Memory，EPROM)、一次可编程只读存储器(One-timeProgrammable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
需要说明的是本说明书各部分操作所需的计算机程序代码可以用任意一种或多种程序语言编写，包括面向对象编程语言如Java、Scala、Smalltalk、Eiffel、JADE、Emerald、C++、C#、VB.NET、Python等，常规程序化编程语言如C语言、Visual Basic、Fortran2003、Perl、COBOL 2002、PHP、ABAP，动态编程语言如Python、Ruby和Groovy，或其他编程语言等。该程序编码可以完全在计算机(PC、嵌入式智能设备等)上运行、或作为独立的软件包在用户计算机上运行、或部分在用户计算机上运行部分在远程计算机运行、或完全在远程计算机或服务器上运行。在后种情况下，远程计算机可以通过任何网络形式与用户计算机连接，比如局域网(LAN)或广域网(WAN)，或连接至外部计算机(例如通过因特网)，或在云计算环境中，或作为服务使用如软件即服务(SaaS)。
最后应说明的是：本发明实施例公开的一种组合导航方法及装置所揭露的仅为本发明较佳实施例而已，仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述各项实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应的技术方案的本质脱离本发明各项实施例技术方案的精神和范围。

Claims (10)

1.一种组合导航方法，其特征在于，所述组合导航方法应用在组合导航系统中，所述组合导航系统包括惯性导航系统和卫星定位系统，所述方法包括：
根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程；
根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程；
根据当前时刻所述惯性导航系统输出的第一测量数据与所述卫星定位系统输出的第二测量数据，确定所述当前时刻的观测误差；其中，所述第一测量数据和所述第二测量数据均包括位置数据和速度数据，所述观测误差包括位置误差和速度误差；
将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差；
根据所述当前时刻的最优估计误差，对所述当前时刻的第一测量数据进行校正，得到所述当前时刻的最优导航估计值，所述最优导航估计值包括最优导航位置值以及最优导航速度值。
2.根据权利要求1所述的组合导航方法，其特征在于，所述根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程，包括：
根据所述惯性导航系统的误差模型，确定所述惯性导航系统的误差状态方程：
Figure FDA0003265584780000011
其中，
Figure FDA0003265584780000012
为所述惯性导航系统的误差状态向量，FI(t)为所述惯性导航系统的状态矩阵，XI(t)为所述惯性导航系统的误差状态量矩阵，GI(t)为所述惯性导航系统的噪声驱动矩阵，WI(t)为所述惯性导航系统的状态白噪声向量，其中XI(t)具体为：
Figure FDA0003265584780000013
其中，φE、φN、φU分别为所述惯性导航系统的三个维度方向的姿态误差，δE、δN、δU分别为所述惯性导航系统的三个维度方向的速度误差，
Figure FDA0003265584780000014
δλ、δh分别为所述惯性导航系统的三个维度方向的位置误差，εx、εy、εz分别为所述惯性导航系统的陀螺仪在三个维度方向上的误差，
Figure FDA0003265584780000021
分别为所述惯性导航系统的加速度计在三个维度方向上的误差；
根据所述卫星定位系统的误差模型，确定所述卫星定位系统的误差状态方程：
Figure FDA0003265584780000022
其中，
Figure FDA0003265584780000023
为所述惯性导航系统的误差状态向量，FS(t)为所述惯性导航系统的状态转移矩阵，XS(t)为所述惯性导航系统的误差状态量矩阵，GS(t)为所述惯性导航系统的噪声驱动矩阵，WS(t)为所述惯性导航系统的状态白噪声向量，其中XS(t)具体为：
Figure FDA0003265584780000024
其中，δvSE、δvSN、δvSU分别为所述惯性导航系统的三个维度方向上的速度误差，
Figure FDA0003265584780000025
δλS、δhS分别为所述惯性导航系统的三个维度方向上的位置误差；
根据所述惯性导航系统的误差状态方程以及所述卫星定位系统的误差状态方程，确定卡尔曼滤波器的状态方程：
Figure FDA0003265584780000026
3.根据权利要求2所述的组合导航方法，其特征在于，所述根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程，包括：
根据所述惯性导航系统和所述卫星定位系统分别测量的位置之差形成的位置差模型以及分别测量的速度之差形成的速度差模型，确定所述卡尔曼滤波器的量测向量：
Figure FDA0003265584780000027
其中，vEI、vNI、vUI分别为所述惯性导航系统测量得到的三个维度方向的速度，
Figure FDA0003265584780000031
λI、hI分别为所述惯性导航系统测量得到的纬度、经度、高度，vES、vNS、vUS分别为所述卫星定位系统测量得到的三个维度方向的速度，
Figure FDA0003265584780000032
λS、hS分别为所述卫星定位系统度测量得到的纬度、经度、高度；
根据所述卡尔曼滤波器的量测值，确定所述卡尔曼滤波器的量测方程：
Figure FDA0003265584780000033
其中，H为观测矩阵，
Figure FDA0003265584780000034
为所述卡尔曼滤波器的状态向量，V为量测白噪声向量，其中
Figure FDA0003265584780000035
具体为：
Figure FDA0003265584780000036
4.根据权利要求1-3任一所述的组合导航方法，其特征在于，所述将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差，包括：
将所述当前时刻的观测误差输入至所述卡尔曼滤波器，根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值，所述先验估计值包括所述当前时刻的状态估计误差和所述当前时刻的协方差预估矩阵；
根据所述当前时刻的先验估计值和所述当前时刻的观测误差，对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差。
5.根据权利要求4所述的组合导航方法，其特征在于，所述对所述卡尔曼滤波器进行量测更新操作，得到所述当前时刻的最优估计误差，包括：
根据所述当前时刻的协方差预估矩阵，确定所述当前时刻的滤波增益；
根据所述滤波增益，分别确定所述当前时刻的先验估计值和所述当前时刻的观测误差对应的权重系数；
对所述当前时刻的先验估计值和所述当前时刻的观测误差进行加权计算，得到所述当前时刻的最优估计误差。
6.根据权利要求4或5所述的组合导航方法，其特征在于，所述根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值之前，所述方法还包括：
判断所述当前时刻的观测误差是否有效；
当判断出所述当前时刻的观测误差无效时，将所述惯性导航系统输出的第一测量数据确定为所述当前时刻的最优导航估计值；
当判断出所述当前时刻的观测误差有效时，触发执行所述的根据所述当前时刻的上一时刻的最优估计误差，对所述卡尔曼滤波器进行时间更新操作，得到所述当前时刻的先验估计值的操作。
7.根据权利要求6所述的组合导航方法，其特征在于，所述将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差之后，所述方法还包括：
根据所述当前时刻的最优估计误差，对所述惯性导航系统执行偏差校正操作，所述偏差校正操作用于修正所述惯性导航系统的累积误差，以校正所述惯性导航系统在所述当前时刻的下一时刻输出的第一测量数据。
8.一种组合导航装置，其特征在于，应用在组合导航系统中，所述组合导航系统包括惯性导航系统和卫星定位系统，所述装置包括：
第一确定模块，用于根据所述惯性导航系统的误差模型和所述卫星定位系统的误差模型，确定卡尔曼滤波器的状态方程；
第二确定模块，用于根据所述惯性导航系统和所述卫星定位系统之间的位置差模型以及速度差模型，确定所述卡尔曼滤波器的量测方程；
第三确定模块，用于根据当前时刻所述惯性导航系统输出的第一测量数据与所述卫星定位系统输出的第二测量数据，确定所述当前时刻的观测误差；其中，所述第一测量数据和所述第二测量数据均包括位置数据和速度数据，所述观测误差包括位置误差和速度误差；
计算模块，用于将所述当前时刻的观测误差输入至所述卡尔曼滤波器进行滤波计算，得到所述当前时刻的最优估计误差；
第一校正模块，用于根据所述当前时刻的最优估计误差，对所述当前时刻的第一测量数据进行校正，得到所述当前时刻的最优导航估计值，所述最优导航估计值包括最优导航位置值以及最优导航速度值。
9.一种组合导航装置，其特征在于，所述装置包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行如权利要求1-7任一项所述的组合导航方法。
10.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机指令，所述计算机指令被调用时用于执行如权利要求1-7任一项所述的组合导航方法。
CN202111086062.5A 2021-09-16 2021-09-16 一种组合导航方法及装置 Active CN113959433B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111086062.5A CN113959433B (zh) 2021-09-16 2021-09-16 一种组合导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111086062.5A CN113959433B (zh) 2021-09-16 2021-09-16 一种组合导航方法及装置

Publications (2)

Publication Number Publication Date
CN113959433A true CN113959433A (zh) 2022-01-21
CN113959433B CN113959433B (zh) 2023-12-08

Family

ID=79461784

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111086062.5A Active CN113959433B (zh) 2021-09-16 2021-09-16 一种组合导航方法及装置

Country Status (1)

Country Link
CN (1) CN113959433B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103941273A (zh) * 2014-03-31 2014-07-23 广东电网公司电力科学研究院 机载惯性/卫星组合导航系统的自适应滤波方法与滤波器
CN106949889A (zh) * 2017-03-17 2017-07-14 南京航空航天大学 针对行人导航的低成本mems/gps组合导航系统及方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN111650617A (zh) * 2020-06-10 2020-09-11 国网湖南省电力有限公司 基于新息加权自适应不敏卡尔曼滤波的晶振频率驯服方法、系统及介质
US20210041240A1 (en) * 2018-06-22 2021-02-11 Southeast University Navigation and positioning system for underwater glider and up floating error correction method
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103941273A (zh) * 2014-03-31 2014-07-23 广东电网公司电力科学研究院 机载惯性/卫星组合导航系统的自适应滤波方法与滤波器
CN106949889A (zh) * 2017-03-17 2017-07-14 南京航空航天大学 针对行人导航的低成本mems/gps组合导航系统及方法
US20210041240A1 (en) * 2018-06-22 2021-02-11 Southeast University Navigation and positioning system for underwater glider and up floating error correction method
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN111650617A (zh) * 2020-06-10 2020-09-11 国网湖南省电力有限公司 基于新息加权自适应不敏卡尔曼滤波的晶振频率驯服方法、系统及介质
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN115932913B (zh) * 2022-11-24 2024-03-08 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置

Also Published As

Publication number Publication date
CN113959433B (zh) 2023-12-08

Similar Documents

Publication Publication Date Title
US8024119B2 (en) Systems and methods for gyrocompass alignment using dynamically calibrated sensor data and an iterated extended kalman filter within a navigation system
EP1837627B1 (en) Methods and systems for implementing an iterated extended kalman filter within a navigation system
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
RU2762143C2 (ru) Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN107121684B (zh) 一种基于残差卡方检验法的gps诱骗识别和阈值决策方法
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112432642A (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114061570A (zh) 车辆定位方法、装置、计算机设备和存储介质
CN113959433B (zh) 一种组合导航方法及装置
JP3095189B2 (ja) ナビゲーション装置
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114018262B (zh) 一种改进的衍生容积卡尔曼滤波组合导航方法
US20230078005A1 (en) Navigation assistance method for a mobile carrier
CN114323007A (zh) 一种载体运动状态估计方法及装置
US11821733B2 (en) Terrain referenced navigation system with generic terrain sensors for correcting an inertial navigation solution
CN108957508B (zh) 车载pos离线组合估计方法和装置
CN116774263B (zh) 面向组合导航系统的导航定位方法及装置
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN113465628B (zh) 惯性测量单元数据补偿方法及系统
US20240159539A1 (en) Method for assisting with the navigation of a vehicle

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 518000, 3rd Floor, Building 40, Baotian Industrial Zone, Chentian Community, Xixiang Street, Bao'an District, Shenzhen City, Guangdong Province

Applicant after: China Southern Power Grid Digital Platform Technology (Guangdong) Co.,Ltd.

Address before: 518053 501, 502, 601 and 602, building D, wisdom Plaza, Qiaoxiang Road, Gaofa community, Shahe street, Nanshan District, Shenzhen, Guangdong

Applicant before: China Southern Power Grid Shenzhen Digital Power Grid Research Institute Co.,Ltd.

GR01 Patent grant
GR01 Patent grant