CN116380119A - 组合导航的校准方法、装置和*** - Google Patents

组合导航的校准方法、装置和*** Download PDF

Info

Publication number
CN116380119A
CN116380119A CN202111600911.4A CN202111600911A CN116380119A CN 116380119 A CN116380119 A CN 116380119A CN 202111600911 A CN202111600911 A CN 202111600911A CN 116380119 A CN116380119 A CN 116380119A
Authority
CN
China
Prior art keywords
error
inertial navigation
output
navigation system
speed
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.)
Pending
Application number
CN202111600911.4A
Other languages
English (en)
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.)
China Telecom Corp Ltd
Original Assignee
China Telecom Corp 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 China Telecom Corp Ltd filed Critical China Telecom Corp Ltd
Priority to CN202111600911.4A priority Critical patent/CN116380119A/zh
Publication of CN116380119A publication Critical patent/CN116380119A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • 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
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本公开涉及一种组合导航的校准方法、装置和***，涉及导航技术领域。本公开的方法包括：根据惯性导航***的误差参数构建卡尔曼滤波器的状态方程；根据惯性导航***输出的位置和GPS***输出的位置之间的位置误差，以及惯性导航***输出的速度和GPS***输出的速度之间的速度误差，构建卡尔曼滤波器的量测方程；根据卡尔曼滤波器的状态方程和卡尔曼滤波器的量测方程，利用卡尔曼滤波器确定惯性导航***的误差参数的估计值；根据惯性导航***的误差参数的估计值，对惯性导航***输出的位置和速度进行校准。

Description

