CN113607176B - 组合导航系统轨迹输出方法及装置 - Google Patents

组合导航系统轨迹输出方法及装置 Download PDF

Info

Publication number
CN113607176B
CN113607176B CN202111183411.5A CN202111183411A CN113607176B CN 113607176 B CN113607176 B CN 113607176B CN 202111183411 A CN202111183411 A CN 202111183411A CN 113607176 B CN113607176 B CN 113607176B
Authority
CN
China
Prior art keywords
navigation system
state quantity
estimated value
error
navigation
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
CN202111183411.5A
Other languages
English (en)
Other versions
CN113607176A (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.)
Zhidao Network Technology Beijing Co Ltd
Original Assignee
Zhidao Network Technology Beijing 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 Zhidao Network Technology Beijing Co Ltd filed Critical Zhidao Network Technology Beijing Co Ltd
Priority to CN202111183411.5A priority Critical patent/CN113607176B/zh
Publication of CN113607176A publication Critical patent/CN113607176A/zh
Application granted granted Critical
Publication of CN113607176B publication Critical patent/CN113607176B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Abstract

本申请是关于一种组合导航系统轨迹输出方法及装置。所述组合导航系统包括全球卫星导航系统、惯性导航系统，该方法包括：建立组合导航系统的卡尔曼滤波器；基于所述卡尔曼滤波器对各个导航状态量进行估计，获得所述各个导航状态量的估计值，其中，所述导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差；根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正；根据所述校正后的所述惯性导航系统的状态，获得所述组合导航系统输出的轨迹。本申请提供的方案，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。

Description

