CN110017850A - 一种陀螺仪漂移估计方法、装置及定位系统 - Google Patents

一种陀螺仪漂移估计方法、装置及定位系统 Download PDF

Info

Publication number
CN110017850A
CN110017850A CN201910319472.6A CN201910319472A CN110017850A CN 110017850 A CN110017850 A CN 110017850A CN 201910319472 A CN201910319472 A CN 201910319472A CN 110017850 A CN110017850 A CN 110017850A
Authority
CN
China
Prior art keywords
kalman filtering
measurement
gyroscope
zero bias
robot
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
CN201910319472.6A
Other languages
English (en)
Other versions
CN110017850B (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.)
Beijing Puppy Vacuum Cleaner Group Co Ltd
Original Assignee
Xiaogou Electric Internet 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 Xiaogou Electric Internet Technology Beijing Co Ltd filed Critical Xiaogou Electric Internet Technology Beijing Co Ltd
Priority to CN201910319472.6A priority Critical patent/CN110017850B/zh
Publication of CN110017850A publication Critical patent/CN110017850A/zh
Application granted granted Critical
Publication of CN110017850B publication Critical patent/CN110017850B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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

Abstract

本发明涉及一种陀螺仪漂移估计方法、装置及定位系统，属于导航技术领域，解决陀螺仪零偏引起的定位误差；方法包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏本发明通过实时估计陀螺仪的零偏并反馈扣除，提高了航位推算算法得到的姿态、位置的精度。

Description