组合导航的校准方法、装置和***
技术领域
本公开涉及导航技术领域，特别涉及一种组合导航的校准方法、 装置和***。
背景技术
车路协同技术不同于传统的车联网技术，其目标是实现真正意义 的自动驾驶。其中车辆精准定位与高可靠通信技术是智能车载***的 关键技术。当前有GPS(GlobalPositioning System，全球定位***)、 北斗导航、基站定位、航迹推算、视觉/雷达感知等多种方法。常用 的卫星导航定位技术存在信号接收数据频率低、易受环境影响等缺陷， 单独使用情况下无法达到车路协同无环境差别的高精度定位发展要 求。
因此，采用GPS和惯性导航(Inertial Navigation System，INS) 融合定位是目前车路协同技术中主要的定位方式。
发明内容
本公开所要解决的一个技术问题是：如何提高INS/GPS组合导航方 ***性。
根据本公开的一些实施例，提供的一种组合导航的校准方法，包括： 根据惯性导航***的误差参数构建卡尔曼滤波器的状态方程，其中， 惯性导航***的误差参数包括：陀螺仪漂移误差、陀螺仪刻度系数误 差、陀螺仪安装误差角、加速度计零偏误差、加速度计刻度系数误差 和加速度计安装误差角中的至少一项，以及失准角、速度误差和位置 误差；根据惯性导航***输出的位置和GPS***输出的位置之间的位 置误差，以及惯性导航***输出的速度和GPS***输出的速度之间的 速度误差，构建卡尔曼滤波器的量测方程；根据卡尔曼滤波器的状态 方程和卡尔曼滤波器的量测方程，利用卡尔曼滤波器确定惯性导航系 统的误差参数的估计值；根据惯性导航***的误差参数的估计值，对 惯性导航***输出的位置和速度进行校准。
在一些实施例中，根据惯性导航***的误差参数的估计值，对惯 性导航***输出的位置和速度进行校准包括：将惯性导航***的误差 参数的估计值反馈至惯性导航***中的惯性导航解算模块，对惯性导 航解算模块中输入的陀螺仪的测量值和加速度计的测量值进行补偿； 惯性导航解算模块根据补偿后的陀螺仪的测量值和加速度计的测量值 确定惯性导航***输出的位置和速度，从而得到校准后的惯性导航系 统输出的位置和速度。
在一些实施例中，根据惯性导航***的误差参数的估计值，对惯性 导航***输出的位置和速度进行校准包括：在卡尔曼滤波器的更新过程 中，确定惯性导航***的每种误差参数的协方差的值；针对惯性导航系 统的每种误差参数的协方差，在该种误差参数的协方差达到对应的预设 值的情况下，根据该种误差参数的估计值，对惯性导航***输出的位置 和速度进行校准。
在一些实施例中，在惯性导航***的误差参数包括：陀螺仪漂移 误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、 加速度计刻度系数误差和加速度计安装误差角的情况下，卡尔曼滤波 器的状态方程为：
Figure BDA0003431754860000021
其中，
Figure BDA0003431754860000022
φ＝[φE φN φU]为 失准角，δvn＝[δvE δvN δvU]为速度误差，δp＝[δL δλ δh]为 位置误差，/>
Figure BDA0003431754860000023
分别表示陀螺仪误差参数组成向量和加速度计误 差参数组成的向量，/>
Figure BDA0003431754860000024
Figure BDA0003431754860000025
δkgxx，δkgyy，δkgzz分别表示x，y， z轴的陀螺仪刻度系数误差，δkgxy，δkgxz，δkgyx，δkgyz，δkgzx， δkgzy为陀螺仪安装误差角，/>
Figure BDA0003431754860000026
为陀螺仪漂移误差， />
Figure BDA0003431754860000027
δkaxx，δkayy，δkazz为加速度计刻度系数误差，δkayx，δkazx，δkazy为加速度 计安装误差角，
Figure BDA0003431754860000031
为加速度计零偏误差，F为状态相 关的确定性矩阵函数，/>
Figure BDA0003431754860000032
为陀螺仪和加速度计的噪声，输 入矩阵为/>
Figure BDA00034317548600000318
为b系到n系的姿态变换矩阵，b系 为导航坐标系，n系为大地坐标系。
在一些实施例中，
Figure BDA0003431754860000035
其中，
Figure BDA0003431754860000036
Figure BDA0003431754860000037
Figure BDA0003431754860000038
Figure BDA0003431754860000039
Figure BDA00034317548600000310
Figure BDA00034317548600000311
Figure BDA00034317548600000312
i表示惯性坐标系，
Figure BDA00034317548600000319
为大地坐标系对惯性坐标系的 角速度，
Figure BDA00034317548600000315
为导航坐标系对大地坐标系的角速度，fb为x、y、z轴加 速度计比力输出组成的向量，RM为子午圈主曲率半径，RN为卯酉圈主 曲率半径，h为载体距地面的高度，L为纬度，/>
Figure BDA00034317548600000316
为地球自转角速度， vE为导航坐标系中东向的速度，vN为导航坐标系中北向的速度，/>
Figure BDA00034317548600000317
为x轴陀螺仪的输出角速度，/>
Figure BDA0003431754860000041
为y轴陀螺仪的输出角速度，/>
Figure BDA0003431754860000042
为z 轴陀螺仪的输出角速度，vn为东、北、天方向速度组成的向量在导航 坐标系下的投影，/>
Figure BDA0003431754860000043
为x轴加速度计的比力输出。
在一些实施例中，卡尔曼滤波器的量测方程为：
Figure BDA0003431754860000044
其中，Δv、Δp分别表示惯性导航***输出的位置和GPS***输 出的位置之间的位置误差，惯性导航***输出的速度和GPS***输出 的速度之间的速度误差，V为观测噪声，
Figure BDA0003431754860000045
根据本公开的另一些实施例，提供的一种组合导航的校准装置，包 括：第一构建模块，用于根据惯性导航***的误差参数构建卡尔曼滤 波器的状态方程，其中，惯性导航***的误差参数包括：陀螺仪漂移 误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、 加速度计刻度系数误差和加速度计安装误差角中的至少一项，以及失 准角、速度误差和位置误差；第二构建模块，用于根据惯性导航*** 输出的位置和GPS***输出的位置之间的位置误差，以及惯性导航系 统输出的速度和GPS***输出的速度之间的速度误差，构建卡尔曼滤 波器的量测方程；滤波模块，用于根据卡尔曼滤波器的状态方程和卡尔曼滤波器的量测方程，利用卡尔曼滤波器确定惯性导航***的误差 参数的估计值；校准模块，用于根据惯性导航***的误差参数的估计 值，对惯性导航***输出的位置和速度进行校准。
根据本公开的又一些实施例，提供的一种组合导航的校准装置，包 括：处理器；以及耦接至处理器的存储器，用于存储指令，指令被处 理器执行时，使处理器执行如前述任意实施例的组合导航的校准方法。
根据本公开的再一些实施例，提供的一种非瞬时性计算机可读存储 介质，其上存储有计算机程序，其中，该程序被处理器执行时实现前 述任意实施例的组合导航的校准方法。
根据本公开的又一些实施例，提供的一种组合导航的校准***，包 括：前述任意实施例的校准装置；以及惯性导航***和GPS***，其 中，惯性导航***包括：陀螺仪和加速度计；惯性导航***用于将输 出位置和速度输入校准装置，并接收校准装置输出的惯性导航***的 误差参数的估计值，对输出的位置和速度进行校准后再输出；GPS系 统用于将输出位置和速度输入校准装置。
本公开中根据惯性导航***的误差参数构建卡尔曼滤波器的状态 方程，根据惯性导航***和GPS***之间的误差构建卡尔曼滤波器的 量测方程，进而通过卡尔曼滤波器对惯性导航***的误差参数进行估 计，并利用惯性导航***的误差参数的估计值对惯性导航***输出的 位置和速度进行校准，提高了组合导航定位的准确的和精度。
通过以下参照附图对本公开的示例性实施例的详细描述，本公开 的其它特征及其优点将会变得清楚。
附图说明
为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将 对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见 地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技 术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得 其他的附图。
图1示出本公开的一些实施例的组合导航的校准方法的流程示 意图。
图2示出本公开的一些实施例的组合导航的校准方法的示意图。
图3示出本公开的一些实施例的组合导航的校准装置的结构示 意图。
图4示出本公开的另一些实施例的组合导航的校准装置的结构 示意图。
图5示出本公开的又一些实施例的组合导航的校准装置的结构 示意图。
图6示出本公开的一些实施例的组合导航的校准***的结构示 意图。
具体实施方式
下面将结合本公开实施例中的附图，对本公开实施例中的技术方案 进行清楚、完整地描述，显然，所描述的实施例仅仅是本公开一部分实 施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实 际上仅仅是说明性的，决不作为对本公开及其应用或使用的任何限制。 基于本公开中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
本公开提出一种INS/GPS组合导航的校准方法，下面结合图1～2 进行描述。
图1为本公开组合导航的校准方法一些实施例的流程图。如图1所 示，该实施例的方法包括：步骤S102～S108。
在步骤S102中，根据惯性导航***的误差参数构建卡尔曼 (Kalman)滤波器的状态方程。
在一些实施例中，惯性导航***的误差参数包括：陀螺仪漂移误 差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、 加速度计刻度系数误差和加速度计安装误差角中的至少一项，以及失 准角、速度误差和位置误差。
例如，如果惯性导航***的误差参数包括上述所有参数，则可以 构建30维状态向量：
Figure BDA0003431754860000061
其中，
Figure BDA0003431754860000062
φ＝[φE φN φU]为 失准角，δvn＝[δvE δvN δvU]为速度误差，δp＝[δL δλ δh]为 位置误差，/>
Figure BDA0003431754860000063
分别表示陀螺仪误差参数组成向量和加速度计误 差参数组成的向量，/>
Figure BDA0003431754860000064
Figure BDA0003431754860000065
δkgxx，δkgyy，δkgzz分别表示x，y， z轴的陀螺仪刻度系数误差，δkgxy，δkgxz，δkgyx，δkgyz，δkgzx， δkgzy为陀螺仪安装误差角，/>
Figure BDA0003431754860000066
为陀螺仪漂移误差， />
Figure BDA0003431754860000067
δkaxx，δkayy，δkazz为加速度计刻度系数误差，δkayx，δkazx，δkazy为加速度 计安装误差角，
Figure BDA0003431754860000071
为加速度计零偏误差。E，N，U分 别表示导航坐标系中的东、北、天方向，L、λ、h分别为纬度、经度 和高度。
惯性导航***的误差参数可以根据实际需求进行选择，不限于所 举示例。
在一些实施例中，通过惯性导航***的误差参数状态向量，建立 卡尔曼滤波状态方程为：
Figure BDA0003431754860000072
其中，X为惯性导航***的误差参数的状态向量，F为状态相关 的确定性矩阵函数，
Figure BDA0003431754860000073
为陀螺仪和加速度计的噪声，输入 矩阵为/>
Figure BDA0003431754860000074
为b系到n系的姿态变换矩阵，b系为 导航坐标系，n系为大地坐标系。
在一些实施例中，
Figure BDA0003431754860000076
其中，
Figure BDA0003431754860000077
Figure BDA0003431754860000078
Figure BDA0003431754860000079
Figure BDA00034317548600000710
Figure BDA00034317548600000711
Figure BDA00034317548600000712
Figure BDA0003431754860000081
i表示惯性坐标系，
Figure BDA0003431754860000082
为大地坐标系对惯性坐标系的 角速度，
Figure BDA0003431754860000084
为导航坐标系对大地坐标系的角速度，fb为x、y、z轴加 速度计比力输出组成的向量，RM为子午圈主曲率半径，RN为卯酉圈主 曲率半径，h为载体距地面的高度，L为纬度，/>
Figure BDA0003431754860000085
为地球自转角速度， vE为导航坐标系中东向的速度，vN为导航坐标系中北向的速度，/>
Figure BDA0003431754860000086
为 x轴陀螺仪的输出角速度，/>
Figure BDA0003431754860000087
为y轴陀螺仪的输出角速度，/>
Figure BDA0003431754860000088
为z 轴陀螺仪的输出角速度，vn为东、北、天方向速度组成的向量在导航 坐标系下的投影，/>
Figure BDA0003431754860000089
为x轴加速度计的比力输出。
在步骤S104中，根据惯性导航***输出的位置和GPS***输出 的位置之间的位置误差，以及惯性导航***输出的速度和GPS***输 出的速度之间的速度误差，构建卡尔曼滤波器的量测方程。
在一些实施例中，卡尔曼滤波器的量测方程为：
Figure BDA00034317548600000810
其中，Δv、Δp分别表示惯性导航***输出的位置和GPS***输 出的位置之间的位置误差，惯性导航***输出的速度和GPS***输出 的速度之间的速度误差，V为观测噪声，
Figure BDA00034317548600000811
在步骤S106中，根据卡尔曼滤波器的状态方程和卡尔曼滤波器的 量测方程，利用卡尔曼滤波器确定惯性导航***的误差参数的估计值。
如图2所示，可以将惯性导航***输出的位置和GPS***输出的 位置之间的位置误差，以及惯性导航***输出的速度和GPS***输出 的速度之间的速度误差输入卡尔曼滤波器，得到输出的惯性导航*** 的误差参数的估计值。
在步骤S108中，根据惯性导航***的误差参数的估计值，对惯性 导航***输出的位置和速度进行校准。
在一些实施例中，将惯性导航***的误差参数的估计值反馈至惯 性导航***中的惯性导航解算模块，对惯性导航解算模块中输入的陀 螺仪的测量值和加速度计的测量值进行补偿；惯性导航解算模块根据 补偿后的陀螺仪的测量值和加速度计的测量值确定惯性导航***输出 的位置和速度，从而得到校准后的惯性导航***输出的位置和速度。
如图2所示，INS误差补充模型包括惯性导航解算模块，根据惯 性导航***的误差参数的估计值对加速度计和陀螺仪的测量值进行补 充，从而修正输出的位置和速度。
在一些实施例中，在卡尔曼滤波器的更新过程中，确定惯性导航 ***的每种误差参数的协方差的值；针对惯性导航***的每种误差参 数的协方差，在该种误差参数的协方差达到对应的预设值的情况下， 根据该种误差参数的估计值，对惯性导航***输出的位置和速度进行 校准。
Kalman滤波器更新时，可以计算IMU(惯性测量单元)误差参 数的协方差，确定协方差的变化情况。协方差变化反映了状态量的可 观测性，相对协方差越接近0，表示可观测性越好。通过Kalman滤 波器中实时记录的所有IMU误差参数的协方差状态，查询可观测性较 强的IMU误差，即它们在滤波器中具有很好的估计效果，从而可以在 后续导航中获得准确的误差参数并进行补偿。
上述实施例中根据惯性导航***的误差参数构建卡尔曼滤波器的 状态方程，根据惯性导航***和GPS***之间的误差构建卡尔曼滤波 器的量测方程，进而通过卡尔曼滤波器对惯性导航***的误差参数进 行估计，并利用惯性导航***的误差参数的估计值对惯性导航***输 出的位置和速度进行校准，提高了组合导航定位的准确的和精度。
本公开提出一种可监测IMU惯性误差及补偿的INS/GPS组合导 航算法，包括基于惯性导航误差补偿模型和可观测IMU惯性误差的 Kalman滤波器。滤波器实时更新获取IMU误差参数的协方差变化， 研究了惯性误差的可观测性和可观测度，实时监测IMU误差参数，进 一步对模型进行误差补偿，提高组合导航精度。
本公开建立了基于惯性导航误差补偿模型和卫星导航的Kalman 滤波器，在考虑INS误差补偿的组合导航中为了有效分析导航误差并 估计误差参数，将IMU误差参数加入惯导解算的误差模型中并建立卡 尔曼滤波器。IMU误差参数包括陀螺零偏、陀螺刻度系数误差、陀螺 安装误差角、加计零偏、加计刻度系数误差和加计安装误差角。通过 计算IMU误差参数的协方差变化，研究了误差参数的可观测性和可观 测度，实时监测IMU误差参数，进而通过误差补偿提高组合导航定位 精度。
第一步：建立基于INS误差补偿的卡尔曼滤波方程，构建基于INS 误差补偿的状态空间，通过GPS量测进行组合导航，将待补偿IMU 误差参数加入惯导解算误差模型中，在滤波器更新时，估计IMU误差 参数。陀螺漂移、陀螺刻度系数误差、陀螺安装误差角、加计零偏、 加计刻度系数误差和加计安装误差角都可以在的状态空间模型中建立。 第二步：IMU误差可观性分析，滤波器更新时，可以计算IMU误差 的协方差变化情况，协方差变化反映了状态量的可观测性，相对协方 差越接近0，表示可观测性越好。通过Kalman滤波器中实时记录的 所有IMU误差参数的协方差状态，查询可观测性较强的IMU误差， 即它们在滤波器中具有很好的估计效果，从而可以在后续导航中获得 准确的误差参数并进行补偿。第三步：IMU误差监测及补偿，在 Kalman滤波更新过程中，借助步骤二，估计可观测性较好的IMU误差参数，并在INS误差补偿模型中进行修正。第四步：补偿后的组合 导航，通过IMU误差参数补偿后，进行组合导航可提高导航精度
本公开构建了IMU误差参数监测模块：将待观测状态量加入状态 方程，可以在Kalman滤波器更新时获取协方差来明确当前可观测性 强的状态量，更有效的提高了通过IMU误差参数补偿后的组合导航的 导航精度。本公开的方案通用性更强，准确率更高，克服了输出校正 的缺点：在实际应用场景实验中发现，组合导航模型具有更好的鲁棒 性。同时经过监测模块选定的状态量进行反馈校正后，误差始终保持 为小量，克服了输出校正的缺点，更真实地反映理论***误差的动态 过程。
本公开还提供一种组合导航的校准装置，下面结合图3进行描述。
图3为本公开组合导航的校准装置的一些实施例的结构图。如图3 所示，该实施例的装置30包括：第一构建模块310，第二构建模块320， 滤波模块330，校准模块340。
第一构建模块310，用于根据惯性导航***的误差参数构建卡尔 曼滤波器的状态方程，其中，惯性导航***的误差参数包括：陀螺仪 漂移误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏 误差、加速度计刻度系数误差和加速度计安装误差角中的至少一项， 以及失准角、速度误差和位置误差。
在一些实施例中，在惯性导航***的误差参数包括：陀螺仪漂移 误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、 加速度计刻度系数误差和加速度计安装误差角的情况下，卡尔曼滤波 器的状态方程为：
Figure BDA0003431754860000111
其中，
Figure BDA0003431754860000112
φ＝[φE φN φU]为 失准角，δvn＝[δvE δvN δvU]为速度误差，δp＝[δL δλ δh]为 位置误差，/>
Figure BDA0003431754860000113
分别表示陀螺仪误差参数组成向量和加速度计误 差参数组成的向量，/>
Figure BDA0003431754860000114
Figure BDA0003431754860000115
δkgxx，δkgyy，δkgzz分别表示x，y， z轴的陀螺仪刻度系数误差，δkgxy，δkgxz，δkgyx，δkgyz，δkgzx， δkgzy为陀螺仪安装误差角，/>
Figure BDA0003431754860000116
为陀螺仪漂移误差， />
Figure BDA0003431754860000117
δkaxx，δkayy，δkazz为加速度计刻度系数误差，δkayx，δkazx，δkazy为加速度 计安装误差角，
Figure BDA0003431754860000118
为加速度计零偏误差，F为状态相 关的确定性矩阵函数，
Figure BDA0003431754860000119
为陀螺仪和加速度计的噪声，输 入矩阵为/>
Figure BDA00034317548600001111
为b系到n系的姿态变换矩阵，b系 为导航坐标系，n系为大地坐标系。
在一些实施例中，
Figure BDA0003431754860000121
其中，
Figure BDA0003431754860000122
Figure BDA0003431754860000123
Figure BDA0003431754860000124
Figure BDA0003431754860000125
Figure BDA0003431754860000126
Figure BDA0003431754860000127
Figure BDA0003431754860000128
i表示惯性坐标系，
Figure BDA0003431754860000129
为大地坐标系对惯性坐标 系的角速度，
Figure BDA00034317548600001211
为导航坐标系对大地坐标系的角速度，fb为x、y、z 轴加速度计比力输出组成的向量，RM为子午圈主曲率半径，RN为卯酉 圈主曲率半径，h为载体距地面的高度，L为纬度，/>
Figure BDA00034317548600001212
为地球自转角 速度，vE为导航坐标系中东向的速度，vN为导航坐标系中北向的速度，/>
Figure BDA00034317548600001213
为x轴陀螺仪的输出角速度，/>
Figure BDA00034317548600001214
为y轴陀螺仪的输出角速度，/>
Figure BDA00034317548600001215
为z轴陀螺仪的输出角速度，vn为东、北、天方向速度组成的向量在 导航坐标系下的投影，/>
Figure BDA00034317548600001216
为x轴加速度计的比力输出。。
第二构建模块320，用于根据惯性导航***输出的位置和GPS系 统输出的位置之间的位置误差，以及惯性导航***输出的速度和GPS ***输出的速度之间的速度误差，构建卡尔曼滤波器的量测方程。
在一些实施例中，卡尔曼滤波器的量测方程为：
Figure BDA0003431754860000131
其中，Δv、Δp分别表示惯性导航***输出的位置和GPS***输 出的位置之间的位置误差，惯性导航***输出的速度和GPS***输出 的速度之间的速度误差，V为观测噪声，
Figure BDA0003431754860000132
滤波模块330，用于根据卡尔曼滤波器的状态方程和卡尔曼滤波 器的量测方程，利用卡尔曼滤波器确定惯性导航***的误差参数的估 计值。
校准模块340，用于根据惯性导航***的误差参数的估计值，对 惯性导航***输出的位置和速度进行校准。
在一些实施例中，校准模块340用于将惯性导航***的误差参数 的估计值反馈至惯性导航***中的惯性导航解算模块，对惯性导航解 算模块中输入的陀螺仪的测量值和加速度计的测量值进行补偿；以便 由惯性导航解算模块根据补偿后的陀螺仪的测量值和加速度计的测量 值确定惯性导航***输出的位置和速度，从而得到校准后的惯性导航 ***输出的位置和速度。
在一些实施例中，校准模块340用于在卡尔曼滤波器的更新过程 中，确定惯性导航***的每种误差参数的协方差的值；针对惯性导航 ***的每种误差参数的协方差，在该种误差参数的协方差达到对应的 预设值的情况下，根据该种误差参数的估计值，对惯性导航***输出 的位置和速度进行校准。
本公开的实施例中的组合导航的校准装置可各由各种计算设备或 计算机***来实现，下面结合图4以及图5进行描述。
图4为本公开组合导航的校准装置的一些实施例的结构图。如图4 所示，该实施例的装置40包括：存储器410以及耦接至该存储器410 的处理器420，处理器420被配置为基于存储在存储器410中的指令， 执行本公开中任意一些实施例中的组合导航的校准方法。
其中，存储器410例如可以包括***存储器、固定非易失性存储 介质等。***存储器例如存储有操作***、应用程序、引导装载程序 (Boot Loader)、数据库以及其他程序等。
图5为本公开组合导航的校准装置的另一些实施例的结构图。如 图5所示，该实施例的装置50包括：存储器510以及处理器520，分 别与存储器410以及处理器420类似。还可以包括输入输出接口530、 网络接口540、存储接口550等。这些接口530，540，550以及存储器510和处理器520之间例如可以通过总线560连接。其中，输入输 出接口530为显示器、鼠标、键盘、触摸屏等输入输出设备提供连接 接口。网络接口540为各种联网设备提供连接接口，例如可以连接到 数据库服务器或者云端存储服务器等。存储接口550为SD卡、U盘 等外置存储设备提供连接接口。
本公开还提供一种组合导航的校准***，下面结合图6进行描述。
图6为本公开组合导航的校准***的一些实施例的结构图。如图 6所示，该实施例的***6包括：前述任意实施例的组合导航的校准 装置30/40/50，以及惯性导航***62和GPS***64，其中，惯性导 航***62包括：陀螺仪和加速度计。
惯性导航***62用于将输出位置和速度输入校准装置30/40/50， 并接收校准装置30/40/50输出的惯性导航***62的误差参数的估计 值，对输出的位置和速度进行校准后再输出；
GPS***64用于将输出位置和速度输入校准装置30/40/50。
本领域内的技术人员应当明白，本公开的实施例可提供为方法、 ***、或计算机程序产品。因此，本公开可采用完全硬件实施例、完 全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公 开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用 非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储 器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(***)、和计算 机程序产品的流程图和/或方框图来描述的。应理解为可由计算机程 序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程 图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序 指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处 理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据 处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个 流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据 处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算 机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现 在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指 定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理 设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产 生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令 提供用于实现在流程图一个流程或多个流程和/或方框图一个方框 或多个方框中指定的功能的步骤。
以上所述仅为本公开的较佳实施例，并不用以限制本公开，凡在本 公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包 含在本公开的保护范围之内。