组合导航系统轨迹输出方法及装置
技术领域
本申请涉及导航技术领域，尤其涉及一种组合导航系统轨迹输出方法及装置。
背景技术
全球卫星导航系统(Global Navigation Satellite System，GNSS)是一种常用的室外定位方式，但是由于GNSS定位对环境的要求极高，在复杂环境中(如城市、立交桥、隧道、林荫道等)，GNSS卫星信号收到遮蔽物的遮蔽后，信号强度减弱，观测噪声大幅增加，有效数据丢失，使得GNSS基本无法提供精准定位。惯性导航系统依靠惯性测量单元(InertialMeasurement Unit，IMU)的原始数据，可不借助外力，通过积分产生导航信息(位置、姿态、速度等)。由于其不借助外力，所以可以应用在所有的复杂环境中。但是其定位误差会随着时间而增大，在需要长时间定位时达不到需要的精度。自动驾驶车辆的GNSS/IMU组合导航系统集合全球卫星导航系统和惯性导航系统两者优点，通过卡尔曼滤波对全球卫星导航系统和惯性导航系统的预测结果进行融合，能够防止GNSS信号的干扰和丢失，并可以校正惯性导航系统的误差。
但相关技术的GNSS/IMU组合导航系统输出的轨迹存在锯齿状波动，不够平滑，对于高速行驶的自动驾驶车辆，定位轨迹存在锯齿状波动是无法接受的，甚至会影响自动驾驶的安全性。总之，相关技术的GNSS/IMU组合导航系统无法为自动驾驶提供稳定平滑的、精度可靠的轨迹。
发明内容
为解决或部分解决相关技术中存在的问题，本申请提供一种组合导航系统轨迹输出方法及装置，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。
本申请第一方面提供一种组合导航系统轨迹输出方法，所述组合导航系统包括全球卫星导航系统、惯性导航系统，所述方法包括：
建立组合导航系统的卡尔曼滤波器；
基于所述卡尔曼滤波器对各个导航状态量进行估计，获得所述各个导航状态量的估计值，其中，所述导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差；
根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正；
根据所述校正后的所述惯性导航系统的状态，获得所述组合导航系统输出的轨迹。
优选的，所述方法还包括：
根据所述各个导航状态量的估计值的修正系数、所述各个导航状态量的估计值，为所述卡尔曼滤波器下一次滤波设置初始值。
优选的，所述根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，包括：
根据所述卡尔曼滤波器的滤波反馈周期、所述各个导航状态量的修正延迟时间参数，分别获得所述各个导航状态量的估计值的修正系数；
根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正。
优选的，所述根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，包括：
根据所述各个导航状态量的估计值的修正系数，分别将所述各个导航状态量的估计值乘以1与各自的修正系数的差，分别获得所述各个导航状态量的估计值的反馈量；
分别将所述各个导航状态量的估计值的反馈量反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，其中，所述惯性导航系统的状态包括位置、速度、姿态、加速度计零偏、陀螺仪零偏。
优选的，所述位置误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍；
所述速度误差的修正延迟时间参数为卡尔曼滤波反馈周期的1-5倍；
所述姿态误差包括横滚角误差、俯仰角误差、航向角误差，横滚角误差的修正延迟时间参数和俯仰角误差的修正延迟时间参数分别为卡尔曼滤波反馈周期的0.5-1.5倍，航向角误差的修正延迟时间参数分别为卡尔曼滤波反馈周期的0.2-0.8倍；
所述加速度计零偏误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍；
所述陀螺仪零偏误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍。
本申请第二方面提供一种组合导航系统轨迹输出装置，所述组合导航系统包括全球卫星导航系统、惯性导航系统，所述装置包括：
建立模块，用于建立组合导航系统的卡尔曼滤波器；
估计模块，用于基于所述建立模块建立的卡尔曼滤波器对各个导航状态量进行估计，获得所述各个导航状态量的估计值，其中，所述导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差；
反馈校正模块，用于根据所述各个导航状态量的估计值的修正系数，分别将所述估计模块获得的各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正；
轨迹模块，用于根据所述反馈校正模块校正后的所述惯性导航系统的状态，获得所述组合导航系统输出的轨迹。
优选的，所述装置还包括：
设置模块，用于根据所述各个导航状态量的估计值的修正系数、所述估计模块获得的各个导航状态量的估计值，为所述建立模块建立的卡尔曼滤波器下一次滤波设置初始值。
优选的，所述装置还包括：
系数模块，用于根据所述建立模块建立的卡尔曼滤波器的滤波反馈周期、所述各个导航状态量的修正延迟时间参数，分别获得所述各个导航状态量的估计值的修正系数。
优选的，所述反馈校正模块具体用于：
根据所述各个导航状态量的估计值的修正系数，分别将所述估计模块获得的各个导航状态量的估计值乘以1与各自的修正系数的差，分别获得所述各个导航状态量的估计值的反馈量；
分别将所述各个导航状态量的估计值的反馈量反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，其中，所述惯性导航系统的状态包括位置、速度、姿态、加速度计零偏、陀螺仪零偏。
本申请第三方面提供一种电子设备，包括：
处理器；以及
存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如上所述的方法。
本申请第四方面提供一种非暂时性机器可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如上所述的方法。
本申请提供的技术方案可以包括以下有益效果：
本申请的技术方案，根据各个导航状态量的估计值的修正系数，将各个导航状态量的估计值反馈至惯性导航系统并分别对惯性导航系统的状态进行校正；根据校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹；能够获得精度更高的位置、速度、姿态、加速度计零偏和陀螺仪零偏，从而获得精度更高的轨迹，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
通过结合附图对本申请示例性实施方式进行更详细的描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号通常代表相同部件。
图1是本申请实施例示出的组合导航系统轨迹输出方法的流程示意图；
图2是本申请实施例示出的组合导航系统轨迹输出方法的另一流程示意图；
图3是本申请实施例示出的组合导航系统轨迹输出装置的结构示意图；
图4是本申请实施例示出的电子设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本申请的实施方式。虽然附图中显示了本申请的实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的范围完整地传达给本领域的技术人员。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语“第一”、“第二”、“第三”等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
本申请实施例提供一种组合导航系统轨迹输出方法，能够为自动驾驶输出稳定平滑的、精度可靠的轨迹。
以下结合附图详细描述本申请实施例的技术方案。
实施例一：
图1是本申请实施例示出的组合导航系统轨迹输出方法的流程示意图。
参见图1，一种组合导航系统轨迹输出方法，组合导航系统包括全球卫星导航系统、惯性导航系统，包括：
在步骤S101中，建立组合导航系统的卡尔曼滤波器。
在一实施方式中，自动驾驶车辆上设有GNSS/IMU组合导航系统，GNSS/IMU组合导航系统包括惯性导航系统和GNSS。惯性导航系统包括惯性测量单元，惯性测量单元包括但不限于三轴加速度计、三轴陀螺仪。
在一实施方式中，惯性导航系统根据三轴加速度计获得的自动驾驶车辆的三轴加速度、三轴陀螺仪获得的自动驾驶车辆的三轴角速度，进行惯性导航解算，获得车辆的位置、速度、姿态。GNSS根据观测数据获得车辆的位置、速度、姿态。根据惯性导航系统获得的车辆的位置与GNSS获得的车辆的位置，获得位置误差，根据惯性导航系统获得的车辆的速度与GNSS获得的车辆的速度，获得速度误差，根据惯性导航系统获得的车辆的姿态与GNSS获得的车辆的姿态，获得姿态误差；基于卡尔曼滤波算法，以位置误差、速度误差、姿态误差、加速度计零偏误差、陀螺仪零偏误差为状态量，建立组合导航系统的卡尔曼滤波器。
在一实施方式中，可以基于卡尔曼滤波算法，通过融合的三轴加速度、三轴角速度、GNSS的观测数据，获得加速度计零偏误差、陀螺仪零偏误差。
在步骤S102中，基于卡尔曼滤波器对各个导航状态量进行估计，获得各个导航状态量的估计值，其中，导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差。
在一实施方式中，可以将同一时刻的惯性导航系统获得的三轴加速度、三轴角速度、GNSS的观测数据导入卡尔曼滤波器，通过卡尔曼滤波器对惯性导航系统的位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差进行估计，获得位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值。
在步骤S103中，根据各个导航状态量的估计值的修正系数，分别将各个导航状态量的估计值反馈至惯性导航系统并分别对惯性导航系统的状态进行校正。
在一实施方式中，根据位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值的修正系数，按设定的算法，将位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值反馈至惯性导航系统，分别对惯性导航系统的状态进行校正。
在步骤S104中，根据校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹。
在一实施方式中，可以使惯性导航系统根据校正后的惯性导航系统的状态，获得车辆的位置、速度、姿态，组合导航系统以连续时刻获得的车辆的位置作为车辆的轨迹进行输出。
本申请的实施例提供的组合导航系统轨迹输出方法，根据各个导航状态量的估计值的修正系数，将各个导航状态量的估计值反馈至惯性导航系统并分别对惯性导航系统的状态进行校正；根据校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹；能够获得精度更高的位置、速度、姿态、加速度计零偏和陀螺仪零偏，从而获得精度更高的轨迹，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。
实施例二：
图2是本申请实施例示出的组合导航系统轨迹输出方法的另一流程示意图。图2相对于图1更详细描述了本申请的方案。
参见图2，一种组合导航系统轨迹输出方法，组合导航系统包括全球卫星导航系统、惯性导航系统，包括：
在步骤S201中，建立组合导航系统的卡尔曼滤波器。
该步骤可以参见步骤S101的描述，此处不再赘述。
在步骤S202中，基于卡尔曼滤波器对各个导航状态量进行估计，获得各个导航状态量的估计值，其中，导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差。
在一实施方式中，可以将同一时刻的惯性导航系统获得的三轴加速度、三轴角速度、GNSS的观测数据导入卡尔曼滤波器，通过卡尔曼滤波器对惯性导航系统的位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差进行估计，获得位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值。
在一具体实施方式中，惯性导航系统的导航坐标系为东北天坐标系。东北天坐标系的X轴指东向，Y轴指北向，Z轴指天向。位置误差的估计值包括经度误差的估计值、纬度误差的估计值、高度误差的估计值。速度误差的估计值包括东向速度误差的估计值、北向速度误差的估计值、天向速度误差的估计值。姿态误差的估计值包括横滚角误差的估计值、俯仰角误差的估计值、航向角误差的估计值。加速度计零偏误差的估计值包括东向加速度计零偏误差的估计值、北向加速度计零偏误差的估计值、天向加速度计零偏误差的估计值。陀螺仪零偏误差的估计值包括东向陀螺仪零偏误差的估计值、北向陀螺仪零偏误差的估计值、天向陀螺仪零偏误差的估计值。
在步骤S203中，根据卡尔曼滤波器的滤波反馈周期、各个导航状态量的修正延迟时间参数，获得各个导航状态量的估计值的修正系数。
在一实施方式中，惯性导航系统的状态包括位置、速度、姿态、加速度计零偏和陀螺仪零偏。加速度计输出可以包括三轴加速度，陀螺仪输出可以包括三轴角速度。位置、速度、姿态、加速度计输出和陀螺仪输出的可观测度是不同的，可以根据位置、速度和加速度计输出的可观测度较强的特性，为位置误差、速度误差和加速度计零偏误差设置较大的修正延迟时间参数τ，可以根据姿态和陀螺仪输出的可观测度较弱的特性，为姿态误差和陀螺仪零偏误差设置较小的修正延迟时间参数τ。
在一具体实施方式中，卡尔曼滤波器的滤波反馈周期T可以根据GNSS的采样频率f确定，T=1/f。例如，GNSS的频率为10Hz，则卡尔曼滤波器的滤波反馈周期T为0.1秒。
在一实施方式中，可以根据卡尔曼滤波器的滤波反馈周期T设定各个导航状态量的修正延迟时间参数τ。位置误差的修正延迟时间参数设置为80T-120T；速度误差的修正延迟时间参数设置为1T-5T；横滚角误差和俯仰角误差的修正延迟时间参数分别设置为0.5T-1.5T，航向角误差的修正延迟时间参数设置为0.2T-0.8T；加速度计零偏误差的修正延迟时间参数设置为80T-120T；陀螺仪零偏误差的修正延迟时间参数设置为80T-120T。
在一具体实施方式中，根据卡尔曼滤波器的滤波反馈周期T，位置误差的修正延迟时间参数设置为100T；速度误差的修正延迟时间参数设置为3T；横滚角误差和俯仰角误差的修正延迟时间参数分别设置为1T，航向角误差的修正延迟时间参数设置为0.5T；加速度计零偏误差的修正延迟时间参数设置为100T；陀螺仪零偏误差的修正延迟时间参数设置为100T。
在一具体实施方式中，可以根据卡尔曼滤波器的滤波反馈周期T、各个导航状态量的修正延迟时间参数τ，获得各个导航状态量的估计值的修正系数α，α=exp(-T/τ)。各个导航状态量的修正延迟时间参数τ是不同的，各个导航状态量的估计值的修正系数α也是不同的，修正延迟时间参数τ决定修正系数α的大小，修正系数α的范围为(0，1]，可以调节导航状态量的修正延迟时间参数τ，调节导航状态量的估计值的修正系数α。
例如，位置误差中的高度误差的修正延迟时间参数τh为100T，高度误差的估计值的修正系数为αh，αh=exp(-T/τh)=exp(-T/100T)=exp(-1/100)。
在步骤S204中，根据各个导航状态量的估计值的修正系数，分别将各个导航状态量的估计值乘以1与各自的修正系数的差，获得各个导航状态量的估计值的反馈量。
在一具体实施方式中，根据各个导航状态量的估计值的具体的修正系数α，分别将各个导航状态量的估计值X乘以（1-各自的修正系数），获得各个导航状态量的估计值的反馈量F，F=X*（1-α）。
例如，高度误差的估计值为Xh，高度误差的估计值的修正系数为αh，高度误差的估计值反馈量为Fh，Fh=Xh*（1-αh）。同理，可以根据其它各个导航状态量的估计值的具体的修正系数，分别将其它各个导航状态量的估计值乘以（1-各自的修正系数），分别获得各个导航状态量的估计值的反馈量。
在步骤S205中，分别将各个导航状态量的估计值的反馈量反馈至惯性导航系统并分别对惯性导航系统的状态进行校正。
在一具体实施方式中，根据反馈至惯性导航系统的各个导航状态量的估计值的反馈量，对惯性导航系统的各个状态Sˊ进行反馈校正，获得校正后的惯性导航系统的状态S，S=Sˊ-F=Sˊ-（1-α）*X。
例如，惯性导航系统的状态的高度为hˊ，高度误差的估计值为Xh，高度误差的估计值的修正系数为αh，高度误差的估计值反馈量为Fh，校正后的惯性导航系统的高度为h，h=hˊ-Fh=hˊ-（1-αh）*Xh。
在一具体实施方式中，修正延迟时间参数τ越大，修正系数α越小，反馈至惯性导航系统的反馈量F越大，能够得到精度更高的惯性导航系统的状态。可以通过调节导航状态量的修正延迟时间参数τ，调节导航状态量的估计值的修正系数α，从而调节反馈至惯性导航系统的导航状态量的估计值的反馈量F，能够获得精度更高的位置、速度、姿态、加速度计零偏、陀螺仪零偏。
在步骤S206中，根据校正各个导航状态量后的惯性导航系统的状态，获得组合导航系统输出的轨迹。
在一实施方式中，可以使惯性导航系统根据校正后的惯性导航系统的状态，获得车辆的位置、速度、姿态，组合导航系统以连续时刻获得的车辆的位置作为车辆的轨迹进行输出。
在一具体实施方式中，可以使惯性导航系统根据校正后的惯性导航系统的位置、速度、姿态、三轴加速度、三轴角速度，进行惯性导航解算，获得车辆的位置、速度、姿态，组合导航系统以连续时刻获得的车辆的位置作为车辆的轨迹进行输出。
在步骤S207中，根据各个导航状态量的估计值的修正系数、各个导航状态量的估计值，为卡尔曼滤波器下一次滤波设置初始值。
在一具体实施方式中，根据各个导航状态量的估计值的具体的修正系数，分别将各个导航状态量的估计值X乘以各自的修正系数α，获得各个导航状态量的估计值的剩余量X¯，X¯=X*α。可以将各个导航状态量的估计值的剩余量反馈至组合导航系统的卡尔曼滤波器，将各个导航状态量的估计值的剩余量设置为卡尔曼滤波器下次迭代滤波的初始值。
本申请的实施例，基于卡尔曼滤波器对各个导航状态量进行估计，获得各个导航状态量的估计值；根据卡尔曼滤波器的滤波反馈周期、各个导航状态量的修正延迟时间参数，获得各个导航状态量的估计值的修正系数；根据各个导航状态量的估计值的修正系数，分别将各个导航状态量的估计值乘以1与各自的修正系数的差，获得各个导航状态量的估计值的反馈量；分别将各个导航状态量的估计值的反馈量反馈至惯性导航系统并分别对惯性导航系统的状态进行校正。例如，东向加速度计零偏误差的估计值为Xba，N，卡尔曼滤波器的滤波反馈周期为T，东向加速度计零偏误差的修正延迟时间参数为τba，N，东向加速度计零偏误差的估计值的修正系数为αba，N，东向加速度计零偏误差的估计值的反馈量为Fba，N，惯性导航系统的东向加速度为áN，校正后的东向加速度为aN。东向加速度计零偏误差的估计值的剩余量X¯ba，N，具体公式如下：
aN=áN-Fba，N
Fba=áN-(1-αba，N)
αba，N=exp(-T/τba，N)
X¯ba，N=Xba，N*αba，N。
本申请的实施例提供的组合导航系统轨迹输出方法，根据卡尔曼滤波器的滤波反馈周期、各个导航状态量的修正延迟时间参数，获得各个导航状态量的估计值的修正系数，不同的导航状态量的修正延迟时间参数不同，从而不同的的导航状态量的估计值的修正系数不同，根据不同的导航状态量的估计值的修正系数，获得不同的导航状态量的估计值的反馈量，将不同的导航状态量的估计值的反馈量部分反馈至惯性导航系统并分别对惯性导航系统的状态进行校正；根据校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹；能够获得精度更高的位置、速度、姿态、加速度计零偏和陀螺仪零偏，有效地降低输出轨迹的锯齿状波动，以提高了轨迹的平滑度，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。
实施例三：
与前述应用功能实现方法实施例相对应，本申请还提供了一种组合导航系统轨迹输出装置、电子设备及相应的实施例。
图3是本申请实施例示出的组合导航系统轨迹输出装置的结构示意图。
参见图3，一种组合导航系统轨迹输出装置，包括建立模块301、估计模块302、反馈校正模块303、轨迹模块304。
建立模块301，用于建立组合导航系统的卡尔曼滤波器。
在一实施方式中，自动驾驶车辆上设有GNSS/IMU组合导航系统，GNSS/IMU组合导航系统包括惯性导航系统和GNSS。惯性导航系统包括惯性测量单元，惯性测量单元包括但不限于三轴加速度计、三轴陀螺仪。
在一实施方式中，惯性导航系统根据三轴加速度计获得的自动驾驶车辆的三轴加速度、三轴陀螺仪获得的自动驾驶车辆的三轴角速度，进行惯性导航解算，获得车辆的位置、速度、姿态。GNSS根据观测数据获得车辆的位置、速度、姿态。建立模块301根据惯性导航系统获得的车辆的位置与GNSS获得的车辆的位置，获得位置误差，根据惯性导航系统获得的车辆的速度与GNSS获得的车辆的速度，获得速度误差，根据惯性导航系统获得的车辆的姿态与GNSS获得的车辆的姿态，获得姿态误差；基于卡尔曼滤波算法，以位置误差、速度误差、姿态误差、加速度计零偏误差、陀螺仪零偏误差为状态量，建立组合导航系统的卡尔曼滤波器。
在一实施方式中，建立模块301可以基于卡尔曼滤波算法，通过融合的三轴加速度、三轴角速度、GNSS的观测数据，获得加速度计零偏误差、陀螺仪零偏误差。
估计模块302，用于基于建立模块301建立的卡尔曼滤波器对各个导航状态量进行估计，获得各个导航状态量的估计值，其中，导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差。
在一实施方式中，估计模块302可以将同一时刻的惯性导航系统获得的三轴加速度、三轴角速度、GNSS的观测数据导入卡尔曼滤波器，通过卡尔曼滤波器对惯性导航系统的位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差进行估计，获得位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值。
反馈校正模块303，用于根据各个导航状态量的估计值的修正系数，分别将估计模块302获得的各个导航状态量的估计值反馈至惯性导航系统并分别对惯性导航系统的状态进行校正。
在一实施方式中，反馈校正模块303根据位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值的修正系数，按设定的算法，将估计模块302获得的位置误差的估计值、速度误差的估计值、姿态误差的估计值、加速度计零偏误差的估计值和陀螺仪零偏误差的估计值反馈至惯性导航系统，分别对惯性导航系统的状态进行校正。
轨迹模块304，用于根据反馈校正模块303校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹。
在一实施方式中，轨迹模块304可以使惯性导航系统根据校正后的惯性导航系统的状态，获得车辆的位置、速度、姿态，组合导航系统以连续时刻获得的车辆的位置作为车辆的轨迹进行输出。
本申请的实施例提供的技术方案，根据各个导航状态量的估计值的修正系数，将各个导航状态量的估计值反馈至惯性导航系统并分别对惯性导航系统的状态进行校正；根据校正后的惯性导航系统的状态，获得组合导航系统输出的轨迹；能够获得精度更高的位置、速度、姿态、加速度计零偏和陀螺仪零偏，获得精度更高的轨迹，能够为自动驾驶提供稳定平滑的、精度可靠的轨迹。
在一实施方式中，组合导航系统轨迹输出装置还包括系数模块；系数模块，用于根据建立模块建立的卡尔曼滤波器的滤波反馈周期、各个导航状态量的修正延迟时间参数，分别获得各个导航状态量的估计值的修正系数。反馈校正模块根据系数模块获得的各个导航状态量的估计值的修正系数，分别将估计模块获得的各个导航状态量的估计值乘以1与各自的修正系数的差，分别获得各个导航状态量的估计值的反馈量；分别将各个导航状态量的估计值的反馈量反馈至惯性导航系统并分别对惯性导航系统的状态进行校正，其中，惯性导航系统的状态包括位置、速度、姿态、加速度计零偏、陀螺仪零偏。
在一实施方式中，组合导航系统轨迹输出装置还包括设置模块；设置模块，用于根据各个导航状态量的估计值的修正系数、估计模块3获得的各个导航状态量的估计值，为建立模块建立的卡尔曼滤波器下一次滤波设置初始值。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不再做详细阐述说明。
图4是本申请实施例示出的电子设备的结构示意图。
参见图4，电子设备40包括存储器401和处理器402。
处理器402可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器401可以包括各种类型的存储单元，例如系统内存、只读存储器（ROM），和永久存储装置。其中，ROM可以存储处理器402或者计算机的其他模块需要的静态数据或者指令。永久存储装置可以是可读写的存储装置。永久存储装置可以是即使计算机断电后也不会失去存储的指令和数据的非易失性存储设备。在一些实施方式中，永久性存储装置采用大容量存储装置（例如磁或光盘、闪存）作为永久存储装置。另外一些实施方式中，永久性存储装置可以是可移除的存储设备（例如软盘、光驱）。系统内存可以是可读写存储设备或者易失性可读写存储设备，例如动态随机访问内存。系统内存可以存储一些或者所有处理器在运行时需要的指令和数据。此外，存储器401可以包括任意计算机可读存储媒介的组合，包括各种类型的半导体存储芯片（DRAM，SRAM，SDRAM，闪存，可编程只读存储器），磁盘和/或光盘也可以采用。在一些实施方式中，存储器401可以包括可读和/或写的可移除的存储设备，例如激光唱片（CD）、只读数字多功能光盘（例如DVD-ROM，双层DVD-ROM）、只读蓝光光盘、超密度光盘、闪存卡（例如SD卡、min SD卡、Micro-SD卡等等）、磁性软盘等等。计算机可读存储媒介不包含载波和通过无线或有线传输的瞬间电子信号。
存储器401上存储有可执行代码，当可执行代码被处理器402处理时，可以使处理器402执行上文述及的方法中的部分或全部。
此外，根据本申请的方法还可以实现为一种计算机程序或计算机程序产品，该计算机程序或计算机程序产品包括用于执行本申请的上述方法中部分或全部步骤的计算机程序代码指令。
或者，本申请还可以实施为一种非暂时性机器可读存储介质（或计算机可读存储介质、或机器可读存储介质），其上存储有可执行代码（或计算机程序、或计算机指令代码），当可执行代码（或计算机程序、或计算机指令代码）被电子设备（或电子设备、服务器等）的处理器执行时，使处理器执行根据本申请的上述方法的各个步骤的部分或全部。
以上已经描述了本申请的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