一种陀螺仪漂移估计方法、装置及定位系统
技术领域
本发明涉及导航技术领域，尤其涉及一种陀螺仪漂移估计方法、装置及定位系统。
背景技术
轮式机器人导航的核心包括定位和建图。定位一般采用两级结构：
S1:使用IMU(Inertial measurement unit，惯性测量单元)数据和里程计数据进行航位推算，得到开环积分位置、姿态；
S2:激光雷达基于SLAM(Simultaneous localization and mapping,即时定位与地图构建)算法、使用上一步的结果为初值，进行扫描匹配得出机器人最终的位置、姿态。
其中，航位推算的特点是具有无界累计误差，但是不受环境特征影响，可以给出持续的位姿。激光雷达SLAM也可以不使用IMU和里程计计算的位姿初值，仅靠激光雷达就可以计算出位姿。激光雷达SLAM算法的位姿经过回环和后端优化之后可以最大程度的消除累计误差。但是激光雷达SLAM在空旷场景和简单重复场景等无特征环境下会失效。航位推算和纯激光雷达SLAM算法在累计误差特性、对于环境特征依赖这两方面具有互补特性。
由于IMU加里程计定位是开环积分，陀螺仪的微小零偏持续积分就会导致较大的姿态、位置误差，由于没有闭环反馈修正，导致姿态、位置误差是无界的增长。
发明内容
鉴于上述的分析，本发明旨在提供一种陀螺仪漂移估计方法、装置及定位系统，采用机器人的定位误差作为量测量，估计出IMU的陀螺仪零偏并反馈，提高航位推算得到的姿态、位置的精度。
本发明的目的主要是通过以下技术方案实现的：
一方面，公开了一种陀螺仪漂移估计方法，包括：
基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
进一步地，在所述以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:
构建IMU模型和航位推算的系统误差方程；
根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。
进一步地，所述陀螺仪零偏为一阶马尔科夫过程随机误差。
进一步地，所述卡尔曼滤波的状态空间模型为所述卡尔曼滤波的量测模型为Z＝HX+V；
其中，为卡尔曼滤波的状态向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。
进一步地，
所述卡尔曼滤波的状态转移矩阵
所述卡尔曼滤波的噪声矩阵
所述卡尔曼滤波的状态噪声向量
所述卡尔曼滤波的量测矩阵
其中，Hf为卡尔曼滤波的比力量测矩阵；Hd为卡尔曼滤波的位置量测矩阵。
进一步地，所述采用卡尔曼滤波实时估计陀螺仪零偏包括：
当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏。
进一步地，所述陀螺仪零偏用于修正陀螺仪量测的角速率。
进一步地，还包括陀螺仪状态监测，所述陀螺仪状态监测包括：
获取卡尔曼滤波产生的测量新息；
对所述测量新息进行归一化得到测量新息的幅值；
比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。
另一方面，还公开了一种陀螺仪漂移估计装置，包括，
航位推算模块，基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
另一方面，还公开了一种定位系统，包括IMU单元、里程计、平面激光雷达和如上所述的陀螺仪漂移估计装置；
所述IMU单元、里程计分别与所述航位推算模块连接，分别输出IMU数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置
所述平面激光雷达，用于采用SLAM算法计算机器人的第二位置并输出至所述定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpn；
所述陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述IMU单元，用于修正IMU单元中陀螺仪量测的角速率。
本发明方案至少可以实现如下之一有益效果：
以基于IMU数据和里程计数据进行航位推算得到机器人位置与基于平面激光雷达计算机器人位置之间的定位误差为量测量，采用卡尔曼滤波实时估计出准确的陀螺仪零偏；并将实时估计出的陀螺仪零偏反馈到IMU进行扣除，解决了航位推算过程中由于陀螺仪零偏持续积分引起的姿态、位置误差，提高了航位推算的姿态、位置精度。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例一提供的一种陀螺仪漂移估计方法的流程图；
图2为本发明实施例二提供的一种陀螺仪漂移估计装置的结构示意图；
图3为本发明实施例三提供的一种定位系统的结构示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
实施例一、
本发明实施例公开了一种陀螺仪漂移估计方法，该方法基于平面激光雷达SLAM定位估计惯性测量单元的陀螺仪漂移，用于陀螺仪的角速率修正。该方法能够应用于轮式机器人，例如轮式工业机器人、轮式家用机器人等，下面的实施例将以家用扫地机器人为例进行说明。
具体方法如图1所示，包括以下步骤：
步骤S1、基于IMU(Inertial measurement unit，惯性测量单元)数据和里程计数据进行航位推算，得到机器人的第一位置
步骤S2、基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
步骤S3、以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
IMU包含了三个单轴的加速度计和三个单轴的陀螺仪。其中，加速度计，用于检测机器人在载体坐标系的三轴加速度信号；陀螺仪，用于检测机器人在载体坐标系的三轴角速率信号。
通过IMU测量机器人在三维空间(载体坐标系)中的角速率和加速度，以此解算得到机器人的姿态矩阵
可选的，采用IMU的六轴融合算法，将加速度计检测的三轴加速度信号和陀螺仪检测的三轴角速率信号进行融合得到机器人的姿态矩阵
具体的，本实施例中，根据姿态矩阵和里程计测量的增量数据(ΔL，ΔR)，进行航位推算(Dead Reckoning,DR)得到机器人的第一位置
利用平面激光雷达采用SLAM(Simultaneous localization and mapping,即时定位与地图构建)算法，计算的机器人第二位置
SLAM算法可以根据平面激光雷达的探测信息，构建机器人周围环境的地图并定位机器人在地图中的位置，该位置即为上述机器人第二位置
SLAM算法可采用HectorSLAM、Cartographer、Gmapping等中的任一算法；
优选的，本实施例采用在长廊及低特征场景中建图效果较好的Gmapping算法；Gmapping算法可以实时构建室内地图，在构建小场景地图所需的计算量较小且定位精度较高；相比HectorSLAM算法对激光雷达频率要求低、鲁棒性高；而相比Cartographer算法在构建小场景地图时，不需要太多的粒子并且没有回环检测，因此计算量小于
Cartographer而精度相当。
根据机器人的第一位置和第二位置计算定位误差δpn，即
基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
通过基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏能够使得基于IMU数据和里程计数据进行航位推算得到的位置更为精确。
由于在本实施例的航位推算(DR)中，不考虑里程计的速度误差以及刻度系数误差，航位推算(DR)的姿态、位置误差主要由于陀螺仪的微小零偏持续积分而导致；因此，在对航位推算结果进行姿态、位置误差校正时，可仅考虑因陀螺仪零偏造成的误差。
优选的，在所述以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:
构建IMU模型和航位推算的系统误差方程；
根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。
构建IMU模型可以是对IMU中的三个单轴的陀螺仪采用白噪声和一阶马尔科夫过程进行建模，得到陀螺仪零偏模型。例如，wε为陀螺仪的角速率白噪声，wrGi是陀螺仪的三个轴的一阶马尔科夫过程激励白噪声，为陀螺仪的一阶马尔科夫过程随机误差。对陀螺仪零偏进行求导，得到微分方程：
该微分过程即为一阶马尔科夫过程。其中，x,y,z为陀螺仪的三个坐标轴，τGi是陀螺仪的三个轴的相关时间常数。
优选的，建立的航位推算系统误差方程，如下：
其中，公式中的为机器人的姿态矩阵，为IMU的姿态失准角；为速度误差、δpn为定位误差，为陀螺零偏漂移，wε为陀螺角速率白噪声。
根据IMU模型和航位推算系统误差方程构建卡尔曼滤波模型：
其中，为卡尔曼滤波模型的状态空间模型；Z＝HX+V为卡尔曼滤波模型的量测模型。
式(3)中，为卡尔曼滤波的向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。
卡尔曼滤波的状态向量中的，姿态失准角是导致速度误差和位置误差的向量，需要在状态向量中表达；定位误差δpn是量测量；速度误差是中间变量，用于在位置误差和陀螺仪零偏之间建立关系；陀螺零偏漂移是状态向量中的待估计量。
通过将低精度的惯性器件的时间相关误差模型为一阶马尔科夫过程模型，能够在长时间组合滤波后避免滤波器过度收敛现象导致的滤波器抗干扰性能变差，且能够降低陀螺仪建模维数和滤波计算量。
且若惯性器件误差中存在较大随机常值成分，可通过滤波器的惯性器件误差反馈校正，消除随机常值误差的影响；
更优选的，卡尔曼滤波状态空间模型中，
卡尔曼滤波的状态转移矩阵
卡尔曼滤波的噪声矩阵
卡尔曼滤波的状态噪声向量
卡尔曼滤波的量测矩阵
其中，Hf为卡尔曼滤波的比力量测矩阵；Hd为卡尔曼滤波的位置量测矩阵；wε为陀螺角速率白噪声，wrG是一阶马尔科夫过程激励白噪声。
本实施例使用比力误差、位置误差双观测量构成卡尔曼滤波的量测矩阵，其中，增加比力误差观测量可以阻尼水平姿态(俯仰、横滚)漂移，让位置误差与陀螺仪漂移误差关系简化，让位置误差观测可以更精确的估计陀螺仪漂移。
具体的，Hf＝[fn×03×3 03×3 03×3]，
Hd＝[03×3 03×3 I3×3 03×3]；
并且，卡尔曼滤波的量测量为比力误差δfn＝fn+gn；
其中，fn为比力在导航坐标系下的投影，gn为重力在导航坐标系下的投影。
卡尔曼滤波的状态转移矩阵F中的βG为陀螺仪的误差模型；
具体的，βG＝diag(1/τx 1/τy 1/τz)；τx、τy、τz分别为陀螺仪在x、y、z轴上的相关时间参数。
根据卡尔曼滤波模型的状态空间模型和量测模型，使用卡尔曼滤波算法估计卡尔曼滤波的状态向量中的陀螺仪零偏待其中的陀螺仪零偏估值稳定后，输出陀螺仪零偏
优选的，当相邻两次的陀螺仪零偏估值的差值小于0.001时，即当时，判定陀螺仪零偏估值稳定，输出陀螺仪零偏否则，不输出。其中，k为卡尔曼滤波的时刻(次数)，k为自然数。这样避免了陀螺仪零偏或者存在估值不准导致的陀螺仪零偏结果不可用的情况。
IMU模型中的陀螺仪角速率为根据以下公式进行陀螺仪量测角速率修正；
使用修正后的陀螺仪量测角速率参与后续的六轴融合算法。
通过将输出的上述陀螺仪零偏反馈至IMU模型，用于修正陀螺仪量测角速率，以提高陀螺仪检测角速率的准确性。
由于，在扫地机器人运动过程中，当扫地机器人打滑时，航位推算的位置也会不准，以此位置计算定位误差δpn，会使卡尔曼滤波后得到的陀螺仪零偏误差增大，从而造成陀螺仪量测角速率修偏的问题。
需要注意的是，对于激光雷达SLAM来说，也可能存在因为环境特征缺乏导致的定位不准确，同样，以此位置计算定位误差δpn，也将造成陀螺仪量测角速率修偏的问题。
由于上述两种状况都会导致估计的陀螺仪零偏不可用，因此，需要检测出该两种状况并在该两种状况发生时断开陀螺仪零偏反馈。
优选的，本实施例通过设置状态监测进行陀螺仪状态检测，并在检测到相应状况时，停止陀螺仪零偏反馈；
具体的，陀螺仪状态监测步骤如下：
1)获取卡尔曼滤波产生的测量新息
卡尔曼滤波的测量新息
式中的zk为k时刻的卡尔曼滤波量测值，为k时刻的先验量测值；
2)对所述测量新息进行归一化得到测量新息的幅值
测量新息的幅值利用下式计算得到：
式中，为测量新息的协方差，即为测量噪声协方差Rk以及转换到测量空间的状态估计的误差协方差之和，即
下标k为卡尔曼滤波的时刻(次数)。
3)比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。
优选的，可对测量新息的幅值进行滤波，判断新的测量是否和以前的测量一致，以检验陀螺仪零偏估计是否异常。优选地，卡尔曼状态异常阈值可优选为滤波值的99.73％。
在一个理想的卡尔曼(Kalman)滤波器中，测量新息的幅值为零均值单位方差的高斯分布，并且一旦估计的状态已经收敛到其对应的真值，这些新息后续的连续输出几乎独立。
考虑到在测量噪声变大或是测量存在时间间断时，状态不确定性变大都会导致测量新息变大，导致测量新息的幅值有波动；同时，考虑到当有测量故障时，比如里程计打滑导致位置误差引入了非算法因素，将会导致测量新息的幅值变化更大。通过对测量新息的幅值滤波可以滤掉短期错误数据，如源于激光SLAM匹配的误定位、测量毛刺噪声等错误数据，以提高状态监测的准确性。
实施例二、
本实施例公开了一种陀螺仪漂移估计装置，该装置能够应用于轮式机器人，例如轮式工业机器人、轮式家用机器人等，下面的实施例将以家用扫地机器人为例进行说明。
如图2所示，包括：
航位推算模块，基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
优选的，本实施例中采用与实施例一中相同的方法，构建IMU模型和航位推算的系统误差方程；并根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。
对IMU中的三个单轴的陀螺仪采用白噪声和一阶马尔科夫过程进行建模，得到的陀螺仪零偏为一阶马尔科夫过程随机误差。
构建卡尔曼滤波的状态空间模型为量测模型为Z＝HX+V。
其中，为卡尔曼滤波的状态空间模型；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测模型；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。
具体的模型中的各参数矩阵的取值与实施例一中的相同。
根据建立的卡尔曼滤波的状态空间模型和量测模型，使用卡尔曼滤波算法估计卡尔曼滤波的状态向量中的
当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏
优选的，所述陀螺仪零偏反馈到IMU模型，用于修正陀螺仪量测的角速率。
优选的，本实施例的定位装置还包括状态监测模块；用于对扫地机器人运动进行监测，在扫地机器人运动过程中，当扫地机器人打滑时，航位推算算法的位置也会不准，或者对于激光雷达SLAM，存在因为环境特征缺乏导致的定位不准确时，防止估计位置误差使陀螺仪零偏修偏问题；
状态监测模块与所述陀螺仪零偏估计模块连接，根据获得卡尔曼滤波过程中产生的测量新息，判断卡尔曼滤波得到的陀螺仪零偏是否异常，如果异常，则停止对陀螺仪量测角速率进行修正。
具体的，状态监测模块，获得卡尔曼滤波过程中产生的测量新息并对测量信息进行归一化得到测量新息的幅值将测量新息的幅值与设置的状态异常阈值进行比较；当测量新息的幅值大于陀螺仪状态异常阈值时，判断陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。
实施例三、
本实施例还公开了一种定位系统，如图3所示，包括IMU单元、里程计、平面激光雷达和如上所述的陀螺仪漂移估计装置；
所述IMU单元、里程计分别与陀螺仪漂移估计装置的航位推算模块连接，分别输出IMU数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置
所述平面激光雷达，用于采用SLAM算法计算机器人的第二位置并输出至所述陀螺仪漂移估计装置的定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpn；
所述陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述IMU单元，用于修正IMU单元中陀螺仪量测的角速率，提高航位推算中得到的姿态、位置的精度。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种陀螺仪漂移估计方法，其特征在于，包括：
基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
2.根据权利要求1所述的陀螺仪漂移估计方法，其特征在于，在所述以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:
构建IMU模型和航位推算的系统误差方程；
根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。
3.根据权利要求1或2所述的陀螺仪漂移估计方法，其特征在于，所述陀螺仪零偏为一阶马尔科夫过程随机误差。
4.根据权利要求2所述的陀螺仪漂移估计方法，其特征在于，所述卡尔曼滤波的状态空间模型为所述卡尔曼滤波的量测模型为Z＝HX+V；
其中，为卡尔曼滤波的状态向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。
5.根据权利要求4所述的陀螺仪漂移估计方法，其特征在于，所述卡尔曼滤波的状态转移矩阵
所述卡尔曼滤波的噪声矩阵
所述卡尔曼滤波的状态噪声向量
所述卡尔曼滤波的量测矩阵
其中，Hf为卡尔曼滤波的比力量测矩阵；Hd为卡尔曼滤波的位置量测矩阵；wε为陀螺角速率白噪声，wrG是一阶马尔科夫过程激励白噪声。
6.根据权利要求1-5任一项所述的陀螺仪漂移估计方法，其特征在于，所述采用卡尔曼滤波实时估计陀螺仪零偏包括：
当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏
7.根据权利要求6所述的陀螺仪漂移估计方法，其特征在于，所述陀螺仪零偏用于修正陀螺仪量测的角速率。
8.根据权利要求1-7任一所述的陀螺仪漂移估计方法，其特征在于，还包括陀螺仪状态监测，所述陀螺仪状态监测包括：
获取卡尔曼滤波产生的测量新息；
对所述测量新息进行归一化得到测量新息的幅值；
比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。
9.一种陀螺仪漂移估计装置，其特征在于，包括，
航位推算模块，基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置
定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；
陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏
10.一种定位系统，其特征在于，包括IMU单元、里程计、平面激光雷达和如权利要求9所述的陀螺仪漂移估计装置；
所述IMU单元、里程计分别与所述航位推算模块连接，分别输出IMU数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置
所述平面激光雷达，用于采用SLAM算法计算机器人的第二位置并输出至所述定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpn；
所述陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述IMU单元，用于修正IMU单元中陀螺仪量测的角速率。
CN201910319472.6A 2019-04-19 2019-04-19 一种陀螺仪漂移估计方法、装置及定位系统 Active CN110017850B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910319472.6A CN110017850B (zh) 2019-04-19 2019-04-19 一种陀螺仪漂移估计方法、装置及定位系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910319472.6A CN110017850B (zh) 2019-04-19 2019-04-19 一种陀螺仪漂移估计方法、装置及定位系统