Claims (10)

1.一种组合导航的校准方法，包括：
根据惯性导航***的误差参数构建卡尔曼滤波器的状态方程，其中，所述惯性导航***的误差参数包括：陀螺仪漂移误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、加速度计刻度系数误差和加速度计安装误差角中的至少一项，以及失准角、速度误差和位置误差；
根据所述惯性导航***输出的位置和GPS***输出的位置之间的位置误差，以及所述惯性导航***输出的速度和所述GPS***输出的速度之间的速度误差，构建所述卡尔曼滤波器的量测方程；
根据卡尔曼滤波器的状态方程和所述卡尔曼滤波器的量测方程，利用所述卡尔曼滤波器确定所述惯性导航***的误差参数的估计值；
根据所述惯性导航***的误差参数的估计值，对所述惯性导航***输出的位置和速度进行校准。
2.根据权利要求1所述的校准方法，其中，所述根据所述惯性导航***的误差参数的估计值，对所述惯性导航***输出的位置和速度进行校准包括：
将所述惯性导航***的误差参数的估计值反馈至所述惯性导航***中的惯性导航解算模块，对所述惯性导航解算模块中输入的陀螺仪的测量值和加速度计的测量值进行补偿；
所述惯性导航解算模块根据补偿后的陀螺仪的测量值和加速度计的测量值确定所述惯性导航***输出的位置和速度，从而得到校准后的所述惯性导航***输出的位置和速度。
3.根据权利要求1所述的校准方法，其中，所述根据所述惯性导航***的误差参数的估计值，对所述惯性导航***输出的位置和速度进行校准包括：
在所述卡尔曼滤波器的更新过程中，确定所述惯性导航***的每种误差参数的协方差的值；
针对所述惯性导航***的每种误差参数的协方差，在该种误差参数的协方差达到对应的预设值的情况下，根据该种误差参数的估计值，对所述惯性导航***输出的位置和速度进行校准。
4.根据权利要求1所述的校准方法，其中，在所述惯性导航***的误差参数包括：陀螺仪漂移误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、加速度计刻度系数误差和加速度计安装误差角的情况下，所述卡尔曼滤波器的状态方程为：
Figure FDA0003431754850000021
其中，
Figure FDA0003431754850000022
φ＝[φE φN φU]为失准角，δvn＝[δvEδvN δvU]为速度误差，δp＝[δL δλ δh]为位置误差，/>
Figure FDA0003431754850000023
分别表示陀螺仪误差参数组成向量和加速度计误差参数组成的向量，/>
Figure FDA0003431754850000024
Figure FDA0003431754850000025
δkgxx，δkgyy，δkgzz分别表示x，y，z轴的陀螺仪刻度系数误差，δkgxy，δkgxz，δkgyx，δkgyz，δkgzx，δkgzy为陀螺仪安装误差角，/>
Figure FDA0003431754850000026
为陀螺仪漂移误差，/>
Figure FDA0003431754850000027
δkaxx，δkayy，δkazz为加速度计刻度系数误差，δkayx，δkazx，δkazy为加速度计安装误差角，
Figure FDA0003431754850000028
为加速度计零偏误差，F为状态相关的确定性矩阵函数，
Figure FDA0003431754850000029
为陀螺仪和加速度计的噪声，输入矩阵为/>
Figure FDA00034317548500000210
Figure FDA00034317548500000211
为b系到n系的姿态变换矩阵，b系为导航坐标系，n系为大地坐标系。
5.根据权利要求4所述的校准方法，其中，
Figure FDA00034317548500000212
其中，
Figure FDA0003431754850000031
Figure FDA0003431754850000032
Figure FDA0003431754850000033
Figure FDA0003431754850000034
Figure FDA0003431754850000035
Figure FDA0003431754850000036
Figure FDA0003431754850000037
i表示惯性坐标系，
Figure FDA0003431754850000038
Figure FDA0003431754850000039
为大地坐标系对惯性坐标系的角速度，/>
Figure FDA00034317548500000310
为导航坐标系对大地坐标系的角速度，fb为x、y、z轴加速度计比力输出组成的向量，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，h为载体距地面的高度，L为纬度，/>
Figure FDA00034317548500000311
为地球自转角速度，vE为导航坐标系中东向的速度，vN为导航坐标系中北向的速度，/>
Figure FDA00034317548500000312
为x轴陀螺仪的输出角速度，/>
Figure FDA00034317548500000313
为y轴陀螺仪的输出角速度，/>
Figure FDA00034317548500000314
为z轴陀螺仪的输出角速度，vn为东、北、天方向速度组成的向量在导航坐标系下的投影，/>
Figure FDA00034317548500000315
为x轴加速度计的比力输出。
6.根据权利要求1所述的校准方法，其中，所述卡尔曼滤波器的量测方程为：
Figure FDA00034317548500000316
其中，Δv、Δp分别表示惯性导航***输出的位置和GPS***输出的位置之间的位置误差，所述惯性导航***输出的速度和所述GPS***输出的速度之间的速度误差，V为观测噪声，
Figure FDA0003431754850000041
7.一种组合导航的校准装置，包括：
第一构建模块，用于根据惯性导航***的误差参数构建卡尔曼滤波器的状态方程，其中，所述惯性导航***的误差参数包括：陀螺仪漂移误差、陀螺仪刻度系数误差、陀螺仪安装误差角、加速度计零偏误差、加速度计刻度系数误差和加速度计安装误差角中的至少一项，以及失准角、速度误差和位置误差；
第二构建模块，用于根据所述惯性导航***输出的位置和GPS***输出的位置之间的位置误差，以及所述惯性导航***输出的速度和所述GPS***输出的速度之间的速度误差，构建所述卡尔曼滤波器的量测方程；
滤波模块，用于根据卡尔曼滤波器的状态方程和所述卡尔曼滤波器的量测方程，利用所述卡尔曼滤波器确定所述惯性导航***的误差参数的估计值；
校准模块，用于根据所述惯性导航***的误差参数的估计值，对所述惯性导航***输出的位置和速度进行校准。
8.一种组合导航的校准装置，包括：
处理器；以及
耦接至所述处理器的存储器，用于存储指令，所述指令被所述处理器执行时，使所述处理器执行如权利要求1-6任一项所述的组合导航的校准方法。
9.一种非瞬时性计算机可读存储介质，其上存储有计算机程序，其中，该程序被处理器执行时实现权利要求1-6任一项所述方法的步骤。
10.一种组合导航的校准***，包括：权利要求7或8所述的校准装置；以及惯性导航***和GPS***，其中，所述惯性导航***包括：陀螺仪和加速度计；
所述惯性导航***用于将输出位置和速度输入所述校准装置，并接收所述校准装置输出的所述惯性导航***的误差参数的估计值，对输出的位置和速度进行校准后再输出；
所述GPS***用于将输出位置和速度输入所述校准装置。
CN202111600911.4A 2021-12-24 2021-12-24 组合导航的校准方法、装置和*** Pending CN116380119A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111600911.4A CN116380119A (zh) 2021-12-24 2021-12-24 组合导航的校准方法、装置和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111600911.4A CN116380119A (zh) 2021-12-24 2021-12-24 组合导航的校准方法、装置和***