Claims (6)

1.一种组合导航系统轨迹输出方法，所述组合导航系统包括全球卫星导航系统、惯性导航系统，其特征在于，包括：
建立组合导航系统的卡尔曼滤波器；
基于所述卡尔曼滤波器对各个导航状态量进行估计，获得所述各个导航状态量的估计值，其中，所述导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差；
根据所述各个导航状态量的估计值的修正系数α，分别将所述各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，包括：根据所述 卡尔曼滤波器的滤波反馈周期T、所述各个导航状态量的修正延迟时间参数τ，分别获得所 述各个导航状态量的估计值的修正系数α，α=exp(-T/τ)，根据所述各个导航状态量的估计 值的修正系数α，分别将所述各个导航状态量的估计值乘以1与各自的修正系数α的差，分别 获得所述各个导航状态量的估计值的反馈量，分别将所述各个导航状态量的估计值的反馈 量反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，其中，所述惯性 导航系统的状态包括位置、速度、姿态、加速度计零偏、陀螺仪零偏；
根据所述校正后的所述惯性导航系统的状态，获得所述组合导航系统输出的轨迹。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
根据所述各个导航状态量的估计值的修正系数、所述各个导航状态量的估计值，为所述卡尔曼滤波器下一次滤波设置初始值。
3.根据权利要求1所述的方法，其特征在于：
所述位置误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍；
所述速度误差的修正延迟时间参数为卡尔曼滤波反馈周期的1-5倍；
所述姿态误差包括横滚角误差、俯仰角误差、航向角误差，横滚角误差的修正延迟时间参数和俯仰角误差的修正延迟时间参数分别为卡尔曼滤波反馈周期的0.5-1.5倍，航向角误差的修正延迟时间参数分别为卡尔曼滤波反馈周期的0.2-0.8倍；
所述加速度计零偏误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍；
所述陀螺仪零偏误差的修正延迟时间参数为卡尔曼滤波反馈周期的80-100倍。
4.一种组合导航系统轨迹输出装置，所述组合导航系统包括全球卫星导航系统、惯性导航系统，其特征在于：
建立模块，用于建立组合导航系统的卡尔曼滤波器；
估计模块，用于基于所述建立模块建立的卡尔曼滤波器对各个导航状态量进行估计，获得所述各个导航状态量的估计值，其中，所述导航状态量包括位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪零偏误差；
系数模块，用于根据所述建立模块建立的卡尔曼滤波器的滤波反馈周期T、所述各个导 航状态量的修正延迟时间参数τ，分别获得所述各个导航状态量的估计值的修正系数α，α= exp(-T/τ)；
反馈校正模块，用于根据所述系数模块获得的各个导航状态量的估计值的修正系数α，分别将所述估计模块获得的各个导航状态量的估计值反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，包括：根据所述各个导航状态量的估计值的修正系数 α，分别将所述估计模块获得的各个导航状态量的估计值乘以1与各自的修正系数α的差，分 别获得所述各个导航状态量的估计值的反馈量，分别将所述各个导航状态量的估计值的反 馈量反馈至所述惯性导航系统并分别对所述惯性导航系统的状态进行校正，其中，所述惯 性导航系统的状态包括位置、速度、姿态、加速度计零偏、陀螺仪零偏；
轨迹模块，用于根据所述反馈校正模块校正后的所述惯性导航系统的状态，获得所述组合导航系统输出的轨迹。
5.根据权利要求4所述的装置，其特征在于，所述装置还包括：
设置模块，用于根据所述各个导航状态量的估计值的修正系数、所述估计模块获得的各个导航状态量的估计值，为所述建立模块建立的卡尔曼滤波器下一次滤波设置初始值。
6.一种非暂时性机器可读存储介质，其特征在于，非暂时性机器可读存储介质上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如权利要求1-3中任一项所述的方法。
CN202111183411.5A 2021-10-11 2021-10-11 组合导航系统轨迹输出方法及装置 Active CN113607176B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111183411.5A CN113607176B (zh) 2021-10-11 2021-10-11 组合导航系统轨迹输出方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111183411.5A CN113607176B (zh) 2021-10-11 2021-10-11 组合导航系统轨迹输出方法及装置