Publications (2)

Publication Number Publication Date
CN110017850A true CN110017850A (zh) 2019-07-16
CN110017850B CN110017850B (zh) 2021-04-20

Family

ID=67191943

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910319472.6A Active CN110017850B (zh) 2019-04-19 2019-04-19 一种陀螺仪漂移估计方法、装置及定位系统

Country Status (1)

Country Link
CN (1) CN110017850B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110555801A (zh) * 2019-07-26 2019-12-10 纵目科技（上海）股份有限公司 一种航迹推演的校正方法、终端和存储介质
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质
CN110988949A (zh) * 2019-12-02 2020-04-10 北京京东乾石科技有限公司 定位方法、定位装置、计算机可读存储介质与可移动设备
CN111197994A (zh) * 2019-12-31 2020-05-26 深圳一清创新科技有限公司 位置数据修正方法、装置、计算机设备和存储介质
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN111721288A (zh) * 2020-06-19 2020-09-29 哈尔滨工业大学 一种mems器件零偏修正方法、装置及存储介质
CN112082529A (zh) * 2020-07-29 2020-12-15 上海谷感智能科技有限公司 基于惯性传感器的小型家电姿态测量方法及姿态识别模块
CN114199236A (zh) * 2021-11-29 2022-03-18 北京百度网讯科技有限公司 定位数据处理的方法、装置、电子设备和自动驾驶车辆

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102004041512A1 (de) * 2004-08-27 2006-03-02 Robert Bosch Gmbh Verfahren zur Kalibrierung eines Gyroskops in einem Ortungsmodul in einem beweglichen System
CN102607595A (zh) * 2012-03-07 2012-07-25 北京航空航天大学 应用激光多普勒测速仪测试捷联挠性陀螺动态随机漂移的方法
CN103591955A (zh) * 2013-11-21 2014-02-19 西安中科光电精密工程有限公司 组合导航系统
CN103630146A (zh) * 2013-09-15 2014-03-12 北京航空航天大学 一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN106969784A (zh) * 2017-03-24 2017-07-21 中国石油大学(华东) 一种并发建图定位与惯性导航的组合误差融合系统
KR20180052075A (ko) * 2016-11-09 2018-05-17 애틀랜틱 이너셜 시스템스 리미티드 항법 시스템

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102004041512A1 (de) * 2004-08-27 2006-03-02 Robert Bosch Gmbh Verfahren zur Kalibrierung eines Gyroskops in einem Ortungsmodul in einem beweglichen System
CN102607595A (zh) * 2012-03-07 2012-07-25 北京航空航天大学 应用激光多普勒测速仪测试捷联挠性陀螺动态随机漂移的方法
CN103630146A (zh) * 2013-09-15 2014-03-12 北京航空航天大学 一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法
CN103591955A (zh) * 2013-11-21 2014-02-19 西安中科光电精密工程有限公司 组合导航系统
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
KR20180052075A (ko) * 2016-11-09 2018-05-17 애틀랜틱 이너셜 시스템스 리미티드 항법 시스템
CN106969784A (zh) * 2017-03-24 2017-07-21 中国石油大学(华东) 一种并发建图定位与惯性导航的组合误差融合系统

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
PAUL D.GROVES: "《GNSS与惯性及多传感器组合导航系统原理》", 31 March 2015 *
QINGQUAN LI: "《Laser-Aided INS and Odometer Navigation System for Subway Track Irregularity Measurement》", 《JOURNAL OF SURVEYING ENGINEERING》 *
RONGBING LI: "《LIDAR/MEMS IMU Integrated Navigation (SLAM) Method for a Small UAV in Indoor Environments》", 《2014 DGON INERTIAL SENSORS AND SYSTEMS (ISS)》 *
余彤: "《LiDAR/INS组合的室内定位与制图(SLAM)算法改进》", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
汪名飞等: "多功能巡检救援机器人系统设计及定位导航", 《机械设计与制造》 *
闫浩月: "《融合激光测距仪和惯导信息的移动机器人室内定位方法研究》", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
韩昊等: "基于ROS和激光雷达的AGV导航系统设计与实现", 《电子测量技术》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110555801A (zh) * 2019-07-26 2019-12-10 纵目科技（上海）股份有限公司 一种航迹推演的校正方法、终端和存储介质
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质
CN110988949A (zh) * 2019-12-02 2020-04-10 北京京东乾石科技有限公司 定位方法、定位装置、计算机可读存储介质与可移动设备
CN111197994A (zh) * 2019-12-31 2020-05-26 深圳一清创新科技有限公司 位置数据修正方法、装置、计算机设备和存储介质
CN111197994B (zh) * 2019-12-31 2021-12-07 深圳一清创新科技有限公司 位置数据修正方法、装置、计算机设备和存储介质
CN111721288A (zh) * 2020-06-19 2020-09-29 哈尔滨工业大学 一种mems器件零偏修正方法、装置及存储介质
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN112082529A (zh) * 2020-07-29 2020-12-15 上海谷感智能科技有限公司 基于惯性传感器的小型家电姿态测量方法及姿态识别模块
CN114199236A (zh) * 2021-11-29 2022-03-18 北京百度网讯科技有限公司 定位数据处理的方法、装置、电子设备和自动驾驶车辆