Publications (1)

Publication Number Publication Date
CN116380119A true CN116380119A (zh) 2023-07-04

Family

ID=86973646

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111600911.4A Pending CN116380119A (zh) 2021-12-24 2021-12-24 组合导航的校准方法、装置和***

Country Status (1)

Country Link
CN (1) CN116380119A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117405145A (zh) * 2023-12-14 2024-01-16 深圳市晟丰达科技有限公司 一种基于智能分析的惯性导航管理方法、***及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117405145A (zh) * 2023-12-14 2024-01-16 深圳市晟丰达科技有限公司 一种基于智能分析的惯性导航管理方法、***及存储介质
CN117405145B (zh) * 2023-12-14 2024-02-20 深圳市晟丰达科技有限公司 一种基于智能分析的惯性导航管理方法、***及存储介质

Similar Documents

Publication Publication Date Title
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN104736963B (zh) 测绘***和方法
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN110501024A (zh) 一种车载ins/激光雷达组合导航***的量测误差补偿方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及***
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
De Agostino et al. Performances comparison of different MEMS-based IMUs
CN112505737B (zh) 一种gnss/ins组合导航方法
CN104880189B (zh) 一种动中通天线低成本跟踪抗干扰方法
CN112595350B (zh) 一种惯导***自动标定方法及终端
CN113783652B (zh) 一种组合导航***的数据同步方法和装置
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及***
CN113405545A (zh) 定位方法、装置、电子设备及计算机存储介质
CN112835085A (zh) 确定车辆位置的方法和装置
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN109579870A (zh) 捷联惯导***的自动对准方法和组合导航装置
CN112066985A (zh) 一种组合导航***初始化方法、装置、介质及电子设备
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN112835086A (zh) 确定车辆位置的方法和装置
CN114061570A (zh) 车辆定位方法、装置、计算机设备和存储介质
US20210190499A1 (en) Method for providing a navigation information, corresponding system and program product
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN116380119A (zh) 组合导航的校准方法、装置和***
CN115790613B (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