Publications (2)

Publication Number Publication Date
CN113607176A CN113607176A (zh) 2021-11-05
CN113607176B true CN113607176B (zh) 2021-12-10

Family

ID=78310923

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111183411.5A Active CN113607176B (zh) 2021-10-11 2021-10-11 组合导航系统轨迹输出方法及装置

Country Status (1)

Country Link
CN (1) CN113607176B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115096304B (zh) * 2022-08-26 2022-11-22 中国船舶重工集团公司第七0七研究所 延时误差修正方法、装置、电子设备和存储介质
CN117647826B (zh) * 2024-01-29 2024-04-12 成都星历科技有限公司 基于干扰源定位的导航欺骗干扰信号的检测系统及方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101122637A (zh) * 2007-08-27 2008-02-13 北京航空航天大学 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法
CN101666868A (zh) * 2009-09-30 2010-03-10 北京航空航天大学 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法
CN101949710A (zh) * 2010-07-28 2011-01-19 北京泰豪联星技术有限公司 Gnss辅助mems惯性传感器零偏的快速在线动态标定方法
CN102176041A (zh) * 2011-01-17 2011-09-07 浙江大学 一种基于gnss/sins组合的车辆导航监控系统
CN102636798A (zh) * 2012-04-12 2012-08-15 南京航空航天大学 基于环路状态自检测的sins/gps深组合导航方法
CN103278837A (zh) * 2013-05-17 2013-09-04 南京理工大学 基于自适应滤波的sins/gnss多级容错组合导航方法
CN103439989A (zh) * 2013-09-03 2013-12-11 湖南工业大学 一种绕包机绕包速度同步控制方法
CN107782307A (zh) * 2016-08-26 2018-03-09 北京自动化控制设备研究所 一种sins/dr组合导航系统里程计异常数据后处理方法
CN111156994A (zh) * 2019-12-31 2020-05-15 西安航天华迅科技有限公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111272177A (zh) * 2020-02-28 2020-06-12 上海航天控制技术研究所 基于时间对准的间接滤波相对导航方法及系统
CN111928869A (zh) * 2020-10-10 2020-11-13 蘑菇车联信息科技有限公司 车辆运动轨迹估计方法、装置及电子设备
CN111947681A (zh) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 一种gnss与惯导组合导航位置输出的滤波校正方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101697645B1 (ko) * 2014-10-06 2017-01-18 현대모비스 주식회사 추측 항법과 gps를 이용한 복합 항법 시스템 및 그 방법

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101122637A (zh) * 2007-08-27 2008-02-13 北京航空航天大学 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法
CN101666868A (zh) * 2009-09-30 2010-03-10 北京航空航天大学 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法
CN101949710A (zh) * 2010-07-28 2011-01-19 北京泰豪联星技术有限公司 Gnss辅助mems惯性传感器零偏的快速在线动态标定方法
CN102176041A (zh) * 2011-01-17 2011-09-07 浙江大学 一种基于gnss/sins组合的车辆导航监控系统
CN102636798A (zh) * 2012-04-12 2012-08-15 南京航空航天大学 基于环路状态自检测的sins/gps深组合导航方法
CN103278837A (zh) * 2013-05-17 2013-09-04 南京理工大学 基于自适应滤波的sins/gnss多级容错组合导航方法
CN103439989A (zh) * 2013-09-03 2013-12-11 湖南工业大学 一种绕包机绕包速度同步控制方法
CN107782307A (zh) * 2016-08-26 2018-03-09 北京自动化控制设备研究所 一种sins/dr组合导航系统里程计异常数据后处理方法
CN111156994A (zh) * 2019-12-31 2020-05-15 西安航天华迅科技有限公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111272177A (zh) * 2020-02-28 2020-06-12 上海航天控制技术研究所 基于时间对准的间接滤波相对导航方法及系统
CN111947681A (zh) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 一种gnss与惯导组合导航位置输出的滤波校正方法
CN111928869A (zh) * 2020-10-10 2020-11-13 蘑菇车联信息科技有限公司 车辆运动轨迹估计方法、装置及电子设备
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
MIMU/GPS组合导航建模及GPS时间延迟补偿算法研究;钱山等;《系统工程与电子技术》;20090630;第31卷(第6期);1432-1435 *
Time Delay Compensation of IIR Notch Filter for CW Interference Suppression in GNSS;Li Xianbin等;《IEEE 11th International Conference on Electronic Measurement and Instruments (ICEMI)》;20131231;全文 *
惯导辅助BDS/GPS高精度动态定位模型研究;韩厚增;《中国优秀博硕士学位论文全文数据库(博士)基础科学辑》;20171115(第11期);A008-7 *