Also Published As

Publication number Publication date
CN110017850B (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN110017850A (zh) 一种陀螺仪漂移估计方法、装置及定位系统
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN105698765B (zh) 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN103776446B (zh) 一种基于双mems-imu的行人自主导航解算算法
CN113091709B (zh) 一种新型gnss接收机倾斜测量方法
Ladetto et al. Digital magnetic compass and gyroscope integration for pedestrian navigation
CN106056664A (zh) 一种基于惯性和深度视觉的实时三维场景重构系统及方法
KR20150089061A (ko) 물체의 움직임을 추정하기 위한 시스템 및 방법
CN107607113A (zh) 一种两轴姿态倾角测量方法
CN109827545A (zh) 一种基于双mems加速度计的在线倾角测量方法
CN109099913A (zh) 一种基于mems惯性器件的穿戴式导航装置和方法
CN108663068A (zh) 一种应用在初始对准中的svm自适应卡尔曼滤波方法
CN107063245A (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN109945859A (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
KR20110085495A (ko) 센서오차의 작동 중 자동교정 방법과 이를 이용한 관성항법장치
JP2021179438A (ja) 機首方位測定システムにおけるセンサ測定の欠如を補償するシステムと方法
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN104406592B (zh) 一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法
CN109612464B (zh) 基于iez框架下的多算法增强的室内导航系统及方法
CN110873813A (zh) 一种水流速度估算方法、组合导航方法及装置
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN114789439B (zh) 斜坡定位校正方法、装置、机器人及可读存储介质
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN113029173A (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
CP01 Change in the name or title of a patent holder

Address after: 7-605, 6th floor, building 1, yard a, Guanghua Road, Chaoyang District, Beijing 100026

Patentee after: Beijing dog vacuum cleaner Group Co.,Ltd.

Address before: 7-605, 6th floor, building 1, yard a, Guanghua Road, Chaoyang District, Beijing 100026

Patentee before: PUPPY ELECTRONIC APPLIANCES INTERNET TECHNOLOGY (BEIJING) Co.,Ltd.

CP01 Change in the name or title of a patent holder