Also Published As

Publication number Publication date
CN113607176A (zh) 2021-11-05

Similar Documents

Publication Publication Date Title
CN113607176B (zh) 组合导航系统轨迹输出方法及装置
CN111928869B (zh) 车辆运动轨迹估计方法、装置及电子设备
CN112146683B (zh) 惯性测量单元标定参数调整方法、装置及电子设备
CN112461269B (zh) 惯性测量单元标定方法、设备及服务器
CN112985462B (zh) 基于卷积神经网络模型的惯性测量数据校正方法及装置
CN112197772B (zh) 一种确定车辆移动轨迹的方法及相关装置
CN112762933B (zh) 基于神经网络模型的车辆定位方法及装置
CN113091770B (zh) 惯性测量传感器的零偏的补偿方法
CN111982158B (zh) 一种惯性测量单元标定方法及装置
CN113375669B (zh) 基于神经网络模型的姿态更新方法及装置
JP2013253985A (ja) 衛星信号から導かれるドリフト推定を使用したｇｐｓ受信機の調整可能クロック
CN113917512B (zh) 自动驾驶车辆的定位方法、装置、电子设备及存储介质
CN113375668B (zh) 卫星导航系统的天线安装角标定方法及装置
CN112284421B (zh) 一种imu内参调整方法及相关装置
CN112348903B (zh) 行车记录仪外参数标定方法、装置及电子设备
CN113280813B (zh) 基于神经网络模型的惯性测量数据补偿方法及装置
CN112229401B (zh) 适用于ins-gps伪距融合的量测信息同步外推方法及系统
CN113175936B (zh) 基于神经网络模型的车辆轨迹推算方法及装置
CN112985463B (zh) 基于卷积神经网络模型的惯性测量单元标定方法及装置
CN112130188A (zh) 车辆定位方法、设备及云服务器
CN113758502B (zh) 组合导航处理方法及装置
CN113124884B (zh) 基于lstm神经网络模型的车辆定位方法及装置
CN115839718B (zh) 一种基于运动约束的融合定位方法及装置
CN115683165A (zh) 飞行汽车的imu安装角标定方法、装置及飞行汽车
CN117741699A (zh) 获取姿态角的方法、设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant